一種連續型機器人空間路徑跟蹤方法

2023-06-22 22:43:46 5

一種連續型機器人空間路徑跟蹤方法
【專利摘要】一種連續型機器人空間路徑跟蹤方法。其包括初始化、判斷路徑起點是否在原點上方、蛇臂上升至路徑起點、蛇臂直接彎曲、蛇臂前進一步、求解蛇臂末節關節段變量、更新蛇臂姿態、判斷路徑跟蹤是否等階段。本發明提供的連續型機器人空間路徑跟蹤方法能夠實現對空間任意給定路徑的跟蹤；採用這種方法能夠實現在整個跟蹤過程中以路徑作為參考，機器人整體形態始終與路徑一致，行進覆蓋的範圍較小，安全性高。
【專利說明】一種連續型機器人空間路徑跟蹤方法
【技術領域】
[0001]本發明屬於自動控制【技術領域】，特別是涉及一種連續型機器人空間路徑跟蹤方法。
【背景技術】
[0002]傳統的離散型機器人(如串、並聯機器人)均採用剛性關節和連杆結構，用於實現在自由空間內的多自由度運動，但由於其一般只具有5?7個自由度，自由度數目有限，因此對工作空間受限的環境適應性不強；與離散型機器人不同，連續型機器人為「無脊椎」的柔性結構，機器人採用形狀可以靈活改變的結構，而不具有任何剛性的關節和連杆；這種新型的仿生機器人具有良好的彎曲性能，可以柔順而靈活地改變自身的形狀，其優良的彎曲特性甚至可以和蛇體、象鼻子以及章魚觸角等生物器官媲美；由於連續型機器人的外形可以靈活改變，因此具有根據環境障礙物的狀況而改變自身形狀的能力，對工作空間受限的環境具有獨特的適應能力；其應用前景廣闊，可以應用於飛機油箱檢查、多障礙物工業環境內的作業、彎曲管道和塌陷建築物內的偵查和搜救、核電站內部管路的維護、人體疾病的診療等場合。
[0003]連續型機器人為由多個關節段構成的串聯機器人，其關節段具有結構上的約束，能進行空間的彎曲和旋轉運動；在空間結構約束很強的環境中，為避免機器人觸碰造成損傷或潛在危險，需要對機器人到達目標區域的行進路徑進行規劃；路徑為由若干個類似連續型關節的曲線構成，滿足其結構約束，是連續型機器人到達目標區域的最終姿態。
[0004]在這種技術背景下，目前仍缺少簡潔有效的能夠對連續型機器人空間路徑進行跟蹤的方法。

【發明內容】

[0005]為了解決上述問題，本發明的目的在於提供一種連續型機器人空間路徑跟蹤方法，實現機器人跟蹤規劃的三維空間路徑，最終到達目標區域，並且儘量減小行進的覆蓋範圍。
[0006]為了達到上述目的，本發明提供的連續型機器人空間路徑跟蹤方法包括按順序執行的下列步驟:
[0007]步驟一、初始化的SOl階段:進行蛇臂位置和姿態初始化，使蛇臂處於垂直狀態，其末端點到達系統坐標系{S}原點Os ；
[0008]步驟二、判斷路徑起點是否在原點上方的S02階段:判斷路徑起點是否處在系統坐標系原點Os的上方，若判斷結果為「是」，則進入下一步S03階段，否則下一步進入S04階段；
[0009]步驟三、蛇臂上升至路徑起點的S03階段:在路徑起點處於系統坐標系{S}原點Os上方時，則末段蛇臂直線前進至路徑起點，然後下一步進入S05階段；
[0010]步驟四、蛇臂直接彎曲的S04階段:路徑起點若處於系統坐標系{S}原點Os下方，則按照S06階段方法求解關節段變量，並控制末段關節段進行彎曲，然後下一步進入S05階段；
[0011]步驟五、蛇臂前進一步的S05階段；蛇臂基座前進一個步長；
[0012]步驟六、求解蛇臂末節關節段變量的S06階段:採用基於已知點的搜索算法計算末節關節段變量，並記錄；
[0013]步驟七、更新蛇臂姿態的S07階段:蛇臂各個關節段按照關節段變量進行更新姿態，蛇臂末節採用S06階段求得的關節段變量，其它各關節依次採用蛇臂末節關節段經過此位置時記錄的關節段變量；
[0014]步驟八、判斷路徑跟蹤是否完成的S08階段:判斷蛇臂末端是否到達路徑末端，如果判斷結果為「是」，則整個流程至此結束，否則下一步重新進入S05階段，繼續跟蹤過程。
[0015]在S06階段中，所述的計算關節段變量的方法為基於已知點的搜索算法求解關節段變量，其包括按順序執行的下列步驟:
[0016]步驟一、將路徑各段均分，求出分點在路徑第一關節段坐標系下坐標的S601階段:
[0017]步驟(a):設關節段長度為L，基座步進的步長為S，則蛇臂完成跟蹤一個路徑段時基座步進次數w為:
【權利要求】
1.一種連續型機器人空間路徑跟蹤方法，其特徵在於:其包括按順序執行的下列步驟: 步驟一、初始化的SOl階段:進行蛇臂位置和姿態初始化，使蛇臂處於垂直狀態，其末端點到達系統坐標系{S}原點Os ； 步驟二、判斷路徑起點是否在原點上方的S02階段:判斷路徑起點是否處在系統坐標系原點Os的上方，若判斷結果為「是」，則進入下一步S03階段，否則下一步進入S04階段；步驟三、蛇臂上升至路徑起點的S03階段:在路徑起點處於系統坐標系{S}原點Os上方時，則末段蛇臂直線 前進至路徑起點，然後下一步進入S05階段； 步驟四、蛇臂直接彎曲的S04階段:路徑起點若處於系統坐標系{S}原點Os下方，則按照S06階段方法求解關節段變量，並控制末段關節段進行彎曲，然後下一步進入S05階段；步驟五、蛇臂前進一步的S05階段；蛇臂基座前進一個步長； 步驟六、求解蛇臂末節關節段變量的S06階段:採用基於已知點的搜索算法計算末節關節段變量，並記錄； 步驟七、更新蛇臂姿態的S07階段:蛇臂各個關節段按照關節段變量進行更新姿態，蛇臂末節採用S06階段求得的關節段變量，其它各關節依次採用蛇臂末節關節段經過此位置時記錄的關節段變量； 步驟八、判斷路徑跟蹤是否完成的S08階段:判斷蛇臂末端是否到達路徑末端，如果判斷結果為「是」，則整個流程至此結束，否則下一步重新進入S05階段，繼續跟蹤過程。
2.根據權利要求1所述的連續型機器人空間路徑跟蹤方法，其特徵在於:在S06階段中，所述的計算關節段變量的方法為基於已知點的搜索算法求解關節段變量，其包括按順序執行的下列步驟: 步驟一、將路徑各段均分，求出分點在路徑第一關節段坐標系下坐標的S601階段:步驟(a):設關節段長度為L，基座步進的步長為S，則蛇臂完成跟蹤一個路徑段時基座步進次數w為:
L
Mi ——
S 步驟(b):將路徑各段均分為w份，在各路徑段坐標系中求解出分點坐標； 步驟(C):求解路徑分點在第一關節段坐標系下的坐標，即將求解出分點坐標，再經過齊次變換轉化到第一關節段坐標系下坐標； 步驟二、求解蛇臂末端關節段旋轉角度的S602階段: 每次基座步進後，以對應的路徑分點為蛇臂末端點要達到的目標點，此點與蛇臂末端關節段構成一個平面，由此確定旋轉角度& 步驟三、求解蛇臂末端關節的彎曲角度的S603階段: 採用數值算法求解彎曲角度I,:按照規定步長遍歷彎曲角度，每次求出蛇臂末端點坐標，並求解末端點到路徑的距離，選取對應最小距離的彎曲角度。
3.根據權利要求1所述的連續型機器人空間路徑跟蹤方法，其特徵在於:在S07階段中，在所述的更新姿態時，記錄每次跟蹤時蛇臂末端關節段的關節段變量，其與路徑位置對應，當其餘關節段末端到達此位置時，關節段變量相同，直接載入數據即可，不需重複求解，相當於其它關節段跟隨末端關節段的軌跡前進。
4.根據權利要求2所述的連續型機器人空間路徑跟蹤方法，其特徵在於:在S601階段的步驟(C)中，所述的求解路徑分點在第一關節段坐標系下的坐標的方法如下: 路徑關節段的坐標係為{Bg} (g=0，l，2，…，n)，路徑中第m段的關節段變量為(0Pm^J{m = \2,3,-,n);設路徑第m段第k個分點為Cm，k，則該點對應的彎曲角度為，可表不為:
5.根據權利要求2所述的連續型機器人空間路徑跟蹤方法，其特徵在於:在S602階段中，所述的求解蛇臂末端關節段旋轉角度的方法如下: 蛇臂第N段起始端處坐標係為{AN_J，在此坐標系中，以路徑分點Cm, k為目標點，能夠確定一個過Cm，k點和^^軸的平面；蛇臂關節段N在此平面內，平面與軸的夾角即為旋轉角度％，若已知Cm，k在坐標系{AnJ中坐標，％則容易求解，設坐標為
6.根據權利要求2所述的連續型機器人空間路徑跟蹤方法，其特徵在於:在S603階段中，所述的求解蛇臂末端關節的彎曲角度的方法如下: 在平面內採用數值算法求解末端關節的彎曲角度；具體步驟如下: (I)求蛇臂末端點坐標 彎曲角度取值範圍IeWM,由於計算過程中％處於分母上，故當時取較小值代替；設該角度步進的步長為Λ Θ，則第i次步進後蛇臂末端關節的彎曲角度為:
【文檔編號】B25J9/16GK103692440SQ201310662862
【公開日】2014年4月2日 申請日期:2013年12月6日 優先權日:2013年12月6日
【發明者】牛國臣, 王力, 高慶吉, 胡丹丹 申請人:中國民航大學