基于代价函数的无人驾驶汽车局部路径规划算法
|
摘要
局部路径规划层作为无人驾驶汽车软件层的重要组成分布,如何有效、安全地到达目的地是当前研究的热点。针对结构化道路信息,充分考虑车道线的约束,在使用Frenet坐标系理论的基础上,提出一种考虑到车道线曲率和障碍物模型信息,得到不同车道上其他道路参与者的位置信息,以便计算其他障碍物模型对本车危险程度,综合算法实时性、轨迹平顺性等要素的最小代价局部路径规划算法。在局部路径规划过程中,沿着参考线(Frenet坐标系下X轴上一段路径)选取多个路径分割点,Frenet坐标系下在每个分割点处沿Y轴进行控制点离散,每个路径分割点处选取1个控制点构成路径控制点集合,使用一元三次方程对每种排列组合路径进行拟合,从而使用代价函数对每种排列组合路径进行评估,代价函数值最小为最优的局部路径。代价函数考虑拟合轨迹到障碍物的危险程度、轨迹平顺性、轨迹到当前参考线(实时在全局路径规划层上根据车速得到一条当前需要跟踪的理想轨迹)的偏离程度、拟合轨迹行驶方向的改变程度、无人驾驶汽车最小转弯半径。研究结果表明:在不同试验场景下,所提出基于代价函数的局部路径规划算法,能规划出一条不与障碍物发生碰撞的最优路径,并能保证无人驾驶汽车行驶轨迹平顺性和路径规划层实时性的要求。
Local path planning is an important software module for driverless cars. Aspects such as how to reach a destination safely and effectively are drawing widespread research interest. In this study, various constraints of lane lines were examined in detail by incorporating the information of structured roads. Based on the Frenet coordinate system theory, location information of road participants in different lanes was estimated by considering the obstacle model information and curvatures of lane lines. To calculate the degree of risk associated with other obstacle models, a real-time minimum-cost local path-planning algorithm was employed for trajectory smoothness of the algorithm. During the process of local path planning, several path segmentation points were selected along the reference line that represented a path on the X-axis of the Frenet coordinate system. The control point was discretized along the Y-axis at each segmentation point in the Frenet coordinate system. A control point was subsequently selected for forming a set of path control points at each path split point. A cubic equation with one unknown attribute was applied for fitting each permutation-and-combination path, and the cost function was then employed for evaluating each permutation-and-combination path. The local path with minimum cost function value was selected as the optimal path. Furthermore, the cost function considered multiple aspects such as the limit of danger between the fit trajectory and obstacle, smoothness of the trajectory, degree of deviation of the trajectory from the reference line(an ideal trajectory obtained in real time according to the speed of a vehicle during global path planning), degree of variation in the driving direction of the fit trajectory, and minimum turning radius of the driverless car. Results obtained from simulation and experimentation with an actual vehicle indicate that under different experimental scenarios, the proposed local path-planning algorithm based on cost function can plan an optimal path that does not coincide with obstacles, can guarantee the smoothness of driving tracks for driverless cars, and can satisfy the real-time requirements of route planning.
Local path planning is an important software module for driverless cars. Aspects such as how to reach a destination safely and effectively are drawing widespread research interest. In this study, various constraints of lane lines were examined in detail by incorporating the information of structured roads. Based on the Frenet coordinate system theory, location information of road participants in different lanes was estimated by considering the obstacle model information and curvatures of lane lines. To calculate the degree of risk associated with other obstacle models, a real-time minimum-cost local path-planning algorithm was employed for trajectory smoothness of the algorithm. During the process of local path planning, several path segmentation points were selected along the reference line that represented a path on the X-axis of the Frenet coordinate system. The control point was discretized along the Y-axis at each segmentation point in the Frenet coordinate system. A control point was subsequently selected for forming a set of path control points at each path split point. A cubic equation with one unknown attribute was applied for fitting each permutation-and-combination path, and the cost function was then employed for evaluating each permutation-and-combination path. The local path with minimum cost function value was selected as the optimal path. Furthermore, the cost function considered multiple aspects such as the limit of danger between the fit trajectory and obstacle, smoothness of the trajectory, degree of deviation of the trajectory from the reference line(an ideal trajectory obtained in real time according to the speed of a vehicle during global path planning), degree of variation in the driving direction of the fit trajectory, and minimum turning radius of the driverless car. Results obtained from simulation and experimentation with an actual vehicle indicate that under different experimental scenarios, the proposed local path-planning algorithm based on cost function can plan an optimal path that does not coincide with obstacles, can guarantee the smoothness of driving tracks for driverless cars, and can satisfy the real-time requirements of route planning.
引文
[1] 李大东,孙秀霞,彭建亮,等.基于可视图法的改进Dijkstra算法[J].电光与控制,2010,17(3):40-43.LI Da-dong,SUN Xiu-xia,PENG Jian-liang,et al.A Modified Dijkstra’s Algorithm Based on Visibility Graph [J].Chinese Journal of Electronics Optics & Control,2010,17 (3):40-43.
[2] ULISES O R,OSCAR M,ROBERTO S.Mobile Robot Path Planning Using Membrane Evolutionary Artificial Potential Field [J].Applied Soft Computing,2019,77:236-251.
[3] 谭宝成,崔佳超.改进人工势场法在无人车避障中的应用[J].西安工业大学学报,2014,34(12):1007-1011,1017.TAN Bao-cheng,CUI Jia-chao.Application of Improved Artificial Potential Field Method in Intelligent Collision Avoidance System on Structured Road [J].Journal of Xi’an Technological University,2014,34 (12):1007-1011,1017.
[4] 毛晨悦,吴鹏勇.基于人工势场法的无人机路径规划避障算法[J].电子科技,2019(7):1-7.MAO Chen-yue,WU Peng-yong.UAV Path Planning Obstacle Avoidance Algorithm Based on Artificial Potential Field Method [J].Journal of Electronic Science and Technology,2019 (7):1-7.
[5] CAO X M,ZOU X J,JIA C Y,et al.RRT-based Path Planning for an Intelligent Litchi-picking Manipulator [J].Computers and Electronics in Agriculture,2019,156:105-118.
[6] 朱曼曼,杜煜,张永华,等.基于模糊逻辑的智能车局部路径规划[J].北京联合大学学报,2016,30(4):29-32.ZHU Man-man,DU Yu,ZHANG Yong-hua,et al.Local Path Planning of Smart Car Based on Fuzzy Logic Method [J].Journal of Beijing Union University,2016,30 (4):29-32.
[7] DENG Y J,ZHANG X K,ZHANG G Q.Fuzzy Logic Based Speed Optimization and Path Following Control for Sailassisted Ships [J].Ocean Engineering,2019,171:300-310.
[8] 贺盼博,邬春学.基于多约束条件的改进遗传算法路径规划[J].软件导刊,2018,17(7):180-183,188.HE Pan-bo,WU Chun-xue.Improved Genetic Algorithm Route Planning Based on Multiple Constraints [J].Chinese Journal of Software Guide,2018,17 (7):180-183,188.
[9] AL-TAHARW A,ALAA S,MOHAMMED A W.A Mobile Robot Path Planning Using Genetic Algorithmin Static Environment [J].Journal of Computer Science,2008,4 (4):341-344.
[10] 罗显跃,高吉普,龙征,等.一种巡检机器人智能路径规划方法[J].湖南科技大学学报:自然科学版,2018,33(4):75-82.LUO Xian-yue,GAO Ji-pu,LONG Zheng,et al.An Intelligent Path Planning Method for Inspection Robot [J].Journal of Hunan University of Science & Technology:Natural Science Edition,2018,33 (4):75-82.
[11] 吕太之,周武,赵春霞.采用粒子群优化和B样条曲线的改进可视图路径规划算法[J].华侨大学学报:自然科学版,2018,39(1):103-108.LU Tai-zhi,ZHOU Wu,ZHAO Chun-xia.Improved Visibility Graph Method Using Particle Swarm Optimization and B-spline Curve for Path Planning [J].Journal of Huaqiao University:Natural Science,2018,39 (1):103-108.
[12] 宋晓琳,周南,黄正瑜,等.改进RRT在汽车避障局部路径规划中的应用[J].湖南大学学报:自然科学版,2017,44(4):30-37.SONG Xiao-lin,ZHOU Nan,HUANG Zheng-yu,et al.An Improved RRT Algorithm of Local Path Planning for Vehicle Collision Avoidance [J].Journal of Hunan University:Natural Sciences,2017,44 (4):30-37.
[13] 伍永健,陈跃东,陈孟元.改进QPSO和Morphin算法下移动机器人混合路径规划[J].电子测量与仪器学报,2017,31(2):295-301.WU Yong-jian,CHEN Yue-dong,CHEN Meng-yuan.Hybrid Path Planning of Mobile Robot Based on Improved QPSO and Morphin Algorithm [J].Journal of Electronic Measurement and Insturmentation,2017,31 (2):295-301.
[14] 周自维,李长乐,赵杰,等.复杂局部地形中的实时路径规划算法设计[J].哈尔滨工业大学学报,2014,46(8):65-71.ZHOU Zi-wei,LI Chang-le,ZHAO Jie,et al.A Real Time Path Planning Algorithm Based on Local Complicated Environment [J].Journal of Harbin Institute of Technology,2014,46 (8):65-71.
[15] 吴宪祥,郭宝龙,王娟.基于粒子群三次样条优化的移动机器人路径规划算法[J].机器人,2009,31(6):556-560.WU Xian-xiang,Guo Bao-long,WANG Juan.Mobile Robot Path Planning Algorithm Based on Particle Swarm Optimization of Cubic Splines [J].Robot,2009,31 (6):556-560.
[16] 吕智.35 kV及以下架空线档距计算中应力方程的优化求解[J].长江大学学报:自然科学版,2011,8(6):115-117.LU Zhi.Optimization Solution of Stress Equation Overhead Wire Pitch Calculation In 35kV and Below [J].Journal of Yangtze University:Natural Science Edition,2011,8 (6):115-117.
[17] 陈虹,申忱,郭洪艳,等.面向动态避障的智能汽车滚动时域路径规划[J].中国公路学报,2019,32(1):162-172.CHEN Hong,SHEN Chen,GUO Hong-yan,et al.Moving Horizon Path Planning for Intelligent Vehicle Considering Dynamic Obstacle Avoidance [J].China Journal of Highway and Transport,2019,32 (1):162-172.
[18] EE S L,PAULINE O,KAH C C .Solving the Optimal Path Planning of a Mobile Robot Using Improved Q-learning [J].Robotics and Autonomous Systems,2019,115:143-161.
[19] SOHEILA S,MOTAMENI H.Optimal Path Planning for Mobile Robot Using Intelligent Water Drops Algorithm [J].Journal of Intelligent & Fuzzy Systems,2014,27 (3):1519-1531.
[20] SONG B Y,WANG Z D,ZOU L,et al.A New Approach to Smooth Global Path Planning of Mobile Robots with Kinematic Constraints [J].International Journal of Machine Learning and Cybernetics,2019,10 (1):107-119.
[21] 冀杰,唐志荣,吴明阳,等.面向车道变换的路径规划及模型预测轨迹跟踪[J].中国公路学报,2018,31(4):172-179.JI Jie,TANG Zhi-rong,WU Ming-yang,et al.Path Planning and Tracking for Lane Changing Based on Model Predictive Control [J].China Journal of Highway and Transport,2018,31 (4):172-179.
[22] SIMON C,PHILIPPE F,ZHAO Y S,et al.Multi-layer Path Planning Control for the Simulation of Manipulation Tasks:Involving Semantics and Topology [J].Robotics and Computer-integrates Manufacturing,2019,57:17-58.
[23] MAXIME C,SEBASTIEN C,THOMAS B,et al.Automatic Multi-axis Path Planning for Thinwall Tubing Through Robotized Wire Deposition [J].Procedia CIRP,2019,79:89-94.
[2] ULISES O R,OSCAR M,ROBERTO S.Mobile Robot Path Planning Using Membrane Evolutionary Artificial Potential Field [J].Applied Soft Computing,2019,77:236-251.
[3] 谭宝成,崔佳超.改进人工势场法在无人车避障中的应用[J].西安工业大学学报,2014,34(12):1007-1011,1017.TAN Bao-cheng,CUI Jia-chao.Application of Improved Artificial Potential Field Method in Intelligent Collision Avoidance System on Structured Road [J].Journal of Xi’an Technological University,2014,34 (12):1007-1011,1017.
[4] 毛晨悦,吴鹏勇.基于人工势场法的无人机路径规划避障算法[J].电子科技,2019(7):1-7.MAO Chen-yue,WU Peng-yong.UAV Path Planning Obstacle Avoidance Algorithm Based on Artificial Potential Field Method [J].Journal of Electronic Science and Technology,2019 (7):1-7.
[5] CAO X M,ZOU X J,JIA C Y,et al.RRT-based Path Planning for an Intelligent Litchi-picking Manipulator [J].Computers and Electronics in Agriculture,2019,156:105-118.
[6] 朱曼曼,杜煜,张永华,等.基于模糊逻辑的智能车局部路径规划[J].北京联合大学学报,2016,30(4):29-32.ZHU Man-man,DU Yu,ZHANG Yong-hua,et al.Local Path Planning of Smart Car Based on Fuzzy Logic Method [J].Journal of Beijing Union University,2016,30 (4):29-32.
[7] DENG Y J,ZHANG X K,ZHANG G Q.Fuzzy Logic Based Speed Optimization and Path Following Control for Sailassisted Ships [J].Ocean Engineering,2019,171:300-310.
[8] 贺盼博,邬春学.基于多约束条件的改进遗传算法路径规划[J].软件导刊,2018,17(7):180-183,188.HE Pan-bo,WU Chun-xue.Improved Genetic Algorithm Route Planning Based on Multiple Constraints [J].Chinese Journal of Software Guide,2018,17 (7):180-183,188.
[9] AL-TAHARW A,ALAA S,MOHAMMED A W.A Mobile Robot Path Planning Using Genetic Algorithmin Static Environment [J].Journal of Computer Science,2008,4 (4):341-344.
[10] 罗显跃,高吉普,龙征,等.一种巡检机器人智能路径规划方法[J].湖南科技大学学报:自然科学版,2018,33(4):75-82.LUO Xian-yue,GAO Ji-pu,LONG Zheng,et al.An Intelligent Path Planning Method for Inspection Robot [J].Journal of Hunan University of Science & Technology:Natural Science Edition,2018,33 (4):75-82.
[11] 吕太之,周武,赵春霞.采用粒子群优化和B样条曲线的改进可视图路径规划算法[J].华侨大学学报:自然科学版,2018,39(1):103-108.LU Tai-zhi,ZHOU Wu,ZHAO Chun-xia.Improved Visibility Graph Method Using Particle Swarm Optimization and B-spline Curve for Path Planning [J].Journal of Huaqiao University:Natural Science,2018,39 (1):103-108.
[12] 宋晓琳,周南,黄正瑜,等.改进RRT在汽车避障局部路径规划中的应用[J].湖南大学学报:自然科学版,2017,44(4):30-37.SONG Xiao-lin,ZHOU Nan,HUANG Zheng-yu,et al.An Improved RRT Algorithm of Local Path Planning for Vehicle Collision Avoidance [J].Journal of Hunan University:Natural Sciences,2017,44 (4):30-37.
[13] 伍永健,陈跃东,陈孟元.改进QPSO和Morphin算法下移动机器人混合路径规划[J].电子测量与仪器学报,2017,31(2):295-301.WU Yong-jian,CHEN Yue-dong,CHEN Meng-yuan.Hybrid Path Planning of Mobile Robot Based on Improved QPSO and Morphin Algorithm [J].Journal of Electronic Measurement and Insturmentation,2017,31 (2):295-301.
[14] 周自维,李长乐,赵杰,等.复杂局部地形中的实时路径规划算法设计[J].哈尔滨工业大学学报,2014,46(8):65-71.ZHOU Zi-wei,LI Chang-le,ZHAO Jie,et al.A Real Time Path Planning Algorithm Based on Local Complicated Environment [J].Journal of Harbin Institute of Technology,2014,46 (8):65-71.
[15] 吴宪祥,郭宝龙,王娟.基于粒子群三次样条优化的移动机器人路径规划算法[J].机器人,2009,31(6):556-560.WU Xian-xiang,Guo Bao-long,WANG Juan.Mobile Robot Path Planning Algorithm Based on Particle Swarm Optimization of Cubic Splines [J].Robot,2009,31 (6):556-560.
[16] 吕智.35 kV及以下架空线档距计算中应力方程的优化求解[J].长江大学学报:自然科学版,2011,8(6):115-117.LU Zhi.Optimization Solution of Stress Equation Overhead Wire Pitch Calculation In 35kV and Below [J].Journal of Yangtze University:Natural Science Edition,2011,8 (6):115-117.
[17] 陈虹,申忱,郭洪艳,等.面向动态避障的智能汽车滚动时域路径规划[J].中国公路学报,2019,32(1):162-172.CHEN Hong,SHEN Chen,GUO Hong-yan,et al.Moving Horizon Path Planning for Intelligent Vehicle Considering Dynamic Obstacle Avoidance [J].China Journal of Highway and Transport,2019,32 (1):162-172.
[18] EE S L,PAULINE O,KAH C C .Solving the Optimal Path Planning of a Mobile Robot Using Improved Q-learning [J].Robotics and Autonomous Systems,2019,115:143-161.
[19] SOHEILA S,MOTAMENI H.Optimal Path Planning for Mobile Robot Using Intelligent Water Drops Algorithm [J].Journal of Intelligent & Fuzzy Systems,2014,27 (3):1519-1531.
[20] SONG B Y,WANG Z D,ZOU L,et al.A New Approach to Smooth Global Path Planning of Mobile Robots with Kinematic Constraints [J].International Journal of Machine Learning and Cybernetics,2019,10 (1):107-119.
[21] 冀杰,唐志荣,吴明阳,等.面向车道变换的路径规划及模型预测轨迹跟踪[J].中国公路学报,2018,31(4):172-179.JI Jie,TANG Zhi-rong,WU Ming-yang,et al.Path Planning and Tracking for Lane Changing Based on Model Predictive Control [J].China Journal of Highway and Transport,2018,31 (4):172-179.
[22] SIMON C,PHILIPPE F,ZHAO Y S,et al.Multi-layer Path Planning Control for the Simulation of Manipulation Tasks:Involving Semantics and Topology [J].Robotics and Computer-integrates Manufacturing,2019,57:17-58.
[23] MAXIME C,SEBASTIEN C,THOMAS B,et al.Automatic Multi-axis Path Planning for Thinwall Tubing Through Robotized Wire Deposition [J].Procedia CIRP,2019,79:89-94.