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

权利要求书

1.一种合作环境下的车辆导航方法，其特征在于，包括步骤如下：

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

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

在所述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方向上的分量，是机体系相对导航系的角速度在机体系下的表示，Ω(k)为中间变量；

2)速度预测公式为：

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

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

通过以下公式计算

其中g为重力加速度；是aⁿ(k)在x，y，z方向上的分量，aⁿ(k)是k时刻机体系相对导航系的加速度在导航系下的表示，通过以下公式计算得到

其中是k时刻机体系相对导航系的加速度在机体系下的表示；是机体系到导航系的转换矩阵，通过下式计算：

其中q₀(k-1)、q₁(k-1)、q₂(k-1)、q₃(k-1)是k-1时刻的姿态四元数；

3)位置预测公式为：

其中，xⁿ(k),y ⁿ(k),zⁿ(k)是k时刻所述待导航车辆在导航系下的三个方向位置；xⁿ(k-1),y ⁿ(k-1),zⁿ(k-1)是k-1时刻所述待导航车辆在导航系下的三个方向位置；

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

所述S3包括如下步骤：

S31、根据所述激光雷达信息中相邻激光点之间的距离，对所述当前时刻已知结构体的点云进行分割，所述激光雷达信息为S(k)；

计算S(k)中相邻激光点之间的距离D

式中，ρ(i)、ρ(i+1)是激光雷达第i个和第i+1个有效点，D(ρ(i),ρ(i+1))是相邻两点之间的距离，D_th是设置的断点距离阈值；

如果D(ρ(i),ρ(i+1))大于D_th，将ρ(i)、ρ(i+1)标记为断点，将断点分割完成的每一部分点云标记为S_d，1≤d≤N_D，N_D是断点分割点云群的数量；

S32、对点云S_d进行角点检测，根据点云S_d的两端点A₀(x₀,y₀)、B₀(x₁,y₁)求解直线方程L；

然后计算点云S_d中各点到直线L的距离d_j(j＝0,1,2,…)，在距离d_j(j＝0,1,2,…)的集合中求出最大值d_max，如果d_max大于设定的角点距离阈值，角点距离阈值的计算公式为则将d_max所对应的点标记为角点，反之，待检测部分点云不存在角点；其中r_max是激光雷达的最大测量距离，θ是激光雷达的分辨率,将根据所述角点和断点分割的点云数据记为S_m，1≤m≤N_M，N_M是角点点云群的数量；

S33、结构体匹配

a)将点云S_d内的点求取平均值；

b)根据所述待导航车辆在导航系下的位置以及所述已知结构体在导航系下的坐标，求解出所述待导航车辆与各个所述已知结构体之间的距离，并根据所述距离信息，筛选出符合要求的点云数据gear_i_m(j)，i表征第i个结构体，i为正整数，j表征符合所述待导航车辆到第i个结构体距离要求的第j个点，j＝1,2,3；

c)根据所述已知结构体在导航系下的位置求解出所述已知结构体彼此间的距离信息，并与gear_i_m(j)各点之间的距离进行比较，从gear_i_m(j)中选出符合距离信息相差小于距离阈值次数最多的点，标记为gear_i_m；

d)计算gear_i_m中各点之间的距离，并与所述已知结构体彼此间的距离信息确定是否符合距离信息相近的要求；

e)根据所述已知物体的位置信息，推算其他物体的位置信息，确认在点云数据中是否存在其他结构体；

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

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

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