一种基于间接卡尔曼滤波的移动平台位姿估计方法
【专利摘要】本发明公开了一种基于间接卡尔曼滤波的移动平台位姿估计方法，该方法包括以下步骤：将所述捷联惯导系统的推算值XI和所述全球定位系统的观测数据为XN二者的差值作为所述间接卡尔曼滤波器的观测输入，间接卡尔曼滤波器内部构造预测方程，滤波器输出为误差的估计值此输出中的部分参数反馈用于校正所述捷联惯导系统以及全球定位系统观测计算中的参数，以修正二者的误差计算，XI与中坐标与姿态等参数的和作为最终系统各项数值的估计值。同传统的采用直接卡尔曼滤波算法的方法相比,本发明不但能够给出更为准确的位姿估计结果,且有效提高机器人的工作效率。
【专利说明】
一种基于间接卡尔曼滤波的移动平台位姿估计方法
技术领域
[0001] 本发明属于移动平台位姿估计方法技术领域，尤其是涉及一种基于间接卡尔曼滤 波的移动平台位姿估计方法。
【背景技术】
[0002] 对于移动平台（例如移动机器人)而言，其核心问题就是自动计算移动平台的位置 和姿态，其中位置为三维空间的笛卡尔坐标(x，y，z)，姿态为三维空间的姿态角（Φ，Θ，γ)， 获取了位姿以后，就可以通过控制算法对移动平台的运动轨迹进行控制，从而使之按照设 定轨迹进行运动。
[0003] 移动平台常用的硬件框架是惯导器件（陀螺仪、加速度计等惯导器件)+观测器件 (如GPS、激光、视觉等观测设备），位姿算法一般采用直接卡尔曼滤波算法，以坐标和姿态角 的直接量作为状态变量，观测器件的直接量为观测值，建立卡尔曼滤波预测和观测方程进 行计算，其滤波方程比较复杂，方程中往往包含有不少非线性公式，无法使用线性卡尔曼滤 波算法进行计算，一般采用扩展卡尔曼滤波算法，从而影响到计算的简化和精度，而且对多 种传感器的数据融合也有不利的影响，因此需要一种更好的方法来进行位姿的估算。
【发明内容】
[0004] 鉴于以上所述现有技术的缺点，本发明的目的在于提供一种基于间接卡尔曼滤波 的移动平台位姿估计方法，。
[0005] 为了达到上述发明目的，解决其技术问题所采用的技术方案如下：
[0006] 本发明公开了一种基于间接卡尔曼滤波的移动平台位姿估计方法，该方法包括以 下步骤：
[0007] 步骤1:构建基于间接卡尔曼滤波的移动平台，该移动平台包括捷联惯导系统、全 球定位系统和间接卡尔曼滤波器；
[0008] 步骤2:根据捷联惯导系统中的陀螺仪的角速度测量值[ω χ ω y ω ζ]，通过递推计算 旋转矩阵，并根据捷联惯导系统中的里程计的测量值计算速度测量值在导航系统中的投 影 V.miV.、>'威:、VmD.;
[0009] 步骤3:采用间接卡尔曼滤波器构建系统的状态预测方程和观测方程，并利用状态 预测方程计算状态的预测值；
[0010] 步骤4:根据捷联惯导系统中的陀螺仪和里程计中计算得到的数据推算Xm、ym、zm， 得到捷联惯导系统的推算值X:;
[0011] 步骤5:将全球定位系统中的观测数据XN和步骤4中的捷联惯导系统中的推算值X: 做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增益矩阵得到误差的估计 值威 1:;
[0012] 步骤6:将误差估计值作为反馈，用于修正捷联惯导系统的递推值和全球定位 系统的观测数据，并将捷联惯导系统的推算值X:和误差估计值结合得到导航系统所需 的移动平台的位置和姿态；
[0013]步骤7:返回步骤1，循环计算，直到达到导航系统设定的停止条件为止。
[0014]进一步的，步骤2具体包括以下步骤：
[0015] 根据捷联惯导系统中的陀螺仪的角速度测量值[ωχωγωζ]，通过四元数递推计算 得出旋转矩阵G ,递推计算公式如下：
[0016] 巧(幻=成1)，初始旋转矩阵/
[0019] 进一步通过旋转矩阵C；：计算出惯导递推的 ，计算公式如下：
[0020]
[0021] 公式2中，^、<、<分别代表在导航坐标系中的角度预测值、y、z表示绕x、y、z 轴旋转的角度，即分别绕北东地旋转的角度;Rn代表旋转矩阵q中的第1行第1列的数据， r12代表旋转矩阵q中的第1行第2列的数据，r13代表旋转矩阵?：中的第1行第3列的数据， R23代表旋转矩阵0；中的第2行第3列的数据，R33代表旋转矩阵q中的第3行第3列的数据；
[0022] 并根据捷联惯导系统中的里程计测量值计算速度测量值在导航系中的投影 V?，.V、VmE、Vmt>，计算公式如下：
[0023]
[0024] 公式3中，V:、<E、分别为里程计测量值计算速度测量值在导航系北、东和地 三个方向上的速度投影。
[0025] 进一步的，步骤3具体包括以下步骤：
[0026]首先，定义系统状态变量：
[0027] 5vn、3ve、3vd、^*.、知2、心、37、52和31(八
[0028] 其中，δνΝ、δνΕ、δν?分别为移动平台在北、东和地三个方向的速度误差， y分别为移动平台在X、y和Ζ三个方向的姿态误差，δχ、δγ、δζ分别为移动平台 在x、y和z三个方向的坐标误差，以及δΚΑ为里程计的刻度误差；
[0029] 其次，构建依据上述系统状态变量的状态预测方程：
[0030]
[0031]公式4中，VN、VE、VD分别为移动平台在北、东和地三个方向上的速度，且考虑到车轮 运动存在打滑的情况，qo~q3即为车轮打滑时速度误差的方差；
[0035] 公式5-7中，考虑到陀螺仪的零偏不稳定性因素，q3~q5即为角度误差方差；
[0036] 5x(k+l) =5vN(k)*T (公式 8)
[0037] 5y(k+l) =5vE(k)*T (公式 9)
[0038] 5z(k+l) =5vD(k)*T (公式 10)
[0039] 公式8-10中，T为两次观测之间的间隔时间；
[0040] KA(k+l) = 5KA(k)+q6 (公式11)
[0041] 公式11中，考虑误差系数存在漂移的情况，q6即为里程计刻度误差系数的方差；
[0042] 再者，构建vn、ve、vd的速度误差与x、y、z方向上的坐标误差的观测方程如下：
[0043] Ζι(1〇 = δνΝ(1〇+ξ〇 (公式 12)
[0044] Z2(k) = δνΕ(1〇+ξι (公式 13)
[0045] Z3(k) = 5vD(k)+|2 (公式 14)
[0046] Z4(k) = δχ(1〇+ξ3 (公式 15)
[0047] Z5(k) = 5y(k)+|4 (公式 16)
[0048] Z6(k) = δζ(1〇+ξ5 (公式 17)
[0049] 公式12-17中，考虑到GPS观测值存在一定误差，其中ξ〇~ξ5即为观测方差。
[0050] 进一步的，上述公式4的具体推算过程如下：
[0051]根据运载系统的比力方程：
[0052]
[0053]将比力方程向导航坐标系中投影得到：
[0054]
[0056] 比较公式19与公式20,并省略二阶以上小量，得到：
[0057]
[0058] 将比力中的向心加速度项考虑作为误差，可认为·Γ=#，δΚΑ为里程计误差系数并 忽略地球自转角速度，因此得到：
[0059]
[0060] 进一步的，步骤4具体包括以下步骤：
[0061] 根据已经推算得到的V:、进一步计算义^^^计算方法如下：
[0062]
[0063]公式22中，xm、ym、zm分别代表在导航坐标系中的xyz坐标，即北东地方向的坐标； <#、<?、分别为里程计测量值计算速度测量值在导航系北、东和地三个方向上的速 度投影。
[0064] 进一步的，步骤5具体包括以下步骤：
[0065]用全球定位系统所测得的3个速度观测数据VN、VE、VD和3个位置观测数据x、y、z与 捷联惯导系统中的推算值乂:做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计 算增益矩阵得到误差的估计值：
[0067] 其中，δνΝ、δνΕ、δν?分别为移动平台在北、东和地三个方向的速度误差， ，0'與分别为移动平台在X、y和ζ三个方向的姿态误差，δ X、δ y、δ ζ分别为移动平台 在x、y和ζ三个方向的坐标误差，以及δΚΑ为里程计的刻度误差。
[0068] 进一步的，步骤6具体包括以下步骤：
[0069]将误差估计值作为反馈，用于修正捷联惯导系统的递推值和全球定位系统的 观测数据，并将捷联惯导系统的推算值X:和误差估计值结合得到导航系统所需的移动 平台的位置和姿态，即：
[0076] 公式23-28中，€、<、€分别代表在导航坐标系中的角度预测值、y、z表示绕 x、y、z轴旋转的角度，即分别绕北东地旋转的角度;xm、ym、zm分别代表在导航坐标系中的xyz 坐标，即北东地方向的坐标。
[0077] 进一步的，步骤7中所述导航系统设定的停止条件为误差值等于0。
[0078] 本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效 果：
[0079] 1.在本发明中，以惯导器件陀螺仪、里程计为状态递推依据，全球定位系统GPS的 观测数据减去捷联惯导系统的递推值作为观测值，共同组成间接卡尔曼滤波的状态和输出 方程。计算公式得到简化且更为直观，卡尔曼滤波计算更快速，计算结果精度也得到有效提 尚；
[0080] 2.同传统的采用直接卡尔曼滤波算法的方法相比，本发明不但能够给出更为准确 的估计结果，且拓宽了系统应用范围；
[0081] 3.本发明位姿估计方法定位准确且易于实现，在移动机器人的仿真过程中，移动 机器人的位姿估计更加精确，有效提高机器人的工作效率。
【附图说明】
[0082] 为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用 的附图作简单的介绍。显而易见，下面描述中的附图仅仅是本发明的一些实施例，对于本领 域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。附 图中：
[0083] 图1为本发明一种基于间接卡尔曼滤波的移动平台位姿估计方法中的系统总体框 架示意图。
【具体实施方式】
[0084] 以下将结合本发明的附图，对本发明实施例中的技术方案进行清楚、完整的描述 和讨论，显然，这里所描述的仅仅是本发明的一部分实例，并不是全部的实例，基于本发明 中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施 例，都属于本发明的保护范围。
[0085] 如图1所示为系统总体框架示意图，所述捷联惯导系统的推算值为X:，所述全球定 位系统的观测数据为Xn，二者的差值作为所述间接卡尔曼滤波器的观测输入，间接卡尔曼 滤波器内部构造预测方程，滤波器输出为误差的估计值此输出中的部分参数反馈用 于校正所述捷联惯导系统以及全球定位系统观测计算中的参数，以修正二者的误差计算， &与八戈1中坐标与姿态等参数的和作为最终系统各项数值的估计值。本实施例中，观测器件 使用的是全球定位系统，但采用激光、视觉等观测设备也能达到同样的技术效果，也是本发 明所保护的技术范围。
[0086] 图中，间接卡尔曼滤波器不直接估计导航系统的状态量(如坐标、姿态等），而是估 计捷联惯导系统与真值之间的误差，因此以全球定位系统的观测数据与捷联惯导系统的推 算值的误差作为观测值求解卡尔曼滤波结果。
[0087]本实施例具体的步骤如下：
[0088] 本发明公开了一种基于间接卡尔曼滤波的移动平台位姿估计方法，该方法包括以 下步骤：
[0089] 步骤1:构建基于间接卡尔曼滤波的移动平台，该移动平台包括捷联惯导系统、全 球定位系统和间接卡尔曼滤波器；
[0090] 步骤2:根据捷联惯导系统中的陀螺仪的角速度测量值[ω χ ω y ω z]，通过递推计算 旋转矩阵并根据捷联惯导系统中的里程计的测量值计算速度测量值在导航系统中的投 Μ/ η ； η . τη ^ VmEs YmD ?
[0091] 步骤3:采用间接卡尔曼滤波器构建系统的状态预测方程和观测方程，并利用状态 预测方程计算状态的预测值；
[0092] 步骤4:根据捷联惯导系统中的陀螺仪和里程计中计算得到的数据推算Xm、ym、zm， 得到捷联惯导系统的推算值心；
[0093] 步骤5:将全球定位系统中的观测数据XN和步骤4中的捷联惯导系统中的推算值X: 做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增益矩阵得到误差的估计 值
[0094] 步骤6:将误差估计值Δ±Ι作为反馈，用于修正捷联惯导系统的递推值和全球定位 系统的观测数据，并将捷联惯导系统的推算值X:和误差估计值Δ?：##合得到导航系统所需 的移动平台的位置和姿态；
[0095]步骤7:返回步骤1，循环计算，直到达到导航系统设定的停止条件为止。
[0096] 具体实施例中，步骤2具体包括以下步骤：
[0097] 根据捷联惯导系统中的陀螺仪的角速度测量值[ωχωγωζ]，通过四元数递推计算 得出旋转矩阵G，递推计算公式如下：
[0098] 二肩：左-i):c?a -1)，初始旋转矩阵αο) ; /
[0101] 进一步通过旋转矩阵q：计算出惯导递推的<、if、计算公式如下：
[0102]
[0103] 公式2中，<、<、<分别代表在导航坐标系中的角度预测值、y、z表示绕x、y、z 轴旋转的角度，即分别绕北东地旋转的角度;Rn代表旋转矩阵中的第1行第1列的数据， r12代表旋转矩阵q*中的第1行第2列的数据，r13代表旋转矩阵q：中的第1行第3列的数据，R 23 代表旋转矩阵q：中的第2行第3列的数据，R33代表旋转矩阵中的第3行第3列的数据；
[0104] 并根据捷联惯导系统中的里程计测量值计算速度测量值在导航系中的投影 vLv、<*、计算公式如下：
[0105]
[0106] 公式3中，<,v、<E、分别为里程计测量值计算速度测量值在导航系北、东和地 三个方向上的速度投影。
[0107] 具体实施例中，步骤3具体包括以下步骤：
[0108] 首先，定义系统状态变量：
[0109] δνΝ、δνΕ、δν?)、?>ν?·、0妙、0碑、3叉、5丫、52和31(八
[0110] 其中，δνΝ、δνΕ、δν?分别为移动平台在北、东和地三个方向的速度误差， d'<m、#势s 分别为移动平台在x、y和ζ三个方向的姿态误差，δχ、δγ、δζ分别为移动平台 在x、y和ζ三个方向的坐标误差，以及δΚΑ为里程计的刻度误差；
[0111] 其次，构建依据上述系统状态变量的状态预测方程：
[0112]
[0113]公式4中，VN、VE、VD分别为移动平台在北、东和地三个方向上的速度，且考虑到车轮 运动存在打滑的情况，qo~q3即为车轮打滑时速度误差的方差；
[0117]公式5-7中，考虑到陀螺仪的零偏不稳定性因素，q3~q5即为角度误差方差；
[0118] 5x(k+l) =5vN(k)*T (公式 8)
[0119] 5y(k+l) =5vE(k)*T (公式 9)
[0120] 5z(k+l) =5vD(k)*T (公式 10)
[0121 ] 公式8-10中，T为两次观测之间的间隔时间；
[0122] KA(k+l) = 5KA(k)+q6 (公式 11)
[0123] 公式11中，考虑误差系数存在漂移的情况，q6即为里程计刻度误差系数的方差；
[0124] 再者，构建VN、VE、VD的速度误差与x、y、z方向上的坐标误差的观测方程如下：
[0125] zi(k) = δνΝ(1〇+ξο (公式 12)
[0126] Z2(k) = δνΕ(1〇+ξι (公式 13)
[0127] Z3(k) = 5vD(k)+|2 (公式 14)
[0128] Z4(k) = δχ(1〇+ξ3 (公式 15)
[0129] Z5(k) = 5y(k)+|4 (公式 16)
[0130] Z6(k) = δζ(1〇+ξ5 (公式 17)
[0131 ]公式12-17中，考虑到GPS观测值存在一定误差，其中ξο~ξ5即为观测方差。
[0132] 具体实施例中，上述公式4的具体推算过程如下：
[0133] 根据运载系统的比力方程：
[0134]
[0135] 将比力方程向导航坐标系中投影得到：
[0136]
[0137]
[0138] 比较公式19与公式20,并省略二阶以上小量，得到：
[0139]
[0140] 将比力中的向心加速度项考虑作为误差，可认为.Γ=ι/?，δΚΑ为里程计误差系数并 忽略地球自转角速度，因此得到：
[0141]
[0142] 具体实施例中，步骤4具体包括以下步骤：
[0143] 根据已经推算得到的ν;;,Λ.、V=.E、V;;,D进一步计算乂 111、7111、2111，计算方法如下：
[0144]
[0145] 公式22中，Xm、ym、Zm分别代表在导航坐标系中的xyz坐标，即北东地方向的坐标； Vlv、<,E、V:,D分别为里程计测量值计算速度测量值在导航系北、东和地三个方向上的速 度投影。
[0146] 具体实施例中，步骤5具体包括以下步骤：
[0147] 用全球定位系统所测得的3个速度观测数据VN、VE、VD和3个位置观测数据x、y、z与 捷联惯导系统中的推算值乂:做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计 算增益矩阵得到误差的估计值：
[0148] 5vn、5ve、5vd、<5^?'、.谷约'、<^碑、5叉、55^、52和51(八
[0149] 其中，δνΝ、δνΕ、δν?分别为移动平台在北、东和地三个方向的速度误差， _、勒y、匆Ζ分别为移动平台在 χ、3^Ρζ三个方向的姿态误差，δχ、δγ、δζ分别为移动平台 在x、y和ζ三个方向的坐标误差，以及δΚ Α为里程计的刻度误差。
[0150] 具体实施例中，步骤6具体包括以下步骤：
[0151] 将误差估计值Δ&作为反馈，用于修正捷联惯导系统的递推值和全球定位系统的 观测数据，并将捷联惯导系统的推算值X:和误差估计值结合得到导航系统所需的移动 平台的位置和姿态，即：
[0152]
[0158] 公式23-28中，<、<、<分别代表在导航坐标系中的角度预测值、y、z表示绕 x、y、Z轴旋转的角度，即分别绕北东地旋转的角度;Xm、ym、Zm分别代表在导航坐标系中的xyz 坐标，即北东地方向的坐标。
[0159] 进一步的，步骤7中所述导航系统设定的停止条件为误差值等于0。
[0160] 本发明中，以惯导器件陀螺仪、里程计为状态递推依据，全球定位系统GPS的观测 数据减去捷联惯导系统的递推值作为观测值，共同组成间接卡尔曼滤波的状态和输出方 程。计算公式得到简化且更为直观，卡尔曼滤波计算更快速，计算结果精度也得到有效提 高。同传统的采用直接卡尔曼滤波算法的方法相比，本发明不但能够给出更为准确的估计 结果，且拓宽了系统应用范围。使得移动机器人的定位更为准确且易于实现，在移动机器人 的仿真过程中，移动机器人的位姿估计更加精确，能有效提高机器人的工作效率。
[0161]以上所述，仅为本发明较佳的【具体实施方式】，但本发明的保护范围并不局限于此， 任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换， 都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围 为准。
【主权项】
1. 一种基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，该方法包括以下 步骤： 步骤1:构建基于间接卡尔曼滤波的移动平台，该移动平台包括捷联惯导系统、全球定 位系统和间接卡尔曼滤波器； 步骤2:根据捷联惯导系统中的陀螺仪的角速度测量值[ωχ coy ωζ]，通过递推计算旋 转矩阵C，并根据捷联惯导系统中的里程计的测量值计算速度测量值在导航系统中的投影 η η η 步骤3:采用间接卡尔曼滤波器构建系统的状态预测方程和观测方程，并利用状态预测 方程计算状态的预测值； 步骤4:根据捷联惯导系统中的陀螺仪和里程计中计算得到的数据推算Xm、ym、zm，得到 捷联惯导系统的推算值XI; 步骤5:将全球定位系统中的观测数据ΧΝ和步骤4中的捷联惯导系统中的推算值X:做差 运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增益矩阵得到误差的估计值 步骤6:将误差估计值々^^作为反馈，用于修正捷联惯导系统的递推值和全球定位系统 的观测数据，并将捷联惯导系统的推算值X:和误差估计值结合得到导航系统所需的移 动平台的位置和姿态； 步骤7:返回步骤1，循环计算，直到达到导航系统设定的停止条件为止。2. 如权利要求1所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，步 骤2具体包括以下步骤： 根据捷联惯导系统中的陀螺仪的角速度测量值[ωχ ωζ]，通过四元数递推计算得 出旋转矩阵q，递推计算公式如下： Μ =我k - \ -1)，初始旋转矩阵= j进一步通过旋转矩阵G计算出惯导递推的计算公式如下：(公式2) 公式2中，少Γ分别代表在导航坐标系中的角度预测值、y、z表示绕x、y、z轴旋 转的角度，即分别绕北东地旋转的角度;Rn代表旋转矩阵Q中的第1行第1列的数据，R12代 表旋转矩阵q中的第1行第2列的数据，R 13代表旋转矩阵中的第1行第3列的数据，R23代表 旋转矩阵G中的第2行第3列的数据，R 33代表旋转矩阵中的第3行第3列的数据； 并根据捷联惯导系统中的里程计测量值计算速度测量值在导航系中的投影 V%v、VmE、计算公式如下： 9(公式3) 公式3中，vLv、<,E、分别为里程计测量值计算速度测量值在导航系北、东和地三个 方向上的速度投影。3.如权利要求1所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，步 骤3具体包括以下步骤： 首先，定义系统状态变量： 其中，δνΝ、δνΕ、δν[)分别为移动平台在北、东和地三个方向的速度误差，?^Λ-、8ψ? 分别为移动平台在x、y和ζ三个方向的姿态误差，δχ、δγ、δζ分别为移动平台在x、y和ζ三个方 向的坐标误差，以及为里程计的刻度误差； 其次，构建依据上述系统状态变量的状态预测方程：(公式4) 公式4中，VN、VE、VD分别为移动平台在北、东和地三个方向上的速度，且考虑到车轮运动 存在打滑的情况，qo~q3即为车轮打滑时速度误差的方差； Ui/? 1- I) - σψ-'wt-Lh(7) 公式5-7中，考虑到陀螺仪的零偏不稳定性因素，q3~q5即为角度误差方差； 5x(k+l) = 5vN(k)*T (公式 8) 5y(k+l) = 5vE(k)*T (公式 9) 5z(k+l) = 5vD(k)*T (公式 10) 公式8-10中，T为两次观测之间的间隔时间； KA(k+l) =5KA(k)+q6 (公式 11) 公式11中，考虑误差系数存在漂移的情况，q6即为里程计刻度误差系数的方差； 再者，构建VN、VE、VD的速度误差与x、y、z方向上的坐标误差的观测方程如下： zi(k) =δνΝ(1〇+ξ〇 (公式 12) Z2(k) =δνΕ(1〇+ξι (公式 13) Z3(k) =5vD(k)+|2 (公式 14) Z4(k) =δχ(1〇+ξ3 (公式 15) Z5(k) =5y(k)H4 (公式 16) Z6(k) =δζ(1〇+ξ5 (公式 17) 公式12-17中，考虑到GPS观测值存在一定误差，其中ξ〇~|5即为观测方差。4. 如权利要求3所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，上 述公式4的具体推算过程如下： 根据运载系统的比力方程：(公式18) 将比力方程向导航坐标系中投影得到：比较公式19与公式20,并省略二阶以上小量，得到：将比力中的向心加速度项考虑作为误差，可认为尸=1>b，ska为里程计误差系数并忽略 地球自转角速度，因此得到：5. 如权利要求1所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，步 骤4具体包括以下步骤： 根据已经推算得到的_V=E、V_!1d进一步计算1111、71 11、2111，计算方法如下：公式22中，xm、ym、zm分别代表在导航坐标系中的xy Z坐标，即北东地方向的坐标； C、<E、V:分别为里程计测量值计算速度测量值在导航系北、东和地三个方向上的速 度投影。6. 如权利要求1所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，步 骤5具体包括以下步骤： 用全球定位系统所测得的3个速度观测数据VN、VE、VD和3个位置观测数据x、y、z与捷联 惯导系统中的推算值乂:做差运算，其差值作为间接卡尔曼滤波器输入的观测值，并计算增 益矩阵得到误差的估计值： 其中，δνΝ、δνΕ、δνΒ分别为移动平台在北、东和地三个方向的速度误差，％:、 分别为移动平台在x、y和z三个方向的姿态误差，δχ、δγ、δζ分别为移动平台在x、y和z三个方 向的坐标误差，以及为里程计的刻度误差。7. 如权利要求1所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，步 骤6具体包括以下步骤： 将误差估计值Δ^Ι作为反馈，用于修正捷联惯导系统的递推值和全球定位系统的观测 数据，并将捷联惯导系统的推算值心和误差估计值结合得到导航系统所需的移动平台 的位置和姿态，即：公式23-28中，识Γ分别代表在导航坐标系中的角度预测值、y、z表示绕x、y、z 轴旋转的角度，即分别绕北东地旋转的角度;Xm、ym、Zm分别代表在导航坐标系中的xyz坐标， 即北东地方向的坐标。8. 如权利要求1所述的基于间接卡尔曼滤波的移动平台位姿估计方法，其特征在于，步 骤7中所述导航系统设定的停止条件为误差值等于0。
【文档编号】G01C21/20GK105865452SQ201610284802
【公开日】2016年8月17日
【申请日】2016年4月29日
【发明人】庞文尧, 陶熠昆, 黄鸿, 章海兵, 王培建
【申请人】浙江国自机器人技术有限公司