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

专利信息
申请号: 201710074005.2 申请日: 2017-02-10
公开(公告)号: CN106679612B 公开(公告)日: 2017-12-22
发明(设计)人: 房建成;卢兆兴;李建利;刘刚;李奕其;曹全 申请(专利权)人: 北京航空航天大学
主分类号: G01B21/32 分类号: G01B21/32
代理公司: 北京科迪生专利代理有限责任公司11251 代理人: 杨学明,顾炜
地址: 100191*** 国省代码: 北京;11
权利要求书: 查看更多 说明书: 查看更多
摘要:
搜索关键词: 一种 基于 惯性 测量 匹配 非线性 挠曲 变形 估计 方法
【权利要求书】:

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

步骤(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 Vszksx 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.]]>

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

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

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

Ωbs-ϵs-ϵs′=Cms(Ωbm+θ·)]]>

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

Cms=C(θx)·C(θy)·C(θz)·C(μx)·C(μy)·C(μz)=1000cosθxsinθx0-sinθxcosθx·cosθy0-sinθy010sinθy0cosθy·cosθzsinθz0-sinθzcosθz0001·1000cosμxsinμx0-sinμxcosμx·cosμy0-sinμy010sinμy0cosμy·cosμzsinμz0-sinμzcosμz0001]]>

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

ZΩ=Ωbs-Ωbm=(Cms-I3)Ωbm+Cmsθ·+ϵs+ϵs′]]>

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

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

arm=r··m+2(ωimm×r·m)+ω·imm×rm+ωimm×(ωimm×rm)]]>

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

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

fbs-▿s-▿s′=Cms(fbm+arm)]]>

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

Zf=fbs-fbm=(Cms-I3)fbm+▿s+▿s′+Cms[r··m+2(ωimm×r·m)+ω·imm×rm+ωimm×(ωimm×rm)]]]>

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

系统的量测量为Z=[ZΩ Zf]T,其中包括3维主、子节点陀螺仪输出匹配量ZΩ和3维主、子节点加速度计输出匹配量Zf

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

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

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

下载完整专利技术内容需要扣除积分,VIP会员可以免费下载。

该专利技术资料仅供研究查看技术是否侵权等信息,商用须获得专利权人授权。该专利全部权利属于北京航空航天大学,未经北京航空航天大学许可,擅自商用是侵权行为。如果您想购买此专利、获得商业授权和技术合作,请联系【客服

本文链接:http://www.vipzhuanli.com/pat/books/201710074005.2/1.html,转载请声明来源钻瓜专利网。

×

专利文献下载

说明:

1、专利原文基于中国国家知识产权局专利说明书;

2、支持发明专利 、实用新型专利、外观设计专利(升级中);

3、专利数据每周两次同步更新,支持Adobe PDF格式;

4、内容包括专利技术的结构示意图流程工艺图技术构造图

5、已全新升级为极速版,下载速度显著提升!欢迎使用!

请您登陆后,进行下载,点击【登陆】 【注册】

关于我们 寻求报道 投稿须知 广告合作 版权声明 网站地图 友情链接 企业标识 联系我们

钻瓜专利网在线咨询

周一至周五 9:00-18:00

咨询在线客服咨询在线客服
tel code back_top