滾裝船內(nèi)受非完整約束的自主車自動泊車

王能建，張德福，周麗杰

(哈爾濱工程大學 機電工程學院，黑龍江 哈爾濱 15001)

大型汽車滾裝船(automobile Ro-Ro ship，ARRS)的貨艙如一大型停車場，擁有眾多的車位，裝載時如何安全快速地將汽車泊車入船是個挑戰(zhàn).事實證明，即使對于熟練的駕駛員，泊車也絕非易事.自動泊車技術應運而生，該技術可以輔助甚至替代駕駛員完成泊車動作，緩解駕駛員負擔并增強下一代客車的安全性.因此，這方面的研究具有實際意義.

傳統(tǒng)泊車算法通常分成3個階段［1］:1)基于聲吶或視覺系統(tǒng)獲得車位的信息，建立一幅泊車位的局部地圖;2)到達倒車的起始位置，同時調整車的首向平行于停車位;3)規(guī)劃出從起始位姿到目標位姿的車的運動.很多學者研究了該種運動規(guī)劃問題，但其目前仍然是個開放問題，需進一步研究.國內(nèi)外解決自動泊車的方法大致分2類［2］:1)路徑規(guī)劃:考慮環(huán)境模型和車輛的動力學約束，預先規(guī)劃到達停車位的幾何路徑，通過控制命令跟蹤該路徑［3-4］;2)應用模糊邏輯或神經(jīng)網(wǎng)絡模仿駕駛技術熟練的駕駛員的泊車行為［1-5］.該方法中沒有參考路徑，依靠車的當前位置與停車位置間的相對誤差來進行控制.針對輪式車輛的并行和斜列式停車問題，文獻［6］提出一種實時規(guī)劃系列無碰操作的泊車方法.文獻［7］針對斜列式停車問題，提出一種模糊控制器.通過聲吶數(shù)據(jù)獲取周圍的環(huán)境信息，文獻［8］提出一種智能自動泊車控制器，能有效地將車泊車入位.

以上研究主要集中在近距離平行泊車，針對裝載時ARRS上部分環(huán)境未知情況下的自動泊車研究還未見報道.ARRS內(nèi)空間小，易形成狹窄通道，車輛起始位姿距離目標車位較遠(可稱為遠程自主泊車)，泊車過程中除了考慮車輛的幾何形狀和非完整約束，還需要規(guī)避未知障礙物.基于人工協(xié)調場(artificial coordinating field，ACF)與彈道成型制導律(trajectory shaping guidance，TSG)，本文提出一個運動規(guī)劃與控制同時執(zhí)行的方法，實現(xiàn)了從滾裝船尾門(或舷門)到船內(nèi)車位間的自主車(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］

式中:規(guī)定角度逆時針為正，順時針為負;輸入量v和ω分別為ALV線速度和前輪轉向角速度，它們受到下面約束:

輸出是可執(zhí)行的運動，對應一個位姿三元組(x，y，θ)T.車上安裝了激光測距傳感器探測周圍的障礙物.障礙物的形狀和位置等信息通過傳感器獲得.

2 基于ACF和TSG的ALV運動規(guī)劃

ALV運動規(guī)劃要考慮避障，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和ξω表示增益系數(shù)，F(xiàn)a是作用在M處的引力，vmax和ωmax分別為速度和角速度上限.θd是引力的方向角.TSG給出的加速度表達式為［10］

式中:nT為目標加速度，Vc是閉合速度:

式中:RTM1和RTM2是車—目標相對距離的x分量和y分量，VTM1和 VTM2是相對速度分量.式(5)中，tgo為車剩余行進時間:

目標靜止，所以 VT=0，nT=0.

如果傳感器探測到障礙物，應用局部避障算法.由于考慮車的矩形外形，不能簡單地將合力作用點等效在參考點.兼顧到算法復雜性和實時性，計入障礙點到車輪廓的最近距離.障礙物產(chǎn)生的斥力通過障礙點和ALV輪廓間的最近距離確定.這種思想可以把車輛的幾何形狀考慮到ACF中，通過確定速度、角速度與外力的關系，將ALV受到的非完整約束考慮進去.激光測距儀數(shù)據(jù)可以直接應用，因此該方法適用于實時避障.

目標qd處的引力場為［12］

式中:引力強度參數(shù)Ka＞0.

斥力作用在車的外形輪廓上.當傳感器探測到一個障礙物點Oi，生成斥力:

式中:qVcontour表示ALV上距離Oi最近的點，Kro為排斥力強度參數(shù).Oi點處的協(xié)調力:

式中:Kno為協(xié)調力強度參數(shù)，λ∈{-1，0，1}為協(xié)調因子，其值待定，正交矩陣:

排斥力強度參數(shù)為

式中:kr＞0為常量，kri為待定項.

如果ALV在接近靜態(tài)障礙物Oi時能夠保證有‖gi(q)-q‖-Rmin＞0，則ALV一定能夠安全繞過障礙物，而不會與障礙物Oi相碰.這里，Rmin為ALV的最小轉彎半徑.按照下式確定排斥力可變參數(shù):

式中:n≥1.

協(xié)調力強度參數(shù)為

式中:kn＞0為常量，Kn_max為協(xié)調力上界.

協(xié)調力可變參數(shù)為

式中:m＞0其值待定.

斥力和協(xié)調力的合力為合力通過引力、斥力和協(xié)調力疊加求得.因為前者與后兩者的作用點不同，故不能簡單求和.由理論力學中平面任意力系向作用面內(nèi)一點簡化原理［13］，把斥力和協(xié)調力疊加后向參考點化簡，得到力矢量Fco'(q)和力偶Mco，在參考點將引力Fa(q)和力矢量Fco'(q)合成得到合力矢量，因此，對于各個障礙物點Oi(i=1，2，…，n)，ALV參考點處的最終合力主矢為

主矩為

為了使ALV沿著合力的方向Ftotal/‖F(xiàn)total‖移動，線速度和角速度分別由合力主矢和主矩映射得到:

當ALV遇到較短的障礙物，如圖2(a)、(b)所示，因為斥力和協(xié)調力的作用點在車的某一側的邊或頂點上.車輛將在合力主矢和主矩的作用下遠離障礙物，這種情況對車的運行平穩(wěn)性影響不大.

圖2 障礙物點生成的斥力Fig.2 The repulsive force generated by obstacle points

當ALV的一側有長的障礙物，其在車運行方向上的長度超出傳感器的探測范圍時，把障礙物看成一堵墻，采用沿墻走的策略，如圖3(a)所示.只要ALV遇到一個障礙物后，在躲避過它之前，協(xié)調因子 λ 的決策能保持一致即可［12］.引力沿著(go(q)-q)/||go(q)-q||方向的分量與斥力平衡;引力沿著λT(q-go(q))/||q-go(q)||方向的分量與協(xié)調力Fno(q)合成得到Ftotal.特殊情況，當在車的同一側探測到2個或2個以上障礙物點到車的輪廓線距離相等時，如圖3(b)，為了實現(xiàn)平穩(wěn)的沿墻走，在參考點處只計入一個障礙點產(chǎn)生的斥力和協(xié)調力，并忽略力偶.

當ALV運行在通道中，即當ALV兩側的障礙物在運行方向上的長度均超出傳感器的探測范圍，在車的兩側各考慮一個最近點，如圖4所示.忽略引力作用，則ALV縱軸兩側的斥力和協(xié)調力向參考點化簡后的合力Fco'(q)基本上沿著通道中心線，力偶Mco1和Mco2符號相反，彼此相消一部分，車輛近似沿著中心線運行，運行平穩(wěn).特殊情況，當兩側檢測到多個障礙點到車的輪廓的距離相等，處理辦法和沿墻走中某一側檢測到多個障礙點相同.

圖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所示.根據(jù)不同的車位情況計算泊車的起始范圍.

圖5 車位示意Fig.5 Schematic diagram of parking spaces

圖6 ALV自主泊車流程Fig.6 Flow chart of ALV autonomous parking

圖5中qtemp表示路點.每個車位設一個路點.在接近車位時，先以qtemp為子目標調整姿態(tài)，到路點后再進入最終位姿qd.圖5(a)中，有

圖5(b)中，有

圖5(c)、(d)可以看成是2個車位，對于垂直車位(c)，有

對于水平車庫圖5(d)，有:

ALV自主泊車流程圖如圖6所示.

4 仿真實驗與分析

實驗參數(shù)設定如下: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所示.車輛在起始階段經(jīng)歷了較大的重定向之后，依然成功實現(xiàn)遠程自主泊車.圖7(a)、(b)中車輛的運行軌跡都驗證了文獻［11］中所描述的人工協(xié)調場的形狀.從圖7(b)中橢圓標注處可見，通過實時決策協(xié)調因子，控制協(xié)調力的方向，成功克服了傳統(tǒng)APF法的局部極小問題.圖7(c)所示為圖7(b)中泊車入B車位過程中的速度和角速度隨仿真時間的變化曲線.圖7(b)中各個車位的泊車目標位姿和最終誤差如表1所示.從表1中實驗數(shù)據(jù)和圖7可見，仿真結果誤差較小，在可接受的范圍之內(nèi).

為了驗證該方法的重規(guī)劃性能，分別對圖7中的環(huán)境做以下改變:1)增加兩堵外形不規(guī)則的障礙墻，模擬滾裝船甲板上的過道;2)增加狹窄通道模擬尾門入口;3)減小兩堵墻間的出口寬度，模擬甲板過道狹窄出口.起始位姿都是(1，4，-π/4)T，以泊車入B車位為例，實驗結果分別如圖8所示.圖8(a)中，通過實時確定協(xié)調力，ALV產(chǎn)生墻壁跟蹤行為，在通道中軌跡近似沿著中線，最終穿越通道到達目標.圖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 部分環(huán)境改變的重規(guī)劃Fig.8 Replanning when environment changes partially

5 結束語

針對大型汽車滾裝船裝載時船上部分環(huán)境未知情況下的非完整自主車自動泊車問題，基于人工協(xié)調場法與彈道成型制導律，本文提出一個運動規(guī)劃與控制同時執(zhí)行的遠程自動泊車方法，計算快捷，易于工程實現(xiàn).分析了船內(nèi)形成狹窄通道和自主車周圍不同形狀大小的障礙物對車運行平穩(wěn)性的影響.考慮車輛的幾何形狀，提出一種新的斥力、協(xié)調力的計算方法.在設計底層控制方式時將合力主矢映射為當前運動方向上的速度標量，將主矩映射為角速度標量，滿足車輛受到的非完整約束.仿真實驗表明，該方法用于非完整自主車運動規(guī)劃、遠程自主泊車是可行、有效的.它適用于任意形狀的受非完整性約束的機器人系統(tǒng)，甚至那些非常復雜的開放性的路徑規(guī)劃問題，對其他非完整系統(tǒng)的研究有重要的參考價值.

未來的工作是將該方法擴展到其他類型的車輛，如多體車的情況.

［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］哈爾濱工業(yè)大學理論力學教研室.理論力學(Ⅰ)［M］.6版.北京:高等教育出版社，2002:41-46.