本發(fā)明涉及一種位置參數(shù)估計(jì)方法，特別是涉及一種基于p3p算法的位置參數(shù)估計(jì)方法，屬于計(jì)算機(jī)視覺技術(shù)領(lǐng)域。

背景技術(shù)：

基于視覺的位姿估計(jì)技術(shù)是計(jì)算機(jī)視覺領(lǐng)域中的研究熱點(diǎn)之一，是視覺定位及導(dǎo)航技術(shù)的重要環(huán)節(jié)?；谝曈X的位姿估計(jì)技術(shù)，通過相機(jī)捕獲空間合作目標(biāo)圖像，通過圖像處理和位姿估計(jì)算法求解相機(jī)位姿參數(shù)，具有精度適中、成本低、使用獨(dú)立靈活及抗電磁干擾的特點(diǎn)。

pnp算法作為常用的位姿測(cè)量方法，以空間中相對(duì)位置已知的n(n≥3)個(gè)點(diǎn)作為特征點(diǎn)，由攝像機(jī)采集一幅圖像，計(jì)算攝像機(jī)相對(duì)于特征點(diǎn)的位姿信息。在實(shí)際應(yīng)用中，特征點(diǎn)數(shù)越多，在相機(jī)與目標(biāo)之間發(fā)生相對(duì)運(yùn)動(dòng)時(shí)，特征點(diǎn)容易超出成像圖像范圍，以及特征點(diǎn)匹配出現(xiàn)錯(cuò)誤，最終導(dǎo)致解算失敗。因此使用的特征點(diǎn)數(shù)越少，解算越靈活，一般使用p3p算法和p4p算法。

雖然，p3p算法精度與p4p算法精度相差不大；但是，p3p算法根據(jù)3個(gè)特征點(diǎn)最多可以解算出四組解，p4p算法根據(jù)4個(gè)特征點(diǎn)可以解得唯一解。然而，在圖像處理階段如果出現(xiàn)某個(gè)特征點(diǎn)提取錯(cuò)誤，導(dǎo)致位姿解算錯(cuò)誤，則該算法無法進(jìn)行自我校驗(yàn)。因此，需要找到一種所需特征點(diǎn)數(shù)少、可以確定唯一解并且能夠?qū)ξㄒ唤膺M(jìn)行校驗(yàn)的位姿解算算法。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的主要目的在于，克服現(xiàn)有技術(shù)中的不足，提供一種基于p3p算法的位置參數(shù)估計(jì)方法，不僅計(jì)算效率高，而且可提高位置參數(shù)解算的準(zhǔn)確性，可應(yīng)用于采用分離式合作目標(biāo)引導(dǎo)無人機(jī)視覺著陸領(lǐng)域。

為了達(dá)到上述目的，本發(fā)明所采用的技術(shù)方案是：

一種基于p3p算法的位置參數(shù)估計(jì)方法，包括以下步驟：

1)對(duì)使用的相機(jī)進(jìn)行標(biāo)定，得到用于計(jì)算相機(jī)內(nèi)參數(shù)模型的相機(jī)內(nèi)參數(shù)，相機(jī)內(nèi)參數(shù)包括相機(jī)的焦距和光心坐標(biāo)；

2)在相機(jī)的當(dāng)前使用位姿下，對(duì)空間中的4個(gè)特征點(diǎn)進(jìn)行成像獲得成像圖像，以4個(gè)特征點(diǎn)中的任意一個(gè)特征點(diǎn)為原點(diǎn)建立世界坐標(biāo)系，并獲取4個(gè)特征點(diǎn)的世界坐標(biāo)及其在成像圖像中的圖像坐標(biāo)；

3)將4個(gè)特征點(diǎn)分成兩組，以4個(gè)特征點(diǎn)中位于對(duì)角位置的兩個(gè)特征點(diǎn)作為公共特征點(diǎn)，將4個(gè)特征點(diǎn)中剩余的2個(gè)特征點(diǎn)分別與公共特征點(diǎn)組成均具有3個(gè)特征點(diǎn)的兩個(gè)子集，兩個(gè)子集即為兩組p3p特征點(diǎn)；

4)根據(jù)每組p3p特征點(diǎn)中特征點(diǎn)的世界坐標(biāo)和圖像坐標(biāo)，利用p3p算法求解出公共特征點(diǎn)位置參數(shù)的一組實(shí)數(shù)解，獲取兩組p3p特征點(diǎn)對(duì)應(yīng)的兩組實(shí)數(shù)解；

5)從兩組實(shí)數(shù)解中選取位置參數(shù)相似度最大的兩個(gè)解并進(jìn)行校驗(yàn)，最終確定出公共特征點(diǎn)的位置參數(shù)。

本發(fā)明進(jìn)一步設(shè)置為：所述步驟1)中的標(biāo)定采用張正友標(biāo)定法。

本發(fā)明進(jìn)一步設(shè)置為：所述步驟2)中的4個(gè)特征點(diǎn)滿足任意3個(gè)特征點(diǎn)不共線、且4個(gè)特征點(diǎn)連線呈菱形的要求。

本發(fā)明進(jìn)一步設(shè)置為：所述步驟2)中的獲取4個(gè)特征點(diǎn)的世界坐標(biāo)及其在成像圖像中的圖像坐標(biāo)，具體為，

2-1)在空間中布置分離式的4個(gè)合作目標(biāo)，每個(gè)合作目標(biāo)的中心表示一個(gè)特征點(diǎn)，則共有4個(gè)特征點(diǎn)；

2-2)以4個(gè)特征點(diǎn)中的任意一個(gè)特征點(diǎn)為原點(diǎn)建立世界坐標(biāo)系，通過工具測(cè)量特征點(diǎn)的世界坐標(biāo)；

2-3)使用已標(biāo)定好焦距的相機(jī)，在相機(jī)的當(dāng)前使用位姿下，對(duì)合作目標(biāo)進(jìn)行成像獲得成像圖像；

2-4)利用圖像處理方法，從相機(jī)采集的合作目標(biāo)的成像圖像中提取特征點(diǎn)的圖像坐標(biāo)。

本發(fā)明進(jìn)一步設(shè)置為：所述工具包括卷尺和全站儀。

本發(fā)明進(jìn)一步設(shè)置為：所述步驟4)中的獲取兩組p3p特征點(diǎn)對(duì)應(yīng)的兩組實(shí)數(shù)解，具體為，

4-1)選取一組p3p特征點(diǎn)，根據(jù)相機(jī)小孔成像模型、相機(jī)內(nèi)參數(shù)模型和相機(jī)外參數(shù)模型構(gòu)建該組p3p特征點(diǎn)中3個(gè)特征點(diǎn)關(guān)于世界坐標(biāo)和相機(jī)坐標(biāo)的關(guān)系式如下，

(c2-c1)^t(c2-c1)＝(p2-p1)^t(p2-p1)

(c3-c1)^t(c3-c1)＝(p3-p1)^t(p3-p1)

(c3-c1)^t(c2-c1)＝(p3-p1)^t(p2-p1)

其中，c1、c2、c3分別為該組p3p特征點(diǎn)中3個(gè)特征點(diǎn)的相機(jī)坐標(biāo)，p1、p2、p3分別為該組p3p特征點(diǎn)中3個(gè)特征點(diǎn)的世界坐標(biāo)；

4-2)將步驟1)得到的相機(jī)參數(shù)和步驟2)獲取的特征點(diǎn)的世界坐標(biāo)及其圖像坐標(biāo)代入到世界坐標(biāo)和相機(jī)坐標(biāo)的關(guān)系式中，得到p3p模型的三個(gè)約束方程為，

其中，λ1、λ2、d1均為待解未知數(shù)，θ1、θ2、θ3分別為相機(jī)光心指向該組p3p特征點(diǎn)中3個(gè)特征點(diǎn)所構(gòu)成的3個(gè)向量之間的夾角；

4-3)利用三角函數(shù)換元法，引入中間變量x、y，通過λ1＝x+cosθ1和λ2＝y(tǒng)+cosθ2來簡(jiǎn)化約束方程，轉(zhuǎn)化成關(guān)于x的一元四次方程，求解出x所有的實(shí)數(shù)解；

對(duì)應(yīng)的x的多解，求解的相機(jī)坐標(biāo)系下的公共特征點(diǎn)坐標(biāo)即位置參數(shù)存在多解，每一組p3p特征點(diǎn)求解出的公共特征點(diǎn)的位置參數(shù)為4個(gè)解以下；

4-4)將一組p3p特征點(diǎn)的公共特征點(diǎn)的位置參數(shù)的解的個(gè)數(shù)記為m，m≤4，其第i個(gè)解記為ai，i∈[1,m]；將另一組p3p特征點(diǎn)的公共特征點(diǎn)的位置參數(shù)的解的個(gè)數(shù)記為n，n≤4，其第j個(gè)解記為bj，j∈[1,n]。

本發(fā)明進(jìn)一步設(shè)置為：所述步驟5)從兩組實(shí)數(shù)解中選取位置參數(shù)相似度最大的兩個(gè)解并進(jìn)行校驗(yàn)，最終確定出公共特征點(diǎn)的位置參數(shù)，具體為，

5-1)計(jì)算兩組實(shí)數(shù)解中的位置參數(shù)相似度ρ(i,j)，計(jì)算公式為，

其中，分別由ai、bj改寫成的一維向量；

5-2)設(shè)置位置參數(shù)相似度閾值，根據(jù)公共特征點(diǎn)在相機(jī)坐標(biāo)系下的位置參數(shù)具有唯一性的特點(diǎn)，選取兩組實(shí)數(shù)解中位置參數(shù)相似度最大的兩個(gè)解，并將該兩個(gè)解與位置參數(shù)相似度閾值進(jìn)行校驗(yàn)；

若其位置參數(shù)相似度大于位置參數(shù)相似度閾值，則認(rèn)定位置參數(shù)解算正確，選取兩個(gè)解的平均值作為公共特征點(diǎn)的位置參數(shù)，直接將公共特征點(diǎn)的位置參數(shù)用于導(dǎo)航；否則，認(rèn)定位置參數(shù)解算錯(cuò)誤，采集下一幀圖像重復(fù)步驟2)到步驟5)。

與現(xiàn)有技術(shù)相比，本發(fā)明具有的有益效果是：

本發(fā)明提高的一種基于p3p算法的位置參數(shù)估計(jì)方法，通過設(shè)置公共特征點(diǎn)組成兩組p3p特征點(diǎn)來確定真值，并通過有效地簡(jiǎn)化約束方程來提高位置參數(shù)估計(jì)的實(shí)時(shí)性和穩(wěn)定性，克服了p3p算法的多解和計(jì)算效率低的問題；同時(shí)，在確定唯一解的基礎(chǔ)上，通過計(jì)算不同解的位置參數(shù)相似度對(duì)位置參數(shù)求解結(jié)果加入一個(gè)校驗(yàn)的環(huán)節(jié)，以提高位置參數(shù)解算的準(zhǔn)確性，使之更具有工程實(shí)用性。

上述內(nèi)容僅是本發(fā)明技術(shù)方案的概述，為了更清楚的了解本發(fā)明的技術(shù)手段，下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步的描述。

附圖說明

圖1為本發(fā)明實(shí)施例的4個(gè)特征點(diǎn)的位置示意圖；

圖2為本發(fā)明實(shí)施例的p3p算法模型的示意圖。

具體實(shí)施方式

下面結(jié)合說明書附圖，對(duì)本發(fā)明作進(jìn)一步的說明。

本發(fā)明提供一種基于p3p算法的位置參數(shù)估計(jì)方法，包括以下步驟：

1)對(duì)使用的相機(jī)進(jìn)行標(biāo)定，得到用于計(jì)算相機(jī)內(nèi)參數(shù)模型的相機(jī)內(nèi)參數(shù)，相機(jī)內(nèi)參數(shù)包括相機(jī)的焦距和光心坐標(biāo)；其中的標(biāo)定采用張正友標(biāo)定法，即張氏標(biāo)定法。

2)在相機(jī)的當(dāng)前使用位姿下，對(duì)空間中的4個(gè)特征點(diǎn)進(jìn)行成像獲得成像圖像，以4個(gè)特征點(diǎn)中的任意一個(gè)特征點(diǎn)為原點(diǎn)建立世界坐標(biāo)系，并獲取4個(gè)特征點(diǎn)的世界坐標(biāo)及其在成像圖像中的圖像坐標(biāo)；其中，4個(gè)特征點(diǎn)滿足任意3個(gè)特征點(diǎn)不共線、且4個(gè)特征點(diǎn)連線呈菱形的要求，如圖1所示。

具體為，

2-1)在空間中布置分離式的4個(gè)合作目標(biāo)，每個(gè)合作目標(biāo)的中心表示一個(gè)特征點(diǎn)，則共有4個(gè)特征點(diǎn)；

2-2)以4個(gè)特征點(diǎn)中的任意一個(gè)特征點(diǎn)為原點(diǎn)o建立世界坐標(biāo)系，通過卷尺和全站儀等工具測(cè)量特征點(diǎn)的世界坐標(biāo)；

2-3)使用已標(biāo)定好焦距的相機(jī)，在相機(jī)的當(dāng)前使用位姿下，對(duì)合作目標(biāo)進(jìn)行成像獲得成像圖像；

2-4)利用圖像處理方法，從相機(jī)采集的合作目標(biāo)的成像圖像中提取特征點(diǎn)的圖像坐標(biāo)。

3)將4個(gè)特征點(diǎn)分成兩組，以4個(gè)特征點(diǎn)中位于對(duì)角位置的兩個(gè)特征點(diǎn)作為公共特征點(diǎn)，將4個(gè)特征點(diǎn)中剩余的2個(gè)特征點(diǎn)分別與公共特征點(diǎn)組成均具有3個(gè)特征點(diǎn)的兩個(gè)子集，兩個(gè)子集即為兩組p3p特征點(diǎn)。

4)根據(jù)每組p3p特征點(diǎn)中特征點(diǎn)的世界坐標(biāo)和圖像坐標(biāo)，利用p3p算法求解出公共特征點(diǎn)位置參數(shù)的一組實(shí)數(shù)解，獲取兩組p3p特征點(diǎn)對(duì)應(yīng)的兩組實(shí)數(shù)解；

具體為，

4-1)選取一組p3p特征點(diǎn)，根據(jù)相機(jī)小孔成像模型、相機(jī)內(nèi)參數(shù)模型和外參數(shù)模型構(gòu)建該組p3p特征點(diǎn)中3個(gè)特征點(diǎn)關(guān)于世界坐標(biāo)和相機(jī)坐標(biāo)的關(guān)系式如下，

(c2-c1)^t(c2-c1)＝(p2-p1)^t(p2-p1)

(c3-c1)^t(c3-c1)＝(p3-p1)^t(p3-p1)

(c3-c1)^t(c2-c1)＝(p3-p1)^t(p2-p1)

其中，c1、c2、c3分別為該組p3p特征點(diǎn)中3個(gè)特征點(diǎn)的相機(jī)坐標(biāo)，相機(jī)坐標(biāo)根據(jù)步驟2)中獲得的特征點(diǎn)圖像坐標(biāo)與步驟1)標(biāo)定得到的相機(jī)參數(shù)計(jì)算得到，p1、p2、p3分別為該組p3p特征點(diǎn)中3個(gè)特征點(diǎn)的世界坐標(biāo)；

4-2)將步驟1)得到的相機(jī)參數(shù)和步驟2)獲取的特征點(diǎn)的世界坐標(biāo)及其圖像坐標(biāo)代入到世界坐標(biāo)和相機(jī)坐標(biāo)的關(guān)系式中，得到p3p模型的三個(gè)約束方程為，

其中，λ1、λ2、d1均為未知數(shù)，θ1、θ2、θ3分別為相機(jī)光心指向該組p3p特征點(diǎn)中3個(gè)特征點(diǎn)所構(gòu)成的3個(gè)向量之間的夾角，其為圖2所示的p3p算法模型示意圖中向量與與與之間的夾角；

4-3)利用三角函數(shù)換元法，引入中間變量x、y，通過λ1＝x+cosθ1和λ2＝y(tǒng)+cosθ2來簡(jiǎn)化約束方程，轉(zhuǎn)化成關(guān)于x的一元四次方程，求解出x所有的實(shí)數(shù)解；

對(duì)應(yīng)的x的多解，求解的相機(jī)坐標(biāo)系下的公共特征點(diǎn)坐標(biāo)即位置參數(shù)存在多解，每一組p3p特征點(diǎn)求解出的公共特征點(diǎn)的位置參數(shù)為4個(gè)解以下；

4-4)將一組p3p特征點(diǎn)的公共特征點(diǎn)的位置參數(shù)的解的個(gè)數(shù)記為m，m≤4，其第i個(gè)解記為ai，i∈[1,m]；將另一組p3p特征點(diǎn)的公共特征點(diǎn)的位置參數(shù)的解的個(gè)數(shù)記為n，n≤4，其第j個(gè)解記為bj，j∈[1,n]。

5)從兩組實(shí)數(shù)解中選取位置參數(shù)相似度最大的兩個(gè)解并進(jìn)行校驗(yàn)，最終確定出公共特征點(diǎn)的位置參數(shù)；

具體為，

5-1)計(jì)算兩組實(shí)數(shù)解中的位置參數(shù)相似度ρ(i,j)，計(jì)算公式為，

其中，分別由ai、bj改寫成的一維向量；

5-2)設(shè)置位置參數(shù)相似度閾值，根據(jù)公共特征點(diǎn)在相機(jī)坐標(biāo)系下的位置參數(shù)具有唯一性的特點(diǎn)，選取兩組實(shí)數(shù)解中位置參數(shù)相似度最大的兩個(gè)解，并將該兩個(gè)解與位置參數(shù)相似度閾值進(jìn)行校驗(yàn)；

若其位置參數(shù)相似度大于位置參數(shù)相似度閾值，則認(rèn)定位置參數(shù)解算正確，選取兩個(gè)解的平均值作為公共特征點(diǎn)的位置參數(shù)，直接將公共特征點(diǎn)的位置參數(shù)用于導(dǎo)航；否則，認(rèn)定位置參數(shù)解算錯(cuò)誤，采集下一幀圖像重復(fù)步驟2)到步驟5)。

本發(fā)明的創(chuàng)新點(diǎn)在于，基于p3p算法求解位姿出現(xiàn)多解的情況下，通過確定唯一解并對(duì)唯一解進(jìn)行校驗(yàn)，以提高位置參數(shù)解算的效率和準(zhǔn)確性。

以上顯示和描述了本發(fā)明的基本原理、主要特征及優(yōu)點(diǎn)。本行業(yè)的技術(shù)人員應(yīng)該了解，本發(fā)明不受上述實(shí)施例的限制，上述實(shí)施例和說明書中描述的只是說明本發(fā)明的原理，在不脫離本發(fā)明精神和范圍的前提下，本發(fā)明還會(huì)有各種變化和改進(jìn)，這些變化和改進(jìn)都落入要求保護(hù)的本發(fā)明范圍內(nèi)。本發(fā)明要求保護(hù)范圍由所附的權(quán)利要求書及其等效物界定。