張 軍，張衛(wèi)忠，孫文芳

針對(duì)當(dāng)前醫(yī)院、地鐵站、高鐵站等室內(nèi)區(qū)域清掃任務(wù)繁重，勞動(dòng)力成本高的問題，本文基于低成本傳感器，利用三維激光定位與地圖構(gòu)建、軌跡規(guī)劃、運(yùn)動(dòng)控制等核心技術(shù)對(duì)現(xiàn)有傳統(tǒng)清掃機(jī)進(jìn)行智能改進(jìn)設(shè)計(jì).使無人清掃機(jī)具備自主路徑規(guī)劃、自主定位與地圖構(gòu)建、自主避障等功能［1］，可以實(shí)現(xiàn)室內(nèi)環(huán)境下地面清潔，提高了工作效率和效果.

1 機(jī)械結(jié)構(gòu)與工作模式

清掃機(jī)一般工作在半開放環(huán)境下，且速度在5～30 km/h 之間.針對(duì)清掃機(jī)的工作環(huán)境和使用要求，在現(xiàn)有清掃機(jī)基礎(chǔ)上進(jìn)行了機(jī)械結(jié)構(gòu)優(yōu)化改進(jìn)，主要機(jī)械結(jié)構(gòu)如圖1 所示.控制器接收激光雷達(dá)與攝像裝置的信息進(jìn)行運(yùn)算處理后，控制轉(zhuǎn)向電機(jī)工作，通過減速器、轉(zhuǎn)向鏈輪和鏈條控制轉(zhuǎn)向輪工作，從而實(shí)現(xiàn)無人清掃機(jī)運(yùn)行，利用光柵完成轉(zhuǎn)向輪轉(zhuǎn)角的測(cè)量，雷達(dá)和攝像裝置監(jiān)測(cè)和獲取清掃機(jī)周邊的環(huán)境信息，其中核心部件是控制器部分，其必須能夠及時(shí)處理獲取的信息，并利用算法作出合理決策.但由于算法、傳感器、基礎(chǔ)設(shè)施、網(wǎng)絡(luò)環(huán)境等因素尚不成熟，現(xiàn)階段還無法實(shí)現(xiàn)全場(chǎng)景的完全自動(dòng)駕駛，因此，針對(duì)不同工作環(huán)境研發(fā)與其相適應(yīng)的軟、硬件設(shè)備，成為了自動(dòng)駕駛的主要實(shí)現(xiàn)路徑.路徑規(guī)劃就是無人清掃機(jī)在具有障礙物的環(huán)境中按照一定的評(píng)價(jià)標(biāo)準(zhǔn)，尋找一條從給定點(diǎn)到達(dá)目標(biāo)點(diǎn)的無碰路徑［2］.

圖1 智能無人清掃機(jī)結(jié)構(gòu)簡圖

2 控制器技術(shù)方案

為了提高無人清掃機(jī)自動(dòng)駕駛的安全性與可靠性，降低計(jì)算功耗，無人清掃機(jī)的控制器采用主核“ARM”處理單元和輔核系統(tǒng)Freescale MCP 564xA 的主輔核雙核系統(tǒng)架構(gòu).控制器在開發(fā)過程中，將充分考慮安全功能、失效模式、駕駛場(chǎng)景、可控性，以及安全目標(biāo)、安全時(shí)間和安全狀態(tài)等因素，最終努力做到控制器功能安全認(rèn)證.

基于多個(gè)DCU 的多域控制器MDC（Multi-Domain Controller）能夠處理來自不同域的傳感器信號(hào)，從而滿足自動(dòng)駕駛技術(shù)對(duì)無人清掃機(jī)各域信息進(jìn)行融合處理的需求.攝像頭、雷達(dá)、GPS、輪速傳感器等屬于不同域的傳感器信號(hào)在一個(gè)控制器中處理，保證控制器作出最優(yōu)的駕駛決策.這種統(tǒng)一處理的方式也推進(jìn)了系統(tǒng)功能的集成化，提高了處理器和存儲(chǔ)器的利用效率［3］.

2.1 主控制器設(shè)計(jì)

主處理器基于2 塊Nvidia TX2 高性能嵌入式芯片，實(shí)時(shí)采集/監(jiān)測(cè)高精度地圖信息、攝像頭信息、毫米波雷達(dá)信息、激光波雷達(dá)信息、GPS/慣導(dǎo)組合信息、清掃機(jī)狀態(tài)信息、5G通訊信息等，實(shí)現(xiàn)數(shù)據(jù)融合及智能決策功能，通過CAN 總線向清掃機(jī)ECU 發(fā)出控制指令，最終實(shí)現(xiàn)清掃機(jī)安全有效的自動(dòng)駕駛功能.此外，主控系統(tǒng)還可以對(duì)無人清掃機(jī)行駛狀態(tài)的關(guān)鍵數(shù)據(jù)，通過存儲(chǔ)單元進(jìn)行保存.如圖2 所示.

圖2 主處理器結(jié)構(gòu)框圖

在主處理器系統(tǒng)中實(shí)現(xiàn)智能駕駛功能，發(fā)出控制指令，最終實(shí)現(xiàn)清掃機(jī)安全有效的自動(dòng)駕駛功能.在系統(tǒng)啟動(dòng)時(shí)主處理器系統(tǒng)具備自檢功能，在自動(dòng)駕駛過程中主處理器系統(tǒng)有識(shí)別清掃機(jī)系統(tǒng)故障和通信故障的功能，并針對(duì)故障作出相應(yīng)保護(hù).

2.2 輔控制器

輔控制器系統(tǒng)可以實(shí)時(shí)監(jiān)測(cè)主處理器系統(tǒng)的工作狀態(tài)，當(dāng)主處理器功能失常時(shí)，輔處理器可以完成無人清掃機(jī)的基本控制，控制無人清掃機(jī)進(jìn)入跛行模式，并實(shí)現(xiàn)報(bào)警.輔處理器在系統(tǒng)運(yùn)行后實(shí)時(shí)監(jiān)測(cè)主處理器工作狀態(tài)，并接受GPS 和毫米波雷達(dá)數(shù)據(jù)，當(dāng)主處理器工作失常時(shí)輔處理器可以及時(shí)作出相應(yīng)的保護(hù)措施，利用GPS 和毫米雷達(dá)信息進(jìn)入跛行模式，并進(jìn)行聲光報(bào)警.輔處理器系統(tǒng)結(jié)構(gòu)如圖3 所示.

圖3 輔處理器系統(tǒng)其結(jié)構(gòu)圖

輔處理器需要更高的可靠性，擬選用飛思卡爾Freescale MCP 564xA.MPC564xA 系列MCU，其具有增強(qiáng)的動(dòng)力總成功能.輔控制器主要完成電源管理、報(bào)文、指令監(jiān)聽、主控制器監(jiān)測(cè)、遙控?cái)?shù)據(jù)處理等功能［4］.

3 軟件技術(shù)方案

針對(duì)典型低速工況自動(dòng)駕駛需求，采用開放分層式體系結(jié)構(gòu)，該結(jié)構(gòu)擁有明確的接口體系，以及遞進(jìn)的性能表現(xiàn)，如圖4 所示.根據(jù)該體系技術(shù)架構(gòu)，將自動(dòng)駕駛系統(tǒng)軟件功能劃分為四大部分：環(huán)境感知系統(tǒng)、決策規(guī)劃系統(tǒng)、運(yùn)動(dòng)控制系統(tǒng)和執(zhí)行系統(tǒng).

圖4 總體技術(shù)架構(gòu)

3.1 環(huán)境感知系統(tǒng)

感知系統(tǒng)如同無人清掃機(jī)的“眼睛”，它通過處理激光雷達(dá)、毫米波雷達(dá)、相機(jī)和GPS/INSG 等傳感器收集的信息，使自動(dòng)駕駛平臺(tái)“感覺”和“理解”自身與周圍環(huán)境的相對(duì)關(guān)系，對(duì)工作環(huán)境各種局部場(chǎng)景進(jìn)行表達(dá).依據(jù)功能劃分，感知系統(tǒng)包括：障礙物感知、環(huán)境建模、定位定向和信息融合.

采用車載高清相機(jī)、激光雷達(dá)和毫米波雷達(dá)實(shí)現(xiàn)多模態(tài)信息融合的目標(biāo)檢測(cè)跟蹤和通行區(qū)域建模，由于各個(gè)傳感器對(duì)場(chǎng)景要素的檢測(cè)識(shí)別是相互獨(dú)立的，為了給無人清掃機(jī)決策系統(tǒng)提供統(tǒng)一可靠表達(dá)，基于對(duì)高清相機(jī)數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)的類別識(shí)別，本文擬利用分散式傳感器融合框架（如圖5 所示），基于D-S 理論［5］，采用多維屬性信息的分域、層次融合方式，利用初始信息確定高層融合所需的概率分布的近似算法對(duì)數(shù)據(jù)進(jìn)行融合處理，并對(duì)可能存在的證據(jù)沖突問題，給出了算法的修正；最終給出駕駛場(chǎng)景關(guān)鍵要素的統(tǒng)一表達(dá)和管理，并通過相機(jī)、激光雷達(dá)等信息的融合，實(shí)現(xiàn)目標(biāo)物體的檢測(cè)與識(shí)別.

圖5 分散式多源傳感器融合框架

基于激光雷達(dá)檢測(cè)數(shù)據(jù)、視覺傳感器檢測(cè)結(jié)果等進(jìn)行多傳感器信息融合，提高障礙物識(shí)別檢測(cè)的準(zhǔn)確性，生成帶有障礙物、路沿、車道線等信息的柵格地圖，對(duì)無人清掃機(jī)行駛的局部環(huán)境進(jìn)行直觀、有效的描述.如圖6 所示.

圖6 局部環(huán)境構(gòu)建方法示意圖

此外，為了使無人清掃機(jī)運(yùn)行更加精確，本文還采用基于GPS/IMU 與激光雷達(dá)里程計(jì)融合算法，該算法核心在于激光雷達(dá)里程計(jì)的實(shí)現(xiàn)，它包含四個(gè)基本模塊：基于編碼器的速度/航向提取、基于激光雷達(dá)的掃描匹配算法、基于卡爾曼濾波器的信息融合算法，以及航跡推算算法.

基于編碼器的速度/航向信息獲取.基于無人清掃機(jī)本身的編碼器信息（一般為無人清掃機(jī)轉(zhuǎn)向輪的轉(zhuǎn)動(dòng)信息），可以獲取清掃機(jī)本身航向，以及速度信息.

基于激光雷達(dá)的掃描匹配算法.為避免傳統(tǒng) ICP（Iterative Closest Point，最近點(diǎn)迭代）算法存在的實(shí)時(shí)性差的缺點(diǎn)，提出了基于NDT（Normal Distribution Transformation，正態(tài)分布變換）的掃描匹配算法，實(shí)現(xiàn)清掃機(jī)實(shí)時(shí)速度與航向的獲?。?］.

NDT 算法的關(guān)鍵在于將單幀點(diǎn)云轉(zhuǎn)化為分段連續(xù)可微的概率分布（正態(tài)分布）.

首先，將清掃機(jī)周圍的3D 空間劃分為規(guī)則的固定大小的柵格（體素，voxel）.

然后，計(jì)算每個(gè)柵格內(nèi)所有激光點(diǎn)的均值與協(xié)方差矩陣∑，基于均值與協(xié)方差矩陣，可以得到每個(gè)柵格的概率分布

當(dāng)使用NDT 進(jìn)行點(diǎn)云匹配時(shí)，目標(biāo)是找到當(dāng)前激光雷達(dá)（即無人清掃機(jī)）的姿態(tài)，使得當(dāng)前掃描的點(diǎn)云位于參考掃描表面上的可能性最大化，那么需要優(yōu)化的參數(shù)就是對(duì)當(dāng)前掃描點(diǎn)云的變換（旋轉(zhuǎn)與平移），用變換參數(shù)來表示. 設(shè)當(dāng)前掃描點(diǎn)云為X={x1,x2,…,xn} ，給定掃描點(diǎn)集合X和，令空間變換函數(shù)表示使當(dāng)前點(diǎn)云X通過變換參數(shù)轉(zhuǎn)換到參考點(diǎn)云表面上.最好的變換應(yīng)該是最大化這個(gè)變換，即：

最大化這個(gè)似然函數(shù)即最小化其最小負(fù)對(duì)數(shù)似然：

通過牛頓法，即可求得相應(yīng)的最優(yōu)參數(shù)，進(jìn)而求得相應(yīng)的速度和航向.

基于卡爾曼濾波器的信息融合算法.由于利用編碼器，以及激光雷達(dá)點(diǎn)云匹配均能夠獲得無人清掃機(jī)的實(shí)時(shí)運(yùn)動(dòng)速度與航向，但是均存在估計(jì)誤差，項(xiàng)目中采用卡爾曼濾波器進(jìn)行信息融合，減少誤差的累積.卡爾曼濾波包含狀態(tài)預(yù)測(cè)和測(cè)量更新兩個(gè)不斷迭代更新的過程，具體如圖7 所示.

圖7 基于卡爾曼濾波的信息融合過程

3.2 決策規(guī)劃系統(tǒng)

決策規(guī)劃系統(tǒng)如同無人清掃機(jī)的“大腦”，是在分析環(huán)境感知系統(tǒng)提供的感知地圖、全局路網(wǎng)等信息的基礎(chǔ)上進(jìn)行運(yùn)動(dòng)規(guī)劃、行為決策的過程，實(shí)現(xiàn)障礙物避障與速度決策，并將期望軌跡與速度發(fā)送給運(yùn)動(dòng)規(guī)劃系統(tǒng).

無人清掃機(jī)的局部規(guī)劃系統(tǒng)需要在應(yīng)對(duì)每一種行為決策結(jié)果時(shí)，均能相應(yīng)地生成一條清掃機(jī)可執(zhí)行軌跡.而對(duì)于軌跡的規(guī)劃事實(shí)上可以理解為一個(gè)約束優(yōu)化問題.其約束條件包括無人清掃機(jī)當(dāng)前狀態(tài)（即規(guī)劃初始狀態(tài)）、行為確定的期望目標(biāo)狀態(tài)（行為決策指令）、無人清掃機(jī)運(yùn)動(dòng)學(xué)特性、動(dòng)力學(xué)特性、橫縱向最大加速度指標(biāo)、安全距離指標(biāo)、時(shí)效性等，這些構(gòu)成了最終軌跡優(yōu)化和評(píng)價(jià)的標(biāo)準(zhǔn).通過算法得到的眾多軌跡中，能滿足這些標(biāo)準(zhǔn)要求的就可以作為最后的輸出軌跡［7］.

建立清掃機(jī)運(yùn)動(dòng)學(xué)模型，基于模型預(yù)測(cè)的軌跡規(guī)劃算法可以生成一條便于控制又符合環(huán)境約束的可行運(yùn)動(dòng)軌跡.

首先采用自適應(yīng)采樣算法，根據(jù)道路幾何特征自適應(yīng)地對(duì)終端狀態(tài)（即位置坐標(biāo)、航向、曲率）進(jìn)行采樣，然后基于模型預(yù)測(cè)生成方法生成一個(gè)從初始狀態(tài)開始的運(yùn)動(dòng)可行路徑集.利用清掃機(jī)運(yùn)動(dòng)學(xué)模型作為預(yù)測(cè)運(yùn)動(dòng)模型.約束可以寫成Xf=f(Xi,u) ，其中Xi是初始狀態(tài)，Xf是終端狀態(tài).函數(shù)f() 是無人清掃機(jī)運(yùn)動(dòng)學(xué)模型，包括無人清掃機(jī)整體形成和控制約束，可以用下面的方程表示：

其中：x、y是位置坐標(biāo)，θ是航向，k是曲率.空間路徑的狀態(tài)表示為相對(duì)于曲線長度s而不是時(shí)間t的函數(shù).

連續(xù)曲率可以保證路徑的平滑性，并且避免了干擾的轉(zhuǎn)向行為.為確保每條路徑曲率的連續(xù)性，曲率表示為弧長的多項(xiàng)式函數(shù).為了獲得唯一的解決方案，k（s）中未知參數(shù)的數(shù)量必須等于有用已知條件的數(shù)量.初始曲率k（0）確定了零階參數(shù)a=k（0）.其余的已知條件是終端狀態(tài)和初始位置.因此，采用變量s的三次多項(xiàng)式函數(shù). 未知參數(shù)包括q={b，c，d，sf}.由于這類強(qiáng)非線性微分方程的解析解是非平凡的，用牛頓法對(duì)方程進(jìn)行數(shù)值求解.并通過找到誤差的最小值(ΔXf(q))迭代計(jì)算未知參數(shù).

作為一個(gè)非線性優(yōu)化問題，未知參數(shù)q0的初始猜測(cè)會(huì)大大影響迭代的收斂性.因此，預(yù)先計(jì)算查找存儲(chǔ)未知參數(shù)的初始猜測(cè).如圖8 所示，采樣幾個(gè)最終狀態(tài)，然后根據(jù)無人清掃機(jī)中心坐標(biāo)框架生成相應(yīng)的路徑.存儲(chǔ)這些生成路徑的參數(shù)作為初始猜測(cè).

圖8 生成的離線查找表

3.3 運(yùn)動(dòng)控制系統(tǒng)

運(yùn)動(dòng)控制系統(tǒng)如同無人清掃機(jī)的“手腳”，主要功能是實(shí)現(xiàn)無人清掃機(jī)的運(yùn)動(dòng)控制，其目標(biāo)是盡可能精準(zhǔn)地跟蹤運(yùn)動(dòng)規(guī)劃系統(tǒng)所規(guī)劃的局部期望軌跡，將其轉(zhuǎn)化為方向轉(zhuǎn)角、速度、剎車、喇叭等控制量，并發(fā)送給底層執(zhí)行系統(tǒng)執(zhí)行.

運(yùn)動(dòng)控制系統(tǒng)的運(yùn)動(dòng)控制按照清掃機(jī)的相對(duì)坐標(biāo)系，分為無人清掃機(jī)的橫向運(yùn)動(dòng)和縱向運(yùn)動(dòng).無人清掃機(jī)的橫向運(yùn)動(dòng)包含了路徑跟隨、清掃機(jī)環(huán)道、彎道行駛等功能；無人清掃機(jī)的縱向控制包含了無人清掃機(jī)速度控制、停車控制等功能，能夠提高控制器對(duì)速度的自適應(yīng)能力［8］.

（1）縱向控制.設(shè)計(jì)車速反饋串聯(lián)矯正結(jié)構(gòu)，如圖9 所示.

其中設(shè)定車速由用戶輸入或者由決策系統(tǒng)給定.用戶可以通過控制系統(tǒng)程序界面輸入最高行駛速度.縱向控制采用PID 控制方法［9］.PID 參數(shù)的整定通過實(shí)驗(yàn)進(jìn)行.在無人清掃機(jī)執(zhí)行機(jī)構(gòu)調(diào)試完成和控制程序編寫完成后，通過路試給定不同的初始狀態(tài)和設(shè)定車速（包括從靜止啟動(dòng)，從低速加速到高速，從高速降到低速，制動(dòng)駐車），根據(jù)PID 控制器整定原則，調(diào)整PID 參數(shù)，達(dá)到預(yù)期的控制系統(tǒng)性能指標(biāo)和總體指標(biāo).

圖9 縱向控制結(jié)構(gòu)

（2）橫向控制.無人清掃機(jī)在行駛過程中，當(dāng)?shù)缆返膹澢潭劝l(fā)生變化時(shí)，必然需要不斷變換行駛方向，以此適應(yīng)道路彎曲程度的變化，因此無人清掃機(jī)橫向控制系統(tǒng)的實(shí)質(zhì)就是對(duì)其行走方向的控制.無人清掃機(jī)首先需要收集前方道路情況，之后結(jié)合自身在道路中的位置、方向作出判斷，進(jìn)而轉(zhuǎn)動(dòng)方向盤，對(duì)無人清掃機(jī)姿態(tài)進(jìn)行不斷地修正，使其保持在期望的行駛軌跡上.

若將上述過程進(jìn)行拆分，首先無人清掃機(jī)需要一條規(guī)劃軌跡代替人眼識(shí)別的道路軌跡，規(guī)劃軌跡來自于決策系統(tǒng)，為一個(gè)GPS 經(jīng)緯度點(diǎn)集構(gòu)成的線束；之后，橫向運(yùn)動(dòng)控制算法仿照駕駛員的行為方式在即將經(jīng)過的前方規(guī)劃軌跡中獲取道路信息，在本算法中稱之為預(yù)瞄距離和預(yù)瞄點(diǎn)的選取；預(yù)瞄距離就好比是駕駛員向前方觀察路徑時(shí)所觀察的距離，而預(yù)瞄點(diǎn)就是在預(yù)瞄距離處所獲取的位于規(guī)劃軌跡上的路點(diǎn)，并以此分析道路的彎曲程度；最后，通過將預(yù)瞄點(diǎn)的信息同清掃機(jī)本身的GPS 經(jīng)緯度信息和航向角信息進(jìn)行對(duì)比，就可以得出無人清掃機(jī)轉(zhuǎn)向輪在每個(gè)控制周期所應(yīng)當(dāng)?shù)玫降淖藨B(tài)修正值，即無人清掃機(jī)應(yīng)當(dāng)轉(zhuǎn)過的方向和角度.無人清掃機(jī)橫向運(yùn)動(dòng)控制系統(tǒng)最終也是通過控制無人清掃機(jī)轉(zhuǎn)向輪的轉(zhuǎn)動(dòng)角度來控制清掃機(jī)完成轉(zhuǎn)向動(dòng)作，從而完成在不同彎曲程度上的道路行駛.

由此，可以得到無人清掃機(jī)的基本控制原理：通過不斷對(duì)比無人清掃機(jī)與所要經(jīng)過的道路之間的相對(duì)位置、方向信息，判斷無人清掃機(jī)與所要經(jīng)過的路徑的相對(duì)關(guān)系，并由此得出相應(yīng)的方向盤轉(zhuǎn)角，從而驅(qū)動(dòng)無人清掃機(jī)向期望的方向移動(dòng)，控制原理框圖如圖10 所示.

圖10 橫向控制原理框圖

3.4 執(zhí)行系統(tǒng)

執(zhí)行系統(tǒng)主要負(fù)責(zé)接收來自運(yùn)動(dòng)控制系統(tǒng)的指令（如轉(zhuǎn)向、速度、制動(dòng)等），實(shí)現(xiàn)相關(guān)執(zhí)行機(jī)構(gòu)行為的控制.

（1）底層控制器.實(shí)現(xiàn)轉(zhuǎn)向、制動(dòng)等控制量到各分執(zhí)行機(jī)構(gòu)的協(xié)調(diào)控制；實(shí)現(xiàn)人工/自動(dòng)/遙控三種模式切換.

（2）電源模塊.確保清掃機(jī)各傳感器及設(shè)備長期平穩(wěn)運(yùn)行.

（3）執(zhí)行機(jī)構(gòu)改造.實(shí)現(xiàn)轉(zhuǎn)向、制動(dòng)、油門線控化.

4 結(jié)論

綜上所述，根據(jù)清掃機(jī)的工作環(huán)境和工作狀態(tài)，通過對(duì)現(xiàn)有清掃機(jī)的機(jī)械部分進(jìn)行改進(jìn)，增加激光雷達(dá)定位、攝像裝置與輪速傳感器等設(shè)備，并從環(huán)境感知系統(tǒng)、決策規(guī)劃系統(tǒng)、運(yùn)動(dòng)控制系統(tǒng)和執(zhí)行系統(tǒng)四個(gè)方面對(duì)控制器進(jìn)行軟、硬件研發(fā)設(shè)計(jì).實(shí)驗(yàn)表明，室內(nèi)智能無人清掃機(jī)滿足設(shè)計(jì)要求.