【技术实现步骤摘要】
本专利技术涉及自动驾驶，具体涉及一种基于因子图的多位姿源融合定位方法。
技术介绍
1、在矿山无人驾驶领域，矿卡的精准定位是实现自动化运输和提高作业效率的关键技术之一。然而，由于矿山环境的复杂性和多变性，传统的单一定位技术难以提供全面、精确的矿卡位置信息，例如，传统使用的里程计模块和rtk技术分别存在以下缺陷：
2、里程计模块：在矿山无人驾驶中，常用的里程计模块(如轮式里程计、惯性测量单元等)可以提供车辆的相对位移信息。但是，这些设备容易受到累积误差的影响，随着时间的推移和距离的增加，误差会逐渐累积，导致定位结果的不准确。
3、rtk技术：虽然实时动态差分全球定位系统(rtk gps)技术能够提供高精度的位置信息，但在矿山环境中，由于地形遮挡、多路径效应等因素，rtk信号可能会受到干扰，导致定位结果出现跳变，影响定位的连续性和稳定性。
4、此外，当前的自主定位算法包括slam(同步定位与建图)和基于机器学习、深度学习的方法。其中，基于机器学习和深度学习的方法需要大量的标注数据进行训练，且存在鲁棒性弱，对于新环境的适应能力有限。而在矿山这种特殊环境下，收集大量高质量的训练数据是非常困难的，因此这些方法的应用受到了限制。
5、综上，目前自动驾驶领域已着力研究多个传感器信息融合定位的方法，这样的优势在于不依赖于某一个特定的传感器。然而，由于各个传感器均具有特性，如何融合定位以及利用不同传感器之间的信息差异如何优化不同位姿源的融合结果仍是项具有挑战性的研究。
技术
1、为了解决上述技术问题，本专利技术提供了一种基于因子图的多位姿源融合定位方法，所述方法利用rtk、lidar、imu以及轮速传感器进行融合定位矿卡于卸料口区域的状态，所述方法包括以下步骤：
2、s1、因子图建立：将融合开始时刻的rtk状态信息作为当前状态下的先验因子，轮速-imu里程计信息作为中间约束因子，点云地图重定位里程计信息作为一元边因子，建立因子图；
3、s2、所述点云地图重定位里程计信息优化：利用imu数据进行前向传播，建立状态量误差模型；利用前向传播的预测值对lidar原始点云进行后向传播来进行去畸变操作，利用去畸变后的点云建立观测方程；再采用iekf进行更新，求解残差，至满足迭代截止条件后更新所述点云地图重定位里程计的最终状态；
4、s3、所述轮速-imu里程计信息优化：根据imu提供的角速度利用imu kinematic进行状态前向传播，得到旋转误差的观测方程；同时根据imu提供的加速度建立一个关于当前旋转的量测方程，通过卡尔曼滤波得到优化后的imu旋转信息，并结合轮速计算得到imu数据时刻下的轮速-imu里程计信息；
5、s4、多位姿源融合：将所述先验因子、点云地图重定位里程计因子和轮速-imu里程计因子进行累乘，得到最大概率的系统状态。
6、进一步的：
7、所述点云地图重定位里程计信息优化中的状态量误差的雅可比矩阵其计算公式为：
8、
9、所述点云地图重定位里程计信息优化中的噪声量wi的雅可比矩阵其计算公式为：
10、
11、同时计算得到与预测协方差其计算公式为：
12、
13、其中，xi+1表示存在噪声的真值，表示噪声为零的预测状态，qi表示噪声量的协方差矩阵。
14、进一步的，所述点云地图重定位里程计信息优化还包括：
15、s21、利用相对位姿变换将其他时刻的点云从各自的局部坐标系转换到扫描时刻下的统一坐标系以完成去畸变操作，其满足公式：
16、
17、其中，itl表示lidar到imu的外参；
18、s22、利用外参itl将去噪后的点云lpj+lnj从lidar坐标系转换到imu坐标系，再利用先验状态量中的位姿将点云从imu坐标系转换到全局坐标系；
19、s23、统一坐标系后，点云找到对应的若干个对应点gqj，且点云与对应点组成的向量与局部平面的法向量内积的理想状态应为0，其满足等式：
20、
21、并计算对应的雅可比矩阵。
22、进一步的，所述s2中采用iekf进行更新还包括：
23、s24、采用iekf进行更新的过程中，通过迭代进行重复观测，以逐渐逼近最优线性化点；
24、s25、假设系统初始先验状态为对应的误差状态为δx；则记第k次迭代时，状态向量先验值为待求的误差状态为δxk；根据最大后验估计求出δxk后，将所述δxk加回到得到重新观测求解残差z与雅可比矩阵h；再次求解其满足公式：
25、k＝(htr-1h+p-1)-1htr-1
26、
27、其中，i表示单位矩阵，j表示雅可比矩阵，p表示协方差；
28、s26、如此往复所述s25，直到满足迭代截止条件后，更新所述点云地图重定位里程计信息的最终状态。
29、进一步的，将所述s3中的旋转误差作为预测方程，其满足公式：
30、
31、
32、其中，θ表示旋转角度，bg表示偏移量，t表示时间，ωm表示角速度，i表示单位矩阵，fx表示预测状态的雅可比矩阵，fi表示噪声量的雅可比矩阵，k表示时刻。
33、进一步的，所述轮速-imu里程计信息优化还包括：
34、s31、建立量测方程，其满足公式：
35、
36、其中，at表示重力方向，vg表示理想的重力方向[0 0 1]，
37、计算雅可比矩阵h，其满足公式：
38、
39、s32、利用协方差矩阵p、雅克比矩阵h和噪声na计算卡尔曼增益k，其满足公式：
40、k＝p·ht·(h·pht+na)-1
41、s33、利用所述卡尔曼增益k计算优化后的旋转矩阵、偏差值以及预测协方差，得到优化后的旋转信息后，计算当前imu数据时刻前、后桢的轮速数据在imu数据时刻的线性插值结果，对应为imu数据时刻下的轮速数据；
42、s34、将运动化为n等分，n趋近于无穷，得到位姿变换公式，作为轮速-imu里程计因子，所述位姿变换公式为：
43、
44、其中，c表示所述s32得到的优化后的旋转矩阵，j表示旋转矩阵对应的雅可比矩阵。
45、进一步的，所述最大概率的系统状态由贝叶斯定律推导所得，其满足公式：
46、
47、其中，zi表示观测，xi表示系统状态。
48、与现有技术相比，本专利技术具有如下有益效果：
49、本专利技术通过结合rtk、轮速、imu以及lidar多个传感器数据，利用误差卡尔曼滤波以及因子图优化的方法对这些数据进行融合优化，得到短时间内矿卡精确的自主定位信息。
本文档来自技高网...【技术保护点】
1.一种基于因子图的多位姿源融合定位方法，其特征在于，所述方法利用RTK、LiDAR、IMU以及轮速传感器进行融合，获取矿卡于卸料口区域的状态，所述方法包括以下步骤：
2.根据权利要求1所述的基于因子图的多位姿源融合定位方法，其特征在于：
3.根据权利要求2或1所述的基于因子图的多位姿源融合定位方法，其特征在于，所述点云地图重定位里程计信息优化还包括：
4.根据权利要求3所述的基于因子图的多位姿源融合定位方法，其特征在于，所述S2中采用IEKF进行更新还包括：
5.根据权利要求1所述的基于因子图的多位姿源融合定位方法，其特征在于，将所述S3中的旋转误差作为预测方程，其满足公式：
6.根据权利要求5所述的基于因子图的多位姿源融合定位方法，其特征在于，所述轮速-IMU里程计信息优化还包括：
7.根据权利要求1所述的基于因子图的多位姿源融合定位方法，其特征在于，所述最大概率的系统状态由贝叶斯定律推导所得，其满足公式：
【技术特征摘要】
1.一种基于因子图的多位姿源融合定位方法，其特征在于，所述方法利用rtk、lidar、imu以及轮速传感器进行融合，获取矿卡于卸料口区域的状态，所述方法包括以下步骤：
2.根据权利要求1所述的基于因子图的多位姿源融合定位方法，其特征在于：
3.根据权利要求2或1所述的基于因子图的多位姿源融合定位方法，其特征在于，所述点云地图重定位里程计信息优化还包括：
4.根据权利要求3所述的基于因子图的多位姿源融合定位方法...
【专利技术属性】
技术研发人员：何申中，潘伟，汪贵冬，王炜杰，张帅乾，
申请(专利权)人：安徽海博智能科技有限责任公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。