一种基于RGB-DSLAM的无人机三维重建方法、无人机与流程

本发明属于图像数据处理技术领域，尤其涉及一种基于rgb-dslam的无人机三维重建方法、无人机。

背景技术：

目前，业内常用的现有技术是这样的：

多旋翼无人机是一种具备垂直起降，悬停并具有优良低速飞行性能的无人驾驶飞行器，在航拍，农业植保，电力巡线以及快递运输等诸多领域，得到了广泛的应用和发展。但是，无人机是一种科技含量很高的产品，未经过培训的普通大众很难正确理解和掌握，容易造成一定的损失和危害，不利于无人机的普及和发展。当前，大多数无人机都是室外飞行，不便于课堂讲授和演示。一款室内飞行性能优良，便于课堂教学的无人机，成为无人机教育领域的刚需。

无人机室内飞行一直是无人机领域的研究热点，室内应用也暂时是无人机应用的空白区域，这是因为室内环境相对复杂，人员走动，色调单一，光线多变，没有gps信号等诸多因素，无人机难于定位和完成路径规划。以上问题促使大部分无人机在室内的飞行性能并不稳定，所以研究人员就需要应用其他方法为无人机的位置进行精确定位，与此同时还能够构建出环境地图。

由于室内无人机无法较好的接收卫星信号，所以无法通过卫星定位，其需要依靠自身所携带的传感器来进行定位，比如激光雷达，彩色相机等等。通过上述传感器接收到的信息如何转变为无人机的位置信息，需要复杂的处理过程。在室内环境未知的条件下，人们根据已有传感器信息来进行无人机定位更是难上加难。slam(simultaneouslocalizationandmapping)问题就是在这样的需求背景下提出的，是当前无人机定位技术研究的一个重要课题。slam技术的重要意义是它很好的解决了无人机定位与环境地图构建之间的耦合问题，使二者能够同时被求解出来。slam问题基本包括视觉里程计、后端、回环检测、构建地图这四部分，其中视觉里程计被称作为前端部分，在这部分中，slam技术很好的融合了计算机视觉与信号处理、刚体运动学等领域相关的内容；slam算法的后端又与几何学，图优化以及概率论等学科相融合。此外，现有技术中，点云地图的误差是比较大的，甚至后面还有多出的部分，这是与实际严重不符的；现有技术中无人机转弯过快或者飞行过快而导致的数据丢失问题，是无人机飞行过慢导致数据重复而使得算法运行速率降低的问题；现有技术中，构图精度较低，降低运行效率。在转弯的过程中，无人机需要考虑飞行安全性能，降低了无人机的飞行速度，而相机采集的帧率是没有改变的，导致在该处采集的数据重复，解决该问题的方法是将采集的帧率与无人机的飞行速度结合起来，能够以不同的帧率满足高速和低速状态下不同的数据采集需求，提高了数据集的精度。

综上所述，现有技术存在的问题是：现有技术中无人机转弯过快或者飞行过快而导致的数据丢失，构图精度较低，运行效率低。

解决上述技术问题的难度和意义：难度在于如何将相机的采集帧率与无人机的飞行速度结合起来，同时在结合的过程中其中的参数如何设计，即多高的速度对应多少帧率，保证数据不丢失，不重复。解决该问题的意义重大，能够解决当前大多采用固定帧率采集数据不够精确的问题。

技术实现要素：

针对现有技术存在的问题，本发明提供了一种基于rgb-dslam的无人机三维重建方法。

本发明是这样实现的，一种基于rgb-dslam的无人机三维重建方法，具体包括以下步骤：

步骤一：kinect相机数据提取与预处理：采用开源openni程序框架进行kinect相机数据的提取，采用通讯方式，对提取到的彩色rgb图片信息与深度depth图片信息进行储存；

步骤二：rgb-dslam算法的实现：利用前端算法对rgb-d图像进行特征检测与描述符提取，对提取到的描述进行特征匹配，根据匹配结果估算运动变换并进行优化；后端算法根据前端算法的结果构建位姿图，然后进行闭环检测与位姿图优化，最终得到的最优相机位姿进行相机定位与三维地图环境重建；

步骤三：地图转化与路径规划：在生成的三维点云的基础上，将其转化成为八叉树地图，进而生成环境空间的二维地图，对无人机的路径进行规划。

进一步，步骤一中，数据预处理包括有相机针孔模型和深度信息获取模型。

进一步，步骤一中，相机针孔模型具体为：

物体所在的环境为世界坐标系o-x-y-z，所成倒立像的平面称之为成像坐标系o′-x′-y′-z′，相机本身又有着自己的坐标系，称之为相机坐标系o-x-y-z；本发明取相机坐标系o-x-y-z与成像坐标系o′-x′-y′-z′的z轴重合，x轴向右，y轴由右手定则确定，o称为相机光心；

为了简化模型，将现实中的物体抽象为一个质点a。该质点在相机坐标系o-x-y-z的坐标为[x，y，z]^t，其反射的光通过相机光心o成像在成像坐标系o′-x′-y′-z′，坐标为[x′，y′，z′]^t，同时成像的平面距离相机光心距离为相机的焦距f0；根据三角形相似规则，可得到下式所示：

通常，在成像之后，相机内部会自动将倒立的像转变成为正的图像，所以公式中并未出现负号，默认相机已经自动调节完成，这也是为了后文处理方便。下面将成像坐标系下的坐标单独放到等号的一侧，可得到如式(2)所示：

上式就是质点坐标从相机坐标系o-x-y-z到成像坐标系o′-x′-y′-z′的变换公式；既然是彩色相机成像，就需要储存彩色信息；通畅彩色图片的信息是以像素的形式进行储存的；在计算机中，每个像素点存储着对应的二进制数值，二进制数值也就表示着每个点的颜色；

下面定义像素坐标系c-u-v，其中c为像素坐标系的原心，相对于成像坐标系o′-x′-y′-z′的原心o′平移了[cx，cy]^t；u相当于成像坐标系o′-x′-y′-z′的x′轴，但与之不同的是，图像在u轴上缩放了α倍。同理，v相当于成像坐标系o′-x′-y′-z′的y′轴，图像在v轴上缩放了β倍；这样，由图像坐标转换为像素坐标可由式(3)表示：

结合式(2)与式(3)，可得到质点坐标从相机坐标系o-x-y-z到像素坐标系c-u-v的变换公式，如式(4)所示：

下面，令fx＝αf0、fy＝βf0，f0为相机的焦距，单位为米，α、β单位均为像素/米，所以fx、fy的单位均为像素，整理式(4)可得：

上式即为针孔模型的数学表述，描述了物体坐标从相机坐标系转换到像素坐标系，如果考虑到畸变影响，则需对式(5)进行一定的矫正。

畸变主要分为两种，即径向畸变与切向畸变，二者均是由于光线穿过透镜所引起的，透镜自身对光线的影响以及在安装时透镜不能完全平行于像素平面，这两点因素均会造成畸变；引入畸变纠正系数对其进行修正，不加证明的给出畸变修正方程：

其中，是归一化后的坐标值，即为式(5)中的[x/z，y/z]^t，[xcorrect，ycorrect]^t是经过修正后的坐标；称修正系数k1、k2、k3为径向修正系数，称修正系数p1、p2为切向修正系数；在得到了修正后的坐标后，只需将式(5)中的坐标[x/z，y/z]^t替换成为[xcorrect，ycorrect]^t即可；

通过上述变换，得到彩色相机成像的基本模型；实际上，每个像素坐标[u，v]^t会储存一个数值矩阵i(u，v)∈r^l×h，数值矩阵i中存储着二进制数值，这些二进制数通过确定的关系与计算机中红绿蓝三个通道对应，通过这三种颜色的搭配来组合出实际物体的颜色。

进一步，步骤一中，深度信息获取模型具体为：

选用的传感器为kinect2.0相机，其测量像素深度的原理是飞行时间法；所谓飞行时间法，相机的发射端发出一束脉冲，经过物体的反射再回到相机的接收端，相机通过这个过程所消耗的时间t来计算出飞行距离s，进而得到像素点的深度值；如下式所示。

d＝s/2＝vt/2(7)

进一步，步骤一中，基于相机针孔模型和深度信息获取模型获得数据处理的原则具体为：

(1)由于kinect2.0输入帧频为每秒30帧，设定一个阈值，若无人机的移动距离或者转弯角度没有达到该阈值，则认为采集到的数据集有重复，舍弃到采集到的数据；

(2)由于kinect2.0相机对于彩色rgb图像与深度depth图像的采集是分为两个通道进行的，若在一次采集周期中，彩色rgb图像与深度depth图像时间相差为1/30秒，则认为数据异步，忽略掉采集的数据；

(3)kinect2.0相机的有效采集范围是0.5m～4.5m，超过此范围所采集的数据集均认为是无效数据集；这种做法能够提升整个slam的构图精度，同时也会降低数据集的维度，提高运行效率。

进一步，步骤二中，rgb-dslam算法可以分为前端与后端两部分。

进一步，步骤二中，rgb-dslam算法前端的任务为提取不同观察结果之间的空间关系，后段的任务为使用非线性误差函数优化位姿图中相机的位姿信息。

进一步，步骤三中，八叉树转二维地图具体包括：

(1)建立八叉树模型；

(2)假设相机在深度图中测量到了一个空间点的深度为d，这就代表八叉树模型中d的深度范围之内是没有任何八叉树结点的，说明该条路径没有被占据，可以通过；

(3)取某一高度下的八叉树平面，将某物体所占据的小立方体的最远距离计算出来，以此距离作为半径做圆，将该圆作为无人机的禁飞区，在此基础上进行路径规划。

综上所述，本发明的优点及积极效果为：本发明对比于只有前端时得到了有效的改善，环境已经可以被直观的认出，基本达到预先的要求；同时通过使用预处理数据原则，有效的解决了数据维度过大的问题，使得构建出的地图趋于真实场景。本发明与激光扫描不同的是，飞行时间法可以一次性得到整个图像的深度信息，并不需要逐点扫描，这也增加了效率。本发明kinect2.0输入帧频为每秒30帧，设定一个阈值，若无人机的移动距离或者转弯角度没有达到该阈值，则认为采集到的数据集有重复，舍弃到采集到的数据；此种做法会减少数据集的维度，有效的提升算法的处理性能，并且不会对全局地图的构建与无人机的定位产生明显的影响。

本发明kinect2.0相机的有效采集范围是0.5m～4.5m，超过此范围所采集的数据集均认为是无效数据集；能够提升整个slam的构图精度，同时也会降低数据集的维度，提高运行效率。

附图说明

图1是本发明实施例提供的基于rgb-dslam的无人机三维重建方法流程图。

图2是本发明实施例提供的针孔相机模型示意图。

图3是本发明实施例提供的成像相似三角形示意图。

图4是本发明实施例提供的飞行时间原理示意图。

图5是本发明实施例提供的八叉树模型示意图。

图6是本发明实施例提供的二维地图的建立示意图。

图7是本发明实施例提供的无人机转角限制示意图。

图8是本发明实施例提供的改进a*搜索路径示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

下面结合附图对本发明的应用原理做详细描述。

如图1所示，本发明实施例提供的基于rgb-dslam的无人机三维重建方法，具体包括以下步骤：

s101：kinect相机数据提取与预处理：采用开源openni程序框架进行kinect相机数据的提取，采用通讯方式，对提取到的彩色rgb图片信息与深度depth图片信息进行储存；

s102：rgb-dslam算法的实现：利用前端算法对rgb-d图像进行特征检测与描述符提取，对提取到的描述进行特征匹配，根据匹配结果估算运动变换并进行优化；后端算法根据前端算法的结果构建位姿图，然后进行闭环检测与位姿图优化，最终得到的最优相机位姿进行相机定位与三维地图环境重建；

s103：地图转化与路径规划：在生成的三维点云的基础上，将其转化成为八叉树地图，进而生成环境空间的二维地图，对无人机的路径进行规划。

步骤s101中，本发明实施例提供的数据预处理包括有相机针孔模型和深度信息获取模型。

步骤s101中，本发明实施例提供的相机针孔模型具体为：

物体所在的环境为世界坐标系o-x-y-z，所成倒立像的平面称之为成像坐标系o′-x′-y′-z′，相机本身又有着自己的坐标系，称之为相机坐标系o-x-y-z；本发明取相机坐标系o-x-y-z与成像坐标系o′-x′-y′-z′的z轴重合，x轴向右，y轴由右手定则确定，o称为相机光心；

为了简化模型，将现实中的物体抽象为一个质点a。该质点在相机坐标系o-x-y-z的坐标为[x，y，z]^t，其反射的光通过相机光心o成像在成像坐标系o′-x′-y′-z′，坐标为[x′，y′，z′]^t，同时成像的平面距离相机光心距离为相机的焦距f0；根据三角形相似规则，可得到下式所示：

通常，在成像之后，相机内部会自动将倒立的像转变成为正的图像，所以公式中并未出现负号，默认相机已经自动调节完成，这也是为了后文处理方便。下面将成像坐标系下的坐标单独放到等号的一侧，可得到如式(2)所示：

上式就是质点坐标从相机坐标系o-x-y-z到成像坐标系o′-x′-y′-z′的变换公式；既然是彩色相机成像，就需要储存彩色信息；通畅彩色图片的信息是以像素的形式进行储存的；在计算机中，每个像素点存储着对应的二进制数值，二进制数值也就表示着每个点的颜色；

下面定义像素坐标系c-u-v，其中c为像素坐标系的原心，相对于成像坐标系o′-x′-y′-z′的原心o′平移了[cx，cy]^t；u相当于成像坐标系o′-x′-y′-z′的x′轴，但与之不同的是，图像在u轴上缩放了α倍。同理，v相当于成像坐标系o′-x′-y′-z′的y′轴，图像在v轴上缩放了β倍；这样，由图像坐标转换为像素坐标可由式(3)表示：

结合式(2)与式(3)，可得到质点坐标从相机坐标系o-x-y-z到像素坐标系c-u-v的变换公式，如式(4)所示：

下面，令fx＝αf0、fy＝βf0，f0为相机的焦距，单位为米，α、β单位均为像素/米，所以fx、fy的单位均为像素，整理式(4)可得：

上式即为针孔模型的数学表述，描述了物体坐标从相机坐标系转换到像素坐标系，如果考虑到畸变影响，则需对式(5)进行一定的矫正。

畸变主要分为两种，即径向畸变与切向畸变，二者均是由于光线穿过透镜所引起的，透镜自身对光线的影响以及在安装时透镜不能完全平行于像素平面，这两点因素均会造成畸变；引入畸变纠正系数对其进行修正，不加证明的给出畸变修正方程：

其中，是归一化后的坐标值，即为式(5)中的[x/z，y/z]^t，[xcorrect，ycorrect]^t是经过修正后的坐标；称修正系数k1、k2、k3为径向修正系数，称修正系数p1、p2为切向修正系数；在得到了修正后的坐标后，只需将式(5)中的坐标[x/z，y/z]^t替换成为[xcorrect，ycorrect]^t即可；

通过上述变换，得到彩色相机成像的基本模型；实际上，每个像素坐标[u，v]^t会储存一个数值矩阵i(u，v)∈r^l×h，数值矩阵i中存储着二进制数值，这些二进制数通过确定的关系与计算机中红绿蓝三个通道对应，通过这三种颜色的搭配来组合出实际物体的颜色。

步骤s101中，本发明实施例提供的深度信息获取模型具体为：

选用的传感器为kinect2.0相机，其测量像素深度的原理是飞行时间法；所谓飞行时间法，相机的发射端发出一束脉冲，经过物体的反射再回到相机的接收端，相机通过这个过程所消耗的时间t来计算出飞行距离s，进而得到像素点的深度值；如下式所示。

d＝s/2＝vt/2(7)

步骤s101中，本发明实施例提供的基于相机针孔模型和深度信息获取模型获得数据处理的原则具体为：

(1)由于kinect2.0输入帧频为每秒30帧，设定一个阈值，若无人机的移动距离或者转弯角度没有达到该阈值，则认为采集到的数据集有重复，舍弃到采集到的数据；

(2)由于kinect2.0相机对于彩色rgb图像与深度depth图像的采集是分为两个通道进行的，若在一次采集周期中，彩色rgb图像与深度depth图像时间相差为1/30秒，则认为数据异步，忽略掉采集的数据；

(3)kinect2.0相机的有效采集范围是0.5m～4.5m，超过此范围所采集的数据集均认为是无效数据集；这种做法能够提升整个slam的构图精度，同时也会降低数据集的维度，提高运行效率。

步骤s102中，本发明实施例提供的rgb-dslam算法可以分为前端与后端两部分。

步骤s102中，本发明实施例提供的rgb-dslam算法前端的任务为提取不同观察结果之间的空间关系，后段的任务为使用非线性误差函数优化位姿图中相机的位姿信息。

步骤s103中，本发明实施例提供的八叉树转二维地图具体包括：

(1)建立八叉树模型；

(2)假设相机在深度图中测量到了一个空间点的深度为d，这就代表八叉树模型中d的深度范围之内是没有任何八叉树结点的，说明该条路径没有被占据，可以通过；

(3)取某一高度下的八叉树平面，将某物体所占据的小立方体的最远距离计算出来，以此距离作为半径做圆，将该圆作为无人机的禁飞区，在此基础上进行路径规划。

下面结合而具体实施例对本发明的应用原理做详细说明；

实施例1：

(一)本发明实施例提供的基于rgb-dslam的无人机三维重建方法，具体包括以下步骤：

s101：kinect相机数据提取与预处理：采用开源openni程序框架进行kinect相机数据的提取，采用通讯方式，对提取到的彩色rgb图片信息与深度depth图片信息进行储存；

s102：rgb-dslam算法的实现：利用前端算法对rgb-d图像进行特征检测与描述符提取，对提取到的描述进行特征匹配，根据匹配结果估算运动变换并进行优化；后端算法根据前端算法的结果构建位姿图，然后进行闭环检测与位姿图优化，最终得到的最优相机位姿进行相机定位与三维地图环境重建；

s103：地图转化与路径规划：在生成的三维点云的基础上，将其转化成为八叉树地图，进而生成环境空间的二维地图，对无人机的路径进行规划。

具体的原理及公式如下所示：

1、预处理数据原则：

如图2所示，相机针孔模型；物体所在的环境为世界坐标系o-x-y-z，所成倒立像的平面称之为成像坐标系o′-x′-y′-z′，相机本身又有着自己的坐标系，称之为相机坐标系o-x-y-z；本发明取相机坐标系o-x-y-z与成像坐标系o′-x′-y′-z′的z轴重合，x轴向右，y轴由右手定则确定，o称为相机光心；

如图3所示，为了简化模型，将现实中的物体抽象为一个质点a。该质点在相机坐标系o-x-y-z的坐标为[x，y，z]^t，其反射的光通过相机光心o成像在成像坐标系o′-x′-y′-z′，坐标为[x′，y′，z′]^t，同时成像的平面距离相机光心距离为相机的焦距f0；根据三角形相似规则，可得到下式所示：

通常，在成像之后，相机内部会自动将倒立的像转变成为正的图像，所以公式中并未出现负号，默认相机已经自动调节完成，这也是为了后文处理方便。下面将成像坐标系下的坐标单独放到等号的一侧，可得到如式(2)所示：

上式就是质点坐标从相机坐标系o-z-y-z到成像坐标系o′-x′-y′-z′的变换公式；既然是彩色相机成像，就需要储存彩色信息；通畅彩色图片的信息是以像素的形式进行储存的；在计算机中，每个像素点存储着对应的二进制数值，二进制数值也就表示着每个点的颜色；

下面定义像素坐标系c-u-v，其中c为像素坐标系的原心，相对于成像坐标系o′-x′-y′-z′的原心o′平移了[cx，cy]^t；u相当于成像坐标系o′-x′-y′-z′的x′轴，但与之不同的是，图像在u轴上缩放了α倍。同理，v相当于成像坐标系o′-x′-y′-z′的y′轴，图像在v轴上缩放了β倍；这样，由图像坐标转换为像素坐标可由式(3)表示：

结合式(2)与式(3)，可得到质点坐标从相机坐标系o-x-y-z到像素坐标系c-u-v的变换公式，如式(4)所示：

下面，令fx＝αf0、fy＝βf0，f0为相机的焦距，单位为米，α、β单位均为像素/米，所以fx、fy的单位均为像素，整理式(4)可得：

上式即为针孔模型的数学表述，描述了物体坐标从相机坐标系转换到像素坐标系，如果考虑到畸变影响，则需对式(5)进行一定的矫正。

畸变主要分为两种，即径向畸变与切向畸变，二者均是由于光线穿过透镜所引起的，透镜自身对光线的影响以及在安装时透镜不能完全平行于像素平面，这两点因素均会造成畸变；引入畸变纠正系数对其进行修正，不加证明的给出畸变修正方程：

其中，是归一化后的坐标值，即为式(5)中的[x/z，y/z]^t，[xcorrect，ycorrect]^t是经过修正后的坐标；称修正系数k1、k2、k3为径向修正系数，称修正系数p1、p2为切向修正系数；在得到了修正后的坐标后，只需将式(5)中的坐标[x/z，y/z]^t替换成为[xcorrect，ycorrect]^t即可；

通过上述变换，得到彩色相机成像的基本模型；实际上，每个像素坐标[u，v]^t会储存一个数值矩阵i(u，v)∈r^l×h，数值矩阵i中存储着二进制数值，这些二进制数通过确定的关系与计算机中红绿蓝三个通道对应，通过这三种颜色的搭配来组合出实际物体的颜色。

深度信息获取模型：选用的传感器为kinect2.0相机，其测量像素深度的原理是飞行时间法。

如图4所示，所谓飞行时间法，就是相机的发射端发出一束脉冲，经过物体的反射再回到相机的接收端，相机通过这个过程所消耗的时间t来计算出飞行距离s，进而得到像素点的深度值。如式(7)所示：

d＝s/2＝vt/2(7)

与激光扫描不同的是，飞行时间法可以一次性得到整个图像的深度信息，并不需要逐点扫描，这也增加了效率。

基于上述模型提出针对kinect2.0相机采集到的数据的处理原则为：

(1)由于kinect2.0输入帧频为每秒30帧，以本发明中无人机的飞行性能来看，本发明不需要考虑无人机转弯过快或者飞行过快而导致的数据丢失问题，需要考虑的是无人机飞行过慢导致数据重复而使得算法运行速率降低的问题，该问题会影响slam的实时性，处理方式为：设定一个阈值，若无人机的移动距离或者转弯角度没有达到该阈值，则认为采集到的数据集有重复，舍弃到采集到的数据。此种做法会减少数据集的维度，有效的提升算法的处理性能，并且不会对全局地图的构建与无人机的定位产生明显的影响。

(2)由于kinect2.0相机对于彩色rgb图像与深度depth图像的采集是分为两个通道进行的，所以不得不考虑采集数据的同步性问题，处理方式为：若在一次采集周期中，彩色rgb图像与深度depth图像时间相差为1/30秒，则认为数据异步，忽略掉采集的数据。

(3)如前文所述，kinect2.0相机的有效采集范围是0.5m～4.5m，超过此范围所采集的数据集均认为是无效数据集。这种做法能够提升整个slam的构图精度，同时也会降低数据集的维度，提高运行效率。

2、八叉树转二维地图：

如图5所示，八叉树是指将一个立方体的每个面四等分，这样就得到了八个小立方体。在定义了分割精度以后，可以将空间全部用小立方体表示，比如定义深度d＝8，一个小立方体的精度为1cm³，这样所分割整个空间的大小为8⁸cm³＝16.78m³。整个空间从大到小细分的过程，就是八叉树模型建立的过程。

假设相机在深度图中测量到了一个空间点的深度为d，这就代表八叉树模型中d的深度范围之内是没有任何八叉树结点的，说明该条路径没有被占据，可以通过。

根据此思想，取某一高度下的八叉树平面，将某物体所占据的小立方体的最远距离计算出来，以此距离作为半径做圆，将该圆作为无人机的禁飞区，在此基础上进行路径规划。

(二)结果

rgb-d算法前端与后端产生的地图中，点云地图的误差是比较大的，甚至后面还有多出的部分，这是与实际严重不符的。在后端非线性优化，并且使用了回环检测后，整个地图较只有前端时得到了改善，环境已经可以被直观的认出，基本达到了预先的要求。该结果使用了上述预处理数据原则，有效的解决数据维度过大的问题，使得构建出的地图趋于真实场景。

使用数据集继续进行仿真实验，可得到八叉树地图。

由图可知看出每个障碍物出现的位置，一般情况下，无人机在飞行阶段只考虑横向运动，所以可以将无人机三维航路规划问题转换成为在某一确定高度下的无人机二维航路规划。因此，所要规划的问题为：在某一确定高度下，无人机始末位置之间的路径。考虑到环境中出现的障碍物，可以考虑将其设定为某一平面下的圆来近似，该圆所围成的区域是最小涵盖障碍物的。通过对障碍的简化处理，得到障碍分布平面，在此基础上进行航路规划。主要目标是：尽可能规避各障碍，并考虑无人机飞行性能限制，达到预期飞行目的。

如图6所示，该仓库在高度为1.2m时的障碍分布平面，后文的部分将依据此二维地图进行仿真。图中左下角为无人机起点，右上角为无人机终点，黑色曲线为无人机可能飞行的路径，两个圆代表障碍区域。该地图长为4米，宽为6米，起点位置坐标(0m，0m}，终点位置坐标(4m，6m}，两个障碍区域的圆心坐标为(1.5m，3m}和(4m，3m}，半径分别为1.2m和0.8m。

根据图6所构建出的二维平面地图进行路径规划，采用a*搜索算法进行搜索。

a*算法是有效的寻路搜索算法，其基本思路是给定一个代价函数f(n)，代价函数由下面两个部分组成，分别为起始点到当前点的代价g(n)、当前点到目标点的代价h(n)。具体公式如式(8)所示：

f(n)＝g(n)+h(n)(8)

此处的代价函数选用的是欧式距离，即h(n)与g(n)均采用两点之间距离。虽然a*算法搜索效率很高，但是直接应用到无人机上面会面临很多问题，比如a*只考虑到实际代价并未考虑无人机的最大转弯角度的限制。基于此问题，提出一种改进a*算法，将无人机的最大转角考虑进去。

如图7所示，假设无人机在每次制导周期内的转角大小为θmax。

如图8所示，改进a*搜索路径的示意图。

依照上述改进模型，同时采用图6所示二维地图，改进a*算法产生的路径如图8所示。图8的横纵坐标并不代表真实长度，而是a*算法中用到的网格，图8与实际长度的换算关系为：一单位网格等于0.05m。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。