基于自運(yùn)動的冗余機(jī)械臂實(shí)時避障軌跡規(guī)劃

高其遠(yuǎn)，陳 麗

（上海工程技術(shù)大學(xué) 航空運(yùn)輸學(xué)院，上海 201620）

0 引言

冗余機(jī)械臂的關(guān)節(jié)自由度高，工作范圍大，能夠應(yīng)對多種工作環(huán)境，完成復(fù)雜任務(wù)。在實(shí)際工作中，需要根據(jù)機(jī)械臂的工作任務(wù)，規(guī)劃運(yùn)動軌跡，并滿足避障需求。

現(xiàn)有研究中，針對機(jī)械臂的軌跡規(guī)劃算法主要有人工勢場法、隨機(jī)搜索法。其中，人工勢場法由Khatib提出，通過在目標(biāo)與機(jī)械臂之間引入吸引力，在障礙物與機(jī)械臂之間引入排斥力，從而實(shí)現(xiàn)機(jī)械臂的避障軌跡規(guī)劃。人工勢場法在計算時容易陷入局部極小值，難以適應(yīng)復(fù)雜環(huán)境。Laliberte 等人針對人工勢場法容易發(fā)生局部極小的不足，提出了離散勢場法，從而降低了發(fā)生局部極小的可能性。隨機(jī)搜索法通過在機(jī)械臂關(guān)節(jié)空間內(nèi)任意搜索機(jī)械臂運(yùn)動路徑點(diǎn)，適合復(fù)雜環(huán)境下的路徑搜索。Lozano-Perze提出的一種基于空間法的自由空間法，先根據(jù)機(jī)械臂的各個關(guān)節(jié)約束范圍建立機(jī)械臂所有可行空間，再將障礙物映射到空間內(nèi)，進(jìn)而求解空間內(nèi)除去障礙物的補(bǔ)集空間，即機(jī)械臂不會發(fā)生碰撞的自由空間。賈慶軒等人引用A算法在冗余機(jī)械臂的空間內(nèi)進(jìn)行搜索，獲得無碰撞路徑。隨機(jī)搜索法在機(jī)械臂自由度高時，機(jī)械臂自由運(yùn)動的空間構(gòu)建困難，難以滿足實(shí)時性需求?；陔S機(jī)采樣的路徑搜索算法在機(jī)械臂的關(guān)節(jié)空間進(jìn)行搜索，不必求解高維空間，代表算法有快速擴(kuò)展隨機(jī)樹法（Rapidly-exploring Random Trees，RRT）、隨機(jī)路圖法（Probabilistic roadmaps，PRM），由于是隨機(jī)采樣生成路徑點(diǎn)，在復(fù)雜環(huán)境下可能無法搜索出無碰撞路徑，且難以控制過程中末端執(zhí)行器位姿，也難于滿足需要約束機(jī)械臂抓取姿態(tài)的任務(wù)要求。

冗余機(jī)械臂具有自運(yùn)動的特性，相同的末端姿態(tài)可對應(yīng)多組關(guān)節(jié)角度，故可利用該特性避障。姜力等人通過引入臂平面和避障面表達(dá)機(jī)械臂的零空間自運(yùn)動。齊志剛等人根據(jù)臂型角參數(shù)化完善逆運(yùn)動學(xué)，得到32 組完備逆解集。Sakata 等人基于梯度投影法提出了一種空間冗余機(jī)械臂的避障路徑規(guī)劃算法，該方法主要利用冗余機(jī)械臂自運(yùn)動的特點(diǎn)實(shí)現(xiàn)其實(shí)時的避障路徑規(guī)劃。

本文針對冗余機(jī)械臂的特點(diǎn)，在對已知任務(wù)空間運(yùn)動求關(guān)節(jié)空間逆解的基礎(chǔ)上，引入機(jī)械臂與障礙物之間的虛擬排斥速度場，從虛擬排斥速度作用點(diǎn)處將機(jī)械臂拆成2 段并聯(lián)的機(jī)械臂分別求解關(guān)節(jié)空間避障角速度分量，并向零空間投影得到自運(yùn)動角速度分量，最后加上完成末端主運(yùn)動的最小范數(shù)求得完整避障速度解，最終獲得機(jī)械臂避障運(yùn)動軌跡。

1 冗余機(jī)械臂

本文的研究主體為Kinova Gen3 型七自由度冗余機(jī)械臂，機(jī)械臂的結(jié)構(gòu)如圖1 所示。

圖1 7 自由度冗余機(jī)械臂Fig.1 7-DOF redundant manipulator

建立機(jī)械臂的標(biāo)準(zhǔn)D-H 參數(shù)模型，見表1。

表1 機(jī)械臂D-H 模型參數(shù)Tab.1 D-H model parameters of the manipulator

其中，T為第-1 關(guān)節(jié)到第關(guān)節(jié)坐標(biāo)軸變換矩陣，（q）為第關(guān)節(jié)坐標(biāo)系在關(guān)節(jié)角q下的旋轉(zhuǎn)矩陣。

機(jī)械臂的第1、3、5、7 關(guān)節(jié)軸向與連桿延伸方向平行，第2、4、6 關(guān)節(jié)軸向與連桿延伸方向垂直，第1、3、5、7 關(guān)節(jié)所連接的2 段連桿不會在空間結(jié)構(gòu)上發(fā)生變化，故在碰撞檢測時可將機(jī)械臂視作4 段串聯(lián)機(jī)械臂分別計算，繞桿軸向轉(zhuǎn)動作用可等效至桿件兩端。

2 關(guān)節(jié)角速度的最小范數(shù)解

其中，為由的個線性無關(guān)的列重新排列組成的非奇異矩陣；為中除去組成列向量以外的部分。

3 冗余機(jī)械臂零空間自運(yùn)動避障

利用冗余機(jī)械臂可以在關(guān)節(jié)空間齊次解所在的零空間自運(yùn)動而不影響末端執(zhí)行器位姿的特性，可實(shí)現(xiàn)一定程度的避障功能。本文提出一種通過在機(jī)械臂與障礙物之間構(gòu)建虛擬排斥速度場獲得排斥速度作用點(diǎn)以及該點(diǎn)的避障排斥速度，進(jìn)一步地，將機(jī)械臂視為末端在排斥速度作用點(diǎn)的并聯(lián)機(jī)械臂，由逆運(yùn)動學(xué)求解各個關(guān)節(jié)的避障速度，最后向零空間投影，得到一組利用自運(yùn)動實(shí)現(xiàn)避障的關(guān)節(jié)速度解。

3.1 碰撞檢測

障礙物可能具有不同的形狀，可近似地看作規(guī)則幾何形狀的組合，使用超二次函數(shù)描述障礙物的表面，在笛卡爾坐標(biāo)系下，曲面方程可表示為：

滿足上式的點(diǎn)（，，）組成的集合即為該曲面。對于幾何中心為O（，，）、幾何參數(shù)為，，的典型障礙物，其曲面可采用如下解析表達(dá)式進(jìn)行描述：

其中，通過改變冪指數(shù)，，的值，得到不同形狀障礙物；通過改變，，的取值，改變障礙物中心的位置；通過改變，，的取值，可以表述具有不同輪廓大小的障礙物。

其中，是一個常系數(shù)（0 ≤≤1）。機(jī)械臂上最近點(diǎn)X由參數(shù)λ表示的坐標(biāo)可以通過最小偽距離求出。表面方程在基系下可以表示為：

其中，矩陣、向量和標(biāo)量是常量。任意點(diǎn)在笛卡爾空間中的偽距離為：

代入機(jī)械臂上任意一點(diǎn)表達(dá)式：

可以等價地寫成：

當(dāng)上式對的導(dǎo)數(shù)為0 時有最小偽距離：

若λ ＜0，則令λ取值為0，此時X就是點(diǎn)；若λ ＞1，則令λ取值為1，此時X就是點(diǎn)。

將得到的桿與障礙物最近點(diǎn)X代入到偽距離函數(shù)（）中確定唯一的防碰撞條件約束：

其中，d為安全距離。

3.2 虛擬排斥速度場構(gòu)建

在基系笛卡爾空間內(nèi)構(gòu)建冗余機(jī)械臂與障礙物之間的排斥速度場，如圖2 所示。

圖2 虛擬排斥速度場示意圖Fig.2 Schematic diagram of virtual exclusion velocity field

當(dāng)機(jī)械臂與障礙物之間最近距離趨于0時，排斥速度趨于無窮大。

3.3 關(guān)節(jié)空間避障速度分量求解

將機(jī)械臂拆分為以排斥速度作用點(diǎn)為末端執(zhí)行器的2 段并聯(lián)機(jī)械臂，分別命名為機(jī)械臂，機(jī)械臂，如圖3 所示。

圖3 拆分后的2 段并聯(lián)機(jī)械臂示意圖Fig.3 Schematic diagram of two parallel manipulator after splitting

由Kinova Gen3 型機(jī)械臂的結(jié)構(gòu)將機(jī)械臂整體視為4 段連桿，每個關(guān)節(jié)具有2 個自由度，排斥速度作用點(diǎn)處坐標(biāo)系在機(jī)械臂、下的坐標(biāo)變換分別為T，T。當(dāng)排斥速度點(diǎn)作用于原機(jī)械臂基座至第一關(guān)節(jié)和最后一個關(guān)節(jié)至末端執(zhí)行器之間的連桿時，機(jī)械臂不存在自運(yùn)動，作用于當(dāng)中的任意一點(diǎn)時T，T有統(tǒng)一表達(dá)式：

由于已知當(dāng)前各個關(guān)節(jié)角度，故可直接解出T，T，進(jìn)一步地可得到2 段機(jī)械臂在當(dāng)前時刻的雅可比矩陣J、J。又因?yàn)樵瓩C(jī)械臂自由度為7，分成的2 段機(jī)械臂不是冗余機(jī)械臂，故可直接解出各關(guān)節(jié)的排斥速度分量：

其中，為任意常數(shù)，對全時刻關(guān)節(jié)角速度進(jìn)行積分得到各關(guān)節(jié)角度值，重復(fù)上文循環(huán)計算至末端到達(dá)終點(diǎn)，最終獲得一條能實(shí)現(xiàn)末端軌跡跟蹤，并同時具備避障能力的冗余機(jī)械臂關(guān)節(jié)空間軌跡。

4 仿真試驗(yàn)

4.1 仿真環(huán)境

為了驗(yàn)證算法效果，本文基于Kinova 公司Gen3型七自由度冗余機(jī)械臂，在Matlab R2020b 環(huán)境下進(jìn)行仿真試驗(yàn)。

機(jī)械臂初始位姿設(shè)置各個關(guān)節(jié)角度為［90°，15°，180°，230°，10°，55°，90°］，運(yùn)動目標(biāo)為使末端執(zhí)行器在笛卡爾空間相對位移［0.4 m，0.4 m，-0.4 m］，并繞軸與軸旋轉(zhuǎn)60°在基系笛卡爾空間內(nèi)設(shè)置2 個障礙物，幾何中心坐標(biāo)為［0.2 m，0 m，0.5 m］，［-0.1 m，-0.4 m，0.8 m］，障礙物包絡(luò)球半徑為0.05 m，機(jī)械臂連桿半徑0.03 m。

4.2 仿真試驗(yàn)結(jié)果與分析

試驗(yàn)首先根據(jù)末端執(zhí)行器目標(biāo)位姿，利用五次多項(xiàng)式插補(bǔ)出一條末端軌跡，再進(jìn)行逆運(yùn)動學(xué)關(guān)節(jié)角速度求解，同時計算本文提出的虛擬排斥速度場下避障角速度分量，至此積分計算關(guān)節(jié)角度解，獲得完整軌跡。冗余機(jī)械臂的運(yùn)動過程如圖4 所示。整個運(yùn)動過程平穩(wěn)，末端執(zhí)行器成功到達(dá)目標(biāo)位姿，過程中未與障礙物發(fā)生碰撞。

圖4 機(jī)械臂運(yùn)動過程Fig.4 The movement process of the manipulator

為了驗(yàn)證算法是否具部避障功能，記錄整個過程中的障礙物與機(jī)械臂最近距離、虛擬排斥速度大小、以及虛擬排斥速度引起的避障角速度分量。避障關(guān)節(jié)角速度分量變化如圖5 所示。

圖5 避障關(guān)節(jié)角速度分量Fig.5 The angular velocity component of the joint for obstacle avoidance

障礙物與機(jī)械臂最近距離以及虛擬排斥速度大小對照如圖6 所示。

圖6 障礙物與機(jī)械臂距離對照及虛擬排斥速度大小對照Fig.6 The distance between the obstacle and the manipulator was compared with the virtual rejection speed

兩者在整個運(yùn)動過程中呈鏡像變化趨勢，當(dāng)大于建立的虛擬排斥速度場范圍d時，虛擬排斥速度V大小為0，當(dāng)小于安全距離d時，認(rèn)為發(fā)生碰撞。仿真試驗(yàn)過程中，機(jī)械臂與障礙物距離始終大于安全距離，機(jī)械臂在成功運(yùn)動至目標(biāo)位姿的同時，實(shí)現(xiàn)了避障。

5 結(jié)束語

本文提出的冗余機(jī)械臂的軌跡規(guī)劃算法，不僅能有效完成末端執(zhí)行器的軌跡跟蹤任務(wù)，同時還利用了冗余機(jī)械臂自運(yùn)動的特性實(shí)現(xiàn)了避障，在障礙物周圍建立虛擬排斥速度場，將冗余機(jī)械臂從與障礙物最近的虛擬排斥速度作用點(diǎn)處分成2 段并聯(lián)機(jī)械臂，對于本文的研究對象、七自由度冗余機(jī)械臂，拆分后的機(jī)械臂都不冗余，故可快速求解在虛擬排斥速度作用下的關(guān)節(jié)角速度分量。同時結(jié)合改進(jìn)梯度投影法快速求解實(shí)現(xiàn)末端執(zhí)行器主運(yùn)動的最小范數(shù)解，達(dá)到冗余機(jī)械臂實(shí)時軌跡規(guī)劃的目的。