CN105667743A - 一种水下滑翔器的控制方法 - Google Patents

Abstract

本发明公开了一种水下滑翔器的控制方法，首先进行任务下载，并进行部件测试和任务参数设置；其次进行任务控制，使水下滑翔器接收任务指令并进行相应的动作，同时进行航向控制，将水下滑翔器调整到正确的航行角度；所述任务控制的方法为根据任务的优先级为之分别设定扫描的间隔时间；每到达相应的任务的间隔时间时，自动执行该间隔时间所对应的任务；所述航向控制的方法为以所设定的航向角度为0°建立新坐标系，并在新坐标系中计对当前航向角度进行角度范围划分，根据不同的角度范围控制水下滑翔器的航向。本发明使得控制中心运行速度最大化，且使得航向的控制效率更高。

Description

一种水下滑翔器的控制方法

技术领域

本发明涉及水下滑翔器领域，具体涉及一种水下滑翔器的控制方法。

背景技术

水下滑翔器在水下工作时，需同时控制两方面的内容，即进行任务控制和航向控制。

对于任务控制，水下滑翔器在一次下水后，需能够长时间的按照设定的任务自动地进行作业，所以水下滑翔器控制器需要实时处理多项任务，同时由于嵌入式控制器本身的限制，其运行内存和存储内存都收到了极大的限制，控制效率过低。对于多任务的控制方法，现有技术中常用的方法是将多任务加入嵌入式操作系统，但由于嵌入式操作系统自身的运行需要消耗控制器一定的存储容量和运行容量，反而更加占用了运行内存和储存内存。

对于航向控制，水下滑翔器的航向控制方法由航向决策方法和航向运动方法构成。在进行航向决策时需要判别实际航向与设定航向之间的差值从而决定航向如何控制，由于航向角度的输出形式是以0°到360°的角度输出，在对360°的航向角度进行航向判别时不能用简单的角度大小比较进行判断，必须分区间分别进行，但是这样会带来整个程序的复杂性。而且由于角度范围过广，判断时往往需要划分多个区域进行判断，这样会带来判断方法的复杂化。

另外，水下滑翔器在开始工作之前，需进行任务下载，并进行部件测试和调试，在此步骤中，常用的水下滑翔器任务单片机进行任务程序下载的方法是采用JTAG下载的方式，该方法无法适合长距离下载。并且在现有技术中，接收任务下载和测试调试命令时，由于传输方法的限制，需要预留多个端口，不能满足水下滑翔器水下耐压与水密性要求。

发明内容

针对现有技术的不足，本发明公开了一种水下滑翔器的控制方法。

本发明的技术方案如下：

一种水下滑翔器的控制方法，

首先进行任务下载，并进行部件测试和任务参数设置；

其次进行任务控制，使水下滑翔器接收任务指令并进行相应的动作，同时进行航向控制，将水下滑翔器调整到正确的航行角度；

所述任务控制的方法为，将任务分为阶段性任务和实时性任务；所述阶段性任务为只需在某一时间段执行的任务；所述实时性任务为在水下滑翔器的整个工作时间内都需要实时处理的任务；根据阶段性任务和实时性任务的优先级为之分别设定扫描的间隔时间；在一个任务扫描周期内，每到达相应的任务的间隔时间时，自动执行该间隔时间所对应的任务，完成水下滑翔器的动作；

所述航向控制的方法为，通过电子罗盘获得水下滑翔器的当前航向角度，以所设定的航向角度为0°建立新坐标系，并在新坐标系中计算水下滑翔器的当前航向角度；之后对新坐标系中的当前航向角度进行角度范围划分，根据不同的角度范围以及水下滑翔器的上浮或者下潜状态，控制水下滑翔器的姿态调节装置中的滑块位置，进而控制水下滑翔器的航向。

其进一步的技术方案为：所述实时性任务包括实时性紧急状态监测任务和实时性存储任务；所述实时性紧急状态监测任务为监测水下滑翔器下潜是否超时、超深以及上浮是否超时；所述实时性存储任务为定时记录水下滑翔器中的传感器的数值；在一个扫描周期中，间隔时间t1进行阶段性任务，间隔时间t2进行实时性紧急状态监测任务，间隔时间t3进行实时性存储任务；任务控制的方法具体包括以下步骤：

步骤11、扫描周期开始，控制器计时；

步骤12、判断时间t1是否到；如果是，则开始阶段性任务，如果否，则进入步骤13；

步骤13、判断时间t2是否到；如果是，则开始实时性紧急状态监测任务；如果否，则进入步骤14；

步骤14、判断时间t3是否到，如果是，则开始实时性存储任务；如果否，则进入步骤15；

步骤15、扫描周期结束。

其进一步的技术方案为：进行所述阶段性任务的具体步骤包括：

步骤2a、判断下水后等待任务标志是否置位，如果是，则进入步骤2b，如果否，则进入步骤2d；

步骤2b、执行下水后等待任务；

步骤2c、清除下水后等待任务标志，置放通信任务标志；

步骤2d、判断通信任务标志是否置位，如果是，则进入步骤2e，如果否，则进入步骤2i；

步骤2e、姿态调节装置开始工作，水下滑翔器进入通信状态，并进入步骤2f；

步骤2f、发送位置信息，并进入步骤2g；

步骤2g、判断是否收到命令信号，如果是，则进入步骤2h，如果否，则进入步骤2i；

步骤2h、根据所收到的命令信号进行水下滑翔器的相关动作；

步骤2i、判断通信等待任务标志是否置位，如果是，则进入步骤2j，如果否则进入步骤2l；

步骤2j、执行通信等待任务；

步骤2k、清除通信等待任务标志，置放潜伏作业任务标志；

步骤2l、判断潜伏作业任务标志是否置位，如果是，则进入步骤2m，如果否，则进入步骤13；

步骤2m、水下滑翔器上浮或者下潜，之后进入步骤13。

其进一步的技术方案为：在所述步骤2h中，所收到的命令信号包括：

放平命令，若收到放平命令，则姿态调节装置进行放平动作；

重发位置信息命令，若收到重发位置信息命令，则重发位置信息；

发送传感器参数命令，若收到发送传感器参数命令，则发送传感器信息；

下潜命令，若收到下潜命令，则姿态调节装置进行放平动作，且清除通信任务标志，放置通信等待任务标志。

其进一步的技术方案为：在所述步骤2m中，如果为上浮态，则判断是否到上浮深度，如果是，则水下滑翔器回油；如果为下潜态，则判断是否到下潜深度，如果是，则水下滑翔器出油。

其进一步的技术方案为：进行所述实时性紧急状态监测任务的具体步骤包括：

步骤3a、判断下潜是否超深，如果是，则进入步骤3c，如果否，则进入步骤3b；

步骤3b、判断下潜是否超时，如果是，则进入步骤3c，如果否，则进入步骤3d；

步骤3c、紧急上浮，之后进入步骤3d；

步骤3d、判断上浮是否超时，如果是，则进入步骤3e，如果否，则进入步骤14；

步骤3e、抛载，之后进入步骤14。

其进一步的技术方案为：所述航向控制具体包括下述步骤：

步骤21、航向采集，通过电子罗盘来获得水下滑翔器的当前航向角ANGI；水下滑翔器的当前航向角ANGI为0～360°；

步骤22、航向坐标系变换，以所设定航向角DANG为0°，重新建立坐标系；航向坐标系变换方法，即经过坐标变换的航向角度ANGO、当前航向角ANGI以及所设定航向角DANG的关系为：

$ANGO=\left\{\begin{array}{cc}ANGI-DANG& (DANG\&le;ANGI<360)\\ 360-(DANG-ANGI)& (0\&le;ANGI<DANG)\end{array}\right.;$

步骤23、经过坐标变换的航向角状态判断，在步骤22所建立的坐标系中，判断经过坐标变换的航向角度ANGO的状态；相对所设定航向角DANG，即0°角，航向角度ANGO的状态为偏左、正中或者偏右；

步骤24、水下滑翔器的状态判断，判断此时水下滑翔器处于上浮态或者下潜态，并结合步骤23所得到的航向角度ANGO的状态，向电机控制器发出纠正命令；如果处于上浮态且航向角度ANGO偏左，则发出向右纠正命令；如果处于下潜态且航向角度ANGO偏左，则发出向左纠正命令；如果处于上浮态或者下潜态且航向角度ANGO正中，则发出回中间命令；如果处于上浮态且航向角度ANGO偏右，则发出向左纠正命令；如果处于下潜态且航向角度ANGO偏右，则发出向右纠正命令；

步骤25、判断滑块是否处于运动中，若滑块在运动过程中，则进入等待阶段直至滑块静止；如果滑块为静止状态，则判断滑块状态；滑块状态为处于中间、处于左边或者处于右边；

步骤26、根据步骤24所发出的纠正命令和步骤25所判断的滑块状态，电机控制器控制滑块移动，控制方法具体如下：

如果收到向左纠正命令，且滑块当前位置在右边或者中间时，则电机控制器控制滑块向左；如果收到向左纠正命令，且滑块当前位置在在左边，则不进行操作；

如果收到向右纠正命令，且滑块当前位置在左边或者中间时，则电机控制器控制滑块向右；如果收到向右纠正命令，且滑块当前位置在在右边，则不进行操作；

如果收到回中间命令，且滑块当前位置在左边，则电机控制器控制滑块向右至中间；如果收到回中间命令，且滑块当前位置在右边，则电机控制器控制滑块向左至中间；如果收到回中间命令，且滑块当前位置在右边，则不进行操作。

其进一步的技术方案为，所述步骤23中，相对于所设定航向角DANG，航向角度ANGO的状态具体为：

当180°＜ANGO＜345°时，航向角度ANGO偏右；

当345°≤ANGO≤360°或者0°≤ANGO≤15°时，航向角度ANGO居中；

当15°＜ANGO≤180°，航向角度ANGO偏左。

其进一步的技术方案为，所述任务下载的具体方法为：在水下滑翔器的内存区预存一段Bootload程序；水下滑翔器通电后，所述Bootload程序通过RS232串口由任务程序下载电脑接收任务程序，并遵循Ymodem协议，自动把任务程序由载入到水下滑翔器的控制中心；当超过一定的时间未收到任务程序时，水下滑翔器的控制中心自动执行上次载入的任务程序；

所述部件测试和任务参数设置的具体方法为：首先，为水下滑翔器的每个部件设置独立的命令编号；其次，外部使用者通过水下滑翔器的外部调试接口，发送相应的命令编号，由水下滑翔器的控制中心解析，操控水下滑翔器的动作装置以及传感器装置，使其反馈部件状态信息；最后，调用任务参数设置命令，执行任务参数设置过程。

其进一步的技术方案为：调用任务参数设置命令，执行任务参数设置过程的具体步骤如下：

步骤1a、开始接收命令；

步骤1b、判断是否到100ms，如果是，则进入步骤1c，如果否，则重新进行步骤1b；

步骤1c、判断是否收到命令，如果是，则进入步骤1d，如果否，则回到步骤1b，重新开始查询；

步骤1d、判断该命令是否是命令库中已有的命令，如果是，则进入步骤1e，如果否，则返回命令错误；

步骤1e、判断该命令参数是否正确，如果是，则进入步骤1f，如果否，则返回命令错误；

步骤1f、执行该测试和调试命令；

步骤1g、反馈执行完成该命令的信息。。

本发明的有益技术效果是：

1、本发明没有使用操作系统，而是利用通过前后台的操作方式，通过区分不同的任务在不同的时间内的必要性来划分任务优先级，来使得控制中心运行速度最大化。

2、本发明在进行航向决策时，通过判别实际航向与设定航向之间的差值决定航向如何控制，并通过坐标系变换的方法解决了航向角度过零变换导致的计算过于复杂的问题，而且将坐标变换后的角度分为三个区间，将判断方法的简单化，使得航向的控制效率更高。

3、由于水下滑翔器自身的限制使得无法为每个部件预留相应的调试口，本发明通过改善任务下载，并进行部件测试和参数设置的方法，只预留了一个RS232调试口，对所有部件的调试动作都是通过该调试口把命令发送到控制中心然后由控制中心解析各个命令，控制各个部件的动作，同时各个部件反馈的信息也是经由控制中心反馈到调试口中。这样可以将各种信息传递通道都集成到一个调试口上，同时本发明中水下滑翔器端口只预留一个5芯端口通过RS232方式进行。可更好的满足水下滑翔器水下耐压与水密性要求。

附图说明

图1为本发明的任务控制的流程图。

图2是本发明的航向控制的流程图。

图3是水下滑翔器任务下载的具体实施例的示意图。

图4是调用命令执行任务参数设置的程序流程图。

具体实施方式

本发明的水下滑翔器的控制方法，包括准备工作和水下工作两部分。

准备工作是在正式的水下工作之前，进行任务下载，并进行部件测试和任务参数设置。

水下工作是对水下滑翔器进行任务控制，使水下滑翔器接收任务指令并进行相应的动作，同时进行航向控制，将水下滑翔器调整到正确的航行角度。

任务控制的方法为，将水下滑翔器的任务分为阶段性任务和实时性任务。阶段性任务为只需在某一时间段执行的任务，实时性任务为在水下滑翔器的整个工作时间内都需要实时处理的任务。根据阶段性任务和实时性任务优先级为之分别设定扫描间隔时间；在一个任务扫描周期内，每到达相应的任务扫描间隔时间时，自动执行该任务扫描间隔时间所对应的任务，完成水下滑翔器的相应动作。

航向控制的方法为，通过电子罗盘获得水下滑翔器的当前航向角度，以所设定的航向角度为0°建立新坐标系，并在新坐标系中计算水下滑翔器的当前航向角度；之后对新坐标系中的当前航向角度进行角度范围划分，根据不同的角度范围以及水下滑翔器的上浮或者下潜状态，控制水下滑翔器的姿态调节装置中的滑块位置，进而控制水下滑翔器的航向。

在对水下滑翔器任务进行划分时，阶段性任务是指只是在某一时间段执行的任务，控制器只需在某一阶段执行该任务其他时间段无需执行也无需扫描以节省其运行空间。实时性任务是在整个滑翔器的运行周期内都需要实时处理的任务，该任务占用了控制器的极大的资源。本发明通过更具不同任务的优先级调整不同任务的扫描时间来节省控制器的资源。

在本发明中，阶段性任务具体包括下水后等待任务，通信任务，通信后等待任务，潜伏作业任务等。实时性任务包括实时性紧急状态监测任务和实时性存储任务。对于阶段性任务，控制器只在该任务周期内执行，所以对于某一特定的时间段控制器只执行某一特定的阶段性任务。对于实时性任务。控制器将会一直执行。

水下滑翔器控制中心在对水下滑翔器进行运动控制时对每个任务的执行都是通过不断的周期性的扫描来完成的。可设定在一个扫描周期中，间隔时间t1进行阶段性任务，间隔时间t2进行实时性紧急状态监测任务，间隔时间t3进行实时性存储任务。

图1为本发明的任务控制流程图。在本实施例中，时间t1为10ms，时间t2为20ms，时间t3为1s。即在图1中，任务控制被分为三大任务片，每10ms扫描的是阶段性任务，每20ms扫描的是实时性紧急状态监测任务，每1s扫描的是实时性存储任务。

在一个扫描周期中，水下滑翔器的任务控制方法具体包括以下步骤：

步骤11、扫描周期开始，控制器计时；

步骤12、判断时间10ms是否到。如果是，则进入步骤2a，开始阶段性任务，如果否，则进入步骤13。阶段性任务为其中下水后等待任务，通信任务，通信后等待任务以及潜伏作业任务。

步骤13、判断时间20ms是否到。如果是，则进入步骤3a，开始实时性紧急状态监测任务，如果否，则进入步骤14。实时性紧急状态监测任务具体为监测水下滑翔器下潜是否超时、超深以及上浮是否超时

步骤14、判断时间1s是否到。如果是，则进入步骤4a，开始实时性存储任务，如果否，则进入步骤15。实时性存储任务具体为定时记录水下滑翔器中的传感器的数值。

步骤15、扫描周期结束。

步骤12为阶段性任务，阶段性任务由多个任务组成，如果每个时间片都去轮询每一个任务，会延迟控制中心对水下滑翔器的实时性控制，本发明中对每个阶段性任务设置一个状态标志，该标志是否置位决定控制器是否轮询该任务，从而根据执行条件决定是否执行该任务。对于实时性任务，控制中心会根据其需求程度在特定的时间内轮番执行该任务。

在步骤12中，进行阶段性任务具体步骤包括：

步骤2a、判断下水后等待任务标志是否置位，如果是，则进入步骤2b，如果否，则进入步骤2d。在一次轮询周期开始后，控制器首先判断10ms时间是否到，时间到后根据状态标志查询是否执行下水后等待任务，即步骤2a，如果状态标志未置位表示该任务不是该阶段所要执行的任务，直接跳过该任务，去查询其他任务标志，即下一个任务，通信任务的标志。

步骤2b、执行下水后等待任务，等待一段设置好的时间。

步骤2c、清除下水后等待任务标志，置放通信任务标志。

步骤2d、判断通信任务标志是否置位，如果是，则进入步骤2e，如果否，则进入步骤2h。

步骤2e、姿态调节装置开始工作，水下滑翔器进入通信状态，并进入步骤3f。

步骤2f、发送位置信息，并进入步骤2g。

步骤2g、判断是否收到命令信号，如果是，则进入步骤2h，如果否，则进入步骤2i。

步骤2h、根据所收到的命令信号进行相关动作。

在步骤2e～步骤2g中，水下滑翔器的姿态调节装置动作，水下滑翔器进入到通信态，在该状态水下滑翔器会发送自身的位置信息，同时也可以接受外部输入的命令信号。在该段对滑翔器控制的命令信号有四种，1：下潜命令，即下潜进行潜伏作业；2：放平命令，即水下滑翔器放平后等待回收的命令；3：重发位置信息命令，即重发该位置的GPS点的命令；4：发送传感器参数命令，即发送水下滑翔器所采集到的传感器数据的命令。除了下潜命令，其他三种命令在执行完后水下滑翔器依然保持在通信态，依然可以接受命令。在收到下潜后水下滑翔器进入到下潜态不再接收其他命令，置通信等待任务标志位，等待设定的时间后，置潜伏作业任务标志位，进行潜伏作业。

步骤2i、判断通信等待任务标志是否置位，如果是，则进入步骤2j，如果否则进入步骤2l。

步骤2j、执行通信等待任务。

步骤2k、清除通信等待任务标志，置放潜伏作业任务标志。

步骤2l、判断潜伏作业任务标志是否置位，如果是，则进入步骤2m，如果否，则进入步骤13。

步骤2m、水下滑翔器上浮或者下潜。在进入下潜态后，水下滑翔器通过驱动浮力装置回油，使得其重力大于浮力，开始下潜，在达到下潜深度后，通过驱动浮力装置出油，使得其浮力大于重力，开始上浮。

进行完步骤2m，继续进入步骤13。即在阶段性任务轮询完后开始轮询实时性任务，只要是设定轮询时间到后，每次必将执行实时性任务。

对于实时性紧急状态监测任务，其具体轮迅步骤包括：

步骤3a、判断下潜是否超深，如果是，则进入步骤3c，如果否，则进入步骤3b；

步骤3b、判断下潜是否超时，如果是，则进入步骤3c，如果否，则进入步骤3d；

步骤3c、紧急上浮，之后进入步骤3d；

步骤3d、判断上浮是否超时，如果是，则进入步骤3e，如果否，则进入步骤14；

步骤3e、抛载，之后进入步骤14。

步骤14即使对传感器数值进行记录。之后进入步骤15，一个扫描周期结束。

图2是本发明的航向控制的流程图。航向控制具体包括下述步骤：

步骤21、航向采集，水下滑翔器航向控制首先需要通过水下滑翔器自身所携带的电子罗盘对其航向进行采集。通过电子罗盘并通过RS232通讯方式来获得水下滑翔器的当前航向角ANGI。水下滑翔器的当前航向角ANGI向由0°到360°的形式输出表示。

步骤22、航向坐标系更换，以所设定航向角DANG为0°建立坐标系，并在新的坐标系中重新计算水下滑翔器的航向角。通过对比电子罗盘的数值与所设定航向的偏差来决定姿态调节装置的运动方向，由于角度涉及到0°到360°，在进行转化时，需要对角度进行区域性判断，由于角度范围过广，判断时往往需要划分多个区域进行判断，这样会带来判断方法的复杂化，本发明通过变换坐标系的方法，以所设定航向角DANG为0°，进行坐标系变化，来简化角度判断的方法。

坐标系变换方法，即经过坐标变换的航向角度ANGO、当前航向角ANGI以及所设定航向角DANG的关系为：

$ANGO=\left\{\begin{array}{cc}ANGI-DANG& (DANG\&le;ANGI<360)\\ 360-(DANG-ANGI)& (0\&le;ANGI<DANG)\end{array}\right.$

步骤23、经过坐标变换的航向角判断，在步骤22所建立的新坐标系中，判断经过坐标变换的航向角度ANGO的状态；相对于所设定航向角DANG，航向角度ANGO的状态为偏左、正中或者偏右。

在本实施例中，相对于所设定航向角DANG，即0°角，航向角度ANGO的状态具体为：

当180°＜ANGO＜345°时，航向角度ANGO偏右；

当345°≤ANGO≤360或者0≤ANGO≤15°时，航向角度ANGO居中；

当15°＜ANGO≤180°，航向角度ANGO偏左。

步骤24、水下滑翔器的状态判断，判断此时水下滑翔器处于上浮态或者下潜态，并结合步骤23所得到的经过坐标变换的航向角度ANGO，向电机控制器发出纠正命令。

水下滑翔器中安装有姿态调节装置。如果水下滑翔器处于上浮态且航向角度ANGO偏左，则发出向右纠正命令；如果处于下潜态且航向角度ANGO偏左，则发出向左纠正命令；如果处于上浮态或者下潜态且航向角度ANGO正中，则发出回中间命令；如果处于上浮态且航向角度ANGO偏右，则发出向左纠正命令；如果处于下潜态且航向角度ANGO偏右，则发出向右纠正命令。

步骤25、判断滑块是否处于运动中，若滑块在运动过程中，则进入等待阶段直至滑块静止；如果滑块为静止状态，则判断滑块状态；滑块状态为处于中间、处于左边或者处于右边。由于水下滑翔器姿态调节装置向右，向左以及回中间运动需要一定的时间，所以在收到控制命令后，需要判断姿态调节装置是否在运动过程中，如果在运动过程中，则该命令进入等待阶段。只有当姿态调节装置运动结束后才会进行该次运动。

步骤26、根据步骤24所发出的纠正命令和步骤25所判断的滑块状态，电机控制器控制水下滑翔器的姿态调节装置中的滑块位置，控制方法具体如下：

如果收到向左纠正命令，且滑块当前位置在右边或者中间时，则电机控制器控制滑块向左；如果收到向左纠正命令，且滑块当前位置在在左边，则不进行操作；

如果收到向右纠正命令，且滑块当前位置在左边或者中间时，则电机控制器控制滑块向右；如果收到向右纠正命令，且滑块当前位置在在右边，则不进行操作；

如果收到回中间命令，且滑块当前位置在左边，则电机控制器控制滑块向右；如果收到回中间命令，且滑块当前位置在右边，则电机控制器控制滑块向左；如果收到回中间命令，且滑块当前位置在右边，则不进行操作。

姿态调节装置中滑块位置的控制方式是由RS485通讯方式，通过发送相应的命令信号给电机控制器完成。

结合图2可知，对于航向角度ANGO，上述控制流程可具体详述如下：

如果180°＜ANGO＜345°、水下滑翔器处于上浮态且滑块居左，则控制滑块右移到头；

如果180°＜ANGO＜345°、水下滑翔器处于上浮态且滑块居中，则控制滑块右移到头；

如果180°＜ANGO＜345°、水下滑翔器处于上浮态且滑块居右，则控制滑块不动；

如果180°＜ANGO＜345°、水下滑翔器处于下潜态且滑块居左，则控制滑块不动；

如果180°＜ANGO＜345°、水下滑翔器处于上浮态且滑块居中，则控制滑块左移到头；

如果180°＜ANGO＜345°、水下滑翔器处于上浮态且滑块居右，则控制滑块左移到头；

如果345°≤ANGO≤360°或者0°≤ANGO≤15°、水下滑翔器处于上浮态且滑块居左，则控制滑块右移回中；

如果345°≤ANGO≤360°或者0°≤ANGO≤15°、水下滑翔器处于上浮态且滑块居中，则控制滑块不动；

如果345°≤ANGO≤360°或者0°≤ANGO≤15°、水下滑翔器处于上浮态且滑块居右，则控制滑块左移回中；

如果345°≤ANGO≤360°或者0°≤ANGO≤15°、水下滑翔器处于下潜态且滑块居左，则控制滑块右移回中；

如果345°≤ANGO≤360°或者0°≤ANGO≤15°、水下滑翔器处于下潜且滑块居中，则控制滑块不动；

如果345°≤ANGO≤360°或者0°≤ANGO≤15°、水下滑翔器处于下潜且滑块居右，则控制滑块左移回中；

如果15°＜ANGO≤180°、水下滑翔器处于上浮态且滑块居左，则控制滑块不动；

如果15°＜ANGO≤180°、水下滑翔器处于上浮态且滑块居中，则控制滑块左移到头；

如果15°＜ANGO≤180°、水下滑翔器处于上浮态且滑块居左，则控制滑块不动；

如果15°＜ANGO≤180°、水下滑翔器处于下潜态且滑块居左，则控制滑块右移到头；

如果15°＜ANGO≤180°、水下滑翔器处于下潜态且滑块居中，则控制滑块右移到头；

如果15°＜ANGO≤180°、水下滑翔器处于下潜态且滑块居右，则控制滑块不动。

在水下工作开始之前，还需要进行准备工作，即任务下载和部件测试和任务参数设置的程序。

任务下载的具体方法为：在水下滑翔器的内存区预存一段Bootload程序；水下滑翔器通电后，所述Bootload程序通过RS232串口由任务程序下载电脑接收任务程序，并遵循Ymodem协议，自动把任务程序由载入到水下滑翔器的控制中心；当超过一定的时间未收到任务程序时，水下滑翔器的控制中心自动执行上次载入的任务程序。其中Bootload程序、Ymodem协议都为常见的现有技术，可直接编写调用。

图3是水下滑翔器任务下载的具体实施例的示意图。该任务下载方法遵循Ymodem协议，在水下滑翔器的内存区预存的Bootload程序在开始程序下载时率先发送字符“C”给任务程序下载电脑，在收到该字符后任务程序下载电脑随即发送一帧程序数据给水下滑翔器，当校验成功后，水下滑翔器发送正确应答字符给电脑，同时将该程序帧写入水下滑翔器微处理器的flash中，依照该方法写入待下载程序的所有帧数据，待写入完成后程序下载电脑连续发送2次“EOT”结束符给水下滑翔器，水下滑翔器在收到该字符，同意结束后，发送ACK应答符给任务程序下载电脑同意下载结束，随后发送字符“C”结束下载。

部件测试和任务参数设置的具体步骤为：

首先，为每个部件设置独立编号，

其次，外部使用者通过外部调试接口，发送相应的命令编号，操控水下滑翔器的动作装置以及传感器装置，反馈部件状态信息；即进行部件测试。

最后，调用任务参数设置命令，执行任务参数设置过程。

水下滑翔器在下水之前需要对各个部件进行测试，如传感器采集是否正确，浮力调节，姿态调节装置运动是否正常。水下滑翔器的每个部件都有独立的命令编号，下水前只需通过外部调试接口，由外部使用者发送相应的命令编号，由水下滑翔器的控制中心解析，操控水下滑翔器的动作装置以及传感器装置，同时反馈部件状态信息，以方便调试各个器件的好坏。

另外，水下滑翔器下水前必须进行相应的任务参数设置，本发明中水下滑翔器完成相关任务所运行的关键参数都是先定义出相关变量，这样在进行下水前参数设置时，调用相关的命令，改变其相应的相关变量值即可以达到任务参数设置的目的。

图4是调用命令执行任务参数设置的程序流程图。执行任务参数设置时调用命令的具体步骤如下：

步骤1a、开始接收命令；

步骤1b、判断是否到100ms，如果是，则进入步骤1c，如果否，则重新进行步骤1b；

步骤1c、判断是否收到命令，如果是，则进入步骤1d，如果否，则回到步骤1b，重新开始查询；

步骤1d、判断该命令是否是命令库中已有的命令，如果是，则进入步骤1e，如果否，则返回命令错误；

步骤1e、判断该命令参数是否正确，如果是，则进入步骤1f；

步骤1f、执行该命令。

以上所述的仅是本发明的优选实施方式，本发明不限于以上实施例。可以理解，本领域技术人员在不脱离本发明的精神和构思的前提下直接导出或联想到的其他改进和变化，均应认为包含在本发明的保护范围之内。

Claims (10)

1.一种水下滑翔器的控制方法，其特征在于：

首先进行任务下载，并进行部件测试和任务参数设置；

其次进行任务控制，使水下滑翔器接收任务指令并进行相应的动作，同时进行航向控制，将水下滑翔器调整到正确的航行角度；

所述任务控制的方法为，将任务分为阶段性任务和实时性任务；所述阶段性任务为只需在某一时间段执行的任务；所述实时性任务为在水下滑翔器的整个工作时间内都需要实时处理的任务；根据阶段性任务和实时性任务的优先级为之分别设定扫描的间隔时间；在一个任务扫描周期内，每到达相应的任务的间隔时间时，自动执行该间隔时间所对应的任务，完成水下滑翔器的动作；

所述航向控制的方法为，通过电子罗盘获得水下滑翔器的当前航向角度，以所设定的航向角度为0°建立新坐标系，并在新坐标系中计算水下滑翔器的当前航向角度；之后对新坐标系中的当前航向角度进行角度范围划分，根据不同的角度范围以及水下滑翔器的上浮或者下潜状态，控制水下滑翔器的姿态调节装置中的滑块位置，进而控制水下滑翔器的航向。

2.如权利要求1所述的水下滑翔器的控制方法，其特征在于：所述实时性任务包括实时性紧急状态监测任务和实时性存储任务；所述实时性紧急状态监测任务为监测水下滑翔器下潜是否超时、超深以及上浮是否超时；所述实时性存储任务为定时记录水下滑翔器中的传感器的数值；在一个扫描周期中，间隔时间t1进行阶段性任务，间隔时间t2进行实时性紧急状态监测任务，间隔时间t3进行实时性存储任务；任务控制的方法具体包括以下步骤：

步骤11、扫描周期开始，控制器计时；

步骤12、判断时间t1是否到；如果是，则开始阶段性任务，如果否，则进入步骤13；

步骤13、判断时间t2是否到；如果是，则开始实时性紧急状态监测任务；如果否，则进入步骤14；

步骤14、判断时间t3是否到，如果是，则开始实时性存储任务；如果否，则进入步骤15；

步骤15、扫描周期结束。

3.如权利要求2所述的水下滑翔器的控制方法，其特征在于：进行所述阶段性任务的具体步骤包括：

步骤2a、判断下水后等待任务标志是否置位，如果是，则进入步骤2b，如果否，则进入步骤2d；

步骤2b、执行下水后等待任务；

步骤2c、清除下水后等待任务标志，置放通信任务标志；

步骤2d、判断通信任务标志是否置位，如果是，则进入步骤2e，如果否，则进入步骤2i；

步骤2e、姿态调节装置开始工作，水下滑翔器进入通信状态，并进入步骤2f；

步骤2f、发送位置信息，并进入步骤2g；

步骤2g、判断是否收到命令信号，如果是，则进入步骤2h，如果否，则进入步骤2i；

步骤2h、根据所收到的命令信号进行水下滑翔器的相关动作；

步骤2i、判断通信等待任务标志是否置位，如果是，则进入步骤2j，如果否则进入步骤2l；

步骤2j、执行通信等待任务；

步骤2k、清除通信等待任务标志，置放潜伏作业任务标志；

步骤2l、判断潜伏作业任务标志是否置位，如果是，则进入步骤2m，如果否，则进入步骤13；

步骤2m、水下滑翔器上浮或者下潜，之后进入步骤13。

4.如权利要求3所述的水下滑翔器的控制方法，其特征在于：在所述步骤2h中，所收到的命令信号包括：

放平命令，若收到放平命令，则姿态调节装置进行放平动作；

重发位置信息命令，若收到重发位置信息命令，则重发位置信息；

发送传感器参数命令，若收到发送传感器参数命令，则发送传感器信息；

下潜命令，若收到下潜命令，则姿态调节装置进行放平动作，且清除通信任务标志，放置通信等待任务标志。

5.如权利要求3所述的水下滑翔器的控制方法，其特征在于：在所述步骤2m中，如果为上浮态，则判断是否到上浮深度，如果是，则水下滑翔器回油；如果为下潜态，则判断是否到下潜深度，如果是，则水下滑翔器出油。

6.如权利要求2所述的水下滑翔器的控制方法，其特征在于：进行所述实时性紧急状态监测任务的具体步骤包括：

步骤3a、判断下潜是否超深，如果是，则进入步骤3c，如果否，则进入步骤3b；

步骤3b、判断下潜是否超时，如果是，则进入步骤3c，如果否，则进入步骤3d；

步骤3c、紧急上浮，之后进入步骤3d；

步骤3d、判断上浮是否超时，如果是，则进入步骤3e，如果否，则进入步骤14；

步骤3e、抛载，之后进入步骤14。

7.如权利要求1所述的水下滑翔器的控制方法，其特征在于：所述航向控制具体包括下述步骤：

步骤21、航向采集，通过电子罗盘来获得水下滑翔器的当前航向角ANGI；水下滑翔器的当前航向角ANGI为0～360°；

步骤22、航向坐标系变换，以所设定航向角DANG为0°，重新建立坐标系；航向坐标系变换方法，即经过坐标变换的航向角度ANGO、当前航向角ANGI以及所设定航向角DANG的关系为：

$ANGO=\left\{\begin{array}{cc}ANGI-DANG& (DANG\&le;ANGI<360)\\ 360-(DANG-ANGI)& (0\&le;ANGI<DANG)\end{array}\right.;$

步骤23、经过坐标变换的航向角状态判断，在步骤22所建立的坐标系中，判断经过坐标变换的航向角度ANGO的状态；相对所设定航向角DANG，即0°角，航向角度ANGO的状态为偏左、正中或者偏右；

步骤24、水下滑翔器的状态判断，判断此时水下滑翔器处于上浮态或者下潜态，并结合步骤23所得到的航向角度ANGO的状态，向电机控制器发出纠正命令；如果处于上浮态且航向角度ANGO偏左，则发出向右纠正命令；如果处于下潜态且航向角度ANGO偏左，则发出向左纠正命令；如果处于上浮态或者下潜态且航向角度ANGO正中，则发出回中间命令；如果处于上浮态且航向角度ANGO偏右，则发出向左纠正命令；如果处于下潜态且航向角度ANGO偏右，则发出向右纠正命令；

步骤25、判断滑块是否处于运动中，若滑块在运动过程中，则进入等待阶段直至滑块静止；如果滑块为静止状态，则判断滑块状态；滑块状态为处于中间、处于左边或者处于右边；

步骤26、根据步骤24所发出的纠正命令和步骤25所判断的滑块状态，电机控制器控制滑块移动，控制方法具体如下：

如果收到向左纠正命令，且滑块当前位置在右边或者中间时，则电机控制器控制滑块向左；如果收到向左纠正命令，且滑块当前位置在在左边，则不进行操作；

如果收到向右纠正命令，且滑块当前位置在左边或者中间时，则电机控制器控制滑块向右；如果收到向右纠正命令，且滑块当前位置在在右边，则不进行操作；

如果收到回中间命令，且滑块当前位置在左边，则电机控制器控制滑块向右至中间；如果收到回中间命令，且滑块当前位置在右边，则电机控制器控制滑块向左至中间；如果收到回中间命令，且滑块当前位置在右边，则不进行操作。

8.如权利要求7所述的水下滑翔器航向控制方法，其特征在于，所述步骤23中，相对于所设定航向角DANG，航向角度ANGO的状态具体为：

当180°＜ANGO＜345°时，航向角度ANGO偏右；

当345°≤ANGO≤360°或者0°≤ANGO≤15°时，航向角度ANGO居中；

当15°＜ANGO≤180°，航向角度ANGO偏左。

9.如权利要求1所述的水下滑翔器控制方法，其特征在于，所述任务下载的具体方法为：在水下滑翔器的内存区预存一段Bootload程序；水下滑翔器通电后，所述Bootload程序通过RS232串口由任务程序下载电脑接收任务程序，并遵循Ymodem协议，自动把任务程序由载入到水下滑翔器的控制中心；当超过一定的时间未收到任务程序时，水下滑翔器的控制中心自动执行上次载入的任务程序；

所述部件测试和任务参数设置的具体方法为：首先，为水下滑翔器的每个部件设置独立的命令编号；其次，外部使用者通过水下滑翔器的外部调试接口，发送相应的命令编号，由水下滑翔器的控制中心解析，操控水下滑翔器的动作装置以及传感器装置，使其反馈部件状态信息；最后，调用任务参数设置命令，执行任务参数设置过程。

10.如权利要求9所述的水下滑翔器控制方法，其特征在于：调用任务参数设置命令，执行任务参数设置过程的具体步骤如下：

步骤1a、开始接收命令；

步骤1b、判断是否到100ms，如果是，则进入步骤1c，如果否，则重新进行步骤1b；

步骤1c、判断是否收到命令，如果是，则进入步骤1d，如果否，则回到步骤1b，重新开始查询；

步骤1d、判断该命令是否是命令库中已有的命令，如果是，则进入步骤1e，如果否，则返回命令错误；

步骤1e、判断该命令参数是否正确，如果是，则进入步骤1f，如果否，则返回命令错误；

步骤1f、执行该测试和调试命令；

步骤1g、反馈执行完成该命令的信息。