[发明专利]一种多推进器结合的水下目标低速抵近与悬停潜航体

权利要求书

1.一种多推进器结合的水下目标低速抵近与悬停潜航体，其特征在于，还包括：水平推进器(10)、垂向推进器(11)和两个侧向推进器(12)；

所述水平推进器(10)固定连接在潜航体的正尾部，其轴线与潜航体的轴线重合；

在潜航体的浮心位置处开设一个垂直槽道，所述垂向推进器(11)固定连接在垂直槽道的内壁上；

在潜航体航体艏段和艉段分别开设一个水平贯穿潜航体的水平槽道，一个侧向推进器固定连接在艏段水平槽道的内壁上为前侧向推进器，另一个侧向推进器固定连接在艉段水平槽道的内壁上为后侧向推进器，两个侧向推进器(12)的螺旋桨朝向相反；

控制算法包括如下7个算法：

算法一、潜航体高速航行时的深度控制算法：

h_rud_set_angle＝d₁×(depth-depth0)-d₂×pitching_angle×E；

depth是潜航体当前深度，通过深度计获取；depth0是潜航体预先设定的目标深度；d₁是水平舵机中预设的深度控制系数；d₂是水平舵机中预设的俯仰角控制系数；E为角度转换系数；pitching_angle为由惯导获取的潜航体的俯仰角；h_rud_set_angle为计算出的水平舵机的控制角度；

算法二、潜航体高速航行时航向控制算法如下：

vct_rud_set_angle＝course_para3×[H×(course-course0)-G×course_speed]course是潜航体当前航向，由惯性导航设备INS获得；course0是预设的潜航体的目标航向；course_para3是水平舵机中预设的航向控制系数；H为水平舵机中预设的预设的与航向有关的航向控制参数；G为水平舵机中预设的与航向角速度有关的航向控制参数；course_speed为航向角速度，由惯性导航设备INS获得；vct_rud_set_angle为计算出的垂直舵机的控制角度；

算法三、潜航体低速航行的速度控制算法为：

mmotor_speed＝speed_para×[A₁×(speed-speed0)-B₁×(speed1-speed0)-C₁×(speed2-speed0)]

speed是潜航体当前速度，由多普勒计程仪DVL获得；speed0是潜航体预先设定的速度；speed1是潜航体上一时刻的速度；speed2是潜航体上上时刻的速度；speed_para是预设的速度控制的比例系数；mmotor_speed是计算出的水平推进器的控制速度，A₁、B₁和C₁分别为预设的水平推进器的控制参数；

算法四、潜航体低速航行时航向控制算法如下：

fcmotor_speed＝course_para×[A₂×(course-course0)-B₂×(course1-course0)-C₂×(course2-course0)]/[1+fabs(mmotor_speed)/D₁]；

bcmotor_speed＝-course_para×[A₃×(course-course0)-B₃×(course1-course0)-C₃×(course2-course0)]/[1+fabs(mmotor_speed)/D₂]；

course0是预设的潜航体目标航向；course是潜航体当前航向；course1是潜航体上一时刻的航向；course2是潜航体上上时刻的航向；course_para是预设的航向控制的比例系数；mmotor_speed是水平推进器的控制速度；A₂、B₂和C₂分别为预设的前侧向推进器的控制参数；D₁为预设的前侧向推进器作用因子；fcmotor_speed为计算出的前侧向推进器的控制速度；A₃、B₃和C₃分别为预设的后侧向推进器的控制参数；D₂为预设的后侧向推进器作用因子；bcmotor_speed为计算出的后侧向推进器的控制速度；fabs为取绝对值函数；

算法五、潜航体低速航行时深度控制算法如下：

vmotor_speed＝depth_para×[A₄×(depth-depth0)-B₄×(depth1-depth0)–C₄×(depth2-depth0)]/[1+fabs(speed)/F]；

h_rud_set_angle2＝depth_para1×(depth-depth0)-depth_para2×pitching_angle×π/180；

depth0是预设的潜航体目标深度；depth是潜航体当前深度；depth1是潜航体上一时刻的深度；depth2是潜航体上上时刻的深度；depth_para是预设的潜航体深度控制的比例系数；speed是潜航体当前速度；fabs为取绝对值函数，F为预设的速度影响因子；A₄、B₄和C₄分别为预设的垂直推进器的控制参数；depth_para1是预设的舵机深度控制系数；depth_para2是预设的舵机俯仰角控制系数；pitching_angle为由惯导获取的潜航体的俯仰角；vmotor_speed为计算出的垂向推进器的控制速度，h_rud_set_angle2为此时计算出的水平舵机的控制角度；

算法六、潜航体悬停对准时的距离控制的算法如下：

mmotor_speed2＝dis_para×[A₅×(distance-distance0)-B₅×(distance1-distance0)-C₅×(distance2-distance0)]/[1+fabs(speed/F₁)]；

distance0是潜航体离目标点的设定观测安全距离；distance是潜航体离目标点的距离；distance1是潜航体上一时刻离目标点的距离；distance2是潜航体上上时刻离目标点的距离；dis_para是预设的距离控制的比例系数；mmotor_speed2为此时计算出的水平推进器的控制速度；F₁为预设的速度影响因子，A₅、B₅、C₅为预设的潜航体距离控制参数；

算法七、潜航体围绕目标进行悬停旋转的控制算法如下：

fcmotor_speed1＝course_para1×[A₂×(course-course0)-B₂×(course1-course0)-C₂×(course2-course0)]/(1+fabs(mmotor_speed3)/D₁)；

bcmotor_speed1＝-1200+course_para2×[A₃×(course-course0)-B₃×(course1-course0)-C₃×(course2-course0)])/[1+fabs(mmotor_speed3)/D₂]；

course是潜航体当前航向；course1是潜航体上一时刻的航向；course2是潜航体上上时刻的航向；course0是预设的潜航体目标航向；course_para1是预设的航向控制中前侧推进器的比例系数；course_para2是预设的航向控制中后侧推进器的比例系数；mmotor_speed3是此时水平推进器的控制速度；fcmotor_speed1是此时计算出的前侧向推进器的控制速度，bcmotor_speed1是此时计算出的后侧向推进器的控制速度；

当潜航体高速航行到离目标只有40米时，潜航体速度无条件降为3节，从离目标40米到5米，潜航体速度线性减小到0，且航行的过程中潜航体一直指向目标，在减速过程中采用算法三和算法四进行控制；

潜航体围绕目标进行悬停抵近观测采用算法五、算法六、算法七三种算法进行控制，控制流程为：当潜航体运动到离目标只有5米时；算法六开始启动，控制水平推进器使潜航体在离目标5米处进行机动；算法五计算出垂向推进器的控制速度，从而控制潜航体在目标深度处进行深度保持；算法七计算出两个侧向推进器的控制速度，使潜航体顺时针旋转运动，且头部一直指向目标，保证潜航体头部一直指向目标，悬停的时间根据需要设定。

2.如权利要求1所述的一种多推进器结合的水下目标低速抵近与悬停潜航体，其特征在于，所述潜航体包括：壳体(1)、惯性导航设备INS(2)、自动驾驶仪(3)、第一电源控制单元(4)、垂直舵机(5)、水平舵机(6)、第二电源控制单元(7)、深度计(8)、多普勒计程仪DVL(9)和无线电台(13)；

所述壳体(1)为流线型壳体；

所述惯性导航设备INS(2)接收来自多普勒计程仪DVL的测量数据，并将其与惯性导航设备INS内部传感器检测的信息组合计算，得到潜航体的方位信息和姿态信息并向自动驾驶仪输出；

所述自动驾驶仪(3)接收惯性导航设备INS传来的方位信息和姿态信息、以及深度计(8)传来的深度信息，由潜航体内置控制算法运算后，输出针对水平推进器(10)、水平舵机(6)、垂直舵机(5)、侧向推进器(12)、垂向推进器(11)的控制指令，控制水平推进器(10)、水平舵机(6)、垂直舵机(5)高速航行到目标点周围，然后低速抵近目标：控制侧向推进器(12)、垂向推进器(11)、水平推进器(10)以及水平舵机(6)，在目标周围进行悬停；

在低速抵近目标时：控制水平推进器(10)以控制潜航体的前后移动速度；控制侧向推进器(12)和垂向推进器(11)以控制潜航体的航向或进行指向性的旋转运动；控制垂向推进器(11)和水平舵机(6)共同控制潜航体的深度；

所述第一电源控制单元(4)为整个潜航器提供电能供应；

所述垂直舵机(5)安装在潜航体艉段后部去流段，垂直舵机的两块垂直舵面在壳体(1)上下两边对称安装，垂直舵机(5)的驱动电机和控制器安装在壳体内部，垂直舵机(5)的控制器接收控制指令，控制垂直舵机(5)的驱动电机转动，驱动垂直舵面运行到指定角度；

所述水平舵机(6)安装在潜航体艉段后部去流段，水平舵机(6)的两块水平舵面在壳体左右两边对称安装，水平舵机(6)的驱动电机和控制器安装在壳体内部；水平舵机(6)的控制器接收控制指令，控制水平舵机(6)的驱动电机转动，驱动水平舵面运行到指定角度；

所述深度计(8)用来测量潜航体当前的深度信息；

所述多普勒计程仪DVL(9)用来测量潜航体前进的速度和离水底的高度，并将测量数据发给惯性导航设备INS；

无线电台安装在潜航体内部，用来接收外部的操控任务信息。