專利名稱：載人潛水器自動躲避障礙方法
技術(shù)領(lǐng)域：
本發(fā)明涉及載人潛水器在海底航行時自動躲避障礙的方法。
背景技術(shù)：
載人潛水器在水下航行時要有自動躲避障礙的動能。現(xiàn)在載人潛水器的自動躲避障礙的方法，是由單一的距離信號判斷障礙物與載人潛水器的距離，然后躲避障礙。

發(fā)明內(nèi)容
為了實(shí)現(xiàn)載人潛水器的自動躲避障礙功能，本發(fā)明的目的是提供一種載人潛水器自動躲避障礙的方法，采用本發(fā)明載人潛水器可根據(jù)多路測距傳感器的信息判斷海底地形或障礙物的復(fù)雜情況，自動改變縱向傾角、速度或航向角，避免和障礙物碰撞。
為了實(shí)現(xiàn)上述目的，本發(fā)明的技術(shù)方案是接收多路測距傳感器的距離信號，通過計(jì)算直接給出載人潛水器航行的縱向傾角；具體是，首先由多部測距傳感器獲得載人潛水器與障礙物的距離數(shù)據(jù)，然后計(jì)算出載人潛水器應(yīng)該運(yùn)動的縱向傾角角度即避碰角ψ和速度值，將算出的避碰角ψ和速度值送給控制系統(tǒng)，以實(shí)現(xiàn)載人潛水器的自動躲避障礙功能；所述多路測距傳感器的安裝位置是在載人潛水器的前部不同方向至少安裝三只測距聲納換能器，分別是在潛水器的正下方安裝一只，斜下方45°安裝一只，正前方安裝一只；它們所測量的載人潛水器與障礙物體的距離分別是d1、d2、d3；同時在載人潛水器上安裝一只測量載人潛水器縱向傾角的傳感器，用以測量載人潛水器目前的縱傾角θ；在載人潛水器的前左、右下方45°另外安裝兩個測距換能器，載人潛水器在圍繞障礙物運(yùn)動時，用于測出障礙物的弧度；所述計(jì)算是計(jì)算載人潛水器應(yīng)該運(yùn)動的縱向傾角角度即避碰角ψ和速度值具體方法如下1)當(dāng)只有正下方安裝的測距聲納換能器可以測量到距離數(shù)據(jù)d1，其它的換能器測量值均為∞時，設(shè)載人潛水器的給定避碰角ψ＝0°，既載人潛水器應(yīng)該在水平的定深航行狀態(tài)；2)當(dāng)只有斜下方45度測距聲納換能器可以測量到距離數(shù)據(jù)d2，其它的換能器測量值均為∞時，設(shè)d2的余弦值為載人潛水器距離障礙物的垂直距離，這時載人潛水器處于水平的定深航行狀態(tài)，潛水器的給定避碰角ψ＝0°；3)當(dāng)只有正前方測距聲納換能器可以測量到距離數(shù)據(jù)d3，其它的換能器均為∞時，載人潛水器的避碰角ψ可以為90°；當(dāng)載人潛水器為上升運(yùn)動時，并已經(jīng)繞過障礙d3＝∞，如果載人潛水器上升一段時間后，d3值不變，讓載人潛水器側(cè)移運(yùn)動，繞過障礙物；4)如果只有d3＝∞，正下方安裝的測距聲納換能器和斜下方45度測距聲納換能器換能器有測量值d1、d2，假設(shè)載人潛水器目前的縱向傾角是θ；地形的坡度角也就是載人潛水器的避碰角ψ1可以由公式(1)求出ψ1=arctan[d1cosθ-d2cos(π4+θ)d1sinθ-d2sin(π4+θ)]---(1);]]>將避碰角ψ1送給載人潛水器的控制系統(tǒng)，載人潛水器就可以按照地形相同的角度航行，避免發(fā)生碰撞。
5)如果只有d1＝∞，斜下方45度測距聲納換能器和正前方測距聲納換能器有測量值d2、d3，避碰角ψ2可以由公式(2)求出ψ2=arctan[d2cos(π4+θ)-d3cos(π2+θ)d2sin(π4+θ)-d3sin(π4+θ)]---(2);]]>6)如果只有d2＝∞，正下方安裝的測距聲納換能器和正前方測距聲納換能器有測量值d1、d3，斜下方45度測距聲納換能器所測量的方向上沒有障礙物，也有可能斜下方45度測距聲納換能器換能器損壞，通過正下方安裝的測距聲納換能器和正前方測距聲納換能器也可以測量出地形的坡度，其避碰角ψ3的公式(3)為ψ3=arctan[d3cos(π2+θ)-d1cosθd3sin(π2+θ)-d1sinθ]---(3);]]>
在只有d1＝∞，斜下方45度測距聲納換能器和正前方測距聲納換能器有測量值d2、d3情況下，有可能地形的傾斜角很大，如果超過載人潛水器允許的傾斜角，那么，載人潛水器就要采取繞行的方法躲避障礙。
本發(fā)明具有如下優(yōu)點(diǎn)1、本發(fā)明提供的載人潛水器自動躲避障礙的方法，可以直接提供給載人潛水器躲避障礙時的航行縱向傾角。這個縱向傾角和障礙物的角度相同，可以保證載人潛水器不與障礙物發(fā)生碰撞，并且可以保持載人潛水器相對于障礙物定高航行。
2、如果在載人潛水器的前左、右下方45°安裝兩個測距換能器，載人潛水器在圍繞障礙物運(yùn)動時，可以測出障礙物的弧度，載人潛水器可以保持與障礙物等距離繞行。
3、本發(fā)明提出的載人潛水器自動躲避障礙方法，同樣適用于其它的水下機(jī)器人，包括自治水下機(jī)器人(AUV)和遙控水下機(jī)器人(ROV)。


圖1為載人潛水器測距聲納安裝示意圖。
圖2為測量避碰角ψ1示意圖。
圖3為測量避碰角ψ2示意圖。
圖4為測量避碰角ψ3示意圖。
具體實(shí)施例方式
下面結(jié)合附圖和實(shí)施方式進(jìn)一步說明本發(fā)明。
實(shí)施例1本發(fā)明的載人潛水器自動躲避障礙方法是接收幾路測距傳感器的距離信號，通過計(jì)算直接給出載人潛水器航行的縱向傾角。具體功能實(shí)現(xiàn)的方法是，首先由多部測距傳感器獲得載人潛水器與障礙物的距離數(shù)據(jù)，然后由一套算法，計(jì)算出載人潛水器應(yīng)該運(yùn)動的縱向傾角角度即避碰角ψ和速度值，將算出的避碰角ψ和速度值送給控制系統(tǒng)，以實(shí)現(xiàn)載人潛水器的自動躲避障礙功能；具體是在載人潛水器的前部不同方向至少安裝三只測距聲納換能器，換能器用于測試在各個方向上障礙物體距離載人潛水器的距離，同時在載人潛水器上安裝一只測量載人潛水器縱向傾角的傳感器，用以測量載人潛水器目前的縱傾角θ；三只測距聲納換能器分別在潛水器的正下方安裝一只，斜下方45°安裝一只，正前方安裝一只；它們所測量的載人潛水器與障礙物體的距離分別是d1、d2、d3(如圖1)。
下面分別敘述測距聲納測量到數(shù)據(jù)的幾種情況，然后根據(jù)測量的數(shù)據(jù)，計(jì)算出如何調(diào)整載人潛水器的航行姿態(tài)。
1、當(dāng)只有正下方安裝的測距聲納換能器可以測量到距離數(shù)據(jù)d1，其它的換能器測量值均為∞時，說明載人潛水器附近沒有障礙物，這時載人潛水器的給定避碰角ψ＝0°，既載人潛水器應(yīng)該在水平的定深航行狀態(tài)；2、當(dāng)只有斜下方45度測距聲納換能器可以測量到距離數(shù)據(jù)d2，其它的換能器測量值均為∞時，說明只在載人潛水器的斜下方有障礙；這時只需要注意d2的余弦值，即載人潛水器距離障礙物的垂直距離。這時載人潛水器應(yīng)該處于水平的定深航行狀態(tài)，潛水器的給定避碰角ψ＝0°；3、當(dāng)只有正前方測距聲納換能器可以測量到距離數(shù)據(jù)d3，其它的換能器均為∞時，這時說明在載人潛水器的前方有障礙，其它方向沒有障礙；理論上載人潛水器的避碰角ψ＝90°；但是載人潛水器不可能是垂直航行。所以這時載人潛水器上升運(yùn)動，如果d3＝∞，說明載人潛水器已經(jīng)繞過障礙；如果載人潛水器上升一段時間后，d3值不變，就要讓載人潛水器側(cè)移運(yùn)動，繞過障礙物；4、如果只有d3＝∞，正下方安裝的測距聲納換能器和斜下方45度測距聲納換能器換能器有測量值d1、d2，這時就可以計(jì)算出載人潛水器下面地形的坡度，假設(shè)載人潛水器目前的縱向傾角是θ；通過圖2可以看出，地形的坡度角也就是載人潛水器的避碰角ψ1可以由公式(1)求出ψ1=arctan[d1cosθ-d2cos(π4+θ)d1sinθ-d2sin(π4+θ)]---(1)]]>將避碰角送給載人潛水器的控制系統(tǒng)，載人潛水器就可以按照地形相同的角度航行，避免發(fā)生碰撞。
5、如果只有d1＝∞，斜下方45度測距聲納換能器和正前方測距聲納換能器有測量值d2、d3，這時可能是地形的坡度角和載人潛水器的傾斜角相差較大，通過圖3可以看出，這時的避碰角ψ2可以由公式(2)求出。
ψ2=arctan[d2cos(π4+θ)-d3cos(π2+θ)d2sin(π4+θ)-d3sin(π4+θ)]---(2)]]>在這種情況下，有可能地形的傾斜角很大，如果超過載人潛水器允許的傾斜角，那么，載人潛水器就要采取繞行的方法躲避障礙。
6、如果只有d2＝∞，正下方安裝的測距聲納換能器和正前方測距聲納換能器有測量值d1、d3，這時斜下方45度測距聲納換能器所測量的方向上沒有障礙物，也有可能斜下方45度測距聲納換能器換能器損壞。這時通過正下方安裝的測距聲納換能器和正前方測距聲納換能器也可以測量出地形的坡度。參考圖4，可以得到避碰角ψ3的公式(3)。
ψ3=arctan[d3cos(π2+θ)-d1cosθd3sin(π2+θ)-d1sinθ]---(3)]]>載人潛水器躲避自動躲避障礙時，還要考慮載人潛水器的速度和潛水器與障礙的距離關(guān)系。當(dāng)載人潛水器距離障礙物近時，要控制載人潛水器的運(yùn)動速度。具體的速度和距離的關(guān)系曲線，要根據(jù)不同載人潛水器的體積和載人潛水器的制動性能來決定。
實(shí)施例2本發(fā)明在載人潛水器的前左、右下方45°增加兩個測距換能器，載人潛水器在圍繞障礙物運(yùn)動時，可以測出障礙物的弧度，載人潛水器可以保持與障礙物等距離繞行。
權(quán)利要求
1.一種載人潛水器自動躲避障礙方法，其特征在于接收多路測距傳感器的距離信號，通過計(jì)算直接給出載人潛水器航行的縱向傾角；具體是，首先由多部測距傳感器獲得載人潛水器與障礙物的距離數(shù)據(jù)，然后計(jì)算出載人潛水器應(yīng)該運(yùn)動的縱向傾角角度即避碰角φ和速度值，將算出的避碰角φ和速度值送給控制系統(tǒng)，以實(shí)現(xiàn)載人潛水器的自動躲避障礙功能。
2.按照權(quán)利要求1所述載人潛水器自動躲避障礙方法，其特征在于所述多路測距傳感器的安裝位置是在載人潛水器的前部不同方向至少安裝三只測距聲納換能器，分別是在潛水器的正下方安裝一只，斜下方45°安裝一只，正前方安裝一只；它們所測量的載人潛水器與障礙物體的距離分別是d1、d2、d3；同時在載人潛水器上安裝一只測量載人潛水器縱向傾角的傳感器，用以測量載人潛水器目前的縱傾角θ。
3.按照權(quán)利要求2所述載人潛水器自動躲避障礙方法，其特征在于在載人潛水器的前左、右下方45°另外安裝兩個測距換能器，載人潛水器在圍繞障礙物運(yùn)動時，用于測出障礙物的弧度。
4.按照權(quán)利要求1所述載人潛水器自動躲避障礙方法，其特征在于所述計(jì)算是計(jì)算載人潛水器應(yīng)該運(yùn)動的縱向傾角角度即避碰角φ和速度值具體方法如下1)當(dāng)只有正下方安裝的測距聲納換能器可以測量到距離數(shù)據(jù)d1，其它的換能器測量值均為∞時，設(shè)載人潛水器的給定避碰角φ＝0°，既載人潛水器應(yīng)該在水平的定深航行狀態(tài)；2)當(dāng)只有斜下方45度測距聲納換能器可以測量到距離數(shù)據(jù)d2，其它的換能器測量值均為∞時，設(shè)d2的余弦值為載人潛水器距離障礙物的垂直距離，這時載人潛水器處于水平的定深航行狀態(tài)，潛水器的給定避碰角φ＝0°；3)當(dāng)只有正前方測距聲納換能器可以測量到距離數(shù)據(jù)d3，其它的換能器均為∞時，載人潛水器的避碰角φ可以為90°；當(dāng)載人潛水器為上升運(yùn)動時，并已經(jīng)繞過障礙d3＝∞，如果載人潛水器上升一段時間后，d3值不變，讓載人潛水器側(cè)移運(yùn)動，繞過障礙物；4)如果只有d3＝∞，正下方安裝的測距聲納換能器和斜下方45度測距聲納換能器換能器有測量值d1、d2，假設(shè)載人潛水器目前的縱向傾角是θ；地形的坡度角也就是載人潛水器的避碰角φ1可以由公式(1)求出ψ1=arctan[d1cosθ-d2cos(π4+θ)d1sinθ-d2sin(π4+θ)]---(1);]]>將避碰角φ1送給載人潛水器的控制系統(tǒng)，載人潛水器就可以按照地形相同的角度航行，避免發(fā)生碰撞。5)如果只有d1＝∞，斜下方45度測距聲納換能器和正前方測距聲納換能器有測量值d2、d3，避碰角φ2可以由公式(2)求出ψ2=arctan[d2cos(π4+θ)-d3cos(π2+θ)d2sin(π4+θ)-d3sin(π4+θ)]---(2);]]>6)如果只有d2＝∞，正下方安裝的測距聲納換能器和正前方測距聲納換能器有測量值d1、d3，斜下方45度測距聲納換能器所測量的方向上沒有障礙物，也有可能斜下方45度測距聲納換能器換能器損壞，通過正下方安裝的測距聲納換能器和正前方測距聲納換能器也可以測量出地形的坡度，其避碰角φ3的公式(3)為ψ3=arctan[d3cos(π2+θ)-d1cosθd3sin(π2+θ)-d1sinθ]---(3).]]>
5.按照權(quán)利要求4所述載人潛水器自動躲避障礙方法，其特征在于在只有d1＝∞，斜下方45度測距聲納換能器和正前方測距聲納換能器有測量值d2、d3情況下，有可能地形的傾斜角很大，如果超過載人潛水器允許的傾斜角，那么，載人潛水器就要采取繞行的方法躲避障礙。
全文摘要
本發(fā)明涉及載人潛水器在海底航行時自動躲避障礙的方法。本發(fā)明可接收多路測距傳感器的距離信號，通過計(jì)算直接給出載人潛水器航行的縱向傾角；具體是，首先由多部測距傳感器獲得載人潛水器與障礙物的距離數(shù)據(jù)，然后計(jì)算出載人潛水器應(yīng)該運(yùn)動的縱向傾角角度即避碰角φ和速度值，將算出的避碰角φ和速度值送給控制系統(tǒng)，以實(shí)現(xiàn)載人潛水器的自動躲避障礙功能。采用本發(fā)明載人潛水器可根據(jù)多路測距傳感器的信息判斷海底地形或障礙物的復(fù)雜情況，自動改變縱向傾角、速度或航向角，避免和障礙物碰撞。
文檔編號B63G8/38GK1718563SQ20041002092
公開日2006年1月11日 申請日期2004年7月7日 優(yōu)先權(quán)日2004年7月7日
發(fā)明者于開洋 申請人:中國科學(xué)院沈陽自動化研究所