[發明專利]一種相鄰關節空間-笛卡爾空間軌跡過渡方法及裝置

有效

• 申請號: 202011134624.4
• 申請日: 2020-10-21
• 公開（公告）號: CN112276949B
• 公開（公告）日: 2022-03-11
• 發明（設計）人: 劉鵬飛;郭龍;王權
• 申請（專利權）人: 哈工大機器人（合肥）國際創新研究院
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 合肥市浩智運專利代理事務所(普通合伙) 34124
• 代理人: 丁瑞瑞

1.一種相鄰關節空間-笛卡爾空間軌跡過渡方法，其特征在于，所述方法包括：

步驟一：對各軸在相鄰空間內進行時間同步，使多軸同步時間到達每一個位置；具體為：以笛卡爾空間為基準空間，采用基于速度的S形速度規劃，算出位置運動時間和姿態運動時間，取兩者中最大時間為基準時間，兩者中時間短的做基于基準時間的S速度規劃，實現姿態與位置時間同步；其中，姿態為包含起始點、終點、速度以及加速度的四元數；

步驟二：將當前段軌跡的末速度作為對下一段軌跡的初速度，當前段軌跡的初速度作為上一段軌跡的末速度，進行軌跡規劃，使相鄰空間速度平滑過渡；具體為：當關節空間軌跡的前一條指令為笛卡爾空間軌跡時，把笛卡爾空間軌跡的終點和末速度通過雅可比映射得到笛卡爾空間軌跡對應的關節空間起始點位置和初速度，把對應的關節空間起始點位置和初速度傳給下一條關節空間指令，完成相鄰空間平滑過渡；當笛卡爾空間軌跡的前一條指令為關節空間軌跡時，根據笛卡爾空間軌跡的初速度，經過雅可比映射到關節空間作為關節空間軌跡的末速度，完成相鄰空間平滑過渡；

當關節空間的初速度不合理時，將笛卡爾空間軌跡的初速度向量乘以系數保證速度方向不變，只修改速度大小，通過雅可比矩陣求對應的關節空間末速度，輸入關節空間末速度返回實際可達的關節空間末速度，判斷每個關節空間末速度誤差，當每個實際可達的關節空間末速度與輸入的關節空間末速度誤差大于預設數值時，即為失敗，繼續進行笛卡爾空間軌跡的初速度的迭代修改，修改系數使得系數累加或者累減，當超出最大修改次數，仍然返回失敗時，說明關節空間的初速度無法到達與笛卡爾空間相切的速度大小，對關節空間的初速度進行同樣迭代修改，如果返回失敗，則說明輸入數據不合理，修改位置數據，誤差小于等于預設數值時，表示成功，采用此時修正的關節空間末速度作為關節空間末速度，保證關節空間到笛卡爾空間速度連續；

步驟三：根據軌跡規劃的輸出數據，進行插補運算，以使軌跡連續；其中，輸出數據包括初速度、末速度以及位置。

2.一種相鄰關節空間-笛卡爾空間軌跡過渡裝置，其特征在于，所述裝置包括：

時間同步模塊，用于對各軸在相鄰空間內進行時間同步，使多軸同步時間到達每一個位置；具體為：以笛卡爾空間為基準空間，采用基于速度的S形速度規劃，算出位置運動時間和姿態運動時間，取兩者中最大時間為基準時間，兩者中時間短的做基于基準時間的S速度規劃，實現姿態與位置時間同步；其中，姿態為包含起始點、終點、速度以及加速度的四元數；

軌跡規劃模塊，用于將當前段軌跡的末速度作為對下一段軌跡的初速度，當前段軌跡的初速度作為上一段軌跡的末速度，進行軌跡規劃，使相鄰空間速度平滑過渡；具體為：當關節空間軌跡的前一條指令為笛卡爾空間軌跡時，把笛卡爾空間軌跡的終點和末速度通過雅可比映射得到笛卡爾空間軌跡對應的關節空間起始點位置和初速度，把對應的關節空間起始點位置和初速度傳給下一條關節空間指令，完成相鄰空間平滑過渡；當笛卡爾空間軌跡的前一條指令為關節空間軌跡時，根據笛卡爾空間軌跡的初速度，經過雅可比映射到關節空間作為關節空間軌跡的末速度，完成相鄰空間平滑過渡；

當關節空間的初速度不合理時，將笛卡爾空間軌跡的初速度向量乘以系數保證速度方向不變，只修改速度大小，通過雅可比矩陣求對應的關節空間末速度，輸入關節空間末速度返回實際可達的關節空間末速度，判斷每個關節空間末速度誤差，當每個實際可達的關節空間末速度與輸入的關節空間末速度誤差大于預設數值時，即為失敗，繼續進行笛卡爾空間軌跡的初速度的迭代修改，修改系數使得系數累加或者累減，當超出最大修改次數，仍然返回失敗時，說明關節空間的初速度無法到達與笛卡爾空間相切的速度大小，對關節空間的初速度進行同樣迭代修改，如果返回失敗，則說明輸入數據不合理，修改位置數據，誤差小于等于預設數值時，表示成功，采用此時修正的關節空間末速度作為關節空間末速度，保證關節空間到笛卡爾空間速度連續；

插補運算模塊，用于根據軌跡規劃的輸出數據，進行插補運算，以使軌跡連續；其中，輸出數據包括初速度、末速度以及位置。