urse_speed]
[0076]course是潜航体当前航向，由INS获得；courseO是预设的潜航体的目标航向；course_para3是水平舵机中预设的航向控制系数;H为水平舵机中预设的预设的与航向有关的航向控制参数;G为水平舵机中预设的与航向角速度有关的航向控制参数；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是潜航体当前速度，由DVL获得;speedO是潜航体预先设定的速度;speedl是潜航体上一时刻的速度;speed2是潜航体上上时刻的速度；speed_para是预设的速度控制的比例系数;mmotorjpeed是计算出的水平推进器的控制速度，AhBjPC1*别为预设的水平推进器的控制参数。
[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是预设的潜航体目标航向；course是潜航体当前航向；coursel是潜航体上一时刻的航向；course2是潜航体上上时刻的航向；course_para是预设的航向控制的比例系数;臟社证_叩的(1是水平推进器的控制速度;如、82和(:2分别为预设的前侧向推进器的控制参数为预设的前侧向推进器作用因子;fcmotor_speed为计算出的前侧向推进器的控制速度;A3、B3和C3分别为预设的后侧向推进器的控制参数;D2为预设的后侧向推进器作用因子;bcmotor_speed为计算出的后侧向推进器的控制速度;fabs为取绝对值函数。
[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是预设的潜航体目标深度;depth是潜航体当前深度;depthl是潜航体上一时刻的深度;depth2是潜航体上上时刻的深度;depth_para是预设的潜航体深度控制的比例系数;speed是潜航体当前速度;fabs为取绝对值函数，F为预设的速度影响因子;A4、B4和C4分别为预设的垂直推进器的控制参数;depth_paral是预设的舵机深度控制系数;depth_para2是预设的舵机俯仰角控制系数;pitching_angle为由惯导获取的潜航体的俯仰角；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是潜航体离目标点的设定观测安全距离;distance是潜航体离目标点的距离;distancel是潜航体上一时刻离目标点的距离;distance2是潜航体上上时刻离目标点的距离;di s_para是预设的距离控制的比例系数;mmo tor_speed2为此时计算出的水平推进器的控制速度;F1为预设的速度影响因子，A5、B5、C5为预设的潜航体距离控制参数。
[0091 ]算法七、潜航体围绕目标进行悬停旋转的控制算法如下:
[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是潜航体当前航向；coursel是潜航体上一时刻的航向；course2是潜航体上上时刻的航向；courseO是预设的潜航体目标航向；course_paraI是预设的航向控制中前侧推进器的比例系数;course_para2是预设的航向控制中后侧推进器的比例系数;mmotor_speed3是此时水平推进器的控制速度;fcmotor_speedl是此时计算出的前侧向推进器的控制速度，bcmotor_speedl是此时计算出的后侧向推进器的控制速度。
[0095]当潜航体高速航行到离目标只有40米时，潜航体速度无条件降为3节，从离目标40米到5米，潜航体速度线性减小到0，且航行的过程中潜航体一直指向目标，在减速过程中采用速度控制算法三和航向控制算法四，在减速的过程中，深度控制算法五根据速度的大小动态调整水平舵机和垂向推进器的控制比例，速度越低，垂向推进器的控制比例越大。
[0096]潜航体围绕目标进行悬停抵近观测米用算法五、算法六、算法七二种算法。控制流程为:当潜航体运动到离目标只有5米时;距离控制算法六开始启动，控制水平推进器使潜航体在离目标5米处进行机动;深度控制算法五计算出垂向推进器的转速控制潜航体在目标深度处进行深度保持;悬停对准算法七计算出两个侧向推进器的转速，使潜航体顺时针旋转运动，且头部一直指向目标，保证潜航体头部一直指向目标，悬停的时间根据需要设定。
[0097]综上，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
【主权项】
1.一种多推进器结合的水下目标低速抵近与悬停潜航体，其特征在于，还包括:水平推进器(10)、垂向推进器(11)和两个侧向推进器(12); 所述水平推进器(10)固定连接在潜航体的正尾部，其轴线与潜航体的轴线重合； 在潜航体的浮心位置处开设一个垂直槽道，所述垂向推进器(11)固定连接在垂直槽道的内壁上； 在潜航体航体艏段和艉段分别开设一个水平贯穿潜航体的水平槽道，一个侧向推进器固定连接在艏段水平槽道的内壁上为前侧向推进器，另一个侧向推进器固定连接在艉段水平槽道的内壁上为后侧向推进器，两个侧向推进器(12)的螺旋桨朝向相反。2.如权利要求1所述的一种多推进器结合的水下目标低速抵近与悬停潜航体，其特征在于，所述潜航体包括:壳体(I)、惯性导航设备INS(2)、自动驾驶仪(3)、电源控制单元(4)、垂直舵机(5)、水平舵机(6)、电源控制单元(7)、深度计(8)、多普勒计程仪DVL(9)和无线电ο (13)； 所述壳体(I)为流线型壳体； 所述INS(2)接收来自DVL的测量数据，并将其与INS内部传感器检测的信息组合计算，得到潜航体的方位信息和姿态信息并向自动驾驶仪输出； 所述自动驾驶仪(3)接收INS传来的方位信息和姿态信息、以及深度计(8)传来的深度信息，由潜航体内置控制算法运算后，输出针对水平推进器(10)、水平舵机(6)、垂直舵机(5)、侧向推进器(12)、垂向推进器(11)的控制指令，控制水平推进器(10)、水平舵机(6)、垂直舵机(5)高速航行到目标点周围，然后低速抵近目标:控制侧向推进器(12)、垂向推进器(11)、水平推进器(10)以及水平舵机(6)，在目标周围进行悬停； 在低速抵近目标时:控制水平推进器(10)以控制潜航体的前后移动速度;控制侧向推进器(12)和垂向推进器(11)以控制潜航体的航向或进行指向性的旋转运动;控制垂向推进器(11)和水平舵机(6)共同控