CN115855072B

CN115855072B - 驾驶模拟平台的姿态估算方法、装置、设备及存储介质

Info

Publication number: CN115855072B
Application number: CN202310195657.7A
Authority: CN
Inventors: 李迪; 杨宝华
Original assignee: Beijing Qianzhong Huanying Technology Co ltd
Current assignee: Beijing Qianzhong Huanying Technology Co ltd
Priority date: 2023-03-03
Filing date: 2023-03-03
Publication date: 2023-05-09
Anticipated expiration: 2043-03-03
Also published as: CN115855072A

Abstract

本申请的实施例提供了一种驾驶模拟平台的姿态估算方法、装置、设备及存储介质，属于数据处理技术领域，所述方法包括：基于平台的定位装置计算当前时刻平台位置姿态的测量值，将所述平台位置姿态的初始测量值作为平台位置姿态的初始估算值，所述平台位置姿态的计算包括平台移动向量、平台角度向量和方差的计算；基于平台定位装置、平台机械尺寸及运动学参数建立运动学模型，基于前一时刻所述平台位置姿态的估算值与所述运动学模型，计算当前时刻平台位置姿态的模型值；根据所述位置姿态的测量值与所述位置姿态的模型值计算当前时刻平台位置姿态的估算值。本申请对于驾驶模拟平台姿态的估算成本更低，且能实时，高精度的计算当前平台的位置姿态。

Description

驾驶模拟平台的姿态估算方法、装置、设备及存储介质

技术领域

本申请的实施例涉及数据处理技术领域，尤其涉及一种驾驶模拟平台的姿态估算方法、装置、设备及存储介质。

背景技术

目前对于机械臂的运动平台实时姿态的获取，传统方式根据机械臂的角度以及机械尺寸，进行运动平台正解获得，但是传统方式的计算量过大，且存在奇异点，导致运动性能异变；此外可通过加装各自由度的角度及轴向惯性传感器，采集姿态信息后计算得到，但这种方式往往需要设置多种检测姿态的传感器，高精度及高实时度的传感器其带来的成本也非常高，需要增加过多的额外费用。

发明内容

为了解决上述至少一项技术问题，本申请的实施例提供了一种驾驶模拟平台的姿态估算方法、装置、设备及存储介质。

第一方面，本申请的实施例提供了一种驾驶模拟平台的姿态估算方法，该方法包括：

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

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

根据所述位置姿态的测量值与所述位置姿态的模型值计算当前时刻平台位置姿态的估算值。

在一种可能的实现方式中，所述基于平台的定位装置计算当前时刻平台位置姿态的测量值包括：

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

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

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

在一种可能的实现方式中，采用如下公式计算平台位置姿态的移动向量和平台位置姿态的姿态矩阵：

,

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

在一种可能的实现方式中，采用如下公式反算平台位置姿态的角度向量：

,

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

在一种可能的实现方式中，所述计算当前时刻平台位置姿态的模型值包括：

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

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

在一种可能的实现方式中，所述基于平台每个机械臂作用在平台上的驱动力以及前一时刻所述平台位置姿态的估算值计算当前时刻的轴向速度和角度速度包括：

基于平台每个机械臂作用在平台上的驱动力计算驱动力的合力；

基于所述驱动力的合力计算当前时刻的轴向加速度以及平台每个机械臂驱动力的力矩；

基于平台每个机械臂驱动力的力矩计算当前时刻的角度加速度；

基于所述轴向加速度以及前一时刻所述平台位置姿态的估算值计算当前时刻的轴向速度，基于所述角度加速度以及前一时刻所述平台位置姿态的估算值计算当前时刻的角度速度。

在一种可能的实现方式中，采用如下公式计算当前时刻平台位置姿态的估算值：

,

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

第二方面，本申请的实施例提供了一种驾驶模拟平台的姿态估算装置，该装置包括：

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

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

第二计算模块，用于根据所述位置姿态的测量值与所述位置姿态的模型值计算当前时刻平台位置姿态的估算值。

第三方面，本申请提供一种计算机设备，采用如下技术方案：包括存储器和处理器，所述存储器上存储有计算机程序，所述处理器执行所述程序时实现如以上所述的方法。

第四方面，本申请提供一种计算机可读存储介质，采用如下技术方案：存储有能够被处理器加载并执行上述任一种地图定位方法的计算机程序。

综上所述，本申请包括以下有益技术效果：

通过平台的定位装置计算当前时刻平台位置姿态的测量值，将平台位置姿态的初始测量值作为平台位置姿态的初始估算值，基于平台定位装置、平台机械尺寸及运动学参数建立运动学模型，基于前一时刻平台位置姿态的估算值与运动学模型，计算当前时刻平台位置姿态的模型值，根据位置姿态的测量值与位置姿态的模型值计算当前时刻平台位置姿态的估算值，本申请对于驾驶模拟平台姿态的估算成本更低，且能实时，高精度的计算当前平台的位置姿态。

应当理解，发明内容部分中所描述的内容并非旨在限定本申请的实施例的关键或重要特征，亦非用于限制本申请的范围。本申请的其他特征将通过以下的描述变得容易理解。

附图说明

结合附图并参考以下详细说明，本申请各实施例的上述和其他特征、优点及方面将变得更加明显。在附图中，相同或相似的附图标记表示相同或相似的元素。

图1示出了本申请实施例的驾驶模拟平台的姿态估算方法的流程图。

图2示出了本申请实施例的驾驶模拟平台的姿态估算装置的方框图。

图3示出了本申请实施例的一种电子设备的结构图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述。

为了便于对本申请实施例的理解，首先对本申请实施例涉及的部分术语进行解释。

驾驶模拟平台，由视觉仿真系统、听觉仿真系统、6自由度运动平台和中央控制系统等部分组成。其中，视觉仿真系统主要由前后共5块屏幕来实现，可模拟的汽车前向视野角度约为200度，后向视野角度约为50度；听觉仿真系统由置于周围的扬声器实现；中央控制系统主要包括车辆动力学控制单元。运动平台利用6根由电机控制的主缸，可以模拟汽车运动的六个自由度，模拟真实驾驶感受。

六自由度运动平台，用于飞行器、运动器(如飞机、车辆)模拟训练的动感模拟装置，是一种并联运动机构，它通过改变六个可以伸缩的作动筒来实现平台的空间六自由度运动(垂直向、横向、纵向、俯仰、滚转、摇摆)，即X、Y、Z方向的平移和绕X、Y、Z轴的旋转运动，以及这些自由度的复合运动。

接下来对本申请实施例所涉及的应用场景进行介绍。需要说明的是，本申请实施例描述的应用场景为驾驶模拟平台姿态值计算的场景，仅仅是为了更加清楚地说明本申请实施例的技术方案，并不构成对本申请实施例提供的技术方案的限定。本申请实施例提供的驾驶模拟平台的姿态估算方法在其他模拟平台姿态值的相似或类似的场景同样适用。

图1示出了本申请实施例的驾驶模拟平台的姿态估算方法的流程图，参见图1，该方法包括如下步骤：

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

在本申请实施例中，平台的定位装置为安装在驾驶模拟平台上的两个定位点，对应六自由度平台的六个自由度；在一些可能的实现方式中，采用其他定位装置如组合定位系统等实现平台六自由度的定位。

具体地，首先基于平台的定位装置确定两个定位点的初始坐标以及平台位置姿态变化后的测量坐标；根据所述初始坐标与测量坐标计算平台位置姿态的移动向量和平台位置姿态的姿态矩阵；根据平台位置姿态的姿态矩阵计算平台位置姿态的角度向量。

其中，采用如下公式计算平台位置姿态的移动向量和平台位置姿态的姿态矩阵：

,

进一步地，在本申请实施例中，因为物体在空间中的有限转动可以分解为依一定顺序绕惯性坐标轴的三次有限转动，每次转动的角度即是欧拉角。经过以上三次有限旋转后，可以得到当前平台角度姿态相对平台原状态的的姿态矩阵Tb_senser。

其中，采用如下公式反算平台位置姿态的角度向量：

,

进一步地，根据上述公式反算出以上三个角度：俯仰角、翻滚角和航向角，从而得到传感器的期望角度向量即平台位置姿态的角度向量：；传感器期望移动向量即平台位置姿态的移动向量：=Ta_senser。

最终得到传感器的测量结果，如下式所示，传感器测量结果的方差为。

，

其中，为传感器的测量结果，为平台位置姿态的移动向量,为平台位置姿态的角度向量。

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

具体地，首先获取平台每个机械臂作用在平台上的驱动力Fr_i，如下式所示。其中i为1-6任意一个，代表六个机械臂。

，

其中，Fr_i为第i个机械臂作用在平台上的驱动力，为第i个作用力在x方向的分量,为第i个作用力在y方向的分量,为第i个作用力在z方向的分量。

进一步地，采用下式计算驱动力的合力。

,

其中，F_sum为驱动力的合力，Fr_i为第i个机械臂作用在平台上的驱动力。

进一步地，采用下式计算平台每个机械臂驱动力的力矩，并求出每个机械臂驱动力的力矩之和。

,

其中，MOI_i为第i个机械臂作用在平台上的驱动力的力矩，Fr_i为第i个机械臂作用在平台上的驱动力，F_sum为驱动力的合力，Li为各个力矩作用点相对平台中心的坐标向量，MOI_sum为每个机械臂驱动力的力矩之和。

进一步地，基于所述驱动力的合力计算当前时刻的轴向加速度，如下式所示。

,

其中，F_sum为驱动力的合力，为当前时刻的轴向加速度，M为平台质量。

进一步地，基于平台每个机械臂驱动力的力矩之和计算当前时刻的角度加速度，如下式所示。

,

其中，为当前时刻的角度加速度，MOI_sum为每个机械臂驱动力的力矩之和,J为机械平台的转动惯量。

进一步地，基于所述轴向加速度以及前一时刻所述平台位置姿态的估算值计算当前时刻的轴向速度，如下式所示。

,

其中，为当前时刻的轴向速度，为当前时刻的轴向加速度,T为前一时刻到当前时刻的所需时间，为前一时刻的轴向速度。

进一步地，基于所述角度加速度以及前一时刻所述平台位置姿态的估算值计算当前时刻的角度速度，如下式所示。

,

其中，为当前时刻的角度速度，为当前时刻的角度加速度，为前一时刻的角度速度，T为前一时刻到当前时刻的所需时间。

进一步地，根据所述轴向速度计算平台移动向量的模型值，如下式所示。

,

其中，为当前时刻平台移动向量的模型值，为前一时刻平台移动向量的估算值，为当前时刻的轴向速度，为前一时刻的轴向速度，T为前一时刻到当前时刻的所需时间。

进一步地，根据所述角度速度计算平台角度向量的模型值，如下式所示。

,

其中，为当前时刻平台角度向量的模型值，为当前时刻的角度速度，为前一时刻的角度速度，T为前一时刻到当前时刻的所需时间，为前一时刻平台角度向量的估算值。

进一步地，最终得到模型的测量结果，如下式所示，模型测量结果的方差为。

,

其中，为模型的测量结果，为当前时刻平台移动向量的模型值,为当前时刻平台角度向量的模型值。

需要说明的是，初始计算过程中无法得到前一时刻的移动向量、角度向量等时，将平台位置姿态的初始测量结果作为平台位置姿态的初始估算结果参与计算。

步骤103，根据所述位置姿态的测量值与所述位置姿态的模型值计算当前时刻平台位置姿态的估算值。

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

,

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

,

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

具体地，当前时刻平台位置姿态的估算值包括平台移动向量和平台角度向量，如下式所示。

,

其中，为当前时刻平台位置姿态的估算值，为当前时刻平台移动向量，为当前时刻平台角度向量。

在本申请实施例中，平台移动向量包含3个轴向自由度，平台角度向量包含3个角度自由度。

具体地，其移动向量如下：

,

其中，为当前时刻平台移动向量，为当前时刻X轴平移最终估算值，为当前时刻Y轴平移最终估算值，为当前时刻Z轴平移最终估算值。

其角度向量如下：

,

其中，为当前时刻平台角度向量，为当前时刻Pitch俯仰角最终估算值，为当前时刻Roll翻滚角最终估算值，为当前时刻Yaw航向角最终估算值。

根据本公开的实施例，实现了以下技术效果：

本申请通过平台的定位装置计算当前时刻平台位置姿态的测量值，将平台位置姿态的初始测量值作为平台位置姿态的初始估算值，基于平台定位装置、平台机械尺寸及运动学参数建立运动学模型，基于前一时刻平台位置姿态的估算值与运动学模型，计算当前时刻平台位置姿态的模型值，根据位置姿态的测量值与位置姿态的模型值计算当前时刻平台位置姿态的估算值，本申请对于驾驶模拟平台姿态的估算成本更低，且能实时，高精度的计算当前平台的位置姿态。

需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本申请并不受所描述的动作顺序的限制，因为依据本申请，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于可选实施例，所涉及的动作和模块并不一定是本申请所必须的。

以上是关于方法实施例的介绍，以下通过装置实施例，对本申请所述方案进行进一步说明。

图2示出了本申请实施例的驾驶模拟平台的姿态估算装置的方块图，该装置应用于布置于平台上的控制设备或控制平台的远程控制设备，或者可被实现为应用于布置于平台上的控制设备或控制平台的远程控制设备。参见图2，该装置包括确定模块201、第一计算模块202以及第二计算模块203。

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

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

第二计算模块203，用于根据所述位置姿态的测量值与所述位置姿态的模型值计算当前时刻平台位置姿态的估算值。

需要说明的是：上述实施例提供的驾驶模拟平台的姿态估算装置在进行姿态估算时，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将装置的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。另外，上述实施例提供驾驶模拟平台的姿态估算装置与驾驶模拟平台的姿态估算方法实施例属于同一构思，其具体实现过程详见方法实施例，这里不再赘述。

图3示出了本申请实施例的一种电子设备的结构图。在一些可选的实施方式中，图3所示的电子设备为布置在模拟平台上的控制设备或控制模拟平台的远程控制设备。

参见图3，电子设备300包括处理器301和存储器303。其中，处理器301和存储器303相连，如通过总线302相连。可选地，电子设备300还可以包括收发器304。需要说明的是，实际应用中收发器304不限于一个，该电子设备300的结构并不构成对本申请实施例的限定。

处理器301可以是CPU（Central Processing Unit，中央处理器），通用处理器，DSP（Digital Signal Processor，数据信号处理器），ASIC（Application SpecificIntegrated Circuit，专用集成电路），FPGA（Field Programmable Gate Array，现场可编程门阵列）或者其他可编程逻辑器件、晶体管逻辑器件、硬件部件或者其任意组合。其可以实现或执行结合本申请公开内容所描述的各种示例性的逻辑方框，模块和电路。处理器301也可以是实现计算功能的组合，例如包含一个或多个微处理器组合，DSP和微处理器的组合等。

总线302可包括一通路，在上述组件之间传送信息。总线302可以是PCI（Peripheral Component Interconnect，外设部件互连标准）总线或EISA（ExtendedIndustry Standard Architecture，扩展工业标准结构）总线等。总线302可以分为地址总线、数据总线、控制总线等。为便于表示，图3中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。

存储器303可以是ROM（Read Only Memory，只读存储器）或可存储静态信息和指令的其他类型的静态存储设备，RAM（Random Access Memory，随机存取存储器）或者可存储信息和指令的其他类型的动态存储设备，也可以是EEPROM（Electrically ErasableProgrammable Read Only Memory，电可擦可编程只读存储器）、CD-ROM（Compact DiscRead Only Memory，只读光盘）或其他光盘存储、光碟存储（包括压缩光碟、激光碟、光碟、数字通用光碟、蓝光光碟等）、磁盘存储介质或者其他磁存储设备、或者能够用于携带或存储具有指令或数据结构形式的期望的程序代码并能够由计算机存取的任何其他介质，但不限于此。

存储器303用于存储执行本申请方案的应用程序代码，并由处理器301来控制执行。处理器301用于执行存储器303中存储的应用程序代码，以实现地图的定位。

其中，电子设备包括但不限于：移动电话、笔记本电脑、数字广播接收器、PDA（个人数字助理）、PAD（平板电脑）、PMP（便携式多媒体播放器）、车载终端（例如车载导航终端）等等的移动终端以及诸如数字TV、台式计算机等等的固定终端。需要说明的是，图3示出的电子设备仅仅是一个示例，不应对本申请实施例的功能和使用范围带来任何限制。

在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意结合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机指令时，全部或部分地产生按照本申请实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络或其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如：同轴电缆、光纤、数据用户线(digital subscriber line，DSL))或无线(例如：红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质，或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质(例如：软盘、硬盘、磁带)、光介质(例如：数字通用光盘(digital versatile disc，DVD))或半导体介质(例如：固态硬盘(solid state disk，SSD))等。值得注意的是，本申请实施例提到的计算机可读存储介质可以为非易失性存储介质，换句话说，可以是非瞬时性存储介质。

应当理解的是，本文提及的“至少一个”是指一个或多个，“多个”是指两个或两个以上。在本申请实施例的描述中，除非另有说明，“/”表示或的意思，例如，A/B可以表示A或B；本文中的“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，为了便于清楚描述本申请实施例的技术方案，在本申请的实施例中，采用了“第一”、“第二”等字样对功能和作用基本相同的相同项或相似项进行区分。本领域技术人员可以理解“第一”、“第二”等字样并不对数量和执行次序进行限定，并且“第一”、“第二”等字样也并不限定一定不同。

以上所述为本申请提供的示例性实施例，并不用以限制本申请实施例，凡在本申请实施例的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims

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所述的方法，其特征在于，所述计算当前时刻平台位置姿态的模型值包括：

6.根据权利要求5所述的方法，其特征在于，所述基于平台每个机械臂作用在平台上的驱动力以及前一时刻所述平台位置姿态的估算值计算当前时刻的轴向速度和角度速度包括：

7.一种驾驶模拟平台的姿态估算装置，其特征在于，包括：

第二计算模块，用于

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

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

8.一种计算机设备，其特征在于，包括存储器和处理器，所述存储器上存储有能够被处理器加载并执行如权利要求1至6中任一种方法的计算机程序。

9.一种计算机可读存储介质，其特征在于，存储有能够被处理器加载并执行如权利要求1至6中任一种方法的计算机程序。