CN105865452B - 一种基于间接卡尔曼滤波的移动平台位姿估计方法 - Google Patents

Abstract

本发明公开了一种基于间接卡尔曼滤波的移动平台位姿估计方法，该方法包括以下步骤：将所述捷联惯导系统的推算值X_I和所述全球定位系统的观测数据为X_N二者的差值作为所述间接卡尔曼滤波器的观测输入，间接卡尔曼滤波器内部构造预测方程，滤波器输出为误差的估计值此输出中的部分参数反馈用于校正所述捷联惯导系统以及全球定位系统观测计算中的参数，以修正二者的误差计算，X_I与中坐标与姿态等参数的和作为最终系统各项数值的估计值。同传统的采用直接卡尔曼滤波算法的方法相比,本发明不但能够给出更为准确的位姿估计结果,且有效提高机器人的工作效率。

Description

一种基于间接卡尔曼滤波的移动平台位姿估计方法

技术领域

本发明属于移动平台位姿估计方法技术领域，尤其是涉及一种基于间接卡尔曼滤波的移动平台位姿估计方法。

背景技术

对于移动平台(例如移动机器人)而言，其核心问题就是自动计算移动平台的位置和姿态，其中位置为三维空间的笛卡尔坐标(x，y，z)，姿态为三维空间的姿态角(φ,θ,γ)，获取了位姿以后，就可以通过控制算法对移动平台的运动轨迹进行控制，从而使之按照设定轨迹进行运动。

移动平台常用的硬件框架是惯导器件(陀螺仪、加速度计等惯导器件)+观测器件(如GPS、激光、视觉等观测设备)，位姿算法一般采用直接卡尔曼滤波算法，以坐标和姿态角的直接量作为状态变量，观测器件的直接量为观测值，建立卡尔曼滤波预测和观测方程进行计算，其滤波方程比较复杂，方程中往往包含有不少非线性公式，无法使用线性卡尔曼滤波算法进行计算，一般采用扩展卡尔曼滤波算法，从而影响到计算的简化和精度，而且对多种传感器的数据融合也有不利的影响，因此需要一种更好的方法来进行位姿的估算。

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种基于间接卡尔曼滤波的移动平台位姿估计方法，。

为了达到上述发明目的，解决其技术问题所采用的技术方案如下：

本发明公开了一种基于间接卡尔曼滤波的移动平台位姿估计方法，该方法包括以下步骤：

步骤1：构建基于间接卡尔曼滤波的移动平台，该移动平台包括捷联惯导系统、全球定位系统和间接卡尔曼滤波器；

步骤2：根据捷联惯导系统中的陀螺仪的角速度测量值[ω_xω_yω_z]，通过递推计算旋转矩阵并根据捷联惯导系统中的里程计的测量值计算速度测量值在导航系统中的投影

步骤3：采用间接卡尔曼滤波器构建系统的状态预测方程和观测方程，并利用状态预测方程计算状态的预测值；

步骤4：根据捷联惯导系统中的陀螺仪和里程计中计算得到的数据推算x_m、y_m、z_m，得到捷联惯导系统的推算值X_I；

步骤5：将全球定位系统中的观测数据X_N和步骤4中的捷联惯导系统中的推算值X_I做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增益矩阵得到误差的估计值

步骤6：将误差估计值作为反馈，用于修正捷联惯导系统的递推值和全球定位系统的观测数据，并将捷联惯导系统的推算值X_I和误差估计值结合得到导航系统所需的移动平台的位置和姿态；

步骤7：返回步骤1，循环计算，直到达到导航系统设定的停止条件为止。

进一步的，步骤2具体包括以下步骤：

根据捷联惯导系统中的陀螺仪的角速度测量值[ω_xω_yω_z]，通过四元数递推计算得出旋转矩阵递推计算公式如下：

初始旋转矩阵

公式1中，

进一步通过旋转矩阵计算出惯导递推的计算公式如下：

公式2中，分别代表在导航坐标系中的角度预测值、y、z表示绕x、y、z轴旋转的角度，即分别绕北东地旋转的角度；R₁₁代表旋转矩阵中的第1行第1列的数据，R₁₂代表旋转矩阵中的第1行第2列的数据，R₁₃代表旋转矩阵中的第1行第3列的数据，R₂₃代表旋转矩阵中的第2行第3列的数据，R₃₃代表旋转矩阵中的第3行第3列的数据；

并根据捷联惯导系统中的里程计测量值计算速度测量值在导航系中的投影计算公式如下：

公式3中，分别为里程计测量值计算速度测量值在导航系北、东和地三个方向上的速度投影。

进一步的，步骤3具体包括以下步骤：

首先，定义系统状态变量：

δv_N、δv_E、δv_D、δx、δy、δz和δK_A

其中，δv_N、δv_E、δv_D分别为移动平台在北、东和地三个方向的速度误差，分别为移动平台在x、y和z三个方向的姿态误差，δx、δy、δz分别为移动平台在x、y和z三个方向的坐标误差，以及δK_A为里程计的刻度误差；

其次，构建依据上述系统状态变量的状态预测方程：

公式4中，v_N、v_E、v_D分别为移动平台在北、东和地三个方向上的速度，且考虑到车轮运动存在打滑的情况，q₀～q₃即为车轮打滑时速度误差的方差；

公式5-7中，考虑到陀螺仪的零偏不稳定性因素，q₃～q₅即为角度误差方差；

δx(k+1)＝δv_N(k)*T (公式8)

δy(k+1)＝δv_E(k)*T (公式9)

δz(k+1)＝δv_D(k)*T (公式10)

公式8-10中，T为两次观测之间的间隔时间；

K_A(k+1)＝δK_A(k)+q₆ (公式11)

公式11中，考虑误差系数存在漂移的情况，q₆即为里程计刻度误差系数的方差；

再者，构建v_N、v_E、v_D的速度误差与x、y、z方向上的坐标误差的观测方程如下：

z₁(k)＝δv_N(k)+ξ₀ (公式12)

z₂(k)＝δv_E(k)+ξ₁ (公式13)

z₃(k)＝δv_D(k)+ξ₂ (公式14)

z₄(k)＝δx(k)+ξ₃ (公式15)

z₅(k)＝δy(k)+ξ₄ (公式16)

z₆(k)＝δz(k)+ξ₅ (公式17)

公式12-17中，考虑到GPS观测值存在一定误差，其中ξ₀～ξ₅即为观测方差。

进一步的，上述公式4的具体推算过程如下：

根据运载系统的比力方程：

将比力方程向导航坐标系中投影得到：

比较公式19与公式20，并省略二阶以上小量，得到：

将比力中的向心加速度项考虑作为误差，可认为δK_A为里程计误差系数并忽略地球自转角速度，因此得到：

进一步的，步骤4具体包括以下步骤：

根据已经推算得到的进一步计算x_m、y_m、z_m，计算方法如下：

公式22中，x_m、y_m、z_m分别代表在导航坐标系中的xyz坐标，即北东地方向的坐标；分别为里程计测量值计算速度测量值在导航系北、东和地三个方向上的速度投影。

进一步的，步骤5具体包括以下步骤：

用全球定位系统所测得的3个速度观测数据v_N、v_E、v_D和3个位置观测数据x、y、z与捷联惯导系统中的推算值X_I做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增益矩阵得到误差的估计值：

δv_N、δv_E、δv_D、δx、δy、δz和δK_A

其中，δv_N、δv_E、δv_D分别为移动平台在北、东和地三个方向的速度误差，分别为移动平台在x、y和z三个方向的姿态误差，δx、δy、δz分别为移动平台在x、y和z三个方向的坐标误差，以及δK_A为里程计的刻度误差。

进一步的，步骤6具体包括以下步骤：

将误差估计值作为反馈，用于修正捷联惯导系统的递推值和全球定位系统的观测数据，并将捷联惯导系统的推算值X_I和误差估计值结合得到导航系统所需的移动平台的位置和姿态，即：

公式23-28中，分别代表在导航坐标系中的角度预测值、y、z表示绕x、y、z轴旋转的角度，即分别绕北东地旋转的角度；x_m、y_m、z_m分别代表在导航坐标系中的xyz坐标，即北东地方向的坐标。

进一步的，步骤7中所述导航系统设定的停止条件为误差值等于0。

本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效果：

1.在本发明中，以惯导器件陀螺仪、里程计为状态递推依据，全球定位系统GPS的观测数据减去捷联惯导系统的递推值作为观测值，共同组成间接卡尔曼滤波的状态和输出方程。计算公式得到简化且更为直观，卡尔曼滤波计算更快速，计算结果精度也得到有效提高；

2.同传统的采用直接卡尔曼滤波算法的方法相比,本发明不但能够给出更为准确的估计结果,且拓宽了系统应用范围；

3.本发明位姿估计方法定位准确且易于实现，在移动机器人的仿真过程中，移动机器人的位姿估计更加精确，有效提高机器人的工作效率。

附图说明

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

图1为本发明一种基于间接卡尔曼滤波的移动平台位姿估计方法中的系统总体框架示意图。

具体实施方式

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

如图1所示为系统总体框架示意图，所述捷联惯导系统的推算值为X_I，所述全球定位系统的观测数据为X_N，二者的差值作为所述间接卡尔曼滤波器的观测输入，间接卡尔曼滤波器内部构造预测方程，滤波器输出为误差的估计值此输出中的部分参数反馈用于校正所述捷联惯导系统以及全球定位系统观测计算中的参数，以修正二者的误差计算，X_I与中坐标与姿态等参数的和作为最终系统各项数值的估计值。本实施例中，观测器件使用的是全球定位系统，但采用激光、视觉等观测设备也能达到同样的技术效果，也是本发明所保护的技术范围。

图中，间接卡尔曼滤波器不直接估计导航系统的状态量(如坐标、姿态等)，而是估计捷联惯导系统与真值之间的误差，因此以全球定位系统的观测数据与捷联惯导系统的推算值的误差作为观测值求解卡尔曼滤波结果。

本实施例具体的步骤如下：

本发明公开了一种基于间接卡尔曼滤波的移动平台位姿估计方法，该方法包括以下步骤：

步骤1：构建基于间接卡尔曼滤波的移动平台，该移动平台包括捷联惯导系统、全球定位系统和间接卡尔曼滤波器；

步骤2：根据捷联惯导系统中的陀螺仪的角速度测量值[ω_xω_yω_z]，通过递推计算旋转矩阵并根据捷联惯导系统中的里程计的测量值计算速度测量值在导航系统中的投影

步骤3：采用间接卡尔曼滤波器构建系统的状态预测方程和观测方程，并利用状态预测方程计算状态的预测值；

步骤4：根据捷联惯导系统中的陀螺仪和里程计中计算得到的数据推算x_m、y_m、z_m，得到捷联惯导系统的推算值X_I；

步骤5：将全球定位系统中的观测数据X_N和步骤4中的捷联惯导系统中的推算值X_I做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增益矩阵得到误差的估计值

步骤6：将误差估计值作为反馈，用于修正捷联惯导系统的递推值和全球定位系统的观测数据，并将捷联惯导系统的推算值X_I和误差估计值结合得到导航系统所需的移动平台的位置和姿态；

步骤7：返回步骤1，循环计算，直到达到导航系统设定的停止条件为止。

具体实施例中，步骤2具体包括以下步骤：

根据捷联惯导系统中的陀螺仪的角速度测量值[ω_xω_yω_z]，通过四元数递推计算得出旋转矩阵递推计算公式如下：

初始旋转矩阵

公式1中，

进一步通过旋转矩阵计算出惯导递推的计算公式如下：

公式2中，分别代表在导航坐标系中的角度预测值、y、z表示绕x、y、z轴旋转的角度，即分别绕北东地旋转的角度；R₁₁代表旋转矩阵中的第1行第1列的数据，R₁₂代表旋转矩阵中的第1行第2列的数据，R₁₃代表旋转矩阵中的第1行第3列的数据，R₂₃代表旋转矩阵中的第2行第3列的数据，R₃₃代表旋转矩阵中的第3行第3列的数据；

并根据捷联惯导系统中的里程计测量值计算速度测量值在导航系中的投影计算公式如下：

公式3中，分别为里程计测量值计算速度测量值在导航系北、东和地三个方向上的速度投影。

具体实施例中，步骤3具体包括以下步骤：

首先，定义系统状态变量：

δv_N、δv_E、δv_D、δx、δy、δz和δK_A

其中，δv_N、δv_E、δv_D分别为移动平台在北、东和地三个方向的速度误差，分别为移动平台在x、y和z三个方向的姿态误差，δx、δy、δz分别为移动平台在x、y和z三个方向的坐标误差，以及δK_A为里程计的刻度误差；

其次，构建依据上述系统状态变量的状态预测方程：

公式4中，v_N、v_E、v_D分别为移动平台在北、东和地三个方向上的速度，且考虑到车轮运动存在打滑的情况，q₀～q₃即为车轮打滑时速度误差的方差；

公式5-7中，考虑到陀螺仪的零偏不稳定性因素，q₃～q₅即为角度误差方差；

δx(k+1)＝δv_N(k)*T (公式8)

δy(k+1)＝δv_E(k)*T (公式9)

δz(k+1)＝δv_D(k)*T (公式10)

公式8-10中，T为两次观测之间的间隔时间；

K_A(k+1)＝δK_A(k)+q₆ (公式11)

公式11中，考虑误差系数存在漂移的情况，q₆即为里程计刻度误差系数的方差；

再者，构建v_N、v_E、v_D的速度误差与x、y、z方向上的坐标误差的观测方程如下：

z₁(k)＝δv_N(k)+ξ₀ (公式12)

z₂(k)＝δv_E(k)+ξ₁ (公式13)

z₃(k)＝δv_D(k)+ξ₂ (公式14)

z₄(k)＝δx(k)+ξ₃ (公式15)

z₅(k)＝δy(k)+ξ₄ (公式16)

z₆(k)＝δz(k)+ξ₅ (公式17)

公式12-17中，考虑到GPS观测值存在一定误差，其中ξ₀～ξ₅即为观测方差。

具体实施例中，上述公式4的具体推算过程如下：

根据运载系统的比力方程：

将比力方程向导航坐标系中投影得到：

比较公式19与公式20，并省略二阶以上小量，得到：

将比力中的向心加速度项考虑作为误差，可认为δK_A为里程计误差系数并忽略地球自转角速度，因此得到：

具体实施例中，步骤4具体包括以下步骤：

根据已经推算得到的进一步计算x_m、y_m、z_m，计算方法如下：

公式22中，x_m、y_m、z_m分别代表在导航坐标系中的xyz坐标，即北东地方向的坐标；分别为里程计测量值计算速度测量值在导航系北、东和地三个方向上的速度投影。

具体实施例中，步骤5具体包括以下步骤：

用全球定位系统所测得的3个速度观测数据v_N、v_E、v_D和3个位置观测数据x、y、z与捷联惯导系统中的推算值X_I做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增益矩阵得到误差的估计值：

δv_N、δv_E、δv_D、δx、δy、δz和δK_A

其中，δv_N、δv_E、δv_D分别为移动平台在北、东和地三个方向的速度误差，分别为移动平台在x、y和z三个方向的姿态误差，δx、δy、δz分别为移动平台在x、y和z三个方向的坐标误差，以及δK_A为里程计的刻度误差。

具体实施例中，步骤6具体包括以下步骤：

将误差估计值作为反馈，用于修正捷联惯导系统的递推值和全球定位系统的观测数据，并将捷联惯导系统的推算值X_I和误差估计值结合得到导航系统所需的移动平台的位置和姿态，即：

公式23-28中，分别代表在导航坐标系中的角度预测值、y、z表示绕x、y、z轴旋转的角度，即分别绕北东地旋转的角度；x_m、y_m、z_m分别代表在导航坐标系中的xyz坐标，即北东地方向的坐标。

进一步的，步骤7中所述导航系统设定的停止条件为误差值等于0。

本发明中，以惯导器件陀螺仪、里程计为状态递推依据，全球定位系统GPS的观测数据减去捷联惯导系统的递推值作为观测值，共同组成间接卡尔曼滤波的状态和输出方程。计算公式得到简化且更为直观，卡尔曼滤波计算更快速，计算结果精度也得到有效提高。同传统的采用直接卡尔曼滤波算法的方法相比,本发明不但能够给出更为准确的估计结果,且拓宽了系统应用范围。使得移动机器人的定位更为准确且易于实现，在移动机器人的仿真过程中，移动机器人的位姿估计更加精确，能有效提高机器人的工作效率。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (7)

1.一种基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，该方法包括以下步骤：

步骤1：构建基于间接卡尔曼滤波的移动平台，该移动平台包括捷联惯导系统、全球定位系统和间接卡尔曼滤波器；

步骤2：根据捷联惯导系统中的陀螺仪的角速度测量值[ω_x ω_y ω_z]，通过递推计算旋转矩阵并根据捷联惯导系统中的里程计的测量值计算速度测量值在导航系统中的投影

步骤2具体包括以下步骤：

根据捷联惯导系统中的陀螺仪的角速度测量值[ω_x ω_y ω_z]，通过四元数递推计算得出旋转矩阵递推计算公式如下：

初始旋转矩阵

公式1中，T是数据周期，即k-1时刻到k时刻之间的时间间隔；指角速度矢量的模，即矢量大小；

进一步通过旋转矩阵计算出惯导递推的计算公式如下：

公式2中，表示根据陀螺仪角速度计算的载体在导航坐标系中的三轴方位角度，因导航坐标系选取的是北东地坐标系，所以分别表示载体在北东地坐标系下的横滚角、俯仰角、航向角；R₁₁代表旋转矩阵中的第1行第1列的数据，R₁₂代表旋转矩阵中的第1行第2列的数据，R₁₃代表旋转矩阵中的第1行第3列的数据，R₂₃代表旋转矩阵中的第2行第3列的数据，R₃₃代表旋转矩阵中的第3行第3列的数据；

并根据捷联惯导系统中的里程计测量值计算速度测量值在导航系中的投影计算公式如下：

公式3中，分别为里程计测量值计算速度测量值在导航系北、东和地三个方向上的速度投影；

步骤3：采用间接卡尔曼滤波器构建系统的状态预测方程和观测方程，并利用状态预测方程计算状态的预测值；

步骤4：根据捷联惯导系统中的陀螺仪和里程计中计算得到的数据推算x_m、y_m、z_m，得到捷联惯导系统的推算值X_I；

步骤5：将全球定位系统中的观测数据X_N和步骤4中的捷联惯导系统中的推算值X_I做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增益矩阵得到误差的估计值

步骤6：将误差估计值作为反馈，用于修正捷联惯导系统的递推值和全球定位系统的观测数据，并将捷联惯导系统的推算值X_I和误差估计值结合得到导航系统所需的移动平台的位置和姿态；

步骤7：返回步骤1，循环计算，直到达到导航系统设定的停止条件为止。

2.如权利要求1所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，步骤3具体包括以下步骤：

首先，定义系统状态变量：

δv_N、δv_E、δv_D、δx、δy、δz和δK_A

其中，δv_N、δv_E、δv_D分别为移动平台在北、东和地三个方向的速度误差，分别为移动平台在x、y和z三个方向的姿态误差，δx、δy、δz分别为移动平台在x、y和z三个方向的坐标误差，以及δK_A为里程计的刻度误差；

其次，构建依据上述系统状态变量的状态预测方程：

公式4中，v_N、v_E、v_D分别为移动平台在北、东和地三个方向上的速度，且考虑到车轮运动存在打滑的情况，q₀～q₃即为车轮打滑时速度误差的方差；

公式5-7中，考虑到陀螺仪的零偏不稳定性因素，q₃～q₅即为角度误差方差；

δx(k+1)＝δv_N(k)*T (公式8)

δy(k+1)＝δv_E(k)*T (公式9)

δz(k+1)＝δv_D(k)*T (公式10)

公式8-10中，T为两次观测之间的间隔时间；

K_A(k+1)＝δK_A(k)+q₆ (公式11)

公式11中，考虑误差系数存在漂移的情况，q₆即为里程计刻度误差系数的方差；

再者，构建v_N、v_E、v_D的速度误差与x、y、z方向上的坐标误差的观测方程如下：

z₁(k)＝δv_N(k)+ξ₀ (公式12)

z₂(k)＝δv_E(k)+ξ₁ (公式13)

z₃(k)＝δv_D(k)+ξ₂ (公式14)

z₄(k)＝δx(k)+ξ₃ (公式15)

z₅(k)＝δy(k)+ξ₄ (公式16)

z₆(k)＝δz(k)+ξ₅ (公式17)

公式12-17中，考虑到GPS观测值存在一定误差，其中ξ₀～ξ₅即为观测方差。

3.如权利要求2所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，上述公式4的具体推算过程如下：

根据运载系统的比力方程：

将比力方程向导航坐标系中投影得到：

比较公式19与公式20，并省略二阶以上小量，得到：

将比力中的向心加速度项考虑作为误差，可认为δK_A为里程计误差系数并忽略地球自转角速度，因此得到：

4.如权利要求1所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，步骤4具体包括以下步骤：

根据已经推算得到的进一步计算x_m、y_m、z_m，计算方法如下：

公式22中，x_m、y_m、z_m分别代表在导航坐标系中的xyz坐标，即北东地方向的坐标；分别为里程计测量值计算速度测量值在导航系北、东和地三个方向上的速度投影。

5.如权利要求1所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，步骤5具体包括以下步骤：

用全球定位系统所测得的3个速度观测数据v_N、v_E、v_D和3个位置观测数据x、y、z与捷联惯导系统中的推算值X_I做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增益矩阵得到误差的估计值：

δv_N、δv_E、δv_D、δx、δy、δz和δK_A

其中，δv_N、δv_E、δv_D分别为移动平台在北、东和地三个方向的速度误差，分别为移动平台在x、y和z三个方向的姿态误差，δx、δy、δz分别为移动平台在x、y和z三个方向的坐标误差，以及δK_A为里程计的刻度误差。

6.如权利要求1所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，步骤6具体包括以下步骤：

将误差估计值作为反馈，用于修正捷联惯导系统的递推值和全球定位系统的观测数据，并将捷联惯导系统的推算值X_I和误差估计值结合得到导航系统所需的移动平台的位置和姿态，即：

公式23-28中，分别代表在导航坐标系中的角度预测值、y、z表示绕x、y、z轴旋转的角度，即分别绕北东地旋转的角度；x_m、y_m、z_m分别代表在导航坐标系中的xyz坐标，即北东地方向的坐标。

7.如权利要求1所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，步骤7中所述导航系统设定的停止条件为误差值等于0。