一種基于單目相機和主動結(jié)構(gòu)光的障礙探測方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種基于單目相機和主動結(jié)構(gòu)光的障礙探測方法，適用于在月球、火 星等外星球表面進行探測任務(wù)的巡視器的障礙識別，也可用于在野外惡劣環(huán)境進行作業(yè)的 移動機器人的障礙識別。
【背景技術(shù)】
[0002] 巡視探測器在行星工作階段，需要控制巡視探測器在非結(jié)構(gòu)復(fù)雜地面環(huán)境中實現(xiàn) 長距離行駛，并安全到達預(yù)先指定的工作點，以完成特定的科學(xué)考察任務(wù)，并保障自身的安 全和穩(wěn)定工作。由于行星表面環(huán)境的不可預(yù)知性，巡視器必須具有環(huán)境感知的能力，在選擇 配備了立體相機作為環(huán)境感知的主要手段之后，考慮到行星表面陰影區(qū)完全無光照，巡視 器進入陰影區(qū)或背對太陽時無法探測環(huán)境進行安全行走，所以考慮增加主動結(jié)構(gòu)光，利用 投射出的離散激光點進行陰影區(qū)內(nèi)的障礙識別判斷，保證行走安全。
[0003] 針對相機和主動結(jié)構(gòu)光的障礙探測方式，考慮冗余配置問題，設(shè)計了主動結(jié)構(gòu)光 分別和雙目相機、左單目、右單目相機配合的三種工作方式。其中雙目相機的方式類似立體 視覺重建，而單目相機和主動結(jié)構(gòu)光的配合使用，就需要用到結(jié)構(gòu)光本身的信息，為此提出 了激光點空間出射方程的高精度標定方法，在主動結(jié)構(gòu)光加工完成后，在地面進行了離散 激光出射線的標定，通過標定結(jié)果和相機拍攝的激光點圖像實現(xiàn)了主動結(jié)構(gòu)光和左相機或 者右相機的激光點坐標解算，保證在任何一個相機失效的情況下，主動結(jié)構(gòu)光探測仍能進 行，提高了結(jié)構(gòu)光探測的可靠性和安全性。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明要解決的技術(shù)問題是：為克服現(xiàn)有技術(shù)的不足，提出一種基于單目相機和 主動結(jié)構(gòu)光的障礙探測方法，在基準相機失效的情況下，利用備份相機仍能實現(xiàn)障礙物的 快速識別和判斷。
[0005] 本發(fā)明技術(shù)解決方案：
[0006] 一種基于單目相機和主動結(jié)構(gòu)光的障礙探測方法，具體步驟如下：
[0007] (1)基準相機與主動結(jié)構(gòu)光坐標標定
[0008] 在主動結(jié)構(gòu)光的外部粘接基準鏡作為外測基準建立基準鏡下坐標系，同時將 基準鏡作為基準相機虛擬坐標系的外引基準，建立與主動結(jié)構(gòu)光外測基準之間的關(guān)系， 得到基準相機坐標系下主動結(jié)構(gòu)光出射線的標定方程；假設(shè)主動結(jié)構(gòu)光一共設(shè)計有N 個激光出射點，標定得到的主動結(jié)構(gòu)光的出射線方程在基準相機坐標系下的描述如下
【主權(quán)項】
1. 一種基于單目相機和主動結(jié)構(gòu)光的障礙探測方法，其特征在于，具體步驟如下： (1) 基準相機與主動結(jié)構(gòu)光坐標標定 在主動結(jié)構(gòu)光的外部粘接基準鏡作為外測基準建立基準鏡下坐標系，同時將基準 鏡作為基準相機虛擬坐標系的外引基準，建立與主動結(jié)構(gòu)光外測基準之間的關(guān)系，得 到基準相機坐標系下主動結(jié)構(gòu)光出射線的標定方程；假設(shè)主動結(jié)構(gòu)光一共設(shè)計有N個 激光出射點，標定得到的主動結(jié)構(gòu)光的出射線方程在基準相機坐標系下的描述如下
(A0i，B0i，C0i)為直線上一點坐標，（Pi，qi，ri)為直 線的矢量方向；用基準相機對激光點成像，得到基準圖像； (2) 備份相機與基準相機坐標轉(zhuǎn)換 可另設(shè)備份相機，備份相機到基準相機坐標系的轉(zhuǎn)換矩陣為
，平移 矩陣為
，同時用備份相機對激光點成像，得到備份圖像； (3) 激光圖像處理 將步驟（1)及步驟（2)得到的基準圖像和備份圖像中灰度值大于灰度閾值的像素點分 割出來，剔除偽激光點及由其他雜光在圖像中引起的噪點； 根據(jù)從圖像中提取出的激光點的能量中心，得到激光點能量中心在基準圖像中的坐標 和備份圖像中的坐標，并確定圖像中的激光點對應(yīng)的編號； (4) 激光點三維恢復(fù) 結(jié)合相機參數(shù)，對步驟（3)中識別出的各激光點的坐標進行校正，消除其畸變誤差，進 行基準相機、備份相機與主動結(jié)構(gòu)光的坐標解算，激光點在基準圖像上三維坐標確定方法 為：
其中為基準相機焦距，（xli,ylDi= 1?N為基準圖像中激光點坐標，
M= (A)_1C 其中M= 〇^)6><1，（A)4表示對矩陣A進行逆運算
得到激光點的三維坐標； 其中（xp(i),yp(i),zp⑴）i= 1?N為地面投射的主動結(jié)構(gòu)光點在基準相機坐標系下 的坐標； 激光點在備份圖像上二維坐標確定方法為：
其中： f2為備份相機焦距，（x2i，y2i)i= 1?N為備份圖像中激光點的坐標， Ll-(r 23X2i+r2lf2) * (r33y2i+r32f2) _ (r33X2i+r3lf2) * (r23y2i+r22f2) [2 - _(r 33X2i+r3lf2) * (ri3y2i+ri2f2) + (ri3X2i+rilf2) * (r33y2i+r32f2) L3- _(r 13X2i+rilf2) * (r23y2i+r22f2) + (r23X2i+r2lf2) * (ri3y2i+ri2f2)
M= (A)_1C 其中M= (11^)6><1，（Ar1表示對矩陣A進行逆運算
得到激光點的三維坐標； 其中（xp(i),yp(i),zp⑴）i= 1?N為地面投射的主動結(jié)構(gòu)光點在備份相機坐標系下 的坐標； (5)障礙判斷 結(jié)合相機的安裝及巡視器的姿態(tài)，得到由相機坐標系到水平坐標系的轉(zhuǎn)換矩陣，然后 進行平面擬合，得到各激光點到擬合平面的距離，進而判斷激光投射區(qū)域是否存在障礙。
2.根據(jù)權(quán)利要求1所述的一種基于單目相機和主動結(jié)構(gòu)光的障礙探測方法，其特征在 于：所述步驟（5)中相機坐標系的定義為：原點位于相機光心，Z。軸沿相機的光軸向前，與 圖像平面垂直，X。軸與Z。軸垂直，指向相機系統(tǒng)的右側(cè)，Y。軸與X。軸、Z。軸滿足右手定則。
【專利摘要】本發(fā)明公開了一種基于單目相機和主動結(jié)構(gòu)光的障礙探測方法，實現(xiàn)步驟包括基準相機與主動結(jié)構(gòu)光坐標標定、備份相機與基準相機坐標轉(zhuǎn)換、激光圖像處理、激光點三維恢復(fù)、障礙判斷。本發(fā)明能夠在陰影區(qū)域或者缺乏紋理特征區(qū)域進行環(huán)境感知；利用結(jié)構(gòu)光的先驗信息和單個相機配合進行快速三維恢復(fù)，計算量小，處理時間短，能夠?qū)崿F(xiàn)障礙物的快速識別和判斷。
【IPC分類】G01V8-10, G01C3-00
【公開號】CN104570147
【申請?zhí)枴緾N201410829101
【發(fā)明人】毛曉艷, 邢琰, 劉祥, 滕寶毅, 賈永
【申請人】北京控制工程研究所
【公開日】2015年4月29日
【申請日】2014年12月26日