[發明專利]基于手勢識別的機械臂位姿控制系統

有效

• 申請號: 201711088712.3
• 申請日: 2017-11-08
• 公開（公告）號: CN107943283B
• 公開（公告）日: 2021-02-02
• 發明（設計）人: 禹鑫燚;林美新;歐林林;朱峰;張強
• 申請（專利權）人: 浙江工業大學
• 主分類號: G06F3/01
• 分類號: G06F3/01;B25J9/16;B25J9/00;B25J13/08;H04L29/08;H04W4/80;G08C17/02
• 代理公司: 杭州天正專利事務所有限公司 33201
• 代理人: 王兵;黃美娟

1.一種基于手勢識別的機械臂位姿控制系統，其特征在于：包括依次連接的智能腕帶模塊、遠程客戶端模塊、藍牙通信模塊、數據處理模塊、仿真模塊和機械臂執行模塊；智能腕帶通過藍牙無線連接遠程客戶端，將智能腕帶模塊采集的肌電信號傳遞給遠程客戶端；遠程客戶端接收到信號后，將信號傳輸到數據處理模塊，在數據處理模塊中，對信號進行濾波降噪處理，處理后將手勢分類；然后將手勢動作的信號發送給仿真模塊中的仿真機械臂模型；將仿真機械臂的末端位置傳遞給仿真模塊中的仿真機械臂模型；仿真模塊中的仿真機械臂模型將角度信號作為指令發送給機械臂執行模塊；

使用智能腕帶進行信號采集，將采集到的信息進行處理，并且智能腕帶通過藍牙適配器與遠程客戶端進行無線連接，遠程客戶端與工作機械臂通過TCP/IP協議連接；

各模塊的具體構成是：

智能腕帶模塊：該模塊通過藍牙模塊連接遠程客戶端，并且該模塊是可穿戴的，智能腕帶模塊為無線智能腕帶，其包含九軸慣性測量單元，三軸陀螺儀，三軸加速度計和三軸磁力計，進行陀螺儀信號的采集；智能腕帶還有數塊大小厚薄不一的生物電傳感器單元，每個傳感器又分為若干個電極，通過這電極就捕捉操作者手臂肌肉運動時產生的肌電信號；

藍牙通信模塊：藍牙模塊一端連接智能腕帶模塊另一端通過藍牙的無線傳輸功能實現與遠程客戶端的無線連接；藍牙通信模塊通過無線連接實現藍牙模塊與智能腕帶的雙向通信；藍牙模塊通過接收遠程客戶端傳輸過來的數據和指令信號，并將其發送給智能腕帶模塊；

遠程客戶端模塊：遠程客戶端模塊是基于Linux進行開發的，一端通過藍牙通信模塊連接智能腕帶模塊，另一端連接數據處理模塊；遠程客戶端模塊通過藍牙通信模塊接收智能腕帶模塊采集到的肌電信號和陀螺儀信號數據，再將肌電信號和陀螺儀信號傳輸給數據處理模塊；

數據處理模塊：一端連接遠程客戶端，另一端連接仿真模塊；數據處理模塊接收來自遠程客戶端模塊中的肌電信號和陀螺儀信號，將接收到的信號與數據通過卡爾曼濾波、低通濾波和正逆運動學求解計算出操作者手臂的關節角度，同時對于可疑的數據進行剔除，將范圍內的關節角度數據傳遞給仿真模塊下的仿真機械臂使其做出與人體相同的動作；數據處理模塊利用正逆運動學求解計算出操作者手臂的關節角度的具體方式如下：

數據處理模塊接收到信號與數據后：

3.1)首先對仿真機械臂構建其D-H參數，即關節1≤i≤6的連桿長度a_i、連桿轉角α_i、連桿偏距d_i和關節角度θ_i作為輸入量，選擇工作機械臂各個關節的關節角度θ_i作為輸出量；

3.2)由于各桿件相對參考坐標系有轉動和平移兩個動作，因此對每個桿件沿關節軸建立一個關節坐標系，用以下四個參數來描述桿件：

(1)連桿長度a_i：關節軸i和關節軸i+1之間公垂線的長度；

(2)連桿轉角α_i：作一個與兩關節軸之間的公垂線垂直的平面，將關節軸i和關節軸i+1投影到該平面，在平面內軸i按右手法則繞a_i轉向軸i+1，其轉角即為連桿轉角；

(3)連桿偏距d_i：公垂線a_i-1與關節軸i的交點到公垂線a_i與關節軸i+1的交點的有向距離長度；

(4)關節角度θ_i：a_i-1的延長線與a_i之間繞關節軸i旋轉所形成的夾角角度，即采集得到的仿真機械臂關節角度；

3.3)以基座坐標系作為T₀坐標系，表示i關節坐標系相對i-1關節坐標系的位置和姿態，則：

其中，sθ_i＝sinθ_i，cθ_i＝cosθ_i，sα_i＝sinα_i，cα_i＝cosα_i；

3.4)對于六自由度機械臂，獲得工作機械臂末端執行器位姿相對固定參考坐標系的變換矩陣，即工作機械臂末端位置點坐標的變換矩陣：

其中，表示工作機械臂末端執行器位姿相對于基軸參考坐標系的變換矩陣，r_mn表示變換矩陣中第m行第n列的參數，p_x表示x軸的參數；p_y表示y軸的參數；p_z表示z軸的參數；

3.5)通過兩個腕帶獲取關節角度的方法：

建立世界坐標系(x_G,y_G,z_G)以及建立操作者手臂的上臂坐標系(x_H,y_H,z_H)，前臂坐標系(x_F,y_F,z_F)；R代表旋轉矩陣；上標i代表初始位置；下標G、H、F分別代表世界坐標系框架，上臂框架以及前臂框架；相對于第一個智能腕帶的坐標系和相對于第二個智能腕帶對應的下標的坐標系可以表示為：

其中下標“U”表示佩戴在上臂上的第一個MYO臂章框架，下標“L”表示穿在下臂上的第二個MYO臂章框架；當操作者手臂做出一個新動作之后，上臂和前臂在世界坐標系的框架下可以用下面的旋轉矩陣來描述：

上標f代表操作者手臂的一個新動作；通過第一個腕帶的陀螺儀，可以得到一個四元數q＝[x,y,z,w]^T，可由以下方程求得：

q＝xi+yj+zk+w (3)

其中(x,y,z)是標量，表示各軸方向，w是矢量，i，j，k分別為各自參數值；

通過上面等式(3)中的四元數，上臂在全局框架中可以表示為：

其中代表上臂相對于全局的坐標系，通過(5)計算出歐拉角α,β,γ，分別代表肩部的三個角度，α代表偏航角，β代表俯仰角，γ代表滾動角；Atan2為反正切函數；s為sin的縮寫，sβ即sinβ；

根據兩個智能腕帶測到的數據，可以計算出兩個肘部關節的角度，A_fe代表肘部彎曲的角度，A_ps代表肘部滾動的角度，計算公式如下：

A_fe＝arccos(a₁₂r₁₃+a₂₂r₂₃+a₃₂r₃₃) (6)

A_ps＝arccos(r₁₁a₁₁+r₂₁a₂₁+r₃₁a₃₃) (7)

經過濾波處理以及角度判斷之后將角度值α,β,γ，A_fe，A_ps發送給仿真模塊中的仿真機械臂，通過計算得出手腕的末端位置p(x,y,z)；得到末端位置之后通過逆運動學求解出工作機械臂的六個關節角度，則可按順序求解得到：

θ₁表示第一個關節角度參數，求解方法為：

由第一個關節角度，通過中間運算公式可得第二個和第三個關節角度為：

其中，a₂為連桿1的長度，d₄為連桿4的偏距，c₃是cosθ₃的縮寫，s₃是sinθ₃的縮寫，K為逆運動學求解時的固定常量；

以此類推可以求出第四關節角度θ₄，并通過c₅和s₅求出θ₅：

θ₄＝Atan2(-r₁₃s₁+r₂₃c₁,-r₁₃c₁c₂₃-r₂₃s₁c₂₃+r₃₃s₂₃) (10)

其中，c₂₃＝c₂c₃-s₂s₃，s₂₃＝c₂s₃+s₂c₃；r₁₃表示變換矩陣中第一行第三列的參數值；

同理可以通過求解c₆和s₆，求出θ₆：

其中，s_i＝sinθ_i，c_i＝cosθ_i；

在每個關節角度求出后，接著判斷每個關節角度是不是在限定范圍內，將各關節角度發送到仿真模塊中的仿真機械臂進行仿真；

仿真模塊：一端與數據處理模塊相連接，另一端通過TCP/IP協議與執行機械臂相連接；模擬工作機械臂的工作執行情況；仿真模塊通過網絡通信接收數據處理模塊的數據，再將接收到的數據傳輸到仿真模塊中的仿真機械臂中，使仿真模塊中的仿真機械臂模擬出操作者手臂的動作；再通過TCP/IP協議，向機械臂執行模塊發送仿真的數據包；

機械臂執行模塊：通過TCP/IP協議與仿真模塊相連接，通過接收仿真模塊的數據包使工作機械臂完成與仿真機械臂相同的動作。