基于BDS和IMU的挖掘機鏟斗位姿測量方法與試驗

胡 煉，彭靖怡，賴桑愉，馮達文，陳高隆，王晨陽，羅錫文

基于BDS和IMU的挖掘機鏟斗位姿測量方法與試驗

胡 煉1,2，彭靖怡1,2，賴桑愉1，馮達文1,2，陳高隆1,2，王晨陽1,2，羅錫文1,2※

（1. 華南農業大學南方農業機械與裝備關鍵技術教育部重點實驗室，廣州 510642；2.嶺南現代農業科學與技術廣東省實驗室，廣州 510642）

為提高農田建設中挖掘機施工作業精度和智能化程度，該研究提出了一種基于北斗衛星導航系統（BeiDou Navigation Satellite System, BDS）和慣性測量單元（Inertial Measurement Unit, IMU）的挖掘機鏟斗位姿測量方法。首先，采用IMU測量挖掘機各執行機構的姿態角信息，解算獲得挖掘機車體坐標系下鏟斗末端的三維坐標，利用雙天線BDS和IMU檢測車體的位置和姿態建立了挖掘機鏟斗末端三維坐標的實時解算模型，并設計了融合雙天線BDS和IMU輸出高頻率高精度位姿的卡爾曼濾波算法。模擬挖掘機實際施工場景進行了靜態和動態試驗，采用全站儀驗證鏟斗末端三維坐標解算值。試驗結果表明，該方法能準確實時測量挖掘機鏟斗末端三維坐標，挖掘機鏟斗末端三維坐標解算值與全站儀實測值的運動軌跡變化一致，同一時刻空間兩坐標點距離均方根偏差小于30 mm，三個軸向坐標動態測量均方根偏差均在20 mm內，絕對偏差≤30 mm的數據占比不低于95.35%，挖掘機鏟斗位姿的準確測量為挖掘機精準施工智能引導提供了基礎。

農業機械；挖掘機；BDS；IMU；坐標轉換；位姿測量

0 引 言

挖掘機是常用的工程機械，也是農業生產和農田建設廣泛應用的機械，用于河渠水下淤泥清理、農田溝渠修建和農田改造平整等[1]，為高標準農田建設發揮了重要作用。目前挖掘機的施工作業全憑操作人員的經驗，在高精度造型、平整，以及視野受限的水下作業和地底挖掘等施工場景對于操作人員的要求很高，依靠操作人員經驗難以保證工作質量和效率[2-4]。

近年來，人們對挖掘機的施工作業精度與作業效率的要求也越來越高，挖掘機智能化和自動化已成為重要發展方向，如精確感知挖掘機本身及其作業部件位姿信息實現軌跡控制和遠程控制等[5-6]。為準確獲取挖掘機作業部件位姿參數，目前基于傳感器技術估計位姿是主要研究熱點[7]，如采用旋轉編碼器和電位計[8]、旋轉電位計和傾角傳感器[9]獲取挖掘機的姿態信息。牛大偉[10]采用微機電系統傳感器對挖掘機姿態信息進行測量，提高了姿態檢測系統的工作穩定性與可靠性。日本小松挖掘機通過裝載位移行程油缸獲取油缸行程轉換得到各機構臂的空間姿態角[5]、李海虹等[11]提出以液壓缸行程的線位移測量取代關節轉角測量，但位移行程油缸和轉角傳感器安裝較困難，且接觸式的轉角傳感器因機械磨損降低精度。在基于視覺技術中，為避免了傳統位移傳感器在惡劣工況與環境下的碰撞損壞與測量精度低的問題，倪佳敏等[12]通過神經網絡建立油缸位移長度與標識點坐標間映射關系的工作裝置虛擬位移傳感器系統，但其只針對油缸位移傳感器實現了非接觸測量，功能較為單一；Xu等[13]提出一種利用基于視覺的神經網絡系統估計液壓機械手姿態，在測試平臺進行了使用經過訓練的神經網絡估計器來估計機械手液壓缸位移的仿真測試。Mulligan等[14]開發了一種基于邊緣檢測的倒角匹配方法來估計挖掘機操縱器的姿態，但是其圖像處理方法的性能嚴重依賴于輸入圖像的質量；Liang等[7]采用深度卷積神經網絡訓練挖掘機圖片集，基于堆疊沙漏網絡算法估計機器二維姿態信息，再對三維位姿進行預測和重構，但其三維姿態是以二維姿態估計結果作為輸入易產生累積誤差，且該網絡只能識別一臺機器的位姿，不適用多機器共同作業場景；王海波等[15]基于視覺技術測量挖掘機工作姿態、朱建新等[16]提出了一種基于點云聚類特征值方圖的目標識別方法，但在復雜的施工背景下提取圖像特征以及提高算法的魯棒性和準確性有待進一步研究。

位姿由位置和姿態兩部分組成。在三維坐標系中，可以用質點的坐標表示位置，質點坐標與坐標原點組成的三維向量表示姿態，位置的變化即質點的平移過程，姿態的變化是向量的旋轉過程。其中姿態的測量可通過多種方法實現，周云成等[17]提出一種基于時序一致性約束的自監督位姿變換估計模型以實現溫室環境下機器人行進過程中的位置及姿態跟蹤。李晨陽等[18]利用高頻率里程計信息估計機器人位姿，但在農田等地面不平整的環境中，里程計信息存在一定誤差。在基于傳感器的方法中，使用了如慣性測量單元（Inertial Measurement Unit, IMU）、光學姿態測量系統、GPS姿態測量系統、無線局域網（WLAN）、射頻識別（RFID）和基于超寬帶（UWB）等方法[19-21]。在基于WLAN、RFID和UWB的三維姿態估計中，信號源被放置在固定位置，這在動態工作現場條件下并不適用[22]。IMU不受氣候條件、空間條件限制，方便攜帶，成本因精度而定，適用于對測量精度、動態性能、實時性均有較高要求的領域，然而連續測量角度變化會受到磁干擾和漂移問題的影響。在基于GPS的姿態估計方法中，GPS估計的每個位置和航向相互獨立，這解決了漂移問題，但其無姿態信息、數據率低且易受環境因素干擾[23-26]。隨著中國北斗衛星導航系統（BeiDou Navigation Satellite System，BDS）建成，其已經在測量航向和姿態方面得到了廣泛應用，為運動目標提供三維姿態信息，可以達到毫米級的靜態定位精度和厘米級的動態測量精度[27-28]。

為此，本文提出基于BDS和IMU的挖掘機鏟斗位姿測量方法，采用卡爾曼濾波算法獲取車體準確位姿信息，建立鏟斗三維坐標解算模型，解算挖掘機鏟斗末端三維坐標。擬解決上述由于傳感器特性及安裝造成的精度、測量范圍和抗振能力較差的問題，引導挖掘機精準完成平整、造型施工和水下施工等作業，減少人力成本和勞動強度，在達到精確作業的同時提高智能化程度。

1 基于BDS和IMU的鏟斗位姿測量方法

1.1 坐標系轉換

為了說明質點的位置、運動的快慢和方向等，必須選擇相應的坐標系[28]。中國北斗衛星導航系統定位坐標采用的是大地坐標系，在工程上常用“高斯投影”方法將大地坐標系中的點（，，）轉換成地理坐標系（x，y，z）（本文選擇“東北天”為地理坐標系，簡稱G系，定義為：軸指向東，軸指向北，軸垂直于當地水平面，沿當地垂線向上），再平移至施工本地坐標系（簡稱t系）。為使鏟斗末端姿態解算結果可直接用于施工，設施工本地坐標系的原點為，在G系的坐標為（x，y，z）[29]，將地理坐標系的坐標（x，y，z）轉換為施工本地坐標系下的坐標（x，y，z）可通過式（1）獲得。

1.2 鏟斗位姿測量原理

基于BDS和IMU的挖掘機鏟斗位姿測量系統主要由BDS基站、BDS雙天線、IMU姿態傳感器、車載終端構成，如圖1所示，在車頂上搭載BDS雙天線以獲取車體位姿信息（航向角與空間位置信息），并保證雙天線間連線與駕駛室和各機械臂方向保持垂直；在車體上安裝IMU姿態傳感器讀取車體橫滾角與俯仰角；在挖掘機的動臂、斗桿、鏟斗合適處安裝IMU姿態傳感器獲取各機構姿態角變化；在駕駛室內安裝車載終端連接BDS雙天線和IMU，實時讀取信息并解算出鏟斗末端三維坐標，車載終端顯示的鏟斗末端三維坐標信息為操作人員提供準確的施工作業引導。

1.BDS基站 2.BDS雙天線 3.車身IMU 4.車載終端 5.動臂IMU 6.斗桿IMU 7.鏟斗IMU 8.鏟斗末端

1.BDS (BeiDou Navigation Satellite System) base station 2.BDS dual antenna 3.IMU (Inertial Measurement Unit) on the body 4.Vehicular terminal 5.IMU on the boom 6.IMU on the stick 7.IMU on the bucket 8.End of excavator bucket

注：坐標系xyz為車體坐標系，以車體前進方向為x軸，y軸與x軸垂直指向車體方向的左側，z軸垂直于xy平面向上；坐標系xyz為施工本地坐標系，x軸指向東，y軸指向北，z軸垂直于當地水平面，沿當地垂線向上；，，分別為車體偏航角、橫滾角和俯仰角，(°)。

Note:xyzis the vehicle body coordinate system,xrepresents the forward direction of the car body,yperpendicular toxand point to the left side of the vehicle body,zaxis perpendicular toxyplane and upward;xyzis the local construction coordinate system,xpoints east,ypoints north,zis perpendicular to the local horizontal plane and upward along the local vertical line;,,are yaw, roll and pitch angles of the body respectively, (°).

圖1 挖掘機鏟斗位姿測量系統

Fig.1 The position and posture measurement system of excavator bucket

1.3 鏟斗位姿測量算法

基于BDS和IMU的挖掘機鏟斗位姿測量算法主要是建立挖掘機鏟斗末端的坐標解算模型，包括以下幾個步驟：1）利用雙天線BDS和IMU卡爾曼濾波融合算法輸出車體的位置和姿態，計算姿態旋轉矩陣；2）根據幾何關系或機器人運動學和所測量的各執行機構姿態角信息，解算基于車體坐標系的挖掘機鏟斗末端的位置信息；3）測量計算挖掘機車身參數及BDS天線到質心位置的坐標增量，計算BDS天線至鏟斗末端的坐標增量，解算基于施工本地坐標系下鏟斗末端的三維坐標?；谕诰驒C建立坐標系，坐標系及測量單元安裝位置如圖1所示，通過獲取BDS雙天線的位置和航向角信息以及車體、動臂、斗桿及鏟斗的姿態信息，基于所獲取信息和執行機構幾何關系建立求解鏟斗末端的三維坐標解算模型，并計算獲得施工本地坐標系下鏟斗末端的三維坐標。坐標解算模型如式（2）。

式中為施工本地坐標系下鏟斗末端的三維坐標；為BDS天線的位置信息；1，2，3為各執行機構上IMU姿態傳感器角度測量值。

位姿解算步驟如下：

1）BDS和IMU的卡爾曼濾波融合算法，計算車體姿態旋轉矩陣

由于GNSS輸出頻率較低且易受干擾、IMU在動態測量過程中受頻繁振動影響測量精度。因此，設計了卡爾曼濾波融合算法，融合BDS（頻率10 Hz）輸出定位信息和IMU（頻率50 Hz）輸出三軸加速度、角速度信息來估計最優車體定位信息和航向信息，保證位姿測量的實時性和準確性，以滿足挖掘機實際應用的要求。

系統模型建立：

通過卡爾曼濾波融合具體輸出過程如下：

（3）計算時刻的濾波增益；

式中為測量噪聲協方差矩陣；

式中為測量向量。

（5）更新時刻誤差估計的協方差

重復計算過程，直到算法結束，得到最優估計的車體定位信息和航向信息。

車體坐標系通過繞歐拉角橫滾、俯仰、偏航3次旋轉到與施工本地坐標系對齊。則()、()、() 3個旋轉矩陣分別為

得車體坐標系轉為施工本地坐標系的旋轉矩陣：

2）計算基于車體坐標系下鏟斗末端三維坐標0

挖掘機的動臂、斗桿及鏟斗在同一平面內，其幾何結構如圖2所示，建立車體坐標系xoz和施工本地坐標系xoz，根據幾何關系推算基于車體坐標系下鏟斗末端三維坐標0。

求得車體坐標系下0的坐標為

注：坐標系xoz為施工本地坐標系；坐標系xoz為車體坐標系；()表示車體與動臂之間的關節，是施工本地（車體）坐標系的原點；1點表示動臂與斗桿之間的關節；2點表示斗桿與鏟斗之間的關節；3點表示鏟斗末端測量點；1表示動臂的長度，為()到點1的直線距離，m；2表示斗桿的長度，為點1到點2的直線距離，m；3表示鏟斗的長度，為點2到點3的直線距離，m；1、2、3分別為動臂IMU、斗桿IMU和鏟斗IMU測量值，(°)；為車體在運動狀態下車體坐標系基于施工本地坐標系的俯仰角，(°)。

Note: xozis the local construction coordinate system;xozis the car body coordinate system;() represents the joint between the body and the boom and is the origin of the local construction (body) coordinate system;1represents the joint between the boom and the stick;2represents the joint between the stick and the bucket;3represents the measuring points at the end of the bucket;1is the length of the boom, represents the linear distance from point() to Point1, m;2is the length of the stick, represents the linear distance from point1to Point2, m;3is the length of the bucket，represents the linear distance from point2to point3, m;1,2,3are the measured values of boom IMU, stick IMU and bucket IMU respectively, (°);is the pitch angle of the car body coordinate system based on the local construction coordinate system when the car body is in motion, (°).

圖2 挖掘機幾何結構圖

Fig.2 Geometric structure diagram of excavator

3）計算基于施工本地坐標系下鏟斗末端三維坐標

測量車體坐標系下BDS天線至車體坐標系原點的坐標增量0，可得基于車體坐標系下BDS天線至作業部件末端的坐標增量：

=0+0（15）

經過歐拉角旋轉變換至施工本地坐標系下，可得：

2 挖掘機模型平臺試驗

2.1 試驗材料

1）挖掘機模型試驗平臺

設計由鏟斗、斗桿、動臂和回轉平臺組成的挖掘機模型平臺，可模擬挖掘機各機械臂和回轉關節運動，并在挖掘機模型平臺上安裝BDS雙天線，如圖3所示。試驗選擇施工本地坐標系的、、3個軸向來觀察挖掘機鏟斗末端真實值與解算值的誤差，采用直線導軌滑塊機構保證動態試驗的3個坐標軸向運動直線度，導軌安裝有位移傳感器測量軸向運動位移[30]，位移傳感器采用WXY15M型，最大量程為400 mm，輸入0～5 V電壓，輸出為模擬量，分辨率0.01，線性精度為0.02%FS，設置拉線傳感器的采樣頻率為10 Hz。

1.斗桿長 2.動臂長 3.回轉平臺 4.拉線傳感器 5.鏟斗長 6.鏟斗末端 7.BDS雙天線

1.The length of excavator stick 2.The length of excavator boom 3.Rotary platform 4.Cable sensor 5.The length of excavator bucket 6.The end of excavator bucket 7.BDS dual antenna

注：車體坐標系軸指向正東，軸指向正北，軸垂直于平面向上;導軌滑塊平臺縱向分別與車體、、軸平行。

Note:Theaxis of the vehicle body coordinate system points to the east, theaxis points to the north, theaxis perpendicular toplane and upward. Make the longitudinal direction of the slide rail platform parallel to the,andaxes of the vehicle body, respectively.

圖3 挖掘機模型試驗平臺

Fig.3 Model test platform of excavator

2）其他材料

BDS雙天線系統（司南導航K726定位板卡，靜態差分精度為水平面2.5 mm、高程5 mm，輸出頻率為10 Hz，航向角測量精度為0.2°/。其中，為雙天線基線長，m）、DTU（型號：CM510-71F）、HWT605傳感器（角度精度：、軸靜態0.05°，動態0.1°，輸出頻率為0.2～200 Hz可調）、5 V直流穩壓電源、NI myRIO模塊、PCAN-USB 模塊、USB 轉串口線、筆記本計算機、多串口卡、Labview 軟件和Matlab 軟件。

2.2 試驗方案

靜態試驗以車體坐標系軸指向東西南北4個方向且挖掘機機械臂在不同姿態的情況下，每個方向各進行3組試驗。用BDS設備測量并記錄鏟斗末端三維坐標，轉換到施工本地坐標系下，與鏟斗位姿測量算法解算值計算3個軸向偏差。

動態試驗采用直線導軌滑塊平臺實現3個軸向動態變化模擬真實挖掘機運動時鏟斗末端三維坐標的變化，如圖4所示。采用BDS設備以10 Hz的采樣頻率測量并記錄導軌滑塊平臺的起始點的定位信息后，由導軌平臺本身的幾何特性及位移傳感器的測量值推算此時鏟斗末端三維坐標的真實值。試驗選擇施工本地坐標系下的、、3個軸向來觀察鏟斗末端真實值與算法解算值的偏差。

圖4 挖掘機模型平臺動態試驗

2.3 試驗結果與分析

2.3.1 靜態試驗

統計各組試驗偏差：由鏟斗位姿測量算法獲取解算值，計算算法解算值與鏟斗末端真實值兩坐標點間的距離偏差和、、3個軸向偏差，結果如表1所示。

表1 鏟斗末端三維坐標的靜態試驗結果

通過試驗數據可知，在各種姿態下，測得、、3個軸向的最大絕對偏差分別為15.15、15.48和23.57 mm，均小于30 mm。解算值與真實值（驗證值）兩坐標點間的距離最小偏差為11.94 mm、最大偏差為29.03 mm、平均偏差為19.55 mm。

2.3.2 動態試驗

統計6組試驗偏差，根據挖掘機正常工作經驗速度和試驗模型尺寸綜合考慮，試驗挖掘機鏟斗末端分別沿、、3個軸向以0.1和0.2 m/s水平運動進行2組試驗，試驗數據統計結果如表2所示。

表2 挖掘機模型平臺鏟斗末端三維坐標動態試驗結果

由于每組試驗認為只有當前坐標軸變化，所以試驗結果中的偏差均為解算點與真實點之間的偏差。通過、、3個軸向的兩組試驗數據可知，解算值與真實值（驗證值）之間的平均絕對偏差分別為8.81、14.87和16.37 mm，均方根偏差分別為10.74、18.15和18.85 mm，均小于20 mm；軸和軸的最大絕對偏差分別達到42.33和47.45 mm，這是因為挖掘機模型關節間運動時存在間隙，從而產生更大的偏差。

3 挖掘機現場試驗

3.1 試驗材料

試驗采用山河swe40UF智能挖掘機、BDS雙天線系統、HWT605姿態傳感器、Android車載終端。試驗中，采用徠卡MS60高速影像全站掃描儀（測量精度：1 mm，追蹤運動軌跡輸出頻率為10 Hz）動態測量鏟斗末端三維坐標。

3.2 試驗方案

如圖5所示，將基于BDS和IMU的挖掘機鏟斗位姿測量系統安裝在挖掘機上，在挖掘機車頂上安裝雙天線北斗衛星系統（BDS）以10 Hz的采樣頻率獲取定位和航向角信息，并保證雙天線間連線與駕駛室和各臂方向保持垂直；在車體上安裝HWT605姿態傳感器以50 Hz的采樣頻率讀取車體橫滾角與俯仰角；在挖掘機的動臂、斗桿、鏟斗合適處安裝HWT605姿態傳感器獲取各機械臂姿態角信息；在駕駛室內安裝Android車載終端并連接BDS雙天線和姿態傳感器，以鏟斗棱鏡放置處為測量點，讀取實時信息解算鏟斗末端三維坐標。依據《建筑地基基礎工程施工質量驗收規范》（GB50202-2018）的場地平整土方開挖≤50 mm、填土≤30 mm和《高標準基本農田建設標準》（TD/T 1033-2012）田面高差應小于±30 mm的要求統計試驗結果絕對偏差≤30 mm的數據占比。試驗分為靜態試驗和動態試驗。

圖5 挖掘機動作試驗現場圖

靜態試驗時，挖掘機車體和機械臂在不同航向角和姿態為一組試驗，采用全站儀連續采集每組試驗動作下挖掘機鏟斗處棱鏡的三維坐標，計算與鏟斗位姿測量算法實時解算值之間的偏差。

動態試驗針對挖掘機施工作業中深挖、整平、刷坡等作業場景，操作挖掘機分別以試驗組1：車身航向不動，各機構臂動作；試驗組2：車身航向轉動，各機構臂不動；試驗組3：車身航向和各機構臂同時動作，3種試驗動作來模擬實際施工場景。采用全站儀以10 Hz的采樣頻率自動追蹤放置于鏟斗的棱鏡實時采集鏟斗的三維坐標來驗證與解算值之間的偏差。

3.3 試驗結果與分析

3.3.1 靜態試驗

靜態試驗統計10組試驗數據，求取每組試驗數據平均值，數據統計結果如表3所示。

表3 挖掘機鏟斗末端三維坐標靜態試驗結果

由表3可知，在不同姿態動作下，測量點、、3個軸向最大絕對偏差分別為17.69、14.99和11.68 mm，均小于20 mm；解算值與真實值（驗證值）兩坐標點間的距離最小偏差為7.40 mm、最大偏差為20.65 mm、平均偏差為13.57 mm。

3.3.2 動態試驗

以動態試驗方案模擬挖掘機進行3組作業場景試驗，試驗數據統計結果如表4所示。

從表4中3組試驗數據可以看出，在不同的試驗動作下，、、3個軸向坐標的解算值與真實值（驗證值）之間的平均絕對偏差和均方根偏差均小于20 mm，且絕對偏差≤30 mm的數據占比均不低于95.35%。同一時刻，解算值與真實值（驗證值）兩坐標點間的距離均方根偏差分別為27.49、26.30和23.50 mm，均小于30 mm。

如圖6所示，為挖掘機鏟斗測量點在動態試驗組1動作過程中，由鏟斗測量點的三維坐標解算值和真實值（驗證值）擬合成的三維空運動間軌跡圖，挖掘機鏟斗末端三維坐標解算值與全站儀實測值的運動軌跡變化一致。

表4 挖掘機鏟斗末端三維坐標動態試驗結果

圖6 鏟斗末端測量點空間軌跡圖

試驗結果表明基于BDS和IMU的挖掘機鏟斗位姿測量方法能準確測量鏟斗位姿，、、3個軸向均方根誤差均小于20 mm，解算值與真實值兩坐標點間的距離均方根誤差均小于30 mm，在實現智能化、自動化作業的同時滿足機械挖土施工要求和高標準農田建設標準要求。

4 結 論

1）提出了一種基于BDS和IMU的挖掘機鏟斗位姿測量方法，利用雙天線BDS和IMU傳感器測量車體的位姿，采用IMU測量挖掘機各執行機構的姿態角信息，設計了挖掘機鏟斗位姿測量系統。

2）設計了一種基于BDS和IMU的挖掘機鏟斗位姿解算算法，并基于雙天線BDS和IMU的卡爾曼濾波融合算法獲取車體的位置和姿態，建立姿態旋轉矩陣解算得到基于施工本地坐標系下鏟斗末端的三維坐標。

3）以山河swe40UF智能挖掘機進行試驗，將解算值與全站儀實測值比較，結果表明挖掘機鏟斗末端三維坐標解算值與全站儀實測值的運動軌跡變化一致，同一時刻空間兩坐標點距離均方根偏差小于30 mm，3個軸向坐標的動態測量均方根偏差均在20 mm內，絕對偏差≤30 mm的數據占比不低于95.35%，該方法可為挖掘機鏟斗三維坐標實時解算和精準施工提供精確測量和智能引導，滿足工程機械挖填土施工質量驗收中國國家標準要求和高標準基本農田建設標準要求。

[1] 葛磊，董致新，李運華，等. 系列化液壓挖掘機數字樣機研究[J]. 機械工程學報，2019，55(14)：186-196.

Ge Lei, Dong Zhixin, Li Yunhua, et al. Research on digital prototypes of serial hydraulic excavators[J]. Journal of Mechanical Engineering, 2019, 55(14): 186-196. (in Chinese with English abstract)

[2] Lee S U, Chang P H. Control of a heavy-duty robotic excavator using time delay control with integral sliding surface[J]. Control Engineering Practice, 2002, 10(7): 697-711.

[3] Papadopoulos E, Mu B, Frenette R. On modeling, identification, and control of a heavy-duty electrohydraulic harvester manipulator[J]. IEEE/ASME Transactions on Mechatronics, 2003, 8(2): 178-187.

[4] Yusof A A, Saadun M, Khairi M M, et al. Position control mathematical modelling and operational evaluation of tele-operated electro-hydraulic actuator (T-EHA)[J]. Applied Mechanics and Materials, 2015, 4014(773-774).

[5] 李傲傲. 挖掘機姿態檢測系統研究[D]. 泉州：華僑大學，2019.

Li Ao'ao. The Research on Attitude Detection System of Excavator[D]. Quanzhou: Huaqiao University, 2019. (in Chinese with English abstract)

[6] 李運華，范茹軍，楊麗曼，等. 智能化挖掘機的研究現狀與發展趨勢[J]. 機械工程學報，2020，56(13)：165-178.

Li Yunhua, Fan Rujun, Yang Liman, et al. Research Status and Development Trend of Intelligent Excavators [J]. Journal of Mechanical Engineering, 2020, 56(13): 165-178. (in Chinese with English abstract)

[7] Liang C J, Lundeen K M, Mcgee W, et al. A vision-based marker-less pose estimation system for articulated construction robots[J]. Automation in Construction, 2019, 104: 80-94.

[8] Yamamoto H, Moteki M, Ootuki T, et al. Development of the autonomous hydraulic excavator prototype using 3-D information for motion planning and control[J]. Transactions of the Society of Instrument and Control Engineers, 2012, 48(8): 488-497.

[9] Gu J, Ma X D, Ni J F. Linear and nonlinear control of a robotic excavator[J]. Journal of Central South University, 2012, 19(7): 1823-1831.

[10] 牛大偉. 基于MEMS傳感器的挖掘機姿態檢測系統的研究[D]. 泉州：華僑大學, 2015.

Niu Dawei. The Research of the Excavator Attitude Detection System Based on MEMS Sensor[D]. Quanzhou: Huaqiao University, 2015. (in Chinese with English abstract)

[11] 李海虹，解晶琳，鄒久禮，等. 液壓傳動機構位姿檢測方法研究[J]. 實驗技術與管理，2015，32(4)：33-35.

Li Haihong, Xie Jinglin, Zou Jiuli, et al. Study on position and orientation measurement for a hydraulic transmission mechanism[J].Experimental Technology and Management, 2015, 32(4): 33-35. (in Chinese with English abstract)

[12] 倪佳敏，馬偉，童欣，等. 基于視覺的挖掘機位姿測量虛擬傳感器研究[J]. 電子測量技術，2022，45(9)：44-49.

Ni Jiamin, Ma Wei, Tong Xin, et al. Vision-based virtual sensor research for excavator position measurement[J].Electronic Measurement Technology, 2022, 45(9): 44-49. (in Chinese with English abstract)

[13] Xu J Q, Yoon H S. Vision-based estimation of excavator manipulator pose for automated grading control[J]. Automation in Construction, 2019, 98: 122-131.

[14] Mulligan I J, Mackworth A K, Lawrence P D. A model-based vision system for manipulator position sensing[C]. Workshop on Interpretation of 3D Scenes, Austin, TX, USA, 1989.

[15] 王海波，鄒海龍，張如照. 基于視覺測量的挖掘機工作裝置姿態測量系統[J]. 農業機械學報，2015，46(4)：302-308.

Wang Haibo, Zou Hailong, Zhang Ruzhao. Attitude measurement system for excavator’s manipulator based on vision measurement[J]. Transactions of the Chinese Society for Agricultural Machinery, 2015, 46(4): 302-308. (in Chinese with English abstract)

[16] 朱建新，沈東羽，吳鈧. 基于激光點云的智能挖掘機目標識別[J]. 計算機工程，2017，43(1)：297-302.

Zhu Jianxin, Shen Dongyu, Wu Kang. Target recognition for intelligent excavator based on laser point cloud[J]. Computer Engineering, 2017, 43(1): 297-302. (in Chinese with English abstract)

[17] 周云成，許童羽，鄧寒冰，等. 基于自監督學習的溫室移動機器人位姿跟蹤[J]. 農業工程學報，2021，37(9)：263-274.

Zhou Yuncheng, Xu Tongyu, Deng Hanbing, et al. Self-supervised pose estimation method for a mobile robot in greenhouse[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(9): 263-274. (in Chinese with English abstract)

[18] 李晨陽，彭程，張振乾，等. 融合里程計信息的農業機器人定位與地圖構建方法[J]. 農業工程學報，2021，37(21)：16-23.

Li Chenyang, Peng Cheng, Zhang Zhenqian, et al. Positioning and map construction for agricultural robots integrating odometer information[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(21): 16-23. (in Chinese with English abstract)

[19] 班朝，任國營，王斌銳, 等. 基于IMU的機器人姿態自適應EKF測量算法研究[J]. 儀器儀表學報，2020，41(2)：33-39.

Ban Zhao, Ren Guoying, Wang Binrui, et al. Research on self-adaptive EKF algorithm for robot attitude measurement based on IMU[J]. Chinese Journal of Scientific Instrument, 2020, 41(2): 33-39. (in Chinese with English abstract)

[20] 趙自雨，馮仲科，田藝，等. UWB定位攝影測樹儀設計與試驗[J]. 農業工程學報，2020，36(17)：167-173.

Zhao Ziyu, Feng Zhongke, Tian Yi, et al. Design and test of photographic dendrometer based on Ultra Wide Band (UWB) positioning[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2020, 36(17): 167-173. (in Chinese with English abstract)

[21] 戴厚德，曾現萍，游鴻修，等. 基于光學運動跟蹤系統的機器人末端位姿測量與誤差補償[J]. 機器人，2019，41(2)：206-215.

Dai Houde, Zeng Xianping, You Hongxiu, et al. Pose measurement and error compensation of the robot end-effector based on an optical tracking system[J]. Robot, 2019, 41(2): 206-215. (in Chinese with English abstract)

[22] Mahmood B, Han S U, Seo J. Implementation experiments on convolutional neural network training using synthetic images for 3D pose estimation of an excavator on real images[J]. Automation in Construction, 2022, 133: 103996.

[23] 鐘銀，薛夢琦，袁洪良. 智能農機GNSS/INS組合導航系統設計[J]. 農業工程學報，2021，37(9)：40-46.

Zhong Yin, Xue Mengqi, Yuan Hongliang. Design of the GNSS/INS integrated navigation system for intelligent agricultural machinery[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(9): 40-46.(in Chinese with English abstract)

[24] 趙福生，楊宏瑞，薛艷麗. GNSS與IMU集成原理及其改進分析[J]. 測繪通報，2016(6)：60-63.

Zhao Fusheng, Yang Hongrui, Xue Yanli. GNSS and IMU integration principle and improvement analysis[J]. Bulletin of Surveying and Mapping, 2016(6): 60-63. (in Chinese with English abstract)

[25] Takai R, Yang L , Noguchi N. Development of crawler-type robot tractor based on GNSS and IMU[J]. IFAC Proceedings, 2013, 46(4): 95-98.

[26] 景云鵬，劉剛，金志坤. GNSS雙天線結合AHRS測量農田地形[J]. 農業工程學報，2019，35(21)：166-174.

Jing Yunpeng, Liu Gang, Jin Zhikun. Topographic survey of farmland based on GNSS dual antenna combined with AHRS[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(21): 166-174. (in Chinese with English abstract)

[27] 許江寧，朱濤，卞鴻巍. GPS姿態測量技術綜述[J]. 海軍工程大學學報，2003，15(3)：17-23.

Xu Jiang, Zhu Tao, Bian Hongwei. Review on GPS attitude determination[J]. Journal of Naval University of Engineering, 2003, 15(3): 17-23. (in Chinese with English abstract)

[28] 陳云, 何艷. 基于GNSS姿態與電機編碼器的農機轉向角度測量系統研制[J]. 農業工程學報, 2021,37(10):10-17.

Chen Yun, He Yan. Development of agricultural machinery steering wheel angle measuring system based on GNSS attitude and motor encoder[J]. Transactions of the Chinese Society of Agricultural Engineering, 2021,37(10):10-17.(in Chinese with English abstract)

[29] He J, Luo X W, Zhang Z G, et al. Positioning correction method for rice transplanters based on the attitude of the implement[J]. Computers and Electronics in Agriculture, 2020, 176: 105598.

[30] Hu L, Yang W W, He J, et al. Roll angle estimation using low cost MEMS sensors for paddy field machine[J]. Computers and Electronics in Agriculture, 2019,158: 183-188.

Method and experiments of excavator bucket position and attitude measurement using BDS and IMU

Hu Lian1,2, Peng Jingyi1,2, Lai Sangyu1, Feng Dawen1,2, Chen Gaolong1,2, Wang Chenyang1,2, Luo Xiwen1,2※

(1.,,510642,; 2.,510642,)

High precision and intelligent degree of excavator construction can be perferred in farmland reconstruction in modern agriculture. It is a high demand to real-time acquire the bucket position and attitude for the intelligent and accurate operation of excavator. In this study, a series of approaches were proposed to measure the bucket position and attitude of excavator using BeiDou Navigation Satellite System (BDS) and Inertial Measurement Unit (IMU). A real-time solution model was established for the three-dimensional coordinates of the excavator bucket end: Firstly, the body parameters of excavator were measured to establish the body coordinate system. The IMU attitude sensors were installed at the appropriate positions of the boom, stick, and bucket of the excavator, in order to measure the attitude angle information of each actuator in the excavator. The data was finally collected to obtain the three-dimensional coordinates of the bucket end under the excavator body coordinate system; Then, the BDS dual antenna was installed on the roof to obtain the yawing angle of vehicle body and spatial position. The IMU attitude sensor was also installed on the vehicle body for the rolling angle and pitching angle of the vehicle body. Then, the Kalman filtering algorithm is used to fuse the dual antenna BDS and IMU output high-frequency and high-precision position and attitude information to construct attitude rotation matrix. Among them, the three-dimensional coordinates of the excavator bucket end under the vehicle body coordinate system were rotated to the local construction coordinate system. Static and dynamic tests were carried out to simulate the actual construction scene of the excavator. In the static test, the three-dimensional coordinates of the prism were continuously collected at the excavator bucket under each group of test actions by the total station under different heading angles and attitudes of the simulated operating excavator body and mechanical arm. The deviation was then calculated between the measured of total station and solution of bucket pose measurement. The results show that the new model performed better to accurately measure the three-dimensional coordinates at the end of the excavator bucket. The maximum absolute deviations were 17.69, 14.99, and 11.68 mm (all less than 20 mm) in the,, andaxial coordinates of bucket measuring points, respectively. The minimum deviation, maximum deviation and average deviation of the distance between the two coordinate points of the calculated and the real value (verification value) were 7.40, 20.65, and 13.57 mm, respectively. In the dynamic test, the excavator was operated in test group 1: where the body heading remained still, as each mechanism arm acted; Test group 2: the vehicle body rotated in the heading, and each mechanism arm remained still; Test group 3: The body heading and each mechanism arm acted at the same time, in order to simulate the actual construction operation scene, such as deep excavation, leveling, and slope brushing in the excavator construction. The total station was used to automatically follow the prism on the bucket. The three-dimensional coordinates of the bucket were collected in real time to verify the three-dimensional coordinate calculation of the bucket end. The results show that the average absolute deviation and root mean square deviation were less than 20 mm between the calculated values of the,,three axial coordinates and the real value under different test actions. The proportion of the data with the absolute deviation less than 30 mm were not less than 95.35%. The calculated three-dimensional coordinates at the end of the excavator bucket were better consistent with the movement track change of the measured total station. The root mean square deviations of the distance between the two coordinate points of the calculated and the real value were 27.49, 26.30, and 23.50 mm, respectively, which were less than 30 mm. The accurate measurement for the position and posture of the excavator bucket can provide a practical basis for the intelligent guidance of the precise construction of the excavator.

agricultural machinery; excavator; BDS; IMU; coordinate transformation; position and attitude measurement

10.11975/j.issn.1002-6819.2022.23.002

TU621; S222.5

A

1002-6819(2022)-23-0012-08

胡煉，彭靖怡，賴桑愉，等. 基于BDS和IMU的挖掘機鏟斗位姿測量方法與試驗[J]. 農業工程學報，2022，38(23)：12-19.doi：10.11975/j.issn.1002-6819.2022.23.002 http://www.tcsae.org

Hu Lian, Peng Jingyi, Lai Sangyu, et al. Method and experiments of excavator bucket position and attitude measurement using BDS and IMU[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2022, 38(23): 12-19. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2022.23.002 http://www.tcsae.org

2022-09-25

2022-11-22

嶺南現代農業科學與技術廣東省實驗室科研項目（NT2021009）；廣東省科技計劃項目（2021B1212040009）；國家自然科學基金項目（32101623）

胡煉，博士，青年教授，研究方向為智能農機裝備和無人農場。Email：lianhu@scau.edu.cn

羅錫文，教授，中國工程院院士，研究方向為智能農機裝備研究。Email：xwluo@scau.edu.cn