一种基于多传感器信息融合的智能车定位方法与流程
本发明涉及智能车定位方法领域，具体是一种基于多传感器信息融合的智能车定位方法。
背景技术：
智能车自主驾驶或辅助驾驶的前提之一就是要准确知道自己的位置。当前智能车自主驾驶或辅助驾驶领域，使用最多的定位方法是基于GPS定位，或者GPS与里程计融合进行定位。单独使用GPS设备来定位，由于设备本身的误差，定位往往会出现漂移现象，同时在某些场合，比如隧道、高楼林立的市区，GPS信号会迅速减弱甚至丢失。GPS与里程计融合定位，在GPS信号弱时会更倾向使用里程计来推算定位，进而减少纯靠GPS定位引发的误差，但是由于里程计在车辆行驶过程中会有累积误差和漂移，里程计推算定位同样不精准。
本发明使用同步定位与地图构建技术，使用雷达数据对里程计数据进行校正，得到智能车局部的位置估计，再结合GPS定位，通过坐标转换和卡尔曼滤波，将二者进行融合，得到精准的智能车位置估计。相对于GPS与里程计融合定位，本发明能够更好地完成智能车的定位，为智能车的自动驾驶或辅助驾驶提供保障。
技术实现要素：
本发明的目的是提供一种基于多传感器信息融合的智能车定位方法，以解决现有技术智能车自主驾驶或辅助驾驶领域GPS定位不准的的问题。
为了达到上述目的，本发明所采用的技术方案为：
一种基于多传感器信息融合的智能车定位方法，其特征在于：包括以下步骤：
（1）、基于智能车的光电编码器和激光雷达采集的数据，并利用同步定位与地图构建方法（SLAM）得到智能车在SLAM方法构建的局部地图中的位置信息；
（2）、通过智能车的GPS定位获得智能车在地理坐标系中的位置信息；
（3）、将步骤（1）和步骤（2）得到的智能车在局部地图坐标系中、地理坐标系中的位置信息通过坐标转换统一在大地坐标系中；
（4）、基于步骤（3）的坐标转换过程，采用卡尔曼滤波技术将步骤（1）得到的智能车的位置信息、步骤（2）得到的智能车的位置信息进行融合，得到智能车精准的位置估计。
所述的一种基于多传感器信息融合的智能车定位方法，其特征在于：步骤（1）的具体过程如下：
（1.1）、以智能车启动时车体中心为原点Oo，沿车前进方向为Xo轴方向，垂直Xo轴指向车体左侧为Yo轴方向，Zo轴垂直向上建立里程计坐标系。由于只考虑智能车在水平面内运动且没有俯仰和翻滚动作，因此Z轴坐标始终为0且只有偏航角的变化，因此可以得到智能车的位姿模型(x, y, θ)。通过智能车后轮左、右轮的光电编码器，推算智能车里程计信息，具体过程如下：
（1.1.1）、0时刻，智能车在里程计坐标系中的位姿(x0, y0, θ0)为(0,0,0).
（1.1.1）、单位采样时间内，在里程计坐标系下，车轮位置的增量ΔS可以通过以下公式得到：
其中ΔQ为光电码盘输出的脉冲增量，D为车轮直径，Ts为采样时间，N为光电码盘上的光栅总数，K为光电编码器减速率；
（1.1.2）、左右轮之间的距离为ω，单位采样时间内左、右轮位置的增量分别为ΔSL和ΔSR，则汽车从k – 1时刻Sk-1 = (xk-1,yk-1,θk-1)到k时刻Sk = (xk,yk,θk)的位姿变化为
（1.1.3）、使用上述方法可以累积得到时刻k智能车的里程计信息；
（1.2）、通过车载激光雷达获得并记录时刻k智能车周边环境的观测信息；
（1.3）、建立与里程计坐标系同原点同轴的地图坐标系，并在地图坐标系下通过SLAM算法来进行构图和定位。通过步骤（1.1）得到的里程计信息作为SLAM算法的控制输入信息，通过步骤（1.2）得到的雷达扫描信息作为SLAM算法观测信息，使用一种基于粒子滤波器的SLAM算法得到智能车在地图坐标系下的优化位姿，同时根据优化后的位姿将k时刻智能车周边的障碍物信息进行建图。
所述的一种基于多传感器信息融合的智能车定位方法，其特征在于：步骤（1.3）中，使用粒子滤波器进行优化以得到智能车的更优位姿与地图时，智能车当前时刻的位姿根据前一时刻的位姿来推算得到的。知道k-1时刻智能车的位姿xk-1，k时刻的控制信息即里程计信息uk，k时刻的观测信息zk，估计智能车的当前位姿的xk的后验概率；
在粒子滤波器中采用序贯重要性采样算法和采样重要性重采样算法来获得后验概率分布，通过对参考分布的采样粒子进行加权来近似得到后验概率分布，
具体包括以下步骤：
（1.3.1）、初始化系统，设置N个粒子，每个粒子代表智能车当前的位姿，采样 QUOTE ，计算权重 QUOTE ；
（1.3.2）、重要性采样，在时刻k，根据k时刻智能车的控制信息uk推算k时刻智能车位姿的分布，对每个粒子进行采样 QUOTE ；
（1.3.3）、根据k时刻的观测信息zk和粒子前一时刻的权值，计算k时刻粒子重要性权值 QUOTE ，并进行归一化权值;
（1.3.4）、重采样，增加重要性权值高的粒子，删除重要性权值低的粒子，计算后验概率。
所述的一种基于多传感器信息融合的智能车定位方法，其特征在于：步骤（2）中，通过智能车的GPS定位获得智能车在WGS-84地理坐标系中的原始纬度、经度、高度坐标。
所述的一种基于多传感器信息融合的智能车定位方法，其特征在于：步骤（3）中坐标转换统一过程如下：
（3.1）、将智能车在WGS-84坐标系中的原始纬度、经度采用通用的高斯投影，转换到平面坐标系，并转换到以智能车起始位置车体中心为原点的“东北天”大地坐标系，并以此坐标系为主坐标系;
（3.2）、将智能车SLAM算法中的地图坐标系转换到大地坐标系，具体转换方法如下：智能车启动时通过GPS设备获得启动时刻智能车的经、纬度和航向角φ，通过步骤（3.1）的方法得到此时智能车在大地坐标系中的坐标(x0,y0)，则在地图坐标系中的点(xm,ym)在大地坐标系中坐标(xp,yp)为：
所述的一种基于多传感器信息融合的智能车定位方法，其特征在于：步骤（4）的卡尔曼滤波模型中，步骤（1）通过SLAM算法得到的智能车在大地坐标系下的位姿作为预测值；步骤（2）通过GPS得到的智能车在大地坐标系下的位姿作为观测值。
定义状态空间转移方程和状态空间的观测方程如下：
，
，
其中X(k)为k时刻的位姿状态向量，A(k)为过程矩阵，H(k)为测量矩阵，W(k)为过程噪声矩阵，其协方差为Q，V(k)为测量噪声，其协方差为R。卡尔曼滤波的过程如下：
预测方程组：
，
，
更新方程组：
，
，
，
其中：
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)的协方差。
所述的一种基于多传感器信息融合的智能车定位方法，其特征在于：状态噪声矩阵W(k)和测量噪声矩阵V(k)的值均为高斯白噪声，其中测量噪声矩阵V(k)根据GPS设备定位质量自适应调节。
本发明使用同步定位与地图构建技术，使用雷达数据对里程计数据进行校正，得到智能车局部的优化的位置估计，再结合GPS定位，通过坐标转换和卡尔曼滤波，将二者进行融合，得到精准的智能车位置估计。相对于GPS与里程计融合定位，本发明能够更好地完成智能车的定位，为智能车的自动驾驶或辅助驾驶提供保障。
与已有技术相比，本发明的有益效果体现在：
本发明的一种基于激光雷达、编码器、GPS多信息融合的智能车定位方法，与现有的智能车的定位方法相比，在多利用一个传感器激光雷达的情况下，使用先进的同步定位与构建方法，并采用信息融合的方法，有效地提高智能车的定位精度，为智能车自动驾驶或辅助驾驶提供保障。
附图说明
图1是本发明的基于多传感器信息融合的智能车定位方法的流程图。
图2是本发明使用的同步定位与地图构建方法的结构图。
图3是本发明的智能车传感器结构图。
具体实施方式
下面结合附图对本发明作进一步详细说明。
如图1所示，一种基于多传感器信息融合的智能车定位方法，包括以下步骤：
1、一种基于多传感器信息融合的智能车定位方法，其特征在于：包括以下步骤：
（1）、基于智能车的光电编码器和激光雷达采集的数据，并利用同步定位与地图构建方法（SLAM）得到智能车在SLAM方法构建的局部地图中的位置信息；
（2）、通过智能车的GPS定位获得智能车在地理坐标系中的位置信息；
（3）、将步骤（1）和步骤（2）得到的智能车在局部地图坐标系中、地理坐标系中的位置信息通过坐标转换统一在大地坐标系中；
（4）、基于步骤（3）的坐标转换过程，采用卡尔曼滤波技术将步骤（1）得到的智能车的位置信息、步骤（2）得到的智能车的位置信息进行融合，得到智能车精准的位置估计。
对于步骤1所述的同步定位与地图构建方法，具体流程如图2，同时按照图3所示，将激光雷达装在智能车车头正中央，编码器装在后轮的左右轮的轴上，算法具体包括以下步骤：
（1.1）、以智能车启动时车体中心为原点Oo，沿车前进方向为Xo轴方向，垂直Xo轴指向车体左侧为Yo轴方向，Zo轴垂直向上建立里程计坐标系。由于只考虑智能车在水平面内运动且没有俯仰和翻滚动作，因此Z轴坐标始终为0且只有偏航角的变化，因此可以得到智能车的位姿模型(x, y, θ)。通过智能车后轮左、右轮的光电编码器，推算智能车里程计信息，具体过程如下：
（1.1.1）、0时刻，智能车在里程计坐标系中的位姿(x0, y0, θ0)为(0,0,0).
（1.1.1）、单位采样时间内，在里程计坐标系下，车轮位置的增量ΔS可以通过以下公式得到：
其中ΔQ为光电码盘输出的脉冲增量，D为车轮直径，Ts为采样时间，N为光电码盘上的光栅总数，K为光电编码器减速率；
（1.1.2）、左右轮之间的距离为ω，单位采样时间内左、右轮位置的增量分别为ΔSL和ΔSR，则汽车从k – 1时刻Sk-1 = (xk-1,yk-1,θk-1)到k时刻Sk = (xk,yk,θk)的位姿变化为
（1.1.3）、使用上述方法可以累积得到时刻k智能车的里程计信息；
（1.2）、通过车载激光雷达获得并记录时刻k智能车周边环境的观测信息；
（1.3）、建立与里程计坐标系同原点同轴的地图坐标系，并在地图坐标系下通过SLAM算法来进行构图和定位。通过步骤（1.1）得到的里程计信息作为SLAM算法的控制输入信息，通过步骤（1.2）得到的雷达扫描信息作为SLAM算法观测信息，使用一种基于粒子滤波器的SLAM算法得到智能车在地图坐标系下的优化位姿，同时根据优化后的位姿将k时刻智能车周边的障碍物信息进行建图。
步骤（1.3）中，使用粒子滤波器进行优化以得到智能车的更优位姿与地图时，智能车当前时刻的位姿根据前一时刻的位姿来推算得到的。知道k-1时刻智能车的位姿xk-1，k时刻的控制信息即里程计信息uk，k时刻的观测信息zk，估计智能车的当前位姿的xk的后验概率；
在粒子滤波器中采用序贯重要性采样算法和采样重要性重采样算法来获得后验概率分布，通过对参考分布的采样粒子进行加权来近似得到后验概率分布，
具体包括以下步骤：
（1.3.1）、初始化系统，设置N个粒子，每个粒子代表智能车当前的位姿，采样 QUOTE ，计算权重 QUOTE ；
（1.3.2）、重要性采样，在时刻k，根据k时刻智能车的控制信息uk推算k时刻智能车位姿的分布，对每个粒子进行采样 QUOTE ；
（1.3.3）、根据k时刻的观测信息zk和粒子前一时刻的权值，计算k时刻粒子重要性权值 QUOTE ，并进行归一化权值;
（1.3.4）、重采样，增加重要性权值高的粒子，删除重要性权值低的粒子，计算后验概率。
步骤（2）中，通过智能车的GPS定位获得智能车在WGS-84地理坐标系中的原始纬度、经度、高度坐标。
步骤（3）中坐标转换统一过程如下：
（3.1）、将智能车在WGS-84坐标系中的原始纬度、经度采用通用的高斯投影，转换到平面坐标系，并转换到以智能车起始位置车体中心为原点的“东北天”大地坐标系，并以此坐标系为主坐标系;
（3.2）、将智能车SLAM算法中的地图坐标系转换到大地坐标系，具体转换方法如下：智能车启动时通过GPS设备获得启动时刻智能车的经、纬度和航向角φ，通过步骤（3.1）的方法得到此时智能车在大地坐标系中的坐标(x0,y0)，则在地图坐标系中的点(xm,ym)在大地坐标系中坐标(xp,yp)为：
步骤（4）的卡尔曼滤波模型中，步骤（1）通过SLAM算法得到的智能车在大地坐标系下的位姿作为预测值；步骤（2）通过GPS得到的智能车在大地坐标系下的位姿作为观测值。
定义状态空间转移方程和状态空间的观测方程如下：
，
，
其中X(k)为k时刻的位姿状态向量，A(k)为过程矩阵，H(k)为测量矩阵，W(k)为过程噪声矩阵，其协方差为Q，V(k)为测量噪声，其协方差为R。卡尔曼滤波的过程如下：
预测方程组：
，
，
更新方程组：
，
，
，
其中：
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)的协方差。
状态噪声矩阵W(k)和测量噪声矩阵V(k)的值均为高斯白噪声，其中测量噪声矩阵V(k)根据GPS设备定位质量自适应调节。
- 还没有人留言评论。精彩留言会获得点赞！