[发明专利]位姿估计方法、装置、设备及介质

权利要求书

1.一种位姿估计方法，其特征在于，包括：

获取车辆在T时刻和T+1时刻之间的轮速计观测数据；其中，所述T时刻和所述T+1时刻为指定传感器在进行数据观测时的两个相邻时刻，且所述指定传感器为所述车辆上设置的用于观测车辆位姿的传感器；

根据预设的轮速计模型和车辆运动学模型，获取所述轮速计观测数据表征的车辆名义状态的时间导数；

对所述轮速计观测数据表征的车辆名义状态的时间导数进行数值积分，基于积分结果获取所述车辆在所述T时刻和所述T+1时刻之间的名义位姿增量；所述名义位姿增量为基于所述轮速计观测数据得到的车辆位姿增量；

根据误差传播算法、所述轮速计模型、所述车辆运动学模型以及所述车辆名义状态的时间导数，得到所述T时刻和所述T+1时刻之间对应的轮速计误差置信度；其中，所述轮速计误差置信度是所述T时刻和所述T+1时刻之间真实位姿增量与所述名义位姿增量的误差的置信度；

根据所述指定传感器在所述T时刻的观测数据和所述T+1时刻的观测数据、所述轮速计观测数据对应的名义位姿增量以及所述轮速计误差置信度，对所述车辆在所述T时刻和所述T+1时刻的真实位姿进行估计。

2.根据权利要求1所述的方法，其特征在于，所述轮速计误差置信度通过所述T时刻和所述T+1时刻之间真实位姿增量与所述名义位姿增量之间的误差的协方差表征。

3.根据权利要求1所述的方法，其特征在于，根据误差传播算法、所述轮速计模型、所述车辆运动学模型以及所述车辆名义状态的时间导数，得到所述T时刻和所述T+1时刻之间对应的轮速计误差置信度的步骤，包括：

基于所述轮速计模型和所述车辆运动学模型获取车辆真实状态的时间导数；

根据误差传播算法，采用所述车辆真实状态的时间导数结合所述车辆名义状态的时间导数，确定所述T时刻和所述T+1时刻之间真实位姿增量与所述名义位姿增量的误差的时间导数；

对所述误差的时间导数进行数值积分，获得所述误差的转移方程；

基于所述误差的转移方程递推得到所述T时刻和所述T+1时刻之间真实位姿增量与所述名义位姿增量之间的误差的协方差，并采用误差的协方差表征T时刻和T+1时刻之间真实位姿增量与名义位姿增量之间对应的轮速计误差置信度。

4.根据权利要求2所述的方法，其特征在于，在所述误差的协方差不满秩的情况下，所述误差基于指定的三维数据表示，且所述指定的三维数据包括六自由度模型中的T时刻车辆坐标系对应的X轴方向位移、Y轴位移以及偏航角。

5.根据权利要求2所述的方法，其特征在于，在所述误差的协方差不满秩的情况下，所述误差基于六自由度模型表征，且所述六自由度模型中添加有垂直于车辆瞬时平面方向的速度噪声、翻滚角的角速度噪声和俯仰角的角速度噪声。

6.根据权利要求1所述的方法，其特征在于，根据所述指定传感器在所述T时刻的观测数据和所述T+1时刻的观测数据，所述轮速计观测数据对应的名义位姿增量以及所述轮速计误差置信度，对所述车辆在所述T时刻和所述T+1时刻的真实位姿进行估计的步骤，包括：

根据所述指定传感器在所述T时刻的观测数据和所述T+1时刻的观测数据，获取所述车辆在所述T时刻的第一名义位姿和第一传感器误差置信度，以及所述车辆在所述T+1时刻的第二名义位姿和第二传感器误差置信度；

根据所述轮速计观测数据对应的名义位姿增量和所述轮速计误差置信度，以及所述指定传感器对应的第一名义位姿、第一传感器误差置信度、第二名义位姿和第二传感器误差置信度，对所述车辆在所述T时刻和所述T+1时刻的真实位姿进行估计。