一種用于乘用車(chē)車(chē)底圖像采集的自主勘察機(jī)器人

高文斌，張豐豐，董偉光，余曉流

(1.安徽工業(yè)大學(xué)a.機(jī)械工程學(xué)院；b.特種重載機(jī)器人安徽省重點(diǎn)實(shí)驗(yàn)室，安徽馬鞍山 243032；2.公安部交通管理科學(xué)研究所，江蘇無(wú)錫 214151)

隨著我國(guó)經(jīng)濟(jì)、社會(huì)的快速發(fā)展，2021 年我國(guó)機(jī)動(dòng)車(chē)保有量超過(guò)3.95 億輛，汽車(chē)保有量超過(guò)3 億輛。近年，我國(guó)每年發(fā)生近20萬(wàn)起交通事故，造成大量人員傷亡和財(cái)產(chǎn)損失。交通事故現(xiàn)場(chǎng)勘察是交通事故調(diào)查處理中一道重要的工序，其中車(chē)輛底盤(pán)勘察取證對(duì)還原車(chē)輛交通事故事實(shí)、認(rèn)定事故責(zé)任起到關(guān)鍵性的作用[1-2]。

車(chē)底勘察取證裝備分成手持式、固定式和移動(dòng)式。手持式勘察裝備由手柄、顯示器、檢視裝置及照明燈組成，需手持裝備進(jìn)行車(chē)底勘察，體力消耗大、效率低[3]。固定式勘察裝備要求車(chē)輛從勘察裝備上方通過(guò)才能獲取車(chē)輛底盤(pán)圖像信息，該類(lèi)裝備攜帶不便，還需要車(chē)輛具有正常的移動(dòng)功能，對(duì)事故現(xiàn)場(chǎng)底盤(pán)勘察的適應(yīng)性差[4-5]。研究表明[6-10]，移動(dòng)式勘察裝備已廣泛用于危險(xiǎn)環(huán)境的探查取樣、城市救援救災(zāi)、戰(zhàn)場(chǎng)偵察、反恐排爆等領(lǐng)域。但是，移動(dòng)式勘察裝備受車(chē)輛底盤(pán)高度的限制而通過(guò)性較差；操作人員在遙控機(jī)器人運(yùn)行軌跡的同時(shí)需手動(dòng)采集車(chē)底圖像信息，難以滿(mǎn)足實(shí)際勘察需求；已有專(zhuān)用移動(dòng)式勘察裝備本體及控制終端體積較大，不便于攜帶和運(yùn)輸。鑒于此，根據(jù)乘用車(chē)事故現(xiàn)場(chǎng)勘察的實(shí)際需求，研制一種可自主運(yùn)行的底盤(pán)勘察機(jī)器人(簡(jiǎn)稱(chēng)機(jī)器人)，通過(guò)手動(dòng)或自主巡航方式覆蓋整個(gè)車(chē)底采集車(chē)輛底盤(pán)圖像，以實(shí)現(xiàn)交通事故的快速準(zhǔn)確勘察。

1 機(jī)器人本體設(shè)計(jì)

普通轎車(chē)的最小離地間隙為130～200 mm，運(yùn)動(dòng)型多用途汽車(chē)(sport utility vehicle，SUV)的最小離地間隙為200～250 mm，行駛路面主要為混凝土和柏油路面[11]。根據(jù)機(jī)器人的工作環(huán)境、通用性和便攜性要求，確定機(jī)器人主要設(shè)計(jì)參數(shù)，如表1。

表1 主要設(shè)計(jì)參數(shù)Tab.1 Main design parameters

如圖1 所示，設(shè)計(jì)的機(jī)器人系統(tǒng)包括機(jī)器人本體、控制終端和手提箱。其中機(jī)器人本體由車(chē)體、輪式移動(dòng)機(jī)構(gòu)、云臺(tái)、圖像采集模塊、底盤(pán)識(shí)別傳感器和避障傳感器組成。底盤(pán)識(shí)別傳感器安裝于車(chē)體上方，用于車(chē)輛底盤(pán)及底盤(pán)邊界識(shí)別；障礙物檢測(cè)傳感器安裝于車(chē)體前端和左右兩側(cè)，用于周?chē)h(huán)境信息獲取。

圖1 機(jī)器人系統(tǒng)和本體示意圖Fig.1 Schematic diagram of robot system and body

為滿(mǎn)足機(jī)器人高度約束要求，設(shè)計(jì)一種通過(guò)中空軸內(nèi)部走線、由數(shù)字舵機(jī)驅(qū)動(dòng)的半潛式兩自由度云臺(tái)(圖2)，用于圖像采集模塊的姿態(tài)調(diào)整。

圖2 兩自由度云臺(tái)Fig.2 2-DOF pan-tilt platform

云臺(tái)通過(guò)繞垂直軸左右轉(zhuǎn)動(dòng)來(lái)調(diào)整航向角，傳動(dòng)裝置布置在車(chē)體內(nèi)部；通過(guò)繞水平軸上下翻轉(zhuǎn)來(lái)調(diào)整俯仰角，傳動(dòng)裝置布置在云臺(tái)內(nèi)腔。云臺(tái)搭載一個(gè)發(fā)光二極管(light emitting diode，LED)，在光照不佳的環(huán)境下為圖像采集補(bǔ)充照明。

2 機(jī)器人控制系統(tǒng)設(shè)計(jì)

2.1 硬件結(jié)構(gòu)

控制系統(tǒng)硬件主要包括核心控制器、運(yùn)動(dòng)控制系統(tǒng)、圖像采集系統(tǒng)和傳感器系統(tǒng)，如圖3。采用STM32F407ZGT6 芯片作為核心控制器，負(fù)責(zé)數(shù)據(jù)處理、指令解析和運(yùn)動(dòng)控制；運(yùn)動(dòng)控制系統(tǒng)包括驅(qū)動(dòng)器和直流伺服減速電機(jī)；圖像采集系統(tǒng)包括高清攝像頭、LED 光源和云臺(tái)；傳感器系統(tǒng)包括紅外傳感器、超聲波傳感器和慣性導(dǎo)航模塊。

圖3 控制系統(tǒng)硬件結(jié)構(gòu)Fig.3 Hardware structure of the control system

2.2 控制流程

基于μC/OS-II 嵌入式實(shí)時(shí)操作系統(tǒng)架構(gòu)開(kāi)發(fā)機(jī)器人機(jī)載控制系統(tǒng)，協(xié)調(diào)各項(xiàng)硬件資源完成作業(yè)功能，實(shí)現(xiàn)多任務(wù)分配和調(diào)度。機(jī)載控制系統(tǒng)軟件功能主要包括指令接收與解析、底盤(pán)識(shí)別、局部路徑規(guī)劃、自主遍歷等，控制流程如圖4。

圖4 控制系統(tǒng)流程圖Fig.4 Flowchart of the control system

機(jī)器人可在手動(dòng)模式和自主運(yùn)行模式之間自由切換，當(dāng)接收到上位機(jī)下發(fā)的自主運(yùn)行指令后，進(jìn)入自主運(yùn)行模式。機(jī)器人初次駛?cè)氡豢辈燔?chē)輛車(chē)底時(shí)進(jìn)行方向校準(zhǔn)，使前進(jìn)方向與車(chē)輛車(chē)長(zhǎng)方向垂直，并在首次駛?cè)胲?chē)底處建立世界坐標(biāo)系XOY(圖5)。機(jī)器人以“弓”字形軌跡遍歷運(yùn)行，直至完成勘察，駛出車(chē)底。在勘察過(guò)程中，機(jī)器人處于車(chē)底時(shí)，以100 ms 為周期實(shí)時(shí)掃描周?chē)偷妆P(pán)等環(huán)境信息，若檢測(cè)到障礙物，則根據(jù)障礙物特征類(lèi)型進(jìn)行局部路徑規(guī)劃，并判斷能否避讓。若能避讓?zhuān)瑒t在運(yùn)動(dòng)規(guī)則庫(kù)中選擇相應(yīng)避讓策略；若不能避讓?zhuān)瑒t中止自主運(yùn)行，跳轉(zhuǎn)至手動(dòng)模式。當(dāng)機(jī)器人處于車(chē)底邊緣時(shí)，若有障礙物，則進(jìn)行自主避障；若無(wú)障礙物，則相對(duì)底盤(pán)邊緣進(jìn)行方向校準(zhǔn)。當(dāng)機(jī)器人駛出至車(chē)底之外時(shí)，中止自主運(yùn)行進(jìn)入手動(dòng)模式。

圖5 機(jī)器人運(yùn)動(dòng)學(xué)模型Fig.5 Kinematics model of the robot

2.3 底盤(pán)識(shí)別

機(jī)器人在自主運(yùn)行模式下進(jìn)行車(chē)輛底盤(pán)遍歷時(shí)需估算機(jī)器人在車(chē)輛底盤(pán)下的位置，通常有如下兩種方法：建立坐標(biāo)系，使用機(jī)器人的位置坐標(biāo)估算機(jī)器人與車(chē)輛底盤(pán)的相對(duì)位置，受勘察車(chē)輛車(chē)型的影響，這種方法通用性差；通過(guò)紅外傳感器陣列識(shí)別車(chē)輛底盤(pán)，估算機(jī)器人與車(chē)輛底盤(pán)的相對(duì)位置，這種方法通用性好，但受車(chē)輛底盤(pán)復(fù)雜結(jié)構(gòu)的影響，尤其是凹陷部位，易造成誤判。為提高車(chē)輛底盤(pán)識(shí)別功能的準(zhǔn)確性，保證作業(yè)過(guò)程中機(jī)器人始終處于車(chē)輛底盤(pán)下，結(jié)合機(jī)器人位置坐標(biāo)和紅外傳感器陣列被遮擋部分面積對(duì)機(jī)器人與車(chē)輛底盤(pán)的相對(duì)位置進(jìn)行估算，可降低車(chē)輛底盤(pán)結(jié)構(gòu)對(duì)檢測(cè)結(jié)果的影響。

圖5中XOY為世界坐標(biāo)系，原點(diǎn)O為機(jī)器人首次駛?cè)胲?chē)底的位置，Y軸與車(chē)寬方向平行，X軸與車(chē)長(zhǎng)方向平行；xoy為機(jī)器人坐標(biāo)系，機(jī)器人坐標(biāo)系與世界坐標(biāo)系的夾角為θ，位姿可表示為[x,y,θ]T。在機(jī)器人坐標(biāo)系中，機(jī)器人沿x，y軸方向的速度為vx，vy；在世界坐標(biāo)系中機(jī)器人的速度vX，vY，相對(duì)世界坐標(biāo)系的角速度為θ˙，兩坐標(biāo)下的速度滿(mǎn)足式(1)。設(shè)機(jī)器人第1 次駛?cè)胲?chē)輛底盤(pán)時(shí)的位姿為[x0,y0,θ0]T，如式(2)，經(jīng)時(shí)間t后，機(jī)器人位姿為[xt,yt,θt]T。機(jī)器人沿車(chē)寬方向(世界坐標(biāo)系Y軸)遍歷的距離y=yt-y0。

如圖6(a)所示，機(jī)器人頂部安裝16 個(gè)呈矩形分布的紅外傳感器陣列，被遮擋兩梯形或兩四邊形的面積(圖6(b))之和A的計(jì)算方法如式(3)。h1，h2為陣列矩形邊長(zhǎng)；L1，L2，L3，L4為陣列矩形四邊被遮擋的傳感器之間的距離。當(dāng)h1h2≤A ≤2h1h2時(shí)，機(jī)器人位于底盤(pán)下。

圖6 紅外傳感器陣列被遮擋區(qū)域Fig.6 Shadowed area of infrared sensor array

根據(jù)乘用車(chē)輛寬度主要在1 500～2 000 mm 之間，一般不超過(guò)2 000 mm 的先驗(yàn)知識(shí)[11]，結(jié)合y和A的取值范圍，識(shí)別機(jī)器人與車(chē)輛底盤(pán)的相對(duì)位置，可歸納為3種情況，如表2。

表2 底盤(pán)識(shí)別Tab.2 Chassis recognition

2.4 底盤(pán)邊緣對(duì)齊

如圖7 所示，陰影部分為不同工況下紅外傳感器陣列被車(chē)輛底盤(pán)遮擋區(qū)域特征。矩形長(zhǎng)邊為機(jī)器人本體長(zhǎng)度方向，短邊為機(jī)器人本體寬度方向，填充圓圈表示被觸發(fā)(被底盤(pán)遮擋)的傳感器，中空?qǐng)A圈表示未被觸發(fā)的傳感器。紅外傳感器陣列共有216種信號(hào)組合，根據(jù)機(jī)器人實(shí)際工作情況歸納出10種特征工況。圖7(a)～(f)為機(jī)器人前進(jìn)方向與被勘察車(chē)輛底盤(pán)長(zhǎng)度方向垂直，圖7(g)～(j)為機(jī)器人前進(jìn)方向與被勘察車(chē)輛底盤(pán)長(zhǎng)度方向平行。規(guī)定特征工況1，2表示機(jī)器人在汽車(chē)底盤(pán)下；工況3，4表示機(jī)器人位于底盤(pán)邊緣之下，且前進(jìn)方向與底盤(pán)邊緣接近垂直；工況5，6 表示機(jī)器人在底盤(pán)外；工況7～10 表示機(jī)器人位于底盤(pán)邊緣之下，且前進(jìn)方向與底盤(pán)邊緣接近平行。

圖7 紅外傳感器陣列信號(hào)特征Fig.7 Signal characteristics of infrared sensor array

機(jī)器人在進(jìn)行自主勘察作業(yè)時(shí)，慣性導(dǎo)航模塊受車(chē)輛底盤(pán)鐵制磁性材料影響漂移嚴(yán)重。為實(shí)現(xiàn)機(jī)器人按照指定的路徑自主運(yùn)行，當(dāng)機(jī)器人處于底盤(pán)邊緣時(shí)，利用紅外傳感器陣列對(duì)機(jī)器人運(yùn)行方位進(jìn)行校正。如圖8 所示，兩相鄰紅外傳感器的間距為l，機(jī)器人前進(jìn)方向與底盤(pán)邊緣存在一個(gè)偏角δ，最大值為δmax，δmax= arctan。機(jī)器人通過(guò)四輪原地差速轉(zhuǎn)動(dòng)消除δ。

圖8 機(jī)器人偏角Fig.8 Robot declination

機(jī)器人各零部件左右對(duì)稱(chēng)布置，假設(shè)路面絕對(duì)平坦，機(jī)器人只受到重力和路面垂直方向的支持力與水平方向的摩擦力作用。根據(jù)水平方向受力分析(圖9)，可得機(jī)器人動(dòng)力學(xué)微分方程，如式(4)[12-13]。

圖9 機(jī)器人受力分析Fig.9 Forces analysis of the robot

式中：m為機(jī)器人質(zhì)量；γ為機(jī)器人相對(duì)大地參考系的角速度；Fxi，F(xiàn)yi分別為地面對(duì)各車(chē)輪摩擦力在車(chē)長(zhǎng)和車(chē)寬方向的分量；Iz為機(jī)器人相對(duì)質(zhì)心o的轉(zhuǎn)動(dòng)慣量；W為輪距；L為前后輪軸距；d為前軸到質(zhì)心的距離。機(jī)器人在原地勻速轉(zhuǎn)動(dòng)時(shí)，|vx|=0，vy=γh，h為機(jī)器人質(zhì)心o到旋轉(zhuǎn)中心O的距離，則有式(5)。

機(jī)器人繞旋轉(zhuǎn)中心O勻速旋轉(zhuǎn)時(shí)，由對(duì)稱(chēng)關(guān)系知

勻速轉(zhuǎn)動(dòng)時(shí)γ為非零常數(shù)，m＞0，可知h= 0，即機(jī)器人旋轉(zhuǎn)中心與質(zhì)心重合，但與紅外傳感器陣列的幾何中心O′不重合。因此在進(jìn)行邊界校準(zhǔn)時(shí)，旋轉(zhuǎn)中心O到前后對(duì)稱(chēng)的兩紅外傳感器的距離不同，即OA≠OB，如圖10。

圖10 對(duì)齊過(guò)程分析Fig.10 Analysis of the alignment process

校準(zhǔn)主要步驟如下：

1)機(jī)器人順時(shí)針旋轉(zhuǎn)，當(dāng)?shù)谝粋€(gè)紅外傳感器在B點(diǎn)被底盤(pán)遮擋狀態(tài)發(fā)生改變時(shí)，機(jī)器人開(kāi)始計(jì)時(shí)，同時(shí)以角速度γ逆時(shí)針勻速旋轉(zhuǎn)；

2)經(jīng)時(shí)間t后，第二個(gè)傳感器在A′點(diǎn)被底盤(pán)遮擋狀態(tài)發(fā)生改變，機(jī)器人方向與底盤(pán)邊緣夾角為α；

3)機(jī)器人順時(shí)針旋轉(zhuǎn)角度α后，前進(jìn)方向與車(chē)輛底盤(pán)邊緣所在直線平行。

O點(diǎn)為機(jī)器人的旋轉(zhuǎn)中心，根據(jù)機(jī)器人設(shè)計(jì)參數(shù)已知OB′，OA′，OA，OB，AB，A′B′線段的長(zhǎng)度及∠AOB，∠A′OB′，且

由余弦定理知：

式中l(wèi)為線段的長(zhǎng)度。由式(8)可求得∠COC′即α，故在步驟3)時(shí)，機(jī)器人原地順時(shí)針旋轉(zhuǎn)α后與底盤(pán)邊緣平行。

2.5 路徑規(guī)劃

事故車(chē)輛底盤(pán)勘察機(jī)器人工作于未知環(huán)境中，基于傳感器信息，借鑒模糊控制思想實(shí)現(xiàn)局部路徑規(guī)劃[14-16]，完成自主避障和車(chē)輛底盤(pán)遍歷等自主運(yùn)行任務(wù)。對(duì)于乘用車(chē)底盤(pán)離地間隙較小，超聲波傳感器波束角較大時(shí)，探測(cè)結(jié)果易受地面或車(chē)輛底盤(pán)反射聲波的干擾；紅外傳感器波束角較小時(shí)，障礙物顏色對(duì)觸發(fā)距離影響大。故組合使用將超聲波傳感器和紅外傳感器，以提高檢測(cè)精度。組合1為1個(gè)超聲波傳感器和1個(gè)紅外傳感器；組合2為1個(gè)超聲波傳感器和2 個(gè)紅外傳感器。如表3 所示，0 表示無(wú)障礙物、1 表示有障礙物。如圖11 所示，C1，C2，C3，C5，C6和C7為組合1；C4為組合2。如圖12 所示，根據(jù)障礙物在機(jī)器人周?chē)姆植?，抽象? 種障礙物特征類(lèi)型[17]，記Ei={E1,E2,E3,E4,E5,E6,E7,E8}。如圖13 所示，根據(jù)機(jī)器人在車(chē)底的相對(duì)位姿抽象出4 種狀態(tài)，分別為狀態(tài)一、狀態(tài)二、狀態(tài)三、狀態(tài)四，記為Sj={S1,S2,S3,S4}。將Ei和Sj作為輸入變量映射為機(jī)器人動(dòng)作Yk=EiSj，k= 32，實(shí)現(xiàn)機(jī)器人自主避障和自主運(yùn)行。

表3 避障傳感器狀態(tài)組合Tab.3 Status combination of obstacle avoidance sensors

圖11 避障傳感器分布Fig.11 Distribution of the obstacle avoidance sensors

圖12 障礙物特征Fig.12 Characteristics of the obstacles

圖13 機(jī)器人在車(chē)底的運(yùn)行狀態(tài)Fig.13 Running state of robot under the vehicle

3 機(jī)器人道路測(cè)試

在柏油和混凝土路面上，對(duì)研制的機(jī)器人開(kāi)展自主運(yùn)行模式下的測(cè)試試驗(yàn)。

1)如圖14所示，紅外傳感器陣列實(shí)時(shí)判斷機(jī)器人與底盤(pán)關(guān)系，完成自主運(yùn)行。根據(jù)提出的3 步式校準(zhǔn)法修正機(jī)器人前進(jìn)方向。

圖14 底盤(pán)識(shí)別和方向校準(zhǔn)Fig.14 Chassis identification and attitude alignment

2)如圖15所示，當(dāng)機(jī)器人自主運(yùn)行過(guò)程中遇到路肩、車(chē)輪等障礙物時(shí)，避讓后繼續(xù)完成遍歷。

圖15 避障及自主運(yùn)行Fig.15 Obstacle avoidance and autonomous running

3)機(jī)器人在自主運(yùn)行過(guò)程中，可根據(jù)設(shè)定完成拍照，實(shí)時(shí)上傳底盤(pán)圖像并保存至控制終端。部分采集圖像如圖16，可滿(mǎn)足交警部門(mén)對(duì)事故鑒定的照片質(zhì)量要求。

圖16 底盤(pán)圖像采集Fig.16 Chassis images acquisition

4 結(jié)論

針對(duì)交警部門(mén)對(duì)離地間隙狹小乘用車(chē)輛底盤(pán)拍照取證困難的問(wèn)題，研制一種用于乘用車(chē)車(chē)底圖像采集的自主勘察機(jī)器人。結(jié)合機(jī)器人位置坐標(biāo)和頂部紅外傳感器陣列觸發(fā)信息來(lái)自動(dòng)識(shí)別汽車(chē)底盤(pán)圖像信息；采用基于紅外傳感器陣列觸發(fā)狀態(tài)的機(jī)器人相對(duì)車(chē)輛邊沿位姿校正的方法，解決慣性導(dǎo)航模塊在鐵磁性車(chē)輛底盤(pán)下嚴(yán)重漂移導(dǎo)致的導(dǎo)航失效問(wèn)題；基于慣性導(dǎo)航信息和避障傳感器的組合，實(shí)現(xiàn)對(duì)車(chē)輛底盤(pán)拍照取證的自主遍歷。道路測(cè)試結(jié)果表明，研制的機(jī)器人在自主運(yùn)行過(guò)程中，可根據(jù)設(shè)定完成車(chē)輛底盤(pán)取證拍照，實(shí)時(shí)上傳照片并保存至控制終端，上傳的照片可滿(mǎn)足交警部門(mén)對(duì)事故鑒定的圖像質(zhì)量要求。