專利名稱：仿人機(jī)器人機(jī)械臂避碰運(yùn)動軌跡規(guī)劃方法
技術(shù)領(lǐng)域：
本發(fā)明屬于機(jī)器人領(lǐng)域，尤其是一種仿人機(jī)器人機(jī)械臂避碰運(yùn)動軌跡規(guī)劃方法。
背景技術(shù)：
機(jī)械臂的軌跡規(guī)劃是指，在滿足一定約束的條件下，規(guī)劃一條從起始位姿到目標(biāo)位姿的無碰撞的運(yùn)動路徑。根據(jù)規(guī)劃空間的不同，軌跡規(guī)劃方法可分為基于關(guān)節(jié)空間的軌跡規(guī)劃和基于任務(wù)空間(即笛卡爾空間)的軌跡規(guī)劃兩種。這兩種方法均有一定的局限性，由于機(jī)械臂作業(yè)通常在笛卡爾空間描述，所以基于關(guān)節(jié)空間的軌跡規(guī)劃方法對任務(wù)的描述不直觀，而基于任務(wù)空間的軌跡規(guī)劃則需要實現(xiàn)將笛卡爾空間的位姿進(jìn)行逆運(yùn)動學(xué)求解，轉(zhuǎn)換為機(jī)械臂各個關(guān)節(jié)角，需要大量的運(yùn)算，而且無法保證整條笛卡爾空間的軌跡中不存在奇異構(gòu)型，所以基于笛卡爾空間的軌跡規(guī)劃方法很難應(yīng)用于實時控制。目前，軌跡規(guī)劃方法主要有矩形加速度函數(shù)法，梯形加速度函數(shù)法，B樣條曲線法，多項式插值法等。其中矩形加速度函數(shù)法計算簡単，易于實現(xiàn)，可應(yīng)用于實時應(yīng)用場合。目前，對機(jī)械臂無碰撞軌跡規(guī)劃的研究已有很多，Lazona-Perze在Automaticplanning of manipulator transfer movement 和 bpacial planning: a configurationspace approach兩篇文章中，提出了一種基于C空間規(guī)劃方法的自由空間法，該方法能夠規(guī)劃出無碰撞的機(jī)械臂運(yùn)動軌跡，但該方法中對障礙物的建模方法比較復(fù)雜，所以規(guī)劃的效率比較低，不適于實時應(yīng)用。Khabit在Real-time obstacle avoidance formanipulators and mobile robots 一文中提出，在障礙處定義一個排斥勢場，在目標(biāo)點出定義ー個吸弓I勢場，機(jī)械臂的運(yùn)動就由排斥勢場和吸弓I勢場的共同作用來決定，該方法在處理全局路徑規(guī)劃的動態(tài)避障問題時，非常有效，但易于陷入局部最小點處。申請?zhí)枮?01110037726. 9的專利中，描述了一種用于エ業(yè)機(jī)器人的無碰撞軌跡規(guī)劃的方法，該方法將整個笛卡爾空間劃分成三個部分，障礙物區(qū)域，無碰撞區(qū)域和發(fā)生碰撞的區(qū)域，每個區(qū)域用一個或多個圓柱形空間描述，機(jī)械臂的軌跡僅在無碰撞區(qū)域進(jìn)行規(guī)劃，可見這種方法沒有從根本上解決碰撞問題。

發(fā)明內(nèi)容
本發(fā)明的目的在于針對現(xiàn)有技術(shù)的不足，提供了一種仿人機(jī)器人機(jī)械臂避碰運(yùn)動軌跡規(guī)劃方法。本發(fā)明適用于具有七個自由度的機(jī)械臂，其七個關(guān)節(jié)角分別為肩俯仰角、肩滾動角、肩偏擺角、肘滾動角、肘偏擺角、腕滾動角和腕俯仰角。并且，固定機(jī)械臂從初始位姿到目標(biāo)位姿的運(yùn)動時間為I從目標(biāo)位姿返回初始位姿的時間為Λ。本發(fā)明ー種仿人機(jī)器人機(jī)械臂避碰運(yùn)動軌跡規(guī)劃方法包括以下步驟
步驟ー根據(jù)桌面與機(jī)器人的相對位置，對桌子進(jìn)行建摸。在笛卡爾空間下，以機(jī)器人肩心為世界坐標(biāo)系，將桌子模型建立為ー個平面以下的區(qū)域。步驟ニ 碰撞檢測及分類。
為了滿足實時性要求，將笛卡爾空間描述的初始位姿和目標(biāo)任務(wù)位姿轉(zhuǎn)換成關(guān)節(jié)空間的初始關(guān)節(jié)角序列和目標(biāo)關(guān)節(jié)角序列，在關(guān)節(jié)空間下采用矩形函數(shù)加速度法對各個關(guān)節(jié)角進(jìn)行插補(bǔ)，生成關(guān)節(jié)空間下的軌跡，然后對這條軌跡進(jìn)行正運(yùn)動學(xué)運(yùn)算得到笛卡爾空間的軌跡，如果機(jī)械臂采用這條軌跡完成動作的過程中，整條機(jī)械臂都未經(jīng)過桌子所在的區(qū)域，說明機(jī)械臂此次運(yùn)動沒有與桌子發(fā)生碰撞，則采用該關(guān)節(jié)空間的軌跡控制機(jī)械臂完成任務(wù)。否則若機(jī)械臂的軌跡與桌面模型有交集，則可確定機(jī)械臂在此次任務(wù)中與桌面發(fā)生了碰撞，需要對機(jī)械臂的軌跡進(jìn)行處理，然后再完成動作。此時需要根據(jù)發(fā)生碰撞的時刻Γ區(qū)分碰撞的三種情況(I)『,初始位姿到目標(biāo)位姿的運(yùn)動過程發(fā)生碰撞，返回過程未發(fā)生碰撞；(2) Γ ,初始位姿到目標(biāo)位姿的運(yùn)動過程未發(fā)生碰撞，返回初始位姿的過程發(fā)生碰撞；(3)以上兩種碰撞都發(fā)生了，即初始位姿到目標(biāo)位姿的運(yùn)動過程和返回過程都發(fā)生碰撞。步驟三計算肘滾動角的“臨界值”，井根據(jù)該臨界值調(diào)整肘滾動角的軌跡，避免發(fā)
生碰撞O針對發(fā)生碰撞的情況，在碰撞的時刻使機(jī)械臂的肘部適當(dāng)?shù)膹澢敝羷偤貌慌c桌面發(fā)生碰撞，即在碰撞時刻對造成機(jī)械臂與桌面碰撞影響最顯著的位于肘部的肘滾動角的值進(jìn)行修改，使得修改后得到的肘滾動角的“臨界值”，結(jié)合肩俯仰角、肩滾動角、肩偏擺角、肘偏擺角、腕滾動角和腕俯仰角的值形成的機(jī)械臂的構(gòu)型，剛好不與桌面碰撞。肘滾動角臨界值的求解步驟第一歩，根據(jù)碰撞時刻肩俯仰角、肩滾動角、肩偏擺角、肘偏擺角、腕滾動角和腕俯仰角的值以及假設(shè)ー個肘滾動角的初始值，計算機(jī)械臂的正運(yùn)動學(xué)即可得到肘關(guān)節(jié)方向向量的兩個頂點A、B及機(jī)械臂上的ー個檢
測點C的坐標(biāo)分別為A ( , )，B ( , , ) , C ( - . ),并計算此時肘關(guān)節(jié)的方
向向量AB ;第二歩，計算過點C且垂直于肘關(guān)節(jié)方向向量AB的平面L，可得其方程為
(>■ -^3)( -ろ)+ O -ァ3)(Λ - ) + (z-2s)(z2 -ζι) = 0 ;第三步，計算肘關(guān)節(jié)方向向量AB與上
一歩得到的平面L的交點0( ,J0,)，以此交點ο為球心，以點ο到點c的距離||cc||為半徑的球面方程 Ψ，其表達(dá)式為(x~xo)2 +(J->'ο)2 +(ζ~ζη)2 = (x3- xo)2 + OfS~λ)2 + ( ~zo)2 ；第四步，求該球面Ψ、第二步的平面L以及桌面的交點D ( ,ヌ4，24 );第五步，計算ZCOD，并用右手定則判斷ZCOZ)正負(fù)得到的值設(shè)為,然后計算機(jī)械臂與桌面發(fā)生碰撞時肘滾動角的值，設(shè)為？4& 麗ゴ，則勵i = 90+Ag4 ,最后在基礎(chǔ)上增加一定余量，即可得到肘滾動角的臨界值。取該碰撞時刻和該時刻肘滾動角的臨界值作為ー個中間狀態(tài)，以及機(jī)械臂在初始時刻的肘滾動角為其初始狀態(tài)和在目標(biāo)時刻的肘滾動角為目標(biāo)狀態(tài)，將肘滾動角的軌跡進(jìn)行分段生成。則如果發(fā)生第(I)種碰撞，三段軌跡分別為初始狀態(tài)到中間狀態(tài)，中間狀態(tài)到目標(biāo)狀態(tài)，目標(biāo)狀態(tài)返回初始狀態(tài)；如果發(fā)生第(2)種碰撞，三段軌跡為初始狀態(tài)到目標(biāo)狀態(tài)，目標(biāo)狀態(tài)到中間狀態(tài)，中間狀態(tài)返回初始狀態(tài)；如果發(fā)生第(3)種碰撞，說明機(jī)械臂運(yùn)動過程中，共需要處理與桌面發(fā)生的兩次碰撞，三段軌跡為初始狀態(tài)到第一次避碰的中間狀態(tài)，第一次碰撞的中間狀態(tài)到第二次碰撞的中間狀態(tài)，第二次碰撞的中間狀態(tài)返回初始狀態(tài)。本發(fā)明的有益效果是本方案采用關(guān)節(jié)空間的軌跡規(guī)劃方法，對手臂各個關(guān)節(jié)分別進(jìn)行軌跡插補(bǔ)，在充分發(fā)揮各電機(jī)的性能、滿足關(guān)節(jié)限幅的要求、避免出現(xiàn)奇異位形的同吋，實現(xiàn)機(jī)械臂與桌面的避碰。達(dá)到提高機(jī)械臂完成任務(wù)的成功率和擴(kuò)大作業(yè)空間的目的。


圖I為機(jī)械臂各 關(guān)節(jié)的軸向方向及相關(guān)坐標(biāo)系的建立方法；
圖2為矩形加速度函數(shù)軌跡規(guī)劃法中加速度曲線 圖3為矩形加速度函數(shù)軌跡規(guī)劃法中速度曲線 圖4為矩形加速度函數(shù)軌跡規(guī)劃法中位移曲線 圖5為機(jī)械臂的ー組擊球位姿下的構(gòu)型；
圖6為求擊球過程中間點的流程 圖7求解返回過程中間點的流程圖。
具體實施例方式以下結(jié)合附圖對本發(fā)明作進(jìn)ー步說明。步驟ー根據(jù)桌面與機(jī)器人的相對位置，對桌面進(jìn)行建摸。如圖I所示，為ー個仿人機(jī)器人右機(jī)械臂的簡單模型，其中為其各個關(guān)
節(jié)的軸向方向向量，為世界坐標(biāo)系，( ,乃，ろM = I 7為各關(guān)節(jié)的局部坐標(biāo)系。在世界坐標(biāo)系下，將桌子建立成ー個平面以下的區(qū)域。即根據(jù)桌面高度和肩心高度，可以計算桌面所在平面在世界坐標(biāo)系下的描述為z。步驟ニ 碰撞檢測及分類。將笛卡爾空間的初始位姿及目標(biāo)位姿轉(zhuǎn)換成關(guān)節(jié)空間下的初始關(guān)節(jié)角序列和目標(biāo)關(guān)節(jié)角序列，采用矩形加速度函數(shù)規(guī)劃法生成機(jī)械臂在關(guān)節(jié)空間的軌跡，顧名思義，矩形加速度函數(shù)規(guī)劃法即加速度函數(shù)的曲線為矩形，如圖2所示，然后對加速度進(jìn)行積分得到速度曲線，如圖3所示，在對速度函數(shù)積分得到位移曲線，如圖4所示；然后進(jìn)行正運(yùn)動學(xué)計算，將該軌跡轉(zhuǎn)換成笛卡爾空間的軌跡。若機(jī)械臂沿該運(yùn)動軌跡運(yùn)動時，機(jī)械臂上的檢測點的軌跡與步驟一中所建立的桌子模型沒有交集，說明機(jī)械臂未與桌面發(fā)生碰撞，則采用關(guān)節(jié)空間的軌跡控制機(jī)械臂完成任務(wù)。否則，根據(jù)發(fā)生碰撞的時刻Γ，區(qū)分發(fā)生碰撞的類型(1) Tift,初始位姿到目標(biāo)位姿的運(yùn)動過程發(fā)生碰撞，返回過程未發(fā)生碰撞；(2)坑 ft +ra ,初始位姿到目標(biāo)位姿的運(yùn)動過程未發(fā)生碰撞，返回初始位姿的過程發(fā)生碰撞；(3)以上兩種碰撞都發(fā)生了，即兩個發(fā)生碰撞的時刻為5和5，且F1 ξζ ,^<T, + ，初始位姿到目標(biāo)位姿的運(yùn)動過程和返回過程都發(fā)生碰撞。步驟三調(diào)整肘滾動角的軌跡，避免發(fā)生碰撞。針對上述步驟ニ中提到的發(fā)生碰撞的三種情況，對造成機(jī)械臂與桌面碰撞影響最顯著的肘滾動角的軌跡進(jìn)行處理后再控制機(jī)械臂的運(yùn)動。具體處理方法如下①發(fā)生情況(I)的碰撞，記錄碰撞發(fā)生的時刻Γ。此時已知條件為由矩形加速度函數(shù)法得到的其余六個關(guān)節(jié)角在該時刻的值以及機(jī)械臂與桌面的相對位置，將這些已知條件計算該時刻肘滾動角的ー個臨界值，這個“臨界值”是指肘滾動角取該值時，結(jié)合由一般軌跡插補(bǔ)方法得到的Γ時刻其余六個關(guān)節(jié)角的值，組成的七個關(guān)節(jié)角序列，使得此時刻采用此關(guān)節(jié)序列得到的機(jī)械臂構(gòu)型，不與桌面發(fā)生碰撞。肘滾動角的臨界值的計算過程如下如圖5所示，點Α’是機(jī)械臂豎直狀態(tài)下肘關(guān)節(jié)點，點B’是機(jī)械臂豎直狀態(tài)下肘滾動角的軸向向量的頂點，假設(shè)ー個機(jī)械臂姿態(tài)肘滾動角初值選為90度，且其余六個關(guān)節(jié)角的值分別取碰撞時刻的六個值。點A是該姿態(tài)下肘關(guān)節(jié)點，點B是該姿態(tài)下肘滾動角的軸向向量的頂點，點C為所取的取得機(jī)械臂末端執(zhí)行器的邊沿檢測點之一，平面L為過點C且垂
直于向量AB的平面，D點為肘關(guān)節(jié)在90度基礎(chǔ)上轉(zhuǎn)動一定角度(繞ち逆時針轉(zhuǎn)為正，順時針轉(zhuǎn)為負(fù))后與桌面發(fā)生碰撞的碰撞點。可見，肘關(guān)節(jié)角取值為M)+s;gn(z；CD￡))時，機(jī)械臂剛好與桌子發(fā)生碰撞，從而可在90+s/p (ZCOZ))基礎(chǔ)上増大肘關(guān)節(jié)角的取值，即可避免碰撞。
根據(jù)肘滾動角的初值90度，以及插補(bǔ)得到的Γ時刻其余六個關(guān)節(jié)角的值，計算機(jī)械臂的正運(yùn)動學(xué)即可得到A、B、C三點的坐標(biāo)A 乃, )，B ( , , ) , C ( . - )。從而可得平面L的描述方程為
權(quán)利要求
1.仿人機(jī)器人機(jī)械臂避碰運(yùn)動軌跡規(guī)劃方法，其特征在于該方法包括以下步驟 步驟ー根據(jù)桌面與機(jī)器人的相對位置，對桌子進(jìn)行建模 在笛卡爾空間下，以機(jī)器人肩心為世界坐標(biāo)系，將桌子模型建立為ー個平面以下的區(qū)域； 步驟ニ 碰撞檢測及分類 為了滿足實時性要求，將笛卡爾空間描述的初始位姿和目標(biāo)任務(wù)位姿轉(zhuǎn)換成關(guān)節(jié)空間的初始關(guān)節(jié)角序列和目標(biāo)關(guān)節(jié)角序列，在關(guān)節(jié)空間下采用矩形函數(shù)加速度法對各個關(guān)節(jié)角進(jìn)行插補(bǔ)，生成關(guān)節(jié)空間下的軌跡，然后對這條軌跡進(jìn)行正運(yùn)動學(xué)運(yùn)算得到笛卡爾空間的軌跡，如果機(jī)械臂采用這條軌跡完成動作的過程中，整條機(jī)械臂都未經(jīng)過桌子所在的區(qū)域，說明機(jī)械臂此次運(yùn)動沒有與桌子發(fā)生碰撞，則采用該關(guān)節(jié)空間的軌跡控制機(jī)械臂完成任務(wù)，否則若機(jī)械臂的軌跡與桌面模型有交集，則可確定機(jī)械臂在此次任務(wù)中與桌面發(fā)生了碰撞，需要對機(jī)械臂的軌跡進(jìn)行處理，然后再完成動作，此時需要根據(jù)發(fā)生碰撞的時刻 Γ區(qū)分碰撞的三種情況(I)，初始位姿到目標(biāo)位姿的運(yùn)動過程發(fā)生碰撞，返回過程未發(fā)生碰撞；(2) If1 ,初始位姿到目標(biāo)位姿的運(yùn)動過程未發(fā)生碰撞，返回初始位姿的過程發(fā)生碰撞；(3)以上兩種碰撞都發(fā)生了，即初始位姿到目標(biāo)位姿的運(yùn)動過程和返回過程都發(fā)生碰撞； 步驟三計算肘滾動角的“臨界值”，井根據(jù)該臨界值調(diào)整肘滾動角的軌跡，避免發(fā)生碰撞 針對發(fā)生碰撞的情況，在碰撞的時刻使機(jī)械臂的肘部適當(dāng)?shù)膹澢敝羷偤貌慌c桌面發(fā)生碰撞，即在碰撞時刻對造成機(jī)械臂與桌面碰撞影響最顯著的位于肘部的肘滾動角的值進(jìn)行修改，使得修改后得到的肘滾動角的“臨界值”，結(jié)合肩俯仰角、肩滾動角、肩偏擺角、肘偏擺角、腕滾動角和腕俯仰角的值形成的機(jī)械臂的構(gòu)型，剛好不與桌面碰撞，肘滾動角臨界值的求解步驟第一歩，根據(jù)碰撞時刻肩俯仰角、肩滾動角、肩偏擺角、肘偏擺角、腕滾動角和腕俯仰角的值以及假設(shè)ー個肘滾動角的初始值，計算機(jī)械臂的正運(yùn)動學(xué)即可得到肘關(guān)節(jié)方向向量的兩個頂點A、B及機(jī)械臂上的一個檢測點C的坐標(biāo)分別為A ( ッ1, ) ,B ( , , )，C ( , , ),并計算此時肘關(guān)節(jié)的方向向量AB ;第二歩，計算過點C且垂直于肘關(guān)節(jié)方向向量 AB 的平面 L，可得其方程為(x-x3Xx2 — ^i)+ (Ji-J3)O72 +—ろ)=O;第三歩，計算肘關(guān)節(jié)方向向量AB與上一歩得到的平面L的交點O (￥M),以此交點O為球心，以點O到點C的距離||OC|為半徑的球面方程Ψ，其表達(dá)式為￠^- )2 +(y_ Jo)2 +(β-ζ0)2 = ( — xO)2 + 0 — JjO)2 + (Z3 - z0.y ;第四步，求該球面 Ψ、第二步的平面L以及桌面的交點D ( , , );第五步,計算ZCOD ,并用右手定則判斷ZCDD正負(fù)得到的值設(shè)為,然后計算機(jī)械臂與桌面發(fā)生碰撞時肘滾動角的值，設(shè)為04&31 ゴ，^llJqAbound = 90+Ag4，最后在基礎(chǔ)上增加一定余量，即可得到肘滾動角的臨界值； 取該碰撞時刻和該時刻肘滾動角的臨界值作為ー個中間狀態(tài)，以及機(jī)械臂在初始時刻的肘滾動角為其初始狀態(tài)和在目標(biāo)時刻的肘滾動角為目標(biāo)狀態(tài)，將肘滾動角的軌跡進(jìn)行分段生成，則如果發(fā)生第(I)種碰撞，三段軌跡分別為初始狀態(tài)到中間狀態(tài)，中間狀態(tài)到目標(biāo)狀態(tài)，目標(biāo)狀態(tài)返回初始狀態(tài)；如果發(fā)生第(2)種碰撞,三段軌跡為初始狀態(tài)到目標(biāo)狀態(tài)，目標(biāo)狀態(tài)到中間狀態(tài)，中間狀態(tài)返回初始狀態(tài)；如果發(fā)生第(3)種碰撞，說明機(jī)械臂運(yùn)動過程中，共需要處理與桌面發(fā)生的兩次碰撞，三段軌跡 為初始狀態(tài)到第一次避碰的中間狀態(tài)，第一次碰撞的中間狀態(tài)到第二次碰撞的中間狀態(tài)，第二次碰撞的中間狀態(tài)返回初始狀態(tài)。
全文摘要
本發(fā)明提出了一種仿人機(jī)器人機(jī)械臂避碰運(yùn)動軌跡規(guī)劃方法，現(xiàn)有技術(shù)沒有從根本上解決碰撞問題，本發(fā)明采用基于關(guān)節(jié)空間插補(bǔ)方法生成機(jī)械臂運(yùn)動軌跡所帶來的機(jī)械臂與桌子碰撞的問題，主要針對仿人機(jī)器人的擬人手臂面向桌面或類似桌面物體的作業(yè)，其中機(jī)械臂的自由度分布為肩部三個自由度，肘部兩個自由度，腕部兩個自由度。具體實現(xiàn)方法是，在能夠保證固定時刻完成目標(biāo)作業(yè)的前提下，在手臂運(yùn)動過程中發(fā)生碰撞的時刻點，讓肘部適當(dāng)彎曲，避免碰撞。這種方案使機(jī)械臂實現(xiàn)了在具有較高的靈活性和較大作業(yè)空間的同時，無碰撞的運(yùn)動。
文檔編號G06F17/50GK102646148SQ20121012378
公開日2012年8月22日 申請日期2012年4月25日 優(yōu)先權(quán)日2012年4月25日
發(fā)明者吳俊 , 張斌, 熊蓉, 黃秋蘭 申請人:浙江大學(xué)