無人駕駛機(jī)器人機(jī)械腿模糊監(jiān)督控制*

王和榮，陳 剛

（南京理工大學(xué)機(jī)械工程學(xué)院，南京 210094）

前言

從解放駕駛員雙手，緩解駕駛疲勞感、提高出行安全性出發(fā)，“無人駕駛”的理念越來越為大眾接受。國內(nèi)外許多機(jī)構(gòu)也都紛紛轉(zhuǎn)向無人駕駛方面的研究，國外駕駛機(jī)器人技術(shù)仍處于保密階段，只有德國、美國、英國以及日本等少數(shù)發(fā)達(dá)國家擁有此項(xiàng)技術(shù)。國內(nèi)駕駛機(jī)器人的研究相對(duì)較晚，第一代駕駛機(jī)器人是由東南大學(xué)與南京汽車研究所聯(lián)合研制［1-4］，但近年來很多高校與研究機(jī)構(gòu)參與到了無人駕駛機(jī)器人的研究中，包括南京理工大學(xué)、北京航空航天大學(xué)、太原理工大學(xué)、清華大學(xué)、上海交通大學(xué)、中國汽車技術(shù)研究中心和南京農(nóng)業(yè)大學(xué)等［5-12］。

無人駕駛機(jī)器人關(guān)鍵技術(shù)之一是控制執(zhí)行器跟蹤目標(biāo)位移，即使執(zhí)行機(jī)構(gòu)作用下的汽車跟蹤目標(biāo)車速。國內(nèi)南京信息工程大學(xué)的袁安富提出了一種油門機(jī)械腿模糊自整定PID控制方法，但對(duì)于模型的仿真驗(yàn)證，只利用階躍信號(hào)與正弦信號(hào)進(jìn)行驗(yàn)證，并沒有涉及汽車循環(huán)車速行駛工況的測(cè)試［13］；太原理工大學(xué)牛喆等采用CATIA建立了駕駛機(jī)器人執(zhí)行器模型并借助ADAMS對(duì)其速度、位移和加速度進(jìn)行了仿真，但缺少利用執(zhí)行器操縱車輛的仿真曲線［5］；北京航空航天大學(xué)劉坤明等建立了機(jī)械腿的動(dòng)力學(xué)模型并結(jié)合CarSim進(jìn)行了車輛速度的跟蹤仿真，但是在系統(tǒng)的控制算法上，僅僅采用普通PID控制，同時(shí)結(jié)果缺少與實(shí)驗(yàn)數(shù)據(jù)曲線的對(duì)比分析［14］。

由于單純使用模糊控制器跟蹤波動(dòng)較大的位移信號(hào)時(shí)，系統(tǒng)會(huì)趨于不穩(wěn)定。根據(jù)Lyapunov穩(wěn)定性原理，設(shè)計(jì)了起穩(wěn)定控制作用的監(jiān)督控制器，使得系統(tǒng)的跟蹤誤差發(fā)散時(shí)，利用模糊控制器與監(jiān)督控制器相結(jié)合的方法，使得系統(tǒng)趨于穩(wěn)定［15-16］。本文中分析了機(jī)械腿的運(yùn)動(dòng)特征，建立了機(jī)械腿的動(dòng)力學(xué)模型，利用模糊監(jiān)督控制方法實(shí)現(xiàn)了對(duì)油門／制動(dòng)機(jī)械腿的位置跟蹤控制；并設(shè)計(jì)了油門／制動(dòng)切換控制器，以車輛跟蹤速度和加速度誤差為輸入，協(xié)調(diào)控制油門／制動(dòng)機(jī)械腿的運(yùn)動(dòng)，仿真跟蹤結(jié)果與實(shí)驗(yàn)數(shù)據(jù)進(jìn)行比較，驗(yàn)證了方法的有效性。

1 駕駛機(jī)器人機(jī)械腿模型

1.1 機(jī)械腿運(yùn)動(dòng)學(xué)模型

駕駛機(jī)器人機(jī)械腿的三維結(jié)構(gòu)如圖1所示，該結(jié)構(gòu)將直線電機(jī)與箱體進(jìn)行固定，電機(jī)驅(qū)動(dòng)時(shí)電機(jī)推桿沿直線前后運(yùn)動(dòng)，帶動(dòng)滑塊沿著機(jī)械小腿桿滑動(dòng)，同時(shí)小推桿繞著與箱體的鉸接點(diǎn)作旋轉(zhuǎn)運(yùn)動(dòng)?；瑝K與電機(jī)推桿之間通過推桿鎖夾裝置和吊耳連接，卡爪與汽車的踏板連接，在機(jī)械小腿桿的作用下沿著固定的踏板軌跡運(yùn)動(dòng)。

圖1 機(jī)械腿三維圖

利用D-H法描述機(jī)械腿的運(yùn)動(dòng)，如圖2所示，圖中x0Oy0為固定參考系，x1Oy1，x2Ay2和x3By3分別為對(duì)應(yīng)各桿的運(yùn)動(dòng)連體參考系，h，θ1和θ2分別為電機(jī)推桿高度、OA桿轉(zhuǎn)動(dòng)夾角和AB桿轉(zhuǎn)動(dòng)夾角。汽車踏板在運(yùn)動(dòng)過程中軌跡是預(yù)定的，其可簡化為繞固定點(diǎn)P的圓弧。為方便寫出各坐標(biāo)系之間的齊次轉(zhuǎn)換關(guān)系，由圖2機(jī)械腿連桿坐標(biāo)系得到機(jī)械腿連桿參數(shù)，如表1所示。

圖2 機(jī)械腿連桿坐標(biāo)系

表1 機(jī)械腿的連桿參數(shù)

機(jī)械腿運(yùn)動(dòng)末端相對(duì)于固定坐標(biāo)系的變換矩陣為

同時(shí)由于在運(yùn)動(dòng)過程中，機(jī)械腿末端B點(diǎn)是繞著固定的踏板軌跡——以P點(diǎn)為圓心的一段圓弧運(yùn)動(dòng)，得B點(diǎn)滿足運(yùn)動(dòng)學(xué)方程：

式中Bx，By，Px，Py和 r分別為 B點(diǎn)、P點(diǎn)的橫縱坐標(biāo)和踏板軌跡半徑。

1.2 機(jī)械腿動(dòng)力學(xué)模型

利用拉格朗日方程建立動(dòng)力學(xué)方程：

式中Ek和Ep分別為系統(tǒng)總的動(dòng)能和勢(shì)能。

通過對(duì)機(jī)械腿各桿件運(yùn)動(dòng)關(guān)系的推導(dǎo)，得系統(tǒng)總的動(dòng)能與勢(shì)能為

式中：m1和m2分別為OA和AB桿的質(zhì)量；a和b分別為OA和AB桿的長度。

根據(jù)下式計(jì)算關(guān)節(jié)驅(qū)動(dòng)力矩矢量：

2 模糊監(jiān)督控制方法

模糊監(jiān)督控制是由主控制器——模糊控制器，與穩(wěn)定性補(bǔ)償控制器——監(jiān)督控制器兩層控制器構(gòu)成。當(dāng)機(jī)械腿跟蹤誤差在給定誤差范圍內(nèi)時(shí)，主控制器單獨(dú)作用；當(dāng)誤差超出給定值時(shí)，第2層開始作用，以確保系統(tǒng)的穩(wěn)定性。圖3為在模糊監(jiān)督控制下油門／制動(dòng)機(jī)械腿操縱自動(dòng)擋車的車速跟蹤方框圖，為了使得汽車跟蹤目標(biāo)車速，以汽車跟蹤的加速度誤差和速度誤差作為油門／制動(dòng)機(jī)械腿切換控制器的輸入，以推桿直線位移量作為輸出，通過坐標(biāo)轉(zhuǎn)換成各機(jī)械腿的轉(zhuǎn)角來驅(qū)動(dòng)機(jī)械腿運(yùn)動(dòng)。油門／制動(dòng)機(jī)械腿在模糊監(jiān)督控制下跟蹤目標(biāo)轉(zhuǎn)角，并實(shí)時(shí)輸出油門開度與制動(dòng)力的大小。整個(gè)系統(tǒng)在速度閉環(huán)控制下，實(shí)現(xiàn)汽車對(duì)目標(biāo)速度的精確跟蹤。

圖3 模糊監(jiān)督控制作用下車速跟蹤方框圖

2.1 模糊控制器

模糊控制器的工作原理如圖4所示，模糊控制器以位移跟蹤誤差及誤差變化率為輸入，通過對(duì)輸入量的模糊處理，再經(jīng)由預(yù)先設(shè)置好的模糊推理規(guī)則，實(shí)時(shí)調(diào)整PID參數(shù)值，使得作用于機(jī)械腿上的驅(qū)動(dòng)控制量能夠隨著誤差大小變化。

圖4 模糊控制器方框圖

對(duì)變量進(jìn)行模糊化處理，定義誤差及誤差變化率的論域：

其模糊子集為

子集中元素分別代表負(fù)大、負(fù)中、負(fù)小、零、正小、正中、正大。輸出變量Δkp，Δki和Δkd的論域分別為

其模糊子集為｛NB，NM，NS，ZE，PS，PM，PB｝，為提高控制靈敏度，輸入輸出變量的隸屬函數(shù)都采用Z型函數(shù)、三角函數(shù)與Sigmoid函數(shù)相結(jié)合的形式。跟蹤誤差信號(hào)經(jīng)過模糊化之后，按照模糊規(guī)則實(shí)時(shí)推理出PID參數(shù)的增量，則新的控制量為

表2 Δk p的模糊規(guī)則

表3 Δk i的模糊規(guī)則

2.2 監(jiān)督控制器

2.2.1 控制律設(shè)計(jì)

監(jiān)督控制器是在模糊控制系統(tǒng)趨于不穩(wěn)定時(shí)開始工作，以確保系統(tǒng)穩(wěn)定，其設(shè)計(jì)思想是在模糊控制器 ufuzz（x）上添加一個(gè)監(jiān)督控制器 us（x），us（x）只是在狀態(tài)變量達(dá)到約束集｛x：｜x｜≤Mx｝的邊界時(shí)才不為0，其中Mx為設(shè)計(jì)者給定的大于0的實(shí)數(shù)。

監(jiān)督控制器設(shè)計(jì)為

表4 Δk d的模糊規(guī)則

模糊控制規(guī)則在專家經(jīng)驗(yàn)的基礎(chǔ)上進(jìn)行修改建立，如表2～表4所示。本文中采用Zadeh模糊邏輯進(jìn)行and操作，并采用質(zhì)心法去模糊化。

由于機(jī)械腿是多輸入多輸出的雙關(guān)節(jié)結(jié)構(gòu)，為便于監(jiān)督控制器的設(shè)計(jì)，式（6）可以寫成：

由式（11）可見，當(dāng) si→0時(shí)，ei→0（i=1，2），則控制目標(biāo)轉(zhuǎn)變?yōu)槭?si→0（i=1，2）。

2.2.2 穩(wěn)定性分析

監(jiān)督控制器的設(shè)計(jì)是使得閉環(huán)系統(tǒng)為全局穩(wěn)定的，由Lyapunov穩(wěn)定性分析理論，定義Lyapunov函數(shù)為

由式（20），當(dāng) V≤0時(shí)，監(jiān)督控制器為

可得

由于

對(duì)式（15）求導(dǎo)，將式（18）帶入得

解微分方程

由 Lyapunov穩(wěn)定律可知，t→∞時(shí)，si（t）→0；ei及它的導(dǎo)數(shù)均一致收斂到0，系統(tǒng)穩(wěn)定。

2.3 油門／制動(dòng)切換控制器

車速跟蹤是制動(dòng)／油門踏板交替作用的效果，當(dāng)一個(gè)工作時(shí)另一個(gè)輸出應(yīng)該為0。同時(shí)由圖1機(jī)械腿的結(jié)構(gòu)可知，直線電機(jī)推桿前進(jìn)的距離是有限的，最遠(yuǎn)使得滑塊運(yùn)動(dòng)到A處。同時(shí)考慮到仿真中如果直接用飽和限制模塊，切換控制器輸出給制動(dòng)／油門機(jī)械腿的跟蹤目標(biāo)位移曲線將是階躍型波動(dòng)的，這與實(shí)際不相符，同時(shí)階躍控制量也會(huì)使微分結(jié)果無窮，導(dǎo)致仿真失敗。為限制最大運(yùn)動(dòng)位移且使目標(biāo)位移曲線光滑變動(dòng)，定義油門／制動(dòng)切換控制器的輸出為

式中：xmax為推桿運(yùn)動(dòng)的最大位移；q為PID控制器的輸出，q隨速度跟蹤誤差的增大而增大。

油門／制動(dòng)切換控制器以速度誤差Δv、加速度誤差Δa和PID控制器的輸出為輸入量，其控制規(guī)則如下。

（1）當(dāng) Δv≥EU，或 0＜Δv＜EU＆Δa＞0時(shí)，驅(qū)動(dòng)油門，即跟蹤車速低于目標(biāo)車速的量大于閾值EU，或跟蹤車速低于目標(biāo)車速的量介于0～EU之間，但呈繼續(xù)擴(kuò)大趨勢(shì)時(shí)，應(yīng)控制踩油門以減小跟蹤誤差。

（2）當(dāng) Δv≤-EU，或-EU＜Δv＜0＆Δa＜0時(shí)，驅(qū)動(dòng)制動(dòng)，即跟蹤車速高于目標(biāo)車速的量大于閾值EU，或是跟蹤車速高于目標(biāo)車速的量介于0～EU之間，但呈繼續(xù)擴(kuò)大趨勢(shì)時(shí)，應(yīng)控制踩制動(dòng)以減小跟蹤誤差。

（3）當(dāng)Δv和Δa處于其它情況時(shí)，不對(duì)制動(dòng)和油門作任何動(dòng)作。

3 仿真與試驗(yàn)驗(yàn)證

為了驗(yàn)證提出控制方法的有效性，對(duì)搭建的機(jī)械腿控制模型設(shè)置仿真參數(shù)，給定一正弦位移信號(hào)，模型跟蹤曲線如圖5所示，實(shí)線為目標(biāo)轉(zhuǎn)角，虛線為機(jī)械腿跟蹤轉(zhuǎn)角。由圖可知，跟蹤曲線與目標(biāo)曲線的重合度較高，跟蹤誤差在控制允許的范圍內(nèi)。表5為仿真設(shè)置參數(shù)。

圖5 機(jī)械腿轉(zhuǎn)角跟蹤曲線

表5 仿真參數(shù)設(shè)置

為進(jìn)一步驗(yàn)證提出方法的有效性，進(jìn)行了無人駕駛機(jī)器人車輛排放耐久性仿真試驗(yàn)，以及無人駕駛機(jī)器人車輛和人類駕駛員車輛的底盤測(cè)功機(jī)排放耐久性實(shí)車試驗(yàn)。仿真的控制方法分別采用提出方法和模糊PID控制方法，關(guān)鍵仿真參數(shù)見表5和表6。根據(jù)相關(guān)試驗(yàn)標(biāo)準(zhǔn)［17］，經(jīng)過車輛性能自學(xué)習(xí)，在BOCO NJ 150／80型底盤測(cè)功機(jī)上由無人駕駛機(jī)器人對(duì)長安悅翔自動(dòng)擋轎車進(jìn)行長時(shí)間的排放耐久性試驗(yàn)，部分試驗(yàn)現(xiàn)場(chǎng)圖如圖6所示。此外，利用人類駕駛員做同樣的排放耐久性試驗(yàn)［18］，并記錄車速變化數(shù)據(jù)。

表6 油門／制動(dòng)切換控制器仿真參數(shù)設(shè)置

圖6 無人駕駛機(jī)器人車輛進(jìn)行排放耐久性試驗(yàn)

圖7為車速跟蹤過程中機(jī)械腿制動(dòng)力對(duì)比曲線，實(shí)線為試驗(yàn)所測(cè)制動(dòng)力曲線，虛線為制動(dòng)機(jī)械腿仿真模型制動(dòng)力輸出曲線，兩條曲線基本重合驗(yàn)證了機(jī)械腿動(dòng)力學(xué)模型的有效性。圖8為車速跟蹤過程中油門的開度對(duì)比曲線，實(shí)線為試驗(yàn)中油門開度曲線，虛線為仿真模型的油門開度曲線，由于試驗(yàn)采用的是普通PID控制，可以看出曲線的波動(dòng)較大，同時(shí)與圖7相比，可以看出制動(dòng)與油門機(jī)械腿同一時(shí)刻只有一條機(jī)械腿在動(dòng)作，與實(shí)際相符。

圖7 制動(dòng)力對(duì)比曲線

圖8 油門開度

圖9為車速跟蹤對(duì)比曲線，虛線為輸入的目標(biāo)跟蹤車速，點(diǎn)畫線為在PID控制下的試驗(yàn)車速跟蹤曲線，帶標(biāo)志“▲”實(shí)線為記錄的人類駕駛員車速跟蹤曲線，實(shí)線為無監(jiān)督下的模糊自適應(yīng)PID車速跟蹤曲線，點(diǎn)虛線為提出的模糊監(jiān)督控制下的車速跟蹤曲線。由圖可知，普通PID控制下的車速跟蹤波動(dòng)較大，而模糊自適應(yīng)PID相比普通PID控制跟蹤精度有所提高，但是在局部位置誤差較大，相比之下，提出的模糊監(jiān)督控制下的跟蹤曲線比較平穩(wěn)，雖然跟蹤曲線在勻速階段與目標(biāo)車速曲線存在一定的誤差，但誤差保持在允許的誤差±2 km／h之內(nèi)。3種控制方法相比人類駕駛員，在車速跟蹤控制方面都有較大的改善。

圖9 車速跟蹤試驗(yàn)與仿真對(duì)比曲線

4 結(jié)論

鑒于模糊控制器針對(duì)不同結(jié)構(gòu)的控制對(duì)象，需反復(fù)進(jìn)行參數(shù)調(diào)節(jié)才使系統(tǒng)達(dá)到閉環(huán)穩(wěn)定，本文中利用Lyapunov穩(wěn)定性分析原理，設(shè)計(jì)了模糊監(jiān)督控制器，使得系統(tǒng)能夠時(shí)刻檢測(cè)跟蹤誤差，當(dāng)誤差超過給定值時(shí)，利用模糊監(jiān)督控制器能夠使誤差迅速收斂，使得系統(tǒng)重新保持穩(wěn)定。并將設(shè)計(jì)的模糊監(jiān)督控制器用于油門／制動(dòng)機(jī)械腿的控制，通過對(duì)比機(jī)械腿位置跟蹤曲線與目標(biāo)曲線的重合度，驗(yàn)證了控制方法的有效性。在此基礎(chǔ)上，設(shè)計(jì)了油門／制動(dòng)切換控制器，對(duì)目標(biāo)車速進(jìn)行跟蹤控制，用本文中提出的模糊監(jiān)督控制方法操縱機(jī)械腿進(jìn)行車速跟蹤的精度比模糊自適應(yīng)PID方式跟蹤的精度更高，且車速跟蹤相對(duì)平滑、穩(wěn)定，同時(shí)相比人類駕駛員反應(yīng)速度更加靈敏，更能滿足汽車耐久性試驗(yàn)下對(duì)不斷變化的車速的跟蹤需求。但本文中在建立機(jī)械腿的動(dòng)力學(xué)模型上，未考慮實(shí)際模型中參數(shù)的變化以及外界擾動(dòng)對(duì)建模帶來的誤差，這部分內(nèi)容將作為接下來研究工作的一部分。