非结构化环境下的多车协同轨迹规划算法研究

非结构化环境下的多车协同轨迹规划任务，目的是在园区、仓库等非结构化 的场景下，为每一辆车从起点到终点规划出一条符合车辆运动学约束且不发生碰 撞的轨迹。该问题不仅关注单一车辆的轨迹规划问题，还需要考虑到多个车辆之 间的交互和协同作业，以保障所有车辆行驶的安全性和最大化车辆整体的作业效 率。因此，该问题的研究在现代交通系统、自动化仓库物流等领域具有重要意义。

本研究将该问题分为多车全局路径规划、车辆轨迹优化、多车协同避障三个阶段 进行求解，从而能够高效且高质量地解决该问题。 在多车全局路径规划阶段，本研究采用序贯的方式，依次为每一辆车在地图 中进行采样搜索来规划路径。针对在空间和时间维度进行搜索的 A* 算法效率低下 的问题，本研究基于跳点搜索法的核心思想，提出了四条剪枝规则来减少算法不 必要的搜索。仿真实验表明，本研究所提出剪枝规则在保证车辆路径长度最优性 的前提下，有效地提高了原时空搜索的 A* 算法的搜索效率，并且为多辆车在地图 中规划出不同源的路径。

在车辆轨迹优化阶段，本研究进一步优化上一阶段生成的路径。本研究基于 微分平坦理论来描述车辆运动学约束，采用多个圆来描述车辆足迹以及凸可行集 算法来描述车辆避障约束，将整个轨迹优化问题建模成一个非线性优化问题。随 后，采用交替方向乘子法将原问题分解成两个子问题单独进行求解。仿真实验表 明，本研究所提出的方法在保证轨迹质量良好的前提下，有效地提高了原优化问 题求解的成功率和速度。

在多车协同避障阶段，本研究在车辆沿上一阶段求解出的轨迹上行驶过程中， 共享所有车辆的轨迹，并且对车辆进行实时的速度优化。当车辆之间检测到碰撞 将要发生时，本研究设定了优先级评估函数来决定车辆优先级别的高低，对低优 先级别的车辆进行速度优化，让其避让高优先级的车辆，从而实现车辆之间的相 互避障。通过仿真实验，验证了本研究提出的速度优化算法能有效地实现了车辆 之间的相互避让，提高车辆行驶的速度和缩短车辆行驶的总时间。

[1] 中华人民共和国国家发展和改革委员会. 智能汽车创新发展战略[EB/OL]. 2020-02-24.https://www.gov.cn/zhengce/zhengceku/2020-02/24/content_5482655.htm.
[2] 中华人民共和国交通运输部和科技部. 关于科技创新驱动加快建设交通强国的意见[EB/OL]. 2021-08-29. https://www.gov.cn/zhengce/zhengceku/2021-08/29/content_5637125.htm.
[3] 中华人民共和国国务院.“十四五”现代综合交通运输体系发展规划[EB/OL]. 2022-01-18.https://www.gov.cn/zhengce/content/2022-01/18/content_5669049.htm.
[4] 中华人民共和国工业信息化部. 关于开展智能网联汽车准入和上路通行试点工作的通知[EB/OL]. 2023-11-17. https://www.miit.gov.cn/jgsj/zbys/wjfb/art/2023/art_4a67648dc58e483bab554f97045a8579.html.
[5] DIJKSTRA E W. A note on two problems in connexion with graphs[M]//Edsger Wybe Dijkstra:His Life, Work, and Legacy. 2022: 287-290.
[6] HART P E, NILSSON N J, RAPHAEL B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE transactions on Systems Science and Cybernetics, 1968, 4(2):100-107.
[7] DOLGOV D, THRUN S, MONTEMERLO M, et al. Path planning for autonomous vehicles in unknown semi-structured environments[J]. The International Journal of Robotics Research,2010, 29(5): 485-501.
[8] YOON S, YOON S E, LEE U, et al. Recursive path planning using reduced states for car-like vehicles on grid maps[J]. IEEE Transactions on Intelligent Transportation Systems, 2015, 16(5): 2797-2813.
[9] PIVTORAIKO M, KELLY A. Efficient constrained path planning via search in state lattices[C]//International Symposium on Artificial Intelligence, Robotics, and Automation in Space.Munich Germany, 2005: 1-7.
[10] PIVTORAIKO M, KNEPPER R A, KELLY A. Differentially constrained mobile robot motion planning in state lattices[J]. Journal of Field Robotics, 2009, 26(3): 308-333.
[11] ZIEGLER J, STILLER C. Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios[C]//2009 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2009: 1879-1884.
[12] KAVRAKI L E, SVESTKA P, LATOMBE J C, et al. Probabilistic roadmaps for path planning in high-dimensional configuration spaces[J]. IEEE Transactions on Robotics and Automation,1996, 12(4): 566-580.
[13] LAVALLE S M, KUFFNER J J, DONALD B, et al. Rapidly-exploring random trees: Progress and prospects[J]. Algorithmic and computational robotics: new directions, 2001, 5: 293-308.
[14] WEBB D J, VAN DEN BERG J. Kinodynamic RRT*: Asymptotically optimal motion planning for robots with linear dynamics[C]//2013 IEEE International Conference on Robotics and Automation(ICRA). IEEE, 2013: 5054-5061.
[15] DUBINS L E. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents[J]. American Journal of mathematics,1957, 79(3): 497-516.
[16] REEDS J, SHEPP L. Optimal paths for a car that goes both forwards and backwards[J]. Pacific journal of mathematics, 1990, 145(2): 367-393.
[17] PASTVA T A. Bezier curve fitting[D]. Monterey, California. Naval Postgraduate School, 1998.
[18] LIU X, JIANG D, TAO B, et al. Genetic algorithm-based trajectory optimization for digital twin robots[J]. Frontiers in Bioengineering and Biotechnology, 2022, 9: 1433.
[19] AKROUR R, NEUMANN G, ABDULSAMAD H, et al. Model-free trajectory optimization for reinforcement learning[C]//International Conference on Machine Learning(ICML). PMLR,2016: 2961-2970.
[20] BAYERLEIN H, DE KERRET P, GESBERT D. Trajectory optimization for autonomous flying base station via reinforcement learning[C]//2018 IEEE 19th International Workshop on Signal Processing Advances in Wireless Communications (SPAWC). 2018: 1-5.
[21] BOJARSKI M, DEL TESTA D, DWORAKOWSKI D, et al. End to end learning for self-driving cars[A]. 2016.
[22] CHEN Z, HUANG X. End-to-end learning for lane keeping of self-driving cars[C]//2017 IEEE intelligent vehicles symposium (IV). IEEE, 2017: 1856-1860.
[23] FERNANDEZ N. Two-stream convolutional networks for end-to-end learning of self-driving cars[A]. 2018.
[24] VISWANATH P, NAGORI S, MODY M, et al. End to end learning based self-driving using JacintoNet[C]//2018 IEEE 8th International Conference on Consumer Electronics-Berlin(ICCE-Berlin). IEEE, 2018: 1-4.
[25] WÄCHTER A, BIEGLER L T. On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming[J]. Mathematical programming, 2006, 106:25-57.
[26] BETTS J T. Very low-thrust trajectory optimization using a direct SQP method[J]. Journal of Computational and Applied Mathematics, 2000, 120(1-2): 27-40.
[27] RÖSMANN C, FEITEN W, WÖSCH T, et al. Trajectory modification considering dynamic constraints of autonomous robots[C]//ROBOTIK 2012; 7th German Conference on Robotics. VDE, 2012: 1-6.
[28] RÖSMANN C, HOFFMANN F, BERTRAM T. Kinodynamic trajectory optimization and control for car-like robots[C]//2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017: 5681-5686.
[29] HAN Z, WU Y, LI T, et al. An efficient spatial-temporal trajectory planner for autonomous vehicles in unstructured environments[J]. IEEE Transactions on Intelligent Transportation Systems, 2023.
[30] SHIN H, KIM D, YOON S E. Kinodynamic comfort trajectory planning for car-like robots[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE,2018: 6532-6539.
[31] SHI S, XIONG Y, CHEN J, et al. A bilevel optimal motion planning (BOMP) model with application to autonomous parking[J]. International Journal of Intelligent Robotics and Applications,2019, 3(4): 370-382.
[32] LI B, ACARMAN T, ZHANG Y, et al. Optimization-based trajectory planning for autonomous parking with irregularly placed obstacles: A lightweight iterative framework[J]. IEEE Transactions on Intelligent Transportation Systems, 2021, 23(8): 11970-11981.
[33] HE R, ZHOU J, JIANG S, et al. TDR-OBCA: A reliable planner for autonomous driving in free-space environment[C]//2021 American Control Conference (ACC). IEEE, 2021: 2927-2934.
[34] ZHOU J, HE R, WANG Y, et al. Autonomous driving trajectory optimization with dual-loop iterative anchoring path smoothing and piecewise-jerk speed optimization[J]. IEEE Robotics and Automation Letters, 2020, 6(2): 439-446.
[35] ZHANG X, LINIGER A, SAKAI A, et al. Autonomous parking using optimization-based collision avoidance[C]//2018 IEEE Conference on Decision and Control (CDC). IEEE, 2018:4327-4332.
[36] LI B, LIU H, XIAO D, et al. Centralized and optimal motion planning for large-scale AGV systems: A generic approach[J]. Advances in Engineering Software, 2017, 106: 33-46.
[37] BURGER C, LAUER M. Cooperative multiple vehicle trajectory planning using miqp[C]//2018 IEEE International Conference on Intelligent Transportation Systems (ITSC). IEEE, 2018: 602-607.
[38] PATNAIK A, HOTA A R. Optimization Based Collision Avoidance for Multi-Agent Dynamic calSystems in Goal Reaching Task[A]. 2021.
[39] ZHANG X, CHENG Z, MA J, et al. Parallel collaborative motion planning with alternating direction method of multipliers[C]//IECON 2021–47th Annual Conference of the IEEE Industrial Electronics Society. IEEE, 2021: 1-6.
[40] CHEN R, LIANG Z, CHENG J, et al. Multi-Agent Cooperative Motion Planning Based on Alternating Direction Method of Multipliers[J]. IEEE Control Systems Letters, 2023, 7: 3307-3312.
[41] OUYANG Y, LI B, ZHANG Y, et al. Fast and Optimal Trajectory Planning for Multiple Vehicles in a Nonconvex and Cluttered Environment: Benchmarks, Methodology, and Experiments[C]//2022 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2022: 10746-10752.
[42] LI B, OUYANG Y, ZHANG Y, et al. Optimal cooperative maneuver planning for multiple nonholonomic robots in a tiny environment via adaptive-scaling constrained optimization[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 1511-1518.
[43] LI J, RAN M, XIE L. Efficient trajectory planning for multiple non-holonomic mobile robots via prioritized trajectory optimization[J]. IEEE Robotics and Automation Letters, 2020, 6(2):405-412.
[44] CHEN M, FISAC J F, SASTRY S, et al. Safe sequential path planning of multi-vehicle systems via double-obstacle Hamilton-Jacobi-Isaacs variational inequality[C]//2015 European Control Conference (ECC). IEEE, 2015: 3304-3309.
[45] ROBINSON D R, MAR R T, ESTABRIDIS K, et al. An efficient algorithm for optimal trajectory generation for heterogeneous multi-agent systems in non-convex environments[J]. IEEE Robotics and Automation Letters, 2018, 3(2): 1215-1222.
[46] PARK J, KIM J, JANG I, et al. Efficient multi-agent trajectory planning with feasibility guarantee using relative bernstein polynomial[C]//2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020: 434-440.
[47] WEN L, LIU Y, LI H. CL-MAPF: Multi-agent path finding for car-like robots with kinematic and spatiotemporal constraints[J]. Robotics and Autonomous Systems, 2022, 150: 103997.
[48] XU H, FENG S, ZHANG Y, et al. A grouping-based cooperative driving strategy for CAVs merging problems[J]. IEEE Transactions on Vehicular Technology, 2019, 68(6): 6125-6136.
[49] DELIMPALTADAKIS I M, BECHLIOULIS C P, KYRIAKOPOULOS K J. Decentralizedplatooning with obstacle avoidance for car-like vehicles with limited sensing[J]. IEEE Robotics and Automation Letters, 2018, 3(2): 835-840.
[50] VAN DEN BERG J, LIN M, MANOCHA D. Reciprocal velocity obstacles for real-time multi-agent navigation[C]//2008 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2008: 1928-1935.
[51] BERG J V D, GUY S J, LIN M, et al. Reciprocal n-body collision avoidance[M]//Robotics research. Springer, 2011: 3-19.
[52] CHEN J, LIU C, TOMIZUKA M. Foad: Fast optimization-based autonomous driving motion planner[C]//2018 Annual American Control Conference (ACC). IEEE, 2018: 4725-4732.
[53] ZHOU H, LIU C. Distributed Motion Coordination Using Convex Feasible Set Based Model Predictive Control[C]//2021 IEEE International Conference on Robotics and Automa tion (ICRA). IEEE, 2021: 8330-8336.
[54] LI B, ZHANG Y, OUYANG Y, et al. Fast trajectory planning for agv in the presence of moving obstacles: A combination of 3-dim a* search and qcqp[C]//2021 33rd Chinese Control and Decision Conference (CCDC). IEEE, 2021: 7549-7554.
[55] HARABOR D, GRASTIEN A. Online graph pruning for pathfinding on grid maps[C]//Proceedings of the AAAI Conference on Artificial Intelligence: volume 25. 2011: 1114-1119.
[56] LIU C, TOMIZUKA M. Real time trajectory optimization for nonlinear robotic systems: Relaxation and convexification[J]. Systems & Control Letters, 2017, 108: 56-63.
[57] RAJAMANI R. Vehicle dynamics and control[M]. Springer Science & Business Media, 2011.
[58] 李柏. 复杂约束下自动驾驶车辆运动规划的计算最优控制方法研究[D]. 杭州: 浙江大学,2018.
[59] SUN C, LI Q, LI B, et al. A successive linearization in feasible set algorithm for vehicle motion planning in unstructured and low-speed scenarios[J]. IEEE Transactions on Intelligent Transportation Systems, 2021, 23(4): 3724-3736
[60] EIRAS F, HAWASLY M, ALBRECHT S V, et al. A two-stage optimization-based motion planner for safe urban driving[J]. IEEE Transactions on Robotics, 2021, 38(2): 822-834.
[61] SCHWARTING W, ALONSO-MORA J, PAULL L, et al. Safe nonlinear trajectory generation for parallel autonomy with a dynamic vehicle model[J]. IEEE Transactions on IntelligentTransportation Systems, 2017, 19(9): 2994-3008.
[62] GILBERT E G, JOHNSON D W, KEERTHI S S. A fast procedure for computing the distance between complex objects in three-dimensional space[J]. IEEE Journal on Robotics and Automation, 1988, 4(2): 193-203.
[63] MURRAY R M, RATHINAM M, SLUIS W. Differential flatness of mechanical control systems: A catalog of prototype systems[C]//ASME International Mechanical Engineering Congress and Exposition. Citeseer, 1995.
[64] RICHTER C, BRY A, ROY N. Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments[C]//Robotics Research: The 16th International Symposium ISRR. Springer, 2016: 649-666.
[65] WANG Z, ZHOU X, XU C, et al. Geometrically Constrained Trajectory Optimization for Multicopters[J]. IEEE Transactions on Robotics, 2022, 38(5): 3259-3278.
[66] COLESANTI A, HUG D. Hessian measures of semi-convex functions and applications to support measures of convex bodies[J]. manuscripta mathematica, 2000, 101(2): 209-238.
[67] 刘浩洋. 最优化: 建模, 算法与理论[M]. 北京: 高等教育出版社, 2020.
[68] ANTONIOU A, LU W S. Practical optimization: algorithms and engineering applications: volume 19[M]. Springer, 2007.
[69] AGARWAL S, MIERLE K, TEAM T C S. Ceres Solver[CP/OL]. 2023. https://github.com/ceres-solver/ceres-solver.
[70] STELLATO B, BANJAC G, GOULART P, et al. OSQP: An operator splitting solver forquadratic programs[J]. Mathematical Programming Computation, 2020, 12(4): 637-672.
[71] WERLING M, ZIEGLER J, KAMMEL S, et al. Optimal trajectory generation for dynamic street scenarios in a frenet frame[C]//2010 IEEE International Conference on Robotics and Automation. IEEE, 2010: 987-993.