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

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

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

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

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

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

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

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

2.1 主控制器設(shè)計

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

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

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

2.2 輔控制器

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

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

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

3 軟件技術(shù)方案

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

最大化這個似然函數(shù)即最小化其最小負對數(shù)似然：

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

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

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

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

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

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

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

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

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

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

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

圖8 生成的離線查找表

3.3 運動控制系統(tǒng)

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

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

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

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

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

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

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

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

圖10 橫向控制原理框圖

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

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

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

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

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

4 結(jié)論

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