﻿ 基于二段多项式的窄空间平行泊车路径规划方法
 计算机系统应用  2020, Vol. 29 Issue (8): 211-216 PDF

1. 武汉科技大学 计算机科学与技术学院, 武汉 430065;
2. 武汉科技大学 智能信息处理与实时工业系统湖北省重点实验室, 武汉 430065;
3. 武汉科技大学 信息科学与工程学院/人工智能学院, 武汉 430081

Path Planning by Two-Piece Polynomial Equation in Narrow Space for Parallel Parking
XIONG Ying1,2, MAO Xue-Song3
1. School of Computer Science and Technology, Wuhan University of Science and Technology, Wuhan 430065, China;
2. Hubei Province Key Laboratory of Intelligent Information, Processing and Real-time Industrial System, Wuhan University of Science and Technology, Wuhan 430065, China;
3. School of Information Science and Engineering/School of Artificial Intelligence, Wuhan University of Science and Technology, Wuhan 430081, China
Abstract: To solve the problem of path planning for parallel parking in narrow spaces, a two-piece five order polynomial equation based method was proposed for calculating approaching path and reverse path respectively, among which the approaching path was applied for adjusting the vehicle pose so that a curvature optimal path for reversing car into parking lot is available. Simulation was performed by considering the general size of parking lot and family car, as well as the kinematic model of vehicle motion, to find a collision free path in free space. The result shows that the two-piece five order polynomial equation-based method can realize intelligent parking in narrow spaces, and the steering angle of wheel changes continuously on the linked path.
Key words: unmanned driving     self-parking     constrained space     polynomial equation     path planning

1 仿真场景与车辆模型

 图 1 平行泊车仿真场景

 $\left[ {\begin{array}{*{20}{c}} {\dot x} \\ {\dot y} \\ {\dot \theta } \\ {\dot \delta } \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\cos \theta } \\ {\sin \theta } \\ {{{\tan \delta } / l}} \\ 0 \end{array}} \right]v + \left[ {\begin{array}{*{20}{c}} 0 \\ 0 \\ 0 \\ 1 \end{array}} \right]w$ (1)

2 二段多项式泊车路径规划方法 2.1 5阶多项式连接相邻路点

 $\tan \delta = \kappa l$ (2)

 $\kappa = \dfrac{{\left| {y''} \right|}}{{{{\left( {1 + {{y'}^2}} \right)}^{{3 / 2}}}}}$ (3)

 图 2 前进与倒车路径以及路点标识

2.2 多项式系数的求解

 $y = \left[ {\begin{array}{*{20}{c}} {{a_0}}&{{a_1}}& \cdots &{{a_r}} \end{array}} \right]{\left[ {\begin{array}{*{20}{c}} 1&x& \cdots &{{x^r}} \end{array}} \right]^{\rm T}}$ (4)

 $\left( {{x_0},{y_0}} \right){\text{、}}y'{|_{x = {x_0}}} = 0{\text{、}}y''{|_{x = {x_0}}} = 0$ (5)

 $\left( {{x_f},{y_f}} \right){\text{、}}y'{|_{x = {x_f}}} = 0{\text{、}}y''{|_{x = {x_0}}} < C$ (6)

 $y = \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}} {a} {\left[ {\begin{array}{*{20}{c}} 1&x&{{x^2}}&{{x^3}}&{{x^4}}&{{x^5}} \end{array}} \right]^{\rm T}}$ (7)
 $y' = \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}} {a} {\left[ {\begin{array}{*{20}{c}} 0&1&{2x}&{3{x^2}}&{4{x^3}}&{5{x^4}} \end{array}} \right]^{\rm T}}$ (8)
 $y'' = \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\rightharpoonup}$}} {a} {\left[ {\begin{array}{*{20}{c}} 0&0&2&{6x}&{12{x^2}}&{20{x^3}} \end{array}} \right]^{\rm T}}$ (9)

 图 3 高阶系数修正的泊车路径

 $\left[ {\begin{array}{*{20}{c}} {{a_0}}&{{a_1}}&{{a_2}}&{{a_3}}&{{a_4}}&{{a_5}} \end{array}} \right] = {B^{ - 1}}Y$

 $B = \left[ {\begin{array}{*{20}{c}} 1&{{x_{l0}}}&{x_{l0}^2}&{x_{l0}^3}&{x_{l0}^4}&{x_{l0}^5} \\ 0&1&{2{x_{l0}}}&{3x_{l0}^2}&{4x_{l0}^3}&{5x_{l0}^4} \\ 0&0&2&{6{x_{l0}}}&{12x_{l0}^2}&{20x_{l0}^3} \\ 1&{{x_{lf}}}&{x_{lf}^2}&{x_{lf}^3}&{x_{lf}^4}&{x_{lf}^5} \\ 0&1&{2{x_{lf}}}&{3x_{lf}^2}&{4x_{lf}^3}&{5x_{lf}^4} \\ 0&0&2&{6{x_{lf}}}&{12x_{lf}^2}&{20x_{lf}^3} \end{array}} \right]$ (10)
 $Y = {\left[ {\begin{array}{*{20}{c}} {{y_{l0}}}&{y'{|_{{x_{l0}}}}}&{y''{|_{{x_{l0}}}}}&{{y_{lf}}}&{y'{|_{{x_{lf}}}}}&{y''{|_{{x_{lf}}}}} \end{array}} \right]^{\rm T}}$ (11)

2.3 中间点的搜索

 $\left( {\begin{array}{*{20}{c}} {{x_{i1}} - l\cos \theta }&{{y_{i1}} - l\sin \theta }&{\tan \theta }&0 \end{array}} \right)$ (12)

2.4 边界条件的数学模型

 ${v_2}\left( x \right) < {c_0}\left( x \right),{v_2}\left( y \right) > {b_0}\left( y \right)$ (13)

 $\left\{ \begin{array}{l} {v_2}\left( x \right) > {a_2}\left( x \right) \cap {v_2}\left( y \right) > {a_2}\left( y \right) \\ {v_1}\left( y \right) > {d_1}\left( y \right) \\ {v_2}\left( y \right) > {d_1}\left( y \right) \\ \end{array} \right.$ (14)
2.5 目标函数的定义

 $T = \alpha \int {{\kappa _1}dx + \left( {1 - \alpha } \right)\int {{\kappa _2}dx} }$ (15)

3 仿真结果

 图 4 最优二段多项式泊车路径以及泊车行驶轨迹

yf = 1.05 m, 对应于图5(d)各点处泊车路径的曲率如图6所示, 可见对应于每一个xi1, 存在一个最佳yi1和方向角. 同样对应于每一个yi1值, 存在一个最优xi1值, 使得曲率最低. 另外, 对于固定xi1yi1, 观察θ对另一变量也具有下凸性, 因此, 在搜索区域范围内式(15)定义的T函数具有下凸性, 可使用常规优化算法在搜索空间内找到最优解. 例如选取前轮中间点位姿xi1 = 18.0 m, yi1= 10 m和θ = π/4, 利用最速下降法搜索, 设置收敛精度为2 cm, 程序运行于Intel(R) Core(TM) i5-4460 CPU@3.2GHz, 内存8 GB, 得到最优解的时间约xi1 = 14.6 m, yi1= 7.3 m和θ = 0.79, 所用时间为0.73 s.

 图 5 泊车终点位置的纵向坐标对中间点存在区域的影响, 箭头指向该位置处的最佳方向角

 图 6 对应于图5(d)各点的安全路径曲率

4 结束语

 [1] Wang DB, Liang HW, Mei T, et al. Research on self-parking path planning algorithms. Proceedings of 2011 IEEE International Conference on Vehicular Electronics and Safety. Beijing, China. 2011. 258–162. [2] 黄江, 魏德奎, 秦良艳, 等. 自动泊车系统路径规划与跟踪控制方法研究. 汽车技术, 2019(8): 39-45. [3] 李红, 王文军, 李克强. 基于B样条理论的平行泊车路径规划. 中国公路学报, 2016, 29(9): 143-151. DOI:10.3969/j.issn.1001-7372.2016.09.019 [4] Elbanhawi M, Simic M, Jazar R. Randomized bidirectional B-spline parameterization motion planning. IEEE Transac-tions on Intelligent Transportation Systems, 2016, 17(2): 406-419. DOI:10.1109/TITS.2015.2477355 [5] 林蓁蓁, 李庆, 梁艳菊, 等. 一种基于路径规划的自动平行泊车算法. 计算机应用研究, 2012, 29(5): 1713-1715. DOI:10.3969/j.issn.1001-3695.2012.05.029 [6] Zhang SW, Simkani M, Zadeh MH. Automatic vehicle parallel parking design using fifth degree polynomial path planning. Proceedings of 2011 IEEE Vehicular Technology Conference. San Francisco, CA, USA. 2011.1–4. [7] Demirli K, Khoshnejad M. Autonomous parallel parking of a car-like mobile robot by a neuro-fuzzy sensor-based controller. Fuzzy Sets and Systems, 2009, 160(19): 2876-2891. DOI:10.1016/j.fss.2009.01.019 [8] Cheng LP, Liu CX, Yan B. Improved hierarchical A-star algorithm for optimal parking path planning of the large parking lot. Proceedings of 2014 IEEE International Conference on Information and Automation. Hailar, China. 2014. 695–698. [9] Guruji AK, Agarwal H, Parsediya DK. Time-efficient A* algorithm for robot path planning. Procedia Technology, 2016, 23: 144-149. DOI:10.1016/j.protcy.2016.03.010 [10] Ajanovic Z, Lacevic B, Shyrokau B, et al. Search-based optimal motion planning for automated driving. Proceedings of 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid, Spain. 2018.4523–4530. [11] Wang YB, Jha DK, Akemi Y. A two-stage RRT path planner for automated parking. Proceedings of the 2017 13th IEEE Conference on Automation Science and Engineering. Xi’an, China. 2017. 496–502. [12] Pfrommer J, Warrington J, Schildbach G, et al. Dynamic vehicle redistribution and online price incentives in shared mobility systems. IEEE Transactions on Intelligent Transportation Systems, 2014, 5(4): 1567-1578. DOI:10.1109/TITS.2014.2303986 [13] Samuel M, Hussein M, Mohamad MB. A review of some pure pursuit based path tracking techniques for control of autonomous vehicle. International Journal of Computer Applications, 2016, 135(1): 35-38. DOI:10.5120/ijca2016908314 [14] Thrun S, Montemerlo M, Dahlkamp H, et al. Stanley: The robot that won the DARPA Grand Challenge. Journal of Field Robotics, 2006, 23(9): 661-692. DOI:10.1002/rob.20147 [15] Zhao YN, Collins EG Jr. Robust automatic parallel parking in tight spaces via fuzzy logic. Robotics and Autonomous Systems, 2005, 51(23): 111-127. DOI:10.1016/j.robot.2005.01.002