[發明專利]力控牽引和擺位的多自由度機械臂控制裝置及其控制方法

有效

• 申請號: 201610051244.1
• 申請日: 2016-01-26
• 公開（公告）號: CN105583824B
• 公開（公告）日: 2017-05-24
• 發明（設計）人: 王廣志;李寧;丁輝
• 申請（專利權）人: 清華大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 北京萬象新悅知識產權代理事務所(普通合伙)11360
• 代理人: 王巖

1.一種力控牽引和擺位的多自由度機械臂控制裝置，其特征在于，所述控制裝置包括：上位機、多自由度機械臂系統和六自由度力傳感器系統；其中，所述多自由度機械臂系統包括多自由度機械臂和機械臂控制器；所述六自由度力傳感器系統包括力信號傳感器和力信號控制器；所述上位機包括力信號控制器通訊模塊、末端工具重力補償模塊、力-位及力-速控制算法模塊、位姿坐標解算模塊、機械臂控制模塊和圖形用戶界面模塊；所述上位機分別與機械臂控制器和力信號控制器聯接；所述力信號傳感器具有相對且平行的安裝平面和工具平面，所述安裝平面通過安裝連接件固定在機械臂末端，在工具平面上安裝末端工具，保持末端工具與力信號傳感器之間的相對固定；機械臂末端坐標系的Z軸與力信號傳感器坐標系的Z軸共線且同向，機械臂末端坐標系的Y軸與力信號傳感器坐標系的Y軸平行；外部作用力作用在末端工具上，力信號傳感器檢測到外力，外力包括外部作用力和末端工具的重力，傳輸至力信號控制器；力信號控制器將模擬信號轉化為數字信號后傳輸至上位機；上位機的力信號控制器通訊模塊接收數字信號得到外力的值，末端工具重力補償模塊解算出實際作用在力信號傳感器上的外部作用力沿力信號傳感器坐標系三軸的分力與分力矩；力-位及力-速控制算法模塊將外部作用力的信息轉化為在力信號傳感器坐標系下的機械臂末端的位姿偏移量和速度值；位姿坐標解算模塊將在力信號傳感器坐標系下機械臂末端的位姿偏移量與速度值轉換成在機械臂基坐標系下機械臂末端的位姿偏移量與速度值，并將當前采樣時刻解算得到的位姿偏移量與當前采樣時刻的預期位姿坐標值進行矢量相加，得到機械臂末端下一采樣時刻在機械臂基坐標系下的預期位姿坐標值；機械臂控制模塊按照機械臂末端的下一采樣時刻在機械臂基坐標下的預期位姿坐標值控制機械臂末端運動，帶動末端工具到達預期指定的位姿，實現基于力控牽引和擺位控制。

2.如權利要求1所述的控制裝置，其特征在于，所述安裝連接件為可拆卸連接件，具有相對且平行的機械臂末端安裝面和力信號傳感器安裝面；機械臂末端安裝面與機械臂末端固定并唯一確定安裝連接件的方向；力信號傳感器安裝面與力信號傳感器固定并唯一確定力信號傳感器的安裝方向。

3.如權利要求2所述的控制裝置，其特征在于，所述機械臂末端安裝面的中心具有機械臂末端安裝面定中心凸臺，邊緣設置機械臂末端安裝面定位銷槽；定中心凸臺嵌在法蘭盤的圓環中，法蘭盤定位銷槽與機械臂末端安裝面定位銷槽通過定位銷對齊并固定；力信號傳感器安裝面的中心具有力信號傳感器安裝面定中心凸臺，邊緣設置力信號傳感器安裝面定位銷槽；力信號傳感器安裝面定中心凸臺嵌在力信號傳感器的圓環中，力信號傳感器上的圓環定位銷槽與力信號傳感器安裝面定位銷槽通過定位銷對齊并固定。

4.如權利要求1所述的控制裝置，其特征在于，所述多自由度機械臂系統包括多自由度機械臂和機械臂控制器；多自由度機械臂具有多個移動副或旋轉副組成的串聯結構；機械臂控制器包括狀態檢測模塊和運動控制模塊。

5.如權利要求1所述的控制裝置，其特征在于，所述六自由度力傳感器系統包括能夠檢測六自由度力/力矩分量信息的力信號傳感器以及能夠處理六自由度力/力矩信息的力信號控制器；力信號控制器包括模數轉換模塊和偏置校正模塊。