[發明專利]一種連續型機器人空間路徑跟蹤方法

有效

• 申請號: 201310662862.6
• 申請日: 2013-12-06
• 公開（公告）號: CN103692440A
• 公開（公告）日: 2014-04-02
• 發明（設計）人: 牛國臣;王力;高慶吉;胡丹丹
• 申請（專利權）人: 中國民航大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 天津才智專利商標代理有限公司 12108
• 代理人: 龐學欣

1.一種連續型機器人空間路徑跟蹤方法，其特征在于：其包括按順序執行的下列步驟：

步驟一、初始化的S01階段：進行蛇臂位置和姿態初始化，使蛇臂處于垂直狀態,其末端點到達系統坐標系{S}原點O_S；

步驟二、判斷路徑起點是否在原點上方的S02階段：判斷路徑起點是否處在系統坐標系原點O_S的上方，若判斷結果為“是”，則進入下一步S03階段，否則下一步進入S04階段；

步驟三、蛇臂上升至路徑起點的S03階段：在路徑起點處于系統坐標系{S}原點O_S上方時，則末段蛇臂直線前進至路徑起點，然后下一步進入S05階段；

步驟四、蛇臂直接彎曲的S04階段：路徑起點若處于系統坐標系{S}原點O_S下方，則按照S06階段方法求解關節段變量，并控制末段關節段進行彎曲，然后下一步進入S05階段；

步驟五、蛇臂前進一步的S05階段；蛇臂基座前進一個步長；

步驟六、求解蛇臂末節關節段變量的S06階段：采用基于已知點的搜索算法計算末節關節段變量，并記錄；

步驟七、更新蛇臂姿態的S07階段：蛇臂各個關節段按照關節段變量進行更新姿態，蛇臂末節采用S06階段求得的關節段變量，其它各關節依次采用蛇臂末節關節段經過此位置時記錄的關節段變量；

步驟八、判斷路徑跟蹤是否完成的S08階段：判斷蛇臂末端是否到達路徑末端，如果判斷結果為“是”，則整個流程至此結束，否則下一步重新進入S05階段，繼續跟蹤過程。

2.根據權利要求1所述的連續型機器人空間路徑跟蹤方法，其特征在于：在S06階段中，所述的計算關節段變量的方法為基于已知點的搜索算法求解關節段變量，其包括按順序執行的下列步驟：

步驟一、將路徑各段均分，求出分點在路徑第一關節段坐標系下坐標的S601階段：

步驟(a):設關節段長度為L，基座步進的步長為s，則蛇臂完成跟蹤一個路徑段時基座步進次數w為：

w=Ls]]>

步驟(b):將路徑各段均分為w份，在各路徑段坐標系中求解出分點坐標；

步驟（c）:求解路徑分點在第一關節段坐標系下的坐標，即將求解出分點坐標，再經過齊次變換轉化到第一關節段坐標系下坐標；

步驟二、求解蛇臂末端關節段旋轉角度的S602階段：

每次基座步進后，以對應的路徑分點為蛇臂末端點要達到的目標點，此點與蛇臂末端關節段構成一個平面，由此確定旋轉角度

步驟三、求解蛇臂末端關節的彎曲角度的S603階段：

采用數值算法求解彎曲角度按照規定步長遍歷彎曲角度，每次求出蛇臂末端點坐標，并求解末端點到路徑的距離，選取對應最小距離的彎曲角度。

3.根據權利要求1所述的連續型機器人空間路徑跟蹤方法，其特征在于：在S07階段中，在所述的更新姿態時，記錄每次跟蹤時蛇臂末端關節段的關節段變量，其與路徑位置對應，當其余關節段末端到達此位置時，關節段變量相同，直接載入數據即可，不需重復求解，相當于其它關節段跟隨末端關節段的軌跡前進。