[发明专利]一种合作环境下的车辆导航方法

说明书

本发明公开了一种合作环境下的车辆导航方法，属于自主导航与制导领域。本发明包括：对激光雷达的点云数据进行预处理，完成对有效点的提取，点云的聚类以及角点的判断；将聚类的点云进行识别，完成对结构体的辨别，并根据结构体的形状特征，求解出结构体的中心点；利用结构体的位置信息，求解出车辆的位置；最后进行基于惯导/里程计/lidar的融合算法研究，提高了车辆导航定位的鲁棒性和准确性。相对于传统的激光雷达SLAM算法，本发明公开的方法可以有效提高在点云稀疏环境下车辆的导航定位精度。

技术领域

本发明属于自主导航与制导领域，尤其涉及一种合作环境下的车辆导航方法。

背景技术

现代传感器技术的飞速发展，推进了汽车产业的发展和无人驾驶技术的进步。车辆在人们的日常生活变得越来越普遍。无人驾驶技术的首要前提是能够实现自身的精确定位。激光雷达是常用的导航传感器，通常使用同步定位与构图(Simultaneous Localiza-tion and Mapping，SLAM)技术来进行定位。但是在结构特征稀少、点云信息不丰富的环境中，SLAM方法匹配误差难，不能够实现精确定位。

合作环境是指结构特征稀少、点云信息不丰富，但结构特征已知的实验环境。在该环境下激光雷达通过SLAM估计车辆的位姿，会出现匹配误差大，精度低的问题，不能满足定位的精度需求。因此需要一种新的定位方法满足车辆在合作环境下的精准定位问题。

发明内容

针对于上述现有技术的不足，本发明的目的在于提供一种合作环境下的车辆导航方法，以解决车辆在合作环境下的精准定位问题。为达到上述目的，本发明的技术方案为：

一种合作环境下的车辆导航方法，包括步骤如下：

S1、采集当前时刻待导航车辆的车载传感器信息，包括激光雷达点云信息、陀螺仪信息、加速度计信息、里程计信息、惯性传感器信息；

S2、根据所述惯性传感器信息，根据扩展卡尔曼滤波算法，预测当前时刻待导航车辆的姿态、速度和位置；

S3、根据已知结构体的导航系位置、所述上一时刻待导航车辆的位置，对当前时刻已知结构体的点云信息进行辨识，得到当前时刻已知结构体的机体系位置；

S4、根据所述当前时刻已知结构体的机体系位置，通过多点定位方法，得到待导航车辆的导航系位置；根据初始时刻和k时刻已知结构体的机体系坐标变换，得到待导航车辆变化后的姿态；

S5、根据所述待导航车辆的导航系位置、所述待导航车辆变化后的姿态、待导航车辆的速度、航向，通过卡尔曼滤波器对待导航车辆的速度、位置、姿态进行矫正；

S6、矫正后进入下一采样周期，循环执行S1-S5。

进一步的，在所述S2中，所述预测当前时刻待导航车辆的姿态、速度和位置采用如下公式：

1)姿态四元数预测公式为：

其中，k时刻为所述当前时刻，Q(k)＝[q0(k)，q1(k)，q2(k)，q3(k)]是k时刻的四元数；Q(k-1)＝[q0(k-1)，q1(k-1)，q2(k-1)，q3(k-1)]是k-1时刻的四元数，k-1时刻即所述当前时刻的前一采样时刻；上标T表示矩阵的转置；Δt是离散采样周期；Ω(k)为中间变量，通过以下公式计算：

其中是在x，y，z方向上的分量，是机体系相对导航系的角速度在机体系下的表示；

2)速度预测公式为：

其中是vⁿ(k)在x，y，z方向上的分量，vⁿ(k)是k时刻机体系相对导航系的线速度在导航系下的表示；