CN113777589B - 一种基于点特征的lidar与gps/imu联合标定方法 - Google Patents
一种基于点特征的lidar与gps/imu联合标定方法 Download PDFInfo
- Publication number
- CN113777589B CN113777589B CN202110947142.9A CN202110947142A CN113777589B CN 113777589 B CN113777589 B CN 113777589B CN 202110947142 A CN202110947142 A CN 202110947142A CN 113777589 B CN113777589 B CN 113777589B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- lidar
- calibration
- imu
- vehicle
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 82
- 230000033001 locomotion Effects 0.000 claims abstract description 36
- 238000013519 translation Methods 0.000 claims abstract description 36
- 239000011159 matrix material Substances 0.000 claims description 29
- 230000008569 process Effects 0.000 claims description 21
- 238000005457 optimization Methods 0.000 claims description 11
- 238000006243 chemical reaction Methods 0.000 claims description 9
- 238000009434 installation Methods 0.000 claims description 7
- 238000005096 rolling process Methods 0.000 claims description 7
- 230000009466 transformation Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 4
- 238000012546 transfer Methods 0.000 claims description 3
- 230000000694 effects Effects 0.000 abstract description 3
- 230000015556 catabolic process Effects 0.000 abstract description 2
- 238000006731 degradation reaction Methods 0.000 abstract description 2
- 238000002474 experimental method Methods 0.000 description 5
- 238000005065 mining Methods 0.000 description 5
- 238000001514 detection method Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 208000023445 Congenital pulmonary airway malformation Diseases 0.000 description 1
- 102000002274 Matrix Metalloproteinases Human genes 0.000 description 1
- 108010000684 Matrix Metalloproteinases Proteins 0.000 description 1
- 108700009949 PTP protocol Proteins 0.000 description 1
- NIXOWILDQLNWCW-UHFFFAOYSA-N acrylic acid group Chemical group C(C=C)(=O)O NIXOWILDQLNWCW-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000005284 excitation Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 230000004083 survival effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/20—Integrity monitoring, fault detection or fault isolation of space segment
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/23—Testing, monitoring, correcting or calibrating of receiver elements
- G01S19/235—Calibration of receiver components
Abstract
本发明公开了一种基于点特征的LIDAR与GPS/IMU联合标定方法，首先确定参数初值，在此基础上不断优化得到最优旋转参数，最后根据最优旋转参数计算平移参数，通过与阈值进行比较确定是否对平移参数进行优化，平移参数的标定可以补偿最优旋转参数的偏差，使得最终效果接近真实值。本发明的方法能够在线实时标定，在完成预设轨迹后快速解算出外参；且不依赖人工设计的特定标定场，只需要一定范围的平坦区域和标定板就可以解决3D运动估计6D外部参数的退化问题。
Description
技术领域
本发明属于矿区传感器标定技术领域，尤其涉及一种适用于矿区弱约束条件的基于点特征的LIDAR与GPS/IMU联合标定方法。
背景技术
建设智慧矿山，变革生产方式已成为矿山行业和企业生存、发展的重大战略需要。无人矿车的发展是智慧矿山建设的必然趋势。面对复杂多变的行驶环境，无人矿车需要融合多种异构传感器去感知交通参与者的速度、种类、位置等信息。多传感器的时空对准分为时间同步和空间同步，是实现无人矿车数据融合及高精度制图的保障。时间同步需要在同一时刻采集各个传感器的数据。空间同步需要计算单个传感器的内在参数和多个传感器之间的外在参数，从而统一到车辆坐标系。
现有空间同步即传感器标定方法大致可分为四类：标定物辅助标定(TargetAssisted Method，TAM)，点云优化标定(Point cloud Optimization Methods，POM)，手眼标定(Hand-Eye Methods，HEM)和概率标定(Probabilistic Calibration Methods，PCM)。
摄像头内参标定是最典型的标定物辅助标定(TAM)，需要在不同位置拍摄标准棋盘格，通过检测角点进行内参标定。TAM依赖人为设置特定形状或特征的标定物进行标定，受标定物的影响较大，虽然前期需要设置标定物，但是标定精度更高。
点云优化标定(POM)主要根据周围环境的平面、角点等特征，利用不同的特征提取算法和点云匹配算法进行计算，最后优化参数使误差最小。常用的3D点云匹配方法有迭代最近点(Iterative Closest Point，ICP)、正态分布变换(Normal DistributionsTransform，NDT)等算法。在每次迭代过程中，POM都要计算点云的特征，较为繁琐，复杂度高。另外，POM方法对于室内机器人的标定是有效的，室内障碍物距离近，点云密集且误差较小，而且可以使用室内转角平面或垂直墙壁等强约束条件进行标定。然而，由于户外无人车与周围建筑的约束较弱，容易因为约束不足导致标定失败。
手眼标定(HEM)通常分为eye-to-hand和eye-in-hand，在机器人领域被广泛应用。无人车传感器标定是典型的eye-in-hand方法，手眼标定法的核心是解方程AX＝XB。在LIDAR与GPS/IMU标定中，LIDAR里程计通常是根据已有地图和当前LIDAR扫描帧进行重定位，例如KITTI排行榜的LOAM算法。然而，手眼标定结果精度取决于IMU位姿变换精度和LIDAR里程计精度。因此必须采用高精度的IMU，而LIDAR里程计要想获得较高的精度，需要标定场景含有较为丰富的特征，对标定场要求较高。
概率标定(PCM)考虑了传感器的噪声引起的不确定性，从而提高标定的精度。根据有无人为设置的特定标定物，上述方法还可以分为两大类：有标定物(Targeted Methods，TM)和无标定物(Targetless Methods，TLM)。TM为了提高精度，需要提供具有明显特征的标定物。TLM虽然不需要标定物，但是依赖LIDAR点云提取的环境特征，特征不足会严重影响标定精度。从LIDAR提取的特征来看，TM和TLM本质上是相同的，都依赖外部环境。为了使标定结果更加可靠，大部分方法都要求车辆进行6个自由度的运动。在数据采集时，需要车辆进行大幅度的偏航、俯仰和侧倾动作。例如百度开源无人驾驶平台Apollo利用“∞”形轨迹进行标定，驾驶时间需要超过2分钟，以确保IMU有可用的数据进行校准。矿区环境单一且道路危险，难以构建特征丰富的标定场。而且矿车体积庞大，车辆负载大，难以进行较大幅度的俯仰，侧倾运动。
综上，目前流行的标定方法只适用于具有强约束的室内环境，或对室外标定场景和特定标定物的要求较高，亦或对车辆本身的运动轨迹和姿态有强依赖，均不能在简单且缺少丰富特征的矿区环境下有效地输出标定结果。
发明内容
为了解决上述已有技术存在的不足，本发明提出一种适用于矿区弱约束条件的基于点特征的LIDAR与GPS/IMU联合标定方法，仅需空旷平面和竖直标定板即可完成，通过实时提取标定板点云并计算其中心坐标，利用点特征进行外参标定，继而对参数进行优化得到最终结果。本发明的具体技术方案如下：
一种基于点特征的LIDAR与GPS/IMU联合标定方法，车辆停放在地面，标定板与地面垂直，所述方法包括以下步骤：
S1：构建标定板从LIDAR坐标系到UTM坐标系的转换关系，计算出标定板的UTM位置，根据WGS-84坐标系与UTM坐标系的转换关系将车辆GPS定位坐标转换为UTM坐标；
S2：设IMU的安装平面与车辆停靠的平面平行，提取车辆前方水平地面的LIDAR点云并根据其法向量计算LIDAR俯仰角和滚转角的初值，利用标定板中心的偏移计算偏航角的初值；
S3：设车辆在标定场地平面上保持直线运动，不具备俯仰、滚转、偏航角度的变化，利用恒定姿态标定方法将旋转角度的求解转化为最优化问题，优化旋转参数；
S4：设多帧数据之间标定板UTM位置不变，车辆在标定场地平面上进行大角度平面圆周运动，根据标定板在世界坐标系位置唯一的约束利用圆周运动求解出平移参数；
S5：将步骤S4求解的平移参数与设定的阈值比较，若满足要求则执行步骤S6，否则持续执行步骤S3和步骤S4，直至得到平移参数的最优解；
S6：结合步骤S3和步骤S5的结果实现LIDAR与GPS/IMU外参的标定。
进一步地，所述步骤S1的具体过程为：
将GPS接收机输出的WGS-84坐标转换到UTM坐标系，车辆前方目标物的UTM坐标为：
其中，Uobject是目标物在UTM坐标系的唯一坐标；为i时刻从IMU坐标系到UTM坐标系的旋转矩阵；/>是从LIDAR坐标系到IMU坐标系的旋转矩阵；Pi为点云的三维坐标；/>是从LIDAR坐标系到IMU坐标系的平移向量；Ui为第i时刻GPS定位中心在UTM坐标系的坐标，转换公式为：
其中，为第i时刻WGS-84坐标系到UTM坐标系的转换矩阵，Gi为第i时刻GPS定位中心；
同一目标在UTM坐标系只有唯一坐标，因此，LIDAR在不同位置探测到的静止标定物在UTM坐标系下位置相同，等式关系为：
其中，等式左边为第i时刻目标物LIDAR点云在UTM坐标系下的坐标；等式右边为第i+1时刻目标物LIDAR点云在UTM坐标系下的坐标。
进一步地，所述步骤S2计算俯仰角、滚转角、偏航角的初值，具体过程为：
首先，假设IMU安装的平面与车辆所在的地面平行，根据LIDAR探测到的地面点云的法向量计算β0和γ0，α0角通过式(4)计算，通过式(5)求解的初值：
α0＝atan2(Dy,Dx) (4)
其中，Dx和Dy分别为标定板中心到LIDAR的横向距离和纵向距离，为WGS-84坐标系到IMU坐标系的旋转矩阵，/>为LIDAR坐标系到WGS-84坐标系的旋转矩阵，能够根据α0、β0、γ0求解；
设IMU安装平面与地面平行，故为单位矩阵，/>的计算转化为/>的求解，在三维空间中，旋转矩阵REuler通过三个欧拉角偏航角α、滚转角β和俯仰角γ根据式(6)计算，同理LIDAR到WGS-84坐标系的旋转矩阵/>也由欧拉角初值α0,β0γ0得到；
REuler＝RγRβRα (6)
其中，
进一步地，所述步骤S3的利用恒定姿态标定方法优化旋转参数的过程为：
车辆在平坦路面上保持匀速直线运动，避免俯仰和侧倾，当的值为常量，即车辆的姿态恒定，则式(3)简化为：
式(7)描述的是和/>的旋转关系；
由于外界因素，实际的数值一直处于波动状态，式(7)不严格相等，因此构造最小误差函数χ：
通过最优化方法求解旋转参数的最优值。
进一步地，所述步骤S4的计算平移参数的方法为：
S4-1：将公式(3)移项计算得到的公式(9)：
S4-2：将步骤S3求解的最优旋转参数代入公式(9)。
进一步地，所述步骤S5的具体过程为：
S5-1：将计算得到的平移向量结合旋转矩阵/>得到LIDAR和GPS/IMU的相对位置关系；
S5-2：判断平移向量的参数、旋转矩阵/>的参数是否满足设定阈值，如果满足，则继续执行步骤S6；如果超出阈值，则返回步骤S3迭代优化旋转参数，求解新的平移参数，直至旋转参数和平移参数均满足设定阈值。
本发明的有益效果在于：
1.本发明针对现有方法的局限性，基于标定板UTM坐标唯一的特点，提出了一种标定方法，能够在线实时标定，在完成预设轨迹后快速解算出外参；
2.本发明的方法对车辆运动自由度要求较低，不需要俯仰、侧倾和垂直方向的运动，不依赖人工设计的特定标定场，只需要一定范围的平坦区域和标定板就可以解决3D运动估计6D外部参数的退化问题；
3.本发明的方法不依赖具有强约束的标定场，适用于户外无人车标定尤其是特征稀少的空旷区域，配合以固定位置的标定板即可完成标定过程；
4.可以实时在线标定且耗时较短，在操作简便性和标定精度上达到了较优的匹配，车辆在保证能够探测到标定板的区域范围内完成直线和圆周轨迹后可快速解算出外参。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，可以根据这些附图获得其他的附图。其中：
图1是本发明的输入和输出流程图；
图2是本发明的标定方法流程图；
图3是本发明的坐标系转换示意图；
图4是本发明的标定原理图；
图5是本发明的3D车辆运动姿态示意图；
图6是本发明的2D圆周运动示意图；
图7是本发明的偏航角误差；
图8是本发明的LIDAR探测模型；
图9为中心点匹配误差；
图10为标定场景示意图；
图11为时间同步策略；
图12是数据采集的实验车辆；
图13为标定场地；
图14为LIDAR坐标系转UTM坐标系，其中，(a)为A车三个坐标轴的误差，(b)为B车的误差；
图15为采用手持GPS测量标定板中心的UTM坐标，其中，(a)为标定板中心在UTM坐标系X轴方向的误差；(b)为UTM坐标系Y轴方向的误差；(c)为高度误差；
图16为转弯行驶主路径轨迹，其中，(a)为车辆在像素坐标系下的行驶主路径，(b)为车辆在激光雷达坐标系下的行驶主路径；
图17为直线行驶主路径轨迹，其中，(a)为车辆在像素坐标系系下的行驶主路径，(b)为车辆在激光雷达坐标系下的行驶主路径。
具体实施方式
为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本发明的实施例及实施例中的特征可以相互组合。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。
如图1-2所示，图1是本发明的输入和输出流程图，明确了本发明方法的输入是车辆做恒定姿态直线运动和水平圆周运动时所记录的原始激光雷达点云和GPS/IMU数据；图2是本发明的标定方法流程图，表明在保证输入数据正确的基础上通过最小误差函数进行迭代优化并与设定阈值进行比较得到最终的旋转矩阵和平移向量。
具体地，一种基于点特征的LIDAR与GPS/IMU联合标定方法，车辆停放在地面，标定板与地面垂直，方法包括以下步骤：
S1：构建标定板从LIDAR坐标系到UTM坐标系的转换关系，计算出标定板的UTM位置，根据WGS-84坐标系与UTM坐标系的转换关系将车辆GPS定位坐标转换为UTM坐标；
S2：设IMU的安装平面与车辆停靠的平面平行，提取车辆前方水平地面的LIDAR点云并根据其法向量计算LIDAR俯仰角和滚转角的初值，利用标定板中心的偏移计算偏航角的初值；
S3：设车辆在标定场地平面上保持直线运动，不具备俯仰、滚转、偏航角度的变化，利用恒定姿态标定方法将旋转角度的求解转化为最优化问题，对旋转参数进行优化迭代；
S4：设多帧数据之间标定板UTM位置不变，车辆在标定场地平面上进行大角度平面圆周运动，根据标定板在世界坐标系位置唯一的约束利用圆周运动求解出平移参数；
S5：将步骤S4求解的平移参数与设定的阈值比较，若满足要求则执行步骤S6，否则持续执行步骤S3和步骤S4，直至得到平移参数的最优解；
S6：结合步骤S3和步骤S5的结果实现LIDAR与GPS/IMU外参的标定。
标定板采用亚克力材质，规格为1.8m(长)x1.5m(高)的矩形板，配备安装支架，保证与地面垂直，上边沿距离地面高度约为2m，标定板尺寸不做特定要求，与本发明所使用标定板规格相近，能够保证表面平整且与地面垂直即可。
旋转参数和平移参数标定的伪代码如表1所示；
表1标定方法伪代码
如图3所示，首先建立激光雷达坐标系和车体坐标系的关系，其次将激光雷达坐标系转换到IMU坐标系，再将IMU坐标系转换到WGS84坐标系，最后将WGS84坐标系转换到UTM坐标系，由此建立障碍物(目标物)到UTM坐标系的转换关系；步骤S1的具体过程为：
将GPS接收机输出的WGS-84坐标转换到UTM坐标系，车辆前方目标物的UTM坐标为：
其中，Uobject是目标物在UTM坐标系的唯一坐标；为i时刻从IMU坐标系到UTM坐标系的旋转矩阵；/>是从LIDAR坐标系到IMU坐标系的旋转矩阵；Pi为点云的三维坐标；/>是从LIDAR坐标系到IMU坐标系的平移向量；Ui为第i时刻GPS定位中心在UTM坐标系的坐标，转换公式为：
其中，为第i时刻WGS-84坐标系到UTM坐标系的转换矩阵，Gi为第i时刻GPS定位中心；
同一目标在UTM坐标系只有唯一坐标，因此，LIDAR在不同位置探测到的静止标定物在UTM坐标系下位置相同，等式关系为：
其中，等式左边为第i时刻目标物LIDAR点云在UTM坐标系下的坐标；等式右边为第i+1时刻目标物LIDAR点云在UTM坐标系下的坐标。显然，等式(3)需要6个自由度的运动才能完成标定，否则会因为某个自由度激励不足导致结果退化，引起该自由度上标定精度偏差。
图4是本发明的标定原理图，本发明提出的恒定姿态标定方法标定旋转矩阵车辆在平坦路面上运动的过程中，尽可能保持匀速直线运动，避免俯仰和侧倾；步骤S2利用初值测量方法计算偏航角的初值，具体过程为：
首先，假设IMU安装的平面与车辆所在的地面平行，根据LIDAR探测到的地面点云的法向量计算β0和γ0，α0角通过式(4)计算，通过式(5)求解的初值：
α0＝atan2(Dy,Dx) (4)
其中，Dx和Dy分别为标定板中心到LIDAR的横向距离和纵向距离，为地面坐标系到IMU坐标系的旋转矩阵，/>为LIDAR坐标系到地面坐标系的旋转矩阵，能够根据α0、β0、γ0求解；
设IMU安装平面与地面平行，故为单位矩阵，/>的计算转化为/>的求解，在三维空间中，旋转矩阵REuler通过三个欧拉角偏航角α、滚转角β和俯仰角γ根据式(6)计算，同理LIDAR到地面坐标系的旋转矩阵/>也由欧拉角初值α0,β0γ0得到；
REuler＝RγRβRα (6)
其中，
在一些实施方式中，步骤S3的利用恒定姿态标定方法优化旋转参数的过程为：
车辆在平坦路面上保持匀速直线运动，避免俯仰和侧倾，当的值为常量，即车辆的姿态恒定，则式(3)简化为：
式(7)描述的是和/>的旋转关系；
由于外界因素，实际的数值一直处于波动状态，式(7)不严格相等，因此构造最小误差函数χ：
通过最优化方法求解旋转参数的最优值。
根据罗德里格斯公式求解出RRodrigues：
RRodrigues＝cosθI+(1-cosθ)nnT+sinθn^ (9)
其中，I为单位矩阵，符号^是向量到反对称的转换符，由于旋转轴上的向量在旋转后不发生改变，故旋转轴n是矩阵RRodrigues特征值1对应的特征向量，向量夹角θ，旋转轴n由下列公式计算：
旋转向量nrot定义为方向与旋转轴n一致，而长度等于旋转角的向量，罗德里格斯公式完成由旋转向量nrot到旋转矩阵RRodrigues的过程。但是，罗德里格斯公式绕旋转轴n旋转一定的θ角，而本发明涉及到的旋转矩阵都是由欧拉角计算的矩阵，两种矩阵虽然旋转效果一致，但是矩阵完全不同，如不等式12所示。另外，LIDAR-IMU旋转角度(俯仰角、滚转角、偏航角)的初值通过测量得到，但是测量精度不高且操作复杂。本发明依赖于步骤S2提出的旋转参初值测量方法，减小了旋转矩阵求解过程中的误差。
在一些实施方式中，步骤S4的计算平移参数的方法为：
S4-1：将公式(3)移项计算得到的公式(13)：
S4-2：将步骤S3求解的最优旋转参数代入公式(13)。
在求解平移参数时，为了提高标定参数的精度，车辆需要在x、y、z方向上有三个自由度的位移Tx，Ty和Tz；大转角和小位移运动可以提高标定的精度，实际路面Z方向的位移较小，而且在安装LIDAR和GPS/IMU时，坐标系的Z轴应平行或接近平行；同时，点云地图对高度信息不敏感，因此可以直接测量两个传感器之间的高度差Tz。
在一些实施方式中，步骤S5的具体过程为：
S5-1：将计算得到的平移向量结合旋转矩阵/>得到LIDAR和GPS/IMU的相对位置关系；
S5-2：判断平移向量的参数、旋转矩阵/>的参数是否满足设定阈值，如果满足，则继续执行步骤S6；如果超出阈值，则返回步骤S3迭代优化旋转参数，求解新的平移参数，直至旋转参数和平移参数均满足设定阈值。
图5是本发明的3D车辆运动姿态示意图，车辆在平坦路面上运动的过程中，尽可能保持匀速直线运动，避免俯仰和侧倾，即车辆的姿态恒定。车辆i时刻所处虚线为第i时刻理想车辆的运动轨迹；车辆i+1时刻所处虚线为第i+1时刻理想的车辆运动轨迹；由于地面与轮胎的相互作用等因素，使车辆的运动轨迹产生偏移δx,δy,δz；Yaw，Pitch，Roll为车辆在i+1时刻相对第i时刻的偏航角，俯仰角和滚转角；r为旋转半径。由于车辆在平坦路面上匀速行驶，俯仰角Pitch和滚转角Roll几乎不变，偏航角Yaw会左右波动，因此可以近似成2D圆周运动。
图6是本发明的2D圆周运动示意图，由于车辆在平坦路面上匀速行驶，俯仰角Pitch和滚转角Roll几乎不变，偏航角Yaw会左右波动，因此可以近似成2D圆周运动，偏航角Yaw可根据不同时刻车辆位置与圆心的夹角表示。
图7是本发明的偏航角误差，说明Yaw误差与运动半径的关系，在不同的运动速度下，Yaw误差随着运动半径增大而不断减小；同一半径下，速度越高，误差越大。因此，在标定旋转参数时尽可能保持车辆直线低速行驶。经计算，当r＝500m，v＝2m/s，t＝0.1s时，误差为0.0229°。
图8是本发明的LIDAR探测模型，θV表示LIDAR垂直角分辨率，θH为水平角分辨率。在靠近或者远离标定物的过程中，LIDAR发射的光线并不能够探测在同一个位置，而且距离越近，探测越密集，距离越远，探测越稀疏。
图9是中心点匹配误差，图中浅色星号为实际标定板中心。图(a)中，第i帧标定板的左上角被检测到，边缘深色点点是恰好在标定板边缘但未扫描到标定板的LIDAR点，此时计算出来的中心点(深色星号)会向左上角偏移。同理，图(b)中，第i+1帧标定板的右下角被测到，此时计算出的中心点(浅色星号)向右下角偏移。
图10是标定场景示意图，将车辆停靠在平坦地面(斜面/平面)，在LIDAR正前方放置一块标定板，通过地面点云的法向量计算LIDAR相对于地面的俯仰角和滚转角；利用标定板的中心计算偏航角。车辆沿靠近标定板的方向作直线运动，在此过程中求解出最优旋转参数；在适当位置转弯，标定出平移参数Tx和Ty。如图所示，同心圆虚线区域为旋转参数初值的计算区域，行驶轨迹中的直线段为旋转参数优化时的行驶路径，曲线段为计算平移参数时的行驶轨迹。
图11是时间同步策略，首先处理器对GPRMC报文进行解析，将卫星时间与处理器本地时间进行同步，并将处理器作为主时钟，通过PTP协议对LIDAR进行授时，IMU则在组合导航内部处理，保持与GPS相同时间源。同时GPS采用自带的PPS秒脉冲对LIDAR和IMU时间进行周期性校对，消除时钟同步过程中的累计误差。在对系统中的各个传感器节点进行时间同步之后，以LIDAR当前帧开始扫描的时刻为基准，通过软触发的方式保存GPS/IMU数据，此方法可保证多传感器的数据采集频率均为10Hz,并保证异构传感器的数据帧一一对应，帧间同步误差在30ms左右，在标定场景下满足精度需求。
为了方便理解本发明的上述技术方案，以下通过具体实施例对本发明的上述技术方案进行详细说明。
实施例1
采用Ouster的64线激光雷达和上海华测导航技术股份有限公司的GPS/IMU设备采集标定数据。图12是数据采集的实验车辆，采用双GPS天线计算车辆航向角，载波相位差分(RTK)信号进行车辆定位，根据时间同步机制将传感器数据保存至本地，标定场地如图13所示。
采用C++编程语言进行算法开发。首先利用初始旋转参数进行点云粗校正，根据高度阈值滤除地面等干扰数据，筛选出标定板点云；然后将目标点云根据初值旋转参数逆变换到原始状态，利用RANSAC提取标定板平面，滤除支撑杆点云，计算所提取平面的中心坐标。根据WGS-84与UTM转换规则将车辆定位的GPS数据转换到UTM坐标系下。为了减小点云的畸变，在所设计的场景下采集数据时，车速最高为10km/h。
1.LIDAR坐标系转UTM坐标系
实验1：采用实验车A和实验车B进行标定，计算随机帧的标定板中心坐标与其均值的误差。如图14所示，图a为A车三个坐标轴的误差，图b为B车的误差。可以看出，同一辆车在不同位置计算的标定板中心误差小于10cm。表2分别采用A车，B车计算标定板中心坐标，不同车辆计算的标定板中心误差在20cm左右。
表2标定板UTM坐标
实验2：采用手持GPS测量标定板中心的UTM坐标。如图15所示，图(a)为标定板中心在UTM坐标系X轴方向的误差；图(b)为UTM坐标系Y轴方向的误差；图(c)为高度误差。由于手持GPS的测量误差较大，从图中可以看出A车和B车计算出的标定板中心与手持GPS定位的标定板中心有固定的偏差，但是偏差保持一致，进一步验证本发明方法的精度。
2.UTM坐标系转LIDAR坐标系
实验1：利用地图车B录制主路径GPS行驶轨迹。实验车辆A在主路径上进行循迹行驶，根据自车的定位数据，利用标定的旋转参数和平移参数将主路径GPS轨迹转换到A车的LIDAR坐标系下，利用摄像头与LIDAR的内外参，将主路径GPS轨迹转换到像素坐标系下。图16为转弯行驶时，车辆在像素坐标系和激光雷达坐标系下的行驶主路径，图17为直线行驶时，车辆在像素坐标系和激光雷达坐标系下的行驶主路径。从图中可以看出，实验车的行驶轨迹与主路径GPS轨迹相同。
本发明使用的LIDAR角分辨率为0.35°，0.1758°，的最大误差0.683％。表3对比了KITTI[20]排行榜上不同的(Lidar Odometry,LO)算法的精度。传统的手眼标定方法需要提取特征点并对点云进行配准等方法计算LIDAR里程计，对环境的特征有较强的依赖。综上，CPAM与现在最先进的LO方法精度相当，而且速度更快，操作更简便。
表3LIDAR里程计效果对比
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (6)
1.一种基于点特征的LIDAR与GPS/IMU联合标定方法，其特征在于，车辆停放在地面，标定板与地面垂直，所述方法包括以下步骤：
S1：构建标定板从LIDAR坐标系到UTM坐标系的转换关系，计算出标定板的UTM位置，根据WGS-84坐标系与UTM坐标系的转换关系将车辆GPS定位坐标转换为UTM坐标；
S2：设IMU的安装平面与车辆停靠的平面平行，提取车辆前方水平地面的LIDAR点云并根据其法向量计算LIDAR俯仰角和滚转角的初值，利用标定板中心的偏移计算偏航角的初值；
S3：设车辆在标定场地平面上保持直线运动，不具备俯仰、滚转、偏航角度的变化，利用恒定姿态标定方法将旋转角度的求解转化为最优化问题，优化旋转参数；
S4：设多帧数据之间标定板UTM位置不变，车辆在标定场地平面上进行大角度平面圆周运动，根据标定板在世界坐标系位置唯一的约束利用圆周运动求解出平移参数；
S5：将步骤S4求解的平移参数与设定的阈值比较，若满足要求则执行步骤S6，否则持续执行步骤S3和步骤S4，直至得到平移参数的最优解；
S6：结合步骤S3和步骤S5的结果实现LIDAR与GPS/IMU外参的标定。
2.根据权利要求1所述的一种基于点特征的LIDAR与GPS/IMU联合标定方法，其特征在于，所述步骤S1的具体过程为：
将GPS接收机输出的WGS-84坐标转换到UTM坐标系，车辆前方目标物的UTM坐标为：
其中，Uobject是目标物在UTM坐标系的唯一坐标；为i时刻从IMU坐标系到UTM坐标系的旋转矩阵；/>是从LIDAR坐标系到IMU坐标系的旋转矩阵；Pi为点云的三维坐标；/>是从LIDAR坐标系到IMU坐标系的平移向量；Ui为第i时刻GPS定位中心在UTM坐标系的坐标，转换公式为：
其中，为第i时刻WGS-84坐标系到UTM坐标系的转换矩阵，Gi为第i时刻GPS定位中心；
同一目标在UTM坐标系只有唯一坐标，因此，LIDAR在不同位置探测到的静止标定物在UTM坐标系下位置相同，等式关系为：
其中，等式左边为第i时刻目标物LIDAR点云在UTM坐标系下的坐标；等式右边为第i+1时刻目标物LIDAR点云在UTM坐标系下的坐标。
3.根据权利要求2所述的一种基于点特征的LIDAR与GPS/IMU联合标定方法，其特征在于，所述步骤S2计算俯仰角、滚转角、偏航角的初值，具体过程为：
首先，假设IMU安装的平面与车辆所在的地面平行，根据LIDAR探测到的地面点云的法向量计算β0和γ0，α0角通过式(4)计算，通过式(5)求解的初值：
α0＝atan2(Dy,Dx) (4)
其中，Dx和Dy分别为标定板中心到LIDAR的横向距离和纵向距离，为WGS-84坐标系到IMU坐标系的旋转矩阵，/>为LIDAR坐标系到WGS-84坐标系的旋转矩阵，能够根据α0、β0、γ0求解；
设IMU安装平面与地面平行，故为单位矩阵，/>的计算转化为/>的求解，在三维空间中，旋转矩阵REuler通过三个欧拉角偏航角α、滚转角β和俯仰角γ根据式(6)计算，同理LIDAR到WGS-84坐标系的旋转矩阵/>也由欧拉角初值α0,β0γ0得到；
REuler＝RγRβRα (6)
其中，
4.根据权利要求2所述的一种基于点特征的LIDAR与GPS/IMU联合标定方法，其特征在于，所述步骤S3的利用恒定姿态标定方法优化旋转参数的过程为：
车辆在平坦路面上保持匀速直线运动，避免俯仰和侧倾，当的值为常量，即车辆的姿态恒定，则式(3)简化为：
式(7)描述的是和/>的旋转关系；
由于外界因素，实际的数值一直处于波动状态，式(7)不严格相等，因此构造最小误差函数χ：
通过最优化方法求解旋转参数的最优值。
5.根据权利要求2所述的一种基于点特征的LIDAR与GPS/IMU联合标定方法，其特征在于，所述步骤S4的计算平移参数的方法为：
S4-1：将公式(3)移项计算得到的公式(9)：
S4-2：将步骤S3求解的最优旋转参数代入公式(9)。
6.根据权利要求5所述的一种基于点特征的LIDAR与GPS/IMU联合标定方法，其特征在于，所述步骤S5的具体过程为：
S5-1：将计算得到的平移向量结合旋转矩阵/>得到LIDAR和GPS/IMU的相对位置关系；
S5-2：判断平移向量的参数、旋转矩阵/>的参数是否满足设定阈值，如果满足，则继续执行步骤S6；如果超出阈值，则返回步骤S3迭代优化旋转参数，求解新的平移参数，直至旋转参数和平移参数均满足设定阈值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110947142.9A CN113777589B (zh) | 2021-08-18 | 2021-08-18 | 一种基于点特征的lidar与gps/imu联合标定方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110947142.9A CN113777589B (zh) | 2021-08-18 | 2021-08-18 | 一种基于点特征的lidar与gps/imu联合标定方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113777589A CN113777589A (zh) | 2021-12-10 |
| CN113777589B true CN113777589B (zh) | 2024-04-02 |
Family
ID=78838041
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110947142.9A Active CN113777589B (zh) | 2021-08-18 | 2021-08-18 | 一种基于点特征的lidar与gps/imu联合标定方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113777589B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114463507B (zh) * | 2022-04-11 | 2022-06-14 | 国家电投集团科学技术研究院有限公司 | 道路识别方法及装置 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4489322A (en) * | 1983-01-27 | 1984-12-18 | The United States Of America As Represented By The Secretary Of The Air Force | Radar calibration using direct measurement equipment and oblique photometry |
| CN108732582A (zh) * | 2017-04-20 | 2018-11-02 | 百度在线网络技术（北京）有限公司 | 车辆定位方法和装置 |
| CN110146869A (zh) * | 2019-05-21 | 2019-08-20 | 北京百度网讯科技有限公司 | 确定坐标系转换参数的方法、装置、电子设备和存储介质 |
| CN111060132A (zh) * | 2019-11-29 | 2020-04-24 | 苏州智加科技有限公司 | 行车定位坐标的标定方法和装置 |
| CN111398980A (zh) * | 2018-12-29 | 2020-07-10 | 广东瑞图万方科技股份有限公司 | 一种机载LiDAR数据处理的方法及装置 |
| CN112904317A (zh) * | 2021-01-21 | 2021-06-04 | 湖南阿波罗智行科技有限公司 | 一种多激光雷达与gnss_ins系统标定方法 |
-
2021
- 2021-08-18 CN CN202110947142.9A patent/CN113777589B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4489322A (en) * | 1983-01-27 | 1984-12-18 | The United States Of America As Represented By The Secretary Of The Air Force | Radar calibration using direct measurement equipment and oblique photometry |
| CN108732582A (zh) * | 2017-04-20 | 2018-11-02 | 百度在线网络技术（北京）有限公司 | 车辆定位方法和装置 |
| CN111398980A (zh) * | 2018-12-29 | 2020-07-10 | 广东瑞图万方科技股份有限公司 | 一种机载LiDAR数据处理的方法及装置 |
| CN110146869A (zh) * | 2019-05-21 | 2019-08-20 | 北京百度网讯科技有限公司 | 确定坐标系转换参数的方法、装置、电子设备和存储介质 |
| CN111060132A (zh) * | 2019-11-29 | 2020-04-24 | 苏州智加科技有限公司 | 行车定位坐标的标定方法和装置 |
| CN112904317A (zh) * | 2021-01-21 | 2021-06-04 | 湖南阿波罗智行科技有限公司 | 一种多激光雷达与gnss_ins系统标定方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113777589A (zh) | 2021-12-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111272165B (zh) | 一种基于特征点标定的智能车定位方法 | |
| CN110411462B (zh) | 一种gnss/惯导/车道线约束/里程计多源融合方法 | |
| CN111862672B (zh) | 基于顶视图的停车场车辆自定位及地图构建方法 | |
| CN111551958B (zh) | 一种面向矿区无人驾驶的高精地图制作方法 | |
| CN106840179B (zh) | 一种基于多传感器信息融合的智能车定位方法 | |
| CN111142091B (zh) | 一种融合车载信息的自动驾驶系统激光雷达在线标定方法 | |
| CN113945206A (zh) | 一种基于多传感器融合的定位方法及装置 | |
| JP6301828B2 (ja) | 誘導用線路に沿って移動する車両の速度及び位置を測定する装置、並びに対応する方法及びコンピュータプログラム製品 | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| CN113819914A (zh) | 一种地图构建方法及装置 | |
| CN114018248B (zh) | 一种融合码盘和激光雷达的里程计方法与建图方法 | |
| CN110766760B (zh) | 用于相机标定的方法、装置、设备和存储介质 | |
| CN110766761B (zh) | 用于相机标定的方法、装置、设备和存储介质 | |
| CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN113819905A (zh) | 一种基于多传感器融合的里程计方法及装置 | |
| US20220035036A1 (en) | Method and apparatus for positioning movable device, and movable device | |
| CN113947639A (zh) | 基于多雷达点云线特征的自适应在线估计标定系统及方法 | |
| CN113920198B (zh) | 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法 | |
| CN110412596A (zh) | 一种基于图像信息和激光点云的机器人定位方法 | |
| CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
| CN115407357A (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
| CN113777589B (zh) | 一种基于点特征的lidar与gps/imu联合标定方法 | |
| Xiao et al. | LIO-vehicle: A tightly-coupled vehicle dynamics extension of LiDAR inertial odometry | |
| CN111829514B (zh) | 一种适用于车辆底盘集成控制的路面工况预瞄方法 | |
| CN117234203A (zh) | 一种多源里程融合slam井下导航方法 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |