多障碍物环境下车辆编队的轨迹规划研究

自动驾驶因其巨大的潜在社会效益成为近年来最热门的研究课题之一。自动驾驶技术的快速发展揭示了在多障碍物环境中对稳健和安全的运动规划的巨大需求。阿克曼模型的车辆因其动力足、轮胎磨损率低的优点，成为自动驾驶技术的主要研究对象。相较于单个车辆，多个车辆协同完成任务具有更高的效率。在多车协同控制中，编队控制是其中重要的一个研究分支。本文为了实现多个车辆的编队控制，针对两种常见的编队任务场景提出了对应的新颖高效的算法，并且在实验室场景下实现了单个车辆的轨迹跟踪。

对于多个车辆编队导航到指定位置的场景，本文提出了一种基于交替方向乘子法 (Alternating Direction Multiplier Method, ADMM) 和微分平坦输出的优化算法。该算法使用微分平坦输出替代动力学约束，以此为基础建立优化问题。求解优化问题时利用 ADMM 将问题拆分成两个子问题分别进行求解，最终收敛至优化问题的较优解。相较于传统算法，该算法在求解时间上有着明显的优势。通过多组不同障碍物和车辆数目的仿真实验，验证了该算法的优越性和稳定性。

基于上一个场景的衍生场景，多个车辆在编队导航的行驶过程中进行队形的变换，本文提出了一种基于凸可行集 (Convex Feasible Set, CFS) 的轨迹优化算法。为了避让静态障碍物和动态障碍物，在建立好问题模型后，使用 ADMM 框架分解原问题并迭代地求解，CFS 于迭代中不断更新。本文做了不同的仿真实验来论证 该算法相比直接求解原问题，在求解时间上和求解稳定性上的优势。此外，本文对这两种基于 ADMM 框架的算法在各自问题场景下的收敛性进行了数学证明。

最后，为了将本文提出的算法应用到实物上，本文实现了单个车辆轨迹跟踪控制。车辆的轨迹跟踪控制是一个集环境感知、高精度定位和控制算法于一体的庞大工程。本文着眼于控制算法，使用模型预测控制(Model Predictive Control, MPC)的思想，先建立好车辆轨迹跟踪问题的模型，再通过线性化的手段，使得求解所建立的优化问题的时间小于控制时间间隔。控制器利用实验室中的高精度定位，实时计算出车辆的控制量，完成负反馈控制。本文进行了多组轨迹跟踪实验，并且对比了不同实验参数对跟踪效果的影响，其结果表明本文设计的控制算法能够实现预定功能。

Autonomous driving has emerged as a prominent area of research in recent years, driven by its potential to yield significant societal advantages. The evolution of autonomous driving technology underscores the necessity for robust and secure motion planning within complex, multi-obstacle environments. The Ackermann model vehicle stands as a primary focal point in autonomous driving research, owing to its robustness and endurance. Collaborative efforts involving multiple vehicles often demonstrate heightened efficiency compared to individual units when undertaking collective tasks. Within the realm of multi-vehicle collaborative control, formation control is deemed of significant importance as a distinct research domain. To facilitate the realization of formation control among multiple vehicles, corresponding innovative and efficient algorithms for two prevalent formation task scenarios are introduced. Ultimately, the implementation of trajectory tracking for a single vehicle within a laboratory setting is achieved.

For scenarios involving the navigation of vehicle formations towards predetermined locations, an optimization algorithm leveraging the alternating direction multiplier method (ADMM) and differential flat output is proposed. This algorithm employs differential flat outputs instead of dynamic constraints. Subsequently, ADMM is employed to partition the optimization problem into two sub-problems, which are addressed individually. Through iterative refinement, convergence to the optimal solution is achieved. Comparative analysis with traditional algorithms demonstrates notable advantages in solution time, particularly in addressing large-scale problems. Multiple sets of simulation experiments, varying in obstacle density and vehicle count, are conducted to validate the superior performance and robustness of the proposed algorithm.

Building upon the derivative scenario outlined previously, the necessity arises for multiple vehicles to adapt their formations dynamically during navigation. To address this challenge, a trajectory optimization algorithm rooted in the Convex Feasible Set (CFS) algorithm is proposed. Central to this scenario is the intricate task of ensuring vehicle avoidance of both static and dynamic obstacles. Upon formulating the problem model, the ADMM framework is leveraged to decompose the original problem, capitalizing on iterative approaches to exploit the evolving nature of the feasible set of CFS algorithm. Diverse simulation experiments are conducted to underscore the algorithm's efficacy in terms of solution time and stability. Furthermore, mathematical proofs are provided to demonstrate the convergence of the ADMM framework in the proposed algorithms in different problem scenario.

Finally, the practical application of the proposed algorithm necessitates the implementation of vehicle trajectory tracking control within a laboratory setting. Vehicle formation control presents a multifaceted endeavor, requiring the seamless integration of visual recognition, environmental perception, high-precision positioning and control algorithms. This article delves specifically into vehicle control, employing the paradigm of model predictive control (MPC) to initially establish the vehicle model. Subsequently, linearization techniques are employed to expedite the solution time of the optimization problem, ensuring compatibility with the control time interval. The real-time high-precision positioning system in the laboratory environment is leveraged to calculate the control volume of the vehicle, facilitating the execution of negative feedback control. A series of trajectory tracking experiments are conducted in the laboratory, demonstrating the efficacy of the proposed control algorithm in achieving trajectory tracking for individual vehicles.

[1] OICA. Global Monthly Sales Data External[EB/OL]. 2023
[2023-12-27]. https://www.oica.net/.
[2] KAIWARTYA O, ABDULLAH A H, CAO Y, et al. Internet of vehicles: Motivation, layered architecture, network model, challenges, and future aspects[J]. IEEE Access, 2016, 4: 5356-5373.
[3] TAEIHAGH A, LIM H S M. Governing autonomous vehicles: emerging responses for safety, liability, privacy, cybersecurity, and industry risks[J]. Transport Reviews, 2019, 39(1): 103-128.
[4] ONDRUŠ J, KOLLA E, VERTAL’ P, et al. How do autonomous cars work?[J]. Transportation Research Procedia, 2020, 44: 226-233.
[5] 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.
[6] LIMEBEER D J, MASSARO M. Dynamics and optimal control of road vehicles[M]. Oxford University Press, 2018.
[7] JAZAR R N. Vehicle Dynamics Theory and Application[M/OL]. Springer Cham, 2017. https://doi.org/10.1007/978-3-319-53441-1.
[8] GUIGGIANI M. The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars[M/OL]. Springer International Publishing, 2018. https://books.google.com.hk/books?id=SARaDwAAQBAJ.
[9] FARAZANDEH A, AHMED A, RAKHEJA S. Performance enhancement of road vehicles using active independent front steering (AIFS)[J]. SAE International Journal of Passenger Cars-mechanical Systems, 2012, 5(2012-01-2013): 1273-1284.
[10] WANG D, QI F. Trajectory planning for a four-wheel-steering vehicle[C]//Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No. 01CH37164): volume 4. IEEE, 2001: 3320-3325.
[11] MELLINGER D, KUMAR V. Minimum snap trajectory generation and control for quadrotors [C]//2011 IEEE International Conference on Robotics and Automation. IEEE, 2011: 2520-2525.
[12] 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.
[13] 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.
[14] MURRAY R M, SASTRY S S. Nonholonomic motion planning: Steering using sinusoids[J]. IEEE Transactions on Automatic Control, 1993, 38(5): 700-716.
[15] FUCHSHUMER S, SCHLACHER K, RITTENSCHOBER T. Nonlinear vehicle dynamicscontrol-a flatness based approach[C]//Proceedings of the 44th IEEE Conference on Decision and Control. IEEE, 2005: 6492-6497.
[16] PETERS S C, FRAZZOLI E, IAGNEMMA K. Differential flatness of a front-steered vehicle with tire force control[C]//2011 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2011: 298-304.
[17] SETLUR P, WAGNER J R, DAWSON D M, et al. A trajectory tracking steer-by-wire control system for ground vehicles[J]. IEEE Transactions on Vehicular Technology, 2006, 55(1): 76-85.
[18] HWAN JEON J, COWLAGI R V, PETERS S C, et al. Optimal motion planning with the half-car dynamical model for autonomous high-speed driving[C]//2013 American Control Conference. IEEE, 2013: 188-193.
[19] WANG X, SUN W. Trajectory tracking of autonomous vehicle: A differential flatness approach with disturbance-observer-based control[J]. IEEE Transactions on Intelligent Vehicles, 2022, 8 (2): 1368-1379.
[20] WANG Z, DU W, WANG J, et al. Research and application of improved adaptive MOMEDA fault diagnosis method[J]. Measurement, 2019, 140: 63-75.
[21] GAO B, YANG Q, PENG Z, et al. A direct random sampling method for the Fourier amplitude sensitivity test of nonuniformly distributed uncertainty inputs and its application in C/C nozzles [J]. Aerospace Science and Technology, 2020, 100: 105830.
[22] CHEN H, CHEN H, QIANG L. Multi-UAV 3D formation path planning based on improved artificial potential field[J]. Journal of System Simulation, 2020, 32(3): 414-420.
[23] TANG G, TANG C, CLARAMUNT C, et al. Geometric A-star algorithm: An improved A-star algorithm for AGV path planning in a port environment[J]. IEEE Access, 2021, 9: 59196-59210.
[24] LIU L S, LIN J F, YAO J X, et al. Path planning for smart car based on Dijkstra algorithm and dynamic window approach[J]. Wireless Communications and Mobile Computing, 2021, 2021: 1-12.
[25] XIANGRONG T, YUKUN Z, XINXIN J. Improved A-star algorithm for robot path planning in static environment[C]//Journal of Physics: Conference Series: volume 1792. IOP Publishing, 2021: 012067.
[26] QING Z, XU L, LI P, et al. Path Planning for Mobile Robots Based on JPS and Improved A* Algorithm[J]. Journal of Frontiers of Computer Science & Technology, 2021, 15(11): 2233.
[27] KADRY S, ALFEROV G, FEDOROV V, et al. Path optimization for D-star algorithm modification[C]//AIP Conference Proceedings: volume 2425. AIP Publishing, 2022.
[28] MIN H, XIONG X, WANG P, et al. Autonomous driving path planning algorithm based on improved A* algorithm in unstructured environment[J]. Proceedings of the Institution of Mechanical Engineers, Part D: Journal of Automobile Engineering, 2021, 235(2-3): 513-526.
[29] WANG X, LIU Z, LIU J. Mobile robot path planning based on an improved A* algorithm[C]//International Conference on Computer Graphics, Artificial Intelligence, and Data Processing (ICCAID 2022): volume 12604. SPIE, 2023: 1093-1098.
[30] RAHEEM F A, HAMEED U I. Heuristic D* algorithm based on particle swarm optimization for path planning of two-link robot arm in dynamic environment[J]. Al-Khwarizmi Engineering Journal, 2019, 15(2): 108-123.
[31] ELBANHAWI M, SIMIC M. Sampling-based robot motion planning: A review[J]. IEEE Access, 2014, 2: 56-77.
[32] LAVALLE S. Rapidly-exploring random trees: A new tool for path planning[J]. Research Report 9811, 1998.
[33] SHKOLNIK A, WALTER M, TEDRAKE R. Reachability-guided sampling for planning under differential constraints[C]//2009 IEEE International Conference on Robotics and Automation. IEEE, 2009: 2859-2865.
[34] KARAMAN S, FRAZZOLI E. Sampling-based algorithms for optimal motion planning[J]. The International Journal of Robotics Research, 2011, 30(7): 846-894.
[35] 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. IEEE, 2013: 5054-5061.
[36] JIANG C, HU Z, MOURELATOS Z P, et al. R2-RRT*: Reliability-based robust mission planning of off-road autonomous ground vehicle under uncertain terrain environment[J]. IEEE Transactions on Automation Science and Engineering, 2021, 19(2): 1030-1046.
[37] FERACO S, LUCIANI S, BONFITTO A, et al. A local trajectory planning and control method for autonomous vehicles based on the RRT algorithm[C]//2020 AEIT International Conference of Electrical and Electronic Technologies for Automotive (AEIT Automotive). IEEE, 2020: 1-6.
[38] KUMAR S, SIKANDER A. A modified probabilistic roadmap algorithm for efficient mobile robot path planning[J]. Engineering Optimization, 2023, 55(9): 1616-1634.
[39] RAVANKAR A A, RAVANKAR A, EMARU T, et al. HPPRM: hybrid potential based probabilistic roadmap algorithm for improved dynamic path planning of mobile robots[J]. IEEE Access, 2020, 8: 221743-221766.
[40] QIAO L, LUO X, LUO Q. An Optimized Probabilistic Roadmap Algorithm for Path Planning of Mobile Robots in Complex Environments with Narrow Channels[J]. Sensors, 2022, 22(22): 8983.
[41] AJANOVIC Z, LACEVIC B, SHYROKAU B, et al. Search-based optimal motion planning for automated driving[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 4523-4530.
[42] ZHU Z, SCHMERLING E, PAVONE M. A convex optimization approach to smooth trajectories for motion planning with car-like robots[C]//2015 54th IEEE Conference on Decision and Control (CDC). IEEE, 2015: 835-842.
[43] 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.
[44] DING W, ZHANG L, CHEN J, et al. Safe trajectory generation for complex urban environments using spatio-temporal semantic corridor[J]. IEEE Robotics and Automation Letters, 2019, 4(3): 2997-3004.
[45] LIU C, LIN C Y, TOMIZUKA M. The convex feasible set algorithm for real time optimization in motion planning[J]. SIAM Journal on Control and optimization, 2018, 56(4): 2712-2733.
[46] CHEN R, YANG Z, CHENG J, et al. Motion Planning for Nonlinear Robotic System based on ADMM and Convex Feasible Set Algorithm[C]//2022 IEEE 61st Conference on Decision and Control (CDC). IEEE, 2022: 7461-7466.
[47] GUPTA A, DIVEKAR R. Autonomous parallel parking methodology for Ackerman configured vehicles[J]. ACEEE International Journal on Communication, 2010, 1(2): 1-6.
[48] INOUE T, DAO M Q, LIU K Z. Development of an auto-parking system with physical limitations[C]//SICE 2004 Annual Conference: volume 2. IEEE, 2004: 1015-1020.
[49] LIU S, AN X, SHANG E, et al. A path planning method for assistant parallel car-parking[C]//2012 Fifth International Symposium on Computational Intelligence and Design: volume 2. IEEE, 2012: 65-68.
[50] SUNGWOO C, BOUSSARD C, D’ANDRÉA NOVEL B. Easy path planning and robust control for automatic parallel parking[J]. IFAC Proceedings Volumes, 2011, 44(1): 656-661.
[51] CHOI J W, CURRY R E, ELKAIM G H. Continuous Curvature Path Generation Based on Bézier Curves for Autonomous Vehicles.[J]. IAENG International Journal of Applied Mathematics, 2010, 40(2).
[52] VOROBIEVA H, GLASER S, MINOIU-ENACHE N, et al. Automatic parallel parking in tiny spots: Path planning and control[J]. IEEE Transactions on Intelligent Transportation Systems, 2014, 16(1): 396-410.
[53] VOROBIEVA H, MINOIU-ENACHE N, GLASER S, et al. Geometric continuous-curvature path planning for automatic parallel parking[C]//2013 10th IEEE international Conference on Networking, Sensing and Control (ICNSC). IEEE, 2013: 418-423.
[54] MOON J, BAE I, CHA J G, et al. A trajectory planning method based on forward path generation and backward tracking algorithm for automatic parking systems[C]//17th International IEEE Conference on Intelligent Transportation Systems (ITSC). IEEE, 2014: 719-724.
[55] LEWIS M A, TAN K H. High precision formation control of mobile robots using virtual structures[J]. Autonomous Robots, 1997, 4: 387-403.
[56] LI D, GE S S, HE W, et al. Multilayer formation control of multi-agent systems[J]. Automatica, 2019, 109: 108558.
[57] BALCH T, ARKIN R C. Behavior-based formation control for multirobot teams[J]. IEEE Transactions on Robotics and Automation, 1998, 14(6): 926-939.
[58] HACENE N, MENDIL B. Behavior-based autonomous navigation and formation control of mobile robots in unknown cluttered dynamic environments with dynamic target tracking[J]. International Journal of Automation and Computing, 2021: 1-21.
[59] LI X, XIAO J, TAN J. Modeling and controller design for multiple mobile robots formation control[C]//2004 IEEE International Conference on Robotics and Biomimetics. IEEE, 2004: 838-843.
[60] DAI S L, HE S, CHEN X, et al. Adaptive leader–follower formation control of nonholonomic mobile robots with prescribed transient and steady-state performance[J]. IEEE Transactions on Industrial Informatics, 2019, 16(6): 3662-3671.
[61] LIN J, MIAO Z, ZHONG H, et al. Adaptive image-based leader–follower formation control of mobile robots with visibility constraints[J]. IEEE Transactions on Industrial Electronics, 2020, 68(7): 6010-6019.
[62] XU Z, YAN T, YANG S X, et al. Distributed Leader Follower Formation Control of Mobile Robots based on Bioinspired Neural Dynamics and Adaptive Sliding Innovation Filter[J]. IEEE Transactions on Industrial Informatics, 2023.
[63] DESAI J P, OSTROWSKI J, KUMAR V. Controlling formations of multiple mobile robots[C]//Proceedings. 1998 IEEE International Conference on Robotics and Automation (Cat. No.98CH36146): volume 4. IEEE, 1998: 2864-2869.
[64] HU J, BHOWMICK P, LANZON A. Distributed adaptive time-varying group formation tracking for multiagent systems with multiple leaders on directed graphs[J]. IEEE Transactions on Control of Network Systems, 2019, 7(1): 140-150.
[65] REN W, BEARD R W. Consensus seeking in multiagent systems under dynamically changing interaction topologies[J]. IEEE Transactions on Automatic Control, 2005, 50(5): 655-661.
[66] KHATIB O. Real-time obstacle avoidance for manipulators and mobile robots[J]. The International Journal of Robotics Research, 1986, 5(1): 90-98.
[67] PAN Z, WANG D, DENG H, et al. A virtual spring method for the multi-robot path planning and formation control[J]. International Journal of Control, Automation and Systems, 2019, 17:1272-1282.
[68] LIU X, GE S S, GOH C H. Formation potential field for trajectory tracking control of multiagents in constrained space[J]. International Journal of Control, 2017, 90(10): 2137-2151.
[69] BOYD S, PARIKH N, CHU E, et al. Distributed optimization and statistical learning via the alternating direction method of multipliers[J]. Foundations and Trends® in Machine learning, 2011, 3(1): 1-122.
[70] CHEN L, SUN D, TOH K C. A note on the convergence of ADMM for linearly constrained convex optimization problems[J]. Computational Optimization and Applications, 2017, 66(2): 327-343.
[71] WANG Y, YIN W, ZENG J. Global convergence of ADMM in nonconvex nonsmooth optimization[J]. Journal of Scientific Computing, 2019, 78(1): 29-63.
[72] CHENG Z, MA J, ZHANG X, et al. ADMM-based parallel optimization for multi-agent collision-free model predictive control[A]. 2021.
[73] ANDREI N. Quadratic Programming[M/OL]. Cham: Springer International Publishing, 2022: 439-474. https://doi.org/10.1007/978-3-031-08720-2_13.
[74] PHAN-HUY HAO E. Quadratically constrained quadratic programming: Some applications and a method for solution[J]. Zeitschrift für Operations Research, 1982, 26: 105-119.
[75] 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.
[76] LI J, RAN M, WANG H, et al. MPC-based unified trajectory planning and tracking control approach for automated guided vehicles[C]//2019 IEEE 15th International Conference on Control and Automation (ICCA). IEEE, 2019: 374-380.
[77] VANDERBEI R J, SHANNO D F. An interior-point algorithm for nonconvex nonlinear programming[J]. Computational Optimization and Applications, 1999, 13: 231-252.
[78] CHENG J, CHEN R, LIANG Z, et al. Trajectory Planning for Formation Variation of UGVs in Cluttered Environment[C]//2023 IEEE 26th International Conference on Intelligent Transportation Systems (ITSC). IEEE, 2023: 2310-2315.
[79] RAO A V. A survey of numerical methods for optimal control[J]. Advances in the Astronautical Sciences, 2009, 135(1): 497-528.
[80] STELLATO B, BANJAC G, GOULART P, et al. OSQP: an operator splitting solver for quadratic programs[J/OL]. Mathematical Programming Computation, 2020, 12(4): 637-672. https://doi.org/10.1007/s12532-020-00179-2.