7.4 Python集群接口示例

本节导航

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

7.4.1 Python集群接口总体介绍

  • 例程文件夹见RflySimAPIs\PythonVisionAPI。注意Python代码阅读和运行需要安装VS Code并关联平台Python38环境，具体安装流程请参考高级课程第6讲。
  • 集群控制的Python通信接口文件和视觉Python控制接口完全相同，都在PX4MavCtrlV4.py接口文件中，其基本使用方法与高级课程第6讲相同。
  • 例程文件夹中包含了：
    1)基础控制PythonCtrlAPIs
    2)集群控制PythonSwarm
    3)碰撞检测CollisionDemo
    4)数据分析DataAnalysis
    5)灯光秀展示LightShowSwarm
    6 )多Pixhawk自动重启RebootPixViaUDP
    7)无PX4的集群控制SwarmSimNoPX4等例程，覆盖了集群控制多数需求

    注：超过4飞机以上例程进支持高级完整版

  • Python集群控制使用的接口文件为PX4MavCtrlV4.py和PythonVision视觉控制使用的API文件完全相同。
  • 根据第6讲的教程可知，Python与CopterSim/PX4通信包含了两种通信模式（UDP结构体和MAVLink数据流）和两种优化模式（Full和Simple）
  • MAVLink_Full和MAVLink_Simple通信模式下（对应InitMavLoop(2)和InitMavLoop(3) ）直接使用MAVLink数据流进行Python与PX4的通信（经过CopterSim中转），数据量大、占用带宽高，但是和真机更贴近且功能完善，在大规模集群时易阻塞宽带；
  • UDP_Full和UDP_Simple模式（ 对应InitMavLoop(0)和InitMavLoop(1)）下，Python发送精简UDP结构体消息与CopterSim通信，后者压缩或解压后再通过MAVLink与PX4通信，这种方法能够有效的减小局域网内的数据量，适合集群仿真控制。
  • Python脚本与CopterSim通信使用了20100+2i系列接口来接收飞控数据（ InitMavLoop 函数启用监听，uav***变量来读取数据），30100+2i系列接口来接收真值数据（InitTrueDataLoop函数来启用数据监听，true***变量来读取数据）
  • 除了与PX4与CopterSim联合仿真（6DOF高粒度模型）；集群平台还支持Python与RflySim3D精简仿真（质点低粒度模型），使用Python内部精简飞机模型，计算量小，支持大规模集群。

7.4.2 集群接口实验介绍

  • 例程文件夹见：RflySimAPIs\PythonSwarmAPI\PythonSwarm\MAVLinkFull4Swarm
    • 先运行MAVLinkFull4Swarm.bat开启四个飞机SITL仿真，然后运行MAVLinkFull4Swarm.py可以看到飞机起飞降落。
    • Python控制例程的关键在于通过PX4MavCtrl.PX4MavCtrler函数新建4个mav通信实例，分别绑定四个CopterSim的接收端口20100+2\*i，即mav=PX4MavCtrler(20100)、 mav1=PX4MavCtrler(20102)、 mav2= PX4MavCtrler(20104)、 mav3= PX4MavCtrler(20106)
    • 然后，依次使用mav到mav3控制四个飞机的飞行，并获取四个飞机的状态
    • 注：本例子使用了MAVLink_Full的UDP Mode通信模式。在Python控制程序中使用默认值mav.InitMavLoop()或指定值mav.InitMavLoop(2)；在bat脚本中使用UDPSIMMODE=2
    • 发送期望位置和朝向可使用SendPosNED函数（北东地为xyz轴的地面系）和SendPosFRD函数（前右下为xyz轴的机体系），可配合SendCopterSpeed函数来设置飞往目标点的位置
    • 发送期望速度和偏航速度可使用SendVelNED函数（北东地为xyz轴的地面系）和SendVelFRD函数（前右下为xyz轴的机体系）来实现
  • 例程代码解析:
  • 八飞机例子（限完整版）
    • 例程文件夹见：RflySimAPIs\PythonSwarmAPI\PythonSwarm\UDPSimple8Swarm
    • 先运行UDPSimple8Swarm.bat开启八个飞机SITL仿真，然后运行UDPSimple8Swarm.py可以看到飞机起飞，然后飞同心圆。
    • 本Python控制例程的关键在于新建八个PX4MavCtrl通信示例，然后将其存在MavList列表中，使用端口20100+i\*2（其中i=1到8）。本例使用InitMavLoop(1)即UDP_Simple模式来优化通信
    • 在之前的例子中，每个飞机发送SendPosNED都是以其起飞点为原点，但每个飞机起飞点不相同，而在集群控制时，通常需要获取飞机在UE4中的统一全局坐标，来实现协同飞行。
    • 通过UE4全局坐标位置uavGlobalPos与飞机起飞坐标系位置uavPosNED作差，可以求出每个飞机本地位置在UE4中心差值列表Error2UE4Map，通过他可解算UE4坐标系下的期望目标位置targetPosE在飞机本地起飞LocalNED坐标系下的投影
    • 然后，使用SendPosNED发送处理过的目标位置，即可实现所有飞机在同一坐标系下的运动
    • 在本例程中，所有飞机会起飞然后汇合在同一个点，最后一起做飞同心圆运动

7.4.3 Python集群分布式仿真（限完整版）

  • 例程文件夹见：RflySimAPIs\PythonSwarmAPI\PythonSwarm\UDPSimple16Swarm2PC
  • 本例子展示了用多台电脑在局域网内联机，组成大的飞机集群，并实现Python的集群控制。
  • 同路由下两台电脑共16飞机，每台电脑8个飞机，为了减小局域网通信量使用UDP_Simple模式
  • 实验流程如下：
    1. 需要知道两台电脑的IP地址，例如电脑1：192.168.3.55，电脑2：192.168.3.80
    2. 需要将Python总控脚本放在一台电脑上运行（可以是两台仿真电脑中一台，或者是第三天电脑，但是需要知道IP地址），这里设为电脑1（IP为192.168.3.55
    3. 阅读UDPSimple16Swarm.py，根据注释去修改UDPSimple1_8Swarm.bat和UDPSimple9_16Swarm.bat脚本。主要有：START_INDEX（起始序号），TOTOAL_COPTER（总数），IS_BROADCAST（总控电脑IP）这几个量
  • Python代码示例 
# 电脑1配置区，设IP地址为192.168.3.55，同时作为Python控制中心
VehilceNum1=8
IPOfPC1='127.0.0.1' # 因为我的python脚本要运行在第一台电脑上，因此与本机LocalHost通信即可
# 注：UDPSimple1_8Swarm.bat 中的IS_BROADCAST=0 也要修改成IS_BROADCAST=127.0.0.1
# 电脑2配置区，设IP地址为192.168.3.80，作为仿真机
VehilceNum2=8
IPOfPC2='192.168.3.80' # 第二台电脑的IP地址，用于向目标电脑发送Python控制指令
# 注：UDPSimple9_16Swarm.bat 中的IS_BROADCAST=0 也要修改成第一台电脑地址IS_BROADCAST=192.168.3.55
# 注：UDPSimple9_16Swarm.bat的飞机编号要从9开始，因此要修改START_INDEX=0为START_INDEX=9
# 后续电脑配置取
#VehilceNum3=这台电脑上运行飞机数量
#IPOfPC2='第三台电脑IP地址'
# 注：bat脚本中，IS_BROADCAST=第一台（python脚本运行电脑）的IP地址
# 注：bat脚本中，START_INDEX=起始飞机的编号
# 飞机总数
VehilceNum = VehilceNum1+VehilceNum2
# 注：UDPSimple1_8Swarm.bat和UDPSimple9_16Swarm.bat中在START_INDEX下面添加总飞机数的赋值语句，“SET /a
TOTOAL_COPTER=16”
# 注：后续随着飞机的增加，每个bat脚本都需要更新TOTOAL_COPTER为总数，便于本脚本的自动排序，如果不需要排序（所有飞机初始在一个点），这个值不需要设置
  • UDPSimple9_16Swarm.bat脚本关键代码，本脚本需要运行在电脑2上 
REM 设置飞机起始编号为9，总飞机数量为16，“REM”表示注释不会被执行
SET /a START_INDEX=9
SET /a TOTOAL_COPTER=16

REM IS_BROADCAST变量设置联机属性，0表示本机通信，1表示广播通信（局域网负担重，不适合大规模集群），这里也可以设置
目标电脑的IP地址，或者多台目标电脑的IP地址序列。这里的目标电脑是指期望飞机状态和接收控制指令的电脑，本例中是电脑1，因
此IP设置为192.168.3.55
SET IS_BROADCAST=192.168.3.55

REM UDP Mode表示通信协议，包括：0: UDP_FULL,1:UDP_Simple, 2: Mavlink_Full, 3: Mavlink_simple，这里使用UDP_Simple的协议来减小局域网数据量，使仿真更流畅
SET UDPSIMMODE=1

REM 设置本电脑上启动SITL飞机的数量，这里是8
SET /a VehicleNum=8

  • UDPSimple1_8Swarm.bat脚本关键代码，本脚本需要运行在电脑1上 

    SET /a START_INDEX=1
SET /a TOTOAL_COPTER=16
SET IS_BROADCAST=127.0.0.1
SET UDPSIMMODE=1
SET /a VehicleNum=8

  • 实验流程

    1. 修改UDPSimple16Swarm.py和两个bat脚本内电脑的IP列表，使之满足组网关系
    2. 将UDPSimple1_8Swarm.bat拷贝到电脑1，UDPSimple9_16Swarm.bat拷贝到电脑2，分别运行，就能开启16个飞机的局域网仿真，等待所有飞机都初始化完毕（3D Fixed）
    3. 在电脑1运行UDPSimple16Swarm.py，可以看到所有飞机先飞到同一位置，然后开始画圈

7.4.4 Python集群轨迹展示与灯光秀

  • 在进行集群编队飞行时，初步生成（或者仿真实验）得到了一系列的多无人机的轨迹数据，有时需要在三维引擎中进行预览（回看），或者根据场景调整最优估计
  • 通过Python与UE4通信的SendUE4***系列接口，可以实现场景中物体的创建（具体的三维物体与场景创建方法请参考第5讲，Python场景控制接口请参考第6讲教程）
  • 一个灯光秀的例程见：RflySimAPIs\PythonSwarmAPI\LightShowSwarm文件夹
  • 实验流程：先运行NightCitySwarm4.bat，再运行NightCitySwarm4.py即可，可以看到多个灯光体（和车、飞机等创建方法相同）摆成螺旋形，并变换灯光
  • 请自行通过SendUE4系列接口，发送并更新灯光位置，形成运动光点轨线，就形成了灯光秀的演示效果
  • 注：本例程的灯光变换特效，实际上是用了和RflySim3D中C键切换飞机样式相同的接口（不同灯光样式），通过本接口可以实现撞击后坠毁动画的模拟等其他特效。
  • 注：PX4MavCtrlV4.py 中除了发送单机位姿的SendUE4接口，
    还有SendUE4***20和SendUE4***100等一次发送多个飞机的接口
  • RflySim: 如何利用RflySim3D来仿真夜空都市下的灯光秀场景，本视频观看地址：
    优酷：https://v.youku.com/v\_show/id\_XNDcwNjA4NjE1Ng==.html
    YouTube：https://youtu.be/Chpx1uwFVkU

7.4.5 硬件在环仿真Pixhawk远程重启接口

  • 虽然RflySim平台做了较多的优化来实现硬件在环仿真的稳定性，但是同一Pixhawk飞控在进行多次仿真（特别是上次仿真坠机或者进入失效模式）之后，由于飞控内部参数混乱，易导致无法起飞，或者飞行异常的故障，这时候需要重启飞控来重新初始化HITL仿真。
  • 硬件在环重新仿真的方法，可以通过CopterSim界面上的“重新仿真”按钮实现，点击之后会发送指令让飞控重启，同时CopterSim也会等待15s后，自动重新开始仿真。
  • 注：软件在环仿真在每次启动时都会回归原始状态，因此不需要点“重新仿真” 按钮（目前SITL模式也不支持不能点重新仿真按钮，会直接报错，需要关闭当前仿真，重新运行SITLRun脚本来开启新的仿真）
  • 平台也支持通过Python脚本发送给各个CopterSim，让其进行“重新仿真”状态，例程见RflySimAPIs\PythonSwarmAPI\RebootPixViaUDP文件夹。
  • 插入两个飞控（或一个），运行RebootPixViaUDP.bat启动硬件在环仿真，关闭QGC地面站（避免自动连接Pixhawk导致串口占用），运行RebootPixViaUDP.py来发送重启指令，去CopterSim上查看是否进入重启状态（见Pixhawk重启中…按键）
  • 注：本脚本采用广播方式，支持重启局域网内所有HITL仿真

7.4.6 Python简化模型集群实验

  • 从模型精度的角度，使用高精度6DOF模型（CopterSim）+真实飞控系统（PX4）的软/硬件在环仿真闭环的方式，能够有效提高模型可信度，从而减小仿真与真机实验的差距。
  • 但是上述构架的代码运算量较为复杂，导致一台电脑上运行的无人机数量受到限制（通常SITL软件在环<20架，HITL硬件在环<30架）
  • 为了提高单台电脑仿真集群飞机的数量，就需要降低模型精度并使用简化飞控模型。
  • 因此本平台在Python下开发出了质点多旋翼模型，只需Python和RflySim3D两个软件即可在单台电脑上实现百驾级别的无人机集群仿真（免费版只支持最多12驾的例程，完整版无限制），且每个飞机的输入输出接口与SITL和HITL仿真保持一致，确保向真机实验顺利过渡。
  • Python简化质点集群实验例程在文件夹：RflySimAPIs\PythonSwarmAPI\SwarmSimNoPX4
  • 文件夹内包含了4、12、30、100和200个旋翼无人机的集群控制例程。注：免费体验版只支持运行前两个例程（RflySim3D限制最多12个三维实体）
  • 四飞机实验PythonSwarmAPI\SwarmSimNoPX4\NoPX4SITL4Swarm文件夹，本实验与常规的Python集群控制实验PythonSwarmAPI\PythonSwarm\MAVLinkFull4Swarm代码基本相同
    • 区别在于以下几点：
    1. 增加切换RflySim3D地图的代码mav.sendUE4Cmd(b'RflyChangeMapbyName Grasslands’)
    2. 去掉InitMavLoop()和initOffboard()初始化代码，使用质点模型初始化代码（包含设置地形高度、xy位置和偏航）initPointMassModel(intAlt=0,intState=[0,0,0])
    3. 其余状态获取、速度和位置指定发送函数保持相同
  • 由于本例程没有bat脚本和CopterSim来配置飞机的初始位置
    和地形高度，需要手动在初始化脚本中设置，首先需要选定四个飞机的初始位置；本例程采用和bat脚本一样的矩形分布，即X和Y的位置点为(0,0)、 (2,0)、 (0,2)、 (2,2)，将上述坐标输入CopterSim中，可以获取地形高度，然后将其输入到Python脚本的initPointMassModel()函数中

    • 注：另一中选择飞机起点和高度的方法是打开RflySim3D，切换到期望地图，并如下图所示在期望位置双击即可
  • 实验步骤：

    1. 双击运行NoPX4SITL4Swarm.bat脚本，可以打开一个RflySim3D窗口；这里也可以不用bat脚本，手动去桌面点击RflySim3D的快捷方式，也可以打开，效果相同。
    2. 用VS Code打开NoPX4SITL4Swarm.py文件，并运行，观察实验现象。
    3. 可以看到四个飞机起飞，然后各自飞到指定目标位置，几分钟后降落悬停。

      注：由于模型中没有加入噪声和干扰，因此飞机飞行较顺滑无抖动。下面以单个飞机为例，介绍控制流程 
      mav = PX4MavCtrl.PX4MavCtrler(20100) # 创建一号飞机实例
mav.initPointMassModel(-8.086,\[0,0,0\]) # 初始化质点模型循环
print((mav.uavPosNED,mav.truePosNED, # 打印数据
mav.SendPosNED(0, 0, -1.7, 0) # 发送目标位置
mav.SendMavArm(True) # 解锁飞控，飞机起飞
time.sleep(5) # 代码暂停5s，飞机到达起飞点并悬停
mav.SendVelNED(0, 0, 1, 0) # 发送向下速度，飞机降落
mav.EndPointMassModel() # 退出质点模型循环
  • 十二飞机同心圆编队实验
    • 进入RflySimAPIs\PythonSwarmAPI\SwarmSimNoPX4\NoPX4SITL12Swarm目录：
    • 双击运行NoPX4SITL12Swarm.bat脚本，可以打开一个RflySim3D窗口。
    • 用VS Code打开NoPX4SITL12Swarm.py文件，并运行，观察实验现象。
    • 可以看到十二个飞机先起飞并悬停，然后汇集到同一位置，接着开始同步画圆，形成一 个同心圆。
    • 代码解析如下（与4飞机例子区别部分）： 
      MavList=MavList+[PX4MavCtrl.PX4MavCtrler(20100+ii*2)]  #建立飞机实例矩阵
InitPosList=[ ******]  # 配置飞机初值矩阵
MavList[i].initPointMassModel(InitPosList[i][0],InitPosList[i][1:4]) #通过矩阵初始化
Error2UE4Map = Error2UE4Map+[***] # 计算每个飞机起飞坐标系与UE4地图坐标系差值
MavList[0].sendUE4Cmd(b‘RflyChangeViewKeyCmd S’) # RflySim3D显示飞机数字标号
MavList[0].sendUE4Cmd(b‘RflyChangeViewKeyCmd T’) # RflySim3D显示飞机轨迹
MavList[i].SendPosNED(0, 0, -10, 0) # 飞机各自起飞到10m高（以起飞点为坐标系）
targetPosE=np.array([-0,0,-15]) # 设置默认高度15m，所有飞机汇集到本坐标
targetPosE=np.array([10*math.sin(t/2+math.pi/2)-10,10*math.sin(t/2.0),-15]) # 生成圆形轨迹
targetPosE=targetPosE+Error2UE4Map[j] # 将圆形轨迹映射到各飞机起飞坐标系
mav.SendPosNED(targetPosE[0],targetPosE[1],targetPosE[2],0) #发送圆形轨迹
MavList[i].EndPointMassModel() # 各飞机退出仿真循环
  • 多机地形高度获取实验
    • 在上面的十二个飞机的例子中，用到了一个InitPosList矩阵列表，存储了十二个飞机的地面高度、XY初始位置和初始偏航角信息。
    • 这里提供了接口例程（见RflySimAPIs\UE4MapSceneAPI\GetTerrainAPI），使得可以像bat启动脚本一样，给定飞机数量和间距，自动配置飞机初始摆放位置，并根据当前地形求出地形高度。
    • 实验方法：用MATLAB定位到上述GetTerrainAPI例程目录，运行GenSwarmPos12.m，即可打印输出InitPosList信息（用于Python集群例程）、PosX和Y字符串（用于*ITLRunPos.bat脚本）
    • 关键代码解析： 
      mapName=‘Grasslands’;LoadPngData(mapName); %导入地图文件
ORIGIN_POS_X=0; **;VEHICLE_INTERVAL=2; %设定初始位置和间距信息
START_INDEX=1;VehicleNum=12;TOTOAL_COPTER=12; %设定飞机数量和ID
Alt=[Alt,getTerrainAltData(PosX(i),PosY(i))]; %根据位置求取地形高度
  • 30/100飞机集群实验（仅支持完整版）
    • 注：RflySim免费体验版的RflySim3D限制只能显示12个三维物体，因此运行本实验只能看到12个飞机（随机显示12个飞机序号）
    • 进入RflySimAPIs\PythonSwarmAPI\SwarmSimNoPX4目录
    • 30飞机例子：进入NoPX4SITL30Swarm，运行bat与py文件，可以看到飞机起飞并各自飞圆轨迹
    • 100飞机例子：进入NoPX4SITL100Swarm，运行bat与py文件，可以看到飞机起飞并各自飞圆轨迹

  • 大规模分布式联机集群实验（仅支持完整版）
    • 上面介绍了单电脑仿真100飞机的例子（可以通过优化模型和发送频率、提高电脑性能等方法，实现更多飞机仿真），但是一台电脑的性能毕竟有限，平台还提供了多电脑组网扩充飞机数量的方法
    • 这里以两台电脑，每台电脑100个飞机，展示联机集群仿真的方法。例程见：RflySimAPIs\PythonSwarmAPI\SwarmSimNoPX4\NoPX4SITL200Swarm2PC目录
    • 实验流程：
      • 电脑2运行NoPX4SITL200.bat打开RflySim3D窗口，再运行NoPX4SITL200PC2.py开启电脑2控制程序。注：程序中存在一个等待进程，等电脑1的程序运行时，才会开始飞机控制，以保证同步性
      • 电脑1运行NoPX4SITL200.bat打开RflySim3D窗口，再运行NoPX4SITL200PC1.py开启多机控制程序，并发送开启消息，让电脑2也开始控制程序。
      • 可以看到两台电脑的RflySim3D窗口都能显示共200个飞机，然后飞机起飞后，绕着各自的起飞点画圆
    • 关键代码解析
      1. 200个飞机的初始位置和地形高度生成：见RflySimAPIs\UE4MapSceneAPI\GetTerrainAPI\GenSwarmPos2PC200.m例程，关键点重复两次100飞机位置生成代码，配置VehicleNum=100;TOTOAL_COPTER=200;，针对电脑1设置START_INDEX=1;，针对电脑2设置START_INDEX=101;，运行GenSwarmPos2PC200.m可以得到两电脑的InitPosList列表
      2. NoPX4SITL200PC1.py设置START_INDEX=1和NoPX4SITL200PC2.py设置START_INDEX=101，同时初始化脚本使用广播地址255.255.255.255
        MavList=MavList+[PX4MavCtrl.PX4MavCtrler(20100+ii*2,'255.255.255.255’)] 3.targetPosE不经过Error2UE4Map[j]的校准，每个飞机画圆按照自己起飞坐标系来
      3. NoPX4SITL200PC2.py中MavList[0].waitForStartMsg()，启用程序阻塞，等到接收到来自局域网的开始仿真消息后，才会继续执行后续的控制算法
      4. NoPX4SITL200PC1.py中MavList[0].sendStartMsg()，发送开始仿真消息，局域网内所有等待的程序收到本消息后，会自动开始后续程序运行。

results matching ""

    No results matching ""