CN110940344B - 一种用于自动驾驶的低成本传感器组合定位方法 - Google Patents

Abstract

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

Description

一种用于自动驾驶的低成本传感器组合定位方法

技术领域

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

背景技术

组合导航是自动驾驶领域的的重要研究课题，目前常用价格高昂的高精度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；

上式中v_t表示在t时刻车辆的速度，yaw_t表示车辆在t时刻的航向角，x_t、y_t分别表示在t时刻车辆在UTM坐标系下的横坐标和纵坐标，v_t-1表示t-1时刻车辆的速度，yaw_t-1表示t-1时刻车辆的航向角，x_t-1和y_t-1分别代表在t-1时刻车辆在UTM坐标系下的横坐标和纵坐标，Δt表示时间差，

表示t-1时刻车辆的速度比例因子，

表示t-1时刻车辆的航向角偏差，

为t-1时刻航向角的角速度。

其中，v_scale根据车辆位置信息采集单元输出的位置信息和车辆速度信息采集单元输出的速度结合预测模型自行调节更新。

yaw_bias根据车辆位置信息采集单元输出的位置信息和车辆航向信息采集单元输出的航向角结合预测模型自行调节更新。

进一步，所述步骤2中还包括用GPS采集的其航向信息修正车辆航向信息采集单元输入到卡尔曼滤波器中的航向信息；根据公式：

yaw_offset＝yaw_gps-yaw_imu；

yaw_input＝yaw_imu+yaw_offest；

对车辆航向信息采集单元输入到卡尔曼滤波器中的航向信息进行修正，式中yaw_gps为GPS星数大于等于12，车速大于5m/s、水平精度因子小于0.6时，RMC报文输出的航向角度，yaw_imu为车辆航向信息采集单元输出的航向角，yaw_offset为偏移角，yaw_input为输入卡尔曼滤波器的yaw值。

再者，所述步骤2还包括初始化或者修改卡尔曼滤波器参数，其方法为：设定重启状态、GPS无效状态、GPS有效状态和停车/缓慢行驶状态，其中，重启状态时，初始化卡尔曼滤波器参数中的系统协方差矩阵P、测量噪声协方差矩阵R和系统状态量矩阵X；GPS无效状态时，GPS采集的位置信息不输入卡尔曼滤波器；GPS有效状态时，GPS采集的位置信息输入卡尔曼滤波器；停车/缓慢行驶状态时，将GPS的位置信息每间隔1秒输入卡尔曼滤波器一次；当系统上电后，进入重启状态，在重启状态时，当hdop>h_mid或者star_num<10时候，进入GPS无效状态；当hdop<＝h_mid且v_car>v_mid时，进入GPS有效状态；当hdop<＝h_mid且v_car<＝v_mid时，进入停车/缓慢行驶状态；在GPS无效状态时，若hdop<h_good，进入GPS有效状态；若P异常，进入重启状态；在GPS有效状态时，若hdop>h_bad,进入GPS无效状态；若v_car<v_bad，则进入停车/缓慢行驶状态；若P异常，进入重启状态；在停车/缓慢行驶状态时，若v_car>v_good，进入GPS有效状态；若hdop>h_bad，进入GPS无效状态；若P异常，进入重启状态；hdop为水平精度因子、star_num为接收星数、v_car为从ODOM读取的当前车速、P为卡尔曼滤波器的系统协方差矩阵，h_good、h_mid、h_bad为判断GPS定位精度参数，v_good、v_bad、v_mid为判断车辆速度参数。

工作原理：本发明提出的定位方案采用低成本GPS、IMU传感器，其中IMU输出的航向信息(Yaw)、GPS提供的位置信息(Position)、车辆ODOM提供的速度(Velocity)和角速度(Yaw_rate)输入扩展卡尔曼滤波器，本发明加入速度比例因子和航向角偏差用于实时修正车速和航向角；同时，在车辆速度较大、GPS定位稳定时，用GPS的航向信息修正IMU的航向信息。

有益效果：与现有技术相比，本发明利采用的低成本GPS和IMU，降低定位套件的硬件成本，便于自动驾驶市场化；最终系统定位精度可达亚米级。

附图说明

图1为本发明的系统示意图；

图2为本发明提供的方法流程图；

图3为状态机工作流程示意图；

图4为本发明与现有技术的车辆行驶轨迹对比图。

具体实施方式

下面将结合本发明实例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本发明提供了一种用于自动驾驶的导航组合定位系统，包括车辆速度信息采集单元、车辆航向信息采集单元、车辆位置信息采集单元和车辆位置定位单元，其中车辆速度信息采集单元、车辆航向信息采集单元和车辆位置信息采集单元将采集到的信息发送到车辆位置定位单元，车辆位置定位单元采用引入了速度比例因子v_scale和航向角偏差yaw_bias的卡尔曼滤波器对接收到的车辆的信息进行融合，从而得到车辆当前的定位信息，定位信息包括车辆当前的速度，航向角、在UTM平面坐标系下的横坐标和纵坐标。

其中，车辆速度信息采集单元采用设置在车辆上的ODOM，ODOM采集车辆的前进速度信息和旋转速度，其从车上Can总线上可读取，自动驾驶车辆都含有此信息，其通过左右轮脉冲计数器求得；车辆航向信息采集单元采用装在车辆旋转中心位置的IMU，其主要采集和提供车辆的航向信息，本实施例中采用mti-3型号的IMU；车辆位置信息采集单元采用设置在车顶并且位于车辆旋转中心位置的GPS，其主要采集和提供车辆的位置信息，即车辆所在位置的经度和纬度，本实施例中采用GN200B型号的GPS。

如图2所示，本实施例公开的采用上述用于自动驾驶的导航的定位系统的组合定位方法，具体包括以下步骤：

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

步骤2：在车辆位置定位单元构建卡尔曼滤波器，车辆位置定位单元根据获得的车辆位置信息，初始化或者修改卡尔曼滤波器参数，其中卡尔曼滤波器参数包括系统协方差矩阵

测量噪声协方差矩阵

系统状态量矩阵X(x,y,v,yaw,yaw_v,v_scale,yaw_bias)和观测值矩阵Z(z_x，z_y，z_yaw，

z_v)，其中，P_{x_x}表示车辆在UTM坐标系下的横坐标的方差值，p_{x_y}表示车辆在UTM坐标系下的横坐标与纵坐标的协方差值，p_{x_v}表示车辆在UTM坐标系下的横坐标与车辆速度的协方差值，P_{x_yaw}表示车辆在UTM坐标系下的横坐标与航向角的协方差值，

表示车辆在UTM坐标系下的横坐标与航向角速度的协方差值，

表示车辆在UTM坐标系下的横坐标与速度比例因子的协方差值，

表示车辆在UTM坐标系下的横坐标与航向角偏差的协方差值，P_{y_x}表示车辆在UTM坐标系下的纵坐标与横坐标的协方差值，p_{y_y}表示车辆在UTM坐标系下的纵坐标的方差值，p_{y_v}表示车辆在UTM坐标系下的纵坐标与车辆速度的协方差值，p_{y_yaw}表示车辆在UTM坐标系下的纵坐标与航向角的协方差值，

表示车辆在UTM坐标系下的纵坐标与航向角速度的协方差值，

表示车辆在UTM坐标系下的纵坐标与速度比例因子的协方差值，

表示车辆在UTM坐标系下的纵坐标与航向角偏差的协方差值，P_{v_x}表示车辆的速度与在UTM坐标系下横坐标的协方差值，p_{v_y}表示车辆的速度与在UTM坐标系下纵坐标的协方差值，p_{v_v}表示车辆速度的方差值，p_{v_yaw}表示车辆速度与航向角的协方差值，

表示车辆速度与航向角速度的协方差值，

表示车辆速度与速度比例因子的协方差值，

表示车辆速度与航向角偏差的协方差值，P_{yaw_x}表示车辆的航向角与车辆在UTM坐标系下的横坐标的协方差值，p_{yaw_y}表示车辆的航向角与车辆在UTM坐标系下的纵坐标的协方差值，p_{yaw_v}表示车辆的航向角与车辆速度的协方差值，p_{yaw_yaw}表示车辆航向角的方差值，

表示车辆航向角与航向角速度的协方差值，

表示车辆航向角与速度比例因子的协方差值，

表示车辆航向角与航向角偏差的协方差值，

表示车辆航向角速度与车辆在UTM坐标系下的横坐标的协方差值，

表示车辆航向角速度与车辆在UTM坐标系下的纵坐标的协方差值，

表示车辆航向角速度与车辆速度的协方差值，

表示车辆航向角速度与车辆航向角的协方差值，

表示车辆航向角速度的方差值，

表示车辆航向角速度与速度比例因子的协方差值，

表示车辆航向角速度与航向角偏差的协方差值，

表示车辆的速度比例因子与车辆在UTM坐标系下的横坐标的协方差值，

表示车辆的速度比例因子与车辆在UTM坐标系下的纵坐标的协方差值，

表示车辆的速度比例因子与车辆速度的协方差值，

表示车辆的速度比例因子与车辆航向角的协方差值，

表示车辆的速度比例因子与车辆航向角速度的协方差值，

表示车辆的速度比例因子的方差值，

表示车辆的速度比例因子与车辆航向角偏差的协方差值，

表示车辆航向角偏差与车辆在UTM坐标系下的横坐标的协方差值，

表示车辆航向角偏差与车辆在UTM坐标系下的纵坐标的协方差值，

表示车辆航向角偏差与车辆速度的协方差值，

表示车辆航向角偏差与车辆航向角的协方差值，

表示车辆航向角偏差与车辆航向角速度的协方差值，

表示车辆航向角偏差与车辆的速度比例因子的协方差值，

表示车辆航向角偏差的方差值；r_{x_x}表示GPS在横向位置的方差，r_{y_y}表示GPS在纵向位置的方差，r_{v_v}表示车辆ODOM输出的速度的方差，r_{yaw_yaw}表示IMU输出的车辆航向角的方差，

表示IMU输出的车辆航向角速度的方差，

表示车辆的速度比例因子的方差，

表示车辆航向角偏差的方差，其中，矩阵R中的元素是根据传感器的特性设定的方差，其与传感器的精度相关，

和

的值为0；x表示输入卡尔曼滤波器的车辆在UTM坐标系下的横坐标，y表示输入卡尔曼滤波器的车辆在UTM坐标系下的纵坐标，v表示输入卡尔曼滤波器的车辆的速度，yaw表示输入卡尔曼滤波器的车辆的航向角，yaw_v表示输入卡尔曼滤波器的车辆的航向角速度，v_scale表示输入卡尔曼滤波器的速度比例因子，yaw_bias表示输入卡尔曼滤波器的航向角偏差；z_x和z_y分别表示GPS输出的位置信息在UTM坐标系下的横坐标和纵坐标，z_yaw是IMU输出的航向角，z_v为ODMO输出的速度信息，

是IMU输出的航向角的角速度。

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

步骤301：根据卡尔曼滤波器中的CTRV预测模型分别预测下一时刻的系统状态量X中每个量的值；其中，CTRV预测模型为：

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

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

上式中v_t表示在t时刻车辆的速度，yaw_t表示车辆在t时刻的航向角，x_t、y_t分别表示在t时刻车辆在UTM坐标系下的横坐标和纵坐标，v_t-1表示t-1时刻车辆的速度，yaw_t-1表示t-1时刻车辆的航向角，x_t-1和y_t-1分别代表在t-1时刻车辆在UTM坐标系下的横坐标和纵坐标，Δt表示时间差，

表示t-1时刻车辆的速度比例因子，

表示t-1时刻车辆的航向角偏差，

为t-1时刻航向角的角速度。

步骤302：把车辆位置信息采集单元采集的车辆的经纬度数据转转化到UTM平面坐标，卡尔曼滤波器中的更新模型分别根据GPS、IMU、ODOM输出的位置信息、航向、速度值，更新系统状态量矩阵X(x,y,v,yaw,yaw_v,v_scale,yaw_bias)；

其中，v_scale的更新方法：根据预测模型和根据GPS输出的位置(z_x,z_y)和车辆的速度z_v可以计算出v_scale的值，在预测模型计算中系统车辆的速度的计算为

其中含有上一时刻的速度v_t-1和比例因子v_scale，同时将速度v_t带入到公式x_t＝x_t-1+v_t*cos(yaw_t)*Δt和y_t＝y_t-1+v_t*sin(yaw_t)*Δt中；在更新模型中，传感器会输入GPS的位置信息(x,y)和速度v，此时会根据预测值和传感器的输出值和其噪声协方差矩阵R和协方差矩阵P更新系统变量X中的v_scale，用以修正车辆速度。

yaw_bias的更新方法：根据预测模型和根据GPS输出的位置(z_x,z_y)和IMU输出的航向角yaw和yaw_v,可以计算出yaw_bias的值。在预测模型计算中系统车辆的航向角的计算公式为：

其中含有yaw_bias；将航向角yaw_t带入到公式x_t＝x_t-1+v_t*cos(yaw_t)*Δt和y_t＝y_t-1+v_t*sin(yaw_t)*Δt中；在更新模块部分，传感器会输入GPS的位置信息(x，y)和imu输出的航向角以及角速度，此时会根据预测值和传感器的输出值和其噪声协方差矩阵R和协方差矩阵P更新系统变量X中的yaw_bias，用以修正车辆的航向角。

步骤303：不断重复步骤301～步骤302，每0.02秒输出一次车辆的定位信息，输出的定位信息即为当前系统状态量矩阵X。

其中，当GPS定位精度高、车速大的情况的下，用其航向信息修正IMU输入到卡尔曼滤波器中的航向信息。其公式如下：

yaw_offset＝yaw_gps-yaw_imu；

yaw_input＝yaw_imu+yaw_offest；

上式中yaw_gps为GPS星数大于等于12，车速大于5m/s、水平精度因子小于0.6时，RMC报文输出的航向角度，yaw_imu为imu输出的航向角，yaw_offset为偏移角，yaw_input为输入卡尔曼滤波器的yaw值，即为Z_yaw的值。

步骤2中初始化或者修改卡尔曼滤波器参数的方法为：根据GPS在动态和静态情况下定位精度不同、GPS的星数star_num和水平精度因子hdop等参数以及信号特性设置一套状态机调参方法，动态设置系统的观测值Z和测量噪声协方差R。主要包括重启、GPS无效、GPS有效、停车/缓慢行驶四个状态，并据此设置卡尔曼滤波中的测量噪声协方差R，动态调整算法参数。各状态分别如下：

重启状态：初始化卡尔曼滤波器参数中的系统协方差矩阵P、测量噪声协方差矩阵R和系统状态量矩阵X。初始化时，系统状态量矩阵X中的x、y设置为GPS经纬度转换成UTM系下的横、纵坐标，其它值设为0；系统协方差矩阵P对角线上元素值设为0.01,测量噪声协方差矩阵R对角线上元素值设为0.1。

GPS无效状态：当车辆长期经过桥底、隧道等信号遮挡地方时，GPS输出的水平精度因子较高、接收星数较少，此时GPS无效，GPS采集的位置信息不输入卡尔曼滤波器，用系统变量X中的值和传感器输出的速度和航向值输入预测模型进行推算。

GPS有效状态：当车辆在空旷路况或城市道路正常行驶，GPS定位数据可用。但当经过高楼密集或者数目遮挡地方时，GPS的定位数据不稳定，会出现抖动情况。此时根据水平精度因子hdop动态调整GPS观测方差，hdop越小，测量噪声方差越小。即Hdop<0.55时，r_{x_x}和r_{y_y}设置为1.0；0.55<Hdop<0.65时，r_{x_x}和r_{y_y}设置为1.5；0.65<Hdop<0.68时，r_{x_x}和r_{y_y}设置为2.5；0.68<Hdop<0.73时，r_{x_x}和r_{y_y}设置为15；Hdop>0.75时，r_{x_x}和r_{y_y}设置为150。

停车/缓慢行驶状态：当从车辆速度较小时，GPS的定位精度较低。将GPS的位置信息每间隔1秒输入卡尔曼滤波器一次，同时设置GPS的测量噪声方差，将GPS的测量噪声协方差r_{x_x}和r_{y_y}均设置成2.0。其中，r_{x_x}和r_{y_y}分别表示GPS在横向和纵向位置的方差。

如图3所示，当系统上电后，进入重启状态，在重启状态时，当hdop>h_mid或者star_num<10时候，进入GPS无效状态；当hdop<＝h_mid且v_car>v_mid时，进入GPS有效状态；当hdop<＝h_mid且v_car<＝v_mid时，进入停车/缓慢行驶状态。

在GPS无效状态时，若hdop<h_good，说明GPS定位精度提高，此时进入GPS有效状态；若P异常，进入重启状态；

在GPS有效状态时，若hdop>h_bad,说明车辆进入隧道或者高架桥下，此时GPS信息不可以，便进入GPS无效状态；若v_car<v_bad，说明车速降低，则进入停车/缓慢行驶状态；若P异常，进入重启状态；

在停车/缓慢行驶状态时，若v_car>v_good，说明车速提高，则GPS定位精度增加，便进入GPS有效状态；若hdop>h_bad，说明GPS天线受遮挡，便进入GPS无效状态。若P异常，进入重启状态。

其中hdop为水平精度因子、star_num为接收星数、v_car为从ODOM读取的当前车速、P为卡尔曼滤波器的系统协方差，h_good、h_mid、h_bad为算法中判断GPS定位精度参数，h_good的优选范围为0.55-0.65，h_mid的优选范围为0.68-0.73，h_bad的优选范围为0.75-0.77；v_good、v_bad、v_mid为算法中判断车辆速度参数，v_good的优选范围为3-5m/s，v_mid的优选范围为1-2m/s，v_bad的优选范围为0.3—0.5m/s。

其中，当系统初始化或者运行过程中系统的协方差P值出现非正定或其中横纵向和其它参数的协方差值存在明显非对称差异表示P异常。例如，系统协方差矩阵P中的元素p_{x_v}和p_{y_v}差值大于1000则认为是横纵向和其它参数的协方差值存在明显非对称差异，属于P异常的情况。

如图4所示，在城市道路，穿越高架桥时的定位轨迹对比图，其中，黑色轨迹为本发明实现的定位轨迹，灰色轨迹为DJI-A3的定位轨迹。可明显看出黑色轨迹相对平滑，灰色轨迹在桥底出现抖动情况。

采用本发明提供的定位方法与现有技术中的DJI-A3和TrimbleDB982定位效果，以及整个系统的成本等相关内容进行比较，如表一所示，可以看出本发明提供的方法在降低了整个系统硬件成本的同时，保证了系统的定位精度，不管在有遮挡还是没有遮挡的地方，都可以准确的定位到车辆信息。

表1：

Claims (5)

1.一种用于自动驾驶的低成本传感器组合定位方法，其特征在于：包括以下步骤：

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

步骤2：在车辆位置定位单元构建卡尔曼滤波器，初始化卡尔曼滤波器参数；其中，卡尔曼滤波器参数包括系统协方差矩阵P、测量噪声协方差矩阵R、系统状态量矩阵X(x,y,v,yaw,yaw_v,v_scale,yaw_bias)和观测值矩阵

其中，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；

上式中v_t表示在t时刻车辆的速度，yaw_t表示车辆在t时刻的航向角，x_t、y_t分别表示在t时刻车辆在UTM坐标系下的横坐标和纵坐标，v_t-1表示t-1时刻车辆的速度，yaw_t-1表示t-1时刻车辆的航向角，x_t-1和y_t-1分别代表在t-1时刻车辆在UTM坐标系下的横坐标和纵坐标，Δt表示时间差，

表示t-1时刻车辆的速度比例因子，

表示t-1时刻车辆的航向角偏差，

为t-1时刻航向角的角速度。

2.根据权利要求1所述的用于自动驾驶的低成本传感器组合定位方法，其特征在于：v_scale根据车辆位置信息采集单元输出的位置信息和车辆速度信息采集单元输出的速度结合预测模型自行调节更新。

3.根据权利要求1所述的用于自动驾驶的低成本传感器组合定位方法，其特征在于：yaw_bias根据车辆位置信息采集单元输出的位置信息和车辆航向信息采集单元输出的航向角结合预测模型自行调节更新。

4.根据权利要求1所述的用于自动驾驶的低成本传感器组合定位方法，其特征在于：所述步骤2中还包括用GPS采集的其航向信息修正车辆航向信息采集单元输入到卡尔曼滤波器中的航向信息；根据公式：

yaw_offset＝yaw_gps-yaw_imu；

yaw_input＝yaw_imu+yaw_offest；

对车辆航向信息采集单元输入到卡尔曼滤波器中的航向信息进行修正，式中yaw_gps为GPS星数大于等于12，车速大于5m/s、水平精度因子小于0.6时，RMC报文输出的航向角度，yaw_imu为车辆航向信息采集单元输出的航向角，yaw_offset为偏移角，yaw_input为输入卡尔曼滤波器的yaw值。

5.根据权利要求1所述的用于自动驾驶的低成本传感器组合定位方法，其特征在于：所述步骤2还包括初始化或者修改卡尔曼滤波器参数，其方法为：设定重启状态、GPS无效状态、GPS有效状态和停车/缓慢行驶状态，其中，重启状态时，初始化卡尔曼滤波器参数中的系统协方差矩阵P、测量噪声协方差矩阵R和系统状态量矩阵X；GPS无效状态时，GPS采集的位置信息不输入卡尔曼滤波器；GPS有效状态时，GPS采集的位置信息输入卡尔曼滤波器；停车/缓慢行驶状态时，将GPS的位置信息每间隔1秒输入卡尔曼滤波器一次；当系统上电后，进入重启状态，在重启状态时，当hdop>h_mid或者star_num<10时候，进入GPS无效状态；当hdop<＝h_mid且v_car>v_mid时，进入GPS有效状态；当hdop<＝h_mid且v_car<＝v_mid时，进入停车/缓慢行驶状态；在GPS无效状态时，若hdop<h_good，进入GPS有效状态；若P异常，进入重启状态；在GPS有效状态时，若hdop>h_bad,进入GPS无效状态；若v_car<v_bad，则进入停车/缓慢行驶状态；若P异常，进入重启状态；在停车/缓慢行驶状态时，若v_car>v_good，进入GPS有效状态；若hdop>h_bad，进入GPS无效状态；若P异常，进入重启状态；hdop为水平精度因子、star_num为接收星数、v_car为从ODOM读取的当前车速、P为卡尔曼滤波器的系统协方差矩阵，h_good、h_mid、h_bad为判断GPS定位精度参数，v_good、v_bad、v_mid为判断车辆速度参数。

Images