野外无人车路径规划与轨迹规划技术

2021-05-27陈智伟胡劲文赵春晖侯晓磊

无人系统技术 2021年2期

陈智伟，胡劲文，赵春晖，王策，侯晓磊，潘泉，王腾，徐钊

野外无人车路径规划与轨迹规划技术

陈智伟1，胡劲文1，赵春晖1，王策1，侯晓磊1，潘泉1，王腾1，徐钊2

（1. 西北工业大学自动化学院信息融合技术教育部重点实验室，西安 710072； 2. 西北工业大学电子信息学院，西安 710072）

常规布局及结构化的城市环境中的无人地面车，其环境感知和路径规划技术的研究相对成熟。然而，无人车在信息未知、环境复杂的野外进行自主规划和障碍规避尚存在困难。针对复杂环境中无人车的路径规划和轨迹规划技术展开研究，首先，考虑环境相对未知、存在凹凸障碍物和起伏地面等因素，利用激光雷达、相机和IMU组成车载多传感器系统获取复杂的野外环境信息并进行数据处理和校正；其次，通过训练径向基神经网络（RBF网络）对野外环境进行建模；然后，基于环境模型引入起点至目标位置的距离、环境高度和梯度以构建约束，通过优化约束函数实现复杂环境中无人车的路径规划；最后，基于Minimum Snap的思想，利用阶多项式拟合得到的路径，将规划后生成的折线路径优化为最终需要跟踪的轨迹。通过仿真实验验证了方法的有效性：所提出的方法实现了无人地面车在野外环境中自主路径规划和轨迹规划。

无人地面车；环境感知；路径规划；神经网络；约束优化；轨迹规划

1 引言

随着传感、控制、计算等关键性技术的不断发展和硬件系统的升级，无人车成为智能领域的新兴产物，无人驾驶技术也成了科研界的热门研究课题[1]。无人地面车辆是最常见的无人系统之一，在已知环境和简单地形中，无人车的路径规划技术研究相对成熟。然而，在野外环境中，先验信息不足，不仅地面起伏不平，无人车在执行任务时还可能会遇到石块、土堆、凹坑等不同种类的障碍物，野外复杂的环境情况给车辆前进和安全行驶造成困扰。在复杂环境中自主行驶的无人车可代替人类执行危险任务，如作战打击、运输物资、搜索救援等。在科学研究方面，无人车对野外复杂环境的高度适应力在未来外星勘测和采集工作方面具有值得展望的前景；在经济效益方面，野外无人车技术的研究对未来无人汽车减少事故具有应用价值[2]。

野外无人车控制的核心技术是路径规划算法，分为经典路径规划算法和启发式路径规划算法。典型的路径规划算法包括细胞分解法、采样法和基于抽样的方法，如快速探索随机树（RRT）[3]等。状态空间过大或情况相对不可预测时，典型路径规划方法效率降低，甚至无法完成。启发式搜索通过设计评价省略了无意义的计算，常见的启发式搜索算法有遗传算法（GA）[4]、蚁群算法（ACO）[5]、粒子群优化算法（PSO）[6]等，这些算法和基于这些算法简单改进的启发式路径规划方法，在复杂度高和需要实时判断的野外环境中并不适用。解决野外复杂环境中的路径规划问题，需要对传统的路径规划算法进行优化，必要时，需要与其他算法相结合以提高解决问题的有效性。Q-learning算法[7]是增强学习算法之一，基于该方法优化改进的经验‒记忆‒学习（EMQL）算法[8]用以实现自主学习下的路径规划。这类强化学习的方法是一种在研究路径规划时用于提高移动机器人环境适应度的框架，但该方法属于全局规划，难以满足实时性要求。A*算法广泛用于机器人避障，Cheng等人[9]提出了用动态窗和A*算法结合来优化路径规划的技术，检测全局地图中突然出现的障碍物实现局部避障，提高了移动机器人运动时全局最优和实时避障的能力，但该算法的计算量会随环境复杂度提高而增大，收敛慢。韩明等人[10]针对传统方法收敛慢和局部最优的问题，对粒子群优化下路径规划算法进行了改进，这种改进的方法具有较快的收敛速度，但倾向于将环境信息纳入路径规划中同时考虑，对自适应性的要求较高。Germi团队针对移动机器人的GA路径规划算法，提出一种利用自适应势场优化路径规划方法的技术[11]，提高了路径规划对环境的自适应性，但路径规划时只求可行性，无法保证最优性。成昌巍等人[12]针对局部最小值和不可达点的问题改进了人工势场路径规划算法，这种改进型算法能使机器人避免陷入局部最优。路径规划得到的是空间位置点，轨迹规划得到最终的轨迹。Yu等人[13]在机器人任务运动研究中通过多项式对关节定点进行插值，提高了机器人运动的平稳性，但罗列的动力学条件较多，拟合条件比较苛刻。Goll等人[14]提出了基于2D网格的地面机器人轨迹规划方法，通过考虑路径和执行误差实现了非理想运动的轨迹拟合，但该方法是一种局部轨迹规划方法。

本文提出了一种基于环境建模的野外无人车路径规划和轨迹规划算法。直接进行野外环境下的路径规划较为困难，可利用环境感知技术和路径规划技术相结合的方式达到目的。首先，通过传感器获取环境信息并处理，再通过训练RBF神经网络进行环境建模，得到较为精确的野外环境模型。其次，通过模型提取所需的高度和梯度信息作为参考，构建约束并优化约束函数进行从起点到终点的路径规划。最后对路径进行优化，由于得到的路径是由空间位置点连接成的折线路径，基于Minimum Snap思想用阶多项式进行曲线拟合，达到轨迹规划的目的。

2 环境感知与建模

2.1 多传感器系统

在野外环境中，由于光线遮挡和复杂地形等因素，单独使用视觉传感器进行感知会产生偏差。随着环境感知技术的发展，多传感器融合可帮助无人车在复杂的野外环境中获取更加准确的信息。使用三维激光雷达、单目相机和惯性测量单元（IMU）组成传感器系统进行环境感知，雷达是核心传感器，相机和IMU是辅助传感器。雷达、相机和IMU将输入点云信息、图像信息和姿态角信息，其表达式分别为

2.2 点云数据处理和位姿校正

然后利用IMU得到的姿态角信息算出旋转矩阵，通过旋转矩阵实现车体坐标系和地面坐标系的匹配，坐标转换的表达式为

2.3 基于RBF神经网络的环境建模

如何通过传感器信息正确建立野外环境模型是后续做路径规划的基础。野外环境中的障碍物种类复杂多样，包括土坑、树木、石块等。对于路径规划技术，不需要分析具体的障碍物种类，只要获取不同类别的地面信息作为建立环境模型的参考输入即可。野外环境中，高于地面的障碍物称为凸障碍物，低于地面的障碍物称为凹障碍物。激光雷达扫描得到的点云可以反映两类不同的障碍物，把传感器信息作为输入，在建立准确的环境模型后，利用模型正确设计路径规划的约束即可达到避障的目的。环境建模的方法有构建环境的地图模型和数学模型。前者的代表是目前比较热门的同时定位与地图构建技术（SLAM），这种方法需要大量的储存空间和计算成本。相对于前者的高计算成本，无人车系统对实时性的要求更高，本文采用后者的思想，使用将传感器信息放入神经网络训练回归数学模型来进行环境建模。

RBF神经网络是一种局部逼近的神经网络，能实现以高精度逼近任意连续的非线性网络。它是一种单隐层前馈神经网络，包括输入层、隐含层和输出层。输入层是传感系统信号源的输入，隐含层指隐含神经元，输出层则是隐含层的加权线性组合。因为混合高斯模型可拟合任意函数分布，一般取高斯核函数为神经元。如图1即是一个简单的径向基神经网络结构，包含两个信号源输入、四个隐含层高斯核函数，将通过训练得到需要的线性输出。当信息更加复杂时，可增加隐含层神经元的数量，因此，通过个隐含神经元，径向基神经网络不仅可以近似任意函数，而且网络权值求取能由线性方程组求解得到。即网络由输入到输出的映射是非线性的，而网络输出对可调参数又是线性的。

图1 径向基神经网络

2.4 环境模型的复杂性

环境的复杂性因素可以由两方面来描述：一是环境的结构和数据量；二是环境建模的复杂度。结构越复杂、数据越多，反映的信息越详细，但是用于保存环境信息的内存开始变大，难以实时更新和快速规划；相对地，若模型结构简单，数据量小，则会丢失有用的环境细节。环境建模的复杂度则表明建模速度的快慢，若复杂度太大，则建模困难，难以反映环境变化。

本文提出的基于数学模型的建模，其复杂性低于地图建模的SLAM，不论是与建立语义地图还是拓扑地图比，数学模型的数据和结构均相对简单，最重要的是从模型函数中提取数据更为简单。采用数学模型建模时，训练单隐层前馈RBF神经网络比训练多隐含层的BP网络效率高、收敛快、效果好。

3 基于环境模型的无人车路径规划

3.1 基于模型的路径规划策略

环境建模将存在障碍物、情况相对未知的复杂野外环境抽象成数学模型，基于无人地面车辆的路径规划转化成数学模型中的优化求解。此时，野外无人车路径规划的策略为：通过提取环境模型中所需要考虑的环境信息，综合考虑初始位置到目标位置的距离作为约束条件，再通过优化代价函数求解路径点。本文引入距离、梯度和高度构建约束函数，通过优化约束函数求取空间路径点。

3.2 约束函数的构建和路径生成

其中：前两项为环境数学模型对坐标的偏导，用以描述环境梯度，进而通过梯度值是否突变表征是否遇到障碍物地形；第三项为高度。无人车路径规划的目的是找到一条离目标点距离近、高度和梯度综合值比较小的路径。综合考虑这些物理量，约束函数可以设计为

设计好约束函数后，通过求解带不等约束的非线性优化问题生成路径点，达到路径规划的目的。即求解：

4 野外无人车轨迹规划方法

4.1 多项式拟合路径生成轨迹

路径规划求解得到路径点，最终路径是由坐标点连成的折线路径。本节研究将折线路径优化成曲线轨迹的轨迹规划技术。

一个多项式轨迹无法拟合复杂的曲线。在时间同步和顺序不变的前提下，将需要优化成曲线轨迹的路径分成多段，每一段都是一个阶多项式轨迹。即

4.2 基于Minimum Snap思想构建目标函数和等式约束

因此，经过运算可得到优化目的为最小化目标函数：

图2 轨迹规划的等式约束

得到目标函数后，为进行轨迹规划还需要构建等式约束。如图2所示，多项式轨迹拟合的等式约束包括两部分，第一是导数约束，即位置、速度、加速度的约束；第二是连续性约束，即位置、速度、加速度在时间策略下具体如何相等的约束。包括将起点和终点的位置、速度、加速度作为约束条件；将中间所有点的位置、速度和加速度连续作为约束条件；将中间所有点在时间策略下的位置匹配作为约束条件。通过等式约束下最小化目标函数可以求得轨迹参数。

5 仿真实验

5.1 实验平台与实验场景

本文进行的实验基于优艾智和Youibot3无人地面车辆。如图3所示，把无人车放置在一个30 m´20 m的矩形野外环境，地面起伏不平。设置相应的凸障碍物和凹障碍物：实验场地中用三个约0.8 m高的箱子和1个约0.6 m高的土堆代表需要避开的凸障碍物，用一个深0.8 m的土坑代表需要避开的凹障碍物。

图3 实验场景

5.2 环境感知和建模仿真

图5 野外环境模型

5.3 路径规划仿真及分析

图6 路径规划仿真结果

图7 提高环境信息权值后路径规划仿真结果

5.3 轨迹规划仿真

本文采用五阶多项式拟合路径并将其优化成曲线轨迹，得到轨迹规划点后计算每一段路径的距离和总的距离，初始化多项式阶数为5，参数个数为6，轨迹条数为‒1 = 24。初始化总的分配时间为= 25，求解矩阵，利用时间分配构建位置连续约束，通过循环求取其他约束。用二次规划函数求解目标函数和约束下的所有参数，返回参数并画出轨迹图，得到轨迹曲线。最终仿真结果如图8所示，图6得到的折线路径成功拟合成最终需要跟踪的曲线轨迹。

图8 轨迹规划仿真结果

6 结论

在存在障碍物和情况相对未知复杂的野外环境中，无人车的路径规划相对困难和复杂。针对该问题，将环境感知技术作为路径规划的基础，提出了一种基于环境建模的野外无人车路径规划和轨迹规划方法。仿真结果表明，这种路径规划方法可有效实现野外环境中的障碍规避，同时生成最终轨迹。

在未来的工作中，当需要处理的环境信息更加复杂时，如何更好地利用传感器系统得到的数据进行信息融合，使得感知更加准确也是一个研究方向。对于未知环境，随着任务需求更加多元化，多辆野外无人车协同控制也是一个研究方向。在大数据时代，如何将机器学习与控制技术相结合设计一种高效的无人系统框架，可以有效提高无人车的智能水平，这也是一个值得开展的工作。

[1] 李艳. 无人驾驶汽车将奔向何方[J]. 中国商界, 2017(7): 116-119.

[2] 陈云峰, 邹丹. 美国先进野外无人战车发展历程[J]. 轻兵器, 2014(14): 20-23.

[3] Kuffner J J, Lavalle S M. RRT-connect: An efficient approach to single-query path planning[C]. IEEE International Conference on Robotics and Automation(ICRA), San Francisco, USA, April 24-28, 2000.

[4] Gemeinder M, Gerke M. GA-based path planning for mobile robot systems employing an active search algorithm[J]. Applied Soft Computing, 2003(2): 149-158.

[5] Duan H, Yu Y, Zhang X, et al. Three-dimension path planning for UCAV using hybrid meta-heuristic ACO-DE algorithm[J]. Simulation Modelling Practice and Theory, 2010, 18(8): 1104-1115.

[6] Masehian E, Sedighizadeh D. A multi-objective PSO-based algorithm for robot path planning[C]. IEEE International Conference on Industrial Technology(ICIT), Vina del Mar, Chile, May 27, 2010.

[7] Watkins C J, Dayan P. Q-learning[J]. Machine Learning, 1992， 8(3): 279-292.

[8] Zhao M, Lu H, Yang S, et al. The experience-memory Q-learning algorithm for robot path planning in unknown environment[J]. IEEE Access, 2020, 1(1): 98-99.

[9] Cheng C, Hao X, Li J, et al. Global dynamic path planning based on fusion of improved A* algorithm and dynamic window approach[J]. Hsi-An Chiao Tung Ta Hsueh/journal of Xi’an Jiaotong University, 2017, 51(11): 137-143.

[10] 韩明，刘教民，吴朔梅，等. 粒子群优化的移动机器人路径规划算法[J].计算机应用，2017, 37(8): 2258-2263.

[11] Germi S B, Khosravi M A, Fesharakifard R. Adaptive GA-based potential field algorithm for collision-free path planning of mobile robots in dynamic environments[C]. RSI International Conference on Robotics and Mechatronics (ICRM). London, UK, November 19-20, 2018.

[12] 成昌巍, 胡劲文, 王策, 等. 基于改进型人工势场的路径规划方法[J]. 无人系统技术, 2019, 2(6): 10-16.

[13] Yu H, Meng Q, Zhang J, et al. Time-optimal trajectory planning of robot based on improved adaptive genetic algorithm[C]. IEEE Chinese Control and Decision Conference (CCDC), Shen Yang, China, June 9-11, 2018.

[14] Goll S A, Luksha S S, Leushkin V S, et al. Unmanned ground vehicle local trajectory planning algorithm[C]. IEEE Mediterranean Conference on Embedded Computing, 2016(1): 317-321.

Path Planning and Trajectory Planning Technology of Unmanned Ground Vehicle (UGV) in the Field

CHEN Zhiwei1, HU Jinwen1, ZHAO Chunhui1, WANG Ce1, HOU Xiaolei1, PAN Quan1, WANG Teng1, XU Zhao2

(1. Ministry of Education Key Laboratory of Information Fusion Technology, School of Automation, Northwestern Polytechnical University, Xi’an 710072, China; 2. School of Electronic Information, Northwestern Polytechnical University, Xi’an 710072, China)

For the Unmanned Ground Vehicle (UGV) in conventional layout and structured urban environments, the environment perception and path planning technologies are relatively mature. However, the UGV has difficulties in autonomous planning and obstacle avoidance in the field with less information. This article focuses on the path planning and trajectory planning technology of UGV in the complex field environment. Firstly, considering factors like the presence of convex obstacles, concave obstacles and the undulating ground, the on-board sensors system composed of lidar, camera and IMU are used to obtain complex field environment information and data processing. Then, the field environment can be modeled by training Radial Basis Function neural networks (RBF networks). After that, the constraint is constructed by the obtained environment model and the constraint function is optimized for path planning of UGV in the field environment by introducing the distance between start position and the target position, the environment height and gradient. Finally, the path is fit by the N-order polynomial based on the Minimum Snap so that the obtained path can be optimized to the trajectory that needs to be tracked. The effectiveness of this method is verified by simulation experiments that it can achieve the purpose of autonomous path planning and trajectory planning of the UGV in the complex field environment.

Unmanned Ground Vehicle；Environment Perception；Path Planning；RBF Neural Network；Constraints Optimization；Trajectory Planning

TP242.6

2096–5915(2021)02–40‒09

10.19942/j.issn.2096‒5915.2021.2.017

陈智伟，胡劲文，赵春晖，等. 野外无人车路径规划与轨迹规划技术[J]. 无人系统技术，2021，4(2)：40–48.

2020–10–10；

2020–11–27

国家自然科学基金（61803309，61703343）；中央高校基础研究经费（3102019ZDHKY02，3102018JCC003）；陕西省自然科学基金（2018JQ6070，2019JM-254）；中国博士后科学基金（2018M633574）

陈智伟（1997‒），男，硕士研究生，主要研究方向为无人车自主控制、SLAM地图重构与环境感知。