7.2 Simulink集群接口示例

本节导航


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

7.2.1 Simulink集群通信接口总体

  • 平台提供SimulinkPython两种通信接口,底层都是通过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型向量,具体定义(实现MAVLinkOffboard消息)如下
    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节定义)提取出感兴趣的值:GpsHomeLocalPosAngEular
  • GpsHome的经纬高数据转为CopterSim的全局坐标GlobalPosxyz单位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
    
    如下图所示模块
  • 控制原理
    • 轨迹生成:如下图所示,利用正弦和余弦函数的组合形成一个圆形轨迹,高度始终保持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以各自解锁位置为原点,GlobalPosCopterSim/RflySim3D地图中心为原点
    • MATLAB打开RflyUdpFullFour.slx例程,按下图修改本文件,将反馈位置从LocalPos改为GlobalPos,确保所有飞机使用CopterSim地图坐标系
    • 点击RflyUdpFullFour.bat再运行RflyUdpFullFour.slx可以看到飞机依次起飞并悬停到空中同一点,然后开始绕同样圆心画圆飞行


7.2.4 通信接口的SimpleData模式介绍

  • 输入
    输入为5维的double型向量,具体定义(实现MAVLinkOffboard消息)如下
    1. 第1维:ctrlMode; %第一位为标志位,0: 表示地球速度控制模式Earth Vel; 1: 机体速度控制模式Body Vel; 2: 地球位置控制模式Earth Pos; 3:机体位置控制模式Body Pos
    2. 第2~5维:如果ctrlMode =0,则这四维对应了地球坐标系(以解锁时的位置)下的x,y,z位置和yaw偏航信号;如果ctrlMode =1,则这四维对应了机体坐标系(以解锁时机体姿态和位置)下的x,y,z位置和yaw偏航信号;如果ctrlMode =2,则这四维对应了地球坐标系(以解锁时的位置为原点)下的vx,vy,vz速度和yawRate偏航速率信号;如果ctrlMode=3,则这四维对应了机体坐标系(以解锁时机体姿态和位置)下的vx,vy,vz 速度和yawRate偏航速率信号。
    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,对应了CopterSimUDPMode设置为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维:` AngEular[3]`; %` Pixhawk`估计得到的姿态欧拉角,单位弧度  
      3. 第7~9维: `localPos[3]`; % `Pixhawk`估计得到的,以`gpsHome`为原点的相对北东地位置向量,单位m,z轴向下为正  
      4. 第10~12维: `localVel[3]`; % 北东地的运动速度向量,单位m/s  
    

UltraSimple模式和SimpleData模式的唯一区别在于输出的第1~3维从原来的gpsHome变为了GlobalPos,其中GlobalPosgpsHome经过经纬高的坐标转换之后,计算得到的CopterSim中的全局x,y,z(北东地坐标系)坐标
SimpleDataUltraSimple模式的内网传输数据相同,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个飞机起飞并画圆。(飞行效果与电脑配置相关,需要较高CPUGPU
    • 以上例程也可以插入特定数量的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 FunctionsS-Function ExamplesC++ S-functions,从其中可以查看S函数编写方法,轻松实现Simulink控制其他系统

  • S函数官方PDF文档

7.2.7 RflyUdpFast.cpp代码解析

  1. 定义区: S函数名字,层级,导入头文件,自定义变量等
  2. 数据接收结构体,获取Pixhawk转发的MAVLink飞控状态
  3. 其他通信接口结构体
  4. S函数模块参数校验函数
  5. 仿真初始化函数
  6. 开始仿真函数
  7. 更新输出函数
  8. 更新状态函数

results matching ""

    No results matching ""