基于四元數姿態算法的機械手臂系統開發*

閩南理工學院 陶慶鳳

虛擬交互新技術正在進行快速的發展，傳統使用人機交互的輸入設備如手柄、鼠標和鍵盤等已經滿足不了人們生產生活的需求，所以人們急需一種更加符合人體動力學的方式來實現全新的人機交互。手勢控制器輸入法是一種全新的人機交互方式，具有生動、直觀和形象的特點，可以給操作者帶來非常自然的人機交互體驗，在促進虛擬現實體驗的臨場感種具有很重要的意義。在上述背景下，本項目設計出一個基于四元數姿態算法的機械手臂系統，來滿足當今虛擬現實技術的交互需求。

1 開發背景

科學技術以及社會經濟快速發展，促進虛擬現實技術逐漸向各行各業滲透，并因此衍生出了很多新的高科技的研究領域。而在此當中，虛擬交互行業是一個綜合性的領域，它代表著時代的需求，并集合了多種新技術、新算法于一體，它的研究方向包含多學科交叉的內容，比如人機交互的技術、計算機圖形的技術、人工智能技術、信息技術、傳感技術等。

為了促進虛擬交互技術的發展，國家先后出臺了多項政策與規劃措施，保障行業朝著健康有序的方向發展，所以未來虛擬交互行業一定會迎來蓬勃發展的階段。

2 基于互補濾波器的四元數姿態解算方法

在系統開發過程中使用的是標準化的四元數，它的三角函數式可以表示為：，通過該表達式可以計算剛體旋轉運動的數據，表達式中代表旋轉的瞬軸，?代表剛體旋轉的角度[1]。

通常慣性導航過程中，都具有初始狀態時導航坐標系m和載體坐標系a重合的現象，且載體坐標系a是動態坐標系，也就是上述四元數的矢量變換中提到的OX'Y'Z'這個新坐標系，而導航坐標系m對應上述四元數的矢量變換中提到的OXYZ這個舊坐標系。假設a坐標系下有定坐標(xa，ya，za)在m坐標系中的投影是(xm，ym，zm)，將其代入到矩陣表達式中就得出姿態矩陣的表達式為：

四元數的微分方程可以用如下方程式表達：

把表達式寫成矩陣的形式如下：

求解這個四元數的微分方程式，可以得出四元數p0、p1、p2、p3。

3 傳感器的設計

3.1 人體手部骨骼關節分析

人類生產生活中、和大自然的接觸中，手是極其重要的媒介，手部動作有著很高的精度和靈敏度。手部結構也相當復雜，手的結構是由17個關節以及27塊骨骼所組成的，如圖1所示，它相當于一個多關節組成的運動系統。手部結構的自由度等于每根手指的自由度相加，單手一共是二十四個自由度，包括小指、無名指、中指和食指等四指各指頭四個自由度，以及拇指上的五個自由度，除此之外還有手腕的曲度、外展、手掌的弧度等3個自由度。

圖1 人體手部結構圖Fig.1 Structure of the human hand

除拇指外，其余4指的運動狀態都是相似的，例如食指，當它做出伸展的動作時，食指的指掌部位的關節可以完成環轉、彎曲伸展、側向擺動等動作。而拇指的結構跟其余四指比就相對復雜，當拇指做出伸展的動作時，它的指掌關節、遠指關節可以完成彎曲伸展的動作，手掌腕部的關節可以完成對掌、彎曲伸展以及環轉等動作。

3.2 傳感器的布局

在機械手臂的開發中，一個非常重要的事情就是傳感器的布局，優良的傳感器布局方法不但可以確保實現系統的基本功能，還可以進一步精簡系統的內容以及提高系統穩定性、安全性[2]。

在人體手部骨骼分析中，知道了人體手部結構圖的關節特點、自由度以及運動過程。由此可知食指能夠靈活地做出彎曲伸展的活動，依賴于各個關節以及指骨的相互配合，整個動作過程如下：首先根關節也就是指掌關節，它是食指活動的起點，有著指揮官的作用；其次近指關節屬于根關節底下的子關節，作用是連接遠指和指掌2個關節；最后遠指關節也是子關節底下的次子關節，它是最靈敏的一個關節。如圖2所示可以看出遠指骨依賴于遠指關節來活動，近指骨依賴于指掌關節來進行彎曲伸縮活動，而中指骨可以圍繞著近指關節展開相關活動。在運動過程中，近指骨和中指骨圍繞著指掌和近指關節產生的動作是由人的意識來決定的，是屬于主動形式的動作，而遠指骨則相反，遠指骨不是獨立產生的動作，它是附屬遠指關節的動作，是人的意識無法操控的，也就是在沒有外力的時候，它不能自己獨立產生動作，一旦遠指骨運動必然會引起中指骨也做出相應的動作[3]。

圖2 食指活動的各個關節Fig.2 The joints of the index finger

遠指骨和中指骨之間動作互相影響、互相帶動，這種關系可以用一組數學表達式來體現，表達式如下所示：

通過表達式（5）我們知道，遠指骨的活動狀態可以依據中指骨的活動狀態推算出來，那么對于食指來說，我們可以在近指骨以及中指骨的地方分別布局一個傳感器，用來測量近指骨與中指骨活動產生動作的數據，得出相關數據之后，再使用表達式（5）來得出遠指骨活動產生的數據，至此整個食指的活動狀態的數據都可以完整獲取。而拇指數據的獲取，一般來說我們只需要采集近指骨與遠指骨活動產生的數據，所以在近指骨與遠指骨的地方分別布局一個傳感器，來獲取相關數據即可。腕關節是整個手掌的底部，要獲取其數據，只需在手背的地方布局一個傳感器即可。

綜上可以得出整個手部結構傳感器設置的方案：先在手背上布局1個傳感器的節點，然后在四指的中指骨與近指骨的地方各布局1個節點傳感器，最后是大拇指，在其近指骨處與遠指骨處各布局1個節點傳感器，該布局設計能夠完整地計算出手部的運動數據。

3.3 微處理器設計

傳感器的節點是設置在機械手臂相關骨骼上的，是用來計算骨骼運動狀態的設備，其構成主要由磁力計、陀螺儀、加速度計以及微處理器組成。要符合安全流暢的運動狀態，必須要將傳感器的節點大小設計成微型化，所以設計過程采用的是微處理器，它是整個傳感器最核心的器件，在工作時通過IIC的總線來讀取磁力計、加速度計以及陀螺儀的原始數據，然后經過數據融合的操作得到機械手臂的姿態信息，再把該姿態信息的數據發送給CAN總線，最后實現數據采集以及數據處理與數據傳輸等三大功能。

4 系統軟件設計

4.1 系統開發的部分代碼[4]

4.2 系統操作界面

有兩個主要的操作模式界面，分別為滾動條控制界面和按鈕點擊控制界面，可根據用戶的實際需求來進行切換。在進入操作界面前需打開手機藍牙，進入操作界面后點擊右邊的第二個按鈕使得手機藍牙與手勢控制器的藍牙配對，配對之前藍牙的初始值為紅色停止狀態的圖標，配對成功后紅色藍牙圖標變成了藍色工作狀態中的圖標，此時方可進行有效手勢控制器的操作，系統開發過程的主要界面如圖3、圖4所示。

圖3 滾動條操作界面Fig.3 Scroll bar operation interface

圖4 按鈕點擊操作界面Fig.4 Button click operation interface

(1)滾動條操作界面

滾動條控制界面：開始工作前五根手指對應的五根滾動條分別處于各滾動條的中間位置，可手動向上或向下調節滾動條的位置。向上是調節對應手指伸展的狀態，向下是調節對應手指彎曲的狀態，右邊第三個按鈕為恢復初始狀態的按鈕，手勢控制器使用完畢后可點此按鈕恢復初始狀態。

(2)按鈕點擊操作界面

按鈕點擊控制界面：開啟時手勢控制器處于初始狀態，分別點擊大拇指、食指、中指、無名指、小指等按鈕，則對應的機械手指開始產生彎曲，點擊持續時間越久彎曲的程度越深。松開按鈕時，相應的機械手指會逐漸伸展開來，調整完畢后可按右邊第三個按鈕使其恢復初始狀態。

5 結語

本系統開發過程中采用了基于互補濾波器的四元數姿態解算方法，該計算方法能精確地得出機械手臂的運動姿態。同時分析了人體手部結構，包括對小指、無名指、中指、食指和大拇指的各個關節以及指骨進行了細致地分析，對機械手臂上傳感器的布局提供了方案。然后對機械手臂的軟件開發部分進行了介紹，該系統對機械手臂進行運動控制，控制過程中具有良好的負載能力和控制精度，能自動調節手勢控制器的握力大小，該控制器的功能可以進行擴展和挖掘，對后續的二次開發提供了良好的延展性。