本發(fā)明涉及一種工業(yè)機(jī)器人軌跡規(guī)劃方法，尤其是一種工業(yè)機(jī)器人避障軌跡規(guī)劃方法。

背景技術(shù)：

近十幾年工業(yè)機(jī)器人的制造和控制技術(shù)發(fā)展迅速，已經(jīng)廣泛地應(yīng)用于汽車制造業(yè)并能達(dá)到很高的控制精度。隨著我國(guó)勞動(dòng)力成本增加，在“機(jī)器換人”和產(chǎn)業(yè)升級(jí)的要求下各行業(yè)的自動(dòng)化生產(chǎn)水平亟需提高。應(yīng)用工業(yè)機(jī)器人能夠避免重復(fù)繁瑣的人工勞動(dòng)、實(shí)現(xiàn)高穩(wěn)定、高精度生產(chǎn)，從而使工作效率得到很大提升。對(duì)單個(gè)機(jī)器人的路徑、軌跡規(guī)劃大多能通過(guò)人工示教完成，所以已經(jīng)有越來(lái)越多的工廠使用可編程的工業(yè)機(jī)器人代替生產(chǎn)線上的人工。但是對(duì)于一類需要多機(jī)器人互相協(xié)調(diào)配合才能完成的任務(wù)，如何讓機(jī)器人能夠避免可能出現(xiàn)的干涉和碰撞同時(shí)能快速準(zhǔn)確地完成任務(wù)，這便對(duì)機(jī)器人關(guān)節(jié)運(yùn)動(dòng)控制以及避障路徑規(guī)劃提出了更高要求。

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

為了克服已有工業(yè)機(jī)器人軌跡規(guī)劃方法的控制精度較低、無(wú)法有效避障、安全性較低的不足，本發(fā)明提供了一種控制精度較高、有效實(shí)現(xiàn)避障、安全性較高的基于力矩控制的工業(yè)機(jī)器人避障軌跡規(guī)劃方法。

本發(fā)明解決其技術(shù)問(wèn)題所采用的技術(shù)方案是：

一種基于力矩控制的工業(yè)機(jī)器人避障軌跡規(guī)劃方法，首先，通過(guò)辨識(shí)各關(guān)節(jié)運(yùn)動(dòng)學(xué)以及慣性參數(shù)建立機(jī)器人動(dòng)力學(xué)模型；然后，通過(guò)逆運(yùn)動(dòng)學(xué)求解得到運(yùn)動(dòng)到終點(diǎn)姿態(tài)時(shí)各關(guān)節(jié)角的位移；最后，以關(guān)節(jié)角位移作為運(yùn)動(dòng)路程，采用一種正弦加加速度規(guī)劃方法進(jìn)行各關(guān)節(jié)的速度加速度規(guī)劃，將速度和加速度代入動(dòng)力學(xué)模型即可求解運(yùn)動(dòng)路徑中的控制力矩；

對(duì)于運(yùn)行過(guò)程中可能發(fā)的碰撞，采用一種檢測(cè)關(guān)節(jié)投影干涉情況然后判斷關(guān)鍵點(diǎn)距離的方法進(jìn)行碰撞預(yù)測(cè)：將機(jī)器人關(guān)節(jié)向速度向量的法平面投影，若關(guān)節(jié)投影發(fā)生干涉則對(duì)關(guān)節(jié)軸線上各關(guān)鍵點(diǎn)最短距離進(jìn)行判斷，若小于運(yùn)行時(shí)的安全距離則認(rèn)為存在碰撞的風(fēng)險(xiǎn)；預(yù)測(cè)到碰撞可能發(fā)生，然后對(duì)危險(xiǎn)關(guān)節(jié)施加一種阻抗加速度降低關(guān)節(jié)運(yùn)動(dòng)速度從而實(shí)現(xiàn)避障。

進(jìn)一步，所述規(guī)劃方法包括以下步驟：

步驟1：工業(yè)機(jī)器人的動(dòng)力學(xué)參數(shù)辨識(shí)

采用對(duì)機(jī)器人整體進(jìn)行試驗(yàn)的整體辨識(shí)法對(duì)機(jī)器人的動(dòng)力學(xué)參數(shù)進(jìn)行辨識(shí)。為了使機(jī)器人能實(shí)現(xiàn)任意軌跡規(guī)劃，采用周期性軌跡，每個(gè)關(guān)節(jié)的激勵(lì)軌跡是正弦和余弦函數(shù)的代數(shù)和，即有限的傅立葉級(jí)數(shù)函數(shù)，則機(jī)器人每個(gè)關(guān)節(jié)的關(guān)節(jié)位置q規(guī)劃如下：

其中q表示關(guān)節(jié)轉(zhuǎn)角，ai、bi表示軌跡的常系數(shù)，ωf為軌跡的基頻，iωf表示第i階的頻率，t＝kts表示采樣時(shí)刻，k表示采樣個(gè)數(shù)，ts表示采樣周期；

以有限的傅立葉級(jí)數(shù)函數(shù)為機(jī)器人待辨識(shí)關(guān)節(jié)的辨識(shí)激勵(lì)軌跡，機(jī)器人沿預(yù)設(shè)軌跡運(yùn)動(dòng)過(guò)程中，通過(guò)測(cè)電流法采集關(guān)節(jié)的驅(qū)動(dòng)力矩；通過(guò)安裝在電機(jī)上的編碼器采集關(guān)節(jié)轉(zhuǎn)角，對(duì)轉(zhuǎn)角微分得到動(dòng)態(tài)參數(shù)中的角速度和角加速度以最小二乘法作為辨識(shí)算法可以求解出慣性參數(shù)的值：

從而求得精確地動(dòng)力學(xué)模型：其中τ為機(jī)器人的驅(qū)動(dòng)力矩；向量d(q)稱為機(jī)械臂的質(zhì)量矩陣；為科氏力及離心力項(xiàng)；g(q)為重力項(xiàng)；

步驟2：工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)逆解

采用解析解法求解機(jī)器人從初始位置到末端位置各關(guān)節(jié)運(yùn)動(dòng)角位移，對(duì)于逆運(yùn)動(dòng)學(xué)求解出現(xiàn)多解和奇異解的問(wèn)題，除了通過(guò)設(shè)定各個(gè)關(guān)節(jié)的旋轉(zhuǎn)范圍限制來(lái)去除無(wú)效解和避免奇異解的方法以外，還根據(jù)運(yùn)動(dòng)能量最小原則選擇消耗能量最小的逆解值作為關(guān)節(jié)的執(zhí)行量，計(jì)算方法如下：計(jì)算與前一個(gè)關(guān)節(jié)差的平方和，選擇計(jì)算結(jié)果最小的一組關(guān)節(jié)位置作為下一個(gè)關(guān)節(jié)執(zhí)行的位置，逆解選擇的表達(dá)式如下：

步驟3：正弦加加速度關(guān)節(jié)軌跡規(guī)劃

采用正弦加加速度規(guī)劃方法能夠?qū)Ω麝P(guān)節(jié)軌跡進(jìn)行高速、平滑的規(guī)劃，完成從起點(diǎn)到終點(diǎn)的姿態(tài)規(guī)劃。

為了能夠?qū)崿F(xiàn)運(yùn)動(dòng)速度以及加速度控制的能力，對(duì)于機(jī)器人末端操作器的速度，加速度的邊界進(jìn)行限制，有如下三點(diǎn)：

1)運(yùn)動(dòng)角位移：d；

2)運(yùn)動(dòng)學(xué)上邊界：最大角速度ωmax，最大角加速度αmax，最大角加加速度jmax。

3)運(yùn)動(dòng)學(xué)下邊界：

將加加速度設(shè)定為正弦函數(shù)曲線：

在運(yùn)動(dòng)軌跡的限制條件下，運(yùn)動(dòng)距離具有兩個(gè)臨界點(diǎn)，其分別為dmin和dmin2，在維持加加速度不變的情況下，最大速度和最大加速度都能夠達(dá)到，運(yùn)動(dòng)位移有一個(gè)臨界最小值，稱為dmin：

其中dmin為初速為零時(shí)加速到ωmax時(shí)的運(yùn)動(dòng)距離daccinc表示加速到最大速度時(shí)的運(yùn)動(dòng)距離；daccdec表示加ω速度減小到最小值時(shí)運(yùn)動(dòng)的距離；如果最大加速度未能夠達(dá)到但是恰好達(dá)到了最大速度，運(yùn)動(dòng)位移有另一個(gè)臨界值最小值，被稱為dmin2：

其中dt′為重新計(jì)算后的加速運(yùn)動(dòng)時(shí)間，然后根據(jù)運(yùn)動(dòng)邊界值的不同計(jì)算出不同階段的運(yùn)動(dòng)時(shí)間，通過(guò)對(duì)正弦曲線的加加速度積分得到加速度、速度以及位移的運(yùn)動(dòng)軌跡；

步驟4：碰撞預(yù)測(cè)

對(duì)于兩個(gè)機(jī)器人同時(shí)作業(yè)，按照關(guān)節(jié)幾何形狀，采用半球端面圓柱體包絡(luò)兩個(gè)關(guān)節(jié)從而得到標(biāo)準(zhǔn)化后的關(guān)節(jié)邊界，兩球面端點(diǎn)連線為關(guān)節(jié)軸線，長(zhǎng)度為li，i＝1,2；圓柱體半徑為ri，在各軸線按等間距取個(gè)點(diǎn)作為計(jì)算關(guān)節(jié)距離的關(guān)鍵點(diǎn)記為根據(jù)關(guān)節(jié)軸線上所取關(guān)鍵點(diǎn)可得兩關(guān)節(jié)碰撞時(shí)的臨界距離：

根據(jù)dhit設(shè)定碰撞預(yù)測(cè)的極限距離dlim＝k1*dhit以及安全距離dsafe＝k2*dhit，k1,k2為安全系數(shù)；

根據(jù)關(guān)節(jié)角位移以及瞬時(shí)角速度可由正運(yùn)動(dòng)學(xué)求解得各關(guān)鍵點(diǎn)的笛卡爾坐標(biāo)以及關(guān)節(jié)末端速度向量vf，將投影到vf向量的法平面得到關(guān)鍵點(diǎn)的投影點(diǎn)判斷各投影點(diǎn)之間的最小距離若：

則說(shuō)明兩個(gè)關(guān)節(jié)在速度向量法平面的投影發(fā)生干涉，則進(jìn)一步判斷各關(guān)節(jié)關(guān)鍵點(diǎn)之間最短距離，若有：

d＝min{||p11-p21||，||p11-p22||，||p11-p23||...||p11-p24||}＜dsafe

則判定為有可能碰撞的危險(xiǎn)狀態(tài)，否則認(rèn)為是安全運(yùn)行；

步驟5：阻抗控制避障規(guī)劃

如果步驟4中的關(guān)鍵點(diǎn)之間最短距離d<dsafe，則在后續(xù)的運(yùn)行過(guò)程中可能會(huì)發(fā)生碰撞，采用一種通過(guò)判斷d與dsafe、dlim之間大小關(guān)系確定阻抗加速度αv的方法使各關(guān)節(jié)減速?gòu)亩苊庹系K：

直到d>dsafe則更新角速度為當(dāng)前運(yùn)動(dòng)角速度ωi以及路徑距離di，然后繼續(xù)按步驟3的正弦加加速度方法進(jìn)行速度和加速度規(guī)劃。

再進(jìn)一步，所述步驟3中，如果任務(wù)要求的執(zhí)行距離d大于最大加速度和最大速度同時(shí)能夠達(dá)到的最小距離dmin，d≥dmin2，那么將根據(jù)如下公式：

taccinc表示加速到最大速度時(shí)的運(yùn)動(dòng)時(shí)間；taccdec表示加ω速度減小到最小值時(shí)運(yùn)動(dòng)的時(shí)間；tacc表示在加速度增加到最大值恒定階段時(shí)加速過(guò)程中運(yùn)動(dòng)的時(shí)間，tret表示在負(fù)加速度增加到最大值恒定階段時(shí)減速過(guò)程中運(yùn)動(dòng)的時(shí)間，tvel表示恒速運(yùn)動(dòng)的時(shí)間。

所述步驟3中，如果任務(wù)要求的運(yùn)動(dòng)距離d小于滿足最大加速度的最小距離dmin，但是大于能夠達(dá)到最大速度距離，d≥dmin2；這時(shí)運(yùn)動(dòng)達(dá)到的最大加速度以及加速運(yùn)動(dòng)時(shí)間需要重新計(jì)算得：

taccinc＝taccdec＝dt′,tretinc＝tretdec＝dt,tacc＝0,

所述步驟3中，如果任務(wù)給定的運(yùn)動(dòng)距離d不足以使得速度和加速度都達(dá)到給定的速度和加速度，d≤dmin2，那么采取先勻速后減速的運(yùn)動(dòng)策略：

當(dāng)d>αmaxdt，

當(dāng)d<αmaxdt時(shí)減速運(yùn)動(dòng)時(shí)間和最大加速度需要根據(jù)距離重新計(jì)算：

利用正弦加加速度軌跡規(guī)劃根據(jù)當(dāng)前速度以及路徑距離，可以對(duì)后續(xù)運(yùn)動(dòng)的加速度以及速度進(jìn)行平滑地規(guī)劃從而實(shí)現(xiàn)運(yùn)行過(guò)程中的平穩(wěn)力矩控制。

本發(fā)明的技術(shù)構(gòu)思為：本發(fā)明根據(jù)精確的參數(shù)辨識(shí)對(duì)工業(yè)機(jī)器人進(jìn)行動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)建模，以此為基礎(chǔ)實(shí)現(xiàn)對(duì)機(jī)器人各關(guān)節(jié)基于模型的力矩控制：通過(guò)逆運(yùn)動(dòng)學(xué)求解各關(guān)節(jié)運(yùn)動(dòng)角度后，采用正弦加加速度進(jìn)行各關(guān)節(jié)加速度規(guī)劃并通過(guò)機(jī)器人動(dòng)力學(xué)模型求解控制力矩。采用一種建立虛擬阻抗的方法平滑地避免兩個(gè)機(jī)器人運(yùn)動(dòng)時(shí)可能出現(xiàn)的碰撞：通過(guò)判斷關(guān)節(jié)在運(yùn)動(dòng)方向向量法平面上投影的干涉情況以及關(guān)節(jié)上所選點(diǎn)的距離完成碰撞預(yù)測(cè)，通過(guò)建立關(guān)節(jié)之間最短距離阻與抗加速度的函數(shù)關(guān)系進(jìn)行危險(xiǎn)運(yùn)動(dòng)區(qū)域的速度調(diào)節(jié)實(shí)現(xiàn)各關(guān)節(jié)的安全運(yùn)行。

本發(fā)明的有益效果主要表現(xiàn)在：能夠根據(jù)關(guān)節(jié)運(yùn)動(dòng)路程以及當(dāng)前角速度完成其后續(xù)運(yùn)動(dòng)的速度、加速度規(guī)劃；提出了一種檢測(cè)關(guān)節(jié)投影干涉情況然后判斷關(guān)鍵點(diǎn)距離的方法進(jìn)行碰撞預(yù)測(cè)，相較于單純的最短距離檢測(cè)能夠更有效地完成機(jī)器人關(guān)節(jié)運(yùn)動(dòng)過(guò)程的碰撞預(yù)測(cè)；提出了一種根據(jù)關(guān)節(jié)軸線關(guān)鍵點(diǎn)最短距離建立阻抗加速度函數(shù)的方法進(jìn)行避障路徑規(guī)劃，有效實(shí)現(xiàn)避障、安全性較高。

附圖說(shuō)明

圖1是動(dòng)力學(xué)參數(shù)辨識(shí)過(guò)程圖。

圖2是正弦加加速度軌跡規(guī)劃流程圖。

圖3為碰撞預(yù)測(cè)流程圖。

圖4為軌跡規(guī)劃完整流程圖。

具體實(shí)施方式

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

參照?qǐng)D1～圖4，一種基于力矩控制的工業(yè)機(jī)器人避障軌跡規(guī)劃方法，首先，通過(guò)辨識(shí)各關(guān)節(jié)運(yùn)動(dòng)學(xué)以及慣性參數(shù)建立精確的機(jī)器人動(dòng)力學(xué)模型；然后，通過(guò)逆運(yùn)動(dòng)學(xué)求解得到運(yùn)動(dòng)到終點(diǎn)姿態(tài)時(shí)各關(guān)節(jié)角的位移；最后，以關(guān)節(jié)角位移作為運(yùn)動(dòng)路程，采用一種正弦加加速度規(guī)劃方法進(jìn)行各關(guān)節(jié)的速度加速度規(guī)劃。將速度和加速度代入動(dòng)力學(xué)模型即可求解運(yùn)動(dòng)路徑中的控制力矩。對(duì)于運(yùn)行過(guò)程中可能發(fā)的碰撞，本發(fā)明采用一種檢測(cè)關(guān)節(jié)投影干涉情況然后判斷關(guān)鍵點(diǎn)距離的方法進(jìn)行碰撞預(yù)測(cè)：將機(jī)器人關(guān)節(jié)向速度向量的法平面投影，若關(guān)節(jié)投影發(fā)生干涉則對(duì)關(guān)節(jié)軸線上各關(guān)鍵點(diǎn)最短距離進(jìn)行判斷，若小于運(yùn)行時(shí)的安全距離則認(rèn)為存在碰撞的風(fēng)險(xiǎn)。預(yù)測(cè)到碰撞可能發(fā)生，然后對(duì)危險(xiǎn)關(guān)節(jié)施加一種阻抗加速度降低關(guān)節(jié)運(yùn)動(dòng)速度從而實(shí)現(xiàn)避障。所述規(guī)劃方法包括以下步驟：

步驟1：工業(yè)機(jī)器人的動(dòng)力學(xué)參數(shù)辨識(shí)

本發(fā)明采用對(duì)機(jī)器人整體進(jìn)行試驗(yàn)的整體辨識(shí)法對(duì)機(jī)器人的動(dòng)力學(xué)參數(shù)進(jìn)行辨識(shí)。為了使機(jī)器人能實(shí)現(xiàn)任意軌跡規(guī)劃本方法中采用周期性軌跡，每個(gè)關(guān)節(jié)的激勵(lì)軌跡是正弦和余弦函數(shù)的代數(shù)和，即有限的傅立葉級(jí)數(shù)函數(shù)，則機(jī)器人每個(gè)關(guān)節(jié)的關(guān)節(jié)位置q規(guī)劃如下：

其中q表示關(guān)節(jié)轉(zhuǎn)角，ai、bi表示軌跡的常系數(shù)，ωf為軌跡的基頻，iωf表示第i階的頻率，t＝kts表示采樣時(shí)刻，k表示采樣個(gè)數(shù)，ts表示采樣周期。以有限的傅立葉級(jí)數(shù)函數(shù)為機(jī)器人待辨識(shí)關(guān)節(jié)的辨識(shí)激勵(lì)軌跡，機(jī)器人沿預(yù)設(shè)軌跡運(yùn)動(dòng)過(guò)程中，通過(guò)測(cè)電流法采集關(guān)節(jié)的驅(qū)動(dòng)力矩；通過(guò)安裝在電機(jī)上的編碼器采集關(guān)節(jié)轉(zhuǎn)角，對(duì)轉(zhuǎn)角微分可以得到動(dòng)態(tài)參數(shù)中的角速度和角加速度以最小二乘法作為辨識(shí)算法可以求解出慣性參數(shù)的值：

從而求得精確地動(dòng)力學(xué)模型：其中τ為機(jī)器人的驅(qū)動(dòng)力矩；向量d(q)稱為機(jī)械臂的質(zhì)量矩陣；為科氏力及離心力項(xiàng)；g(q)為重力項(xiàng)。

步驟2：工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)逆解

采用解析解法求解機(jī)器人從初始位置到末端位置各關(guān)節(jié)運(yùn)動(dòng)角位移。對(duì)于逆運(yùn)動(dòng)學(xué)求解出現(xiàn)多解和奇異解的問(wèn)題，除了通過(guò)設(shè)定各個(gè)關(guān)節(jié)的旋轉(zhuǎn)范圍限制來(lái)去除無(wú)效解和避免奇異解的方法以外，還根據(jù)運(yùn)動(dòng)能量最小原則選擇消耗能量最小的逆解值作為關(guān)節(jié)的執(zhí)行量，具體計(jì)算方法如下：計(jì)算與前一個(gè)關(guān)節(jié)差的平方和，選擇計(jì)算結(jié)果最小的一組關(guān)節(jié)位置作為下一個(gè)關(guān)節(jié)執(zhí)行的位置。逆解選擇的表達(dá)式如下：

步驟3：正弦加加速度關(guān)節(jié)軌跡規(guī)劃

采用正弦加加速度規(guī)劃方法能夠?qū)Ω麝P(guān)節(jié)軌跡進(jìn)行高速、平滑的規(guī)劃，高效準(zhǔn)確地完成從起點(diǎn)到終點(diǎn)的姿態(tài)規(guī)劃。

為了能夠?qū)崿F(xiàn)運(yùn)動(dòng)速度以及加速度控制的能力，對(duì)于機(jī)器人末端操作器的速度，加速度的邊界進(jìn)行限制，具體有如下三點(diǎn)：

1)運(yùn)動(dòng)角位移：d

2)運(yùn)動(dòng)學(xué)上邊界：最大角速度ωmax，最大角加速度αmax，最大角加加速度jmax。

3)運(yùn)動(dòng)學(xué)下邊界：

這里將加加速度設(shè)定為正弦函數(shù)曲線：

在運(yùn)動(dòng)軌跡的限制條件下，運(yùn)動(dòng)距離具有兩個(gè)臨界點(diǎn)，其分別為dmin和dmin2。在維持加加速度不變的情況下，最大速度和最大加速度都能夠達(dá)到，運(yùn)動(dòng)位移有一個(gè)臨界最小值，稱為dmin：

其中dmin為初速為零時(shí)加速到ωmax時(shí)的運(yùn)動(dòng)距離daccinc表示加速到最大速度時(shí)的運(yùn)動(dòng)距離；daccdec表示加ω速度減小到最小值時(shí)運(yùn)動(dòng)的距離；如果最大加速度未能夠達(dá)到但是恰好達(dá)到了最大速度，運(yùn)動(dòng)位移有另一個(gè)臨界值最小值，被稱為dmin2：

其中dt′為重新計(jì)算后的加速運(yùn)動(dòng)時(shí)間。然后根據(jù)運(yùn)動(dòng)邊界值的不同計(jì)算出不同階段的運(yùn)動(dòng)時(shí)間，通過(guò)對(duì)正弦曲線的加加速度積分得到加速度、速度以及位移的運(yùn)動(dòng)軌跡。

對(duì)軌跡規(guī)劃的各個(gè)步驟以及計(jì)算表達(dá)式介紹如下：

1：在此步驟中，根據(jù)工作任務(wù)要求的運(yùn)動(dòng)速度和加速度以及運(yùn)動(dòng)的位移被初始化。在對(duì)距離的大小進(jìn)行判斷后進(jìn)行各階段時(shí)間的計(jì)算。

2：如果任務(wù)要求的執(zhí)行距離d大于最大加速度和最大速度同時(shí)能夠達(dá)到的最小距離dmin(d≥dmin2)，那么將根據(jù)如下公式：

taccinc表示加速到最大速度時(shí)的運(yùn)動(dòng)時(shí)間；taccdec表示加ω速度減小到最小值時(shí)運(yùn)動(dòng)的時(shí)間；tacc表示在加速度增加到最大值恒定階段時(shí)加速過(guò)程中運(yùn)動(dòng)的時(shí)間。tret表示在負(fù)加速度增加到最大值恒定階段時(shí)減速過(guò)程中運(yùn)動(dòng)的時(shí)間。tvel表示恒速運(yùn)動(dòng)的時(shí)間。對(duì)各個(gè)階段運(yùn)動(dòng)的時(shí)間進(jìn)行計(jì)算，這是由于速度和加速度都能夠充分加速到最大值，所以其中有加速度恒定時(shí)間和速度恒定勻速時(shí)間需要根據(jù)實(shí)際距離計(jì)算其大小。

3：如果任務(wù)要求的運(yùn)動(dòng)距離d小于滿足最大加速度的最小距離dmin，但是大于能夠達(dá)到最大速度距離(d≥dmin2)這時(shí)運(yùn)動(dòng)達(dá)到的最大加速度以及加速運(yùn)動(dòng)時(shí)間需要重新計(jì)算得：

taccinc＝taccdec＝dt′,tretinc＝tretdec＝dt,tacc＝0,

4：如果任務(wù)給定的運(yùn)動(dòng)距離d不足以使得速度和加速度都達(dá)到給定的速度和加速度(d≤dmin2)那么在這個(gè)步驟中采取先勻速后減速的運(yùn)動(dòng)策略:當(dāng)d>αmaxdt:

當(dāng)d<αmaxdt時(shí)減速運(yùn)動(dòng)時(shí)間和最大加速度需要根據(jù)距離重新計(jì)算：

利用正弦加加速度軌跡規(guī)劃根據(jù)當(dāng)前速度以及路徑距離，可以對(duì)后續(xù)運(yùn)動(dòng)的加速度以及速度進(jìn)行平滑地規(guī)劃從而實(shí)現(xiàn)運(yùn)行過(guò)程中的平穩(wěn)力矩控制。

步驟4：碰撞預(yù)測(cè)

對(duì)于兩個(gè)機(jī)器人同時(shí)作業(yè)，其各關(guān)節(jié)為復(fù)雜的圓周運(yùn)動(dòng)，在這樣的運(yùn)動(dòng)狀態(tài)下單純的根據(jù)關(guān)節(jié)間距離并不能準(zhǔn)確地預(yù)判碰撞的發(fā)生。本發(fā)明采用一種先檢測(cè)關(guān)節(jié)投影干涉情況然后判斷關(guān)鍵點(diǎn)之間最短距離的方法完成完成運(yùn)動(dòng)過(guò)程中的碰撞預(yù)測(cè)。以兩個(gè)機(jī)器人末端關(guān)節(jié)為例進(jìn)行說(shuō)明：

按照關(guān)節(jié)幾何形狀，采用半球端面圓柱體包絡(luò)兩個(gè)關(guān)節(jié)從而得到標(biāo)準(zhǔn)化后的關(guān)節(jié)邊界，兩球面端點(diǎn)連線為關(guān)節(jié)軸線，長(zhǎng)度為li(i＝1,2)；圓柱體半徑為ri。在各軸線按等間距取個(gè)點(diǎn)作為計(jì)算關(guān)節(jié)距離的關(guān)鍵點(diǎn)記為根據(jù)關(guān)節(jié)軸線上所取關(guān)鍵點(diǎn)可得兩關(guān)節(jié)碰撞時(shí)的臨界距離：

根據(jù)dhit設(shè)定碰撞預(yù)測(cè)的極限距離dlim＝k1*dhit以及安全距離dsafe＝k2*dhit，k1,k2為安全系數(shù)可根據(jù)實(shí)際選取大于一的常數(shù)。

根據(jù)關(guān)節(jié)角位移以及瞬時(shí)角速度可由正運(yùn)動(dòng)學(xué)求解得各關(guān)鍵點(diǎn)的笛卡爾坐標(biāo)以及關(guān)節(jié)末端速度向量vf。將投影到vf向量的法平面得到關(guān)鍵點(diǎn)的投影點(diǎn)判斷各投影點(diǎn)之間的最小距離若：

則說(shuō)明兩個(gè)關(guān)節(jié)在速度向量法平面的投影發(fā)生干涉，則進(jìn)一步判斷各關(guān)節(jié)關(guān)鍵點(diǎn)之間最短距離，若有：

d＝min{||p11-p21||，||p11-p22||，||p11-p23||...||p11-p24||}＜dsafe

則判定為有可能碰撞的危險(xiǎn)狀態(tài)否則認(rèn)為是安全運(yùn)行。

步驟5：阻抗控制避障規(guī)劃

在上一步中如果關(guān)鍵點(diǎn)之間最短距離d<dsafe，則在后續(xù)的運(yùn)行過(guò)程中可能會(huì)發(fā)生碰撞。本發(fā)明采用一種通過(guò)判斷d與dsafe、dlim之間大小關(guān)系確定阻抗加速度αv的方法使各關(guān)節(jié)減速?gòu)亩苊庹系K：

直到d>dsafe則更新角速度為當(dāng)前運(yùn)動(dòng)角速度ωi以及路徑距離di，然后繼續(xù)按步驟3的正弦加加速度方法進(jìn)行速度和加速度規(guī)劃。