適用于機器人導(dǎo)航的視覺與衛(wèi)星定位傳感器聯(lián)合校準(zhǔn)方法
【專利摘要】本發(fā)明提供一種適用于機器人導(dǎo)航的視覺與衛(wèi)星定位傳感器聯(lián)合校準(zhǔn)方法：裝配時確保衛(wèi)星定位傳感器天線的中心點在視覺傳感器的每幀圖像中都可見；獲取視覺傳感器的前期校正結(jié)果、視覺傳感器與衛(wèi)星定位傳感器天線之間的直線距離、衛(wèi)星定位傳感器天線中心點在某幀圖像的相面坐標(biāo)系中的坐標(biāo)，求取衛(wèi)星定位傳感器天線中心點在相機中心坐標(biāo)系中的坐標(biāo)的絕對值及各項的正負(fù)；確定相機中心坐標(biāo)系的原點在等效坐標(biāo)系中的坐標(biāo)；以及，基于每個采樣時刻衛(wèi)星定位傳感器測得的經(jīng)度、緯度和高度，求取相機中心坐標(biāo)系原點的經(jīng)度、緯度和高度，以便聯(lián)合校準(zhǔn)由于衛(wèi)星定位傳感器與視覺傳感器中心裝配于不同空間點而引起的誤差。
【專利說明】適用于機器人導(dǎo)航的視覺與衛(wèi)星定位傳感器聯(lián)合校準(zhǔn)方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明適用于機器人導(dǎo)航及普通載體的組合導(dǎo)航領(lǐng)域，特別涉及一種適用于機器人導(dǎo)航的視覺與衛(wèi)星定位傳感器聯(lián)合校準(zhǔn)方法。
【背景技術(shù)】
[0002]例如在機器人導(dǎo)航及普通載體的組合導(dǎo)航領(lǐng)域中，為了提高導(dǎo)航的精度及在多種環(huán)境中的適應(yīng)性，常常在載體上裝配多種傳感器，但這些傳感器往往無法裝配于空間中的同一點，也就是說多種傳感器的視場不同，因此，如果直接假設(shè)不同的傳感器之間的觀測源于同樣的基準(zhǔn)進行坐標(biāo)投影，則會造成最終結(jié)果的全局誤差。因此如何對多種傳感器進行校準(zhǔn)是必須的技術(shù)環(huán)節(jié)。最常見的是如何校準(zhǔn)衛(wèi)星定位傳感器與視覺傳感器中心裝配點不重合的問題。
[0003]目前廣泛應(yīng)用的衛(wèi)星定位系統(tǒng)(包括GPS、伽利略、北斗、GL0NASS)的中高端的終端接收機都需要較大的外置天線，這導(dǎo)致天線的裝配無法和視覺傳感器的中心點重合，而衛(wèi)星定位系統(tǒng)接收機的觀測值(通常都可以轉(zhuǎn)化為經(jīng)度、緯度和高度)通常指其天線的中心點的空間方位。而由于前述裝配上的不吻合，使衛(wèi)星定位系統(tǒng)接收機的觀測值并不是視覺傳感器中心點的空間位置。但后續(xù)的多傳感器數(shù)據(jù)融合過程中，需要多傳感器的觀測值處于統(tǒng)一的基準(zhǔn)點。也就是說如何能將衛(wèi)星定位傳感器的經(jīng)度、緯度和高度轉(zhuǎn)換為視覺傳感器中心點的經(jīng)度、緯度和高度，基于此才能將后續(xù)由視覺傳感器的觀測轉(zhuǎn)換為與衛(wèi)星定位傳感器統(tǒng)一的基準(zhǔn)上。
[0004]但是現(xiàn)有技術(shù)需要復(fù)雜的校驗過程，有的甚至需要專門的校驗場，因此限制了系統(tǒng)的廣泛適用性。

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

[0005]本發(fā)明旨在解決視覺傳感器和衛(wèi)星定位傳感器的聯(lián)合校準(zhǔn)的問題，提供一種適用于機器人導(dǎo)航的視覺與衛(wèi)星定位傳感器聯(lián)合校準(zhǔn)方法，能夠在任意時刻基于衛(wèi)星定位傳感器的輸出經(jīng)度、緯度和高度求取視覺傳感器中心的經(jīng)度、緯度和高度。本發(fā)明所描述的方法的簡單易行，有很強的獨創(chuàng)性。
[0006]為了達(dá)到上述目的，本發(fā)明的技術(shù)方案是提供一種適用于機器人導(dǎo)航的視覺與衛(wèi)星定位傳感器聯(lián)合校準(zhǔn)方法，其包含以下步驟:
[0007]第一步，按照特定方式在移動機器人上裝配視覺傳感器和衛(wèi)星定位傳感器天線，該特定方式是指能確保后續(xù)作業(yè)中衛(wèi)星定位傳感器天線的中心點在視覺傳感器所觀測的每幀圖像中都是可見的裝配方式；
[0008]第二步，提取視覺傳感器的前期校正結(jié)果用于后續(xù)步驟；
[0009]第三步，測量視覺傳感器與衛(wèi)星定位傳感器天線之間的直線距離；
[0010]第四步，使用視覺傳感器采集一幀圖像，獲取衛(wèi)星定位傳感器天線中心點在該幀圖像所對應(yīng)的相面坐標(biāo)系中的坐標(biāo)；[0011]第五步，求取衛(wèi)星定位傳感器天線中心點在相機中心坐標(biāo)系中的坐標(biāo)的絕對值；
[0012]第六步，確定衛(wèi)星定位傳感器天線中心點在相機中心坐標(biāo)系中的坐標(biāo)的各項的正負(fù)；
[0013]第七步，確定相機中心坐標(biāo)系的原點在等效坐標(biāo)系中的坐標(biāo)；該等效坐標(biāo)系原點為衛(wèi)星定位傳感器天線中心，該等效坐標(biāo)系坐標(biāo)軸與相機中心坐標(biāo)系坐標(biāo)軸平行；
[0014]所述的第一步至第七步，在移動機器人作業(yè)之前進行一次；
[0015]第八步，在移動機器人作業(yè)過程中的每個采樣時刻都要進行本步驟；
[0016]S卩，基于每個采樣時刻衛(wèi)星定位傳感器測得的經(jīng)度、緯度和高度，求取相機中心坐標(biāo)系原點的經(jīng)度、緯度和高度:該求取過程中包含基于GPS的地心地固坐標(biāo)系下的大地坐標(biāo)，求得相機中心點的大地坐標(biāo)；基于本步驟的計算結(jié)果，將后續(xù)視覺傳感器所有觀測的數(shù)值都轉(zhuǎn)換為地心地固坐標(biāo)系下的大地坐標(biāo)，實現(xiàn)對視覺傳感器和衛(wèi)星定位傳感器的聯(lián)合校準(zhǔn)。
[0017]因此，本發(fā)明能夠校準(zhǔn)由于衛(wèi)星定位傳感器與視覺傳感器中心裝配于不同空間點而引起的誤差。
【專利附圖】

【附圖說明】
[0018]圖1是本發(fā)明所述聯(lián)合校準(zhǔn)方法的流程示意圖；
[0019]圖2是本發(fā)明示例中視覺傳感器和GPS接收機天線裝配的側(cè)視示意圖；
[0020]圖3是本發(fā)明示例中視覺傳感器和GPS接收機天線裝配的俯視示意圖；
[0021]圖4是本發(fā)明示例中視覺傳感器采集的一幀觀測圖像的示意圖；
[0022]圖5是本發(fā)明示例中相面坐標(biāo)系的一個示意圖。
【具體實施方式】
[0023]為了能夠更好地說明問題，這里首先聲明本發(fā)明中涉及的坐標(biāo)系:
[0024]地心地固坐標(biāo)系(本發(fā)明中用e代表)，為右手坐標(biāo)系。地球球心為原點，X軸指向本初子午線，y軸指向東經(jīng)90度，z軸與地球自轉(zhuǎn)軸重合，指向北極。根據(jù)表述不同，e下的坐標(biāo)有兩種等效的表達(dá)方式:笛卡爾坐標(biāo)，用(，ye, ze)表示；大地坐標(biāo)，用經(jīng)度λ，緯度ψ?聞度h表不。
[0025]局部地理坐標(biāo)系(本發(fā)明中用g代表)，為右手坐標(biāo)系。該坐標(biāo)系的原點任意，因此在本發(fā)明的后續(xù)論述中凡表述局部地理坐標(biāo)系時，都需指明其原點。X軸指向東向，y軸指向北極點，z軸指向地球球心的反方向。也稱為東北天坐標(biāo)系。本發(fā)明中所使用慣性傳感器測量特定坐標(biāo)系坐標(biāo)軸與g的旋轉(zhuǎn)角度。
[0026]相機中心坐標(biāo)系，為右手坐標(biāo)系。原點為相機成像中心，即光心，X軸指向攝像頭前方，I軸指向左方，Z軸指向相機上方。
[0027]慣導(dǎo)載體坐標(biāo)系為右手坐標(biāo)系。原點為慣性導(dǎo)航傳感器的中心點，坐標(biāo)軸與相機中心坐標(biāo)系的坐標(biāo)軸平行。
[0028]機器人中心坐標(biāo)系為右手坐標(biāo)系。原點為機器人等效中心點，坐標(biāo)軸與相機中心坐標(biāo)系的坐標(biāo)軸平行。
[0029]注意，在本發(fā)明中將，機器人的等效中心點與相機中心坐標(biāo)系原點重合，通過將慣導(dǎo)傳感器與相機緊密捆綁，可使相機中心坐標(biāo)系與慣導(dǎo)載體坐標(biāo)系重合。因此，將上述三個坐標(biāo)系統(tǒng)稱為C坐標(biāo)系,其原點稱為點C。
[0030]相面坐標(biāo)系(本發(fā)明中用m代表)，該坐標(biāo)系存在于視覺傳感器采集的每一幀圖像，每一幀圖像都具有自己的相面坐標(biāo)系。它是一個二維坐標(biāo)系(一個該坐標(biāo)系的示例示于圖5)，其原點位于一幀圖像的左上角，按照慣例，其橫軸記為U，縱軸記為V。某點在相面坐標(biāo)系中的坐標(biāo)一般以該點在沿橫軸和沿縱軸方向距離相面坐標(biāo)系原點多少個像素點單位表
/Jn ο
[0031]等效坐標(biāo)系(本發(fā)明中用k代表)；該坐標(biāo)系是本發(fā)明為了讓坐標(biāo)轉(zhuǎn)換過程的描述更清晰扼要而假設(shè)出的一個坐標(biāo)系。該坐標(biāo)系的原點位于GPS接收機天線的中心點，而坐標(biāo)軸與相機中心坐標(biāo)系的坐標(biāo)軸平行。
[0032]本發(fā)明后續(xù)的介紹中將頻繁出現(xiàn)關(guān)于特定點在特定坐標(biāo)系中坐標(biāo)的描述。為了方便表述，特將點以如下方式聲明:將某一點α點在某個坐標(biāo)系ζ下的坐標(biāo)，表示為
[0033]在本實施例中，使用GPS接收機作為衛(wèi)星定位傳感器進行說明。而在其他一些不同的實施例中，提到的衛(wèi)星定位傳感器不限定為GPS接收機，可以是伽利略、格朗納斯和北斗等衛(wèi)星導(dǎo)航系統(tǒng)。
[0034]所述聯(lián)合校準(zhǔn)方法具體的實施步驟如下:
[0035]第一步，按照特定方式裝配視覺傳感器20和GPS接收機天線30。要點是必須保證GPS接收機天線30的中心點31在視覺傳感器20所觀測的每幀圖像中都是可見的，視覺傳感器20和GPS接收機天線30裝配的示意圖示于圖1 (此圖為側(cè)視圖)和圖2 (此圖為俯視圖)。.
[0036]本實施例中作為示例，將GPS接收機天線30安裝于視覺傳感器20的右前方，且攝像頭視角下傾，以確保可以在每個采樣時刻都能將GPS接收機天線30涵蓋在觀測的圖像中。這種方式裝配的系統(tǒng)在移動機器人10后續(xù)的作業(yè)過程中采集出的每一幀圖像的右下方都會出現(xiàn)GPS接收機天線30及該天線的中心點31 (因GPS接收機輸出的經(jīng)度、緯度和高度是指該中心點的經(jīng)度、緯度和高度)，經(jīng)這種方式裝配的系統(tǒng)在一幀的觀測圖像的示意圖(此為示意圖，而非實圖)，示于圖3 ;通過這一步可以確保慣性導(dǎo)傳感器所測量的三個歐拉角航向角Ψ、俯仰角Y和橫滾角Θ反映了 c坐標(biāo)系與局部地理坐標(biāo)系坐標(biāo)軸之間的旋轉(zhuǎn)角度。
[0037]第二步，從視覺傳感器的前期校正結(jié)果中提取本發(fā)明后續(xù)步驟需要的參數(shù)；視覺傳感器的校正是領(lǐng)域內(nèi)通用的技術(shù)，領(lǐng)域內(nèi)有很多開源的相機校正工具包可免費使用，例如，使用Matlab語言編寫的相機校正工具包，和使用了 C++語言編寫的相機校正工具包，后者依托于開源的機器視覺庫OpenCV。本發(fā)明實施例的相機前期校正可使用上述兩個工具包中的任意一個進行校正，他們的效果是等效的。視覺傳感器的前期校正中，能得到相機經(jīng)校正后的結(jié)果即固有矩陣K，它可以表示為:
【權(quán)利要求】
1.一種適用于機器人導(dǎo)航的視覺與衛(wèi)星定位傳感器聯(lián)合校準(zhǔn)方法，其特征在于，包含以下步驟: 第一步，按照特定方式在移動機器人上裝配視覺傳感器和衛(wèi)星定位傳感器天線，該特定方式是指能確保后續(xù)作業(yè)中衛(wèi)星定位傳感器天線的中心點在視覺傳感器所觀測的每幀圖像中都是可見的裝配方式； 第二步，提取視覺傳感器的前期校正結(jié)果用于后續(xù)步驟； 第三步，測量視覺傳感器與衛(wèi)星定位傳感器天線之間的直線距離； 第四步，使用視覺傳感器采集一幀圖像，獲取衛(wèi)星定位傳感器天線中心點在該幀圖像所對應(yīng)的相面坐標(biāo)系m中的坐標(biāo)；所述相面坐標(biāo)系m是該巾貞圖像中對應(yīng)存在的一個二維坐標(biāo)系，其原點位于該巾貞圖像的左上角；任意一點在所述相面坐標(biāo)系m中的坐標(biāo),通過該點在沿橫軸u和沿縱軸V方向距離該相面坐標(biāo)系m原點有多少個像素點單位來表示； 第五步，求取衛(wèi)星定位傳感器天線中心點在相機中心坐標(biāo)系中的坐標(biāo)的絕對值；所述相機中心坐標(biāo)系，其原點為相機成像中心，即光心，X軸指向攝像頭前方，y軸指向左方，z軸指向相機上方； 第六步，確定衛(wèi)星定位傳感器天線中心點在相機中心坐標(biāo)系中的坐標(biāo)的各項的正負(fù)；第七步，確定相機中心坐標(biāo)系的原點在等效坐標(biāo)系k中的坐標(biāo)；該等效坐標(biāo)系k原點為衛(wèi)星定位傳感器天線中心，該等效坐標(biāo)系k坐標(biāo)軸與相機中心坐標(biāo)系坐標(biāo)軸平行； 所述的第一步至第七步，在移動機器人作業(yè)之前進行一次； 第八步，在移動機器人作業(yè)過程中的每個采樣時刻都要進行本步驟； SP，基于每個采樣時刻衛(wèi)星定位傳感器測得的經(jīng)度、緯度和高度，求取相機中心坐標(biāo)系原點的經(jīng)度、緯度和 高度:該求取過程中包含基于GPS的地心地固坐標(biāo)系e下的大地坐標(biāo)，求得相機中心點的大地坐標(biāo)；基于本步驟的計算結(jié)果，將后續(xù)視覺傳感器所有觀測的數(shù)值都轉(zhuǎn)換為地心地固坐標(biāo)系e下的大地坐標(biāo)，實現(xiàn)對視覺傳感器和衛(wèi)星定位傳感器的聯(lián)合校準(zhǔn)；其中,所述地心地固坐標(biāo)系e中，以地球球心為原點，X軸指向本初子午線，y軸指向東經(jīng)90度，z軸與地球自轉(zhuǎn)軸重合，指向北極；大地坐標(biāo)，用經(jīng)度λ，緯度賢，高度h表示。
2.如權(quán)利要求1所述的聯(lián)合校準(zhǔn)方法，其特征在于， 所述的第二步中，視覺傳感器的前期校正結(jié)果包含固有矩陣K:
3.如權(quán)利要求2所述的聯(lián)合校準(zhǔn)方法，其特征在于， 所述的第三步中，測量的一端是第一步中特定方式裝配好的衛(wèi)星定位傳感器天線中心點處，測量的另一端是視覺傳感器的中心，測量二者之間的直線距離，記為r。
4.如權(quán)利要求3所述的聯(lián)合校準(zhǔn)方法，其特征在于， 所述的第四步中，包含以下的子步驟:從視覺傳感器采集的一幀圖像中辨識出衛(wèi)星定位傳感器天線中心點，記為go ;確定go在該巾貞圖像的相面坐標(biāo)系m中的坐標(biāo),記為(ug, Vg)。
5.如權(quán)利要求4所述的聯(lián)合校準(zhǔn)方法，其特征在于， 所述的第五步中，求取衛(wèi)星定位傳感器天線中心點在相機中心坐標(biāo)系中的坐標(biāo)的絕對m\4°i \?°ι uf丨)，包含以下過程:
6.如權(quán)利要求5所述的聯(lián)合校準(zhǔn)方法，其特征在于， 所述的第六步中，確定衛(wèi)星定位傳感器天線中心點在相機中心坐標(biāo)系中的坐標(biāo)的各項的正負(fù)，包含以下操作: f恒為正，即Jcfa = η; 對于:1’ 求取Utl-Ug;;如果Utl-Ug大于O,則丨? =αηι如果Utl-Ug小于O,則= -αη.,如果Utl-Ug等于0，則f = O；對于求取V0-Vg ;如果V0-Vg大于O,貝= &?；如果V0-Vg小于O,則=—切；如果VtTVg等于0，則=0。
7.如權(quán)利要求6所述的聯(lián)合校準(zhǔn)方法，其特征在于， 所述的第七步中，根據(jù)衛(wèi)星定位傳感器天線中心點go在相機中心坐標(biāo)系中的坐標(biāo) ，即(Jtf，>f )，來求取所述相機中心坐標(biāo)系的原點CO在等效坐標(biāo)系k中的坐標(biāo)_lf，即包含以下操作:
8.如權(quán)利要求7所述的聯(lián)合校準(zhǔn)方法，其特征在于， 每個采樣時刻t進行的所述第八步中，包含以下操作:a)基于相機中心坐標(biāo)系的原點Co在等效坐標(biāo)系k中的坐標(biāo)if，即
【文檔編號】G01C25/00GK103438906SQ201310410633
【公開日】2013年12月11日 申請日期:2013年9月10日 優(yōu)先權(quán)日:2013年9月10日
【發(fā)明者】孫作雷, 張波, 黃平平, 曾連蓀, 朱大奇 申請人:上海海事大學(xué)