[發(fā)明專利]融合激光雷達(dá)、雙目相機(jī)和ToF深度相機(jī)數(shù)據(jù)的智能機(jī)器人深度圖像構(gòu)建方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110007417.0
• 申請(qǐng)日: 2021-01-05
• 公開（公告）號(hào): CN112766328B
• 公開（公告）日: 2022-08-12
• 發(fā)明（設(shè)計(jì)）人: 吳星明;孔令坤;陳偉海;王建華;趙小銘;馬鑫至
• 申請(qǐng)（專利權(quán)）人: 北京航空航天大學(xué)
• 主分類號(hào): G06K9/62
• 分類號(hào): G06K9/62;G06V10/80;G06T7/50;G06T7/80
• 代理公司: 北京永創(chuàng)新實(shí)專利事務(wù)所 11121
• 代理人: 冀學(xué)軍

1.一種融合激光雷達(dá)、雙目相機(jī)和ToF深度相機(jī)數(shù)據(jù)的智能機(jī)器人深度圖像構(gòu)建方法，其特征在于深度圖像構(gòu)建包括有下列步驟：

步驟一：多源傳感器數(shù)據(jù)注冊(cè)；

在啟動(dòng)激光雷達(dá)(11)、ToF深度相機(jī)(12)、雙目相機(jī)(13)之后，在采樣時(shí)間t_采樣里，多源傳感器數(shù)據(jù)注冊(cè)單元(20)第一方面用于接收激光雷達(dá)(11)輸出的點(diǎn)云數(shù)據(jù)M₁₁；由于激光雷達(dá)(11)掃描獲得的點(diǎn)云數(shù)據(jù)中包含有環(huán)境數(shù)據(jù)，需要工作人員手動(dòng)選取前標(biāo)定物(2)與后標(biāo)定物(3)同時(shí)存在的區(qū)域的點(diǎn)云數(shù)據(jù)，記為所述中存在的點(diǎn)云總個(gè)數(shù)，記為N；

第二方面用于接收ToF深度相機(jī)(12)輸出的ToF深度圖像信息M₁₂；由于ToF深度相機(jī)(12)拍攝獲得的ToF深度圖像信息中包含有環(huán)境數(shù)據(jù)，需要工作人員手動(dòng)選取前標(biāo)定物(2)與后標(biāo)定物(3)同時(shí)存在的區(qū)域的ToF深度圖像信息，記為所述中以任意一個(gè)像素點(diǎn)的像素值來確定深度信息；

第三方面用于接收雙目相機(jī)(13)輸出的雙目圖像信息和雙目深度信息M₁₃；由于雙目相機(jī)(13)拍攝獲得的雙目圖像信息和雙目深度信息中包含有環(huán)境數(shù)據(jù)，需要工作人員手動(dòng)選取前標(biāo)定物(2)與后標(biāo)定物(3)同時(shí)存在的區(qū)域的雙目深度信息，記為所述中以任意一個(gè)像素點(diǎn)的像素值來確定深度信息；

第四方面，根據(jù)像素點(diǎn)的深度信息與ToF深度相機(jī)(12)的內(nèi)參值，利用相機(jī)成像模型將投影為ToF深度相機(jī)(12)的坐標(biāo)系O_ToF-X_ToFY_ToFZ_ToF下的點(diǎn)云信息，記為

根據(jù)像素點(diǎn)的深度信息與雙目相機(jī)(13)的內(nèi)參值，利用相機(jī)成像模型將投影為雙目相機(jī)(13)坐標(biāo)系O_雙目-X_雙目Y_雙目Z_雙目下的點(diǎn)云信息，記為

第五方面，完成異質(zhì)傳感器的位姿標(biāo)定；

利用點(diǎn)云ICP算法，計(jì)算出和之間的位姿變換矩陣R_ZL、Q_ZL、R_ZT、Q_ZT，即可得到激光雷達(dá)(11)、ToF深度相機(jī)(12)、雙目相機(jī)(13)之間的位置關(guān)系，從而完成此三個(gè)傳感器的位姿標(biāo)定；

R_ZL為到的旋轉(zhuǎn)矩陣；

Q_ZL為到的平移矩陣；

R_ZT為到的旋轉(zhuǎn)矩陣；

Q_ZT為到的平移矩陣；

具體的，求解到的位姿變換矩陣：

R_LT為到的旋轉(zhuǎn)矩陣；

Q_LT為到的平移矩陣；

為不斷迭代的所述R_LT的中間量；

為不斷迭代的所述Q_LT的中間量；

N為中點(diǎn)云的總個(gè)數(shù)；

p為中的任意一個(gè)點(diǎn)；

q為中的一個(gè)點(diǎn)且與所述中的點(diǎn)p距離最近；

利用梯度下降法計(jì)算使得公式(1)等式右邊最小，即可獲得R_LT、Q_LT；

具體的，求解到的位姿變換矩陣：

R_LZ為到的旋轉(zhuǎn)矩陣；

Q_LZ為到的平移矩陣；

為不斷迭代的所述R_LZ的中間量；

為不斷迭代的所述Q_LZ的中間量；

N為中點(diǎn)云的總個(gè)數(shù)；

p為中的任意一個(gè)點(diǎn)；

w為中的一個(gè)點(diǎn)且與所述中的點(diǎn)p距離最近；

利用梯度下降法計(jì)算使得公式(2)等式右邊最小，即可獲得R_LZ、Q_LZ；

通過公式(3)、公式(4)、公式(5)、公式(6)可獲得從坐標(biāo)系O_雷達(dá)-X_雷達(dá)Y_雷達(dá)Z_雷達(dá)到坐標(biāo)系O_雙目-X_雙目Y_雙目Z_雙目的位姿變換關(guān)系R_ZL、Q_ZL以及從坐標(biāo)系O_ToF-X_ToFY_ToFZ_ToF到坐標(biāo)系O_雙目-X_雙目Y_雙目Z_雙目的位姿變換關(guān)系R_ZT、Q_ZT：

上角標(biāo)T為坐標(biāo)轉(zhuǎn)置符；

利用坐標(biāo)系的位姿變換關(guān)系，將變換到坐標(biāo)系O_雙目-X_雙目Y_雙目Z_雙目下，從而完成多源傳感器數(shù)據(jù)的注冊(cè)；

步驟二：多源傳感器數(shù)據(jù)關(guān)聯(lián)；

步驟21：異質(zhì)傳感器的統(tǒng)一時(shí)間戳；

首先，計(jì)算機(jī)器人的實(shí)際移動(dòng)速度v；在機(jī)器人平臺(tái)以速度v相對(duì)于標(biāo)記物移動(dòng)的過程中，雙目相機(jī)(13)會(huì)以一定幀率獲得標(biāo)記物的位姿，通過一段時(shí)間的位置微分，即可求得機(jī)器人的實(shí)際移動(dòng)速度v；

其次，計(jì)算ToF深度相機(jī)(12)和雙目相機(jī)(13)的數(shù)據(jù)采集時(shí)間間隔；

ToF深度相機(jī)(12)與雙目相機(jī)(13)各有一時(shí)間軸，雙目相機(jī)(13)在A時(shí)刻觀測(cè)到標(biāo)記所在位置為tag_h_A；ToF深度相機(jī)(12)在b時(shí)刻觀測(cè)到標(biāo)記所在位置為tag_h_b，其中A時(shí)刻為雙目相機(jī)(13)返回?cái)?shù)據(jù)的時(shí)間戳，b時(shí)刻為ToF深度相機(jī)(12)返回?cái)?shù)據(jù)的時(shí)間戳；但由于ToF深度相機(jī)(12)數(shù)據(jù)采集存在較大延遲，因此ToF深度相機(jī)(12)的時(shí)間軸應(yīng)整體向左移動(dòng)；對(duì)于雙目相機(jī)(13)的A時(shí)刻而言，在ToF深度相機(jī)(12)時(shí)間軸上應(yīng)處于d時(shí)刻，若ToF深度相機(jī)(12)與雙目相機(jī)(13)時(shí)間不存在時(shí)間戳偏移，那么雙目相機(jī)(13)的時(shí)間軸上的A時(shí)刻在ToF深度相機(jī)(12)的時(shí)間軸上應(yīng)處于c時(shí)刻；然而，由于時(shí)間戳偏移的存在，雙目相機(jī)(13)的時(shí)間軸上的A時(shí)刻對(duì)應(yīng)在ToF深度相機(jī)(12)的時(shí)間軸上是d時(shí)刻；則A時(shí)刻與d時(shí)刻數(shù)值的差值即為兩傳感器時(shí)間戳之間的偏移量Δt；已知ToF深度相機(jī)(12)的坐標(biāo)系到雙目相機(jī)(13)的坐標(biāo)系的旋轉(zhuǎn)矩陣為R_ZT，平移向量為Q_ZT，有以下關(guān)系：

t₂＝t_b-t_c＝t_b-t_A (8)

t₁表示ToF深度相機(jī)(12)的時(shí)間軸上d時(shí)刻與b時(shí)刻的時(shí)間間隔；

t₂表示ToF深度相機(jī)(12)的時(shí)間軸上c時(shí)刻與b時(shí)刻的時(shí)間間隔；

t_b表示ToF深度相機(jī)(12)的時(shí)間軸上b時(shí)刻的數(shù)值；

t_d表示ToF深度相機(jī)(12)的時(shí)間軸上d時(shí)刻的數(shù)值；

t_c表示ToF深度相機(jī)(12)的時(shí)間軸上c時(shí)刻的數(shù)值；

t_A表示雙目相機(jī)(13)的時(shí)間軸上A時(shí)刻的數(shù)值；

ToF深度相機(jī)(12)與雙目相機(jī)(13)時(shí)間戳的偏移量Δt為：

Δt＝t₂-t₁ (9)

步驟22：異質(zhì)傳感器數(shù)據(jù)同步選取；

雙目相機(jī)(13)的數(shù)據(jù)采集頻率表最高，ToF深度相機(jī)(12)次之，激光雷達(dá)(11)采集頻率最慢；

由于雙目相機(jī)(13)的工作頻率最高，因此主要以雙目相機(jī)(13)時(shí)間軸為參考，選取與雙目相機(jī)(13)深度圖像數(shù)據(jù)時(shí)間點(diǎn)接近的數(shù)據(jù)；當(dāng)接收到雙目相機(jī)(13)的數(shù)據(jù)時(shí)，判斷在前后10ms的時(shí)間差內(nèi)有無接收到ToF深度相機(jī)(12)的數(shù)據(jù)和激光雷達(dá)(11)的數(shù)據(jù)，若有，則將此三幀數(shù)據(jù)選取為有效地同步數(shù)據(jù)；若沒有，則當(dāng)下一次接收到雙目相機(jī)(13)的數(shù)據(jù)時(shí)再進(jìn)行同步數(shù)據(jù)選取；

以雙目相機(jī)(13)的時(shí)間軸上B時(shí)刻為例，在B時(shí)刻雙目相機(jī)(13)輸出深度圖像數(shù)據(jù)，與B時(shí)刻最近的ToF深度相機(jī)(12)的深度數(shù)據(jù)在e時(shí)刻，最近的激光雷達(dá)(11)數(shù)據(jù)在Ⅲ時(shí)刻；在進(jìn)行深度數(shù)據(jù)融合時(shí)，只選取與雙目數(shù)據(jù)時(shí)間點(diǎn)B時(shí)刻最近且時(shí)間差不超過10ms的ToF深度數(shù)據(jù)和激光雷達(dá)數(shù)據(jù)，以減小或消除由傳感器頻率不同所造成的數(shù)據(jù)誤差，保證不同傳感器所捕獲的數(shù)據(jù)在時(shí)間和空間上實(shí)現(xiàn)對(duì)齊；

步驟三：多源深度數(shù)據(jù)融合；

在采樣時(shí)間t_采樣里，激光雷達(dá)(11)掃描獲得的點(diǎn)云數(shù)據(jù)，記為由于點(diǎn)云信息無法與圖像信息直接融合，故利用相機(jī)成像模型將所述轉(zhuǎn)換為深度圖像，記為

在采樣時(shí)間t_采樣里，ToF深度相機(jī)(12)拍攝獲得的ToF深度圖像信息，記為

在采樣時(shí)間t_采樣里，雙目相機(jī)(13)拍攝獲得的雙目深度信息，記為

目標(biāo)深度圖像MPR的優(yōu)化目標(biāo)：

下角標(biāo)i表示像素點(diǎn)在水平方向的坐標(biāo)；

下角標(biāo)j表示像素點(diǎn)在垂直方向的坐標(biāo)；

MPR表示最終優(yōu)化得到的在雙目相機(jī)13坐標(biāo)系O_雙目-X_雙目Y_雙目Z_雙目下成像平面上的融合深度圖像；

MPR^*表示優(yōu)化過程中所述MPR的中間量；

k_i,j表示最終優(yōu)化得到的所述MPR在像素坐標(biāo)點(diǎn)(i,j)處的像素值；

表示優(yōu)化過程中所述MPR^*在像素坐標(biāo)點(diǎn)(i,j)處的像素值；

表示優(yōu)化過程中所述MPR^*在像素坐標(biāo)點(diǎn)(i,j)處的梯度值；

l_i,j表示雷達(dá)深度圖像在像素坐標(biāo)點(diǎn)(i,j)處的像素值；

r_i,j表示依據(jù)狀態(tài)選取的深度圖(或)在像素坐標(biāo)點(diǎn)(i,j)處的像素值；

表示依據(jù)狀態(tài)選取的深度圖(或)在像素坐標(biāo)點(diǎn)(i,j)處的梯度值，簡稱為狀態(tài)下的梯度值；

由于ToF深度相機(jī)12或雙目相機(jī)13的深度圖像在像素坐標(biāo)點(diǎn)(i,j)處存在沒有深度數(shù)據(jù)的情況下，故需依據(jù)狀態(tài)對(duì)狀態(tài)下的梯度值進(jìn)行選擇；

梯度值狀態(tài)A是指在像素坐標(biāo)點(diǎn)(i,j)處存在深度數(shù)據(jù)，此時(shí)，等于在像素坐標(biāo)點(diǎn)(i,j)處的梯度值；

梯度值狀態(tài)B是指在像素坐標(biāo)點(diǎn)(i,j)處不存在深度數(shù)據(jù)，在像素坐標(biāo)點(diǎn)(i,j)處存在深度數(shù)據(jù)，此時(shí)等于在像素坐標(biāo)點(diǎn)(i,j)處的梯度值；

梯度值狀態(tài)C是指和在像素坐標(biāo)點(diǎn)(i,j)處均不存在深度數(shù)據(jù)，此時(shí)為0；

為了獲得最佳的目標(biāo)深度圖像MPR，公式(10)的等式右邊取值最小，將公式(10)書寫為便于求解的二次形形式；直接用初始化目標(biāo)深度圖像MPR，因此公式(10)可省去第一項(xiàng)，即可得到如下公式：

下角標(biāo)x表示圖像的橫軸，下角標(biāo)y表示圖像的縱軸；

表示優(yōu)化過程中所述MPR^*在像素坐標(biāo)點(diǎn)(i,j)處沿x軸方向的梯度值；

表示優(yōu)化過程中所述MPR^*在像素坐標(biāo)點(diǎn)(i,j)處沿y軸方向的梯度值；

表示依據(jù)狀態(tài)選取的深度圖(或)在像素坐標(biāo)點(diǎn)(i,j)處沿x軸方向的梯度值；

表示依據(jù)狀態(tài)選取的深度圖(或)在像素坐標(biāo)點(diǎn)(i,j)處沿y軸方向的梯度值；

由于ToF深度相機(jī)12或雙目相機(jī)13的深度圖像在像素坐標(biāo)點(diǎn)(i,j)處存在沒有深度數(shù)據(jù)的情況下，故需依據(jù)狀態(tài)對(duì)和進(jìn)行選擇；

在梯度值狀態(tài)A時(shí)，等于在像素坐標(biāo)點(diǎn)(i,j)處沿x軸方向的梯度值，等于在像素坐標(biāo)點(diǎn)(i,j)處沿y軸方向的梯度值；

在梯度值狀態(tài)B時(shí)，等于在像素坐標(biāo)點(diǎn)(i,j)處沿x軸方向的梯度值，等于在像素坐標(biāo)點(diǎn)(i,j)處沿y軸方向的梯度值；

在梯度值狀態(tài)C時(shí)，和為0；

選取沿x軸和沿y軸兩種位置關(guān)系作為梯度特征，中每個(gè)有深度數(shù)據(jù)的像素點(diǎn)為中心，將這些像素點(diǎn)的像素值作為真值，通過公式(11)對(duì)和進(jìn)行融合，從而可在復(fù)雜的工作環(huán)境下獲取到更加準(zhǔn)確、稠密的深度圖像信息。