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

摘要

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

主权项

1.一种多推进器结合的水下目标低速抵近与悬停潜航体，其特征在于，还包括：水平推进器(10)、垂向推进器(11)和两个侧向推进器(12)；所述水平推进器(10)固定连接在潜航体的正尾部，其轴线与潜航体的轴线重合；在潜航体的浮心位置处开设一个垂直槽道，所述垂向推进器(11)固定连接在垂直槽道的内壁上；在潜航体航体艏段和艉段分别开设一个水平贯穿潜航体的水平槽道，一个侧向推进器固定连接在艏段水平槽道的内壁上为前侧向推进器，另一个侧向推进器固定连接在艉段水平槽道的内壁上为后侧向推进器，两个侧向推进器(12)的螺旋桨朝向相反；控制算法包括如下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米处进行机动；算法五计算出垂向推进器的控制速度，从而控制潜航体在目标深度处进行深度保持；算法七计算出两个侧向推进器的控制速度，使潜航体顺时针旋转运动，且头部一直指向目标，保证潜航体头部一直指向目标，悬停的时间根据需要设定。