7.5 集群高级功能

本节导航

本讲的例程源码路径：
RflySimAPIs\SimulinkSwarmAPI
RflySimAPIs\PythonVisionAPI

7.5.1 集群数据记录与分析

  • 方法整体介绍，平台的数据包含两种：仿真真值数据和飞行Log数据
    1. 飞行Log数据可以从PX4的log日志体系读取，通过QGC可以获取每个飞控的ulog格式数据，然后通过Python环境将ulog转换为MATLAB或Python可识别的csv格式文件，然后进行分析。
    2. 仿真真值数据可以通过两种方式获取。
      • 一种是离线获取方法，对于第i号飞机，只需要在PX4PSP\CopterSim下新建一个CopterSim+i+.txt的文件（例如，CopterSim1.txt），然后每次仿真后会记录仿真真值数据（同RflySim3D接收数据，包含了位置、速度、电机转速等信息）
      • 另外一种是在线获取方式，例程见Simulink示例：RflySimAPIs\SimulinkSwarmAPI\DataAnalysis和Python示例：RflySimAPIs\PythonSwarmAPI\DataAnalysis文件夹
  • PX4的log记录与分析实验
    • 运行SITLRun快捷方式，并输入1，开启一个飞机软件在环仿真。
    • 用QGC控制飞机起飞，并向前设置一个航路点，让飞机前飞，之后使用Return返航按键，让飞机回到起飞点。
    • 等待飞机降落地面并上锁后，在QGC页面的日志页面 –Log Download – “刷新” –找到最新的日志文件 – 点击“下载”，即可得到log_***.ulg的文件
    • 在线log分析：访问 https://logs.px4.io/ 上传ulog文件，即可分析
    • 本地log分析：重命名log文件为log.ulg，拷贝到RflySimAPIs\Python38Scripts文件夹，然后运行桌面Python38Env快捷方式，在命令窗口输入ulog2csv log.ulg即可得到csv文件，后续可用于MATLAB或Python或Excel数据分析
  • CopterSim仿真log数据获取实验
    • 下面以记录两个飞机（1号和2号）的仿真数据为例，展示数据获取与分析流程
    • 在PX4PSP\CopterSim文件夹下新建两个空白的CopterSim1.m和CopterSim2.m文档文件。（注：需要哪个飞机的就新建对应的.m）
    • 点击桌面SITLRun，并输入2，创建两个飞机，然后进行起飞、前飞、降落等操作（只要点击CopterSim上的“开始仿真”按钮，就会开始记录数据，也可在RflySim3D中按下键盘的“D”键，实时显示当前飞机数据），再结束仿真关闭所有程序
    • 此时用MATLAB打开CopterSim1.m，可以看到内部记录的数据，第一行注释说明了矩阵的数据定义。运行CopterSim1.m和CopterSim2.m文件，可以得到一个CopterData的cell数组
    • 在MATLAB中使用CopterData{i}(j,k)可调用数据，其中i表示飞机ID，j表行数，``k表列数
    • 在MATLAB中可以直接进行数据处理，例如下下图显示了飞机1和2的时间高度图
  • Simulink仿真真值实时显示实验
    • 飞机的仿真真值数据会发送一份给RflySim3D，同时存储一份到.m格式log中，还有一份会发送往30100系列端口，供其他程序实时获取仿真状态
    • 一个Simulink实时读取数据的例子见：RflySimAPIs\SimulinkSwarmAPI\DataAnalysis文件夹
    • 运行SITL脚本或插入两个飞控运行HITL脚本，创建两个飞机的仿真闭环，然后打开DataAnalysisDemo.slx并运行，可以看到左侧UDP模块从30101和30103端口读取到了飞机1和2的数据，并实时绘制轨迹。注：本接口不能联机，只能从本电脑获取数据
  • Python仿真真值实时显示实验实验
    • Python接口文件PX4MavCtrlV4.py也会从30100系列端口实时读取真值数据，并存储在true**系列数据中，请搜索getTrueDataMsg(self)字段来查看代码。
    • 例如：欧拉角trueAngEular、角速度trueAngRate、速度trueVelNED、位置truePosNED，这些数据可以实时绘制轨迹或存储分析。
    • 一个Simulink实时读取数据的例子见：RflySimAPIs\PythonSwarmAPI\DataAnalysis文件夹。其中关键点是调用mav.InitTrueDataLoop()来启用真值数据监听，最后调用mav.EndTrueDataLoop()来结束监听，中间可以用变量self.true***来读取真值数据。
    • 运行DataAnalysisDemo.bat，得到一个飞机的软件在环仿真，再运行DataAnalysisDemo.py即可看到飞机起飞再前飞，同时在程序输出栏观察到真值的输出结果

7.5.2 物理引擎与集群碰撞检测（仅限高级完整版）

  • 从第5讲的教程可知，RflySim平台中RflySim3D只用于可视化的显示，未启用UE4的碰撞引擎。这是因为RflySim平台使用的飞机运动模型是基于Simulink开发的，使平台具备基于模型的快速开发的能力，同时有利于建模的差异化与标准化等
  • 同时，不启用UE4自身的碰撞引擎，也有利于节省UE4的算例空间，使图像更流畅
  • 由于平台不具备物理引擎的特性，为了使CopterSim能对地面的支撑和碰撞做出反应，平台做了三方面的工作
    1. 需要在CopterSim中提前导入地形的.png（高程矩阵）和.txt（尺度校准），使得飞机能在任意位置读取当前坐标的地形高度（不需要从UE4读数据）。
    2. 在Simulink构建的多旋翼模型中增加了高度输入信号TerrainZ（例程见RflySimAPIs\OtherVehicleTypes\MulticopterModelCTRL\MulticopterNoCtrlWithCollision.slx），同时在Force and Moment Model中编写的地面模型Ground Model来计算地面支撑力和力矩，实现飞机着地时的响应
    3. 编写了CollisionDetection碰撞检测模块，它以inFloatsCollision为输入信号（来自UE4的P碰撞模式，含四周射线数据），并响应物理碰撞
  • RflySim3D碰撞引擎P模式实验
    • 运行SITLRun或HITLRun开启仿真闭环，然后在RflySim3D窗口按下“P”键，即可进入碰撞引擎模式，在此模式下飞机会对障碍物做出反应，遇到场景中固有物体（房屋、树木等）会弹开无法穿越，遇到生成的其他飞机（或三维物体）飞机会坠毁。
    • 下面以NeighborhoodPark街区场景为例，展示物理碰撞引擎的效果
    • 进入RflySimAPIs\PythonSwarmAPI\CollisionDemo目录，运行会启动街区场景的一键脚本SITLRunNeighbor.bat或HITLRunNeighbor.bat得到一个飞机仿真闭环
    • 首先，在QGC中将多旋翼起飞，并用鼠标单击一个RflySim3D中房屋方向的位置（例如飞机后方的房屋），点击“前往位置”，飞机就会朝着房屋方向飞去，可以看到飞机此时无物理引擎，可以直接穿越房屋墙壁。此时关闭所有仿真程序，重复上述步骤。
    • 在飞机起飞后，在RflySim3D窗口按下“P”，即可开启碰撞引擎，飞机飞向墙壁时被阻挡，无法穿越，说明碰撞引擎已经工作。
    • 注：发生碰撞后，UE4会提示碰到的物体名称与位置
  • RflySim3D碰撞引擎原理
    • 当按下“P”键时，RflySim3D会在飞机上生成一个前后左右上下的射线来检测飞机各个方向上的距离信息，并实时发送给CopterSim，然后CopterSim将上述射线距离数据发送给Simulink模型的inFloatsCollision输入口，触发其中的物理引擎生成作用力在机体。
    • 也就是说RflySim3D的碰撞引擎并非基于UE4，而是在Simulink中自行编程实现，因此本碰撞模型较为简单但是已经能满足无人机仿真需求（无人机发送碰撞通常坠机，不需要体现精细的碰撞特性）
    • 在Simulink中，碰撞数据inFloatsCollision的前15维为RflySim3D回传的碰撞信息，定义为checksum（校验位取12345）、 CopterID（撞到的飞机ID）、 size（与撞到飞机的重量比）、PosE[3]飞机位置、velE[3]速度、ray[6]四周射线，上述信息通过冲量定理计算出碰撞的力，传到模型的受力模块中，例程见MulticopterNoCtrlWithCollision.slx
    • 注：在碰撞到场景地形和固有物体时， CopterID赋值为0，碰撞引擎会让飞机回弹以不穿越障碍（坠机后可恢复飞行）；当碰撞其他飞机时， CopterID为被撞物体的序号，飞机除了回弹，还会随机让电机发生故障，从而产生坠机效果（坠机后不可恢复飞行）
  • RflySim3D碰撞引擎模式
    • CopterSim与RflySim3D的碰撞引擎信号传递主要包括：所有CopterSim发送飞机数据到RflySim3D中统一显示，开启P模式后，RflySim3D会将障碍信息高速回传给各个CopterSim的30100系列端口。
    • 由于RflySim3D可以接收局域网内的所有CopterSim飞机的数据，在回传时如果单纯采用广播方式通信，会导致局域网内网络拥挤阻塞，因此RflySim3D目前针对局域网通信优化，分为四种模式：P0、P1、P2和P3
    • P0模式（按下P+0键，默认按下P键也会触发本模式）下，RflySim3D会将每个飞机的周围环境距离数据高频传输给本电脑（不会发送局域网）上所有CopterSim。
    • P1模式下，RflySim3D会将每个飞机周围距离数据高频传输给局域网内每个CopterSim（通过指定IP和端口的方式以提高效率）
    • P2模式下，只有飞机发生碰撞过程中（和1秒内），RflySim3D才会将障碍数据低频发送给局域网内的CopterSim（通过指定IP和端口方式），因此从数据频率和目标IP数来优化通信
    • P3模式下，只有飞机发生碰撞和解除碰撞瞬间，RflySim3D会将障碍数据发送给局域网内所有电脑。
    • 单电脑仿真用P0即可；多电脑联机仿真用P1~P3，并根据电脑与飞机数量选择通信优化等级，推荐使用P2模式。注意：多电脑分布式仿真时，可让每台电脑中一个RflySim3D进入P0模式，也可以实现所有飞机的障碍碰撞效果，且碰撞模拟精度最高，但是每个CopterSim的计算量也会较大
    • 注：多电脑分布式仿真时，局域网内只需要一个RflySim3D进入P碰撞引擎模式即可，多个窗口同时开P引擎会造成数据混乱
  • 单电脑两飞机碰撞实验
    • Python实验路径RflySimAPIs\PythonSwarmAPI\CollisionDemo下的UDPModeAPI和MAVLinkAPI文件夹，分别展示了UDP_Mode为UDP模式和MAVLink模式下碰撞例程。
    • 运行CollisionDemo.bat，待两飞机提示3D Fixed后，运行CollisionDemo.py，可以看到Python脚本启用了T轨迹模式和P碰撞模式，然后两个飞机起飞到同一高度的前后位置，1号飞机向前运动并撞上2号飞机，最后两个飞机发生坠机。在RflySim3D和CopterSim和Python输出页面都可以看到碰撞信息。
    • Simulink实验路径为RflySimAPIs\SimulinkSwarmAPI\CollisionDemo，先运行.bat脚本,再运行.slx文件，可以看到与Python相同的实验现象。其中，Simulink可以从20100和30100系列端口获取碰撞数据并显示CrashID，具体实现方法见其中的代码实现与注释
  • 局域网飞机联机碰撞实验
    • 本实验路径为：RflySimAPIs\PythonSwarmAPI\CollisionDemo\LANTest
    • 实验方法：准备局域网内的两台电脑，在电脑1上运行SITLRunNeighborPC1.bat并在电脑2上运行SITLRunNeighborPC2.bat，可以得到飞机1和飞机2的联机仿真。注：两个脚本都开起了联机广播IS_BROADCAST=1且使用街区场景，区别在于START_INDEX不同
    • P0测试实验1：电脑1的RflySim3D进入P0模式，在两台电脑上的QGC分别控制两架飞机起飞，然后电脑1上QGC控制飞机向右飞，飞机相撞。现象：只有飞机1坠毁，飞机2纹丝不动。
    • P0测试实验2：重新打开两飞机联机，这次让电脑1和2的RflySim3D都进入P0模式并相撞。
    • P1~P2测试实验：重新打开两飞机联机，在电脑1的RflySim3D上启用P1或P2模式，然后控制两飞机相撞，发现飞机都能坠落，说明碰撞引擎起作用。
    • P3测试实验：重新打开两飞机联机，在电脑1的RflySim3D上启用P3模式，然后控制两飞机相撞，发现飞机都能坠落，但某些时候某个飞机不会碰撞；重新打开两飞机联机，启用P3模式，去撞击场景中的房屋等，发现有一定穿墙几率。
    • 结论：P0到P3，碰撞引擎的精度逐渐降低，但是能有效降低数据传输和网络延迟。
    • 注意：两飞机发生碰撞时，由于碰撞姿势和位置差异，不一定两个飞机都坠毁，这与实际情况相符
  • 飞机碰撞状态的获取实验
    • RflySim3D在飞机发生碰撞（不包括地面碰撞）时，会向局域网内发送组播（IP为224.0.0.10，端口为20006）结构体{int checksum, int CopterID, int targetID}，其中checksum=1234567890为数据校验位，CopterID为发送碰撞的飞机ID， targetID为本飞机撞到飞机的ID。
    • Python接收这数据的例程为：RflySimAPIs\PythonSwarmAPI\CollisionDemo\CrashMonitorAPI
    • 实验流程：先运行CrashMonitorAPI.bat启动街区场景的2个SITL飞机，再运行CollisionDemo.py开启RflySim3D的碰撞引擎和轨迹显示，并开始实时接收碰撞数据。
    • 然后，用QGC随意控制两个飞机去相互撞击，或者撞击场景内物体
    • 在Python的输出框可以看到当前收到的碰撞信息。
    • 关键代码：
      mav = PX4MavCtrl.PX4MavCtrler(20100) #创建一个实例
mav.sendUE4Cmd(b‘RflyChangeViewKeyCmd P’,0) #开启碰撞引擎
mav.initUE4MsgRec() #开始监听局域网碰撞消息
buf,addr = self.udp_socketUE4.recvfrom(65500) #读取数据
checksum,CopterID,targetID = struct.unpack(‘iii’,buf[0:12]) # 解码
print(‘Vehicle #’,CopterID,‘ Crashed with vehicle #’,targetID) #显示
mav.endUE4MsgRec() #结束监听

results matching ""

    No results matching ""