全自主掃雪機(jī)器人車體運(yùn)動(dòng)控制算法研究

鄧健，李長勇，智泉卿

(新疆大學(xué)機(jī)械工程學(xué)院，新疆 烏魯木齊 830047)

1 引言

冬季，我國北部地區(qū)降雪頻繁，從1995?2017年從受災(zāi)牲畜、受災(zāi)面積、經(jīng)濟(jì)損失和受災(zāi)頻次統(tǒng)計(jì)表明[1]，大雪對(duì)人民日常出行帶來極大不便，隨著人民生活水平的提高，人工掃雪逐漸被大型機(jī)械化除雪設(shè)備取代。

但對(duì)于一些狹窄的區(qū)域，采用大型機(jī)械化除雪存在掃雪環(huán)境區(qū)域較單一，使用全自主掃雪機(jī)器人來進(jìn)行除雪可以有效清除狹窄庭院分布不規(guī)則積雪。通過對(duì)全自主掃雪機(jī)器人車體運(yùn)動(dòng)控制算法進(jìn)行研究和實(shí)驗(yàn)表明，掃雪機(jī)器人的上位機(jī)視覺傳感器融合超聲波傳感器和下位機(jī)多方向覆蓋算法相結(jié)合的方法進(jìn)行掃雪，相對(duì)傳統(tǒng)的控制算法可以有效提高行走偏差率、掃雪覆蓋率和減小除雪重復(fù)率。其次設(shè)計(jì)的全自主掃雪機(jī)器人內(nèi)部設(shè)置有多信息運(yùn)算處理的控制器中心，可以在寒冷的冬季有效降低環(huán)衛(wèi)工人的掃雪勞動(dòng)強(qiáng)度。

除此之外，通過對(duì)全自主掃雪機(jī)器人除雪的調(diào)查研究發(fā)現(xiàn)，掃雪機(jī)器人的智能化水平相對(duì)移動(dòng)機(jī)器人技術(shù)偏低，造成在作業(yè)過程中除雪不徹底和不干凈等現(xiàn)象。針對(duì)此現(xiàn)象，掃雪機(jī)的智能化研究對(duì)掃雪機(jī)器人技術(shù)的發(fā)展具有重要的意義。

2 掃雪機(jī)器人工作環(huán)境

全自主掃雪機(jī)器人工作環(huán)境，如圖1所示。庭院積雪分布情況為無規(guī)則狀，道路部分積雪分布符合“曼哈頓街道”模型，通過對(duì)常規(guī)的大型除雪設(shè)備進(jìn)行綜合評(píng)價(jià)，發(fā)現(xiàn)存在著掃雪機(jī)械設(shè)備笨重，掃雪效果不理想和成本較高現(xiàn)象。

圖1 掃雪區(qū)域場(chǎng)景Fig.1 Scene of the Snow Clearing Area

全自主微型掃雪機(jī)體積相對(duì)較小，對(duì)庭院狹窄地區(qū)除雪具有針對(duì)性特點(diǎn)。針對(duì)此問題提出了全自主掃雪機(jī)器人控制算法研究，此控制算法可以通過視覺傳感器提取出復(fù)雜環(huán)境中的積雪分布，把干擾點(diǎn)進(jìn)行淡化處理，同時(shí)采用超聲波傳感器進(jìn)行測(cè)距，使用多方向覆蓋算法進(jìn)行下位機(jī)優(yōu)先級(jí)分選執(zhí)行。

3 掃雪機(jī)器人系統(tǒng)

3.1 掃雪機(jī)器人車身機(jī)構(gòu)

掃雪機(jī)器人主要由推雪機(jī)構(gòu)、左右轉(zhuǎn)動(dòng)推板機(jī)構(gòu)、推板升降機(jī)構(gòu)、視覺部分、行走機(jī)構(gòu)、浮雪升降機(jī)構(gòu)、浮雪機(jī)構(gòu)等組成，如圖2所示。

圖2 掃雪機(jī)器人車身機(jī)構(gòu)Fig.2 Body Mechanism of the Snowplow Robot

掃雪機(jī)器人以車體中心為圓心，圓心向推板定義為X方向，平行地面且垂直X軸定義為Y軸方向，分別垂直X軸和Y軸且向上定義為Z軸。其中推雪機(jī)構(gòu)可在?Y0X左右45°轉(zhuǎn)動(dòng)和X0Y左右45°轉(zhuǎn)動(dòng)，可以把推板上的積雪推向左右側(cè)面；視覺傳感器下面裝有二軸云臺(tái)，云臺(tái)可在XY軸構(gòu)建的平面繞Z軸175°轉(zhuǎn)動(dòng)，同理也可以YZ軸構(gòu)建的平面繞X軸左右175°轉(zhuǎn)動(dòng)，視覺可以在多方位采集場(chǎng)景圖像信息。

掃雪機(jī)器人根據(jù)機(jī)構(gòu)設(shè)計(jì)原理以及實(shí)際掃雪環(huán)境需求，采取最優(yōu)化設(shè)計(jì)的主要性能參數(shù)，如表1所示。

表1 掃雪機(jī)器人性能參數(shù)Tab.1 Snowplow Robot Performance Parameters

3.2 掃雪機(jī)器人控制算法

3.2.1 控制算法設(shè)計(jì)

掃雪機(jī)器人分為上位機(jī)和下位機(jī)兩部分，其中主要由通訊設(shè)置圖像處理、信息采集和驅(qū)動(dòng)執(zhí)行三個(gè)方面?？刂扑惴鞒?，如圖3所示。

圖3 控制系統(tǒng)流程Fig.3 Process Control System

通訊設(shè)置根據(jù)用戶需求先選取網(wǎng)絡(luò)通訊或者usart232串口通信，然后使用視覺傳感器進(jìn)行圖片采集，通過對(duì)圖像的處理可以采集到掃雪目標(biāo)位置信息，最后完成上位機(jī)操控和下位機(jī)動(dòng)作執(zhí)行程序接口對(duì)接完成交互。其中圖像處理程序采用Microsoft Visual Studio2015且配置OpenCV2.44進(jìn)行編寫。信息采集使用STM32F103zet6芯片為核心的單片機(jī)，主要負(fù)責(zé)采集環(huán)境信息和接收上位機(jī)下達(dá)的指令，然后傳輸給下位機(jī)。驅(qū)動(dòng)執(zhí)行部分是由下位機(jī)驅(qū)動(dòng)執(zhí)行，當(dāng)下位機(jī)的行走驅(qū)動(dòng)電機(jī)接收到上位機(jī)界面發(fā)出的A、E、G、C、Y時(shí)，分別執(zhí)行前、后、左、右、后和停止動(dòng)作。當(dāng)執(zhí)行遠(yuǎn)程操控時(shí)當(dāng)上位機(jī)界面充當(dāng)采用TCP Client時(shí)，首先連接服務(wù)器分配的IP地址和端口號(hào)，其次進(jìn)行遠(yuǎn)程指令下達(dá)給下位機(jī)控制板，進(jìn)行發(fā)送給具體的對(duì)應(yīng)的執(zhí)行機(jī)構(gòu)。

3.2.2 人機(jī)交互界面

掃雪機(jī)器人上位機(jī)界面程序和下位機(jī)控制程序相互獨(dú)立運(yùn)行，通過usart232或TCP/IP通訊建立程序交互，當(dāng)不需要遠(yuǎn)程通訊時(shí)，可使用usart232通過hc?05進(jìn)行通訊，如圖4（a）所示。當(dāng)需要進(jìn)行遠(yuǎn)程通訊時(shí)可使用TCP/IP技術(shù)[2]，連接對(duì)應(yīng)服務(wù)器分配的IP 和端口號(hào)進(jìn)行通訊，如圖4（b）所示。上位機(jī)界面采用Micro?soft Visual Basic 6.0開發(fā)，其中主要功能包括視覺部分、手動(dòng)按鍵操控、語音控制、遠(yuǎn)程操控等，如圖4所示。

圖4 人機(jī)交互界面Fig.4 Interactive Interface of Human?Computer

掃雪機(jī)器人下位機(jī)程序在Keil uVision5 上編寫，其中常編寫的程序有串口通訊、PWM脈沖寬度、定時(shí)器、協(xié)議、SPI通訊和主函數(shù)具體功能實(shí)現(xiàn)等，可以通過設(shè)置中斷優(yōu)先級(jí)順序進(jìn)行依次響應(yīng)。

3.2.3 不同控制算法比較

在掃雪機(jī)器人在進(jìn)行掃雪時(shí)會(huì)遇到各種復(fù)雜多變障礙物環(huán)境，簡(jiǎn)化為二維平面點(diǎn)的集合并且選用最有效的直接搜索方法及A*算法進(jìn)行仿真[3]:

式中：f(k)—在k位置對(duì)象啟動(dòng)點(diǎn)與終點(diǎn)的代價(jià)估計(jì)；

g(k)—在k位置對(duì)象啟動(dòng)點(diǎn)與終點(diǎn)實(shí)際代價(jià)估計(jì)；

h(k)—在k位置啟動(dòng)點(diǎn)與終點(diǎn)最佳代價(jià)估計(jì)。

實(shí)際掃雪機(jī)器人在運(yùn)輸過程中可能遇到多通道路況擇優(yōu)問題，A*算法則會(huì)預(yù)先啟動(dòng)式多通道路徑選出一條最優(yōu)通道。如圖5所示。通過對(duì)A*算法的研究發(fā)現(xiàn)，當(dāng)遇到交叉路口或者障礙物時(shí)可以選擇路徑的優(yōu)化和避障，選取出最優(yōu)一條道路。當(dāng)A*算法使用在掃雪路徑優(yōu)化中，面臨著掃雪面積覆蓋率低等問題。

圖5 A*算法Fig.5 A* Algorithm

采用啟發(fā)式搜索算法在路徑規(guī)劃過程中，通過對(duì)全局面積劃分為各個(gè)局部子區(qū)域，然后在子區(qū)域內(nèi)建立啟發(fā)式沿邊清掃，可使覆蓋率維持在95%，同時(shí)重復(fù)率維持在25%以下［4］。在啟發(fā)式搜索算法和A*算法的基礎(chǔ)上提出了一種新的路徑?方向解析算法，掃雪機(jī)器人的掃雪路線結(jié)合最優(yōu)算法對(duì)局部環(huán)境的避障分析，如圖6所示。

圖6 避障控制算法原理Fig.6 Algorithm Principle of Obstacle Avoidance Control

掃雪機(jī)器人坐標(biāo)系為x0y，障礙物1的坐標(biāo)為（x1，y1），障礙物2的坐標(biāo)為（x2，y2），障礙物3的坐標(biāo)為（x3，y3），目標(biāo)點(diǎn)位置為（x4，y4），出發(fā)點(diǎn)位置為（m，n）。其中出發(fā)點(diǎn)離障礙物（1～3）的距離依次為a、b和c，距離目標(biāo)點(diǎn)距離為d，出發(fā)點(diǎn)與目標(biāo)點(diǎn)之間距離線段與機(jī)器人坐標(biāo)系x軸之間夾角為i，角度j則為世界坐標(biāo)系與搬運(yùn)機(jī)器人坐標(biāo)系下夾角。最后將目標(biāo)點(diǎn)坐標(biāo)轉(zhuǎn)換為在世界坐標(biāo)系下的坐標(biāo)，進(jìn)行優(yōu)化算法分析和求解。通過對(duì)覆蓋算法［5?6］和A*算法的研究發(fā)現(xiàn)，采用分級(jí)優(yōu)先順序方法可以降低掃雪重復(fù)率，通過設(shè)定右下角W為掃雪規(guī)劃起始點(diǎn)，從路徑規(guī)劃起始點(diǎn)到1的方向定義為第一優(yōu)先級(jí)，從起始點(diǎn)到2的位置為第二優(yōu)先級(jí)，其中先后優(yōu)先級(jí)順序設(shè)置在下位機(jī)程序內(nèi)部，通過搶占優(yōu)先級(jí)和響應(yīng)優(yōu)先級(jí)的分配進(jìn)行，當(dāng)遇到障礙物時(shí)可以借助視覺傳感器融合超聲波測(cè)距進(jìn)行優(yōu)化處理，上下位機(jī)通過usart232串口實(shí)時(shí)通訊，最終通過實(shí)驗(yàn)過程可使重復(fù)率控制在18%內(nèi)，覆蓋率維持在95%以上。

首先以世界坐標(biāo)系下物體位置為絕對(duì)坐標(biāo)系建立掃雪機(jī)器人相對(duì)坐標(biāo)，從而可以計(jì)算物體在相對(duì)坐標(biāo)系下對(duì)應(yīng)的絕對(duì)坐標(biāo)。

如世界坐標(biāo)系下目標(biāo)點(diǎn)位置坐標(biāo)公式：

同理可求出障礙物1、2、3坐標(biāo)公式。

3.2.4 視覺處理

由于掃雪環(huán)境和光照強(qiáng)度影響，需要對(duì)視覺傳感器采集圖片進(jìn)行濾波處理，為了能準(zhǔn)確獲取掃雪目標(biāo)信息，需要對(duì)采集照片進(jìn)行灰度處理→二值化→降噪［7］，如圖7所示。

圖7 掃雪環(huán)境處理前后Fig.7 Before and After Snow Removal Environment Treatment

在掃雪場(chǎng)景中需要對(duì)積雪覆蓋的區(qū)域進(jìn)行提取，并設(shè)置灰度值為R=0.3a+0.59b+0.11c[8]突出目標(biāo)A，設(shè)定為白色，同時(shí)把背景淡化并設(shè)置為黑色。

視覺遠(yuǎn)距離存在著畸變現(xiàn)象，并且遠(yuǎn)離與畸變成正比，可以用以下公式來表示[9?10]:

在畸變變化允許范圍內(nèi)進(jìn)行處理可以有效降低定位誤差。

4 實(shí)驗(yàn)測(cè)試數(shù)據(jù)

實(shí)驗(yàn)過程進(jìn)行50次，實(shí)驗(yàn)中掃雪機(jī)運(yùn)行的路線場(chǎng)景為學(xué)?；@球場(chǎng)內(nèi)，任取一塊（20×10)m直線路面進(jìn)行實(shí)驗(yàn)，如表2所示。首先設(shè)定起點(diǎn)和終點(diǎn)后，從中均勻截取10個(gè)部分，從履帶在雪地的壓痕坐標(biāo)可計(jì)算出第一部分偏移量誤差和角度。然后第二部分繼續(xù)測(cè)出偏移量誤差和角度，依次遞增，按照同樣方法和步驟測(cè)完剩下的8部分偏移量誤差和角度，實(shí)驗(yàn)中每部分進(jìn)行5次，取失誤次數(shù)相對(duì)少的三次運(yùn)行軌跡作為實(shí)驗(yàn)樣本，求取平均值和方差。

表2 試驗(yàn)路徑數(shù)據(jù)Tab.2 Test Path Data

起點(diǎn)偏差角度和起點(diǎn)偏移距離，如圖8所示。從圖中可以看出在100cm內(nèi)，起點(diǎn)偏移距離維持在（±2.17）%內(nèi)，起點(diǎn)偏差角度維持在（±0.37）%內(nèi)，根據(jù)得出結(jié)論可合理定制掃雪機(jī)器人帶有障礙物的掃雪路線且符合掃雪機(jī)器人掃雪的精確控制要求。通過對(duì)實(shí)驗(yàn)數(shù)據(jù)的分析發(fā)現(xiàn)產(chǎn)生誤差原因主要有雪面和履帶之間產(chǎn)生的打滑和人工測(cè)量精確度誤差，其中起點(diǎn)偏角為了避免誤差產(chǎn)生，本實(shí)驗(yàn)采取大量重復(fù)實(shí)驗(yàn)作為預(yù)備實(shí)驗(yàn)，最終完成實(shí)驗(yàn)數(shù)據(jù)精確度的要求。

5 結(jié)論

在現(xiàn)有的全自主掃雪機(jī)器人控制算法上進(jìn)行優(yōu)化融合，采用圖像處理融合超聲波傳感器測(cè)距和下位機(jī)路徑—方向解析算法結(jié)合，最終使機(jī)器人在雪地場(chǎng)景100cm內(nèi)，起點(diǎn)偏移距離維持在（±2.17）%內(nèi)，起點(diǎn)偏差角度維持在（±0.37）%內(nèi)，掃雪覆蓋率維持在95%以上和重復(fù)率維持在20%以下，滿足一定范圍全自主除雪需求。