[发明专利]一种基于惯性测量匹配的非线性挠曲变形估计方法

摘要

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

主权项

一种基于惯性测量匹配的非线性挠曲变形估计方法，其特征在于，包括以下步骤：步骤(1)、建立包括安装误差角、挠曲变形、挠曲变形一阶导、挠曲变形角、挠曲变形角一阶导、加速度计常值和随机偏置以及陀螺仪常值和随机漂移的非线性系统状态方程；步骤(2)、将主、子节点的加速度计测量值之差和陀螺仪测量值之差作为量测，建立系统的非线性系统测量方程；步骤(3)、采用非线性滤波方法——中心差分卡尔曼滤波估计方法，估计出tk时刻子节点处的挠曲变形和挠曲变形角，k＝1,2,…,N，不断重复本步骤，直至主子节点惯性测量数据结束；步骤(1)中非线性系统状态方程包括安装误差角、挠曲变形、挠曲变形一阶导、挠曲变形角、挠曲变形角一阶导、加速度计常值和随机偏置以及陀螺仪常值和随机漂移的数学模型，具体为：(11)建立固定安装误差角数学模型相关坐标系包括：地球惯性坐标系Oixiyizi；主节点处惯性测量单元(Inertial Measurement Unit,IMU)坐标系Omxmymzm；子节点处IMU坐标系Osxsyszs；子节点固定安装误差角在系统安装之后就已确定且为常值，其数学模型为：μ·=0]]>其中，μ＝[μx μy μz]T为子节点相对主节点的固定安装误差角，μx、μy和μz表示子IMU坐标系Osxsyszs的x轴、y轴和z轴的安装误差角；(12)弹性变形角数学模型子节点处的弹性变形角θ的变化符合二阶马尔科夫过程，其微分方程为：θ··i+2βiθ·i+βi2θi=ηi,i=x,y,z]]>式中，θ＝[θx θy θz]T，θi表示子IMU坐标系Osxsyszs的i轴方向的弹性变形角，βi＝2.146/τi，τi为二阶马尔科夫过程相关时间，ηi为白噪声，其方差满足其中为弹性变形角θi的方差，βi和为描述弹性变形角的二阶马尔科夫过程参数；(13)弹性变形数学模型初始时刻主子节点之间杆臂在主IMU坐标系Omxmymzm表示为挠曲角为θ0＝03×1，挠曲变形动态过程中，主子节点之间杆臂为r，对应的挠曲变形为Δr，挠曲变形角为θ；主IMU坐标系下x轴方向的挠曲角分量为θx，y轴方向的初始杆臂值分量为ry0，线段OmA表示初始时刻主IMU坐标系下y轴方向的杆臂分量，挠曲变形后为圆弧OmA′，且圆弧OmA′和线段OmA的长度均为ry0，因此线段AB即为y轴方向挠曲变形量线段OmC即为z轴方向挠曲变形量其表达式为：Δrym(θx,ry0)=(sinθxθx-1)·ry0]]>Δrzm(θx,ry0)=1-cosθxθx·ry0]]>同理可得θx和z轴方向的初始杆臂值分量rz0引起y轴方向挠曲变形量和z轴方向的挠曲变形量的表达式为：Δrym(θx,rz0)=1-cosθxθx·rz0]]>Δrzm(θx,ry0)=(sinθxθx-1)·rz0]]>则θx引起的y轴方向的挠曲变形和z轴方向的挠曲变形表达式为：Δrym(θx)=Δrym(θx,ry0)+Δrym(θx,rz0)]]>Δrzm(θx)=Δrzm(θx,ry0)+Δrzm(θx,rz0)]]>同理可得，y轴方向的挠曲角分量θy引起的x轴方向的挠曲变形和z轴方向的挠曲变形z轴方向的挠曲角分量为θz引起的x轴方向的挠曲变形和y轴方向的挠曲变形最终得到挠曲变形的表达式为：Δrxm=(sinθyθy+sinθzθz-2)·rx0+1-cosθyθy·rz0-1-cosθzθz·ry0]]>Δrym=(sinθxθx+sinθzθz-2)·ry0+1-cosθzθz·rx0-1-cosθxθx·rz0]]>Δrzm=(sinθxθx+sinθyθy-2)·rz0+1-cosθxθx·ry0-1-cosθyθy·rx0]]>对上式进行求导即可得到表达式：Δr·xm=(θ·y(θycosθy-sinθy)θy2+θ·z(θzcosθz-sinθz)θz2)·rx0+θ·y(θysinθy+cosθy-1)θy2·rz0-θ·z(θzsinθz+cosθz-1)θz2·ry0]]>Δr·ym=(θ·x(θxcosθx-sinθx)θx2+θ·z(θzcosθz-sinθz)θz2)·ry0+θ·z(θzsinθz+cosθz-1)θz2·rx0-θ·x(θxsinθx+cosθx-1)θx2·rz0]]>Δr·zm=(θ·x(θxcosθx-sinθx)θx2+θ·y(θycosθy-sinθy)θy2)·rz0+θ·x(θxsinθx+cosθx-1)θx2·ry0-θ·y(θysinθy+cosθy-1)θy2·rx0]]>对上式再进行求导，并将挠曲角的二阶表达式带入其中即可得到挠曲变形的二阶导数的表达式：Δr··xm=((-2βyθ·y-βy2θy)(θycosθy-sinθy)θy2-θ·y(2θycosθy-2sinθy+θy2sinθy)θy3+(-2βzθ·z-βz2θz)(θzcosθz-sinθz)θz2-θ·z(2θzcosθz-2sinθz+θz2sinθz)θz3)·rx0+((-2βyθ·y-βy2θy)(θysinθy+cosθy-1)θy2+θ·y(θy2cosθy-2θysinθy-2cosθy+2)θy3)·rz0-((-2βzθ·z-βz2θz)(θzsinθz+cosθz-1)θz2+θ·z(θz2cosθz-2θzsinθz-2cosθz+2)θz3)·ry0]]>Δr··ym=((-2βxθ·x-βx2θx)(θxcosθx-sinθx)θx2-θ·x(2θxcosθx-2sinθx+θx2sinθx)θx3+(-2βzθ·z-βz2θz)(θzcosθz-sinθz)θz2-θ·z(2θzcosθz-2sinθz+θz2sinθz)θz3)·ry0+((-2βzθ·z-βz2θz)(θzsinθz+cosθz-1)θz2+θ·z(θz2cosθz-2θzsinθz-2cosθz+2)θz3)·rx0-((-2βxθ·x-βx2θx)(θxsinθx+cosθx-1)θx2+θ·x(θx2cosθx-2θxsinθx-2cosθx+2)θx3)·rz0]]>Δr··zm=((-2βxθ·x-βx2θx)(θxcosθx-sinθx)θx2-θ·x(2θxcosθx-2sinθx+θx2sinθx)θx3+(-2βyθ·y-βy2θy)(θycosθy-sinθy)θy2-θ·y(2θycosθy-2sinθy+θy2sinθy)θy3)·rz0+((-2βxθ·x-βx2θx)(θxsinθx+cosθx-1)θx2+θ·x(θx2cosθx-2θxsinθx-2cosθx+2)θx3)·ry0-((-2βyθ·y-βy2θy)(θysinθy+cosθy-1)θy2+θ·y(θy2cosθy-2θysinθy-2cosθy+2)θy3)·rx0]]>(14)加速度计常值和随机偏置数学模型由于主系统的惯性器件精度要高于子系统，因此在建立状态方程式不考虑主系统的加速度计误差，则子IMU加速度计常值偏置微分方程为：▿·s=0]]>式中为子节点加速度计常值偏置，分别为子IMU坐标系下的分量；子IMU加速度计随机偏置由一阶马尔科夫过程表示：式中，为一阶马尔科夫过程参数，γsi为白噪声；(15)陀螺仪常值和随机漂移数学模型子IMU陀螺仪常值漂移微分方程为：ϵ·s=0]]>式中εs＝[εsx εsy εsz]T为子节点陀螺仪常值漂移，εsx，εsy，εmz分别为子IMU坐标系下的分量；子IMU陀螺仪随机漂移由一阶马尔科夫过程表示：ϵ·si′+κsiϵsi′=λsi,i=x,y,z]]>式中，κsi为一阶马尔科夫过程参数，λsi为白噪声；(16)非线性系统状态方程建立系统的状态量为X＝[X1 X2 X3 X4]T，其中包括9维安装误差角和挠曲变形角6维挠曲变形量6维子系统加速度计误差变量以及6维子系统陀螺仪误差变量X4＝[εsx εsy εsz ε′sx ε′sy ε′sz]T；系统状态方程的表达形式如下：X·(t)=F[X(t),t]+G(t)w(t)]]>式中，X(t)＝[X1(t) X2(t) X3(t) X4(t)]T为系统状态量；w(t)＝[hx hy hz Vsx Vsy Vsz ksx ksy ksz]T为系统噪声，非线性函数F[X(t),t]的表达式可根据(11)‑(15)得到，G表达式如下：G=03×303×303×303×303×303×3I3×303×303×303×303×303×303×303×303×303×303×303×303×3D103×303×303×303×303×303×3D2D1=γsx000γsy000γsz,D2=λsx000λsy000λsz.]]>