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

方華麗，吳 晴

(河南工業(yè)職業(yè)技術(shù)學院，河南 南陽 473000)

0 引言

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

1 主動防碰撞控制系統(tǒng)總體流程

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

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

移動機械手主動防碰撞控制系統(tǒng)主要內(nèi)容有：

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

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

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

2 移動機械手避障策略

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

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

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

(1)

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

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

(2)

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

2.2 移動機械手避障策略

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

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

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

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

(3)

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

假設(shè)po,i表示障礙物的位置，那么第i個關(guān)節(jié)上的任意點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)

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

3 多傳感信息融合算法的設(shè)計

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

(5)

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

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

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

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

(6)

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

(7)

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

采摘機器人移動機械手在時間節(jié)點k的姿態(tài)為

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

(8)

那么，采摘機器人移動機械手控制系統(tǒng)的狀態(tài)方程為

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

(9)

即

xk+1=f(xk)+wk

(10)

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

4 移動機械手主動防碰撞控制系統(tǒng)

移動機械手主動防碰撞控制系統(tǒng)由嵌入式系統(tǒng)、多傳感器網(wǎng)絡(luò)和電機驅(qū)動構(gòu)成。多傳感器模塊負責采集機械手周邊環(huán)境信息，結(jié)合多傳感信息融合算法，通過電機驅(qū)動電路對移動機械手運動電機進行控制，從而實現(xiàn)對障礙物的主動防碰撞功能。

4.1 硬件框架設(shè)計

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

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

系統(tǒng)硬件各個子系統(tǒng)的功能描述如下：

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

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

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

4.2 系統(tǒng)軟件設(shè)計

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

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

5 實驗結(jié)果分析

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

圖6 采摘機械手主動避障結(jié)果圖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。其主要原因是，移動機械手較大的運動速度，則需要較高的機動性和控制速率，以保證其在較短時間內(nèi)根據(jù)參考軌跡規(guī)劃最優(yōu)避障軌跡。

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

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

據(jù)觀察，當仿真模型中的障礙物位于參考軌跡狀態(tài)中時，系統(tǒng)會改變軌跡以避開障礙物；一旦系統(tǒng)發(fā)現(xiàn)移動軌跡不會與障礙物相交，則會選擇合適的時間步長，盡量規(guī)劃最優(yōu)的避障軌跡。

6 結(jié)論

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