一种基于因子图优化的多车辆协同导航方法

本发明涉及车辆定位，尤其涉及一种基于因子图优化的多车辆协同导航方法。

背景技术：

1、目前，车辆定位最常使用的方法是gnss(全球卫星导航系统)，但是随着机器人技术的发展，例如物流仓库的货运机器人等在室内工作的车辆，以及其他一些如水下和密林等gnss信号受阻的工作环境下，无法使用gps信号，而只能使用车载imu(惯性运动单元)进行定位，误差会随着时间而快速累计，导致系统发散，无法使用。

2、此外，导航系统的精度同样受到滤波算法的影响，目前在导航领域多使用kf(卡尔曼滤波)方式对读取的数据进行融合，kf假设系统是一阶马尔可夫过程，当前状态仅与上一时刻状态有关，忽略了先前的状态信息，并且kf只适用与线性系统，适用非线性系统的ekf(扩展卡尔曼滤波)则会带来额外的误差，并且kf难以处理多传感数据异步问题，kf的滤波性能取决于有关协方差矩阵(q和p)的先验假设的质量，在系统模型知识不足的情况下并不可靠。

3、需要说明的是，在上述背景技术部分公开的信息只用于加强对本公开的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。

技术实现思路

1、本发明的目的在于克服现有技术的缺点，提供了一种基于因子图优化的多车辆协同导航方法，解决了现有技术存在的问题。

2、本发明的目的通过以下技术方案来实现：一种基于因子图优化的多车辆协同导航方法，所述多车辆协同导航方法包括：

3、s1、设置车辆的姿态、速度、位置和传感线协方差矩阵，通过车载传感器采集并处理数据信息；

4、s2、通过imu采集车辆加速度和角速度信息，并将imu得到的数据进行预积分处理，得到低频数据；

5、s3、通过轮式里程计采集车辆速度信息，并通过判断车辆是否进行转弯来调整里程计协方差矩阵；

6、s4、通过uwb测量车辆之间的相对距离与平面角度信息，进而得到两辆车辆之间的观测量；

7、s5、通过因子图优化的方法对采集的所有数据信息进行融合，并通过求解非线性最小二乘解得到状态补偿量，对系统状态量进行补偿后重新计算，直到状态补偿量小于所设阈值，则完成优化，最后根据优化值进行多车辆协同导航。

8、所述设置车辆的姿态、速度、位置和传感线协方差矩阵，通过车载传感器采集并处理数据信息具体包括以下内容：

9、将每辆车辆i的系统状态量设置为xi＝[a,v,p,e,d]t，其中a＝[α,β,γ]表示n系姿态，v＝[vx,vy,vz]表示n系速度，p＝[px,py,pz]表示位置，e＝[ex,ey,ez]代表三轴陀螺仪零偏，d＝[dx,dy,dz]代表三轴加速度计零偏。

10、所述通过imu采集车辆加速度和角速度信息，并将imu得到的数据进行预积分处理，得到低频数据具体包括以下内容：

11、设置其他传感器一次输出为一帧，通过预积分得到通过imu得到加速度计、陀螺仪数据fb,ωb，计算两帧的姿态增量为δa＝[δα,δβ,δγ]，速度增量为δv＝[δvx,δvy,δvz]、位置增量为δp＝[δpx,δpy,δpz]，得到每辆车辆i的预积分因子为：

12、

13、其中，k表示帧数，ω表示观测量协方差矩阵。

14、所述通过轮式里程计采集车辆速度信息，并通过判断车辆是否进行转弯来调整里程计协方差矩阵具体包括以下内容：

15、通过计算车轮转动圈数与车轮半径得到车辆运动里程，将里程进行微分得到车辆在b系下的运动速度vod；

16、通过结合非完整性约束，将车辆在前进过程中的测向和天向速度设置为0，得到车辆速度为[0，vod，0]。

17、通过判断车辆是否进行转弯来调整里程计协方差矩阵，在直行时，减小协方差矩阵，使得系统更加信任里程计数据，在转弯时，增大协方差矩阵；其中，判断车辆是否转为的条件为：当|ωy|＞ω1且|fy|＞f1时，转弯，其中ωy为imu测量的y轴角速度，fy为imu测量的y轴加速度，即侧向角速度和侧向加速度，ω1,f1为判断阈值，协方差矩阵为ωod；

18、则每辆车辆i的观测量为里程计因子为其中，表示第i辆车k时刻状态。

19、所述通过uwb测量车辆之间的相对距离与平面角度信息，进而得到两辆车辆之间的观测量包括：

20、通过uwb测量两辆车辆之间的相对距离为l，平面角度为ρ，并通过三角函数车辆之间的水平距离lx和前后距离ly，当两辆车之间的高度差为0时，则两辆车辆i和j的观测量为uwb因子为其中，表示观测量与状态量之间的关系，×代表向量的反对称矩阵，为i，j车辆中携带uwb基站车辆由n系到b系旋转矩阵；

21、当两辆车之间高度差不为0时，两辆车辆i和j的观测量为

22、判断两辆车之间的高度差包括：当i，j车辆的或时，判断为高度差不为0，反之为0，其中代表i车辆imu测量的侧向、上向角速度，wx，wy代表阈值。

23、所述因子图优化的方法包括：k时刻因子图的最大后验概率值为通过非线性最小二乘找出使得最大后验概率值最小值对应的状态，完成优化。

24、本发明具有以下优点：一种基于因子图优化的多车辆协同导航方法，通过使用imu、od、uwb传感器并使用合理的数据处理方式与融合方法，能大幅延长在没有绝对导航信息的输入的系统中长时间稳定工作，实现系统更长时间的无源自主导航的车辆定位。

技术特征：

1.一种基于因子图优化的多车辆协同导航方法，其特征在于：所述多车辆协同导航方法包括：

2.根据权利要求1所述的一种基于因子图优化的多车辆协同导航方法，其特征在于：所述设置车辆的姿态、速度、位置和传感线协方差矩阵，通过车载传感器采集并处理数据信息具体包括以下内容：

3.根据权利要求2所述的一种基于因子图优化的多车辆协同导航方法，其特征在于：所述通过imu采集车辆加速度和角速度信息，并将imu得到的数据进行预积分处理，得到低频数据具体包括以下内容：

4.根据权利要求1所述的一种基于因子图优化的多车辆协同导航方法，其特征在于：所述通过轮式里程计采集车辆速度信息，并通过判断车辆是否进行转弯来调整里程计协方差矩阵具体包括以下内容：

5.根据权利要求4所述的一种基于因子图优化的多车辆协同导航方法，其特征在于：所述通过uwb测量车辆之间的相对距离与平面角度信息，进而得到两辆车辆之间的观测量包括：

6.根据权利要求5所述的一种基于因子图优化的多车辆协同导航方法，其特征在于：所述因子图优化的方法包括：k时刻因子图的最大后验概率值为通过非线性最小二乘找出使得最大后验概率值最小值对应的状态，完成优化。

技术总结
本发明涉及一种基于因子图优化的多车辆协同导航方法，包括：设置车辆的初始信息，通过车载传感器采集并处理数据信息；通过IMU采集车辆加速度和角速度信息，并将数据进行预积分处理；通过轮式里程计采集车辆速度信息；通过UWB测量车辆之间的相对距离与平面角度信息，进而得到两辆车辆之间的观测量；通过因子图优化的方法对数据信息进行融合，并通过求解非线性最小二乘解得到状态补偿量，对系统状态量进行补偿后重新计算，最后根据优化值进行多车辆协同导航。本发明通过使用IMU、OD、UWB传感器并使用合理的数据处理方式与融合方法，能大幅延长在没有绝对导航信息的输入的系统中长时间稳定工作，实现无源自主导航的车辆定位。

技术研发人员：赵鑫,杜爽,邓斯琪,杨文慧,甘章怡
受保护的技术使用者：电子科技大学
技术研发日：
技术公布日：2024/1/12