一种多推进器结合的水下目标低速抵近与悬停潜航体专利检索-指向性物理专利检索查询-专利查询网

一种多推进器结合的水下目标低速抵近与悬停潜航体

阅读：480发布：2022-02-16

专利汇可以提供一种多推进器结合的水下目标低速抵近与悬停潜航体专利检索，专利查询，专利分析的服务。并且本发明公开了一种多推进器结合的水下目标低速抵近与悬停潜航体，其特征在于，还包括：水平推进器、垂向推进器和两个侧向推进器；水平推进器固定连接在潜航体的正尾部，其轴线与潜航体的轴线重合；在潜航体的浮心位置处开设一个垂直槽道，垂向推进器固定连接在垂直槽道的内壁上；在潜航体航体艏段和艉段分别开设一个水平贯穿潜航体的水平槽道，一个侧向推进器固定连接在艏段水平槽道的内壁上为前侧向推进器，另一个侧向推进器固定连接在艉段水平槽道的内壁上为后侧向推进器，两个侧向推进器的螺旋桨朝向相反。该发明用于解决远程AUV在可疑水下目标周围进行近距离对准观察或处理目标时精确悬停控制的难题。，下面是一种多推进器结合的水下目标低速抵近与悬停潜航体专利的具体信息内容。

权利要求

1.一种多推进器结合的水下目标低速抵近与悬停潜航体，其特征在于，还包括：水平推进器(10)、垂向推进器(11)和两个侧向推进器(12)；
所述水平推进器(10)固定连接在潜航体的正尾部，其轴线与潜航体的轴线重合；
在潜航体的浮心位置处开设一个垂直槽道，所述垂向推进器(11)固定连接在垂直槽道的内壁上；
在潜航体航体艏段和艉段分别开设一个水平贯穿潜航体的水平槽道，一个侧向推进器固定连接在艏段水平槽道的内壁上为前侧向推进器，另一个侧向推进器固定连接在艉段水平槽道的内壁上为后侧向推进器，两个侧向推进器(12)的螺旋桨朝向相反。
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；
无线电台安装在潜航体内部，用来接收外部的操控任务信息。
3.如权利要求2所述的一种多推进器结合的水下目标低速抵近与悬停潜航体，其特征在于，所述控制算法包括如下7个算法：
算法一、潜航体高速航行时的深度控制算法：
h_rud_set_angle＝d1×(depth-depth0)-d2×pitching_angle×E；
depth是潜航体当前深度，通过深度计获取；depth0是潜航体预先设定的目标深度；d1是水平舵机中预设的深度控制系数；d2是水平舵机中预设的俯仰角控制系数；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×[A1×(speed-speed0)-B1×(speed1-speed0)-C1×(speed2-speed0)]
speed是潜航体当前速度，由DVL获得；speed0是潜航体预先设定的速度；speed1是潜航体上一时刻的速度；speed2是潜航体上上时刻的速度；speed_para是预设的速度控制的比例系数；mmotor_speed是计算出的水平推进器的控制速度，A1、B1和C1分别为预设的水平推进器的控制参数；
算法四、潜航体低速航行时航向控制算法如下：
fcmotor_speed＝course_para×[A2×(course-course0)-B2×(course1-course0)-C2×(course2-course0)]/[1+fabs(mmotor_speed)/D1]；
bcmotor_speed＝-course_para×[A3×(course-course0)-B3×(course1-course0)-C3×(course2-course0)]/[1+fabs(mmotor_speed)/D2]；
course0是预设的潜航体目标航向；course是潜航体当前航向；course1是潜航体上一时刻的航向；course2是潜航体上上时刻的航向；course_para是预设的航向控制的比例系数；mmotor_speed是水平推进器的控制速度；A2、B2和C2分别为预设的前侧向推进器的控制参数；D1为预设的前侧向推进器作用因子；fcmotor_speed为计算出的前侧向推进器的控制速度；A3、B3和C3分别为预设的后侧向推进器的控制参数；D2为预设的后侧向推进器作用因子；bcmotor_speed为计算出的后侧向推进器的控制速度；fabs为取绝对值函数；
算法五、潜航体低速航行时深度控制算法如下：
vmotor_speed＝depth_para×[A4×(depth-depth0)-B4×(depth1-depth0)–C4×(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为预设的速度影响因子；A4、B4和C4分别为预设的垂直推进器的控制参数；depth_para1是预设的舵机深度控制系数；depth_para2是预设的舵机俯仰角控制系数；pitching_angle为由惯导获取的潜航体的俯仰角；
vmotor_speed为计算出的垂向推进器的控制速度，h_rud_set_angle2为此时计算出的水平舵机的控制角度；
算法六、潜航体悬停对准时的距离控制的算法如下：
mmotor_speed2＝dis_para×[A5×(distance-distance0)-B5×(distance1-
distance0)-C5×(distance2-distance0)]/[1+fabs(speed/F1)]；
distance0是潜航体离目标点的设定观测安全距离；distance是潜航体离目标点的距离；distance1是潜航体上一时刻离目标点的距离；distance2是潜航体上上时刻离目标点的距离；dis_para是预设的距离控制的比例系数；mmotor_speed2为此时计算出的水平推进器的控制速度；F1为预设的速度影响因子，A5、B5、C5为预设的潜航体距离控制参数；
算法七、潜航体围绕目标进行悬停旋转的控制算法如下：
fcmotor_speed1＝course_para1×[A2×(course-course0)-B2×(course1-course0)-C2×(course2-course0)]/(1+fabs(mmotor_speed3)/D1)；
bcmotor_speed1＝-1200+course_para2×[A3×(course-course0)-B3×(course1-course0)-C3×(course2-course0)])/[1+fabs(mmotor_speed3)/D2]；
course是潜航体当前航向；course1是潜航体上一时刻的航向；course2是潜航体上上时刻的航向；course0是预设的潜航体目标航向；course_para1是预设的航向控制中前侧推进器的比例系数；course_para2是预设的航向控制中后侧推进器的比例系数；mmotor_speed3是此时水平推进器的控制速度；fcmotor_speed1是此时计算出的前侧向推进器的控制速度，bcmotor_speed1是此时计算出的后侧向推进器的控制速度。
当潜航体高速航行到离目标只有40米时，潜航体速度无条件降为3节，从离目标40米到
5米，潜航体速度线性减小到0，且航行的过程中潜航体一直指向目标，在减速过程中采用速度控制算法三和航向控制算法四进行控制；
潜航体围绕目标进行悬停抵近观测采用算法五、算法六、算法七三种算法进行控制，控制流程为：当潜航体运动到离目标只有5米时；距离控制算法六开始启动，控制水平推进器使潜航体在离目标5米处进行机动；深度控制算法五计算出垂向推进器的控制速度，从而控制潜航体在目标深度处进行深度保持；悬停对准算法七计算出两个侧向推进器的控制速度，使潜航体顺时针旋转运动，且头部一直指向目标，保证潜航体头部一直指向目标，悬停的时间根据需要设定。

说明书全文

一种多推进器结合的水下目标低速抵近与悬停潜航体

技术领域

[0001] 本发明涉及水下航行技术领域；具体是一种多推进器结合的水下目标低速抵近与悬停潜航体。

背景技术

[0002] 远程AUV是一种无缆水下机器人，广泛应用于海洋勘探、海洋水文探测、水下工程、水下目标处理等领域。常规的探测型AUV通常只配置了一套水平推进器、一套水平舵机、一套垂直舵机，这就决定了它在水中只能进行运动探测，不能在海洋中某固定处进行悬停控制或对可疑目标进行抵近对准观察。随着远程AUV在军事上的需求越来越多，对远程AUV的运动控制提出了更高的要求。为了拓展远程AUV的作业功能和使用领域，在AUV高速航行时，应当采用尾部水平推进器和舵机控制潜航体运动。当要求潜航体到达某固定目标点进行低速抵近观察或处理目标时，如果只靠尾部主推进器显然是不可能完成任务的。

发明内容

[0003] 有鉴于此，本发明提供了一种多推进器结合的水下目标低速抵近与悬停潜航体，用于解决远程AUV在可疑水下目标周围进行近距离对准观察或处理目标时精确悬停控制的难题。

[0004] 为了达到上述目的，本发明的技术方案为：一种多推进器结合的水下目标低速抵近与悬停潜航体，还包括：水平推进器、垂向推进器和两个侧向推进器。

[0005] 所述水平推进器固定连接在潜航体的正尾部，其轴线与潜航体的轴线重合。

[0006] 在潜航体的浮心位置处开设一个垂直槽道，垂向推进器固定连接在垂直槽道的内壁上。

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

[0008] 进一步地，潜航体包括：壳体、惯性导航设备INS、自动驾驶仪、电源控制单元、垂直舵机、水平舵机、电源控制单元、深度计、多普勒计程仪DVL和无线电台。

[0009] 壳体为流线型壳体。

[0010] INS接收来自DVL的测量数据，并将其与INS内部传感器检测的信息组合计算，得到潜航体的方位信息和姿态信息并向自动驾驶仪输出；

[0011] 自动驾驶仪接收INS传来的方位信息和姿态信息、以及深度计传来的深度信息，由潜航体内置控制算法运算后，输出针对水平推进器、水平舵机、垂直舵机、侧向推进器、垂向推进器的控制指令，控制水平推进器、水平舵机、垂直舵机高速航行到目标点周围，然后低速抵近目标：控制侧向推进器、垂向推进器、水平推进器以及水平舵机，在目标周围进行悬停；

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

[0013] 电源控制单元为整个潜航器提供电能供应。

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

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

[0016] 所述深度计用来测量潜航体当前的深度信息。

[0017] 所述DVL用来测量潜航体前进的速度和离水底的高度，并将测量数据发给INS。

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

[0019] 进一步地，控制算法包括如下7个算法：

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

[0021] h_rud_set_angle＝d1×(depth-depth0)-d2×pitching_angle×E；

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

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

[0024] vct_rud_set_angle＝course_para3×[H×(course-course0)-G×course_speed]

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

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

[0027] mmotor_speed＝speed_para×[A1×(speed-speed0)-B1×(speed1-speed0)-C1×(speed2-speed0)]

[0028] speed是潜航体当前速度，由DVL获得；speed0是潜航体预先设定的速度；speed1是潜航体上一时刻的速度；speed2是潜航体上上时刻的速度；speed_para是预设的速度控制的比例系数；mmotor_speed是计算出的水平推进器的控制速度，A1、B1和C1分别为预设的水平推进器的控制参数；

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

[0030] fcmotor_speed＝course_para×[A2×(course-course0)-B2×(course1-course0)-C2×(course2-course0)]/[1+fabs(mmotor_speed)/D1]；

[0031] bcmotor_speed＝-course_para×[A3×(course-course0)-B3×(course1-course0)-C3×(course2-course0)]/[1+fabs(mmotor_speed)/D2]；

[0032] course0是预设的潜航体目标航向；course是潜航体当前航向；course1是潜航体上一时刻的航向；course2是潜航体上上时刻的航向；course_para是预设的航向控制的比例系数；mmotor_speed是水平推进器的控制速度；A2、B2和C2分别为预设的前侧向推进器的控制参数；D1为预设的前侧向推进器作用因子；fcmotor_speed为计算出的前侧向推进器的控制速度；A3、B3和C3分别为预设的后侧向推进器的控制参数；D2为预设的后侧向推进器作用因子；bcmotor_speed为计算出的后侧向推进器的控制速度；fabs为取绝对值函数；

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

[0034] vmotor_speed＝depth_para×[A4×(depth-depth0)-B4×(depth1-depth0)–C4×(depth2-depth0)]/[1+fabs(speed)/F]；

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

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

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

[0038] mmotor_speed2＝dis_para×[A5×(distance-distance0)-B5×(distance1-distance0)-C5×(distance2-distance0)]/[1+fabs(speed/F1)]；

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

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

[0041] fcmotor_speed1＝course_para1×[A2×(course-course0)-B2×(course1-course0)-C2×(course2-course0)]/(1+fabs(mmotor_speed3)/D1)；

[0042] bcmotor_speed1＝-1200+course_para2×[A3×(course-course0)-B3×(course1-course0)-C3×(course2-course0)])/[1+fabs(mmotor_speed3)/D2]；

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

[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] 壳体1为流线型壳体。

[0060] INS2接收DVL信息并和INS内部传感器信息组合计算，向自动驾驶仪输出潜航体的方位信息和姿态信息。

[0061] 自动驾驶仪3接收INS传来的方位信息和姿态信息、以及深度计传来的深度信息，由潜航体内置控制算法运算后，输出针对水平推进器、水平舵机、垂直舵机、侧向推进器、垂向推进器的控制指令，使得舵机和推进器均执行相应的动作，实现自主航行。

[0062] 电源控制单元为整个潜航器提供电能供应。

[0063] 垂直舵机5安装在潜航体艉段后部去流段，垂直舵机的两块垂直舵面在壳体1上下两边对称安装，垂直舵机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_angle＝d1×(depth-depth0)-d2×pitching_angle×E；

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

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

[0075] vct_rud_set_angle＝course_para3×[H×(course-course0)-G×course_speed]

[0076] course是潜航体当前航向，由INS获得；course0是预设的潜航体的目标航向；course_para3是水平舵机中预设的航向控制系数；H为水平舵机中预设的预设的与航向有关的航向控制参数；G为水平舵机中预设的与航向角速度有关的航向控制参数；course_speed为航向角速度，由INS获得；vct_rud_set_angle为计算出的水平舵机的控制角度。

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

[0078] mmotor_speed＝speed_para×[A1×(speed-speed0)-B1×(speed1-speed0)-C1×(speed2-speed0)]。

[0079] speed是潜航体当前速度，由DVL获得；speed0是潜航体预先设定的速度；speed1是潜航体上一时刻的速度；speed2是潜航体上上时刻的速度；speed_para是预设的速度控制的比例系数；mmotor_speed是计算出的水平推进器的控制速度，A1、B1和C1分别为预设的水平推进器的控制参数。

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

[0081] fcmotor_speed＝course_para×[A2×(course-course0)-B2×(course1-course0)-C2×(course2-course0)]/[1+fabs(mmotor_speed)/D1]。

[0082] bcmotor_speed＝-course_para×[A3×(course-course0)-B3×(course1-course0)-C3×(course2-course0)]/[1+fabs(mmotor_speed)/D2]。

[0083] course0是预设的潜航体目标航向；course是潜航体当前航向；course1是潜航体上一时刻的航向；course2是潜航体上上时刻的航向；course_para是预设的航向控制的比例系数；mmotor_speed是水平推进器的控制速度；A2、B2和C2分别为预设的前侧向推进器的控制参数；D1为预设的前侧向推进器作用因子；fcmotor_speed为计算出的前侧向推进器的控制速度；A3、B3和C3分别为预设的后侧向推进器的控制参数；D2为预设的后侧向推进器作用因子；bcmotor_speed为计算出的后侧向推进器的控制速度；fabs为取绝对值函数。

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

[0085] vmotor_speed＝depth_para×[A4×(depth-depth0)-B4×(depth1-depth0)–C4×(depth2-depth0)]/[1+fabs(speed)/F]；

[0086] h_rud_set_angle＝depth_para1×(depth-depth0)-depth_para2×pitching_angle×π/180。

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

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

[0089] mmotor_speed2＝dis_para×[A5×(distance-distance0)-B5×(distance1-distance0)-C5×(distance2-distance0)]/[1+fabs(speed/F1)]；

[0090] distance0是潜航体离目标点的设定观测安全距离；distance是潜航体离目标点的距离；distance1是潜航体上一时刻离目标点的距离；distance2是潜航体上上时刻离目标点的距离；dis_para是预设的距离控制的比例系数；mmotor_speed2为此时计算出的水平推进器的控制速度；F1为预设的速度影响因子，A5、B5、C5为预设的潜航体距离控制参数。

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

[0092] fcmotor_speed1＝course_para1×[A2×(course-course0)-B2×(course1-course0)-C2×(course2-course0)]/(1+fabs(mmotor_speed3)/D1)；

[0093] bcmotor_speed1＝-1200+course_para2×[A3×(course-course0)-B3×(course1-course0)-C3×(course2-course0)])/[1+fabs(mmotor_speed3)/D2]。

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

[0095] 当潜航体高速航行到离目标只有40米时，潜航体速度无条件降为3节，从离目标40米到5米，潜航体速度线性减小到0，且航行的过程中潜航体一直指向目标，在减速过程中采用速度控制算法三和航向控制算法四，在减速的过程中，深度控制算法五根据速度的大小动态调整水平舵机和垂向推进器的控制比例，速度越低，垂向推进器的控制比例越大。

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

[0097] 综上，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

标题	发布/更新时间	阅读量
一种指向性LED发光件	2020-05-14	911
高增益指向性天线设备	2020-05-14	667
指向性录音模组	2020-05-11	399
指向性天线	2020-05-11	813
平面指向性天线	2020-05-12	113
一种指向性MEMS麦克风	2020-05-14	623
高指向性光源装置	2020-05-14	858
指向性控制方法及装置	2020-05-15	367
具指向性天线装置	2020-05-15	535
指向性MEMS MIC	2020-05-12	987

一种多推进器结合的水下目标低速抵近与悬停潜航体

一种多推进器结合的水下目标低速抵近与悬停潜航体

技术领域

背景技术

发明内容

具体实施方式

该功能需要专业版企业版VIP权限，您可以：