标点的设定观测安全距离;distance是潜航体离目标点的距离;distancel是潜航体上一时刻离目标点的距离;distance2是潜航体上上时刻离目标点的距离;di s_para是预设的距离控制的比例系数;mmo tor_speed2为此时计算出的水平推进器的控制速度;F1为预设的速度影响因子，A5、B5、C5为预设的潜航体距离控制参数；
[0040]算法七、潜航体围绕目标进行悬停旋转的控制算法如下:
[0041 ] f cmotor_speedl = course_paral X [A2 X (course-courseO)-B2 X (course 1_course0)-C2 X (course2-course0) ]/(l+fabs(mmotor_speed3)/Di)；
[0042]bcmotor_speedl = -1200 + course_para2X [A3 X (course-courseO)-B3 X(course1-courseO)-C3X (course2_course0)])/[l+fabs(mmotor_speed3)/D2]；
[0043]course是潜航体当前航向；coursel是潜航体上一时刻的航向；course2是潜航体上上时刻的航向；courseO是预设的潜航体目标航向；course_paraI是预设的航向控制中前侧推进器的比例系数;course_para2是预设的航向控制中后侧推进器的比例系数;mmotor_speed3是此时水平推进器的控制速度;fcmotor_speedl是此时计算出的前侧向推进器的控制速度，bcmotor_speedl是此时计算出的后侧向推进器的控制速度。
[0044]当潜航体高速航行到离目标只有40米时，潜航体速度无条件降为3节，从离目标40米到5米，潜航体速度线性减小到0，且航行的过程中潜航体一直指向目标，在减速过程中采用速度控制算法二和航向控制算法四进行控制；
[0045]潜航体围绕目标进行悬停抵近观测米用算法五、算法六、算法七二种算法进行控制，控制流程为:当潜航体运动到离目标只有5米时;距离控制算法六开始启动，控制水平推进器使潜航体在离目标5米处进行机动;深度控制算法五计算出垂向推进器的控制速度，从而控制潜航体在目标深度处进行深度保持;悬停对准算法七计算出两个侧向推进器的控制速度，使潜航体顺时针旋转运动，且头部一直指向目标，保证潜航体头部一直指向目标，悬停的时间根据需要设定。
[0046]有益效果:
[0047]1、本发明公布了一种多推进器结合的目标低速抵近和悬停潜航体，潜航体低速抵近水下目标和悬停控位时靠水平推进器、侧向推进器、垂向推进器、水平舵机产生的力矩控制。为达到更好的低速航行和悬停控制效果，将两个侧向推进器分别安装在潜航体的艏段和艉段，以产生较大的转动力矩;将垂向推进器安装在潜航体中部，这样可以提高垂向推进器定深时的作用效果;水平推进器安装在潜航体正尾部，可以有效控制潜航体前后移动。潜航体在低速接近目标时采用水平推进器、侧向推进器、垂向推进器、水平舵机结合的方式朝目标靠近。利用水平推进器控制潜航体的前后移动速度;两个侧向推进器控制潜航体的航向或进行指向性的旋转运动(头部指向目标的全方位观测)；垂向推进器和水平舵机共同控制潜航体的深度，其中舵机还负责调整潜航体的俯仰姿态，当潜航体速度较低时水平舵机的作用效果较弱或可忽略不计。
[0048]2、本发明中提供了共七种算法，当潜航体高速航向时，采用算法一、算法二，水平推进器给定固定转速，减速过程中采用速度控制算法三和航向控制算法四，潜航体围绕目标进行悬停抵近观测采用算法五、算法六、算法七三种算法。保证了潜航体低速抵近或悬停时，头部一直指向目标，悬停的时间可以根据需要设定。
【附图说明】
[0049]图1一种多推进器结合的水下目标低速抵近与悬停系统结构示意图；
[0050]1、潜航体壳体2、惯性导航设备3、自动驾驶仪4、电池单元5、垂直舵机6、水平舵机
7、电源控制单元8、深度计9、多普勒计程仪10、水平主推进器11、垂向推进器12、侧向推进器13、无线电台。
[0051]图2系统在水中低速接近目标与抵近观测时的轨迹图。
【具体实施方式】
[0052]下面结合附图并举实施例，对本发明进行详细描述。
[0053]本发明在潜航体艏段和艉段的侧向各加上一个侧向推进器，在潜航体的中部安装一套垂直推进器。采用这种配置后，远程AUV就可以低速抵近目标处进行对准观察或进行悬停控制来处理目标。本发明突破了传统AUV运动控制技术，已达到国内领先水平。
[0054]—种多推进器结合的水下目标低速抵近与悬停潜航体，包括:水平推进器10、垂向推进器11和两个侧向推进器12;
[0055]水平推进器10固定连接在潜航体的正尾部，其轴线与潜航体的轴线重合。
[0056]在潜航体的浮心位置处开设一个垂直槽道，垂向推进器11固定连接在垂直槽道的内壁上。
[0057]在潜航体航体艏段和艉段分别开设一个水平贯穿潜航体的水平槽道，两个侧向推进器12分别固定连接在水平槽道的内壁上，两个侧向推进器12的螺旋桨朝向相反。
[0058]进一步地，潜航体包括:壳体1、惯性导航设备INS2、自动驾驶仪3、电源控制单元4、垂直舵机5、水平舵机6、电源控制单元7、深度计8、多普勒计程仪DVL9和无线电台13;
[0059]壳体I为流线型壳体。
[0060]INS2接收DVL信息并和INS内部传感器信息组合计算，向自动驾驶仪输出潜航体的方位信息和姿态信息。
[0061 ]自动驾驶仪3接收INS传来的方位信息和姿态信息、以及深度计传来的深度信息，由潜航体内置控制算法运算后，输出针对水平推进器、水平舵机、垂直舵机、侧向推进器、垂向推进器的控制指令，使得舵机和推进器均执行相应的动作，实现自主航行。
[0062]电源控制单元为整个潜航器提供电能供应。
[0063]垂直舵机5安装在潜航体艉段后部去流段，垂直舵机的两块垂直舵面在壳体I上下两边对称安装，垂直舵机5的驱动电机和控制器安装在壳体内部，垂直舵机5的控制器可接收控制指令，控制垂直舵机5的驱动电机转动，驱动垂直舵面运行到指定角度。
[0064]水平舵机6安装在潜航体尾段后部去流段，水平舵机6的两块水平舵面在壳体左右两边对称安装，水平舵机6的驱动电机和控制器安装在壳体内部;水平舵机6的控制器接收控制指令，控制水平舵机6的驱动电机转动，驱动水平舵面运行到指定角度。
[0065]深度计用来测量潜航体当前的深度信息。
[0066]DVL用来测量潜航体对水底的速度和离水底的高度，并将测量数据发给INS;
[0067]无线电台安装在潜航体内部，用来接收外部的操控任务信息。
[0068]潜航体通过无线电台接收甲板操控单元的任务信息;DVL测量的数据通过电缆发送给INS; INS组合DVL信息计算出导航位置及姿态等数据，通过电缆发送至自动驾驶仪；自动驾驶仪根据任务信息控制水平推进器、水平舵机、垂直舵机高速航行到目标点周围，然后自动驾驶仪启动低速算法控制侧向推进器、垂向推进器、水平推进器以及水平舵机，低速抵近目标，在目标周围进行悬停，以便执行相应的观察或处理任务。
[0069]潜航体低速抵近水下目标和悬停控位时靠水平推进器、侧向推进器、垂向推进器、水平舵机产生的力矩控制。为达到更好的低速航行和悬停控制效果，将两个侧向推进器分别安装在潜航体的艏段和艉段，以产生较大的转动力矩;将垂向推进器安装在潜航体中部，这样可以提高垂向推进器定深时的作用效果;水平推进器安装在潜航体正尾部，可以有效控制潜航体前后移动。潜航体在低速接近目标时采用水平推进器、侧向推进器、垂向推进器、水平舵机结合的方式朝目标靠近。利用水平推进器控制潜航体的前后移动速度;两个侧向推进器控制潜航体的航向或进行指向性的旋转运动(头部指向目标的全方位观测)；垂向推进器和水平舵机共同控制潜航体的深度，其中舵机还负责调整潜航体的俯仰姿态，当潜航体速度较低时水平舵机的作用效果较弱或可忽略不计。
[0070]本实施例中，控制算法包括如下7个算法:
[0071 ]算法一、潜航体高速航行时的深度控制算法:
[0072]h_rud_set_angIe = di X (depth_depth0)-d2 Xpitching_angle XE;
[0073]depth是潜航体当前深度，通过深度计获取；depthO是潜航体预先设定的目标深度;CU是水平舵机中预设的深度控制系数;d2是水平舵机中预设的俯仰角控制系数;E为角度转换系数;pitching_angle为由惯导获取的潜航体的俯仰角;h_rud_set_angle为计算出的水平舵机的控制角度。
[0074]算法二、潜航体高速航行时航向控制算法如下:
[0075]vct_rud_set_angle = course_para3X[H X(course-courseO)-GXco