CN110749327A - 一种合作环境下的车辆导航方法 - Google Patents

Abstract

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

Description

一种合作环境下的车辆导航方法

技术领域

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

背景技术

现代传感器技术的飞速发展，推进了汽车产业的发展和无人驾驶技术的进步。车辆 在人们的日常生活变得越来越普遍。无人驾驶技术的首要前提是能够实现自身的精确定 位。激光雷达是常用的导航传感器，通常使用同步定位与构图(SimultaneousLocaliza-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时刻机体系相 对导航系的线速度在导航系下的表示；

是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包括如下步骤：

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具体步骤包括：

S41、采用最小二乘法对分割完成的点云数据S_m进行直线拟合，目标函数为 a_mx+b_my-1＝0，a_m，b_m为参数变量；

将q_im标记为S_m的第i个激光点，i＝1，2，3，...，N_m，N_m是S_m中激光点的数量， x_im，y_im是q_im在机体系下的坐标，参数a_m，b_m的计算公式如下：

S42、对拟合得到的直线进行方向判定，判定方法包括两种：

a)当已知结构体有两个或者三个，根据不同结构体上的直线方程，确定平行的直线，再根据平行直线间的距离判断所述直线位于结构体的那一侧；

根据两条直线的夹角ang来判断两条直线是否平行，ang的计算方法为：

其中a_m，b_m是第m条直线L_m的x，y系数；a_n，b_n是第n条直线L_n的x，y系数；如果 ang的绝对值小于5°，则认为这两条直线是平行线；

若直线L_m和L_n平行，则计算直线L_m上的任一激光点到直线L_n的距离，然后根据直线距离判断直线方向；

b)根据已知结构体不同方向的长度来进行判断，点云S_d的两端点A₀(x₀，y₀)、 B₀(x₁，y₁)之间的线段长度为d＝sqrt((x₀-x₁)²+(y₀-y₁)²)，如果线段长度d大于线 段长度阈值，则认为线段是结构体较长的边。线段长度阈值是根据结构体短边的长度确 定的，之后再根据结构体长短边的方位确定直线方向。

S43、车辆位置解算，根据所述已知结构体的形状信息，对拟合得到的直线进行平移，求取直线交点；

直线平移的方程如下，

C_i＝1-b_i/fabs(b_i)*h_i*sqrt(a_i*a_i+b_i*b_i)；

A_i＝a_i；B_i＝b_i；

其中，a_i和b_i是第i条直线L_i的x，y系数；A_i、B_i和C_i是平移后的直线参数；h_i是 直线的平移量；

直线交点的求解方法为：

x_n＝(B_i*C_j-B_j*C_i)/(A_i*B_j-A_j*B_i)；

其中，A_i、B_i和C_i是第i条直线平移后的直线参数；A_j、B_j和C_j是第j条直线平移后 的直线参数；第i条直线和第j条直线是同一结构体上的不同方向的两条直线；

S44、车辆姿态解算，根据初始时刻所述已知结构体在机体系下坐标

以及k 时刻已知结构体在机体系下的坐标

以及k时刻待导航车辆相对于初始时刻的平 移量

求解待导航车辆的状态变化，得到k时刻待导航车辆变化后的航向角ψ_L：

进一步的，所述S5包括如下步骤：

S51、一步预测均方误差P_k|k-1的计算公式如下：

上式中，Φ是状态转移矩阵，Φ_k，k-1是k-1到k时刻的状态转移矩阵；Γ_k-1是k-1时刻滤波器的噪声矩阵，W_k-1是k-1时刻系统的噪声矩阵；

上式中M_4×4，U_4×3，N_3×4均为中间变量，计算方法如下式所示:

其中，

是k-1时刻机体系到导航系的坐标转换矩阵，

分别是

的第一、二、三行；Δt是系统的离散时间；I是单位矩阵。

滤波器的噪声矩阵Γ的计算方法如下式所示：

系统的噪声矩阵

其中，ε_wx，ε_wy，ε_wz分别是

的模型噪声；ε_ax，ε_ay，ε_az分别是

的模型噪声；

分别是

的噪声标准差；

分别是

的噪声标准差；

S52、系统的滤波增益方程的计算方法如下：

其中，K_k是k时刻系统的滤波增益，R_k是量测噪声，H_k是量测矩阵；

当量测信息为激光雷达时，量测方程和量测噪声分别为：

diag表示矩阵对角化，其中

分别为xⁿ,yⁿ,ψ_L的噪声；xⁿ,yⁿ,ψ_L分别为激光雷达估计的车辆x、y方向坐标以及偏航角，计算方法如下：

当量测信息为里程计时，量测方程和量测噪声分别为

其中分别为

ψ_O的噪声；

S53、系统的状态估值方程计算方法如下

式中，

为k时刻状态量的估计值，

为k-1到k时刻的状态变量一步预测值；

Z_k为待导航车辆在k时刻的量测值；当量测信息来源为激光雷达时，Z_k＝[xⁿ yⁿψ_L]；

当量测信息来源为里程计时

S54、系统的估计均方误差方程为：

P_k|k＝(I-K_kH_k)P_k|k-1

式中，P_k|k为k时刻估计均方误差，I为单位矩阵。

本发明的有益效果：

与现有技术相比，本发明能够在合作环境下，完成对车辆的位置、速度、姿态的解算。相比传统的二维激光雷达SLAM方法，本专利提出的算法的导航精度能够得到大幅 度提成。传统的二维激光雷达SLAM方法在该环境下的定位下x，y方向均方根误差为 2.864m，6.594m；而本专利所提算法能达到0.123m和0.062m。

附图说明

图1为本发明的流程图；

图2为仿真平台示意图；

图3为仿真结果轨迹图；

图4为仿真结果中x方向误差；

图5是仿真结果中y方向误差。

具体实施方式

为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。

本发明实施例提供了一种合作环境下的车辆导航方法，流程图如图1所示，包括步骤如下：

步骤一：周期读取k时刻车载传感器信息，包括激光雷达信息S(k)，陀螺仪信息

加速度计信息

里程计信息v_odom(k)，k时刻为当前时刻。

步骤二：通过惯性传感器预测k时刻车辆的姿态、速度和位置。

1)姿态四元数预测采用如下公式

其中，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时刻的四元数；上标T表 示矩阵的转置；Δt是离散采样周期；Ω(k)为中间变量，通过以下公式计算

其中

是

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

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时刻车辆在导航系下的三个方向位置。

步骤三：根据已知结构体在导航系下的位置信息以及k时刻车辆的姿态和位置对k时刻的点云信息进行辨识，确定k时刻已知结构体的机体系下的位置。

1)点云分割

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

式中，ρ(i)、ρ(i+1)是激光雷达第i个和第i+1个有效点，D(ρ(i)，ρ(i+1))是相邻两点 之间的距离，D_th是设置的阈值，其大小根据结构体之间的距离来进行确定。把分割完 成的每一部分点云记为S_d，1≤d≤N_D，N_D是断点分割点云群的数量。

2)角点检测

因为本专利所使用结构体为长方体，在激光雷达扫描得到的点云数据中，每一部分 最多只存在一个角点，可以根据点到直线的距离来判断该部分点云是否存在角点。首先根据根据该部分点云的两端点A₀(x₀，y₀)、B₀(x₁，y₁)求解直线方程L。

之后计算该部分各点到直线L的距离d_j(j＝0,1,2,…)，得到最大值d_max。如果d_max大于阈 值，则该点为角点；反之，该部分不存在角点。将根据角点和断点分割的点云数据记为 S_m，1≤m≤N_M，N_M是角点点云群的数量

3)结构体匹配

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

b)根据车辆在导航系下的位置筛选出可能是已知几何结构的点云数据gear_i_m(j) (gear_i_m(j)为第i个物体，其中j＝1,2,3,)；

c)计算gear_i_m之间的距离与已知结构体的信息进行比较，分别从gear_i_m(j)中选出 符合距离信息相近次数最多的点gear_i_m；

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

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

步骤四：根据已知的计算k时刻已知结构体的机体系下的位置，通过多点定位方法， 确定待导航车辆在其导航系下的位置信息，同时通过已知结构体在机体系下的坐标变换，求解出车辆的姿态变化。

1)直线拟合

本专利是通过最小二乘法进行直线拟合的，其目标函数为a_mx+b_my-1＝0。记q_im是S_m的第i个激光点(i＝1，2，3，...，N_m)，N_m是S_m中激光点的数量，x_im，y_im是q_im在机体系下的坐标，参数a_m，b_m的计算公式如下：

2)直线方向判定

因为在车辆的巡检过程中会存在以下两种情况：激光雷达只能扫描到一个结构体； 根据点云拟合的线段只是结构体的一侧的部分信息。所以设计了两种直线方向判定方法。针对不同的情况

a)当可匹配的已知结构体有两个或者三个。根据不同结构体上的直线方程，确定平行的直线，再根据平行直线间的距离判断这条直线属于结构体的哪一侧。判断两条直 线是否平行是根据两条直线的夹角ang来进行确定的。

ang的计算方法如下式所示。

其中a_m，b_m是第m条直线L_m的x，y系数；a_n，b_n是第n条直线L_n的x，y系数。

如果ang的绝对值小于5°，则认为这两条直线是平行线。然后计算直线L_m上的任一激光点到直线L_n的距离，然后根据直线距离判断直线方向。

b)根据结构体不同方向的长度来进行判断的。

根据该线段的两端点A₀(x₀，y₀)、B₀(x₁，y₁)，计算线段的长度 d＝sqrt((x₀-x₁)²+(y₀-y₁)²)，如果直线的长度大于阈值，则认为其是较长的边。

3)车辆位置解算

根据已知结构体的形状信息，对直线进行平移，求取直线交点。直线平移的方程如下，其中a_i和b_i是第i条直线L_i的x，y系数；A_i、B_i和C_i是平移后的直线参数；h_i是 直线的平移量。

C_i＝1-b_i/fabs(b_i)*h_i*sqrt(a_i*a_i+b_i*b_i)；

A_i＝a_i；

B_i＝b_i；

直线交点的求解方法为：

x_n＝(B_i*C_j-B_j*C_i)/(A_i*B_j-A_j*B_i)；

其中A_i、B_i和C_i是第i条直线平移后的直线参数；A_j、B_j和C_j是第j条直线平移 后的直线参数；第i条直线和第j条直线是同一结构体上的不同方向的两条直线。

4)车辆姿态解算

根据初始时刻所述已知结构体在机体系下坐标

以及k时刻已知结构体在 机体系下坐标

以及k时刻车辆相当于初始时刻的平移量

求解车辆的状态变化，得到k时刻所述待导航车辆变化后的航向角ψ_L：

步骤五：以激光雷达解算的位姿以及里程计的速度和航向角信息作为量测量，通过 卡尔曼滤波器对车辆的速度、位置、姿态进行校正。

一步预测均方误差P_k|k-1的计算公式如下：

上式中，Φ是状态转移矩阵，Φ_k，k-1是k-1到k时刻的状态转移矩阵，Γ_k-1是k-1时刻滤波器的噪声矩阵，W_k-1是k-1时刻系统的噪声矩阵：

上式中M_4×4，U_4×3，N_3×4N_3×4均为中间变量，计算方法下式所示:

其中是k-1时刻机体系到导航系系的坐标转换矩阵，

分别是

的第一、二、三行，Δt是系统的离散时间；I是 单位矩阵。滤波器的噪声矩阵Γ的计算方法如下式所示：

系统的噪声矩阵

其中，ε_wx，ε_wy，ε_wz分别是

的模型噪声；ε_ax，ε_ay，ε_az分别是

的模型噪声；分别是

的噪声标准差；

分别是

的噪声标准差。

b)系统的滤波增益方程的计算方法如下

其中，K_k是k时刻系统的滤波增益，R_k是量测噪声，H_k是量测矩阵，根据量测信息的不同选择不同的量测模型。当量测信息为激光雷达时，量测方程和量测噪声分别为

diag表示矩阵对角化，其中

分别为xⁿ,yⁿ,ψ_L的噪声；xⁿ,yⁿ,ψ_L分别为激光雷达估计的车辆x、y方向坐标以及偏航角，计算方法如下：

当量测信息为里程计时，量测方程和量测噪声分别为

其中

分别为

的噪声。

c)系统的状态估值方程计算方法如下

式中，为k时刻状态量的估计值，

为k-1到k时刻的状态变量一步预测值；

Z_k为待导航车辆在k时刻的量测值；当量测信息来源为激光雷达时，Z_k＝[xⁿ yⁿψ_L]； 当量测信息来源为里程计时

d)系统的估计均方误差方程

P_k|k＝(I-K_kH_k)P_k|k-1

式中，P_k|k为k时刻估计均方误差，I为单位矩阵。

仿真试验：

在仿真实验中，各传感器的参数设置如下：

1)激光雷达的探测距离是25m，扫描角度是-180°～180°，角度分辨率是0.25°；

2)加速度计的精度为0.002g，陀螺仪的精度为0.02°/s；

3)里程计的精度为0.6％。

仿真平台如图2所示，图4是仿真结果轨迹，图5为仿真结果的x，y方向误差。 从图4、5中，我们可以得出本专利所提出的算法相对传统二位激光雷达SLAM算法精度 大幅度提升。传统的二维激光雷达SLAM方法在该环境下的定位下x，y方向均方根误 差达到了2.864m，6.594m，无法满足定位需求，而本专利所提算法只有0.123m和0.062m， 可以保证车辆的精确导航。

Claims (5)

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

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

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

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

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

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

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

2.根据权利要求1所述的一种合作环境下的车辆导航方法，其特征在于，在所述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时刻所述待导航车辆在导航系下的三个方向位置。

3.根据权利要求1所述的一种合作环境下的车辆导航方法，其特征在于，所述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)根据所述已知物体的位置信息，推算其他物体的位置信息，确认在点云数据中是否存在其他结构体。

4.根据权利要求3所述的一种合作环境下的车辆导航方法，其特征在于，所述S4具体步骤包括：

S41、采用最小二乘法对分割完成的点云数据S_m进行直线拟合，目标函数为a_mx+b_my-1＝0，a_m，b_m为参数变量；

将q_im标记为S_m的第i个激光点，i＝1，2，3，...，N_m，N_m是S_m中激光点的数量，x_im，y_im是q_im在机体系下的坐标，参数a_m，b_m的计算公式如下：

S42、对拟合得到的直线进行方向判定，判定方法包括两种：

a)当所述已知结构体的数量有两个或者三个，根据不同结构体上的直线方程，确定平行的直线，再根据平行直线间的距离判断所述直线位于结构体的那一侧；

根据两条直线的夹角ang来判断两条直线是否平行，ang的计算方法为：

其中a_m，b_m是第m条直线L_m的x，y系数；a_n，b_n是第n条直线L_n的x，y系数；如果ang的绝对值小于5°，则认为这两条直线是平行线；

若直线L_m和L_n平行，则计算直线L_m上的任一激光点到直线L_n的距离，然后根据直线距离判断直线方向；

b)根据所述已知结构体不同方向的长度来进行判断，所述点云S_d的两端点A₀(x₀，y₀)、B₀(x₁，y₁)之间的线段长度为d＝sqrt((x₀-x₁)²+(y₀-y₁)²)，如果所述线段长度d大于线段长度阈值，则认为所述线段是所述已知结构体中较长的边；

S43、车辆位置解算，根据所述已知结构体的形状信息，对拟合得到的直线进行平移，求取直线交点；

直线平移的方程如下，

C_i＝1-b_i/fabs(b_i)*h_i*sqrt(a_i*a_i+b_i*b_i)；

A_i＝a_i；B_i＝b_i；

其中，a_i和b_i是第i条直线L_i的x，y系数；A_i、B_i和C_i是平移后的直线参数；h_i是直线的平移量；

直线交点的求解方法为：

x_n＝(B_i*C_j-B_j*C_i)/(A_i*B_j-A_j*B_i)；

其中，A_i、B_i和C_i是第i条直线平移后的直线参数；A_j、B_j和C_j是第j条直线平移后的直线参数；第i条直线和第j条直线是同一结构体上的不同方向的两条直线；

S44、车辆姿态解算，根据初始时刻所述已知结构体在机体系下坐标以及k时刻所述已知结构体在机体系下的坐标

以及k时刻所述待导航车辆相对于初始时刻的平移量求解所述待导航车辆的状态变化，得到k时刻所述待导航车辆变化后的航向角ψ_L：

5.根据权利要求2所述的一种合作环境下的车辆导航方法，其特征在于，所述S5包括如下步骤：

S51、一步预测均方误差P_k|k-1的计算公式如下：

上式中，Φ是状态转移矩阵，Φ_k，k-1是k-1到k时刻的状态转移矩阵，Γ_k-1是k-1时刻滤波器的噪声矩阵，W_k-1是k-1时刻系统的噪声矩阵；

上式中M_4×4，U_4×3，N_3×4均为中间变量，计算方法如下式所示:

其中，是k-1时刻机体系到导航系的坐标转换矩阵，

分别是

的第一、二、三行；Δt是系统的离散时间；I是单位矩阵。

滤波器的噪声矩阵Γ的计算方法如下式所示：

系统的噪声矩阵

其中，ε_wx，ε_wy，ε_wz分别是

的模型噪声；ε_ax，ε_ay，ε_az分别是

的模型噪声；分别是

的噪声标准差；

分别是

的噪声标准差；

S52、系统的滤波增益方程的计算方法如下：

其中，K_k是k时刻系统的滤波增益，R_k是量测噪声，H_k是量测矩阵；

当量测信息为激光雷达时，量测方程和量测噪声分别为：

diag表示矩阵对角化，其中

分别为xⁿ,yⁿ,ψ_L的噪声；xⁿ,yⁿ,ψ_L分别为激光雷达估计的车辆x、y方向坐标以及偏航角，计算方法如下：

当量测信息为里程计时，量测方程和量测噪声分别为

其中

分别为

ψ_O的噪声；

S53、系统的状态估值方程计算方法如下

式中，

为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值；Z_k为待导航车辆在k时刻的量测值；当量测信息来源为激光雷达时，Z_k＝[xⁿ yⁿ ψ_L]；当量测信息来源为里程计时

S54、系统的估计均方误差方程为：

P_k|k＝(I-K_kH_k)P_k|k-1

式中，P_k|k为k时刻估计均方误差，I为单位矩阵。

Images