本發(fā)明涉及空間機(jī)器人執(zhí)行任務(wù)時(shí)的避撞技術(shù)，具體為一種基于多個(gè)控制點(diǎn)的空間機(jī)器人避撞方法。

背景技術(shù)：

衛(wèi)星維護(hù)、在軌裝配和空間碎片去除的需求日益增長(zhǎng)，要求空間機(jī)器人在特定惡劣的空間環(huán)境中執(zhí)行任務(wù)。具體事例有“機(jī)器人技術(shù)試驗(yàn)(rotex)”、“工程試驗(yàn)衛(wèi)星vii(ets-vii)”、“軌道快車(oe)”等。根據(jù)目前世界各空間機(jī)構(gòu)計(jì)劃的空間機(jī)器人，在空間任務(wù)中機(jī)器人數(shù)量和性能的增加在未來已成定局。為了有效地執(zhí)行多項(xiàng)任務(wù)，研究人員主要關(guān)注在有障礙物的工作空間中空間機(jī)器人的運(yùn)動(dòng)控制問題。由于空間環(huán)境的不確定性，對(duì)于模型未知、運(yùn)動(dòng)的障礙物必須進(jìn)行在線避撞規(guī)劃與控制。因此，需要發(fā)展避撞技術(shù)以應(yīng)對(duì)空間機(jī)器人在軌服務(wù)的運(yùn)動(dòng)控制問題。

現(xiàn)有技術(shù)的文獻(xiàn)中已經(jīng)提出了很多空間機(jī)器人在執(zhí)行任務(wù)時(shí)的避障方法和策略。最為經(jīng)典的是faverjon和tournassoud提出的基于局部的避撞規(guī)劃方法，通過減小期望速度和避撞不等式約束下的規(guī)劃速度之間的誤差，從而規(guī)劃?rùn)C(jī)器人的運(yùn)動(dòng)。此方法要求機(jī)器人和障礙物必須是嚴(yán)格凸的目標(biāo)，這在實(shí)際中很難實(shí)現(xiàn)。helguera和zeghloul對(duì)此方法做了改進(jìn)，通過修改任務(wù)描述避免了局部最小值的問題。schlemmer等人提出了一種基于距離梯度的在線避撞方法。sentis和khatib提出了一種利用勢(shì)場(chǎng)的局部避撞方法。

以上避障方法的研究主要關(guān)注將機(jī)器人和障礙物處理成嚴(yán)格凸目標(biāo)后的避撞問題。然而真實(shí)物理環(huán)境中，只有球、橢圓等可視為嚴(yán)格凸目標(biāo)。由于目標(biāo)是非嚴(yán)格凸的，因此目標(biāo)上控制點(diǎn)運(yùn)動(dòng)時(shí)會(huì)產(chǎn)生不連續(xù)的情況。如果這種情況頻繁地發(fā)生，由于控制點(diǎn)跳變會(huì)導(dǎo)致機(jī)械臂振動(dòng)。關(guān)于這種現(xiàn)象kanoun等人采用了一種多速度阻尼約束方法，可以抑制機(jī)械臂在含障礙物的工作空間中運(yùn)動(dòng)時(shí)產(chǎn)生的振動(dòng)問題。但這種方法約束過程復(fù)雜，大大增加了計(jì)算量。

技術(shù)實(shí)現(xiàn)要素：

針對(duì)現(xiàn)有技術(shù)中存在的問題，本發(fā)明提供一種基于多個(gè)控制點(diǎn)的空間機(jī)器人避撞方法，軌跡規(guī)劃時(shí)機(jī)械臂關(guān)節(jié)角速度平滑，避免其發(fā)生不連續(xù)，能夠抑制由單個(gè)控制點(diǎn)跳變引起的振動(dòng)問題。

本發(fā)明是通過以下技術(shù)方案來實(shí)現(xiàn)：

一種基于多個(gè)控制點(diǎn)的空間機(jī)器人避撞方法，包括，

步驟一，將障礙物處理成由三角面構(gòu)成的非凸多面體，將機(jī)械臂處理成線段，確定描述線段與三角形之間的碰撞檢測(cè)算法，得到機(jī)械臂和障礙物之間距離最近的一對(duì)控制點(diǎn)；

步驟二，建立空間機(jī)器人考慮避撞任務(wù)的多任務(wù)運(yùn)動(dòng)學(xué)方程；

步驟三，基于一對(duì)控制點(diǎn)建立空間機(jī)器人的避撞約束條件，結(jié)合步驟二得到的運(yùn)動(dòng)學(xué)方程進(jìn)行避撞求解；

步驟四，基于多個(gè)控制點(diǎn)建立新控制點(diǎn)的運(yùn)動(dòng)信息，根據(jù)多個(gè)控制點(diǎn)的參數(shù)設(shè)計(jì)調(diào)整新控制點(diǎn)參數(shù)，結(jié)合步驟二得到的運(yùn)動(dòng)學(xué)方程進(jìn)行避撞求解；

步驟五，根據(jù)多個(gè)控制點(diǎn)對(duì)一個(gè)障礙物的避撞求解，按空間機(jī)器人機(jī)械臂與障礙物的距離遠(yuǎn)近加入權(quán)重因子，對(duì)空間機(jī)器人的機(jī)械臂進(jìn)行多障礙物的避撞控制。

優(yōu)選的，步驟一的具體步驟如下，

步驟1.1，定義嚴(yán)格凸目標(biāo)和voronoi區(qū)域，

定義嚴(yán)格凸目標(biāo)，令o表示三維空間r³一個(gè)閉集，χ(o)是o的內(nèi)部，對(duì)于且0＜λ＜1，當(dāng)且僅當(dāng)λa+(1-λ)b∈χ(o)時(shí)，認(rèn)為o是嚴(yán)格凸的；

定義voronoi區(qū)域，vr(y)表示特征為y的voronoi區(qū)域；關(guān)于特征為y的一個(gè)voronoi區(qū)域?yàn)橄啾扔谌魏纹渌卣?，最靠近特征y的點(diǎn)集；

步驟1.2，根據(jù)嚴(yán)格凸目標(biāo)和voronoi區(qū)域的定義，障礙物不是嚴(yán)格凸目標(biāo)，將其處理成由若干三角形基礎(chǔ)面組成的多面體；根據(jù)三角形的七個(gè)特征，三角形的面、三條邊以及三個(gè)頂點(diǎn)，通過一個(gè)三角形將三維空間分為七個(gè)voronoi區(qū)域，當(dāng)一條線段位于三角形的不同voronoi區(qū)域時(shí)，利用不同的函數(shù)計(jì)算距離最近的一對(duì)控制點(diǎn)；當(dāng)一條線段位于不止一個(gè)voronoi區(qū)域中時(shí)，利用vr_clip算法搜索距離最近的一對(duì)控制點(diǎn)。

進(jìn)一步的，步驟二的具體步驟如下，

空間機(jī)械臂末端執(zhí)行器的主要任務(wù)和次要避障任務(wù)描述如下：

考慮任務(wù)優(yōu)先級(jí)滿足所有目標(biāo)的逆運(yùn)動(dòng)學(xué)解法描述如下，不考慮零空間運(yùn)動(dòng)h＝0：

其中，是je零空間中矩陣jo的投影。

再進(jìn)一步的，步驟三的具體步驟如下，

障礙物穩(wěn)定時(shí)，分別從機(jī)械臂和障礙物上選取點(diǎn)p1、p1'為一對(duì)控制點(diǎn)，||p1p1'||距離的導(dǎo)數(shù)，即避撞約束，表示如下：

其中，n1是的單位矢量，符號(hào)<u,v>表示矢量u和矢量v的內(nèi)積；

用一個(gè)新雅可比矩陣替換逆運(yùn)動(dòng)學(xué)解中的jo，表示如下：

完成主要任務(wù)和避障任務(wù)的逆運(yùn)動(dòng)學(xué)新解表示如下：

再進(jìn)一步的，步驟四中的具體步驟如下，

在機(jī)械臂上選取一個(gè)結(jié)合p1、p2兩個(gè)控制點(diǎn)信息的新控制點(diǎn)p0，用新的控制點(diǎn)p0代替兩個(gè)控制點(diǎn)p1、p2，描述機(jī)械臂運(yùn)動(dòng)；新控制點(diǎn)p0在機(jī)械臂上連續(xù)運(yùn)動(dòng)，計(jì)算如下：

其中，d1＝||p1p1'||、d2＝||p2p2'||分別是p1、p2與障礙物之間的距離；由于控制點(diǎn)p1更為鄰近，令相應(yīng)的逃逸單位矢量n0等于n1，

期望的臨界點(diǎn)速度vo是指定的標(biāo)稱速度，αc是避撞增益，其值取決于與障礙物的臨界距離：

定義一個(gè)新的臨界距離dsr，稱為安全距離；如果機(jī)械臂距離障礙物太近，即d1≤duf，則取消由調(diào)整參數(shù)αe控制的主要任務(wù)根據(jù)機(jī)械臂動(dòng)力學(xué)特性，能夠預(yù)設(shè)或解算非安全距離duf的大小；

當(dāng)實(shí)現(xiàn)平滑運(yùn)動(dòng)時(shí)，在dsr處，的大小為0；則考慮避障任務(wù)的逆運(yùn)動(dòng)學(xué)解表示如下：

調(diào)整參數(shù)定義如下：

再進(jìn)一步的，步驟五的具體步驟如下，

考慮影響區(qū)內(nèi)所有障礙物，將所有障礙物的逆運(yùn)動(dòng)學(xué)解通過齊次解的加權(quán)和表示如下：

其中，n0是影響區(qū)內(nèi)障礙物數(shù)目，權(quán)重因子ωi定義如下：

權(quán)重因子ωi表示了影響區(qū)內(nèi)不同障礙物的相對(duì)重要性；當(dāng)一個(gè)障礙物比其他障礙物更靠近機(jī)械臂時(shí)，即d1i→0，相應(yīng)的權(quán)重因子增加，對(duì)應(yīng)控制點(diǎn)處速度也增加；從而能夠根據(jù)權(quán)重因子，調(diào)整控制點(diǎn)處遠(yuǎn)離障礙物的速度，對(duì)空間機(jī)器人的機(jī)械臂進(jìn)行多障礙物的避撞控制。

與現(xiàn)有技術(shù)相比，本發(fā)明具有以下有益的技術(shù)效果：

本發(fā)明提出了一種基于多個(gè)控制點(diǎn)的空間機(jī)器人避撞方法，是在空間范圍內(nèi)的一種新的方法，通過考慮真實(shí)物理環(huán)境中障礙物的非凸特性，結(jié)合主要的運(yùn)動(dòng)任務(wù)和次要的避障任務(wù)，不僅不會(huì)對(duì)機(jī)械臂末端執(zhí)行器執(zhí)行主要任務(wù)產(chǎn)生影響，而且建立了更精準(zhǔn)的避撞約束條件，能夠自主調(diào)節(jié)控制點(diǎn)，實(shí)現(xiàn)對(duì)空間機(jī)器人的避撞。

進(jìn)一步的，基于多個(gè)控制點(diǎn)時(shí)，通過在安全距離處對(duì)次要任務(wù)的限定，使得軌跡規(guī)劃時(shí)機(jī)械臂關(guān)節(jié)角速度平滑，避免其發(fā)生不連續(xù)。

附圖說明

圖1為本發(fā)明實(shí)例中所述的基于多個(gè)控制點(diǎn)避撞方法的空間機(jī)器人系統(tǒng)示意圖。

圖2為本發(fā)明實(shí)例中所述線段與三角形空間關(guān)系示意圖。

圖3為本發(fā)明實(shí)例中所述線段與線段、線段與三角形間碰撞檢測(cè)算法。

圖4為本發(fā)明實(shí)例中不加避撞方法的仿真結(jié)果。

圖5為本發(fā)明實(shí)例中基于一對(duì)控制點(diǎn)避撞方法的仿真結(jié)果。

圖6為本發(fā)明實(shí)例中基于兩對(duì)控制點(diǎn)避撞方法的仿真結(jié)果。

具體實(shí)施方式

下面結(jié)合具體的實(shí)施例對(duì)本發(fā)明做進(jìn)一步的詳細(xì)說明，所述是對(duì)本發(fā)明的解釋而不是限定。

本發(fā)明基于多個(gè)控制點(diǎn)，提出一種適用于動(dòng)態(tài)環(huán)境的空間機(jī)器人避撞方法?？紤]到障礙物的非凸特性，借助voronoi區(qū)域的概念，碰撞檢測(cè)時(shí)建立了更精確的避撞約束。基于多個(gè)控制點(diǎn)，可以抑制由單個(gè)控制點(diǎn)跳變引起的振動(dòng)問題，保證軌跡規(guī)劃時(shí)機(jī)械臂關(guān)節(jié)角速度平滑。具體的避撞方法如下。

步驟一：將障礙物處理成由三角面構(gòu)成的非凸多面體，描述線段與三角形之間的碰撞檢測(cè)算法，如圖1、圖2和圖3所示。

首先定義嚴(yán)格凸目標(biāo)和voronoi區(qū)域：

嚴(yán)格凸目標(biāo)：令o表示r³一個(gè)閉集，χ(o)是o的內(nèi)部。對(duì)于且0＜λ＜1，當(dāng)且僅當(dāng)λa+(1-λ)b∈χ(o)時(shí)，認(rèn)為o是嚴(yán)格凸的。

voronoi區(qū)域(vr)：對(duì)于特征y的vr(y)。關(guān)于特征y的一個(gè)voronoi區(qū)域是，相比于任何其它特征而言，最靠近特征y的點(diǎn)集。

線段由一條邊ε和兩個(gè)端點(diǎn)vi(i＝1,2)組成，邊和端點(diǎn)是線段的三個(gè)特征。依據(jù)這三個(gè)特征，線段周圍的三維空間被分為三個(gè)voronoi區(qū)域。一條運(yùn)動(dòng)的線段可處于任意某個(gè)線段的voronoi區(qū)域中，或同時(shí)處于不止一個(gè)voronoi區(qū)域中。根據(jù)vr_clip算法可將此線段分割成若干線段。這是兩條線段之間的碰撞檢測(cè)算法，它是線段與三角形之間碰撞檢測(cè)算法的基礎(chǔ)。

由于障礙物不是嚴(yán)格凸目標(biāo)，可處理成由一些基礎(chǔ)面如三角形組成的多面體。根據(jù)三角形的七個(gè)特征，即三角形的面、邊以及頂點(diǎn)，一個(gè)三角形可以將三維空間分為七個(gè)voronoi區(qū)域。當(dāng)一條線段位于三角形的不同voronoi區(qū)域中時(shí)，利用不同的函數(shù)計(jì)算距離最近的一對(duì)控制點(diǎn)。當(dāng)它位于不止一個(gè)voronoi區(qū)域中時(shí)，利用vr_clip算法搜索距離最近的一對(duì)控制點(diǎn)。

步驟二：建立空間機(jī)器人考慮避撞任務(wù)的多任務(wù)運(yùn)動(dòng)學(xué)方程。

空間機(jī)械臂末端執(zhí)行器的主要任務(wù)和次要避障任務(wù)描述如下：

這里，考慮任務(wù)優(yōu)先級(jí)滿足所有目標(biāo)的逆運(yùn)動(dòng)學(xué)解法描述如下，不考慮零空間運(yùn)動(dòng)h＝0：

其中，是je零空間中矩陣jo的投影。此方程中的每一項(xiàng)均有意義。第一項(xiàng)保證了主要任務(wù)的實(shí)現(xiàn)(為保證穩(wěn)定跟蹤，通常采用而非)。第二項(xiàng)，投影在末端執(zhí)行器雅克比矩陣的零空間，描述了不受末端執(zhí)行器運(yùn)動(dòng)影響的控制點(diǎn)運(yùn)動(dòng)。項(xiàng)描述了由于末端執(zhí)行器運(yùn)動(dòng)誘導(dǎo)的控制點(diǎn)的運(yùn)動(dòng)，矩陣結(jié)合了末端執(zhí)行器的零空間和控制點(diǎn)，用以將操作空間中分配的控制點(diǎn)速度轉(zhuǎn)化到關(guān)節(jié)空間中。

步驟三：基于一對(duì)控制點(diǎn)建立空間機(jī)器人的避撞約束條件，結(jié)合運(yùn)動(dòng)學(xué)方程求解。

考慮笛卡爾空間，如果想驅(qū)動(dòng)避撞方法并且給控制點(diǎn)分配速度，那么則是一個(gè)三維矢量，即jo∈r^3×n，需要3個(gè)額外冗余度去完成三維笛卡爾空間中的避撞。然而，避撞方法僅需要控制控制點(diǎn)和障礙物上相應(yīng)控制點(diǎn)連線方向上的逃逸速度，這是一個(gè)一維約束，一個(gè)冗余度足以規(guī)避碰撞。障礙物穩(wěn)定時(shí)，分別從機(jī)械臂和障礙物上選取點(diǎn)p1、p1'為一對(duì)控制點(diǎn)，||p1p1'||距離的導(dǎo)數(shù)，即避撞約束，表示為下：

n1是的單位矢量，符號(hào)<u,v>表示矢量u和矢量v的內(nèi)積?，F(xiàn)用一個(gè)新雅可比矩陣替換逆運(yùn)動(dòng)學(xué)解中的jo，那么速度和就變成了標(biāo)量，顯著地提高了moore-penrose偽逆的計(jì)算效率，表示如下：

由于是標(biāo)量，在計(jì)算的偽逆時(shí)不需要對(duì)任何矩陣求逆。軌跡規(guī)劃時(shí)大大減少了計(jì)算量。因此，完成主要任務(wù)和避障任務(wù)的逆運(yùn)動(dòng)學(xué)新解可表示如下：

此解隱含的一個(gè)問題是，若雅克比矩陣je或不滿秩，機(jī)械臂就會(huì)發(fā)生奇異，從而導(dǎo)致關(guān)節(jié)角速度趨近無窮大。

步驟四：如圖1所示，基于多個(gè)控制點(diǎn)建立新控制點(diǎn)的運(yùn)動(dòng)信息，設(shè)計(jì)調(diào)整參數(shù)結(jié)合運(yùn)動(dòng)學(xué)方程求解。

為實(shí)現(xiàn)更可靠的避撞，在機(jī)械臂上選取一個(gè)結(jié)合p1、p2兩個(gè)控制點(diǎn)信息的新控制點(diǎn)p0。此外，新控制點(diǎn)p0在機(jī)械臂上連續(xù)運(yùn)動(dòng)。計(jì)算如下：

其中，d1＝||p1p1'||、d2＝||p2p2'||分別是p1、p2與障礙物之間的距離。由于控制點(diǎn)p1更為鄰近，令相應(yīng)的逃逸單位矢量n0等于n1，期望的臨界點(diǎn)速度vo是指定的標(biāo)稱速度，αc是避撞增益，其值取決于與障礙物的臨界距離：

定義一個(gè)新的臨界距離dsr，稱安全距離。如果機(jī)械臂距離障礙物太近，即d1≤duf，則取消由調(diào)整參數(shù)αe控制的主要任務(wù)根據(jù)機(jī)械臂動(dòng)力學(xué)特性，可以預(yù)設(shè)或解算非安全距離duf的大小。

為實(shí)現(xiàn)更平滑的運(yùn)動(dòng)，在dsr處，令的大小為0。考慮避障任務(wù)的逆運(yùn)動(dòng)學(xué)解可表示如下：

調(diào)整參數(shù)定義如下：

從式(8)可以看出，控制點(diǎn)p1進(jìn)入影響區(qū)之后，即d1＜dif，避撞方法開始影響機(jī)械臂構(gòu)型，此時(shí)僅包含部分齊次解。隨著控制點(diǎn)與障礙物之間距離的減小，在dif到dsr之間，執(zhí)行更多的齊次解以減小控制點(diǎn)和障礙物之間的相對(duì)速度。一旦控制點(diǎn)距離出現(xiàn)在dsr范圍以內(nèi)，分配一個(gè)包含完整齊次解的跳變速度此避撞方法可以保證控制點(diǎn)在危險(xiǎn)區(qū)域外移動(dòng)，其次，在機(jī)械臂運(yùn)動(dòng)過程中控制點(diǎn)p1發(fā)生跳變時(shí)，仍可保證其平滑轉(zhuǎn)變。

步驟五：將基于多個(gè)控制點(diǎn)的避撞方法拓展應(yīng)用于多個(gè)障礙物的情形。

當(dāng)工作空間中有多個(gè)障礙物時(shí)，由于缺少冗余度，不能按處理多子任務(wù)的方式處理不同的障礙物。此時(shí)，若仍不加修改地應(yīng)用式(8)，機(jī)械臂上的控制點(diǎn)會(huì)頻繁地跳變，進(jìn)而導(dǎo)致關(guān)節(jié)角速度振蕩。關(guān)節(jié)角速度不連續(xù)對(duì)機(jī)械臂結(jié)構(gòu)和驅(qū)動(dòng)器造成十分不利。為了提高性能，考慮影響區(qū)內(nèi)所有障礙物，齊次解的加權(quán)和表示如下：

其中，n0是影響區(qū)內(nèi)障礙物數(shù)目，權(quán)重因子ωi定義如下：

權(quán)重因子ωi表示了影響區(qū)內(nèi)不同障礙物的相對(duì)重要性。當(dāng)一個(gè)障礙物比其他障礙物更靠近機(jī)械臂時(shí)，即d1i→0，那么相應(yīng)的權(quán)重因子增加，控制點(diǎn)速度也隨之增加。

仿真結(jié)果表明，所提的避撞方法不影響機(jī)械臂末端執(zhí)行器執(zhí)行主要任務(wù)，而且具有自主調(diào)節(jié)控制點(diǎn)、平滑關(guān)節(jié)角速度，避免其發(fā)生不連續(xù)的優(yōu)點(diǎn)，可以擴(kuò)展到未來應(yīng)用中。

圖4顯示了未加入避障方法時(shí)，多關(guān)節(jié)運(yùn)動(dòng)冗余空間機(jī)器人末端穿過類似走廊的運(yùn)動(dòng)情形。空間機(jī)器人可以精確地實(shí)現(xiàn)對(duì)末端路徑的跟蹤，但是，由于未考慮避障問題，空間機(jī)器人與右側(cè)的障礙物發(fā)生了碰撞。圖5顯示了僅考慮單個(gè)控制點(diǎn)避障方法的仿真情形?？梢钥闯?，空間機(jī)器人仍能夠?qū)崿F(xiàn)對(duì)末端路徑的精確跟蹤，同時(shí)實(shí)現(xiàn)了對(duì)右側(cè)障礙物的規(guī)避，但是由于存在控制點(diǎn)的切換問題，故空間機(jī)器人關(guān)節(jié)角速度存在跳變現(xiàn)象，這對(duì)于關(guān)節(jié)的使用壽命十分不利。圖6顯示了考慮兩個(gè)控制點(diǎn)避障方法的仿真情形。與圖4和圖5類似，其末端可以精確實(shí)現(xiàn)對(duì)路徑的跟蹤，并且由于采用多個(gè)控制點(diǎn)進(jìn)行避障，空間機(jī)器人各關(guān)節(jié)的角速度更為平滑，不存在角速度跳變現(xiàn)象，這大大有利于關(guān)節(jié)電機(jī)壽命的延長(zhǎng)，且為后續(xù)空間機(jī)器人軌跡規(guī)劃提供了較好的條件。