7.5 集群高级功能
本讲的例程源码路径:
RflySimAPIs\SimulinkSwarmAPI
RflySimAPIs\PythonVisionAPI
7.5.1 集群数据记录与分析
- 方法整体介绍,平台的数据包含两种:仿真真值数据和飞行
Log
数据- 飞行
Log
数据可以从PX4
的log
日志体系读取,通过QGC
可以获取每个飞控的ulog
格式数据,然后通过Python
环境将ulog
转换为MATLAB
或Python
可识别的csv
格式文件,然后进行分析。 - 仿真真值数据可以通过两种方式获取。
- 一种是离线获取方法,对于第i号飞机,只需要在
PX4PSP
\CopterSim
下新建一个CopterSim+i+.txt
的文件(例如,CopterSim1.txt
),然后每次仿真后会记录仿真真值数据(同RflySim3D
接收数据,包含了位置、速度、电机转速等信息) - 另外一种是在线获取方式,例程见
Simulink
示例:RflySimAPIs\SimulinkSwarmAPI\DataAnalysis
和Python
示例:RflySimAPIs\PythonSwarmAPI\DataAnalysis
文件夹
- 一种是离线获取方法,对于第i号飞机,只需要在
- 飞行
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
能对地面的支撑和碰撞做出反应,平台做了三方面的工作- 需要在
CopterSim
中提前导入地形的.png
(高程矩阵)和.txt
(尺度校准),使得飞机能在任意位置读取当前坐标的地形高度(不需要从UE4
读数据)。 - 在
Simulink
构建的多旋翼模型中增加了高度输入信号TerrainZ
(例程见RflySimAPIs\OtherVehicleTypes\MulticopterModelCTRL\MulticopterNoCtrlWithCollision.slx
),同时在Force and Moment Model
中编写的地面模型Ground Model
来计算地面支撑力和力矩,实现飞机着地时的响应 - 编写了
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
为被撞物体的序号,飞机除了回弹,还会随机让电机发生故障,从而产生坠机效果(坠机后不可恢复飞行)
- 当按下“P”键时,
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() #结束监听