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

权利要求书

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秒输入卡尔曼滤波器一次；当系统上电后，进入重启状态，在重启状态时，当hdoph_mid或者star_num10时候，进入GPS无效状态；当hdop＝h_mid且v_carv_mid时，进入GPS有效状态；当hdop＝h_mid且v_car＝v_mid时，进入停车/缓慢行驶状态；在GPS无效状态时，若hdoph_good，进入GPS有效状态；若P异常，进入重启状态；在GPS有效状态时，若hdoph_bad,进入GPS无效状态；若v_carv_bad，则进入停车/缓慢行驶状态；若P异常，进入重启状态；在停车/缓慢行驶状态时，若v_carv_good，进入GPS有效状态；若hdoph_bad，进入GPS无效状态；若P异常，进入重启状态；hdop为水平精度因子、star_num为接收星数、v_car为从ODOM读取的当前车速、P为卡尔曼滤波器的系统协方差矩阵，h_good、h_mid、h_bad为判断GPS定位精度参数，v_good、v_bad、v_mid为判断车辆速度参数。