勾伯濤，李鵬飛

（沈陽理工大學(xué) 信息科學(xué)與工程學(xué)院，遼寧沈陽， 110159）

0 引言

過去的幾年，隨著科技的發(fā)展和社會的變革，商場產(chǎn)業(yè)迅速發(fā)展，雖然人們的生活水平得到了提升，但是隨之而來的一些安防問題卻始終未能有效解決。目前對于商場的安防問題，一般只有監(jiān)控和保安巡邏兩種傳統(tǒng)的方式，對于監(jiān)控來說它本身就存在著監(jiān)控死角以及停電無法工作的狀態(tài)，而對于人工巡邏來說，也沒有足夠的精力去實現(xiàn)24 小時的不間斷視察巡邏，且人對于周圍環(huán)境的敏感程度也相對比較薄弱，不能及時發(fā)現(xiàn)并排除隱患。

針對上述問題，本文設(shè)計了一個基于計算機(jī)視覺的智能安防巡邏機(jī)器人，實現(xiàn)自主移動定位、人體檢測與追蹤、煙火檢測系統(tǒng)和滅火彈丸的精準(zhǔn)投擲等功能，有效地彌補(bǔ)了傳統(tǒng)方法的漏洞。

1 系統(tǒng)總統(tǒng)設(shè)計方案

智能巡邏機(jī)器人主要由運(yùn)動主控模塊（下位機(jī)）和視覺環(huán)境檢測模塊（上位機(jī)）兩個模塊構(gòu)成，兩個模塊之間通過虛擬串口進(jìn)行通訊，實現(xiàn)各個功能的聯(lián)調(diào)，系統(tǒng)結(jié)構(gòu)框圖如圖1 所示。下位機(jī)實現(xiàn)對機(jī)器人的運(yùn)動狀態(tài)的控制，其中包括前進(jìn)、后退、平移、旋轉(zhuǎn)等多種運(yùn)動狀態(tài)；能夠接收上位機(jī)下發(fā)的信息實現(xiàn)在商場中實現(xiàn)自主建圖、定點巡視、煙火檢測、自動瞄準(zhǔn)發(fā)射滅火彈丸，以及危險人物的自動跟隨與報警。上位機(jī)上裝有視覺主控的MinPC，能夠?qū)崿F(xiàn)上電自啟動，確保代程序?qū)崟r運(yùn)行，也裝有?？倒I(yè)相機(jī)，其圖像質(zhì)量優(yōu)異。通過USB3.0 接口實時傳輸非壓縮圖像，最高幀率可達(dá)249.1fps。由于運(yùn)動場景復(fù)雜，相對于普通USB相機(jī)，它的增益，曝光等參數(shù)調(diào)節(jié)更加便捷，且對圖像處理的速度也有顯著的提高。

圖1 系統(tǒng)結(jié)構(gòu)框圖

2 系統(tǒng)機(jī)械結(jié)構(gòu)設(shè)計

■2.1 底盤框架

整體框架采用了鋁方管立體式結(jié)構(gòu)，并采用了薄壁粗鋁方，在強(qiáng)度足夠的情況下保持了車身的扁平化，減少了離地高度，并采用了大板材，進(jìn)行合理的鏤空，方便相關(guān)模塊的布局和布線，如圖2 所示。

圖2 底盤框架

■2.2 輪組結(jié)構(gòu)

輪組部分采用了上交的聯(lián)軸器，在保證強(qiáng)度的同時，減少了相應(yīng)的長度，采用了小搖臂，并利用soildworks 的拓?fù)溥\(yùn)算功能進(jìn)行了合適的減重，在轉(zhuǎn)軸處利用了擋邊軸承，減少了磨損，如圖3 所示。

圖3 輪組示意圖

■2.3 云臺結(jié)構(gòu)

對于一個性能良好的機(jī)器人來說，三軸穩(wěn)定性是必不可少的，同時還需要保證在高強(qiáng)度的環(huán)境中，擁有精準(zhǔn)的拋投能力。因此在本次研發(fā)中，將重點放在機(jī)構(gòu)的穩(wěn)定性和彈道的精度上面，并對此進(jìn)行測試與改進(jìn)，如圖4 所示。

圖4 云臺示意圖

云臺整體框架（見圖5）采用的是利用板搭結(jié)構(gòu)實現(xiàn)穩(wěn)定性的Y 型框架。比較U 型框架結(jié)構(gòu)和Y 型框架結(jié)構(gòu)，U型結(jié)構(gòu)便于對云臺的零件進(jìn)行擺放，擁有較大的俯仰角，其缺點是板件與連接件之間容易出現(xiàn)晃動現(xiàn)象。Y 型框架利用的板件自身的形狀去抵抗變形，因此在穩(wěn)定性上面有一定的優(yōu)勢。

圖5 云臺框架結(jié)構(gòu)

圖6 是連桿結(jié)構(gòu)進(jìn)行仿真分析，對俯仰角的分析，與設(shè)計預(yù)想值幾乎一致。

圖6 連桿結(jié)構(gòu)仿真分析結(jié)果

圖7 為電機(jī)在轉(zhuǎn)動一定時間后停止的所受角力矩，經(jīng)分析在電機(jī)所能承受范圍內(nèi)。

圖7 電機(jī)在轉(zhuǎn)動一定時間后停止的所受角力矩

3 系統(tǒng)硬件設(shè)計

■3.1 運(yùn)動主控模塊

Robomaster 開發(fā)板C 型采用了ST 公司的STM32F4系列微控制器作為主控芯片。該開發(fā)板具有豐富的外設(shè)和接口，包括多個PWM 輸出、編碼器輸入、CAN 總線接口、USB 接口等。此外，該開發(fā)板還提供了豐富的擴(kuò)展接口，可以方便與其他模塊進(jìn)行通信和連接。通過該開發(fā)板，我們可以方便地進(jìn)行底盤、相機(jī)、激光雷達(dá)等各種硬件模塊的控制和交互，真正實現(xiàn)了機(jī)器人自主運(yùn)動和識別的功能。

■3.2 視覺主控模塊

3.2.1 激光雷達(dá)

環(huán)境檢測重要的一步就是需要知道機(jī)器人當(dāng)前處于環(huán)境的哪個位置，故我們采用激光雷達(dá)來建立地圖，從而實現(xiàn)自主巡邏功能。它通過發(fā)射激光束并測量激光束的反射時間和強(qiáng)度來掃描周圍的環(huán)境，并據(jù)此計算物體的位置和距離。它可以通過其獲取環(huán)境中物體的坐標(biāo)信息，并將其轉(zhuǎn)化為地圖上對應(yīng)的點或線段等幾何形狀，從而實現(xiàn)對環(huán)境的建模和定位。

3.2.2 MniPC

MniPC 是一種小型化的個人電腦，它作為我們機(jī)器人的控制器具有體積小、功耗低、高性能、易于編程等優(yōu)點，能夠提高機(jī)器人的靈活性和工作效率。它也是我們上位機(jī)的首腦，不需要實時檢測，將數(shù)據(jù)發(fā)送至下位機(jī)，而且要監(jiān)聽下位機(jī)的信息，做好實時報警的準(zhǔn)備，以引起周圍工作人員的注意。同時也可以安裝圖傳攝像頭將畫面實時傳輸?shù)奖O(jiān)控室，提高工作人員的工作效率。

4 系統(tǒng)軟件設(shè)計

■4.1 總體運(yùn)動流程設(shè)計

對于底盤的控制，機(jī)器人空間狀態(tài)控制由底盤部分C板完成，底盤程序不斷等待云臺發(fā)送的空間狀態(tài)控制指令，及底盤模式指令。并根據(jù)不同指令分別完成旋轉(zhuǎn)模式、平移模式及正常行走模式的控制。在底盤啟動過程中使用斜坡函數(shù)控制底盤起步的加速度防止輪子打滑又能有效控制功率。機(jī)器人運(yùn)動過程中先對麥克納姆輪解算然后通過PID 計算控制電流值。通過功率控制算法對PID 輸出進(jìn)行縮放進(jìn)而實現(xiàn)軟件層面的功率控制。

云臺的控制，視覺識別到煙火及其他狀態(tài)下首先對獲得的視覺數(shù)據(jù)進(jìn)行卡爾曼預(yù)測，并根據(jù)卡爾曼預(yù)測結(jié)果進(jìn)行滅火彈丸下墜的計算：

并更新云臺目標(biāo)角度。云臺控制時通過AHRS 算法結(jié)算陀螺儀數(shù)據(jù)完成云臺的PID 控制。總體運(yùn)動示意圖如圖8所示。

圖8 總體運(yùn)動示意圖

圖9 數(shù)據(jù)處理代碼

■4.2 激光雷達(dá)定位

激光雷達(dá)和A-LOAM 都具有高精度的定位和運(yùn)動估計能力，可以實現(xiàn)準(zhǔn)確的自我定位和環(huán)境建模，為機(jī)器人導(dǎo)航和路徑規(guī)劃提供更加準(zhǔn)確的參考數(shù)據(jù)。機(jī)器人可以使用這些信息來避開障礙物并規(guī)劃最優(yōu)路徑，從而實現(xiàn)高效、安全的移動和任務(wù)執(zhí)行，故本設(shè)計選其進(jìn)行三維SLAM。

首先使用激光雷達(dá)掃描周圍環(huán)境，并獲取點云數(shù)據(jù)。激光雷達(dá)傳感器由兩部分組成，激光雷達(dá)發(fā)射系統(tǒng)通過多次發(fā)射激光束進(jìn)行測量，層數(shù)越多則測量精度越高，但傳感器也會變得更大。當(dāng)激光束擊中障礙物并反射回來時，形成了一組點云數(shù)據(jù)，這些數(shù)據(jù)實際上是根據(jù)激光束的反射時間測得的。

對點云數(shù)據(jù)進(jìn)行特征提取，根據(jù)曲率和高度來識別地面點以及建非地面點。其次通過匹配不同時間戳下的點云數(shù)據(jù)，估計機(jī)器人在空間中的運(yùn)動軌跡，并計算每個時間戳下相鄰兩幀之間的運(yùn)動變換矩陣。將不同時間戳下的點云數(shù)據(jù)進(jìn)行配準(zhǔn)，生成一個組合的點云地圖。最后使用基于因子圖的優(yōu)化方法，對機(jī)器人的位姿和地圖進(jìn)行優(yōu)化，以最小化所有觀測到的點云數(shù)據(jù)與預(yù)測點云數(shù)據(jù)之間的誤差。將優(yōu)化后的機(jī)器人位姿和地圖更新到全局地圖中，用于后續(xù)的導(dǎo)航和避障。主要實現(xiàn)步驟如下：

①配置激光雷達(dá)參數(shù)：

sensor_msgs::LaserScan laser_scan;

laser_scan.angle_increment = angle_increment; // 激光束角度分辨率

laser_scan.angle_min = angle_min;

// 最小掃描角度

laser_scan.angle_max = angle_max;

// 最大掃描角度

laser_scan.range_min = range_min; // 最小掃描距離

laser_scan.range_max = range_max; // 最大掃描距離

②處理激光雷達(dá)數(shù)據(jù)

首先，該程序利用ROS 中的sensor_msgs::LaserScan消息訂閱激光雷達(dá)的數(shù)據(jù)，并將其轉(zhuǎn)換為pcl 庫中的點云數(shù)據(jù)格式（PCL::PointCloudpcl::PointXYZ）。 轉(zhuǎn)換過程中，遍歷每一個激光束并計算激光束對應(yīng)的點的坐標(biāo)，最終存儲在點云變量laser_cloud 中。其次，使用PreprocessPointCloud 函數(shù)對點云進(jìn)行預(yù)處理。最后，使用Mapping 函數(shù)利用點云數(shù)據(jù)進(jìn)行建圖。將每個時刻的點云數(shù)據(jù)與之前的地圖數(shù)據(jù)進(jìn)行匹配，并估計機(jī)器人的軌跡；根據(jù)需要使用拓展卡爾曼濾波（Extended Kalman Filter，EKF）等算法對機(jī)器人的運(yùn)動進(jìn)行估計和預(yù)測。最終，建圖的結(jié)果通常是一個二維或三維的柵格地圖或點云地圖，用于機(jī)器人自主導(dǎo)航或環(huán)境探測。圖10 為A-Loam 建圖的結(jié)果。

圖10 構(gòu)建地圖效果

■4.3 人體檢測與跟隨

為了避免人體四肢、復(fù)雜的遮擋、自相似部分以及由于著裝、體型、照明，以及許多其他因素。我們采用MediaPipe 結(jié)合OpenCV 來對人體姿態(tài)估計（預(yù)測各種人體關(guān)鍵點）。MediaPipe Pose 是一種用于高保真身體姿勢跟蹤的ML 解決方案，利用我們的BlazePose 研究從RGB視頻幀解算出全身33 個3D 地標(biāo)，該研究也為ML Kit 姿勢檢測API 提供支持。為了實現(xiàn)對人體姿態(tài)的檢測，本設(shè)計采用對MediaPipe 的人體關(guān)節(jié)夾角來對人的位姿進(jìn)行檢測。例如舉手的時候，手臂與水平方向夾角是一定大于0 度的；雙手垂下時，大臂與小臂的夾角大于0 度小于180 度，所以這樣就可以將一些基本動作分類。圖11 為雙手下垂時的檢測結(jié)果。

圖11 雙手下垂檢測畫面

接下來我們使用OpenCV 對識別到的人進(jìn)行人體姿態(tài)解算。本設(shè)計采用SOLVEPNP 算法求解3D 到2D 點的運(yùn)動，給定相機(jī)內(nèi)參矩陣k、n 個空間點在圖像上的像素坐標(biāo)pi 和它們在世界坐標(biāo)系下的坐標(biāo)piw，需要求解相機(jī)的位姿，即計算世界坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣R 和平移向量T。圖12 為坐標(biāo)系轉(zhuǎn)換過程。

圖12 坐標(biāo)系轉(zhuǎn)換

在角度解算中，我們首先利用SOLVEPNP 函數(shù)計算出旋轉(zhuǎn)向量和平移向量，然后使用羅德里格斯公式將旋轉(zhuǎn)向量轉(zhuǎn)換為旋轉(zhuǎn)矩陣，接著使用歐拉角轉(zhuǎn)換公式將旋轉(zhuǎn)矩陣轉(zhuǎn)換為歐拉角。最終，我們就可以得到物體在相機(jī)坐標(biāo)系中的位置和Yaw、Pitch 軸等角度信息。最后將角度信息和距離信息發(fā)送給下位機(jī)，使得云臺能夠?qū)崟r的跟蹤目標(biāo)物體。圖13 為代碼實現(xiàn)過程。

圖13 角度解算

對于滅火彈丸的投擲，由于距離、彈道、環(huán)境等因素的影響，彈道會產(chǎn)生偏差，從而導(dǎo)致命中率下降。為了提高命中率，使用了傳統(tǒng)的視覺彈道補(bǔ)償方案。當(dāng)云臺接收到距離目標(biāo)物體的距離時，結(jié)合彈丸的初速度和角度信息，推算出需要增加的Pitch 軸角度，從而能夠精準(zhǔn)拋投實現(xiàn)滅火。具體實現(xiàn)過程如圖14 所示。

圖14 彈道補(bǔ)償

圖15 模型實現(xiàn)代碼

首先，輸入?yún)?shù)包括“ShootSpeed”（炮彈射出速度）、“PitchAngle”（炮彈發(fā)射角度）和“CoordinateActual”（實際彈道的三維坐標(biāo)）。代碼中定義了彈丸的空氣阻力系數(shù)“KAPPA”和質(zhì)量“M”。然后，根據(jù)輸入?yún)?shù)計算出發(fā)射點到落點的水平距離“p”和與水平面的夾角“pitchAngleRef”。接下來，將三維坐標(biāo)轉(zhuǎn)換為實際的距離（將坐標(biāo)值從毫米轉(zhuǎn)換為米）。然后，根據(jù)公式計算出彈丸在空氣阻力下的速度變化，并根據(jù)經(jīng)典力學(xué)計算出炮彈的飛行時間和軌跡。最后，通過解方程的方式計算出調(diào)整后的發(fā)射角度“pitchAngleRef”，并返回該值。

■4.4 煙火檢測

考慮到機(jī)器人的實時性能和應(yīng)用的場景，本設(shè)計采用了Yolov5 的5s 模型，它相較于其他版本和模型而言，雖然在精度方面有所降低，但在煙火檢測任務(wù)中仍然表現(xiàn)良好。而且Yolov5 框架提供了易于使用的API 和預(yù)訓(xùn)練模型，可以更輕松地完成模型訓(xùn)練和部署。煙火檢測步驟如下：

①收集1500 張帶有煙火的圖片，手動標(biāo)注每個圖片中煙火的位置以及類別，我們使用開源的標(biāo)注工具LabelImg。但由于標(biāo)注量大，故我們選擇先標(biāo)注少量數(shù)據(jù)集，再通過Yolov5 訓(xùn)練一個初始的模型來輔助標(biāo)注，這樣大幅縮短了工作量。

②將已經(jīng)進(jìn)行標(biāo)注的數(shù)據(jù)集按照一定比例劃分成三個部分：訓(xùn)練集、驗證集和測試集。然后使用train.py 腳本來對Yolov5模型進(jìn)行訓(xùn)練。同時需要定義模型架構(gòu)、超參數(shù)、優(yōu)化器等。

③驗證模型：首先使用torch.hub.load()函數(shù)從本地加載YOLOv5 模型，然后使用cv2.imread()函數(shù)加載待檢測的煙火圖像。接著進(jìn)行預(yù)處理。將預(yù)處理后的圖像輸入模型進(jìn)行檢測，得到每個檢測框的坐標(biāo)、置信度和類別。最后，在原圖像上繪制檢測結(jié)果并顯示出來。

需要注意的是，訓(xùn)練樣本的數(shù)量和質(zhì)量、數(shù)據(jù)集的劃分、模型的超參數(shù)等都會影響模型的準(zhǔn)確度，因此我們進(jìn)行了多次實驗才找到最佳的實驗配置。圖16 為檢測效果。

圖16 煙火檢測效果

該測試成功實現(xiàn)了智能巡邏機(jī)器人的煙火識別功能，試驗的效果如表1、表2 所示。

表1 火焰檢測結(jié)果

表2 煙霧檢測結(jié)果

5 結(jié)語

本文以當(dāng)下商場安防問題為背景，設(shè)計了一款能夠自主巡邏的智能安防機(jī)器人，它搭載了電機(jī)驅(qū)動模塊、激光雷達(dá)以及視覺處理等多種模塊，實現(xiàn)了機(jī)器人對環(huán)境的自主監(jiān)測、人體的自主跟隨、煙火的識別及自主拋投滅火等多種功能，有效解決了單靠人工和遠(yuǎn)程監(jiān)控的弊端。試驗結(jié)果表明，此機(jī)器人搭載的一個高性能的懸掛能夠在復(fù)雜的環(huán)境中靈活自主的運(yùn)動，進(jìn)入到人無法進(jìn)入的地方進(jìn)行工作，及時排除存在的安全隱患，實現(xiàn)了24 小時無差錯的工作，真正意義上減少了對人力物力資源的浪費(fèi)。結(jié)合多個傳感器的共同作用，使機(jī)器人的智能化程度不斷提高。