基于動靜態(tài)安全場的機器人避障路徑控制仿真研究

張 郭，許艷英，張 亮

（1.重慶科創(chuàng)職業(yè)學(xué)院人工智能學(xué)院，重慶402160；2.西南交通大學(xué)機械工程學(xué)院，四川成都610031）

隨著自動化程度地提高，機器人在社會發(fā)展中迅速占領(lǐng)了市場，主要應(yīng)用于汽車、電子、醫(yī)療、航空及礦產(chǎn)開發(fā)等領(lǐng)域［1-2］。當(dāng)前，單臂機器人經(jīng)過多年的改進日趨成熟，在生產(chǎn)流水線上應(yīng)用較多，但是面對復(fù)雜任務(wù)或者多重任務(wù)時，不能進行協(xié)調(diào)和配合，受到了一定的局限性。雙臂機器人的誕生，不僅能夠完成復(fù)雜任務(wù)，而且還提高了工作效率，但是其控制難度也在上升。國內(nèi)雙臂機器人研究起步相對較晚，與國外相比，機器人運動路徑規(guī)劃和協(xié)調(diào)控制方面還存在很大差距。因此，研究雙臂機器人控制系統(tǒng)，對于打破國外機器人核心技術(shù)的封鎖具有重要意義。

為了提高機器人避障反應(yīng)速度，縮短機器人運動路徑，提高機器人運動軌跡跟蹤精度，國內(nèi)外學(xué)者對機器人避障展開研究。例如：文獻［3-4］研究了機械臂避障神經(jīng)網(wǎng)絡(luò)控制方法，創(chuàng)建了機械臂動力學(xué)模型，采用多層感知神經(jīng)網(wǎng)絡(luò)逼近原理，通過仿真驗證避障效果，從而提高了機械臂軌跡追蹤精度。文獻［5-6］研究移動機器人避障的模糊神經(jīng)網(wǎng)絡(luò)控制方法，設(shè)計了移動機器人的總體構(gòu)架，采用神經(jīng)網(wǎng)絡(luò)模糊控制規(guī)則，通過模糊推理提高控制系統(tǒng)的穩(wěn)定性，具有較高的適應(yīng)性和通用性。文獻［7-8］研究了移動機器人模糊PID控制方法，推導(dǎo)出機器人運動方程式，設(shè)計了模糊PID控制流程，通過仿真驗證機器人避障過程中產(chǎn)生的誤差，提高控制系統(tǒng)的響應(yīng)速度。但是，以前研究的機器人在動靜態(tài)安全場環(huán)境中移動時，其運動路徑就會變長，跟蹤誤差也隨之增大。對此，本文定義了機器人運動的動態(tài)安全場，給出了安全場二維等值面簡圖。引用滑?？刂破?，采用卡爾曼濾波器對離散滑模控制器進行改進，通過Matlab軟件對改進后的機器人避障效果進行仿真驗證，為更好發(fā)揮機器人避障能力提供參考數(shù)據(jù)。

1 動靜態(tài)安全場

1.1 基本定義

將在R3和局部參考系l中運動的一般剛體視為“危險源”，使得其一個點T的位置由r（tx t y t zt）T給出，而其速度在引入的坐標(biāo)系中為零。用局部坐標(biāo)系中r=[x y z]T和v=[v x v y v z]T分別表示P空間中一般運動點的位置、速度，如圖1所示。

圖1 危險源坐標(biāo)位置Fig.1 Coordinate position of hazard source

運動的旋轉(zhuǎn)角度［9］定義為

為了能夠確定角度φ，其計算方程式為

采用下列函數(shù)表示由運動點質(zhì)量在空間運動點上運動所引起基本運動靜態(tài)安全場為

式中：γ為正常數(shù)。

γ需要滿足下列關(guān)系式：

定義ρt=‖r t-r‖，v=‖v‖，則動靜態(tài)安全場需要滿足下列條件：

安全場明顯受到距離的影響，如式（5）中所強調(diào)的，但它也捕捉到速度向量v的模塊，表示危險源和計算場的點之間的相對速度以及偏角φ。根據(jù)式（6）可知，如果r點的運動指向r t點，則安全場隨著速度的增大而減小。最后，式（7）意味著對于固定速度量級，安全場隨著偏角φ的減小而減小。

1.2 動靜態(tài)安全場

二維空間中運動質(zhì)量T引起的安全場等值面如圖2所示。安全場隨著點質(zhì)量和評估該場的一般點之間距離增加而增大，并且沿T的移動方向取較低的值。

圖2 安全場二維等值面Fig.2 Two-dimensional iso-surface of safety field

計算三角形網(wǎng)格離散的剛體安全場的第一步是將動態(tài)安全場的定義推廣到R3內(nèi)的三角形內(nèi)。在局部固定的參考坐標(biāo)系中，一旦用局部坐標(biāo)表示，危險源的線速度和角速度均為零，用r S表示屬于三角曲面S點的位置，其中r=（x y z）T和v=（vx vy vz）T分別為空間P中一般運動點的位置、速度。分別用r w、vw表示一般空間運動點的絕對位置和速度，如圖3所示。知道局部坐標(biāo)系的位置、線速度和角速度，在全局坐標(biāo)系中分別用r O′、vO′和ω表示，在局部坐標(biāo)系中就可以通過以下變換計算r和v：

圖3 動靜態(tài)安全場運動簡圖Fig.3 Motion diagram of dynamic and static safety field

式中：Awl、Rwl分別為齊次變換矩陣和旋轉(zhuǎn)矩陣；S（ω）為角速度張量的斜對稱矩陣。

根據(jù)式（1），累積運動安全場定義為以下表面積分：

式中：A為三角形表面的面積。

曲面積分是可解的，從而可以推導(dǎo)出

安全區(qū)域為

2 機器人運動控制

2.1 機器人設(shè)計規(guī)則

除了簡單和靈活的危險評估之外，累積安全域可以用于多自由度冗余度機器人的控制，以確保機器人在執(zhí)行規(guī)定的任務(wù)同時提高安全等級。設(shè)計原則是在機器人手臂的幾個感興趣點上，評估由剛體危險源產(chǎn)生的安全場。利用這一信息確定所考慮點的位移?；诎踩虻奈灰茟?yīng)確保機器人手臂避開危險源，以防止機器人與危險源本身可能發(fā)生碰撞。

假設(shè)研究一個具有n個關(guān)節(jié)和廣義坐標(biāo)q∈Rn的機器人手臂。用m表示任務(wù)空間的維數(shù)，我們假設(shè)機器人在運動上相對于任務(wù)m

由于CSF（r，v）是在局部坐標(biāo)系中定義的，為了在全局坐標(biāo)系w中表示Δp0，利用從局部坐標(biāo)系l到全局坐標(biāo)系w的旋轉(zhuǎn)矩陣Rlw。在多個危險源的情況下，捕捉到所有危險源影響的總位移由以下矢量給出：

一旦計算出回避位移，就可以通過將雅可比變換為相應(yīng)的關(guān)節(jié)角增量Δq0，類似于閉環(huán)逆運動學(xué)算法。對于位于機械臂上點S的單個期望位移Δp0，通過以下方法獲得相應(yīng)的Δq0：

式中：kp為正實數(shù)；JS，v（q）為點S相關(guān)聯(lián)的雅克比矩陣JS（q）的前三行。

假設(shè)采樣時間為Δt，根據(jù)式（15）可以推導(dǎo)出機器人關(guān)節(jié)產(chǎn)生的角速度為

將信號q′0輸入到位置、速度命令的閉環(huán)逆運動學(xué)算法中，其單臂控制角速度的解析表達式為

式中：q′T為任務(wù)相關(guān)的角速度；Ke為正定對稱矩陣；xd、x分別為期望和實際位移；J（q）為相應(yīng)的雅可比矩陣；J?（q）為雅可比矩陣右偽逆。

2.2 離散滑?？刂破髟O(shè)計

將機器人末端執(zhí)行器運動傳遞函數(shù)進行離散化，得到機器人末端執(zhí)行器位姿運動狀態(tài)方程［11］為

式中：A為狀態(tài)矩陣；B為輸入矩陣；u（k）為系統(tǒng)的控制律；x（k+1）和x（k）為系統(tǒng)不同時刻的狀態(tài)。

定義切換函數(shù)為

式中：Ce為控制參數(shù)。

定義指數(shù)趨近律函數(shù)為

式中：T為采樣時間；q、ε為正實數(shù)；Δ為函數(shù)切換帶寬度。

根據(jù)式（21）~式（23）可以推導(dǎo)出系統(tǒng)的控制律為

為了提高控制系統(tǒng)的穩(wěn)定性，采樣卡爾曼濾波器對控制系統(tǒng)噪聲進行過濾，假設(shè)控制系統(tǒng)離散狀態(tài)方程式［12-13］為

式中：X為狀態(tài)矩陣；Φ為離散系統(tǒng)矩陣；H為測量矩陣；Y控制系統(tǒng)輸出；W（k）為干擾噪聲；Z（k）為測量噪聲；Γ為噪聲過濾矩陣。

卡爾曼濾波增益的計算過程如下：

（1）系統(tǒng)狀態(tài)預(yù)測過程，其方程式為

式中：X0為系統(tǒng)狀態(tài)估計。

（2）系統(tǒng)狀態(tài)更新過程，其方程式為

式中：Ka為卡爾曼濾波增益系數(shù)。

（3）協(xié)方差預(yù)測和更新過程，其方程式為

式中：P為協(xié)方差矩陣；R為方差；In為單位對角矩陣。

機器人采用卡爾曼濾波器的離散滑模控制流程如圖4所示。

圖4 機器人末端執(zhí)行器位姿控制流程Fig.4 Pose control flow of robot end effector

3 避障仿真及分析

為了驗證機器人末端執(zhí)行器避障效果，以機器人右臂為研究對象，如圖5所示。采用Matlab軟件對機器人右臂執(zhí)行器跟蹤進行仿真。假設(shè)機器人右臂期望運動位姿方程式為x2+y2=0.04。仿真參數(shù)為：右臂關(guān)節(jié)長度l1=l2=l3=0.6 m，末端執(zhí)行器初始位置（x，y）=（－0.3，0.3），關(guān)節(jié)質(zhì)量為m1=m2=m3=0.2 kg，障礙物半徑為Rp=0.025 m，控制參數(shù)Ce=5，q=10。在靜態(tài)安全場（無障礙物干擾）環(huán)境中，機器人采用滑?？刂啤⒏倪M滑?？刂频能壽E跟蹤效果分別如圖6和圖7所示。在動靜態(tài)安全場（有障礙物干擾）環(huán)境中，機器人采用滑?？刂啤⒏倪M滑?？刂频能壽E跟蹤效果分別如圖8和圖9所示。

圖5 雙臂機器人Fig.5 Two-armed robot

對比圖6和圖7可知：機器人在靜態(tài)安全場環(huán)境中，采用滑?？刂品椒?，機器人在軌跡追蹤過程中，搜索路徑較長；采用改進滑?？刂品椒ǎ瑱C器人在軌跡追蹤過程中，搜索路徑較短。對比圖8和圖9可知：機器人在動靜態(tài)安全場環(huán)境中，采用滑?？刂品椒?，機器人在軌跡追蹤過程中，遇到移動障礙物時，反應(yīng)遲鈍，在避開障礙物過程中，運動路徑較長；采用改進滑?？刂品椒ǎ瑱C器人在軌跡追蹤過程中，遇到移動障礙物時，能夠提前做出反應(yīng)，在避開障礙物過程中，運動路徑較短。

圖6 滑?？刂聘櫍o態(tài)安全場）Fig.6 Sliding mode control tracking（static safety field）

圖7 改進滑?？刂聘櫍o態(tài)安全場）Fig.7 Improved sliding mode control tracking（static safety field）

圖8 滑模控制跟蹤（動靜態(tài)安全場）Fig.8 Sliding mode control tracking（dynamic and static safety field）

圖9 改進滑?？刂聘櫍▌屿o態(tài)安全場）Fig.9 Improved sliding mode control tracking（dynamic and static safety field）

因此，雙臂機器人在動靜態(tài)環(huán)境中跟蹤軌跡時，傳統(tǒng)滑模控制系統(tǒng)很難快速地做出反應(yīng)，導(dǎo)致機器人在避障過程中，運動路徑較長，跟蹤誤差較大。但是，采用卡爾曼濾波器對滑?？刂破鬟M行改進后，能夠較好地抑制外界噪聲的干擾，雙臂機器人在避障過程中，縮短了運動路徑，提高了軌跡跟蹤精度。

4 結(jié)論

本文研究雙臂機器人避障路徑和軌跡跟蹤問題，設(shè)計了機器人的改進滑?？刂破?，在不同環(huán)境中對機器人運動路徑跟蹤誤差進行仿真，主要結(jié)論如下：①機器人在靜態(tài)安全場環(huán)境中運動時，采用滑?？刂破?，其運動路徑較長，搜索能力較弱，而采用改進滑?？刂破鳎溥\動路徑較短，搜索能力較強。②機器人在動靜態(tài)安全場環(huán)境中運動時，采用滑?？刂破?，能夠避開障礙物，但是反應(yīng)速度較慢，導(dǎo)致運動路徑較長，跟蹤誤差較大，而采用改進滑?？刂破鳎磻?yīng)速度較快，其運動路徑較短，跟蹤誤差較小。③本文研究的雙臂機器人避障運動路徑和跟蹤誤差，在未來市場中具有重要的應(yīng)用價值，但是機器人末端執(zhí)行器運動速度和加速度變化情況，并沒有展開研究，未來科研人員可以作為進一步研究課題。