7.2 Simulink集群接口示例

本节导航

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

7.2.1 Simulink集群通信接口总体

平台提供Simulink和Python两种通信接口，底层都是通过MAVLink协议实现，因此仿真完的算法是可以很快地部署到实验平台上。这里先介绍Simulink的接口:
打开RflySimAPIs\SimulinkSwarmAPI\RflyUdpFullOne.slx文件，如下图可看到RflySwarmAPI模块即为集群通信模块。
该模块有Simulink S函数通过C++混编实现，源文件见RflyUdpFast.cpp
“RflySwarmAPI”模块需要和RflyUdpFast.mexw64放在同一个文件夹才能调用。因此新建slx项目时，拷贝RflySwarmAPI模块的同时还需要拷贝“RflyUdpFast.mexw64”文件到新建slx文件所在目录
RflyUdpFast.mexw64是源文件RflyUdpFast.cpp在MATLAB中编译出来的。如果修改了.cpp文件，还需要使用命令mex RflyUdpFast.cpp重新编译
如下图，MATLAB输入mex RflyUdpFast.cpp（可通过修改cpp文件来自行开发ROS、串口、TCP等其他通信）可得到RflyUdpFast.mexw64文件
拷贝RflyUdpFast.mexw64到一个文件夹，新建一个new.slx (或其他名字)，将RflyUdpFullOne.slx中的RflySwarmAPI拷贝进去，在里面开发集群算法即可
模块介绍
1. 第一项UDP IP Address是目标电脑的IP地址，输入127.0.0.1则只能接受本机的CopterSim转发的Pixhawk自驾仪状态并进行控制；255.255.255.255则能接收并控制局域网内所有电脑中运行的CopterSim程序（其他电脑的CopterSim需要勾选“联机”按钮）；192.168.1.12之类的指定IP则只会向该IP地址的主机发送控制指令。一般而言，在小范围集群的时候255.255.255.255广播已经能够满足需求，当飞机数量继续增多，则需要启用指定IP来减小网络负载，提高通信速度和可靠性
2. 第二项UDP Port是第一个飞机的初始端口号，默认起始端口是20100。每个CopterSim的收发消息需要各占用一个端口，因此如果本模块需要仿真飞机ID为10~15的飞机，那么这一项需要填20100+10\*2=20120，后面一项Vehicle number飞机数量需要填5.
3. 第三项Vehicle number飞机数量表示需要连接的CopterSim数量，该模块的输入输出端口数量由该选项控制，如果输入10，则模块会自动生成10对输入输出接口
4. 第四项UDP mode是输入输出接口的数据模式协议，主要包含FullData完整模式（数据最全，但传输数据量较大）；SimpleData精简数据模式（较多飞机>8，避免数据过大网络阻塞）和UltraSimple超精简模式（单电脑飞机数>20），延迟更小
5. 第五项是Sample Time采样时间，该时间应该与Simulink仿真时间对应
RflySim: 如何通过Simulink控制软件在环仿真模式下的多无人机集群，本视频观看地址：
优酷：https://v.youku.com/v\_show/id\_XNDcwNjA4NTEwOA==.html
YouTube：https://youtu.be/AMZNuAtRp2w

7.2.2 通信接口的FullData模式介绍

模块输入为15维的double型向量，具体定义（实现MAVLink的Offboard消息）如下
1. 第1维：time_boot_ms; %当前时间戳（填0即可，目前没有使用）
2. 第2维：copterID; %飞机ID（填1即可，目前没有使用）
3. 第3维：type_mask; %输入控制模式（同Offboard定义）
4. 第4维：coordinate_frame; %坐标系模式（同Offboard定义）
5. 第5~15维：ctrls[11]; %分别对应了3维的期望位置pos,3维的期望速度vel，3维的期望加速度acc，1维的期望偏航角yaw，1维的期望偏航角速度yawRate。（同Offboard定义）
该模块与RflySimAPIs\SimulinkControlAPI\OffboardAPI.slx功能相同（见高级课程第3讲3.3节），实现了Offboard控制的所有功能，能实现位置、速度、加速度跟踪等
Offboard消息的详细定义可见 https://mavlink.io/en/messages/common.html#SET\_POSITION\_TARGET\_LOCAL\_NED
模块输出为28维的double型向量（全部转发自Pixhawk内部滤波值），具体定义如下
1. 第1~3维：gpsHome[3]; %Home点（上电之后不会变）的经纬高坐标，经纬度需要除以1e7才能得到度为单位的经纬度，高需要除以1e3才能得到m为单位的高（向上为正）
2. 第4~6维：AngEular[3]; %Pixhawk估计得到的姿态欧拉角，单位弧度
3. 第7~9维：localPos[3]; %Pixhawk估计得到的以gpsHome为原点的相对北东地位置向量，单位m，z轴向下为正
4. 第10~12维：localVel[3]; %北东地的运动速度向量，单位m/s
5. 第13~15维：GpsPos[3]; %实时的GPS位置，单位和gpsHome相同，但是会实时变化
6. 第16~18维：GpsVel[3]; %GPS速度，需要除以100得到m/s为单位的速度
7. 第19维：time_boot_ms；%上电时间
8. 第20维： copterID; %飞机ID
9. 第21维：relative_alt; % GPS相对高度，需要除以1000得到m为单位的高度，向上为正
10. 第22维： hdg; % GPS航向角，需要除以1000得到0~360度范围的角度
11. 第23维： satellites_visible; %可见卫星数量
12. 第24维：fix_type; %定位精度
13. 第25维：resrveInit; % int类型的保留位
14. 第26维： pos_horiz_accuracy; %水平定位精度，单位m
15. 第27维： pos_vert_accuracy; %竖直定位精度，单位m
16. 第28维： resrveFloat; % float型保留位，未被启用
注：使用本模式，CopterSim上的UDP Mode选项要选择UDP_Full

7.2.3 通信接口的FullData模式例子

打开RflySimAPIs\SimulinkSwarmAPI\RflyUdpFullOne.slx即可看到RflySwarmAPI通信接口模块使用FullData模式时的例子
输入映射模块将4维的速度（地球坐标系下速度与偏航角速度控制量）转化为15维信号（见第2.4节定义）给RflySwarmAPI模块的FullData Mode模式
FullData模式可以实现所有Offboard控制功能（需要修改下图所示的一些配置）
将RflySwarmAPI模块的28维输出（见2.5节定义）提取出感兴趣的值:GpsHome、LocalPos、AngEular等

将GpsHome的经纬高数据转为CopterSim的全局坐标GlobalPos的xyz单位m的数据。代码如下：

function \[globalPos,localPos,localVel,AngEular\] = fcn(u)
GpsHomePos = u(1:3);  
AngEular = u(4:6);  
localPos = u(7:9);  
localVel = u(10:12);  
globalPos = \[0,0,0\];  
if ~(abs(GpsHomePos(1))<1&&abs(GpsHomePos(2))<1)   
   globalPos(1)=(GpsHomePos(1)\*1e-7-        40.1540302)/180\*pi\*6362000+localPos(1);
   globalPos(2)=(GpsHomePos(2)\*1e-7-116.2593683)/180\*pi\*4.8823e6+localPos(2);
   globalPos(3)=-GpsHomePos(3)\*1e-3+localPos(3);
end

注： GPSOrigin=[40.1540302,116.2593683,50]是RflySim仿真的GPS原点，在Init.m和RflyUdpFast.cpp等的初始化区域定义。如果通过CopterSim的txt地形文件（见第5讲6.4节）修改了GPS原点坐标，这个地方需要根据实际情况修正。

如下图所示模块

控制原理
- 轨迹生成：如下图所示，利用正弦和余弦函数的组合形成一个圆形轨迹，高度始终保持10，因此是一个定高飞圆的轨迹（这里的轨迹也可以替换成任意其它轨迹，例如8字，请自行尝试修改）
- 期望轨迹与实时位置（LocalPos，相对解锁/起飞点的位置）做差，乘以1形成一个P控制器，将速度误差反馈回Pixhawk（只要有位置误差，速度就不为0，使两者靠拢）
实验现象
- 双击运行RflySimAPIs\SimulinkSwarmAPI\RflyUdpFullOne.bat（或者双击上层目录的SITLRunUdpFull.bat脚本，输入数字1并回车）打开单机的SITL软件在环仿真系统（或运行上层目录的HITLRunUdpFull.bat 开启单机硬件在环仿真）。注意：本脚本中UDPSIMMODE选项需设置成UDP_Full，对应RflySwarmAPI中的数据模式，如下图
- 用MATLAB打开RflyUdpFullOne.slx例程，等待RflySim3D显示3D Fixed: 4/4说明所有CopterSim初始化完毕，可以开始Offboard控制。此时，点击运行，即可看到单机起飞到一定高度，一段时间后（正弦函数前的延迟模块）开始画圆飞行
四个飞机
- 打开RflySimAPIs\SimulinkSwarmAPI\RflyUdpFullFour.slx
- 将RflySwarmAPI模块内vehicle number设成4就能得到4对输入输出，然后每个输入/输出口复制粘贴上数据处理模块，再将控制算法复制4份即可，如下图
四个飞机实验效果
- 双击运行RflySimAPIs\SimulinkSwarmAPI\RflyUdpFullFour.bat（或者双击上层目录的 SITLRunUdpFull.bat脚本，输入数字4并回车）打开多机的SITL软件在环仿真系统（或运行HITLRunUdpFull.bat开启硬件在环仿真）
- 用MATLAB打开RflyUdpFullFour.slx例程，点击运行，即可看到4个起飞依次到一定高度开始画圆飞行（每个飞机的圆心不同，在QGC上切换Vehicle *，查看各飞机的轨迹）
- 注：请修改slx文件，使得各个飞机圆心一致。（提示：将反馈的位置信号从LocalPos改为GlobalPos）
- 注：LocalPos以各自解锁位置为原点，GlobalPos以CopterSim/RflySim3D地图中心为原点
- 用MATLAB打开RflyUdpFullFour.slx例程，按下图修改本文件，将反馈位置从LocalPos改为GlobalPos，确保所有飞机使用CopterSim地图坐标系
- 点击RflyUdpFullFour.bat再运行RflyUdpFullFour.slx可以看到飞机依次起飞并悬停到空中同一点，然后开始绕同样圆心画圆飞行

7.2.4 通信接口的SimpleData模式介绍

输入

输入为5维的double型向量，具体定义（实现MAVLink的Offboard消息）如下

1. 第1维：ctrlMode;  %第一位为标志位，0: 表示地球速度控制模式Earth Vel; 1: 机体速度控制模式Body Vel; 2: 地球位置控制模式Earth Pos; 3:机体位置控制模式Body Pos
2. 第2~5维：如果ctrlMode =0，则这四维对应了地球坐标系（以解锁时的位置）下的vx,vy,vz 速度+ yawRate偏航速率信号；如果ctrlMode =1，则这四维对应了机体坐标系（以解锁时机体姿态和位置）下的vx,vy,vz 速度+ yawRate偏航速率信号；如果ctrlMode =2，则这四维对应了地球坐标系（以解锁时的位置为原点）下的x,y,z位置+ yaw偏航信号；如果ctrlMode =3，则这四维对应了机体坐标系（以解锁时机体姿态和位置）下的x,y,z位置+ yaw偏航信号。
3. 一般ctrlMode=0比较多，直接给定全局速度做轨迹控制。如果做位置和速度的切换，需要实时修改ctrlMode的值，并调整第2~5维信号的定义。

输出
输出为12维double型的向量，顺序定义如下
1. 第1~3维：gpsHome[3]; % Home点（上电之后不会变）的经纬高坐标，经纬度需要除以1e7才能得到度为单位的经纬度，高需要除以1e3才能得到m为单位的高（向上为正）
2. 第4~6维： AngEular[3]; % Pixhawk估计得到的姿态欧拉角，单位弧度
3. 第7~9维：localPos[3]; % Pixhawk估计得到的，以gpsHome为原点的相对北东地位置向量，单位m，z轴向下为正
4. 第10~12维：localVel[3]; % 北东地的运动速度向量，单位m/s
注： gpsHome为经纬高坐标，如果要换算成CopterSim中的地图坐标需要减去其初始点经纬高（116.2593683°，40.1540302 ° ，0m），再换算成单位m
注：SimpleData模式不仅是输入输出精简了，UDP传输的结构体也精简，因此没有FullData模式中的其他信息。使用本模式，CopterSim上的UDP Mode选项要选择UDP_Simple
实验效果
- 双击 RflySimAPIs\SimulinkSwarmAPI\RflyUdpSimpleOne.bat（或者双击上层目录的SITLRunUdpSimple.bat脚本，输入数字1并回车）打开单机的SITL软件在环仿真系统（或插入Pixhawk，运行HITLRunUdpSimple.bat开启单机的硬件在环仿真）
- 用MATLAB打开RflyUdpSimpleOne.slx例程，点击运行，即可看到单个起飞飞机起飞到一定高度，然后一段时间后开始画圆
- 本实验的结果与第3.5节相同，是通过两种接口来实现同样的控制功能
- 在本例程中bat脚本中的UDPSIMMODE设置为了1，对应了CopterSim中UDPMode设置为UDP_Simple，也对应了RflySwarmAPI中的SimpleData模式
输入输出映射解析
- 如下图所示，RflySwarmAPI输入部分不需要映射，直接按SimpleData的数据（见第2.5节定义）给5维的控制信号即可
- 如下图所示， RflySwarmAPI输出模块需要映射，求出CopterSim全局坐标GlobalPos的值（留作多机集群用）

7.2.5 通信接口的UltraSimple模式介绍

输入为5维的double型向量，具体定义与2.6节中的SimpleData模式完全相同

输出为12维double型的向量，顺序定义如下

  1. 第1~3维：` GlobalPos [3]`; % `CopterSim`全局位置，单位m  
  2. 第4~6维：`localPos[3]`; % `Pixhawk`估计得到的，以`gpsHome`为原点的相对北东地位置向量，单位m，z轴向下为正  
  3. 第7~9维：  `localVel[3]`; % 北东地的运动速度向量，单位m/s   
  4. 第10~12维： ` AngEular[3]`; %` Pixhawk`估计得到的姿态欧拉角，单位弧度

注：UltraSimple模式和SimpleData模式的唯一区别在于输出的第1~3维从原来的gpsHome变为了GlobalPos，其中GlobalPos是gpsHome经过经纬高的坐标转换之后，计算得到的CopterSim中的全局x,y,z（北东地坐标系）坐标
注：SimpleData和UltraSimple模式的内网传输数据相同，CopterSim上的UDP Mode选项要选择UDP_Simple

输入输出映射解析
- 打开RflySimAPIs\SimulinkSwarmAPI\RflyUdpUltraSimpleOne.slx可以查看RflySwarmAPI模块使用UltraSimple模式的例子，本例子传输数据量少、延迟少，适合较多飞机的集群速度/位置/轨迹控制的的需求。
- RflySwarmAPI模块UltraSimple模式的输入为5维向量，其定义上一小节内容完全相同；输出为12维向量，其定义可以参考第2.8小节内容。
- 如下图所示，本接口不需要额外处理，直接用向量分解就能获取感兴趣的值（全局位置、本地位置、姿态等）用于控制。
- 本例子要求CopterSim使用UDP_Simple
实验效果
- 进入RflySimAPIs\SimulinkSwarmAPI文件夹
- 双击RflyUdpUltraSimpleOne.bat(或者双击上层目录的 SITLRunUdpSimple.bat脚本，输入数字1并回车)再运行RflyUdpUltraSimpleOne.slx可以看到一个飞机起飞并画圆
- 双击RflyUdpUltraSimpleFour.bat(或者双击上层目录的 RflyUdpUltraSimpleFour.bat脚本，输入数字4并回车)再运行RflyUdpUltraSimpleFour.slx可以看到4个飞机起飞并画圆
- 双击RflyUdpUltraSimpleEight.bat(或者双击上层目录的SITLRunUdpSimple.bat脚本，输入数字8并回车)再运行RflyUdpUltraSimpleEight.slx可以看到8个飞机起飞并画圆。（飞行效果与电脑配置相关，需要较高CPU和GPU）
- 以上例程也可以插入特定数量的Pixhawk，然后运行HITLRunUdpSimple.bat并输入串口号字符串来开启多机硬件在环仿真，并运行对应的slx文件观察效果

7.2.6 RflySwarmAPI模块实现原理解析

本模块调用的S函数由下图模块参数页面设置

• 本模块的配置菜单（图1，图2）由图3的Mask编辑器设定

图1

图2

右键Mask-Edit Mask-Parameters & Dialog

图3

C++ S函数通信模块的优点
- 主要内容包括两个方面：UDP网络通信编程+Simulink S函数编写
- 本模块与Simulink自带的UDP模块的优点
- 效率高：C/C++语言直接实现，比m语言高效
- 运算小：Simulink自带UDP模块输入输出都是高维（几百维）的unit8向量，在飞机增多时，整个项目维度急剧扩张；而S函数方式直接输出double型的位置、速度等数据，维度低（几维），运算量小
  - 延迟低：Simulink自带的UDP模块为了防止数据丢失，会将收到数据全都保存在缓存中，依次调用，这样当外部程序发送频率大于Simulink的运行频率是，就会产生大的延迟；而编写S函数的方法则能避免本问题
- 更可靠：Simulink自带的UDP模块每个仿真步长读取一个数据，如果外部程序发送频率小于Simulink运行频率，则Simulink没有读到数据，会输出0，导致运算出错；而自己编写S函数则可以避免本问题
- 扩展性强：S函数可以轻易扩展为串口、Tcp、共享内存、MAVLink等其他通信协议
S函数学习例子
*Simulink库浏览器-Simulink-User Defined Functions – S-Function Examples – C++ S-functions，从其中可以查看S函数编写方法，轻松实现Simulink控制其他系统
S函数官方PDF文档
- https://www.mathworks.com/help/pdf\_doc/simulink/index.html