本發(fā)明屬于機(jī)械臂跡規(guī)劃領(lǐng)域，具體涉及一種六自由度機(jī)械臂考慮力約束的軌跡規(guī)劃方法。

背景技術(shù)：

機(jī)械臂軌跡規(guī)劃是指在規(guī)定時間內(nèi)，機(jī)械臂按照一定的速度和加速度完成從初始位置姿態(tài)運動到期望位置姿態(tài)的運動。機(jī)械臂考慮力約束的軌跡規(guī)劃是使得機(jī)械臂在期望力的約束下，讓六自由度機(jī)械臂實現(xiàn)期望軌跡的規(guī)劃。基于力約束的機(jī)械臂軌跡規(guī)劃有很多方法，如4-3-4軌跡，3-5-3軌跡以及5段3次軌跡等，這些方法均可進(jìn)行處理機(jī)械臂動力約束下的軌跡規(guī)劃，但面向關(guān)節(jié)空間的笛卡爾路徑會損失部分采樣點。

本發(fā)明提出的方法針對一類典型的六自由度機(jī)械臂，工業(yè)機(jī)械臂大部分屬于此類機(jī)械臂，因此對于這類機(jī)械臂考慮力約束的軌跡規(guī)劃的研究具有很大的意義。

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

本發(fā)明的目的在于提供一種考慮接觸形變基于不同接觸邊緣的六自由度機(jī)械臂考慮力約束的軌跡規(guī)劃方法。

本發(fā)明的目的是這樣實現(xiàn)的：

1.一種六自由度機(jī)械臂考慮力約束的軌跡規(guī)劃方法，其特征在于，包括如下步驟：

(1)將力約束轉(zhuǎn)化為接觸形變和接觸運動速度的約束；

根據(jù)接觸模型將機(jī)械臂末端與環(huán)境的接觸分為靜態(tài)接觸和動態(tài)接觸；

(1.1)靜態(tài)接觸過程中，將力的約束轉(zhuǎn)化為沿法線方向的接觸位移的約束，即

$X_E=\sqrt{F/K_s}\≤\sqrt{F_{max}/K_s}=X_{Emax}$

其中，K_s是接觸剛度；X_E是機(jī)械臂末端沿接觸面法向的形變量；F表示機(jī)械末端力傳感器測量的接觸面法線方向上的接觸力；

(1.2)動態(tài)接觸也就是沖擊，機(jī)械臂末端與外界環(huán)境的接觸碰撞過程在極短的時間內(nèi)完成，將力約束轉(zhuǎn)化為接觸速度的約束，來實現(xiàn)滿足機(jī)械臂軌跡規(guī)劃過程中沖擊最優(yōu)；

(2)優(yōu)化動態(tài)接觸沖擊的機(jī)械臂運動速度；

(2.1)采用S型加減速曲線使加速度連續(xù)；

采用S型速度變化使得運動過程中加速度連續(xù)，減小各關(guān)節(jié)的沖擊，使機(jī)械臂運動和作業(yè)滿足力的約束；

優(yōu)化后的S型速度曲線共分為七個過程：

(2.1.1)加加速度一定、加速度勻速增加過程，加速度從初始值加速到最大加速度；

(2.1.2)勻加加速度加速過程，保持最大加速度不變，速度勻速增加；

(2.1.3)減加速度加速過程，速度繼續(xù)增加，加速度以恒定的加加速度減小直到速度加到最大速度值；

(2.1.4)勻速運動過程，保持最大速度運動，加速度為零；

(2.1.5)減減速度減速過程，速度減小，減速度以恒定的減減速度減??；

(2.1.6)勻減減速度減速過程，保持減速度不變，速度勻速減??；

(2.1.7)減減速度一定減速度勻速減小過程；

(2.2)歸一化時間算子；

笛卡爾空間位置插補(bǔ)的表達(dá)式為：

P(t)＝P_s+l(t)(P_e-P_s)，

其中，P(t)表示插補(bǔ)點的坐標(biāo)；l(t)為歸一化時間算子；P_s為期望位置；P_e為初始位置；

要對時間算子進(jìn)行平滑處理，使得速度曲線平滑，定義歸一化時間算子的表達(dá)式為：

$l\left(t\right)=\frac{S\left(t_{i+1}\right)-S\left(t_i\right)}{L},$

其中，i＝0,1,2,3,4,5,6；l(0)＝0；l(T)＝1；l(t)在區(qū)間內(nèi)單調(diào)遞增；S(t)為各時間段內(nèi)對應(yīng)的總位移；

(3)基于力約束進(jìn)行軌跡規(guī)劃；

(3.1)基于規(guī)則接觸邊緣具有力約束的連續(xù)軌跡規(guī)劃：

規(guī)則接觸邊緣具有力約束的連續(xù)軌跡采用圓弧過渡的方法代替直線-直線連續(xù)軌跡規(guī)劃方法進(jìn)行平滑處理，達(dá)到運動在接觸臨界接觸面時不具有沿接觸面法線方向的位移、速度和加速度分量，過渡圓弧的聯(lián)結(jié)點分別選取在前一段直線的減速段和后一段直線的加速段；

(3.1.1)在接觸模型有效的情況下，得到機(jī)械臂末端與外界環(huán)境的接觸剛度為K，在已知約束力為F的情況下，可求得接觸形變X_e：

X_e＝F/K；

(3.1.2)通過接觸形變，得到臨界接觸點B點的空間位置坐標(biāo)p₂：

p₂＝p_q+X_e；

軌跡終點M的空間位置坐標(biāo)p₃為：

p₃＝p₂+L；

(3.1.3)過渡精度的確定，過渡精度決定過渡圓弧的大小，決定實際軌跡與期望軌跡逼近程度；設(shè)過渡力大小為F'，則過渡精度re為：

re＝F-F'

當(dāng)力傳感器示數(shù)時，進(jìn)行空間直線軌跡運動；當(dāng)力傳感器示數(shù)時，為過渡圓弧的起點；當(dāng)力傳感器示數(shù)時，進(jìn)行圓弧過渡運運動；當(dāng)力傳感器示數(shù)時，為過渡圓弧的終點，之后機(jī)械臂末端與外界環(huán)境保持力F進(jìn)行沿接觸面切平面方向的運動；

(3.1.4)確定過渡圓弧起始聯(lián)結(jié)點p_e1和過渡圓弧終止聯(lián)結(jié)點p_e2；

當(dāng)力傳感器示數(shù)到達(dá)過渡力F'時開始圓弧過渡運動，當(dāng)力傳感器示數(shù)到達(dá)約束力F時結(jié)束圓弧過渡運動；即過渡圓弧聯(lián)結(jié)點為接觸力為F'和F時機(jī)械臂末端的位置點；

當(dāng)力傳感器示數(shù)時，通過位置編碼器可知當(dāng)前機(jī)械臂六個關(guān)節(jié)關(guān)節(jié)角θ_e11～θ_e16，通過機(jī)械臂正運動學(xué)式得到此刻對應(yīng)的機(jī)械臂末端實際位置即為過渡圓弧起始聯(lián)結(jié)點p_e¹；當(dāng)力傳感器示數(shù)時，通過位置編碼器可知當(dāng)前機(jī)械臂六個關(guān)節(jié)關(guān)節(jié)角θ_e21～θ_e26，通過機(jī)械臂正運動學(xué)式得到此刻對應(yīng)的機(jī)械臂末端實際位置即為過渡圓弧終止聯(lián)結(jié)點p_e2；

(3.1.5)軌跡生成器通過路徑點和過渡圓弧起始聯(lián)結(jié)點p_e1和終止聯(lián)結(jié)點p_e2的空間位置坐標(biāo)，采用具有S型速度曲線擬合的空間直線和圓弧規(guī)劃方法生成運動軌跡；

(3.1.6)運動過程中用力傳感器進(jìn)行監(jiān)測，看整個運動過程是否滿足期望力F的約束，如果時，重新規(guī)劃。

(3.2)基于曲率接觸邊緣具有力約束的連續(xù)軌跡規(guī)劃：

(3.2.1)機(jī)械臂實現(xiàn)沿接觸面法線方向直線運動和沿臨界安全接觸面的接觸運動和沿接觸面法線方向遠(yuǎn)離臨界安全接觸面的直線運動，是一個直線-圓弧-直線連續(xù)的軌跡規(guī)劃過程，確定運動過程中在安全接觸圓柱面上的具有力約束運動中的特殊點M、N點；

(3.2.2)采用圓弧過渡來解決直線-圓弧連續(xù)運動時產(chǎn)生的尖角問題；使得末端在沿安全接觸面的運動過程中只有沿接觸面切線方向位移和速度，法線方向速度為零；

(3.2.3)通過調(diào)節(jié)過渡精度來調(diào)整實際軌跡與期望軌跡的精度，滿足接觸模型的基于圓柱接觸面的連續(xù)軌跡規(guī)劃方法與基于規(guī)則接觸面連續(xù)軌跡規(guī)劃方法中過渡圓弧聯(lián)結(jié)點求取方法一致，過渡完后機(jī)械臂末端是在一定接觸力下沿著接觸面的圓弧軌跡。

本發(fā)明的有益效果在于：

本發(fā)明將機(jī)械臂力約束轉(zhuǎn)化為接觸形變和接觸運動速度的約束，再根據(jù)接觸運動學(xué)，提出了動態(tài)接觸沖擊的機(jī)械臂運動速度優(yōu)化方法和考慮接觸形變的基于不同接觸邊緣的連續(xù)軌 跡規(guī)劃方法，此方法為考慮力約束的機(jī)械臂軌跡規(guī)劃提供了新方法。此方法避免了機(jī)械臂末端與外界環(huán)境的接觸碰撞過程在極短時間內(nèi)完成，產(chǎn)生極大瞬間沖擊，導(dǎo)致接觸力超出安全范圍。

附圖說明

圖1是機(jī)械臂具有力約束軌跡規(guī)劃系統(tǒng)結(jié)構(gòu)圖；

圖2是機(jī)械臂末端與規(guī)則接觸面示意圖；

圖3是基于規(guī)則接觸面具有力約束軌跡規(guī)劃示意圖；

圖4是機(jī)械臂末端與具有曲率接觸面示意圖。

具體實施方式

下面結(jié)合附圖對本發(fā)明做進(jìn)一步描述。

本發(fā)明提供的是一類六自由度機(jī)械臂考慮力約束的軌跡規(guī)劃方法，屬于機(jī)械臂軌跡規(guī)劃領(lǐng)域。傳統(tǒng)六自由度機(jī)械臂軌跡規(guī)劃時，只考慮機(jī)械臂末端的位置而對力約束的軌跡規(guī)劃研究很少，但此類問題尤為重要，針對這一現(xiàn)狀，本發(fā)明提出一類六自由度機(jī)械臂考慮力約束的軌跡規(guī)劃方法。本發(fā)明提出的機(jī)械臂考慮力約束的軌跡方法如下：首先利用接觸模型和接觸運動學(xué)將力約束轉(zhuǎn)化為接觸形變和接觸運動速度的約束，然后在優(yōu)化動態(tài)接觸沖擊的軌跡規(guī)劃中，對運動速度進(jìn)行S型曲線處理，最后提出對基于規(guī)則接觸邊緣和基于曲率接觸邊緣的具有力約束的連續(xù)軌跡規(guī)劃方法，一種在力的過渡閾值范圍內(nèi)，采用法線方向接觸運動向切線方向接觸運動過渡的且具有S型速度曲線的連續(xù)軌跡規(guī)劃方法。此方法避免機(jī)械臂與外界環(huán)境的接觸過程中產(chǎn)生過大沖擊造成超出約束要求，且適用于具有力約束的六自由度機(jī)械臂的軌跡規(guī)劃，在不同接觸邊緣提出不同軌跡規(guī)劃，更大程度的滿足現(xiàn)代工程項目對工業(yè)機(jī)械臂軌跡規(guī)劃的要求。

一類六自由度機(jī)械臂考慮力約束的軌跡規(guī)劃方法，包括如下步驟：

1、將力約束轉(zhuǎn)化為接觸形變和接觸運動速度的約束

接觸力不僅與接觸形變有關(guān)，還與接觸過程中的沖擊有關(guān)，于是接觸模型可將機(jī)械臂末端與環(huán)境的接觸分為靜態(tài)接觸和動態(tài)接觸。

⑴靜態(tài)接觸作用時間較長，接觸狀態(tài)變化緩慢勻速或保持不變，接觸過程中，可將力的約束轉(zhuǎn)化為沿法線方向的接觸位移的約束，即

$X_E=\sqrt{F/K_s}\≤\sqrt{F_{max}/K_s}=X_{Emax}$

其中，K_s是接觸剛度，與材料有關(guān)；X_E是機(jī)械臂末端沿接觸面法向的形變量；F表示機(jī)械末端力傳感器測量的接觸面法線方向上的接觸力；

⑵動態(tài)接觸也就是所說的沖擊，機(jī)械臂末端與外界環(huán)境的接觸碰撞過程在極短的時間內(nèi)完成，在此過程中可將力約束轉(zhuǎn)化為接觸速度的約束，來實現(xiàn)滿足機(jī)械臂軌跡規(guī)劃過程中沖擊最優(yōu)。

2、優(yōu)化動態(tài)接觸沖擊的機(jī)械臂運動速度

⑴采用S型加減速曲線

采用S型速度變化使得運動過程中加速度連續(xù)，進(jìn)而減小各關(guān)節(jié)的沖擊，使機(jī)械臂運動和作業(yè)滿足力的約束。

優(yōu)化后的S型速度曲線共分為七個過程：

①加加速度一定加速度勻速增加過程，加速度從初始值加速到最大加速度；

②勻加加速度加速過程，此過程中保持最大加速度不變，速度勻速增加；

③減加速度加速過程，此過程中速度繼續(xù)增加，加速度以恒定的加加速度減小直到速度加到最大速度值；

④勻速運動過程，此過程中保持最大速度運動，加速度為零；

⑤減減速度減速過程，此過程中速度減小，減速度以恒定的減減速度減?。?/p>

⑥勻減減速度減速過程，此過程中保持減速度不變，速度勻速減??；

⑦減減速度一定減速度勻速減小過程；

⑵歸一化時間算子

笛卡爾空間位置插補(bǔ)的表達(dá)式為：

P(t)＝P_s+l(t)(P_e-P_s)

其中，P(t)表示插補(bǔ)點的坐標(biāo)；l(t)為歸一化時間算子；P_s為期望位置；P_e為初始位置。

要對時間算子進(jìn)行平滑處理，使得速度曲線平滑。定義歸一化時間算子的表達(dá)式為：

$l\left(t\right)=\frac{S\left(t_{i+1}\right)-S\left(t_i\right)}{L}$

其中，i＝0,1,2,3,4,5,6；l(0)＝0；l(T)＝1；l(t)在區(qū)間內(nèi)單調(diào)遞增；S(t)為各時間段內(nèi)對應(yīng)的總位移；

3、基于力約束進(jìn)行軌跡規(guī)劃

⑴基于規(guī)則接觸邊緣具有力約束的連續(xù)軌跡規(guī)劃：

規(guī)則接觸邊緣具有力約束的連續(xù)軌跡采用圓弧過渡的方法代替直線-直線連續(xù)軌跡規(guī)劃方法進(jìn)行平滑處理。達(dá)到運動在接觸臨界接觸面時不具有沿接觸面法線方向的位移、速度和加速度分量，過渡圓弧的聯(lián)結(jié)點分別選取在前一段直線的減速段和后一段直線的加速段。

①在接觸模型的有效的情況下，可得機(jī)械臂末端與外界環(huán)境的接觸剛度為K，在已知約 束力為F的情況下，可求得接觸形變X_e：

X_e＝F/K

②通過接觸形變，可得臨界接觸點B點的空間位置坐標(biāo)p₂：

p₂＝p_q+X_e

軌跡終點M的空間位置坐標(biāo)p₃為：

p₃＝p₂+L

③過渡精度的確定，過渡精度決定過渡圓弧的大小，決定實際軌跡與期望軌跡逼近程度。設(shè)過渡力大小為F'，則過渡精度re為：

re＝F-F'

當(dāng)力傳感器示數(shù)時，進(jìn)行空間直線軌跡運動；當(dāng)力傳感器示數(shù)時，為過渡圓弧的起點；當(dāng)力傳感器示數(shù)時，進(jìn)行圓弧過渡運運動；當(dāng)力傳感器示數(shù)時，為過渡圓弧的終點，之后機(jī)械臂末端與外界環(huán)境保持力F進(jìn)行沿接觸面切平面方向的運動。

④確定過渡圓弧聯(lián)結(jié)點p_e1和p_e2。

當(dāng)力傳感器示數(shù)到達(dá)過渡力F'時開始圓弧過渡運動，當(dāng)力傳感器示數(shù)到達(dá)約束力F時結(jié)束圓弧過渡運動。即過渡圓弧聯(lián)結(jié)點為接觸力為F'和F時機(jī)械臂末端的位置點。

當(dāng)力傳感器示數(shù)時，通過位置編碼器可知當(dāng)前機(jī)械臂六個關(guān)節(jié)關(guān)節(jié)角θ_e11～θ_e16，通過機(jī)械臂正運動學(xué)式可得此刻對應(yīng)的機(jī)械臂末端實際位置即為過渡圓弧起始聯(lián)結(jié)點p_e1。同理，當(dāng)力傳感器示數(shù)時，通過位置編碼器可知當(dāng)前機(jī)械臂六個關(guān)節(jié)關(guān)節(jié)角θ_e21～θ_e26，通過機(jī)械臂正運動學(xué)式可得此刻對應(yīng)的機(jī)械臂末端實際位置即為過渡圓弧終止聯(lián)結(jié)點p_e2。

⑤軌跡生成器通過路徑點和過渡圓弧起始聯(lián)結(jié)點p_e1和終止聯(lián)結(jié)點p_e2的空間位置坐標(biāo)，采用具有S型速度曲線擬合的空間直線和圓弧規(guī)劃方法生成運動軌跡。

⑥運動過程中用力傳感器進(jìn)行監(jiān)測，看整個運動過程是否滿足期望力F的約束，如果 時，重新規(guī)劃。

⑵基于曲率接觸邊緣具有力約束的連續(xù)軌跡規(guī)劃：

①機(jī)械臂實現(xiàn)沿接觸面法線方向直線運動和沿臨界安全接觸面的接觸運動和沿接觸面法線方向遠(yuǎn)離臨界安全接觸面的直線運動，是一個直線-圓弧-直線連續(xù)的軌跡規(guī)劃過程，運動過程中M、N點在安全接觸圓柱面上，是具有力約束運動中的特殊點。

②采用圓弧過渡來解決直線-圓弧連續(xù)運動時產(chǎn)生的尖角問題。使得末端在沿安全接觸面 的運動過程中只有沿接觸面切線方向位移和速度，法線方向速度為零，保證運動滿足安全約束。

③通過調(diào)節(jié)過渡精度來調(diào)整實際軌跡與期望軌跡的精度。滿足接觸模型的基于圓柱接觸面的連續(xù)軌跡規(guī)劃方法與基于規(guī)則接觸面連續(xù)軌跡規(guī)劃方法中過渡圓弧聯(lián)結(jié)點求取方法一致，不同的是過渡完后機(jī)械臂末端是在一定接觸力下沿著接觸面的圓弧軌跡。

1、具有接觸形變約束的軌跡規(guī)劃方法

接觸模型的接觸剛度穩(wěn)定不變，將力約束轉(zhuǎn)化為接觸位移的約束來實現(xiàn)機(jī)械臂末端與外界環(huán)境之間的安全接觸運動。具有力約束軌跡規(guī)劃系統(tǒng)結(jié)構(gòu)圖如附1圖所示，整個系統(tǒng)結(jié)構(gòu)由接觸模型、軌跡生成器、力位混合控制器及機(jī)械臂執(zhí)行器組成。其中，F(xiàn)_d為期望力約束；X_e為通過接觸模型得到的接觸形變；Y為實際位置；F為機(jī)械臂腕部六維力傳感器測量的機(jī)械臂末端施加的力。

1)、接觸模型

將力約束轉(zhuǎn)化為接觸形變的約束進(jìn)行機(jī)械臂與外界環(huán)境之間接觸運動。由接觸模型式得出，接觸力不僅與接觸形變有關(guān)還與接觸剛度有關(guān)。機(jī)械臂末端與外界環(huán)境之間的接觸剛度與機(jī)械臂自身末端剛度和環(huán)境剛度有關(guān)。對于機(jī)械臂末端的剛度與其姿態(tài)有關(guān)，在保持末端位置不變的情況下不同姿態(tài)所對應(yīng)的末端剛度不同；環(huán)境剛度與接觸環(huán)境的材料有關(guān)。對于剛度較大的接觸，較小的形變就會產(chǎn)生較大的接觸力。因此，要想實現(xiàn)力約束的轉(zhuǎn)化就先要對接觸模型進(jìn)行驗證。

本發(fā)明將采用實驗測量的方法對接觸模型進(jìn)行驗證，具體步驟如下：

(1)選運動方向沿接觸面法線方向，設(shè)定機(jī)械臂末端起始位置X₀，通過角位移編碼器反饋讀取當(dāng)前實際六個關(guān)節(jié)角角位移值θ₁～θ₆，根據(jù)機(jī)械臂正運動學(xué)可知機(jī)械臂末端實際位置

(2)保持其他方向位移不變，讓機(jī)械臂沿接觸面法線方向運動期望距離ΔX_i，與外界環(huán)境產(chǎn)生接觸，讀取當(dāng)前力傳感器示數(shù)F_i和當(dāng)前實際六個關(guān)節(jié)角角位移值θ_i1～θ_i6，根據(jù)機(jī)械臂正運動學(xué)可知機(jī)械臂末端實際位置進(jìn)行多組數(shù)據(jù)測量，每次的期望運動距離可不同。

(3)對采集到的F_i和進(jìn)行數(shù)據(jù)處理，驗證兩者是否滿足線性關(guān)系，若滿足則證明接觸模型正確，進(jìn)而可以得到接觸剛度值，實現(xiàn)力約束向接觸形變約束的轉(zhuǎn)化。

2)、軌跡生成器

軌跡生成器可以根據(jù)所給的路徑點生成期望軌跡。對于不同的環(huán)境不同的作業(yè)任務(wù)，機(jī)械臂末端與外界物體之間接觸運動的軌跡不同，所選用的軌跡生成方法也不同。若要實現(xiàn)接觸運動直線軌跡，則選用軌跡生成器中直線插補(bǔ)方法；若要求在接觸運動過程中實現(xiàn)圓弧軌跡，則選擇圓弧插補(bǔ)方法；要若要在接觸過程中實現(xiàn)自由曲線軌跡，則選用樣條函數(shù)插值方法。

接觸運動學(xué)，當(dāng)接觸運動過程是沿切線方向運動和法線方向運動的組合運動時，不僅需要考慮軌跡的連續(xù)性還要使得整個運動過程在力的安全約束范圍內(nèi)。對于此要求，本發(fā)明將采用運動過渡的方法實現(xiàn)具有力約束的軌跡規(guī)劃。在接觸模型成立的基礎(chǔ)上，可以將接觸力的約束轉(zhuǎn)化為接觸形變約束，進(jìn)而可以將力安全閾值范圍轉(zhuǎn)化為過渡軌跡。在有力約束的情況下，對于沿接觸面法線方向運動與沿接觸面切平面方向運動軌跡的過渡過程中，解決軌跡尖角問題是實現(xiàn)滿足要求運動的關(guān)鍵。對于連續(xù)軌跡中的尖角問題處理有圓弧過渡和樣條函數(shù)二次插補(bǔ)過渡兩種方法。根據(jù)過渡軌跡要求不同可采取不同的過渡方法，樣條函數(shù)二次插補(bǔ)適合對過渡軌跡精度要求較高或過渡軌跡曲線不確定的運動，相比圓弧過渡，樣條函數(shù)二次插補(bǔ)過渡方法計算量大，效率低。因此，本文將采用圓弧過渡的方法實現(xiàn)具有力約束的軌跡規(guī)劃。

由于機(jī)械臂末端剛度與機(jī)械臂末端姿態(tài)有關(guān)，姿態(tài)的變化會影響接觸剛度的變化，導(dǎo)致末端接觸力變化，末端姿態(tài)的變化還會產(chǎn)生與接觸面之間滾動接觸運動，使得機(jī)械末端與外界環(huán)境之間不僅有力的作用還有力矩的作用，是一個十分復(fù)雜的問題。本文這里假設(shè)運動過程中機(jī)械臂末端相對于接觸環(huán)境的姿態(tài)保持不變即保持接觸剛度不變，只考慮具有沿接觸面法線方向力約束情況和與接觸面之間滑動接觸運動。

3)、力位混合控制器

軌跡生成器根據(jù)給定的路徑點生成笛卡爾空間期望軌跡，并通過機(jī)械臂逆運動學(xué)轉(zhuǎn)化為關(guān)節(jié)空間軌跡。將對應(yīng)的關(guān)節(jié)變量輸入給力位混合控制器，通過控制器實現(xiàn)機(jī)械臂運動。

2、基于規(guī)則接觸邊緣具有力約束的連續(xù)軌跡規(guī)劃方法

本發(fā)明選擇空間平面為規(guī)則接觸面進(jìn)行研究，圖3機(jī)械臂末端與接觸面的空間示意圖。運動情況描述為：機(jī)械臂末端從空間起始點沿接觸面法線方向運動到規(guī)則接觸面，在具有一定接觸力情況下沿著接觸面運動。為了方面研究，將機(jī)械臂末端與環(huán)境的接觸看作為點接觸，接觸過程中假設(shè)接觸剛度不發(fā)生變化，研究滑動接觸運動過程中連續(xù)軌跡規(guī)劃方法。

在接觸模型成立的情況下，由赫茲接觸理論接觸力的計算公式可知，在接觸剛度一定，力指數(shù)為1的情況下，接觸形變與接觸力成線性關(guān)系。對于具有規(guī)則接觸面的平面，已知最大接觸力約束，可得規(guī)則接觸平面對應(yīng)的臨界安全接觸面是一個與規(guī)則接觸面平行的平面如 圖2所示。圖2中，A為起始點，Q為接觸面上的點，B、M為臨界安全接觸面上的點，要求機(jī)械臂末端實現(xiàn)沿接觸面法線和接觸面切平面連續(xù)直線軌跡規(guī)劃任務(wù)，即AB-BM連續(xù)直線軌跡。在AB-BM連續(xù)軌跡中B點在臨界安全接觸面上，是滿足力約束運動中的一個特殊點。

對于空間連續(xù)直線運動，理論上可以采取單獨規(guī)劃每條獨立直線的方法，但是這樣會使得電機(jī)在各段直線間不停的啟動和停止，實際中這種操作會產(chǎn)生較大的振動和摩擦[51]。若進(jìn)行連續(xù)的規(guī)劃，由圖2可以看出，在直線與直線的聯(lián)結(jié)處即B點處會有尖角產(chǎn)生，尖角的存在會使得位移、速度方向發(fā)生突變，不但軌跡不平滑而且會造成運動超出安全接觸形變的約束。對于此種情況，本文將采用圓弧過渡的方法對直線-直線連續(xù)軌跡進(jìn)行平滑處理，過渡圓弧的聯(lián)結(jié)點分別在前一段直線的減速段和后一段直線的加速段，圓弧過渡示意圖如圖2中軌跡所示，這樣不僅可以保證B點的特殊要求，并使得運動在接觸臨界接觸面時不具有沿接觸面法線方向的位移、速度和加速度分量。

對于圖3中所描述的運動，滿足接觸模型的基于規(guī)則接觸平面的連續(xù)軌跡規(guī)劃方法如下：

已知起始點A的位置坐標(biāo)為p₁，外界接觸物體上一點Q坐標(biāo)p_q，期望約束力為F，沿接觸面切平面運動距離為L。

(1)在接觸模型的有效的情況下，可得機(jī)械臂末端與外界環(huán)境的接觸剛度為K，在已知約束力為F的情況下，可求得接觸形變X_e：

X_e＝F/K

(2)通過接觸形變，可得臨界接觸點B點的空間位置坐標(biāo)p₂：

p₂＝p_q+X_e

軌跡終點M的空間位置坐標(biāo)p₃為：

p₃＝p₂+L

(3)過渡精度的確定，過渡精度決定過渡圓弧的大小，決定實際軌跡與期望軌跡逼近程度。設(shè)過渡力大小為F'，則過渡精度re為：

re＝F-F'

當(dāng)力傳感器示數(shù)時，進(jìn)行空間直線軌跡運動；當(dāng)力傳感器示數(shù)時，為過渡圓弧的起點；當(dāng)力傳感器示數(shù)時，進(jìn)行圓弧過渡運運動；當(dāng)力傳感器示數(shù)時，為過渡圓弧的終點，之后機(jī)械臂末端與外界環(huán)境保持力F進(jìn)行沿接觸面切平面方向的運動。

(4)確定過渡圓弧聯(lián)結(jié)點p_e1和p_e2。

當(dāng)力傳感器示數(shù)到達(dá)過渡力F'時開始圓弧過渡運動，當(dāng)力傳感器示數(shù)到達(dá)約束力F時結(jié)束圓弧過渡運動。即過渡圓弧聯(lián)結(jié)點為接觸力為F'和F時機(jī)械臂末端的位置點。

當(dāng)力傳感器示數(shù)時，通過位置編碼器可知當(dāng)前機(jī)械臂六個關(guān)節(jié)關(guān)節(jié)角θ_e11～θ_e16，通過機(jī)械臂正運動學(xué)式可得此刻對應(yīng)的機(jī)械臂末端實際位置即為過渡圓弧起始聯(lián)結(jié)點p_e1。同理，當(dāng)力傳感器示數(shù)時，通過位置編碼器可知當(dāng)前機(jī)械臂六個關(guān)節(jié)關(guān)節(jié)角θ_e21～θ_e26，通過機(jī)械臂正運動學(xué)式可得此刻對應(yīng)的機(jī)械臂末端實際位置即為過渡圓弧終止聯(lián)結(jié)點p_e2。

(5)軌跡生成器通過路徑點A、Q、M和過渡圓弧起始聯(lián)結(jié)點p_e1和終止聯(lián)結(jié)點p_e2的空間位置坐標(biāo)，采用具有S型速度曲線擬合的空間直線和圓弧規(guī)劃方法生成運動軌跡。

(6)運動過程中用力傳感器進(jìn)行監(jiān)測，看整個運動過程是否滿足期望力F的約束，如果 時，重新規(guī)劃。

需要說明的是，在機(jī)械臂末端接觸運動過程中會產(chǎn)生摩擦力，本文暫不考慮摩擦力的影響；對于不同接觸剛度的物體，力約束下的接觸形變大小不同，運動中可以通過調(diào)整過渡精度來調(diào)整跟期望軌跡的逼近程度。

3.基于曲率接觸邊緣具有力約束的連續(xù)軌跡規(guī)劃方法

本發(fā)明選擇圓柱體為具有一定曲率的接觸面進(jìn)行研究，圖4為機(jī)械臂末端與接觸面的空間示意圖。運動情況描述為：機(jī)械臂末端從空間中起始點沿接觸法線方向運動到規(guī)則接觸面，在具有一定接觸力情況下沿著接觸面運動。為了方面研究，將機(jī)械臂末端與環(huán)境的接觸看作為點接觸，接觸過程中假設(shè)接觸剛度不發(fā)生變化，研究滑動接觸運動過程中連續(xù)軌跡規(guī)劃方法。

由赫茲接觸理論接觸力的計算公式可知，在接觸剛度一定，力指數(shù)為1的情況下，接觸形變與接觸力成線性關(guān)系。對于圓柱接觸面，已知最大安全接觸力，可知臨界安全接觸面也是一個與圓柱面通圓心的圓柱面投影圖4所示。只要接觸運動過程中產(chǎn)生的位移不超過臨界安全接觸面，即可使運動滿足力的約束。圖中A為起始點，Q為接觸面上的點，M、U、N為臨界安全接觸面上的點，要求機(jī)械臂實現(xiàn)沿接觸面法線方向直線運動和沿臨界安全接觸面的接觸運動和沿接觸面法線方向遠(yuǎn)離臨界安全接觸面的直線運動，即AM-MN-NB運動，是一個直線-圓弧-直線連續(xù)的軌跡規(guī)劃過程，運動過程中M、N點在安全接觸圓柱面上，是具有力約束運動中的特殊點。規(guī)劃的關(guān)鍵是解決直線與圓弧連接時在接觸點出現(xiàn)的尖角問題，進(jìn)而解決特殊點位置、速度、加速度約束問題，使得運動滿足安全要求。本發(fā)明采用圓弧過渡來解決直線-圓弧連續(xù)運動時產(chǎn)生的尖角問題，如圖4中藍(lán)色軌跡所示。這樣使得末端在沿 安全接觸面的運動過程中只有沿接觸面切線方向位移和速度，法線方向速度為零，保證運動滿足安全約束?？赏ㄟ^調(diào)節(jié)過渡精度來調(diào)整實際軌跡與期望軌跡的精度。滿足接觸模型的基于圓柱接觸面的連續(xù)軌跡規(guī)劃方法與基于規(guī)則接觸面連續(xù)軌跡規(guī)劃方法中過渡圓弧聯(lián)結(jié)點求取方法一致，不同的是過渡完后機(jī)械臂末端是在一定接觸力下沿著接觸面的圓弧軌跡。