水中机器人竞赛2d仿真组系列1多鱼协作系统谢

内容简多机器鱼协作仿真系统素育机器人运动会水中专项2D仿真组竞赛项目所指定等问题。全书共61绪论2统的总体设计3介绍仿真系统的实现过程；第4章介绍仿真系统中碰撞检测的方法和效果；第5章介绍仿真系统在机器鱼2D水球水中专项运动全局视觉组竞赛项目的所有员裁判员和运动员，也适合机器人者参考学前言素机器运动一项国利的伟工程多机器鱼协作仿真系统素育机器人运动会水中专项2D仿真组竞赛项目所指定等问题。全书共61绪论2统的总体设计3介绍仿真系统的实现过程；第4章介绍仿真系统中碰撞检测的方法和效果；第5章介绍仿真系统在机器鱼2D水球水中专项运动全局视觉组竞赛项目的所有员裁判员和运动员，也适合机器人者参考学本书的编者感谢和信息科技大学已毕业和在读的为完成本书所20134第一章绪 引 思考与练习 第二章仿真系统设 引 系统开发选 硬件配 软件环 功能规 架构设 逻辑架 开发架 运行架 思考与练习 第三章仿真系统实 引 思考与练习 第四章碰撞检测和响 引 碰撞建 机器鱼建 碰撞检 算法流 连续碰撞检 碰撞响 算法流 碰撞模拟的实现与结 思考与练习 第五章仿真机器鱼的伪3D绘 问题描 伪3D鱼体绘制方 用Path整合关 尾鳍的伪3D视觉效果处 鱼体建 鱼体坐标 CPG运动学模 绘制技术解 GDI+中的Bezier线 尾鳍建 绘制结果对 思考与练习 第六章仿真物的设计与实 仿真物分 静态 动态 动态物设计思 动态物建模思 动态物实 动态物设计扩 思考与练习 第七章仿真系统应 仿真任务开 仿真任务设计实 仿真任务实现流 仿真策略开 编程相 业务相 调试相 思考与练习 第八章总结与展 参考文 第一章绪论引言自1959年万能动（nimaton公司造出一台真实用工业器1“尤尼Unimae2]水器入深进行摄或样要点是通令进远距作业可以人主要点是有运知学适逻判断交互力可以面分为无人潜水器（RemoyOperatedVehicle，ROV）和自治水中机器人（Autonomous，小范围小规模作业的需求中污染物取样、水中场景图像或等，多数情况下无法使，（Bionics（RobotunaSystm，MCAS；roBOTicsystem；USC大学开发出一个典型群智能机器人系统TheNerdHerd[19]；足球机RoboCupFIRA[20]。，系统。1996年东京大学生产技术（InstituteofIndustrialScience，UniversityofTokyo，IIS）以其研制的AUVTwin-Burger为基础构建了一个水下多机器人系统[22]2001年，海底自治系统（AutonomousUnderseaSystemsInstitute，AUSI）在其研制的多种于小微型水中机器人本身的研究起步就比较晚，且研究者的目光地集中在单个机器人仿生，在小微型水中机器人、仿生机器人和多机器人系统的结合点上，智能控制（InligentControlLaboratory，ICL，作者所在，以下简称PKUICL）设计出了一类PKUICL述三个分支结合点上的研究，并不局限于机器鱼，而是设计了一系列小微型非1.11.2器球比赛这个多水中机器人协作标准问题，被作为第一个中国高校完全自主开发的比赛项器赛暨RoboCup中国公开赛正式项目进行过多届比赛，在机器人科普和水中机器人研究示[6精力实体器人械及耗消耗大可能造人力力的谓浪最要（29]多机器人协作仿真系统多机器人协作是指多个机器人在完成一些集体活动时相互合作的性质。多个机器人通过协调协作以完单机人难完成复杂业可提高机人系在作过的效率可以增强器人统的境适能还使多器人解决的实问拓应用途径陆地多机器人协作仿真系统陆地机器人领域，典型的多机器人仿真系统有卡耐基梅隆大学（CarnegieMellonGaTech）开发的MissionLab[31]；梅森大学（GeorgeMasonUniversity，GMU）开发的MASON[32]；筑波（Tsukuba）电子技术综合（ElectroTechnicalLaboratory，ETL）松原仁（ItsukiNoda）及后续诸多开发者共同开发和的SoccerServer（rcssserver）[33,34]；和·科格勒（MarcoKogler）及后续诸多开发者共同开发和的SimSpark和SoccerServer3D（rcssserver3D）[35]；中国东学佟等人开发和的[36]（NewNeu）系列TBSimTBHard。TeamBots2.0TBHard可以用于控制NomadicTechnologies公司生产的Nomad150机器人和Probotic公司生产的Cye型机器人。而TeamBots仿真环境TBSim最重要的特性是支TBHardTeamBots2.0Nomad150Cye1.31.3TeamBotsTeamBots是开源软件，可以从其。SoccerServerSoccerServer器人足球锦标赛（RobotSoccerWorldCup，RoboCup）[37]的二维（2D）和三维（3D）仿的形状一直都是非常简单的圆形，圆形内填充不同的颜色和数字编号用以指示机器人的方向和区分111.4RcssserverRcssserver3DRcssserverC/S系统，控务端模拟出来的足球机器人。最根本的不同在于Rcssserver3D构建了一个3D仿真3DRcssserver3DRcssserver1.51.5Rcssserver3DRcssserver和Rcssserver3D都是开源软件。前者自1995年开发出来，1997年第一次用于正式比赛，截至20124月已经发布到15.0.1版本20015.26版起，项目托管到著名开源软件网站0.6版之间发布的17个版本与Rcssserver合并管理，在相同的项目主页；自2009年起，0.6.55水中多机器人协作仿真系统AUVAUV的NPSAUVIntegratedSiulato[38]（NPSIS；东京大学水中机器人及其应用（UnderwaterRobotics&ApplicationLaboratory，URAL）Twin-BurgerAUVMulti-VehicleSimulto[39]（MVS；夏威夷大学马诺分校自治系统（Autonomous（DVECS；Solar-poweredAUVCooperativeAUVDevelopmentConcept[41]（CADCON）和中国沈阳自动化所为其AUV[42,43]等。水中仿生机器人协作仿真系统认研究成果与研究目标比较接近时，再进行实体机器人验证的方法具有明显优势和广泛PKUICLRoboCup机器赛，同时也是RoboCup中国公开赛，比赛项目主要采用国际设置，比赛平台多为国际机人竞赛工作委托PKUICL谢广明老师开创自主的水中机器人比赛项目，其中两个重要的子2D3D因此，水中仿生机器人协作仿真平台的研究不仅能为水中仿生机器人协作相关研究提供便利还能水中器人赛提平台动水机器比赛的展此真系模拟生机器各个节的姿变化运动态变情况器比赛行等况一面可以更好地究仿机器的运学理碰理论运策略算等一方还以让的被中水中器赛以及Rbocup机赛所采，取了非好的效果。思考与练习题率；可以增强机器人系统的环境适应能力；还可使多机器人系统解决的实际问题，拓宽(1)(5)57)(8)术。(9)仿人和仿生技术。.第二章仿真系统设计引言系统开发选型硬件配置2.1URSim2DInP42.0GHz或同档次AMDInE73002.66GHz或以2GBDirectX9.0，PixelShader3.0128M80GB软件环境WindowsGDI、DirectX、WPFOpenGL）完全自主构建物理SimSpark、Webots、MRDSSimSparkRoboCup人比赛3D真赛事比赛平台的开发团队（如1.2.1所述）为开发Rcssserver3D这一比赛平台而构建的基础仿真系统2003年发布以来，开发团队一直在对[48]仿真模式方面，模拟的智能体（机器人）客户端通过UDP或TCP协议与SimSpark仿真服务器进机器人改变自身和环境的行为；仿真过程通过仿真更新循环（SimulationUpdateLoop，SUL）址为，可运行于Windows和Linux操作系统。但学（SwissFederalInstituteofTechnology，EPFL）奥利维尔·米歇尔（OlivierMichel）Webots53]置了几乎所有世界知名的实体机器人仿真模型5]，而且这个模型库会随着机器人产业的发展而Webots54]Weotsebots站http://w /download，可运行于Windows、Linux和MacOSX操作系统。但它也采用ODE进行碰撞检测和刚体动力学仿真，主要适用于陆地机器人仿真。的两大技术——并发和协调运行时（ConcurrencyandCoordinationRuntime，CCR）技术决了机器人软件领域最为关键的并发和分布式问题[55]。MRDSEnvironment，VSE个通用的3D仿真器（Simulator基于此仿真器和MRDS提供的基础，可以构建各种具体的机器（Entity（ServiceMRDS（ServiceOrientedArchitecture，SOA）[57]在机器人基础软件领域的一个具体实现，相关组件是CCR和DSS。CCR是一个处理多线程和内部任务同步DSS使用基于简单对象协议（SimpleObjectAccessProtocol，SOAP）的分布式软件服务协议（DecentralizedSoftwareServicesProtocol，DSSP）和超文本传输协议（HypertextMRDS为闭源商业软件，但根据微软的机器人，从MRDS2008R3版开始，提供全功能版本供业界免费使用。MRDS版本可从http:/ /robotics，历史版本均可在微软的中心找到。它只能运行于Windows操作系统，不同版本的MRDS对Windows操作系统版本的要求有所不同，但基本是较新版本的MRDS运行于较新版本的Windows操作系统，比如发布于2010年5月的MRDS2008R3推荐运行的操作系统是Windows操作系统就只有Windows7，需要的.NetFramework版本是4.0。及水环境进行物理建模，便于2D仿真平台向3D仿真平台扩展；二是MRDS有较为便利的编程工具VisualStudio支持；三是MRDS提供免费使用。以MRDS为基础仿真平台，URSim2D开发和运行所需的软件环境便可确定，如表2.2所示。2.2URSim2D操作系 Windows7/XP.NetFrameworkVisualStudio（VS）—编程语 CSharp（C#）Version—XNAFrameworkRedistributableWindowsMediaEncoder9（屏 所需VisualSVNServer StandardEdition2.0.7（SVN版本管理服务器）—功能规划文献[1]提出了一般的多机器人仿真系统所应具备的几个关键功能，分别是对相关模型的器鱼）控制平台的相关特性及MRDS的CCR和DSS组件提供的通信机制，URSim2D的仿真模式考2.1载的动态库（DynamicLinkLibrary，DLL）文件。2.3URSim2DServer2.4URSim2DClient架构设计逻辑架构ClientClientServerClientServerDSSCCRCCR2.1URSim2DSim2DSvr）是服务端提供的DSS服务，主要负责仿真流程控制，是系统的部分；服务端界面（ServerUI，ServerControlBoard）则是服务端提供的人机接口，主要负责仿真过程呈现、人机交互并提供各种辅助功能（如仿真数据实时显示和记录、仿真场景截图和等）接口；ServerControlBoard向Sim2DSvr通过CCR端口进行通信；用户通过服务端界面对各种仿真任务务运行过发送给客户端的状态信息；ClientControlBoard向Sim2DClt通过CCR端口进行通务 2.2URSim2D2.32.3环境设置模块用于构建机器球比赛仿真环境，定义比赛状态控制所需要的环境量等开发架构根据第2.2节所作的软硬件选型，URSim2D采用VisualStudio（VS）2008TeamSuite/ProfessionalwithSP1，C#编程语言进行开发。URSim2D个名为URSim2D.slnVS2008C#解决方案（Solution）生成Solution包含至少10个（可根据需要扩展到）项目（Project，每个Project即为一个逻辑组件，组URSim2D.snk（Assembly。2.5URSim2D无mon.dll加比赛和实验项目的地 实现策略动态加载的辅助无e屏 模块无、 服务端DSS服务和界面模、 客户端DSS服务和界面模 除自有组件外和.NetFrameworkURSim2DMRDSAGEIAPhysX、XNAFramework、WMEncoder9、Excel2003ComLibrary等第组件，如表2.6所示。表2.6URSim2D所需第组MRDS2008不需完整安装。必需的文件有：.Ccr.Adapters.IO.dll .Dss.Runtime.dll、.Dss.Rxy.dll.Dss.Runtime.transform.dll.Dss.Tools.dll等10 运行库文件； mon.dll、 SimulationExy.dll、SimulationEngine.transform.dllDssHost32.exe、DssProxy.exe等3个DSS服务编译运行所需文件。所有文件版本号。MRDS完整安装后bin AGEIA不需要完整安装。必需的文件有：NxCooking.dl、PhysXLoader.dlllXNA需要完整的默认安装。可从微 安装文Excel2003Com不需要完整安装。必需的文件有：office.dll、Interop.VBIDE.dllOffice2003Office11URSim2DSolution（SolutionDirectory）URSim2D。Solution10ProjectProject平行位于SolutionSolution中除各 URSim2D软件在部署方面的设计目标是全绿色，不写表，不往系统拷贝文件。所有要用到的第组件，尽量全部置于程序运行下，实现即拷即用。输出URSim2D下设一级子binbinMRDSDSSDSSDLLMRDS的安装下的bin子中，通过binDssHost.exeDssHost.exebin的上级中寻找名为store的，以查询服务相关的Cache信息，没找到则自动新建该。所以输出URSim2Dbin子。""/referencepath:"$(TargetDir)referencepath:"$(TargetDir)所有Project的输出均设置为“..\URSim2D\bin，不需要完整安装的第组件也置于该下。所有Project的程序集（这里全是指单个DLL文件）的CopyLocal属性设为False于是各Project的程序集的路径部分就全部固定为相对..\URSim2D\bin无论如何生成新版本，都能得到及时更新。Sim2DSvr和Sim2DClt这两个""/referencepath:"$(TargetDir)referencepath:"$(TargetDir)策略Strategy（StrategySolution）所在。StrategySolution提供了大部分仿真任务的策略模板。编写仿真比赛或自编实验项目策略，可直接在StrategySolutionProjectProject。运行架构URSim2DServer.exe（DLL）直接在服务端加RemoteURSm2DClent.ee1V122若URSim2DClient.exe进程与URSim2DServer.exe进程处于不同计算机，则运行URSim2DClient.exe进程计算机上的..\URSim2D\bin\Sim2DClt.manifest.xml<dssp:Service></dssp:Service>配置节，localhost需要修改成URSim2DServer.exe进程所在计算机的IP或主机名（注意，不同主机不能同名，否则客户端连上系统比赛控制模块设计目标任目标任环境信2.42.5球真比状态行控处理，理结经过比转换输到仿数据示界面2.5思考与练习答:本仿真系统提供LocalRemoteLocalURSim2DServer.exeRemote.第三章仿真系统实现引言的软件实现，通常是指从软件生命周期中的从编码到测试的一系列过程[59]。而本书主要阐述URSim2D设计实现过若干问题的解决方案。或多个仿真圆形物（模拟比赛或实验用圆形物的对象。SimulationRoboFish：RoboFish，仿真机器鱼，模拟比赛或实验用仿生水中机器人（机器间隔（如100毫秒。当前平台仿真循环在推荐的硬件配置（见2.2.1）下所耗时间大10-20MsPerycleMPerCyle都取100毫秒，仿周期大110120秒之。真任计时以据MserCyle算出的SimulationAction：仿真动作，包括将策略计算出来的决策命令拷贝到每支队伍每条仿真以仿真任务为中心的对象模型3.1类RoboFish和SimEnvironment虽然从形式上实现了IRoboFish和ISimEnvironment接口，但MissionMission（List<TFish>）成员Fishes monPara）Para，保存一支队伍的各项特性参数如队伍名称、仿真机器鱼仿真环境包括仿真场地、仿真水球、仿真物和仿真通道等元素。仿真环境基类SimEnvironmentFieldFieldInfo，保存仿真场地对象；有一个仿真水球Ball类型的列表（List<Ball>）成员Balls，保存当前仿真环境中的全部仿真水球对象；有一个仿真圆形物RoundedObstacle类型的列表（List<RoundedObstacle>）成员ObstaclesRound，保存当前仿真环境中的全部仿真圆形物对象；有一个仿真方形物RectangularObstacle（List<RectangularObstacle>）ObstaclesRect，保存当前仿真环境中的全部仿真方形物对象有一个仿真通道Channel类型的列（List<Channel>）成员Channels，保存当前仿真环境中的全部仿真通道对象（后期实现时，考虑仿真通道没有实仿真任务基类Mission有一个仿真机器鱼基类RoboFish组成的队伍列表（List<Team<RoboFish>>）TeamsRef；SimEnvironment monPara类型的成员CommonPara具体仿真任务类（如3VS3比赛项目的仿真任务类Match3V3）继承基类Mission，相应的具SimEnvironment。

ServerUICCRServer

List<Rounded

3.1URSim2D3.1URSim2DURSim2D基础模块实现为程序集mon.dll，该程序集提供一个命名空间List<RetangularList<Retangular 的建模功能的直接基础，也是整个URSim2D系统的间接基础，如图3.2所示。图 以仿真循环为主线的运行流程分配决策值SetDecisionsToFishes 程调用SpawnIterator(MissionParaNotification)，实质是将的仿真任务各项参数构计算；计算结果或直接填入或通过网络发送给服务端，由服务端响应事件填入SetDecisionsToFishes所能的数组。基于实验数据的简化动力学和运动学模型简化动力学模型框架介绍指应用于仿真机器鱼的模型，仿真水球和动态物的动力学和运动学计算处理较仿真机器鱼简化动力学模型输入和输出（tCode实体机器鱼运动控制原理：机器鱼本体单片机程序接收到一个速度档位指令和一个转弯档（agularelociy简化动力学模型实质mm/smm/s，具体数据通过实验获取doubledVCodeAndVelocityTable=newdTCodeAndAngularVelocityTable[0dTCodeAndAngularVelocityTable[140~14的角速度值（rad/s，具体数据通过实验获取doubledTCodeAndAngularVelocityTable=new简化动力学模型应用概述仿真周期：仿真周期（iCycleMs，单位毫秒ms）是仿真软件服务端URSim2DServer设定的，3.1（iPsXMmiPsZMm，mm（dodDirectinRadπ、速度值（dVelocityMmPs，单位毫米每秒mm/s、速度方向（直游时与鱼体方向一致）0~2πrad/s。其中绘图中心坐标和鱼体方向统称为位姿。另外，考虑编程方便，可以将速度档位值（iVCode，无量纲，0~1415、转弯档位值（iTCode，无量纲，期参数值，这些参数变量依次命名为iPrePosXMmiPrePosZMmdPreBodyDirectionRad，dPreVelocityMmPsdPreVelocityDirectionRaddPreAngularVelocityRadPsiPreVCode周期的值而更新后参数变量的值称为当前周期的值；也可将更新前参数变量的值称为当前周期应用背景应用目标vCodetCode简化动力学模型应用过程暨运动学模型（iDstVCde（iDestTCode）得到的目标速度值（dVCdeAndeloctyTableiDestCode]（dTCodAndAnularVlocitTableiDestCode]位姿相关计算iPrePosXMmiPrePosXMm=iPosXMm;iPrePosZMm=dPreBodyDirectionRad=dCycleSiCycleMs/1000;SiPosXMm+=(int)(dVelocityMmPs*dCycleS*Cos(dVelocityDirectionRad)+0.5iPosZMm+=(int)(dVelocityMmPs*dCycleS*Sin(dVelocityDirectionRad)+0.5dBodyDirectionRad+=dAngularVelocityRadPs*if(dBodyDirectionRad>=2*PI)dBodyDirectionRad-=速度相关计算c度/角速度所需的时间Tucaucc

Tuc/角加速度值uc

Tuccc 到任一档位c的稳态速度V/角速度W所需的时间Tdcccm ccmm于是加速度值adcm

VTdc/角加速度值dc

WTdcffk个仿真周期，上层策略发下直游档位值cf(k)/转弯档位值cs(k后，经过动力学和运动学计算、碰撞检测和响应计算，仿真机器鱼的速度值V(k，角速度值W(k均为已知，则第k1cf(k1/转弯档位值cs(k1后相应量的计算公ffV(k)

cf(k

V

V(k)

cf(k

auc(k1)T)orV(k)

cf(k

adc(k1)T

auc(k1)T,

V(k)

cf(k

auc(k1)Tfdffssfss

cf(k1)T

V(k)

cf(k1)

cf(k1)TW(k)

scs(ks

W(k)),W(k)

cs(k

uc(k1)T)orW(k)

dcs(kd

dc(k1)TsW(k1)W(k)uc(k1)Ts

W(k)

cs(k

uc(k1)Tsdsd

cs(k

W(k)(Wc(k1)

cs(k1)TD(k)W(k)TD(k1)D(k)W(k)T

W(k)W(kW(k)W(k思考与练习题URSim2D仿真系统采用了哪种模型构建方法，为其基础模块构建了一个以仿真任务为中心（tCode围是{0，1，……，14}1570~6值越小，转弯半径越小；8~14第四章碰撞检测和响应引言仿静态形物在碰处（括测和响rocesColliion过，循环检测每个仿真机器鱼队伍列表中全部仿真机器鱼对象和其他各种对象（包括除自身以外的仿真机鱼对全仿真球对全仿真地界全仿真态圆全仿真静态方等之间碰循检测真水等他动态象和有静对象间的撞（，然由于两个仿真机器鱼之间碰撞检测和响应在任意两个对象间的碰撞处理问题中最为复杂，其他对象间的碰撞检测和响应是其简化版本，在此只探讨两个仿真机器鱼之间的碰撞检测和响碰撞建模簧”建模等[44]。在碰撞检测技术的发展初期，大多采用空间分解法，即将对象用若干个等体的单元格表示[45]基于包围盒的建模方法包围盒（BoundingVolume）在二维平面中占据一定的空间，可包围一个或多个不规则形状14-1AABB坐标轴的最小长方体。AABBAABB包围盒，只需6个浮点数，由此可见，这种包围盒的运算复杂度并不高，检测较为容易。当然，它的缺点也非常明显，AABBOBB是在AABB的基础上，它们的区别主要在于包围盒的轴向是否可变。OBB是包含K-k-DOPK2-14-1K-K-OBB>K-OBB>K-K-圆形方形动态还是静态，Sphere建模方式完全可以符合它的检测需要；由于动态四边形物的轴向和OBB行增加了不少负担。因此，本书利层建模方式，减少了不必要的碰撞检测，首先进行粗略的判断，排除不可能发生的碰撞，其次运用精细化的模型进行进一步检测（4-2。4-2机器鱼建模机器鱼结构及运动原理4-54-44-5机器鱼躯分是内置通讯块制模池等组再刚性料外包绕动尾所产的推运动隐含一个后部向尾行波[4]么机器通过调节三个舵机的摆动频率和摆动幅度，形成由鱼体波幅包络线和正弦曲线合成的鱼体波曲4-64-6包围盒树模型形[50。由此，可以把机器鱼的复杂结构分解开来，逐层检测，由粗略到精细，既提高了平台效的基本模型[514-7外层模型而包围盒树的叶节点则是利用K-Dop模型建模[63]，完成了详细检测阶段的检测任务K-Dop中K的选取是依据叶节点的形状，分别有3-Dop、4-Dop和5-Dop。当机器鱼游动时，其各建模既可以提高平台的运行速率，又可以精确地检测出碰撞的发生。碰撞检测算法流程，时刻但在算机处理只能它顺序执也是说一个间片只能间的碰撞检测进行优先级划分。本书的多物体检测顺序由低到高依次为物与机器鱼和水最高检测在第步仿真台中物对象数目对较所将它其它，4-84-8即可。根据机器鱼的包围盒树建模，可以得到它的凸边形顶点列表Vertex[i][j]，其中i代表叶节点，i[0,8]，j代表每个叶节点下，包围盒K-Dop模型的顶点值，根据选取的k不同，j的取值范围也不同。计算顶点列表中的极值坐标maxxmaxyminxminy，那么，只需比较极值坐标maxxminx与左右边界值maxyminy与上下边界值。4-94-94-10。假设当前碰撞检测的时刻为t1，前一周期的检测时刻为t0100ms时刻，有可能接近于t1时刻，也有可能由于对象运动速度较快，在100ms的时间内，两个检测物t1时刻是否发生碰撞，如果发生碰撞且碰撞距离在设4-10基于分离轴的碰撞检测原理PhasePhase凸多边形的检测假设在二维平面中存在两个凸多边形A和B，如图4-12(a)所示，其中多边形A是凸五边形，B(MinimumIntersectionVector，以下简写为MIV)，具有最小相交向量的垂线记为二者的作用法线(NormalAxisNA)。 4-12for(AB 将AB}球与凸多边形的检测4-13首先，引入一个概念——VoronoiV，我们定义距(xx)2(yy PP(x(xx)2(yy D

VoronoiDr连续碰撞检测坐标转换XOY中，原点坐标为(00)OXOYO(abXOY是以OXOY的Y轴和坐标系ππ)现有一点AXOY坐标系的坐标值为xyXOY坐标系的坐标值(xy如x(xa)cos(yb)siny(xa)sin(yb)cos

（4-（4-路径的发生时刻，所以利用坐标转换，以其中一个运动物体A作为参考系，将另外一物体B置于该坐标系中，判断BAXOY中，假设APA0P(xA0,yA0,A0

（4-（4-PB0P(xB0,yB0,B0

（4-（4-以得到以A（初始时刻）物体为参考坐标系的其它位姿信息，见公式（4-7）和（4-

xB0

yB0

0

0

（4-B0

xBO

yBO

0

y

y

xx

0sin B1 B A A B

（4-

BAA二分法原理14SA)S(B)T=100ms设定碰撞检测阈值为D，当通过分离轴理论计算出的最小相交向量值在阈值范围内AABABBT/2判断AB(1)~(3)步骤，直至检测4-14运动插值和三中有良好的兼容性，故本书运动插值都基于三那么该方法运用到二维平面内首先分析平动插值，假设被检测对象A末状SA0)SA1。在SA0)状态下，A的中心坐标值可表PA0同样的ASA1状态下的中心坐标值为向量PA1。那么利用平动插值后得到的tSAtt0,1，它的中心坐标值为

（4-

qsxiyj

（4-x,y,z，sqsv]，旋转方向R3空间中各轴旋转的角度即可表示为对应的四元数。q [cosX,sinXq q q , q q , （4-将三个向量合成一个向量qyqxqzsxiyjzkscos(Y/2)cos(X/2)cos(Z/2)sin(Y/2)sin(X/2)sin(Z/xcos(Y/2)sin(X/2)cos(Z/2)sin(Y/2)cos(X/2)sin(Z/ysin(Y/2)cos(X/2)cos(Z/2)cos(Y/2)sin(X/2)sin(Z/zcos(Y/2)cos(X/2)sin(Z/2)sin(Y/2)sin(X/2)cos(Z/

（4-SA0)Q0s0x0iy0iz0kSlerp(p,q,h)

psin[(1h)]qsinhQ0sin[(1h)]Q1sinh其中h[0,1]cosp

（4-Q0Q1s0s1v0

（4-

2(sx 12(x2y2)arcsin(2(sy

（4- 2(sz 12(y2z2)碰撞响应算法流程3s按照运动方式的不同，可以将机器鱼二维仿真平台中的动态物体分为两类——一类是机器球它只依靠力在中进的运碰撞过所生的属于式动，而该运行为会因水的力而渐所以合上述两点化条件可以碰撞应4-15的过十分杂由于作用间十短暂所以本将其化为体的移和位移。瞬时碰撞阶段恢复系数撞时不同材质变形恢复能力的参数称为恢复系数，用Cr表示，是碰撞后法向分离速度和碰撞前2/5（a）完全弹性碰撞是刚体间的碰撞，它的恢复系数Cr1（b）非完全弹性碰撞，碰撞后一部分能量被消耗，所以恢复系数0Cr1（c）完全非弹性碰撞情况较为少见，恢复系数Cr0假设A、Bv1n、v2n，碰撞后的法线速度分量分别为v1nv2n，设定正方向为法线方向，那么恢复系数的定义是：vCrv

（4-一样。通过实验可以分别测得球与球碰撞、球与物碰撞、球与边界碰撞的恢复系数。实验过将两条机器鱼鱼头相对放置于水池中，相距大约30cm，如图4-16所示。机器鱼A静止，机器鱼B以速度v2nA动，发生碰撞后记A碰撞后的速度v1n，和机器鱼B后的AAB4-16所示，碰撞点距离机器鱼A的头部顶点为10cm，大约为鱼身长的1/3。机器鱼A静止，机器鱼B以速度v2n向AAv1nBv2nAAB4-19记录方法同（2，其中机器鱼A机器鱼B垂直于A的尾部第一关节放置，碰撞点距离机器鱼A的头部顶点为15cm，大约为鱼体一半的长度。示意图和结果数据如图4-21和4-22所0.026。AAB（2A25cm4-234-240.0081。AAB4-23的鱼体长度（除去尾鳍部分）300mm，可以推出如下的结论：鱼处于摆直状态下的鱼体总长度（除去尾鳍部分）为l，两条机器鱼鱼头相碰的恢复系数为μ，那么与机器鱼BC(1lb)*

（4-C(1lalb)*

（4-4-3物0.05*(1lalb0.05*(1lal0.03*(1lal0.03*(1lal0.05*(1lal响应分析法线等信息。图中的vavbwawb。4-25在法线方向上分解va、vbvan、vbnvat、vbt。mb。mavanmbvbnmavan v

（4-C 最后的速度值vavb。nIri*Iin

（4-其中ri代表作用在质点i上的力矩Ii则是该质点所受内、外力的总和。那么，根据定义量，而机器鱼的转动过程可以认为是围绕其质心进行的，所以作用力矩定义为，质心O作用点P在相应鱼体头尾轴l上投影点的距离d，如图4-26示dPdP4-26依据动量矩定理，机器鱼的动量矩变化L与冲量矩I相同

（4-W为角速度的变化值，规定顺时针方向为正，角度范围是[-π/2,π/2J是转iJ M21i由上述过程，可以得出角速度的变化W

ml

（4-Wmvmv*瞬时响应阶段

1

（4-0.01s(T/10碰撞模拟的实现与结果本书将针对前文总结的碰撞检测方法，分别对仿真平台中涉及到的对象一一进行实验结果分析。由.3水球和物检测连续测包机鱼和的检测水球机器的检机鱼之间的检以结果以仿周期增即时递用截平运行的式来现碰 水球和物的检物物物物 物物物物 物物）） 思考与练习第五章仿真机器鱼的伪3D绘制问题描述URWPGSim2D仿真平台基于RoboticsStudio（MSRS）设计并实现，可使用2维数据分为三个关节，每个关节用一个梯形表示，35.15.1URWPGSim2D1.05.2URWPGSim2D1.0URWPGSim2D仿真平台1.0版本中的物都是固定的，和鱼或球发生碰撞后依旧不变，使URWPGSim2D1.03DGDI+技术[30]，绘制出具有柔性体线的鱼2D3D形间的缝隙，优化了碰撞处理和鱼的体线展现[31]。的动态色彩填充和bezier3D5.3供鱼体各关节的长度，鱼体中心点(FishPosition）的坐标、鱼体朝向（Fishdirection）和鱼尾FishHead.X=FishPosition.X+FishHead.Y=FishPosition.Y+Bezier[33]。如图5.5所示1和点4鳍的始末端点，位于曲线上，而点2和点3线的控制点，在5.5Path渡，需要用Path在GDI+中，绘图工作由Graphics对象来完成，可以创建几个与Graphics分开的Path对象，在pathAll对此，采用GDI+path。可以通过这种有层次的色彩过渡来模拟现实中的立体光影效果。603D3D个。和r;这里，Q为有穷状态集合；S为某一角度下的鱼体状态，至，以5度角顺时针递增；Σ为输状态集F=Q。5.7。5.7GDI+GDI05.8。其中，MaxDistance5.8根据图5.7和图5.8，观察出鱼尾摆动时，随着尾鳍朝向顺时针不断变化，其点集（图5.10）相

Y= Y (5-Y= (5- 其中，θ为尾鳍的方向角。得到公式：TailWidth= 在中轴线上的投影点固定为tailAxis。tailFinRight与tailAxis的距离为distance。只要根据尾鳍方向，给定不同的distance，就能通过固定点tailAxis计算出连续变化的tailFinRight的坐标。而换[34,35]。5.10为了方便测试，先独立建立了一个测试工程，用来鱼体模型。图5.11中标出的点位单位是宽：44底边高：30-20（180）长底边：z（鱼体关节底边定义中的z，和x，y5.11Point[]head=

newPoint(70,25),newPoint(60,17),newPoint[]leftBody=Point[]rightBody=

newPoint(50,25),newPoint(40,50),newPoint(45,80),newPoint(50,100),newPoint(55,130),newPoint(58,140),newnew//newPoint(40,165),newPoint(62,140),newPoint(65,130),newPoint(70,100),newnewPoint(80,50),newPoint(70,25),Point[]leftFin=Point[]rightFin=

newPoint(45,70),newPoint(25,78),newPoint(47,53)newPoint(75,70),newPoint(95,78),newPoint(73,51)角度，线条BA5.12即可。左鱼鳍偏转角度=（鱼的角速度档位-7）*aa5.13其中y5.14CPG中枢模式发生器（CentralPatternGenerator）是一种模拟生物低级神经中枢的自激行为引起自发节律性运动的控制方法[37]。CPG能实现众多的节律运动（如胃肠蠕动、呼吸运动、肢体运动CPG起到调节及稳定作用[38]。相位关系。由于各神经元之间的突触连接具有可塑性，CPG调节CPG网络的连接权重，改变CPG网络的输出模式，可以产生广泛的生物行为[38]。CPG为了实现虚拟仿真机器鱼摆动的平滑性，这里对实体机器鱼的CPG模型进行了改造，并将其应用其中，d是微分符号；n表示周期号码；r表示摆动幅度；i表示关节编号；h为经验常数，通常0，重头开始计算，因此，会出现鱼本书鱼体绘制采用.net自带的GDI+函数库，主要用到技术有bezier曲线，Region归并，渐变画GDI+Bezier图5.15简单曲线示意如图5.15中所示的Bezier曲线，其端点和控制点已标出，起点为p1，前半段曲线受c1吸引，向上弯曲，值得注意的是，c1和p1的连线就是该曲线在p1处引出的切线。后半段手c2吸引，向下弯曲，同样，c2p2p2GDI+bezierGraphicsPenGraphics类中定义了DrawBezier函数Pen类中含有设定的曲线参数Pen类的实例输入到DrawBezierDrawBezierbezier起点的坐标是(00)而两个控制点的坐标分别为(80,40)、(1600,300(200,20)，myGraphics.DrawBezier(myPen,0,0,80,40,160,300,200,5.16abc图5.16曲线及切线示意像变换[3435]，以模拟完整的投影变化。xna.Vector3tailFinLeftStart;xna.Vector3tailFinRightStart;xna.Vector3xna.Vector3xna.Vector3tailFinLeftMiddle;xna.Vector3tailFinRightMiddle;Point[]tailFinLeftArcPoint[]tailFinBackArcPoint[]tailFinRightArc{distanceToAxis1;distanceToAxis4;xna.Vector3temptailAxis2;tailAxis2=tailAxis1;tailAxis1=temp;temp=tailAxis3;tailAxis3=tailAxis4;tailAxis4=temp;}GDI+技术进行如下绘制。#region尾鳍绘制GraphicsPathpathTailFin=newpathTailFin.AddCurve(tailFinLeftArc,0.5f);pathTailFin.AddCurve(tailFinRightArc,0.5f);#regionGraphicsPathpathLeftFin=newxna.Vector3leftFinControl1=newxna.Vector3((pointReal14.X+100*halfSinTheta),0,(pointReal14.Z-100*xna.Vector3leftFinControl2=newxna.Vector3((pointReal7.X+100*halfSinTheta/2),0,(pointReal7.Z-100*halfCosTheta/GraphicsPathpathRightFin=newxna.Vector3rightFinControl1=newxna.Vector3((pointReal13.X-100*halfSinTheta),0,(pointReal13.Z+100*xna.Vector3rightFinControl2=newxna.Vector3((pointReal5.X-100*halfSinTheta/2),0,(pointReal5.Z+100*halfCosTheta/GDI+的线性渐变画刷对鱼体进行染色。GraphicsPathpath=newGraphicsPath();Penmypen=newPen(Color.Red);LinearGradientBrushbrush=newLinearGradientBrush(newPoint(700,800),new//GraphicsPathmyPathLeft=newRegionr=newGraphicsPathmyPathRight=newmyPathRight.AddCurve(rightFin,5.175.183D思考与练习题并，渐变画刷等第六章仿真物的设计与实URWPGSim2D仿真物分物分为静态物和动态物静态物指颜色大小位置都固定不变的物，如抢球大项目中的球门柱和克项目中的边界等。动态物指颜色、大小、位置、速度均可改变的物，如新抢球大项目中球门前上下移动的物，该类物灵活多变，需静态物的质量默认为无穷大，这样和机器鱼或者球碰撞后，不会改变静态物的位置。为了突出物的立体效果，平台中的物的颜色都是从图形中心向图形顶点进行线性6.1图6.1静态动态1、随机动态随机设定速度和轨迹会使物在比赛过缺少预判性也因此为比赛策略的编写平添不可控因素，使得比赛结果的运气成分变大。比如运气差的一方，随机移动的物会一直阻挠2、固定动态固定化物的速度和轨迹可以使选手通过计算准确掌握物的运动特性从而避开障动态物设计思最优化，必须趋吉避凶，动态物的运动特性和形态特性。规避型动态以2v2抢球大为例，如图6.2所示，可以在球门前添加上下移动的长条形物，进攻态物的长边推走而功亏一篑。同样，一旦进球，防守方要把球捞出，亦要避开动态物，不然球会态物的短边碰回自己的门内相对而言防守方更具优势因为防守方只要避开短边将球挡在动态物的长边范围内，静待球态物顶走即可进行成功的防守。图6.2抢球大项目中的动态强运型动态强运型动态物是指可以利用来运球的移动物其显著特征是动态物存在一定该物从左往右移动时，“V”字形状使得球一旦进入其运动范围内，就会陷入夹角中，被障下方的物有利于右方队伍的进攻。得球一旦被，就很难被鱼捞出，易于持球。相反，“V”的背部却很难持球，最多是将球向图6.3水球5vs5中的动态动态物建模思其父类--静态物，以提高代码的复用性。动态物实用面向对象的思想将动态物抽象为RectangularDynamic类，该类实现ICloneable和关于动态物循环运动的实现，在本模型中是以周期计数的方法实现的。用circleTimes变量单趟循环运动的周期数，用TimesCouter变量充当计数器，每个周期加1，当与//////publicclassRectangularDynamic:ICloneable,{publicstringfalsepublicboolIsDeletionAllowed;public{for(inti=0;i<4;i++)PolygonVertices.Add(newxna.Vector3(0,0,}}//////<paramname="strName">仿真方形物名称///<paramname="positionMm">仿真方形物位置（在场地坐标系中，以mm为///<paramname="colorBorder">仿真方形物轮廓颜色///<paramname="colorFilled">仿真方形物填充颜色///<paramname="borderWithPix">仿真方形物轮廓像素宽度///<paramname="lengthMm">仿真方形物长度（以mm为单位///<paramname="widthMm">仿真方形物宽度（以mm为单位///<paramname="directionDeg">仿真方形物方向（在场地坐标系中，以弧paramname="velocityMmPs">当前速度值，单位mm每秒paramname="velocityDirectionRad">当前速度方向备份，单位弧度rad，值域[-PI,PI)</param>///<paramname="angularVelocityRadPs">当前角速度值，单位弧度每秒paramname="circleTimes"publicRectangularDynamic(stringstrName,xna.Vector3positionMm,ColorColorcolorFilled,intborderWithPix,intlengthMm,intwidthMm,floatdirectionDeg,floatvelocityMmPs,floatvelocityDirectionRad,floatangularVelocityRadPs,intcircleTimes){Name=strName;PositionMm=positionMm;ColorBorder=colorBorder;ColorFilled=colorFilled;BorderWidthPix=borderWithPix;LengthMm=lengthMm;WidthMm=DirectionRadxna.MathHelper.ToRadians(directionDeg);IsDeletionAllowed=true; //默认允许删除VelocityMmPs=velocityMmPs;VelocityDirectionRad=velocityDirectionRad;AngularVelocityRadPs=angularVelocityRadPs;CircleTimes=circleTimes;TimesCouterCircleTimespublicCollisionTypepublicCollisionType**，单位publicintCircumcircleRadiusMm;4个顶点列表，4位publicList<xna.Vector3>PolygonVertices=new下所示，构建了一个长宽100mm，水平朝向，边缘厚度为0的矩形动态物：publicvoid{LengthMm=WidthMm=DirectionRad=BorderWidthPix=0}定义以上参数后，调用CalculateCollisionDetectionParas()方法计算出碰撞时刻物外接圆半径、4个顶点坐标的值，具体如下代码所示，其中PositionMm为物的中心坐标。publicvoid{CircumcircleRadiusMm=(int)Math.Sqrt(LengthMm*LengthMm+WidthMm*WidthMm)/2;floatsine=floatcosine=/2+WidthMm*sine/2,0,PositionMm.Z-LengthMm*sine/2-WidthMm*//2+WidthMm*sine/2,0,PositionMm.Z+LengthMm*sine/2-WidthMm*//2-WidthMm*sine/2,0,PositionMm.Z+LengthMm*sine/2+WidthMm*//2-WidthMm*sine/2,0,PositionMm.Z-LengthMm*sine/2+WidthMm*/}动态物绘制模块实及的全部动态仿真物恢复默认位置的情况有以下3种：publicvirtualvoidResetDynamicObstacles(){publicvoidDraw(refGraphics{DrawHelper.DrawRectangle(refg,PositionMm,DirectionRad,LengthMm,WidthMm,BorderWidthPix,ColorBorder,ColorFilled);} 6.4图6.5动态物运动轨迹截动态物设计扩思考与练习题能上划分，分为规避型动态物和强运型动态物。第七章仿真系统应用仿真任务开发URSim2D还可用于水中多机器人协作仿真的实验平台。URSim2D仿真任务开发过程包括设计两个阶MissionURSim2D.Sim2DSvrSim2DSvrServiceprivate仿真任务设计实例下面以水中机器人2D仿真比赛的一个比赛项目[62]——2D仿真协作过孔为例，展示仿真任务比赛内容2D仿真协作过孔为1伍参与，每支队伍2仿真机器鱼，用到1真水球，3比赛场地2D5.17.12D比赛过程初始状态，121项目规则比赛时间计分规则0策略编写2D91，10IsCollidedBallAndFish0AtStage11IsCollidedBallAndFish1AtStage1220水球标志IsCollidedBallAndFish0AtStage21碰过仿真水球标志IsCollidedBallAndFish1AtStage2330志IsCollidedBallAndFish0AtStage3、仿真机器鱼1碰过仿真水球标志IsCollidedBallAndFish1AtStage3440志IsCollidedBallAndFish0AtStage4、仿真机器鱼1碰过仿真水球标志IsCollidedBallAndFish1AtStage4ExecutingTaskNo。8个的数据类型是bool取值为truefalse9个的数据类型是int取值范围为1、2、3、4，取值1表示仿真水球尚未被推过第一个孔，正在执行协作任务1；取值2表示3/43/4。= = intno=Convert.ToInt32if((no1)&&(f01true)&&(f11true))12111if((no2)&&(f02true)&&(f12true))22221f((no3)&&(f03true)&&(f13true))3，2331if((no4)&&f04true)&&f14true))42仿真任务实现流程ProjectMatchCSharp（TwishPushallViChannl.cs，添加mon，.Xna.Framework等必需的并使用using语句引入；也可直在代码文件中新建命名空间MatchMatch命名空间下新建三别继承于RoboFish、SimEnvironment和Mission类（如FishTwoFishPushBallViaChannel、Mission（TwoFishPushBallViaChannel）IMission（Mission一定需要实现的方法，7.17.1是 是是是是是是是否s否否否否在ProjectSim2DSvr下Sim2DSvrService类的privatevoidInitMission(stringstrMissionNameintmissionMinutesSwitch2Dcasecase//实例化TwoFishPushBallViaChannel或获取它的唯一实例并将其接口 的MyMission.IMissionRefMyMission.Instance().MissionRef=(Mission)TwoFishPushBallViaChannel.Instance(); para=new monPara("协作过孔",1,2,60*missionMinutes,100,true,在ProjectSim2DSvr下ServerControlBoard这个Form上的 petitionItem下拉框控最后至少重新生成MatchSim2DSvrProjectProject仿真策略开发以下描述中%URSim2D%URSim2DSolutionDirectoryD:\Mys\VisualStudio编程相关编写策略，首先需要建立基于.NetFramework3.5使用C#编程语言的Windows（ClassLibrary）VS2008（Project）及相应的解决方案（SolutionURim2DStratgy类必须继承自MarshalByRefObject类并实现IStrategy接口，为GetTeamName和GetDecision.Dss.Base.dll、.Xna.Framework.dll mon.dll可以直接使用%URSim2D%\Strategy\下的StrategySolution即Strategy.sln，然后在StrategySolutionProjectStrategyProject。也可新建策略Project（如Strategy3VS3）的同时新建Solution，并让ProjectSolution的下级中，Solution可以放在任意位置，Solution名称可以任意，推荐使用（PropertiesURim2D.tratey修改类名（Classname）等。默认建立的cs文件（如Class1.cs）重命名为Strategy3VS3.cs（如ClassLibrary1URSim2D.Strategyclass（Class1StrategyMarshalByRefObject,IStrategy。类Strategy中必须重载MarshalByRefObject的InitializeLifetimeService现returnnull，StrategyRemotingException）类Strategy的实例在加载完毕后一直存在于内存中，除非仿真任务运行过更换策略，usingusingusingSystem.Collections.Generic;usingSystem.Linq;usingusingxnausingnamespace{publicclassStrategy:MarshalByRefObject,{ codeneverbechangedor//////overridetheInitializeLifetimeServicetoreturnnullinsteadofavalidILeaseimplementation///toensurethistypeofremoteobjectnever//////publicoverrideobject{returnnull;//makestheobjectlive }//////决策类当前对象对应的仿真任务参与队伍的决策数组///privateDecision[]decisions=第一次调用//////returns>队伍名称字符串</returns>publicstringGetTeamName(){return"3VS3Test}/////////用于作为索 Mission对象的TeamsRef队伍列表中代表当前队伍的元///<returns>当前队伍所有仿真机器鱼的决策数据构成的Decision数组对象publicDecision[]GetDecision(Missionmission,int{//决策类当前对象第一次调用GetDecision时Decision数组 if(decisions==null)decisions=new }return}}}Team（GetDecisionStrategy过程较复杂的情况下，建议将各过程封装成Strate