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

Abstract

本发明公开了一种用于自动驾驶的低成本传感器组合定位***，通过采用低成本GPS、IMU传感器，根据低成本GPS的定位特性设计一套状态机，用于动态调整卡尔曼滤波器参数，然后通过卡尔曼滤波器的预测和状态更新模块输出车辆当前的位置以及速度和航向信息。本发明还提供了一种用于自动驾驶的低成本传感器组合定位方法。本发明不仅降低定位套件的硬件成本，便于自动驾驶市场化；而且最终使整个***定位精度可达亚米级。

Description

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

技术领域

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

背景技术

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

发明内容

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

技术方案：为实现上述目的，本发明提供了一种用于自动驾驶的低成本传感器组合定位***，包括车辆速度信息采集单元、车辆航向信息采集单元、车辆位置信息采集单元和车辆位置定位单元，其中，车辆速度信息采集单元用于采集车辆的实时车速信息；车辆航向信息采集单元用于实时采集车辆的航向信息；车辆位置信息采集单元为GPS,用于实时采集车辆位置信息；所述车辆定位单元中包括状态机和卡尔曼滤波器，所述状态机根据GPS反馈的参数对卡尔曼滤波器的参数进行调整，所述卡尔曼滤波器根据车辆速度信息采集单元、车辆航向信息采集单元和车辆位置信息采集单元输入的车辆的信息对车辆进行定位。

进一步，所述状态机中包括重启状态、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_ba，则进入停车/缓慢行驶状态；若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为判断车辆速度参数。这样能够有效的提高定位的精度。

其中，h_good的范围为0.55-0.65，h_mid的范围为0.68-0.73，h_bad的范围为0.75-0.77。v_good的范围为3-5m/s，v_mid的范围为1-2m/s，v_bad的范围为0.3_0.5m/s。这样有效的保证了整个方法的鲁棒性，保证了状态见切换的合理性，使定位效果更好。

进一步，车辆速度信息采集单元采用设置在车辆上的ODOM，ODOM采集车辆的前进速度信息和旋转速度；车辆航向信息采集单元采用装在车辆旋转中心位置的IMU，其主要采集和提供车辆的航向信息；车辆位置信息采集单元采用设置在车顶并且位于车辆旋转中心位置的GPS，其主要采集和提供车辆的位置信息。

本发明还提供了一种基于上述用于自动驾驶的低成本传感器组合定位***的组合定位方法，其包括以下步骤：

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

步骤2：在车辆位置定位单元构建卡尔曼滤波器，车辆位置定位单元中的状态机根据获得的车辆位置信息，初始化或者修改卡尔曼滤波器参数；

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

进一步，其中卡尔曼滤波器参数包括***协方差矩阵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表示GPS输出的位置信息，z_yaw是IMU输出的航向角，z_v为ODMO输出的速度信息，z_yawv是IMU输出的航向角的角速度。速度比例因子v_scale解决由车辆轮胎气压、车辆载重、路面不平整等造成的测量速度和真实速度不一致的情况，引入航向角偏差yaw_bias解决道路磁场环境变换引起的航向角不稳定的情况，这两个状态量能够根据GPS的观测数据和卡尔曼滤波算法的预测模型动态调整。更加有效的提高对车辆定位的精度。

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

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中还包括用GPS采集的其航向信息修正车辆航向信息采集单元输入到卡尔曼滤波器中的航向信息；根据公式：

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值。这样能够有效的提高***的定位精度。

进一步，所述步骤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_ba，则进入停车/缓慢行驶状态；若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的定位特性设计一套状态机，用于动态调整卡尔曼滤波器参数，然后通过卡尔曼滤波器的预测和状态更新模块输出车辆当前的位置以及速度和航向信息。车辆的ODOM信息可从车辆的CAN总线读取，其速度和角速度值分别由安装在左右车轮的脉冲计数器求和平均和做差求出。本发明还加入速度比例因子和航向角偏差用于实时修正车速和航向角；同时，在车辆速度较大、GPS定位稳定时，用GPS的航向信息修正IMU的航向信息。

有益效果：与现有技术相比，本发明利采用的低成本GPS和IMU，降低定位套件的硬件成本，便于自动驾驶市场化；同时，通过状态机调参机制，最终***定位精度可达亚米级。

附图说明

图1为本发明的***示意图；

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

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

具体实施方式

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

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

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

本实施例公开的采用上述用于自动驾驶的低成本传感器的定位***的组合定位方法，具体包括以下步骤：

步骤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}表示车辆航向角的方差值，

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

表示车辆航向角偏差的方差，其中，矩阵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在横向和纵向位置的方差。

如图2所示，当***上电后，进入重启状态，在重启状态时，当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_ba，说明车速降低，则进入停车/缓慢行驶状态；若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异常的情况。

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

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

表1：

Claims (5)

1.一种用于自动驾驶的低成本传感器组合定位***，其特征在于：包括车辆速度信息采集单元、车辆航向信息采集单元、车辆位置信息采集单元和车辆位置定位单元，其中，车辆速度信息采集单元用于采集车辆的实时车速信息；车辆航向信息采集单元用于实时采集车辆的航向信息；车辆位置信息采集单元为GPS，用于实时采集车辆位置信息；所述车辆位置定位单元包括状态机和卡尔曼滤波器，所述状态机根据GPS反馈的参数对卡尔曼滤波器的参数进行调整，所述卡尔曼滤波器根据车辆速度信息采集单元、车辆航向信息采集单元和车辆位置信息采集单元输出的车辆的信息，结合卡尔曼滤波器的预测模型和更新模型不断的迭代得到最终的车辆定位信息并输出对车辆进行定位；所述预测模型为：

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时刻航向角的角速度；

所述状态机中包括重启状态、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为判断车辆速度参数；车辆速度信息采集单元采用设置在车辆上的ODOM，ODOM采集车辆的前进速度信息和旋转速度；车辆航向信息采集单元采用装在车辆旋转中心位置的IMU，其主要采集和提供车辆的航向信息；车辆位置信息采集单元采用设置在车顶并且位于车辆旋转中心位置的GPS，其主要采集和提供车辆的位置信息。

2.根据权利要求1所述的用于自动驾驶的低成本传感器组合定位***，其特征在于：h_good的范围为0.55-0.65，h_mid的范围为0.68-0.73，h_bad的范围为0.75-0.77。

3.根据权利要求1所述的用于自动驾驶的低成本传感器组合定位***，其特征在于：v_good的范围为3-5m/s，v_mid的范围为1-2m/s，v_bad的范围为0.3-0.5m/s。

4.一种基于权利要求1所述的用于自动驾驶的低成本传感器组合定位***的定位方法，其特征在于：包括以下步骤：

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

步骤2：在车辆位置定位单元构建卡尔曼滤波器，车辆位置定位单元中的状态机根据获得的车辆位置信息，初始化或者修改卡尔曼滤波器参数；

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

所述步骤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为判断车辆速度参数；

所述预测模型为：

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时刻航向角的角速度。

5.根据权利要求4所述的定位方法，其特征在于：所述步骤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为IMU 输出的航向角，yaw_offset为偏移角，yaw_input为输入卡尔曼滤波器的yaw值。

Images