CN116674724A - 一种具有辅助自动驾驶功能的蛙人运载器及辅助驾驶方法 - Google Patents

Abstract

本发明公开了一种具有辅助自动驾驶功能的蛙人运载器及辅助驾驶方法，属于潜水运载器领域，所述的运载器包括运载器壳体、仪表显示系统、动力推进系统、导航定位系统、操控系统五部分组成。运载器壳体用于其他系统的安装及蛙人的身体支撑；仪表显示系统用于显示状态信息、导航路径规划等；动力推进系统用于为运载器的行进提供动力；导航定位系统用于航向航速定位测算；操控系统用于控制运载器的上升、下潜、左转、右转等姿态。本运载器有能够在水下进行自动导航。可通过人工驾驶/自动驾驶按规划路径到达指定目标点。

Description

一种具有辅助自动驾驶功能的蛙人运载器及辅助驾驶方法

技术领域

本发明属于潜水运载器领域，更具体的说涉及一种具有辅助自动驾驶功能的蛙人运载器及辅助驾驶方法。

背景技术

国内现有蛙人运载器分为两类，其一为依靠身体姿态调整航姿，其二为使用连杆操控机械舵，长距离和高速航行对蛙人体力消耗巨大，影响蛙人水下骑行。现有技术存在以下缺点：①正常情况下，蛙人通过手上的力量扳动操作把手，以机械力传导的方式使连杆驱动机械舵控制运载器航姿。但高速航行时，舵板受水流冲击大，蛙人难以克服舵板受到的水流冲击扳动操作把手，甚至出现扳不动的情况；②蛙人在水下骑行时，受水流影响，运载器姿态无法保持，容易出现骑行过深、方向偏差大的情况，特别是骑行深度未保持好出现急速下沉会导致发生事故；③蛙人在水下骑行时，无导航容易迷失方向。

发明内容

本发明提出了一种具有辅助自动驾驶功能的蛙人运载器，采用“摇杆+电舵+推进器”方式控制航姿，操作方便省力；通过导航定位系统、动力推进系统、仪表显示系统相互配合完成定向定深，实现自动驾驶航行功能，能为蛙人节省有限的氧气和体力，降低操控难度，有效避免蛙人误操作的情况，最大限度保证水下蛙人航行安全；具有高精度组合导航系统，能实现水面水下精准导航，能实现路径规划功能，可有效避免蛙人迷失方向的情况。

本运载器有能够在水下进行自动导航。可通过人工驾驶/自动驾驶按规划路径到达指定目标点。

为了实现上述目的，本申请是采用以下技术方案实现的：所述的运载器由运载器壳体、仪表显示系统、动力推进系统、导航定位系统、操控系统组成；

运载器壳体用于其他系统的安装及蛙人的身体支撑；

仪表显示系统用于显示状态信息、导航路径规划；

动力推进系统用于为运载器的行进提供动力；

导航定位系统用于航向航速定位测算；

操控系统用于控制运载器的上升、下潜、左转、右转姿态；

所述的仪表显示系统、动力推进系统、导航定位系统、操控系统都安装于运载器壳体内部，导航定位系统与仪表显示系统连接，操控系统与仪表显示系统连接，动力推进系统与仪表显示系统连接。

进一步地，所述的动力推进系统包括电池电器舱、推进电机、垂直舵机、水平舵机，所述的电池电器舱中包含有动力电池和电机驱动器，所述的动力电池与电机驱动器连接，电机驱动器与推进电机、垂直舵机、水平舵机连接，用于对电机的控制。

进一步地，所述的操控系统包括摇杆、垂直舵板、水平舵板、螺旋桨、采集板，所述的摇杆内安装有霍尔传感器，传感器的输出端与采集板连接，采集板输出端与电机驱动器连接，所述螺旋桨与推进电机输出轴连接，垂直舵板与垂直舵机输出轴连接，水平舵板与水平舵机输出轴连接。

进一步地，所述的运载器通过摇杆进行手动驾驶。

进一步地，所述的运载器通过导航系统进行自动驾驶。

再一方面，一种具有辅助自动驾驶功能的蛙人运载器辅助驾驶方法，适用于所述的运载器，所述的辅助驾驶方法:

S1由潜水员通过仪表显示系统输入按钮输入目标点的经纬度坐标(x,y)与目标深度h；

S2仪表显示系统从导航定位系统实时采集运载器的实时经纬度坐标(x_n,y_n)，航向角φ_n，深度h_n，n为当前时刻；

S3仪表显示系统中央处理器实时计算运载器与目标点之间的方位角:

其中，Haversine为半正矢公式，用来计算两点间地理坐标距离；计算航向误差：Δφ_n＝φ_n-φ'_n；深度误差：Δh_n＝h-h_n；

S4根据PID原理，计算航向调节量：

计算深度调节量：

其中，K_P、K_i、K_d为航向PID的比例、积分、微分参数，K'_p、K'_i、K'_d为深度PID的比例、积分、微分参数，需要通过试验来调整确定。

S5将航向调节量与深度调节量分别按比例映射成舵板开角；垂直舵板开角：θ_v＝f(α)；水平舵板开角：θ_h＝f(β)；函数f映射关系与实际舵板最大开度，调节量的最大限定值相关；

S6将θ_v、θ_h输入舵板电机控制器，控制舵板达到θ_v、θ_h开角；

S7重复上述第S2到第S6步骤，达到定向定深功能。

本发明有益效果：

(1)相较于现有蛙人运载器存在的航向指示不精准的问题，本发明提出了一种具有辅助自动驾驶功能的蛙人运载器，其具有高精度组合导航系统，能实现水面水下精准导航，能实现路径规划功能，可有效避免蛙人迷失方向的情况。

(2)相较于现有蛙人运载器存在的无姿态辅助控制功能的问题，本发明提出了一种具有辅助自动驾驶功能的蛙人运载器，其具有辅助自动驾驶等功能，导航定位系统、动力推进系统、仪表显示系统相互配合完成定向定深，实现自动驾驶航行功能，可有效避免蛙人误操作的情况，减少体力消耗，最大限度保证水下蛙人航行安全。

(3)相较于现有蛙人运载器存在的高速水流冲击导致舵板操控困难的问题，本发明提出了一种具有辅助自动驾驶功能的蛙人运载器，采用“摇杆+电舵+推进器”方式控制航姿，操作方便省力，减少蛙人体力消耗。

附图说明

图1为本发明运载器组成图；

图2为本发明摇杆侧视图；

图3为本发明舵板控制图；

图4为本发明运载器控制图；

图5为本发明人工驾驶流程图；

图6为本发明自动驾驶流程图。

图中、1、天线；2、导航舱；3、显示仪表；4、运载器壳体；5、摇杆；6、电池电器舱；7、推进电机；8、螺旋桨；9、垂直舵机；10、垂直舵板；11、水平舵板；12、水平舵机。

具体实施方式

以下将以附图公开本公开的实施方式，为明确说明起见，许多实务上的细节将在以下叙述中一并说明。然而，应了解到，这些实务上的细节不应用以限制本公开。也就是说，在本公开内容部分实施方式中，这些实务上的细节是非必要的。此外，为简化附图起见，一些现有惯用的结构与元件在附图中将以简单示意的方式示出的。

本发明中的思路为：该运载器主要用于运载两名潜水员进行水下作业，潜水员通过运载器推动前进，水下航行时能够最大限度地节省潜水员体力。使用运载器时，可将使用场景的地图导入到显示仪表系统，然后通过仪表显示系统设置目标点、深度、速度完成航行路径规划，潜水员可通过人工驾驶/自动驾驶按规划路径到达指定目标点。可切换使用，节省蛙人体力；具备导航定位及路径规划功能，蛙人可根据定位及规划路径前行，天线露出水面时，自动定位，纠正航向；具备显示屏幕，旋钮选择/输入，按钮确认/模式切换，在水下观察清晰，操作简单方便；具备摇杆-十字舵操控系统，摇杆前后左右转动控制运载器的上升下潜左右转向，操作简单。

在仪表显示系统上导入地图，输入目标点、期待深度、档位，仪表显示系统会生成路线图，开启“自动驾驶”模式，运载器可以按输入档位将蛙人输送至指定目标地点；输入目标点，仪表显示系统会生成路线图，开启“人工驾驶”，蛙人操控摇杆，摇杆左转，运载器左转；摇杆右转，运载器右转；摇杆前倾，运载器头部下潜；摇杆后倾，运载器头部上浮，按路线图操控摇杆，调节速度，最终到达指定地点。

所述的运载器由运载器壳体、仪表显示系统、动力推进系统、导航定位系统、操控系统组成；

运载器壳体用于其他系统的安装及蛙人的身体支撑；

仪表显示系统用于显示状态信息、导航路径规划；

动力推进系统用于为运载器的行进提供动力；

导航定位系统用于航向航速定位测算；

操控系统用于控制运载器的上升、下潜、左转、右转姿态；

所述的仪表显示系统、动力推进系统、导航定位系统、操控系统都安装于运载器壳体内部，导航定位系统与仪表显示系统连接，操控系统与仪表显示系统连接，动力推进系统与仪表显示系统连接。

所述的动力推进系统包括电池电器舱、推进电机、垂直舵机、水平舵机，所述的电池电器舱中包含有动力电池和电机驱动器，所述的动力电池与电机驱动器连接，电机驱动器与推进电机、垂直舵机、水平舵机连接，用于对电机的控制。

所述的操控系统包括摇杆、垂直舵板、水平舵板、螺旋桨、采集板，所述的摇杆内安装有霍尔传感器，传感器的输出端与采集板连接，采集板输出端与电机驱动器连接，所述螺旋桨与推进电机输出轴连接，垂直舵板与垂直舵机输出轴连接，水平舵板与水平舵机输出轴连接。

所述的运载器通过摇杆进行手动驾驶。

潜水员操作摇杆，摇杆前/后/左/右控制十字舵的舵板转动，仪表显示系统采集摇杆转动方向及角度的信息，摇杆向前转动对应控制水平舵机顺时针旋转，摇杆向后转动对应控制水平舵机逆时针旋转，摇杆向左转动对应控制垂直舵机顺时针旋转，摇杆向右转动对应控制垂直舵机逆时针旋转，再通过公式(1)计算出舵板当前转角，从而控制运载器姿态变化实现下潜、上浮、左转、右转；

所述的运载器通过导航系统进行自动驾驶。辅助自动驾驶功能主要由导航定位系统、动力推进系统、仪表显示系统相互配合完成定向定深。所述的辅助驾驶方法:

S1由潜水员通过仪表显示系统输入按钮输入目标点的经纬度坐标(x,y)与目标深度h；

S2仪表显示系统从导航定位系统实时采集运载器的实时经纬度坐标(x_n,y_n)，航向角φ_n，深度h_n，n为当前时刻；

S3仪表显示系统中央处理器实时计算运载器与目标点之间的方位角:

其中，Haversine为半正矢公式，用来计算两点间地理坐标距离；计算航向误差：Δφ_n＝φ_n-φ'_n；深度误差：Δh_n＝h-h_n；

S4根据PID原理，计算航向调节量：

计算深度调节量：

其中，K_P、K_i、K_d为航向PID的比例、积分、微分参数，K'_p、K'_i、K'_d为深度PID的比例、积分、微分参数，需要通过试验来调整确定。

S5将航向调节量与深度调节量分别按比例映射成舵板开角；垂直舵板开角：θ_v＝f(α)；水平舵板开角：θ_h＝f(β)；函数f映射关系与实际舵板最大开度，调节量的最大限定值相关；

S6将θ_v、θ_h输入舵板电机控制器，控制舵板达到θ_v、θ_h开角；

S7重复上述第S2到第S6步骤，达到定向定深功能。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：闪存盘、只读存储器(read-only memory，ROM)、随机存取器(randomaccessmemory，RAM)、磁盘或光盘等。

应当理解，以上借助优选实施例对本发明的技术方案进行的详细说明是示意性的而非限制性的。本领域的普通技术人员在阅读本发明说明书的基础上可以对各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (6)

1.一种具有辅助自动驾驶功能的蛙人运载器，其特征在于：所述的运载器由运载器壳体、仪表显示系统、动力推进系统、导航定位系统、操控系统组成；

运载器壳体用于其他系统的安装及蛙人的身体支撑；

仪表显示系统用于显示状态信息、导航路径规划；

动力推进系统用于为运载器的行进提供动力；

导航定位系统用于航向航速定位测算；

操控系统用于控制运载器的上升、下潜、左转、右转姿态；

所述的仪表显示系统、动力推进系统、导航定位系统、操控系统都安装于运载器壳体内部，导航定位系统与仪表显示系统连接，操控系统与仪表显示系统连接，动力推进系统与仪表显示系统连接。

2.根据权利要求1所述的一种具有辅助自动驾驶功能的蛙人运载器，其特征在于：所述的动力推进系统包括电池电器舱、推进电机、垂直舵机、水平舵机，所述的电池电器舱中包含有动力电池和电机驱动器，所述的动力电池与电机驱动器连接，电机驱动器与推进电机、垂直舵机、水平舵机连接，用于对电机的控制。

3.根据权利要求1所述的一种具有辅助自动驾驶功能的蛙人运载器，其特征在于：所述的操控系统包括摇杆、垂直舵板、水平舵板、螺旋桨、采集板，所述的摇杆内安装有霍尔传感器，传感器的输出端与采集板连接，采集板输出端与电机驱动器连接，所述螺旋桨与推进电机输出轴连接，垂直舵板与垂直舵机输出轴连接，水平舵板与水平舵机输出轴连接。

4.根据权利要求1所述的一种具有辅助自动驾驶功能的蛙人运载器，其特征在于：所述的运载器通过摇杆进行手动驾驶。

5.根据权利要求1所述的一种具有辅助自动驾驶功能的蛙人运载器，其特征在于：所述的运载器通过导航系统进行自动驾驶。

6.一种具有辅助自动驾驶功能的蛙人运载器辅助驾驶方法，适用于权利要求1-5中任一项所述的运载器，其特征在于：所述的辅助驾驶方法:

S1由潜水员通过仪表显示系统输入按钮输入目标点的经纬度坐标(x,y)与目标深度h；

S2仪表显示系统从导航定位系统实时采集运载器的实时经纬度坐标(x_n,y_n)，航向角φ_n，深度h_n，n为当前时刻；

S3仪表显示系统中央处理器实时计算运载器与目标点之间的方位角:

其中，Haversine为半正矢公式，用来计算两点间地理坐标距离；计算航向误差：Δφ_n＝φ_n-φ'_n；深度误差：Δh_n＝h-h_n；

S4根据PID原理，计算航向调节量：

计算深度调节量：

其中，K_P、K_i、K_d为航向PID的比例、积分、微分参数，K'_p、K'_i、K'_d为深度PID的比例、积分、微分参数，需要通过试验来调整确定。

S5将航向调节量与深度调节量分别按比例映射成舵板开角；垂直舵板开角：θ_v＝f(α)；水平舵板开角：θ_h＝f(β)；函数f映射关系与实际舵板最大开度，调节量的最大限定值相关；

S6将θ_v、θ_h输入舵板电机控制器，控制舵板达到θ_v、θ_h开角；S7重复上述第S2到第S6步骤，达到定向定深功能。