[发明专利]驾驶模拟平台的姿态估算方法、装置、设备及存储介质

权利要求书

1.一种驾驶模拟平台的姿态估算方法，其特征在于，包括：

基于平台的定位装置计算当前时刻平台位置姿态的测量值，将所述平台位置姿态的初始测量值作为平台位置姿态的初始估算值，所述平台位置姿态的计算包括平台移动向量、平台角度向量和方差的计算；

基于平台定位装置、平台机械尺寸及运动学参数建立运动学模型，基于前一时刻所述平台位置姿态的估算值与所述运动学模型，计算当前时刻平台位置姿态的模型值；

采用如下公式计算当前时刻平台位置姿态的估算值：

其中，Final_Res_n为当前时刻平台位置姿态的估算值，Senser_Res_n为当前时刻平台位置姿态的测量值,Senser_Res_n为当前时刻平台位置姿态的模型值，Senser_σ²为平台位置姿态的测量方差，Model_σ²为平台位置姿态的模型方差。

2.根据权利要求1所述的方法，其特征在于，所述基于平台的定位装置计算当前时刻平台位置姿态的测量值包括：

基于平台的定位装置确定两个定位点的初始坐标以及平台位置姿态变化后的测量坐标；

根据所述初始坐标与测量坐标计算平台位置姿态的移动向量和平台位置姿态的姿态矩阵；

根据平台位置姿态的姿态矩阵计算平台位置姿态的角度向量。

3.根据权利要求2所述的方法，其特征在于，采用如下公式计算平台位置姿态的移动向量和平台位置姿态的姿态矩阵：

Y₁＝Ta_sener+Tb_sener*X₁

Y₂＝Ta_sener+Tb_sener*X_２

其中，X₁为第一定位点的初始坐标，X₂为第二定位点的初始坐标，Y₁为平台位置姿态变化后第一定位点的测量坐标，Y₂为平台位置姿态变化后第二定位点的测量坐标，Ta_sener为平台位置姿态变化的三个轴向的平移向量，Tb_sener为平台位置姿态变化的三个角度构成的旋转矩阵。

4.根据权利要求2所述的方法，其特征在于，采用如下公式反算平台位置姿态的角度向量：

R₁₁＝cosθcosψ

R₁₂＝cosθsinψ

R₁₃＝-sinθ

R₂₁＝sinφsinθcosψ-cosφsinψ

R₂₂＝sinφsinθsinψ+cosφcosψ

R₂₃＝sinφcosθ

R₃₁＝cosφsinθcosψ+sinφsinψ

R₃₂＝cosφsinθsinψ-sinφcosψ

R₃₃＝cosφcosθ

其中，θ为Pitch俯仰角，ψ为Roll翻滚角，φ为Yaw航向角，Tb_sener为平台位置姿态变化的三个角度构成的旋转矩阵。

5.根据权利要求1所述的方法，其特征在于，所述计算当前时刻平台位置姿态的模型值包括：

基于平台每个机械臂作用在平台上的驱动力以及前一时刻所述平台位置姿态的估算值计算当前时刻的轴向速度和角度速度；

根据所述轴向速度计算平台移动向量的模型值，根据所述角度速度计算平台角度向量的模型值。