urse_speed]
[0076]course是潛航體當(dāng)前航向，由INS獲得；courseO是預(yù)設(shè)的潛航體的目標航向；course_para3是水平舵機中預(yù)設(shè)的航向控制系數(shù);H為水平舵機中預(yù)設(shè)的預(yù)設(shè)的與航向有關(guān)的航向控制參數(shù);G為水平舵機中預(yù)設(shè)的與航向角速度有關(guān)的航向控制參數(shù)；course_speed為航向角速度，由INS獲得;vct_rud_set_angle為計算出的水平舵機的控制角度。
[0077]算法三、潛航體低速航行的速度控制算法為:
[0078]mmotor_speed = speed_paraX [Al X (speed-speedO)-Bi X (speed1-speedO)-Ci X(speed2_speed0)]。
[0079]speed是潛航體當(dāng)前速度，由DVL獲得;speedO是潛航體預(yù)先設(shè)定的速度;speedl是潛航體上一時刻的速度;speed2是潛航體上上時刻的速度；speed_para是預(yù)設(shè)的速度控制的比例系數(shù);mmotorjpeed是計算出的水平推進器的控制速度，AhBjPC1*別為預(yù)設(shè)的水平推進器的控制參數(shù)。
[0080]算法四、潛航體低速航行時航向控制算法如下:
[0081 ] f cmotor_speed = course_paraX[A2 X (course_course0)-B2X (coursel-course0)-C2 X (course2-course0) ]/[l+fabs(mmotor_speed)/Di]。
[0082]bcmotor_speed = _course_para X [A3 X (course-courseO )-B3 X (course 1-course0)-C3 X (course2_course0) ]/[l+fabs(mmotor_speed)/D2]。
[0083]courseO是預(yù)設(shè)的潛航體目標航向；course是潛航體當(dāng)前航向；coursel是潛航體上一時刻的航向；course2是潛航體上上時刻的航向；course_para是預(yù)設(shè)的航向控制的比例系數(shù);臟社證_叩的(1是水平推進器的控制速度;如、82和(:2分別為預(yù)設(shè)的前側(cè)向推進器的控制參數(shù)為預(yù)設(shè)的前側(cè)向推進器作用因子;fcmotor_speed為計算出的前側(cè)向推進器的控制速度;A3、B3和C3分別為預(yù)設(shè)的后側(cè)向推進器的控制參數(shù);D2為預(yù)設(shè)的后側(cè)向推進器作用因子;bcmotor_speed為計算出的后側(cè)向推進器的控制速度;fabs為取絕對值函數(shù)。
[0084]算法五、潛航體低速航行時深度控制算法如下:
[0085]vmotor_speed = depth_para X [A4X (depth-depthO )-B4 X (depth 1-depthO )-C4 X(depth2~depth0)]/[l+fabs(speed)/F]；
[0086]h_rud_set_angle = depth_paral X (depth-depthO )-depth_para2 X pitching_angle Χπ/180ο
[0087]depthO是預(yù)設(shè)的潛航體目標深度;depth是潛航體當(dāng)前深度;depthl是潛航體上一時刻的深度;depth2是潛航體上上時刻的深度;depth_para是預(yù)設(shè)的潛航體深度控制的比例系數(shù);speed是潛航體當(dāng)前速度;fabs為取絕對值函數(shù)，F(xiàn)為預(yù)設(shè)的速度影響因子;A4、B4和C4分別為預(yù)設(shè)的垂直推進器的控制參數(shù);depth_paral是預(yù)設(shè)的舵機深度控制系數(shù);depth_para2是預(yù)設(shè)的舵機俯仰角控制系數(shù);pitching_angle為由慣導(dǎo)獲取的潛航體的俯仰角；vmotor_speed為計算出的垂向推進器的控制速度，h_rud_set_angle2為此時計算出的水平舵機的控制角度。
[0088]算法六、潛航體懸停對準時的距離控制的算法如下:
[0089]mmotor_speed2 = dis_para X [As X (distance_distance0)-B5 X (distancel-distanceO)-C5 X(distance2_distanceO)]/[l+fabs(speed/Fi)]；
[OO9O]di stanceO是潛航體離目標點的設(shè)定觀測安全距離;distance是潛航體離目標點的距離;distancel是潛航體上一時刻離目標點的距離;distance2是潛航體上上時刻離目標點的距離;di s_para是預(yù)設(shè)的距離控制的比例系數(shù);mmo tor_speed2為此時計算出的水平推進器的控制速度;F1為預(yù)設(shè)的速度影響因子，A5、B5、C5為預(yù)設(shè)的潛航體距離控制參數(shù)。
[0091 ]算法七、潛航體圍繞目標進行懸停旋轉(zhuǎn)的控制算法如下:
[0092]f cmotor_speedl = course_paral X [A2 X (course-courseO)-B2 X (course 1_course0)-C2 X (course2-course0) ]/(l+fabs(mmotor_speed3)/Di)；
[0093]bcmotor_speedl = -1200 + course_para2X [A3 X (course-courseO)-B3 X(course1-courseO)-C3X (course2_course0)])/[l+fabs(mmotor_speed3)/D2]。
[0094]course是潛航體當(dāng)前航向；coursel是潛航體上一時刻的航向；course2是潛航體上上時刻的航向；courseO是預(yù)設(shè)的潛航體目標航向；course_paraI是預(yù)設(shè)的航向控制中前側(cè)推進器的比例系數(shù);course_para2是預(yù)設(shè)的航向控制中后側(cè)推進器的比例系數(shù);mmotor_speed3是此時水平推進器的控制速度;fcmotor_speedl是此時計算出的前側(cè)向推進器的控制速度，bcmotor_speedl是此時計算出的后側(cè)向推進器的控制速度。
[0095]當(dāng)潛航體高速航行到離目標只有40米時，潛航體速度無條件降為3節(jié)，從離目標40米到5米，潛航體速度線性減小到0，且航行的過程中潛航體一直指向目標，在減速過程中采用速度控制算法三和航向控制算法四，在減速的過程中，深度控制算法五根據(jù)速度的大小動態(tài)調(diào)整水平舵機和垂向推進器的控制比例，速度越低，垂向推進器的控制比例越大。
[0096]潛航體圍繞目標進行懸停抵近觀測米用算法五、算法六、算法七二種算法。控制流程為:當(dāng)潛航體運動到離目標只有5米時;距離控制算法六開始啟動，控制水平推進器使?jié)摵襟w在離目標5米處進行機動;深度控制算法五計算出垂向推進器的轉(zhuǎn)速控制潛航體在目標深度處進行深度保持;懸停對準算法七計算出兩個側(cè)向推進器的轉(zhuǎn)速，使?jié)摵襟w順時針旋轉(zhuǎn)運動，且頭部一直指向目標，保證潛航體頭部一直指向目標，懸停的時間根據(jù)需要設(shè)定。
[0097]綜上，以上僅為本發(fā)明的較佳實施例而已，并非用于限定本發(fā)明的保護范圍。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進等，均應(yīng)包含在本發(fā)明的保護范圍之內(nèi)。
【主權(quán)項】
1.一種多推進器結(jié)合的水下目標低速抵近與懸停潛航體，其特征在于，還包括:水平推進器(10)、垂向推進器(11)和兩個側(cè)向推進器(12); 所述水平推進器(10)固定連接在潛航體的正尾部，其軸線與潛航體的軸線重合； 在潛航體的浮心位置處開設(shè)一個垂直槽道，所述垂向推進器(11)固定連接在垂直槽道的內(nèi)壁上； 在潛航體航體艏段和艉段分別開設(shè)一個水平貫穿潛航體的水平槽道，一個側(cè)向推進器固定連接在艏段水平槽道的內(nèi)壁上為前側(cè)向推進器，另一個側(cè)向推進器固定連接在艉段水平槽道的內(nèi)壁上為后側(cè)向推進器，兩個側(cè)向推進器(12)的螺旋槳朝向相反。2.如權(quán)利要求1所述的一種多推進器結(jié)合的水下目標低速抵近與懸停潛航體，其特征在于，所述潛航體包括:殼體(I)、慣性導(dǎo)航設(shè)備INS(2)、自動駕駛儀(3)、電源控制單元(4)、垂直舵機(5)、水平舵機(6)、電源控制單元(7)、深度計(8)、多普勒計程儀DVL(9)和無線電ο (13)； 所述殼體(I)為流線型殼體； 所述INS(2)接收來自DVL的測量數(shù)據(jù)，并將其與INS內(nèi)部傳感器檢測的信息組合計算，得到潛航體的方位信息和姿態(tài)信息并向自動駕駛儀輸出； 所述自動駕駛儀(3)接收INS傳來的方位信息和姿態(tài)信息、以及深度計(8)傳來的深度信息，由潛航體內(nèi)置控制算法運算后，輸出針對水平推進器(10)、水平舵機(6)、垂直舵機(5)、側(cè)向推進器(12)、垂向推進器(11)的控制指令，控制水平推進器(10)、水平舵機(6)、垂直舵機(5)高速航行到目標點周圍，然后低速抵近目標:控制側(cè)向推進器(12)、垂向推進器(11)、水平推進器(10)以及水平舵機(6)，在目標周圍進行懸停； 在低速抵近目標時:控制水平推進器(10)以控制潛航體的前后移動速度;控制側(cè)向推進器(12)和垂向推進器(11)以控制潛航體的航向或進行指向性的旋轉(zhuǎn)運動;控制垂向推進器(11)和水平舵機(6)共同控