CN111273312B - 一种智能车辆定位与回环检测方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

式中

为立方体中所有扫描点的均值，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为校正位姿，

定义为广义加法，可对旋转和位移进行统一叠加；

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

与现有技术相比本发明的有益效果是：

1.本发明使用激光雷达传感器数据作为定位方法的输入，该种传感器广泛存在于智能车辆上，具有良好的可实现性；

2.本发明分步完成智能车辆的定位任务，对两种典型算法取长补短，同时保证智能车辆的定位精度和效率；

3.本发明提出一种基于激光雷达的回环检测方法，可以在智能车辆经过重复场景时校正当前位置和姿态，消除行进过程中累积的误差。

附图说明

下面结合附图对本发明作进一步的说明：

图1为本发明所述的一种智能车辆定位与回环检测方法的流程简图；

图2为本发明所述的智能车辆迭代最近点方法中扫描点匹配的示意图；

图3为本发明所述的激光雷达点云回环检测方法示意图。

图中：

具体实施方式

下面结合附图对本发明作详细的描述：

本发明提出一种智能车辆定位与回环检测方法，如图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)构建点云位姿最小二乘估计问题，使用高斯-牛顿非线性优化方法迭代计算使损失函数达到最小值的最优解，即完成了点云的粗略配准，这一步骤拉近了两幅点云之间的距离；

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

由骤一可以得到一组距离被拉近的扫描点云，如图2所示，图2中两条曲线分别代表两组点云的形状，每条曲线上的空心圆代表该组点云中的扫描点，连接两个曲线上的空心圆点的直线代表两幅点云中扫描点的匹配关系。

定义两组点云分别为

和

其中n为两组点云中匹配点对的数量，对于迭代最近点方法，可用如下方程表示两组点云之间的位姿变换关系：

式中，R是行列式值为1的正交矩阵，称作旋转矩阵，

是平移向量；定义误差项构建最小二乘问题：

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

改写式(9)可得：

定义两组点云中各个点的去质心坐标分别为

与

式(11)中的

可改写为：

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

式(13)中的

与R无关，

同样与R无关，

可写为：

上式中，定义

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

W＝U∑′V^T (47)

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

R＝UV^T (48)

计算出旋转后，带入式(11)后并令

即可解算出两幅点云之间的位移变换；

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

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

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

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

c_∑v＝vΛ (51)

式中，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为校正位姿，

定义为广义加法，可对旋转和位移进行统一叠加；

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

图3中，圆圈为智能车辆在上一次经过当前场景时构建的地图，称为先验地图，三角形为先验地图中的扫描点，称为先验点云特征，正方形为智能车辆当前得到的扫描点，称为当前点云特征，实线表示智能车辆的行驶轨迹，虚线表示回环检测算法通过比对当前直方图矩阵和先验直方图矩阵识别到智能车辆正在通过重复场景，进而触发回环检测。回环检测执行完毕后，当前智能车辆的位姿得到校正，避免了行驶过程中定位误差的积累。

Claims (1)

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为校正位姿，

定义为广义加法，可对旋转和位移进行统一叠加；

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

Images