一種基于室內(nèi)四旋翼飛行器的激光雷達三維成像方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種三維成像方法，尤其是一種基于室內(nèi)四旋翼飛行器的三維成像方 法。
【背景技術(shù)】
[0002] 隨著現(xiàn)代航空航天事業(yè)的飛速發(fā)展，自主飛行器也在現(xiàn)代高新技術(shù)條件下快速發(fā) 展。飛行器能在枯燥、危險、惡劣環(huán)境中發(fā)揮重要作用。在軍事領(lǐng)域中能夠?qū)嵤┑涂諅刹欤?情報采集等；在民用領(lǐng)域中可以完成資源勘探、邊境巡邏、災(zāi)情監(jiān)視等。而對于坍塌樓房，礦 井等地面移動機器人無法勝任的環(huán)境下，靈活的自主飛行器對于探測未知環(huán)境是很好的選 擇。
[0003] 對于三維成像方法，基于圖像處理獲取場景信息，通過圖像拼接實現(xiàn)三維成像，該 方法算法復(fù)雜，受光照影響，可靠性低；基于激光的三維成像方法主要應(yīng)用于靜態(tài)對象的三 維建模，激光器的擺放位置嚴格，設(shè)備便捷度低；基于地面移動機器人的三維成像方法受環(huán) 境障礙物影響大，無法全方位實現(xiàn)三維成像。

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

[0004] 本發(fā)明要解決的技術(shù)問題是現(xiàn)有的三維成像方法使用復(fù)雜，且常常由于環(huán)境障礙 物影響無法全方位實現(xiàn)三維成像。
[0005] 為了解決上述技術(shù)問題，本發(fā)明提供了一種基于室內(nèi)四旋翼飛行器的激光雷達三 維成像方法，包括如下步驟：
[0006] 步驟1，讀取四旋翼飛行器慣性導航系統(tǒng)測量的加速度數(shù)據(jù)以及測距系統(tǒng)測量的 距離數(shù)據(jù)，再利用互補濾波法對加速度數(shù)據(jù)和距離數(shù)據(jù)進行數(shù)據(jù)融合，從而獲得四旋翼飛 行器的位移數(shù)據(jù)；
[0007] 步驟2,讀取四旋翼飛行器慣性導航系統(tǒng)測量的慣性測量數(shù)據(jù)，并利用卡爾曼濾波 對慣性測量數(shù)據(jù)進行數(shù)據(jù)融合，獲得四旋翼飛行器的姿態(tài)角度數(shù)據(jù)；
[0008] 步驟3,讀取四旋翼飛行器前進方向上激光雷達采集的深度斷層圖像數(shù)據(jù)，再對 深度斷層圖像數(shù)據(jù)進行自適應(yīng)中值濾波，并進行基于位移和角度的補償，獲得深度斷層圖 像；
[0009] 步驟4,根據(jù)步驟1的位移數(shù)據(jù)和步驟2的姿態(tài)角度數(shù)據(jù)確定四旋翼飛行器運動軌 跡，并對運動軌跡上的相鄰數(shù)據(jù)采集點的深度斷層圖像使用滑動窗口牛頓插值法，從而實 現(xiàn)三維成像。
[0010] 采用四旋翼飛行器作為搭載平臺具有高度的靈活性，相對于地面移動機器人能夠 降低復(fù)雜地面對飛行的影響，具有較高的適應(yīng)能力；采用測量的位移數(shù)據(jù)和姿態(tài)角度數(shù)據(jù) 確定深度斷層圖像數(shù)據(jù)的位置，從而有效提高了激光雷達的掃描范圍，擴大了三維成像的 面積范圍。
[0011] 作為本發(fā)明的進一步限定方案，步驟1中，獲得四旋翼飛行器位移數(shù)據(jù)的具體步 驟為：
[0012] 步驟1. 1，讀取的加速度數(shù)據(jù)為（ax(t)，ay(t)，az(t))，進行二重積分得到t。濟tc 時刻的近似位移（xt，yt，zt)為：
[0013]
(1)
[0014] 瓦甲，CvxUcJ，vyUcAvzUcW艿tc !時刻的速度；
[0015] 步驟1· 2,讀取到t。^t。時刻的距離數(shù)據(jù)為，為 tc丨至丨」t。時刻的近似位移（mxt,myt,mzt)為：
[0016]
^ .....-.」 (2)
[0017] 步驟1. 3,利用互補濾波法進行數(shù)據(jù)融合獲得四旋翼飛行器的位移數(shù)據(jù) 為：
[0018]
[0019]
[0020] 式中，Tu為超聲波傳感器采樣周期，（τχ，τy，τz)為置信程度，（ax，ay，az)為動態(tài) 加權(quán)系數(shù)。
[0021] 由于加速度積分得到的位移累積誤差大，而測距系統(tǒng)得到的距離數(shù)據(jù)準確度高， 但是存在大量噪聲，所以采用互補濾波法進行數(shù)據(jù)融合，能夠得到置信度更高的四旋翼飛 行器位移。
[0022] 作為本發(fā)明的進一步限定方案，步驟2中，獲得四旋翼飛行器的姿態(tài)角度數(shù)據(jù)的 具體步驟為：
[0023] 步驟2. 1，四旋翼飛行器慣性導航系統(tǒng)測量的X軸上慣性測量數(shù)據(jù)包括傾角資、陀 螺儀測量角速度《gyra、陀螺儀測量噪聲cog、螺儀常值誤差b、加速度計測量角度以及角 速度計測量噪聲，四旋翼飛行器慣性導航系統(tǒng)的狀態(tài)方程和輸出方程為：
[0024] (5)
[0025] 步驟2. 2,設(shè)四旋翼飛行器慣性導航系統(tǒng)的采樣周期為Ts，則上述方程離散化后 為：
[0026] X(k|k-1) =AX(k-l|k-l)+BU(k) (6)
[0027] 式中：
X(k-l|k-l)為k-1時刻的后驗估計，X(k|k-1) 為k時刻的先驗估計，X(k|k_l)的協(xié)方差為：
[0028] P(k|k-1) =AP(k-l|k-l)AT+Q(7)
[0029] 式中
b陀螺儀和加速度計的過程噪聲矩陣，P(k-11k-1)為協(xié)方 差在k_l時刻的后驗估計，P(k|k_l)為協(xié)方差矩陣在k時刻的先驗估計；
[0030] 步驟2. 3,利用卡爾曼濾波對慣性測量數(shù)據(jù)進行數(shù)據(jù)融合，卡爾曼增益系數(shù)為：
[0031] K(k) =P(kIk-1) +HT/(HP(k|k-1))HT+R) (8)
[0032] 式中，H= [1 0]，R為測量協(xié)方差矩陣，得到角度和角度協(xié)方差的最優(yōu)估計值 X(k|k)和P(k|k)為：
[0033]
[0034] 式中，Z(k)為當前陀螺儀的測量值；
[0035] 步驟2. 4,四旋翼飛行器慣性導航系統(tǒng)測量的Y軸和Z軸上慣性測量數(shù)據(jù)處理過程 與步驟2. 1~步驟2. 3相同，綜合四旋翼飛行器在X軸、Y軸和Z軸上的慣性測量數(shù)據(jù)可得 到四旋翼飛行器的姿態(tài)角度數(shù)據(jù)。
[0036] 利用卡爾曼濾波對慣性測量數(shù)據(jù)進行數(shù)據(jù)融合，使測量的慣性數(shù)據(jù)具有較高的精 確性，為三維成像提供較準確的位置基礎(chǔ)。
[0037] 作為本發(fā)明的進一步限定方案，步驟3中，獲得深度斷層圖像的具體步驟為：
[0038] 步驟3. 1，以角度Θ為X軸，深度h為Y軸，將深度斷層圖像展開為二維平面上的 曲線h=f(Θ)，對該曲線進行自適應(yīng)中值濾波，消除曲線上的椒鹽噪聲；
[0039] 步驟3.2,設(shè)四旋翼飛行器的位移為傾角為（終.β，α)，對深度斷層圖 ? 像進行補償?shù)木唧w步驟如下：
[0040] 步驟3. 2. 1，四旋翼飛行器前進方向上的傾角為對深度斷層圖像進行補償后展 開對應(yīng)的曲線方程為& =Λ?-的，根據(jù)t。JPt。時刻的傾角動態(tài)加權(quán)得到理想的深度斷層 圖像：
[0041]
[0042] Hj=h't !+st/cos(βt-βtj)cos(at-atj) (11)
[0043] H2=h't+st/tan(βt-βtj)tan(at-atj) (12)
[0044] h" =(1-cos(β t_ β t J cos(at-at丄））Hi+cos(β t_ βtJ cos (at-at丄）H2 (13)
[0045] 式中，h'tJPh't為經(jīng)過"=/(??)補償后t。JPt。時刻的深度，h"為經(jīng)過位移和 傾角補償后的深度值，（atl，0tl)為時刻四旋翼飛行器的其中兩軸姿態(tài)角，（at，0t) 為t。時刻四旋翼飛行器的其中兩軸姿態(tài)角。
[0046] 采用自適應(yīng)中值濾波，能夠消除曲線上的椒鹽噪聲，使深度斷層圖像展開在二維 平面上曲線更加平滑，減小偶然誤差；采用對深度斷層圖像進行補償，能夠得到較理想的深 度斷層圖像。
[0047] 作為本發(fā)明的進一步限定方案，步驟4中，在相鄰深度斷層圖像之間使用滑動窗 口牛頓插值法實現(xiàn)三維成像的具體步驟為：
[0048] 步驟4. 1，對&(i= 0, 1，2, ...，n)時刻獲得的深度斷層圖像補償后展開得到曲線 函數(shù)為：
[0049] θ) (Θ=〇。~360。） (14)
[0050] 式中，對于角度θ=Θj，深度斷層圖像都有一個相對應(yīng)的取值仁（0J;
[0051] 步驟4. 2,根據(jù)步驟1得到各個深度斷層圖像相對于初始位置的距離，對有序數(shù)據(jù) 對（Xl，仁（Θ))進行滑動窗口牛頓插值得到光滑曲線，插值公式如下：
[0052] Pn(x) =f0(x〇)+g[x0,xj(x-x^+gtxo,X1;X2] (x-X〇) (x-Xi) +
[0053] ...+g[x0,Xi,..,xn] (x-x〇)...(x-xni) (15)
[0054] i1I
[0055] 式中，Xl表示各個深度斷層圖像相對于初始位置的位移數(shù)據(jù)，n為滑動窗口大小， 當深度斷層圖像的數(shù)目超過η后，移出第一個深度斷層圖像，同時移入當前更新的深度斷 層圖像，再進行牛頓插值，從而實現(xiàn)基于滑動窗口的牛頓插值；
[0056] 步驟4. 3,對所以Θ值的深度斷層圖像進行步驟4. 2同樣操作，拼接差值后的所有 深度斷層圖像實現(xiàn)三維成像。
[0057] 采用滑動窗口的牛頓插值能夠使三維成像更加平滑，增強了三維成像效果。
[0058] 本發(fā)明的有益效果在于：（1)采用四旋翼飛行器作為搭載平臺具有高度的靈活 性，相對于地面移動機器人能夠降低復(fù)雜地面對飛行的影響，具有較高的適應(yīng)能力；（2)采 用測量的位移數(shù)據(jù)和姿態(tài)角度數(shù)據(jù)確定深度斷層圖像數(shù)據(jù)的位置，從而有效提高了激光雷 達的掃描范圍，擴大了三維成像的面積范圍。
【附圖說明】
[0059] 圖1為本發(fā)明的方法流程圖。
【具體實施方式】
[0060] 如圖1所示，本發(fā)明的基于室內(nèi)四旋翼飛行器的激光雷達三維成像方法，包括如 下步驟：
[0061] 步驟1，讀取四旋翼飛行器慣性導航系統(tǒng)測量的加速度數(shù)據(jù)以及測距系統(tǒng)測量的 距離數(shù)據(jù)，再利用互補濾波法對加速度數(shù)據(jù)和距離數(shù)據(jù)進行數(shù)據(jù)融合，從而獲得四旋翼飛 行器的位移數(shù)據(jù)，具體步驟為：
[0062] 步驟1. 1，讀取的加速度數(shù)據(jù)為（ax (t)，ay (t)，az⑴），進行二重積分得到t。濟t。 時刻的近似位移（Xt，yt，Zt)為：
[0063](? L *
-n j
[0064] 式中，（vjt。vjt。J)為t。i時刻的速度；
[0065] 步驟1· 2,讀取到t。廊t。時刻的距離數(shù)據(jù)為和CVJVZ,?,)，計算 tci至ljt。時刻的近似位移（mxt,myt,mzt)為：
[0066] (2)
[0067] 步驟1. 3,利用互補濾波法進行數(shù)據(jù)融合獲得四旋翼飛行器的位移數(shù)據(jù) 為：
[0068；
[0069；
[0070] 式中，（?,.ρΜ)為t。跗刻進行數(shù)據(jù)融合后的位移，Tu為超聲波傳感器采樣周 期，（τχ,Ty,τζ)為置信程度，（ax，ay，az)為動態(tài)加權(quán)系數(shù)。
[0071] 步驟2,讀取四旋翼飛行器慣性導航系統(tǒng)測量的慣性測量數(shù)據(jù)，并利用卡爾曼濾波 對慣性測量數(shù)據(jù)進行數(shù)據(jù)融合，獲得四旋翼飛行器的姿態(tài)角度數(shù)據(jù)，具體步驟為：
[0072] 步驟2. 1，四旋翼飛行器慣性導航系統(tǒng)測量的X軸上慣性測量數(shù)據(jù)包括傾角％J它 螺儀測量角速度《gyra、陀螺儀測量噪聲cog、螺儀常值誤差b、加速度計測量角度以及角 速度計測量噪聲，四旋翼飛行器慣性導航系統(tǒng)的狀態(tài)方程和輸出方程為：
[0073]
[0074] 步驟2. 2,設(shè)四旋翼飛行器慣性導航系統(tǒng)的采樣周期為Ts，則上述方程離散化后 為：
[0075] X(k|k-1) =AX(k-l|k-l)+BU(k) (6)
[0076] 式中，
X(k-l|k-l)為k-1時刻的后驗估計，X(k|k-1)為 k時刻的先驗估計，X(k|k_l)的協(xié)方差