基于地磁场矢量误差反馈的姿态补偿算法_赵文杰

合集下载

1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

：ＡＡｂｓｔｒａｃｔｎｅｗｃｌｏｓｅｄｌｏｏａｔｔｉｔｕｄｅｃｏｍｅｎｓａｔｉｏｎａｌｏｒｉｔｈｍｆｏｒｍｉｎｉａｅｒｉａｌｒｏｂｏｔｓｗａｓｔｏｓｏｌｖｅｒｏｏｓｅｄ－ｐｐｇｐｐ，ｒｏｂｌｅｍｌｏｂａｌｏｓｉｔｉｏｎｉｎａｔｔｉｔｕｄｅｅｓｔｉｍａｔｉｏｎｄｕｒｉｎｓｓｔｅｍ（ＧＰＳ）ｏｕｔａｅｓ．ＷｈｅｎＧＰＳｉｓａｖａｉｌａｂｌｅｔｈｅｐｇｐｇｇｙｇｔｈｅｃｌａｓｓｉｃａｌｉｎｔｅｒａｔｅｄｎａｖｉａｔｉｏｎｓｓｔｅｍｉｓｕｓｅｄｔｏｅｓｔｉｍａｔｅａｔｔｉｔｕｄｅａｎｄｅｏｍａｎｅｔｉｃｖｅｃｔｏｒ．Ｏｎｃｅｔｈｅｉｎ－ｇｇｙｇｇ，ｔｅｒａｔｅｄｓｓｔｅｍｃａｎｎｏｔｗｏｒｋｅｆｆｅｃｔｉｖｅｌｔｈｅｃｏｎｃｅｔｏｆｆｅｅｄｂａｃｋｃｏｎｔｒｏｌｉｓｉｎｔｒｏｄｕｃｅｄｉｎｔｈｅａｔｔｉｔｕｄｅｅｓｔｉ－ｇｙｙｐｍａｔｉｏｎｒｏｃｅｓｓ．Ｔｈｅｌａｔｅｎｔａｔｔｉｔｕｄｅｉｎｆｏｒｍａｔｉｏｎｅｘｉｓｔｅｄｉｎｔｈｅｅｏｍａｎｅｔｉｃｍｅａｓｕｒｅｍｅｎｔａｎｄｉｔｓｅｓｔｉｍａｔｉｏｎｐｇｇ，ｉｓｍａｄｅｔｈｅｂｅｓｔｕｓｅａｎｄｔｈｅｆｅｅｄｂａｃｋｏｆｅｏｍａｎｅｔｉｃｖｅｃｔｏｒｄｅｖｉａｔｉｏｎｉｓｕｓｅｄｔｏｃｏｒｒｅｃｔｔｈｅａｎｕｌａｒｒａｔｅｇｇｇ，ｍｅａｓｕｒｅｍｅｎｔｓｏｔｈａｔｔｈｅｄｉｖｅｒｅｎｃｏｆｉｎｅｒｔｉａｌｎａｖｉａｔｉｏｎｓｓｔｅｍ（ＩＮＳ）ｅｒｒｏｒｓｃａｎｂｅｌｉｍｉｔｅｄ．Ｔｈｅｎｅｗｇｙｇｙ，ａｌｏｒｉｔｈｍｗａｓｔｅｓｔｅｄｏｎａｍｉｎｉａｅｒｉａｌｒｏｂｏｔａｎｄｃｏｍａｒｅｄｗｉｔｈａｒｅｆｅｒｅｎｃｅｓｓｔｅｍ．Ｅｘｅｒｉｍｅｎｔａｌｒｅｓｕｌｔｓｇｐｙｐ／，ｓｈｏｗｔｈａｔｉｎｃｏｎｔｒａｓｔｔｏｔｈｅＩＮＳＧＰＳｉｎｔｅｒａｔｅｄｎａｖｉａｔｉｏｎｓｓｔｅｍｔｈａｔｗｏｒｋｓｉｎｏｅｎｌｏｏｍｏｄｅｔｈｅｎｅｗ－ｇｇｙｐｐｃｌｏｓｅｄｌｏｏａｔｔｉｔｕｄｅｃｏｍｅｎｓａｔｉｏｎａｌｏｒｉｔｈｍａｃｈｉｅｖｅｓｓｍａｌｌｅｒｅｒｒｏｒｓｆｏｒａｔｔｉｔｕｄｅｅｓｔｉｍａｔｅａｎｄｅｆｆｅｃｔｉｖｅｌｒｅ－－ｐｐｇｙｓｔｒａｉｎｓｔｈｅａｃｃｕｍｕｌａｔｉｏｎｏｆＩＮＳａｔｔｉｔｕｄｅｅｒｒｏｒｓｄｕｒｉｎＧＰＳｏｕｔａｅｓ．ｇｇ：；；；；ＫｅｗｏｒｄｓａｅｒｉａｌｒｏｂｏｔｓｇｖｅｃｔｏｒＧＰＳｏｕｔａｅｓａｔｔｉｔｕｄｅｅｓｔｉｍａｔｅｉｎｅｒｔｉａｌｎａｖｉａｔｉｏｎｓｓｔｅｍｅｏｍａｎｅｔｉｃｇｇｙｇｙ小型空中机器人受到自动化领域的广近几年来，泛关注．姿态反馈是空中机器人自动飞行控制的基
同时将闭环控制的思想引入姿态反馈校正系统把地磁场惯性矢量作为定值控制系统的设定值把估计量作为测量值通过惯性矢量与估计量之间的偏差来反馈补偿姿态角速率的测量误差进而实现姿态信息的闭环估计并最终限制惯性导航系统姿态误差的发散
第４８卷第９期２０１４年９月
）ＪｏｕｒｎａｌｏｆＺｈｅｉａｎＵｎｉｖｅｒｓｉｔＥｎｉｎｅｅｒｉｎＳｃｉｅｎｃｅｊｇｙ（ｇｇ
收稿日期：２０１３１０２３．－－）目（２０１１ＦＺＡ４０３１．，：ｗ作者简介：赵文杰（男，博士生，从事导航、制导与控制的研究．１９８５－）Ｅ－ｍａｉｌｚｈａｏｉｉｃ．ｚｕ．ｅｄｕ．ｃｎ＠ｊｐｊ：通信联系人：方舟，男，副教授．Ｅ－ｍａｉｌｚｆａｎｕ．ｅｄｕ．ｃｎ＠ｚｇｊ
２赵文杰１，方舟２，李平１，
（）浙江大学控制科学与工程学系，浙江杭州３浙江大学航空航天学院，浙江杭州３１．１００２７；２．１００２７）摘要：针对小型空中机器人在全球定位系统（失锁情况下的姿态估计问题，提出一种新的闭环姿态补偿算ＧＰＳ有Ｇ采用组合导航算法对姿态角和地磁场矢量做出估计；当组合系统无法有效工作时，将反法．ＰＳ辅助的情况下，充分利用地磁场测量值与估计值之间隐含的姿态信息，通过地磁场矢量误差反馈控制的思想引入姿态估计过程，馈对姿态角速率测量值进行修正，进而抑制惯性导航系统误差的发散．在小型空中机器人上对算法进行实物验证，／并与参考系统对比．结果显示：在Ｇ与开环工作模式下的Ｉ新的闭环ＰＳ失锁的情况下，ＮＳＧＰＳ组合导航系统相比，姿态补偿算法的姿态估计误差更小，使惯性导航系统的姿态误差积累得到了有效的抑制．关键词：空中机器人；地磁场矢量；姿态估计；惯性导航系统ＧＰＳ失锁；）中图分类号：Ｖ２４９．３２８文献标志码：Ａ文章编号：１００８９７３Ｘ（２０１４０９１７０４０６－－－
［４］的非线性关系，Ｃｈｉａｎｇ等提出用人工智能的方法
，选取惯性导航系统的误差状态变量为３个
方向上的平台误差角 ψ 速度误差 δ ｖｎ、ｅ、ｄ；ｎ、 ψ ψ 位置误差量 δ 建立惯性导航ｖｖＬ、ｈ． δ δ δ λ、 δ ｅ、ｄ；系统误差模型：平台角误差模型为ｎｎ ω ×ψ ＋ε ｉｅ＋ω ｅｎ） ω ． ψ ＝－（速度误差模型为ｎｎｎｂｖ δ δ ＝［ｆ ×］ｆ＋ε ｂａ． ψ ＋Ｃ位置误差模型为（）１（）２
工学版）浙江大学学报（
Ｖｏｌ．４８Ｎｏ．９Ｓｅ．２０１４ｐ
：／ＤＯＩ１０．３７８５．ｉｓｓｎ．１００８９７３Ｘ．２０１４．０９．０２３－ｊ
基于地磁场矢量误差反馈的姿态补偿算法
１２１２，，ＺＨＡＯＷｅｎＦＡＮＧＺｈｏｕＬＩＰｉｎｉｅ－ｇｊ，
（１．ＤｅａｒｔｍｅｎｔｏＣｏｎｔｒｏｌＳｃｉｅｎｃｅａｎｄＥｎｉｎｅｅｒｉｎＺｈｅｉａｎＵｎｉｖｅｒｓｉｔａｎｚｈｏｕ３１００２７，Ｃｈｉｎａ；ｐｆｇｇ，ｊｇｙ，Ｈｇ２．ＳｃｈｏｏｌｏＡｅｒｏｎａｕｔｉｃｓａｎｄＡｓｔｒｏｎａｕｔｉｃｓ，ＺｈｅｉａｎＵｎｉｖｅｒｓｉｔａｎｚｈｏｕ３１００２７，Ｃｈｉｎａ）ｆｊｇｙ，Ｈｇ
第９期
赵文杰，等：基于地磁场矢量误差反馈的姿态补偿算法
１７０５
随着数据融合技术的不断发展，惯性导航系统（）／ＩＮＳＧＰＳ组合导航系统逐渐被广泛应用于空中／机器人．近年来，非线性滤波算法的不断改进，ＩＮＳ
［］可以满足ＧＰＳ组合导航系统的性能逐渐提高１－３，
／１ＭＥＭＳＧＰＳ组合导航算法
１．１惯性导航误差方程本文采用基于误差模型估计（间接法）的松组合方式
［１１］
然而Ｇ大多数空中机器人对精度和动态的要求．ＰＳ信号易被遮挡和干扰，尤其当空中机器人持续特技飞行时，一旦缺少了ＧＧＰＳ信号极易失锁．ＰＳ信息的辅助作用，开环积分状态下的惯性导航系统将无此时，如何准确估计惯法保证姿态信息的有效收敛．导系统的姿态误差并提供连续而准确的状态信息是亟待解决的问题．由于惯性测量信息与ＩＮＳ误差之间存在一定
Ａｔｔｉｔｕｄｅｃｏｍｅｎｓａｔｉｏｎａｌｏｒｉｔｈｍｐｇｂａｓｅｄｏｎｅｒｒｏｒｆｅｅｄｂａｃｋｏｆｅｏｍａｎｅｔｉｃｖｅｃｔｏｒｇｇ
础，因此，姿态估计精度直接关系到机器人实时控制的稳定性和安全性．
／浙江大学学报（工学版）网址：ｗｗｗ．ｏｕｒｎａｌｓ．ｚｕ．ｅｄｕ．ｃｎｅｎｊｇ
）；）；基金项目：国家自然科学基金资助项目（浙江省科技计划资助项目（中央高校基本科研业务费专项资金资助项６１００４０６６２０１１Ｃ２３１０６