應(yīng)用比例導(dǎo)引法的探測機器人機械臂路徑規(guī)劃

劉 瑩，邵 彧

（鄭州西亞斯學(xué)院電子信息工程學(xué)院，河南鄭州 451150）

1 引言

近幾年，中國制造向著更智能、更精密的方向快速發(fā)展，機器人行業(yè)逐漸成為重點研究領(lǐng)域之一。機械臂作為探測機器人行動的主要載體，依靠機器人可以實現(xiàn)自主感知、規(guī)劃路徑以及完成各類作業(yè)等操作。在地質(zhì)勘探、煤礦探測中都起到了非常重要的作用，可執(zhí)行人工無法完成的作業(yè)。因此，對探測機器人機械臂的路徑規(guī)劃研究十分有必要。

當(dāng)機械臂在作業(yè)時，一個合理的預(yù)作業(yè)位姿對于機械臂的探測作業(yè)非常重要。通常情況下，工作人員會在機器人到達現(xiàn)場后再進行位姿的調(diào)整，以實現(xiàn)最短路徑的探測。但當(dāng)作業(yè)空間相對較小時，調(diào)整機器人的位姿十分困難。如果改變這種策略，直接在機器人到達現(xiàn)場時調(diào)整好位姿，那么可直接進行作業(yè)，在一定程度上提高了工作效率。將這種思想應(yīng)用到實際的作業(yè)空間內(nèi)，演變成如何實現(xiàn)機械臂從一個位姿到另一個位姿的最短路徑研究，不少行業(yè)專家進行了深入研究。

文獻[1]提出了綠籬修建機器人機械臂與目標(biāo)之間的路徑規(guī)劃方法。首先，確定作業(yè)環(huán)境內(nèi)障礙物位置信息，輸入到機器人內(nèi)部，建立簡易執(zhí)行模型；然后計算機械臂在到達目標(biāo)時，所遇到的障礙物的碰撞條件，以此獲得機械臂的避碰空間，即可活動范圍；最后，構(gòu)建優(yōu)化模型，通過引入斥力場調(diào)節(jié)策略剔除計算過程中影響因素，再引入擾動人工勢場法實現(xiàn)機械臂的作業(yè)路徑規(guī)劃。運用該方法可使機械臂靈活避礙，找出最優(yōu)路徑，但計算過程較為復(fù)雜，計算量較大；文獻[2]對于分揀機器人提出了多目標(biāo)多機械臂協(xié)同作業(yè)路徑規(guī)劃方法。首先，對矸石目標(biāo)建立位置信息坐標(biāo)系，確定其具體位置信息；然后，在算法中引入機器人的反饋機制，目的是使多機械臂處于協(xié)同作業(yè)下；最后，將影響多機械臂作業(yè)效率的因素綜合考慮在內(nèi)，并對矸石模型進行部分改進，使得機械臂在作業(yè)時避礙能力更強、行駛路徑更短。該方法有效提高了多機械臂協(xié)同工作的效率，但是并未考慮到所抓取的矸石是否滿足分揀需求。

基于此，這里在比例導(dǎo)引法的基礎(chǔ)上，提出了探測機器人機械臂最優(yōu)路徑規(guī)劃方法。通過對機器人的作業(yè)環(huán)境進行自主探測，在確定邊界點信息后，對機械臂與探測目標(biāo)之間的位置關(guān)系展開研究。由于探測目標(biāo)處于不斷運動中，與機械臂的位置存在前后、平行的關(guān)系，確定二者之間的相對運動方程，同時引入比例導(dǎo)引法，跟蹤目標(biāo)運動軌跡，在控制機械臂動態(tài)跟蹤時間最短的前提下實現(xiàn)探測，此時運動路徑為最短路徑。實驗驗證了這里方法具有較強的通用性，可在時間、路徑成本最少的前提下實現(xiàn)對目標(biāo)的精準(zhǔn)探測。

2 機器人機械臂自主環(huán)境探測

2.1 前沿點探索

在機器人機械臂規(guī)劃路徑之前，需要對周圍環(huán)境進行自主探測，這里通過RRT（Rapidly?Exploring Random Trees，快速擴展隨機樹）算法實現(xiàn)。RRT算法的主要作用是在機器人作業(yè)現(xiàn)場周圍不斷檢測、更新邊界點[3]，實現(xiàn)過程，如圖1所示。

圖1 RRT算法探索前沿點實現(xiàn)過程Fig.1 Implementation Process of RRT Algorithm Exploration Frontier

首先，利用RRT算法遍歷樹的所有節(jié)點信息，找到與Xrand距離最近的點Xnearest，并以點Xnearest 為起始點，順著點Xrand 的方向，以增長速率為η的恒定速度增長，直至樹節(jié)點的末端接觸到未知環(huán)境停止，將末端節(jié)點Xfrontier 作為作業(yè)環(huán)境的邊界點。

2.2 前沿點過濾

根據(jù)上述探索到的前沿點進行過濾。首先，對探索到的所有前沿點進行聚類處理，提取聚類中心，并分類存儲。然后，將完成分類的點傳送至分配模塊，將重復(fù)和無用的點刪除掉，由此完成對前沿點的過濾處理。

2.3 前沿點分配

接收到來自過濾模塊的前沿點后，分配模塊對所有前沿點進行評估[4?5]，確定哪些點可以作為機械臂下一步的運動目標(biāo)點F。對篩選目標(biāo)點的過程建立一個數(shù)學(xué)模型，計算每個點的收益E，計算過程，如式（1）、式（2）所示：

式中：Xfp—接收來自過濾模塊的前沿點；Xr—機器人所處位置；i—點Xfp的信息增益，也可以用來表示在點Xfp處初期探測到的未知環(huán)境范圍。i值的計算方法為：以點Xfp為圓心，在其半徑范圍內(nèi)獲得的單元個數(shù)即為i的值。N—路徑規(guī)劃成本，也就是機械臂到目標(biāo)點所消耗的距離成本；λ—為一個常數(shù)項，用來表示信息收益在整個算法中所占的權(quán)重[6]大??；h—為一個函數(shù)項。

根據(jù)前沿點與機械臂之間的距離大小，設(shè)定增益系數(shù)值，將前沿點與機械臂的距離控制在hrad范圍內(nèi)，增益hgain>1，其他情況hgain=1。這樣可以避免機器人探索重復(fù)的前沿點，降低運動成本。E—點Xfp的收益，算法的目的是找出具有最大E值的點Xfp，該點即為機械臂運動最佳目標(biāo)點。

3 比例導(dǎo)引法下探測機器人機械臂路徑規(guī)劃

當(dāng)機械臂對探測目標(biāo)作業(yè)時，機械臂的運動路徑與探測效率呈正相關(guān)。通過應(yīng)用比例導(dǎo)引法，實時勘探探測機器人的作業(yè)環(huán)境以及機械臂的運動路徑，實現(xiàn)最小成本的探測作業(yè)。

假設(shè)探測目標(biāo)在水平向右方向上勻速直線運動，那么機械臂與目標(biāo)的相對運動，如圖2所示。

圖2 機械臂與探測目標(biāo)的相對運動Fig.2 Relative Motion between Manipulator and Detection Target

圖中：R—機械臂末端；T—探測目標(biāo)。

為了進一步驗證機械臂與動態(tài)目標(biāo)之間的距離變化，這里分別對垂直方向和視線方向上的速度向量進行分解計算[7]，由此獲得機械臂與目標(biāo)之間的距離變化值和視線角變化值為：

式中：dr/dt—探測機器人機械臂與目標(biāo)之間距離的變化值；dθ/dt—目標(biāo)視線角的變化值；Vr、Vt—機械臂末端和探測目標(biāo)運動狀態(tài)下的速度值，單位為m/s；εr、εt—機械臂末端和探測目標(biāo)在運動狀態(tài)下的前置角，單位為（°）；r—機械臂末端與目標(biāo)運動狀態(tài)下距離的變化值，單位為m；φr—機械臂末端作業(yè)狀態(tài)下方向角的值，單位為（°）；θ—探測目標(biāo)運動狀態(tài)下視線角的值，單位為（°）。

在確定機械臂與目標(biāo)之間的距離變化值和視線角變化值后，引入比例導(dǎo)引方程，計算探測目標(biāo)的動態(tài)軌跡[8]，得到：

然后，將式（3）～式（7）合并，即可得到機械臂與探測目標(biāo)之間的相對運動方程組。

在實際作業(yè)環(huán)境中，運用比例導(dǎo)引法對探測機器人機械臂進行路徑規(guī)劃時，為了達到理想的作業(yè)效率[9]，會盡可能控制機械臂的動態(tài)跟蹤時間，使得機械臂通過最短路徑到達目標(biāo)處。此時，視線角的變化，也就是視線角速度為0，則：

由式（10）可得，在Vt已知的前提下，可計算得到εt的值，在Vr已知時，可求得εr的值。

實際上，機械臂的運動是通過機器人電機驅(qū)動，電機在啟動過程中存在一個加速度的過程，這個過程可有效控制因啟動速度過大而導(dǎo)致的堵轉(zhuǎn)。所以機械臂在加速運動過程中，速度隨時變化，也就是εr的值時隨時變化，而在機械臂勻速運動時，εr的值為一個固定值[10]。

機器人機械臂探測目標(biāo)的最優(yōu)路徑規(guī)劃過程為：

（1）在完成上一輪的探測任務(wù)后，機器人內(nèi)部控制器會根據(jù)機械臂關(guān)節(jié)角確定機械臂當(dāng)前所處的位置，通過目標(biāo)運動方程確定探測目標(biāo)的位置信息，計算得到視線角θ；

（2）通過求解機械臂加速度方程，得到εr的變化方程式；

（3）通過εr，計算得到φr；

（4）計算任意時刻下機械臂關(guān)節(jié)角的值和加速度；

（5）通過操控機器人，使機械臂按期望路徑對目標(biāo)完成探測任務(wù)。

4 實驗分析

4.1 實驗數(shù)據(jù)

為了驗證這里方法在實際的探測作業(yè)中是否合理有效，進行了實驗分析。將實驗用的機械臂至于某車間的操作臺上，通過程序控制機械臂的運動，發(fā)送機械臂關(guān)節(jié)指令到關(guān)節(jié)控制器，機械臂運行的軌跡數(shù)據(jù)通過中央控制器與總線通信協(xié)議，傳輸機械臂運行的軌跡數(shù)據(jù)。其中，傳輸?shù)臋C械臂傳輸數(shù)據(jù)包括運行速度、運行時間、運行軌跡數(shù)據(jù)以及運行成本數(shù)據(jù)。在實驗中，探測機器人的機械臂從初期的勻加速轉(zhuǎn)換為后期勻速，初期階段，速度從0迅速提升到2m/s，加速度為4m/s，時間為0.5s。這個階段中，機械臂的方向角時刻發(fā)生變化，到后期，速度方向角為固定值，機械臂只會按照直線方向運動，直到探測到目標(biāo)為止。

將目標(biāo)的運動速度設(shè)為0.7m/s，實驗用機械臂，如圖3 所示。

圖3 實驗用機械臂Fig.3 Mechanical Arm for the Experiment

4.2 探測路徑規(guī)劃分析

在實際作業(yè)中，機械臂與探測目標(biāo)之間存在三種狀態(tài)關(guān)系，實驗對此均進行了研究。

（1）追蹤式探測路徑規(guī)劃

此種狀態(tài)下，機械臂處于探測目標(biāo)后方的位置。定義機械臂的初始位置坐標(biāo)為（0，0.5，0），探測目標(biāo)的初始位置坐標(biāo)為（0.5，1.5，0.4），探測到目標(biāo)所花費的時間為1.070s，實驗中的路徑規(guī)劃，如圖4所示。

圖4 追蹤式探測軌跡Fig.4 Tracking?Type Detection Trajectory

（2）平行式探測路徑規(guī)劃

當(dāng)機械臂與探測目標(biāo)處于水平方向上時，稱為平行式探測。定義機械臂的初始位置坐標(biāo)為（0，0.5，0），探測目標(biāo)的初始位置坐標(biāo)為（0，1.5，0.4），探測到目標(biāo)所花費的時間為0.873s。實驗路徑規(guī)劃，如圖5所示。

圖5 平行式探測軌跡Fig.5 Parallel Detection Trajectory

（3）攔截式探測路徑規(guī)劃

攔截式探測指機械臂處于探測目標(biāo)前方的位置。定義機械臂的初始位置坐標(biāo)為（0.5，0.5，0），探測目標(biāo)的初始位置坐標(biāo)為（0，1.5，0.4），探測到目標(biāo)所花費的時間為0.791s。實驗路徑規(guī)劃，如圖6所示。

從圖4～圖6中可以看出，三種狀態(tài)下的機械臂運動路徑，在初期勻加速階段，存在一小部分的曲線，曲率較大，但是彎曲程度較低，整體路徑較為光滑。

圖6 攔截式探測實驗軌跡Fig.6 Blocking Detection Experiment Trajectory

在后期加速階段，機械臂的運動軌跡逐漸成為一條直線，運動時間也相對較短，消耗的能量也就越小，機械臂的穩(wěn)定性也就越高。

不僅如此，從這三幅圖中還可以看出，通過引入比例導(dǎo)引法對機械臂進行路徑規(guī)劃，不管機械臂與探測目標(biāo)的位置關(guān)系如何變化，機械臂都能以最短路徑探測到目標(biāo)，由此也可以證明，這里方法的通用性較強。

4.3 空間大小對規(guī)劃成本影響分析

考慮到探測機器人的作業(yè)環(huán)境為小范圍環(huán)境時，可能對作業(yè)效率、運動路徑產(chǎn)生一定的影響，因此，將這里方法與擾動人工勢場法和多機械臂協(xié)同策略兩種方法在路徑成本和時間成本上進行實驗對比。

實驗中選擇了一個相對較大的空間和一個狹小的空間，大空間為182m2，小空間為49m2。

經(jīng)過統(tǒng)計后得到路徑距離和時間成本的直方圖，如圖7、圖8所示。

從圖7、圖8中可以得出，與擾動人工勢場法和多機械臂協(xié)同策略兩種方法對比，這里方法路徑距離和時間成本最小，這是由于這里方法分別對機械臂與探測目標(biāo)之間的垂直方向與水平方向進行了相對運動方程計算，精準(zhǔn)定位了探測目標(biāo)所在位置，所以運用這里方法，即使在小范圍環(huán)境中同樣可以取得理想效果。

圖7 三種方法下路徑規(guī)劃距離對比圖Fig.7 Comparison of Path Planning Distance Under Three Methods

圖8 三種方法下路徑規(guī)劃時間對比圖Fig.8 Comparison of Path Planning Time Under Three Methods

5 結(jié)論

這里針對探測機器人提出了一種機械臂路徑規(guī)劃方法，使機械臂實現(xiàn)了合理、高效的作業(yè)。

實驗也驗證了這里方法通用性較強。這里方法具有以下優(yōu)勢：

（1）通過分析探測目標(biāo)與機械臂的位置關(guān)系，給出了機械臂實現(xiàn)探測的路徑規(guī)劃步驟。實驗中設(shè)定了三種位置關(guān)系，驗證了這里方法可以平滑、快速地跟蹤到探測目標(biāo)。

（2）考慮到探測機器人的作業(yè)空間大小，設(shè)定大范圍環(huán)境和小范圍環(huán)境的實驗，結(jié)果表明，這里方法依然可以很好地探測到目標(biāo)，基本不受空間大小的影響。