一种小型水下机器人推进控制系统及其舵控制方法

摘要

本发明涉及一种小型水下机器人推进控制系统，包括：航空接插件，推进系统控制板，电机驱动器和推进设备；通过CAN总线获取控制数据并向主控制舱发送推进系统状态信息；其舵控制方法为使用定时器，通过中断使单片机端口产生多路PWM信号，对水下机器人推进系统的多个舵机进行控制；本发明采用模块化的设计思想，电路简单易于扩展，可移植性高；减少电路板的空间占用和电量消耗，简化控制程序，有效利用有限的定时器资源，有较高性价比；本发明设有推进控制系统故障检测与水下舱体安全检测功能，增强了控制系统的安全性与可靠性。

权利要求

1.一种小型水下机器人推进控制系统，其特征在于包括：

航空接插件：与推进系统控制板连接；将外部电池舱的电源和CAN总线信号转接至推进系统控制板；

推进系统控制板：与推进设备的舵机和电机驱动器连接；将推进系统控制板和推进设备所采集的反馈信号通过航空接插件经CAN总线发送给主控制舱；进行电压转换与再分配，给推进系统控制板内的单片机及安全监控电路、CAN通信电路和舵机\电机控制电路供电，并将主控制舱发送过来的CAN总线信号转换为控制命令控制电机驱动器和推进设备；

电机驱动器：控制电机运转；

推进设备：包括多个舵机和电机；

所述推进系统控制板包括：

带有CAN接口的单片机：分别与电源管理电路、安全监控电路、CAN通信电路和舵机\电机控制电路相连接；通过CAN总线接收主控制舱的控制命令，控制推进设备并接收推进系统控制板和推进设备所采集的反馈信号；

电源管理电路：分别与安全监控电路、CAN通信电路和舵机\电机控制电路相连接；将来自电池舱的电源转换成三路相互独立的电源分别供给安全监控电路、CAN通信电路和舵机\电机控制电路；并将来自电池舱的电源供给电机驱动器；

安全监控电路：监测舵机故障、舱内漏水、舱内温度和电机故障，并将接收的推进设备的反馈信号进行处理反馈至单片机；包括舵机电流检测电路、漏水检测电路，温度检测电路和电机检测电路；

CAN通信电路：实现CAN总线与单片机的通信；

舵机\电机控制电路：将单片机发出的控制命令经光电耦合开关控制舵机和电机。

2.根据权利要求1所述的一种小型水下机器人推进控制系统，其特征在于：所述反馈信号包括舵机回路的电流，推进舱内漏水报警信号和推进舱内温度，以及电机的工作状态。

3.根据权利要求1所述的一种小型水下机器人推进控制系统，其特征在于：所述舵机电流检测电路包括电流传感器；电流传感器的输入端接入电源管理电路输出的舵机供电电源；参考端和输出端分别与单片机的不同AD接口连接；参考端和输出端都串联电容接入电流传感器的接地端。

4.根据权利要求1所述的一种小型水下机器人推进控制系统，其特征在于：所述漏水检测电路包括二极管；单片机的输入端通过串联电容接地，还通过串联电阻与二极管的正极连接，二极管的负极与推进舱内的信号线连接，二极管的正极接有上拉电阻。

5.根据权利要求1所述的一种小型水下机器人推进控制系统，其特征在于：所述温度检测电路包括温度传感器；温度传感器一端连接电源管理电路输出的电源；另一端连接电阻和电容组成的并联电路，该端还与单片机的一个AD接口相连接。

6.根据权利要求1所述的一种小型水下机器人推进控制系统，其特征在于：所述电机检测电路将电机驱动器信号反馈端与单片机的I/O接口相连接。

7.根据权利要求1所述的一种小型水下机器人推进控制系统，其特征在于：所述CAN通信电路包括数字隔离器和CAN总线收发器；CAN总线信号经CAN总线收发器进行信号转换后再经数字隔离器进行信号隔离，最后接入单片机的CAN端口。

8.一种小型水下机器人推进控制系统的舵控制方法，其特征在于包括以下步骤：

1)设定单片机的分频比，设定定时器在CTC模式；设置定时器初始值为0；设定比较中断值；IO端口初始状态下输出高电平；

2)从总线获取CAN数据提取出多个舵机的控制量；

3)比较中断模式下，比较定时器的定时器计数值与比较中断模式下发生中断的临界值是否相等；

4)如相等，则发生比较中断，计数变量加1，并判断比较次数i是否小于舵机数量；

如果是，则比较计数变量与i对应舵机的控制量是否相等；

如果相等，则该舵机对应的单片机I/O端口输出低电平，并使i加1；

如果不等，则i加1；

返回步骤4)中判断比较次数i是否小于舵机数量；

如果不是，则比较计数变量与计数最大值是否相等；如果相等，则i对应的单片机I/O端口输出高电平，计数变量清零并返回步骤2)；如果不等，则比较次数i清零，返回步骤

3)；

如不相等，则返回步骤3)，等待相等。

说明书

一种小型水下机器人推进控制系统及其舵控制方法

技术领域

[0001] 本发明涉及水下机器人控制，具体地说是一种基于CAN总线的小型自治水下机器人模块化推进控制系统。

背景技术

[0002] 随着人们对海洋资源的探索与开发日益频繁与迫切，水下机器人作为一种有效载体得到了长足的发展。对于近海、河流、湖泊与水库等相对较浅的地域，小型自治水下机器人以其便携性、低成本、易用性等特点，在环境监测与水下侦查等方面得到了广泛的应用。

[0003] 运动与通信是水下机器人的两项重要的基本功能。对于自治水下机器人，推进系统是整体控制系统的重要组成部分，是完成机器人运动的保障；CAN总线有着实时性、可靠性、可扩展性以及可进行分布式控制等特点，适合水下机器人推进系统与主控制系统之间的通信。

[0004] 小型自治水下机器人因为其载体内部空间的限制，对于其控制部分的硬件体积有着严格要求，同时因为推进系统是所有水下机器人的必要组成部分，尤其对于自治水下机器人由于机动性不强，所有的推进系统功能相似，因此简单易用方便移植也是十分重要的。

发明内容

[0005] 为了解决上述存在的问题，本发明的目的在于提供一种小型自治水下机器人模块化推进控制系统，用于接收主控制器的控制指令，对推进系统相关装置进行控制并回传推进系统的相关信息。该系统具有良好的可移植性。

[0006] 本发明的目的是通过以下技术方案来实现的：一种小型水下机器人推进控制系统，其特征在于包括：

[0007] 航空接插件：与推进系统控制板连接；将外部电池舱的电源和CAN总线信号转接至推进系统控制板；

[0008] 推进系统控制板：与推进设备的舵机和电机驱动器连接；将推进系统控制板和推进设备所采集的反馈信号通过航空接插件经CAN总线发送给主控制舱；进行电压转换与再分配，给推进系统控制板内的单片机及安全监控电路、CAN通信电路和舵机\电机控制电路供电，并将主控制舱发送过来的CAN总线信号转换为控制命令控制电机驱动器和推进设备；

[0009] 电机驱动器：控制电机运转；

[0010] 推进设备：包括多个舵机和电机。

[0011] 所述推进系统控制板包括：

[0012] 带有CAN接口的单片机：分别与电源管理电路、安全监控电路、CAN通信电路和舵机\电机控制电路相连接；通过CAN总线接收主控制舱的控制命令，控制推进设备并接收推进系统控制板和推进设备所采集的反馈信号；

[0013] 电源管理电路：分别与安全监控电路、CAN通信电路和舵机\电机控制电路相连接；将来自电池舱的电源转换成三路相互独立的电源分别供给安全监控电路、CAN通信电路和舵机\电机控制电路；并将来自电池舱的电源供给电机驱动器；

[0014] 安全监控电路：监测舵机故障、舱内漏水、舱内温度和电机故障，并将接收的推进设备的反馈信号进行处理反馈至单片机；包括舵机电流检测电路、漏水检测电路，温度检测电路和电机检测电路；CAN通信电路：实现CAN总线与单片机的通信；

[0015] 舵机\电机控制电路：将单片机发出的控制命令经光电耦合开关控制舵机和电机。

[0016] 所述反馈信号包括舵机回路的电流，推进舱内漏水报警信号和推进舱内温度，以及电机的工作状态。

[0017] 所述舵机电流检测电路包括电流传感器；电流传感器的输入端接入电源管理电路输出的舵机供电电源；参考端和输出端分别与单片机的不同AD接口连接；参考端和输出端都串联电容接入电流传感器的接地端。

[0018] 所述漏水检测电路包括二极管；单片机的输入端通过串联电容接地，还通过串联电阻与二极管的正极连接，二极管的负极与推进舱内的信号线连接，二极管的正极接有上拉电阻。

[0019] 所述温度检测电路包括温度传感器；温度传感器一端连接电源管理电路输出的电源；另一端连接电阻和电容组成的并联电路，该端还与单片机的一个AD接口相连接。

[0020] 所述电机检测电路将电机驱动器信号反馈端与单片机的I/O接口相连接。

[0021] 所述CAN通信电路包括数字隔离器和CAN总线收发器；CAN总线信号经CAN总线收发器进行信号转换后再经数字隔离器进行信号隔离，最后接入单片机的CAN端口。

[0022] 一种小型水下机器人推进控制系统的舵控制方法，其特征在于包括以下步骤：

[0023] 1）设定单片机的分频比，设定定时器在CTC模式；设置定时器初始值为0；设定比较中断值；IO端口初始状态下输出高电平。

[0024] 2）从总线获取CAN数据提取出多个舵机的控制量；

[0025] 3）比较中断模式下，比较定时器的定时器计数值与比较中断模式下发生中断的临界值是否相等；

[0026] 4）如相等，则发生比较中断，计数变量加1，并判断比较次数i是否小于舵机数量；

[0027] 如果是，则比较计数变量与i对应舵机的控制量是否相等；

[0028] 如果相等，则该舵机对应的单片机I/O端口输出低电平，并使i加1；

[0029] 如果不等，则i加1；

[0030] 返回步骤4）中判断比较次数i是否小于舵机数量；

[0031] 如果不是，则比较计数变量与计数最大值是否相等；如果相等，则i对应的单片机I/O端口输出高电平，计数变量清零并返回步骤2）；如果不等，则比较次数i清零，返回步骤3）；

[0032] 如不相等，则返回步骤3），等待相等。

[0033] 本发明具有以下有益效果及优点：

[0034] 1．本发明采用模块化的设计思想，电路简单易于扩展，只需要两根电源线两根信号线即可完成所有控制，可移植性高，适合功能相似的小型自治水下机器人。

[0035] 2．针对水下机器人的特殊性，本发明使用集成CAN控制器的单片机芯片AT90CAN128，减少电路板的空间占用和电量消耗，简化控制程序，有效利用有限的定时器资源，有较高性价比。

[0036] 3．本发明设有推进控制系统模块故障检测与水下舱体安全检测功能，增强了控制系统的安全性与可靠性。

[0037] 4．本发明可以适用于大部分舵机的控制，而且舵机数量原则上可以与单片机的I/O口数量相同。

附图说明

[0038] 图1为本发明的推进控制系统接口线路连接示意图；

[0039] 图2为本发明的推进控制系统控制板结构图；

[0040] 图3为本发明的推进控制系统控制电路板组成示意图；

[0041] 图4为本发明的推进控制系统控制电路板端口接线电路图；

[0042] 图5为本发明的推进控制系统控制电路板电源管理电路图；

[0043] 图6为本发明的推进控制系统控制电路板单片机端口连接电路图；

[0044] 图7为本发明的推进控制系统控制电路板安全监控电路图；

[0045] 图8为本发明的推进控制系统控制电路板舵机\电机控制电路图；

[0046] 图9为本发明的推进控制系统控制电路板CAN通信电路图；

[0047] 图10为本发明的推进系统控制方法总体流程图；

[0048] 图11为本发明的推进系统舵机控制方法流程图；

[0049] 图12为本发明的推进系统电机控制方法流程图。

具体实施方式

[0050] 下面结合附图及实施例对本发明做进一步的详细说明。

[0051] 本发明包括推进控制系统接插件端口设计布置，推进控制系统控制板硬件设计与软件编写。本系统通过CAN总线与主控制计算机取得联系，接收发送而来的控制信息，由于控制方法在主控制计算机上完成，所以推进控制系统的数据传输量与计算量不大，单片机输出的控制信息通过隔离期间传送到执行机构，控制推进电机和舵机的工作。同时系统监测执行机构和环境的状态，通过CAN总线返回给主控制计算机作为其下一步控制的参考。

[0052] 如图1所示，本发明由四个部分组成：航空接插件，推进系统控制板，主推电机驱动器，推进设备。其中航空接插件使用的是XCE24F19系列航空接插件、主推电机驱动器和推进设备是定制的防水无刷电机与配套驱动器以及SANWASDX-901舵机，对于其他型号的无刷电机，只要接口信号满足条件即可。本系统的关键部分是推进系统控制板。推进系统控制板的部分接口说明：J1中，CANH与CANL表示CAN通信的两路信号，WS/EXT与GND/EXT是连接到外部舱段的漏水检测；J2中+36V/T与GND/T表示从外部引入的推进系统供电电源；J3～J6中+VD与GND/D表示舵机供电电压（+6V），PWM0～3表示四个舵机的控制信号；J7中STEP表示PWM控制信号，DIR是方向控制，ENA是电机使能控制，FAT是电机的报警反馈，+VT与GND/T是驱动器产生的+5V电压，作为驱动电机的高低电平参考；J8中WS与GND为本系统自处理的漏水检测；接口电路见图3。

[0053] 如图3所示，推进系统控制板中的单片机采用AT90CAN128，安全监控电路包括舵机电流检测电路、漏水检测电路，温度检测电路和电机检测电路，舵机\电机控制电路采用光电耦合开关隔离。

[0054] 如图2所示，推进系统控制板包括带有CAN接口的单片机AT90CAN128、电源管理电路、安全监控电路和舵机\电机控制电路。单片机通过CAN总线接收主控板的控制命令，控制推进设备并接收推进设备的反馈信号；电源管理电路将来自主控板的输入电源转换成三路相互独立的电源分别供给安全监控电路、CAN通信电路和舵机\电机控制电路；安全监控电路：监测舵机故障、舱内漏水和电机故障，并将接收的推进设备的反馈信号进行处理反馈至单片机；CAN通信电路实现CAN总线与单片机的通信；舵机\电机控制电路将单片机发出的控制命令经光电耦合开关控制舵机和电机。

[0055] 如图4所示，J1-J7为单片机的各接口。

[0056] 由于不同的设备电压不同，即使电压相同有的需要进行电气隔离以增加可靠性与稳定性，因此电源管理单元将引进推进系统的电源转换为3路，分别给控制元件、通信元件和舵机供电（主推进电机供电电源不需要变化）。具体电路连接见图5，包括三个电源转换模块，将一路输入的电源转换成三路相互独立电源，简化了外部接口的设计，方便进行推进系统的电源集中管理。

[0057] 图5（a）是给舵机和电机的供电电路，将外部电池舱接入的36V电源经模块V48C8C150BG转换成6V；由于标称输出为8V，所以在输出端加入电阻R6进行电压调整；图5（b）是给安全监控电路和CAN通信电路的供电电路，将外部主控板接入的36V电源经THL20-4811WI模块转换成5V供给安全监控电路，经TSM0505S模块转换成另一路5V电源供给CAN通信电路；电源的输入与输出端都连接了电容进行滤波稳压。

[0058] 安全监控单元包括舱内漏水检测，舵机状态检测，电机状态检测，温度检测，并且可以加入限流保险的扩展功能。

[0059] 本系统中使用的推进电机自身带有状态反馈量（传感器测量的转速），可以直接得到电机是否正常工作；出于成本的考虑，本系统使用的舵机为三线制舵机，即有正负电源线和控制信号线，没有状态反馈线，因此舵机的工作状态检测只能从其他方面间接得到，如果舵机因为其他物体阻碍而卡住，舵翼没有正常的转动，会使得舵机处于大电流超负荷状态，本系统使用电流传感器来检测舵机回路中的电流值，间接监测舵机的工作状态，本设计中电流的计算方法为I＝16×|ADC0-ADC1|；设计舱内有漏水检测触点，一旦舱体漏水，WS与GND短接，PB0端口得到的电平值将由高变低（由1变0），保证在漏水时可以及时得到相应信息，同时还有连接到外部舱段漏水检测，防止推进舱瞬间大量进水之后造成瞬间瘫痪，无法独立处理漏水状况；温度监测功能采用温度传感器AD590，利用其特性，其相当于一个电流源，电流大小通过公式IAD590＝(273+t)uA来计算，t为当前舱内摄氏温度值，通过检测电阻R28两端电压值间接检测温度值，具体电路连接见图7；限流保险丝也可以扩展到本系统之中。

[0060] 如图7所示，如图7（a）为舵机回路电流检测电路，采用LEM HXS20电流传感器，输入端连接到舵机回路中，输出端通过输出值和参考值求解出该回路的电流大小；如图7（b）为舱内漏水检测电路，采用PB0端口高低电平检测的方法间接检测舱内是否进水，D1为二极管，限制电流流向；如图7（c）为温度检测电路，主要采用AD590温度传感器，一端接5V电源，另一端接固定电阻之后接地，通过检测电阻两端电压检测舱内温度，电阻两端接有电容稳压。

[0061] 舵机与推进电机的控制都由单片机、CAN通信电路以及舵机\电机控制电路完成，由于单片机AT90CAN128集成了CAN总线控制器，因此程序的难度大大降低。单片机的控制引脚定义见图6（b）与表1所示。图6（a）为JTAG仿真器插口，能够通过仿真器与电脑相连，进行单片机的程序烧写。

[0062] 表1控制对应端口与信号含义

[0063]

[0064] 单片机的整体控制程序流程见图10，首先进行单片机定时器、端口与CAN通信等地初始化，打开中断等待定时器与CAN通信的中断，所有的信息处理都在中断程序中完成。定时器0中断程序处理的是系统的各项安全监测信息；定时器1中断程序对舵机进行控制（详见图11）；定时器3中断程序对电机进行控制（详见图11）；CAN通信中断程序对舵机进行控制，主要进行数据接收处理以及控制系统数据采集与回传。

[0065] 单片机内有CAN通信程序和相应的电机与舵机控制程序，其中舵机控制程序只使用一个定时器即可控制4路（可以更多）舵机的转动，节省定时器资源；控制策略由外部主控制系统制定并通过CAN与推进系统之间进行可定义的信息传递。

[0066] 使用单片机的PC0～PC3来控制舵机舵角的转动。现在大部分舵机的控制信号都是周期为20ms的PWM波，舵机的偏转角度由输入PWM控制信号波的占空比决定。由于有四个舵机，这里使用一个计时器在PC端口上产生四路的PWM波，具体实现如下：

[0067] 1)使用16MHz外部晶振，使用16位定时/计数器，选择8分频，CTC模式；

[0068] 2)初始值设置，定时器计数起始值TCNT1=0x0000，比较中断模式下发生中断的临界值OCR1A=ctc_num，PC端口值输出高电平；

[0069] 3)比较中断模式下，定时器从TCNT1开始一直增加当与OCR1A相等的时候产生中断，自动回到初始值继续重复，设定计数变量count_timer1初始值为0，每比较中断一次则计数变量加1，设定计数最大值为count_max，当count_timer1达到count_max时则将计数变量清零，重新计数。

[0070] 4)控制信息经过CAN总线传输到单片机，经处理提取出舵机的控制量rudder_motor[0]～[3]，时刻比较count_timer1与rudder_motor[i]（i=0～3）的值，当二者相等的时候，将对应的端口拉低，在count_timer1清零的时候，再次将PC端口输出高电平；

[0071] 5)循环往复，在各个端口就会产生占空比与rudder_motor[i]大小相关的PWM波。

[0072] 下面给出产生的PWM波相关参数的计算方法：

[0073] 周期计算

[0074] 占空比

[0075] 其中f为使用的晶振频率值，K为选择的分频比，舵机控制方法流程图见图11。

[0076] 主推电机的转速由PWM波的频率控制，采用定时器溢出中断，每个周期轮流输出高低电平，产生PWM波，定时器周期设定值为0xffff，为了保证输入与转速的一致性，当输入数值变大时，将定时器周期设定值0xffff与输入值之差设为新的周期值，这样频率相应变大，转速也就提高，电机控制方法流程图见图12。

[0077] 控制电路板舵机\电机控制电路见图8，采用TLP521-4高速光电耦合器芯片，输入端由VCC串联限流电阻经光耦与单片机的IO端口相连，输出端由相应设备的驱动电压接限流电阻与光耦相连，输入输出端的信号相同，强度不同，增强了信号的驱动能力；同时舵机\电机控制电路设计上将控制信号与执行器（舵机和电机）进行光电耦合隔离，增加安全性与可靠性，执行器舵机能够被分别控制，可以满足尾部十字舵、尾部斜十字舵与前后分离转向升沉舵等多种舵翼的安装方式。

[0078] 推进系统的CAN总线程序主要包括两个部分：CAN总线初始化，数据的接收与发送。控制电路板CAN通信电路连接图见图9，由于使用ATmega公司集成CAN控制器的AT90CAN128单片机，可以很容易的通过寄存器操作编写CAN通信程序。

[0079] 如图9所示，采用数字隔离器ADuM1201和高速CAN总线收发器TJA1050，数字隔离器两端采用相互隔离的电源，保证单片机输出的信号和接到CAN总线收发器上的信号内容相同但是相互隔离，增加了系统的抗干扰性，经过CAN收发器输出的CAN信号直接与其他舱段的CAN总线相连，输出端增加跳线端子供调试时候使用。

[0080] 在本系统中，主控制计算机和推进系统之间的数据交换有：主控制计算机向推进系统发送主推进电机的速度、方向与使能控制量以及四个舵机的转动角度控制量；推进系统向主控制计算机报告主推进电机和舵机的状态以及推进控制舱的情况（安全信息）。通过使用以上的CAN总线通信，我们可以通过上位机很容易的了解推进系统的运行状态，控制主推进电机和舵机，进而操纵载体的运动。

[0081] 本发明的工作原理为：通过CAN总线进行系统间的数据传输，提取数据作为控制量，使用单片机端口进行不同频率和多路同频率不同占空比PWM信号的模拟，频率与占空比通过控制量可控，进而控制电机和舵机，同时监测系统的状态，使得水下推进系统可以正常工作运转。

[0082] 本发明在小型自治水下机器人中的应用：将本系统集成到自治水下机器人的尾部推进舱，连接电源线与信号线，通过主控制系统向推进系统发布控制量，通过CAN总线传输到推进系统单片机，单片机进行数据提取之后对执行机构进行控制，同时返回给主控制系统推进系统的工作状态，一旦固定了数据格式，就可以把尾部推进系统移植到其他的水下机器人之中（只适用于单个尾推的有舵水下机器人，不限制舵机的数量和分布），适合水下机器人的产品化和模块化要求。