一种水下剖面监测机器人控制器及自动轨迹跟踪控制方法

摘要

本发明涉及一种水下剖面监测机器人控制器，包括置于船载控制箱内的控制器A和置于水下机器人仪器舱内的控制器B两部分，二者通过铠装的脐带电缆进行通信，控制器A实时采集船载控制器控制面板的遥控控制指令，并将控制指令按照自定义的通信协议进行编码和组装成帧，经由通信模块发送到水下控制器B；水下控制器B实时接收船载控制器A发送的协议帧，并进行协议解码，按照协议格式解析出控制指令，进而控制继电器驱动板上的对应的继电器的开关动作，从而实现对水下机器人运动姿态的控制。本发明采用有限时间轨迹跟踪控制技术实现水下机器人对预定轨迹点的跟踪，能够实现水下剖面监测机器人的大范围、高精度运动和姿态控制。

权利要求

1.一种水下剖面监测机器人控制器，其特征在于：包括置于船载控制箱内的控制器A和置于水下机器人仪器舱内的水下控制器B两部分，二者通过铠装的脐带电缆进行通信，所述控制器A实时采集船载控制器控制面板的遥控控制指令，并将控制指令按照自定义的通信协议进行编码和组装成帧，经由通信模块发送到水下控制器B；所述水下控制器B实时接收船载控制器A发送的协议帧，并进行协议解码，按照协议格式解析出控制指令，进而控制继电器驱动板上的对应的继电器的开关动作，继而驱动推进电机进行工作，从而实现对水下机器人运动姿态的控制；所述控制器A和水下控制器B还包括有串口通信模块、串口/光纤转换器和电压转换和稳压模块A、电压转换和稳压模块B；用于水下机器人控制器和船载控制器间的相互通信，其还包括铠装的脐带通信电缆，通过电压转换和稳压模块B将供电电压转换为机器人所需的工作电压，并提供其与控制器A间的远程数据通信，保证信号传输的无失真；所述的电压转换和稳压模块A包含电压转换和稳压电路，用于将岸上电源进行AC-DC电压转换和低压差线性稳压的两级电压转换，从而为电路提供稳定的工作电压；电压转换和稳压模块B，对电压进行转换和稳压，为控制器B和机器人驱动电机提供必需的工作电压；所述水下机器人控制器之间的通信协议，采用自定义的通信协议帧，水下控制器B按照协议格式解析出控制器A发送的控制指令；所述协议帧头和帧尾均由1个字节组成，控制命令字由1个字节组成，其相应低4位数据位从右至左分别代表左侧水平推进器、右侧水平推进器、左侧垂直推进器和右侧垂直推进器，高4为自动/手动切换；最高位用于区分手动操控还是自动轨迹跟踪控制，如果为0，则为手动操控，如果为1，则为自动轨迹跟踪控制；循环冗余检测CRC也由1个字节组成，控制器B通过CRC检测算法计算出检测未位的值，并与接收到的CRC值进行对比，完成对数据帧的差错检测，出现错误则丢弃该数据包，不进行机器人操控；由水下控制器B、深度传感器和驱动电机组成水下剖面监测机器人的水下闭环随动控制系统；该水下闭环随动控制系统实现水下机器人对预定轨迹点的跟踪控制，采用有限时间跟踪控制技术，使水下机器人在有限时间内实现对预定轨迹点的跟踪；考虑到外界的海流扰动、环境噪声的影响以及轨迹控制的快速性，水下机器人的有限时间轨迹控制器就是设计v，ω的估计值 的控制律使水下机器人在有限的时间内跟踪上预定的参考轨迹点，其中设计的有限时间控制律为：从而实现水下机器人对一系列预定轨迹点的跟踪控制；上式中，v，ω分别为水下机器人的线速度和角速度， 分别为v，ω经过卡尔曼滤波后的估计值，νd，ωd分别为水下机器人线速度和角速度的参考控制输入，k0，k1，k2，k3，k4，k5，k6＞0，0＜γ0，γ1＜1，0＜β0，β1＜1，β2＝2β1/(1+β1)都为有限时间控制律的调节系数， 分别为xe，ye，θe经过卡尔曼滤波后的估计值，其中xe，ye，θe可以表示为 这里xd，yd表示水下机器人位置坐标的期望值，θd表示水下机器人运动方向的期望值，x，y表示水下机器人位置坐标的当前值，θ表示水下机器人运动方向的当前值。

说明书

一种水下剖面监测机器人控制器及自动轨迹跟踪控制方法

技术领域

[0001] 本发明涉及一种水下剖面监测机器人控制器及自动轨迹跟踪控制方法，属于水下监测机器人控制器设计技术领域。

背景技术

[0002] 水下机器人控制器一般采用集中控制方式，仅在水下机器人仪器舱内设有一个控制器，通过多芯脐带电缆将岸上的控制命令发送到水下控制器进行采集，摇杆状态与电缆的芯数相对应，这样会增加脐带电缆的重量，对水下机器人的姿态控制带来了外界不利影响。另一方面，供电方式多采用船载220VAC电源直接供电方式，通过脐带电缆为水下机器人供电，水下机器人仪器舱内设有电压转换模块；但由于交流电会对通信质量带来影响，使通信误码率较高，水下推进器出现误动作的几率较高。另外，若船载直接转换为水下控制器电路板所需的工作电压，则电压在传输过程中由于电容效应会出现压降，达不到控制板电路板的正常工作电压。控制器之间较多采用485通信方式，其传输距离多能达到1000米左右，由于受到水下扰动的影响，其传输距离进一步缩短；485通信方式难以满足剖面监测具有的大范围、高深度监测的特点。一般情形下光缆通信方式传输距离能达到几公里，而且光缆通信质量好，误码率极低，质量轻，对水下剖面监测机器人带来的扰动小，有利于机器人的精确控制。为此，本发明旨在针对水下监测机器人控制在电压传输、通信距离和精度控制方面的实际需求，进行一种分布式水下剖面监测机器人控制器的设计。为了减小机器人到达预定轨迹点的时间，提出一种有限时间的轨迹点跟踪控制方法，其在机器人的水下控制器中实现，能实现机器人轨迹的快速无抖振控制。

发明内容

[0003] 本发明旨在针对遥控式水下监测机器人控制的实际需求，设计一种水下遥控机器人控制器及自动轨迹跟踪控制方法，可用于船载实时遥控水下机器人，实现水下机器人运动姿态的高精度姿态控制；还可实现水下机器人的自动轨迹跟踪控制。

[0004] 本发明的技术方案如下：

[0005] 一种水下剖面监测机器人控制器，包括置于船载控制箱内的控制器A和置于水下机器人仪器舱内的控制器B两部分，二者通过铠装的脐带电缆进行通信，其中，控制器A实时采集船载控制器控制面板的遥控控制指令，并将控制指令按照自定义的通信协议进行编码和组装成帧，经由通信模块发送到水下控制器B；水下控制器B，实时接收船载控制器A发送的协议帧，并进行协议解码，按照协议格式解析出控制指令，进而控制继电器驱动板上的对应的继电器的开关动作，继而驱动推进电机进行工作，从而实现对水下机器人运动姿态的控制。

[0006] 进一步地，控制器A和控制器B还包括有串口通信模块、串口/光纤转换器和电压转换和稳压模块A、电压转换和稳压模块B；用于水下机器人控制器和船载控制器间的相互通信，其还包括铠装的脐带通信电缆，通过电压转换和稳压模块B将供电电压转换为机器人所需的工作电压，并提供其与控制器A间的远程数据通信，保证信号传输的无失真。

[0007] 进一步地，所述的电压转换和稳压模块A包含电压转换和稳压电路，用于将岸上电源进行AC-DC电压转换和低压差线性稳压的两级电压转换，从而为电路提供稳定的工作电压；电压转换和稳压模块B，用于将电压转换和稳压模块1转换为水下机器人所需工作电压，为控制器B和机器人驱动电机提供必需的工作电压。

[0008] 进一步地，所述水下机器人控制器之间的通信协议，采用自定义的通信协议帧，控制器B按照协议格式解析出控制器A发送的控制指令。协议帧头和帧尾均由1个字节组成。控制命令字由1个字节组成，其相应低4位数据位(从右至左)分别代表左侧水平推进其、右侧水平推进器、左侧垂直推进器和右侧垂直推进器，高4为自动/手动切换。最高位用于区分手动操控还是自动轨迹跟踪控制，如果为0，则为手动操控，如果为1，则为自动轨迹跟踪控制；循环冗余检测(CRC)也由1个字节组成，控制器B通过CRC检测算法计算出检测未位的值，并与接收到的CRC值进行对比，完成对数据帧的差错检测，出现错误则丢弃该数据包，不进行机器人操控。

[0009] 为实现水下机器人对预定轨迹点的跟踪控制，本发明中的自动轨迹跟踪控制方法为，采用有限时间跟踪控制技术，使机器人在有限时间内实现对预定轨迹点的跟踪控制，所述的有限时间控制方法为：

[0010] 在水下机器人运动平面内建立两个坐标系，分别为惯性坐标系和机器人本体坐标系。水下机器人标称运动学方程为

[0011]

[0012] yc＝p

[0013] 其中p＝[x，y，θ]T表示机器人的运动轨迹，q＝[v，ω]T表示机器人方程的控制输入。(x，y)为机器人在惯性坐标系下的位置坐标， 为驶向角。v，ω分别为机器人的线速度和角速度，yc为可测量的机器人运动输出状态。采用的有限时间跟踪控制技术，可使水下机器人在有限时间内实现对预定轨迹点的跟踪。考虑到外界的海流扰动、环境噪声的影响以及轨迹控制的快速性，水下机器人的有限时间轨迹控制器就是设计v，ω的估计值 的控制律使水下机器人在有限的时间内跟踪上预定的参考轨迹点，其中设计的有限时间控制律为：

[0014]

[0015]

[0016] 该发明的有益效果在于：本发明通过两次电压变换技术，通过光纤通信方式实现两个控制器之间的通信，结合使用的零浮力脐带缆，水下通信距离可达数千米。考虑环境噪声对控制系统的影响，对带环境噪声的系统进行扩展卡尔曼滤波(EKF)以得到运动误差状态的最优估计。将上述设计的 和 控制律带入系统方程，得到滤波后的稳态误差系统，使用 和 控制律实现水下机器人对一系列预定轨迹点的跟踪控制，实现水下剖面监测机器人的大范围、高精度运动和姿态控制。

附图说明

[0017] 图1是本发明实施例中的水下剖面监测机器人控制方式结构示意图。

[0018] 图2是本发明实施例中船载控制器(控制器A)结构示意图。

[0019] 图3是本发明实施例中水下运动控制器(控制器B)结构示意图。

[0020] 图4是本发明实施中的船载控制器与水下运动控制器之间的通信协议格式示意图。

[0021] 图5是本发明实施例中船载控制器的软件程序流程图。

[0022] 图6是本发明实施例中水下运动控制器的软件程序流程图。

[0023] 图7是本发明实施例中的机器人轨迹跟踪控制框图。

具体实施方式

[0024] 下面结合附图和实施例进一步地阐述上述发明方案。

[0025] 实施例

[0026] 本发明实施例中的水下剖面监测机器人控制器，包括置于船载控制箱内的控制器A和置于水下机器人仪器舱内的控制器B两部分，二者通过铠装的脐带电缆进行通信，如图1所示。如图2所示的控制器A，实时采集船载控制器控制面板的遥控控制指令，并将控制指令按照自定义的通信协议进行编码和组装成帧，经由通信模块发送到水下控制器B。如图3所示的水下控制器B，实时接收船载控制器A发送的协议帧，并进行协议解码，按照协议格式解析出控制指令，进而控制继电器驱动板上的对应的继电器的开关动作，继而驱动推进电机进行工作，从而实现对水下机器人运动姿态的控制。

[0027] 该控制器A和控制器B还包括有串口通信模块、串口/光纤转换器和电压转换和稳压模块。用于水下机器人控制器和船载控制器间的相互通信，其还包括铠装的脐带通信电缆。通过电压转换和稳压模块B将供电电压转换为机器人所需的工作电压，并提供其与控制器A间的远程数据通信，保证信号传输的无失真。

[0028] 所述的电压转换和稳压模块A包含电压转换和稳压电路，用于将岸上电源进行AC-DC电压转换和低压差线性稳压的两级电压转换，从而为电路提供稳定的工作电压。

[0029] 电压转换和稳压模块B，用于将电压转换和稳压模块1转换为水下机器人所需工作电压，为控制器B和机器人驱动电机提供必需的工作电压。

[0030] 所述水下机器人控制器之间的通信协议，如图4所示，其采用自定义的通信协议帧，控制器B按照协议格式解析出控制器A发送的控制指令。协议帧头和帧尾均由1个字节组成。控制命令字由1个字节组成，如图5所示，其相应低4位数据位(从右至左)分别代表左侧水平推进其、右侧水平推进器、左侧垂直推进器和右侧垂直推进器，高4为自动/手动切换。最高位用于区分手动操控还是自动轨迹跟踪控制，如果为0，则为手动操控，如果为1，则为自动轨迹跟踪控制，如图6所示。循环冗余检测(CRC)也由1个字节组成，控制器B通过CRC检测算法计算出检测未位的值，并与接收到的CRC值进行对比，完成对数据帧的差错检测，出现错误则丢弃该数据包，不进行机器人操控。

[0031] 为实现水下机器人对预定轨迹点的跟踪控制，采用一种有限时间跟踪控制技术，使机器人在有限时间内实现对预定轨迹点的跟踪控制，如图7所示。所述的有限时间控制方法为：

[0032] 在水下机器人运动平面内建立两个坐标系，分别为惯性坐标系和机器人本体坐标系。水下机器人标称运动学方程为

[0033]

[0034] yc＝p

[0035] 其中p＝[x，y，θ]T表示机器人的实际运动轨迹，q＝[v，ω]T表示机器人方程的控制输入。(x，y)为机器人在惯性坐标系下的位置坐标， 为机器人的姿态角。v，ω分别为机器人的线速度和角速度，yc为采用多传感器测量的机器人运动输出状态。

[0036] 水下机器人的自动轨迹控制问题就是要找到控制律v，ω的控制方法使机器人跟踪预定的参考轨迹[xd，yd，θd]T和参考控制输入vd，ωd，如图7所示。

[0037] 在水下机器人本体坐标系下的位姿误差定义为

[0038]

[0039] 其中Te为从惯性坐标系到机器人本体坐标系的转换矩阵。

[0040] 对式(1)求时间的导数，所以位姿状态误差的动力学方程可以写为

[0041]

[0042] 通常实际轨迹跟踪控制会受到各种水下传感器噪声和机器人控制系统自身噪声的影响，采用水下惯性测量模块测定机器人的实际位置坐标，采用磁罗盘测定机器人的实际姿态角。则机器人运动输出状态完全可测定，系统输出ye＝pe，所以带有噪声的机器人位置误差系统可以写为

[0043]

[0044] yev＝pe+V

[0045] 其中W，V分别为机器人位置控制系统过程噪声和机器人位置和姿态传感器的量测噪声，均为高斯白噪声且W和V的协方差分别为Q和R。

[0046] 设 表示p的估计值， 表示pe的估计值，即 表示ye的估计值。所以经过滤波后的稳态误差系统可以写为

[0047]

[0048]

[0049] 水下机器人运动学模型的有限时间轨迹点跟踪问题即通过控制输入 其中 分别可以表示为

[0050]

[0051] 使得对任意初始位置和姿态误差[xe(0)，ye(0)，θe(0)]T，方程(3)的闭环轨迹在有限时间内收敛到0，即 其中tr为首次到达0的时刻。轨迹跟踪控制系统框图如图7所示，其中参考轨迹pd是参考输入qd通过位置和姿态参考系统得到的机器人系统位姿输出。∑1为受控的水下机器人系统，∑d为参考机器人系统的运动学方程

[0052]

[0053] 考虑到水下机器人运动学模型特性，所述自动轨迹跟踪控制需要在有限时间内实现机器人的达到预定的轨迹点。将有限时间控制技术应用于水下机器人的自动轨迹跟踪控制，可按如下设计步骤进行：

[0054] 1)对系统(3)的状态 进行有限时间控制设计，选择 的控制律为

[0055]

[0056] 其中k1，k2＞0，则有 因此，系统(3)在有限时间ts内实现姿态角的镇定， 其中 且稳态的位置误差系统(3)在有限时
间tθ内实现 为了减小机器人系统在预定轨迹点的位置抖动，在预定轨迹点处的两侧设置一个切换控制边界层，用来减缓有限时间控制器姿态的过冲影响。

[0057] 当t＝tθ时，其到达切换控制边界层，但此时到达切换控制边界层时的速度必将引起位置抖振。为消除位置抖振，在预定轨迹的两侧设计一个准滑动模层，其切换控制边界层的宽度为σ＝2k2/k1，机器人运动轨迹到达准滑动模层的时间为t＝tr。当到达切换控制边界层时，的控制律切换为 这样可以确保姿态角 快速达到预定的姿态角。总上，的控制律分为准滑动模层内外两部分，进行设计：

[0058]

[0059] 其中0＜γ1，γ2＜1，对位置误差系统(3)在t＞tr时只需考虑另2个运动状态的控制律设计。采用控制律(5)，将其带入位置误差系统(3)，故此时原误差控制系统可转化为[0060]

[0061] 由于 为在平衡态附近变化的小量，并且设定的的速度分量vd(t)是连续、大小有限的标量，故 所以式(6)可以进一步转化为

[0062]

[0063] 所以上述将状态 首先调整为0的控制律设计方法使得状态 的变化完全由控制分量 来控制。

[0064] 下面对状态 组成的机器人位置误差系统进行设计。

[0065] 对转化后的机器人运动系统(6)，取 的控制律为

[0066]

[0067] 其中k3，k4，k5，k6＞0，0＜β1＜1，β2＝2β1/(1+β1)，得到系统(6)的闭环控制系统形式为

[0068]

[0069] 由式(8)可知，机器人的位置误差 是有限时间到达设计的准滑动模边界层。当到达准滑动模边界层时，的控制律切换为 所以控制律 的设计也分为准滑动模内外两部分进行设计：

[0070]

[0071] 考虑环境噪声对控制系统的影响，对带环境噪声的系统(2)，进行扩展卡尔曼滤波(EKF)以得到运动误差状态的最优估计。将上述设计的 和 控制律带入系统方程，得到系统(2)滤波后的稳态误差系统(3)，使用控制律(5)和(9)实现水下机器人的有限时间控制。

[0072] 本发明实施例中，控制器A和控制器B的电路工作电压为5VDC。工作电压由电压转换和稳压模块A、电压转换和稳压模块B分别提供。电压模块单元通过稳压电路的两级稳压，分别为AC-DC电压转换和低压差线性稳压的电压转换，为装置提供稳定的工作电压。

[0073] 控制器A是水下机器人控制器的核心单元之一，它接收操作指令并通过控制器B控制着整个机器人的工作状态，这里采用低功耗的单片机AVRMega16为控制芯片。在装置上电时，控制器A首先检测其他各个单元的状态，然后启动通信模块，与控制器B进行通信连接。

[0074] 在本实施例中，船载控制器(控制器A)按一定时间间隔采集单片机端口的遥控柄开关状态数据和手动/自动切换状态，并按照自定义的协议帧格式，如图4所示，进行打包组装成帧，然后通过串口通信模块以及串口/光纤转换器，发送到水下控制器(控制器B)，如图5所示。

[0075] 控制器B实时接收船载控制器A发送的协议帧，并进行协议解码，按照自定义的协议格式的相应位解析出控制指令，如图6所示，首先检测控制协议的最高位，如果为0，则为手动操控指令，进而控制继电器驱动板上的对应的继电器的开关动作，继而驱动推进电机进行工作，从而实现对水下机器人的前进、后退、上升、下潜、左右旋转等运动控制；如果为1，则为自动轨迹跟踪控制，该协议的低7位表示此时设定的轨迹点。例如，控制器B接收到控制命令字为0*****XX，首先检测最高位，这里为0，表示手动操控，然后检测最后两位的状态，如果后两位全为1，则为前进指令；如果后两位全为0，则为后退指令；如果为10则为左旋指令，如果为01则为右旋指令。接收到上升和下沉指令时，控制命令字为0***XX**，检测最高位后，再检测第3和第4位的状态，如果全为1，则为上升指令；如果全为0，则为下降指令；
为避免机器人的横滚，当最高位为0时，设定其他形式则为无效的控制字，予以丢弃。控制器B接收到控制命令字为1*******时，此时表示自动轨迹跟踪控制，低7位组成的二进制数表示需要设定的轨迹点，此时只控制垂向电机的正反转。控制器B发出控制指令，控制继电器驱动板上的对应垂向的继电器的开关动作，继而驱动推进垂向电机进行工作。控制器B实时检测深度传感器的值，通过控制器B的具有比较功能的端口，与设定跟踪控制轨迹点的预定值进行比较，利用在控制器B中的有限时间自动跟踪控制方法(如图7所示)，实现对水下机器人快速轨迹跟踪控制，该控制方法在一定程度上能抑制海流的扰动。

[0076] 以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。