[发明专利]一种智能车辆定位与回环检测方法

说明书

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

技术领域

本发明属于智能车辆状态估计领域，涉及一种智能车辆定位与回环检测方法，更加具体来讲，涉及一种智能车辆分步位姿估计和基于激光雷达点云的回环检测方法。

背景技术

近年来智能车辆技术飞速发展，现有框架一般将智能车辆分为感知、规划、决策和控制四个模块，解决上述问题的一个关键前提是获取智能车辆的精确定位，只有确定了车身的位置和姿态信息，才能将感知到的周车及障碍物相信息统一在同一坐标系下，进而为后续模块提供输入信息。目前智能车辆定位手段主要依赖于惯导和GPS卫星定位，然而惯导在单独使用时会由于传感器自身特性产生累计误差，且高精度惯导设备成本较高，GPS卫星定位技术易受到外部信号干扰，即使是短时间内的信号丢失也会对智能车辆行车安全构成巨大威胁。

利用智能车辆上装载的激光雷达进行实时定位是一种切实可行的方案，其面临的问题是如何在定位精度和计算效率上进行取舍，同时消除导航定位误差的累积是激光雷达定位必须解决的问题。

发明内容

本发明为了解决智能车辆定位及误差积累问题，提供了一种智能车辆定位与回环检测方法。

为解决上述技术问题，本发明是采用如下技术方案实现的：

一种智能车辆定位与回环检测方法，在全球定位系统信号不能持续稳定输入车辆定位系统时，以激光雷达数据为输入，激光雷达安装于智能车顶端，通过收发激光束测量障碍物相对于本车的位置和距离，实现对周围场景的感知，利用基于联合概率密度分布的正态分布变换方法和基于空间点拟合的迭代最近点方法分步解算智能车辆当前位置和姿态，保证车身的定位精度和效率，其特征在于，本方法具体步骤如下：

步骤一、基于正态分布变换的点云粗略配准：

激光雷达在每个工作周期内采集到周围物体反射的一系列的离散点，定义这些离散点为扫描点，扫描点包含位置信息、距离信息和反射率信息，定义所有扫描点的集群为点云，且上一时刻采集的点云定义为参考点云。

使用正太分布变换方法进行点云配准的目的是求取车辆的旋转和位移，使当前时刻采集的点云以最大的概率投影到参考点云上；首先将对当前时刻采集的点云进行网格化，用等尺寸立方体分割点云片段并计算每个立方体的概率密度函数，步骤如下：

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

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

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

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