劉 瑩，邵 彧

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

1 引言

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

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

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

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

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

2.1 前沿點(diǎn)探索

在機(jī)器人機(jī)械臂規(guī)劃路徑之前，需要對(duì)周?chē)h(huán)境進(jìn)行自主探測(cè)，這里通過(guò)RRT（Rapidly?Exploring Random Trees，快速擴(kuò)展隨機(jī)樹(shù)）算法實(shí)現(xiàn)。RRT算法的主要作用是在機(jī)器人作業(yè)現(xiàn)場(chǎng)周?chē)粩鄼z測(cè)、更新邊界點(diǎn)[3]，實(shí)現(xiàn)過(guò)程，如圖1所示。

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

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

2.2 前沿點(diǎn)過(guò)濾

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

2.3 前沿點(diǎn)分配

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

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

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

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

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

假設(shè)探測(cè)目標(biāo)在水平向右方向上勻速直線運(yùn)動(dòng)，那么機(jī)械臂與目標(biāo)的相對(duì)運(yùn)動(dòng)，如圖2所示。

圖2 機(jī)械臂與探測(cè)目標(biāo)的相對(duì)運(yùn)動(dòng)Fig.2 Relative Motion between Manipulator and Detection Target

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

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

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

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

然后，將式（3）～式（7）合并，即可得到機(jī)械臂與探測(cè)目標(biāo)之間的相對(duì)運(yùn)動(dòng)方程組。

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

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

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

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

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

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

（3）通過(guò)εr，計(jì)算得到φr；

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

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

4 實(shí)驗(yàn)分析

4.1 實(shí)驗(yàn)數(shù)據(jù)

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

將目標(biāo)的運(yùn)動(dòng)速度設(shè)為0.7m/s，實(shí)驗(yàn)用機(jī)械臂，如圖3 所示。

圖3 實(shí)驗(yàn)用機(jī)械臂Fig.3 Mechanical Arm for the Experiment

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

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

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

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

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

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

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

圖5 平行式探測(cè)軌跡Fig.5 Parallel Detection Trajectory

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

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

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

圖6 攔截式探測(cè)實(shí)驗(yàn)軌跡Fig.6 Blocking Detection Experiment Trajectory

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

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

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

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

實(shí)驗(yàn)中選擇了一個(gè)相對(duì)較大的空間和一個(gè)狹小的空間，大空間為182m2，小空間為49m2。

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

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

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

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

5 結(jié)論

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

實(shí)驗(yàn)也驗(yàn)證了這里方法通用性較強(qiáng)。這里方法具有以下優(yōu)勢(shì)：

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

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