10 中南大学学报 ( 自然科学版 ) 第 44 卷自动泊车技术 (auto parking system, APS) 是智能汽车技术的重要组成部分, 其目的是辅助或者替代驾驶员完成车辆的安全泊位随着汽车行业的发展, 乘用车辆的普及, 城市泊车空间越来越小, 增加了驾驶员泊车的困难, 因

Size: px

Start display at page:

Download "10 中南大学学报 ( 自然科学版 ) 第 44 卷自动泊车技术 (auto parking system, APS) 是智能汽车技术的重要组成部分, 其目的是辅助或者替代驾驶员完成车辆的安全泊位随着汽车行业的发展, 乘用车辆的普及, 城市泊车空间越来越小, 增加了驾驶员泊车的困难, 因"

萌稔瓜尔佳
7 years ago
Views:

1 第 44 卷第 1 期中南大学学报 ( 自然科学版 ) Vol.44 No 年 1 月 Journal of Central South University (Science and Technology) Jan. 013 基于 Matlab 的多约束自动平行泊车轨迹规划李红 1, 郭孔辉 1,, 宋晓琳 1, 李飞龙 1 (1. 湖南大学汽车车身先进设计制造国家重点实验室, 湖南长沙,41008;. 吉林大学动态模拟国家重点实验室, 吉林长春,1300) 摘要 : 分析泊车轨迹曲线特点, 在已有轨迹函数基础上提出新的轨迹函数, 通过对实际泊车轨迹的拟合证明该函数的可用性分析泊车环境中存在的可能碰撞点, 建立相应的避撞约束函数以泊车环境障碍约束车辆自身参数约束泊车初始点方位约束泊车终点位置约束为轨迹函数的约束函数, 以泊车终点车辆与车位夹角最小为目标函数建立单目标多约束轨迹方程分别对一般泊车环境和狭小空间泊车环境进行泊车轨迹规划, 利用 Matlab 软件非线性约束优化函数求得轨迹函数参数仿真结果表明 : 对于一般泊车环境, 该方法能满足泊车轨迹曲率连续性, 使车辆无碰撞进入车位, 并使车辆与车位平行 ; 对于狭小空间泊车环境, 虽未能使车辆与车位平行, 但实现了车辆无碰撞地泊车入位并保证轨迹曲率的连续性由此可知该方法可实现车辆无碰撞地泊车入位, 并满足轨迹曲率连续性要求, 有效地解决了泊车过程中停车转向问题关键词 : 轨迹规划 ; 自动泊车 ; 避撞 ; 多约束 ; 优化中图分类号 :TP73 文献标志码 :A 文章编号 : (013) Trajectory planning of automatic parallel parking with multi constraints based on Matlab LI Hong 1, GUO Konghui 1,, SONG Xiaolin 1, LI Feilong 1 (1. State Key Laboratory of Advanced Design and Manufacturing for Vehicle Body, Hunan University, Changsha 41008, China;. State Key Laboratory of Automobile Dynamic Simulation, Jilin University, Changchun 1300, China) Abstract: Parking trajectory characteristics were analyzed, and a new trajectory function was proposed based on the known trajectory functions. It was proved that the new function was feasible for trajectory planning by parking trajectory fitting comparison. Potential collision points were analyzed and constraint equations were established accordingly. The orientation constraints at the start point, the location constraint at the parking point and the restriction of vehicle were considered, and the constraint equations were established respectively. Parking trajectory optimization function was presented to minimize the vehicle orientation angle at the parking point with restrictions mentioned above. The approach was used by trajectory planning for general parking space and tight parking space, and the trajectory function was solved using Matlab software. The simulation results show that the car can reverse into parking set safely in general parking space, with the continuously trajectory curvature and the car paralleled with the parking set at the parking point. This approach fails to make the car paralleled with the parking set in tight parking space, but the car can move into parking set safely with the continuous trajectory curvature. It is proved that the approach proposed can find a collision free path in different parking spaces and meet the demand of continuous trajectory curvature, so the problem of shutting down to steer the wheel in parking process is solved. Key words: trajectory planning; automatic parking; collision free; multi constraints; optimization 收稿日期 : ; 修回日期 : 基金项目 : 国家自然科学基金资助项目 ( ); 湖南省科技计划项目 (01GK3161) 通信作者 : 李红 (1984 ), 女, 山东定陶人, 博士研究生, 从事汽车动态模拟与仿真研究 ; 电话 : ;E mail: lihong379@163.com

2 10 中南大学学报 ( 自然科学版 ) 第 44 卷自动泊车技术 (auto parking system, APS) 是智能汽车技术的重要组成部分, 其目的是辅助或者替代驾驶员完成车辆的安全泊位随着汽车行业的发展, 乘用车辆的普及, 城市泊车空间越来越小, 增加了驾驶员泊车的困难, 因此, 自动泊车技术在汽车智能技术中愈来愈重要国内外学者对自动泊车技术进行了研究, 如 :Mukherjee 等 [1] 对自动泊车的非完整约束路径规划进行了研究 ;Divelbiss 等 [] 对车辆轨迹跟踪控制进行了研究 ;Chang 等 [3] 采用模糊逻辑对车辆进行了自动泊车控制 ; Paromtchik [4] 研究了自动泊车对转向及速度的要求 ;Zhao 等 [5] 基于模糊逻辑对狭小空间泊车鲁棒性进行了研究 ;Young 等 [6] 基于改进的模糊逻辑控制对未采用 Odometry 的自动泊车鲁棒性了研究 ; Lee 等 [7] 进行了基于轨迹跟踪控制器的泊车控制研究 ; 张辉 [8] 进行了轿车自动倒车转向控制几何推到算法和模糊逻辑算法研究 ; 魏言华 [9] 进行了基于视觉的车辆后方障碍物检测算法研究 ; 宋金泽等 [10] 融合动力学约束对自动平行泊车轨迹进行了研究 ; 姜辉 [11] 基于模糊逻辑对自动平行泊车转向控制器进行了设计 ; 单恩忠 [1] 基于微分平坦与样条理论进行了自主泊车轨迹规划研究目前, 只有少数汽车公司如大众丰田雷诺等已开发出相关的自动泊车产品, 但仅仅安装到高档轿车中, 而我国暂未有自主知识产权的自动泊车系统路径规划是自动泊车系统中的重要组成部分, 为此, 本文作者对多约束情况下的泊车轨迹进行研究 1/R min (R min 为车辆最小转弯半径 ) 平滑的曲线, 使车辆沿该轨迹运动时不与周围物体发生碰撞为避免车辆在泊车过程中原地转向, 要求曲线曲率连续 ; 为避免车辆在泊车初始点原地转向, 则要求曲线初始曲率尽量小甚至为 0 车辆停放的规范性要求车辆应尽量保持与车位平行, 故泊车轨迹曲线在终点处的切线与车位对称线夹角尽量小泊车方式可根据车位方向分为平行泊车垂直泊车和斜向泊车, 根据泊车复杂程度可分为后退式泊车和多次进退式泊车, 本文仅对后退式平行泊车进行研究 1 自动泊车系统工作流程自动泊车系统工作过程如图 1 所示自动泊车系统开启后, 车辆沿道路缓慢行驶, 通过传感器感知泊车环境信息, 检测停车空间及环境障碍若停车空间过小不能满足要求, 则继续行驶 ; 找到满足泊车空间要求的车位后, 确定泊车初始点及目标位置, 建立泊车轨迹坐标, 根据环境障碍信息及车辆参数信息规划泊车路径若规划成功, 则提示驾驶员是否采取自动泊车 ; 若不成功, 则继续行驶, 寻找下一个车位泊车路径规划成功后, 若驾驶员采用自动泊车, 泊车系统控制根据传感器采集的车速及环境障碍信息控制方向盘完成泊车 ; 若驾驶员不采用自动泊车, 则自动泊车系统关闭另外, 自动泊车系统还需保证驾驶员可随时接管并控制车辆, 以保证泊车过程中的安全性自动平行泊车的轨迹规划问题可描述如下 : 车辆的起始点与终止点之间找到 1 条曲率不大于图 1 平行自动泊车系统工作流程 Fig.1 Flow chart of parallel parking process 泊车轨迹规划.1 泊车轨迹函数在自动泊车过程中, 为避免停车转向造成的轮胎磨损, 要求泊车轨迹曲线曲率连续, 且轨迹起点曲率为 0 或尽量小, 以降低车轮初始转向角 ; 同时, 泊车轨迹曲线起始点切线尽量保持与车位纵向线平行, 以降低对车辆泊车初始方位角的要求据文献 [11], 选择 Sigmoid 函数与反正切函数为泊车轨迹函数 :

3 第 1 期李红, 等 : 基于 Matlab 的多约束自动平行泊车轨迹规划 103 a y ( k ( x x c )) 1 + e (1) y = d arctan( a x + b ) + c () 和 L 分别为车位长和宽 ;A,B,C 和 D 分别为车辆右后端点右前端点左前端点和左后端点 ;θ 为车辆方位角 ;φ 为前轴中点等效转角式 (1) 和 () 均满足轨迹曲线曲率的连续性, 但不能满足轨迹起点为小曲率或零曲率的要求, 故出现原地转向问题, 且轨迹初始点切线与车位纵向线夹角较大, 车辆需具有较大的初始方位角才能成功进入停车车位综合考虑反正切函数及多项式函数, 采用 origin 软件自定义公式拟合功能编辑函数, 对已有的泊车轨迹曲线进行拟合并改进, 得出新的轨迹拟合函数 : y = a 1 arctan( a x + a 3 ) 3 4 x + a 5 x + a 6 x + a 7 ) ( a + a (3) 8 通过对某一泊车轨迹曲线拟合图像图可知 : 该轨迹函数更能满足泊车轨迹曲率连续性及初始点小曲率或无曲率的要求, 且初始点切线与车位纵向线夹角较小图 3 平行泊车参数示意图 Fig.3 Parameters of parallel parking process 由后轴中点轨迹函数可得泊车过程中车辆在任意点方位角 θ 及曲率 ρ 为 : d y θ = arctan d x y ρ = [1 + ( y ) ] 3/ (4) (5) 由几何关系可得 A,B,C 和 D 随后轴中点移动轨迹为 : 图 3 个函数轨迹拟合结果对比 Fig. Fitting result of three functions. 建立多非线性约束轨迹模型轨迹规划是找到 1 条平滑连续的曲线, 当车辆沿着这条曲线行驶时, 绕过周围环境的障碍, 安全地进入停车位中 : 因此, 在进行轨迹规划时, 首先应对泊车过程中的障碍约束进行分析选择后轴中点轨迹为本文泊车轨迹规划对象, 以该点初始位置为坐标原点, 则后轴中点轨迹终点为 (s o L r,h o +/), 如图 3 所示图 3 中 :s o 为后轴中点距车位底线距离 ; h o 为后轴中点距障碍物横向距离 ; h 为路宽 ;L r 为后轴中点距车辆后端距离 ;L f 为前轴中点距车辆前端距离 ;L 为车辆轴距 ; 为车宽 ;L 1 xa = x + L r cosθ sin θ ya y L r sinθ = + + (6) xb = x L f cosθ sin θ yb y L f sinθ = + (7) xc = x L f cosθ + sin θ yc y L f sinθ = (8) xd = x + L r cosθ + sin θ yd y L r sinθ = + (9) 车辆与周围障碍有可能发生碰撞的位置共有 4

4 104 中南大学学报 ( 自然科学版 ) 第 44 卷处 :(a) 车辆泊车过程中与左侧道路边界的碰撞或者与对面行驶车辆间的碰撞 ;(b) 车辆进入车位前与右侧障碍物或者车辆的碰撞 ;(c) 车辆进入车位后右前端与右侧障碍物或者前方车位中的车辆的碰撞 ;(d) 车辆进入车位后右后端与右侧障碍物或者道路边界的碰撞如图 4 所示 1 R = (11) ρ 由式 (5),(10) 和 (11) 可得前轴转角 : φ = arctan L y ( 1 + ( y ) ) 3/ (1) d φ ψ = (13) dt dx d y v = + dt d t 由式 (13)~(14) 可得 : (14) ψ = ν d φ 1 + ( y ) d x (15) (a) 左前侧碰撞 ;(b) 右后侧入库前碰撞 ; (c) 左前侧碰撞 ;(d) 右后侧入库后碰撞图 4 泊车过程中车辆可能碰撞点示意图 Fig.4 Possible collision points in parallel parking 为避免车辆与周围障碍物碰撞, 轨迹函数须满足以下要求 : 当 x A [0 s o L 1 ] 时,y A <h o ; 当 x A [0 s o L r ] 时,y A <h o +L ; 当 x B [0 s o L 1 ] 时,y B <h o ; 当 x B [0 s o L r ] 时,y B <h o +L ; 当 x [0 s o L r ] 时,y C >h o +L h; 当 x=s o L r 时,y C >h o ; 当 x=s o L r 时,y D >h o ; 为使轨迹曲线通过汽车起始点和终止点, 须满足以下要求 : 当 x=0 时,y=0; 当 x=s o L r 时,y=h o +L / 为避免车辆在泊车在初始点原地转向及方位角问题, 须满足以下要求 : 当 x=0 时,θ=0; 当 x=0 时,ρ=0 为保正停车的规范性, 轨迹终点方位角应很小最好为 0 汽车参数约束如下由 Ackerman 关系可知 : L tan φ = (10) R 车辆性能的约束主要体现在个方面 : (1) 最小转弯半径约束, 即后轴中点轨迹曲率 ρ 1/R min () 方向转角角速度约束, 即 ψ ψ max 因此, 建立车辆参数约束 : y 当 x [0 s o L r ] 时, ( ) 3/ ν 1 + ( y ) d x d φ ψ max 1 + ( y ) 1 R min, 且以泊车终点方位角最小化为目标建立多非线性约束轨迹方程 : min / (,,,,,,,, ) 即 : θ x= so L a r 1 a a a3 a4 a5 a6 a7 a 8 y = 0, x = 0 L y = ho +, x = so L r ya < ho, xa [ 0, so L 1 ] ya < ho + L, xa [ 0, so L r ] yb < ho, xb [ 0, so L 1 ] yb < ho + L, xb [ 0, so L r ] ho + L h < yc, x [ 0, so L 1 ) ho yc, x = so L r ho < yd, x = so L r θ = 0, x = 0 ρ = 0, x = 0 1 ρ, x [ 0, so L r ] R min d φ ψ max 1 + ( y ), x [0, so L r ] d x v (16)

5 第 1 期李红, 等 : 基于 Matlab 的多约束自动平行泊车轨迹规划仿真实例及分析考虑实际自动操纵中传感器的测量误差, 为保证泊车的安全性, 选择安全余量 [13], 整车外形尺寸增加 0.1 m, 车辆参数及安全裕量见表 1 建立泊车约束数学模型, 据文献 [14], 选择一般泊车环境和狭小空间泊车环境参数, 并分别对其进行泊车轨迹规划, 利用 Matlab 软件非线性多约束优化函数 fmincon 求得相应泊车轨迹函数参数, 具体参数见表 ; 据仿真模型所得仿真结果见图 5 和图 6 表 1 车辆参数 Table 1 Vehicle parameters 参数数值安全裕量仿真值轴距 L/m 前轴与前端距离 L f /m 后轴与后端距离 L r /m 车宽 /m 最小转弯半径 R min /m 最大转速 ψ max /(rad s 1 ) 6.8 车速 v/(km h 1 ) 表环境参数及轨迹参数 Table Parameters of parking environment and trajectory functions 参数环境 1 环境 Fig.5 (a) 侧向位移与纵向位移的关系 ; (b) 轨迹曲率与纵向位移的关系图 5 环境 1 中泊车轨迹及曲率 Parallel parking trajectory and curvature curves in the first condition s o /m h o /m 1.5 L 1 /m L /m.5.4 h/m a a a a a a a a 由图 5 所示的泊车轨迹可知 : 在整个泊车过程中, 车辆轮廓线与周围环境障碍无交点, 可证明车辆实现了无碰撞地进入车位, 并且泊车结束后, 车身与车位基本平行, 满足了对自动泊车系统泊车规范性要求由泊车轨迹曲率可知 : 轨迹曲率连续变化且初始点处曲率为 0, 解决了泊车过程中需停车转向的问题由车辆最小转弯半径可知泊车轨迹曲率不大于 1/R min, 即由轨迹曲率曲线可知: 轨迹曲率在 [ 0.19,0.19] 范围内, 满足车辆最小转弯半径约束由图 6 可知 : 由于泊车空间的限制, 泊车结束后未能使车身与车位平行, 但实现了无碰撞地进入车位, 满足了避障约束要求由轨迹曲率曲线可知 : 轨迹曲率连续变化, 初始点处曲率为 m 1, 即车辆初始转角为 0.41, 可忽略不计, 轨迹曲率在 [ 1.9,1.9] 范围内, 满足了车

6 106 中南大学学报 ( 自然科学版 ) 第 44 卷 () 对泊车过程中可能碰撞点进行分析, 通过车辆运动学关系建立避障约束函数对车辆初始位置及方位角约束进行分析, 建立位置及角度约束函数根据车辆自身参数约束, 建立泊车轨迹最小转弯半径约束函数为满足泊车规范性, 选择轨迹终点处车身与车位夹角最小为优化函数, 建立单目标多约束泊车轨迹方程 (3) 选择种泊车环境, 利用 Matlab 软件多非线性约束优化功能求解泊车轨迹方程, 得到可行轨迹曲线仿真结果表明 : 当泊车空间足够大时, 通过该方法均能求得 1 条轨迹曲线可使车辆无碰撞地进入车位, 且与车位保持平行停放 ; 当泊车空间狭小时, 仍能求得轨迹曲线, 该曲线虽然不能保证车辆停放的规范性, 但能保证车辆无碰撞地进入车位, 满足了泊车安全性要求 (4) 与圆弧直线圆弧轨迹规划方法相比, 该方法得到的泊车轨迹曲率连续, 解决了泊车过程中需原地转向的问题, 降低了转向电机的工作负荷与轮胎磨损 ; 与模糊逻辑泊车控制相比无需大量实验, 节省时间提高了效率, 可为自动泊车提供一定参考参考文献 : (a) 纵向位移与侧向位移的关系 ; (b) 轨迹曲率与纵向位移的关系图 6 环境中泊车轨迹及轨迹曲率 Fig.6 Parallel parking trajectory and curvature curves in the second condition 辆自身参数约束因此可知 : 当泊车空间足够大时, 均可通过上述方法求得一组函数参数构成 1 条满足车辆避障约束和车辆最小转弯半径约束条件的轨迹曲线, 在实际泊车过程中, 通过车辆纵向位置判断合适的方向盘转角, 连续调节方向盘转角, 使车辆后轴中点沿所规划的轨迹移动实现车辆安全进入车位, 且具有规范泊车能力 4 结论 (1) 为避免泊车过程中停车转向现象对轮胎的磨损, 在已有泊车轨迹函数基础上提出新的泊车轨迹函数, 并通过对实车泊车轨迹曲线的拟合对比证明了曲线的可行性 [1] Mukherjee R, Anderson D P. A surface integral approach to the motion planning of nonholonomic systems[j]. ASME Journal of Dynamic Systems, Measurement and Control, 1994, 116(9): [] Divelbiss A, en J T. Trajectory tracking control of a car trailer system[j]. IEEE Transactions on Control Systems Technology, 1997, 5(3): [3] Chang S J, Li T H S. Design and implementation of fuzzy parallel parking control for a car type mobile robot[j]. Journal of Intelligent and Robotic Systems, 004, 34(): [4] Paromtchik I. Steering and velocity commands for parking assistance[c]//proceedings of the 10th IASTED International Conference on Robotics and Applications. Honolulu, Hawaii, USA, 004: [5] ZHAO Yanan, Jr Cdlins E G. Robust automatic parallel parking in tight space via fuzz logic[j]. Robotics and Autonomous Systems, 005, 51: [6] Young R, Se Y O, Senior M. Robust automatic parking without odomety using enhanced fuzzy logic controller[j]. IEEE International Conference on Fuzzy System. Vancouver B C, Canada, 006: [7] Lee K, Kim D, Chung, et al. Car parking control using a trajectory tracking controller[c]//sice ICASE International

7 第 1 期李红, 等 : 基于 Matlab 的多约束自动平行泊车轨迹规划 107 Joint Conference. Bexco, Korea, 006: [8] 张辉. 轿车自动倒库转向控制几何推到算法和模糊逻辑算法研究 [D]. 长春 : 吉林大学汽车工程学院,007: ZHANG Hui. Research on geometry algorithm and fuzzy logic for automatic garage parking of sedan[d]. Changchun: Jilin University. College of Automotive Engineering, 007: [9] 魏言华. 基于视觉的车辆后方障碍物检测算法研究与实现 [D]. 沈阳 : 东北大学信息科学与工程学院, 008: EI Yanhua. Research and implementation of vehicle back over obstacle detection algorithm based on vision[d]. Shenyang: Northeastern University. College of Information and Engineering, 008: [10] 宋金泽, 戴斌, 单恩忠, 等. 融合动力学约束的自主平行泊车轨迹生成方法 [J]. 中南大学学报 : 自然科学版, 009, 40(1): SONG Jinze, DAI Bin, SHAN Enzhong, et al. Trajectory planning approach of autonomous parallel parking with kinodynamic constraints[j]. Journal of Central South University: Science and Technology, 009, 40(1): [11] 姜辉. 自动平行泊车系统转向控制策略的研究 [D]. 长春 : 吉林大学汽车工程学院, 010: JIANG Hui. Research on strategies of automatic parallel parking steering control[d]. Changchun: Jilin University. College of Automotive Engineering, 010: [1] 单恩忠. 基于微分平坦与样条理论的自主泊车轨迹规划研究 [D]. 长沙 : 国防科学技术大学机电工程与自动化学院, 009: SHAN Enzhong. Research on trajectory planning of auto parking based on differential flatness and spline theory[d]. Changsha: National University of Defense Technology. College of Mechatronic Engineering and Automatic, 009: [13] Moritz O. Parkmate enhanced odometry[d]. Zurich: Swiss Federal Institute of Technology Zurich, Department of Information Technology and Electrical Engineering, 004: [14] JGJ , 汽车库建筑设计规范 [S]. JGJ , Design code for garage[s]. ( 编辑陈灿华 )

f 2 f 2 f q 1 q 1 q 1 q 2 q 1 q n 2 f 2 f 2 f H = q 2 q 1 q 2 q 2 q 2 q n f 2 f 2 f q n q 1 q n q 2 q n q n H R n n n Hessian

f 2 f 2 f q 1 q 1 q 1 q 2 q 1 q n 2 f 2 f 2 f H = q 2 q 1 q 2 q 2 q 2 q n f 2 f 2 f q n q 1 q n q 2 q n q n H R n n n Hessian 2012 10 31 10 Mechanical Science and Technology for Aerosace Engineering October Vol. 31 2012 No. 10 1 2 1 2 1 2 1 2 1 300387 2 300387 Matlab /Simulink Simulink TH112 A 1003-8728 2012 10-1664-06 Dynamics