一种基于罗德里格参数和二阶非线性量测的滤波对准算法
一种基于罗德里格参数和二阶非线性量测的滤波对准算法
【技术领域】
[0001] 本发明涉及惯性导航技术领域，具体是一种基于罗德里格参数和二阶非线性量测 的滤波对准算法。
【背景技术】
[0002] 动基座对准能够有效提高捷联惯导系统载体平台的机动性能，具有很高的军事应 用价值。动基座对准的研宄内容主要包括两个方面，一是建立大失准角条件下的非线性误 差模型 [1~3];二是设计相应的非线性滤波估计算法[2~9]。
[0003] 依据姿态描述方式的不同，可以得到不同的非线性误差模型，如基于四元数非线 性误差模型 [1]、基于欧拉角非线性误差模型[2]、基于修正Rodrigues参数非线性误差模型
[3] ，以及大方位失准角条件下，基于方位角正余弦函数的非线性误差模型[4]等。其中，四 元数非线性误差模型无奇异点，使用最为广泛，但在设计滤波算法时需要考虑其模值约束 的影响。欧拉角姿态描述法存在奇异点，因此不适用于任意姿态对准，且基于欧拉角的非 线性误差模型中含有状态量的正余弦函数，使得误差模型非线性增大。传统的基于修正 Rodrigues参数的误差模型虽可避免奇异点，但是模型非线性度同样很大。此外，传统方法 建立动基座对准非线性误差模型时，均以动态载体系和动态导航系之间的实时姿态为估计 对象，系统方程及量测方程均建立在动态的导航坐标系下。以速度为观测量时，以上误差模 型只有系统方程为非线性，量测方程则是线性的。
[0004] 在非线性滤波算法的选择上，常规EKF滤波算法需要求导计算Jacobian矩阵，且 在处理严重非线性问题时，可能出现滤波误差增大甚至发散的现象。因此，一类基于sigma 点的非线性滤波算法成为研宄的热点，如UKF滤波[2'3'4]、改进强跟踪UKF滤波[5]、粒子滤波 [6]、Gauss-Hermite滤波[7'8]，以及容积卡尔曼滤波（CKF)[9]等。基于sigma点的非线性滤 波算法，用确定性或随机采样策略逼近非线性函数的概率分布，无需对非线性模型求导，且 可以通过优化采样策略，减小计算量并提高非线性函数概率分布的近似精度。总体来讲，基 于sigma点的非线性滤波算法可以做到计算量与EKF滤波相当，但是精度优于EKF滤波。
【发明内容】
[0005] 本发明的目的在于提供一种基于罗德里格参数和二阶非线性量测的滤波对准算 法，以解决上述【背景技术】中提出的问题。
[0006] 为实现上述目的，本发明提供如下技术方案：
[0007] 一种基于罗德里格参数和二阶非线性量测的滤波对准算法，由惯性系动基座对准 过程、二阶非线性量测滤波估计算法、经典Rodrigues参数奇异点及其处理方法，
[0008] 具体描述如下：
[0009] 步骤1，惯性系动基座对准过程
[0010] 惯性系动基座对准算法以实时姿态阵的链式分解为基础，
[0012] 其中，nt系为实时导航坐标系，即载体时变位置东北天地理坐标系；in为导航惯性 系，与动基座对准开始时刻的n系重合；b为载体坐标系；ib为载体惯性系，与对准开始时刻 的b系重合；式（1)中，是运动的nt系相对于导航惯性系i^勺姿态阵，由GPS输出位置 信息解析计算；能利用陀螺输出进行姿态跟踪，所以，惯性系动基座对准过程是对常值姿 态阵ct的估计；
[0013] 利用牛顿第二定律和哥氏定理，得到惯性系比力方程如下
[0015] 其中，P⑴为t时刻载体对地速度在导航惯性系in内的投影；#(0为t时刻载体 所在位置重力加速度在导航惯性系in内投影；_T'(〇为t时刻理想比力值；
[0016] 对式（2)两端分别进行积分，并记
[0019] 利用GPS输出完成式⑶中"仏)的求解，
[0024] 进一步，在tk_^tk更新周期内，假设，为常矢量，导航惯性系in内对地速度 为线性函数，即
[0027] 其中，tG[tH，tk];T=VtH为GPS量测更新周期
[0028] 将式（9)、（10)代入式（7)、（8)中，整理得
[0032] 利用捷联惯导姿态、速度二子样更新算法实现对式（4)中)^化)的求解；进一步， 考虑陀螺仪随机常值漂移eb和加速度计随机常值零偏V6的影响，推导得
[0036] 其中，#为姿态误差角；汾。仏)为加速度计惯性系比力积分误差；
[0037]由式（2)、（3)、⑷和式（13)可得，求解常值姿态阵q的观测方程为
[0039] 进一步，用经典Rodrigues参数法来等价描述姿态阵，记对应Rodrigues参数 为1，则二者满足凯莱变换关系式，即
[0041] 将式（17)代入式（16)，整理得，
[0043] 其中，#^ 匕）；wv包含惯性器件测量噪声的积分和随机扰动的积分，且有
[0045] 式（18)即是与姿态阵q等价的Rodrigues参数1的观测方程，若能估计出 Rodrigues参数1，则依据式（17)得到<^ . ?
[0046] 综上，惯性系动基座对准选取如下15维状态
[0048] 由上述推导，系统方程及量测方程分别为，
[0051] 利用式（21)、式（22)设计滤波算法实现对Rodrigues参数1的估计，进而得到姿 态阵q,通过式（1)即实现动基座对准；
[0052] 步骤2,二阶非线性量测滤波估计算法
[0053] 式（21)描述的系统方程为线性，式（22)描述的量测方程为非线性，但仅是状态量 的二阶非线性函数，能用有限阶Taylor级数展开描述，即
[0055] 其中，Xk(l为Taylor级数展开点；
ei是第i个分量为1，其余元素为 0的3维单位向量；Hk为非线性函数h的雅克比阵；Di为非线性函数h的二阶偏导数阵；Tr为矩阵求迹函数，且有
[0059] 其中，h=IXh2h3]T;
[0060] 同时，对于二阶非线性函数，其二阶偏导数阵为常值矩阵，故由式（21)、（22)、 (25)，知DiS15维常值对称阵，其中非零元素仅有
[0061] Di(2,6) = 1，D2(l，6) = -1，D3(l，5) = 1，
[0062] Di(3,5) = -1，D2(3,4) = 1，D3(2,4) = -1
[0063] Di(6,2) = 1，D2(6,1) =-1，D3(5,1) = 1，
[0064] Di(5,3) =-1，D2(4,3) = 1，D3(4,2) =-1 (26)
[0065] 步骤2. 1，滤波时间更新算法
[0066] 式（21)系统方程为线性，采用标准卡尔曼滤波算法完成时间更新，得到状态量和 估计误差方差阵的一步预测，即和用状态一步预测结果;代替式（23)中Xk(l， 建立起当前观测量与状态一步预测关系式，进而设计量测更新算法对一步预测结果 进行校正，得到当前时刻的状态最优估计值下面推导基于式（23)二阶泰勒级数量测方 ? 程的滤波量测更新算法；
[0067] 步骤2. 2,滤波量测更新算法
[0068] 量测更新形式定义为与线性卡尔曼滤波量测更新一致，假设k时刻状态估计结果 为
[0070] 其中，Lk为引入的补偿项，和最佳增益Kk一样均为待定值；Lk和Kk的确定原则分 别为使为无偏估计和使的均方误差阵Pk的迹最小；
[0071] 定义状态估计误差
[0074]由式（23)、式（24)、式（27)、式（28)整理得
[0076] 要使&为无偏估计，即要求&期望为零；假设时间更新为无偏估计，则对式 (29)右端取期望并令结果为零，得补偿项Lk为
[0078] 其中，E[ ?]表示对括号内变量求期望；
[0079] 将式（30)代入式（29)中，整理得
[0081]其中，
[0083]由于E[足]=〇，E[wv]= 0,且^与A不相关，故由式（31)、式（32)得Pk为
[0085] 其中
[0086]Ak=E[AAt] (34)
[0087] 式（32)中，A为3维列向量，从而Ak为3阶方阵；利用式（32)，经过推导得Ak第 i行第j列元素为
[0089] 式（33)中协方差阵更新方式与标准卡尔曼滤波形式一致，从而式（27)中最佳增 益阵Kk，
[0090] 考察量测更新方程式（27)、（33)、（36)，基于二阶泰勒级数的量测更新算法与标准 卡尔曼滤波在形式上完全一致，仅增加了对Lk、Ak的计算；而由式（30)、（35)知，Lk、Ak的 求解简单；考虑到DiS常值稀疏矩阵，将Lk、Ak描述为仅与Pk/H相关的形式，如此进一步 减小在线计算量；
[0091] 步骤3,经典Rodrigues参数奇异点及其处理方法
[0092] 经典Rodrigues参数是最少参数姿态描述方法之一，存在奇异点，一种等价表示 方法为
[0094] 其中，u为两坐标系之 间等效旋转矢量的方向向量；巾为等效旋转矢量转过的角 度；
[0095] 当巾取值为±Jr时，1值为无穷大，是经典Rodrigues参数法进行姿态描述时的 奇异点，对ib系进行某种虚拟转动得到ibl，使得ibl系相对于in系的Rodrigues参数远离 奇异点，然后采用前述滤波对准算法完成C,；：1对应Rodrigues参数的估计，进而借助已知的 虚拟转动，计算 〇
[0096] 作为本发明进一步的方案：所述步骤3中具体包括如下步骤：
[0097]步骤3. 1，奇异点判别函数设计
[0098]当巾取值为± 31时，ib系至in系姿态矩阵描述为
[0100] 此时，任意矢量R在两坐标系内投影满足
[0102] 在式（39)两端分别加上，并利用单位向量恒等关系式I+(uX)2=uuT，则有
[0104] 也即是，此时任意矢量在两坐标系内的投影之和均共线，且与等效旋转矢量平 行；
[0105] 因此，对当前滤波变量1是否接近奇异点的判断等价于判断式（19)中不同时刻的 气是否接近共线；为了缩短得出有效判断所需时间，设定如下判别函数
[0107] 其中，&为气的方向向量；| ? |表示取方阵行列式；
[0108] 若用于计算gk的所有&均共线，即当前对变量1的估计位于奇异点时，gk理论值 为零；
[0109] 步骤3. 2,虚拟转动设计
[0110] 虚拟转动的设计依据：若ib系至in系等效旋转矢量转角大于n/2,将ib系绕着其 某个坐标轴转动JT角得到ibl系，使ibl系至in系等效旋转矢量转角小于JT/2;
[0111] 据此，记ib系绕其x轴转动Jr角得到ibx系；ib系绕其y轴转动jt角得到iby系； ib系绕其z轴转动it角得到ibz系，则有
[0113] 其中
为已知常值对角阵，对角元为1或-1 ;
[0114] 无论载体动基座对准开始时刻真实姿态为何值，姿态阵
所对 应的四个等效旋转矢量中，至少有一个等效旋转矢量的转角小于n/2,远离奇异点；
[0115] 步骤3. 3,奇异点规避的滤波算法
[0116] 依据上面设计的判别函数和虚拟转动，得到奇异点规避的滤波算法：
[0117] (1)给定奇异点判别时间tk，在tk时刻以前，利用步骤1、步骤2推导的对准滤波 算法，同时处理四个独立的滤波过程，分别龙
进行估计；其中，对Ct> 进行估计时，式（21)、式（22)中相关参变量需要进行相应的坐标变换；所有滤波 器状态量初值均设为零，考虑式（27)及虚拟转动分析结论，将1对应的方差阵初值设定为P〇0= 1，P11= 1，P22= 1 ;
[0118] (2)在tk时刻，分别计算姿态阵
所对应的判别函数值，gk、gkx、 gky、gkz，仅选择最大判别函数值所对应的滤波过程完成后续滤波对准。
[0119] 与现有技术相比，本发明的有益效果是：本发明对于捷联惯导系统动基座对准问 题，给出了一种基于罗德里格参数和二阶非线性量测的滤波对准算法，并对罗德里格参数 奇异点问题进行了详细讨论，设计了处理方案。仿真分析结果表明，该算法在车辆启动段典 型机动条件下，能够快速收敛，对准精度高，以与线性卡尔曼滤波相当的计算量及计算复杂 度实现了任意失准角非线性动基座对准。
【附图说明】
[0120] 图1是姿态1时判别函数随时间变化曲线；
[0121] 图2是姿态1时姿态误差角均方根曲线；
[0122] 图3是姿态1时姿态误差角均方根曲线；
[0123] 图4是姿态2时判别函数随时间变化曲线；
[0124] 图5是姿态2时姿态误差角均方根曲线；
[0125] 图6是姿态2时姿态误差角均方根曲线。
【具体实施方式】
[0126] 下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述， 显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的 实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都 属于本发明保护的范围。
[0127] 本发明提供了一种基于罗德里格参数和二阶非线性量测的滤波对准算法，利用姿 态阵分解，将动态姿态的估计问题转化为一个常值姿态的估计，借助经典Rodrigues参数 和姿态阵之间的凯莱变换，建立了在惯性坐标系描述的系统方程线性，量测方程具有二阶 非线性的弱非线性误差模型。对于这种非线性系统的估计，采用标准线性卡尔曼滤波算法 完成时间更新。对于量测更新，借鉴高阶EKF量测更新的思路，推导了一种基于二阶非线性 量测完整泰勒级数展开的量测更新算法。新的量测更新算法在实现形式上与线性卡尔曼滤 波量测更新完全一致，且计算量相当。综合时间更新和量测更新，可以认为新算法用线性滤 波方式解决了大失准角非线性滤波对准问题，在计算量上远小于导航系内基于sigma点的 各类非线性滤波对准算法。同时对经典Rodrigues参数描述姿态存在奇异点的问题，也进 行了详细分析并给出了解决办法。根据上述方案进行仿真分析，具体描述如下。
[0128] 实施例1
[0129] 本发明实施例中，以车载SINS/GPS动基座对准为例，仿真轨迹为：由静止加速至 l〇m/S，加速度为0. 5m/s2;然后匀速行驶50s;之后转弯90°，角速率为15° /s;之后匀速 直线行驶。对准总时间取为120s。
[0130] 惯性器件精度：陀螺随机常值漂移为〇.or/h，角随机游走系数为
;加 速度计随机常值偏置为5Xl(T5g，加速度计白噪声方差强度为
，不考虑其他误差 项。
[0131] GPS精度：速度测量噪声为0.lm/s，水平位置测量噪声为3m，高度位置测量噪声为 5m，GPS数据周期为Is。
[0132] 考虑车载SINS测量坐标系与右前上车体坐标系近似安装，故水平姿态角通常为 小角，奇异点只出现在方位角接近180°时。此外，由小节3的分析易知，采用奇异点规避策 略后，最终对准算法所估计的姿态对应的等效旋转矢量转角一定不大于Ji/2。综上，仿真 中，验证两种姿态初值条件下的对准效果：姿态1，俯仰角、滚转角、方位角分别为5°、3°、 80° ;姿态2,俯仰角、滚转角、方位角分别为5°、3°、180°，对上述两种姿态初值条件下的 初始对准算法各进行100次的蒙特卡洛仿真，仿真结果如图1~图6所示。
[0133]图1、图4描述的是两种初始姿态条件下，奇异点规避策略所使用判别函数的值随 时间变化曲线，该函数值为无量纲量。从图1、4中可以得出两点结论：第一，不同等效旋转 矢量对应的判别函数在较短时间内即具有很高的区分度，这样奇异点判别时间tk即可设计 的很小，以仿真为例，tk最短可以选择为2s;第二，判别函数的值与对准过程中载体机动相 关。仿真程序中tk设计为5s。
[0134] 姿态1时对准姿态误差收敛曲线如图2、图3所示，其中，图2为整个对准区间收敛 曲线；图3为70s至120s的收敛曲线。姿态2时对准姿态误差收敛曲线如图5、图6所示， 图5对应整个对准区间收敛曲线，图6为70s到120s收敛曲线。
[0135] 与gk、gkx、gky、gkz相关的等效旋转矢量转角，在姿态1初值下，分别为80. 05°、 174.24°、179. 08 °、100. 26 ° ;在姿态 2 初值条件下，分别为 179. 87 °、177 °、175 °、 5. 83°。由图1、图4易知，姿态1初值条件下，等效旋转矢量转角为80. 05°的滤波过程在 判别完成后被保留；姿态2初值条件下，等效旋转矢量转角为5. 83°的滤波过程在判别完 成后被保留。
[0136] 两种姿态初值条件下，滤波对准结果如图2、图5所示。在车辆加速机动条件下，对 准算法能够快速收敛，在150s对准结束时刻，姿态1初值条件下，两水平姿态误差角和方位 姿态误差角均方根分别为9.6"、10"、3. 87';姿态2初值条件下，两水平姿态误差角和方 位姿态误差角均方根分别为9.9"、9. 4"、3. 18'。
[0137] 对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在 不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论 从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权 利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有 变化囊括在本发明内。
[0138] 此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包 含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当 将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员 可以理解的其他实施方式。
【主权项】
1. 一种基于罗德里格参数和二阶非线性量测的滤波对准算法，包括惯性系动基座对准 过程、二阶非线性量测滤波 估计算法、经典Rodrigues参数奇异点及其处理方法，其特征在 于，具体描述如下： 步骤1，惯性系动基座对准过程 惯性系动基座对准算法以实时姿态阵的链式分解为基础， Ctb - CtbCi^ ^ HS (1) 其中，nt系为实时导航坐标系，即载体时变位置东北天地理坐标系；1"为导航惯性系， 与动基座对准开始时刻的η系重合；b为载体坐标系；ib为载体惯性系，与对准开始时刻的 b系重合；式（1)中，是运动的nt系相对于导航惯性系i "的姿态阵，由GPS输出位置信息 解析计算；能利用陀螺输出进行姿态跟踪，所以，惯性系动基座对准过程是对常值姿态阵 Ct的估计； 利用牛顿第二定律和哥氏定理，得到惯性系比力方程如下 V1" (〇 + ^ X V1'' (〇 - g1" (〇 = p (〇 ⑵ 其中，为t时刻载体对地速度在导航惯性系in内的投影；为t时刻载体所在 位置重力加速度在导航惯性系1内投影；/"(0为t时刻理想比力值； 对式（2)两端分别进行积分，并记利用GPS输出完成式（3)中F/"(X)的求解，进一步，在七^至t k更新周期内，假设为常矢量，导航惯性系in内对地速度，为线 性函数，即其中，t e [t^，tk] ;T = GPS量测更新周期； 将式（9)、（10)代入式（7)、⑶中，整理得利用捷联惯导姿态、速度二子样更新算法实现对式（4)中Fj(4)的求解；进一步，考虑 陀螺仪随机常值漂移ε b和加速度计随机常值零偏V6的影响，推导得其中，#为<^姿态误差角；汾。仏)为加速度计惯性系比力积分误差； 由式（2)、（3)、（4)和式（13)可得，求解常值姿态阵q的观测方程为 V'm {h)~ (tk ) = Cfn Vy" {tk ) (邱 进一步，用经典Rodrigues参数法来等价描述姿态阵，记对应Rodrigues参数为1， 则二者满足凯莱变换关系式，BP CI=(I + {1^(I-{1^) (17) 将式（17)代入式（16)，整理得，其中，. Wv包含惯性器件测量噪声的积分和随机扰动的积分，且有式（18)即是与姿态阵等价的Rodrigues参数1的观测方程，若能估计出Rodrigues 参数1，则依据式（17)得到 综上，惯性系动基座对准选取如下15维状态由上述推导，系统方程及量测方程分别为，利用式（21)、式（22)设计滤波算法实现对Rodrigues参数1的估计，进而得到姿态阵 ，通过式（1)即实现动基座对准； 步骤2,二阶非线性量测滤波估计算法 式（21)描述的系统方程为线性，式（22)描述的量测方程为非线性，但仅是状态量的二 阶非线性函数，能用有限阶Taylor级数展开描述，即其中，Xktl为Taylor级数展开点；束t -XftO; ei是第i个分量为1，其余元素为0的 3维单位向量；Hk为非线性函数h的雅克比阵；D i为非线性函数h的二阶偏导数阵；Tr为矩 阵求迹函数，且有其中，h = Qi1 h2 h3]T; 同时，对于二阶非线性函数，其二阶偏导数阵为常值矩阵，故由式（21)、（22)、（25)，知 DiS 15维常值对称阵，其中非零元素仅有 Di(2,6) = 1，D2(1，6) = -1，D3(1，5) = 1， Di(3,5) = -1，D2(3,4) = 1，D3(2,4) = -I Di(6,2) = 1，D2(6，1) = -1，D3(5，1) = 1， 0八5,3) = -1，D2(4,3) = 1，D3(4,2) = -I (26) 步骤2.1，滤波时间更新算法 式（21)系统方程为线性，采用标准卡尔曼滤波算法完成时间更新，得到状态量和估计 误差方差阵的一步预测，即用状态一步预测结果尤μ代替式（23)中Xktl，建立 起当前观测量与状态一步预测关系式，进而设计量测更新算法对一步预测结果毛/(^进行校 正，得到当前时刻的状态最优估计值+I下面推导基于式（23)二阶泰勒级数量测方程的滤 波量测更新算法； 步骤2. 2,滤波量测更新算法 量测更新形式定义为与线性卡尔曼滤波量测更新一致，假设k时刻状态估计结果为其中，Lk为引入的补偿项，和最佳增益K k一样均为待定值；L k和K k的确定原则分别为 使^^为无偏估计和使&的均方误差阵Pk的迹最小； 定义状态估计误差其中，文嫩-1 = A . ， 由式（23)、式（24)、式（27)、式（28)整理得要使为无偏估计，即要求期望为零；假设时间更新为无偏估计，则对式（29) 右端取期望并令结果为零，得补偿项Lk为其中，Ε[ ·]表示对括号内变量求期望； 将式（30)代入式（29)中，整理得由于Ε[足]=0，E[wv] = 0,且^与A不相关，故由式（31)、式（32)得Pk为其中 Ak= E [AAT] (34) 式（32)中，A为3维列向量，从而Ak为3阶方阵；利用式（32)，经过推导得Λ ,第1行 第j列元素为式（33)中协方差阵更新方式与标准卡尔曼滤波形式一致，从而式（27)中最佳增益阵考察量测更新方程式（27)、（33)、（36)，基于二阶泰勒级数的量测更新算法与标准卡尔 曼滤波在形式上完全一致，仅增加了对Lk、Ak的计算；而由式（30)、（35)知，L k、Ak的求解 简单；考虑到DiS常值稀疏矩阵，将L k、Λ k描述为仅与P ,/η相关的形式，如此进一步减小 在线计算量； 步骤3,经典Rodrigues参数奇异点及其处理方法 经典Rodrigues参数是最少参数姿态描述方法之一，存在奇异点，一种等价表示方法 为(37) 其中，u为两坐标系之间等效旋转矢量的方向向量；φ为等效旋转矢量转过的角度； 当Φ取值为± π时，1值为无穷大，是经典Rodrigues参数法进行姿态描述时的奇异 点，对ib系进行某种虚拟转动得到i bl，使得ibl系相对于i n系的Rodrigues参数远离奇异 点，然后采用前述滤波对准算法完成Cf对应Rodrigues参数的估计，进而借助已知的虚拟 转动，计算q。2.根据权利要求1所述的基于罗德里格参数和二阶非线性量测的滤波对准算法，其特 征在于，所述步骤3中具体包括如下步骤： 步骤3.1，奇异点判别函数设计 当Φ取值为± π时，ib系至i n系姿态矩阵描述为此时，任意矢量R在两坐标系内投影满足在式（39)两端分别加上并利用单位向量恒等关系式I+(uX)2= uuT，则有(40) 也即是，此时任意矢量在两坐标系内的投影之和均共线，且与等效旋转矢量平行； 因此，对当前滤波变量1是否接近奇异点的判断等价于判断式（19)中不同时刻的\是 否接近共线；为了缩短得出有效判断所需时间，设定如下判别函数(41) 其中，&为\的方向向量；I · I表示取方阵行列式； 若用于计算gk的所有 '均共线，即当前对变量1的估计位于奇异点时，gk理论值为零； 步骤3. 2,虚拟转动设计 虚拟转动的设计依据：若ib系至in系等效旋转矢量转角大于η/2,将ib系绕着其某个 坐标轴转动π角得到ibl系，使ibl系至in系等效旋转矢量转角小于π/2; 据此，记ib系绕其X轴转动π角得到i bx系；i b系绕其y轴转动π角得到i by系；i b系 绕其Z轴转动JT角得到ibz系，则有 其中，、cZ、为已知常值对角阵，对角元为1或_l; 无论载体动基座对准开始时刻真实姿态为何值，姿态阵所对应的 四个等效旋转矢量中，至少有一个等效旋转矢量的转角小于JT/2,远离奇异点； 步骤3. 3,奇异点规避的滤波算法 依据上面设计的判别函数和虚拟转动，得到奇异点规避的滤波算法： (1)给定奇异点判别时间tk，在tk时刻以前，利用步骤1、步骤2推导的对准滤波算 法，同时处理四个独立的滤波过程，分别对q；、?；、q；进行估计；其中，对c,t、 Ct进行估计时，式（21)、式（22)中相关参变量需要进行相应的坐标变换；所有滤波 器状态量初值均设为零，考虑式（27)及虚拟转动分析结论，将1对应的方差阵初值设定为 P〇0= 1，P 11= 1，P 22= 1 ; ⑵在tk时刻，分别计算姿态阵q；所对应的判别函数值，gk、gkx、 gky、 gkz，仅选择最大判别函数值所对应的滤波过程完成后续滤波对准。
【专利摘要】本发明公开了一种基于罗德里格参数和二阶非线性量测的滤波对准算法，包括由惯性系动基座对准过程、二阶非线性量测滤波估计算法、经典罗德里格参数奇异点及其处理方法，利用姿态阵分解，将动态姿态的估计问题转化为一个常值姿态的估计，借助经典Rodrigues参数和姿态阵之间的凯莱变换，建立了在惯性坐标系描述的系统方程线性，量测方程具有二阶非线性的弱非线性误差模型。同时对经典罗德里格参数描述姿态存在奇异点的问题，也进行了详细分析并给出了解决办法。本发明在车辆启动段典型机动条件下，能够快速收敛，对准精度高，以与线性卡尔曼滤波相当的计算量及计算复杂度实现了任意失准角非线性动基座对准。
【IPC分类】G01C25/00, G01C21/16
【公开号】CN104897170
【申请号】CN201510297409
【发明人】严恭敏, 梅春波, 付强文, 秦永元
【申请人】西北工业大学
【公开日】2015年9月9日
【申请日】2015年5月29日
【技术领域】
[0001] 本发明涉及惯性导航技术领域，具体是一种基于罗德里格参数和二阶非线性量测 的滤波对准算法。
【背景技术】
[0002] 动基座对准能够有效提高捷联惯导系统载体平台的机动性能，具有很高的军事应 用价值。动基座对准的研宄内容主要包括两个方面，一是建立大失准角条件下的非线性误 差模型 [1~3];二是设计相应的非线性滤波估计算法[2~9]。
[0003] 依据姿态描述方式的不同，可以得到不同的非线性误差模型，如基于四元数非线 性误差模型 [1]、基于欧拉角非线性误差模型[2]、基于修正Rodrigues参数非线性误差模型
[3] ，以及大方位失准角条件下，基于方位角正余弦函数的非线性误差模型[4]等。其中，四 元数非线性误差模型无奇异点，使用最为广泛，但在设计滤波算法时需要考虑其模值约束 的影响。欧拉角姿态描述法存在奇异点，因此不适用于任意姿态对准，且基于欧拉角的非 线性误差模型中含有状态量的正余弦函数，使得误差模型非线性增大。传统的基于修正 Rodrigues参数的误差模型虽可避免奇异点，但是模型非线性度同样很大。此外，传统方法 建立动基座对准非线性误差模型时，均以动态载体系和动态导航系之间的实时姿态为估计 对象，系统方程及量测方程均建立在动态的导航坐标系下。以速度为观测量时，以上误差模 型只有系统方程为非线性，量测方程则是线性的。
[0004] 在非线性滤波算法的选择上，常规EKF滤波算法需要求导计算Jacobian矩阵，且 在处理严重非线性问题时，可能出现滤波误差增大甚至发散的现象。因此，一类基于sigma 点的非线性滤波算法成为研宄的热点，如UKF滤波[2'3'4]、改进强跟踪UKF滤波[5]、粒子滤波 [6]、Gauss-Hermite滤波[7'8]，以及容积卡尔曼滤波（CKF)[9]等。基于sigma点的非线性滤 波算法，用确定性或随机采样策略逼近非线性函数的概率分布，无需对非线性模型求导，且 可以通过优化采样策略，减小计算量并提高非线性函数概率分布的近似精度。总体来讲，基 于sigma点的非线性滤波算法可以做到计算量与EKF滤波相当，但是精度优于EKF滤波。
【发明内容】
[0005] 本发明的目的在于提供一种基于罗德里格参数和二阶非线性量测的滤波对准算 法，以解决上述【背景技术】中提出的问题。
[0006] 为实现上述目的，本发明提供如下技术方案：
[0007] 一种基于罗德里格参数和二阶非线性量测的滤波对准算法，由惯性系动基座对准 过程、二阶非线性量测滤波估计算法、经典Rodrigues参数奇异点及其处理方法，
[0008] 具体描述如下：
[0009] 步骤1，惯性系动基座对准过程
[0010] 惯性系动基座对准算法以实时姿态阵的链式分解为基础，
[0012] 其中，nt系为实时导航坐标系，即载体时变位置东北天地理坐标系；in为导航惯性 系，与动基座对准开始时刻的n系重合；b为载体坐标系；ib为载体惯性系，与对准开始时刻 的b系重合；式（1)中，是运动的nt系相对于导航惯性系i^勺姿态阵，由GPS输出位置 信息解析计算；能利用陀螺输出进行姿态跟踪，所以，惯性系动基座对准过程是对常值姿 态阵ct的估计；
[0013] 利用牛顿第二定律和哥氏定理，得到惯性系比力方程如下
[0015] 其中，P⑴为t时刻载体对地速度在导航惯性系in内的投影；#(0为t时刻载体 所在位置重力加速度在导航惯性系in内投影；_T'(〇为t时刻理想比力值；
[0016] 对式（2)两端分别进行积分，并记
[0019] 利用GPS输出完成式⑶中"仏)的求解，
[0024] 进一步，在tk_^tk更新周期内，假设，为常矢量，导航惯性系in内对地速度 为线性函数，即
[0027] 其中，tG[tH，tk];T=VtH为GPS量测更新周期
[0028] 将式（9)、（10)代入式（7)、（8)中，整理得
[0032] 利用捷联惯导姿态、速度二子样更新算法实现对式（4)中)^化)的求解；进一步， 考虑陀螺仪随机常值漂移eb和加速度计随机常值零偏V6的影响，推导得
[0036] 其中，#为姿态误差角；汾。仏)为加速度计惯性系比力积分误差；
[0037]由式（2)、（3)、⑷和式（13)可得，求解常值姿态阵q的观测方程为
[0039] 进一步，用经典Rodrigues参数法来等价描述姿态阵，记对应Rodrigues参数 为1，则二者满足凯莱变换关系式，即
[0041] 将式（17)代入式（16)，整理得，
[0043] 其中，#^ 匕）；wv包含惯性器件测量噪声的积分和随机扰动的积分，且有
[0045] 式（18)即是与姿态阵q等价的Rodrigues参数1的观测方程，若能估计出 Rodrigues参数1，则依据式（17)得到<^ . ?
[0046] 综上，惯性系动基座对准选取如下15维状态
[0048] 由上述推导，系统方程及量测方程分别为，
[0051] 利用式（21)、式（22)设计滤波算法实现对Rodrigues参数1的估计，进而得到姿 态阵q,通过式（1)即实现动基座对准；
[0052] 步骤2,二阶非线性量测滤波估计算法
[0053] 式（21)描述的系统方程为线性，式（22)描述的量测方程为非线性，但仅是状态量 的二阶非线性函数，能用有限阶Taylor级数展开描述，即
[0055] 其中，Xk(l为Taylor级数展开点；
ei是第i个分量为1，其余元素为 0的3维单位向量；Hk为非线性函数h的雅克比阵；Di为非线性函数h的二阶偏导数阵；Tr为矩阵求迹函数，且有
[0059] 其中，h=IXh2h3]T;
[0060] 同时，对于二阶非线性函数，其二阶偏导数阵为常值矩阵，故由式（21)、（22)、 (25)，知DiS15维常值对称阵，其中非零元素仅有
[0061] Di(2,6) = 1，D2(l，6) = -1，D3(l，5) = 1，
[0062] Di(3,5) = -1，D2(3,4) = 1，D3(2,4) = -1
[0063] Di(6,2) = 1，D2(6,1) =-1，D3(5,1) = 1，
[0064] Di(5,3) =-1，D2(4,3) = 1，D3(4,2) =-1 (26)
[0065] 步骤2. 1，滤波时间更新算法
[0066] 式（21)系统方程为线性，采用标准卡尔曼滤波算法完成时间更新，得到状态量和 估计误差方差阵的一步预测，即和用状态一步预测结果;代替式（23)中Xk(l， 建立起当前观测量与状态一步预测关系式，进而设计量测更新算法对一步预测结果 进行校正，得到当前时刻的状态最优估计值下面推导基于式（23)二阶泰勒级数量测方 ? 程的滤波量测更新算法；
[0067] 步骤2. 2,滤波量测更新算法
[0068] 量测更新形式定义为与线性卡尔曼滤波量测更新一致，假设k时刻状态估计结果 为
[0070] 其中，Lk为引入的补偿项，和最佳增益Kk一样均为待定值；Lk和Kk的确定原则分 别为使为无偏估计和使的均方误差阵Pk的迹最小；
[0071] 定义状态估计误差
[0074]由式（23)、式（24)、式（27)、式（28)整理得
[0076] 要使&为无偏估计，即要求&期望为零；假设时间更新为无偏估计，则对式 (29)右端取期望并令结果为零，得补偿项Lk为
[0078] 其中，E[ ?]表示对括号内变量求期望；
[0079] 将式（30)代入式（29)中，整理得
[0081]其中，
[0083]由于E[足]=〇，E[wv]= 0,且^与A不相关，故由式（31)、式（32)得Pk为
[0085] 其中
[0086]Ak=E[AAt] (34)
[0087] 式（32)中，A为3维列向量，从而Ak为3阶方阵；利用式（32)，经过推导得Ak第 i行第j列元素为
[0089] 式（33)中协方差阵更新方式与标准卡尔曼滤波形式一致，从而式（27)中最佳增 益阵Kk，
[0090] 考察量测更新方程式（27)、（33)、（36)，基于二阶泰勒级数的量测更新算法与标准 卡尔曼滤波在形式上完全一致，仅增加了对Lk、Ak的计算；而由式（30)、（35)知，Lk、Ak的 求解简单；考虑到DiS常值稀疏矩阵，将Lk、Ak描述为仅与Pk/H相关的形式，如此进一步 减小在线计算量；
[0091] 步骤3,经典Rodrigues参数奇异点及其处理方法
[0092] 经典Rodrigues参数是最少参数姿态描述方法之一，存在奇异点，一种等价表示 方法为
[0094] 其中，u为两坐标系之 间等效旋转矢量的方向向量；巾为等效旋转矢量转过的角 度；
[0095] 当巾取值为±Jr时，1值为无穷大，是经典Rodrigues参数法进行姿态描述时的 奇异点，对ib系进行某种虚拟转动得到ibl，使得ibl系相对于in系的Rodrigues参数远离 奇异点，然后采用前述滤波对准算法完成C,；：1对应Rodrigues参数的估计，进而借助已知的 虚拟转动，计算 〇
[0096] 作为本发明进一步的方案：所述步骤3中具体包括如下步骤：
[0097]步骤3. 1，奇异点判别函数设计
[0098]当巾取值为± 31时，ib系至in系姿态矩阵描述为
[0100] 此时，任意矢量R在两坐标系内投影满足
[0102] 在式（39)两端分别加上，并利用单位向量恒等关系式I+(uX)2=uuT，则有
[0104] 也即是，此时任意矢量在两坐标系内的投影之和均共线，且与等效旋转矢量平 行；
[0105] 因此，对当前滤波变量1是否接近奇异点的判断等价于判断式（19)中不同时刻的 气是否接近共线；为了缩短得出有效判断所需时间，设定如下判别函数
[0107] 其中，&为气的方向向量；| ? |表示取方阵行列式；
[0108] 若用于计算gk的所有&均共线，即当前对变量1的估计位于奇异点时，gk理论值 为零；
[0109] 步骤3. 2,虚拟转动设计
[0110] 虚拟转动的设计依据：若ib系至in系等效旋转矢量转角大于n/2,将ib系绕着其 某个坐标轴转动JT角得到ibl系，使ibl系至in系等效旋转矢量转角小于JT/2;
[0111] 据此，记ib系绕其x轴转动Jr角得到ibx系；ib系绕其y轴转动jt角得到iby系； ib系绕其z轴转动it角得到ibz系，则有
[0113] 其中
为已知常值对角阵，对角元为1或-1 ;
[0114] 无论载体动基座对准开始时刻真实姿态为何值，姿态阵
所对 应的四个等效旋转矢量中，至少有一个等效旋转矢量的转角小于n/2,远离奇异点；
[0115] 步骤3. 3,奇异点规避的滤波算法
[0116] 依据上面设计的判别函数和虚拟转动，得到奇异点规避的滤波算法：
[0117] (1)给定奇异点判别时间tk，在tk时刻以前，利用步骤1、步骤2推导的对准滤波 算法，同时处理四个独立的滤波过程，分别龙
进行估计；其中，对Ct> 进行估计时，式（21)、式（22)中相关参变量需要进行相应的坐标变换；所有滤波 器状态量初值均设为零，考虑式（27)及虚拟转动分析结论，将1对应的方差阵初值设定为P〇0= 1，P11= 1，P22= 1 ;
[0118] (2)在tk时刻，分别计算姿态阵
所对应的判别函数值，gk、gkx、 gky、gkz，仅选择最大判别函数值所对应的滤波过程完成后续滤波对准。
[0119] 与现有技术相比，本发明的有益效果是：本发明对于捷联惯导系统动基座对准问 题，给出了一种基于罗德里格参数和二阶非线性量测的滤波对准算法，并对罗德里格参数 奇异点问题进行了详细讨论，设计了处理方案。仿真分析结果表明，该算法在车辆启动段典 型机动条件下，能够快速收敛，对准精度高，以与线性卡尔曼滤波相当的计算量及计算复杂 度实现了任意失准角非线性动基座对准。
【附图说明】
[0120] 图1是姿态1时判别函数随时间变化曲线；
[0121] 图2是姿态1时姿态误差角均方根曲线；
[0122] 图3是姿态1时姿态误差角均方根曲线；
[0123] 图4是姿态2时判别函数随时间变化曲线；
[0124] 图5是姿态2时姿态误差角均方根曲线；
[0125] 图6是姿态2时姿态误差角均方根曲线。
【具体实施方式】
[0126] 下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述， 显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的 实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都 属于本发明保护的范围。
[0127] 本发明提供了一种基于罗德里格参数和二阶非线性量测的滤波对准算法，利用姿 态阵分解，将动态姿态的估计问题转化为一个常值姿态的估计，借助经典Rodrigues参数 和姿态阵之间的凯莱变换，建立了在惯性坐标系描述的系统方程线性，量测方程具有二阶 非线性的弱非线性误差模型。对于这种非线性系统的估计，采用标准线性卡尔曼滤波算法 完成时间更新。对于量测更新，借鉴高阶EKF量测更新的思路，推导了一种基于二阶非线性 量测完整泰勒级数展开的量测更新算法。新的量测更新算法在实现形式上与线性卡尔曼滤 波量测更新完全一致，且计算量相当。综合时间更新和量测更新，可以认为新算法用线性滤 波方式解决了大失准角非线性滤波对准问题，在计算量上远小于导航系内基于sigma点的 各类非线性滤波对准算法。同时对经典Rodrigues参数描述姿态存在奇异点的问题，也进 行了详细分析并给出了解决办法。根据上述方案进行仿真分析，具体描述如下。
[0128] 实施例1
[0129] 本发明实施例中，以车载SINS/GPS动基座对准为例，仿真轨迹为：由静止加速至 l〇m/S，加速度为0. 5m/s2;然后匀速行驶50s;之后转弯90°，角速率为15° /s;之后匀速 直线行驶。对准总时间取为120s。
[0130] 惯性器件精度：陀螺随机常值漂移为〇.or/h，角随机游走系数为
;加 速度计随机常值偏置为5Xl(T5g，加速度计白噪声方差强度为
，不考虑其他误差 项。
[0131] GPS精度：速度测量噪声为0.lm/s，水平位置测量噪声为3m，高度位置测量噪声为 5m，GPS数据周期为Is。
[0132] 考虑车载SINS测量坐标系与右前上车体坐标系近似安装，故水平姿态角通常为 小角，奇异点只出现在方位角接近180°时。此外，由小节3的分析易知，采用奇异点规避策 略后，最终对准算法所估计的姿态对应的等效旋转矢量转角一定不大于Ji/2。综上，仿真 中，验证两种姿态初值条件下的对准效果：姿态1，俯仰角、滚转角、方位角分别为5°、3°、 80° ;姿态2,俯仰角、滚转角、方位角分别为5°、3°、180°，对上述两种姿态初值条件下的 初始对准算法各进行100次的蒙特卡洛仿真，仿真结果如图1~图6所示。
[0133]图1、图4描述的是两种初始姿态条件下，奇异点规避策略所使用判别函数的值随 时间变化曲线，该函数值为无量纲量。从图1、4中可以得出两点结论：第一，不同等效旋转 矢量对应的判别函数在较短时间内即具有很高的区分度，这样奇异点判别时间tk即可设计 的很小，以仿真为例，tk最短可以选择为2s;第二，判别函数的值与对准过程中载体机动相 关。仿真程序中tk设计为5s。
[0134] 姿态1时对准姿态误差收敛曲线如图2、图3所示，其中，图2为整个对准区间收敛 曲线；图3为70s至120s的收敛曲线。姿态2时对准姿态误差收敛曲线如图5、图6所示， 图5对应整个对准区间收敛曲线，图6为70s到120s收敛曲线。
[0135] 与gk、gkx、gky、gkz相关的等效旋转矢量转角，在姿态1初值下，分别为80. 05°、 174.24°、179. 08 °、100. 26 ° ;在姿态 2 初值条件下，分别为 179. 87 °、177 °、175 °、 5. 83°。由图1、图4易知，姿态1初值条件下，等效旋转矢量转角为80. 05°的滤波过程在 判别完成后被保留；姿态2初值条件下，等效旋转矢量转角为5. 83°的滤波过程在判别完 成后被保留。
[0136] 两种姿态初值条件下，滤波对准结果如图2、图5所示。在车辆加速机动条件下，对 准算法能够快速收敛，在150s对准结束时刻，姿态1初值条件下，两水平姿态误差角和方位 姿态误差角均方根分别为9.6"、10"、3. 87';姿态2初值条件下，两水平姿态误差角和方 位姿态误差角均方根分别为9.9"、9. 4"、3. 18'。
[0137] 对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在 不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论 从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权 利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有 变化囊括在本发明内。
[0138] 此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包 含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当 将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员 可以理解的其他实施方式。
【主权项】
1. 一种基于罗德里格参数和二阶非线性量测的滤波对准算法，包括惯性系动基座对准 过程、二阶非线性量测滤波 估计算法、经典Rodrigues参数奇异点及其处理方法，其特征在 于，具体描述如下： 步骤1，惯性系动基座对准过程 惯性系动基座对准算法以实时姿态阵的链式分解为基础， Ctb - CtbCi^ ^ HS (1) 其中，nt系为实时导航坐标系，即载体时变位置东北天地理坐标系；1"为导航惯性系， 与动基座对准开始时刻的η系重合；b为载体坐标系；ib为载体惯性系，与对准开始时刻的 b系重合；式（1)中，是运动的nt系相对于导航惯性系i "的姿态阵，由GPS输出位置信息 解析计算；能利用陀螺输出进行姿态跟踪，所以，惯性系动基座对准过程是对常值姿态阵 Ct的估计； 利用牛顿第二定律和哥氏定理，得到惯性系比力方程如下 V1" (〇 + ^ X V1'' (〇 - g1" (〇 = p (〇 ⑵ 其中，为t时刻载体对地速度在导航惯性系in内的投影；为t时刻载体所在 位置重力加速度在导航惯性系1内投影；/"(0为t时刻理想比力值； 对式（2)两端分别进行积分，并记利用GPS输出完成式（3)中F/"(X)的求解，进一步，在七^至t k更新周期内，假设为常矢量，导航惯性系in内对地速度，为线 性函数，即其中，t e [t^，tk] ;T = GPS量测更新周期； 将式（9)、（10)代入式（7)、⑶中，整理得利用捷联惯导姿态、速度二子样更新算法实现对式（4)中Fj(4)的求解；进一步，考虑 陀螺仪随机常值漂移ε b和加速度计随机常值零偏V6的影响，推导得其中，#为<^姿态误差角；汾。仏)为加速度计惯性系比力积分误差； 由式（2)、（3)、（4)和式（13)可得，求解常值姿态阵q的观测方程为 V'm {h)~ (tk ) = Cfn Vy" {tk ) (邱 进一步，用经典Rodrigues参数法来等价描述姿态阵，记对应Rodrigues参数为1， 则二者满足凯莱变换关系式，BP CI=(I + {1^(I-{1^) (17) 将式（17)代入式（16)，整理得，其中，. Wv包含惯性器件测量噪声的积分和随机扰动的积分，且有式（18)即是与姿态阵等价的Rodrigues参数1的观测方程，若能估计出Rodrigues 参数1，则依据式（17)得到 综上，惯性系动基座对准选取如下15维状态由上述推导，系统方程及量测方程分别为，利用式（21)、式（22)设计滤波算法实现对Rodrigues参数1的估计，进而得到姿态阵 ，通过式（1)即实现动基座对准； 步骤2,二阶非线性量测滤波估计算法 式（21)描述的系统方程为线性，式（22)描述的量测方程为非线性，但仅是状态量的二 阶非线性函数，能用有限阶Taylor级数展开描述，即其中，Xktl为Taylor级数展开点；束t -XftO; ei是第i个分量为1，其余元素为0的 3维单位向量；Hk为非线性函数h的雅克比阵；D i为非线性函数h的二阶偏导数阵；Tr为矩 阵求迹函数，且有其中，h = Qi1 h2 h3]T; 同时，对于二阶非线性函数，其二阶偏导数阵为常值矩阵，故由式（21)、（22)、（25)，知 DiS 15维常值对称阵，其中非零元素仅有 Di(2,6) = 1，D2(1，6) = -1，D3(1，5) = 1， Di(3,5) = -1，D2(3,4) = 1，D3(2,4) = -I Di(6,2) = 1，D2(6，1) = -1，D3(5，1) = 1， 0八5,3) = -1，D2(4,3) = 1，D3(4,2) = -I (26) 步骤2.1，滤波时间更新算法 式（21)系统方程为线性，采用标准卡尔曼滤波算法完成时间更新，得到状态量和估计 误差方差阵的一步预测，即用状态一步预测结果尤μ代替式（23)中Xktl，建立 起当前观测量与状态一步预测关系式，进而设计量测更新算法对一步预测结果毛/(^进行校 正，得到当前时刻的状态最优估计值+I下面推导基于式（23)二阶泰勒级数量测方程的滤 波量测更新算法； 步骤2. 2,滤波量测更新算法 量测更新形式定义为与线性卡尔曼滤波量测更新一致，假设k时刻状态估计结果为其中，Lk为引入的补偿项，和最佳增益K k一样均为待定值；L k和K k的确定原则分别为 使^^为无偏估计和使&的均方误差阵Pk的迹最小； 定义状态估计误差其中，文嫩-1 = A . ， 由式（23)、式（24)、式（27)、式（28)整理得要使为无偏估计，即要求期望为零；假设时间更新为无偏估计，则对式（29) 右端取期望并令结果为零，得补偿项Lk为其中，Ε[ ·]表示对括号内变量求期望； 将式（30)代入式（29)中，整理得由于Ε[足]=0，E[wv] = 0,且^与A不相关，故由式（31)、式（32)得Pk为其中 Ak= E [AAT] (34) 式（32)中，A为3维列向量，从而Ak为3阶方阵；利用式（32)，经过推导得Λ ,第1行 第j列元素为式（33)中协方差阵更新方式与标准卡尔曼滤波形式一致，从而式（27)中最佳增益阵考察量测更新方程式（27)、（33)、（36)，基于二阶泰勒级数的量测更新算法与标准卡尔 曼滤波在形式上完全一致，仅增加了对Lk、Ak的计算；而由式（30)、（35)知，L k、Ak的求解 简单；考虑到DiS常值稀疏矩阵，将L k、Λ k描述为仅与P ,/η相关的形式，如此进一步减小 在线计算量； 步骤3,经典Rodrigues参数奇异点及其处理方法 经典Rodrigues参数是最少参数姿态描述方法之一，存在奇异点，一种等价表示方法 为(37) 其中，u为两坐标系之间等效旋转矢量的方向向量；φ为等效旋转矢量转过的角度； 当Φ取值为± π时，1值为无穷大，是经典Rodrigues参数法进行姿态描述时的奇异 点，对ib系进行某种虚拟转动得到i bl，使得ibl系相对于i n系的Rodrigues参数远离奇异 点，然后采用前述滤波对准算法完成Cf对应Rodrigues参数的估计，进而借助已知的虚拟 转动，计算q。2.根据权利要求1所述的基于罗德里格参数和二阶非线性量测的滤波对准算法，其特 征在于，所述步骤3中具体包括如下步骤： 步骤3.1，奇异点判别函数设计 当Φ取值为± π时，ib系至i n系姿态矩阵描述为此时，任意矢量R在两坐标系内投影满足在式（39)两端分别加上并利用单位向量恒等关系式I+(uX)2= uuT，则有(40) 也即是，此时任意矢量在两坐标系内的投影之和均共线，且与等效旋转矢量平行； 因此，对当前滤波变量1是否接近奇异点的判断等价于判断式（19)中不同时刻的\是 否接近共线；为了缩短得出有效判断所需时间，设定如下判别函数(41) 其中，&为\的方向向量；I · I表示取方阵行列式； 若用于计算gk的所有 '均共线，即当前对变量1的估计位于奇异点时，gk理论值为零； 步骤3. 2,虚拟转动设计 虚拟转动的设计依据：若ib系至in系等效旋转矢量转角大于η/2,将ib系绕着其某个 坐标轴转动π角得到ibl系，使ibl系至in系等效旋转矢量转角小于π/2; 据此，记ib系绕其X轴转动π角得到i bx系；i b系绕其y轴转动π角得到i by系；i b系 绕其Z轴转动JT角得到ibz系，则有 其中，、cZ、为已知常值对角阵，对角元为1或_l; 无论载体动基座对准开始时刻真实姿态为何值，姿态阵所对应的 四个等效旋转矢量中，至少有一个等效旋转矢量的转角小于JT/2,远离奇异点； 步骤3. 3,奇异点规避的滤波算法 依据上面设计的判别函数和虚拟转动，得到奇异点规避的滤波算法： (1)给定奇异点判别时间tk，在tk时刻以前，利用步骤1、步骤2推导的对准滤波算 法，同时处理四个独立的滤波过程，分别对q；、?；、q；进行估计；其中，对c,t、 Ct进行估计时，式（21)、式（22)中相关参变量需要进行相应的坐标变换；所有滤波 器状态量初值均设为零，考虑式（27)及虚拟转动分析结论，将1对应的方差阵初值设定为 P〇0= 1，P 11= 1，P 22= 1 ; ⑵在tk时刻，分别计算姿态阵q；所对应的判别函数值，gk、gkx、 gky、 gkz，仅选择最大判别函数值所对应的滤波过程完成后续滤波对准。
【专利摘要】本发明公开了一种基于罗德里格参数和二阶非线性量测的滤波对准算法，包括由惯性系动基座对准过程、二阶非线性量测滤波估计算法、经典罗德里格参数奇异点及其处理方法，利用姿态阵分解，将动态姿态的估计问题转化为一个常值姿态的估计，借助经典Rodrigues参数和姿态阵之间的凯莱变换，建立了在惯性坐标系描述的系统方程线性，量测方程具有二阶非线性的弱非线性误差模型。同时对经典罗德里格参数描述姿态存在奇异点的问题，也进行了详细分析并给出了解决办法。本发明在车辆启动段典型机动条件下，能够快速收敛，对准精度高，以与线性卡尔曼滤波相当的计算量及计算复杂度实现了任意失准角非线性动基座对准。
【IPC分类】G01C25/00, G01C21/16
【公开号】CN104897170
【申请号】CN201510297409
【发明人】严恭敏, 梅春波, 付强文, 秦永元
【申请人】西北工业大学
【公开日】2015年9月9日
【申请日】2015年5月29日
最新回复(0)