第14卷第4期 智能系统学报 Vol.14 No.4 2019年7月 CAAI Transactions on Intelligent Systems Jul.2019 D0:10.11992/tis.201803031 网络出版地址：http:/kns.cnki.net/kcms/detail/23.1538.TP.20190109.1422.002.html 基于改进D*算法的无人机室内路径规划 张飞，白伟2，乔耀华3，邢伯阳2，周鹏程 (1.山东鲁能智能技术有限公司，山东济南250101；2.北京理工大学自动化学院，北京100081；3.国网山东省 电力公司，山东济南250000,4.昆明北理工产业技术研究院有限公司，云南昆明650000) 摘要：针对多旋翼飞行器室内无GPS信号时的导航问题，本文采用二维码阵列构建室内定位系统，基于改进 D*算法实现无人机室内路径规划，从而实现飞行器在室内的自主导航和避障。基于AUco二维码设计了地面 阵列为无人机提供了全局精确定位信息，使用改进D*算法保证了无人机在飞行过程中能自主进行路径规划和 飞行。通过设计实验对改进D*算法进行了数值仿真验证，并在实际无人机的飞行中应用。实验结果证明：所 提改进算法较传统D*算法能更好地保证无人机的飞行安全，同时基于二维码阵列的定位方式不但具有较高精 度同时成本低易于实现。 关键词：无人机；室内定位系统；路径规划；自主导航；避障；二维码数组；ArUco;改进D*算法 中图分类号：TP29文献标志码：A文章编号：1673-4785(2019)04-0662-08 中文引用格式：张飞，白伟，乔耀华，等.基于改进D*算法的无人机室内路径规划机.智能系统学报，2019,14(4)：662-669. 英文引用格式：ZHANG Fei,BAI Wei,QIAO Yaohua,etal.UAV indoor path planning based on improved D*algorithmJ.CAAI transactions on intelligent systems,2019,14(4):662-669. UAV indoor path planning based on improved D*algorithm ZHANG Fei',BAI Wei,QIAO Yaohua',XING Boyang',ZHOU Pengcheng' (1.Shandong Luneng Intelligence Technology Company Limited,Ji'nan 250101,China;2.School of Automation,Beijing Institute of Technology,Beijing 100081,China;3.State Grid Shandong Electric Power Company,Ji'nan 250000,China;4.BIT Industrial Tech- nology Research Institute,Kunming Company Limited,Kunming 650000,China) Abstract:Considering the navigation problem when there is no indoor GPS signal in a multi-rotorcraft,in this study,an indoor positioning system is constructed using a two-dimensional code array,and the indoor path planning of the un- manned aerial vehicle(UAV)is realized based on an improved D*algorithm,in order to realize autonomous navigation and obstacle avoidance of the aircraft indoors.Based on an ArUco two-dimensional code,the ground array was de- signed to provide accurate global positioning information for the UAV.The improved D*algorithm was used to ensure that the UAV can autonomously conduct path planning and flight during flight.Design experiments were carried out to simulate and verify the improved D*algorithm,which was further verified in an actual UAV flight application.The ex- perimental results show that the improved algorithm can better guarantee the flight safety of the drone compared with the traditional D*algorithm.Moreover,the positioning method based on the two-dimensional code array is highly ac- curate,low-cost,and easy to implement. Keywords:UAV;indoor positioning system;path planning;autonomous navigation;obstacle avoidance;QR code ar- ray;ArUco;improved D*algorithm 近年来，多旋翼无人机由于其结构简单、控 组合导航等56，而对于缺乏GPS信号的无人机室 制灵活、成本低廉以及可扩展性强等优点，成为 内导航则常采用视觉导航、激光雷达、组合导航 了人们关注和研究的热点。其中自主导航是 等方法。随着无人机的室内使用场景日益增 实现无人机飞行的最重要内容之一，常用的无人 多，无人机室内导航已成为众多学者研究的热点 机导航方法有：惯性导航、GPS导航、视觉导航、 问题。路径规划是无人机室内导航的重要环 收稿日期：2018-03-20.网络出版日期：2019-0-10. 节，通过路径规划可以实现无人机在复杂室内环 通信作者：张飞.E-mail:821661887@qq.com. 境中避让障碍物、高效飞行
DOI: 10.11992/tis.201803031 网络出版地址: http://kns.cnki.net/kcms/detail/23.1538.TP.20190109.1422.002.html 基于改进 D*算法的无人机室内路径规划 张飞1，白伟2，乔耀华3，邢伯阳2，周鹏程4 （1. 山东鲁能智能技术有限公司，山东 济南 250101; 2. 北京理工大学 自动化学院，北京 100081; 3. 国网山东省 电力公司，山东 济南 250000; 4. 昆明北理工产业技术研究院有限公司，云南 昆明 650000） 摘 要：针对多旋翼飞行器室内无 GPS 信号时的导航问题，本文采用二维码阵列构建室内定位系统，基于改进 D*算法实现无人机室内路径规划，从而实现飞行器在室内的自主导航和避障。基于 ArUco 二维码设计了地面 阵列为无人机提供了全局精确定位信息，使用改进 D*算法保证了无人机在飞行过程中能自主进行路径规划和 飞行。通过设计实验对改进 D*算法进行了数值仿真验证，并在实际无人机的飞行中应用。实验结果证明：所 提改进算法较传统 D*算法能更好地保证无人机的飞行安全，同时基于二维码阵列的定位方式不但具有较高精 度同时成本低易于实现。 关键词：无人机；室内定位系统；路径规划；自主导航；避障；二维码数组；ArUco；改进 D*算法 中图分类号：TP29 文献标志码：A 文章编号：1673−4785(2019)04−0662−08 中文引用格式：张飞, 白伟, 乔耀华, 等. 基于改进 D*算法的无人机室内路径规划 [J]. 智能系统学报, 2019, 14(4): 662–669. 英文引用格式：ZHANG Fei, BAI Wei, QIAO Yaohua, et al. UAV indoor path planning based on improved D* algorithm[J]. CAAI transactions on intelligent systems, 2019, 14(4): 662–669. UAV indoor path planning based on improved D* algorithm ZHANG Fei1 ，BAI Wei2 ，QIAO Yaohua3 ，XING Boyang2 ，ZHOU Pengcheng4 (1. Shandong Luneng Intelligence Technology Company Limited, Ji'nan 250101, China; 2. School of Automation, Beijing Institute of Technology, Beijing 100081, China; 3. State Grid Shandong Electric Power Company, Ji’nan 250000, China; 4. BIT Industrial Technology Research Institute, Kunming Company Limited, Kunming 650000, China) Abstract: Considering the navigation problem when there is no indoor GPS signal in a multi-rotorcraft, in this study, an indoor positioning system is constructed using a two-dimensional code array, and the indoor path planning of the unmanned aerial vehicle (UAV) is realized based on an improved D* algorithm, in order to realize autonomous navigation and obstacle avoidance of the aircraft indoors. Based on an ArUco two-dimensional code, the ground array was designed to provide accurate global positioning information for the UAV. The improved D* algorithm was used to ensure that the UAV can autonomously conduct path planning and flight during flight. Design experiments were carried out to simulate and verify the improved D* algorithm, which was further verified in an actual UAV flight application. The experimental results show that the improved algorithm can better guarantee the flight safety of the drone compared with the traditional D* algorithm. Moreover, the positioning method based on the two-dimensional code array is highly accurate, low-cost, and easy to implement. Keywords: UAV; indoor positioning system; path planning; autonomous navigation; obstacle avoidance; QR code array; ArUco; improved D* algorithm 近年来，多旋翼无人机由于其结构简单、控 制灵活、成本低廉以及可扩展性强等优点，成为 了人们关注和研究的热点[1-4]。其中自主导航是 实现无人机飞行的最重要内容之一，常用的无人 机导航方法有：惯性导航、GPS 导航、视觉导航、 组合导航等[5-6] ，而对于缺乏 GPS 信号的无人机室 内导航则常采用视觉导航、激光雷达、组合导航 等方法[7-8]。随着无人机的室内使用场景日益增 多，无人机室内导航已成为众多学者研究的热点 问题[9]。路径规划是无人机室内导航的重要环 节，通过路径规划可以实现无人机在复杂室内环 境中避让障碍物、高效飞行。 收稿日期：2018−03−20. 网络出版日期：2019−01−10. 通信作者：张飞. E-mail：821661887@qq.com. 第 14 卷第 4 期 智 能 系 统 学 报 Vol.14 No.4 2019 年 7 月 CAAI Transactions on Intelligent Systems Jul. 2019
第4期 张飞，等：基于改进D*算法的无人机室内路径规划 ·663· Stentz-1首次提出了D*路径规划算法，用 利用OPENCV和ARUCO库可以生成使用的 于自主机器人在部分已知或完全未知环境中通过 二维码。首先，通过选择aruco模块中一个预定 局部修正路径，减少计算量，实时获得一条次优 义的字典来创建一个字典对象，具体而言，这个 路径。Hrabar针对使用立体视觉导航的无人机 字典是由250个marker组成的，每个marker的大 提出了一种将D*算法与随机路线图法结合的路 小为(7×7)bit。然而利用语句cv:aruco:drawMark 径规划方法，通过该方法可以实现无人机在未知 er(dictionary,i,200,markerImage,l)生成ID编号 环境下三维空间避让障碍物的路径规划。郑昌 为1的二维码。其中，第1个参数为之前创建的 文]利用稀疏A*算法实现了离线的三维航迹规 字典对象，第3个参数为输出marker的大小，第 划，并通过对稀疏A*算法的动态化实现了无人机 4个参数为输出图像。 遇到未知威胁时的动态轨迹重规划。符小卫等叫 二维码的识别步骤主要有4步： 研究了基于Voronoi图和Dijkstra算法的三次样 1)图像分割：提取灰度图像中最突出的轮 条曲线和序列二次规划的方法。这些研究大多是 廓。采用局部自适应阈值的方法来分割marker 基于D*算法的无人机三维空间避让障碍物的路 (图2(b)。 径规划，未考虑无人机在轨迹移动时如果距离障 2)轮廓提取和过滤：使用Suzuki等1算法对 碍物较近时会发生与障碍物碰撞或者穿越的情 阈值图像进行轮廓提取。它产生一组图像轮廓， 况，不利于无人机室内高精度导航。 其中大部分与检测目标无关（见图2（©）)。然后， 本文以通过增加碰撞因子改进D*算法实现 使用Douglas-Peucker叨算法进行多边形近似。由 了无人机的室内路径规划，并通过布置室内二维 于标记被包围在矩形轮廓中，所以没有近似于 码数组图像对改进D*算法进行了试验，验证了改 4顶点多边形的那些被丢弃。最后，简化了附近 进D*算法对于无人机室内路径规划的适用性。 的轮廓，只留下外部轮廓。图2()显示了从该过 程得到的多边形。 1 ArUco码的识别和位置求解 l.1 ArUco码的识别 本文采用ArUco码作为地面二维标志来设计 定位阵列，ArUco系统1由Cordoba大学AVA 团队设计并提供面向全平台的SDK,ArUco码（下 (a)输入图片 (b)边缘检测结果 文简称二维码)由7×7方块阵列，二维码边框为黑 色其余方块可以是白色(1)或黑色(0)。 四 一个ArUco标记是一种合成的方形标记，由 一个宽的黑色边框和内部的二进制矩阵构成，这 决定了它的标识符(D)。标记的大小决定了内部 (©)四边形检测结果 维码识别结果 矩阵的大小。Aruco码能被当作7×7的布尔居 中，外环被黑色充满，能被图像快速检测 (e)透视度()编码 ArUco码内部数据信息量为(5×5)bit,最大的编码 换和网络化提取 数量为1024个，每一行使用2bit进行编号，使用 图2 ArUco码检测步骤 另外3bit进行纠错能在任意方向识别出唯一码 Fig.2 ArUco code detection steps 号，如图1所示解码结果为(0000100101)即为 3)标记代码提取：分析这些轮廓的内部区域 37号。 以提取其内部代码。首先，通过计算单应性矩阵 标记代码 来消除透视投影（图2(e)。使用Otsu的方法1阁对 二进制 所得到的图像进行阈值化，其提供了图像分布是 00 双峰的（在这种情况下是正确的）最佳图像阈 00 10 01 0 值。然后，二值化图像被划分为规则网格，并且 01 每个元素根据其中的大多数像素的值被分配值 01 0或1（图5）。第1个测试包括检测黑色边框的存 图1No.37 ArUco二维码 在。如果边界的所有位为0，则使用下述方法分 Fig.1 No.37 ArUco QR code 析内部网格
Stentz[10-11] 首次提出了 D*路径规划算法，用 于自主机器人在部分已知或完全未知环境中通过 局部修正路径，减少计算量，实时获得一条次优 路径。Hrabar[12] 针对使用立体视觉导航的无人机 提出了一种将 D*算法与随机路线图法结合的路 径规划方法，通过该方法可以实现无人机在未知 环境下三维空间避让障碍物的路径规划。郑昌 文 [13] 利用稀疏 A*算法实现了离线的三维航迹规 划，并通过对稀疏 A*算法的动态化实现了无人机 遇到未知威胁时的动态轨迹重规划。符小卫等[14] 研究了基于 Voronoi 图和 Dijkstra 算法的三次样 条曲线和序列二次规划的方法。这些研究大多是 基于 D*算法的无人机三维空间避让障碍物的路 径规划，未考虑无人机在轨迹移动时如果距离障 碍物较近时会发生与障碍物碰撞或者穿越的情 况，不利于无人机室内高精度导航。 本文以通过增加碰撞因子改进 D*算法实现 了无人机的室内路径规划，并通过布置室内二维 码数组图像对改进 D*算法进行了试验，验证了改 进 D*算法对于无人机室内路径规划的适用性。 1 ArUco 码的识别和位置求解 1.1 ArUco 码的识别 本文采用 ArUco 码作为地面二维标志来设计 定位阵列，ArUco 系统[15] 由 Cordoba 大学 AVA 团队设计并提供面向全平台的 SDK，ArUco 码 (下 文简称二维码) 由 7×7 方块阵列，二维码边框为黑 色其余方块可以是白色 (1) 或黑色 (0)。 一个 ArUco 标记是一种合成的方形标记，由 一个宽的黑色边框和内部的二进制矩阵构成，这 决定了它的标识符 (ID)。标记的大小决定了内部 矩阵的大小。Aruco 码能被当作 7×7 的布尔居 中，外环被黑色充满，能被图像快速检 测 ArUco 码内部数据信息量为 (5×5)bit，最大的编码 数量为 1 024 个，每一行使用 2bit 进行编号，使用 另外 3bit 进行纠错能在任意方向识别出唯一码 号，如图 1 所示解码结果为 (00 00 10 01 01) 即为 37 号。 二进制 标记代码 00 00 10 01 01 1 1 0 1 1 0 0 1 0 1 1 1 1 1 1 0 1 0 0 0 0 0 0 0 0 图 1 No.37 ArUco 二维码 Fig. 1 No.37 ArUco QR code 利用 OPENCV 和 ARUCO 库可以生成使用的 二维码。首先，通过选择 aruco 模块中一个预定 义的字典来创建一个字典对象，具体而言，这个 字典是由 250 个 marker 组成的，每个 marker 的大 小为 (7×7)bit。然而利用语句 cv::aruco::drawMarker(dictionary, i, 200, markerImage, 1) 生成 ID 编号 为 i 的二维码。其中，第 1 个参数为之前创建的 字典对象，第 3 个参数为输出 marker 的大小，第 4 个参数为输出图像。 二维码的识别步骤主要有 4 步： 1) 图像分割：提取灰度图像中最突出的轮 廓。采用局部自适应阈值的方法来分割 marker (图 2(b))。 2) 轮廓提取和过滤：使用 Suzuki 等 [16] 算法对 阈值图像进行轮廓提取。 它产生一组图像轮廓， 其中大部分与检测目标无关 (见图 2(c))。然后， 使用 Douglas-Peucker[17] 算法进行多边形近似。由 于标记被包围在矩形轮廓中，所以没有近似于 4 顶点多边形的那些被丢弃。最后，简化了附近 的轮廓，只留下外部轮廓。图 2(d) 显示了从该过 程得到的多边形。 (a) 输入图片 (b) 边缘检测结果 (c) 四边形检测结果 (d) 二维码识别结果 (f) 编码 提取 (e) 透视度 换和网络化 图 2 ArUco 码检测步骤 Fig. 2 ArUco code detection steps 3) 标记代码提取：分析这些轮廓的内部区域 以提取其内部代码。首先，通过计算单应性矩阵 来消除透视投影 (图 2(e))。使用 Otsu 的方法[18] 对 所得到的图像进行阈值化，其提供了图像分布是 双峰的 (在这种情况下是正确的) 最佳图像阈 值。然后，二值化图像被划分为规则网格，并且 每个元素根据其中的大多数像素的值被分配值 0 或 1(图 5)。第 1 个测试包括检测黑色边框的存 在。如果边界的所有位为 0，则使用下述方法分 析内部网格。 第 4 期 张飞，等：基于改进 D*算法的无人机室内路径规划 ·663·
·664· 智能系统学报 第14卷 4)标记识别：在这一点上，有必要确定获得 p0S:= ylocal -R'.T 的标记候选哪些属于字典，哪些只是环境的一部 (3) 分。提取标记的候选码之后，就可获得4个不同 根据式(2)可以计算出摄像头当前欧拉角， 的标识符。为了加快这个过程，字典元素使用平 式(3)可以计算出摄像头当前相对某个二维码的 衡二叉树排序。为此，标记由通过连接其所有位 局部位置，然后根据该二维码的全局坐标计算出 获得的整数值表示。 摄像头当前的全局坐标位置。进一步文中基于扩 1.2二维码阵列的设计 展卡尔曼滤波器对二维码位置、光流传感器速度测 对一幅图像中多个二维码进行识别可以计算 量值和机载MU数据进行融合得到最优位姿估计。 出摄像头相对每个二维码的位置和姿态，未被识 别的标志不参与位置解算。同时摄像头能在任意 2改进D*路径规划算法 方向识别出唯一码号，通过在室内合理布置二维 无人机室内全局路径规划是无人机研究领域 码，综合多个标志解算的信息能够提供较大范围 的一个重要课题。路径规划的含义为已知无人机 内的无人机全局位置信息。地标阵列除了使用规 运动的起点和终点，通过机载传感器对周围环境 则排列外，可以通过任意的形式和距离组合，只需 进行描述，得到自由区域和障碍区域，从而规划 要通过人工一次性标定后制表存入机载导航系统 出一条从起点到终点并且避开障碍物的最短路 存储器中即可。文中以最简单的矩阵规则排列形 径。本文在对各种主流路径规划算法比较之后， 式为例，所建立的地面定位阵列（后文简称二维码 基于室内地面二维码阵列标志，选用D*算法2， 阵列)，当确定某二维码为全局坐标系原点后即可 并对D*算法做了改进，实现无人机室内自主飞行。 以通过几何关系计算出其他二维码在全局坐标中 2.1D*路径规划 的位置，进而将摄像头解算出相对某二维码的位 为了在室内环境中寻找最短路径，首先需要 置转换到全局坐标系下。本文根据AUco库，生成 建立环境模型。D*算法采用栅格表示地图，将地 ID编号为1~100的二维码集，设计如图3所示的 图信息记录在每一个栅格上，定义该栅格的属性 二维码地面阵列。其中，单个二维码边长为18cm, 为自由或者障碍。无人机的飞行轨迹被栅格离散 码与码的间距为18cm,ID编号为1的码的中心 化之后，每个栅格上的运动信息表征了无人机在 为原点，行方向为x方向，列方向为y方向。例如 这个栅格上的飞行方向。理论上，无人机在每个 编号为55的二维码位置为(18×8cm,18×10cm)0 栅格上的运动有无数种，但考虑到模型建立的复 杂性，一般在实际运用中只定义8个方向的运动， 分别为：前、后、左、右、右前、右后、左前、左后， 如图4所示。 图3二维码地面阵列 Fig.3 QR code ground array 1.3摄像头位姿估计和全局坐标获取 摄像头经过标定之后，可以解算出二维码相 对于摄像头光学中心的旋转向量r和平移向量 图4栅格地图及无人机可能运动方向 T以及旋转矩阵R四，解算过程如下所示： Fig.4 Raster map and possible direction of movement of 0=norm(r) the drone r=r/0 地图上的栅格采用矩阵方式储存1，单个栅 R=cos(0)/+(1-cos0)rr+ (1) 格的信息记录在矩阵中的第i行和第j列，记为 0 -I R11 R12 R13 G(i,)。采用这种矩阵存储栅格地图的方法，可建 sin 0 -rx R21 R22 R23 立起环境的整幅地图。 -Ty 0 R31 R32 R33 在室内无人机飞行空间中，只考虑水平方向， arctan( 23 可将飞行空间二维化，其中无人机高度控制由超 R33 euler; arcsin(Ri3) (2) 声波传感器实现。栅格地图模型建立后，需要对 每个栅格设置编码信息。编码设置如下：1为含 arctan( R 有障碍的栅格，0为自由栅格（无障碍），start为起
4) 标记识别：在这一点上，有必要确定获得 的标记候选哪些属于字典，哪些只是环境的一部 分。提取标记的候选码之后，就可获得 4 个不同 的标识符。为了加快这个过程，字典元素使用平 衡二叉树排序。为此，标记由通过连接其所有位 获得的整数值表示。 1.2 二维码阵列的设计 对一幅图像中多个二维码进行识别可以计算 出摄像头相对每个二维码的位置和姿态，未被识 别的标志不参与位置解算。同时摄像头能在任意 方向识别出唯一码号，通过在室内合理布置二维 码，综合多个标志解算的信息能够提供较大范围 内的无人机全局位置信息。地标阵列除了使用规 则排列外，可以通过任意的形式和距离组合，只需 要通过人工一次性标定后制表存入机载导航系统 存储器中即可。文中以最简单的矩阵规则排列形 式为例，所建立的地面定位阵列 (后文简称二维码 阵列)，当确定某二维码为全局坐标系原点后即可 以通过几何关系计算出其他二维码在全局坐标中 的位置，进而将摄像头解算出相对某二维码的位 置转换到全局坐标系下。本文根据 ArUco 库，生成 ID 编号为 1~100 的二维码集，设计如图 3 所示的 二维码地面阵列。其中，单个二维码边长为 18 cm， 码与码的间距为 18 cm，ID 编号为 1 的码的中心 为原点，行方向为 x 方向，列方向为 y 方向。例如 编号为 55 的二维码位置为 (18×8 cm,18×10 cm)。 图 3 二维码地面阵列 Fig. 3 QR code ground array 1.3 摄像头位姿估计和全局坐标获取 摄像头经过标定之后，可以解算出二维码相 对于摄像头光学中心的旋转向量 r 和平移向量 T 以及旋转矩阵 R [19] ，解算过程如下所示： θ = norm(r) r = r/θ R = cos(θ)I +(1−cos θ)rrT+ sinθ 0 −rz ry rz 0 −rx −ry rx 0 = R11 R12 R13 R21 R22 R23 R31 R32 R33 (1) euleri = γlocal θlocal ψlocal = arctan( R23 R33 ) −arcsin(R13) arctan( R12 R11 ) (2) posi = [ xlocal ylocal zlocal ] = −R ′ ·T (3) 根据式 (2) 可以计算出摄像头当前欧拉角， 式 (3) 可以计算出摄像头当前相对某个二维码的 局部位置，然后根据该二维码的全局坐标计算出 摄像头当前的全局坐标位置。进一步文中基于扩 展卡尔曼滤波器对二维码位置、光流传感器速度测 量值和机载 IMU 数据进行融合得到最优位姿估计。 2 改进 D*路径规划算法 无人机室内全局路径规划是无人机研究领域 的一个重要课题。路径规划的含义为已知无人机 运动的起点和终点，通过机载传感器对周围环境 进行描述，得到自由区域和障碍区域，从而规划 出一条从起点到终点并且避开障碍物的最短路 径。本文在对各种主流路径规划算法比较之后， 基于室内地面二维码阵列标志，选用 D*算法[20] ， 并对 D*算法做了改进，实现无人机室内自主飞行。 2.1 D*路径规划 为了在室内环境中寻找最短路径，首先需要 建立环境模型。D*算法采用栅格表示地图，将地 图信息记录在每一个栅格上，定义该栅格的属性 为自由或者障碍。无人机的飞行轨迹被栅格离散 化之后，每个栅格上的运动信息表征了无人机在 这个栅格上的飞行方向。理论上，无人机在每个 栅格上的运动有无数种，但考虑到模型建立的复 杂性，一般在实际运用中只定义 8 个方向的运动， 分别为：前、后、左、右、右前、右后、左前、左后， 如图 4 所示。 图 4 栅格地图及无人机可能运动方向 Fig. 4 Raster map and possible direction of movement of the drone 地图上的栅格采用矩阵方式储存[21] ，单个栅 格的信息记录在矩阵中的第 i 行和第 j 列，记为 G(i, j)。采用这种矩阵存储栅格地图的方法，可建 立起环境的整幅地图。 在室内无人机飞行空间中，只考虑水平方向， 可将飞行空间二维化，其中无人机高度控制由超 声波传感器实现。栅格地图模型建立后，需要对 每个栅格设置编码信息。编码设置如下：1 为含 有障碍的栅格，0 为自由栅格 (无障碍)，start 为起 ·664· 智 能 系 统 学 报 第 14 卷
第4期 张飞，等：基于改进D*算法的无人机室内路径规划 ·665· 点，end为终点。本文所建NxW地图如图5所示， f(n)=g(n)+h(n) (4) 其中N=10(可调节)。 式中：fm)是节点n的代价函数；g(n)是从终点到该 节点的实际代价；h(n)是从节点n到起点的最短路 径代价的的估计值，这里选取曼哈顿距离： 1 8 9 10 h(n)=max(lxstan-xil,lystan -ya) (5) 1112 14 15 16 17 1920 212223 2425 26 27 2829 30 D*算法的实现步骤为从目标节点开始遍历 31323334353637383940 搜索直到找到起始节点。给定起始点s和目标点 414243444546 47 8 50 d,创建2个空链表OpenList和CloseList,,计算流 515253 54 55 56 57 58 59 60 程如图6所示。 6162636465 66 67 68 69 70 71273 74 75 76 77 78 79 80 2.2改进D*算法 8182838485 86 8> 8990 当无人机沿着D*算法规划的轨迹在二维码 919293949596979899100 阵列上移动时，距离设定的障碍物比较近，容易 发生碰撞，导致无人机事故；而且在同一直线上 图5栅格地图 的路径点，如果无人机沿着设定轨迹飞行会浪费 Fig.5 raster map 一定的时间，因为从一个二维码飞到一个二维码 D*算法是一种典型的启发式搜索算法，其启 存在加速、减速、判断等待时间。因此本文对 发中的代价用代价函数表示为 D*算法做了一定的改进。 开始 搜索目标点d,并将其加入到OpenLis过中 OpenList为空 路径规划 失败 IN 选择OpenList中F值最小的节点作为 待扩展节点A,将节点A从OpenList 删除，加入CloseList A为起始节点 结束D*算法，实现 无人机路径规划 N 扩展节点A周围的8个子节点，并且计算G值 结束 子节点在OpenList中 N N 新G值< 子节点在CloseList中 旧G值 Y 将该节点插入OpenList中 N 新G值<旧G值 y 将子节点从CloseList删除 加人OpenList 更新G值，将父节点指向节点A 重新计算子节点的F值 图6D*算法流程图 Fig.6 D*algorithm flow chart
点，end 为终点。本文所建 N×N 地图如图 5 所示， 其中 N=10(可调节)。 X Y O 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 图 5 栅格地图 Fig. 5 raster map D*算法是一种典型的启发式搜索算法，其启 发中的代价用代价函数表示为 f(n) = g(n)+h(n) (4) f(n) g(n) h(n) 式中： 是节点 n 的代价函数； 是从终点到该 节点的实际代价； 是从节点 n 到起点的最短路 径代价的的估计值，这里选取曼哈顿距离： h(n) = max(|xstart − xi |,|ystart −yi |) (5) D*算法的实现步骤为从目标节点开始遍历 搜索直到找到起始节点。给定起始点 s 和目标点 d，创建 2 个空链表 OpenList 和 CloseList，计算流 程如图 6 所示。 2.2 改进 D*算法 当无人机沿着 D*算法规划的轨迹在二维码 阵列上移动时，距离设定的障碍物比较近，容易 发生碰撞，导致无人机事故；而且在同一直线上 的路径点，如果无人机沿着设定轨迹飞行会浪费 一定的时间，因为从一个二维码飞到一个二维码 存在加速、减速、判断等待时间。因此本文对 D*算法做了一定的改进。 开始 搜索目标点 d, 并将其加入到 OpenList 中 OpenList 为空 Y Y N N N N N Y Y Y Y N 路径规划 失败 选择 OpenList 中 F 值最小的节点作为 待扩展节点 A, 将节点 A 从 OpenList 删除, 加入 CloseList A 为起始节点 结束 D*算法, 实现 无人机路径规划 扩展节点 A 周围的 8 个子节点, 并且计算 G 值 结束 子节点在 OpenList 中 子节点在 CloseList 中 将该节点插入 OpenList 中 将子节点从 CloseList 删除 加入 OpenList 新 G 值 < 旧 G 值 新 G 值 < 旧 G 值 更新 G 值, 将父节点指向节点 A 重新计算子节点的 F 值 图 6 D*算法流程图 Fig. 6 D* algorithm flow chart 第 4 期 张飞，等：基于改进 D*算法的无人机室内路径规划 ·665·
·666· 智能系统学报 第14卷 D*算法中G值为父节点G值与父节点移动 算出飞行时x、y方向控制量，实现无人机在二维 到当前节点G值之和，其中父节点移动到当前节 码阵列上的自主导航，控制结构如图8所示。 点的G值定义为10（直线移动）或者14（斜向移 期望位置 位置环PID 动)。本文通过加入碰撞因子，来改变G值的计算 速度环PID 控制器 控制器 方式。具体操作方式为：在依次扩展周围8个子 节点时，加入对每个子节点周围8个节点的判断， 光流传感器 如果是子节点的直线方向（前、后、左、右）存在障 得到的速度 碍点，加入碰撞因子O,则当前节点移动到子节 检测二维码 点的路径代价G值为10+O(代价增加O。=8);如 得到的位置 果是子节点的斜向方向（右前、右后、左前、左 图8串级PD控制器框图 后)存在障碍点，则当前节点移动到子节点的路 Fig.8 Cascade PID controller block diagram 径代价G值为10+OJ2(代价增加O=4),因为直线 采用位置环变积分PD控制器。这里采用变 方向上存在障碍点的碰撞儿率要比斜向方向的 积分PD,在系统偏差较大时，减弱积分作用或者 大。实验规划的路径结果如图7所示，可以看出 消除积分作用，在系统偏差较小时增大积分作 在图7b)中路径避开了一些危险点。 用。可以避免因积分作用太大导致系统超调和积 分饱和，也可以避免积分作用太小导致难以消除 C:\windows\system32\cmd.exe 静态误差。因此加入了变积分系数index, ex=r-Xe即 ey=y-yexp ux kp,ex +kis e.(d6+kd de.() Jo d 000000 e(⑥)d6+kd de,() 00 d 3->14->25->35->44->55->66 (a)D*路径规划 ux kpxex +index.kis 0 e.⊙d6+kd, de.() C:\windows\system32\cmd.exe 式中：x、y为Kalman滤波器位置输出值；xexp和 yexp为期望位置；ex、e,为x、y方向的位置误差； 4、山，为x、y方向的位置控制量；kp、ki、kd为 PID参数；index为变积分PID的积分系数为 000 0 e>200 0日日 200-e index 0· 其他 (6) 3->4->15->26->35->44->54->65->66 1 e<100 (b)改进D*路径规划 因位置环已经做了变积分，而且位置环的输 图7改进前后D*路径规划 出为速度环的输入，因此速度环PD控制器设计 Fig.7 Before and after improved D*path planning 为一般PID控制形式为 通过增加碰撞因子规划出的安全路径之后， ev=Vx-Vt_exp 无人机要沿着路径上二维码的编号依次飞行。对 eny=v,-Vy_exp 于在同一条直线上的二维码节点，依次沿着码飞 uux kpuevx+kinx vr(t) e(d6+kd. dt (7) 行只会降低飞行效率。而且到达某个码附近之后 ew(⑥)d6+kd de(t) 还需要做细微的调整直到满足一定的死区范围才 ury=kpiyery +kivy 能飞往下一个二维码，这样大大增加了飞行时 间。本文对同一直线上的二维码路径信息做出判 3 实验和分析 断，跳过中间点，直接从直线的起点飞往终点，大 硬件系统包括：330mm轴距四旋翼无人机平 大节省了无人机的飞行时间。 台、自制飞控系统、超声波测距仪、光流传感器、 2.3控制器设计 摄像头云台和树莓派，文中实验系统通过2.4GHz 将无人机在二维码阵列中的全局坐标与期望 射频信号与地面站通信同时接受遥控信号。实验 坐标做差，将差值送入串级PD控制器四，最终计 系统框图如图9所示
D*算法中 G 值为父节点 G 值与父节点移动 到当前节点 G 值之和，其中父节点移动到当前节 点的 G 值定义为 10(直线移动) 或者 14(斜向移 动)。本文通过加入碰撞因子，来改变 G 值的计算 方式。具体操作方式为：在依次扩展周围 8 个子 节点时，加入对每个子节点周围 8 个节点的判断， 如果是子节点的直线方向 (前、后、左、右) 存在障 碍点，加入碰撞因子 Ob，则当前节点移动到子节 点的路径代价 G 值为 10+Ob (代价增加 Ob=8)；如 果是子节点的斜向方向 (右前、右后、左前、左 后) 存在障碍点，则当前节点移动到子节点的路 径代价 G 值为 10+Ob /2(代价增加 Ob=4)，因为直线 方向上存在障碍点的碰撞几率要比斜向方向的 大。实验规划的路径结果如图 7 所示，可以看出 在图 7(b) 中路径避开了一些危险点。 (a) D* 路径规划 (b) 改进 D* 路径规划 图 7 改进前后 D*路径规划 Fig. 7 Before and after improved D* path planning 通过增加碰撞因子规划出的安全路径之后， 无人机要沿着路径上二维码的编号依次飞行。对 于在同一条直线上的二维码节点，依次沿着码飞 行只会降低飞行效率。而且到达某个码附近之后 还需要做细微的调整直到满足一定的死区范围才 能飞往下一个二维码，这样大大增加了飞行时 间。本文对同一直线上的二维码路径信息做出判 断，跳过中间点，直接从直线的起点飞往终点，大 大节省了无人机的飞行时间。 2.3 控制器设计 将无人机在二维码阵列中的全局坐标与期望 坐标做差，将差值送入串级 PID 控制器[22] ，最终计 算出飞行时 x、y 方向控制量，实现无人机在二维 码阵列上的自主导航，控制结构如图 8 所示。 期望位置 位置环 PID 控制器 速度环 PID 控制器 − − 光流传感器 得到的速度 检测二维码 得到的位置 图 8 串级 PID 控制器框图 Fig. 8 Cascade PID controller block diagram 采用位置环变积分 PID 控制器。这里采用变 积分 PID，在系统偏差较大时，减弱积分作用或者 消除积分作用，在系统偏差较小时增大积分作 用。可以避免因积分作用太大导致系统超调和积 分饱和，也可以避免积分作用太小导致难以消除 静态误差。因此加入了变积分系数 index。 ex = x− xexp ey = y−yexp ux = kpx ex +λxkix ∫ t 0 ex(δ)dδ+kdx dex(t) dt uy = kpyey +λykiy ∫ t 0 ey(δ)dδ+kdy dey(t) dt ux = kpxex +index · kix ∫ t 0 ex (δ)dδ+kdx dex (t) dt ex ey ux uy 式中：x、y 为 Kalman 滤波器位置输出值；xexp 和 yexp 为期望位置； 、 为 x、y 方向的位置误差； 、 为 x、y 方向的位置控制量； kp、ki、kd 为 PID 参数；index 为变积分 PID 的积分系数为 index = 0, e > 200 200−e 100 , 其他 1, e < 100 (6) 因位置环已经做了变积分，而且位置环的输 出为速度环的输入，因此速度环 PID 控制器设计 为一般 PID 控制形式为 evx = vx −vx_exp evy = vy −vy_exp uvx = kpvxevx +kivx ∫ t 0 evx(δ)dδ+kdvx devx(t) dt uvy = kpvyevy +kivy ∫ t 0 evy(δ)dδ+kdvy devy(t) dt (7) 3 实验和分析 硬件系统包括：330 mm 轴距四旋翼无人机平 台、自制飞控系统、超声波测距仪、光流传感器、 摄像头云台和树莓派，文中实验系统通过 2.4 GHz 射频信号与地面站通信同时接受遥控信号。实验 系统框图如图 9 所示。 ·666· 智 能 系 统 学 报 第 14 卷
第4期 张飞，等：基于改进D*算法的无人机室内路径规划 ·667· 一期型轨迹 脓合轨迹(EKF) 原始二维码定位轨迹 1.0 0 35 3.0 心 凸由中由凸凸 2.5 (a)四旋翼实验平台 05西西西西西西色白中 1.0 中西中西由杏西中由 超声波 IMU 光流模块 0L白出内皮也也皮点度 3.53.02.52.01.51.00.50 X/m RTC 导航模块 图11二维的阵列中圆形轨迹跟踪 Fig.11 Circular trajectory tracking in a two-dimensional 云 飞控 array 摄像头 树莓派 D* -0.2 (b)飞行控制系统原理图 -0.4 -0.6 6 10 12 图9实验系统框图 (a)X轴速度融合结果 Fig.9 Experimental system block diagram 0.4r 3.1飞行过程中二维码的识别效果 0.2 0 图10为飞行过程中二维码识别效果，可以看 -0.2 出机载摄像头可以同时识别出多个二维码，并可 -0.4 1012 以解算出与单个二维码的局部坐标位置，进而融 (b)Y轴速度融合结果 合得到无人机的全局坐标位置。 图12无人机速度融合 Fig.12 UAV speed fusion 3.2改进D*自主路径规划 图13为实验中规划的无人机路径信息，已用 实线标出，图中最下方3、4、26、44、54、65、66为 无人机运动路径上的二维码编号。 (a)图像模糊影响识别 (b)地面反光影响识别 Cwindowsisystem32cmd exe 图10二维码识别效果 Fig.10 QR code recognition effect 文中采用扩展卡尔曼滤波器(EKF)结合飞行 器机载MU数据，光流传感器速度测量数据和二 维码识别位置数据进行融合得到平滑连续的全局 >44->54-65->66 54-2-y44-354->65-66 定位信息。图11为采用EKF融合得到的无人机 图13仿真路径 位置估计作为反馈实现的圆形轨迹跟踪，可以看 Fig.13 simulation path 到融合后的无人机位置信息光滑连续，所设计控 图14为在地面站显示的无人机的实际路 制器能较好地跟踪期望命令。 径，其中绿色方块位置为起点，蓝色方块为无人 图12为无人机速度估计结果，可以看到EKF 机最后停止位置，实线为无人机运动路径。可 估计出的速度相比Pixflow传感器或PLK光流法 以看出，无人机实际移动路径与规划路径形状 得到的原始光流测量速度更加平滑，噪声更小。 相似
摄像头 IMU 光流模块 导航模块 飞控 树莓派 超声波 云台 RTC 2.4 GHz I/O D* (a) 四旋翼实验平台 (b) 飞行控制系统原理图 图 9 实验系统框图 Fig. 9 Experimental system block diagram 3.1 飞行过程中二维码的识别效果 图 10 为飞行过程中二维码识别效果，可以看 出机载摄像头可以同时识别出多个二维码，并可 以解算出与单个二维码的局部坐标位置，进而融 合得到无人机的全局坐标位置。 (a) 图像模糊影响识别 (b) 地面反光影响识别 图 10 二维码识别效果 Fig. 10 QR code recognition effect 文中采用扩展卡尔曼滤波器（EKF）结合飞行 器机载 IMU 数据，光流传感器速度测量数据和二 维码识别位置数据进行融合得到平滑连续的全局 定位信息。图 11 为采用 EKF 融合得到的无人机 位置估计作为反馈实现的圆形轨迹跟踪，可以看 到融合后的无人机位置信息光滑连续，所设计控 制器能较好地跟踪期望命令。 图 12 为无人机速度估计结果，可以看到 EKF 估计出的速度相比 Pixflow 传感器或 PLK 光流法 得到的原始光流测量速度更加平滑，噪声更小。 1.0 0.5 0 3.5 3.0 2.5 2.0 1.5 1.0 0.5 0 Y/m −3.5 −3.0 −2.5−2.0−1.5 −1.0−0.5 0 X/m Z/m 期望轨迹 融合轨迹 (EKF) 原始二维码定位轨迹 图 11 二维的阵列中圆形轨迹跟踪 Fig. 11 Circular trajectory tracking in a two-dimensional array 0.4 0.2 0 −0.2 −0.4 −0.6 0 2 4 6 8 10 12 X/(m·s−1 ) Y/(m·s−1 ) 0.4 0.2 0 −0.2 −0.4 0 2 4 6 8 10 12 t/s t/s PLK Px4flow EKF PLK Px4flow EKF (a) X 轴速度融合结果 (b) Y 轴速度融合结果 图 12 无人机速度融合 Fig. 12 UAV speed fusion 3.2 改进 D*自主路径规划 图 13 为实验中规划的无人机路径信息，已用 实线标出，图中最下方 3、4、26、44、54、65、66 为 无人机运动路径上的二维码编号。 图 13 仿真路径 Fig. 13 simulation path 图 14 为在地面站显示的无人机的实际路 径，其中绿色方块位置为起点，蓝色方块为无人 机最后停止位置，实线为无人机运动路径。可 以看出，无人机实际移动路径与规划路径形状 相似。 第 4 期 张飞，等：基于改进 D*算法的无人机室内路径规划 ·667·
·668· 智能系统学报 第14卷 二维码 [S]唐强，张翔伦，左玲，无人机航迹规划算法的初步研究 -EKF 0 内世2中杏内色？电0 [).航空计算技术，2003,33(1)125-128,132 -0.5 心海他沙些独件秒 TANG Qiang,ZHANG Xianglun,ZUO Ling.Initial study -1.0 on the path planning's algorithms for unmanned aerial -1.5 -2.0 vehicles[J].Aeronautical computer technique,2003,33(1): -2.5 125-128.132. -3.0 [6]王俊，周树道，朱国涛，等.无人机航迹规划常用算法 -3.5 火力与指挥控制，2012,37(8)：5-8 -4.0 WANG Jun,ZHOU Shudao,ZHU Guotao,et al.Research -4.5 -3.5 -2.5 -1.5 -0.5 0.5 X/m of common route planning algorithms for unmanned air 图14实际飞行实验 vehicle[J].Fire control and command control,2012,37(8): Fig.14 Actual flight experiment 5-8. 图14为采用所提改进D*算法进行的实际飞 [7]白志君.四旋翼无人机室内自主导航系统的研究与实现 [D1.厦门：厦门大学，2014 行实验，实验中给定期望航点，同时设定3个障碍 物如图中红色矩形所示，使用所提路径规划算法 BAI Zhijun.Study and realization on indoor autonomous navigation system for quad-copter[D].Xiamen:Xiamen 进行规划并控制飞行器按给定路径飞行，从图中 University,2014. 可以看到所提算法成功地规划出了经过各航点所 [8]何雨枫.室内微小型无人机路径规划算法研究D].南 需的期望路径并与障碍物保证一定距离，飞行器 京：南京航空航天大学，2014 能较好地按规划路线进行飞行。 HE Yufeng.Research on indoor MUAV path planning D]. 4结束语 Nanjing:Nanjing University of Aeronautics and Astronaut- ics,2014. 文中提出了一种改进D*算法，在传统算法上 [9]黄媛媛.基于数据融合的室内无人机导航避障研究D] 基于引入障碍因子之后使无人机与障碍物保持一 成都：成都理工大学，2017. 定的安全距离，通过斜率判断只取直线路径的起 HUANG Yuanyuan.Research on navigation avoidance of 始点，省略中间点，提高了无人机飞行效率。基 indoor UAV based on data fusion[D].Chengdu:Chengdu 于二维码阵列实现了室内低成本高精度的定位系 University of Technology,2017. 统，进一步使用扩展卡尔曼滤波器对二维码、光 [10]STENTZ A.The D*algorithm for real-time planning of 流和IMU数据融合得到了无人机精确连续的状 optimal traverses[R].Carnegie:Carnegie Mellon Uni- 态估计，保证了轨迹跟踪反馈控制的可靠性，所 versity,1994. [11]STENTZ A.The focussed D*algorithm for real-time re- 设计的控制器能较好地跟踪期望轨迹，文中系统 planning[C]//Proceedings of the 14th International Joint 满足室内低成本微小型无人机的导航和定位需求。 Conference on Artificial Intelligence.Montreal.Canada. 参考文献： 1995:1652-1659. [12]HRABAR S.3D path planning and stereo-based obstacle [1]LIM H,PARK J,LEE D,et al.Build your own quadrotor: avoidance for rotorcraft UAVs[Cl//Proceedings of 2008 open-source projects on unmanned aerial vehicles[J].IEEE IEEE/RSJ International Conference on Intelligent Robots robotics and automation magazine,2012,19(3):33-45. and Systems.Nice,France,2008:807-814. [2]RASMUSSEN J.NIELSEN J.GARCIA-RUIZ F,et al.Po- [13]郑昌文.飞行器航迹规划方法研究D].武汉：华中科技 tential uses of small unmanned aircraft systems (UAS)in 大学，2003. weed research[J].Weed research,2013,53(4):242-248. ZHENG Changwen.Research on route planning for air [3]XIONG Jingjing,ZHENG Enhui.Position and attitude vehicles[D].Wuhan:Huazhong University of Science and tracking control for a quadrotor UAV[J].ISA transactions, Technology,2003. 2014,53(3):725-731. [14]符小卫，高晓光.一种无人机路径规划算法研究[.系 [4]KOTTATH R,NARKHEDE P,KUMAR V,et al.Mul- 统仿真学报，2004,16(1)：20-21,34 tiple model adaptive complementary filter for attitude es- FU Xiaowei,GAO Xiaoguang.Study on a kind of path timation[J].Aerospace science and technology,2017,69: planning algorithm for UAV[J].Journal of system simula- 574-581. tion,2004,16(1):20-21,34
0 −0.5 −1.0 −1.5 −2.0 −2.5 −3.0 −3.5 −4.0 −4.5 −3.5 −2.5 −1.5 −0.5 0.5 X/m Y/m EKF 二维码 图 14 实际飞行实验 Fig. 14 Actual flight experiment 图 14 为采用所提改进 D*算法进行的实际飞 行实验，实验中给定期望航点，同时设定 3 个障碍 物如图中红色矩形所示，使用所提路径规划算法 进行规划并控制飞行器按给定路径飞行，从图中 可以看到所提算法成功地规划出了经过各航点所 需的期望路径并与障碍物保证一定距离，飞行器 能较好地按规划路线进行飞行。 4 结束语 文中提出了一种改进 D*算法，在传统算法上 基于引入障碍因子之后使无人机与障碍物保持一 定的安全距离，通过斜率判断只取直线路径的起 始点，省略中间点，提高了无人机飞行效率。基 于二维码阵列实现了室内低成本高精度的定位系 统，进一步使用扩展卡尔曼滤波器对二维码、光 流和 IMU 数据融合得到了无人机精确连续的状 态估计，保证了轨迹跟踪反馈控制的可靠性，所 设计的控制器能较好地跟踪期望轨迹，文中系统 满足室内低成本微小型无人机的导航和定位需求。 参考文献： LIM H, PARK J, LEE D, et al. Build your own quadrotor: open-source projects on unmanned aerial vehicles[J]. IEEE robotics and automation magazine, 2012, 19(3): 33–45. [1] RASMUSSEN J, NIELSEN J, GARCIA-RUIZ F, et al. Potential uses of small unmanned aircraft systems (UAS) in weed research[J]. Weed research, 2013, 53(4): 242–248. [2] XIONG Jingjing, ZHENG Enhui. Position and attitude tracking control for a quadrotor UAV[J]. ISA transactions, 2014, 53(3): 725–731. [3] KOTTATH R, NARKHEDE P, KUMAR V, et al. Multiple model adaptive complementary filter for attitude estimation[J]. Aerospace science and technology, 2017, 69: 574–581. [4] 唐强, 张翔伦, 左玲. 无人机航迹规划算法的初步研究 [J]. 航空计算技术, 2003, 33(1): 125–128, 132. TANG Qiang, ZHANG Xianglun, ZUO Ling. Initial study on the path planning's algorithms for unmanned aerial vehicles[J]. Aeronautical computer technique, 2003, 33(1): 125–128, 132. [5] 王俊, 周树道, 朱国涛, 等. 无人机航迹规划常用算法 [J]. 火力与指挥控制, 2012, 37(8): 5–8. WANG Jun, ZHOU Shudao, ZHU Guotao, et al. Research of common route planning algorithms for unmanned air vehicle[J]. Fire control and command control, 2012, 37(8): 5–8. [6] 白志君. 四旋翼无人机室内自主导航系统的研究与实现 [D]. 厦门: 厦门大学, 2014. BAI Zhijun. Study and realization on indoor autonomous navigation system for quad-copter[D]. Xiamen: Xiamen University, 2014. [7] 何雨枫. 室内微小型无人机路径规划算法研究 [D]. 南 京: 南京航空航天大学, 2014. HE Yufeng. Research on indoor MUAV path planning[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2014. [8] 黄媛媛. 基于数据融合的室内无人机导航避障研究 [D]. 成都: 成都理工大学, 2017. HUANG Yuanyuan. Research on navigation avoidance of indoor UAV based on data fusion[D]. Chengdu: Chengdu University of Technology, 2017. [9] STENTZ A. The D* algorithm for real-time planning of optimal traverses[R]. Carnegie: Carnegie Mellon University, 1994. [10] STENTZ A. The focussed D* algorithm for real-time replanning[C]//Proceedings of the 14th International Joint Conference on Artificial Intelligence. Montreal, Canada, 1995: 1652–1659. [11] HRABAR S. 3D path planning and stereo-based obstacle avoidance for rotorcraft UAVs[C]//Proceedings of 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Nice, France, 2008: 807–814. [12] 郑昌文. 飞行器航迹规划方法研究 [D]. 武汉: 华中科技 大学, 2003. ZHENG Changwen. Research on route planning for air vehicles[D]. Wuhan: Huazhong University of Science and Technology, 2003. [13] 符小卫, 高晓光. 一种无人机路径规划算法研究 [J]. 系 统仿真学报, 2004, 16(1): 20–21, 34. FU Xiaowei, GAO Xiaoguang. Study on a kind of path planning algorithm for UAV[J]. Journal of system simulation, 2004, 16(1): 20–21, 34. [14] ·668· 智 能 系 统 学 报 第 14 卷
第4期 张飞，等：基于改进D*算法的无人机室内路径规划 ·669· [15]GARRIDO-JURADO S,MUNOZ-SALINAS,R,MAD- WEI Ning,LIU Yisong.Research on Grid-Base global RID-CUEVAS F J,et al.Generation of fiducial marker path planning for mobile robot[J].Control and automa- dictionaries using mixed integer linear programming[J]. tion,2008,2411)y229-231 Pattern recognition,2016,3(51):481-491. [22]NEMRA A.AOUF N.Robust INS/GPS sensor fusion for [16]SUZUKI S,ABE K.Topological structural analysis of di- UAV localization using SDRE nonlinear filtering[J]. gitized binary images by border following[J].Computer IEEE sensors journal,2010,10(4):789-798. vision,graphics,and image processing,1985,30(1): 32-46. 作者简介： [17]DOUGLAS D H,PEUCKER T K.Algorithms for the re- 张飞，男，1988年生，工程师，主 duction of the number of points required to represent a di- 要研究方向为无人机检测、无人机巡 gitized line or its caricature[J].Cartographica:the interna- 检技术。 tional journal for geographic information and geovisualiz- ation,1973.,10(2):112-122. [18]OTSU N.A threshold selection method from gray-level histograms[J].IEEE transactions on systems,man,and 白伟，男，1992年生，硕土研究 cybernetics,1979,9(1):62-66. 生，主要研究方向为无人机控制。 [19]KRAJNIK T,VONASEK V,FISER D,et al.AR-Drone as a platform for robotic research and education[J//OB- DRZALEK D,GOTTSCHEBER A.Research and Educa- tion in Robotics-EUROBOT 2011.Berlin,Heidelberg: Springer,.2011:172-186. [20]FERGUSON D,STENTZ A.Field D*:an interpolation- 乔耀华，男，1982年生，副高级工程 based path planner and replanner[M]//THRUN S, 师，主要研究方向为输电运维检修、输 电线路运行、检修管理。发表学术论 BROOKS R,DURRANT-WHYTE H.Robotics Re- 文专著6项。 search.Berlin,Heidelberg:Springer,2007:239-253. [21]魏宁，刘一松.基于栅格模型的移动机器人全局路径规 划研究.微计算机信息，2008.2411)：229-231
GARRIDO-JURADO S, MUÑOZ-SALINAS,R, MADRID-CUEVAS F J, et al. Generation of fiducial marker dictionaries using mixed integer linear programming[J]. Pattern recognition, 2016,3(51): 481-491. [15] SUZUKI S, ABE K. Topological structural analysis of digitized binary images by border following[J]. Computer vision, graphics, and image processing, 1985, 30(1): 32–46. [16] DOUGLAS D H, PEUCKER T K. Algorithms for the reduction of the number of points required to represent a digitized line or its caricature[J]. Cartographica: the international journal for geographic information and geovisualization, 1973, 10(2): 112–122. [17] OTSU N. A threshold selection method from gray-level histograms[J]. IEEE transactions on systems, man, and cybernetics, 1979, 9(1): 62–66. [18] KRAJNÍK T, VONÁSEK V, FIŠER D, et al. AR-Drone as a platform for robotic research and education[J]//OBDRŽÁLEK D, GOTTSCHEBER A. Research and Education in Robotics-EUROBOT 2011. Berlin, Heidelberg: Springer, 2011: 172–186. [19] FERGUSON D, STENTZ A. Field D*: an interpolationbased path planner and replanner[M]//THRUN S, BROOKS R, DURRANT-WHYTE H. Robotics Research. Berlin, Heidelberg: Springer, 2007: 239–253. [20] 魏宁, 刘一松. 基于栅格模型的移动机器人全局路径规 划研究 [J]. 微计算机信息, 2008, 24(11): 229–231. [21] WEI Ning, LIU Yisong. Research on Grid-Base global path planning for mobile robot[J]. Control and automation, 2008, 24(11): 229–231. NEMRA A, AOUF N. Robust INS/GPS sensor fusion for UAV localization using SDRE nonlinear filtering[J]. IEEE sensors journal, 2010, 10(4): 789–798. [22] 作者简介： 张飞，男，1988 年生，工程师，主 要研究方向为无人机检测、无人机巡 检技术。 白伟，男，1992 年生，硕士研究 生，主要研究方向为无人机控制。 乔耀华,男,1982 年生,副高级工程 师,主要研究方向为输电运维检修、输 电线路运行、检修管理。发表学术论 文专著 6 项。 第 4 期 张飞，等：基于改进 D*算法的无人机室内路径规划 ·669·