[发明专利]一种基于多传感器信息融合的智能车定位方法

权利要求书

1.一种基于多传感器信息融合的智能车定位方法，其特征在于：包括以下步骤：

(1)、基于智能车的光电编码器和激光雷达采集的数据，并利用同步定位与地图构建方法SLAM得到智能车在SLAM方法构建的局部地图中的位置信息；

步骤(1)的具体过程如下：

(1.1)、以智能车启动时车体中心为原点O_o，沿车前进方向为X_o轴方向，垂直X_o轴指向车体左侧为Y_o轴方向，Z_o轴垂直向上建立里程计坐标系；由于只考虑智能车在水平面内运动且没有俯仰和翻滚动作，因此Z轴坐标始终为0且只有偏航角的变化，因此可以得到智能车的位姿模型(x,y,θ)；通过智能车后轮左、右轮的光电编码器，推算智能车里程计信息，具体过程如下：

(1.1.1)、0时刻，智能车在里程计坐标系中的位姿(x₀,y₀,θ₀)为(0,0,0)；

(1.1.2)、单位采样时间内，在里程计坐标系下，车轮位置的增量ΔS通过以下公式得到：

其中ΔQ为光电码盘输出的脉冲增量，D为车轮直径，T_s为采样时间，N为光电码盘上的光栅总数，K为光电编码器减速率；

(1.1.3)、左右轮之间的距离为ω，单位采样时间内左、右轮位置的增量分别为ΔS_L和ΔS_R，则汽车从k–1时刻S_k-1＝(x_k-1,y_k-1,θ_k-1)到k时刻S_k＝(x_k,y_k,θ_k)的位姿变化为：

x_k＝x_k-1+ΔD_k cos(θ_k-1+Δθ_k)，

y_k＝y_k-1+ΔD_ksin(θ_k-1+Δθ_k)，

θ_k＝θ_k-1+Δθ_k，

(1.1.4)、使用上述方法累积得到时刻k智能车的里程计信息；

(1.2)、通过车载激光雷达获得并记录时刻k智能车周边环境的观测信息；

(1.3)、建立与里程计坐标系同原点同轴的局部地图坐标系，并在局部地图坐标系下通过SLAM算法来进行构图和定位；通过步骤(1.1)得到的里程计信息作为SLAM算法的控制输入信息，通过步骤(1.2)得到的雷达扫描信息作为SLAM算法观测信息，使用一种基于粒子滤波器的SLAM算法得到智能车在地图坐标系下的优化位姿，同时根据优化后的位姿将k时刻智能车周边的障碍物信息加入到局部地图中；

使用粒子滤波器进行优化以得到智能车的更优位姿与地图时，智能车当前时刻的位姿根据前一时刻的位姿来推算得到的；知道k-1时刻智能车的位姿x_k-1，k时刻的控制信息即里程计信息u_k，k时刻的观测信息z_k，估计智能车的当前位姿的x_k的后验概率；

在粒子滤波器中采用序贯重要性采样算法和采样重要性重采样算法来获得后验概率分布，通过对参考分布的采样粒子进行加权来近似得到后验概率分布，

具体包括以下步骤：

(1.3.1)、初始化系统，设置N个粒子，每个粒子代表智能车当前的位姿，采样计算权重

(1.3.2)、重要性采样，在时刻k，根据k时刻智能车的控制信息u_k推算k时刻智能车位姿的分布，对每个粒子进行采样

(1.3.3)、根据k时刻的观测信息z_k和粒子前一时刻的权值，计算k时刻粒子重要性权值并进行归一化权值；

(1.3.4)、重采样，增加重要性权值高的粒子，删除重要性权值低的粒子，计算后验概率；

(2)、通过智能车的GPS定位获得智能车在WGS-84坐标系中的位置信息；

(3)、将步骤(1)和步骤(2)得到的智能车在局部地图坐标系中、WGS-84坐标系中的位置信息通过坐标转换统一在大地坐标系中；

(4)、基于步骤(3)的坐标转换过程，采用卡尔曼滤波技术将步骤(1)得到的智能车的位置信息、步骤(2)得到的智能车的位置信息进行融合，得到智能车精准的位置估计；

卡尔曼滤波模型中，步骤(1)通过SLAM算法得到的智能车在大地坐标系下的位姿作为预测值；步骤(2)通过GPS得到的智能车在大地坐标系下的位姿作为观测值；

定义状态空间转移方程和状态空间的观测方程如下：

X(k)＝AX(k-1)+W(k)，

Z(k)＝HX(k)+V(k)，

其中X(k)为k时刻的位姿状态向量，A为过程矩阵，H为测量矩阵，W(k)为过程噪声矩阵，其协方差为Q，V(k)为测量噪声，其协方差为R；卡尔曼滤波的过程如下：

预测方程组：

X(k|k-1)＝AX(k-1|k-1)，

P(k|k-1)＝AP(k-1|k-1)A′+Q，

更新方程组：

X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))，

P(k|k)＝(1-Kg(k)H)P(k|k-1)，

其中：

X(k|k-1)是k时刻的预估位姿状态向量；

X(k-1|k-1)是k-1时刻的最优位姿状态；

Kg(k)是k时刻的卡尔曼增益；

X(k|k)是k时刻的最优位姿状态估计；

Q是系统过程噪声W(k)的协方差；

R是系统测量噪声V(k)的协方差。