技術(shù)特征：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(2.1.4)勻速運(yùn)動(dòng)過程，保持最大速度運(yùn)動(dòng)，加速度為零；

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

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

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

(2.2)歸一化時(shí)間算子；

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

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

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

要對時(shí)間算子進(jìn)行平滑處理，使得速度曲線平滑，定義歸一化時(shí)間算子的表達(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)為各時(shí)間段內(nèi)對應(yīng)的總位移；

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

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

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

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

X_e＝F/K；

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

p₂＝p_q+X_e；

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

p₃＝p₂+L；

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

re＝F-F'

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

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

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

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

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

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

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

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

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

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