一種六自由度機械臂的控制系統設計

龍 騰

(長沙理工大學 電氣與信息工程學院，湖南 長沙 410005)

0 引言

機械臂作為工業自動化的一種設備，在整個工業領域有著非常廣泛的應用。實現對機械臂的精準調控是當下研究比較多的課題，也是提高生產效率，改善產品品質的一種方式[1]。針對機械臂的精準調控，人們進行了大量的研究，當下研究比較多的是滕冠[2]等人提出的基于模糊PID控制算法進行機械臂的運動控制，采用角度測量傳感器作為反饋單元，然后將輸入參數和反饋參數進行比較，再傳遞給模糊PID核心算法，實現對機械臂的運動控制。李進生[3]等人提出的機械臂運動軌跡的算法設計，通過分析機械臂的正運動學和逆運動學，然后建立對應的仿真模型進行軌跡設計，實現機械臂的軌跡更優、避障等功能。陳楊洋[4]等人實現了基于總線控制的機械臂控制系統，該系統采用PROFIBUS總線實現對機械臂運動軌跡的在線實時監測。上述方法都能較好地實現對機械臂的運動控制，但是運動精度和響應效率不是那么理想。

針對以上分析，本文選用意法半導體公司的STM32F4作為主控制器，關節姿態信息檢測器選用陀螺儀傳感器，關節的運動控制采用舵機驅動模塊，以上三部分構成了機械臂運動單元的硬件控制系統。機械臂運動單元的軟件部分采用擴展卡爾曼濾波算法，將陀螺儀采集的關節狀態參數作為反饋參數，傳遞給擴展卡爾曼濾波算法模塊，進行預估機械臂的運動狀態，實現對機械臂運動過程中的偏差調控[5-6]，這樣從硬件和軟件兩個方面保證了機械臂控制系統的閉環控制。

1 系統整體設計

圖1所示是整個機械臂控制系統的架構圖，從圖中可以看出，整個控制系統分為4層，分別為用戶層、估計規劃層、關節控制層以及關節狀態檢測層。用戶層主要是用戶輸入想要機械臂實現的運動軌跡；軌跡規劃層主要是根據機械臂的逆運動模型和用戶層的輸入，進行運動軌跡規劃，同時還具有預估機械臂的碰撞狀態；關節控制層主要是根據軌跡規劃層和關節狀態檢測層進行軌跡調控。關節狀態檢測層主要是作為反饋環節，給軌跡規劃層和關節控制層提供關節的當前狀態信息。而關節控制器層的調控，本文選用擴展卡爾曼濾波算法進行估算和調節。

圖1 機械臂控制系統架構圖

2 系統詳細設計

機械臂的控制系統主要包括硬件部分和軟件部分，根據機械臂的設計需求，下面分別從硬件和軟件兩個方面進行闡述。

2.1 硬件設計

圖2所示為機械臂的控制系統硬件構成。用于和用戶交互的液晶顯示部分選用正點原子設計10寸的觸摸屏，主控制器選用ST公司的STM32F4，STM32的時鐘可以達到168 MHz，能夠處理各種復雜的指令，響應速度非常快，舵機驅動模塊主要是實現對機械的運動控制，主控制器通過脈寬調制波(PWM)進行對舵機的調控[7]。陀螺儀主要是作為反饋單元，采集機械臂的運動狀態，給軌跡規劃層和關節控制層提供關節進行決策使用。陀螺儀傳感器和主控制器之間采用I2C接口進行通信，I2C總線的通信主要是通過時鐘線和數據線兩根線進行的，在空閑的時候，時鐘線和數據線都是高電平，當需要進行數據通信時，都是在時鐘的邊沿觸發的。舵機驅動器和主控制器之間采用PWM進行通信，PWM主要是通過調節主控制器輸出的脈沖寬度進行對舵機驅動器的控制。

圖2 機械臂控制系統硬件框圖

2.2 軟件設計

機械臂控制系統的軟件設計主要包括硬件各個模塊(液晶顯示模塊、舵機驅動模塊、陀螺儀數據采集模塊等)的驅動程序設計和EKF算法的設計。軟件設計的主流程圖如圖3所示。

圖3 機械臂控制系統軟件主流程圖

系統主流程首先實現主控制器硬件的初始化，然后等待用戶輸入對應的軌跡坐標，接著陀螺儀開始采集關節的狀態，將采集的參數傳遞給EKF算法，主控制器根據EKF算法輸出的結果判斷是否需要進行舵機調控位姿，最后系統接著檢測是否有新的用戶任務進來，這樣機械臂控制系統進行循環運行，直到到達設定的坐標位置。

EKF算法是在卡爾曼濾波算法的基礎上擴展出來的，該算法是一個隨著時間變化的離散的動態系統，它當前的系統狀態可以由系統的過程方程和描述系統的觀測方程進行表述，如式(1)所示：

y(n)=C(n)x(n)+v2(n)

(1)

式(1)中，y代表一個NX1的向量，表示系統在n時刻的觀測向量矩陣;C代表的是系統的觀測矩陣;v2代表的是擾動信號向量，它的矩陣維數大小和觀測向量的維數一致。

狀態方程為：

x(n+1)=F(n+1,n)x(n)+v1

(2)

式中，x代表一個MX1的向量，表示系統在n時刻的系統狀態向量矩陣；F是一個MXM的常量矩陣，定義為狀態轉移矩陣；v1表示帶入系統的擾動信號向量。

式(3)是基于條件概率模型，也是貝葉斯先驗模型[8]。已知x、y事件發生的概率，并且知道在x事件發生的情況下y事件發生的概率，如果已知上面的三種概率事件，就可以通過貝葉斯先驗模型推導出在事件y發生的情況下x發生的概率是多少。EKF就是基于以上概率模型進行條件預測的。

(3)

3 系統驗證與分析

該系統采用擴展卡爾曼濾波算法，將陀螺儀采集的關節狀態參數傳遞給EKF核心算法進行預估機械臂的運動狀態，實現對機械臂運動過程中的偏差調控。本文分析和仿真了滕冠[2]等人提出的基于模糊PID控制算法進行機械臂的運動控制，通過和本系統提出的EKF算法進行對比分析，發現該系統在控制精度上優于模糊PID的控制策略。圖4為滕冠[2]等人提出的基于模糊PID控制算法仿真結果，從圖中可以看出，模糊PID在整個軌跡調節過程中波動相對較大，實際軌跡和調整后的軌跡偏差達到25 mm，偏差的中心值在10 mm。圖5為本文提出的基于EKF算法的仿真結果，相比模糊PID算法，本文提出的EKF算法波動相對較少，實際軌跡和調整后的軌跡偏差最大達到18 mm,偏差的中心值為6 mm。

綜上所述，相比于模糊PID控制算法，本文采用的EKF算法在機械臂精度調節、軌跡平滑度等方面具有一定的優勢，對于機械臂的性能優化具有一定的實際意義。

圖4 模糊PID控制算法仿真圖

圖5 EKF算法仿真圖