基于B样条函数的激光雷达-惯性-车辆融合定位方法

本发明涉及机器人导航定位领域，具体涉及一种基于b样条函数的激光雷达-惯性-车辆融合定位方法。

背景技术：

1、同时定位与建图(slam，simultaneous localization and mapping)技术是移动机器人中的关键技术，该技术可以不依赖于外部信号，而利用搭载的局部测量传感器信息估计载体当前位姿信息，同时构建周界环境的地图模型。目前，视觉和激光雷达是slam技术中两种主要使用的外部传感器，相较于视觉传感器，激光雷达由于其可以获得高精度距离信息以及不受环境光照与纹理影响的特性而在实际应用中被广泛使用。然而，激光雷达强依赖于环境的结构特征信息，导致在隧道、空旷平地等环境中出现退化。

2、为了克服激光雷达的上述缺点，一些研究将其他类型传感器，如imu、轮速计等，与激光雷达进行融合定位。现有方法在利用imu、轮速计以及转角传感器等信息时，采用积分获得帧间相对变化的方法进行融合，而没有使用原始采集数据，容易在积分的过程中将原始数据噪声放大，从而影响融合定位精度。除此之外，车辆在行驶过程中还会由于轮胎与地面之间的侧向力，而出现瞬时速度方向偏离车头朝向的侧偏现象，运动学模型与轮速计并不能准确描述该车辆行驶过程中普遍出现的现象。且车辆模型参数在车辆行驶过程中由于载荷、地面情况等因素而与预先标定值有所偏差，导致引入测量误差。

3、因此，为了减小上述现象给融合定位带来的影响，本发明提出了一种基于b样条函数的激光雷达-惯性-车辆融合定位方法，并且首次在定位问题中引入了车辆的横向加速度约束，用于描述车辆的运动特性并为定位提供先验信息，提高车辆在复杂环境中自主定位的精度和鲁棒性。

技术实现思路

1、本发明的目的在于针对现有技术的不足，提供一种基于b样条函数的激光雷达-惯性-车辆融合定位方法，其利用车辆动力学模型对轮速计、转角传感器数据进行处理，有效考虑了车辆的侧偏现象，同时能够直接融合imu以及车辆动力学测量的原始数据，避免积分导致噪声放大，并在线校正车辆模型参数在行驶过程中的漂移，引入车辆的横向加速度约束，保证了车辆在复杂多样的运行环境中实现鲁棒的高精度自主定位。

2、本发明的目的是通过以下技术方案来实现的：一种基于b样条函数的激光雷达-惯性-车辆融合定位方法，包括以下步骤：

3、s1：采集激光雷达实时扫描点云、imu数据、轮速计读数以及转角传感器读数；进行车辆轨迹估计：利用两条b样条函数分别对车载imu的位置p以及姿态r进行建模，得到车载imu任意时间的位置、姿态和速度数据，进而得到imu运动轨迹；

4、s2：使用车辆动力学模型对s1中的轮速计读数以及转角传感器读数进行处理，得到车辆动力学测量，在线优化校正因子向量，对车辆动力学模型参数进行校正；

5、s3：在轨迹更新前最后一个控制点到最新加入的控制点之间的时间段，通过对s1中imu数据进行离散积分处理，得到该时间段内每个imu采样时刻对应的位姿集合以及速度集合；

6、通过对s2中的车辆动力学测量进行离散积分处理，得到该时间段内每个得到车辆动力学测量的时刻对应的位姿集合将离散时刻的所有位姿集合以及速度集合作为测量值，约束b样条函数在该离散时刻采样得到的位姿以及速度，构建第一优化问题，用于初始化新加入b样条的控制点；

7、s4：在车辆轨迹估计中加入车辆横向加速度约束，计算横向加速度约束残差；

8、s5：利用s1得到的每一帧激光雷达点云，与环境地图中提取的线特征以及面特征进行匹配，计算激光雷达点云残差；根据s1中的imu数据，计算imu残差；根据s2中的车辆动力学测量，计算车辆动力学测量残差；

9、s6：通过点云残差、imu残差、车辆动力学测量残差和横向加速度约束残差构建第二优化问题，以s3初始化后的新加入b样条的控制点为优化问题初值，同时优化s1中的样条函数控制点以及s2中的校正因子向量，得到用于定位的车辆实时位姿轨迹以及车辆模型参数校正值。

10、进一步地，所述s1中利用两条b样条函数分别对车载imu的位置p以及姿态r进行建模，具体为：

11、r(t)＝λ(t)dθ

12、p(t)＝λ(t)dt

13、其中，r(t)为表示姿态的b样条函数，p(t)为表示位置的b样条函数；λ(t)为样条基函数，dθ为与姿态b样条函数控制点相关的样条参数，dt为与位置b样条函数控制点相关的样条参数；对上述表示姿态的b样条函数进行一阶和二阶求导可得角速度以及角加速度：

14、

15、

16、对表示位置的b样条函数进行一阶和二阶求导可得线速度和线加速度：

17、

18、

19、上述描述姿态、角速度、角加速度以及位置、线速度、线加速度的b样条函数在t时刻的采样值。

20、进一步地，所述s2具体为：利用车辆的动力学模型，从轮速计读数以及转角传感器读数中得到车辆的滑移角α以及yaw角变化率

21、通过滑移角和yaw角变化率进而得到以车辆质心为坐标系原点的线速度和角速度：

22、

23、

24、加入待优化的车辆动力学模型参数校正因子向量[cv cα cω]，则有车辆动力学测量为：

25、

26、

27、

28、其中，cr和cf分别为车辆前轮和后轮的侧偏刚度，lr和lf分别为后轮车轴中心和前轮车轴中心到车辆质心的距离，l＝lr+lf；m为车辆质量；为车辆轮速计读数，δ为车辆转角传感器读数；为车辆动力学测量的线速度部分，为车辆动力学测量的角速度部分；cv为关于线速度大小的校正因子，cα为关于车辆滑移角的校正因子，cω为关于车辆角速度大小的校正因子。

29、进一步地，所述s3具体为：通过对imu数据进行离散积分处理，得到该时间段内每个imu采样时刻对应的位姿集合以及速度集合对车辆动力学测量进行离散积分处理，得到该时间段内每个车辆动力学测量获得时刻对应的位姿集合将递推得到的离散时刻位姿和速度作为测量值，构建的第一优化问题目标函数为：

30、

31、其中，φnew为新加入的控制点，为imu离散积分得到的位姿，为imu离散积分得到的速度；为车辆动力学测量离散积分得到位姿；为imu数据的采集时刻，为车辆动力学测量的获得时刻；为车载imu坐标系到车辆质心坐标系之间的外参，上角标t代表转置。

32、进一步地，所述s4中在车辆轨迹估计中加入车辆横向加速度约束，计算车辆横向加速度约束残差具体为：

33、

34、其中u＝[0 1 0]是选择矩阵，av(ts)为imu运动轨迹进行二阶求导后由imu坐标系转换到车辆质心坐标系的线加速度，为imu陀螺仪三轴角速度读数，vv(ts)为imu运动轨迹进行一阶求导后由imu坐标系转换到车辆质心坐标系的线速度，[·]∧为向量转换为其对应反对称矩阵的操作，[·]z为取z轴对应轴角变换率的操作；ts为设定的轨迹采样时间，与imu的采集时间一致。

35、进一步地，所述s5中车辆动力学测量残差具体为：构建激光雷达点云残差，利用原始imu三轴加速度以及三轴角速度测量与imu运动轨迹一阶和二阶导数直接构建imu残差，利用车辆动力学测量与所估计轨迹一阶导数直接构建车辆动力学残差，公式如下：

36、

37、

38、

39、其中，el、ei、ev分别为激光雷达点云残差、imu残差以及车辆动力学残差；为通过外参由世界坐标系下的imu轨迹转换到世界坐标系下的激光雷达的轨迹，为该帧激光雷达第k个采样点的采样时间，π为平面特征点到平面以及边缘特征点到直线的投影方程；为世界坐标系下的imu轨迹，ti为imu数据的采集时间，ωb(ti)为imu轨迹一阶求导后得到imu坐标系下的角速度，为imu的三轴角速度测量，ba和bg分别为imu加速度计以及陀螺仪的偏置；vv(tv),ωv(tv)分别为轨迹一阶求导后通过外参得到的车辆质心坐标系下的线速度和角速度，tv为车辆动力学测量的获得时间，cv为关于车辆动力学测量线速度大小的校正因子，cα为关于车辆动力学测量滑移角的校正因子，cω为关于车辆动力学测量角速度大小的校正因子。

40、进一步地，所述s6中第二优化问题的目标函数为：

41、

42、其中，优化变量为校正因子向量c＝[cv cα cω]以及轨迹的激活控制点集φ＝{φi,…,φi+k-1}，k为b样条函数的阶数；el、ei、ev、em分别为激光雷达点云残差、imu残差、车辆动力学残差以及车辆横向加速度约束残差；σl、σi、σv分别为激光雷达点云残差、imu残差、车辆动力学残差的权重矩阵，为σ权重矩阵所对应的马氏范数。

43、与现有技术相比，本发明的有益效果是：本发明采用车辆动力学模型对轮速计和转角传感器数据进行处理，考虑了车辆的侧偏现象，获得更为准确的动力学测量值；使用b样条函数对轨迹进行建模，利用其可在连续时间内采样且平滑可导的性质，直接融合imu以及车辆动力学测量的原始数据，避免积分导致噪声放大而影响定位精度；在线校正车辆模型参数在行驶过程中的漂移，减小了车辆载荷、行驶路面等因素变化给车辆动力学测量值带来的影响；引入车辆的横向加速度约束，为车辆的轨迹估计提供先验信息，有效提高车辆自主定位的鲁棒性和精度。