CN116774247A - 基于ekf的多源信息融合的slam前端策略 - Google Patents

Abstract

本发明针对激光雷达前端子图构建中扫描匹配依赖初始值问题，提出了基于拓展卡尔曼滤波的多源信息融合算法。并把EKF融合了轮边编码器和IMU预积分，基于此得到的里程位姿(位置+姿态)，进而获得融合之后的位姿以及协方差矩阵，并将融合位姿进行四元数球面插值去除激光雷达运动畸变；本发明还针对激光雷达帧间匹配需要较多特征点的问题，使用当前激光雷达数据帧与局部地图相关扫描匹配的策略改进了帧与子图匹配之间的匹配策略；同时融合位姿作为激光雷达迭代的初值，优化了迭代的时间，提高了定位精度。最后通过gazebo平台进行仿真，验证了方法的有效性。

Description

基于EKF的多源信息融合的SLAM前端策略

技术领域

本发明属于同步定位与建图(Simultaneous Localization and Mapping，简称SLAM)技术领域，具体涉及一种基于EKF的多源信息融合的SLAM前端策略。

背景技术

在同步定位与建图(Simultaneous Localization and Mapping，简称SLAM)领域，SLAM系统的框架主要包含两个：前端与后端。

在激光SLAM中，前端主要是在已知前一帧的位姿情况下提出当前扫描点来估计当前位姿，后端主要是使用基于滤波器或者图优化的优化方法来进行优化。子图扫描匹配是一种基于子图的SLAM前端方法。它将整个地图分为多个子图，每个子图又拥有一个独立的坐标系。在子图的扫描匹配中，通过对各子图的图像进行图像匹配，从而获得各子图的姿态信息。再将各点的姿态估算结果进行加权平均值，即可获得各点的姿态估算结果。由于在子图扫描匹配中，初始姿态的估算对匹配效果有很大的影响。所以，子图的扫描匹配过于依赖初值。因此如何处理好这一问题便成为当前工程研究的主要方向。

发明内容

本发明属于同步定位与建图(Simultaneous Localization andMapping，简称SLAM)领域，基于对传统SLAM的前端优化过程呈非线性的状态的问题，利用拓展卡尔曼滤波进行线性化处理，并将融合之后的位姿信息用以解决激光雷达的运动畸变。

为了保障前端匹配不依赖初值，本发明提出了一种基于EKF的多源信息融合的SLAM前端策略

本发明所进行的工作至少包括：第一：针对SLAM前端子图扫描匹配依赖初值的问题，提出了一种融合轮边编码器和IMU的EKFMSF方法，将融合之后的位姿作为激光雷达扫描匹配的初始值。第二：针对单线低频率激光雷达在运动过程中出现运动畸变问题，将融合位姿基于激光雷达的激光点进行四元数球面插值，然后将每个激光点得到的相对位姿作为补偿去运动畸变；第三：针对激光雷达帧间匹配需要较多特征点的问题，改进了帧与子图匹配的匹配策略。最后构建概率栅格地图，并进行仿真实验验证了EKFMSF-SLAM算法的有效性。

本发明解决其技术问题具体采用的技术方案是：

一种基于EKF的多源信息融合的SLAM前端策略，其特征在于：

针对激光雷达前端子图构建中扫描匹配依赖初始值问题，采用基于拓展卡尔曼滤波的多源信息融合算法，并将IMU和轮边编码器计算得到的里程数据进行融合，以融合之后的位姿作为激光雷达扫描匹配的初始值；

通过融合位姿基于激光雷达的激光点进行四元数球面插值，然后将每个激光点得到的相对位姿作为补偿去运动畸变。

进一步地，使用当前激光雷达数据帧与局部地图相关扫描匹配的策略改进帧与子图匹配之间的匹配策略；并融合位姿作为激光雷达迭代的初值，以优化迭代的时间，提高定位精度。

进一步地，所述基于拓展卡尔曼滤波的多源信息融合算法考虑建立的地图为二维栅格地图，对于状态方程z_k＝{x,y,z,roll,pitch,yaw}中的坐标z以及绕x轴的旋转roll和绕y轴的旋转pitch都为0，以简化为z_k＝{x,y,0,0,0,yaw}；因此，从无人车获得的编码器和IMU两个传感器的数据融合，就能获得两个高斯线性系统观测方程；而运动方程因为无人车的运动过程唯一，对应的运动方程只有一个，以此得到系统的状态方程；

再采用松耦合的方式，初始状态无人车预测位姿均为0，当编码器和IMU其中之一的观测数据到来，则另一个畸形线性插值得到时间对齐的位姿观测数据，再将两者的数据进行融合，以输出最优的位姿估计；

其中，采用卡尔曼公式对两个观测数据进行融合，以融合观测估计的标准差最小为目标，确定卡尔曼增益系数。

进一步地，将获得的融合位姿进行四元数球面插值去除激光雷达的运动畸变具体为：

采用平移矩阵表示无人车的位移，用四元数表示无人车的旋转变化，并对四元数采用球面线性插值；

其中，设p对应的时刻为t₁，q对应的时刻为t₂，t′时刻对应的四元数为r，对应所求得四元数球面线性插值；

于是比例系数t满足：

单位四元数p和q之间的夹角为θ，p和r之间的夹角为tθ，q和r之间的夹角为(1-t)θ；

以上公式中Slerp表示四元数球面线性插值，根据该公式的线性插值获得每一个激光点相对于起始激光点的相对位姿，然后进行位姿变换。

进一步地，所述位姿变换的具体步骤为：

1)获取激光雷达观测scan数据，包括激光雷达起始时间、结束时间，通过线性计算每一个点对应的时刻；

2)求解每一个点对应的时刻相对于起点时刻的相对时间，并对融合位姿公式进行线性插值得到位姿增量；

3)将各点的位姿增量加入到激光雷达观测中，做去运动畸变，最后将激光雷达观测转换到世界坐标系中。

进一步地，采用当前激光雷达数据帧与局部地图进行相关性扫描匹配CSM的策略进行帧与子图的匹配；

其基于概率栅格地图运行，每个栅格都维护一个对数形式的占据概率；对于新输入的激光帧，将帧数据中所有的点通过一个预估位姿投影到栅格地图上，由此，每个激光点都会落在一个栅格中，激光点所在栅格的对数概率值之和为当前位姿的得分，代表着这个位姿的可信度，最后求出得分最高的备选位姿，作为最优结果输出。

进一步地，在将轮边里程计和IMU的位姿信息进行融合，并基于激光雷达当前帧的各激光雷达点进行插值，对各激光点进行了相应的位姿变换完成去运动畸变，并基于这个位姿变换作为激光雷达的扫描匹配迭代初值，然后激光雷达通过CSM匹配得到前端里程计的基础上，构建EKFMSF-SLAM局部子图：

采用概率来描述栅格中是否有障碍物，占据概率越大表明栅格中存在障碍物的可能性就越高：

经过数学建模后，将若干帧激光雷达观测定义为一个子图，子图中坐标为真实世界的坐标，然后将地图进行离散化，离散的栅格边长为r；地图中的点(x，y)在栅格地图中的位置为(x/r，y/r)；

此时无人车的状态为(x，y，θ)，θ为航向角，无人车和激光雷达距离为l，激光雷达对于障碍的观测距离为d，夹角为α，故当前观测的障碍物在全局的位置(x_o,y_o)为：

故障碍图在栅格中的位置为：此时认为该栅格被障碍物占据。

进一步地，采用的坐标系变换包括：由激光雷达坐标系到里程计坐标系转换，再由融合框架将里程计坐标系到IMU坐标系，最后由IMU坐标系到无人车基座标系转换。

相比于现有技术，本发明及其优选方案针对激光雷达前端子图构建中扫描匹配依赖初始值问题，提出了基于拓展卡尔曼滤波的多源信息融合(Extended Kalman filtermulti-source fusion，简称EKFMSF)算法。并把EKF融合了轮边编码器和IMU预积分，基于此得到的里程位姿(位置+姿态)，进而获得融合之后的位姿以及协方差矩阵，并将融合位姿进行四元数球面插值去除激光雷达运动畸变；本发明还针对激光雷达帧间匹配需要较多特征点的问题，使用当前激光雷达数据帧与局部地图相关扫描匹配(correlative scanmatching，简称CSM)的策略改进了帧与子图匹配之间的匹配策略；同时融合位姿作为激光雷达迭代的初值，优化了迭代的时间，提高了定位精度。最后通过gazebo平台进行仿真，验证了方法的有效性。

附图说明

下面结合附图和具体实施方式对本发明进一步详细的说明：

图1为本发明实施例阿克曼转向小车运动模型示意图；

图2为本发明实施例EKF融合框架示意图；

图3为本发明实施例直线运动激光雷达运动畸变示意图；

图4为本发明实施例激光雷达畸变示意图；

图5为本发明实施例单位四元数球面插值示意图；

图6为本发明实施例坐标变换示意图；

图7为本发明实施例点云匹配流程图；

图8为本发明实施例CSM各量之间的约束示意图；

图9为本发明实施例栅格地图状态示意图；

图10为本发明实施例扫描更新示意图；

图11为本发明实施例去运动畸变实验结果示意图；

其中，左图为直线运动右图旋转运动；

图12为本发明实施例轨迹对比图；

图13为本发明实施例XY方向轨迹对比图；

图14为本发明实施例局部放大图；

其中，a)为图12的局部放大图，b)为图13的局部放大图；

图15为本发明实施例姿态对比图；

图16为本发明实施例局部姿态放大对比图；

其中，上图为仿真环境，下图为激光雷达初始观测；

图17为本发明实施例仿真环境与激光雷达初始观测图；

其中，a)EKFMSF-SLAM多个子图结果，b)局部细节放大；

图18为本发明实施例EKFMSF-SLAM多个子图结果与局部细节放大示意图；

图19为本发明实施例IMU预积分位姿绝对位姿误差示意图；

图20为本发明实施例里程计估计位姿绝对位姿误差示意图；

图21为本发明实施例融合位姿绝对位姿误差示意图；

图22为本发明实施例方法流程图。

具体实施方式

为让本专利的特征和优点能更明显易懂，下文特举实施例，作详细说明如下：

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本说明书使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

如图22所示，以下提供分步骤的实施例对本发明方案作详细介绍：

步骤一：通过多传感器采集无人车数据并进行数据预处理；

从车载传感器中采集激光雷达、惯性测量单元(IMU)与轮边编码器传感器的信息。本发明实施例使用的车辆模型为阿克曼转向小车，如图1所示。

图中α为无人车转向轮内轮的转角，β为无人车转向轮外轮的转角，θ为单车模型航向角，L为无人车轴距，K为无人车轮距，R为无人车转弯半径。因为单车模型和阿克曼模型在运动机理方面可以视为一致，阿克曼模型相比于单车模型，就是把前轮的转向角换成了两前轮的平均转向角，其余全部与单车运动学模型保持一致，故可将阿克曼模型简化成单车模型。单车模型如图中虚线所示，通过分析单车模型可以更方便的了解阿克曼转向机理。

依据图1，阿克曼几何的核心公式为：

由几何关系可得：

结合上述的几何核心公式可以得到单车模型的转角与阿克曼模型的左右转向轮转角关系为：

假设无人车转向内轮转弯半径为R_L，转向外轮转弯半径为R_R，它们之间存在以下关系：

假设无人车转向内轮速度为v_L，转向外轮速度为v_R，无人车线速度为v，角速度为w，可得：

所以得单车模型的速度与阿克曼模型的左右轮速度关系为：

于是便可以将阿克曼模型简化成单车模型，并以单车前轮中心为原点，得到单车模型相对世界参考系在x轴和y轴的速度以及相对点O的角速度ω表达式为：

上述公式等号左边均表示导数。于是从t到t+1这一时间段Δt中，无人车运动状态公式为：

其中δ为简化成的无人车单车模型转向角，为δ的导数。于是可以通过上述公式来表示无人车在世界坐标系下的轮边编码器中X轴的位移、Y轴方向的位移以及航向角和前轴转向角的递推公式，这样就完成了从阿克曼转向模型到单车模型的转换。

步骤二：由步骤一预处理所得的数据，推导出误差递推公式；

IMU的加速度和角速度/>连续观测模型为：

公式中下标t表示在IMU坐标系下的结果，a为实际加速度，b_a表示IMU加速度计偏置，w为实际测量角速度，b_w为陀螺仪偏置，i为第t时刻与第t+1时刻之间的某一时刻，故状态值PVQ基于中值积分的离散形式为：

公式中p,v,q分别表示无人车位姿、速度以及四元数，δt表示时间的一次导数。根据IMU预积分公式：

将积分过程进行离散化得到基于中值定理的预积分公式中的为和/>为：

公式中w为世界坐标系，b_k为世界坐标系，为坐标w到b_k的旋转矩阵，/>和的结果为i+1时刻相对b_k的相对位姿变化，初始状态/>和/>为0，/>为单位四元数，i表示第k时刻与第k+1时刻之间的某一时刻，δt为i到i+1时刻之间的时间间隔，p,v,q分别表示无人车位姿、速度以及四元数，g表示重力加速度，/>表示四元数乘法。由公式可知后续的状态不受b_k的状态影响，故将其作为非线性优化的变量时，可以避免前一时刻状态的重复传递，从而避免了求解后续状态的重复积分的过程。

由于上述公式积分出来的值存在误差，且IMU受到加速度计的偏置b_a、陀螺仪的偏置b_w、噪声n_a和n_w的影响，这些都是产生误差的来源，因此要对误差进行分析。把加速度偏置b_a和陀螺仪偏置b_w建模为随机游走，其导数为高斯性的：

假设噪声n_a和n_w符合高斯噪声得到：

由于四元数被过参数化，故其误差项围绕自身均值分布为：

故可以得到误差项为δb_at、δb_wt

对这些误差项分别进行求导，其中根据定义可得：

对于考虑噪声n_a和n_w时为：

公式中[·]_×表示反对称矩阵。

对于的求解，得先求/>其导数/>在不考虑噪声公式如下：

其中：

考虑噪声n_a和n_w的真实测量值时，/>为：

根据导数性质有：

即得到等式：

结合上述公式，可以得到的表达式为：

综上可得t时刻误差项的线性化递推方程为：

简化后可以写为：公式中F_t是15x15，/>是15x1，G_t是15x12，n_t是12x1。

同理得到离散形式的误差：

简写为：δz_k+1＝Fδz_k+Vn

公式中F_t是15x15，是15x1，V^15×18是15x18，n是18x1。

步骤三：使用基于拓展卡尔曼滤波的多源信息融合(EKFMSF)的SLAM前端策略将IMU和轮边编码器计算得到的里程数据进行融合，得到融合位姿；

由贝叶斯定律刻画两种条件之间可能性的联系，其核心公式为：

将公式用在无人车的状态估计X＝{x₀,...,x_N}表示车辆的位姿，Z＝{z₀,...,z_N}表示当前车辆的观测，其中的观测为在位姿x₀到x_N有路标y₁,...,y_M，故无人车的运动模型和观测模型可以表示为：

由于当前的位姿和路标点都是需要估计的变量，令x_k为k时刻所有的未知量，它包含了当前时刻无人车的位姿和m个路标点，可以写成：

x_k＝{x_k,y₁,...,y_m}

同时把k时刻的所有观测记作z_k，于是公式可以简化为：

从0到k的数据来估计现在的状态分布为：P(x_k∣x₀,u_1:k,z_1:k)，其中1：k表示从1到k时刻的数据，结合上述的核心公式可得：

P(x_k∣x₀,u_1:k,z_1:k)∝P(z_k∣x_k)P(x_k∣x₀,u_1:k,z_1:k-1)

上述公式左边为似然概率，右边为先验概率，似然可以由观测方程得到，而先验概率可以基于过去的状态来推导，自此就完成了无人车状态估计的贝叶斯方法。

经典的卡尔曼滤波方法假设马尔可夫性，即当前的状态只与上一时刻状态有关，并且运动方程与观测方程都为线性高斯系统，于是状态方程为：

通过马尔可夫假设，先验概率按x_k-1时刻为条件概率展开得到：

P(x_k∣x₀,u_1:k,z_1:k-1)＝∫P(x_k∣x_k-1,x₀,u_1:k,z_1:k-1)P(x_k-1∣x₀,u_1:k,z_1:k-1)dx_k-1

＝∫P(x_k∣x_k-1,u_k)P(x_k-1∣x₀,u_1:k-1,z_1:k-1)dx_k-1

上述公式第二个等式因为x_k时刻与x_k-1之前的状态无关，故第一个概率可以简化为只与x_k-1和u_k有关的形式，并且k-1时刻的状态与k时刻的输入量无关，所以第二个概率可以去除u_k。

假设噪声满足均值为零的高斯分布：

w_k～N(0,R),v_k～N(0,Q)

卡尔曼滤波的第一步为预测，即结合运动方程确定先验概率分布也是高斯分布：

记其中/>表示先验概率，/>表示后验概率。根据观测方程可得：

P(z_k∣x_k)＝N(C_kx_k,Q)

根据上述已经完成的无人车状态估计的贝叶斯方法，假设无人车位姿服从的高斯分布，可得：

因为公式两侧都是高斯分布，故只需对比指数部分，将指数部分展开可以得到：

为了求解和/>将两边进行展开，对比x_k的二次方系数得：

上述公式给出了协方差的计算过程，为了便于后面计算，定义一个中间变量，这个变量就是卡尔曼增益K：

根据此定义，在上述公式左右各乘有：

故可得：

然后比较上述指数部分展开的公式x_k的一次项的系数，有：

取系数并转置得：

左右乘以并将其表达式代入得后验均值为：/>

自此推导了卡尔曼滤波构成的线性系统的最优无偏估计，但是实际的SLAM过程运动方程和观测方程都是非线性函数，为了解决非线性系统问题，本发明采用拓展卡尔曼滤波，在工作点x_k附近将运动方程和观测方程进行一阶泰勒展开，从而将非线性方程通过泰勒展开近似成线性高斯系统，线性化公式为：

记则上述后验概率方程就变为：

结合无人车的运动方程，此时预测步的先验期望和协方差就可变为：

结合无人车的观测方程可得后验概率：

同理，此时的卡尔曼增益为：

故拓展卡尔曼滤波的后验形式为：

自此本发明通过公式推导给出了SLAM单次线性高斯近似下的最大后验概率。由于本发明实施例要建立的地图为二维栅格地图，因此对于状态方程z_k＝{x,y,z,roll,pitch,yaw}中的坐标z以及绕x轴的旋转roll和绕y轴的旋转pitch都为0，故状态方程可以简化为z_k＝{x,y,0,0,0,yaw}。这样将从无人车获得的编码器和IMU两个传感器的数据融合，就能获得两个高斯线性系统观测方程；而运动方程因为无人车的运动过程唯一，故运动方程只有一个，得到系统的状态方程为：

具体的融合过程流程图如图2所示，本发明采用松耦合的方式，首先初始状态无人车预测位姿均为0，然后编码器和IMU其中一个观测数据到来，另一个做线性插值得到时间对齐的位姿观测数据，然后将两者的数据进行融合，最后输出最优的位姿估计。

具体的融合过程为：假设两个传感器数据符合高斯分布，即：

基于卡尔曼公式可以得到融合观测估计为：

z_k＝z₁(k)+K_k(z₂(k)-z₁(k))

现在要求解卡尔曼增益K_k使得标准差最小也就是方差最小，融合系统的方差为：σ²＝Var(z₁(k)+K_k(z₂(k)-z₁(k)))＝Var((1-K_k)z₁(k)+K_kz₂(k))

公式中Var代表方差，由于编码器和IMU两个传感器相互独立，故可得：

公式对K_k求导，并令其导数为零求极值就可以求得方差最小时K_k为：

公式中K_k为卡尔曼增益，相当于调节两个传感器的权重，当其取值等于0时，此时观测只有z₁(k)，当取值等于1时，此时观测只有z₂(k)，故其范围为[0,1]，分析方差公式可得融合之后的方差比两个方差中的任何一个都要小，这将使得融合之后的位姿更加准确。

步骤四：将步骤三得到的融合位姿进行四元数球面插值去除激光雷达的运动畸变；

激光雷达里程计除了比较依赖初值外，激光雷达自身观测数据也存在运动畸变，运动畸变就是激光雷达随着无人车的运动，机械式的激光雷达通过电动马达实现360度的环境扫描，比如本发明所用的激光雷达为10HZ，即激光雷达旋转一周产生的第一个激光点与最后一个点之间存在时间间隔为0.1s，如图3所示，左边圆圈点表示无人车静止时激光雷达旋转一圈所能达到的最远位置，这时扫描起点和终点可以比较好的闭合；右边为无人车做直线运动(无旋转)时，激光雷达产生的一圈激光所能到达的最远位置，这时扫描起点和终点就会发生扭曲，这是因为图中从起点到一圈扫描结束时的位置已近发生了变化，即之后的激光点所对应的激光雷达坐标系原点发生了变化。

激光雷达点云运动畸变产生本质上是一帧中每一个点的坐标系不同。当无人车除了直线运动还有旋转时，对于同一直线的物体的观测就会产生很大的误差，如图4所示，上图中p1～p3表示激光雷达依次扫描到的三个位置点，此时无人车静止且这三点在真实世界中共线。但由于无人车在一帧时间内存在“剧烈”运动，如图4中间部分所示，雷达自身分别在三个不同的实际姿态下对三个点进行了扫描。因此在最后得到的点云中(最下面)，三个点坐标实际处于不同的坐标系，这样三个观测点不仅不再共线，还与其真实的位置存在严重的误差。

综上所述，激光雷达运动畸变就是无人车的运动使得每个激光雷达点所在的激光雷达坐标系发生了变化，如果可以将所有的激光点与初始点坐标变化关系求出来，那么就可以解决激光雷达运动畸变问题。

将步骤三融合之后的位姿作为激光雷达各激光点对应的位姿变化关系，并且由于融合以后的位姿可达100HZ，本发明采用的激光雷达角分辨率为0.225°，也就是旋转一圈产生1600个激光点，故融合的数据与激光雷达点的时间不同，需要采用插值的方式进行估计。

最常用的旋转表示方法是四元数、欧拉角和旋转矩阵，但是欧拉角存在万向锁问题，即在Z-Y-X顺序中，若绕Y轴的旋转pitch为正负90度时，则第三次和第一次旋转会绕同一个轴，使得系统丢失一个自由度，故欧拉角不适用于插值和迭代。四元数只需要储存四个数值，而旋转矩阵需要创建3×3的矩阵，需要同时储存9个元素，故四元数相比旋转矩阵更节省存储空间，同时也更方便插值。故本实施例用平移矩阵来表示无人车的位移，用四元数来表示无人车的旋转变化，其中的平移矩阵可以在欧式空间中进行线性插值。但是四元数的插值向量得保证其运动轨迹在单位球面上，故对于四元数采用球面线性插值。

单位四元数有三个虚部和一个实部，可以表达三维空间的旋转，表达式为：

q＝w+i·x+j·y+k·z

单位四元数满足以下性质：

||q||＝1q^-1＝q^*q＝cos(θ)+u·sin(θ)

公式中u为四元数的旋转轴，θ为四元数绕u轴的旋转角度；通过上述公式可以推导出单位四元数与旋转向量之间的相互换算关系为：

log(q)＝log(uθ)＝uθ

如图5所示，设p对应的时刻为t₁，q对应的时刻为t₂，t′时刻对应的四元数为r，即为所求得四元数球面线性插值。

于是比例系数t满足：

假设单位四元数p和q之间的夹角为θ，p和r之间的夹角为tθ，q和r之间的夹角为(1-t)θ。按一般的插值公式写出r的表达式如下：

r(t)＝a(t)p+b(t)q

同时有：

cosθ＝p·q,cos(tθ)＝p·r,cos[(1-t)θ]＝q·r

用p同时点乘差值公式两边，同理用q同时点乘插值公式两边可以得到：

联合求解可以解得：

于是有：

公式中Slerp(Spherical linear interpolation)表示四元数球面线性插值，根据该公式的线性插值可得每一个激光点相对于起始激光点的相对位姿，然后进行位姿变换，具体步骤为：

1)获取激光雷达观测scan数据，即激光雷达起始时间、结束时间，通过线性计算

1600点中每一个点对应的时刻。

2)求解这些点对应的时刻相对于起点时刻这段时间的Δ_t，并对融合位姿公式进行线性插值得到位姿增量。

3)将各点的位姿增量加入到激光雷达观测中，做去运动畸变，最后将激光雷达观测转换到世界坐标系中。

步骤三中涉及的坐标系变换如图6所示，主要由激光雷达坐标系到里程计坐标系转换，再由融合框架将里程计坐标系到IMU坐标系，最后由IMU坐标系到无人车基座标系转换。

步骤五：分析激光里程计原理，并且采用当前激光雷达数据帧与局部地图相关扫描匹配(CSM)的策略来进行帧与子图的匹配策略；

激光雷达通过激光源发射激光，经反射镜反射到激光雷达之外，同时伺服电机带动反射镜旋转，遇到障碍物的激光束反射回接收器上，通过光学编码器计算发射与接收的时间差，乘以光速就是激光雷达与障碍物距离的两倍，通过旋转一圈就可以得到周围障碍物相对于激光雷达的相对位置，根据当前激光雷达的相对角度还可以得到障碍物相对雷达的方位，这样就得到了障碍物相对激光雷达坐标系的位姿。

激光雷达测距公式L为：

公式中c为光速，约为：3×10^8m/s，Δ_t为激光发射到接收的时间差。

得到激光雷达周围的障碍物相对于激光雷达的相对位姿后，通过点云匹配算法，如图7所示，点云匹配首先通过提取关键点并进行特征描述，然后将两个点云集的特征点进行匹配并去除一些误匹配的特征点，其次使用帧间匹配算法或者雷达帧与局部地图进行匹配，得到R和t并且构建误差的最小二乘，最后收敛或达到迭代次数从而得到激光雷达前后的位姿差，即里程数据。

传统的帧间匹配就是将两个激光雷达帧进行匹配得到两帧之间的相对位姿，最典型的一种数据匹配方法是最近点迭代算法(Iterative ClosestPoint，ICP)，它的特点是：寻找源点云与对象点云的对应点，根据对应点，构建一个变换矩阵，然后根据该矩阵将源点云转换成对象点云的坐标，然后估算出转换后的源点云与对象点云的偏差，如果偏差值大于设定的阈值，那么就反复进行迭代操作，直至小于一定的偏差值为止。

传统的匹配算法为ICP点云帧间匹配算法，通过把上一帧最近的点当作同一点的不同时刻的对应点，这样容易陷入局部的最优值，并且随着无人车的运动，该算法会将之前的误差一直累积，对于大场景容易出现相对位姿误差明显等问题。因此本发明采用当前激光雷达数据帧与局部地图相关扫描匹配(correlative scan matching，CSM)的策略，可以减少误差的累积。

与ICP点云匹配算法不同的是，CSM算法匹配的不是上一帧的点，而是之前一段时间的激光雷达点云构成的子图，该算法基于概率栅格地图运行，每个栅格都维护一个对数形式的占据概率；对于新进来的激光帧，将帧数据中所有的点通过一个预估位姿投影到栅格地图上，这样每个激光点都会落在一个栅格中，激光点所在栅格的对数概率值之和为当前位姿的得分，代表着这个位姿的可信度，最后求出得分最高的备选位姿，作为最优结果输出。

该问题的数学建模过程为：如图8所示，将无人车位置初始化在位置x_i，无人车对环境的观测是z_i，在上一个相邻位置x_i-1上，无人车对环境的观测为z_i-1，从x_i-1到x_i，无人车自身也能通过轮边里程计或者IMU计算得到初始的位姿变化，记这个位姿变化为u，如果已知x_i-1和z_i-1，通过观测也得到u和z_i，要反推无人车的精确位姿x_i就需要使得概率p(x_i|x_i-1,u,z_i-1,z_i)最大，而这个概率不是一个概率值而是一个关于三元变量的联合概率分布，当概率最大时就是无人车的最优位姿估计。

本方法建立的地图为二维栅格地图，故无人车的位姿x_i和x_i-1为三维向量，包含了平面坐标x、y和航向角yaw，u描述的是相邻位姿的变化，同样是一个三维向量；z_i和z_i-1作为激光雷达的观测，是一定数量的二维坐标的集合。

接下来的推导用m取代z_i-1，这样的表示更具一般性，可以代表之前任意的一帧激光雷达数据，也可以表示之前若干帧组成的子图帧，这种替换对数学处理无影响，因为无论表示z_i-1还是z_i-1+z_i+2+...，都是以栅格地图的形式体现。

从图8中可得当前位姿x_i服从两个约束为：

x_i-1+u≈x_i

z_i|x_i～m

上述公式第一个表达式表示上一时刻的位姿x_i-1加上观测估计u就是当前时刻的位姿x_i，第二个表达式表示观测z_i是发生在位姿x_i上，因此当前观测应当服从已知的局部地图m。根据这两个约束可以得到以下的观测方程：

p(x_i∣x_i-1,u,m,z_i)∝p(z_i∣x_i,m)p(x_i∣x_i-1,u)

公式中∝表示正比于，即求解的整体概率分布这个问题等价于求解取得全局最大的那个。上述公式中的p(x_i∣x_i-1,u)为运动模型，符合常规的高斯分布N(μ,σ²)，故求解最大似然估计即可，具体为：

公式中f(x)为概率密度分布函数，μ为均值，σ为标准差，σ²为方差，具体计算公式为：

联合概率密度方程为：

故最大的似然函数及对数形式为：

求解该公式的最大值就是求解二元函数的极值问题，求偏导然后令其为零，其中大的那个极值就是所求的，这里就不再赘述。

上述观测方程公式中p(z_i∣x_i,m)为观测模型，概率分布模型不是高斯分布，具体计算为：假设中的各个激光点位置的概率分布是彼此独立的，则：

对上述公式两侧同时取对数，这样乘法计算就转化为加法计算，于是：

上述公式就是按照位姿把当前的观测投影到子图中，把所有被击中的栅格的概率值相加，不同的投影位姿得分不一样，最后把最大的得分S的位姿输出就是当前位姿的最优估计，即：

自此我们就得到了基于CSM的激光雷达里程计，通过将连续的激光雷达帧当作子图，并将当前雷达帧与子图做匹配，解决了帧间匹配带来的误差累积，有效的减少了求解当前位姿的误差。

步骤六：构建概率栅格地图，通过数学建模提出一个新的多元融合局部子图方案；

上面的步骤已经将轮边里程计和IMU的位姿信息进行了融合，并基于激光雷达当前帧的各激光雷达点进行了插值，对各激光点进行了相应的位姿变换完成了去运动畸变，并基于这个位姿变换作为激光雷达的扫描匹配迭代初值，然后激光雷达通过CSM匹配得到了前端里程计，接下来将构建EKFMSF-SLAM局部子图。

一个地图点通常是有两种状态，一种是存在障碍物，用1表示。还有一种是空闲，用0表示，但是用这一个值表示无法直接与新的观测建立联系，比如新的观测可能存在与之前的值相同或者不同的情况，为了统一表示并且建立与新的激光雷达观测之间的联系，本发明采用如图9所示的概率栅格地图的形式，即用概率来描述栅格中是否有障碍物，占据概率越大表明栅格中存在障碍物的可能性就越高。

对于占据栅格地图(Occupy grid map)的每个栅格，用p(s＝1)来表示该栅格是空闲状态，公式中用Free来表示，另一个占用状态用p(s＝0)来表示，公式中用Occupied来表示，本发明用空闲与占用的概率比值来表示栅格的状态，公式如下：

上述公式中Odd表示概率。当新的数据到来，即激光雷达当前的观测数据z～{0,1}到来时，栅格的概率状态需要进行更新，新的状态为：

根据贝叶斯公式得出以下两个式子：

将上述两个式子，代入Odd(s∣z)之后，可以得出：

对上述公式左右取对数，可以得到：

此时，栅格中含有测量值的项就只有该测量值存在两种状态：空闲(gridFree)、占用(gridOccupied)，表达式为：

此时，如果用logOdd(s)来表示栅格s的状态S的话，则更新规则进一步化简为：

其中，S⁺表示更新状态之后栅格s的状态，同理S^-就表示更新之前的状态。栅格的状态空闲和占用的概率都设置为0.5，于是栅格起始的状态S_init为：

/>

此时，经过数学建模，更新一个栅格的状态只需要在原先的基础上加上一个概率即可，如下公式所示：

经过数学建模后，本发明将100帧激光雷达观测定义为一个子图，如图10所示，图中坐标为真实世界的坐标，然后将地图进行离散化，离散的栅格边长r为0.05m，故地图中的点(x，y)在栅格地图中的位置为(x/r，y/r)。

此时无人车的状态为(x，y，θ)，无人车和激光雷达距离为l，激光雷达对于障碍的观测距离为d，夹角为α，故当前观测的障碍物在全局的位置(x_o,y_o)为：

故障碍图在栅格中的位置为：此时认为该栅格被障碍物占据。

步骤七：通过实车实验证明步骤五提出的多元融合去运动畸变效果的优越性，并通过Gazebo仿真平台完成SLAM前端概率栅格子图的构建与EKFMSF-SLAM算法的仿真验证；

首先，为了验证多元融合去运动畸变的效果，本发明在室内走廊环境中进行实车实验。同时为了更好的验证步骤五提出算法的鲁棒性，分别进行一段任意行走和连续旋转，运动速度为1m/s，实验结果如图11所示。

图中直线部分表示实验小车的轨迹，白色点为激光雷达实时的观测点云，黑色点为经过多源融合去激光雷达运动畸变的点云，从实验结果图a可以看出在快速直线运动中，与小车近的障碍物出现的畸变较少，于小车远的障碍物畸变就比较严重，但主要误差是与准确位姿相差一个旋转，其主要原因小车无旋转，且远的障碍物相对小车位姿较大，故误差就越大；图b为小车在小范围内连续旋转，这时就出现了严重的畸变，跟直线运动相比除了相差一个旋转，部分激光雷达扫描位置还存在较大的平移误差。实验结果可以看出，在这两者容易出现畸变的情况中，本方法提出的多源融合方法实现了较好的去运动畸变，在一定程度上还原了真实的激光雷达观测。

其次，为了验证步骤三融合算法的有效性，本方法在Gazebo仿真环境中进行实验，取仿真环境中的小车实际位姿作为真值，将得到的里程计估计位姿、IMU预积分的估计位姿、EKFMSF融合位姿轨迹与真实位姿进行对比如图12到图16所示。

图14(a)从上到下依次是融合轨迹、轨迹位姿真值、IMU预积分轨迹与里程计估计轨迹。图14(b)从上到下依次是IMU预积分轨迹、里程计估计轨、轨迹位姿真值与融合轨迹。图16从左到右依次是轨迹位姿真值、融合轨迹、IMU预积分轨迹与里程计估计轨。从以上实验结果可以得到里程计单独的估计位姿与真值位姿之间存在较大误差，出现这种情况的原因是小车在仿真环境中存在打滑现象；IMU在这种环境下单独估计误差比里程计小的多，但是本方法提出的EKFMSF融合里程计无论是X反向、Y方向以及航向角的误差都比融合之前的里程计估计位姿和IMU预积分得到的估计位姿都要更接近真实的位姿轨迹。

最后，为了验证多元融合局部子图方案的有效性，本方法在如图17(a)所示的Gazebo仿真环境中进行仿真实验，仿真环境为10x10m的方形地图，中间的障碍物为1×1×1m的立方体，仿真小车的激光雷达扫描半径设置为10米，最终实验结果在Rviz中展示。

图17中b为小车初始化时激光雷达的一帧观测，圆圈状点云代表着某束激光点碰到障碍物后返回经过公式计算并做坐标变换呈现出来的全局位置，圆圈点与激光雷达之间的栅格被更新空闲状态，方块状的栅格所在的位置更新为占有状态，近一步实验就可以得到图19。

图18(a)为小车开始运动，并将多个子图重叠输出的结果，图18(b)为局部放大图，可以看到放大后，每个栅格都是正方形，正方形中存在多个障碍物的位置估计，每个估计可以认为是一个子图的输出结果，从结果中可以看出，随着小车的运动和子图的增多，地图中出现了严重的重叠，但是每个子图没有出现大的畸变，这验证了本方法局部地图构建的有效性，不同的子图之间重叠严重，出现这种现象的原因是子图的不断构建会不断积累误差，产生的误差不仅会产生畸变，同时还会影响匹配的精度，从而导致多个子图之间位姿的大差别，每个子图将自己的结果输出就出现了重叠问题。

对里程计估计位姿、IMU预积分位姿和融合位姿进行绝对位姿误差分析分别得到图19到图21。对各误差进行统计得到如下表：

从表格结果可以得到本方法提出的EKFMSF-SLAM的融合策略比融合之前的数据都要更加精确，整体估计数据与真值的误差都控制在2cm之内，无论是平均值还是标准差都要远小于单独一个传感器的估计位姿。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

本专利不局限于上述最佳实施方式，任何人在本专利的启示下都可以得出其它各种形式的基于EKF的多源信息融合的SLAM前端策略，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本专利的涵盖范围。

Claims (8)

1.一种基于EKF的多源信息融合的SLAM前端策略，其特征在于：

针对激光雷达前端子图构建中扫描匹配依赖初始值问题，采用基于拓展卡尔曼滤波的多源信息融合算法，并将IMU和轮边编码器计算得到的里程数据进行融合，以融合之后的位姿作为激光雷达扫描匹配的初始值；

通过融合位姿基于激光雷达的激光点进行四元数球面插值，然后将每个激光点得到的相对位姿作为补偿去运动畸变。

2.根据权利要求1所述的基于EKF的多源信息融合的SLAM前端策略，其特征在于：使用当前激光雷达数据帧与局部地图相关扫描匹配的策略改进帧与子图匹配之间的匹配策略；并融合位姿作为激光雷达迭代的初值，以优化迭代的时间，提高定位精度。

3.根据权利要求1所述的基于EKF的多源信息融合的SLAM前端策略，其特征在于：所述基于拓展卡尔曼滤波的多源信息融合算法考虑建立的地图为二维栅格地图，对于状态方程z_k＝{x,y,z,roll,pitch,yaw}中的坐标z以及绕x轴的旋转roll和绕y轴的旋转pitch都为0，以简化为z_k＝{x,y,0,0,0,yaw}；因此，从无人车获得的编码器和IMU两个传感器的数据融合，就能获得两个高斯线性系统观测方程；而运动方程因为无人车的运动过程唯一，对应的运动方程只有一个，以此得到系统的状态方程；

再采用松耦合的方式，初始状态无人车预测位姿均为0，当编码器和IMU其中之一的观测数据到来，则另一个畸形线性插值得到时间对齐的位姿观测数据，再将两者的数据进行融合，以输出最优的位姿估计；

其中，采用卡尔曼公式对两个观测数据进行融合，以融合观测估计的标准差最小为目标，确定卡尔曼增益系数。

4.根据权利要求3所述的基于EKF的多源信息融合的SLAM前端策略，其特征在于：

将获得的融合位姿进行四元数球面插值去除激光雷达的运动畸变具体为：

采用平移矩阵表示无人车的位移，用四元数表示无人车的旋转变化，并对四元数采用球面线性插值；

其中，设p对应的时刻为t₁，q对应的时刻为t₂，t′时刻对应的四元数为r，对应所求得四元数球面线性插值；

于是比例系数t满足：

单位四元数p和q之间的夹角为θ，p和r之间的夹角为tθ，q和r之间的夹角为(1-t)θ；

以上公式中Slerp表示四元数球面线性插值，根据该公式的线性插值获得每一个激光点相对于起始激光点的相对位姿，然后进行位姿变换。

5.根据权利要求4所述的基于EKF的多源信息融合的SLAM前端策略，其特征在于：

所述位姿变换的具体步骤为：

1)获取激光雷达观测scan数据，包括激光雷达起始时间、结束时间，通过线性计算每一个点对应的时刻；

2)求解每一个点对应的时刻相对于起点时刻的相对时间，并对融合位姿公式进行线性插值得到位姿增量；

3)将各点的位姿增量加入到激光雷达观测中，做去运动畸变，最后将激光雷达观测转换到世界坐标系中。

6.根据权利要求5所述的基于EKF的多源信息融合的SLAM前端策略，其特征在于：

采用当前激光雷达数据帧与局部地图进行相关性扫描匹配CSM的策略进行帧与子图的匹配；

其基于概率栅格地图运行，每个栅格都维护一个对数形式的占据概率；对于新输入的激光帧，将帧数据中所有的点通过一个预估位姿投影到栅格地图上，由此，每个激光点都会落在一个栅格中，激光点所在栅格的对数概率值之和为当前位姿的得分，代表着这个位姿的可信度，最后求出得分最高的备选位姿，作为最优结果输出。

7.根据权利要求6所述的基于EKF的多源信息融合的SLAM前端策略，其特征在于：

在将轮边里程计和IMU的位姿信息进行融合，并基于激光雷达当前帧的各激光雷达点进行插值，对各激光点进行了相应的位姿变换完成去运动畸变，并基于这个位姿变换作为激光雷达的扫描匹配迭代初值，然后激光雷达通过CSM匹配得到前端里程计的基础上，构建EKFMSF-SLAM局部子图：

采用概率来描述栅格中是否有障碍物，占据概率越大表明栅格中存在障碍物的可能性就越高：

对于占据栅格地图的每个栅格，用p(s＝1)来表示该栅格是空闲状态，公式中用Free来表示，另一个占用状态用p(s＝0)来表示，公式中用Occupied来表示，采用空闲与占用的概率比值来表示栅格的状态，公式如下：

其中Odd表示概率，当新的数据到来，即激光雷达当前的观测数据z～{0,1}到来时，栅格的概率状态需要进行更新，新的状态为：

根据贝叶斯公式得出以下两个式子：

代入Odd(s∣z)之后，得出：

左右取对数得到：

此时，栅格中含有测量值的只有该测量值存在两种状态：空闲gridFree、占用gridOccupied，对应的表达式为：

此时，用logOdd(s)表示栅格s的状态，则更新规则进一步化简为：

其中，S⁺表示更新状态之后栅格s的状态，S^-表示更新之前的状态；栅格的状态空闲和占用的概率都设置为0.5，于是栅格起始的状态S_init为：

此时，更新一个栅格的状态只需要在原先的基础上加上一个概率即可，如下公式所示：

经过数学建模后，将若干帧激光雷达观测定义为一个子图，子图中坐标为真实世界的坐标，然后将地图进行离散化，离散的栅格边长为r；地图中的点(x，y)在栅格地图中的位置为(x/r，y/r)；

此时无人车的状态为(x，y，θ)，θ为航向角，无人车和激光雷达距离为l，激光雷达对于障碍的观测距离为d，夹角为α，故当前观测的障碍物在全局的位置(x_o,y_o)为：

故障碍图在栅格中的位置为：此时认为该栅格被障碍物占据。

8.根据权利要求1所述的基于EKF的多源信息融合的SLAM前端策略，其特征在于；

采用的坐标系变换包括：由激光雷达坐标系到里程计坐标系转换，再由融合框架将里程计坐标系到IMU坐标系，最后由IMU坐标系到无人车基座标系转换。