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

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


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