適用于機器人導航的視覺與衛星定位傳感器聯合校準方法
【專利摘要】本發明提供一種適用于機器人導航的視覺與衛星定位傳感器聯合校準方法：裝配時確保衛星定位傳感器天線的中心點在視覺傳感器的每幀圖像中都可見；獲取視覺傳感器的前期校正結果、視覺傳感器與衛星定位傳感器天線之間的直線距離、衛星定位傳感器天線中心點在某幀圖像的相面坐標系中的坐標，求取衛星定位傳感器天線中心點在相機中心坐標系中的坐標的絕對值及各項的正負；確定相機中心坐標系的原點在等效坐標系中的坐標；以及，基于每個采樣時刻衛星定位傳感器測得的經度、緯度和高度，求取相機中心坐標系原點的經度、緯度和高度，以便聯合校準由于衛星定位傳感器與視覺傳感器中心裝配于不同空間點而引起的誤差。
【專利說明】適用于機器人導航的視覺與衛星定位傳感器聯合校準方法
【技術領域】
[0001]本發明適用于機器人導航及普通載體的組合導航領域，特別涉及一種適用于機器人導航的視覺與衛星定位傳感器聯合校準方法。
【背景技術】
[0002]例如在機器人導航及普通載體的組合導航領域中，為了提高導航的精度及在多種環境中的適應性，常常在載體上裝配多種傳感器，但這些傳感器往往無法裝配于空間中的同一點，也就是說多種傳感器的視場不同，因此，如果直接假設不同的傳感器之間的觀測源于同樣的基準進行坐標投影，則會造成最終結果的全局誤差。因此如何對多種傳感器進行校準是必須的技術環節。最常見的是如何校準衛星定位傳感器與視覺傳感器中心裝配點不重合的問題。
[0003]目前廣泛應用的衛星定位系統(包括GPS、伽利略、北斗、GL0NASS)的中高端的終端接收機都需要較大的外置天線，這導致天線的裝配無法和視覺傳感器的中心點重合，而衛星定位系統接收機的觀測值(通常都可以轉化為經度、緯度和高度)通常指其天線的中心點的空間方位。而由于前述裝配上的不吻合，使衛星定位系統接收機的觀測值并不是視覺傳感器中心點的空間位置。但后續的多傳感器數據融合過程中，需要多傳感器的觀測值處于統一的基準點。也就是說如何能將衛星定位傳感器的經度、緯度和高度轉換為視覺傳感器中心點的經度、緯度和高度，基于此才能將后續由視覺傳感器的觀測轉換為與衛星定位傳感器統一的基準上。
[0004]但是現有技術需要復雜的校驗過程，有的甚至需要專門的校驗場，因此限制了系統的廣泛適用性。
【發明內容】
[0005]本發明旨在解決視覺傳感器和衛星定位傳感器的聯合校準的問題，提供一種適用于機器人導航的視覺與衛星定位傳感器聯合校準方法，能夠在任意時刻基于衛星定位傳感器的輸出經度、緯度和高度求取視覺傳感器中心的經度、緯度和高度。本發明所描述的方法的簡單易行，有很強的獨創性。
[0006]為了達到上述目的，本發明的技術方案是提供一種適用于機器人導航的視覺與衛星定位傳感器聯合校準方法，其包含以下步驟:
[0007]第一步，按照特定方式在移動機器人上裝配視覺傳感器和衛星定位傳感器天線，該特定方式是指能確保后續作業中衛星定位傳感器天線的中心點在視覺傳感器所觀測的每幀圖像中都是可見的裝配方式；
[0008]第二步，提取視覺傳感器的前期校正結果用于后續步驟；
[0009]第三步，測量視覺傳感器與衛星定位傳感器天線之間的直線距離；
[0010]第四步，使用視覺傳感器采集一幀圖像，獲取衛星定位傳感器天線中心點在該幀圖像所對應的相面坐標系中的坐標；[0011]第五步，求取衛星定位傳感器天線中心點在相機中心坐標系中的坐標的絕對值；
[0012]第六步，確定衛星定位傳感器天線中心點在相機中心坐標系中的坐標的各項的正負；
[0013]第七步，確定相機中心坐標系的原點在等效坐標系中的坐標；該等效坐標系原點為衛星定位傳感器天線中心，該等效坐標系坐標軸與相機中心坐標系坐標軸平行；
[0014]所述的第一步至第七步，在移動機器人作業之前進行一次；
[0015]第八步，在移動機器人作業過程中的每個采樣時刻都要進行本步驟；
[0016]S卩，基于每個采樣時刻衛星定位傳感器測得的經度、緯度和高度，求取相機中心坐標系原點的經度、緯度和高度:該求取過程中包含基于GPS的地心地固坐標系下的大地坐標，求得相機中心點的大地坐標；基于本步驟的計算結果，將后續視覺傳感器所有觀測的數值都轉換為地心地固坐標系下的大地坐標，實現對視覺傳感器和衛星定位傳感器的聯合校準。
[0017]因此，本發明能夠校準由于衛星定位傳感器與視覺傳感器中心裝配于不同空間點而引起的誤差。
【專利附圖】
【附圖說明】
[0018]圖1是本發明所述聯合校準方法的流程示意圖；
[0019]圖2是本發明示例中視覺傳感器和GPS接收機天線裝配的側視示意圖；
[0020]圖3是本發明示例中視覺傳感器和GPS接收機天線裝配的俯視示意圖；
[0021]圖4是本發明示例中視覺傳感器采集的一幀觀測圖像的示意圖；
[0022]圖5是本發明示例中相面坐標系的一個示意圖。
【具體實施方式】
[0023]為了能夠更好地說明問題，這里首先聲明本發明中涉及的坐標系:
[0024]地心地固坐標系(本發明中用e代表)，為右手坐標系。地球球心為原點，X軸指向本初子午線，y軸指向東經90度，z軸與地球自轉軸重合，指向北極。根據表述不同，e下的坐標有兩種等效的表達方式:笛卡爾坐標，用(，ye, ze)表示；大地坐標，用經度λ，緯度ψ?聞度h表不。
[0025]局部地理坐標系(本發明中用g代表)，為右手坐標系。該坐標系的原點任意，因此在本發明的后續論述中凡表述局部地理坐標系時，都需指明其原點。X軸指向東向，y軸指向北極點，z軸指向地球球心的反方向。也稱為東北天坐標系。本發明中所使用慣性傳感器測量特定坐標系坐標軸與g的旋轉角度。
[0026]相機中心坐標系，為右手坐標系。原點為相機成像中心，即光心，X軸指向攝像頭前方，I軸指向左方，Z軸指向相機上方。
[0027]慣導載體坐標系為右手坐標系。原點為慣性導航傳感器的中心點，坐標軸與相機中心坐標系的坐標軸平行。
[0028]機器人中心坐標系為右手坐標系。原點為機器人等效中心點，坐標軸與相機中心坐標系的坐標軸平行。
[0029]注意，在本發明中將，機器人的等效中心點與相機中心坐標系原點重合，通過將慣導傳感器與相機緊密捆綁，可使相機中心坐標系與慣導載體坐標系重合。因此，將上述三個坐標系統稱為C坐標系,其原點稱為點C。
[0030]相面坐標系(本發明中用m代表)，該坐標系存在于視覺傳感器采集的每一幀圖像，每一幀圖像都具有自己的相面坐標系。它是一個二維坐標系(一個該坐標系的示例示于圖5)，其原點位于一幀圖像的左上角，按照慣例，其橫軸記為U，縱軸記為V。某點在相面坐標系中的坐標一般以該點在沿橫軸和沿縱軸方向距離相面坐標系原點多少個像素點單位表
/Jn ο
[0031]等效坐標系(本發明中用k代表)；該坐標系是本發明為了讓坐標轉換過程的描述更清晰扼要而假設出的一個坐標系。該坐標系的原點位于GPS接收機天線的中心點，而坐標軸與相機中心坐標系的坐標軸平行。
[0032]本發明后續的介紹中將頻繁出現關于特定點在特定坐標系中坐標的描述。為了方便表述，特將點以如下方式聲明:將某一點α點在某個坐標系ζ下的坐標，表示為
[0033]在本實施例中，使用GPS接收機作為衛星定位傳感器進行說明。而在其他一些不同的實施例中，提到的衛星定位傳感器不限定為GPS接收機，可以是伽利略、格朗納斯和北斗等衛星導航系統。
[0034]所述聯合校準方法具體的實施步驟如下:
[0035]第一步，按照特定方式裝配視覺傳感器20和GPS接收機天線30。要點是必須保證GPS接收機天線30的中心點31在視覺傳感器20所觀測的每幀圖像中都是可見的，視覺傳感器20和GPS接收機天線30裝配的示意圖示于圖1 (此圖為側視圖)和圖2 (此圖為俯視圖)。.
[0036]本實施例中作為示例，將GPS接收機天線30安裝于視覺傳感器20的右前方，且攝像頭視角下傾，以確保可以在每個采樣時刻都能將GPS接收機天線30涵蓋在觀測的圖像中。這種方式裝配的系統在移動機器人10后續的作業過程中采集出的每一幀圖像的右下方都會出現GPS接收機天線30及該天線的中心點31 (因GPS接收機輸出的經度、緯度和高度是指該中心點的經度、緯度和高度)，經這種方式裝配的系統在一幀的觀測圖像的示意圖(此為示意圖，而非實圖)，示于圖3 ;通過這一步可以確保慣性導傳感器所測量的三個歐拉角航向角Ψ、俯仰角Y和橫滾角Θ反映了 c坐標系與局部地理坐標系坐標軸之間的旋轉角度。
[0037]第二步，從視覺傳感器的前期校正結果中提取本發明后續步驟需要的參數；視覺傳感器的校正是領域內通用的技術，領域內有很多開源的相機校正工具包可免費使用，例如，使用Matlab語言編寫的相機校正工具包，和使用了 C++語言編寫的相機校正工具包，后者依托于開源的機器視覺庫OpenCV。本發明實施例的相機前期校正可使用上述兩個工具包中的任意一個進行校正，他們的效果是等效的。視覺傳感器的前期校正中，能得到相機經校正后的結果即固有矩陣K，它可以表示為:
【權利要求】
1.一種適用于機器人導航的視覺與衛星定位傳感器聯合校準方法，其特征在于，包含以下步驟: 第一步，按照特定方式在移動機器人上裝配視覺傳感器和衛星定位傳感器天線，該特定方式是指能確保后續作業中衛星定位傳感器天線的中心點在視覺傳感器所觀測的每幀圖像中都是可見的裝配方式； 第二步，提取視覺傳感器的前期校正結果用于后續步驟； 第三步，測量視覺傳感器與衛星定位傳感器天線之間的直線距離； 第四步，使用視覺傳感器采集一幀圖像，獲取衛星定位傳感器天線中心點在該幀圖像所對應的相面坐標系m中的坐標；所述相面坐標系m是該巾貞圖像中對應存在的一個二維坐標系，其原點位于該巾貞圖像的左上角；任意一點在所述相面坐標系m中的坐標,通過該點在沿橫軸u和沿縱軸V方向距離該相面坐標系m原點有多少個像素點單位來表示； 第五步，求取衛星定位傳感器天線中心點在相機中心坐標系中的坐標的絕對值；所述相機中心坐標系，其原點為相機成像中心，即光心，X軸指向攝像頭前方，y軸指向左方，z軸指向相機上方； 第六步，確定衛星定位傳感器天線中心點在相機中心坐標系中的坐標的各項的正負；第七步，確定相機中心坐標系的原點在等效坐標系k中的坐標；該等效坐標系k原點為衛星定位傳感器天線中心，該等效坐標系k坐標軸與相機中心坐標系坐標軸平行； 所述的第一步至第七步，在移動機器人作業之前進行一次； 第八步，在移動機器人作業過程中的每個采樣時刻都要進行本步驟； SP，基于每個采樣時刻衛星定位傳感器測得的經度、緯度和高度，求取相機中心坐標系原點的經度、緯度和 高度:該求取過程中包含基于GPS的地心地固坐標系e下的大地坐標，求得相機中心點的大地坐標；基于本步驟的計算結果，將后續視覺傳感器所有觀測的數值都轉換為地心地固坐標系e下的大地坐標，實現對視覺傳感器和衛星定位傳感器的聯合校準；其中,所述地心地固坐標系e中，以地球球心為原點，X軸指向本初子午線，y軸指向東經90度，z軸與地球自轉軸重合，指向北極；大地坐標，用經度λ，緯度賢，高度h表示。
2.如權利要求1所述的聯合校準方法，其特征在于， 所述的第二步中，視覺傳感器的前期校正結果包含固有矩陣K:
3.如權利要求2所述的聯合校準方法，其特征在于， 所述的第三步中，測量的一端是第一步中特定方式裝配好的衛星定位傳感器天線中心點處，測量的另一端是視覺傳感器的中心，測量二者之間的直線距離，記為r。
4.如權利要求3所述的聯合校準方法，其特征在于， 所述的第四步中，包含以下的子步驟:從視覺傳感器采集的一幀圖像中辨識出衛星定位傳感器天線中心點，記為go ;確定go在該巾貞圖像的相面坐標系m中的坐標,記為(ug, Vg)。
5.如權利要求4所述的聯合校準方法，其特征在于， 所述的第五步中，求取衛星定位傳感器天線中心點在相機中心坐標系中的坐標的絕對m\4°i \?°ι uf丨)，包含以下過程:
6.如權利要求5所述的聯合校準方法，其特征在于， 所述的第六步中，確定衛星定位傳感器天線中心點在相機中心坐標系中的坐標的各項的正負，包含以下操作: 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.如權利要求6所述的聯合校準方法，其特征在于， 所述的第七步中，根據衛星定位傳感器天線中心點go在相機中心坐標系中的坐標 ，即(Jtf，>f )，來求取所述相機中心坐標系的原點CO在等效坐標系k中的坐標_lf，即包含以下操作:
8.如權利要求7所述的聯合校準方法，其特征在于， 每個采樣時刻t進行的所述第八步中，包含以下操作:a)基于相機中心坐標系的原點Co在等效坐標系k中的坐標if，即
【文檔編號】G01C25/00GK103438906SQ201310410633
【公開日】2013年12月11日 申請日期:2013年9月10日 優先權日:2013年9月10日
【發明者】孫作雷, 張波, 黃平平, 曾連蓀, 朱大奇 申請人:上海海事大學