基于空-地协同的轨迹规划方法

黄肖肖,曹 凯,王 杰

(山东理工大学 交通学院，山东 淄博 255049)



基于空-地协同的轨迹规划方法

黄肖肖,曹 凯,王 杰

(山东理工大学 交通学院，山东 淄博 255049)

针对车载传感器感知全局环境信息能力不足以及车辆导航规划轨迹的平滑性、安全性的问题，提出一种空-地协同的轨迹规划方法。为此，构筑了无人机全局信息采集平台及地面车辆信息处理平台，其中无人机从空中采集广域地面环境图像信息，并实时回传给地面车辆信息处理平台，采用图像拼接算法构筑全景地理信息图。为了实现车辆导航规划轨迹的平滑性和安全性，提出引入FM2算法计算全局最佳轨迹。仿真结果表明：空-地协同能够大幅提升地面车辆对全局环境信息的掌控能力，而且FM2算法保证了全局规划轨迹的平滑性和安全性。

智能运输系统；轨迹规划；FM2算法；空-地协同；智能车辆；无人机

0 引言

智能车辆是未来交通领域重要的研究方向，目前国内外都在研究并取得许多成果，应用于军事、消防、救灾和智能交通等领域。其中，作为环境地图构建、避障技术、车道识别及导航核心技术的轨迹规划是当前研究的重点问题。

在环境信息未知或部分可知的情况下，由于基于车载传感器的智能车辆对环境感知能力存在较大的局限[1]，只能实现局部的轨迹规划，无法实现全局轨迹规划[2]。而无人机具有机动灵活和视野宽阔的特点，可以在特定高度对地面环境进行大范围的观测。因此，实现无人机与地面智能车辆的空地协同可以弥补智能车辆全局环境感知方面的缺陷，提高智能车辆全局轨迹的把握能力[3-6]。

在传统的轨迹规划方法中，许多方法需要基于外部环境的结构化和符号进行推理。由于这些方法过于依赖理想化的环境模型，不适用于动态、不确定的环境。针对这个问题，近年来提出了一系列进化算法与其他算法组合的规划方法，如进化算法与递归神经网络组合、进化算法与竞争机制结合、进化算法与遗传算法结合等。然而，这些算法计算复杂，而且随着环境参数的增加，增加存储量导致计算性能急剧下降，因而在实际环境中使用缺乏灵活性和实时性。

为了适应环境特征，提出基于随机采样的轨迹规划方法，该方法通过人工势场的负梯度方向搜索一条从初始点到目标点的轨迹，并用随机步长避免了势场函数局部极小值对规划的影响。但这种规划方法在遇到狭窄通道问题时会受到阻碍。为此，以Barraquand和LaValle为主的研究者提出了一系列基于随机采样的改进规划算法[7-9]，与以前算法相比，这些算法通用性更强，而且规划过程中并不用精确地计算空间，而是通过碰撞检测来判断位姿点是否位于自由空间中，由此避免了对复杂自由空间和障碍空间进行描述的前期计算。

目前，基于随机采样的快速行进方法[10-12](Fast Marching Method，FMM)是一种有效的轨迹规划方法，是基于数值计算的网格算法，以速度快和精度高的特点而广被应用。但是，其缺点就在于基于随机(或伪随机)空间采样可能导致在远离最佳点产生一种非最优随机轨迹，而且存在一些轨迹是非平滑，与障碍物间隙过小使得在通行过程中可能产生危险等问题。

针对智能车辆感知空间的限制和上述轨迹规划算法存在的问题，本文提出协同空中无人机与地面车辆构建一个空-地协同轨迹规划系统。系统中的无人机采集地面环境图像，并实时回传给地面车辆的车载信息处理平台，通过采用图像拼接技术构筑全景地理信息图，计算出全局最佳轨迹；而车载信息处理平台融合规划轨迹以及该轨迹周边的环境信息对地面车辆进行导航。为了使规划的轨迹更平滑且安全性更高，本文引入一种可以提高以采样为基础的轨迹平滑度和轨迹安全度的轨迹规划算法。

1 空地协同及全局环境信息构建

1.1 空地协同系统构建

如图1所示，系统主要由无人机子系统和地面智能车辆子系统两部分构成。无人机子系统主要包括四旋翼无人机，该无人机带有两个240×320分辨率的彩色摄像头，一个采集前方空域信息，保证无人机在一定高度空域避障的需要,另一个采集地面环境信息。此外，无人机下腹部还装载了一个用于测量高度的超声波传感器。而且通过UDP协议(无人机采用WiFi(802.11))与车载信息处理平台进行双向通信。通过WiFi，无人机将环境信息传递给车载信息处理平台，或接收车载信息处理平台上传的控制指令及任务。

图1 空地协同流程Fig.1 Flowchart of air-ground coordination

智能车辆子系统采用的是Mobile-Robots公司生产的Pioneer 3-AT地面智能车辆。该智能车辆子系统包括两个平台：无人机起降平台和信息处理平台。无人机起降平台提供无人机自主起降功能(无人机的自主起降另文阐述)，信息处理平台提供无人机位姿控制、图像处理、轨迹规划计算以及车辆控制等。

1.2 全景环境信息图构建

空地协同过程中，图像序列拼接是将一组有一定重合地理环境区域的图像序列拼接为一幅能够更为全面描述环境内容的全景图像的过程，它能够弥补单幅图像分辨率低、视野范围小的缺陷，有助于地面车辆对全局环境有更全面、更直观的了解，便于依据全局环境特征规划安全轨迹。

空地协同系统中的无人机依据任务要求自主从起降平台起飞，按照设定的任务航线进行飞行，遵从设定的拍摄方式对未知环境进行地理信息采集，实时回传给车载信息处理平台，并通过图像拼接算法，构建未知环境全景地理信息图。

基于地理环境区域的图像拼接具体做法是：采用的是尺度不变特征变换(SIFT)算法[13]，通过对全局图像进行特征点匹配，实现对图像的粗线条对接，然后利用随机抽样一致性(Random Sample Consensus，RANSAC)算法对提取特征点中的局外点进行剔除，从而解决一致性问题，实现图像的精确拼接。

SIFT特征匹配步骤：

(1)尺度空间极小点检测;

(2)特征点精确定位;

(3)特征点方向分配;

(4)生成本地特征描述符;

(5)特征点匹配。

在SIFT特征点匹配基础上，为进一步实现图像准确拼接，需要获得单应矩阵，使待拼接图像满足影射变换关系。为此，采用RANSAC算法对提取的特征点中的局外点进行剔除，从而达到特征一致。最后，依据匹配的特征点对获得的图像间变换参数进行拼接和融合。

图像拼接流程如图2所示。

图2 图像拼接流程Fig.2 Flowchart of image mosaicing

根据上述图像拼接算法及流程，以本研究空路协同轨迹规划验证环境图为例，描述拼接验证环境图像过程如下：

(1)车载信息处理平台读取无人机图传的图像，如图3所示。

图3 待拼接图像Fig.3 Images to be mosaiced

(2)利用SIFT算法获取待拼接图像的特征点，如图4所示。

图4 特征点提取Fig.4 Feature point extraction

(3)利用RANSAC方法对特征点进行匹配，如图5所示。

图5 特征点匹配Fig.5 Feature point matching

(4)最后图像拼接结果如图6所示。

图6 图像拼接结果Fig.6 Result of image mosaicing

2 空地协同轨迹规划

空路协同轨迹规划是以全局环境信息为基础，因此，在取得全局信息之后，有效的轨迹规划算法尤为重要。目前随机采样的轨迹规划方法是普遍采用的有效方法，其中，FMM算法以速度快和精度高的特点而被广泛应用。

2.1 FMM算法

在快速行进方法中，给定点的运动方程可以用Eikonal方程表示：

F(x)|T(x)|=1，

(1)

式中，x为给定点的位置；F(x)为波在该点的传播速度，且始终为非负值；T(x)为波前到达该点所需的时间。

由式(1)可以看出，到达时间函数T(x)的梯度大小和速度函数F(x)成反比例关系。由于F(x)始终为非负值，则表明波始终向前传播，因此，离起点距离越远，到达时间T越长。

在二维空间中，Sethian[15]对Eikonal函数曾提出一种利用栅格进行离散的求解方案。即，设i,j分别表示与实际环境中点P的坐标(xi,yj)对应的栅格地图的行和列。于是，可以通过如下公式对时间梯度T(x)进行离散处理。

T=Ti,j，

T1=min(Ti-1,j,Ti+1,j)，

T2=min(Ti,j-1,Ti,j+1)，

(3)

并代入式(2)中，可以得到二维空间的Eikonal方程为：

(4)

假设波前的速度恒为正，那么波前未经过坐标(i,j)点的到达时间T一定大于T1和T2。那么可以将式(4)简化为：

(5)

式中，T1,T2分别为在x和y轴方向的最小到达时间。

为了实现对式(4)求解，在栅格地图上采用多次迭代的方法。为此，对栅格地图中的单元按以下规则进行标注：

(1)Unknown：波还未经过该点，到达时间T是未知的。

(2)Narrow Band：下一次迭代过程中波前的栅格单元。该点的到达时间已知，但是可能在下一个迭代过程中改变。

(3)Frozen：波前经过的栅格单元，其到达时间T为固定值。

快速行进算法共分初始化、循环和结束3个阶段。在初始化阶段，设定波起始位置栅格的到达时间T=0，并标记为frozen。然后，将与初始位置距离为曼哈顿距离的所有栅格标记为narrow band，利用式(4)计算每个narrow band的到达时间T。在每次循环迭代过程中，利用式(4)对与narrow band距离为曼哈顿距离的所有栅格单元进行求解，得到到达时间T。然后将narrow band标记为frozen。一直循环迭代，直至所有的栅格单元标记为frozen止，算法结束。整个迭代过程可形象地表示图7的过程。

图7 FMM迭代过程Fig.7 Iterative process of FMM

FMM的伪代码实现过程如下：

FMM实现输入:栅格地图G，大小为m×n，起始点用x0表示。输出:带有到达时间T的栅格地图g。{初始化}1:for∀gij∈x0do2:gij·T←0;3:gij·state←FROZEN;4:for∀gkl∈gij·neighboursdo5:ifgkl=FROZENthen6:skip;7:else8:gkl·T←solveEikonal(gkl);9:ifgkl·state=NAVROWBANDthen10:narrow_band.update_position(gkl)11:endif12:ifgkl·state=UNKNOWNthen13:gkl·state←NARROWBAND;14:narrow_band.insert_in_position(gkl)15:endif16:endif17:endfor{Loop}18:whilenarrow_bandNOTEMPTYdo19:gij←narrow_band.pop_first()20:for∀gkl∈gij·neighboursdo21:ifgkl=FROZENthen22:skip;23:else24:gkl·T←solveEikonal(gkl)25:endif26:ifgkl·state=NARROWBANDthen27:narrow_band.update_position(gkl);

FMM实现28:endif29:ifgkl·state=UNKNOWNthen30:gkl·state←NARROWBAND;31:narrow_band.insert_in_position(gkl);32:endif33:endfor34:endwhile35:endfor

2.2 FM2算法

目前，基于FMM的最优轨迹评价是按照最小欧式距离准则进行的。但是，在由此生成的规划轨迹中有些部分可能距离障碍物太近，因此安全性差。有些部分平滑性差，在车辆行进方向上可能会出现陡峭的变化，导致智能车或者急降车速或者急转弯如图8所示，致使智能车的轨迹追踪误差较大。针对FMM的轨迹规划存在安全性、平滑性等缺点，本文提出采用FM2(Fast Marching Square，FM2)[16]算法。

图8 FMM轨迹生成图Fig.8 The result of FMM path planning

所谓的FM2算法是对FMM方法的一种改进，它首先通过FMM生成一个速度势场关系图，并根据与障碍物之间的距离关系调整波的传播速度大小，生成速度图谱(velocity map)，从而规划出既平滑又安全的轨迹。

FM2算法步骤如下：

(1)环境构建。通过处理无人机采集的图像信息，生成全局环境的二值图像。为了使规划的轨迹更加安全，以智能车的外接圆半径为参考，对二值图像中的障碍物进行膨胀(dilate)处理。

(2)速度图谱构筑。以膨胀后的栅格地图作为FMM算法的输入，经过处理后输出带有到达时间T的栅格图，并将栅格中所有的障碍物均视为波源。然后将到达时间T和车辆速度映射到区间[0,1]上，于是，距离障碍物越近的像素点的时间T和速度值也就越接近于0，反之，T和速度就接近于1，即接近车辆限定速度。因此可以将该栅格图视为速度图谱，像素代表不同的速度。

(3)生成波传播的时间图。将步骤(2)中生成的速度图谱中作为输入，再次使用FMM，生成带有到达时间的传播图。在这一过程中，波的传播速度和速度图谱中的速度相对应，即在(x,y)点波是按照速度图谱中(x,y)点的速度进行传播。

(4)获得最优轨迹。从起始点开始，沿波前梯度下降方向一直到达目标点，所形成的轨迹就是FM2算法最终规划的最优轨迹。

FM2算法代码实现输入:栅格地图G，起始点为xinit，目标点为xgoal，智能车辆尺寸为d，障碍物栅格为g输出:规划轨迹为ρ，控制执行U{初始化}1:c_space_map←Creat_c_space_map(G，d){FM2模型构建，膨胀，创建地图}2:p_map←FMM(c_space_map，λ){FMM算法一次执行}3:fm2_map←FMM(vp_map，xinit，xgoal){FMM算法二次执行}4:ρ←Geodesic_path(fm2_map，xinit，xgoal)5:U←control_actions(fm2_map，path)

总结以上FM2算法过程，其优势如下：

(1)该算法构造的势场函数可以确保当且仅当在目标点处出现局部极小，从而保证了在目标点之外不会陷入局部极小。

(2)该算法复杂度较低，并且输入信息较为简单，因此信息处理速度快，从而大大降低了计算时间。

(3)只要速度图谱是连续的，就可以保证通过该算法所获得轨迹的平滑性，而且不需要再次优化处理。

(4)该算法不仅保证了规划轨迹的平滑性，而且回避了在小于车辆最大尺寸的通道处规划轨迹，因此提高了车辆通行的安全性和可靠性，避免了局部避障和全局规划之间的冲突问题。

(5)只要该算法有解，总会从起始点到目标点之间找到一条轨迹，从而保证了轨迹的完整性。

3 实例验证

该试验假设了一个道路交通事故救援场景，见图9(a)，图9(b)的标记“*”代表智能车辆的目前所处位置(又称起始点)，标记“×”表示事故现场(又称目标点)。智能车辆得到交通事故救援信息后，无人机从智能车辆上起飞，获取从智能车辆到事故现场的地理环境全景图。

图9 全局环境信息Fig.9 Global environment information

图10 基于FMM算法的波传播及轨迹规划图Fig.10 Wave propagation and trajectory plan based on FMM algorithm

根据FMM算法的计算过程，波从目标点开始向起始点进行探索，见图10(a)，然后从起始点开始采用梯度下降虚线算法规划出一条时间上最短的轨迹,见图10(b)部分。但是如图10(b)所示，在该轨迹上存在陡峭的转折点，智能车辆不仅需要变速、转弯前行，而且轨迹距离障碍物过近，使车辆的安全性无法得到保障，所以该轨迹不是最理想的轨迹。

为了使轨迹安全可靠，采用FM2算法进行轨迹规划。首先对障碍物及墙体进行膨胀扩大，利用FMM形成速度图谱，如图11(a)所示。在速度图谱中，越明亮的地方，表明对智能车辆的干扰越小，智能车辆的行进速度就越快。在速度图谱中再次利用FMM计算得到轨迹，如图11(b)所示。

图11 速度图谱及轨迹Fig.11 Velocity map and trajectory

图12 基于FM2算法的轨迹规划Fig.12 Path planning based on FM2 algorithm

最后，通过FM2算法规划的完整轨迹如图12所示，与图10(b)所示的快速行进方法规划的轨迹相比，更加光滑，没有陡峭的转折点，且与障碍物及墙体之间拥有足够的安全距离，所以由此规划的轨迹可以满足智能车辆的约束条件。

4 结论

针对智能车辆车载传感器感知空间的限制，无法获取全局的环境信息的问题，提出无人机与地面智能车辆协同轨迹规划的系统。该系统利用无人机视野宽阔，可以采集智能车辆无法感知到的未知环境信息，大大扩展了智能车辆对环境的感知能力。由于引入了FM2算法，与传统轨迹规划算法相比较，规划轨迹平滑性更好，安全性更高，并且除了目标点外，全局轨迹上不会出现局部极小问题，因此，规划的轨迹更适合智能车辆的轨迹跟踪控制。

[1] 梅元刚. 空地机器人协作环境建模与路径规划[D]. 北京：中国科学院大学, 2014. MEI Yuan-gang. Aerial Robot and Ground Robot Cooperate for Environment Modeling and Path Planning[D]. Beijing: University of Chinese Academy of Sciences, 2014.[2] 曹凯,周芦芦,张政新. 基于道路势场的智能车辆机动驾驶控制算法[J]. 系统仿真学报, 2011, 23(10):2206-2210. CAO Kai, ZHOU Lu-lu, ZHANG Zheng-xin. Controlled Algorithm for Intelligent Vehicle Maneuver Driving Based on Road Potential Field [J]. Journal of System Simulation, 2011, 23(10):2206-2210.

[3] SAUTER J A, MATHEWS R S, YINGGER A, et al. Distributed Pheromone-Based Swarming Control of Unmanned Air and Ground Vehicles for RSTA[C] // Proceedings of SPIE: The International Society for Optical Engineering. Orland：Soar Tech Press，2008.

[4] GIAKOUMIDIS N, BAK J U, GOMEZ J V, et al. Pilot-Scale Development of a UAV-UGV Hybrid with Air-based UGV Path Planning[C] //Proceedings of 10th International Conference on Frontiers of Information Technology. Islamabad: IEEE, 2012: 204-208.

[5] MATHEW N, SMITH S L, WASLANDER S L. Planning Paths for Package Delivery in Heterogeneous Multirobot Teams[J]. IEEE Transactions on Automation Science & Engineering, 2015, 12(4):1298-1308.

[6] 谷丰,王争,宋琦,等. 空地机器人协作导航方法与实验研究[J]. 中国科学技术大学学报,2012,42(5):398-404. GU Feng, WANG Zheng, SONG Qi,et al. Theoretical and Experimental Study of Air-ground Cooperative Navigation[J]. Journal of University of Science and Technology of China, 2012, 42(5):398-404.

[7] BARRAQUAND J, LATOMBE J C. A Monte-Carlo Algorithm for Path Planning with Many Degrees of Freedom[C]//1990 IEEE International Conference on Robotics & Automation. Cincinnati: IEEE, 1990: 1712-1717.

[8] KAVRAKI L E,VESTKA, PETR, LATOMBE J C, et al. Probabilistic Roadmaps for Path Planning in High-dimensional Configuration Spaces[J]. IEEE Transactions on Robotics & Automation, 1996, 12(4):566-580.

[9] LAVALLE S M, KUFFNER Jr, J J. Rapidly-exploring Random Trees: A New Tool for Path Planning, TR 98-11 [R]. Ames: Iowa State University, 1998.

[10]GARRIDO S, MALFAZ M, BLANCO D. Application of the Fast Marching Method for Outdoor Motion Planning in Robotics[J]. Robotics and Autonomous Systems, 2013, 61(2): 106-114.

[11]GOMEZ J V, LUMBIER A, GARRIDO S, et al. Planning Robot Formations with Fast Marching Square Including Uncertainty Conditions[J]. Robotics and Autonomous Systems, 2013, 61(2): 137-152.

[12]DO Q H, MITA S, YONEDA K. Narrow Passage Path Planning Using Fast Marching Method and Support Vector Machine[C]//2014 Intelligent Vehicles Symposium Proceedings. Dearborn: IEEE,2014: 630-635.

[13]YIN J J, CHENG G J, LIU N, et al. Rock Core Thin Section Image Stitching Based on SIFT Features[J]. Advanced Materials Research, 2014, 1049/1050: 398-401.

[14]GOMEZ J V, LUMBIER A, GARRIDO S, et al. Planning Robot Formations with Fast Marching Square Including Uncertainty Conditions[J]. Robotics and Autonomous Systems, 2013, 61(2): 137-152.

[15]SETHIAN J A. Level Set Methods and Fast Marching Methods[J]. Journal of Computing and Information Technology, 2003, 11(1): 1-2. [16]VALERO-GOMEZ A, GOMEZ J V, GARRIDO S, et al. The Path to Efficiency: Fast Marching Method for Safer, More Efficient Mobile Robot Trajectories [J]. IEEE Robotics & Amp Automation Magazine, 2013, 20(4):111-120.

A Trajectory Planning Method Based on Air-ground Cooperation

HUANG Xiao-xiao, CAO Kai, WANG Jie

(School of Transportation, Shandong University of Technology, Zibo Shandong 255049, China)

In view of the problems of insufficient ability of vehicular sensor to perceive the global environment information and the smoothness and safety of vehicle navigation planning trajectory, a trajectory planning method based on air-ground cooperation is proposed. To this end, the UAV global information collection platform and the ground vehicle information processing platform are constructed, where UAV collects wide-area ground environment image information from the air and passes back to the information processing platform on the ground vehicle for constructing panoramic map of geographic information with an image mosaic algorithm. In order to realize the smoothness and the safety of vehicle navigation planning trajectory, a FM2algorithm is proposed to calculate the global optimal trajectory. The simulation result shows that the air-ground coordination can significantly improve the control ability of ground vehicle to global environmental information, and the FM2algorithm can guarantee the smoothness and the safety of the planned global trajectory.

ITS; trajectory planning; FM2algorithm; air-ground cooperation; intelligent vehicle; unmanned aerial vehicle (UAV)

2016-02-01

国家自然科学基金项目(61573009)

黄肖肖(1991-)，男，山东高青人，硕士.(18369959856@163.com)

10.3969/j.issn.1002-0268.2016.11.020

U463.2; TP18

A

1002-0268(2016)11-0134-06