危險環(huán)境消防偵察機(jī)器人視軸規(guī)劃算法研究

劉滿祿，李銘浩，敖天翔，趙 皓

1.中國科學(xué)技術(shù)大學(xué) 信息科學(xué)技術(shù)學(xué)院，合肥 230026

2.西南科技大學(xué) 信息工程學(xué)院，四川 綿陽 621000

1 引言

社會經(jīng)濟(jì)的不斷發(fā)展導(dǎo)致建筑物愈加密集，發(fā)生火災(zāi)的可能性和危害越來越高。不僅給人民群眾的生命財產(chǎn)安全帶來重大損失，更威脅消防人員的生命安全。例如天津港8.12特大爆炸事故以及近期發(fā)生的倫敦公寓樓大火等。在工業(yè)環(huán)境中，各種工業(yè)原料和產(chǎn)品的燃燒會使災(zāi)害現(xiàn)場充滿有毒有害物質(zhì)，同時伴隨的爆炸會嚴(yán)重破壞現(xiàn)場結(jié)構(gòu)，消防人員無法入內(nèi)[1-2]。隨著機(jī)器人技術(shù)的發(fā)展，消防偵察機(jī)器人開始替代消防人員進(jìn)入高危災(zāi)害現(xiàn)場，進(jìn)行現(xiàn)場偵察，并完成圖像、數(shù)據(jù)等信息采集，有效地幫助消防人員進(jìn)行搶險救災(zāi)。

國際上，美國、日本、英國、德國在消防機(jī)器人研究領(lǐng)域處于領(lǐng)先地位。如美國的Pandora、HAZBOTⅢ和Thermite消防機(jī)器人；日本的FLYGO機(jī)器人。我國已研制出JMX-LT50型消防機(jī)器人及上海強(qiáng)獅消防偵查機(jī)器人等[1-3]。目前，消防偵察機(jī)器人已經(jīng)成為各國消防技術(shù)領(lǐng)域的研究熱點(diǎn)，已有多種不同功能的消防機(jī)器人用于救災(zāi)現(xiàn)場。圖1為自主研發(fā)的一臺針對危險環(huán)境的消防偵察機(jī)器人，機(jī)器人為履帶式車身，高清相機(jī)裝載在一個3自由度機(jī)械臂末端，車身搭載線激光，溫度傳感器，氣體傳感器等各種傳感器，用于在危險環(huán)境中采集信息。利用后端控制臺可以控制車體與機(jī)械臂關(guān)節(jié)移動。為了實(shí)現(xiàn)機(jī)器人更加智能地采集圖像信息，利用計(jì)算機(jī)輔助操作者采集圖像信息，能夠使操作端的操作更加便捷高效。

圖1 危險環(huán)境消防偵查機(jī)器人實(shí)物圖

災(zāi)害現(xiàn)場的非結(jié)構(gòu)性容易對相機(jī)畫面產(chǎn)生遮擋，針對此問題，Su Yong Kim等[3]提出一種通過集成系統(tǒng)來提升偽超聲波移動機(jī)器人的定位精度，在超聲波發(fā)射器和接收器之間存在障礙時，通過卡爾曼濾波器提出一種故障檢測和計(jì)算方法并通過實(shí)驗(yàn)驗(yàn)證了系統(tǒng)的定位性能和故障定位算法的有效性；Gustavo Figueiredo等[4]分析了二維和三維的視軸遮擋問題，并認(rèn)為貪婪算法不能很好地解決逼近問題，并提出一種采用局部搜索算法來達(dá)到1+1/K階逼近的猜想；Lavi M.Zamstein等[5]對處于未知環(huán)境中的移動機(jī)器人使用Q學(xué)習(xí)算法來解決此問題。朱柏林[6]按照云臺技術(shù)指標(biāo)，對路面條件下視軸偏轉(zhuǎn)角進(jìn)行了分析；王宣[7]采用基于局部敏感直方圖的目標(biāo)跟蹤算法，進(jìn)行抗遮擋跟蹤研究；本文提出了一種機(jī)器人在固定位置下，利用外部環(huán)境位置信息，自主尋找最優(yōu)視角拍攝圖像的方法。該方法利用蒙特卡洛采樣思想，利用凸包檢測的方法進(jìn)行碰撞檢測，在機(jī)械臂構(gòu)型空間進(jìn)行均勻采樣。為保證視線區(qū)域沒有遮擋，在觀察區(qū)域與相機(jī)之間建立虛擬連桿，通過碰撞檢測算法保證視角中心無碰撞[8]。在采樣獲得點(diǎn)云中尋找無碰撞的位姿，搜索最佳方向作為最佳拍攝位姿。

2 機(jī)械臂正運(yùn)動學(xué)

為了求解機(jī)械臂運(yùn)動后機(jī)械臂的位置情況，需要建立機(jī)械臂正運(yùn)動學(xué)模型。本文使用指數(shù)積建模法建立機(jī)械臂正運(yùn)動學(xué)方程，與傳統(tǒng)D-H參數(shù)建模相比，指數(shù)積模型[9-10]更加直觀、間接。指數(shù)積建模需要機(jī)械臂在初始位型時的關(guān)節(jié)旋量。

在機(jī)器人底端中心位置建立坐標(biāo)系，按照圖2、3的初始位型參數(shù)和每個關(guān)節(jié)的旋轉(zhuǎn)方向，建立關(guān)節(jié)旋量如下：

其中，w的第n列代表第n個旋轉(zhuǎn)軸ξn的方向。

γ的第n列代表第n個旋轉(zhuǎn)軸ξn的某一個點(diǎn)的位置（單位：mm）。

Pend(0)為末端位置的初始位型（單位：mm）。設(shè)θ=(θ1θ2θ3)，有：

根據(jù)式（4）和（5）可知：

設(shè)Pend(θ)為移動之后的末端點(diǎn)位姿。

圖2 機(jī)器人側(cè)視圖

圖3 機(jī)器人俯視圖

這種建模方法可以根據(jù)機(jī)械臂上各點(diǎn)在初始位型上的位姿信息以一種統(tǒng)一的形式求解關(guān)節(jié)角度變化后機(jī)械臂上各個點(diǎn)位置。

3 碰撞檢測

3.1 利用Minkowski和進(jìn)行問題轉(zhuǎn)換

假設(shè)平面移動物體R是一個凸多邊形，而且障礙物是凸的。如果移動物體的參考點(diǎn)在(x,y)，就將R的位置記作R(x,y)。對于移動物體R而言，每一個障礙物P對應(yīng)的空間障礙區(qū)域都被定義為空間的一個點(diǎn)集，如果R對應(yīng)的位置對于該點(diǎn)集中的某個點(diǎn)，R就會和P相交。因此如果將P的空間障礙區(qū)域記作CP，有：

為畫出CP的形狀，使R沿著P的邊界滑行一圈，R的參考點(diǎn)經(jīng)過的軌跡曲線，就為CP的邊界。

由此，得到Minkowski和[11-12]。對于任意兩個集合S1，S2，其Minkowski和為：

為了能夠利用Minkowski和來敘述空間障礙區(qū)域還需要借助原點(diǎn)對稱鏡像概念，對于二維點(diǎn)p=(px,py)，令-p=(-px,-py)。同理對于任何集合S，定義-S={-p|p∈S}?？傻茫喝鬜為移動物體，P為任意障礙物體，則P對應(yīng)的障礙區(qū)域?yàn)镻⊕(-R(x,y))，又P與R為凸包，則P的障礙區(qū)域?yàn)镻對應(yīng)的凸包邊界點(diǎn)減去R在參考坐標(biāo)點(diǎn)下表示的凸包邊界點(diǎn)形成的點(diǎn)集得到的新凸包。

如此可把空間中兩凸物體的碰撞問題轉(zhuǎn)化參考點(diǎn)的區(qū)域判定問題，如果移動物體的參考點(diǎn)在世界坐標(biāo)系下落在障礙物的碰撞區(qū)域即可認(rèn)為物體發(fā)生了碰撞。

針對本文研究的消防偵察機(jī)器人，障礙物為外部物體和車身，中間連桿和末端相機(jī)及其連桿為移動物體。在移動物體的中心建立參考坐標(biāo)系，得到移動物體邊緣點(diǎn)在此坐標(biāo)系下位置坐標(biāo)。通過外部傳感器獲得外部障礙物的邊緣點(diǎn)位置，用上述方法能建立出障礙物對于移動物體的障礙區(qū)域，然后根據(jù)參考點(diǎn)是否落在障礙區(qū)域內(nèi)判斷物體是否碰撞。

3.2 三維凸包構(gòu)造

借助隨機(jī)增量式算法構(gòu)造P的凸包CH(P)，首先選出不共面的4個點(diǎn)，它們形成的凸包必是一個四面體[13-14]。用雙向鏈接邊表[13]來存儲凸包的4個邊界點(diǎn)、12條半邊和4個面。其次將其余的各點(diǎn)排列成序并按照次序逐點(diǎn)處理，動態(tài)地維護(hù)凸包。不斷循環(huán)將新的點(diǎn)Pr插入到凸包中，即將CH(Pr-1)轉(zhuǎn)化為CH(Pr)，期間可能出現(xiàn)兩種情況。

（1）若Pr落在CH(Pr-1)的內(nèi)部或者邊界上。此新插入點(diǎn)對凸包無影響，即CH(Pr)=CH(Pr-1)。

（2）若Pr落在CH(Pr-1)的外部。在Pr位置上，朝CH(Pr-1)可以看到CH(Pr-1)的一些平面，這些可見的平面組成一個聯(lián)通區(qū)域，稱為Pr在CH(Pr-1)上的可見區(qū)域，也稱有向平面對Pr可見（由半邊的包圍方向使用右手定則確定方向），設(shè)Ef為某個面指向的半邊，Pf為半邊Ef的起始點(diǎn)，Pp為半邊Ef的前向邊的起始點(diǎn)，Pn為半邊Ef的后向邊的起始點(diǎn)。通過下式，若v＞0，則面對Pr可見。

如圖4所示，可見區(qū)域由CH(Pr-1)的一些邊和一條封閉折線圍成，此折線為Pr在CH(Pr-1)的地平線。新插入一個點(diǎn)后，地平線恰好是可見和不可見部分之間的邊界，連接Pr與地平線生成若干新的平面并替換原有可見平面，同時保留不可見平面。雙向鏈接邊表處理：將CH(Pr-1)中所有對Pr可見的平面的信息從雙向鏈邊表中刪除，將Pr地平線連接，并將新平面的信息存儲其中。值得注意的是，當(dāng)Pr與CH(Pr-1)的某平面 f共面時，必須構(gòu)成統(tǒng)一平面。

圖4 可視區(qū)域與地平線

通過遍歷CH(Pr-1)的各個面查找出對Pr可見的面，通過維護(hù)一些附加信息，建立一個沖突列表G來減少復(fù)雜度。具體來說，對于當(dāng)前的凸包CH(Pr)的每個平面 f，都維護(hù)一個 f對其可見的點(diǎn)組成的集合同樣，對于每個點(diǎn)Pt(t＞r)，也要維護(hù)一個集合Fconflict(f)，此集合由CH(Pr)中所有對Pt可見的平面組成。若有一點(diǎn) p∈Pconflict(f)加入到凸包中，平面 f就必須被刪除。同理，為將Pr插入CH(Pr-1)中，只需在G中找出對Pr可見的所有平面并刪除，由Pr與地平線相連接而生成的平面代替，得到更新后的凸包。上述算法的偽代碼如下：

輸入：三維空間中n個點(diǎn)組成的集合P

輸出：P的凸包CH(P)

取點(diǎn) p1、p2、p3、p4，構(gòu)成一個4面體 CH({p1,p2,p3,p4})，任取其余各點(diǎn)的一個隨機(jī)排列：p5、p6、…、pn

初始化沖突圖G，令G={(pt,f)|f為C H上的小平面，pt為對 f可見的點(diǎn)，t＞4}

For r←5 to n //即遍歷CH外的各個點(diǎn)

Do（將 pr插入到CH 中）

If（CH中有小平面對pr可見）//即pr在CH外部

Then在CH中刪除這些小平面，沿著 pr地平線各邊組成有序表Lorder

For(?l∈Lorder) //將各地平線與新插入點(diǎn)相連

Do（生成三角形小平面 f連接l與 pr）

If（和f相鄰于l的平面f′與f共面）

//此時 f′與 f為同一平面

Then將 f與 f′合二為一

Else（確認(rèn) f引起了沖突）

在G中實(shí)例化一個對象 f

令原凸包上與l關(guān)聯(lián)兩平面為 f1和 f2

在圖G中尋找所有與 f1和 f2有關(guān)聯(lián)的點(diǎn)p，若 f對 p可見，添加( )p,f→G

在G中刪除 pr節(jié)點(diǎn)，刪除對 pr可見各平面以及相關(guān)聯(lián)的所有弧線。

Return CH

3.3 碰撞檢測

機(jī)器人在調(diào)節(jié)拍攝視角的時候需要驅(qū)動前置的機(jī)械臂各個關(guān)節(jié)，故可將機(jī)械臂的第二個連桿、攝像機(jī)及其連桿看作移動物體，在此兩個移動物體的中心建立參考坐標(biāo)系，分別提取兩個移動物體的邊緣特征點(diǎn)，在各自的坐標(biāo)系下表示并存儲。其次，將車身、底盤、履帶、后箱與環(huán)境中的其他物體視為障礙物。在某一個固定機(jī)械臂位型下建立移動物體對于每一個障礙物的障礙區(qū)域，每一個障礙物對于一個移動物體都存在一個以三維凸包形式存在的障礙區(qū)域。對于每一個參考點(diǎn)和障礙區(qū)域，分別遍歷障礙區(qū)域的所有面。如圖5和6所示。

圖5 機(jī)械臂第二連桿凸包與障礙物凸包

圖6 對障礙物凸包點(diǎn)進(jìn)行遍歷

設(shè)Pr為參考點(diǎn)，由式（10）計(jì)算v，若存在v＞0，可判斷移動物體和障礙物無碰撞，否則發(fā)生碰撞，遍歷所有障礙物的障礙區(qū)域可得移動物體是否與周圍環(huán)境有無碰撞發(fā)生，對每個移動物體做類似處理可判斷整個機(jī)械臂是否與環(huán)境發(fā)生碰撞。此算法的偽代碼為：

輸入：每個關(guān)節(jié)對應(yīng)的控制量Q1,Q2,Q3

輸出：是否碰撞

已知參數(shù)：core1,core2,…,coren移動物體對應(yīng)的參考坐標(biāo)系

Set1,Set2,…,Setn移動物體對應(yīng)邊緣點(diǎn)集合

Obstacles1,Obstacles2,…,Obstaclesm為障礙物的邊緣點(diǎn)集合

For i=1:n（n為移動物體數(shù)量）

利用正運(yùn)動學(xué)計(jì)算更新移動物體的參考坐標(biāo)系corei

更新相關(guān)物體邊緣點(diǎn)集合信息

For j=1:m

根據(jù)障礙物 j的邊緣點(diǎn)集與移動物體i的邊緣點(diǎn)集建立障礙區(qū)域點(diǎn)集

建立障礙區(qū)域凸包

遍歷凸包面查看參考坐標(biāo)點(diǎn)是否落在障礙區(qū)域內(nèi)

If（落在障礙區(qū)域內(nèi)）

Return（發(fā)生碰撞）

Return（無碰撞）

4 蒙特卡洛方法尋找適合的視軸

機(jī)器人需要找到合適的位置擺放機(jī)械臂的末端攝像頭以便觀察固定的熱點(diǎn)區(qū)域，特別是對于存在障礙物的情況下要保證視線和機(jī)械臂同時避開障礙物，同時保證熱點(diǎn)區(qū)域處于攝像頭的中心位置。本文采用蒙特卡洛隨機(jī)采樣的方法[15-16]，在機(jī)械臂的關(guān)節(jié)空間隨機(jī)采樣足夠多的點(diǎn)，利用碰撞檢測算法檢測機(jī)械臂是否與外界發(fā)生碰撞。保存無碰撞的采樣點(diǎn)，根據(jù)正運(yùn)動學(xué)方程，建立末端位置矢量圖。

依據(jù)算法得到的位置方向矢量圖，計(jì)算攝像機(jī)朝向向量與末端到目標(biāo)點(diǎn)朝向向量的相關(guān)度度。使用末端攝像機(jī)的朝向和相機(jī)與熱點(diǎn)區(qū)域的方向向量的點(diǎn)乘作為計(jì)算相關(guān)度λ。

其中，end為末端攝像頭的朝向，Pend為末端點(diǎn)位置，Phot為熱點(diǎn)區(qū)域的位置。得到一個相關(guān)度大于閾值的備選集合。按照備選集，在熱點(diǎn)區(qū)域與攝像機(jī)末端建立虛擬連桿[17-18]，利用碰撞檢測算法檢測碰撞，將視角的遮擋問題轉(zhuǎn)化為三維物體的碰撞檢測問題，保證視線不被遮擋。

選擇滿足條件的備選采樣點(diǎn)，計(jì)算角度變化量，選擇角度變化總和最小的采樣點(diǎn)為最佳的位置。

5 仿真驗(yàn)證

在SolidWorks（版本號：2014）中建立機(jī)器人的CAD模型，導(dǎo)入至V-REP（版本號：3.3.2），將各個模塊從新組裝起來，同時添加轉(zhuǎn)動關(guān)節(jié)，建立起機(jī)器人本體，添加兩個方塊作為障礙物。搭建實(shí)驗(yàn)仿真環(huán)境仿真環(huán)境。如圖7所示。

圖7 仿真環(huán)境圖

利用均勻采樣和碰撞檢測算法可以在角空間形成柵格地圖，在笛卡爾空間可以生成對應(yīng)的末端點(diǎn)位置和方向矢量，可以用圖8矢量圖表示（構(gòu)型空間隨機(jī)采樣10 000個點(diǎn)，第一個關(guān)節(jié)范圍為?180°～+180°，第二個關(guān)節(jié)范圍為?120°～+120°，第三個關(guān)節(jié)為?90°～+90°）。

圖8 末端位置矢量圖

依據(jù)算法得到的位置方向矢量圖，計(jì)算攝像機(jī)朝向向量與末端到目標(biāo)點(diǎn)朝向向量的相關(guān)度，得到一個相關(guān)度大于閾值的備選集合。設(shè)置朝向評價指標(biāo)范圍為0.85～1，在目標(biāo)點(diǎn)與機(jī)械臂末端點(diǎn)間建立虛擬連桿，遍歷所有備選采樣點(diǎn)可以建立一個無碰撞，無遮擋，方向指標(biāo)滿足的采樣集合，然后再在集合中計(jì)算根據(jù)各個關(guān)節(jié)角變化值之和選擇最小的采樣點(diǎn)以此確定最佳的相機(jī)擺放方法。

在仿真中以一個小球作為機(jī)器人重點(diǎn)觀察的熱點(diǎn)區(qū)域，左下角浮窗為相機(jī)拍攝到的畫面，不斷改變小球的位置（從左、中至右，如圖9），利用上述算法可以實(shí)現(xiàn)機(jī)械臂對熱點(diǎn)區(qū)域的定位拍攝。

如圖10所示，對于同一個熱點(diǎn)位置多次運(yùn)行獲得的拍攝畫面差異度也極小。

仿真實(shí)驗(yàn)平均耗時為34 s，算法主要耗時在第一步建立位置矢量圖，需要約30 s，如果外部環(huán)境不發(fā)生變化，機(jī)器人本體不發(fā)生移動，則不需要更新位置矢量圖，如圖11所示算法可以在5 s內(nèi)運(yùn)算出結(jié)果。適量減少采樣點(diǎn)數(shù)可以減少算法時間。

圖9 熱點(diǎn)區(qū)域不同位置仿真圖

圖10 同一熱點(diǎn)多次拍攝圖

圖11 多次實(shí)驗(yàn)耗時統(tǒng)計(jì)

實(shí)驗(yàn)證明，基于上述方法的機(jī)器人視角規(guī)劃方法可以自主地尋找到優(yōu)化的攝像頭擺放位置，為操作人員提供決策支持。

6 結(jié)束語

本文使用旋量理論對3自由度機(jī)械臂建立正運(yùn)動學(xué)模型，對機(jī)械臂末端相機(jī)的視軸進(jìn)行避障規(guī)劃并對重點(diǎn)區(qū)域進(jìn)行視軸鎖定。利用Minkowski和將空間中兩凸物體的碰撞問題轉(zhuǎn)換為轉(zhuǎn)化參考點(diǎn)的區(qū)域判定問題，借助隨機(jī)增量式算法構(gòu)造三維凸包，并使用雙向鏈接邊表對數(shù)據(jù)進(jìn)行存儲，在相機(jī)和重點(diǎn)觀察區(qū)域之間建立虛擬連桿，使用碰撞檢測算法解決機(jī)械臂與周圍環(huán)境碰撞以及相機(jī)視軸無遮擋問題，并給出相應(yīng)偽代碼。最后使用蒙特卡洛算法求取獲得機(jī)械臂末端相機(jī)最佳位姿。實(shí)驗(yàn)證明，該方法能完成機(jī)械臂末端相機(jī)視軸無遮擋規(guī)劃，仿真效果良好。