一种激光雷达与旋转惯导一体化构架与信息融合的SLAM方法与流程

技术特征：
1.一种激光雷达与旋转惯导一体化构架与信息融合的slam方法，其特征在于，包括：(1)惯导对准：利用递归小波神经网络rwnn模型处理融合惯性传感器imu数据，进行惯导系统动基座对准，对imu误差进行修正，并输出修正后的惯导系统姿态和惯性信息；(2)特征提取：使用修正后的惯性信息，计算激光帧之间的平移与旋转，消除原始点云中的运动畸变，从中提取稳定的边缘与平面特征，通过特征匹配计算两帧激光数据间的位姿变换；(3)采用激光惯性里程计lio模型进行导航解算：使用修正后的惯性信息进行惯性导航解算，作为滤波器估计值，再利用激光雷达的位姿解算结果作为滤波器的观测值，进行迭代卡尔曼滤波，输出系统在全局坐标系下的位姿以及未失真的特征；(4)建图：采用基于全局地图的匹配，输出修正后的位姿，然后使用新的特征更新三维点云地图。2.根据权利要求1所述的方法，其特征在于，rwnn模型使系统的输出与参考值之间的偏差最小，系统动力学公式如下：其中，和分别表示估计状态向量与估计输出向量，使用估计状态向量以及传感器输出向量u作为rwnn的输入，得到不确定向量δu的近似值，τ是输出的权重向量，ψ
w
表示小波向量，以及分别表示输入反馈权重、小波的缩放和平移量，观测器的增益由l表示，并且满足矩阵为hurwitz矩阵；采用表示rwnn的近似值，动态模拟状态估计误差量和输出误差量，如公式(2)所示：其中，和分别表示状态误差量和输出误差量；将部分改写成如下形式：其中，τ
*t
ψ(δx,u,ζ
*
,t
*
,d
*
) ε是λ(δx,u)的理想近似集，ε表示有界近似误差值，||ε||≤ε
m
且ε
m
为正实数。3.根据权利要求1所述的方法，其特征在于，rwnn模型所采用的rwnn网络包含输入层，递归小波层，产生层以及输入层四层结构；算法中母小波为mexican-hat小波形式，如下所示：式中，μ(s)ε[0,1]是标准化的输入向量，而||μ(s)||2＝μ
t
(s)μ(s)；首先该算法将输入的传感器输出数据与最新状态向量归一化在[0,1]范围内，归一过程如下：
接着在递归小波层中，每个输入信号(μ
i
)都与j索引指定的三个激活函数进行向量积计算；此激活函数是根据公式(4)的母小波推导得到的，具体过程如下：σ
d,t
(μ(s))＝2
0.5d
σ(2
d
μ(s)-t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)其中，d和t分别表示小波函数的缩放和平移量；然后，通过使用项σ
d,t
(z-1
(s))来存储网络的过去信息，在该项中，根据上一刻的存储项计算下一刻的存储项计算下一刻的存储项式中表示递归小波层反馈项，将公式(7)改写成：在产生层中，每一个节点在小波层由π函数表示，乘以来自第二层的输入信号，如下所示：最后，根据公式(10)计算rwnn的最终输出其中，out
l
是近似的不确定度向量，表示输出层的连接权重。4.根据权利要求1所述的方法，其特征在于，激光惯性里程计lio模型包括状态递推子模块和更新子模块，该模块的状态方程如式(11)、(12)所示：模块和更新子模块，该模块的状态方程如式(11)、(12)所示：其中，表示相对于的位姿，表示描述从到的相对转换的局部状态，是相对于的位置，是描述从到旋转的单位四元数，和表示从到的平移和旋转，是相对于的速度，b
a
是加速度偏置，b
g
是陀螺仪偏置，是局部重力用表示；在建模过程中略去b
a
、b
g
，如式(13)所示：使用误差状态表示来求解用δ表示一个误差项，并将的误差向量定义为：
δx＝[δp,δv,δθ,δg]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)其中δθ是3dof姿态误差角；根据eskf的框架，求解δx后，通过将误差δx代入到先验状态中来获得最终的观测状态是通过自定义运算符#进行的，该运算符定义为：其中表示四元数乘积，exp运算符通过李群将δθ映射到四元数旋转；随后在状态更新过程中，根据imu的测量值，递推得到误差状态δx、误差状态协方差矩阵p
k
和先验状态据此进行状态更新，状态更新方程如式(16)、(17)、(18)：据此进行状态更新，状态更新方程如式(16)、(17)、(18)：δx
j 1
＝δx
j
δx
j
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(18)每次更新完成后，根据式(14)进行状态合成更新全局位姿，完成lio模块的任务

技术总结
本发明公开了一种基于激光雷达与旋转惯导一体化构架与信息融合的SLAM方法，所述方法包括：特征提取步骤，通过IMU数据计算激光帧之间的平移与旋转，消除原始点云中的运动畸变，从中提取稳定的边缘与平面特征。LIO步骤，由状态递推子模块和更新子模块组成，进行迭代卡尔曼滤波，输出初始里程以及未失真的特征。建图步骤，用全局地图改进初始里程，输出新的里程，然后使用新的特征更新地图。本发明所提出方法可应用于中低精度的MEMS惯导，有效提高了长时间运行时惯导系统的定位精度，从而提升了建图的准确性和可靠性。的准确性和可靠性。的准确性和可靠性。


技术研发人员：金扬 蔡洁萱 李冠宇 万游 钱伟行 胡迪宇 冯素格 瞿安朝 苏晓林
受保护的技术使用者：江苏省中以产业技术研究院
技术研发日：2021.10.27
技术公布日：2022/2/8