[發(fā)明專利]一種起重機(jī)慣性視覺組合定位方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202211023413.2
• 申請(qǐng)日: 2022-08-25
• 公開（公告）號(hào): CN115615424A
• 公開（公告）日: 2023-01-17
• 發(fā)明（設(shè)計(jì)）人: 何禎鑫;劉忠業(yè);安小逸;于傳強(qiáng);王欣;馮永保;韓小霞;李良
• 申請(qǐng)（專利權(quán)）人: 中國人民解放軍火箭軍工程大學(xué)
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01C21/20
• 代理公司: 西安方諾專利代理事務(wù)所(普通合伙) 61285
• 代理人: 景麗娜

1.一種起重機(jī)慣性視覺組合定位方法，其特征在于：包括

步驟一：在起重機(jī)工作過程中，采集同步IMU數(shù)據(jù)與相機(jī)圖像，并對(duì)采集的圖像和IMU數(shù)據(jù)進(jìn)行預(yù)處理；

步驟二：采用多尺度特征點(diǎn)檢測方法提取增強(qiáng)后的相機(jī)圖像中的特征點(diǎn)；

步驟三：對(duì)提取到的亞像素特征點(diǎn)進(jìn)行光流跟蹤匹配；

步驟四：對(duì)同步IMU數(shù)據(jù)進(jìn)行預(yù)處理；

步驟五：對(duì)獲得的圖像特征點(diǎn)和預(yù)處理后的IMU預(yù)積分模型進(jìn)行視覺慣性聯(lián)合初始化；

步驟六：對(duì)經(jīng)視覺慣性聯(lián)合初始化后的圖像的關(guān)鍵幀進(jìn)行選取，構(gòu)建關(guān)鍵幀數(shù)據(jù)庫；

步驟七：采用滑動(dòng)窗口的優(yōu)化方式對(duì)滑動(dòng)窗口內(nèi)的視覺慣性信息進(jìn)行優(yōu)化；

步驟八：在獲取關(guān)鍵幀數(shù)據(jù)庫和優(yōu)化滑動(dòng)窗口后，進(jìn)行回環(huán)檢測和全局優(yōu)化，完成起重機(jī)定位過程。

2.根據(jù)權(quán)利要求1所述的一種起重機(jī)慣性視覺組合定位方法，其特征在于：步驟一所述的對(duì)采集圖像進(jìn)行與處理的過程為采用自適應(yīng)直方圖均衡化的方法對(duì)圖像進(jìn)行均衡化，其過程包括

S101.采用自適應(yīng)直方圖均衡化的方法，將模板W在采集的圖像A上逐行進(jìn)行移動(dòng)，并且模板W的中心c(x₀,y₀)對(duì)應(yīng)圖像A上的點(diǎn)f(x₀,y₀)；

S102.計(jì)算模板W區(qū)域的直方圖均衡化變化關(guān)系：g(x,y)＝T(f(x,y)，計(jì)算模板中心點(diǎn)c(x₀,y₀)的均衡化對(duì)應(yīng)像素值：g(x₀,y₀)＝T(f(x₀,y₀))；

S103.用g(x₀,y₀)替代f(x₀,y₀)，逐行計(jì)算，得到整幅圖像的自適應(yīng)直方圖均衡化圖像，完成對(duì)采集圖像的均衡化。

3.根據(jù)權(quán)利要求1所述的一種起重機(jī)慣性視覺組合定位方法，其特征在于：步驟二所述的特征點(diǎn)的提取過程包括步驟

S201.首先采用Fast和Harris檢測算法對(duì)增強(qiáng)后的相機(jī)圖像進(jìn)行第一輪角點(diǎn)檢測；

S202.然后利用最小二乘估計(jì)法的亞像素級(jí)角點(diǎn)檢測方法對(duì)已檢測出的像素級(jí)角點(diǎn)進(jìn)行二次篩選，獲得精度更高的亞像素特征；

S203.最后利用亞像素檢測結(jié)果進(jìn)行圖像邊緣化約束，對(duì)亞像素特征點(diǎn)進(jìn)行優(yōu)化。

4.根據(jù)權(quán)利要求1所述的一種起重機(jī)慣性視覺組合定位方法，其特征在于：步驟三所述的對(duì)提取到的亞像素特征點(diǎn)進(jìn)行光流跟蹤匹配的過程包括

S301.在圖像坐標(biāo)系下，設(shè)像素I時(shí)刻坐標(biāo)為(x,y)，經(jīng)過dt時(shí)間后，其坐標(biāo)為(x+dx,y+dy)

I(x+dx,y+dy,t+dt)＝I(x,y,t) (7)

對(duì)式(7)的左邊進(jìn)行一階泰勒展開后：

S302.基于灰度不變假設(shè)，得到：

將dx、dy同時(shí)對(duì)dt求導(dǎo)，得：

S303.將記為u，作為像素在圖像坐標(biāo)系x方向的速度；將記為v，作為像素在圖像坐標(biāo)系y方向的速度；將分別記為圖像在x、y方向的梯度，記為I_x、I_y；將圖像灰度對(duì)時(shí)間的梯度記為I_t，得到如式(11)所示：

S304.在光流法中，設(shè)w×w大小的一個(gè)窗口中含有w²個(gè)像素，若窗口內(nèi)每個(gè)像素具有相同的運(yùn)動(dòng)過程，則存在w²個(gè)方程：

則：

將方程(13)簡寫為：

對(duì)式(14)進(jìn)行最小二乘法求解，得：

S304.將當(dāng)前幀與前一幀互換，進(jìn)行反向光流，將反向光流匹配的特征點(diǎn)與正向光流匹配的特征點(diǎn)進(jìn)行距離比較，即判斷兩次光流跟蹤的特征點(diǎn)是否一致，將差別較大的特征點(diǎn)進(jìn)行剔除，完成亞像素特征點(diǎn)的光流跟蹤匹配。