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

权利要求书

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

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

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

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

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

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

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

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

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

具体的，令φ_x表示绕x轴旋转的欧拉角，φ_y表示绕y轴旋转的欧拉角，φ_z表示绕z轴旋转的欧拉角，t_x表示沿x轴方向的位移，t_y表示沿y轴方向的位移，t_z表示沿z轴方向的位移，坐标变换函数可表示为：

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

其中，

结合式(6)构建点云位姿最小二乘估计问题，使用高斯-牛顿非线性优化方法迭代计算使损失函数达到最小值的最优解，即完成了点云的粗略配准，这一步骤拉近了两幅点云之间的距离；

步骤二、基于迭代最近点方法的点云精细配准，以提高无人驾驶汽车的定位精度：

由骤一可以得到一组距离被拉近的扫描点云，定义两组点云分别为和其中n为两组点云中匹配点对的数量，对于迭代最近点方法，可用如下方程表示两组点云之间的位姿变换关系：

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

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

改写式(10)可得：

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

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

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

上式中，定义

对式(16)进行奇异值分解可得：

W＝U∑′V^T (17)

式中，∑′是由奇异值组成的对角矩阵，其对角元素按照从大到小的顺序排列，U和V是正交矩阵，当W为满秩时，可以解算最优旋转矩阵：

R＝UV^T (18)

计算出旋转后，带入式(12)后并令即可解算出两幅点云之间的位移变换并定义旋转矩阵R为旋转的估计值R_estimation、位移变换为位移的估计值即旋转的估计值R_estimation＝R，位移的估计值旋转的估计值R_estimation和位移的估计值构成的智能车辆当前位姿的估计值Pose_estimation表示为：

步骤三、建立雷达点云回环检测模型：

回环检测的定义是：载体行进过程中记录场景信息，当通过重复场景时，用当前的观测对自身状态进行校正，消除累计误差；

首先，将三维扫描点用立方体分割，每一个立方体称为一个胞元，其中胞元的位置是固定的，不断地有新的扫描点进入和离开，为了避免重复计算，给出递归形式：

式中，m为立方体中扫描点的数量，j表示对所有扫描点的索引，c_μ为当前时刻胞元中扫描点的均值，m为立方体中扫描点的数量，c_μ′为上一时刻胞元中扫描点的均值；c_∑为当前时刻胞元中扫描点的协方差矩阵，c_∑′为上一时刻胞元内扫描点的协方差矩阵，对协方差矩阵进行特征值分解：

c_∑v＝vΛ (22)

式中，v是协方差矩阵的特征向量，Λ是一个角矩阵，其主对角线元素为协方差矩阵特征值的降序排列，记为λ₁,λ₂,λ₃，对每一个胞元做如下判定：

(1)若0.5≤λ₁/λ₂/λ₃≤1.5，判定此胞元是球形；

(2)若λ₁＞＞λ₂和λ₃，或λ₂＞＞λ₁和λ₃，或λ₃＞＞λ₁和λ₂，判定此胞元是直线形；

(3)若λ₁＜＜λ₂和λ₃，或λ₂＜＜λ₁和λ₃，或λ₃＜＜λ₁和λ₂，判定此胞元是平面形；

通过上述判定原则，可以确定各胞元所属的形状类型，并可以计算胞元中心点相对于雷达坐标系的偏航角和俯仰角，雷达坐标系定义为以激光雷达质心为坐标原点，沿着车身前进的方向为x轴的正方向，x轴沿逆时针方向旋转90度的方向为y轴，垂直于x-y平面方向为z轴的坐标系；

设胞元中心点在雷达坐标系下的坐标为(X,Y,Z),则：

式中，θ是胞元中心点相对于激光雷达的俯仰角，ψ是胞元中心点相对于激光雷达的偏航角；使用三个45×45矩阵构建三个二维的直方图矩阵，分别为球形胞元直方图矩阵、直线形胞元直方图矩阵和平面形胞元直方图矩阵，每个直方图矩阵中的每一个元素表示对应该类型胞元在0°～180°区间内以4°划分的子区间内的数量；

具体来说，某一类型直方图矩阵可表示该类型胞元中心点相对于激光雷达的俯仰角和偏航角在如下区间时的数量：

上式中，A与B均为正整数且0≤A≤44，0≤B≤44，通过式(25)可以得到三种不同形状胞元关于俯仰角、偏航角的直方图矩阵；

设两幅点云的直方图矩阵分别为H₁和H₂，通过归一化互相关方法计算它们之间的相似度：

式中S(H₁,H₂)为衡量两个直方图矩阵相似度的归一化互相关指数，I为直方图矩阵的中各个元素的索引，和为两幅点云直方图各自的均值，具体计算方法为：

对于球形胞元，若S(H₁,H₂)≥0.4，则判定检测到了回环，即智能车辆当前经过重复场景；

对于线形胞元，若S(H₁,H₂)≥0.65，则判定检测到了回环，即智能车辆当前经过重复场景；

对于平面形胞元，若S(H₁,H₂)≥0.9，则判定检测到了回环，即智能车辆当前经过重复场景；

定义智能车辆上一次经过当前场景时获得的点云为C_previous，当前时刻获得的点云为C_current，具体为：

式中，N为两组点云中匹配点对的数量，将上式带入式(12)可得：

式中，w∈N代表匹配点对的索引，代表智能车辆上一次经过当前场景时获得的点云C_previous中的扫描点，代表当前时刻获得的点云C_current中的扫描点，为智能车辆上一次经过当前场景时获得的点云C_previous的质心，为当前时刻获得的点云C_current的质心，可由式(11)计算，R_correction代表智能车辆上一次经过当前场景时获得的点云C_previous和当前时刻获得的点云C_current之间的旋转，代表智能车辆上一次经过当前场景时获得的点云C_previous和当前时刻获得的点云C_current之间的位移；

利用式(18)可以计算出智能车辆上一次经过当前场景时获得的点云C_previous和当前时刻获得的点云C_current之间的旋转，令可进一步计算智能车辆上一次经过当前场景时获得的点云C_previous和当前时刻获得的点云C_current之间的位移，定义智能车辆上一次经过当前场景时获得的点云C_previous和当前时刻获得的点云C_current之间的旋转和位移为校正位姿，记作

在触发回环检测后，智能车辆的当前位姿为：

式中，Pose_current表示智能车辆当前时刻的位姿，Pose_estimation是通过步骤一、二计算得到的智能车辆当前位姿的估计值，由式(19)得到，Pose_correction为校正位姿，定义为广义加法，可对旋转和位移进行统一叠加；

利用上述过程，可使用智能车辆上一次通过当前场景时的观测信息校正智能车辆当前的位置及姿态信息，消除行驶过程中累积的定位误差。