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

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

A vehicle navigation method in cooperative environment

【技术实现步骤摘要】
一种合作环境下的车辆导航方法
本专利技术属于自主导航与制导领域，尤其涉及一种合作环境下的车辆导航方法。
技术介绍
现代传感器技术的飞速发展，推进了汽车产业的发展和无人驾驶技术的进步。车辆在人们的日常生活变得越来越普遍。无人驾驶技术的首要前提是能够实现自身的精确定位。激光雷达是常用的导航传感器，通常使用同步定位与构图(SimultaneousLocaliza-tionandMapping，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)速度预测公式为：其中是vn(k)在x，y，z方向上的分量，vn(k)是k时刻机体系相对导航系的线速度在导航系下的表示；是vn(k-1)在x，y，z方向上的分量，vn(k-1)是k-1时刻机体系相对导航系的线速度在导航系下的表示；通过以下公式计算其中g为重力加速度；是an(k)在x，y，z方向上的分量，an(k)是k时刻机体系相对导航系的加速度在导航系下的表示，通过以下公式计算得到其中是k时刻机体系相对导航系的加速度在机体系下的表示；是机体系到导航系的转换矩阵，通过下式计算：其中q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)是k-1时刻的姿态四元数；3)位置预测公式为：其中，xn(k)，yn(k)，zn(k)是k时刻所述待导航车辆在导航系下的三个方向位置；xn(k-1)，yn(k-1)，zn(k-1)是k-1时刻所述待导航车辆在导航系下的三个方向位置。进一步的，所述S3包括如下步骤：S31、根据所述激光雷达信息中相邻激光点之间的距离，对所述当前时刻已知结构体的点云进行分割，所述激光雷达信息为S(k)；计算S(k)中相邻激光点之间的距离D式中，ρ(i)，ρ(i+1)是激光雷达第i个和第i+1个有效点，D(ρ(i)，ρ(i+1))是相邻两点之间的距离，Dth是设置的断点距离阈值。如果D(ρ(i)，ρ(i+1))大于Dth，则将ρ(i)、ρ(i+1)标记为断点。将断点分割完成的每一部分点云标记为Sd，1≤d≤ND，ND是断点分割点云群的数量；S32、对点云Sd进行角点检测，根据点云Sd的两端点A0(x0，y0)、B0(x1，y1)求解直线方程L；然后计算点云Sd中各点到直线L的距离dj(j＝0,1,2,…)，在距离dj(j＝0,1,2,…)的集合中求出最大值dmax，如果dmax大于设定的角点距离阈值，角点距离阈值的计算公式为则将dmax所对应的点标记为角点，反之，待检测部分点云不存在角点。其中rmax是激光雷达的最大测量距离，θ是激光雷达的分辨率。将根据所述角点和断点分割的点云数据记为Sm，1≤m≤NM，NM是角点点云群的数量。S33、结构体匹配a)将点云Sd内的点求取平均值；b)根据所述待导航车辆在导航系下的位置以及已知结构体在导航系下的坐标，求解出待导航车辆与各个已知结构体之间的距离，并根据该距离信息，筛选出符合要求的点云数据geari_m(j)，i表征第i个结构体，i为正整数，j表征符合待导航车辆到第i个结构体距离要求的第j个点，j＝1,2,3；c)根据已知结构体在导航系下的位置求解出已知结构体彼此间的距离信息，并将其与geari_m(j)各点之间的距离进行比较，从geari_m(j)中选出符合距离信息相差小于距离阈值次数最多的点，标记为geari_m，距离阈值根据结构体的大小来进行确定；d)计算geari_m中各点之间的距离，并与已知结构体彼此间的距离信息确定是否符合距离信息相近的要求；e)根据所述已知物体的位置信息，推算其他物体的位置信息，确认在点云数据中是否存在其他结构体。进一步的，所述S4具体步骤包括：S41、采用最小二乘法对分割完成的点云数据Sm进行直线拟合，目标函数为amx+bmy-1＝0，am，bm为参数变量；将qim标记为Sm的第i个激光点，i＝1，2，3，...，Nm，Nm是Sm中激光点的数量，xim，yim是qim在机体系下的坐标，参数am，bm的计算公式如下：S42、对拟合得到的直线进行方向判定，判定方法包括两种：a)当已知结构体有两个或者三个，根据不同结构体上的直线方程，确定平行的直线，再根据平行直线间的距离判断所述直线位于结构体的那一侧；根据两条直线的夹角ang来判断两条直线是否平行，ang的计算方法为：其中am，bm是第m条直线Lm的x，y系数；an，bn是第n条直线Ln的x，y系数；如果ang的绝对值小于5°，则认为这两条直线是平行线；若直线Lm和Ln平行，则计算直线Lm上的任一激光点到直线L本文档来自技高网...

【技术保护点】
1.一种合作环境下的车辆导航方法，其特征在于，包括步骤如下：/nS1、采集当前时刻待导航车辆的车载传感器信息，包括激光雷达点云信息、陀螺仪信息、加速度计信息、里程计信息、惯性传感器信息；/nS2、根据所述惯性传感器信息，根据扩展卡尔曼滤波算法，预测当前时刻待导航车辆的姿态、速度和位置；/nS3、根据已知结构体的导航系位置、上一时刻待导航车辆的位置，对当前时刻已知结构体的点云信息进行辨识，得到当前时刻已知结构体的机体系位置；/nS4、根据所述当前时刻已知结构体的机体系位置，通过多点定位方法，得到待导航车辆的导航系位置；根据初始时刻和k时刻已知结构体的机体系坐标变换，得到待导航车辆变化后的姿态；/nS5、根据所述待导航车辆的导航系位置、所述待导航车辆变化后的姿态、待导航车辆的速度、航向，通过卡尔曼滤波器对待导航车辆的速度、位置、姿态进行矫正；/nS6、矫正后进入下一采样周期，循环执行S1-S5。/n

【技术特征摘要】
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)速度预测公式为：



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

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

通过以下公式计算



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



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



其中q0(k-1)、q1(k-1)、q2(k-1)、q3(k-1)是k-1时刻的姿态四元数；
3)位置预测公式为：









其中，xn(k)，yn(k)，zn(k)是k时刻所述待导航车辆在导航系下的三个方向位置；
xn(k-1)，yn(k-1)，zn(k-1)是k-1时刻所述待导航车辆在导航系下的三个方向位置。


3.根据权利要求1所述的一种合作环境下的车辆导航方法，其特征在于，所述S3包括如下步骤：
S31、根据所述激光雷达信息中相邻激光点之间的距离，对所述当前时刻已知结构体的点云进行分割，所述激光雷达信息为S(k)；
计算S(k)中相邻激光点之间的距离D



式中，ρ(i)、ρ(i+1)是激光雷达第i个和第i+1个有效点，D(ρ(i)，ρ(i+1))是相邻两点之间的距离，Dth是设置的断点距离阈值；
如果D(ρ(i)，ρ(i+1))大于Dth，将ρ(i)、ρ(i+1)标记为断点，将断点分割完成的每一部分点云标记为Sd，1≤d≤ND，ND是断点分割点云群的数量；
S32、对点云Sd进行角点检测，根据点云Sd的两端点A0(x0，y0)、B0(x1，y1)求解直线方程L；



然后计算点云Sd中各点到直线L的距离dj(j＝0,1,2,…)，在距离dj(j＝0,1,2,…)的集合中求出最大值dmax，如果dmax大于设定的角点距离阈值，角点距离阈值的计算公式为则将dmax所对应的点标记为角点，反之，待检测部分点云不存在角点；其中rmax是激光雷达的最大测量距离，θ是激光雷达的分辨率,将根据所述角点和断点分割的点云数据记为Sm，1≤m≤NM，NM是角点点云群的数量；
S33、结构体匹配
a)将点云Sd内的点求取平均值；
b)根据所述待导航车辆在导航系下的位置以及所述已知结构体在导航系下的坐标，求解出所述待导航车辆与各个所述已知结构体之间的距离，并根据所述距离信息，筛选出符合要求的点云数据geari_m(j)，i表征第i个结构体，i为正整数，j表征符合所述待导航车辆到第i个结构体距离要求的第j个点，j＝1,2,3；
c)根据所述已知结构体在导航系下的位置求解出所述已知结构体彼此间的距离信息，并与geari_m(j)各点之间的距离进行比较，从geari_m(j)中选出符合距离信息相差小于距...

【专利技术属性】
技术研发人员：赖际舟，付相可，吕品，何洪磊，岑益挺，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏;32