[發(fā)明專利]一種光源無約束排布的雙目定位方法

在審

• 申請?zhí)枺?/td>: 202310491479.2
• 申請日: 2023-05-05
• 公開（公告）號: CN116518963A
• 公開（公告）日: 2023-08-01
• 發(fā)明（設(shè)計）人: 謝心月;鄒德岳;趙楠
• 申請（專利權(quán)）人: 大連理工大學(xué)
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 遼寧鴻文知識產(chǎn)權(quán)代理有限公司 21102
• 代理人: 王海波

1.一種光源無約束排布的雙目定位方法，其特征在于，所述方法為一種使用無跡卡爾曼濾波器將慣性傳感器的數(shù)據(jù)和雙目相機數(shù)據(jù)融合的可見光定位方法；首先在無跡卡爾曼濾波算法框架中，建立過程方程來預(yù)測目標(biāo)物體在三維空間的位置和姿態(tài)角，然后根據(jù)雙目相機的像素圖像建立測量方程來修正過程方程中計算的目標(biāo)物體的可見光特征點位置和航向角。

2.根據(jù)權(quán)利要求1所述的一種光源無約束排布的雙目定位方法，其特征在于，具體包括以下步驟：

1.初始化

1.1相機標(biāo)定

令雙目相機左右兩個相機分別為相機1，相機2；對相機1和相機2進行雙目相機標(biāo)定，分別獲得兩個相機的內(nèi)參和外參；所述內(nèi)參包括焦距f，像素圖像中心點(u₀,v₀)，像元尺寸dx、dy)，相機1和相機2的內(nèi)參一致；所述外參包括相機2相對相機1的旋轉(zhuǎn)矩陣R_CCS，CCS表示相機坐標(biāo)系，平移矩陣T_CCS；

1.2建立無跡卡爾曼濾波器的狀態(tài)向量

以相機1的鏡頭的光學(xué)中心為原點，鏡頭向外的方向為z軸正方向建立一個右手的三維坐標(biāo)系，該坐標(biāo)系為相機坐標(biāo)系1(CCS1)，同理在相機2上建立相機坐標(biāo)系2(CCS2)，令CCS1為主坐標(biāo)系；令計算周期的第k個周期的狀態(tài)向量為χ_k＝[P_1,k?P_2,k?P_3,k?T_k?β_k]^T，第k個周期以下稱k時刻，狀態(tài)向量以下稱狀態(tài)，其中，P_i,k(i＝1,2,3)為第k時刻時第i個可見光特征點隨目標(biāo)物體運動中的CCS1下的坐標(biāo)；T_k記為k時刻的平移向量；β_k向為k時刻的航向角；

當(dāng)k＝0時為無跡卡爾曼濾波器的初始狀態(tài)，根據(jù)當(dāng)前雙目相機與目標(biāo)物體的距離和相對姿態(tài)設(shè)置即可；

1.3建立無跡卡爾曼濾波器的過程方程

首先，對k時刻的狀態(tài)向量χ_k建立如下等式：

等式左端為k時刻的狀態(tài)；等式右端為使用k-1時刻的狀態(tài)計算k時刻狀態(tài)的等式，P_i,k-1為k-1時刻的CCS1下的第i個可見光特征點隨目標(biāo)物體運動的三維空間中的坐標(biāo)；T_k-1記為k-1時刻的平移向量；R_k記為k時刻的旋轉(zhuǎn)矩陣，通過k時刻的俯仰角α_k和翻滾角γ_k以及k-1時刻的俯仰角計算得到，計算公式為：

，

該公式為歐拉-羅德里格斯公式；使用計算k時刻的前m個周期的平移向量輸出的平均值，用以計算T_k；同理，使用為計算k時刻的前m個周期的航向角輸出的平均值，用以計算β_k；w_k則為k時刻預(yù)測狀態(tài)的噪聲，由無跡卡爾曼濾波算法自動估計；

上述的等式統(tǒng)稱為過程方程χ_k＝F(χ_k-1,α_k,γ_k)，該方程描述了如何由k-1時刻狀態(tài)χ_k-1預(yù)測出k時刻的狀態(tài)χ_k；

1.4建立無跡卡爾曼濾波器的測量方程

相機將三維空間的任意點成像到相機的像素圖像上，如果能獲得相機對三維空間的任意點坐標(biāo)與相機像素圖像的位置關(guān)系，那么在已知該點在相機的像素圖像上的坐標(biāo)之后，就能得到該點三維空間的坐標(biāo)的部分信息，可以利用該信息和卡爾曼濾波算法對1.2中預(yù)測的狀態(tài)χ_k進行矯正，該矯正過程稱為測量；

在1.1中對雙目相機已經(jīng)進行了標(biāo)定，令：

該矩陣A稱為內(nèi)參矩陣；

對于第k時刻，在CCS1下的第i個特征點坐標(biāo)P_i,k(i＝1,2,3)改為增廣形式P_i,k＝(x_i,k,y_i,k,z_i,k,1)^T(i＝1,2,3)，該點在相機1圖像的像素坐標(biāo)其中上標(biāo)p1為相機1的像素圖像，為該點在像素圖像的橫縱坐標(biāo)；P_i,k(i＝1,2,3)和的關(guān)系為：

將該等式記為：其中E₃為三階單位矩陣；同時，還需獲得P_i,k(i＝1,2,3)在相機2的像素圖像上的坐標(biāo)上標(biāo)p2為相機2的像素圖像，先計算P_i,k(i＝1,2,3)在CCS2下的坐標(biāo)下的坐標(biāo)為：

則為：

將該式記為：

最后，將等式和合并為：

該等式即為測量方程，測量方程將CCS1下的點坐標(biāo)(P_i,k(i＝1,2,3))轉(zhuǎn)化為雙目相機的相機1和相機2的像素圖像坐標(biāo)，也就是可以從1.2中的預(yù)測狀態(tài)χ_k中的P_i,k(i＝1,2,3)計算出相機1和相機2的像素圖像坐標(biāo)；從雙目相機中獲得以及從雙目相機標(biāo)定結(jié)果獲得R_CCS、T_CCS后，使用無跡卡爾曼濾波器對P_i,k(i＝1,2,3)進行修正；

1.5無跡卡爾曼濾波器的初始化參數(shù)

在1.2、1.3、1.4中得到狀態(tài)、過程方程、測量方程的卡爾曼濾波器，還需確定無跡卡爾曼濾波器的初值，該初值為目標(biāo)物體的起始位置和姿態(tài)；需針對過程方程對實際目標(biāo)物體運動時的三個特征點坐標(biāo)變化和姿態(tài)角變化描述的準(zhǔn)確性設(shè)置過程噪聲協(xié)方差矩陣S；對應(yīng)的，針對測量方程對相機成像描述的準(zhǔn)確性設(shè)置測量噪聲協(xié)方差矩陣L；

2.無跡卡爾曼濾波器迭代

2.1俯仰角和翻滾角獲取

對于帶有三個無約束排布可見光特征點的目標(biāo)物體，安裝六軸慣性傳感器以獲得在三維空間運動過程中三個實時變化的姿態(tài)角，分別為：俯仰角，航向角，翻滾角，任意的第k時刻的這三個姿態(tài)角分別記為：α_k，β_k，γ_k；

2.2圖像獲取

對于帶有三個無約束排布可見光特征點的目標(biāo)物體，使用雙目相機捕捉在其運動過程中的三個可見光特征點在相機像素圖像中的位置，任意的k時刻的第i個可見光特征點在第j個相機的像素圖像的坐標(biāo)記為P_i^pj(i＝1,2,3；j＝1,2)，上標(biāo)pj為第j個相機的像素圖象；

2.3計算k-1時刻狀態(tài)的sigma點集

將k-1時刻的最優(yōu)狀態(tài)X_k-1的按照以下規(guī)則分散成2n+1個狀態(tài)sigma點集：

χ_l,k-1(l＝0,1,...,2n)則為k-1時刻的最優(yōu)狀態(tài)X_k-1的sigma點集，即k-1時刻狀態(tài)的sigma點集；n為X_k-1的成員數(shù)量；λ為對角線因子，為常數(shù)，計算公式為λ＝ε²(n+κ)-n，ε計為分布范圍因子，為常數(shù)，κ為分布因子，為常數(shù)；Q_k-1為k-1時刻的最優(yōu)狀態(tài)協(xié)方差矩陣，由無跡卡爾曼濾波器自動估計；

2.4計算k時刻過程的sigma點集

在1.3中建立了過程方程F(χ_k-1,α_k,γ_k,w_k-1)，將2.3中的k-1時刻狀態(tài)的sigma點集χ_l,k-1(l＝0,1,...,2n)帶入該方程計算狀態(tài)的sigma點集在過程方程中的更新，即為k時刻過程的sigma點集χ_l,k(l＝0,1,...,2n)：

其中W_l^m(l＝0,1,...,2n)為均值權(quán)重向量，計算公式為：

為過程的sigma點集χ_l,k(l＝0,1,...,2n)在均值權(quán)重向量W_l^m(l＝0,1,...,2n)下的過程的sigma點集的均值；

2.5計算k時刻的狀態(tài)協(xié)方差矩陣

為k時刻的狀態(tài)協(xié)方差矩陣；W_l^c(l＝0,1,...,2n)為方差權(quán)重向量，計算公式為：

S為過程噪聲協(xié)方差矩陣，在1.5中設(shè)置；

2.6計算k時刻的測量的sigma點集

類似地，在1.4中建立測量方程H(A,P_i,k,R_CCS,T_CCS)，將2.4中的k時刻過程的sigma點集χ_l,k(l＝0,1,...,2n)帶入該方程計算該點集在測量方程中的更新(k時刻測量的sigma點集)Z_l,k(l＝0,1,...,2n)：

Z_l,k(l＝0,1,...,2n)為k時刻的過程sigma點集，為k時刻在均值權(quán)重向量W_l^m(l＝0,1,...,2n)下的過程sigma點集均值；

2.7計算k時刻的測量協(xié)方差矩陣

為k時刻的測量協(xié)方差矩陣；L為測量噪聲協(xié)方差矩陣，在1.5中設(shè)置；

2.8計算k時刻的卡爾曼增益

為k時刻的狀態(tài)-測量聯(lián)合協(xié)方差矩陣；K_k為k時刻的卡爾曼增益；

2.9計算k時刻的最優(yōu)狀態(tài)和重復(fù)迭代

2.9.1計算k時刻的最優(yōu)狀態(tài)

X_k為k時刻的最優(yōu)狀態(tài)；z_k為從2.2中獲得的k時刻的可見光特征點的像素圖像的坐標(biāo)P_k為k時刻的最優(yōu)狀態(tài)的協(xié)方差矩陣；

2.9.2輸出和重復(fù)迭代

在2.9.1中計算出k時刻的最優(yōu)狀態(tài)和k時刻的最優(yōu)狀態(tài)協(xié)方差之后，k時刻的迭代完成，X_k即是目標(biāo)物體的三個可見光特征點在CCS1下的坐標(biāo)和姿態(tài)角；令此k時刻為k-1時刻，下一時刻為k時刻，返回2.1重新計算k時刻的最優(yōu)狀態(tài)和k時刻的最優(yōu)狀態(tài)協(xié)方差。