一種用於水下機器人的單目視覺定位方法與流程
2023-07-02 10:16:36
本發明涉及視覺定位技術領域，具體為一種用於水下機器人的單目視覺定位方法，是一種在二維平面內進行單目視覺定位的簡易方法，特別適用於無人水下航行器(autonomousunderwatervehicle，auv)等水下機器人的對接回收工作。
背景技術：
能夠確定自身相對於特定目標的位置，是水下機器人成功執行作業任務的必要條件。但是，複雜的水下環境給機器人的精確定位帶來很多難題。由於不能使用gps信號進行定位，高精度慣性導航系統融合水聲定位系統是目前通常採用的水下作業定位方法。此類系統的成本很高，而且水聲定位周期較長、近距離相對誤差較大，不滿足水下精確作業的要求。隨著圖像處理技術的日漸成熟，視覺定位方法越來越多地應用到水下機器人的作業任務中。與水聲定位系統相比，視覺系統具有更新率高、測量精度高、成本低、系統可靠等優點，並且可以應用於非結構自然環境中。
從相機的數目來區分，視覺定位可分為單目定位與雙目定位。雙目定位可以直接獲取目標物的深度信息，但是該定位算法比較複雜，運算周期長，而且水下環境的複雜性影響了圖像特徵提取與匹配的精度。因此，雙目視覺定位並沒有廣泛地應用到水下視覺定位任務中。
很多研究者嘗試改進單目視覺定位方法以獲得深度信息。一般說來，常見的方法有兩種：一種是幾何方法，一種是擴展卡爾曼濾波(extendedkalmanfilter,ekf)算法。許多學者在理論上對ekf定位算法做了研究並改進。但是，該方法要求機器人的模型精確已知——在大多數情況下，這是很難實現的。並且，該方法的計算繁雜，不適用於對實時性要求較高的系統。
技術實現要素：
為解決現有技術存在的問題，本發明提出一種用於水下機器人的單目視覺定位方法，採用幾何算法，既能夠擺脫對系統模型的依賴，又能夠極大地簡化計算過程，而且該方法便於實施，能夠用到水下機器人的實際定位任務中。
本發明實現過程中，需要兩個標誌物，標誌物是機器人通過視覺將要識別的特徵，在實驗開始前應精確已知兩標誌物之間的距離，通過對兩個標誌物的識別和一定的幾何運算，機器人能夠獲得本體相對於兩個標誌物連線中點的坐標。為了便於識別、減小定位誤差，標誌物應當與背景有足夠的區分度，並且標誌物的幾何尺寸與兩標誌物的間距之比應當較小(例如小於1/20)。發明人所做實驗中使用的標誌物為杆狀標記物，其中杆的外徑為5cm，兩桿間距為2.0m。兩根杆分別為紅色和綠色，與水體背景有足夠的區分度。
水下機器人將通過搭載的相機採集圖像，通過搭載的計算機進行圖像特徵提取及定位計算，並利用定位信息執行一定的操作。本發明採用基於幾何方法的定位算法，為了取得較好的定位效果，相機應安裝在機器人的中軸線上。發明人所做實驗中使用的水下機器人為全驅動結構，最大航行速度1m/s。相機的解析度為780×580，焦距5mm，最大幀率67fps(實驗使用幀率5fps)。
方法實現中需要定義如圖1所示的兩個坐標系：全局坐標系e和載體坐標系b。本發明使用幾何方法解決單目視覺定位問題。圖1中點4代表所需識別的兩個標誌物。全局坐標系的原點oe選在兩個標誌物連線的中點處，載體坐標系的原點ob選在機器人的重心處。各坐標軸的正方向參見圖1。ψ表示機器人在全局坐標系下的航向角，定義為xe軸與xb軸的夾角，正方向如圖1所示。
圖2表示該方法的幾何原理圖。以單目相機的位置來代表機器人當前所處的位置，以a點表示。表示相機的半視角大小(也即∠dac)，相機的視角值可以通過查閱數據手冊或者相機標定等手段獲得。ψ表示機器人在全局坐標系下的航向角，可以由機器人搭載的導航元件測出。m點位於ye軸，滿足am⊥ye。d點表示相機視角的角平分線ad與ye軸的交點。p點及q點代表兩個標誌物，且p、q在全局坐標系下的坐標已知。p′q′所在直線表示成像平面，且有p′q′⊥ad。d′點、c′點、p′點、q′、o′點分別表示d點、c點、p點、q點、o點在成像平面上的投影。應注意，在圖2表示的坐標系下：當ψ>0時，c點選在ye負半軸上，且q點與q′點重合；當ψ0，並且oc<cm；或者ψcm，則修正θ的值θ＝-θ；根據公式
x＝-oacosθ
y＝-oasinθ
計算a點位置在全局坐標系中的坐標；
步驟2.8：判斷步驟2.7計算得到的a點位置坐標與步驟2.1設置的a點位置坐標之間的誤差是否滿足設定要求，若滿足，則得到一組由mag、dd和ψ組成的數據點，並進行步驟3，若不滿足，則修改放大倍數mag並返回步驟2.3；所述dd表示在水平方向上，圖像中點偏離標誌物連線中點的像素長度l1與圖像像素寬度一半l2的比值；
步驟3：重新在全局坐標系中任意選擇一個全局坐標已知的點作為機器人當前所處位置a，並且指定航向角ψ的取值確保機器人搭載的相機在該點能夠拍攝兩個標誌物，重複步驟2，得到新的一組數據點；當得到設定組數的數據點後，進入步驟4；
步驟4：根據得到的若干組數數據點，擬合得到以mag因變量，dd以及ψ為自變量的函數關係；
步驟5：實際定位過程中，採集相機圖像，得到機器人在全局坐標系下的航向角ψ以及當前狀態下的dd，根據步驟4得到的擬合函數，得到當前狀態下的放大倍數mag；
步驟6：根據步驟5採集的相機圖像，利用步驟5得到的當前狀態下的放大倍數mag，計算機器人實際坐標：
步驟6.1：計算從pq到p′q′的折算係數λ，以放大倍數mag修正後得到λ*
其中p點及q點代表兩個標誌物，且p、q在全局坐標系下的坐標已知，p′點、q′點分別表示p點、q點在成像平面上的投影，p′點、q′點在圖像中的坐標已知；
步驟6.2：根據公式
計算線段dc和ac的長度，其中c點為相機半視角邊緣在全局坐標系ye軸上的點，d點表示相機視角的角平分線ad與ye軸的交點，表示相機的半視角大小，d′點、c′點分別表示d點、c點在成像平面上的投影；
步驟6.3：根據公式
計算線段am和cm的長度，其中m點位於ye軸上，滿足am⊥ye；
步驟6.4：根據公式
om＝|cm-oc|
計算線段oc和om的長度，其中o′點表示o點在成像平面上的投影；
步驟6.5：根據公式
計算a點位置極坐標，其中如果ψ>0，並且oc<cm；或者ψcm，則修正θ的值θ＝-θ；根據公式
x＝-oacosθ
y＝-oasinθ
計算機器人a點在全局坐標系中的坐標。
有益效果
本發明的有益效果是，設計了一套簡便易行、精度較高的單目視覺定位方法。該方法應用範圍較廣，通過合理地布置兩個標誌物的位置，該方法能夠應用於水下機器人的定點回收、目標跟蹤、動力定位等任務中。所用方法在確保較高定位精度的情況下，極大降低了單目視覺定位方法的複雜度，提高了時效性，使得其可以應用到沒有精確數學模型的系統中，或者對實時性要求較高的系統中，或者價格低廉、處理性能有限的硬體平臺上。
本發明的附加方面和優點將在下面的描述中部分給出，部分將從下面的描述中變得明顯，或通過本發明的實踐了解到。
附圖說明
本發明的上述和/或附加的方面和優點從結合下面附圖對實施例的描述中將變得明顯和容易理解，其中：
圖1為全局坐標系e和載體坐標系b的示意圖。
圖2為幾何方法的原理圖。
圖3為圖像中點與標誌物連線中點在水平方向上偏離的示意圖。
圖4為放大倍數mag的擬合結果。
圖中1.機器人，2.載體坐標系原點，3.單目相機，4.特徵點，5.全局坐標系原點。
具體實施方式
下面詳細描述本發明的實施例，所述實施例是示例性的，旨在用於解釋本發明，而不能理解為對本發明的限制。
為解決現有技術存在的問題，本發明提出一種用於水下機器人的單目視覺定位方法，採用幾何算法，既能夠擺脫對系統模型的依賴，又能夠極大地簡化計算過程，而且該方法便於實施，能夠用到水下機器人的實際定位任務中。
一、系統組成
本發明用於水下機器人的單目視覺定位系統主要由兩個標誌物和水下機器人兩部分組成。標誌物是機器人通過視覺將要識別的特徵，在實驗開始前應精確已知兩標誌物之間的距離，通過對兩個標誌物的識別和一定的幾何運算，機器人能夠獲得本體相對於兩個標誌物連線中點的坐標。為了便於識別、減小定位誤差，標誌物應當與背景有足夠的區分度，並且標誌物的幾何尺寸與兩標誌物的間距之比應當較小(例如小於1/20)。發明人所做實驗中使用的標誌物為杆狀標記物，其中杆的外徑為5cm，兩桿間距為2.0m。兩根杆分別為紅色和綠色，與水體背景有足夠的區分度。
水下機器人將通過搭載的相機採集圖像，通過搭載的計算機進行圖像特徵提取及定位計算，並利用定位信息執行一定的操作。本發明採用基於幾何方法的定位算法，為了取得較好的定位效果，相機應安裝在機器人的中軸線上。發明人所做實驗中使用的水下機器人為全驅動結構，最大航行速度1m/s。相機的解析度為780×580，焦距5mm，最大幀率67fps(實驗使用幀率5fps)。
二、基於幾何方法的單目視覺定位模型
方法實現中需要定義如圖1所示的兩個坐標系：全局坐標系e和載體坐標系b。本發明使用幾何方法解決單目視覺定位問題。圖1中點4代表所需識別的兩個標誌物。全局坐標系的原點oe選在兩個標誌物連線的中點處，載體坐標系的原點ob選在機器人的重心處。各坐標軸的正方向參見圖1。ψ表示機器人在全局坐標系下的航向角，定義為xe軸與xb軸的夾角，正方向如圖1所示。
圖2表示該方法的幾何原理圖。以單目相機的位置來代表機器人當前所處的位置，以a點表示。表示相機的半視角大小(也即∠dac)，相機的視角值可以通過查閱數據手冊或者相機標定等手段獲得。ψ表示機器人在全局坐標系下的航向角，可以由機器人搭載的導航元件測出。m點位於ye軸，滿足am⊥ye。d點表示相機視角的角平分線ad與ye軸的交點。p點及q點代表兩個標誌物，且p、q在全局坐標系下的坐標已知。p′q′所在直線表示成像平面，且有p′q′⊥ad。d′點、c′點、p′點、q′、o′點分別表示d點、c點、p點、q點、o點在成像平面上的投影。應注意，在圖2表示的坐標系下：當ψ>0時，c點選在ye負半軸上，且q點與q′點重合；當ψ0，並且oc<cm；或者ψcm，則修正θ的值θ＝-θ；根據公式
x＝-oacosθ
y＝-oasinθ
計算a點位置在全局坐標系中的坐標；
步驟2.8：判斷步驟2.7計算得到的a點位置坐標與步驟2.1設置的a點位置坐標之間的誤差是否滿足設定要求，若滿足，則得到一組由mag、dd和ψ組成的數據點，並進行步驟3，若不滿足，則修改放大倍數mag並返回步驟2.3；所述dd表示在水平方向上，圖像中點偏離標誌物連線中點的像素長度l1與圖像像素寬度一半l2的比值；
步驟3：重新在全局坐標系中任意選擇一個全局坐標已知的點作為機器人當前所處位置a，並且指定航向角ψ的取值確保機器人搭載的相機在該點能夠拍攝兩個標誌物，重複步驟2，得到新的一組數據點；當得到設定組數的數據點後，進入步驟4；
步驟4：根據得到的若干組數數據點，擬合得到以mag因變量，dd以及ψ為自變量的函數關係；
步驟5：實際定位過程中，採集相機圖像，得到機器人在全局坐標系下的航向角ψ以及當前狀態下的dd，根據步驟4得到的擬合函數，得到當前狀態下的放大倍數mag；
步驟6：根據步驟5採集的相機圖像，利用步驟5得到的當前狀態下的放大倍數mag，計算機器人實際坐標：
步驟6.1：計算從pq到p′q′的折算係數λ，以放大倍數mag修正後得到λ*
其中p點及q點代表兩個標誌物，且p、q在全局坐標系下的坐標已知，p′點、q′點分別表示p點、q點在成像平面上的投影，p′點、q′點在圖像中的坐標已知；
步驟6.2：根據公式
計算線段dc和ac的長度，其中c點為相機半視角邊緣在全局坐標系ye軸上的點，d點表示相機視角的角平分線ad與ye軸的交點，表示相機的半視角大小，d′點、c′點分別表示d點、c點在成像平面上的投影；
步驟6.3：根據公式
計算線段am和cm的長度，其中m點位於ye軸上，滿足am⊥ye；
步驟6.4：根據公式
om＝|cm-oc|
計算線段oc和om的長度，其中o′點表示o點在成像平面上的投影；
步驟6.5：根據公式
計算a點位置極坐標，其中如果ψ>0，並且oc<cm；或者ψcm，則修正θ的值θ＝-θ；根據公式
x＝-oacosθ
y＝-oasinθ
計算機器人a點在全局坐標系中的坐標。
基於上述技術方案，下面給出兩個實施實例：
坐標系定義、各參數定義及正方向定義都參照圖1和圖2。已知pq＝2.0m，相機的像素寬度為800pixel。依賴具體的實驗條件，以上參數可以不同。
放大倍數mag的擬合函數為：
mag＝1.013+1.105×10-2dd+1.175×10-2|ψ|-7.832×10-2dd2-2.426×10-2dd|ψ|
【實施例1】
設相機當前採集的圖像中，p′＝156pixel，q′＝315pixel，機器人上搭載的慣性器件測得當前時刻的航向角ψ＝5°。下面計算機器人在全局坐標系中的坐標。
第一步：dd＝0.411，根據擬合函數計算得到mag＝1.013；
第二步：計算得到λ*＝80.55；
第三步：計算得到dc＝4.97，ac＝8.22；
第四步：計算得到am＝6.97，cm＝4.36；
第五步：計算得到oc＝2.92，om＝1.43；
第六步：計算得到oa＝7.12，θ＝11.61°；
第七步：此時，ψ>0，並且oc0，並且oc>cm，因此不需對θ進行修正；
第八步：計算得到機器人在全局坐標系中的坐標為x＝-5.01，y＝-0.51。
誤差分析：本例中，機器人在全局坐標系中的真實坐標為(-5.0,-0.5)，因此，相對定位誤差為(0.2％,2％)。
儘管上面已經示出和描述了本發明的實施例，可以理解的是，上述實施例是示例性的，不能理解為對本發明的限制，本領域的普通技術人員在不脫離本發明的原理和宗旨的情況下在本發明的範圍內可以對上述實施例進行變化、修改、替換和變型。