移動機械手主動防碰撞控制系統研究——基于多傳感信息融合

方華麗，吳 晴

(河南工業職業技術學院，河南 南陽 473000)

0 引言

隨著電子、計算機、網絡、機械,以及智能控制技術的發展，針對農業機械人的應用研究已經成熟。但是,對于采摘機器人移動機械手的防碰撞研究，由于其作業周邊環境復雜多變，該領域的障礙物識別和多傳感信息的融合處理一直是研究的重難點。為此,通過研究移動機械手的避障策略和多傳感信息融合算法，設計了移動機械手主動防碰撞控制系統，對于采摘機器人自動采摘作業具有較大的現實意義。

1 主動防碰撞控制系統總體流程

采摘機器人機械手在進行采摘作業中，會遇到樹枝、葉子以及其他非果樹上的障礙物等，為了防止機械手經常發生碰撞出現損壞，一般需要其對障礙物進行主動防碰撞。因此，移動機械手控制系統最重要的部分是對移動機械手進行主動防碰撞控制。移動機械手主動防碰撞控制系統總體流程如圖1所示。

圖1 移動機械手主動防碰撞控制系統總體流程圖Fig.1 The overall flow chart of active collision avoidance control system for mobile manipulator

移動機械手主動防碰撞控制系統主要內容有：

1)機器人移動信息。獲取系統得到的傳感器組的信息，主要包括移動機器人的前行速度、前方障礙物、機械手和障礙物相對距離以及方位角等。

2)障礙物的判斷與識別。通過CCD攝像機和機械手激光雷達獲得前方目標信息，計算出機械手和目標物的相對距離和速度，判斷是否為障礙物。

3)安全狀態評估。根據機器人移動信息和障礙物的判斷與識別兩塊的參數信息，建立機械手和障礙物的安全距離模型和安全距離，并根據機械手和目標物的相對距離和速度，判斷是否需要進行主動防碰撞的控制。

2 移動機械手避障策略

2.1 移動機械手和障礙物之間碰撞條件

移動機械手是采摘機器人的核心部件，是保證在園林作業中準確采摘目標果實的綜合性剛體。移動機械手包含多個可運動環節，整體運動復雜，因此需要提前進行避障分析。在移動機械手避障策略中，一般采用限制機械手與其周圍障礙物之間的相對速度來避開障礙物。因此，假設障礙物是凸形或可以用包圍的凸包絡來表示。移動機械手和障礙物相對距離示意如圖2所示。

圖2 移動機械手和障礙物距離示意圖Fig.2 The distance sketch diagram of mobile manipulator and obstacle

(1)

其中，ξ為調整收斂速度的正系數，該值越大，移動機械手和障礙物之間允許的相對速度越大。ds(≤di)為安全距離的正值。等式(1)稱為d的速度壩，其表示當它小于di時，d不能減小得太快。

如果滿足初始條件d(0)≥ds，則可以推導出

(2)

式(2)表明：如果機械手和障礙物之間的距離決不會小于ds，這意味著二者之間的碰撞永遠不會發生。

2.2 移動機械手避障策略

從前一節可知，移動機械手和障礙物可以通過限制兩個物體上最近兩點之間的相對速度來避免碰撞，并假設每個障礙物都具有凸形或者可以用包圍的凸包絡來表示。移動機械手和障礙物之間的最近點如圖3所示。

圖3 移動機械手和障礙物之間的最近點Fig.3 The closest point between a mobile manipulator and an obstacle

圖3中，i為移動機械手第i個連桿，考慮到采摘機器人的構造，已知連接的第i個連桿和第(i+1)個連桿的位置。由于移動機械手是凸形的，因此第i個關節上的任何點都可以表示為

pi,k=λpi+(1-λ)pi+1, 0≤λ≤1

(3)

其中，pi和pi+1分別是第i個關節和第(i+1)個關節的位置矢量。

假設po,i表示障礙物的位置，那么第i個關節上的任意點pi，k與第i個障礙物之間的距離可以表示為

‖pi,k-po,i‖=(pi-pi+1)2λ2+ 2(pi-pi+1)(pi+1-po,i)λ+(pi+1-po,i)2

(4)

為了避免移動機械手與障礙物發生碰撞，可以利用前面的方法計算關節與障礙物之間的最近距離；同時，通過分別限制移動機械手與障礙物之間的相對速度，控制移動機械手避免與障礙物發生碰撞。

3 多傳感信息融合算法的設計

多傳感信息融合算法較多，但不同的算法運行機制不同。根據移動機械手主動防碰撞控制系統的傳感器網絡硬件平臺和安全狀態評估流程，本文選擇EKF(Extended Kalman Filter，擴展卡爾曼濾波器)算法。EKF算法一般用于處理較為復雜的狀態轉移與觀測模型的系統，其轉移模型為

(5)

其中，xk為時間節點k的狀態；uk-1為時間節點k-1的系統輸入；f(xk-uk-1)為時間節點k-1的狀態轉移矩陣；zk為時間節點k的觀測函數；h(xk)為時間節點k的觀測模型矩陣；vk為時間節點k的高斯噪聲。EKF算法的流程如下：

4)跟新計算協方差誤差矩陣Pk|k。

針對移動機械手主動防碰撞控制系統的控制特點，通過編碼器推算的Odom數據和慣性測量數據對機械手運行狀態進行建模，其運行狀態量為

xk=[Xk,Yk,ψk,vk,ωk]T

(6)

假設當時間節點為k時速度量輸入為[vk,ωk]T，編碼器檢測一次的時間差為△t，則

(7)

其中，vl、vr和v分別為左輪、右輪和采摘機器人移動速度；T和ω為采摘機器人旋轉角速度；D和η分別為采摘機器人前后的輪間距離和轉彎系數；μ為編碼器和輪子之間的轉換系數。

采摘機器人移動機械手在時間節點k的姿態為

xk+1=Xk+vkcos(ψk)Δt,Yk+vksin(ψk)Δt,ψk+ωkΔt,vk,ωk

(8)

那么，采摘機器人移動機械手控制系統的狀態方程為

xk+1=φk+1|kxk+ΓkWk

(9)

即

xk+1=f(xk)+wk

(10)

因此，根據多傳感信息的數據融合，可以得到采摘機器人移動機械手的姿態角，然后實現對其狀態的控制。

4 移動機械手主動防碰撞控制系統

移動機械手主動防碰撞控制系統由嵌入式系統、多傳感器網絡和電機驅動構成。多傳感器模塊負責采集機械手周邊環境信息，結合多傳感信息融合算法，通過電機驅動電路對移動機械手運動電機進行控制，從而實現對障礙物的主動防碰撞功能。

4.1 硬件框架設計

采摘機器人的移動機械手在作業中，需要實時檢測周邊環境信息，主動對樹枝和樹葉等障礙物進行避障。因此，需要移動機械手配合車速傳感器、機械手激光雷達和CCD攝像機進行操作和控制。該系統采用韓國三星公司的Exynos4412嵌入式芯片為核心處理器，系統硬件平臺主要由嵌入式控制系統、多傳感器信息采集子系統、電機驅動模塊、采摘機器人主體、以太網口、串口、觸摸屏和接近報警裝置等組成。系統硬件框架如圖4所示。

圖4 移動機械手主動防碰撞控制系統硬件設計框架Fig.4 The hardware design framework of active collision avoidance control system for mobile manipulator

系統硬件各個子系統的功能描述如下：

1)嵌入式控制系統。通過獲取多傳感器模塊的參數信息及系統設定的程序流程，實現對移動機械手的主動防碰撞控制。

2)多傳感器信息采集子系統。通過不同位置、不同類型的車速傳感器、機械手激光雷達以及CCD傳感器，增加對環境探測的準確性；然后，利用多傳感器融合算法，獲得對周邊環境的較為統一的感知信息，提高移動機械手主動防碰撞的決策能力。

3)伺服驅動模塊。通過多傳感器信息采集子系統獲得的環境感知信息，由嵌入式系統控制電機驅動器電路，實現對移動機械手運動電機的控制。

4.2 系統軟件設計

移動機械手主動防碰撞控制系統軟件體系包括嵌入式Linux交叉編譯環境的搭建、內核的裁剪、文件系統的移植及多傳感器信息采集子系統驅動程序的編寫等。其中，多傳感器信息采集子系統驅動程序的編寫采用模塊化的思想設計，其基本流程如圖5所示。

圖5 多傳感器信息采集子系統驅動程序的編寫流程Fig.5 The compiling process of driver for multi-sensor information collection subsystem

5 實驗結果分析

為了驗證本文設計的移動機械手主動防碰撞控制系統的實時性、穩定性和可靠性，在完成關鍵技術研究及移動機械手主動防碰撞控制系統整機研制的基礎上，利用MatLab搭建了移動機械手的仿真系統，并進行了實際的性能測試實驗。移動機械手質量m=3kg，重力加速度為g=9.81m/s2。系統控制參數如下：控制輸入、輸出權重分別為ρ=[0.01 0.01 0.01]T和η=[1 1 1]T。靜態障礙物的坐標為Ll=[1.4
1.4
1.4]T和Ul=[1.9 2.5 2.9]T。試驗結果如圖6所示。

圖6 采摘機械手主動避障結果圖Fig.6 The result diagram of the active obstacle avoidance of the picking manipulator

從圖6可以看出：隨著移動機械手移動速度的增加，軌跡控制誤差也會增加。例如，當移動機械手移動速度v=0.5m/s時，其運動軌跡誤差保持在8cm以下；而當速度v=2.0m/s時，誤差接近23.5cm。其主要原因是，移動機械手較大的運動速度，則需要較高的機動性和控制速率，以保證其在較短時間內根據參考軌跡規劃最優避障軌跡。

為了更好地分析系統，考慮到障礙物位于內部參考軌跡中，給出了如圖7所示的移動機械手主動避障的2D圖形。

圖7 移動機械手主動避障的2D圖形Fig.7 2D graphics for active obstacle avoidance of mobile manipulator

據觀察，當仿真模型中的障礙物位于參考軌跡狀態中時，系統會改變軌跡以避開障礙物；一旦系統發現移動軌跡不會與障礙物相交，則會選擇合適的時間步長，盡量規劃最優的避障軌跡。

6 結論

移動機械手在不同速度的情況下，都可以主動避開障礙物，而當移動機械手移動速度越大時，軌跡控制誤差也會增加，移動機械手的移動路徑也會增加，驗證了系統的實時性、穩定性和可靠性。