一种智能车辆定位与回环检测方法技术

本发明专利技术提供了一种智能车辆定位与回环检测方法，以激光雷达测量障碍物相对于本车的位置和距离，实现对周围场景的感知，利用基于联合概率密度分布的正态分布变换方法和基于空间点拟合的迭代最近点方法分步解算智能车辆当前位置和姿态，通过基于雷达点云的闭环检测方法，在智能车辆通过重复场景时使用上一次通过时的对周围环境的观测校正当前智能车辆的位姿，避免行驶过程中定位误差的积累，本方法使用激光雷达传感器数据作为定位方法的输入，该种传感器广泛存在于智能车辆上，具有良好的可实现性；分步完成智能车辆的定位任务，对两种典型算法取长补短，同时保证智能车辆的定位精度和效率，消除行进过程中累积的误差。

An intelligent vehicle location and loop detection method

【技术实现步骤摘要】
一种智能车辆定位与回环检测方法
本专利技术属于智能车辆状态估计领域，涉及一种智能车辆定位与回环检测方法，更加具体来讲，涉及一种智能车辆分步位姿估计和基于激光雷达点云的回环检测方法。
技术介绍
近年来智能车辆技术飞速发展，现有框架一般将智能车辆分为感知、规划、决策和控制四个模块，解决上述问题的一个关键前提是获取智能车辆的精确定位，只有确定了车身的位置和姿态信息，才能将感知到的周车及障碍物相信息统一在同一坐标系下，进而为后续模块提供输入信息。目前智能车辆定位手段主要依赖于惯导和GPS卫星定位，然而惯导在单独使用时会由于传感器自身特性产生累计误差，且高精度惯导设备成本较高，GPS卫星定位技术易受到外部信号干扰，即使是短时间内的信号丢失也会对智能车辆行车安全构成巨大威胁。利用智能车辆上装载的激光雷达进行实时定位是一种切实可行的方案，其面临的问题是如何在定位精度和计算效率上进行取舍，同时消除导航定位误差的累积是激光雷达定位必须解决的问题。
技术实现思路
本专利技术为了解决智能车辆定位及误差积累问题，提供了一种智能车辆定位与回环本文档来自技高网...

【技术保护点】
1.一种智能车辆定位与回环检测方法，在全球定位系统信号不能持续稳定输入车辆定位系统时，以激光雷达数据为输入，激光雷达安装于智能车顶端，通过收发激光束测量障碍物相对于本车的位置和距离，实现对周围场景的感知，利用基于联合概率密度分布的正态分布变换方法和基于空间点拟合的迭代最近点方法分步解算智能车辆当前位置和姿态，保证车身的定位精度和效率，其特征在于，本方法具体步骤如下：/n步骤一、基于正态分布变换的点云粗略配准：/n激光雷达在每个工作周期内采集到周围物体反射的一系列的离散点，定义这些离散点为扫描点，扫描点包含位置信息、距离信息和反射率信息，定义所有扫描点的集群为点云，且上一时刻采集的点云定义为参考...

【技术特征摘要】
1.一种智能车辆定位与回环检测方法，在全球定位系统信号不能持续稳定输入车辆定位系统时，以激光雷达数据为输入，激光雷达安装于智能车顶端，通过收发激光束测量障碍物相对于本车的位置和距离，实现对周围场景的感知，利用基于联合概率密度分布的正态分布变换方法和基于空间点拟合的迭代最近点方法分步解算智能车辆当前位置和姿态，保证车身的定位精度和效率，其特征在于，本方法具体步骤如下：
步骤一、基于正态分布变换的点云粗略配准：
激光雷达在每个工作周期内采集到周围物体反射的一系列的离散点，定义这些离散点为扫描点，扫描点包含位置信息、距离信息和反射率信息，定义所有扫描点的集群为点云，且上一时刻采集的点云定义为参考点云；
使用正太分布变换方法进行点云配准的目的是求取车辆的旋转和位移，使当前时刻采集的点云以最大的概率投影到参考点云上；首先将对当前时刻采集的点云进行网格化，用等尺寸立方体分割点云片段并计算每个立方体的概率密度函数，步骤如下：



式中为立方体中所有扫描点的均值，m为立方体中扫描点的数量，表示一个立方体内的单个扫描点；基于此，每个立方体中扫描点的协方差矩阵为：



式中cov为描述立方体内扫描点分散程度的协方差矩阵；均值和协方差矩阵cov描述了扫描点的概率密度函数，服从正太分布：



式中，f(x)为概率密度函数，此时存在坐标变换函数表示使用位姿变换旋转和位移扫描点变换参数的最优解通过最大似然函数表示为：



对于此类优化问题，取负对数形式：



针对上式求解问题，在车辆坐标系下使用欧拉角旋转和向量位移的方式构建位姿变换参数车辆坐标系定义为：以车辆动力学模型中车辆质心为坐标原点，沿着车辆前进的方向为x轴的正方向，x轴沿逆时针方向旋转90度的方向为y轴，垂直于x-y平面方向为z轴的坐标系；
具体的，令φx表示绕x轴旋转的欧拉角，φy表示绕y轴旋转的欧拉角，φz表示绕z轴旋转的欧拉角，tx表示沿x轴方向的位移，ty表示沿y轴方向的位移，tz表示沿z轴方向的位移，坐标变换函数可表示为：



式中，Rx表示绕x轴的旋转矩阵，Ry表示绕y轴的旋转矩阵，Rz表示绕z轴的旋转矩阵，以为变量，对上式求导可得雅可比矩阵：



其中，



结合式(6)构建点云位姿最小二乘估计问题，使用高斯-牛顿非线性优化方法迭代计算使损失函数达到最小值的最优解，即完成了点云的粗略配准，这一步骤拉近了两幅点云之间的距离；
步骤二、基于迭代最近点方法的点云精细配准，以提高无人驾驶汽车的定位精度：
由骤一可以得到一组距离被拉近的扫描点云，定义两组点云分别为和其中n为两组点云中匹配点对的数量，对于迭代最近点方法，可用如下方程表示两组点云之间的位姿变换关系：



式中，R是行列式值为1的正交矩阵，称作旋转矩阵，是位移向量；定义误差项构建最小二乘问题：



定义两组点云的质心分别为：



改写式(10)可得：



定义两组点云中各个点的去质心坐标分别为与式(12)中的可改写为：



式中R*为待求解的旋转矩阵，进一步展开有：



式(14)中的与R无关，同样与R无关，可写为：



上式中，定义



对式(16)进行奇异值分解可得：
W＝U∑′VT(17)
式中，∑′是由奇异值组成的对角矩阵，其对角元素按照从大到小的顺序排列，U和V是正交矩阵，当W为满秩时，可以解算最优旋转矩阵：
R＝UVT(18)
计算出旋转后，带入式(12)后并令即可解算出两幅点云之间的位移变换并定义旋转矩阵R为旋转的估计值Restimation、位移变换为位移的估计值即旋转的估计值Restimation＝R，位移的估计值旋转的估计值Restimation和位移的估计值构成的智能车辆当前位姿的估计值Poseestimation表示为：



步骤三、建立雷达点云回环检测模型：
回环检测的定义是：载体行进过程中记录场景信息，当通过重复场景时，用当前的观测对自身状态进行校正，消除累计误差；
首先，将三维扫描点用立方体分割，每一个立方体称为一个胞元，其中胞元的位置是固定的，不断地有新的扫描点进入和离开，为了避免重复计算，给出...

【专利技术属性】
技术研发人员：郭洪艳，孟庆瑜，赵小明，赵旭，戴启坤，高炳钊，
申请(专利权)人：吉林大学，
类型：发明
国别省市：吉林;22