基于STM32的果實收獲機械臂運動控制系統研究

劉卓遠,李林升

(1.南華大學 機械工程學院，湖南 衡陽 421001；2.上海電機學院 電氣學院，上海 201306)

0 引言

在農業現代化的過程中，農業機器人是必不可少的推動者，因此對農業機器人的研究已經成為農業領域的最大研究熱點。果實收獲機器人作為實現農業自動化的一種設備，實現對其機械臂的精準調控是重點，也是提高生產效率、改善產品品質的一種方式[1]。

針對機械臂的精準調控,人們進行了大量的研究，當下研究較多的有以卡爾曼濾波反饋調節的機械臂模型，以DSP為控制器設計控制系統；其次就是用單片機控制電機和舵機驅動電路,完成機械臂的抓取過程；還有一種基于微機控制的機械臂控制系統。上述方法都能較好地實現對機械臂的運動控制，但是運動精度和響應效率不是那么理想[2]。

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

1機械臂正運動學分析

D-H參數是一個用四個參數表達兩對關節連桿之間位置角度關系的機械臂數學模型和坐標系確定系統。它通過限制原點位置和X軸的方向，人為減少了兩個自由度，因此它只需要用四個參數即可表達關節之間原本是六自由度的坐標變換[4]。

確定坐標系，對于每個關節都必須指定一個Z軸和X軸。指定Z軸，如果關節是旋轉的，則Z軸位于按右手規則旋轉的方向。繞Z軸的旋轉角是關節的變量；如果關節是滑動關節，則Z軸為沿直線運動的方向。沿Z軸的連桿長度d是關節變量。

指定X軸，當兩關節不平行或相交時，Z軸通常是斜線，但總有一條距離最短的公垂線，它正交于任意兩條斜線。在公垂線方向上定義本地參考坐標系的軸。如果an表示Zn-1之間的公垂線，則Xn的方向將沿an。

當然也有特殊情況。當兩關節的Z軸平行，就會有無數條公垂線。此時可挑選與前一關節的公垂線共線的一條，可簡化模型；兩關節相交，它們之間沒有垂線，這時可將垂直于兩條軸線構成的平面的直線定義為X軸，可簡化模型。

給每個關節都附上對應坐標系之后，如圖1所示。

確定好坐標系后，可以用更簡潔的方法來表示D-H四個參數：

Link length(連桿長度)ai-1：沿Xi-1的Zi-1到Zi的距離。

Link twist(連桿扭轉)αi-1：Zi相對于Zi-1繞Xi-1旋轉的角度。

Link offset(連桿偏移)di：沿Zi的Xi-1到Xi的距離。

Joint angle(關節轉角)θi：Xi相對于Xi-1繞Zi旋轉的角度。

接下來寫出機械臂的D-H參數表，如表1所示。

表1 D-H參數表

根據公式：

(1)

依次計算每個關節，最后得到機械臂的正運動學公式：

(2)

得到各關節的旋轉矩陣后即可根據下面公式得到末端的坐標：

(3)

2 機械臂逆運動學分析

逆運動學是決定要達成所需要的姿勢所要設置的關節可活動對象的參數的過程。對于機械臂而言，就是給出夾持器的位置和朝向后求每個關節的旋轉角度。為了簡化模型，去掉下方云臺的旋轉關節，這樣就可以在二維的平面上進行運動學分析[5]。進行逆運動學分析要進行大量的矩陣運算，先使用幾何法對機械臂進行分析，如圖2所示。

圖2 機械臂幾何分析示意圖

從圖2知機械臂的端點P的坐標(x,y),最終由三個部分組成(x1+x2+x3,y1+y2+y3)。其中圖中的θ1，θ2，θ3就是要求解的舵機的角度。α是爪子與水平面的夾角。從圖上看，顯然α=θ1+θ2+θ3。

據此可以列出下式：

(4)

其中x、y為目標物所在坐標，l1、l2、l3為機械臂的機械結構固有屬性。

為了方便計算，將已知部分處理一下，作整體考慮：

(5)

將m、n代入已有方程，再化簡可得：

l2=(l1cosθ1+m)2+(l0sinθ1+n)2

(6)

通過計算可得：

(7)

上式是一元二次方程的求根公式的形式，其中

(8)

據此求出θ1，同理求出θ2。如此便可求出三個舵機的角度，然后根據角度控制舵機即可實現坐標位置的控制。

輸入目標位置的x，y坐標和爪子的俯仰角度，即可計算出目標的舵機角度并轉到對應角度上去。

3 機械臂運動控制

為了驗證上述分析的正確性，將使用LeArm開源機械手臂進行運動控制實驗。先使用基于PC+STM32運動控制模塊的方法，需要將兩者通過串口聯接，這個串口用來與電腦上位機通信。

初始化串口，將串口配置為波特率115 200 b/s、8位數據位、1位停止位、無校驗位。另外用兩個IO口來控制單線串口的收發狀態，發送的指令需要舵機返回數據，在發送了指令之后將單線串口設為接收狀態。根據接收到的指令做出對應的操作，如圖3所示。

發送使用輪詢寄存器方法實現，接收使用接收中斷。先讓舵機轉動，單片機本身具備單線串口通信的功能,STM32上的串口自身可以支持單線通信，底板上設計有兩線串口轉為單線串口的電路。

使用邏輯電路可以簡化程序代碼，同時提高代碼的通用性。和普通的串口通信一樣使用串口，按照通信協議即可控制總線舵機。函數組織好指令之后，調用UART_TX_ENABLE將單線串口設為發送模式，然后調用發送函數將數據發送出去，這樣就能實現機械臂的運動控制[6]。機械臂運動控制系統設計的流程如圖4所示。

圖4 PC+STM32控制模塊的實驗框圖

4 仿真與結論

本文研究LeArm開源機械手臂，整體結構如圖5所示。

圖5 LeArm開源機械手臂

該機器臂為滿足pieper準則的6—DOF機器臂，為了研究其位置控制，實驗仿真只針對其關節2與關節3進行研究[7]。該機器人采用改進的D-H參數進行標識，建模之后的結構如圖6所示。

圖6 LeArm開源機械手臂模型圖

通過實驗選擇定義機械臂的起始點、中間點、終止點坐標，仿真得出其對應的初始點位姿、中間點位姿與末端點位姿，如表2所示。

表2 位姿點4×4矩陣表

由于該機器人滿足pieper準則，因此其逆解可以采用代數法進行求解，其逆運動學采用代數法對其進行求解[8]。在笛卡爾坐標系下給定機器人的末端位姿的軌跡圖，如圖7所示。

圖7 笛卡爾坐標系下給定機器人的位姿的軌跡圖

由以上仿真結果分析可得，采用改進后的軌跡規劃方法能夠縮短整個機械臂的運行時間，并且能夠保證規劃出來的位移、速度、加速度曲線在整個運動過程中的連續性和平滑性在一個較理想的范圍內，從而使得在縮短時間的前提下還能繼續保證整個機械臂的穩定運行。