·5·
兵工自动化
Ordnance Industry Automation
2018-03 37(3)
doi: 10.7690/bgzdh.2018.03.002
lanm张庆刚1，孙康博2，崔 渠3
(1. 西北工业大学计算机学院，西安 710129；2. 西北工业大学电子信息学院，西安 710129；
3. 西北工业大学教育实验学院，西安 710129)
摘要：基于URWPGSim2D 仿真平台，对现有的比赛策略进行优化和改进。首先从整体层次出发提出一种全新的比赛策略；然后针对平台不确定性，引入了PID 模糊控制对基础动作进行优化；最后，针对近距离带球难以精确控制的问题，提出了近距离尾切式顶球算法，并进行模拟实验验证。实验结果表明： 带球总耗时降低约34.78%，带球动作的精准度明显提高，能够实现快速带球的目标。
关键词：2D 仿真平台；PID 模糊控制；顶球算法 中图分类号：TJ6 文献标志码：A
Strategy Optimization for 2D Simulation Water Transportation of Underwater Robot
Zhang Qinggang 1, Sun Kangbo 2, Cui Qu 3
(1. School of Computer Science & Engineering , Northwestern Polytechnical University , Xi ’an 710129, China ; 2. School of Electronics & Information , Northwestern Polytechnical University , Xi ’an 710129, China ;
3. Honors College , Northwestern Polytechnical University , Xi ’an 710129, China )
Abstract: Optimize and improve the existing competition strategy based on URWPGSim2D Simulation platform. First, a new game strategy is proposed from the whole level. Then, based on the uncertainty of the platform, PID fuzzy control[1] is introduced to optimize the basic action. Finally, to solve the problem that it is difficult to dribble balls in a close distance, the algorithm of close distance tail-cutting dribbling is proposed and tested by simulated experiments. The experiment results show that the total time taken by dribbling the ball is reduced by 34.78%, and the accuracy of
the ball movement is improved obviously, which can achieve the goal of fast dribbling.
Keywords: 2D Simulation platform; PID fuzzy control; algorithm of dribbling
0 引言
水中机器人比赛URWPGSim2D 仿真平台是一个仿生机器鱼水球比赛实时仿真系统，通过模拟水中仿生机器鱼各个关节的位姿变化、运动状态变化情况，对水中仿生机器鱼运动学理论、水波扰动理论、碰撞理论、运动策略算法等水中仿生机器鱼体系结构进行仿真测试，从而对水中机器鱼结构系统及控制策略系统进行改进。URWPGSim2D 平台中加入了很多不确定因素，例如平台返回数据具有的随机误差等不确定状态。这些因素的加入，使得平台更具有真实性，能够更好地模拟真实世界的鱼在水中的状态，因此能实现鱼活动的各种解决方案。
1 整体策略优化
该比赛要求在最短的时间内，在水中2D 平台
上用2条机器鱼将0～5号等6个水球搬运到相应编号的球框内。因为要搬运球的数量远多于机器鱼的数量，所以要想尽量缩短搬运时间，2条鱼的选
球策略和每条鱼的带球策略都至关重要。 1.1 现有策略
当前主流的选球策略是：2条机器鱼同时出发，按照现有的水球阵型，从上下2个方向入手逐次搬运水球，这种策略没有考虑2条鱼之间的相互协作关系，只是简单地平分工作任务，不仅费时费工，而且在2个目标球框距离较近时，2条鱼因为没有建立协作关系，无法感知到对方的存在，很容易出现相互干扰的情况。 1.2 策略优化
针对上述选球策略的不足，笔者根据平台中球框与水球的相对位置，并借鉴协同论[1]的相关知识，提出了一套相对完善的动态选球策略。 1.2.1 一鱼带多球 首先，要打破一条鱼一次只能带一个水球进框的思维定式。由图1可知：0号球和4号球位于同一水平线上，所以完全能以一条鱼带2个球的方式，
1
收稿日期：2017-11-20；修回日期：2017-12-26
作者简介：张庆刚(1996—)，男，河南人，本科，从事计算机仿真、水中机器人仿真研究。
·6·
兵工自动化第37卷
先将0号和4号球带进球框。这样一来，不仅可以
自由落体运动实验
减少机器鱼来回往返带球的次数，还可以扩大剩余水球的相对间隔，减少2条鱼带球时出现相互干扰的情况。
1
2
3
4
5
2
1
图1 一鱼带多球
1.2.2 构造评价函数动态选球[2]
在其中一条鱼一次带进2球的前提下，剩余球的相对位置很可能被打乱，这时，动态的选球策略就显得极为关键，笔者根据2条鱼相对于球的距离和角度来动态地决定带球顺序和方式。笔者可以构造出一个优先选择评价函数F (α,β,l )，其中：α表示鱼刚体中心到球中心的角度与标准方向的角度差；β表示鱼速度方向与标准方向的角度差；l 表示鱼刚体中心到球心的距离。综合考虑角度和距离的影响因素，可得到评价函数
()max max max max
+0.1+0.9L l
F L ΦαβΦ--=⨯⨯。
(1)
在如图2所示的案例中，按照式(1)中评价函数
的计算方法，可得到此时剩余球相对于2条鱼选球的评价结果如表1所示。
3
3
44
2
1
5
10
5
1
2
2
图2 动态选球示例 表1 动态选球评价系数
F 1号球
2号球
4号球
5号球
１号鱼 0.105 0.134 0.975 0.071 ２号鱼
0.865 0.433 0.046 0.312
由表中的评价结果可知：此时1号鱼应当去带4号球，2号鱼应当选择1号球，观察图2中2条
鱼相对于水球的位置，这样的选球策略无疑是最佳的，综合角度和距离得到的评价函数可以实现动态选球的效果。
2 基础动作改进
2.1 2 维坐标系内带球动作分解 2.1.1 建立动态坐标系
在机器鱼带球过程中，不仅需要考虑鱼体与球体的相对位置，而且需要考虑球体和球框的相对位置，而此三者的位置一直在不断变化，若是直接以平台默认的坐标轴为标准，决策时的条件判断将会变得十分繁杂。针对这个问题，笔者提出建立动态2维坐标系的解决思路：以球指向目标点的方向为X 轴正方向，以不断移动的球心为原点实时更新2维平面坐标系。动态坐标系如图3所示。
2
服务器管理
31
2
X
Z
立式导热油加热器
X ′
Z ′3
10
24
5
图3 动态坐标系
2.1.2 根据动态坐标将带球动作模块化分解 借鉴分治算法的思想，在以水球为中心的动态坐标平面中，可以根据鱼体所处的位置将整个带球动作分解成远程球、绕球、直冲、近距离修球4个基本模块。每个模块的带球动作都有不同的侧重点，例如球动作更侧重于快速，不强调动作的稳定性，而直冲动作要求在尽可能快的情况下更注重带球动作的稳定。所以将带球动作模块化分解，可以针对具体的情况采取更合适的带球动作，不仅在很大程度上提高带球动作的精准度，还能有效缩短总耗时。带球动作模块化分解如图4所示。
图4 带球动作模块化分解
·7·
张庆刚等：水中机器人2D
仿真水中搬运策略优化第3期 2.2 引入 PID 模糊控制算法
由于2D 水鱼仿真平台的模糊性，且平台返回
的所有数据具有随机性误差，因此没有考虑鱼的跑点和顶球的控制系统误差是不完备的。有必要在原有控制系统的基础之上，借鉴PID 模糊控制[3]算法对原有基础动作模型进行优化。 2.2.1 PID 控制的数学原理
1
()[()()d d ()/d ]u t kp e t e t t TD e t t TI =+
+⎰
。 (2)
2.2.2 基于PID 控制和模糊控制的基础动作优化 由于平台的模糊性以及各种系统误差的存在，该控制
系统是一个不稳定的非线性系统，如果只是
单纯地引入PID 控制，
很难保证带球动作的精准度；因此，笔者综合PID 控制和模糊控制的优点，把系统偏差和球、目标点的相对位置关系联系起来，建立了系统偏差下的负反馈模型。该负反馈机制是把机器鱼将要寻下一个目标点的动作看作是在上一次决策的基础上做出下一次自组织决策的方法。首先要确定一个较小的周期，在每个周期内根据机器鱼反馈回来的动态变量即时进行自主调整。并在反馈调节过程中引入一个隐性变量(模糊控制系数)来矫正鱼体上一拍动作和当前状态误差因素带来的影响。在本模型中笔者利用最大期望算法寻该隐性变量的概率模型中误差参数的最大似然估计。
3 顶球算法
3.1 现有顶球算法
在实现带球策略的时候，有一项关键技术——顶球，即鱼以何种方式控制球的前进。现有的顶球
动作一般采取的是切入圆算法[4-5]，如图5所示，可以描述为机器鱼先直线运动到切入圆上，再沿切入圆的轨迹运动到最佳顶球点G ，最后鱼头以一定的速度顶球。此种顶球方式需要对鱼体进行精确控制，但是当球体离球框较近时，切入圆的直径会急剧变小，鱼体的活动空间也随之减少，在这种情况
下很难实现精准带球动作，带球进框的难度很大。
F F F (1,2)鱼体中心位置
图5 切入圆算法
3.2 近距离尾切式顶球算法
以往的带球动作仅考虑了使用鱼头来顶球的情况，而笔者在代码实测的过程中发现，当球体离球框较近时，仅仅需要鱼尾轻轻一扫就能达到进球目的；因此，建立合适的鱼体几何模型、利用鱼尾或者鱼侧体的摆动可以很巧妙地实现带球要求。
3.2.1 尾切式顶球入框的几何模型
在运动过程中，鱼体尾部一直在中心轴线上下摆动，摆动偏离中心轴线的距离记作L ，经测试发现该距离L 的大小与鱼的速度档位密切相关，相关关系如图6所示。将图中16组数据进行拟合可以得到距离L 关于v 的函数表达式：
320.01220.2239 4.2873 2.2817L v v v =-++。
(3)
6050403020100
5 10 15速度档位
图6 偏移距离变化曲线
3.2.2 近距离尾切式顶球算法
sata连接器
如图7所示，若此时球体中心离球框中心的距离为d ，鱼头离球心的距离为l ，鱼尾离球心距离为m ，鱼尾到球心的角度为b ，则利用三角形的余弦定理，根据鱼体当前的前进方向和鱼尾离球心的距离m ，可以推算出鱼尾到达Y 轴正上方时，鱼尾到球的距离n 。如图8所示，若要利用鱼尾将球压入球框，鱼尾的摆动偏移距离L 必须大于n 和d 之和，由式(3)可知L 的大小和速度的档位有对应的函数关系，由此可推断出鱼体在图7所示的位置处速度和角速度档位选择，从而实现利用鱼尾摆动进球的带球策略。
图7 尾切式顶球入框
手动折弯机
·8·
兵工自动化
第37卷
图8 尾切式顶球入框
4 测试结果和分析
4.1 仿真实验
优化前、优化后带球轨迹如图9、10所示，优
化前后总耗时如图11所示。
图9 优化前带球轨迹
图10 优化后带球轨迹
260240
220200
180160140
120
测试编号
图11 优化前后总耗时变化曲线
4.2 实验结果分析
通过图9和图10的对比可以很明显地看出，优化之后带球轨迹更趋向于平稳；由图11可以发现，优化之后带球总耗时大概降低了34.78%。实验结果证明：这一系列的优化措施既提高了策略的稳定性，又实现了快速带球，达到预期的目标[6]。
5 结束语
笔者基于URWPGSim2D 仿真平台，从整体策略、基础动作和顶球算法3个方面对现有的比赛策略进
行优化和改进。模拟实验验证结果表明：优化后，带球动作的精准度有了明显提高，并且带球总耗时显著降低。
参考文献：
[1] 李柏洲, 董媛媛. 基于协同论的企业原始创新动力系
统构建[J]. 科学学与科学技术管理, 2009, 30(1): 56-60. [2] 高俊杰, 贾翠玲, 李卫国. 水中机器人2D 仿真水球斯
诺克策略优化[J]. 兵工自动化, 2013, 32(12): 78-80. [3] 王述彦, 师宇, 冯忠绪. 基于模糊PID 控制器的控制方
法研究[J]. 机械科学与技术, 2011, 30(1): 166-172.
[4] 苏琦, 李卫京. 基于弦端点法的机器鱼寻迹策略[J].
兵工自动化, 2012, 31(11): 91-92, 96.
[5] 刘艳红, 李建鹏, 徐劲夫, 等. 2D 仿真水球搬运策略[J].
兵工自动化, 2015, 34(12): 90-92.
[6] 姚鹏飞, 高涛，鲍官军, 等. 长臂式仿生软体机器人及
其主动弯曲模型[J]. 机电工程, 2017, 34(4): 346-350.
