CN106679612A - 一种基于惯性测量匹配的非线性挠曲变形估计方法 - Google Patents

Abstract

本发明公开了一种基于惯性测量匹配的非线性挠曲变形估计方法。首先，建立包括安装误差角、挠曲变形、挠曲变形一阶导、挠曲变形角、挠曲变形角一阶导、加速度计常值和随机偏置以及陀螺仪常值和随机漂移的非线性系统状态方程；其次，将主、子节点的加速度计测量值之差和陀螺仪测量值之差作为量测，建立系统的非线性系统测量方程；最后，采用非线性滤波方法——中心差分卡尔曼滤波(Central Difference Kalman Filter，CDKF)估计方法，估计出每个时刻子节点处的挠曲变形和挠曲变形角。本发明具有计算精度高、易于实现的特点，提高了挠曲变形的估计精度。

Description

一种基于惯性测量匹配的非线性挠曲变形估计方法

技术领域

本发明涉及一种基于惯性测量匹配的非线性挠曲变形估计方法，可用于飞机、舰船以及车辆的挠曲变形测量及挠曲变形角测量。

背景技术

船体、机体的挠曲变形使得各个节点处的运动参数存在较大差异，如不对挠曲变形进行测量和补偿，这种差异将严重影响各个节点处运动参数的测量精度。因此要实现多个节点处运动参数的高精度测量，必须对挠曲变形进行测量和补偿。

挠曲变形的测量方法主要有两类，一类是基于光学传感器的光学测量法，另一类是基于分布式位置姿态测量系统(Position and Orientation System,POS)的测量法。光学测量法系统安装复杂，且要求光束收发无阻断，容易受天气等外部因素影响。分布式POS由一个主POS和多个惯性测量单元(Inertial Measurement Unit，IMU)(子IMU)组成，主POS的GPS天线和主IMU通过滤波的方法实现惯性/卫星组合导航，子IMU通过与主POS进行传递对准测量子节点运动参数。基于分布式POS的测量法需要建立包含主、子系统姿态误差、速度误差、位置误差、惯性器件误差、安装误差、挠曲变形等多种误差方程，并进行包含主子系统捷联解算、主系统惯性/卫星组合导航、子系统传递对准等多种复杂运算，且需要引入GPS，增加了变形测量系统的成本。

发明内容

本发明要解决的技术的问题是：针对现有技术的不足，提供了一种基于惯性测量匹配的非线性挠曲变形估计方法，在充分利用惯性测量数据的同时，既降低了测量系统成本，又减小了运算量。

本发明解决上述技术问题采用的技术方案为：一种基于惯性测量匹配的非线性挠曲变形估计方法，具体步骤如下：

步骤(1)、建立包括安装误差角、挠曲变形、挠曲变形一阶导、挠曲变形角、挠曲变形角一阶导、加速度计常值和随机偏置以及陀螺仪常值和随机漂移的非线性系统状态方程；

步骤(2)、将主、子节点的加速度计测量值之差和陀螺仪测量值之差作为量测，建立系统的非线性系统测量方程；

步骤(3)、采用非线性滤波方法——中心差分卡尔曼滤波估计方法，估计出t_k时刻子节点处的挠曲变形和挠曲变形角，k＝1,2,…,N，不断重复本步骤，直至主子节点惯性测量数据结束。

本发明的原理：在船体、机体、船体等的各测量节点处安装IMU，利用挠曲变形引起的各节点处惯性测量量的差异，建立包括子节点安装误差角、挠曲变形、挠曲变形角、主子节点加速度计常值和随机偏置以及主子节点陀螺仪常值和随机漂移的非线性系统状态方程，以及包含主、子节点加速度计测量值之差和陀螺仪测量值之差的非线性量测方程，最后通过非线性滤波方法估计挠曲变形和挠曲变形角。

本发明与现有技术相比的优点在于：本发明弥补了基于分布式POS的测量法模型复杂、运算量大以及成本高的不足，利用各节点处IMU测量量即可实现挠曲变形测量，且建立的系统非线性状态方程和量测方程准确，计算精度高。

附图说明

图1为基于惯性测量匹配的非线性挠曲变形估计方法流程图；

图2为挠曲变形示意图，其中，地球惯性坐标系为O_ix_iy_iz_i，主IMU坐标系为O_mx_my_mz_m，初始时刻子IMU坐标系为O_s0x_s0y_s0z_s0，主子节点之间杆臂r₀，动态过程中子IMU坐标系为O_sx_sy_sz_s，主子节点之间杆臂为r，对应的挠曲变形为△r，挠曲变形角为θ，R_m和R_s分别表示动态过程中O_ix_iy_iz_i坐标系下主子节点的位置矢量；

图3为x轴方向的挠曲角以及y轴方向的初始杆臂引起的挠曲变形示意图，其中，θ_x为x轴方向挠曲角，线段O_mA表示初始时刻主IMU坐标系下y轴方向的杆臂分量，杆臂经过挠曲变形后为圆弧O_mA′。

具体实施方式

下面结合附图以及具体实施方式进一步说明本发明。

如说明书附图1所示，本发明的具体实施包括以下步骤：

1、建立包括安装误差角、挠曲变形、挠曲变形一阶导、挠曲变形角、挠曲变形角一阶导、加速度计常值和随机偏置以及陀螺仪常值和随机漂移的非线性系统状态方程，具体为：

(1)建立固定安装误差角数学模型

如附图1系统挠曲变形示意图中所示，相关坐标系包括：地球惯性坐标系O_ix_iy_iz_i；主节点处惯性测量单元(Inertial Measurement Unit,IMU)坐标系O_mx_my_mz_m；子节点处IMU坐标系O_sx_sy_sz_s。

子节点固定安装误差角在系统安装之后就已确定且为常值，其数学模型为：

其中，μ＝[μ_x μ_y μ_z]^T为子节点相对主节点的固定安装误差角，μ_x、μ_y和μ_z表示子IMU坐标系O_sx_sy_sz_s的x轴、y轴和z轴的安装误差角；

(2)弹性变形角数学模型

子节点处的弹性变形角θ的变化符合二阶马尔科夫过程，其微分方程为：

式中，θ＝[θ_x θ_y θ_z]^T，θ_i表示子IMU坐标系O_sx_sy_sz_s的i轴方向的弹性变形角，β_i＝2.146/τ_i，τ_i为二阶马尔科夫过程相关时间，η_i为白噪声，其方差满足其中为弹性变形角θ_i的方差，β_i和为描述弹性变形角的二阶马尔科夫过程参数；

(3)弹性变形数学模型

如附图1系统挠曲变形示意图中所示，相关坐标系包括：地球惯性坐标系O_ix_iy_iz_i；主节点处惯性测量单元(Inertial Measurement Unit,IMU)坐标系O_mx_my_mz_m；子节点处IMU坐标系O_sx_sy_sz_s。初始时刻主子节点之间杆臂在主IMU坐标系O_mx_my_mz_m表示为挠曲角为θ₀＝0_3×1，挠曲变形动态过程中，主子节点之间杆臂为r，对应的挠曲变形为△r，挠曲变形角为θ。附图2为x轴方向的挠曲角分量θ_x以及y轴方向的初始杆臂值分量r_y0引起的挠曲变形示意图。线段O_mA表示初始时刻主IMU坐标系下y轴方向的杆臂分量，挠曲变形后为圆弧O_mA′，且圆弧O_mA′和线段O_mA的长度均为r_y0。由此线段AB即为y轴方向挠曲变形量线段O_mC即为z轴方向挠曲变形量其表达式为：

同理可得θ_x和r_z0引起y轴方向挠曲变形量和z轴方向的挠曲变形量的表达式为：

则θ_x引起的y轴方向的挠曲变形和z轴方向的挠曲变形表达式为：

同理可得，θ_y引起的x轴方向的挠曲变形和z轴方向的挠曲变形θ_z引起的x轴方向的挠曲变形和y轴方向的挠曲变形

最终得到挠曲变形的表达式为：

对上式进行求导即可得到表达式：

对上式再进行求导，并将挠曲角的二阶表达式带入其中即可得到挠曲变形的二阶导数的表达式：

(4)加速度计常值和随机偏置数学模型

由于主系统的惯性器件精度要高于子系统，因此在建立状态方程式不考虑主系统的加速度计误差，则子IMU加速度计常值偏置微分方程为：

式中为子节点加速度计常值偏置，分别为子IMU坐标系下的分量。

子IMU加速度计随机偏置由一阶马尔科夫过程表示：

式中，为一阶马尔科夫过程参数，γ_si为白噪声。

(5)陀螺仪常值和随机漂移数学模型

子IMU陀螺仪常值漂移微分方程为：

式中ε_s＝[ε_sx ε_sy ε_sz]^T为子节点陀螺仪常值漂移，ε_sx，ε_sy，ε_mz分别为子IMU坐标系下的分量。

子IMU陀螺仪随机漂移由一阶马尔科夫过程表示：

式中，κ_si为一阶马尔科夫过程参数，λ_si为白噪声。

(6)非线性系统状态方程建立

系统的状态量为X＝[X₁ X₂ X₃ X₄]^T，其中包括9维安装误差角和挠曲变形角6维挠曲变形量6维子系统加速度计误差变量以及6维子系统陀螺仪误差变量X₄＝[ε_sx ε_sy ε_sz ε′_sx ε′_sy ε′_sz]^T。

系统状态方程的表达形式如下：

式中，X(t)＝[X₁(t) X₂(t) X₃(t) X₄(t)]^T为系统状态量； 为系统噪声，非线性函数F[X(t),t]的表达式可根据(1)-(5)得到。G表达式如下：

2、将主、子节点的加速度计测量值之差和陀螺仪测量值之差作为量测，建立系统的非线性系统测量方程，具体过程如下：

(1)陀螺仪输出匹配量测方程

主IMU陀螺仪输出值和子IMU陀螺仪输出值之间的非线性关系可以表示为：

式中，分别为主子IMU的陀螺漂移，包括常值漂移和随机漂移两部分。为挠曲角变化率，为主IMU与子IMU之间的坐标转换矩阵，其表达式可以表示为：

由此得到陀螺仪输出匹配的非线性量测方程：

(2)加速度计输出匹配量测方程

挠曲杆臂产生的加速度表达式为：

式中，为主系统陀螺仪输出角速度，r^m、和分别为主IMU坐标系下挠曲杆臂及其一阶导数和二阶导数，表达式已经在步骤2中给出。

主IMU加速度计输出值和子IMU加速度计输出值之间的非线性关系可以表示为：

由此得到加速度计输出匹配的非线性量测方程：

(3)非线性系统量测方程建立

系统的量测量为Z＝[Z_Ω Z_f]^T，其中包括3维主、子节点陀螺仪输出匹配量Z_Ω和3维主、子节点加速度计输出匹配量Z_f。

系统量测方程的表达形式如下：

Z(t)＝H(X,t)+v(t)

式中为量测噪声，其中为加速度计量测噪声，为陀螺仪量测噪声，非线性函数H(X,t)的表达式可根据(1)-(2)得到。

Claims (3)

1.一种基于惯性测量匹配的非线性挠曲变形估计方法，其特征在于，包括以下步骤：

步骤(1)、建立包括安装误差角、挠曲变形、挠曲变形一阶导、挠曲变形角、挠曲变形角一阶导、加速度计常值和随机偏置以及陀螺仪常值和随机漂移的非线性系统状态方程；

步骤(2)、将主、子节点的加速度计测量值之差和陀螺仪测量值之差作为量测，建立系统的非线性系统测量方程；

步骤(3)、采用非线性滤波方法——中心差分卡尔曼滤波估计方法，估计出t_k时刻子节点处的挠曲变形和挠曲变形角，k＝1,2,…,N，不断重复本步骤，直至主子节点惯性测量数据结束。

2.根据权利要求1所述的一种基于惯性测量匹配的非线性挠曲变形估计方法，其特征在于，步骤(1)中非线性系统状态方程包括安装误差角、挠曲变形、挠曲变形一阶导、挠曲变形角、挠曲变形角一阶导、加速度计常值和随机偏置以及陀螺仪常值和随机漂移的数学模型，具体为：

(11)建立固定安装误差角数学模型

相关坐标系包括：地球惯性坐标系O_ix_iy_iz_i；主节点处惯性测量单元(InertialMeasurement Unit,IMU)坐标系O_mx_my_mz_m；子节点处IMU坐标系O_sx_sy_sz_s；

子节点固定安装误差角在系统安装之后就已确定且为常值，其数学模型为：

$\stackrel{\·}{\mathrm{\μ}}=0$

其中，μ＝[μ_x μ_y μ_z]^T为子节点相对主节点的固定安装误差角，μ_x、μ_y和μ_z表示子IMU坐标系O_sx_sy_sz_s的x轴、y轴和z轴的安装误差角；

(12)弹性变形角数学模型

子节点处的弹性变形角θ的变化符合二阶马尔科夫过程，其微分方程为：

${\stackrel{\·\·}{\mathrm{\θ}}}_i+2{\mathrm{\β}}_i{\stackrel{\·}{\mathrm{\θ}}}_i+{\mathrm{\β}}_i^2{\mathrm{\θ}}_i={\mathrm{\η}}_i,i=x,y,z$

式中，θ＝[θ_x θ_y θ_z]^T，θ_i表示子IMU坐标系O_sx_sy_sz_s的i轴方向的弹性变形角，β_i＝2.146/τ_i，τ_i为二阶马尔科夫过程相关时间，η_i为白噪声，其方差满足其中为弹性变形角θ_i的方差，β_i和为描述弹性变形角的二阶马尔科夫过程参数；

(13)弹性变形数学模型

初始时刻主子节点之间杆臂在主IMU坐标系O_mx_my_mz_m表示为挠曲角为θ₀＝0_3×1，挠曲变形动态过程中，主子节点之间杆臂为r，对应的挠曲变形为△r，挠曲变形角为θ；主IMU坐标系下x轴方向的挠曲角分量为θ_x，y轴方向的初始杆臂值分量为r_y0，线段O_mA表示初始时刻主IMU坐标系下y轴方向的杆臂分量，挠曲变形后为圆弧O_mA′，且圆弧O_mA′和线段O_mA的长度均为r_y0，因此线段AB即为y轴方向挠曲变形量线段O_mC即为z轴方向挠曲变形量其表达式为：

${\mathrm{\Δr}}_y^m({\mathrm{\θ}}_x,r_{y0})=(\frac{{\mathrm{sin\θ}}_x}{{\mathrm{\θ}}_x}-1)\·r_{y0}$

${\mathrm{\Δr}}_z^m({\mathrm{\θ}}_x,r_{y0})=\frac{1-{\mathrm{cos\θ}}_x}{{\mathrm{\θ}}_x}\·r_{y0}$

同理可得θ_x和z轴方向的初始杆臂值分量r_z0引起y轴方向挠曲变形量和z轴方向的挠曲变形量的表达式为：

${\mathrm{\Δr}}_y^m({\mathrm{\θ}}_x,r_{z0})=\frac{1-{\mathrm{cos\θ}}_x}{{\mathrm{\θ}}_x}\·r_{z0}$

${\mathrm{\Δr}}_z^m({\mathrm{\θ}}_x,r_{y0})=(\frac{{\mathrm{sin\θ}}_x}{{\mathrm{\θ}}_x}-1)\·r_{z0}$

则θ引起的y轴方向的挠曲变形和z轴方向的挠曲变形表达式为：

${\mathrm{\Δr}}_y^m\left({\mathrm{\θ}}_x\right)={\mathrm{\Δr}}_y^m({\mathrm{\θ}}_x,r_{y0})+{\mathrm{\Δr}}_y^m({\mathrm{\θ}}_x,r_{z0})$

${\mathrm{\Δr}}_z^m\left({\mathrm{\θ}}_x\right)={\mathrm{\Δr}}_z^m({\mathrm{\θ}}_x,r_{y0})+{\mathrm{\Δr}}_z^m({\mathrm{\θ}}_x,r_{z0})$

同理可得，y轴方向的挠曲角分量θ_y引起的x轴方向的挠曲变形和z轴方向的挠曲变形z轴方向的挠曲角分量为θ_z引起的x轴方向的挠曲变形和y轴方向的挠曲变形

最终得到挠曲变形的表达式为：

${\mathrm{\Δr}}_x^m=(\frac{{\mathrm{sin\θ}}_y}{{\mathrm{\θ}}_y}+\frac{{\mathrm{sin\θ}}_z}{{\mathrm{\θ}}_z}-2)\·r_{x0}+\frac{1-{\mathrm{cos\θ}}_y}{{\mathrm{\θ}}_y}\·r_{z0}-\frac{1-{\mathrm{cos\θ}}_z}{{\mathrm{\θ}}_z}\·r_{y0}$

${\mathrm{\Δr}}_y^m=(\frac{{\mathrm{sin\θ}}_x}{{\mathrm{\θ}}_x}+\frac{{\mathrm{sin\θ}}_z}{{\mathrm{\θ}}_z}-2)\·r_{y0}+\frac{1-{\mathrm{cos\θ}}_z}{{\mathrm{\θ}}_z}\·r_{x0}-\frac{1-{\mathrm{cos\θ}}_x}{{\mathrm{\θ}}_x}\·r_{z0}$

${\mathrm{\Δr}}_z^m=(\frac{{\mathrm{sin\θ}}_x}{{\mathrm{\θ}}_x}+\frac{{\mathrm{sin\θ}}_y}{{\mathrm{\θ}}_y}-2)\·r_{z0}+\frac{1-{\mathrm{cos\θ}}_x}{{\mathrm{\θ}}_x}\·r_{y0}-\frac{1-{\mathrm{cos\θ}}_y}{{\mathrm{\θ}}_y}\·r_{x0}$

对上式进行求导即可得到表达式：

$\begin{array}{c}\mathrm{\Δ}{\stackrel{\·}{r}}_x^m=(\frac{{\stackrel{\·}{\mathrm{\θ}}}_y({\mathrm{\θ}}_y{\mathrm{cos\θ}}_y-{\mathrm{sin\θ}}_y)}{{\mathrm{\θ}}_y^2}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_z({\mathrm{\θ}}_z{\mathrm{cos\θ}}_z-2{\mathrm{sin\θ}}_z)}{{\mathrm{\θ}}_z^2})\·r_{x0}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_y({\mathrm{\θ}}_y{\mathrm{sin\θ}}_y+{\mathrm{cos\θ}}_y-1)}{{\mathrm{\θ}}_y^2}\·r_{z0}\\ -\frac{{\stackrel{\·}{\mathrm{\θ}}}_z({\mathrm{\θ}}_z{\mathrm{sin\θ}}_z+{\mathrm{cos\θ}}_z-1)}{{\mathrm{\θ}}_z^2}\·r_{y0}\end{array}$

$\begin{array}{c}\mathrm{\Δ}{\stackrel{\·}{r}}_y^m=(\frac{{\stackrel{\·}{\mathrm{\θ}}}_x({\mathrm{\θ}}_x{\mathrm{cos\θ}}_x-{\mathrm{sin\θ}}_x)}{{\mathrm{\θ}}_x^2}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_z({\mathrm{\θ}}_z{\mathrm{cos\θ}}_z-2{\mathrm{sin\θ}}_z)}{{\mathrm{\θ}}_z^2})\·r_{y0}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_z({\mathrm{\θ}}_z{\mathrm{sin\θ}}_z+{\mathrm{cos\θ}}_z-1)}{{\mathrm{\θ}}_z^2}\·r_{x0}\\ -\frac{{\stackrel{\·}{\mathrm{\θ}}}_x({\mathrm{\θ}}_x{\mathrm{sin\θ}}_x+{\mathrm{cos\θ}}_x-1)}{{\mathrm{\θ}}_x^2}\·r_{z0}\end{array}$

$\begin{array}{c}\mathrm{\Δ}{\stackrel{\·}{r}}_z^m=(\frac{{\stackrel{\·}{\mathrm{\θ}}}_x({\mathrm{\θ}}_x{\mathrm{cos\θ}}_x-{\mathrm{sin\θ}}_x)}{{\mathrm{\θ}}_x^2}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_y({\mathrm{\θ}}_y{\mathrm{cos\θ}}_y-{\mathrm{sin\θ}}_y)}{{\mathrm{\θ}}_y^2})\·r_{z0}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_x({\mathrm{\θ}}_x{\mathrm{sin\θ}}_x+{\mathrm{cos\θ}}_x-1)}{{\mathrm{\θ}}_x^2}\·r_{y0}\\ -\frac{{\stackrel{\·}{\mathrm{\θ}}}_y({\mathrm{\θ}}_y{\mathrm{sin\θ}}_y+{\mathrm{cos\θ}}_y-1)}{{\mathrm{\θ}}_y^2}\·r_{x0}\end{array}$

对上式再进行求导，并将挠曲角的二阶表达式带入其中即可得到挠曲变形的二阶导数的表达式：

$\begin{array}{c}\mathrm{\Δ}{\stackrel{\·\·}{r}}_x^m=(\frac{(-2{\mathrm{\β}}_y{\stackrel{\·}{\mathrm{\θ}}}_y-{\mathrm{\β}}_y^2{\mathrm{\θ}}_y)({\mathrm{\θ}}_y{\mathrm{cos\θ}}_y-{\mathrm{sin\θ}}_y)}{{\mathrm{\θ}}_y^2}-\frac{{\stackrel{\·}{\mathrm{\θ}}}_y(2{\mathrm{\θ}}_y{\mathrm{cos\θ}}_y-2{\mathrm{sin\θ}}_y+{\mathrm{\θ}}_y^2{\mathrm{sin\θ}}_y)}{{\mathrm{\θ}}_y^3}\\ +\frac{(-2{\mathrm{\β}}_z{\stackrel{\·}{\mathrm{\θ}}}_z-{\mathrm{\β}}_z^2{\mathrm{\θ}}_z)({\mathrm{\θ}}_z{\mathrm{cos\θ}}_z-{\mathrm{sin\θ}}_z)}{{\mathrm{\θ}}_z^2}-\frac{{\stackrel{\·}{\mathrm{\θ}}}_z(2{\mathrm{\θ}}_z{\mathrm{cos\θ}}_z-2{\mathrm{sin\θ}}_z+{\mathrm{\θ}}_z^2{\mathrm{sin\θ}}_z)}{{\mathrm{\θ}}_z^3})\·r_{x0}\\ +(\frac{(-2{\mathrm{\β}}_y{\stackrel{\·}{\mathrm{\θ}}}_y-{\mathrm{\β}}_y^2{\mathrm{\θ}}_y)({\mathrm{\θ}}_y{\mathrm{sin\θ}}_y+{\mathrm{cos\θ}}_y-1)}{{\mathrm{\θ}}_y^2}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_y({\mathrm{\θ}}_y^2{\mathrm{cos\θ}}_y-2{\mathrm{\θ}}_y{\mathrm{sin\θ}}_y-2{\mathrm{cos\θ}}_y+2)}{{\mathrm{\θ}}_y^3})\·r_{z0}\\ -(\frac{(-2{\mathrm{\β}}_z{\stackrel{\·}{\mathrm{\θ}}}_z-{\mathrm{\β}}_z^2{\mathrm{\θ}}_z)({\mathrm{\θ}}_z{\mathrm{sin\θ}}_z+{\mathrm{cos\θ}}_z-1)}{{\mathrm{\θ}}_z^2}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_z({\mathrm{\θ}}_z^2{\mathrm{cos\θ}}_z-2{\mathrm{\θ}}_z{\mathrm{sin\θ}}_z-2{\mathrm{cos\θ}}_z+2)}{{\mathrm{\θ}}_z^3})\·r_{y0}\end{array}$

$\begin{array}{c}\mathrm{\Δ}{\stackrel{\·\·}{r}}_y^m=(\frac{(-2{\mathrm{\β}}_x{\stackrel{\·}{\mathrm{\θ}}}_x-{\mathrm{\β}}_x^2{\mathrm{\θ}}_x)({\mathrm{\θ}}_x{\mathrm{cos\θ}}_x-{\mathrm{sin\θ}}_x)}{{\mathrm{\θ}}_x^2}-\frac{{\stackrel{\·}{\mathrm{\θ}}}_x(2{\mathrm{\θ}}_x{\mathrm{cos\θ}}_x-2{\mathrm{sin\θ}}_x+{\mathrm{\θ}}_x^2{\mathrm{sin\θ}}_x)}{{\mathrm{\θ}}_x^3}\\ +\frac{(-2{\mathrm{\β}}_z{\stackrel{\·}{\mathrm{\θ}}}_z-{\mathrm{\β}}_z^2{\mathrm{\θ}}_z)({\mathrm{\θ}}_z{\mathrm{cos\θ}}_z-{\mathrm{sin\θ}}_z)}{{\mathrm{\θ}}_z^2}-\frac{{\stackrel{\·}{\mathrm{\θ}}}_z(2{\mathrm{\θ}}_z{\mathrm{cos\θ}}_z-2{\mathrm{sin\θ}}_z+{\mathrm{\θ}}_z^2{\mathrm{sin\θ}}_z)}{{\mathrm{\θ}}_z^3})\·r_{y0}\\ +(\frac{(-2{\mathrm{\β}}_z{\stackrel{\·}{\mathrm{\θ}}}_z-{\mathrm{\β}}_z^2{\mathrm{\θ}}_z)({\mathrm{\θ}}_z{\mathrm{sin\θ}}_z+{\mathrm{cos\θ}}_z-1)}{{\mathrm{\θ}}_z^2}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_z({\mathrm{\θ}}_z^2{\mathrm{cos\θ}}_z-2{\mathrm{\θ}}_z{\mathrm{sin\θ}}_z-2{\mathrm{cos\θ}}_z+2)}{{\mathrm{\θ}}_z^3})\·r_{x0}\\ -(\frac{(-2{\mathrm{\β}}_x{\stackrel{\·}{\mathrm{\θ}}}_x-{\mathrm{\β}}_x^2{\mathrm{\θ}}_x)({\mathrm{\θ}}_x{\mathrm{sin\θ}}_x+{\mathrm{cos\θ}}_x-1)}{{\mathrm{\θ}}_x^2}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_x({\mathrm{\θ}}_x^2{\mathrm{cos\θ}}_x-2{\mathrm{\θ}}_x{\mathrm{sin\θ}}_x-2{\mathrm{cos\θ}}_x+2)}{{\mathrm{\θ}}_x^3})\·r_{z0}\end{array}$

$\begin{array}{c}\mathrm{\Δ}{\stackrel{\·\·}{r}}_z^m=(\frac{(-2{\mathrm{\β}}_x{\stackrel{\·}{\mathrm{\θ}}}_x-{\mathrm{\β}}_x^2{\mathrm{\θ}}_x)({\mathrm{\θ}}_x{\mathrm{cos\θ}}_x-{\mathrm{sin\θ}}_x)}{{\mathrm{\θ}}_x^2}-\frac{{\stackrel{\·}{\mathrm{\θ}}}_x(2{\mathrm{\θ}}_x{\mathrm{cos\θ}}_x-2{\mathrm{sin\θ}}_x+{\mathrm{\θ}}_x^2{\mathrm{sin\θ}}_x)}{{\mathrm{\θ}}_x^3}\\ +\frac{(-2{\mathrm{\β}}_y{\stackrel{\·}{\mathrm{\θ}}}_y-{\mathrm{\β}}_y^2{\mathrm{\θ}}_y)({\mathrm{\θ}}_y{\mathrm{cos\θ}}_y-{\mathrm{sin\θ}}_y)}{{\mathrm{\θ}}_y^2}-\frac{{\stackrel{\·}{\mathrm{\θ}}}_y(2{\mathrm{\θ}}_y{\mathrm{cos\θ}}_y-2{\mathrm{sin\θ}}_y+{\mathrm{\θ}}_y^2{\mathrm{sin\θ}}_y)}{{\mathrm{\θ}}_y^3})\·r_{z0}\\ +(\frac{(-2{\mathrm{\β}}_x{\stackrel{\·}{\mathrm{\θ}}}_x-{\mathrm{\β}}_x^2{\mathrm{\θ}}_x)({\mathrm{\θ}}_x{\mathrm{cos\θ}}_x+{\mathrm{cos\θ}}_x-1)}{{\mathrm{\θ}}_x^2}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_x({\mathrm{\θ}}_x^2{\mathrm{cos\θ}}_x-2{\mathrm{\θ}}_x{\mathrm{sin\θ}}_x-2{\mathrm{cos\θ}}_x+2)}{{\mathrm{\θ}}_x^3})\·r_{y0}\\ -(\frac{(-2{\mathrm{\β}}_y{\stackrel{\·}{\mathrm{\θ}}}_y-{\mathrm{\β}}_y^2{\mathrm{\θ}}_y)({\mathrm{\θ}}_y{\mathrm{sin\θ}}_y+{\mathrm{cos\θ}}_y-1)}{{\mathrm{\θ}}_y^2}+\frac{{\stackrel{\·}{\mathrm{\θ}}}_y({\mathrm{\θ}}_y^2{\mathrm{cos\θ}}_y-2{\mathrm{\θ}}_y{\mathrm{sin\θ}}_y-2{\mathrm{cos\θ}}_y+2)}{{\mathrm{\θ}}_y^3})\·r_{x0}\end{array}$

(14)加速度计常值和随机偏置数学模型

由于主系统的惯性器件精度要高于子系统，因此在建立状态方程式不考虑主系统的加速度计误差，则子IMU加速度计常值偏置微分方程为：

${\stackrel{\·}{\▿}}_s=0$

式中为子节点加速度计常值偏置，分别为子IMU坐标系下的分量；

子IMU加速度计随机偏置由一阶马尔科夫过程表示：

式中，为一阶马尔科夫过程参数，γ_si为白噪声；

(15)陀螺仪常值和随机漂移数学模型

子IMU陀螺仪常值漂移微分方程为：

${\stackrel{\·}{\mathrm{\ϵ}}}_s=0$

式中ε_s＝[ε_sx ε_sy ε_sz]^T为子节点陀螺仪常值漂移，ε_sx，ε_sy，ε_mz分别为子IMU坐标系下的分量；

子IMU陀螺仪随机漂移由一阶马尔科夫过程表示：

${\stackrel{\·}{\mathrm{\ϵ}}}_{si}^{\′}+{\mathrm{\κ}}_{si}{\mathrm{\ϵ}}_{si}^{\′}={\mathrm{\λ}}_{si},i=x,y,z$

式中，κ_si为一阶马尔科夫过程参数，λ_si为白噪声；

(16)非线性系统状态方程建立

系统的状态量为X＝[X₁ X₂ X₃ X₄]^T，其中包括9维安装误差角和挠曲变形角6维挠曲变形量6维子系统加速度计误差变量以及6维子系统陀螺仪误差变量X₄＝[ε_sx ε_sy ε_sz ε′_sx ε′_sy ε′_sz]^T；

系统状态方程的表达形式如下：

$\stackrel{\·}{X}\left(t\right)=F\[X\left(t\right),t\]+G\left(t\right)w\left(t\right)$

式中，X(t)＝[X₁(t) X₂(t) X₃(t) X₄(t)]^T为系统状态量；为系统噪声，非线性函数F[X(t),t]的表达式可根据(11)-(15)得到，G表达式如下：

$\begin{array}{ccc}G=\left[\begin{array}{ccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ I_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& D_1& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& D_2\end{array}\right]& D_1=\left[\begin{array}{ccc}{\mathrm{\γ}}_{sx}& 0& 0\\ 0& {\mathrm{\γ}}_{sy}& 0\\ 0& 0& {\mathrm{\γ}}_{sz}\end{array}\right],& D_2=\left[\begin{array}{ccc}{\mathrm{\λ}}_{sx}& 0& 0\\ 0& {\mathrm{\λ}}_{sy}& 0\\ 0& 0& {\mathrm{\λ}}_{sz}\end{array}\right]\end{array}.$

3.根据权利要求2所述的一种基于惯性测量匹配的非线性挠曲变形估计方法，其特征在于步骤(2)中的非线性系统量测方程将主、子节点的加速度计测量值之差和陀螺仪测量值之差均作为量测，具体建立步骤为：

(21)陀螺仪输出匹配量测方程

主IMU陀螺仪输出值和子IMU陀螺仪输出值之间的非线性关系可以表示为：

${\mathrm{\Ω}}^{b_s}-{\mathrm{\ϵ}}_s-{\mathrm{\ϵ}}_s^{\′}=C_m^s({\mathrm{\Ω}}^{b_m}+\stackrel{\·}{\mathrm{\θ}})$

式中，分别为主子IMU的陀螺漂移，包括常值漂移和随机漂移两部分，为挠曲角变化率，为主IMU与子IMU之间的坐标转换矩阵，其表达式可以表示为：

$\begin{array}{c}{C}_m^s=C\left({\mathrm{\θ}}_x\right)\·C\left({\mathrm{\θ}}_y\right)\·C\left({\mathrm{\θ}}_z\right)\·C\left({\mathrm{\μ}}_x\right)\·C\left({\mathrm{\μ}}_y\right)\·C\left({\mathrm{\μ}}_z\right)\\ =\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_x& {\mathrm{sin\θ}}_x\\ 0& -{\mathrm{sin\θ}}_x& {\mathrm{cos\θ}}_x\end{array}\right]\·\left[\begin{array}{ccc}{\mathrm{cos\θ}}_y& 0& -{\mathrm{sin\θ}}_y\\ 0& 1& 0\\ {\mathrm{sin\θ}}_y& 0& {\mathrm{cos\θ}}_y\end{array}\right]\·\left[\begin{array}{ccc}{\mathrm{cos\θ}}_z& {\mathrm{sin\θ}}_z& 0\\ -{\mathrm{sin\θ}}_z& {\mathrm{cos\θ}}_z& 0\\ 0& 0& 1\end{array}\right]\\ \·\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\μ}}_x& {\mathrm{sin\μ}}_x\\ 0& -{\mathrm{sin\μ}}_x& {\mathrm{cos\μ}}_x\end{array}\right]\·\left[\begin{array}{ccc}{\mathrm{cos\μ}}_y& 0& -{\mathrm{sin\μ}}_y\\ 0& 1& 0\\ {\mathrm{sin\μ}}_y& 0& {\mathrm{cos\μ}}_y\end{array}\right]\·\left[\begin{array}{ccc}{\mathrm{cos\μ}}_z& {\mathrm{sin\μ}}_z& 0\\ -{\mathrm{sin\μ}}_z& {\mathrm{cos\μ}}_z& 0\\ 0& 0& 1\end{array}\right]\end{array}$

由此得到陀螺仪输出匹配的非线性量测方程：

$\begin{array}{c}{Z}_{\mathrm{\Ω}}={\mathrm{\Ω}}_{b_s}-{\mathrm{\Ω}}_{b_m}\\ =(C_m^s-I_3){\mathrm{\Ω}}_{b_m}+C_m^s\stackrel{\·}{\mathrm{\θ}}+{\mathrm{\ϵ}}_s+{\mathrm{\ϵ}}_s^{\′}\end{array}$

(22)加速度计输出匹配量测方程

挠曲杆臂产生的加速度表达式为：

$a_r^m={\stackrel{\·\·}{r}}^m+2({\mathrm{\ω}}_{im}^m\×{\stackrel{\·}{r}}^m)+{\stackrel{\·}{\mathrm{\ω}}}_{im}^m\×r^m+{\mathrm{\ω}}_{im}^m\×({\mathrm{\ω}}_{im}^m\×r^m)$

式中，为主系统陀螺仪输出角速度，r^m、和分别为主IMU坐标系下挠曲杆臂及其一阶导数和二阶导数；

主IMU加速度计输出值和子IMU加速度计输出值之间的非线性关系可以表示为：

$f^{b_s}-{\▿}_s-{\▿}_s^{\′}=C_m^s(f^{b_m}+a_r^m)$

由此得到加速度计输出匹配的非线性量测方程：

$\begin{array}{c}{Z}_f=f^{b_s}-f^{b_m}\\ =(C_m^s-I_3)f^{b_m}+{\▿}_s+{\▿}_s^{\′}+C_m^s\[{\stackrel{\·\·}{r}}^m+2({\mathrm{\ω}}_{im}^m\×{\stackrel{\·}{r}}^m)+{\stackrel{\·}{\mathrm{\ω}}}_{im}^m\×r^m+{\mathrm{\ω}}_{im}^m\×({\mathrm{\ω}}_{im}^m\×r^m)\]\end{array}$

(3)非线性系统量测方程建立

系统的量测量为Z＝[Z_Ω Z_f]^T，其中包括3维主、子节点陀螺仪输出匹配量Z_Ω和3维主、子节点加速度计输出匹配量Z_f；

系统量测方程的表达形式如下：

Z(t)＝H(X,t)+v(t)

式中为量测噪声，其中为加速度计量测噪声，为陀螺仪量测噪声，非线性函数H(X,t)的表达式可根据(21)-(22)得到。