郭大山 于艷 李壯 徐樹生 張柏博

摘 要：在山地丘陵地帶果樹植保機(jī)飛行過(guò)程中，擁有對(duì)地形的全感知能力顯得尤為重要，丘陵地帶植保機(jī)作業(yè)過(guò)程中，地形起伏較大，需要植保機(jī)具有避障能力?；诩す饫走_(dá)的無(wú)人機(jī)避障系統(tǒng)使用激光雷達(dá)作為主傳感器，機(jī)載電腦融合IMU所給出的飛機(jī)狀態(tài)量信息，綜合評(píng)判飛行器所處的狀態(tài)，通過(guò)對(duì)比和預(yù)測(cè)飛機(jī)狀態(tài)實(shí)現(xiàn)對(duì)障礙物的躲避。

關(guān)鍵詞：激光雷達(dá);場(chǎng)勢(shì)能算法;地形跟隨;避障;無(wú)人機(jī)

DOI：10.16640/j.cnki.37-1222/t.2019.05.146

0 引言

近年來(lái)，農(nóng)用植保無(wú)人機(jī)發(fā)展迅速。與傳統(tǒng)人工施藥方式相比，植保無(wú)人機(jī)噴灑速度快效率高，能有效的降低人工成本，提高工作效率。由植保機(jī)械代替人工噴灑農(nóng)藥將是未來(lái)農(nóng)業(yè)的發(fā)展趨勢(shì)。但是目前適用于平原大田作物的植保機(jī)因無(wú)法進(jìn)行有效的地形跟隨和避障并不適合在山地丘陵地區(qū)進(jìn)行果樹植保作業(yè)。制約我國(guó)果樹植保行業(yè)無(wú)人機(jī)推廣的主要瓶頸問(wèn)題已由飛行平臺(tái)與控制系統(tǒng)的穩(wěn)定性問(wèn)題轉(zhuǎn)化為無(wú)人植保機(jī)智能化程度低、環(huán)境感知功能缺失的問(wèn)題。完善的自主避障系統(tǒng)將能夠在很大程度上減少因人員操作失誤造成的無(wú)人機(jī)損壞，增加作業(yè)效率。本文主要利用pixhawk飛控與HOKUYO UST-30LX激光雷達(dá)完成植保無(wú)人機(jī)自主避障的功能并做出相應(yīng)的算法優(yōu)化。

1 系統(tǒng)構(gòu)架與工作原理

本文研制了一種新型的適用于植保無(wú)人機(jī)上的基于激光雷達(dá)的避障系統(tǒng)，這套避障系統(tǒng)由激光雷達(dá)、Intel mini計(jì)算機(jī)系統(tǒng)（以下簡(jiǎn)稱機(jī)載計(jì)算機(jī)）、USB轉(zhuǎn)GPIO端口控制模塊、供電模塊構(gòu)成，其系統(tǒng)拓?fù)鋱D如下圖所示：

無(wú)人機(jī)避障系統(tǒng)使用激光雷達(dá)模塊獲取周圍點(diǎn)云信息，激光雷達(dá)模塊可以直接輸出所探測(cè)到的周圍15M范圍以內(nèi)的障礙物的點(diǎn)的位置信息、角度信和距離信息到計(jì)算機(jī)中，機(jī)載計(jì)算機(jī)通過(guò)處理激光雷達(dá)所給出的信息生成真實(shí)環(huán)境數(shù)據(jù)，提供給避障程序做避障判定，程序判定需要進(jìn)行避障以后，發(fā)送相應(yīng)的控制代碼和障礙物位置信息給飛行控制系統(tǒng)，飛行控制系統(tǒng)自行判斷是否需要進(jìn)行避障，是否按照激光雷達(dá)避障系統(tǒng)所給出的避障操作進(jìn)行避障。并且返回若干個(gè)真值給避障系統(tǒng)，供給避障系統(tǒng)做下一次的判斷使用。

在這套激光雷達(dá)避障系統(tǒng)中，機(jī)載計(jì)算機(jī)起到對(duì)所收集的所有信息進(jìn)行匯總處理的一個(gè)作用，因?yàn)閘inux不是實(shí)時(shí)操作系統(tǒng)，需要對(duì)linux系統(tǒng)打補(bǔ)丁使它具有實(shí)時(shí)操作系統(tǒng)的特性，在毫秒級(jí)系統(tǒng)時(shí)間內(nèi)完成對(duì)程序系統(tǒng)樹優(yōu)先級(jí)的判定和檢測(cè)，對(duì)比激光雷達(dá)所給出的數(shù)據(jù)信息完成對(duì)障礙物位置的判定和距離檢測(cè)，同時(shí)給飛控輸出障礙物位置信息和避障指令，機(jī)載計(jì)算機(jī)通過(guò)Mavlink協(xié)議獲取飛控實(shí)時(shí)的狀態(tài)量，會(huì)有一定的延遲。為了能縮短避障指令執(zhí)行所需要的時(shí)間，我們把控制飛行器避障的過(guò)程交給飛控來(lái)完成。

機(jī)載計(jì)算機(jī)只提供必要的障礙物信息和經(jīng)過(guò)計(jì)算以后的躲避障礙物所規(guī)劃的路線，飛控通過(guò)融合本身GPS信息、IMU所給出的傾斜角度信息、加速度信息判斷躲避障礙物的路線是否擁有可執(zhí)行力，如果判斷按照目前狀態(tài)會(huì)撞擊到障礙物，就按照機(jī)載計(jì)算機(jī)所給出的路徑進(jìn)行回避。如果飛控根據(jù)當(dāng)前姿態(tài)計(jì)算并不會(huì)撞上障礙物，就會(huì)按照目前狀態(tài)繼續(xù)執(zhí)行飛行任務(wù)。

2 避障系統(tǒng)硬件搭配

在硬件上，載機(jī)我們使用基于pixhawk2飛控的植保無(wú)人機(jī)，pixhawk2飛行控制系統(tǒng)搭載ardupilot3.5固件，可以實(shí)現(xiàn)跟機(jī)載計(jì)算機(jī)通過(guò)Mavlink通訊協(xié)議進(jìn)行通訊。

避障感知系統(tǒng)使用HOKUYO-UTM-30LX激光雷達(dá)，可以實(shí)現(xiàn)0.35-30M距離內(nèi)的比較精準(zhǔn)的測(cè)距，超過(guò)0.35M距離激光雷達(dá)會(huì)輸出一個(gè)激光點(diǎn)強(qiáng)度值，可以通過(guò)激光點(diǎn)強(qiáng)度值和位置信息的變換來(lái)進(jìn)行真值點(diǎn)的及計(jì)算，排除0.35M以外的噪點(diǎn)，實(shí)現(xiàn)較為準(zhǔn)確的距離的測(cè)算。

3 STM32控制算法實(shí)現(xiàn)

本方案采用激光雷達(dá)傳感器獲取飛行環(huán)境信息，對(duì)障礙物的相對(duì)距離、角度等進(jìn)行計(jì)算。其優(yōu)點(diǎn)為精度高，弱光環(huán)境下表現(xiàn)較好，有不同的防護(hù)等級(jí)，可在噴灑農(nóng)藥的環(huán)境下正常使用;缺點(diǎn)為易受強(qiáng)光干擾，成本較高。

如圖2所示，激光雷達(dá)的發(fā)射器發(fā)射出一束激光，激光光束遇到物體后，經(jīng)過(guò)漫反射，返回至激光接收器，雷達(dá)根據(jù)發(fā)送和接收信號(hào)的時(shí)間間隔乘以光速，再除以2，即可計(jì)算出發(fā)射器與物體的距離。

本項(xiàng)目選擇了HOKUYO-UTM-30LX激光雷達(dá)作為傳感器，配合無(wú)人機(jī)上搭載的微型計(jì)算機(jī)來(lái)完成避障。UTM-30LX擁有30m， 270°測(cè)量范圍，掃描頻率最高可達(dá)到40Hz，擁有IP64防護(hù)等級(jí)可以在噴灑藥液的同時(shí)正常使用。激光雷達(dá)將掃描到的周圍環(huán)境數(shù)據(jù)通過(guò)SCIP2.0協(xié)議輸出到機(jī)載計(jì)算機(jī)中，使用Linux下的ROS機(jī)器人操作系統(tǒng)對(duì)避障信息進(jìn)行處理、分類歸納。

ROS是一種分布式模塊化的開源軟件框架，可以運(yùn)行在一個(gè)計(jì)算機(jī)上或者分布不同位置的多個(gè)計(jì)算機(jī)上。這些執(zhí)行程序被按照功能包和功能包集的方式進(jìn)行分組，統(tǒng)一管理發(fā)布的消息和每個(gè)消息處理模塊所接受的消息，實(shí)現(xiàn)機(jī)器人研發(fā)過(guò)程中的代碼復(fù)用，減輕工作量。

ROS系統(tǒng)完成整個(gè)避障消息的處理后會(huì)輸出一個(gè)不具體的位置坐標(biāo)，可以虛擬為一個(gè)可變化的空間向量，通過(guò)解析空間向量可以得知ROS系統(tǒng)所給出的具體避障角度和避障距離，機(jī)載計(jì)算機(jī)通過(guò)Mavlink協(xié)議跟飛控進(jìn)行通訊，Mavlink協(xié)議中封裝了關(guān)于飛控所接受的信息的種類和具體接受的方式。

在植保機(jī)自動(dòng)避障控制中，使用了人工勢(shì)場(chǎng)法定義運(yùn)動(dòng)規(guī)律和預(yù)測(cè)運(yùn)動(dòng)軌跡。人工勢(shì)場(chǎng)法的基本思想就是將無(wú)人機(jī)在環(huán)境中的運(yùn)動(dòng)視為一種虛擬的人工場(chǎng)中的運(yùn)動(dòng)。障礙物對(duì)無(wú)人機(jī)產(chǎn)生斥力，形成斥力場(chǎng)，隨著無(wú)人機(jī)遠(yuǎn)離障礙而減弱;目標(biāo)對(duì)無(wú)人機(jī)產(chǎn)生引力，形成引力場(chǎng)。引力場(chǎng)和斥力場(chǎng)共同作用于無(wú)人機(jī)，使無(wú)人機(jī)不斷調(diào)整自己運(yùn)動(dòng)的速度和方向，逐漸接近并最終安全達(dá)到目標(biāo)。

如圖3所示，無(wú)人機(jī)的目標(biāo)位置為PG，且環(huán)境中存在障礙物 Oi （i1，2，3，n）n為障礙物的數(shù)目，無(wú)人機(jī)在位置p時(shí)的人工勢(shì)場(chǎng) Uart（p）可表示為目標(biāo)勢(shì)場(chǎng) UG（p） 和障礙勢(shì)場(chǎng) U（p） 的和，進(jìn)而得出作用于無(wú)人機(jī)的抽象力 F（p） 為：

勢(shì)場(chǎng)函數(shù)的選擇比較靈活，一般具體情況具體設(shè)計(jì)。建立的目標(biāo)勢(shì)場(chǎng)和障礙物勢(shì)場(chǎng)函數(shù)如下：

其中：k、為常數(shù);是無(wú)人機(jī)到障礙物的最短距離，為一閾值。

首先，根據(jù)障礙物的距離和方向計(jì)算斥力，根據(jù)目標(biāo)點(diǎn)距離和方向計(jì)算引力，最后計(jì)算合力大小和方向，確定合力以后，把合力轉(zhuǎn)化成轉(zhuǎn)化成無(wú)人機(jī)的速度矢量，通過(guò)MAVlink協(xié)議，將速度設(shè)定值發(fā)送給飛控，通過(guò)控制速度來(lái)實(shí)現(xiàn)避障。

初期對(duì)算法進(jìn)行了實(shí)際測(cè)試，實(shí)驗(yàn)環(huán)境室外，飛行器靜止懸停在墻壁前，測(cè)試人員操控?zé)o人機(jī)以2米每秒的速度飛向墻壁，在距離墻壁5m處，無(wú)人機(jī)檢測(cè)到障礙，執(zhí)行避障任務(wù)，在距離墻壁2.3米處速度降為0并開始后退，最后懸停在距墻壁5m處，實(shí)現(xiàn)避障。

在對(duì)障礙物距離測(cè)定上，如果激光點(diǎn)超過(guò)20M就會(huì)出現(xiàn)一定的誤差，機(jī)載電腦通過(guò)獲取IMU的數(shù)據(jù)，得到飛行器的姿態(tài)信息，通過(guò)兩次、三次掃描同一位置的激光點(diǎn)數(shù)據(jù)做加速度計(jì)的數(shù)據(jù)融合，得到激光點(diǎn)位置的幾個(gè)相對(duì)位置，在視場(chǎng)圖上經(jīng)過(guò)點(diǎn)校驗(yàn)表現(xiàn)出來(lái)，幾個(gè)不同值之間進(jìn)行聯(lián)合平差運(yùn)算，取得泊松分布中最靠近0.7位置的點(diǎn)為真值點(diǎn)輸出，用來(lái)降低20M外數(shù)據(jù)噪點(diǎn)量，提升數(shù)據(jù)質(zhì)量，提高測(cè)量精度。

4 試驗(yàn)結(jié)果和過(guò)程分析

如圖所示，激光雷達(dá)在進(jìn)行掃描的時(shí)候會(huì)掃描每一個(gè)激光點(diǎn)所到達(dá)物體的時(shí)間接受回激光接受器的時(shí)間，在實(shí)際激光雷達(dá)工作做工中所掃描的點(diǎn)可以簡(jiǎn)略認(rèn)為是一個(gè)270度的面，每個(gè)面上每間隔0.25度就有一個(gè)點(diǎn)，激光雷達(dá)得到這個(gè)點(diǎn)距離本身的具體位置和這個(gè)點(diǎn)與激光雷達(dá)的夾角。

避障系統(tǒng)中的激光雷達(dá)首先掃描每個(gè)點(diǎn)到本身激光雷達(dá)的角度和距離信息，然后獲取飛控IMU的數(shù)據(jù)姿態(tài)，根據(jù)數(shù)據(jù)姿態(tài)的三維空間中進(jìn)行向量累加，判定單點(diǎn)重復(fù)掃描誤差2CM以內(nèi)時(shí)，確定這個(gè)點(diǎn)是一個(gè)真值點(diǎn)，并標(biāo)注這個(gè)點(diǎn)為障礙物點(diǎn)，通過(guò)累計(jì)掃描從而形成一個(gè)視場(chǎng)圖，標(biāo)注出來(lái)每個(gè)障礙物與激光雷達(dá)本身的距離。

在綜合IMU的數(shù)據(jù)以后，實(shí)現(xiàn)對(duì)物體距離方位的感知。在外場(chǎng)測(cè)試過(guò)程中因?yàn)橹車h(huán)境比較空曠，所以機(jī)載計(jì)算機(jī)需要處理的數(shù)據(jù)量較小，可以非常迅速的實(shí)現(xiàn)外場(chǎng)環(huán)境視圖的掃描和搭建，超過(guò)20m距離的激光點(diǎn)認(rèn)為是無(wú)限遠(yuǎn)距離，距離10M以內(nèi)的障礙物并且在飛行器航路上的點(diǎn)認(rèn)為是障礙物，可以獲取比較好的避障效果。

5 結(jié)論

基于激光雷達(dá)的避障系統(tǒng)可以使得無(wú)人機(jī)有效的躲避障礙物，通過(guò)ROS系統(tǒng)環(huán)境的搭建可以實(shí)現(xiàn)簡(jiǎn)單便捷的避障設(shè)計(jì)，在比較短的時(shí)間上完成避障操作，經(jīng)過(guò)外場(chǎng)試驗(yàn)測(cè)試在非強(qiáng)光條件下避障效果較好。

山東省農(nóng)機(jī)裝備研發(fā)創(chuàng)新計(jì)劃項(xiàng)目，項(xiàng)目編號(hào)2017022620076

作者簡(jiǎn)介：郭大山（1995-），男，山東人，本科，初級(jí)工程師，研究方向：農(nóng)機(jī)測(cè)試技術(shù)。