CN114296059A - 一种面向大型救援装备的激光雷达点云畸变矫正方法 - Google Patents
一种面向大型救援装备的激光雷达点云畸变矫正方法 Download PDFInfo
- Publication number
- CN114296059A CN114296059A CN202111555737.6A CN202111555737A CN114296059A CN 114296059 A CN114296059 A CN 114296059A CN 202111555737 A CN202111555737 A CN 202111555737A CN 114296059 A CN114296059 A CN 114296059A
- Authority
- CN
- China
- Prior art keywords
- laser
- laser radar
- coordinate system
- angle
- ith
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 37
- 238000012937 correction Methods 0.000 title claims abstract description 25
- 239000011159 matrix material Substances 0.000 claims abstract description 64
- 230000009466 transformation Effects 0.000 claims abstract description 50
- 238000013519 translation Methods 0.000 claims abstract description 25
- 230000008859 change Effects 0.000 claims abstract description 16
- 238000001914 filtration Methods 0.000 claims abstract description 15
- 238000005259 measurement Methods 0.000 claims description 12
- 230000007246 mechanism Effects 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 3
- 239000013598 vector Substances 0.000 claims description 3
- 230000009471 action Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明提出了一种面向大型救援装备的激光雷达点云畸变矫正方法,首先利用激光雷达处三个姿态角变化确定点云的旋转变换矩阵,然后利用激光雷达处位置变化确定点云的平移变换矩阵,最后利用卡尔曼滤波提高激光雷达点云畸变矫正的准确性。该方法兼顾激光雷达处旋转的角度和角速度,保证旋转变换矩阵的准确性;分析大型救援装备灾后施救的操作,将激光雷达处的位置变化分为两种,提高平移变换矩阵使用场景的适应性;利用卡尔曼滤波降低旋转平移变换矩阵中大型救援装备运动学参数的噪声,进而提高激光雷达点云畸变矫正的准确性。
Description
技术领域
本发明涉及一种激光雷达点云畸变矫正方法,具体的是一种面向大型救援装备的激光雷达点云畸变矫正方法,属于灾后救援领域。
背景技术
我国是全球受地质灾害最为严重的国家之一,地质灾害包括地震、泥石流、滑坡等,具有发生频率高、危害程度高等特点。地质灾害发生后,救援队伍需要借助工程机械开展应急救援工作。现有的以人作业为主的大型救援装备施救时,现场需要大量的救援人员,导致现场安全无保障、救援效率低下等突出问题。为克服上述问题,需要提高大型救援装备的智能化程度,因此在装备上增加了很多传感器,包括激光雷达、摄像头、毫米波雷达等。
激光雷达具有较广的覆盖范围,能够适用于各种比较恶劣的环境;具有较强的抗干扰能力,能够有效地规避周边环境中无线电波的干扰;具有较高的测距精度,能够准确地获得周围环境中目标的位置信息。当激光雷达收集周围环境的点云时,由于载体的运动或转向,每一帧点云中的第一个点和最后一个点的坐标系间都存在一定的位姿差。也就是说,在同一帧点云中各个点都不在同一个坐标系中,这就导致了点云畸变。
激光雷达点云畸变矫正的原理为:利用系列的坐标变换将一帧中所有激光点都变换到第一个激光点的坐标系下。何磊等针对无人驾驶车辆,利用惯性导航***和轮速传感器的信息,提出了一种激光雷达点云去畸变方法(专利申请号:201811479464.X)。该方法可以去除因激光雷达载体航向角变化和平面运动产生的误差,但忽略了载体俯仰角和侧倾角的影响。***等针对无人船,综合考虑船身的三个姿态角和位置变化,以矫正激光雷达点云的畸变(专利申请号:201911222073.4)。
大型救援装备中最常使用的是履带式挖掘机,装备的上部转台和行走机构可独立工作,本发明以挖掘机为例介绍提出的方法。装备行驶时,不得做回转动作,激光雷达与装备的位置变化保持一致;装备进行回转操作时,装备的位置保持不变,而激光雷达进行圆周运动。然而现有方法认为激光雷达与其载体的位置变化相同,未考虑激光雷达与其载体的位置变化相异的场景。大型救援装备作业和在凹凸不平路面上行驶时,激光雷达安装处的俯仰角、侧倾角和航向角会持续变化,通过传感器测量存在一定的误差。然而现有方法未考虑如何保障三个角度测量的准确性。
发明内容
针对以上问题,本发明提出一种面向大型救援装备的激光雷达点云畸变矫正方法,该方法兼顾激光雷达处旋转的角度和角速度,保证旋转变换矩阵的准确性;分析大型救援装备灾后施救的操作,将激光雷达处的位置变化分为两种,提高平移变换矩阵使用场景的适应性;利用卡尔曼滤波降低旋转平移变换矩阵中大型救援装备运动学参数的噪声,进而提高激光雷达点云畸变矫正的准确性。
本发明提供一种面向大型救援装备的激光雷达点云畸变矫正方法,具体步骤如下:
步骤一:利用激光雷达处三个姿态角变化确定点云的旋转变换矩阵;
激光雷达固定在装备驾驶舱的顶部,坐标系满足右手准则,Z轴正方向指向驾驶舱的正上方,Y轴正方向指向驾驶舱的正前方,X轴正方向指向驾驶舱的正右方;
在一帧点云中,第i个激光点的坐标系需要依次绕X,Y,Z轴旋转axi,ayi,azi度才能与第1个激光点的坐标系保持对应坐标轴同向且平行,旋转方向满足右手法则记为正值,反之记为负值;
第i个激光点的旋转变换矩阵F为:
步骤二:利用激光雷达处位置变化确定点云的平移变换矩阵;
装备行驶时,以第1个激光点对应的激光雷达位置为原点建立平面直角坐标系的正方向指向正东方向,的正方向指向正北方向;在坐标系中,第i个激光点对应的激光雷达位置坐标为sj为第j个激光点时装备的速度;bj为第j个激光点时装备行驶方向与正东方向的夹角,为装备行走机构中心处的航向角;坐标系逆时针旋转度得到第1个激光点的XY坐标系,在第1个激光点的XY坐标系中,第i个激光点的平移变换矩阵为
装备执行回转操作时,以上部转台的旋转中心为原点建立平面直角坐标系的正方向指向正东方向,的正方向指向正北方向;在坐标系中,第1个激光点对应的激光雷达位置坐标为第i个激光点对应的激光雷达位置坐标为d为激光雷达距旋转中心的距离;ei为在坐标系中第i个激光点对应的激光雷达位置的极角,在坐标系中,第i个激光点对应的激光雷达位置坐标为在第1个激光点的XY坐标系中,第i个激光点的平移变换矩阵为
第i个激光点的平移变换矩阵为:
步骤三:利用卡尔曼滤波提高激光雷达点云畸变矫正的准确性;
针对装备救援过程,取状态量为其中,分别为激光雷达处的俯仰角、侧倾角和航向角,分别为激光雷达处的俯仰角速度、侧倾角速度和航向角速度,分别为装备行走机构中心处的航向角和航向角速度;以激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角和装备行走机构中心处惯性传感器输出的航向角为观测量,即其中,分别为激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角,为装备行走机构中心处惯性传感器输出的航向角;
离散化后的卡尔曼滤波的状态方程和观测方程为:
递推过程包括时间更新和测量更新,下面递推过程的前两步为时间更新,剩余的三步为测量更新;
时间更新:
测量更新:
有益效果:
1.本发明兼顾激光雷达坐标系旋转的角度和角速度,从而保证旋转变换矩阵的准确性;
2.本发明分析大型救援装备灾后施救的操作,将激光雷达处的位置变化分为两种,提高平移变换矩阵使用场景的适应性;
3.本发明利用卡尔曼滤波降低旋转平移变换矩阵中大型救援装备运动学参数的噪声,进而提高激光雷达点云畸变矫正的准确性。
附图说明
图1为用于大型救援装备的激光雷达点云畸变矫正方法总体设计方案图;
图2为激光雷达坐标系旋转方向的示意图。
具体实施方式
以下将结合具体实例对本发明提供的技术方案进行详细说明,应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
本发明提出了一种面向大型救援装备的激光雷达点云畸变矫正方法。首先利用激光雷达处三个姿态角变化确定点云的旋转变换矩阵,然后利用激光雷达处位置变化确定点云的平移变换矩阵,最后利用卡尔曼滤波提高激光雷达点云畸变矫正的准确性。该方法兼顾激光雷达处旋转的角度和角速度,保证旋转变换矩阵的准确性;分析大型救援装备灾后施救的操作,将激光雷达处的位置变化分为两种,提高平移变换矩阵使用场景的适应性;利用卡尔曼滤波降低旋转平移变换矩阵中大型救援装备运动学参数的噪声,进而提高激光雷达点云畸变矫正的准确性。
总体设计方案如图1所示,具体步骤包括:
步骤一:利用激光雷达处三个姿态角变化确定点云的旋转变换矩阵;
激光雷达固定在装备驾驶舱的顶部,坐标系满足右手准则,Z轴正方向指向驾驶舱的正上方,Y轴正方向指向驾驶舱的正前方,X轴正方向指向驾驶舱的正右方。激光雷达在垂直方向上有多个激光器,同一时刻发出多束激光。由于每束激光的点云矫正方法相同,故本发明对单束激光的点云矫正方法进行介绍。
在一帧点云中,第i个激光点的坐标系需要依次绕X,Y,Z轴旋转axi,ayi,azi度才能与第1个激光点的坐标系保持对应坐标轴同向且平行,旋转方向满足右手法则记为正值,反之记为负值,如图2所示。
分别为第i个激光点对应的激光雷达处俯仰角、侧倾角、航向角,角度增大方向满足右手法则,在本发明中航向角定义为与正北方向的夹角;分别为第j个激光点时三个角度对应的角速度,j=1,2,…,i-1;f为同一帧点云中相邻两个激光点的间隔时间,通过激光雷达的产品手册获得。
因此,第i个激光点的旋转变换矩阵F为:
步骤二:利用激光雷达处位置变化确定点云的平移变换矩阵;
灾后现场施救时,在激光雷达旋转一周的时间里,大型救援装备可看成做平面运动,即认为激光雷达位置在垂直方向上保持不变。装备行驶时,由于不得做回转动作,激光雷达与装备的位置变化保持一致;装备进行回转操作时,装备位置保持不变,而激光雷达进行圆周运动。
装备行驶时,以第1个激光点对应的激光雷达位置为原点建立平面直角坐标系的正方向指向正东方向,的正方向指向正北方向。在坐标系中,第i个激光点对应的激光雷达位置坐标为sj为第j个激光点时装备的速度,通过行走马达信息换算获得;bj为第j个激光点时装备行驶方向与正东方向的夹角,为装备行走机构中心处的航向角。坐标系逆时针旋转度得到第1个激光点的XY坐标系,在第1个激光点的XY坐标系中,第i个激光点的平移变换矩阵为
装备执行回转操作时,以上部转台的旋转中心为原点建立平面直角坐标系的正方向指向正东方向,的正方向指向正北方向。在坐标系中,第1个激光点对应的激光雷达位置坐标为第i个激光点对应的激光雷达位置坐标为d为激光雷达距旋转中心的距离,事先静态测量获得;ei为在坐标系中第i个激光点对应的激光雷达位置的极角,在坐标系中,第i个激光点对应的激光雷达位置坐标为在第1个激光点的XY坐标系中,第i个激光点的平移变换矩阵为
因此,第i个激光点的平移变换矩阵为:
步骤三:利用卡尔曼滤波提高激光雷达点云畸变矫正的准确性
旋转变换矩阵F和平移变换矩阵B中的参数涉及到激光雷达处的三轴角度、角速度和行走机构中心处的航向角,考虑到灾后施救时大型救援装备处于动态,惯性传感器所获得的数据存在较大的随机误差,进而影响激光雷达点云畸变矫正的准确性。因此利用卡尔曼滤波提高上述参数的精度。
针对装备救援过程,取状态量为其中,分别为激光雷达处的俯仰角、侧倾角和航向角,分别为激光雷达处的俯仰角速度、侧倾角速度和航向角速度,分别为装备行走机构中心处的航向角和航向角速度。以激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角和装备行走机构中心处惯性传感器输出的航向角为观测量,即其中,分别为激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角,为装备行走机构中心处惯性传感器输出的航向角。
离散化后的卡尔曼滤波的状态方程和观测方程为:
式中,t表示离散化时刻;A表示状态转移矩阵,c表示离散周期;H表示观测矩阵,由于状态量的参数中包含观测量的参数,故 q和r是互不相关的零均值***白噪声和观测白噪声向量,q对应的***噪声协方差阵为Q,r对应的观测噪声协方差阵为R。
递推过程包括时间更新和测量更新,下面递推过程的前两步为时间更新,剩余的三步为测量更新;
时间更新:
测量更新:
Claims (1)
1.一种面向大型救援装备的激光雷达点云畸变矫正方法,其特征在于,具体步骤如下:
步骤一:利用激光雷达处三个姿态角变化确定点云的旋转变换矩阵;
激光雷达固定在装备驾驶舱的顶部,坐标系满足右手准则,Z轴正方向指向驾驶舱的正上方,Y轴正方向指向驾驶舱的正前方,X轴正方向指向驾驶舱的正右方;
在一帧点云中,第i个激光点的坐标系需要依次绕X,Y,Z轴旋转axi,ayi,azi度才能与第1个激光点的坐标系保持对应坐标轴同向且平行,旋转方向满足右手法则记为正值,反之记为负值;
第i个激光点的旋转变换矩阵F为:
步骤二:利用激光雷达处位置变化确定点云的平移变换矩阵;
装备行驶时,以第1个激光点对应的激光雷达位置为原点建立平面直角坐标系 的正方向指向正东方向,的正方向指向正北方向;在坐标系中,第i个激光点对应的激光雷达位置坐标为sj为第j个激光点时装备的速度;bj为第j个激光点时装备行驶方向与正东方向的夹角, 为装备行走机构中心处的航向角;坐标系逆时针旋转度得到第1个激光点的XY坐标系,在第1个激光点的XY坐标系中,第i个激光点的平移变换矩阵为
装备执行回转操作时,以上部转台的旋转中心为原点建立平面直角坐标系 的正方向指向正东方向,的正方向指向正北方向;在坐标系中,第1个激光点对应的激光雷达位置坐标为第i个激光点对应的激光雷达位置坐标为d为激光雷达距旋转中心的距离;ei为在坐标系中第i个激光点对应的激光雷达位置的极角,在坐标系中,第i个激光点对应的激光雷达位置坐标为在第1个激光点的XY坐标系中,第i个激光点的平移变换矩阵为
第i个激光点的平移变换矩阵为:
步骤三:利用卡尔曼滤波提高激光雷达点云畸变矫正的准确性;
针对装备救援过程,取状态量为其中,分别为激光雷达处的俯仰角、侧倾角和航向角,分别为激光雷达处的俯仰角速度、侧倾角速度和航向角速度,分别为装备行走机构中心处的航向角和航向角速度;以激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角和装备行走机构中心处惯性传感器输出的航向角为观测量,即其中,分别为激光雷达处惯性传感器输出的俯仰角、侧倾角、航向角,为装备行走机构中心处惯性传感器输出的航向角;
离散化后的卡尔曼滤波的状态方程和观测方程为:
递推过程包括时间更新和测量更新,下面递推过程的前两步为时间更新,剩余的三步为测量更新;
时间更新:
测量更新:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111555737.6A CN114296059A (zh) | 2021-12-17 | 2021-12-17 | 一种面向大型救援装备的激光雷达点云畸变矫正方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111555737.6A CN114296059A (zh) | 2021-12-17 | 2021-12-17 | 一种面向大型救援装备的激光雷达点云畸变矫正方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114296059A true CN114296059A (zh) | 2022-04-08 |
Family
ID=80968488
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111555737.6A Pending CN114296059A (zh) | 2021-12-17 | 2021-12-17 | 一种面向大型救援装备的激光雷达点云畸变矫正方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114296059A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115079128A (zh) * | 2022-08-23 | 2022-09-20 | 深圳市欢创科技有限公司 | 一种激光雷达点云数据去畸变的方法、装置及机器人 |
-
2021
- 2021-12-17 CN CN202111555737.6A patent/CN114296059A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115079128A (zh) * | 2022-08-23 | 2022-09-20 | 深圳市欢创科技有限公司 | 一种激光雷达点云数据去畸变的方法、装置及机器人 |
CN115079128B (zh) * | 2022-08-23 | 2022-12-09 | 深圳市欢创科技有限公司 | 一种激光雷达点云数据去畸变的方法、装置及机器人 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110631593B (zh) | 一种用于自动驾驶场景的多传感器融合定位方法 | |
US11243081B2 (en) | Slam assisted INS | |
CN109946730B (zh) | 一种车路协同下基于超宽带的车辆高可靠融合定位方法 | |
CN109946731B (zh) | 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法 | |
CN110208842A (zh) | 一种车联网环境下车辆高精度定位方法 | |
EP0936519B1 (en) | Integrated vehicle positioning and navigation system, apparatus and method | |
CN107132563B (zh) | 一种里程计结合双天线差分gnss的组合导航方法 | |
CN111338342B (zh) | 一种轮式工程机械自动循迹行驶控制***及方法 | |
CN113311436B (zh) | 一种移动平台上激光测风雷达运动姿态测风订正方法 | |
CN104535061A (zh) | 一种基于多传感器数据融合的导航*** | |
CN1731091A (zh) | 车载快速定位定向方法 | |
CN105929836B (zh) | 用于四旋翼飞行器的控制方法 | |
CN111982114A (zh) | 一种采用imu数据融合估计三维位姿的救援机器人 | |
CN112034479A (zh) | 一种应用于煤矿井下智能巡检无人机的定位方法及*** | |
CN115993825A (zh) | 一种基于空地协同的无人车集群控制*** | |
CN114296059A (zh) | 一种面向大型救援装备的激光雷达点云畸变矫正方法 | |
CN116728410A (zh) | 一种在狭窄工作环境下的机器人绝对定位精度误差补偿方法 | |
RU195749U1 (ru) | Интеллектуальная система технического зрения беспилотного летательного аппарата для решения задач навигации, построения трехмерной карты окружающего пространства и препятствий и автономного патрулирования | |
CN114061565B (zh) | 一种无人船舶slam及其应用方法 | |
CN115031726A (zh) | 一种数据融合导航定位方法 | |
Zorina et al. | Enhancement of INS/GNSS integration capabilities for aviation-related applications | |
CN113581320A (zh) | 矿山自主三维测绘无人车及测绘方法 | |
Wang et al. | A robust lidar state estimation and map building approach for urban road | |
CN113670318B (zh) | 协同定位方法及定位*** | |
Zhao et al. | Cooperative localization based on robust GPS and Radar fusion for multiple aerial vehicles |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |