[发明专利]一种用于自动驾驶的低成本传感器组合定位方法

说明书

本发明公开了一种用于自动驾驶的低成本传感器组合定位方法，通过采用低成本GPS、IMU传感器，采集车辆信息输入扩展卡尔曼滤波器中对车辆进行实施定位，本发明在卡尔曼滤波器的参数中加入速度比例因子和航向角偏差用于实时修正车速和航向角；同时，在车辆速度较大、GPS定位稳定时，用GPS的航向信息修正IMU的航向信息。本发明不仅降低定位套件的硬件成本，便于自动驾驶市场化；而且最终使整个系统定位精度可达亚米级。

技术领域

本发明属于自动驾驶领域，特别涉及一种用于自动驾驶的低成本传感器组合定位方法。

背景技术

组合导航是自动驾驶领域的的重要研究课题，目前常用价格高昂的高精度GPS和IMU。RTK(高精度GPS)套件设备价格数万元；此外其移动终端通过通信模块与基站和数据处理中心处理卫星信号，在通信环境恶劣地方，其无法有效工作。高精度IMU由磁力计、陀螺仪、加速度计组成，可获取当前车辆的姿态信息。工业级IMU的价格在数千到数万元之间。常用组合导航设备因传感器价格昂贵无法，无法实现自动驾驶技术市场化。

发明内容

发明目的：本发明针对现有技术存在的问题，提出了一种成本低，定位效果好的用于自动驾驶的低成本传感器组合定位方法。

技术方案：为实现上述目的，本发明提供了一种用于自动驾驶的低成本传感器组合定位方法，包括以下步骤：

步骤1：车辆速度信息采集单元、车辆航向信息采集单元和车辆位置信息采集单元分别将采集到车辆的速度、车辆的航向信息和车辆的位置信息发送到车辆位置定位单元；

步骤2：在车辆位置定位单元构建卡尔曼滤波器，初始化卡尔曼滤波器参数；其中，卡尔曼滤波器参数包括系统协方差矩阵P、测量噪声协方差矩阵R、系统状态量矩阵X(x,y,v,yaw,yaw_v,v_scale,yaw_bias)和观测值矩阵Z(z_x，z_y，z_yaw，z_v)，其中，x表示输入卡尔曼滤波器的车辆在UTM坐标系下的横坐标，y表示输入卡尔曼滤波器的车辆在UTM坐标系下的纵坐标，v表示输入卡尔曼滤波器的车辆的速度，yaw表示输入卡尔曼滤波器的车辆的航向角，yaw_v表示输入卡尔曼滤波器的车辆的航向角速度，v_scale表示输入卡尔曼滤波器的速度比例因子，yaw_bias表示输入卡尔曼滤波器的航向角偏差z_x，z_y表示车辆位置信息采集单元输出的位置信息，z_yaw是车辆航向信息采集单元输出的航向角，z_v为车辆速度信息采集单元输出的速度信息；

步骤3：卡尔曼滤波器根据接收到的车辆的速度、车辆的航向信息和车辆的位置信息，结合其中的预测模型和更新模型不断的迭代得到最终的车辆定位信息并输出。

进一步，所述预测模型为：

x_t＝x_t-1+v_t*cos(yaw_t)*Δt；

y_t＝y_t-1+v_t*sin(yaw_t)*Δt；