[發明專利]一種氣浮臺抓捕試驗機械臂控制方法

有效

• 申請號: 201210504433.1
• 申請日: 2012-11-30
• 公開（公告）號: CN103010491A
• 公開（公告）日: 2013-04-03
• 發明（設計）人: 朱志斌;何英姿;魏春嶺;唐強;張勇智
• 申請（專利權）人: 北京控制工程研究所
• 主分類號: B64G4/00
• 分類號: B64G4/00
• 代理公司: 中國航天科技專利中心 11009
• 代理人: 褚鵬蛟

1.一種氣浮臺抓捕試驗機械臂控制方法，所述機械臂為兩個多自由度機械臂，其特征在于，步驟如下：

(1)根據相對測量敏感器輸出的相對距離確定是否進行預抓捕，當追蹤氣浮臺與目標氣浮臺之間的相對距離小于預抓捕設定距離時，兩個機械臂進行預抓捕，將機械臂伸展到目標氣浮臺大包絡范圍；

(2)采集相對測量敏感器輸出的相對距離和相對姿態信息，如果相對距離和相對姿態在機械臂抓捕有效作用范圍內，則根據相對測量敏感器輸出的相對距離和相對姿態信息計算每個機械臂目標特征抓捕部位坐標；根據每個機械臂目標特征抓捕部位坐標和運動學逆解算法解算每個機械臂各關節目標轉角；根據各關節目標轉角與當前關節轉角確定各個關節期望運動軌跡；然后開始進行第一時間間隔的計時；

(3)對于每個機械臂，通過機械臂關節位置和速度閉環控制各關節沿期望運動軌跡運動；

(4)對于每個機械臂，根據安裝在機械臂作用末端的觸碰傳感器輸出的信號判斷機械臂是否與目標氣浮臺接觸，如果機械臂接觸目標氣浮臺，則轉入步驟(5)，否則判斷是否到達設定的第一時間間隔，如果到達第一時間間隔則轉入步驟(2)，如果沒有到達第一時間間隔則轉入步驟(3)；

(5)根據機械臂作用末端安裝的壓力傳感器輸出的信號進行力閉環控制，使得機械臂目標特征抓捕部位與目標氣浮臺的接觸力恒定。

2.根據權利要求1所述的控制方法，其特征在于，所述多自由度機械臂為三自由度機械臂，該三自由度機械臂包括三個連桿和三個關節，其中第一連桿、第二連桿的長度為l₁，l₂；第三個連桿長度為0；所述步驟(2)中的運動學逆解算法如下：

其中，θ₁，θ₂，θ₃分別為第1關節、第2關節、和第3關節目標轉角；(p_x，p_y)為機械臂目標特征抓捕部位坐標。

3.根據權利要求1所述的控制方法，其特征在于，所述步驟(3)的力閉環控制方法如下：

首先由設定的抓捕末端與目標氣浮臺期望壓力與力傳感器輸出壓力值做差，通過兩次積分及運動學逆解轉換得到期望的關節轉角；

然后由期望的關節轉角與碼盤測角裝置獲得的實際關節轉角做差，并通過第一PID控制器濾波，得到期望的關節角速度；

最后對碼盤測角裝置獲得的實際關節轉角進行差分獲得實際關節角速度，對期望關節角速度與實際關節角速度做差，通過第二PID控制器調整得到機械臂各關節電機的驅動指令；最后通過各關節電機驅動機械臂運動。