滚装船内受非完整约束的自主车自动泊车

王能建，张德福，周丽杰

(哈尔滨工程大学 机电工程学院，黑龙江 哈尔滨 15001)

大型汽车滚装船(automobile Ro-Ro ship，ARRS)的货舱如一大型停车场，拥有众多的车位，装载时如何安全快速地将汽车泊车入船是个挑战.事实证明，即使对于熟练的驾驶员，泊车也绝非易事.自动泊车技术应运而生，该技术可以辅助甚至替代驾驶员完成泊车动作，缓解驾驶员负担并增强下一代客车的安全性.因此，这方面的研究具有实际意义.

传统泊车算法通常分成3个阶段［1］:1)基于声呐或视觉系统获得车位的信息，建立一幅泊车位的局部地图;2)到达倒车的起始位置，同时调整车的首向平行于停车位;3)规划出从起始位姿到目标位姿的车的运动.很多学者研究了该种运动规划问题，但其目前仍然是个开放问题，需进一步研究.国内外解决自动泊车的方法大致分2类［2］:1)路径规划:考虑环境模型和车辆的动力学约束，预先规划到达停车位的几何路径，通过控制命令跟踪该路径［3-4］;2)应用模糊逻辑或神经网络模仿驾驶技术熟练的驾驶员的泊车行为［1-5］.该方法中没有参考路径，依靠车的当前位置与停车位置间的相对误差来进行控制.针对轮式车辆的并行和斜列式停车问题，文献［6］提出一种实时规划系列无碰操作的泊车方法.文献［7］针对斜列式停车问题，提出一种模糊控制器.通过声呐数据获取周围的环境信息，文献［8］提出一种智能自动泊车控制器，能有效地将车泊车入位.

以上研究主要集中在近距离平行泊车，针对装载时ARRS上部分环境未知情况下的自动泊车研究还未见报道.ARRS内空间小，易形成狭窄通道，车辆起始位姿距离目标车位较远(可称为远程自主泊车)，泊车过程中除了考虑车辆的几何形状和非完整约束，还需要规避未知障碍物.基于人工协调场(artificial coordinating field，ACF)与弹道成型制导律(trajectory shaping guidance，TSG)，本文提出一个运动规划与控制同时执行的方法，实现了从滚装船尾门(或舷门)到船内车位间的自主车(autonomous land vehicle，ALV)避障和泊车位姿控制.

1 ALV模型

ALV形状近似为矩形，后轮驱动，前轮转向，如图1所示.L为车长，W为车宽.u为前轮转向角，u∈［-π/6，π/6］.θ是 ALV 和 x轴夹角，θ∈［-π，π］.后轴中点 M(x，y)作为参考点.定义 q=(x，y)T，q 看成是位姿向量(x，y，θ)T的子向量.

图1 ALV模型Fig.1 ALV model

ALV的运动学模型可以描述为［9］

式中:规定角度逆时针为正，顺时针为负;输入量v和ω分别为ALV线速度和前轮转向角速度，它们受到下面约束:

输出是可执行的运动，对应一个位姿三元组(x，y，θ)T.车上安装了激光测距传感器探测周围的障碍物.障碍物的形状和位置等信息通过传感器获得.

2 基于ACF和TSG的ALV运动规划

ALV运动规划要考虑避障，TSG［10］自身不具有避障的功能，需结合其他方法.JING等［11-12］提出ACF法，克服了人工势场(artificial potential field，APF)所存在的局部极小.但是，JING等提出的ACF法用于完整机器人.基于JING等的工作，本文提出一个局部避障算法.

在当前的位姿，如果传感器没有探测到障碍物，ALV在ACF的引力Fa和TSG的作用下向目标位姿运动，速度和角速度分别为

式中:e(Fa)表示沿Fa方向的单位向量，nc(t)是由TSG确定的车的加速度，Vm是车的速度，ξv和ξω表示增益系数，Fa是作用在M处的引力，vmax和ωmax分别为速度和角速度上限.θd是引力的方向角.TSG给出的加速度表达式为［10］

式中:nT为目标加速度，Vc是闭合速度:

式中:RTM1和RTM2是车—目标相对距离的x分量和y分量，VTM1和 VTM2是相对速度分量.式(5)中，tgo为车剩余行进时间:

目标静止，所以 VT=0，nT=0.

如果传感器探测到障碍物，应用局部避障算法.由于考虑车的矩形外形，不能简单地将合力作用点等效在参考点.兼顾到算法复杂性和实时性，计入障碍点到车轮廓的最近距离.障碍物产生的斥力通过障碍点和ALV轮廓间的最近距离确定.这种思想可以把车辆的几何形状考虑到ACF中，通过确定速度、角速度与外力的关系，将ALV受到的非完整约束考虑进去.激光测距仪数据可以直接应用，因此该方法适用于实时避障.

目标qd处的引力场为［12］

式中:引力强度参数Ka＞0.

斥力作用在车的外形轮廓上.当传感器探测到一个障碍物点Oi，生成斥力:

式中:qVcontour表示ALV上距离Oi最近的点，Kro为排斥力强度参数.Oi点处的协调力:

式中:Kno为协调力强度参数，λ∈{-1，0，1}为协调因子，其值待定，正交矩阵:

排斥力强度参数为

式中:kr＞0为常量，kri为待定项.

如果ALV在接近静态障碍物Oi时能够保证有‖gi(q)-q‖-Rmin＞0，则ALV一定能够安全绕过障碍物，而不会与障碍物Oi相碰.这里，Rmin为ALV的最小转弯半径.按照下式确定排斥力可变参数:

式中:n≥1.

协调力强度参数为

式中:kn＞0为常量，Kn_max为协调力上界.

协调力可变参数为

式中:m＞0其值待定.

斥力和协调力的合力为合力通过引力、斥力和协调力叠加求得.因为前者与后两者的作用点不同，故不能简单求和.由理论力学中平面任意力系向作用面内一点简化原理［13］，把斥力和协调力叠加后向参考点化简，得到力矢量Fco'(q)和力偶Mco，在参考点将引力Fa(q)和力矢量Fco'(q)合成得到合力矢量，因此，对于各个障碍物点Oi(i=1，2，…，n)，ALV参考点处的最终合力主矢为

主矩为

为了使ALV沿着合力的方向Ftotal/‖Ftotal‖移动，线速度和角速度分别由合力主矢和主矩映射得到:

当ALV遇到较短的障碍物，如图2(a)、(b)所示，因为斥力和协调力的作用点在车的某一侧的边或顶点上.车辆将在合力主矢和主矩的作用下远离障碍物，这种情况对车的运行平稳性影响不大.

图2 障碍物点生成的斥力Fig.2 The repulsive force generated by obstacle points

当ALV的一侧有长的障碍物，其在车运行方向上的长度超出传感器的探测范围时，把障碍物看成一堵墙，采用沿墙走的策略，如图3(a)所示.只要ALV遇到一个障碍物后，在躲避过它之前，协调因子 λ 的决策能保持一致即可［12］.引力沿着(go(q)-q)/||go(q)-q||方向的分量与斥力平衡;引力沿着λT(q-go(q))/||q-go(q)||方向的分量与协调力Fno(q)合成得到Ftotal.特殊情况，当在车的同一侧探测到2个或2个以上障碍物点到车的轮廓线距离相等时，如图3(b)，为了实现平稳的沿墙走，在参考点处只计入一个障碍点产生的斥力和协调力，并忽略力偶.

当ALV运行在通道中，即当ALV两侧的障碍物在运行方向上的长度均超出传感器的探测范围，在车的两侧各考虑一个最近点，如图4所示.忽略引力作用，则ALV纵轴两侧的斥力和协调力向参考点化简后的合力Fco'(q)基本上沿着通道中心线，力偶Mco1和Mco2符号相反，彼此相消一部分，车辆近似沿着中心线运行，运行平稳.特殊情况，当两侧检测到多个障碍点到车的轮廓的距离相等，处理办法和沿墙走中某一侧检测到多个障碍点相同.

图3 ALV沿墙壁跟踪Fig.3 Follow wall behavior of ALV

图4 ALV在通道中Fig.4 ALV in channel

避障结束后，ALV在引力Fa和TSG作用下向目标位姿运动.当运动到距离目标为d(d=ηRmin，η＞0)时，应用TSG控制ALV向目标位姿运动，速度和角速度分别为

3 ALV远程自主泊车方案

假设车位信息已知，本文考虑以下几种车位，如图5所示.根据不同的车位情况计算泊车的起始范围.

图5 车位示意Fig.5 Schematic diagram of parking spaces

图6 ALV自主泊车流程Fig.6 Flow chart of ALV autonomous parking

图5中qtemp表示路点.每个车位设一个路点.在接近车位时，先以qtemp为子目标调整姿态，到路点后再进入最终位姿qd.图5(a)中，有

图5(b)中，有

图5(c)、(d)可以看成是2个车位，对于垂直车位(c)，有

对于水平车库图5(d)，有:

ALV自主泊车流程图如图6所示.

4 仿真实验与分析

实验参数设定如下:L=0.4，W=0.2，Ka=1，n=2，kr=5，kn=30，m=2，Kn_max=100，Rmin=0.8m，K=2，η =3，sR=1.5m，ξv=0.5，ξω=0.5，vmax=0.5m/s，ωmax=0.5rad/s.激光传感器的方向分辨率 1°，取样时间 Δt=1.0s.

变换不同的起始位姿，分别向A-F车位自主泊车的仿真实验结果如图7所示.车辆在起始阶段经历了较大的重定向之后，依然成功实现远程自主泊车.图7(a)、(b)中车辆的运行轨迹都验证了文献［11］中所描述的人工协调场的形状.从图7(b)中椭圆标注处可见，通过实时决策协调因子，控制协调力的方向，成功克服了传统APF法的局部极小问题.图7(c)所示为图7(b)中泊车入B车位过程中的速度和角速度随仿真时间的变化曲线.图7(b)中各个车位的泊车目标位姿和最终误差如表1所示.从表1中实验数据和图7可见，仿真结果误差较小，在可接受的范围之内.

为了验证该方法的重规划性能，分别对图7中的环境做以下改变:1)增加两堵外形不规则的障碍墙，模拟滚装船甲板上的过道;2)增加狭窄通道模拟尾门入口;3)减小两堵墙间的出口宽度，模拟甲板过道狭窄出口.起始位姿都是(1，4，-π/4)T，以泊车入B车位为例，实验结果分别如图8所示.图8(a)中，通过实时确定协调力，ALV产生墙壁跟踪行为，在通道中轨迹近似沿着中线，最终穿越通道到达目标.图8(b)中，对于起点附近障碍物形成窄的通道，如果应用APF法，车辆会陷入局部极小，应用本文提出的局部避障方法，车辆成功穿越.图8(c)中，车辆先后穿越起点附近的狭窄通道和墙壁右端狭窄出口，泊车入位.

图7 不同起始位姿下的仿真结果Fig.7 Simulation results under different initial postures

表1 图7(b)中各车位的泊车误差Table 1 Error of each parking space in Fig.7(b)

图8 部分环境改变的重规划Fig.8 Replanning when environment changes partially

5 结束语

针对大型汽车滚装船装载时船上部分环境未知情况下的非完整自主车自动泊车问题，基于人工协调场法与弹道成型制导律，本文提出一个运动规划与控制同时执行的远程自动泊车方法，计算快捷，易于工程实现.分析了船内形成狭窄通道和自主车周围不同形状大小的障碍物对车运行平稳性的影响.考虑车辆的几何形状，提出一种新的斥力、协调力的计算方法.在设计底层控制方式时将合力主矢映射为当前运动方向上的速度标量，将主矩映射为角速度标量，满足车辆受到的非完整约束.仿真实验表明，该方法用于非完整自主车运动规划、远程自主泊车是可行、有效的.它适用于任意形状的受非完整性约束的机器人系统，甚至那些非常复杂的开放性的路径规划问题，对其他非完整系统的研究有重要的参考价值.

未来的工作是将该方法扩展到其他类型的车辆，如多体车的情况.

［1］DEMIRLI K，KHOSHNEJAD M.Autonomous parallel parking of a car-like mobile robot by a neuro-fuzzy sensor-based controller［J］.Fuzzy Sets and Systems，2009，160:2876-2891.

［2］ZHAO Yanan，COLLINS E G，Jr.Robust automatic parallel parking in tight spaces via fuzzy logic［J］.Robotics and Autonomous Systems，2005，51:111-127.

［3］GOMEZ-BRAVO F，CUESTA F，OLLEROB A，VIGURIA A.Continuous curvature path generation based on β-spline curves for parking manoeuvres［J］.Robotics and Autonomous Systems，2008，56:360-372.

［4］宋金泽，戴斌，单恩忠，等.融合动力学约束的自主平行泊车轨迹生成方法［J］.中南大学学报:自然科学版，2009，40(suppl.1):135-141.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，2009，40(Suppl.1):135-141.

［5］LI T H S，CHANG S J.Autonomous fuzzy parking control of a car-Like mobile robot［J］.IEEE Transactions on Systems，Man，and Cybernetics-Part A:Systems and Humans，2003，33(4):451-465.

［6］GOMEZ-BRAVO F，CUESTA F，OLLERO A.Parallel and diagonal parking in nonholonomic autonomous vehicles［J］.Engineering Applications of Artificial Intelligence，2001，14:419-434.

［7］BATURONE I，MORENO-VELO F J，SANCHEZ-SOLANO S，et al.Automatic design of fuzzy controllers for car-like autonomous robots［J］.IEEE Transaction on Fuzzy System，2004，12(4):447-465.

［8］LI T H S，YEH Y C，WU J D，et al.Multifunctional intelligent autonomous parking controllers for carlike mobile robots［J］.IEEE Transactions on Industrial Electronics，2010，57(5):1687-1700.

［9］LAUMOND J P，SEKHAVAT S，LAMIRAUX F.Robot motion planning and control［M］.Berlin:Springer-Verlag，1998:1-53.

［10］ZARCHAN P.Tactical and strategic missile guidance［M］.5th ed.Reston:Progress in Astronautics and Aeronautics，2007:11-29，541-569.

［11］JING Xingjian，WANG Yuechao.Local minima-free design of artificial coordinating fields［J］.Journal of Control Theory and Applications，2004，4:371-380.

［12］JING Xingjian，WANG Yuechao，TAN Dalong.Artificial coordinating field and its application to motion planning of robots in uncertain dynamic environments［J］.Science in China，Series E:Engineering＆ Materials Science，2004，47(5):577-594.

［13］哈尔滨工业大学理论力学教研室.理论力学(Ⅰ)［M］.6版.北京:高等教育出版社，2002:41-46.