一种基于激光雷达和IMU的多传感器融合定位方法

一种基于激光雷达和imu的多传感器融合定位方法
技术领域
1.本发明涉及定位导航技术领域，具体而言，涉及一种基于激光雷达和imu的多传感器融合定位方法。


背景技术：

2.定位技术是移动机器人的关键技术之一，它可以为移动机器人其他技术包括slam技术、导航技术等提供先验位姿信息。在单一传感器定位方法中，基于激光雷达的激光定位输出频率低且在特定环境下易退化，而基于imu的惯性定位的误差随时间发散，因此通过单一传感器定位方法无法满足工作生产要求。
3.目前基于激光雷达和imu的融合定位方法可以根据是否联合优化原始测量数据分为紧耦合和松耦合两种方法。文献(zhao s,fang z,li h l,et al.a robust laser-inertial odometry and mapping method for large-scale highway environments[c]//2019 ieee/rsj international conference on intelligent robots and systems(iros).ieee,2019:1285-1292.)采用误差状态卡尔曼滤波器对激光雷达和imu进行松耦合定位，这种方法虽然计算简单，但是在定位精度和鲁棒性方面不如紧耦合定位。松耦合定位存在精度不足、可靠性较差的缺陷。


技术实现要素：

[0004]
本发明解决的问题是如何有效提高移动机器人的定位精度和可靠性。
[0005]
为解决上述问题，本发明提供一种基于激光雷达和imu的多传感器融合定位方法，包括步骤：
[0006]
s1：通过激光雷达和imu分别获取激光数据和惯性数据，并在激光雷达和imu之间进行空间同步；
[0007]
s2：利用imu的预积分模型对惯性数据进行预积分处理，获取紧耦合定位系统的名义状态预测量；
[0008]
s3：通过紧耦合定位系统的名义状态预测量对激光数据进行运动畸变校正，得到校正后的激光数据；
[0009]
s4：根据imu的测量模型和imu的误差运动模型建立紧耦合定位系统的误差状态预测方程；
[0010]
s5：根据激光配准误差模型建立紧耦合定位系统的误差状态更新方程；
[0011]
s6：基于校正后的激光数据获取激光误差状态量，并获取相应时间内的imu误差状态量；
[0012]
s7：将激光误差状态量和imu误差状态量代入紧耦合定位系统进行迭代优化，获取误差状态最优估计量；
[0013]
s8：通过误差状态最优估计量对紧耦合定位系统的名义状态预测量进行校正，达到紧耦合定位。
[0014]
在上述方法中，获取惯性数据时需要考虑重力对惯性数据的影响，根据imu姿态消除惯性数据中受重力影响的部分，并获取优化后的惯性数据。采用迭代误差状态卡尔曼滤波器作为紧耦合定位系统，利用imu预积分模型对惯性数据进行预积分处理，获取系统的名义状态预测量。
[0015]
在上述方法中，通过系统的名义状态预测量对激光数据进行运动畸变校正，获取校正后的激光数据；根据imu测量模型和imu误差运动模型建立系统的误差状态预测方程。根据激光配准误差模型建立系统的误差状态更新方程。通过前后两帧校正后的激光数据得到激光误差状态量，并获取其相应时间内的imu误差状态量。将两种误差状态量代入系统进行迭代优化，得到误差状态最优估计量；通过误差状态最优估计量对系统的名义状态预测量进行校正，从而实现紧耦合定位。
[0016]
进一步地，所述步骤s1中的空间同步包括：
[0017]
s11：对激光雷达和imu的相对位姿参数进行标定。
[0018]
进一步地，所述步骤s2中的紧耦合定位系统采用迭代误差状态卡尔曼滤波器；
[0019]
所述滤波器的状态量之间关系为：
[0020][0021]
其中，x
t
表示滤波器的真实状态量，x表示滤波器的名义状态量，δx表示滤波器的误差状态量，表示标量与矢量的加减运算。
[0022]
进一步地，所述步骤s2中imu的预积分模型为：
[0023][0024][0025][0026]
其中，表示imu从i时刻到j时刻的位姿预测量，表示imu从i时刻到j时刻的速度预测量，表示imu从i时刻到j时刻的四元数姿态预测量；
[0027]
所述名义状态预测量为：
[0028]
x＝[p v q b
a b
g g]
τ
；
[0029]
其中，p表示名义位移量，v表示名义速度量，q表示名义四元数姿态量，ba表示名义加速度计随机游走量，bg表示名义陀螺仪随机游走量，g表示名义重力矢量。
[0030]
进一步地，所述步骤s3包括：
[0031]
s31：计算获取得到的一帧激光数据内每个激光点的时间戳；
[0032]
s32：基于激光点的时间戳，根据紧耦合定位系统的名义状态预测量找到对应时间戳的位姿先验值；
[0033]
s33：根据对应时间戳的位姿先验值，对每个激光点的坐标进行坐标转换，得到优化后的一帧激光数据。
[0034]
进一步地，所述步骤s4中imu测量模型为：
[0035][0036][0037]
其中，表示imu在体坐标系下的惯性加速度测量值，表示从世界坐标系到体坐标系的坐标转换，aw和gw分别表示世界坐标系下的惯性加速度真实值和重力加速度，ba和na分别表示imu中加速度随机游走量和加速度高斯噪声，表示imu在体坐标系下的角速度测量值，ωb表示体坐标系下的角速度真实值，bg和ng分别表示imu中角速度随机游走量和角速度高斯噪声；
[0038]
所述imu的误差运动模型为：
[0039]
δp(t+δt)＝δp+δvδt
[0040][0041][0042]
δba(t+δt)＝δba+ηa[0043]
δbg(t+δt)＝δbg+ηg[0044]
δg(t+δt)＝δg；
[0045]
其中，r表示imu四元数姿态转换成的旋转矩阵，ηv表示速度项的噪声，η
θ
表示姿态项的噪声，ηa表示加速度随机游走项的噪声，ηg表示角速度随机游走项的噪声。
[0046]
进一步地，所述步骤s4中误差状态预测方程为：
[0047]
δx＝fδx+w；
[0048]
其中，f表示对滤波器状态传播方程线性化后的状态传播矩阵。
[0049]
进一步地，所述步骤s4中的误差状态预测方程根据imu的随机漂移误差参数，得到状态预测协方差矩阵；
[0050]
所述状态预测协方差方程为：
[0051]
p＝fpf
t
+q；
[0052]
其中，p表示紧耦合定位系统的状态协方差矩阵，q表示紧耦合定位系统的噪声协方差矩阵。
[0053]
进一步地，所述步骤s5中的激光配准误差模型为：
[0054]
f(x)＝(n
τ
[xp-q1])2；
[0055]
其中，x表示前后两帧激光数据相对位姿变化量，p表示后一帧激光数据上的激光点，q1表示前一帧激光数据上p点对应直线段的端点之一，n
t
表示对应直线段上的归一化法向量；
[0056]
误差状态更新方程为：
[0057][0058]
其中，δx表示imu误差状态量，表示激光误差状态量，-x表示紧耦合定位系统的名义状态预测量。
[0059]
进一步地，所述步骤s8包括：
[0060]
s81：利用误差状态最优估计量对紧耦合定位系统的名义状态预测量进行校正，得
到紧耦合定位系统的真实状态量；
[0061]
s82：获取紧耦合定位系统的真实状态量之后，对紧耦合定位系统进行重置处理，将误差状态量清零，并保留状态协方差矩阵的估计。
[0062]
本发明采用上述技术方案包括以下有益效果：
[0063]
本发明采用了迭代误差状态卡尔曼滤波器作为紧耦合定位系统，可以更好的发挥各个传感器的优势，从而有效提高移动机器人定位精度和可靠性。紧耦合定位系统通过紧耦合定位方法能够更高效的利用各种传感器的优势，不仅能提高定位精度和可靠性，还能保证及时校正惯性导航所累积的误差，并且即使在激光数据长时间退化的情况下，也能够保持误差在可接受的范围内。
附图说明
[0064]
图1为本发明实施例提供的基于激光雷达和imu的多传感器融合定位方法流程图一；
[0065]
图2为本发明实施例提供的基于激光雷达和imu的多传感器融合定位方法流程图二。
具体实施方式
[0066]
为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
[0067]
以下是本发明的具体实施例并结合附图，对本发明的技术方案作进一步的描述，但本发明并不限于这些实施例。
[0068]
实施例
[0069]
本实施例提供了一种基于激光雷达和imu的多传感器融合定位方法，如图1和图2所示，本方法包括步骤：
[0070]
s1：通过激光雷达和imu分别获取激光数据和惯性数据，并在激光雷达和imu之间进行空间同步；
[0071]
s2：利用imu的预积分模型对惯性数据进行预积分处理，获取紧耦合定位系统的名义状态预测量；
[0072]
s3：通过紧耦合定位系统的名义状态预测量对激光数据进行运动畸变校正，得到校正后的激光数据；
[0073]
s4：根据imu的测量模型和imu的误差运动模型建立紧耦合定位系统的误差状态预测方程；
[0074]
s5：根据激光配准误差模型建立紧耦合定位系统的误差状态更新方程；
[0075]
s6：基于校正后的激光数据获取激光误差状态量，并获取相应时间内的imu误差状态量；
[0076]
s7：将激光误差状态量和imu误差状态量代入紧耦合定位系统进行迭代优化，获取误差状态最优估计量；
[0077]
s8：通过误差状态最优估计量对紧耦合定位系统的名义状态预测量进行校正，达到紧耦合定位。
[0078]
参阅图2，具体的，步骤s1中的空间同步包括s11：对激光雷达和imu的相对位姿参数进行标定。
[0079]
其中，根据激光雷达和imu在移动机器人上安装的位置设定传感器之间的外部参数，完成空间同步设置。
[0080]
具体的，步骤s2中的紧耦合定位系统采用迭代误差状态卡尔曼滤波器；
[0081]
滤波器的状态量之间关系为：
[0082][0083]
其中，x
t
表示滤波器的真实状态量，x表示滤波器的名义状态量，δx表示滤波器的误差状态量，表示标量与矢量的加减运算。
[0084]
具体的，步骤s2中imu的预积分模型为：
[0085][0086][0087][0088]
其中，表示imu从i时刻到j时刻的位姿预测量，表示imu从i时刻到j时刻的速度预测量，表示imu从i时刻到j时刻的四元数姿态预测量；
[0089]
名义状态预测量为：
[0090]
x＝[p v q b
a b
g g]
τ
；
[0091]
其中，p表示名义位移量，v表示名义速度量，q表示名义四元数姿态量，ba表示名义加速度计随机游走量，bg表示名义陀螺仪随机游走量，g表示名义重力矢量。
[0092]
参阅图2，具体的，步骤s3包括：
[0093]
s31：计算获取得到的一帧激光数据内每个激光点的时间戳；
[0094]
s32：基于激光点的时间戳，根据紧耦合定位系统的名义状态预测量找到对应时间戳的位姿先验值；
[0095]
s33：根据对应时间戳的位姿先验值，对每个激光点的坐标进行坐标转换，得到优化后的一帧激光数据。
[0096]
具体的，步骤s4中imu测量模型为：
[0097][0098][0099]
其中，表示imu在体坐标系下的惯性加速度测量值，表示从世界坐标系到体坐标系的坐标转换，aw和gw分别表示世界坐标系下的惯性加速度真实值和重力加速度，ba和na分别表示imu中加速度随机游走量和加速度高斯噪声，表示imu在体坐标系下的角速度测量值，ωb表示体坐标系下的角速度真实值，bg和ng分别表示imu中角速度随机游走量和角速度高斯噪声；
[0100]
imu的误差运动模型为：
[0101]
δp(t+δt)＝δp+δvδt
[0102][0103][0104]
δba(t+δt)＝δba+ηa[0105]
δbg(t+δt)＝δbg+ηg[0106]
δg(t+δt)＝δg；
[0107]
其中，r表示imu四元数姿态转换成的旋转矩阵，ηv表示速度项的噪声，η
θ
表示姿态项的噪声，ηa表示加速度随机游走项的噪声，ηg表示角速度随机游走项的噪声。
[0108]
具体的，步骤s4中误差状态预测方程为：
[0109]
δx＝fδx+w；
[0110]
其中，f表示对滤波器状态传播方程线性化后的状态传播矩阵。
[0111]
具体的，步骤s4中的误差状态预测方程根据imu的随机漂移误差参数，得到状态预测协方差矩阵；
[0112]
状态预测协方差方程为：
[0113]
p＝fpf
t
+q；
[0114]
其中，p表示紧耦合定位系统的状态协方差矩阵，q表示紧耦合定位系统的噪声协方差矩阵。
[0115]
具体的，步骤s5中的激光配准误差模型为：
[0116]
f(x)＝(n
τ
[xp-q1])2；
[0117]
其中，x表示前后两帧激光数据相对位姿变化量，p表示后一帧激光数据上的激光点，q1表示前一帧激光数据上p点对应直线段的端点之一，n
t
表示对应直线段上的归一化法向量；
[0118]
误差状态更新方程为：
[0119][0120]
其中，δx表示imu误差状态量，表示激光误差状态量，-x表示紧耦合定位系统的名义状态预测量。
[0121]
具体的，步骤s8包括：
[0122]
s81：利用误差状态最优估计量对紧耦合定位系统的名义状态预测量进行校正，得到紧耦合定位系统的真实状态量；
[0123]
s82：获取紧耦合定位系统的真实状态量之后，对紧耦合定位系统进行重置处理，将误差状态量清零，并保留状态协方差矩阵的估计。
[0124]
其中，根据激光配准误差模型，通过前后两帧校正后的激光数据获取激光误差状态量，并通过校正后的惯性数据获取其相应时间内的imu误差状态量。将两种误差状态量代入系统进行迭代优化，获取误差状态最优估计量，根据非线性联合优化模型，对获取的激光误差状态量和imu误差状态量进行迭代优化，获取误差状态最优估计量。利用误差状态最优估计量对系统的名义状态预测量进行校正，从而实现紧耦合定位。根据步骤s2中的三种状
态量之间的关系，利用误差状态最优估计量对系统的名义状态预测量进行校正，获取系统的真实状态量。获取系统的真实状态量之后，对系统进行重置处理，即对系统的误差状态量清零，同时保留状态协方差矩阵的估计。
[0125]
本方法由紧耦合定位系统通过紧耦合定位方法能够更高效的利用各种传感器的优势，不仅能提高定位精度和可靠性，还能保证及时校正惯性导航所累积的误差，并且即使在激光数据长时间退化的情况下，也能够保持误差在可接受的范围内。
[0126]
虽然本公开披露如上，但本公开的保护范围并非仅限于此。本领域技术人员，在不脱离本公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

技术特征：
1.一种基于激光雷达和imu的多传感器融合定位方法，其特征在于，包括步骤：s1：通过激光雷达和imu分别获取激光数据和惯性数据，并在激光雷达和imu之间进行空间同步；s2：利用imu的预积分模型对惯性数据进行预积分处理，获取紧耦合定位系统的名义状态预测量；s3：通过紧耦合定位系统的名义状态预测量对激光数据进行运动畸变校正，得到校正后的激光数据；s4：根据imu的测量模型和imu的误差运动模型建立紧耦合定位系统的误差状态预测方程；s5：根据激光配准误差模型建立紧耦合定位系统的误差状态更新方程；s6：基于校正后的激光数据获取激光误差状态量，并获取相应时间内的imu误差状态量；s7：将激光误差状态量和imu误差状态量代入紧耦合定位系统进行迭代优化，获取误差状态最优估计量；s8：通过误差状态最优估计量对紧耦合定位系统的名义状态预测量进行校正，达到紧耦合定位。2.根据权利要求1所述的基于激光雷达和imu的多传感器融合定位方法，其特征在于，所述步骤s1中的空间同步包括：s11：对激光雷达和imu的相对位姿参数进行标定。3.根据权利要求1所述的基于激光雷达和imu的多传感器融合定位方法，其特征在于，所述步骤s2中的紧耦合定位系统采用迭代误差状态卡尔曼滤波器；所述滤波器的状态量之间关系为：其中，x
t
表示滤波器的真实状态量，x表示滤波器的名义状态量，δx表示滤波器的误差状态量，表示标量与矢量的加减运算。4.根据权利要求3所述的基于激光雷达和imu的多传感器融合定位方法，其特征在于，所述步骤s2中imu的预积分模型为：所述步骤s2中imu的预积分模型为：所述步骤s2中imu的预积分模型为：其中，表示imu从i时刻到j时刻的位姿预测量，表示imu从i时刻到j时刻的速度预测量，表示imu从i时刻到j时刻的四元数姿态预测量；所述名义状态预测量为：x＝[p v q b
a b
g g]
τ
；
其中，p表示名义位移量，v表示名义速度量，q表示名义四元数姿态量，b
a
表示名义加速度计随机游走量，b
g
表示名义陀螺仪随机游走量，g表示名义重力矢量。5.根据权利要求1所述的基于激光雷达和imu的多传感器融合定位方法，其特征在于，所述步骤s3包括：s31：计算获取得到的一帧激光数据内每个激光点的时间戳；s32：基于激光点的时间戳，根据紧耦合定位系统的名义状态预测量找到对应时间戳的位姿先验值；s33：根据对应时间戳的位姿先验值，对每个激光点的坐标进行坐标转换，得到优化后的一帧激光数据。6.根据权利要求1所述的基于激光雷达和imu的多传感器融合定位方法，其特征在于，所述步骤s4中imu测量模型为：所述步骤s4中imu测量模型为：其中，表示imu在体坐标系下的惯性加速度测量值，表示从世界坐标系到体坐标系的坐标转换，a
w
和g
w
分别表示世界坐标系下的惯性加速度真实值和重力加速度，b
a
和n
a
分别表示imu中加速度随机游走量和加速度高斯噪声，表示imu在体坐标系下的角速度测量值，ω
b
表示体坐标系下的角速度真实值，b
g
和n
g
分别表示imu中角速度随机游走量和角速度高斯噪声；所述imu的误差运动模型为：δp(t+δt)＝δp+δvδtδp(t+δt)＝δp+δvδtδb
a
(t+δt)＝δb
a
+η
a
δb
g
(t+δt)＝δb
g
+η
g
δg(t+δt)＝δg；其中，r表示imu四元数姿态转换成的旋转矩阵，η
v
表示速度项的噪声，η
θ
表示姿态项的噪声，η
a
表示加速度随机游走项的噪声，η
g
表示角速度随机游走项的噪声。7.根据权利要求6所述的基于激光雷达和imu的多传感器融合定位方法，其特征在于，所述步骤s4中误差状态预测方程为：δx＝fδx+w；其中，f表示对滤波器状态传播方程线性化后的状态传播矩阵。8.根据权利要求7所述的基于激光雷达和imu的多传感器融合定位方法，其特征在于，所述步骤s4中的误差状态预测方程根据imu的随机漂移误差参数，得到状态预测协方差矩阵；所述状态预测协方差方程为：p＝fpf
t
+q；其中，p表示紧耦合定位系统的状态协方差矩阵，q表示紧耦合定位系统的噪声协方差
矩阵。9.根据权利要求8所述的基于激光雷达和imu的多传感器融合定位方法，其特征在于，所述步骤s5中的激光配准误差模型为：f(x)＝(n
τ
[xp-q1])2；其中，x表示前后两帧激光数据相对位姿变化量，p表示后一帧激光数据上的激光点，q1表示前一帧激光数据上p点对应直线段的端点之一，n
t
表示对应直线段上的归一化法向量；误差状态更新方程为：其中，δx表示imu误差状态量，表示激光误差状态量，-x表示紧耦合定位系统的名义状态预测量。10.根据权利要求9所述的基于激光雷达和imu的多传感器融合定位方法，其特征在于，所述步骤s8包括：s81：利用误差状态最优估计量对紧耦合定位系统的名义状态预测量进行校正，得到紧耦合定位系统的真实状态量；s82：获取紧耦合定位系统的真实状态量之后，对紧耦合定位系统进行重置处理，将误差状态量清零，并保留状态协方差矩阵的估计。

技术总结
本发明提供了一种基于激光雷达和IMU的多传感器融合定位方法，涉及定位导航技术领域，本方法包括步骤S1：通过激光雷达和IMU分别获取激光数据和惯性数据，并在激光雷达和IMU之间进行空间同步；S2：利用IMU的预积分模型对惯性数据进行预积分处理，获取紧耦合定位系统的名义状态预测量；S3：通过紧耦合定位系统的名义状态预测量对激光数据进行运动畸变校正，得到校正后的激光数据；S4：根据IMU的测量模型和IMU的误差运动模型建立紧耦合定位系统的误差状态预测方程；S5：根据激光配准误差模型建立紧耦合定位系统的误差状态更新方程。本方法能够通过紧耦合定位方法有效提高移动机器人的定位精度和可靠性。定位精度和可靠性。定位精度和可靠性。


技术研发人员：方灶军 孙晨阳 潘聪 张弛 杨桂林
受保护的技术使用者：中国科学院宁波材料技术与工程研究所
技术研发日：2023.05.30
技术公布日：2023/10/15