[发明专利]一种针对柔性杆臂的传递对准方法

权利要求书

1.一种针对柔性杆臂的传递对准方法，其特征在于：该方法包括如下步骤：

S1，建立针对柔性杆臂的误差模型；具体步骤为：

S1.1，建立柔性杆臂与挠曲变形角的关系式：

其中：r^m指主惯导坐标系m系下的柔性杆臂矢量，分别指柔性杆臂矢量的一阶和二阶微分形式，r₀^m是杆臂矢量初始状态；θ^s为子惯导实际坐标系s系下挠曲变形角向量，θ_orien为挠曲变形角向量θ^s标量形式，orien＝x,y,z时指向子惯导系统x、y、z轴三个方向，分别指变形角向量的一阶和二阶微分形式；β为相关系数，β_orien,orien＝x,y,z表示相关系数在三个方向的分量，w为白噪声，w_orien表示白噪声在三个方向的分量，挠曲变形角单个方向的标量满足(2)式，表示挠曲变形为噪声激励的二阶马尔可夫随机过程；L₀、L₁、L₂为系数矩阵，具体表达式如下：

r₀^m＝[r_x r_y r_z]^T

式中，r_x、r_y、r_z是杆臂矢量初始状态r₀^m在x、y、z轴的分量，T表示矩阵的转置；

S1.2，建立杆臂矢量与杆臂加速度之间的耦合关系：

其中：为投影在n系下的杆臂加速度矢量，r表示杆臂；为主惯导坐标系m系下载体角速率及其微分；为主惯导坐标系m系到理想导航坐标系n系的方向余弦矩阵；i表示惯性坐标系；

S1.3，建立姿态误差方程：

为s系到n系下的角速率值在实际子惯导坐标系上的投影；φ_m为主惯导坐标系m系到计算子惯导计算坐标系系的相对姿态角，也称为量测失准角；表示φ_m的一阶导数；μ^s为安装误差角；ε^s表示陀螺常值漂移项；

此处对所涉及到的各坐标系m系、n系、s系和系的相对关系进行简要说明：主惯导系m系、子惯导计算坐标系系与理想导航系n系之间的转换关系由方向余弦矩阵和表示，子惯导实际坐标系s系与m系之间存在挠曲变形角θ^s、静态安装误差角μ^s组合的相对姿态角，m系和系之间存在相对姿态角用φ_m表示；

S1.4，建立速度误差方程：

其中：δVⁿ为补偿固定杆臂效应后的子惯导与主惯导计算速度差在导航坐标系中的投影；是速度误差δVⁿ的微分形式；f^s为子惯导敏感到的加速度；和分别为计算子惯导坐标系和主惯导系到理想导航坐标系的方向余弦矩阵；表示加表零偏；指地球自转角速率，指运动载体所在导航系相对地球转动角速度；e表示地球坐标系；

S1.5，建立安装误差项、加表零偏、陀螺常值漂移误差方程：

式中，是静态安装误差μ^s的一阶微分形式，是加表零偏一阶微分形式，是陀螺常值漂移ε^s一阶微分形式；

S2，针对柔性杆臂系统建立系统状态方程和量测方程；

S2.1，建立针对柔性杆臂的传递对准系统状态方程：

选取22维系统状态量：δV^T和为二维状态量；

状态量中，δV^T表示速度误差状态量δV的转置，忽略天向通道的速度误差；φ_m^T表示量测失准角φ_m的转置；μ^T表示安装误差角μ的转置；θ^T表示挠曲变形角θ的转置；表示挠曲变形角速率的转置，表示加表零偏的转置，忽略无法估计的天向加表零偏，ε^T表示陀螺漂移ε的转置，r^T表示柔性杆臂r的转置；

传递对准系统状态方程如式(7)所示：

其中，表示系统状态量X一阶导数；F表示状态转移矩阵；W表示系统噪声向量；G表示系统噪声分配矩阵；

状态转移矩阵如式(8)：

其中:

[f^s×]表示f^s的反对称阵；表示的反对称阵,是子惯导敏感角速率，是导航系相对惯性空间的运动角速率，是矩阵转置；

A₁₁、A₁₆、A₁₇、A₇₆、A₇₇、A₈₇分别为矩阵对应位置的系数矩阵，具体表达式如下：

A₈₇＝L₀/2；

系统噪声向量为：

w_ε^T、w_w^T分别是加表随机游走噪声、陀螺随机游走噪声、挠曲变形过程噪声；

系统噪声分配矩阵为：

展开(10)式中cs_tj,t,j＝1,2,3是矩阵中对应元素；

S2.2，建立针对柔性杆臂的“速度”匹配方式量测方程：

式中，和分别表示子惯导和主惯导解算速度值，V_L表示柔性杆臂造成的杆臂速度误差；

S2.3，建立针对柔性杆臂的“姿态”匹配方式量测方程：

式中，表示计算子惯导坐标系与主惯导坐标系之间的方向余弦矩阵，分别为φ_m在x轴，y轴和z轴向上的投影；

S2.4，建立针对柔性杆臂的“角速率”匹配方式量测方程：

式中，Δω表示主子惯导角速率测量之差；分别为子惯导和主惯导敏感的角速率；

S2.5，建立针对柔性杆臂的“速度+姿态+角速率”匹配方案量测矩阵：

传递对准系统量测方程如式(14)所示：

Z＝HX+V (14)

式中，Z表示观测量，H表示观测矩阵，X表示系统状态量，V表示观测噪声；

观测量：

式中：表示2维速度、3维姿态和3维角速率差三组量测量；和分别表示子惯导和主惯导解算速度值；δVⁿ表示主子惯导速度误差；表示固定杆臂造成的速度误差，φ_m为量测失准角；分别为子惯导和主惯导敏感的角速率；

观测矩阵：

量测噪声向量为：

ν_V^T、ν_ω^T、分别是速度误差观测噪声、陀螺角速率观测噪声、量测失准角观测噪声；

S3，最后设计基于“速度+角速率+姿态”对准匹配方案的卡尔曼滤波算法，采用卡尔曼滤波方法进行最优估计，具体包括：

S3.1，进行时间更新，根据式(18)计算状态一步预测值：

其中，表示状态一步预测值；Φ_k,k-1为k-1时刻至k时刻的一步状态转移矩阵；表示k-1时刻状态估计值；

根据式(19)计算一步预测均方误差方差阵P_k,k-1：

其中，P_k,k-1表示一步预测误差方差阵；表示Φ_k,k-1的转置；P_k-1表示k-1时刻的估计误差方差阵；表示k-1时刻系统噪声阵计算值；

根据式(20)计算增益矩阵K_k：

其中，K_k表示k时刻的滤波增益；P_k,k-1表示一步预测均方误差阵；表示量测噪声方差阵计算值；

S3.2，进行量测更新：

根据式(21)计算状态最优估计：

其中，表示状态估计值；K_k表示滤波增益；Z_k表示k时刻测量值；H_k表示k时刻量测量矩阵；表示状态一步预测值；

根据式(22)更新状态估计误差方差阵P_k：

P_k＝[I-K_kH_k]P_k,k-1 (22)

返回到步骤S3.1，直至滤波结束，根据式(21)计算得到的状态估计值对子惯导导航解算系统中的诸参数加表零偏、陀螺常值漂移、安装误差角、挠曲变形量进行修正，完成传递对准过程。