[发明专利]用于确定无人驾驶车辆位姿的方法和装置

权利要求书

1.一种用于确定无人驾驶车辆位姿的方法，其中，所述无人驾驶车辆设置有全球导航卫星系统GNSS接收机和惯性测量单元IMU，所述方法包括：

实时接收所述IMU采集的速度信息和所述GNSS接收机解算得到的定位信息，将所收到的速度信息添加到当前速度信息序列中，其中，速度信息包括所述无人驾驶车辆的加速度和角速度，定位信息包括所述无人驾驶车辆的位置、速度和航向角；

响应于接收到所述GNSS接收机解算得到的定位信息，将所收到的定位信息确定为当前定位信息，执行位姿确定操作，其中，所述位姿确定操作包括：将所述当前速度信息序列作为输入量，将上次的定位信息中的无人驾驶车辆的位置和航向角作为状态变量构建状态转移方程，得到本次的无人驾驶车辆的位置和航向角的预测值，再以所述本次的无人驾驶车辆的位置和航向角的预测值作为状态变量，所述当前定位信息作为观测量，利用卡尔曼滤波算法确定所述无人驾驶车辆的当前位置和当前航向角，以及清空所述当前速度信息序列；

从收到定位信息的时间开始直到再次接收到定位信息，每隔预设更新周期，根据本次收到的定位信息和当前时间与本次收到定位信息的时间之间的时间差确定所述当前定位信息，以及执行所述位姿确定操作。

2.根据权利要求1所述的方法，其中，所述预设更新周期大于所述IMU的采样周期且小于所述IMU的积分误差漂移间隔时长。

3.根据权利要求1或2所述的方法，其中，所述根据本次收到的定位信息和当前时间与本次收到定位信息的时间之间的时间差确定所述当前定位信息，包括：

根据本次收到的定位信息和当前时间与本次收到定位信息的时间之间的时间差确定当前定位位置；

将所述当前定位信息中的位置、速度和航向角分别确定为所确定的当前定位位置、本次收到的定位信息中的速度和本次收到的定位信息中的航向角。

4.根据权利要求3所述的方法，其中，所述GNSS接收机解算的所述无人驾驶车辆的位置包括东北天坐标系中的东向坐标和北向坐标；以及

所述根据本次收到的定位信息和当前时间与本次收到定位信息的时间之间的时间差确定当前定位位置，包括：

按照如下公式，根据本次收到的定位信息和当前时间与本次收到定位信息的时间之间的时间差确定当前定位位置：

x(t′)＝x(t)+v(t)·sin(angle(t))·(t′-t)

y(t′)＝y(t)+v(t)·cos(angle(t))·(t′-t)

其中，t是本次收到定位信息的时间，t′是当前时间，x(t)和y(t)分别是本次收到的定位信息中的位置中的东向坐标和北向坐标，angle(t)是本次收到的定位信息中的航向角，x(t′)和y(t′)分别是计算所得的当前定位位置中的东向坐标和北向坐标，v(t)是本次收到的定位信息中的速度。

5.根据权利要求4所述的方法，其中，所述响应于接收到所述GNSS接收机解算得到的定位信息，将所收到的定位信息确定为当前定位信息，包括：

响应于确定接收到所述GNSS接收机解算得到的定位信息，确定所收到的定位信息中的速度是否小于预设速度阈值；

响应于确定所收到的定位信息中的速度小于预设速度阈值，按照如下公式计算得到校正后定位位置：

x(t′)＝x(t)+δ_x(t)·(t′-t)

y(t′)＝y(t)+δ_y(t)·(t′-t)

其中，t是上次接收到定位信息的时间，t′是本次收到定位信息的时间，x(t)和y(t)分别是上次收到的定位信息中的位置中的东向坐标和北向坐标，x(t′)和y(t′)分别是计算所得的校正后定位位置中的东向坐标和北向坐标；

将所述当前定位信息中的位置、速度和航向角分别确定为计算得到的校正后定位位置、上次收到的定位信息中的速度和上次收到的定位信息中的航向角。