跟踪控制进行协调控制。簇长通信距离远，具有大范围机动能力，簇长的动力 学行为代表了该簇的行为，其他机器人只需按照轨迹跟踪控制方法跟踪预定规划路径就可 以实现路径跟踪。
[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日