基于导航信息双向融合的行人-移动机器人协同导航方法-钱伟行

第22卷第1期中国惯性技术学报Vol.22No.12014年2月JournalofChineseInertialTechnologyFeb.2014收稿日期：2013-08-11；修回日期：2013-11-28基金项目：国家自然科学基金（61304227，61273057，60904091）；江苏省高校自然科学基金（13KJB590001，12KJB470011）作者简介：钱伟行（1981—），男，讲师，博士，从事惯性与组合导航技术研究。E-mail：[e-mail]文章编号：1005-6734(2014)01-0074-05doi:10.13695/j.cnki.12-1222/o3.2014.01.015基于导航信息双向融合的行人/移动机器人协同导航方法钱伟行1，彭晨1，田恩刚1，李荣冰2（1.南京师范大学电气与自动化工程学院，南京210042；2.南京航空航天大学自动化学院，南京210016）摘要：为提高卫星导航系统失效且环境存在电磁干扰的情况下行人导航系统与移动机器人导航系统的定位性能，研究了一种基于人机一体化智能系统的信息双向融合协同导航方法。该方法利用行人导航系统与移动机器人导航系统不同的误差特性，构建信息双向融合滤波器同步修正两者的导航信息误差，即利用移动机器人捷联惯导系统较高的传感器精度完成对行人导航系统磁航向误差的实时修正，并利用行人导航系统较高的位移精度修正机器人惯导系统误差，实现同步提高两套导航系统的定位与航向精度。导航定位实验表明，该方法可有效提高人机一体化智能系统的导航定位精度，行人导航系统定位误差约为行进距离的3.3%，移动机器人导航系统的误差积累速度降低为单独工作时的1/3。本文所提出的方法利用了人机一体化智能系统的结构特征，在卫星导航系统失效的电磁干扰环境中有效提高了一体化系统的综合导航定位性能，具有较高的理论研究与工程应用价值。关键词：人机一体化智能系统；协同导航；信息双向融合；磁航向误差；零速修正中图分类号：U666.1文献标志码：APedestrian/mobilerobotcooperativenavigationmethodbasedonnavigationinformationbidirectionalfusionQIANWei-xing1,PENGChen1,TIANEn-gang1,LIRong-bing2(1.SchoolofElectricalandAutomationEngineering,NanjingNormalUniversity，Nanjing210042,China;2.CollegeofAutomation,NanjingUniversityofAeronauticsandAstronautics,Nanjing210016,China)Abstract:Inordertoimprovethepositioningprecisionofpedestriannavigationsystemandmobilerobotnavigationsystemineletromagneticinterferenceenvironment,aninformationbidirectionalfusioncooperativenavigationmethodbasedonhuman-machineintelligentsystemisstudied.Themethodutilizesthedifferenterrorcharacteristicsofpedestriannavigationsystemandmobilerobotstrapdowninertialnavigationsystem(SINS)tobuildinformationbidirectionalfusionfiltersforcorrectingtheerrorsoftwonavigationsystemssynchronously.Themethodmakesuseofthehigh-accuracysensorsofmobilerobotSINStocompletereal-timemodifyofmagneticheadingerrorinpedestriannavigationsystem,andusethehighdisplacementaccuracyofpedestriannavigationsystemtocorrectinertialnavigationsystemerrorsinrobottoimprovethepositioningandheadingaccuracyoftwonavigationsystemssynchronously.Experimentsresultsshowthatthepositioningerrorofpedestriannavigationsystemisabout3.3%ofthewalkingdistance,andthesystemerroraccumulationrateofmobilerobotSINSisreducedto1/3oftheratewhenworkingalone.Thestudiedmethodcaneffectivelyimprovethenavigationandpositioningaccuracyofthehuman-machineintelligentsystemineletromagneticinterferenceenvironment,andhashightheoreticalresearchandengineeringapplicationvalue.Keywords:human-machineintelligentsystem;cooperativenavigation;informationbidirectionalfusion;magneticazimutherror;zero-velocityupdate第1期钱伟行等：GNSS失效环境中基于微惯性/地磁测量组件的个人导航方法75人机一体化智能系统（简称人机智能系统）是一类人与智能机器共同感知、共同认知、共同决策，实现人与智能机器在决策层面上进行融合，产生更高行动效率的智能系统，该系统在军事行动、工程作业、残疾人行动辅助等领域具有很高的应用价值。人机智能系统中各类信息的交互与融合是建立人机耦合协同作业机制的重要前提[1]。利用人体与移动机器人可构建人机一体化智能导航系统，该系统由安装于人体的行人导航系统与移动机器人上安装的组合导航系统组成，其中行人导航系统采用足部微惯性/卫星组合导航系统，移动机器人导航系统采用光纤捷联惯性/卫星紧组合导航系统。在卫星导航系统可用的环境中，两者的导航系统可独立工作，但在进入存在电磁干扰的未知环境时，卫星导航系统将会受到严重影响甚至失效，单独采用惯性系统将无法满足人机智能系统的导航定位性能需求。针对该问题本文研究了一种基于行人导航系统与移动机器人惯性导航系统的信息双向融合协同导航方法。该方法利用两种导航系统不同的误差特性，构建信息双向融合滤波器，同步修正两者的导航信息误差，即利用移动机器人光纤捷联惯导系统较高的传感器精度完成对行人导航系统磁航向误差的实时修正，同时利用行人导航系统较高的单步位移精度修正机器人捷联惯导系统的误差，从而实现同时提高两类载体导航系统的定位与航向精度。1基于中高精度惯性系统的行人导航系统磁航向误差实时修正方法1.1磁航向误差修正的基本原理地球磁场在空间中构成一个三维磁矢量,基于对该磁矢量进行检测与分解，即可求得运动载体当前的磁航向信息。然而，实际应用中磁传感器所测量的信号，是地磁场与环境干扰磁场源所共同形成的磁场。由于环境中干扰磁场源的位置、数量、类型等往往无法确定，且随着载体的移动呈现不可预测的特点，因此对各类干扰源产生的磁场进行建模与分析是十分复杂的，但若可对环境中某一位置地磁场与其他磁场源所产生的磁场进行辨识与分离，则可有效修正由环境干扰源所导致的磁航向误差[2]。磁航向的常规解算方法是将与载体固联的磁传感器检测的地磁场矢量投影在水平面上，通过水平分量之间的三角函数关系求解载体磁航向。因此，通过分析磁场矢量在水平面上的投影可进行磁场的辨识方法的研究。未知环境中的磁场分布示意图如图1所示。图中emag表示地磁场矢量在水平面上的投影，dmag表示环境干扰磁场源所产生的磁场矢量和在水平面上的投影，hmag表示环境中某位置总磁场矢量的水平分量，即emag与dmag的矢量和。忽略地磁场异常等现象且暂时忽略磁偏角，在某地域内的地磁场矢量可视为已知常量，表示为emag，并将磁偏角表示为e；在存在磁场干扰的环境中，将磁传感器所测量的环境中总磁场矢量表示为hmag，类似地将带有误差的磁偏角表示为h。磁传感器磁场源2磁场源1磁场源N……….地磁场矢量磁传感器敏感的总磁场矢量方向磁传感器磁场源2磁场源2磁场源1磁场源1磁场源N磁场源N……….地磁场矢量磁传感器敏感的总磁场矢量方向图1未知环境中磁传感器所敏感的磁矢量示意图Fig.1SchematicdiagramofmagneticvectormagneticsensordetectedinunknownenvironmentNEθdθemagemagd0NEθdθemagemagd0图2地磁场与环境磁源产生磁场的水平分量示意图Fig.2Schematicdiagramofthehorizontalcomponentofthemagneticfieldproducedbygeomagneticfieldandenvironmentalmagneticsource设三轴磁传感器测量的地磁场emag在载体坐标系下的投影为bemag，而测量总磁场矢量hmag在载体坐标系下的投影为bhmag，其中magmagmagexbeeyezmag,magmagmaghxbhhyhzmag.若已知与磁传感器固联载体的横滚角与俯仰角分别为与，则可将bemag投影到水平面与垂直的方向上。76中国惯性技术学报第22卷Tmagmagmag100cos0sin0cossin0100sincossin0cosnnnneexeyezbemagmag(1)tan(mag/mag)mag0&mag02πtan(mag/mag)mag0&mag0πtan(mag/mag)mag0&mag0πtan(mag/mag)mag0&mag0nnnnexexexeynnnnexexexeyennnnexexexeynnnnexexexeyaaaa(2)通过相同方法也可以求得d：Tmagmagmag100cos0sin0cossin0100sincossin0cosnnnnhhxhyhzbhmagmag(3)dtan(mag/mag)mag0&mag02πtan(mag/mag)mag0&mag0πtan(mag/mag)mag0&mag0πtan(mag/mag)mag0&mag0nnnnhxhyhxhynnnnhxhyhxhynnnnhxhyhxhynnnnhxhyhxhyaaaa(4)因此即可求得该地理位置上的磁航向角修正值de。由nemag与nemag物理含义可知，与载体的姿态无关，只与载体位置与磁场干扰源的相对位置有关。因此，可通过该特性实现高精度惯性系统对低精度惯性系统的航向修正。1.2基于中高精度惯性系统的行人导航系统磁航向误差修正方法基于上节所分析的磁航向角修正值，利用人机一体化智能系统中移动机器人惯性导航系统中较高精度的传感器实时修正行人导航系统的磁航向误差。具体实现方法如下：1