四旋翼无人飞行器的室内定位、导航与建图

方登建，戴宇进，赵红超，康宇航

（海军航空工程学院a.7系；b.研究生管理大队，山东烟台264001）

四旋翼无人飞行器的室内定位、导航与建图

方登建a，戴宇进a，赵红超a，康宇航b

（海军航空工程学院a.7系；b.研究生管理大队，山东烟台264001）

根据室内环境半结构化的特点，通过激光扫描仪采集的数据得到特征点与特征线段，从而建立出室内环境的特征地图。利用特征点实现四旋翼无人飞行器在室内对自身的定位，同时根据现阶段特征点的个数制定导航算法，当四旋翼无人飞行器飞到下一位置时，再次建立局部地图，与此前建立的全局地图进行融合，实现全局地图的更新。

四旋翼无人飞行器；定位；导航；建图

四旋翼无人飞行器通过自身携带的激光测距仪获得室内未知环境中的距离信息，增量式地构建室内地图，同时利用所构建的室内地图实现定位，并用所获得的距离信息与所构建的室内地图进行导航。该研究始于上世纪80年代中期[1]，目前已有多种解决此问题的方法，主要包括非概率方法和基于概率估计方法，其中基于概率估计方法为主流[2-3]。

本文针对室内半结构化环境特征线段与特征点比较多的特点，制定了基于特征地图的定位、导航与建图方法。

依靠自身携带的激光扫描仪，四旋翼无人飞行器能够将室内环境中的数据采集；再通过特征检测算法（加权最小二乘算法、哈夫变换、小波变换）将特征点与特征线段提取出来，根据特征点的个数确定导航点与导航策略，并根据特征线段制定相应的避障策略；特征点与特征线段是几何地图的基本组成要素，利用提取出的特征点与特征线段可以将局部地图创建出来，结合已经建立起来的全局地图进行特征匹配，根据匹配后的结果将全局地图进行更新，实现对四旋翼无人飞行器的自定位，定位、导航与建图算法结构图如图1所示。

图1 定位、导航与建图算法结构图Fig.1 Algorithm structure of localization, navigation and mapping

1 环境地图表示方法

四旋翼无人飞行器在未知环境中工作首先要解决定位问题。而定位问题与创建地图是交织在一起的，这就是同步地图创建与定位——SLAM问题[4-6]。地图创建的精确性将直接影响四旋翼无人飞行器定位的精度与无人飞行器后续导航工作的好坏。

目前环境地图的表示方法主要有3种：栅格地图、特征地图和拓扑地图。

栅格地图是由Moravec和Elfes提出，将整个环境分成若干个大小相同的小栅格，根据每个小栅格中是否有障碍物来创建地图。这一方法的优点是：①地图的创建与维护相对简单；②可以很方便地根据实际需求来更改地图精度；③地图模型比较简单，就是二维数组。缺点是：①当未知环境比较大且地图的精度要求比较高时，地图的维护将会变得非常难；②当栅格划分比较细时，对处理器的要求非常高，实时性一般不好。

几何地图是根据机载传感器采集的数据，从中提取抽象的几何特征，比如线段、点或者圆，通过这些几何特征来描述环境。这一方法的优点是：①信息量小，便于存储；②实时性比较高，对处理器要求也没有那么高；③可以将多种传感器的数据融合在一起使用。缺点是：①各种传感器采集的数据需要处理后才能一起使用；②不适合太复杂的环境；③在小环境的地图中，地图的精度可能不够。

拓扑地图是将环境表示成一张拓扑图，图中的节点对应于环境中的一个特征状态，节点间的连线表示2节点直接是相通的。这一方法的优点是：①比较适合做大范围航路规划的任务；②信息量最小，非常容易存储；③对处理器要求最低，可以使用很多比较成熟的搜索算法。缺点是：①地图精度最低；②容易出现误匹配的情况；③在非结构环境中，这种方法很可能失效。

文献[7-8]采用栅格地图对机器人进行定位导航，虽然精度比较高，但是仅适合小环境。而本文针对的是面积适中的室内环境，如果使用栅格地图很难满足实时性的要求。

文献[9]采用了拓扑地图对移动机器人进行定位导航，但该方法对于结构性很强的室内环境非常容易造成误匹配（尤其是线段特征），所以最终选择用激光测距仪采集的数据来建立几何特征地图，实现无人飞行器的定位与导航。

2 四旋翼无人飞行器的自定位

四旋翼无人飞行器是在三维空间中运动，如果需要描述其精确位姿，需要用(x,y,z)描述其位置，用(ψ,θ,φ)描述其在三维坐标中的姿态，其中：ψ为四旋翼无人飞行器的滚转角；θ为四旋翼无人飞行器的俯仰角；φ为四旋翼无人飞行器的偏航角。

本文搭建的四旋翼无人飞行器的IMU并不能提供加速度、角速度这些信息，仅仅能通过激光测距离提供距离信息。

在室内未知环境飞行时，无人飞行器的高度保持不变，主要是在二维平面内运动。结合上述特点，可以用三维状态向量(x,y,γ)来描述四旋翼无人飞行器的位姿信息，其中(x,y)为无人飞行器在所飞行平面内的坐标，γ为四旋翼无人飞行器的朝向，即相对于起飞时偏转的角度。

四旋翼无人飞行器所采用的定位方法与传感器密切相关。文中选用激光测距仪和磁力计，但是激光测距仪和磁力计都是安装在四旋翼无人飞行器的中心，用这2个传感器测得的数据都是基于机体坐标系OrXrYr的，若是想要建立全局地图的话，还需要选定参考点建立全局坐标系OXY。

本文选起飞点作为全局坐标系OXY的原点O，四旋翼无人飞行器起飞点至窗户中线的连线方向为Y轴，向前为正，以垂直于Y轴方向为X轴，向右为正。机体坐标系OrXrYr的原点Or为四旋翼无人飞行器的中心，旋翼1与旋翼2之间的中线为Yr轴，机头方向为正，以垂直于Yr方向为Xr轴，向右为正。建立四旋翼无人飞行器飞行示意图如图2所示。

图2 飞行示意图Fig.2 Schematic diagram of flying

其位姿方程为：

式（1）～（5）中：k=1,2,…,n,i=1,2,…,n；xk、yk和xk-1、yk-1分别为k时刻和k-1时刻四旋翼无人飞行器在全局坐标系的坐标；分别为k时刻和k-1时刻四旋翼无人飞行器与第i个特征点在机体坐标系中的横坐标和纵坐标；Δdk为k时刻到k-1时刻四旋翼无人飞行器的位移；φk为k时刻四旋翼无人飞行器航向角（此航向角是相对于全局坐标系OXY的航向角）；φk-1为k-1时刻四旋翼无人飞行器航向角；αk-1为k时刻和k-1时刻的航向角之差；γk为k时刻四旋翼无人飞行器相对于起点方位偏转的角度；γk-1为k-1时刻四旋翼无人飞行器相对于起点方位偏转的角度。

此外，对于室内环境的特征点，主要包括由墙到门（由墙到走廊）的突变点、由门到墙（由走廊到墙）的突变点、内墙角特征点，在该环境中这些特征点相对于全局坐标系是静止不动的量，其在全局坐标系中的坐标表示如下：

式（8）、（9）中，R为转移矩阵[10-11]。

四旋翼无人飞行器以起飞点为原点，在起飞点检测特征点并得到其坐标（在这个时刻特征点相对于全局坐标系的坐标与机体坐标系的坐标是一致的），以此对其自身进行定位，在飞行的过程中，检测出环境中的新特征点并得到新特征点相对于机体坐标系OrXrYr的坐标，通过无人飞行器自身的坐标，计算出新特征点相对于全局坐标系OXY的坐标，然后再以新特征点来对无人飞行器进行定位，检测后续的特征点，以此实现其自定位。

3 四旋翼无人飞行器的导航

所谓导航，就是把飞机、航天器、舰船等航行体按预先制定的计划和要求，从一个地方引导到目的地的过程[12]。本文采用的导航方法是基于激光测距仪的视线导航，导航点通过特征检测的特征点计算得出。

激光测距仪扫描范围是-45°到270°，通过其采集的数据能够提取出线段特征与点特征（内墙角点、外墙角点），当四旋翼无人飞行器飞行时，激光测距仪能够找到左右两侧的特征点，利用该特征点无人飞行器能够对自身进行定位。

1）在机体坐标系下，当k时刻在四旋翼无人飞行器的前方（Xr为正的方向）出现至少2个特征点时，从这些特征点中找出xr坐标值最小的2个特征点，将这2个特征点的中值作为导航点。

2）当k时刻在四旋翼无人飞行器的前方出现1个特征点，且其后方出现至少2个特征点时，则无人飞行器需要找出后方距离最近的2个特征点，算出2个特征点的中值，将其作为导航点。

3）当k时刻在四旋翼无人飞行器的前方出现1个特征点，且其后方出现的特征点少于2个时，此时仅以前方的特征点对四旋翼无人飞行器进行定位，控制无人飞行器沿着平行于走廊两侧的墙飞行，直至前方出现2个特征点后再计算导航点。

4）当k时刻在四旋翼无人飞行器的前方没有出现特征点，且其后方出现的特征点为1个时，此时仅以后方的特征点对四旋翼无人飞行器进行定位，控制无人飞行器沿着平行于走廊两侧墙飞行，直至出现2个特征点后再计算导航点。

4 地图创建

四旋翼无人飞行器的定位与导航是同步进行的，需要作为统一的过程来处理。当无人飞行器的激光采集一次数据处理后，提取特征点和特征线段，构建局部地图。构建的局部地图需要与已经构建的全局地图进行特征匹配，合并特征点与特征线段。无人飞行器根据合并后的特征点实现定位，这样在完成其定位的同时也把未知环境的全局地图进行了更新，建立新的全局地图。

本文只是使用激光测距仪来完成地图的创建，通过特征检测可以将每一组激光扫描数据中的特征点和特征线段提取出来，这样将能建立起局部地图。而定位与建图的关键在于将每一时刻建立的局部地图与全局地图进行特征匹配，完成全局地图的更新。

4.1 点特征匹配

对于特征点的匹配，本文采用最小距离法则。假设在k-1时刻检测出m1个特征点，为其坐标，而在k时刻检测出m2个特征点为其坐标，对于k-1时刻的特征点，依次计算k时刻所有特征点与k-1时刻特征点的距离，当满足如下条件时，表示相邻时刻检测出的特征点为同一个特征点。

式中：i=1,2,…,m1;j=1,2,…,m2；disvalue是设定的一个门限值，通过实验得到。

如果出现多个特征点满足上述条件，则距离最近的点为同一特征点。特征点的每一次匹配都是在分类之后进行，即都是在由墙到门（由墙到走廊）的特征点、由门到墙（由走廊到墙）的特征点、内墙角点这些特征点内部进行特征点匹配，不会出现外墙角点与内墙角点进行特征点匹配。对于不满足上述条件的k时刻的特征点，则将其作为新出现的特征点，加入到特征点集中。特征点的匹配更多的是为四旋翼无人飞行器的自定位提供信息，在确定相邻两次特征点为同一个特征点时，才能根据特征点变化的距离而确定其位姿。

4.2 线段特征匹配

线段特征是构成地图的主要元素，全局地图更新的准确度取决于线段匹配的好坏。假设全局地图共有m3条特征线段，记为L={l1,l2,…,lm3}，而局部地图共有m4条特征线段，记为G={g1,g2,…,gm4}，L与G构成了匹配空间，线段特征由斜率k、截距b，线段起点坐标、线段终点坐标构成，且先线段特征都已经转换在了全局坐标系下表示，不需要再进行坐标转换。

qij=(li,gj)表示线段特征li与gj的相似度，即表示li与gj为同一条线段的可能性，影响线段相似度的匹配条件主要有：

1）同向性。线段特征li与gj之间的夹角小于某一阈值。

式中：θli与θgj分别为线段特征li与gj的倾角；δθ为阈值。

2）共线性。原点到线段特征li与gj的距离相近，且线段特征li的中心点到gj的距离小于某一阈值。

式（14）、（15）中：bli与bgj分别为线段特征li与gj方程的截距，kli为线段特征li的斜率，δb与δd都是阈值。

3）一致性。线段特征li与gj中心点之间的距离不能大于线段特征li与gj长度之和的一半。

式中，dli与dgj分别为线段特征li与gj的长度[13-14]。

对以上3个条件中的4个因子分别赋予不同的权重wl1、wl2、wl3、wl4，并且wl1〉wl2〉wl3〉wl4，qij等于以上3个条件乘以权重因子的值[15-16]。

由于3个条件的量纲不一样，需要对各个条件进行归一化处理。

建立一个m3×m4的相似矩阵Q，Q中的元素由qij组成，其中m3与m4分别表示线段特征li与gj的数目。qij越大，则线段特征li与gj越相似，两者也越可能是同一条线段。找出相似矩阵Q中的最大值qab，其所对应的2条线段匹配成功；将上一次相似矩阵中最大值所对应行（第a行）的所有元素与列（第b列）的所有元素去除，组成新的矩阵Q1，找出新的相似矩阵Q1中的最大值，继续匹配，直到匹配完全局地图中的所有线段。

如果在线段的匹配过程中，某一行（某一列）出现多个最大值，说明全局地图中有一条线段与局部地图中的多条线段相似（全局地图中有多条线段与局部地图中的某一条线段相似），此时需要比较影响相似度值的各个条件，依照各个条件权重的不同依次比较匹配条件。首先，比较同一行或者同一列中拥有相同相似度值元素的越大则2条线段越可能是同一条线段，匹配成功；若是也相等，则再依次比较它们的，直到匹配成功。

在整个线段特征的匹配过程中，越来越多的线段被匹配，使得剩余元素的匹配值qij越来越小，这样可能不是同一条线段的2条线段也进行了匹配，这就出现了误匹配。为了防止这一情况，可以设定1个匹配阈值δmat，当相似度值小于匹配阈值δmat时，匹配终止，而局部地图中剩余未匹配的线段则作为新的线段特征加入到全局地图中。匹配阈值越大，则匹配出来的线段越准确，但是由此可能出现局部地图中与全局地图中的同一条线段未进行匹配，而将局部地图中的线段特征作为新的线段特征加入到全局地图中的情况；而匹配阈值越小，局部地图中将有越多的线段特征被匹配，随之带来的误匹配的概率也越大。

对特征点的匹配可以实现四旋翼无人飞行器的定位，同时对特征点与特征线段的匹配能够更新全局地图，实现无人飞行器未知环境中的建图。在图3所示的比赛环境中，对四旋翼无人飞行器进行定位、导航与建图实验。通过特征匹配建图，在比赛过程完成了室内环境地图的建立，如图4所示。四旋翼无人飞行器在起飞之前先进行起始点的定位，起飞后，从左上角的入口进入室内，通过寻找室内环境中的特征点（图中的拐角点）确定导航点，同时将室内局部地图构建出来，与全局地图进行特征匹配；根据构建出来的局部地图再对其进行自定位。图4中四旋翼无人飞行器从起飞点出发，在室内进行定位导航，进入右下角的房间的时候实现了室内环境地图构建。地图建立结果与实际地图吻合，证明了本文所用方法的可行性与准确性。

图3 国际空中机器人大赛比赛场地示意图Fig.3 Competition arena schematic diagram of international aerial robotics competition

图4 室内环境地图Fig.4 Indoor environment map

5 结论

本文首先建立了机体坐标系与全局坐标系，在机体坐标系下无人飞行器能够运用特征检测算法将激光测距仪采集数据中的特征点提取出来，结合四旋翼无人飞行器的方位信息，实现无人飞行器的自定位；针对室内环境特征点个数的情况，制定了四旋翼无人飞行器的导航策略与避障策略，并设计了利用2个同向特征点来确定导航点的导航算法，使其能够安全、准确地导航；最后，通过特征点与特征线段的匹配得到了地图，实验结果证明了此方法的可行性。

[1] SMITH R，SELF M，CHEESEMAN P.A stochastic map for uncertain spatial relationships[C]//Proceedings of the 4th International Symposium of Robotics Research.California：MIT Press，1987：467-474.

[2] THRUN S，BURGARD W，FOX D，et al.Probabilistic Robotics[M].Cambridge：MIT Press，2005：248-256.

[3] DURRANT-WHYTE H F，BAILEY T.Simultaneous localization and mapping：part I[J].IEEE Transactions on Robotics and Automation，2006，13（2）：99-108.

[4] FILLIAT D，MEYER J A.Map-based navigation in mobile robots：I.a review of localization strategies[J].Cognitive systems Research，2003，4（4）：243-282.

[5] THRUN S.Robotic mapping：a survey exploring artificial intelligence in the new millenium[M].San Francisco：Morgan Kaufmann.2002：1-25.

[6] MEYER J A FILLIAT D.Map-based navigation in mobile robots：II.A review of map-learning and path-planning strategies[J].Cognitive Systems Research，2003，4（4）：283-317.

[7] MORAVEC H.Sensor fusion in certainty grids for mobile robots[J].AI Magezine，1988，34（2）：61-74.

[8] GRZONKA S，GRISETTI G，BURGARD W.Towards a navigation system for autonomous indoor flying[C]//Proceedings of IEEE International Conference on Robotics and Automation.Kobe，2009：2878-2883.

[9] 石朝侠，洪炳镕，周彤，等.大规模环境下的拓扑地图创建与导航[J].机器人，2007，29（5）：433-438. SHI CHAOXIA，HONG BINGRONG，ZHOU TONG，et al.Navigation based on topology map in large-scale environments[J].Robot，2007，29（5）：433-438.（in Chinese）

[10] 陶辉，吴怀宇，程磊，等.基于特征地图的移动机器人EKF-SLAM和FastSLAM算法自主导航研究[J].北京联合大学学报：自然科学版，2010，24（2）：18-24. TAO HUI，WU HUAIYU，CHENG LEI，et al.Research on EKF-SLAMand FastSLAMautonomous navigation algorithms of mobile robots based on feature maps[J]. Journal of Beijing Union University：Natural Science，2010，24（2）：18-24.（in Chinese）

[11] JOSE GUIVANT，EDUARDO NEBOT.Optimization of the simultaneous location and map building algorithm for real time implementation[J].IEEE Transactions on Robotics and Automation，2001，17（3）：242-257.

[12]张国良.组合导航原理技术[M].西安：西安交通大学出版社，2008：1-10. ZHANG GUOLIANG.Combination navigation principles and techniques[M].Xi'an：Xi'an Jiaotong University Press，2008：1-10.（in Chinese）

[13] IP Y L，RAD A B，CHOW K M，et al.Segment-based map building using enhanced adaptive fuzzy clustering algorithm for mobile robot applications[J].Journal of Intelligent and Robotic Systems，2002，35（3）：221-245.

[14] GE KUNFENG，CHEN WEIDONG.Scan matching using line segment relationships for map-based localization of mobile robot[J].Journal of Harbin Institute of Technology：English Edition，2008，15（6）：757-761.

[15] 郑宏.移动机器人导航和SLAM系统研究[D].上海：上海交通大学，2007：32-41. ZHENG HONG.Research on mobile robot navigation and SLAM[D].Shanghai：Shanghai Jiaotong University，2007：32-41.（in Chinese）

[16] 熊蓉.室内未知环境线段特征地图构建[D].杭州：浙江大学，2009：100-105. XIONG RONG.Line features map building in indoor unknown environment[D].Hangzhou：Zhejiang University，2009：100-105.（in Chinese）

Indoor Location,Navigation and Map Building of Four Rotor UAV

FANG Deng-jiana,DAI Yu-jina,ZHAO Hong-chaoa,KANG Yu-hangb
（Naval Aeronautical and Astronautical University a.No.7 Department; b.Graduate Students'Brigade,Yantai Shandong 264001,China）

According to semi-structured indoor environment characteristics,obtaining the feature point and line segments from data points collected by the laser scanner,and then the indoor environment feature map was established.Making use of feature points to realize four rotor UAV self-location indoor,while developing navigation algorithm based on the number of feature points at this stage.When the four rotor UAV fly to the next position,a local map was established,and it was integrated with the established global maps,to achieve the global map updates.

four rotor UAV;location;navigation;map building

V249.122

A

1673-1522（2014）04-0345-06

10.7682/j.issn.1673-1522.2014.04.010

2014-03-20；

2014-05-10

国家自然科学基金资助项目（61174031）

方登建（1977-），男，讲师，硕士。