[發明專利]一種基于二分法的平面冗余機械臂碰撞位置檢測方法

有效

• 申請號: 202011018189.9
• 申請日: 2020-09-24
• 公開（公告）號: CN112157678B
• 公開（公告）日: 2021-10-22
• 發明（設計）人: 張建華;徐永強;趙巖;李輝;劉璇;劉延超
• 申請（專利權）人: 河北工業大學
• 主分類號: B25J19/02
• 分類號: B25J19/02;B25J19/00;B25J9/16
• 代理公司: 天津翰林知識產權代理事務所(普通合伙) 12210
• 代理人: 王瑞;張國榮

1.一種基于二分法的平面冗余機械臂碰撞位置檢測方法，其特征在于，該方法包括以下步驟：

步驟一、搭建平面冗余機械臂碰撞位置檢測系統，構建平面冗余機械臂的動力學模型，并判斷平面冗余機械臂與障礙物是否發生碰撞；

步驟二、當平面冗余機械臂與障礙物發生碰撞時，設定平面冗余機械臂的第m根臂桿與障礙物發生碰撞，發生碰撞后上位機控制第m-2個關節、第m-1個關節和第m個關節繼續轉動，使第m根臂桿繞其臂桿上的二分點從碰撞位置朝向理想位置旋轉；設定第m根臂桿期望繞二分點旋轉的次數為n，在平面冗余機械臂旋轉過程中，首先第m根臂桿繞其臂桿二分點做首次旋轉運動至一號理想位置，第m根臂桿始終以上一次旋轉運動檢測出的碰撞位置所在區間的兩端處的兩個二分點的中點為下一次旋轉運動的轉動中心繼續作旋轉運動，進一步檢測碰撞位置所在區間，每次旋轉運動都將碰撞位置所在區間縮小，直到第m根臂桿完成第n次旋轉運動，得到碰撞位置，碰撞位置位于第n次旋轉運動確定的區間的中點；二分點指第m根臂桿上由第m+1關節指向第m個關節方向上的位于臂桿長度(k為不大于2ⁿ的自然數)處的點；具體過程為：

2-1在第m根臂桿繞其臂桿二分點作首次旋轉運動至一號理想位置過程中，上位機監測前m個關節處的電機理論工作電流與實際工作電流的差值是否超過閾值；

當第m根臂桿繞其臂桿二分點逆時針旋轉時，若上位機監測前m個關節處的電機理論工作電流與實際工作電流的差值超過閾值，則表示碰撞位置位于第m根臂桿二分點靠近第m+1個關節的一側，取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心，繼續執行步驟2-2；若未超過閾值，則表示碰撞位置位于第m根臂桿二分點遠離第m+1個關節的一側，取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心，繼續執行步驟2-3；

當第m根臂桿繞其臂桿二分點順時針旋轉時，若上位機監測前m個關節處的電機理論工作電流與實際工作電流的差值超過閾值，表示碰撞位置位于第m根臂桿二分點遠離第m+1個關節的一側，取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心，繼續執行步驟2-3；若未超過閾值，則表示碰撞位置位于第m根臂桿二分點靠近第m+1個關節的一側，取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心，繼續執行步驟2-2；

2-2上位機控制平面冗余機械臂的第m-2個關節、第m-1個關節和第m個關節從一號理想位置恢復至碰撞位置時的關節角度，然后控制平面冗余機械臂的第m-2個關節、第m-1個關節和第m個關節繼續轉動，使第m根臂桿繞其臂桿二分點作第二次旋轉運動至二號理想位置；

當第m根臂桿繞其臂桿二分點逆時針旋轉時，若上位機監測前m個關節處的電機理論工作電流與實際工作電流的差值超過閾值，表示碰撞位置位于第m根臂桿二分點靠近第m+1個關節的一側，則取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心繼續旋轉；若未超過閾值，則表示碰撞位置位于第m根臂桿二分點遠離第m+1個關節的一側，取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心繼續旋轉；

當第m根臂桿繞其臂桿二分點順時針旋轉時，若上位機監測前m個關節處的電機理論工作電流與實際工作電流的差值超過閾值，表示碰撞位置位于第m根臂桿二分點遠離第m+1個關節的一側，取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心繼續旋轉；若未超過閾值，表示碰撞位置位于第m根臂桿二分點靠近第m+1個關節的一側，則取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心繼續旋轉；

2-3上位機控制平面冗余機械臂的第m-2個關節、第m-1個關節和第m個關節從一號理想位置恢復至碰撞位置時的關節角度，然后控制平面冗余機械臂的第m-2個關節、第m-1個關節和第m個關節轉動，使第m根臂桿繞其臂桿二分點作第二次旋轉運動至三號理想位置；

當第m根臂桿繞其臂桿二分點逆時針旋轉時，若上位機監測前m個關節處的電機理論工作電流與實際工作電流的差值超過閾值，表示碰撞位置位于第m根臂桿二分點靠近第m+1個關節的一側，則取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心繼續旋轉；若未超過閾值，則表示碰撞位置位于第m根臂桿二分點遠離第m+1個關節的一側，此時取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心繼續旋轉；

當第m根臂桿繞其臂桿二分點順時針旋轉時，若上位機監測前m個關節處的電機理論工作電流與實際工作電流的差值超過閾值，表示碰撞位置位于第m根臂桿二分點遠離第m+1個關節的一側，此時取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心繼續旋轉；若未超過閾值，表示碰撞位置位于第m根臂桿二分點靠近第m+1個關節的一側，則取第m根臂桿二分點與第m根臂桿二分點的中點作為轉動中心繼續旋轉；

重復執行上述操作，第m根臂桿始終以與上一次旋轉運動檢測出的碰撞位置所在區間的兩端處的兩個二分點的中點為下一次旋轉運動的轉動中心繼續作旋轉運動，進一步檢測碰撞位置所在區間，每次旋轉運動都將碰撞位置所在區間縮小，直到第m根臂桿完成第n次旋轉運動，得到碰撞位置，碰撞位置為第n次旋轉運動確定的區間的中點。