[发明专利]一种自动驾驶过程中使车辆位置平滑的装置及方法

权利要求书

1.一种自动驾驶过程中使车辆位置平滑的装置，其特征在于：所述装置包括如下部分：

建立初始模型，设定参数建立滤波模型，设定最大偏航数值初始值MaxYaw0，所述最大偏航数值初始值MaxYaw0是最大横摆角速度与获取车辆当前时刻和上一时刻位置的时间差的乘积；由所述最大偏航数值初始值MaxYaw0赋值给偏航数值MaxYaw，所述滤波模型为卡尔曼滤波模型，所述卡尔曼滤波模型使用OpenCV库提供的函数进行计算，卡尔曼滤波模型公式如下：

其中，x为车辆的经度，y为车辆的纬度，vx车辆的东向速度，vy车辆的北向速度,表示时刻k车辆的状态,表示时刻k+1车辆的状态；

车辆位置信息模块，获取车辆位置信息数据，对滤波模型进行初始化；

车辆位置预测模块，利用滤波模型对当前车辆位置进行预测，并结合惯导获取的当前车辆位置的观测值进行校正；

车辆航向约束模块，根据所述偏航数值MaxYaw，对校正后的车辆位置进行修正，获取更新的车辆位置；具体包括：当校正后的位置满足车辆的航向约束时，将校正后的位置输出；若校正后的位置不满足航向约束时，则对校正后的车辆位置进行修正，并将修正后的位置参数输出，利用车辆的偏航数值MaxYaw对车辆的行进轨迹进行航向约束。

2.根据权利要求1所述自动驾驶过程中使车辆位置平滑的装置，其特征在于：所述车辆航向约束模块中，所述最大偏航数值初始值MaxYaw0赋值给偏航数值MaxYaw，车辆位置出现异常情况时，所述偏航数值MaxYaw会进行更新，车辆位置出现异常情况是指上上一个时刻位置与上一个时刻位置的连线和上一个时刻位置与当前时刻位置的连线的向量夹角大于某个给定的阈值，车辆位置恢复正常时指上述向量夹角小于所述偏航数值MaxYaw且偏航数值MaxYaw大于最大偏航数值初始值MaxYaw0。

3.根据权利要求1至2中任一项所述自动驾驶过程中使车辆位置平滑的装置，其特征在于：所述车辆位置信息模块中，从高精度组合惯导中提取经度、纬度、东向速度、北向速度参数，确定车辆位置信息。

4.一种自动驾驶过程中使车辆位置平滑的方法，其特征在于：所述方法包括以下步骤：

步骤1)设定参数建立滤波模型，设定最大偏航数值初始值MaxYaw0，所述最大偏航数值初始值MaxYaw0是最大横摆角速度与获取车辆当前时刻和上一时刻位置的时间差的乘积；所述最大偏航数值初始值MaxYaw0赋值给偏航数值MaxYaw，所述滤波模型为卡尔曼滤波模型，所述卡尔曼滤波模型使用OpenCV库提供的函数进行计算，卡尔曼滤波模型公式如下：

其中，x为车辆的经度，y为车辆的纬度，vx车辆的东向速度，vy车辆的北向速度,表示时刻k车辆的状态，表示时刻k+1车辆的状态；

步骤2)获取车辆位置数据；

步骤3)判断当前模型是否初始化，若没有初始化，则进入步骤4)，若已经初始化，则进入步骤5)；

步骤4)根据当前时刻车辆的位置数据，对滤波模型进行初始化，将当前时刻车辆的位置输出，跳转到步骤7)；

步骤5)利用滤波模型对当前车辆位置进行预测，并结合惯导获取的当前车辆位置的观测值进行校正；

步骤6)根据车辆的航向约束偏航数值MaxYaw，对校正后的车辆位置进行修正，获取更新的车辆位置；具体包括：当校正后的位置满足车辆的航向约束时，将校正后的位置输出，若校正后的位置不满足航向约束时，则对校正后的车辆位置进行修正，并将修正后的位置参数输出，利用车辆的偏航数值MaxYaw对车辆的行进轨迹进行航向约束；

步骤7)等待下一时刻，跳转到步骤2)。

5.根据权利要求4所述自动驾驶过程中使车辆位置平滑的方法，其特征在于：所述步骤6)中，所述最大偏航数值初始值MaxYaw0赋值给偏航数值MaxYaw，车辆位置出现异常情况时，所述偏航数值MaxYaw会进行更新，车辆位置出现异常情况是指上上一个时刻位置与上一个时刻位置的连线和上一个时刻位置与当前时刻位置的连线的向量夹角大于某个给定的阈值，车辆位置恢复正常时指上述向量夹角小于偏航数值MaxYaw且偏航数值MaxYaw大于所述最大偏航数值初始值MaxYaw0。

6.根据权利要求4至5中任一项所述自动驾驶过程中使车辆位置平滑的方法，其特征在于：所述步骤1)中，从高精度组合惯导中提取经度、纬度、东向速度、北向速度参数，确定车辆位置信息。