CN114323007A - 一种载体运动状态估计方法及装置 - Google Patents

Abstract

本发明提供了一种载体运动状态估计方法及装置，该方法包括：获取目标载体的加速度信息和角速度信息以及卫星导航系统下目标载体的第一速度和位置信息；基于加速度信息和角速度信息，采用互补滤波算法计算四元数；基于四元数构建旋转矩阵；利用旋转矩阵进行目标载体速度和位置的解算，得到惯性导航系统下的第二速度和位置信息；基于第一速度和位置信息和第二速度和位置信息进行速度和位置的估计融合，得到目标载体在当前时刻的运动状态估计结果。从而实现了对位置、速度的较好估计，减少了硬件依赖，并充分利用了加速度计的低频信息和陀螺仪的高频信息，实现了优势互补，使用互补滤波进行四元数的估计，简化了计算过程，提高了效率。

Description

一种载体运动状态估计方法及装置

技术领域

本发明涉及导航技术领域，具体涉及一种载体运动状态估计方法及装置，

背景技术

近几年，科技发展迅速，同时也出现了多种导航系统，例如，多普勒导航系统，惯性导航系统，罗兰导航系统，卫星导航系统等。但是应用比较广泛的还是惯性导航系统(SINS)和卫星导航系统(GPS)。惯性导航系统短时间内定位精度高，但由于积分的特性会产生长时间下误差累积而使得定位结果发散，因此系统长期的稳定性差；而卫星导航系统(GPS)在短时间内定位精度不如惯性导航系统，但是在长时间内具有较好的系统稳定性。当两种导航系统用于载体运动状态估计时均无法同时兼顾定位精度和稳定性。

发明内容

有鉴于此，本发明实施例提供了一种载体运动状态估计方法及装置以解决现有技术中载体运动状态估计方法无法同时兼顾定位精度和稳定性的问题。

根据第一方面，本发明实施例提供了一种载体运动状态估计方法，包括：

获取目标载体的加速度信息和角速度信息以及卫星导航系统下所述目标载体的第一速度和位置信息；

基于所述加速度信息和所述角速度信息，采用互补滤波算法计算四元数；

基于所述四元数构建旋转矩阵；

利用所述旋转矩阵进行所述目标载体速度和位置的解算，得到惯性导航系统下的第二速度和位置信息；

基于所述第一速度和位置信息和所述第二速度和位置信息进行速度和位置的估计融合，得到所述目标载体在当前时刻的运动状态估计结果。

可选地，所述获取目标载体的加速度信息和角速度信息，包括：

获取所述目标载体上加速度计测量得到的加速度信息以及陀螺仪测量得到的角速度信息。

可选地，所述基于所述加速度信息和所述角速度信息，采用互补滤波算法计算四元数，包括：

将所述加速度信息中进行归一化处理，得到归一化的加速度信息；

将重力加速度转换至载体坐标系下，得到参考加速度；

对所述归一化的加速度信息和所述参考加速度进行叉积运算，得到叉积误差；

对所述叉积误差进行PI运算，对所述角速度信息进行补偿，得到补偿后的角速度信息；

基于补偿后的角速度信息计算所述四元数。

可选地，所述四元数通过如下公式计算：

其中，q₀,q₁,q₂,q₃为四元数，T为周期，w_kx,w_ky,w_kz为补偿后的角速度。

可选地，所述旋转矩阵通过如下公式表示：

其中，

为旋转矩阵，q₀,q₁,q₂,q₃为四元数。

可选地，所述基于所述第一速度和位置信息和所述第二速度和位置信息进行速度和位置的估计融合，得到所述目标载体在当前时刻的运动状态估计结果，包括：

利用卡尔曼滤波算法对所述第一速度和位置信息和所述第二速度和位置信息进行融合，得到所述目标载体在当前时刻的速度和位置估计结果。

可选地，所述基于所述第一速度和位置信息和所述第二速度和位置信息进行速度和位置的估计融合，得到所述目标载体在当前时刻的运动状态估计结果，包括：

基于所述第一速度和位置信息和所述第二速度和位置信息进行速度误差和位置误差的估计，得到误差估计结果；

基于所述误差估计结果对所述第二速度和位置信息进行校正，确定所述目标载体在当前时刻的运动状态估计结果。

可选地，所述基于所述第一速度和位置信息和所述第二速度和位置信息进行速度误差和位置误差的估计，得到误差估计结果，包括：

结合位置和速度的误差状态方程，利用卡尔曼滤波算法对所述第一速度和位置信息和所述第二速度和位置信息进行速度误差和位置误差的估计，得到误差估计结果。

可选地，所述方法还包括：

基于所述加速度信息和所述角速度信息，采用互补滤波算法计算所述目标载体在当前时刻的姿态角估计结果。

根据第二方面，本发明实施例提供了一种载体运动状态估计装置，包括：

获取模块，用于获取目标载体的加速度信息和角速度信息以及卫星导航系统下所述目标载体的第一速度和位置信息；

第一处理模块，用于基于所述加速度信息和所述角速度信息，采用互补滤波算法计算四元数；

第二处理模块，用于基于所述四元数构建旋转矩阵；

第三处理模块，用于利用所述旋转矩阵进行所述目标载体速度和位置的解算，得到惯性导航系统下的第二速度和位置信息；

第四处理模块，用于基于所述第一速度和位置信息和所述第二速度和位置信息，进行速度和位置的估计融合，得到所述目标载体在当前时刻的运动状态估计结果。

根据第三方面，本发明实施例提供了一种非暂态计算机可读存储介质，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令被处理器执行时实现本发明第一方面及其任意一种可选方式所述的方法。

根据第四方面，本发明实施例提供了一种电子设备，包括：存储器和处理器，所述存储器和所述处理器之间互相通信连接，所述存储器中存储有计算机指令，所述处理器通过执行所述计算机指令，从而执行本发明第一方面及其任意一种可选方式所述的方法。

本发明技术方案，具有如下优点：

本发明实施例提供了一种载体运动状态估计方法及装置，通过获取目标载体的加速度信息和角速度信息以及卫星导航系统下目标载体的第一速度和位置信息；基于加速度信息和角速度信息，采用互补滤波算法计算四元数；基于四元数构建旋转矩阵；利用旋转矩阵进行目标载体速度和位置的解算，得到惯性导航系统下的第二速度和位置信息；基于第一速度和位置信息和第二速度和位置信息进行速度和位置的估计融合，得到目标载体在当前时刻的运动状态估计结果。从而通过利用互补滤波对载体的四元数先进行估计，再基于估计信息构建旋转矩阵对位置和速度进行解算和与卫星导航系统的速度位置进行融合，实现了对位置、速度的较好估计，避免了在四元数计算的部分使用其它的传感器如罗盘等，减少了硬件依赖，并通过结合加速度计和陀螺仪的各自的优点，充分利用了加速度计的低频信息和陀螺仪的高频信息，实现了优势互补，从而对四元数进行了更新，然后构建出旋转矩阵能够更加准确的解算出速度和位置信息，此外，使用互补滤波进行四元数的估计，避免了后续进行融合估计时在角度计算过程中所引起的一些计算奇异和物理奇异问题，简化了计算过程，提高了效率。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例中载体运动状态估计方法的流程图；

图2为本发明实施例中互补滤波算法的频域结构示意图；

图3为本发明实施例中互补滤波算法的时域结构示意图；

图4为本发明实施例中改进的互补滤波算法的结构示意图；

图5为本发明实施例中载体运动状态估计的工作过程示意图；

图6为本发明实施例中载体运动状态估计的二维位置状态估计结果示意图；

图7为本发明实施例中载体运动状态估计的工作过程示意图；

图8为本发明实施例中载体运动状态估计装置的结构示意图；

图9为本发明实施例中的电子设备的结构示意图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

此外，下面所描述的本发明不同实施方式中所涉及的技术特征只要彼此之间未构成冲突就可以相互结合。

近几年，科技发展迅速，同时也出现了多种导航系统，例如，多普勒导航系统，惯性导航系统，罗兰导航系统，卫星导航系统等。但是应用比较广泛的还是惯性导航系统(SINS)和卫星导航系统(GPS)。惯性导航系统短时间内定位精度高，但由于积分的特性会产生长时间下误差累积而使得定位结果发散，因此系统长期的稳定性差；而卫星导航系统(GPS)在短时间内定位精度不如惯性导航系统，但是在长时间内具有较好的系统稳定性。当两种导航系统用于载体运动状态估计时均无法同时兼顾定位精度和稳定性。

基于上述问题，本发明实施例提供了一种载体运动状态估计方法，如图1所示，该载体运动状态估计方法具体包括如下步骤：

步骤S101：获取目标载体的加速度信息和角速度信息以及卫星导航系统下目标载体的第一速度和位置信息。

其中，获取目标载体的加速度信息和角速度信息，包括：获取目标载体上加速度计测量得到的加速度信息以及陀螺仪测量得到的角速度信息。具体地，加速度信息为目标载体上的惯性测量单元中的加速度计输出的加速度矢量，角速度信息为目标载体上的惯性测量单元中的陀螺仪输出的角速度矢量。卫星导航系统下目标载体的第一速度和位置信息具体为GPS输出的目标载体的位置和速度信息。上述目标载体可以是火箭、飞机、汽车等移动对象，在本发明实施例中是以目标载体是飞机为例进行的说明，仅以此为例，本发明并不以此为限。

步骤S102：基于加速度信息和角速度信息，采用互补滤波算法计算四元数。

具体地，根据IMU输出的加速度信息和角速度信息结合互补滤波算法可以得到补偿的角速度，进而更新四元数。

在本发明实施例中，互补滤波就是利用加速度计低频信号较好，陀螺仪高频信号较好的特性，利用加速度计的低频信息和陀螺仪的高频信息，对加速度计进行低通滤波处理，对陀螺仪进行高通滤波处理。加速度计是容易受到外界干扰的一种传感器，但是其测量值是每一时刻的加速度测量，因此测量值的误差不会随着时间的变化而变化。陀螺仪输出的角速度信息经过积分可以得到载体的角度，它的动态性能较好，受到的外界干扰较小，但是随着积分的次数增加会造成误差累积。因此，将加速度和陀螺仪配合使用，就可以得到较好的姿态估计效果。图2为互补滤波算法的频域结构图，从而得到了姿态角的估计值。

为陀螺仪输出的角速度向量，

为加速度计输出得到的角度向量，

是为陀螺仪设计的高通滤波器，

是为加速度计设计的低通滤波器，且M₁+M₂＝1。图3和图2原理一致，图3是时域下的表达。

步骤S103：基于四元数构建旋转矩阵。

具体地，惯性导航系统基于得到的四元数信息构建旋转矩阵的数学平台。

步骤S104：利用旋转矩阵进行目标载体速度和位置的解算，得到惯性导航系统下的第二速度和位置信息。

具体地，通过利用上述数学平台进行位置和速度的解算，得到上述第二速度和位置信息。

步骤S105：基于第一速度和位置信息和第二速度和位置信息进行速度和位置的估计融合，得到目标载体在当前时刻的运动状态估计结果。

通过执行上述步骤，本发明实施例提供的载体运动状态估计方法，通过利用互补滤波对载体的四元数先进行估计，再基于估计信息构建旋转矩阵对位置和速度进行解算和与卫星导航系统的速度位置进行融合，实现了对位置、速度的较好估计，避免了在四元数计算的部分使用其它的传感器如罗盘等，减少了硬件依赖，并通过结合加速度计和陀螺仪的各自的优点，充分利用了加速度计的低频信息和陀螺仪的高频信息，实现了优势互补，从而对四元数进行了更新，然后构建出旋转矩阵能够更加准确的解算出速度和位置信息，此外，使用互补滤波进行四元数的估计，避免了后续进行融合估计时在角度计算过程中所引起的一些计算奇异和物理奇异问题，简化了计算过程，提高了效率。

具体地，在一实施例中，如图4所示，上述的步骤S102具体包括如下步骤：

步骤S201：将加速度信息中进行归一化处理，得到归一化的加速度信息。

具体地，按照如下公式(1)对加速度计输出的比力信息进行归一化处理：

其中，a_x,a_y,a_z为加速度计的比力信息，A_x,A_y,A_z为归一化后的加速度信息。

步骤S202：将重力加速度转换至载体坐标系下，得到参考加速度。

具体地，通过将重力加速度通过旋转矩阵转换到载体坐标系下，通过公式(2)计算参考加速度：

其中，g_B为参考加速度，g为重力加速度，

为旋转矩阵。

步骤S203：对归一化的加速度信息和参考加速度进行叉积运算，得到叉积误差。

具体地，通过将归一化后的加速度和参考加速度进行叉积运算，可得到叉积误差，如公式(3)所示。

其中，e_x,e_y,e_z为叉积误差，g_{B_x},g_{B_y},g_{B_z}为参考加速度分量，A_x,A_y,A_z为归一化后的加速度。

步骤S204：对叉积误差进行PI运算，对角速度信息进行补偿，得到补偿后的角速度信息。

具体地，针对叉积误差做PI运算，通过如下公式(4)对角速度做补偿：

w_kx＝w_x+k_p*e_x+∫e_x

w_ky＝w_y+k_p*e_y+∫e_y

w_kx＝w_z+k_p*e_z+∫e_z (4)

其中，w_kx，w_ky，w_kz为补偿后的角速度，w_x，w_y，w_z分别为陀螺仪输出的三维角速度，k_p是加速度计和陀螺仪设计的滤波器参数，e_x，e_y，e_z为叉积误差。

本发明实施例通过引入了PI控制器对角速度进行补偿，使得稳定时间更短且估计精度更高。

步骤S205：基于补偿后的角速度信息计算四元数。

具体地，四元数通过如下公式(5)计算：

其中，q₀，q₁，q₂，q₃为四元数，T为周期，w_kx，w_ky，w_kz为补偿后的角速度。

具体地，在一实施例中，上述的步骤S103中的旋转矩阵通过如下公式(6)表示：

其中，

为旋转矩阵，q₀,q₁,q₂,q₃为更新后的四元数。

具体地，在一实施例中，上述的步骤S105通过利用卡尔曼滤波算法对第一速度和位置信息和第二速度和位置信息进行融合，得到目标载体在当前时刻的速度和位置估计结果。

具体地，通过将卫星导航系统和惯性导航系统的输出进行融合，通过计算卡尔曼滤波的状态一步预测，计算状态估计值，计算一步预测协方差，计算滤波增益，计算估计均方误差就可以得到融合后的速度和位置的值。

具体地，如图5所示，通过将构建的旋转矩阵作为数学平台把加速度计测得的沿载体坐标系的比力信息转换到导航坐标系，惯性导航根据如下形式的递推方程输出当前时刻的位置和速度：

其中，

表示的是上一时刻的位置和速度，

表示的是当前时刻的位置和速度。

在卡尔曼融合过程中的量测值部分，根据卫星导航系统输出的信号可以得到三维的位置和速度分量，记为GPS_px,GPS_py,GPS_pz,GPS_vx,GPS_vy,GPS_vz。

然后根据惯性导航和卫星导航的输出进行卡尔曼滤波融合，基于卡尔曼滤波和互补滤波的结合，就可以循环迭代的对载体的状态进行估计。

在卡尔曼滤波部分，主要是根据卡尔曼滤波算法使用SINS结合GPS对位置和速度进行融合。首先在滤波开始之前完成初始化，然后结合系统状态方程和量测方程对系统状态进行卡尔曼滤波融合。卡尔曼滤波融合的主要过程为：

其中，

为k-1时刻的状态的估计值，Φ_k/k-1为状态转移矩阵，

为根据k-1时刻得到的k时刻的预测值，K_k为滤波增益，z_k为状态量测向量，H_k为状态量测矩阵，

为状态估计值，p_k/k-1为一步预测协方差矩阵，R_k为量测噪声矩阵，K_k为所求的增益，p_k-1为k-1时刻的估计均方误差矩阵，Γ_k-1为系统噪声驱动阵，Q_k-1为系统噪声矩阵，p_k/k-1为一步预测均方误差阵。上述第一个式子是先根据上一时刻的估计值预测得到一个这一时刻的先验估计值，第二个式子是基于计算的增益进行校正更新，即得到当前时刻的最优估计值，第三个式子是由上一次的误差协方差和过程噪声预测新的预测协方差，第四个式子是计算卡尔曼增益，第五个式子是得到当前时刻使用的估计协方差值。

通过这一方法设计的组合载体运动状态估计方案的估计结果如图6所示，由图6可以看出，本发明实施例所提出的技术方案以kalman表示，其与GPS实际测量的结果相比，使用kalman融合后的结果可达到较小的估计误差。

具体地，在另一可替换实施方式中，上述的步骤S105具体地包括如下步骤：

步骤S501：基于第一速度和位置信息和第二速度和位置信息进行速度误差和位置误差的估计，得到误差估计结果。

具体地，结合位置和速度的误差状态方程，利用卡尔曼滤波算法对第一速度和位置信息和第二速度和位置信息进行速度误差和位置误差的估计，得到误差估计结果。

步骤S502：基于误差估计结果对第二速度和位置信息进行校正，确定目标载体在当前时刻的运动状态估计结果。

具体地，基于数学平台结合GPS和SINS的输出可进行卡尔曼滤波算法ESKF融合过程，最后，根据估计得到的位置误差和速度误差对状态参数做出校正，从而得到了对位置和速度的估计。

如图7所示，通过利用上述旋转矩阵对加速度计输出的比力信息进行转换：

其中，a_n为转换后的加速度计的比力信息，a_b为加速度计的比力信息，

为旋转矩阵。

ESKF中选取得位置和速度的状态方程的数学模型为：

其中，e_vx,e_vy,e_vz分别是x,y,z方向的速度误差，e_px,e_py,e_pz分别是x,y,z方向的位置误差，Δx,Δy,Δz分别是x,y,z方向的加速度误差。

载体的运动状态方程的数学模型为：

V_k+1＝V_k+a_k+1*t

其中，P_k,V_k为k时刻的位置和速度信息，P_k+1,V_k+1为k+1时刻的位置和速度信息，a_k+1为k+1时刻的加速度信息。

卡尔曼滤波将惯性导航和卫星导航的结果进行融合，卡尔曼滤波融合的过程为：

计算预测值：

其中，

为k-1时刻的状态的估计值，Ψ_k/k-1为状态转移矩阵，

为根据k-1时刻得到的k时刻的预测值。

计算状态估计值：

其中，G_k为滤波增益，z_k为状态量测向量，C_k为状态量测矩阵，

为预测状态值，

为状态估计值。

计算增益：

其中，P_k/k-1为一步预测协方差矩阵，R_k为量测噪声矩阵，G_k为所求的增益。

计算一步预测均方误差阵：

其中，P_k-1为k-1时刻的估计均方误差矩阵，Γ_k-1为系统噪声驱动阵，Q_k-1为系统噪声矩阵，P_k/k-1为一步预测均方误差阵。

计算估计均方误差矩阵：

通过补偿后的角速度对四元数进行校正后即可得姿态角信息。在ESKF融合过程中，选取的状态为运动参数的误差，首先对误差状态做出预测，然后对预测的结果计算协方差，然后计算卡尔曼滤波增益，然后对位置和速度进行估计，然后对融合结果计算协方差，然后将这一误差补偿给位置和速度的输出，即可达到实时的对状态的校正和估计。

V_x＝V_{x_output}-e_vx

V_y＝V_{y_output}-e_vy

V_z＝V_{z_output}-e_vz

P_x＝P_{x_output}-e_px

P_y＝P_{y_output}-e_py

P_z＝P_{z_output}-e_pz

其中，V_{x_output},V_{y_output},V_{z_output},P_{x_output},P_{y_output},P_{z_output}分别是三维速度和三维位置的输出，e_vx,e_vy,e_vz,e_px,e_py,e_pz分别是融合估计得到的三位速度和三维位置的误差，V_x,V_y,V_z,P_x,P_y,P_z分别是校正后的三维速度和三维位置。

通过把互补滤波和ESKF结合，可以对载体的位置、速度进行实时的估计，且通过仿真可证实有较好的估计效果。

具体地，在一实施例中，上述的载体运动状态估计方法具体还包括如下步骤：

步骤S106：基于加速度信息和角速度信息，采用互补滤波算法计算目标载体在当前时刻的姿态角估计结果。

具体地，通过根据IMU输出的加速度信息和角速度信息结合互补滤波算法可以实现姿态角的估计，使用互补滤波进行四元数的估计，避免了使用卡尔曼滤波进行滤波融合估计时在角度计算过程中所引起的一些计算奇异和物理奇异问题，简化了计算过程。

通过执行上述步骤，本发明实施例提供的载体运动状态估计方法，通过利用互补滤波对载体的四元数先进行估计，再基于估计信息构建旋转矩阵对位置和速度进行解算和与卫星导航系统的速度位置进行融合，实现了对位置、速度的较好估计，避免了在四元数计算的部分使用其它的传感器如罗盘等，减少了硬件依赖，并通过结合加速度计和陀螺仪的各自的优点，充分利用了加速度计的低频信息和陀螺仪的高频信息，实现了优势互补，从而对四元数进行了更新，然后构建出旋转矩阵能够更加准确的解算出速度和位置信息，此外，使用互补滤波进行四元数的估计，避免了后续进行融合估计时在角度计算过程中所引起的一些计算奇异和物理奇异问题，简化了计算过程，提高了效率。

在实际应用中，经常使用的滤波方法有维纳滤波，卡尔曼滤波等方法，维纳滤波和卡尔曼滤波都可以在一定程度上解决对于含噪声信号的最优估计。在使用维纳滤波的时候要求输入过程是广义平稳的且输入过程的统计特性已知，因而在实际使用中很受限，而卡尔曼滤波可用于平稳和不平稳的随机过程。同时维纳滤波适用于标量滤波的情况，而卡尔曼滤波则适用于矢量滤波。维纳过滤是依据所有过去和当前观测的数据来估计信号的当前值，而卡尔曼滤波是用前一个估计值和最近的一个观测数据(而不需要所有过去的观测数据)结合来估计信号的当前值，因此在运用计算机计算时更方便。因此，卡尔曼滤波器相比于维纳滤波有更多的优势且可以递推实现。

针对卡尔曼滤波进行组合导航可以对载体的位置、速度和姿态进行估计，对于位置和速度这两个状态在惯性导航结合GPS导航的卡尔曼滤波下，可以实现较好的融合效果，对于姿态的估计则一般需要使用磁罗盘才可以实现较好的估计，本发明在姿态这部分使用了互补滤波进行处理，然后基于姿态的估计结果，进一步进行位置和速度的估计。

互补滤波是一种基于两个传感器，然后通过一些互补滤波方法将两者的数据进行融合的滤波算法。这两个传感器的优缺点互补，加速度计的低频信号较好，而陀螺仪的高频信号较好，从而结合加速度计和陀螺仪的各自的优点就可以进行互补滤波估计，从而可以实现对角速度的补偿，进一步得到更新的四元数。

因此，本发明提出了一种新的组合导航估计方案，通过使用互补滤波更新了四元数，基于四元数的估计可以得到旋转矩阵，在下一步的位置和速度估计中，基于旋转矩阵利用卡尔曼滤波方法实现了对位置和速度较为准确的估计。与传统的融合方案相比，本发明结合了互补滤波对姿态角进行估计，首先利用互补滤波对载体的四元数先进行估计，再基于估计的姿态角信息继续使用卡尔曼滤波对位置和速度进行估计融合，实现了对位置、速度和姿态的较好估计。通过互补滤波结合卡尔曼滤波的优点如下：

1.避免了在四元数计算的部分使用其它的传感器如罗盘等。

2.通过结合加速度计和陀螺仪的各自的优点，充分利用了加速度计的低频信息和陀螺仪的高频信息，实现了优势互补，从而对四元数进行了更新，然后构建出数学平台在卡尔曼滤波中使用。

3.使用互补滤波进行四元数的估计，避免了使用卡尔曼滤波进行滤波融合估计时在角度计算过程中所引起的一些计算奇异和物理奇异问题，简化了计算过程。

本发明实施例还提供了一种载体运动状态估计装置，如图8所示，该载体运动状态估计装置具体包括：

获取模块101，用于获取目标载体的加速度信息和角速度信息以及卫星导航系统下目标载体的第一速度和位置信息。详细内容参见上述方法实施例中步骤S101的相关描述，在此不再进行赘述。

第一处理模块102，用于基于加速度信息和角速度信息，采用互补滤波算法计算四元数。详细内容参见上述方法实施例中步骤S102的相关描述，在此不再进行赘述。

第二处理模块103，用于基于四元数构建旋转矩阵。详细内容参见上述方法实施例中步骤S103的相关描述，在此不再进行赘述。

第三处理模块104，用于利用旋转矩阵进行目标载体速度和位置的解算，得到惯性导航系统下的第二速度和位置信息。详细内容参见上述方法实施例中步骤S104的相关描述，在此不再进行赘述。

第四处理模块105，用于基于第一速度和位置信息和第二速度和位置信息，进行速度和位置的估计融合，得到目标载体在当前时刻的运动状态估计结果。详细内容参见上述方法实施例中步骤S105的相关描述，在此不再进行赘述。

上述各个模块的更进一步的功能描述与上述对应方法实施例相同，在此不再赘述。

通过上述各个组成部分的协同合作，本发明实施例提供的载体运动状态估计装置，通过利用互补滤波对载体的四元数先进行估计，再基于估计信息构建旋转矩阵对位置和速度进行解算和与卫星导航系统的速度位置进行融合，实现了对位置、速度的较好估计，避免了在四元数计算的部分使用其它的传感器如罗盘等，减少了硬件依赖，并通过结合加速度计和陀螺仪的各自的优点，充分利用了加速度计的低频信息和陀螺仪的高频信息，实现了优势互补，从而对四元数进行了更新，然后构建出旋转矩阵能够更加准确的解算出速度和位置信息，此外，使用互补滤波进行四元数的估计，避免了后续进行融合估计时在角度计算过程中所引起的一些计算奇异和物理奇异问题，简化了计算过程，提高了效率。

本发明实施例还提供了一种电子设备，如图9所示，该电子设备可以包括处理器901和存储器902，其中处理器901和存储器902可以通过总线或者其他方式连接，图9中以通过总线连接为例。

处理器901可以为中央处理器(Central Processing Unit，CPU)。处理器901还可以为其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等芯片，或者上述各类芯片的组合。

存储器902作为一种非暂态计算机可读存储介质，可用于存储非暂态软件程序、非暂态计算机可执行程序以及模块，如本发明实施例中的方法所对应的程序指令/模块。处理器901通过运行存储在存储器902中的非暂态软件程序、指令以及模块，从而执行处理器的各种功能应用以及数据处理，即实现上述方法。

存储器902可以包括存储程序区和存储数据区，其中，存储程序区可存储操作装置、至少一个功能所需要的应用程序；存储数据区可存储处理器901所创建的数据等。此外，存储器902可以包括高速随机存取存储器，还可以包括非暂态存储器，例如至少一个磁盘存储器件、闪存器件、或其他非暂态固态存储器件。在一些实施例中，存储器902可选包括相对于处理器901远程设置的存储器，这些远程存储器可以通过网络连接至处理器901。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

一个或者多个模块存储在存储器902中，当被处理器901执行时，执行上述方法。

上述电子设备具体细节可以对应参阅上述方法实施例中对应的相关描述和效果进行理解，此处不再赘述。

本领域技术人员可以理解，实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，实现的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)、随机存储记忆体(Random Access Memory，RAM)、快闪存储器(Flash Memory)、硬盘(Hard Disk Drive，缩写：HDD)或固态硬盘(Solid-State Drive，SSD)等；存储介质还可以包括上述种类的存储器的组合。

以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求范围当中。

Claims (10)

1.一种载体运动状态估计方法，其特征在于，包括：

获取目标载体的加速度信息和角速度信息以及卫星导航系统下所述目标载体的第一速度和位置信息；

基于所述加速度信息和所述角速度信息，采用互补滤波算法计算四元数；

基于所述四元数构建旋转矩阵；

利用所述旋转矩阵进行所述目标载体速度和位置的解算，得到惯性导航系统下的第二速度和位置信息；

基于所述第一速度和位置信息和所述第二速度和位置信息进行速度和位置的估计融合，得到所述目标载体在当前时刻的运动状态估计结果。

2.根据权利要求1所述的方法，其特征在于，所述获取目标载体的加速度信息和角速度信息，包括：

获取所述目标载体上加速度计测量得到的加速度信息以及陀螺仪测量得到的角速度信息。

3.根据权利要求1所述的方法，其特征在于，所述基于所述加速度信息和所述角速度信息，采用互补滤波算法计算四元数，包括：

将所述加速度信息中进行归一化处理，得到归一化的加速度信息；

将重力加速度转换至载体坐标系下，得到参考加速度；

对所述归一化的加速度信息和所述参考加速度进行叉积运算，得到叉积误差；

对所述叉积误差进行PI运算，对所述角速度信息进行补偿，得到补偿后的角速度信息；

基于补偿后的角速度信息计算所述四元数。

4.根据权利要求3所述的方法，其特征在于，所述四元数通过如下公式计算：

其中，q₀,q₁,q₂,q₃为四元数，T为周期，w_kx,w_ky,w_kz为补偿后的角速度。

5.根据权利要求4所述的方法，其特征在于，所述旋转矩阵通过如下公式表示：

其中，

为旋转矩阵，q₀,q₁,q₂,q₃为四元数。

6.根据权利要求1所述的方法，其特征在于，所述基于所述第一速度和位置信息和所述第二速度和位置信息进行速度和位置的估计融合，得到所述目标载体在当前时刻的运动状态估计结果，包括：

利用卡尔曼滤波算法对所述第一速度和位置信息和所述第二速度和位置信息进行融合，得到所述目标载体在当前时刻的速度和位置估计结果。

7.根据权利要求1所述的方法，其特征在于，所述基于所述第一速度和位置信息和所述第二速度和位置信息进行速度和位置的估计融合，得到所述目标载体在当前时刻的运动状态估计结果，包括：

基于所述第一速度和位置信息和所述第二速度和位置信息进行速度误差和位置误差的估计，得到误差估计结果；

基于所述误差估计结果对所述第二速度和位置信息进行校正，确定所述目标载体在当前时刻的运动状态估计结果。

8.一种载体运动状态估计装置，其特征在于，包括：

获取模块，用于获取目标载体的加速度信息和角速度信息以及卫星导航系统下所述目标载体的第一速度和位置信息；

第一处理模块，用于基于所述加速度信息和所述角速度信息，采用互补滤波算法计算四元数；

第二处理模块，用于基于所述四元数构建旋转矩阵；

第三处理模块，用于利用所述旋转矩阵进行所述目标载体速度和位置的解算，得到惯性导航系统下的第二速度和位置信息；

第四处理模块，用于基于所述第一速度和位置信息和所述第二速度和位置信息，进行速度和位置的估计融合，得到所述目标载体在当前时刻的运动状态估计结果。

9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储计算机指令，所述计算机指令被处理器执行时实现如权利要求1-7中任一项所述的方法。

10.一种电子设备，其特征在于，包括：

存储器和处理器，所述存储器和所述处理器之间互相通信连接，所述存储器中存储有计算机指令，所述处理器通过执行所述计算机指令，以执行如权利要求1-7中任一项所述方法。

Images