微小型飞行器室内激光/INS融合定位方法研究
王平,鲁可,曹云峰3,王起
(1.南京航空航天大学自动化学院南京210016:2.中国航空工业直升机设计研究所江西景德镇33001)3.南京航空航天大学高斯技术研究院南京210016;
摘要:针对激光测距仪精度高、犹干扰能力强等优点,选用二维激光测距仪为外部传感器,辅助MIMU(微惯性测IDC和Coxl)E配算法组合策略(R-1-C激光扫描匹配算法),来用Kalman(卡尔曼)滤波器通行激光/MIMU融合定 量组合)组成微小型飞行器的室内导航系统:结合留点(Residue Points) Cox和IDC算法转点,设计了R-1-C(留点、位;最后,在以ARM为核心效理器的硬件平台上进行定位仿真.仿具始果表明:R1C算法满足设计要求;激光/MIMU敬合定位方法其有良好的性能,能实现强小型飞行器的室内高精度定位.
关键词:懂性导航系统;室内定位;激光测距仪;扣据区配:卡尔受选波器
文章编号:1674-5620(2013)04-0340-05
DOI:10 7642/j issn 1674-5620 2013-04-0340-05
MAVIndoorLaser/INSFusionLocationMethod
WANG Ping LU Ke'-" CAO Yunfeng² WANG Biao
(1. College of Automation and Enginring Nanjing University of Aeronutis and Astronautis Nanjng 210016;3 Aesemy of Frntier Seiene Nanjing Univerity of Aeroutics and Astronautics Nenjing 210016) 2. AVIC Helicopter Research and Development Institute Jingdezhen Jisngxi Province 333001;
Abstraet: An indoor navigation system is built with MIMU ( Micro-Inertial Measurement Ueit) asisted by an external sen-sor of a 2D laser range finder for MAV's (Micro Air Vehicles) because a laser finder has high accuracy and anti-interlerencealgorthms. Kalman fiter is used in laser/MIMU fusion localiation Finally the algorithm is transplanted to an ARM performance R-I-C laser scan matching algorithm is designed incorporating the features of residue points Cox and IDChanware plstfom and siulation shows tht R-1C algoithm mets design equirements and with exceent performncelase/MIMU fuzion localization faclitates high-acuracy indoor location for MAVs
Keywords: Interial Navigation System (INS) : indoor locations laser range finder scan mstching: Kalman filter
度定位方法是目前研究的热点.
0引言
在室内环境下,由于不能获取GPS(全球定位系统)信号,导航一般通过INS(惯性导航系统)实现,考虑到体积和成本等因素,微小型飞行器上采用MIMU(微惯性测量组合)来实现INS的功能.但是,由于其建立在积分原理基础上,随着时间的增加,测量误差必然不断积累并发散.这种误差的积 累导致定位精度的降低对室内飞行来说不能容忍,而且MIMU只能测量飞行器的自身姿态信息,不能获取距离等外部数据,因此需要选用合适的外部传
微小型飞行器或小型机器人具有机动灵活、便于携带、成本低廉且不受辐射等环境影响的特点,是 室内监测的有效平台.但微小型飞行器或小型机器人安装空间小,承载能力弱,一般要求传感器的器件数量少、体积小、重量轻、价格低,伴随面来的问题是传感器精度低,不能满足室内近距离定位、导航的需求.另外室内环境空间小,障碍物多等特点 要求微小型飞行器必须具有高精度的自主导航能力.所以为微小型飞行器设计一种室内近距离高精
感器提供外部相关信息来修正MIMU的测量误差,辅助其实现定位:
工学院的Abraham Bachrach等人合作研制了一个其 慕尼黑工业大学的MarkusAchtelik和麻省理利用LRF(激光测距仪)和立体视党的四旋翼飞行器,能够在给定机构化环境中同时实现定位和建图,本文采用1个二维LRF和1个IMU(惯性测量组合).解决了MAV(微型飞行器)在多边形室 内环境中的高精度定位问题:针对常用的几种激光扫描匹配算法的不足,提出了R-1-C匹配算法组合策略,在减少计算量的同时,提高定位精度;并在激光/MIMU融合定位中采用卡尔曼滤波技术实现激成其硬件设计.通过实验验证,该方法可以实现室内精确定位.
1基于激光测距仪的室内定位方案
基于光测距仪的室内精确定位方法就是通过激光扫描匹配算法对激光测距仪测量的扫描数据进 行解算处理,得到微小型飞行器在建立的导航坐标系中的位置和方位信息.其关键技术就是激光扫描匹配算法,该算法的主要功能就是对激光测距仪的2组算,得到当前扫描下的状态位置和方位信息,与估算常用的匹配算法主要有:ICP算法,由Besl和的参考值进行对比,得到微小型飞行器的精确位置.
1.1激光测距仪定位原理
二维散光测距仪扫描得到1组有限的距离数据序列,该组距离数据序列就称为一次扫描,用S表表测量的方向角.因此,一次扫描可以用S={D,D,",D来表示.
图)二维激光测距仪数据扫描几何图Fig . 1 Data scanning gometry of a 2D laser range finder
二维激光测距仪得到的激光扫描数据在极坐标下的示意图,可以用激光一次扫描的点集来表示
其中D为二维激光测距仪在第:次扫描时得到 的数据;p为第i次扫描中第j个扫描点到激光测距仪的距离数据;a十a,为第i次扫描中第j个扫描点相对于导航坐标系中的角度数据,.为二维激光测距仪扫捕起始角度,入为二维激光测距仅的扫描分辨率:N为每次扫描点数,可见,二维激光测距 仪进行激光扫描的数据格式就是在极坐标下的一组点集,一次扫描中的每个激光扫描点都是用激光测距仪到障碍物间的距离及其对应的角度在极坐标中
本系统中激光定位的功能主要是通过激光扫描匹配算法对当前扫描S,和参考扫描S,以及相对应 的扫描状态--当前扫描状态P.和参考扫描状态P,进行处理,得到激光测距仪在导航坐标系中的位置和航向信息,由此可知,二维激光测距仅在激光定位系统中的主要功能是给本系统提供当前扫描数 据,即二维激光测距仪在导航坐标系中某参考点上的一组扫描数据,这些数据就是微小型飞行器到各障碍物之间的一组距离的点集.
扫描数据,即当前扫描和参考扫描,进行一系列的运1.2R-1-C激光扫描匹配算法
McKay于1992年首次提出;IMRP算法和IDC算法,由FengLu提出;Cox算法,由LJ.Cox提出,以图像匹配为背景.
ICP算法是IDC算法的基础,适用于非多边形过程如图I所示.二维激光测距仪在每个角度测得算法在标准ICP算法的基础上,增加了MRP对应 示,其相当于一个点集.二维激光测距仪数据扫描环境,但收效速度慢,且对转动的辨识能力差,IDC的数据就相当于点集中的一个点,用D.(r,0.)表规则,使得算法能更加精确地求取转动量,同时算法示,其中代表二维激光测距仪测得的距离.0.代收效速度大大提高.Guttmann等人引人点云重心后,计算量也大大降低.Cox算法是点一线对应算法,其计算速度和精度都相当可观,但是要求环境中点一点对应算法,但是在对应规则中引人该点处的 必须具备足够的线特征.RS/LS本质上也是一种切线作为判断依据,该算法具有全局搜索转动量的能力,但由于是一维搜索且距离函数不平滑,其精度比较低,
本文整合上述算法思想,引入角点匹配算法和基于留点的ICP算法,设计了一套更加完善的匹配算法组合策略,由于该组合匹配算法主要应用了留点(ResiduePoints)、IDC算法和Cox算法,称之为
R-1-C匹配算法组合策略,或R-I-C激光扫描匹配算
法,简称R-1-C算法,算法原理如图2所示.
图2R-I-C算法原理框图Fig. 2 Block diagram ef R-1-C algorithm
当线段特征足够时采用Cox算法,否则采用IDC算法,利用留点或者RS/LS算法为它们提供较准确的估计值,由于Cox算法具有处理全局转 动的能力,故当留点数目太少的时候,不为Cox提供估计值.面IDC算法不具备处理全局转动的能力,因此须为其提供一个较准确的估计值,当提取的切线多于留点数的时候采用RS/LS算法,否个估计值. 则采用角点或留点匹配算法,为IDC算法提供一
Fig 3 Structure of laser/MIMU fusion location 围3激光/MIMU融合定位结构图
R-1-C组合策略虽然引人了众多已有算法,并设定了多个判定环节,但对于某一具体环境下的2组扫描器,实际调用的算法只有1~2种,在调用2种算法的情况下,首先被调用的算法往往不需要高 精度的解,所以选代次数可以设定为较小的固定值.算法耗时主要集中在Cox算法和IDC算法,但Cox算法自身速度足够快,进行改进后需要匹配的点更少,速度更快.
2.1滤波器系统状态方程
下18个系统状态 系统状态方程就是各个误差方程,本文选取以
8LaV.VttoEVV8LbEMIMU速度误差:、p为数学平台误差角:c 其中8L、8A为MIMU位置误差:8V.、8V.为n为陀螺常值漂移:t、、c为陀螺随机误差:7..V.为加速度计常值零偏:BL、ba、a为激光扫描纬度、经度、航向误差.
2激光/MIMU融合定位Kalman(卡 尔曼)滤波器设计
系统的状态方程为
Kalman滤波实质上是一种递推线性最小方差滤波方法,它不要求储存过去的量测值,只要根据当时的量测值和前一时刻的估计值,就可以实时地计算出所需信号的估计值.
其中 X(r)18x1为系统状态矩阵;F(t)x为系统矩阵:G(r)sx为系统噪声矩阵;W(t)ux为系统噪声.
目前在飞行器的惯导系统中,一般都采用间接法、校正的Kalman滤波器,滤波周期一般为儿秒到1min.本文采用间接法滤波,滤波器结构如图3所示.
2.2滤波器量测方程
连续Kalman滤波器的量测方程为
(1)
图5所示,实线表示飞行器实际位置,折划线表示单纯MIMU定位曲线,点划线表示激光/MIMU融合定位给出的位置,由图可见该融合定位方法能很好地跟踪定位,200s仿真时间内定位误差不超过5 cm.由于短时间内MiMU给出的航向与实际航向误差较小,故该融合定位方法得到的航向误差也很小,在200s仿真时间内小于0.3°,
其中Z(t)为量测值;H(t)为量测矩阵;N(t)为系统的量测噪声,本文考虑为白噪声.
本文研究中所取的量测值为激光扫描匹配算得的位置和航向误差同MIMU捷联惯导解算的位置 和航向误差之差.通过式(1)求得的位置和航向误差之差实际上是两者的位置和航向之差.若将激光扫描匹配求得位置和航向作为真实值,则量测值可以理解为位置和航向的误差,即
则量测矩阵H为
3激光/MIMU融合定位仿真
3.1激光/MIMU融合定位算法软件仿真
本文仿真中,设定飞行器在某一水平面内飞行,其航迹为椭圆,长半轴为4m,短半轴为2.5m
图5激光/MIMU融合定位效果围Fig 5 Illustration of laser/MIMU fusion location effect
可见,位置误差随着时间推移呈指数增长,形象地反 图4为MIMU系统仿真误差示意图.从图中映了惯性导航系统误差的积分特性.当仿真进行200s时位置误差接近2m.这对尺度通常为10m左右的室内环境来说是不能容忍的.由于采用四元数法进行姿态解算,故航向角误差没有呈现指数增 长形式,面是呈周期振荡.
3.2激光/MIMU融合定位算法硬件实现
以ARM为核心的数据采集系统具有成本低、功能丰富、开发简单、处理能力强、功耗低等优点,还 可以加载嵌人式操纵系统,能引领数据采集系统中的数据采集部分向强实时、多参数、高精度的方向发展:数据存储部分向大容量、微型化、使携式的方向发展;数据传输部分向多通信方式、远距离数据传输的方向发展.
本设计选用友善之臂的mini2440.它采用三星S3C2440微处理器,并采用专业稳定的CPU(中央处理器)内核电源芯片和复位芯片来保证系统运行时的稳定性.开发板配有64MbyteSDRAM(同步1个USB(通用串行总线)Host,1个USBSlaveB 动态存储器),256Mbyte NandFlash,3个串行口,型接口,1个SD卡存储接口,还据有较为丰富的扩展接口--1个34针的GPIO接口和1个40针的系统总线接口,并且整体尺寸只有100mm×100mm.其不仅在硬件接口和存储能力 上能满足需要,面且质量和体积也符合微小型的设计理念.
图4MIMU系统仿真误差示意图Fig. 4lilustration of MIMU system simulation error
本系统实验主要采用各个硬件模块搭建微小型飞行器室内精确定位系统实验的硬件平台、该硬件
采用激光/MIMU融合定位方法的仿真结果如
[2]苏立.室内环境下移动机器人双目视觉SLAM研究[D].西安:西安理工大学,2010.Suli. Research on the binocular vision SLAM for mobile rohot in indoor environment[D]. Xi' an: Xi' an University of Tech-nology .201ocIn Chinese) [3]由晓需,基于ARM-Linux的空中机器人嵌人式飞控系统实 现研究[D].南京:南京航空航天大季,2002.Qu Xisolei. The research oe develiopment of embedded aerialrobot ffight control system based on ARM-linux[D]. Nan- jing: Nanjing University of Aeronautics and Astronautics 2002(In Chinese) [4]王柳文.微型飞行器自主控制系统的研究及设计[D].南京: 南京航空航天大学,2002.Wang Liuwes The study and denign of sutonomous fightcontrol system for micro seral vehicle[D] Nanjing: Nanjing University of Aeromauties and Aatronsutirs 2002 ( In Chi-nese).[ 5 ] Achteik M Bachrach A.He Ruije et al. Stereo vision and laser odometry for sstonomous helicopters in GPS-denied in-door environments[C]/ SPIE Procredings of SPIE Orlndo[6 ] locchi L Pellegrini S Tipaldi G D. Building multi-level pla 2009 733219-10.nar maps integrating LRF stereo vision and IMU semso[C]/IEEE Intermational Workshop on Salety Security and Res cue Robotics. Rome. 2007:1-6.[ 7 ] Hesch J A Mirsaei F M Mariottini G L etal. A lser-sidedinertial navigation system (L-INS) for human localiatiom in unknown indoor environments[C]// IEEE Intermational Con-fere cn Roboties and Atmtios. Anchorge -[ 8 ] Besl P M MeKay N D. A method for regisrwtion of 3-D 5382.shapesEJ]. IEEE Trsnsactions on Patterm Analysis and Ma-chine lotelligence 1992 14(2) ; 239-256.
平台搭建的基本思路是将友善之臂mini2440开发板的3个UART串口模块通过相应的接口通讯模块的转换,与输人接口模块(二维激光测距仪和再通过设计的电源模块完成各硬件组件的供电,该 IMU)和输出接口模块(PC机)进行数据通讯连接,系统实验的整体硬件平台示意图如图6所示.
图6系统整体硬件平台示意围Fig. 6Hardware platform setup
4结论
仿真结果证明,本文提出的激光/MIMU融合定位方法具有良好的性能.平移定位精度可达到屋 米级,航向定位精度小于1°,满足室内高精度定位的要求,
参考文献(References)
[1]张梁,曹云峰,王彪,等,基于Kalman游波器的室内激光/ INS融合定位方法[J3.信息与电子工程,2012,10(4):43-440.Zhang Liang Cao Yunfeng Wang Biao et al. Kalman filser “[1 oμaool xoopu oj qeade uosag SNI/asq| uo pieqInformation and Electronic Engineering.2012.10(4):436-40(In Chinese)
(本文编辑:吴丽娟)