一种用于水下机器人群协同控制的通信装置与方法
一种用于水下机器人群协同控制的通信装置与方法
【技术领域】
[0001] 本发明涉及一种用于水下机器人群协同控制的通信装置与方法。
【背景技术】
[0002] 多个水下机器人的协同控制是多机器人协调合作中的一个典型问题。多个水下机 器人的协同控制可以完成单个机器人无法完成的作业任务，显著提高任务的完成效率。虚 拟结构法和跟随领航者方法是当前主要采用的方法。虚拟结构法要求机器人以刚体上的相 应点的位置和姿态信息作为各自的跟踪目标来形成一定的队形，机器人之间存在较强的耦 合关系，无法实现灵活的队形变换。而跟随领航者方法要求控制一台机器人以一定的角度 和距离跟踪一个或者几个机器人，能够实现编队变形或队形变换，对障碍具有好的适应性， 但要求机器人间的实时通信，对角度和距离测量传感器的性能要求很高，随着距离增加通 信量急剧增大。
[0003] 本发明提供一种用于水下机器人群协同控制的通信装置与方法，采用一种主从递 阶结构协调控制方法，可以根据不同的作业任务，通过协调控制器实现路径规划。将整个机 器人群在空间上分为若干簇，每簇选定一个通讯能力强的机器人作为该簇的簇长，只有簇 长之间进行通信以协调每簇行为，簇间按照刚性结构进行协调控制而簇内则按照主从模式 进行协调控制，这样就可以控制大规模的水下机器人群，有益于节省机器人之间的通信量。
【发明内容】
[0004] 本发明要解决的技术问题是提供一种用于水下机器人群协同控制的通信装置。
[0005] 本发明要解决的另外一个技术问题是提供一种用于水下机器人群协同控制的通 {目方法。
[0006] 对于用于水下机器人群协同控制的通信装置，本发明采用的技术方案是：包括球 形换能器、水声通信模块、协调控制器、运动控制器；
[0007] 球形换能器、水声通信模块、协调控制器、运动控制器依次连接。
[0008] 作为优选，球形换能器为中心频率为35kHz的全方位球形陶瓷换能器。
[0009] 作为优选，水声通信模块采用的主控芯片为Atmegal6。
[0010] 水声通信模块采用中断方式实现信号的接收和发送，发送功率放大模块为16 : 270变压器，脉冲电压峰峰值约800V;其前端接收端包括低通和带通滤波器以及差分放大 器；水声通信模块为双工模式工作，根据通信协议完成数据帧的组帧和拆帧，完成水声信号 的检测、判断与编码、数据的组包和解码；
[0011] 作为优选，协调控制器采用的主控芯片为Atmegal6。
[0012] 作为优选，协调控制器通过RS232串口通信模块分别与水声通信模块和运动控制 器连接。
[0013] 对于用于水下机器人群协同控制的通信方法，本发明采用的技术方案是包括以下 步骤：
[0014] (1)首先建立两个坐标系，分别为惯性坐标系X-0-Y和机器人本体坐标系X-〇' -y， 机器人在惯性坐标系下的坐标用经度、炜度、高度以及三个方向的旋转角度表示，共6个自 由度；
[0015] 水下机器人动力学方程可以描述为
[0017] u=AF
[0018] 其中M(h)GR6X6为对称正定惯性矩阵，
为离心力和科里奥 利力矩阵，ueR6为控制量，A为控制输入转换矩阵，F为机器人的控制力和力矩，
[0019] 令
，则公式⑴可写为
[0021] 引入状态变量
[0023] 可以将公式（1)写为状态方程形式：
[0025] 其中
[0027] (2)将整个机器人群在空间上分为若干簇，每簇选定一个通讯能力强的机器人作 为该簇的簇长，簇长之间进行通信以协调每簇行为，簇间按照刚性结构进行协调控制而簇 内则按照路径跟踪控制进行协调控制；簇长通信距离远，具有大范围机动能力，簇长的动力 学行为代表了该簇的行为，其他机器人只需按照轨迹跟踪控制方法跟踪预定规划路径就可 以实现路径跟踪。
[0028] 本发明的有益效果是：
[0029] 采用的球形换能器具有全方位通信能力，不受机器人运动姿态的约束。协调控制 器采用的主从递阶结构协调控制方法，通过协调控制器实现路径规划，将整个机器人群在 空间上分为若干簇，每簇选定一个通讯能力强的机器人作为该簇的簇长，只有簇长之间进 行通信以协调每簇行为，簇间按照刚性结构进行协调控制而簇内则按照主从模式进行协调 控制，这样就可以控制大规模的机器人群，并且有益于减少机器人之间的通信量。采用最优 轨迹跟踪控制方法完成机器人对期望路径的跟踪，可以最大程度节省机器人的能力消耗， 延长工作时间。
【附图说明】
[0030] 下面结合附图和【具体实施方式】对本发明作进一步详细的说明。
[0031] 图1是本发明水下机器人群协同控制的通信装置实施例的系统组成。
[0032] 图2是本发明实施例的协调控制器的主从递阶结构协调控制方法。
[0033] 图3是本发明实施例的协调控制器的主从递阶结构协调控制方法的簇间控制结 构。
[0034] 图4是本发明实施例的协调控制器的主从递阶结构协调控制方法的簇内控制结 构。
[0035] 图5是本发明实施例的用于机器人之间相互通信的数据帧协议组成结构示意图。
【具体实施方式】
[0036] 图1是一种用于水下机器人群协同控制的通信装置，由球形换能器、水声通信模 块、协调控制器、运动控制器依次连接而成。
[0037] 球形换能器为中心频率为35kHz的全方位球形陶瓷换能器。
[0038] 协调控制器和水声通信模块均采用主控芯片Atmegal6。
[0039] 水声通信模块采用中断方式实现信号的接收和发送，发送功率放大模块为16 : 270变压器，脉冲电压峰峰值约800V;其前端接收端包括低通和带通滤波器以及差分放大 器；水声通信模块为双工模式工作，根据通信协议完成数据帧的组帧和拆帧，完成水声信号 的检测、判断与编码、数据的组包和解码；
[0040] 协调控制器通过RS232串口通信模块分别与水声通信模块和运动控制器连接。
[0041] 上述通信装置具体使用时，由运动控制器通过电机控制水下机器人的运动姿态， 水下机器人再通过位置和姿态传感器（图1中简写为传感器）将位置和姿态信号反馈到运 动控制器。
[0042] 采用上述通信装置对水下机器人群协同控制的方法，具体步骤如下：
[0043] (1)首先建立两个坐标系，分别为惯性坐标系X-0-Y和机器人本体坐标系x-o' -y， 机器人在惯性坐标系下的坐标用经度、炜度、高度以及三个方向的旋转角度表示，共6个自 由度。水下机器人动力学方程可以描述为
[0045] u=AF
[0046] 其中M(h)GR6X6为对称正定惯性矩阵，
为离心力和科里奥 利力矩阵，ueR6为控制量，A为控制输入转换矩阵，F为机器人的控制力和力矩，
[0047] 令
，则⑴可写为
[0049] 引入状态变量
[0051] 可以将（1)写为状态方程形式：
[0053]其中
[0055] (2)将整个机器人群在空间上分为若干簇，每簇选定一个通讯能力强的机器人作 为该簇的簇长，簇长之间进行通信以协调每簇行为，簇间按照刚性结构进行协调控制而簇 内则按照路径 跟踪控制进行协调控制。簇长通信距离远，具有大范围机动能力，簇长的动力 学行为代表了该簇的行为，其他机器人只需按照轨迹跟踪控制方法跟踪预定规划路径就可 以实现路径跟踪。
[0056] (3)图3是簇长的协调控制器软件框架，其中最高层G为离散事件监测器并负责 本簇的性能指标计算，可以演化出一系列的队形模式，并且簇长间可以通过相互通信来拥 有队形模式的副本；其输入为簇长自身的性能指标％，簇内所有簇员的性能指标％,"以及 来自其他簇员的性能（解帧后从字段中提取）；其输出为该簇的期望轨迹坐标凡/, 和性能指标4。图4是簇内机器人的协调控制器软件框架，与簇长的协调控制器不同，簇员 的簇长的协调控制器没有最高层的离散事件监测器。A为路径规划模块，其产生并向下传 送自身协调变量备（期望轨迹坐标），其输入是控制器（的输出：局部跟踪误差为 控制器模块，其输入为机器人的输出向量八.（实际轨迹坐标）和协调变量4 (期望轨迹坐 标），并控制机器人跟踪该期望轨迹，其输出为控制向量和系统局部跟踪误差5。最底层 的Sjf表机器人系统，其输入就是控制器A/,的输出向量气,.，其输出向量只,.代表机器人的实 际位置和姿态。簇长采用广播方式而非点对点通信地将期望轨迹坐标广播给所有该簇 的簇员，簇员进行解帧处理后，将自身需要的期望轨迹字段提取出来，簇员将自身性能 点对点通信的方式的传送给该簇的目标簇员。从而每个簇的簇长可独立地计算本簇的性能 指标。
[0057] 各个机器人性能指标计算方式为
[0059] 其中，e(t)eR12为跟踪误差矢量，％为性能指标计算起始时间，tf为性能指标结 束时间，Q(t)为12X12半正定矩阵，P(t)和
为12X12正定矩阵，化⑴为 6X6正定矩阵。性能指标中第一项反映了对终点误差的要求，积分号后第一项反映了对控 制过程中误差的要求，第二项反省了对能量消耗的要求。
[0060] 对于公式（2)，取Hamilton函数
[0066] 故u#为最优控制，对应得最优误差方程和最优Hamilton函数H#分别为
[0069] 所以要完全确定出最优控制u%还必须求出
[0070] 由Hamilton-Jacobi方程的边界条件知
[0072]由J(e(tf)，tf)为二次型，即
，所以
[0074] 并将其带入Hamilton-Jacobi方程，得
[0076] 所以K(t)应该满足Riccati方程
[0077]
及边界条件K(tf) =P(tf)
[0078] 所以，最优控制为
[0080] 其中
[0081]
，（/tp/h/c均为n阶方阵）是对称正定矩阵。
[0082] 当tf= 且取性能指标中P= 0, Q和R均为常值矩阵，则最优控制律为
[0084] 其中，
，均为6阶方阵）是对称正定矩阵，为满足Riccati 方程
的解。
[0085] 第i簇机器人其性能指标计算方式为
[0087]其中队为第i簇机器人数目，wu为第i簇内第j个机器人的跟踪性能权重；则整 个机器人群的性能指标可表示为
[0089]其中N为簇数目，％为第i簇跟踪性能权重。
[0090]图5是用于机器人之间相互通信的通信帧格式，其帧头占1个字节，其每一位的格 式为00001111，当水声通信模块检测到1个字节的该信号时，表示接受到了数据帧。
[0091] 地址字段包括源地址和目的地址，各占1个字节，其格式为从00000001到 11111111，共127个地址；表示最大可以容纳127个通信终端。通信类型占1个字节，其格 式为00000000,其中00000000表示广播通信方式，11111111表示点到点通信方式。通信内 容字段包括期望轨迹坐标（其中深度值1个字节和方位角一个字节）和实时性能指标1个 字节；然后是循环冗余（CRC)检测字段，占1个字节，帧尾占1个字节，其格式为00001111。 [0092] 以上所述的本发明实施方式，并不构成对本发明保护范围的限定。任何在本发明 的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明的权利要求保护范 围之内。
【主权项】
1. 一种用于水下机器人群协同控制的通信装置，其特征在于，包括球形换能器、水声通 信模块、协调控制器、运动控制器； 所述球形换能器、水声通信模块、协调控制器、运动控制器依次连接。2. 根据权利要求1所述的通信装置，其特征在于，所述球形换能器为中心频率为35kHz 的全方位球形陶瓷换能器。3. 根据权利要求1所述的通信装置，其特征在于，所述水声通信模块采用的主控芯片 为 Atmegal6〇 水声通信模块采用中断方式实现信号的接收和发送，发送功率放大模块为16 :270变 压器，脉冲电压峰峰值约800V ;其前端接收端包括低通和带通滤波器以及差分放大器；水 声通信模块为双工模式工作，根据通信协议完成数据帧的组帧和拆帧，完成水声信号的检 测、判断与编码、数据的组包和解码。4. 根据权利要求1所述的通信装置，其特征在于，所述协调控制器采用的主控芯片为 Atmegal6〇5. 根据权利要求1所述的通信装置，其特征在于，所述协调控制器通过RS232串口通信 模块分别与水声通信模块和运动控制器连接。6. -种用于水下机器人群协同控制的通信方法，其特征在于包括以下步骤： (1)首先建立两个坐标系，分别为惯性坐标系X-O-Y和机器人本体坐标系x-o' -y，机 器人在惯性坐标系下的坐标用经度、炜度、高度以及三个方向的旋转角度表示，共6个自由 度； 水下机器人动力学方程可以描述为 (1) u = AF其中M(h) e R6x6为对称正定惯性矩阵，为离心力和科里奥利 力矩阵，u e R6为控制量，A为控制输入转换矩阵，F为机器人的控制力和力矩， h = [g η ζ φ θ φ\· 令= 则公式（1)可写为 引入状态变量可以将公式（1)写为状态方程形式：(2) 其中(2)将整个机器人群在空间上分为若干簇，每簇选定一个通讯能力强的机器人作为该 簇的簇长，簇长之间进行通信以协调每簇行为，簇间按照刚性结构进行协调控制而簇内则 按照路径跟踪控制进行协调控制；簇长通信距离远，具有大范围机动能力，簇长的动力学行 为代表了该簇的行为，其他机器人只需按照轨迹跟踪控制方法跟踪预定规划路径就可以实 现路径跟踪。
【专利摘要】本发明提供一种用于水下机器人群协同控制的通信装置与方法。该通信装置包括球形换能器、水声通信模块、协调控制器、运动控制器，依次连接。球形换能器完成水声信号的发送和接收，水声通信模块完成水声信号的检测、判断与编码、数据的组包和解码，根据定义的通信协议完成数据帧的组帧和拆帧。协调控制器采用一种主从递阶结构协调控制方法，完成机器人运动路径的规划，运动控制器根据规划的运动路径完成每个机器人对期望运动路径的跟踪。该协同控制方法可以解决由于通讯范围和带宽的约束所导致的整个机器人群跟踪性能下降的问题。
【IPC分类】G05B19/418
【公开号】CN104898594
【申请号】CN201510154636
【发明人】袁健, 周忠海, 李俊晓, 牟华, 张 浩
【申请人】山东省科学院海洋仪器仪表研究所
【公开日】2015年9月9日
【申请日】2015年4月2日
【技术领域】
[0001] 本发明涉及一种用于水下机器人群协同控制的通信装置与方法。
【背景技术】
[0002] 多个水下机器人的协同控制是多机器人协调合作中的一个典型问题。多个水下机 器人的协同控制可以完成单个机器人无法完成的作业任务，显著提高任务的完成效率。虚 拟结构法和跟随领航者方法是当前主要采用的方法。虚拟结构法要求机器人以刚体上的相 应点的位置和姿态信息作为各自的跟踪目标来形成一定的队形，机器人之间存在较强的耦 合关系，无法实现灵活的队形变换。而跟随领航者方法要求控制一台机器人以一定的角度 和距离跟踪一个或者几个机器人，能够实现编队变形或队形变换，对障碍具有好的适应性， 但要求机器人间的实时通信，对角度和距离测量传感器的性能要求很高，随着距离增加通 信量急剧增大。
[0003] 本发明提供一种用于水下机器人群协同控制的通信装置与方法，采用一种主从递 阶结构协调控制方法，可以根据不同的作业任务，通过协调控制器实现路径规划。将整个机 器人群在空间上分为若干簇，每簇选定一个通讯能力强的机器人作为该簇的簇长，只有簇 长之间进行通信以协调每簇行为，簇间按照刚性结构进行协调控制而簇内则按照主从模式 进行协调控制，这样就可以控制大规模的水下机器人群，有益于节省机器人之间的通信量。
【发明内容】
[0004] 本发明要解决的技术问题是提供一种用于水下机器人群协同控制的通信装置。
[0005] 本发明要解决的另外一个技术问题是提供一种用于水下机器人群协同控制的通 {目方法。
[0006] 对于用于水下机器人群协同控制的通信装置，本发明采用的技术方案是：包括球 形换能器、水声通信模块、协调控制器、运动控制器；
[0007] 球形换能器、水声通信模块、协调控制器、运动控制器依次连接。
[0008] 作为优选，球形换能器为中心频率为35kHz的全方位球形陶瓷换能器。
[0009] 作为优选，水声通信模块采用的主控芯片为Atmegal6。
[0010] 水声通信模块采用中断方式实现信号的接收和发送，发送功率放大模块为16 : 270变压器，脉冲电压峰峰值约800V;其前端接收端包括低通和带通滤波器以及差分放大 器；水声通信模块为双工模式工作，根据通信协议完成数据帧的组帧和拆帧，完成水声信号 的检测、判断与编码、数据的组包和解码；
[0011] 作为优选，协调控制器采用的主控芯片为Atmegal6。
[0012] 作为优选，协调控制器通过RS232串口通信模块分别与水声通信模块和运动控制 器连接。
[0013] 对于用于水下机器人群协同控制的通信方法，本发明采用的技术方案是包括以下 步骤：
[0014] (1)首先建立两个坐标系，分别为惯性坐标系X-0-Y和机器人本体坐标系X-〇' -y， 机器人在惯性坐标系下的坐标用经度、炜度、高度以及三个方向的旋转角度表示，共6个自 由度；
[0015] 水下机器人动力学方程可以描述为
[0017] u=AF
[0018] 其中M(h)GR6X6为对称正定惯性矩阵，
为离心力和科里奥 利力矩阵，ueR6为控制量，A为控制输入转换矩阵，F为机器人的控制力和力矩，
[0019] 令
，则公式⑴可写为
[0021] 引入状态变量
[0023] 可以将公式（1)写为状态方程形式：
[0025] 其中
[0027] (2)将整个机器人群在空间上分为若干簇，每簇选定一个通讯能力强的机器人作 为该簇的簇长，簇长之间进行通信以协调每簇行为，簇间按照刚性结构进行协调控制而簇 内则按照路径跟踪控制进行协调控制；簇长通信距离远，具有大范围机动能力，簇长的动力 学行为代表了该簇的行为，其他机器人只需按照轨迹跟踪控制方法跟踪预定规划路径就可 以实现路径跟踪。
[0028] 本发明的有益效果是：
[0029] 采用的球形换能器具有全方位通信能力，不受机器人运动姿态的约束。协调控制 器采用的主从递阶结构协调控制方法，通过协调控制器实现路径规划，将整个机器人群在 空间上分为若干簇，每簇选定一个通讯能力强的机器人作为该簇的簇长，只有簇长之间进 行通信以协调每簇行为，簇间按照刚性结构进行协调控制而簇内则按照主从模式进行协调 控制，这样就可以控制大规模的机器人群，并且有益于减少机器人之间的通信量。采用最优 轨迹跟踪控制方法完成机器人对期望路径的跟踪，可以最大程度节省机器人的能力消耗， 延长工作时间。
【附图说明】
[0030] 下面结合附图和【具体实施方式】对本发明作进一步详细的说明。
[0031] 图1是本发明水下机器人群协同控制的通信装置实施例的系统组成。
[0032] 图2是本发明实施例的协调控制器的主从递阶结构协调控制方法。
[0033] 图3是本发明实施例的协调控制器的主从递阶结构协调控制方法的簇间控制结 构。
[0034] 图4是本发明实施例的协调控制器的主从递阶结构协调控制方法的簇内控制结 构。
[0035] 图5是本发明实施例的用于机器人之间相互通信的数据帧协议组成结构示意图。
【具体实施方式】
[0036] 图1是一种用于水下机器人群协同控制的通信装置，由球形换能器、水声通信模 块、协调控制器、运动控制器依次连接而成。
[0037] 球形换能器为中心频率为35kHz的全方位球形陶瓷换能器。
[0038] 协调控制器和水声通信模块均采用主控芯片Atmegal6。
[0039] 水声通信模块采用中断方式实现信号的接收和发送，发送功率放大模块为16 : 270变压器，脉冲电压峰峰值约800V;其前端接收端包括低通和带通滤波器以及差分放大 器；水声通信模块为双工模式工作，根据通信协议完成数据帧的组帧和拆帧，完成水声信号 的检测、判断与编码、数据的组包和解码；
[0040] 协调控制器通过RS232串口通信模块分别与水声通信模块和运动控制器连接。
[0041] 上述通信装置具体使用时，由运动控制器通过电机控制水下机器人的运动姿态， 水下机器人再通过位置和姿态传感器（图1中简写为传感器）将位置和姿态信号反馈到运 动控制器。
[0042] 采用上述通信装置对水下机器人群协同控制的方法，具体步骤如下：
[0043] (1)首先建立两个坐标系，分别为惯性坐标系X-0-Y和机器人本体坐标系x-o' -y， 机器人在惯性坐标系下的坐标用经度、炜度、高度以及三个方向的旋转角度表示，共6个自 由度。水下机器人动力学方程可以描述为
[0045] u=AF
[0046] 其中M(h)GR6X6为对称正定惯性矩阵，
为离心力和科里奥 利力矩阵，ueR6为控制量，A为控制输入转换矩阵，F为机器人的控制力和力矩，
[0047] 令
，则⑴可写为
[0049] 引入状态变量
[0051] 可以将（1)写为状态方程形式：
[0053]其中
[0055] (2)将整个机器人群在空间上分为若干簇，每簇选定一个通讯能力强的机器人作 为该簇的簇长，簇长之间进行通信以协调每簇行为，簇间按照刚性结构进行协调控制而簇 内则按照路径 跟踪控制进行协调控制。簇长通信距离远，具有大范围机动能力，簇长的动力 学行为代表了该簇的行为，其他机器人只需按照轨迹跟踪控制方法跟踪预定规划路径就可 以实现路径跟踪。
[0056] (3)图3是簇长的协调控制器软件框架，其中最高层G为离散事件监测器并负责 本簇的性能指标计算，可以演化出一系列的队形模式，并且簇长间可以通过相互通信来拥 有队形模式的副本；其输入为簇长自身的性能指标％，簇内所有簇员的性能指标％,"以及 来自其他簇员的性能（解帧后从字段中提取）；其输出为该簇的期望轨迹坐标凡/, 和性能指标4。图4是簇内机器人的协调控制器软件框架，与簇长的协调控制器不同，簇员 的簇长的协调控制器没有最高层的离散事件监测器。A为路径规划模块，其产生并向下传 送自身协调变量备（期望轨迹坐标），其输入是控制器（的输出：局部跟踪误差为 控制器模块，其输入为机器人的输出向量八.（实际轨迹坐标）和协调变量4 (期望轨迹坐 标），并控制机器人跟踪该期望轨迹，其输出为控制向量和系统局部跟踪误差5。最底层 的Sjf表机器人系统，其输入就是控制器A/,的输出向量气,.，其输出向量只,.代表机器人的实 际位置和姿态。簇长采用广播方式而非点对点通信地将期望轨迹坐标广播给所有该簇 的簇员，簇员进行解帧处理后，将自身需要的期望轨迹字段提取出来，簇员将自身性能 点对点通信的方式的传送给该簇的目标簇员。从而每个簇的簇长可独立地计算本簇的性能 指标。
[0057] 各个机器人性能指标计算方式为
[0059] 其中，e(t)eR12为跟踪误差矢量，％为性能指标计算起始时间，tf为性能指标结 束时间，Q(t)为12X12半正定矩阵，P(t)和
为12X12正定矩阵，化⑴为 6X6正定矩阵。性能指标中第一项反映了对终点误差的要求，积分号后第一项反映了对控 制过程中误差的要求，第二项反省了对能量消耗的要求。
[0060] 对于公式（2)，取Hamilton函数
[0066] 故u#为最优控制，对应得最优误差方程和最优Hamilton函数H#分别为
[0069] 所以要完全确定出最优控制u%还必须求出
[0070] 由Hamilton-Jacobi方程的边界条件知
[0072]由J(e(tf)，tf)为二次型，即
，所以
[0074] 并将其带入Hamilton-Jacobi方程，得
[0076] 所以K(t)应该满足Riccati方程
[0077]
及边界条件K(tf) =P(tf)
[0078] 所以，最优控制为
[0080] 其中
[0081]
，（/tp/h/c均为n阶方阵）是对称正定矩阵。
[0082] 当tf= 且取性能指标中P= 0, Q和R均为常值矩阵，则最优控制律为
[0084] 其中，
，均为6阶方阵）是对称正定矩阵，为满足Riccati 方程
的解。
[0085] 第i簇机器人其性能指标计算方式为
[0087]其中队为第i簇机器人数目，wu为第i簇内第j个机器人的跟踪性能权重；则整 个机器人群的性能指标可表示为
[0089]其中N为簇数目，％为第i簇跟踪性能权重。
[0090]图5是用于机器人之间相互通信的通信帧格式，其帧头占1个字节，其每一位的格 式为00001111，当水声通信模块检测到1个字节的该信号时，表示接受到了数据帧。
[0091] 地址字段包括源地址和目的地址，各占1个字节，其格式为从00000001到 11111111，共127个地址；表示最大可以容纳127个通信终端。通信类型占1个字节，其格 式为00000000,其中00000000表示广播通信方式，11111111表示点到点通信方式。通信内 容字段包括期望轨迹坐标（其中深度值1个字节和方位角一个字节）和实时性能指标1个 字节；然后是循环冗余（CRC)检测字段，占1个字节，帧尾占1个字节，其格式为00001111。 [0092] 以上所述的本发明实施方式，并不构成对本发明保护范围的限定。任何在本发明 的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明的权利要求保护范 围之内。
【主权项】
1. 一种用于水下机器人群协同控制的通信装置，其特征在于，包括球形换能器、水声通 信模块、协调控制器、运动控制器； 所述球形换能器、水声通信模块、协调控制器、运动控制器依次连接。2. 根据权利要求1所述的通信装置，其特征在于，所述球形换能器为中心频率为35kHz 的全方位球形陶瓷换能器。3. 根据权利要求1所述的通信装置，其特征在于，所述水声通信模块采用的主控芯片 为 Atmegal6〇 水声通信模块采用中断方式实现信号的接收和发送，发送功率放大模块为16 :270变 压器，脉冲电压峰峰值约800V ;其前端接收端包括低通和带通滤波器以及差分放大器；水 声通信模块为双工模式工作，根据通信协议完成数据帧的组帧和拆帧，完成水声信号的检 测、判断与编码、数据的组包和解码。4. 根据权利要求1所述的通信装置，其特征在于，所述协调控制器采用的主控芯片为 Atmegal6〇5. 根据权利要求1所述的通信装置，其特征在于，所述协调控制器通过RS232串口通信 模块分别与水声通信模块和运动控制器连接。6. -种用于水下机器人群协同控制的通信方法，其特征在于包括以下步骤： (1)首先建立两个坐标系，分别为惯性坐标系X-O-Y和机器人本体坐标系x-o' -y，机 器人在惯性坐标系下的坐标用经度、炜度、高度以及三个方向的旋转角度表示，共6个自由 度； 水下机器人动力学方程可以描述为 (1) u = AF其中M(h) e R6x6为对称正定惯性矩阵，为离心力和科里奥利 力矩阵，u e R6为控制量，A为控制输入转换矩阵，F为机器人的控制力和力矩， h = [g η ζ φ θ φ\· 令= 则公式（1)可写为 引入状态变量可以将公式（1)写为状态方程形式：(2) 其中(2)将整个机器人群在空间上分为若干簇，每簇选定一个通讯能力强的机器人作为该 簇的簇长，簇长之间进行通信以协调每簇行为，簇间按照刚性结构进行协调控制而簇内则 按照路径跟踪控制进行协调控制；簇长通信距离远，具有大范围机动能力，簇长的动力学行 为代表了该簇的行为，其他机器人只需按照轨迹跟踪控制方法跟踪预定规划路径就可以实 现路径跟踪。
【专利摘要】本发明提供一种用于水下机器人群协同控制的通信装置与方法。该通信装置包括球形换能器、水声通信模块、协调控制器、运动控制器，依次连接。球形换能器完成水声信号的发送和接收，水声通信模块完成水声信号的检测、判断与编码、数据的组包和解码，根据定义的通信协议完成数据帧的组帧和拆帧。协调控制器采用一种主从递阶结构协调控制方法，完成机器人运动路径的规划，运动控制器根据规划的运动路径完成每个机器人对期望运动路径的跟踪。该协同控制方法可以解决由于通讯范围和带宽的约束所导致的整个机器人群跟踪性能下降的问题。
【IPC分类】G05B19/418
【公开号】CN104898594
【申请号】CN201510154636
【发明人】袁健, 周忠海, 李俊晓, 牟华, 张 浩
【申请人】山东省科学院海洋仪器仪表研究所
【公开日】2015年9月9日
【申请日】2015年4月2日
最新回复(0)