多旋翼飞行器设计与控制实践 第12讲-失效保护逻辑设计实验

第十二讲失效保护逻辑设计实验1.实验原理2.基础实验3.分析实验4.设计实验5.小结在多旋翼起飞前、飞行中都存在影响多旋翼安全的问题，这些安全问题与多旋翼决策存在着紧密的联系。在这里，主要考1.通信故障(CommunicationBreakdown)2.传感器失效(SensorFailure)3.动力系统异常(PowerSystemAnomaly)遥控器未校准是指多旋翼在第一次飞行之前有让多旋翼“知道”遥控器每个摇杆和开关的具体作用。若旋翼飞行过程中其飞行控制系统无法识别用户的指令，甚至令2）遥控器失联遥控器失联是指在多旋翼起飞前或飞行过程中，传感器失效主要是指多旋翼上机载传感器测量不准2）电子罗盘(Compass)失效3）电子罗盘(Compass)失效4）惯导系统(InertialNavigationSystem)失效动力系统异常主要指电池失效，以及电调、电机和1）电池失效。通常是由于电池电量不足或者电池寿命减少导致的多旋翼供电不足。2）电调故障。主要表现在两个方面：①电调无法正确识别自驾仪给出的PWM指令；②电调无法给电机提供正确的输出电压。3）电机故障。主要表现在一定的输入电压下，输出不正确的转速。 4）螺旋桨失效。桨的故障主要表现在桨叶松动，桨叶磨损和折断等。多旋翼事件定义是多旋翼状态和飞行模态的切换的根据，是多旋翼决策的重要基础。在这里，我们主要定义两种事件：人工输入事件和飞行器自触发事件。1)人工输入事件主要是指飞控手利用遥控器或者地面站发出的指令，并基于此改变多旋翼状态和飞行模态的输入事件。MIE2=1时切换到手动飞行模式，MIE2=2时切换到返航模式，MIE2=3时切换到自动着陆模式。该指令通过遥控器上（如图）的通道5（三段拨动开关）进行切换操作。多旋翼事件定义是多旋翼状态和飞行模态的切换的根据，是多旋翼决策的重要基础。在这里，我们主要定义两种事件：人工输入事件和飞行器自身事件。2）飞行器自触发事件（ATE）：飞行器自触发事件与飞控手操作无关，主要取决于机上各部件的工作状态及多旋翼状态。在基础实验中假定遥控器连接正常，即不把飞行器自触发事件作为判断条件，仅仅以人工输入事件作为条件进行考虑。着陆C4C5C3C2着陆C4C5C3C2以基础实验为例，状态转移如下图所示返航返航手动手动飞行C1在模型根据输入事件不同进行判断后产生状态转换后，还需要根据不同的状态对控制器施加相应的指令以使多旋翼达到不同状态的要求：2）返航状态：返航状态要求多旋翼返回起飞点，这属于定点位置控制，故这时姿态控制器输入需要接入位置控制器输出接口。故这时的位置控制指令为pxd=0,pyd=0,即给控制器加入返回原点的指令。同时高度通道上，若当前高度高于设定的安全高度，则pzd=z,即控制多旋翼高度不变；若当前高度低于设定安全高度，则多旋翼先上升到设定高度再返回起飞原点。3）着陆状态：着陆状态要求多旋翼保持原来的水平位置，而调整油门指令实现降落。此时姿态控制器输入需要接入位置控制器输出接口，同时，给位置控制器的控制指令为水平期望速度vxd=0,vxd=0,使多旋翼维持原来的水平位置不变；高度通道上pzd=0,使多旋翼实现降落。状态机可以在Stateflow工具箱内实现，Stateflow是MathWorks开发的利用状态机和流程图用于对响应系统进行建模的工具。它是一种图形化的设计工具，能够实现有限状态机，实现各个状态间的转换。它和Simulink同时使用时可以接收Simulink的输入，同时给Simulink输出，实现和Simulink中模型的联系。将设计好的状态机在Stateflow中实现。模式切换中的手动飞行模式需要遥控器来发送控制指令，故添加指令时模拟遥控器实际发送的信号值，模式切换中的手动飞行模式需要遥控器来发送控制指令，故添加指令时模拟遥控器实际发送的信号值，如右图所示，遥控器的端口中，通道1控制遥控器的前后运动，通道2控制其左右运动，通道3控制油指令，通道5是三段拨动开关，刚好可以满足三种模围内，这里为了更加接近真实情况，仿真时也输入赵峙尧,戴训华,任锦瑞,邓恒译.《多旋翼飞行器设计与控制》,电子（2）软件：MATLAB2017b或以上的版本，基于Simulink的控制器设计与仿真平台，硬件在环仿真平台，实验指导包“e8.1”（下载地址：/course）。（1）在Simulink仿真环境中，在手动模式下，实现飞行器的返航与着陆，并记录和在最初10s时为1200，对应于手动控制模式，MIE2=1；10s后遥控器ch5的值为1500，即为QuadcopterDynamicsControlSystem1Z-d在最初10s时为1200，对应于手动控制模式，MIE2=1；10s后遥控器ch5的值为1500，即为QuadcopterDynamicsControlSystem1Z-d1）参数初始化打开文件“e8\e8.1\Init_control.m”进行参数初始化，“e81Sim.slx”将会自动打开如图所示。chch1ch2ch3ch4ch5ch6pqrrollpitchyawxyzvxvyvzPWMLED2）返航仿真仿真结果如右图所示。可以看到0~10s时在手动控制模式下，多旋翼在给定指令下在空间小段时间调整后保持不变，水平位置逐渐趋于0，最后稳定在0位置，完成返航。3）着陆仿真这里，“ch5”值的修改如下：前10s输入1200，0~10s内，四旋翼在给定的指令下以手动飞行方式起式，水平位置保持不变，高度逐渐下降到0。Ch1Ch2Ch3Ch4Ch5Ch6-C-GyroX(p)GyroY(q)GyroZ(r)QuaternionphiCh1Ch2Ch3Ch4Ch5Ch6-C-GyroX(p)GyroY(q)GyroZ(r)Quaternionphithetapsiqquat2eulvehicle_attitudeRGB_MODERGBRGB_COLORModeColorRGB_Mode_SubsystemControlSystem1）参数初始化运行“e8\e8.1\HIL\Init_control.m”初始化模型参数,“e81HIL.slx”将会自动打开，如右图。注意这里的“ControlSystem”模块与软件在环仿真相同。ch1ch1ch2ch3ch4ch5ch6pqryawxyzvxvyvzxyxyzvxvyvz2）硬件连接需要注意的是，在进行硬件在环仿真时机架类型应该是“HILQuadcopterX”。数据线，另一数据线，另一端连电脑USB口乐迪R9DS接收机Pixhawk。最左侧排针，顺序白JR信号线：红为电源正，黒为地线，白为信号线图.硬件系统连接3）代码编译及下载将硬件在环仿真模型编译并下载文件到Pixhawk自驾仪中。这样就可以在Pixhawk自驾仪中运行我们自己设计的失效保护控制程序。点击编译点击下载下载完成点击下载图.代码编译及下载流程4）模型仿真器软件配置。213双击CopterSim桌面快捷方式即213可打开CopterSim。读者可以选择不同的动力系统模型，步骤如下：单击“模型参数”自定义参数，然后单击“存储并使用参数”存储并使用参数。软件会自动匹配串口号，单击“开始仿真”按钮就可以进入硬件在环仿真模式。此时可以看到如所示的界面左下角收到自驾仪返回的相关消息。5）3DDisplay配置双击3DDisplay桌面快捷方式打开3DDisplay。解锁多旋翼，并将四旋翼飞行到一定高度。将CH5拨到中间位置实现返航。如果再往上拨四旋翼仍处于返航状态，这是因为状态转移条件中，没有返航与着陆的转换。随后拨动CH5使四旋翼重新进入手动控制模式，快速拨动CH5到离位置速度欧拉角实际飞行轨迹（2）软件：MATLAB2017b或以上的版本，基于Simulink的控制器设计与仿真（1）在基础实验的基础上，添加相应的状态转移，在手动模式下，实现飞行器的返航和着陆，并且返航和着陆之间可以相互切换。本实验在基础实验的基础上增加返航和着陆之间的切换，其对应的状态机如下图所示。新增的两个状态的转移条件为：该条件描述的是多旋翼从返航模式到着陆模式，多旋翼必须满足:飞控手利用遥控器手动将多旋翼切换到着陆模式（MIE2=3）该条件描述的是多旋翼从着陆模式到返航模式，多旋翼必须满足：飞控手利用遥控器手动将多旋翼切换到返航模式（MIE2=2）ch1ch2ch3ch4ch5ch6pqryawxyzvxvyvzControlSystemQuadcopterDynamicsmodestate ch1ch2ch3ch4ch5ch6pqryawxyzvxvyvzControlSystemQuadcopterDynamicsmodestate 1)复制基础实验中的模型，找到模型中的ControlSystem模型（右上图）中名为modeswitch的状态机（右下chch5control_mode2)修改状态机。在状态机的转换条件中，是以事件为判断条件进行的，比如MIE2，这是输入的条件。在状态机中加入返航和着陆的转换条件，如右图。landlandentry:state=2[MIE2==1][MIE2==3][(MIE2==1)]1manualentry:state=0return1entry:state=1[MIE2==2]2entry:state=121entry:state=0entry:state=2[MIE2==2]21[MIE2==3]213)修改模型输入。将模型中“ch5”通道的输入改为可模拟遥控器三段输入的形式，最终实现在0~10s，输入为1200，即为手动模式，MIE2=1；10~30s，输入为1500，即为返航模式，MIE2=2；30~50s输入为1800，即为着陆模式，MIE2=3。整个过程模拟遥控器由手动切换到返航，再切换到着陆的过程。4)保存模型，并初始化参数。将模型保存到文件“e8\e8.2\e82sim.slx”。运行“e8\e8.2\Init_control.m”进行参数初始化。5)仿真效果直接得到仿真结果如右图所示。可以看到0~10s多旋翼在空间中自由飞行，10~30s为返航状态，高度保持不变，水平位置回到（0，0）。30s之后变成着陆模式，水平位置依旧保持（0，0），高度下uORBWriteTopic:'actuator_outputs'phiqxyzvxvyvzRGB_MODERGBRGB_COLORControlSystemuORBWriteTopic:'actuator_outputs'phiqxyzvxvyvzRGB_MODERGBRGB_COLORControlSystem1）参数初始化打开“e8\e8.2\HIL\Init_control.m”进行参数初始化,“e82HIL.slx”模型会自动打开，如右图。值得注意的是，这里的“ControlSystem”模块与软件在环仿真的chch1ch2ch3ch4ch5ch6pqrrollpitchyawxyzvxvyvzPWMLED相同。2）硬件连接需要注意的是，在进行硬件在环仿真时机架类型应该是“HILQuadcopterX”。数据线，另一数据线，另一端连电脑USB口乐迪R9DS接收机Pixhawk。最左侧排针，顺序白JR信号线：红为电源正，黒为地线，白为信号线图.硬件系统连接3）代码编译及下载将硬件在环仿真模型编译并下载文件到Pixhawk自驾仪中。这样就可以在Pixhawk自驾仪中运行我们自己设计的失效保护控制程序。点击编译点击下载下载完成点击下载图.代码编译及下载流程4）模型仿真器软件配置。213双击CopterSim桌面快捷方式即213可打开CopterSim。读者可以选择不同的动力系统模型，步骤如下：单击“模型参数”自定义参数，然后单击“存储并使用参数”存储并使用参数。软件会自动匹配串口号，单击“开始仿真”按钮就可以进入硬件在环仿真模式。此时可以看到如所示的界面左下角收到自驾仪返回的相关消息。5）3DDisplay配置双击3DDisplay桌面快捷方式打开欧拉角实际飞行轨迹位置速度（1）多旋翼在返航和着陆的过程中需要一点时间，在仿真过程中应该空出足够的时间让多旋翼保持稳定，以保证出现和实际情况相符的仿真结果，最终达到良好的仿真效果。（2）在更改状态机时，要特别注意状态转移之间的关系和转移实现的条件，按照已有的状态机的设计例程，完成新的转移条件的添加，并且进行相应的仿真。（1）硬件：Pixhawk自驾仪系统，多旋翼硬件系统；（1）在前面实验的基础上，增加遥控器断电失联事件，完成新的模态和切换设计，即加入失效返航和失效着陆两个状态，完成状态机的设计。此外还需加入如下功能：如果飞行器离基地很近，直接着陆；而如果飞行器离基地有一定距离，先进行返航，在进行着陆；（2）完成硬件在环仿真实验和实飞实验。为了方便逻辑设计，四旋翼从飞行到着陆分为五种飞行模式，添加了两种失效的模式定义同基本原理）4）失效着陆模式：遥控器断电时自动着陆 5）失效返航模式：遥控器断电时自动返航）：2）飞行器自触发事件（ATE）：飞行器自触发事件与飞控手操作无关，主要取决于机上各部件的工作状C10C12C11C13C10C12C11C13C16C14C15C15）；(C7)、失效着陆到返航(C10)以及失效着陆到返航到返航模式（MIE2=2动将多旋翼切换到手动飞行模式（MIE2=1陆到着陆(C12)以及失效返航到着陆(C14)的条件，多旋翼必须满足：遥控C10C12C11C13C10C12C11C13C16C14C15C15C8:ATE1=0&ATE3=0该条件描述的是多旋翼从手动飞行到失效返航的条件，多旋翼必须满足：遥控器连接异常（ATE1=0）且旋翼到原点位置的距离高于设定阈值（ATE3=0）C9:ATE1=0&ATE3=1该条件描述的是多旋翼从手动飞行到失效着陆的条件，多旋翼必须满足：遥控器连接异常（ATE1=0）且旋翼到原点位置的距离低于设定阈值（ATE3=1）该条件描述的是多旋翼从返航到失效返航(C11)以及着陆到失效着陆(C13)的条件，多旋翼必须满足：遥控器连接异常（ATE1=0）C16:ATE1=0&ATE3=1该条件描述的是多旋翼从失效返航到失效着陆的条件，多旋翼必须满足：遥控器连接异常（ATE1=0）且旋翼到原点位置的距离低于设定阈值（ATE3=1此时多旋翼飞行器才可以实现着陆 1）修改状态机模型析实验的基础上，修改了状态机模型，如右图所示。21PosEVelEAngEulerAngRateBMotorRPMSinPWMs-C--C-PosE21AngEularMotorPWMs-C--C-3-C-ch5AngEularAngEularAngEularrc_lostAngRateBMotorPWMsQuadcopterDynamicsToFlightGearControlSystemPosEVelEAngEulerAngRateBMotorRPMSinPWMs-C--C-PosE21AngEularMotorPWMs-C--C-3-C-ch5AngEularAngEularAngEularrc_lostAngRateBMotorPWMsQuadcopterDynamicsToFlightGearControlSystem2）修改遥控器通道输入在原有模型上增加一个遥控器通道模拟遥控器失联事件，其输入值前10s控器正常连接10s后，连接失效事故。3）保存模型ch6pqryawxyzvxvyvzAngEular将模型保存到“e8\e8.3\sim\e8_3.slx”。运行“Init_control.m”文件进行参数初始化。点击“Run”按钮，即得到仿真结果，如图所示。可以看到在0~10s，多旋翼处于手动模式，飞行器自由飞行。10s后遥控器失联，同时水平位置偏离起飞原点较远，高度小于阈值，多旋翼先上升到安全高度，然后进入返航模式，18s基本完成返航操作，水平位置回到原点，多旋翼进入着陆模式，35s完成着陆。Ch1Ch2Ch3Ch4Ch5Ch6RCFailsafe'timestamp'uORBWriteTopic:'actuator_outputs''output'DigitalClock-C-GyroX(p)GyroY(q)GyroZ(r)QuaternionphithetaqRGB_MODERGBRGB_COLORquat2eulModeColorxyzvxvyvzRGB_Mode_SubsystemlocalPositionControlSystemCh1Ch2Ch3Ch4Ch5Ch6RCFailsafe'timestamp'uORBWriteTopic:'actuator_outputs''output'DigitalClock-C-GyroX(p)GyroY(q)GyroZ(r)QuaternionphithetaqRGB_MODERGBRGB_COLORquat2eulModeColorxyzvxvyvzRGB_Mode_SubsystemlocalPositionControlSystem1）打开Simulink仿真模型运行“e8/e8.3/HIL/Init_control.m”,初始化参数。“e83HIL.slx”文件会里的“ControlSystem”模块与软件在环仿真中的相同。chch1ch2ch3ch4ch5ch6pqryawxyzvxvyvz2）硬件连接需要注意的是，在进行硬件在环仿真时机架类型应该是“HILQuadcopterX”。数据线，另一数据线，另一端连电脑USB口乐迪R9DS接收机Pixhawk。最左侧排针，顺序白JR信号线：红为电源正，黒为地线，白为信号线图.硬件系统连接3）代码编译及下载将硬件在环仿真模型编译并下载文件到Pixhawk自驾仪中。这样就可以在Pixhawk自驾仪中运行我们自己设计的失效保护控制程序。点击编译点击下载下载完成点击下载图.代码编译及下载流程4）模型仿真器软件配置。213双击CopterSim桌面快捷方式即213可打开CopterSim。读者可以选择不同的动力系统模型，步骤如下：单击“模型参数”自定义参数，然后单击“存储并使用参数”存储并使用参数。软件会自动匹配串口号，单击“开始仿真”按钮就可以进入硬件在环仿真模式。此时可以看到如所示的界面左下角收到自驾仪返回的相关消息。位置速度5）3位置速度双击3DDisplay桌面快捷方式打开三维显示软件。欧拉角6）仿真效果欧拉角器失联了，可以看到飞行器自动返航并着陆。实际飞行轨迹实际飞行试