【代码】合同网colavoidance4

共有四个Matlab文件，main.m、AirCraft.m、Arena.m和mission_planner.m

main.m

主文件比较简短，主要是模拟和控制无人机的飞行路径。

首先是一些初始定义和设置：

1. num_of_UAVs = 4: 定义无人机的数量为4。
2. time = 1300: 定义总时间为1300秒。
3. sample_arena = Arena: 定义一个模拟竞技场sample_arena，其位置和障碍物可以通过Arena.m文件进行修改。
4. tic: 开始一个计时器，用于计算代码运行时间。
5. for i = 1:1:num_of_UAVs : 对于每个无人机，初始化一个飞行器对象。
6. UAVs(i) = AirCraft(i): 将每个无人机与一个飞行器对象关联。
7. sample_mission_planner = mission_planner(UAVs, sample_arena): 使用给定的无人机和竞技场初始化任务规划器。
8. N = time/sample_arena.dt: 计算模拟的时间步数。

主循环：

1. for i = 1:1:N : 主循环，模拟无人机的行为。
2. for k = 1:1:numel(UAVs) : 对于每个无人机，计划其飞行任务。
3. sample_mission_planner.plan_mission(k): 为第k个无人机计划任务。
4. UAVs(k).move(sample_arena): 让第k个无人机移动。
5. toc: 停止计时器，并显示运行时间。

绘图部分：

1.对于每个无人机，绘制其飞行路径：

• X = [UAVs(l).vehical_log.x]; Y = [UAVs(l).vehical_log.y]; Z = [UAVs(l).vehical_log.h];: 获取每个无人机的位置信息。
• 使用不同的颜色绘制不同无人机的路径。

2.对于每个无人机，提取其速度、航向、倾斜度和高度信息：

• 速度随时间的变化。
• 航向随时间的变化。
• 倾斜度随时间的变化。
• 高度随时间的变化。

AirCraft.m

整个文件是用来设置无人机的飞行命令，并根据这些命令移动无人机，每段命令的大概格式如下：

语法：sample_Aircraft = AirCraft; %(生成一个名为Aircraft的对象)

说明：

• 更新命令：通过sample_AirCraft.cmd来访问。
• sample_AirCraft.cmd.type 可以设置为 'primitive'、'wayPt' 或 'orbit'。
• 当type为'primitive'时，sample_AirCraft.cmd.primitive设置为[airspd;alt;yaw]。其中airspd是速度（m/s），alt是高度（m），yaw是偏航角（从北方向开始顺时针测量，单位为度）。
• 当type为'wayPt'时，sample_AirCraft.cmd.wayPt设置为[E; N; U; airspd]。其中E、N、U是东、北、上的坐标（m），airspd是速度（m/s）。无人机将按照指定的速度和坐标飞行。
• 当type为'orbit'时，sample_AirCraft.cmd.orbit设置为[E; N; U; fly2orbAirspd; orbAirspd; radius; direction]。其中E、N、U是轨道中心的坐标（m），fly2orbAirspd是飞往轨道的速度（m/s），orbAirspd是在轨道上飞行的速度（m/s），radius是轨道的半径（m），direction表示方向，如果为1则为顺时针，如果为-1则为逆时针。

方法：sample_Aircraft.move; %(根据命令移动飞机)

首先用以下代码定义无人机的属性，包括识别号、保存飞行日志的属性和当前的步骤号，这些属性使对象能够保存和管理关于无人机状态和位置的信息。

1. properties(SetAccess = private)：定义了一个属性块，其中SetAccess = private意味着这个对象的属性只能在其类内部被访问和修改，而不能从外部访问或修改。

2. AC_ID：定义名为AC_ID的属性。从注释中可以知道，这是分配给无人机的识别号

3. vehical_log：定义名为vehical_log的属性。从注释中我们知道，这个属性用于保存无人机的日志数据，是一个结构体数组，其中每个元素都包含无人机在特定步骤中的位置和状态信息。

   • obj.vehical_log(step_no).x：表示在第step_no步中无人机的x坐标（东方向）
   • obj.vehical_log(step_no).y：表示在第step_no步中无人机的y坐标（北方向）
   • obj.vehical_log(step_no).h：表示在第step_no步中无人机的高度
   • obj.vehical_log(step_no).v：表示在第step_no步中无人机的速度
   • obj.vehical_log(step_no).gamma：表示在第step_no步中无人机的飞行路径角
   • obj.vehical_log(step_no).psi：表示在第step_no步中无人机的偏航角
   • obj.vehical_log(step_no).phi：表示在第step_no步中无人机的倾斜角
   • obj.vehical_log(step_no).cmdAirspd = obj.cmd.primitive(1);：将无人机的命令速度保存到日志中

4. step_number =1：定义名为step_number的属性，并初始化为1。从注释中我们知道，这个属性用于访问步骤号。

---

 接下来定义无人机的一些私有常量属性，包含无人机的基本参数和转换系数。

1. properties(Constant , GetAccess = private)：定义一个属性块，其中Constant表示这些属性是常量，即它们的值在创建对象后不能被修改。GetAccess = private表示这些属性只能从类内部访问，而不能从外部访问。

2. conversion转换系数：r2d = 180/pi，定义常量r2d，表示弧度到度的转换系数。π弧度等于180度，所以转换系数是180除以π。

3. 无人机基本参数：
   • W = 15：定义常量W，表示无人机的重量，单位是千克（kg）
   • span = 2.5：定义常量span，表示无人机的翼展，单位是米（m）
   • S = 0.8：定义常量S，表示无人机的翼参考面积，单位是平方米（m^2）
   • delta = 0.01：定义常量delta，它用于β的计算（升力系数Cl对阻力系数Cd的系数）。这个值通常是基于实验数据或经验确定的
   • Cdo = 0.02：定义常量Cdo，表示寄生阻力系数。寄生阻力是由于空气与无人机结构（如机身、机翼和尾翼）之间的摩擦和形状引起的阻力。这个值也是基于实验数据或经验确定的
4. 无人机推力控制或速度控制相关参数：

   • tau_T = 45：定义推力响应的时间常数tau_T，时间常数通常用于一阶或二阶动态系统来描述系统响应速度的快慢

   • kd_T = 0.1：定义推力控制中的微分增益kd_T，在控制系统中，微分增益用于改善系统的阻尼特性，减少超调和振荡

   • zero_T = -10：定义推力控制的零点位置zero_T，零点通常用于控制系统设计，以确保系统在某一点上的响应为零

   • kp_T = -AirCraft.zero_T * AirCraft.kd_T：定义推力控制的比例增益kp_T。比例增益用于调整系统对输入指令的敏感度。这里，kp_T的值是基于之前定义的zero_T和kd_T计算得出的

   • T_max = 100：定义推力的最大值T_max。这通常用于限制无人机的最大推力输出

   • T_min = -100：定义推力的最小值T_min。这同样用于限制无人机的推力输出，确保它在安全范围内

   • v_dot_max = 3：定义最大加速度指令v_dot_max，单位是米每秒平方（m/s^2）。这用于限制无人机可以接受的最大加速度

   • v_dot_min = -3：定义最小加速度指令v_dot_min，单位是米每秒平方（m/s^2）。这用于限制无人机可以接受的最小加速度

   • v_max = 16.5：定义最大速度指令v_max，单位是米每秒（m/s）。这个值大约是海平面处马赫数0.8的速度

   • v_min = 12.4：定义最小速度指令v_min，单位是米每秒（m/s）。这用于确保无人机不会飞得太慢，可能导致失速或其他不稳定行为

5. 无人机爬升、高度和飞行路径角的控制参数：

   • tau_Cl = 1：定义升力系数响应的时间常数tau_Cl。这个参数用于描述无人机升力系数变化的速率

   • kd_Cl = 0.5：定义升力系数控制中的微分增益kd_Cl。微分增益用于改善系统的阻尼特性，减少超调和振荡

   • zero_Cl = -1：定义升力系数控制的零点位置zero_Cl。零点通常用于控制系统设计，以确保系统在某一点上的响应为零

   • kp_Cl = -AirCraft.zero_Cl * AirCraft.kd_Cl：定义升力系数控制的比例增益kp_Cl。比例增益用于调整系统对输入指令的敏感度。这里，kp_Cl的值是基于之前定义的zero_Cl和kd_Cl计算得出的

   • k_gamma = 0.007：定义保持高度稳定性的比例增益k_gamma。这个参数通常用于高度控制回路中，以确保无人机能够准确地保持给定的高度

   • Cl_max = 100：定义最大升力系数Cl_max。升力系数是描述无人机产生升力能力的无量纲数，Cl_max用于限制无人机的最大升力产生能力

   • Cl_min = -100：定义最小升力系数Cl_min。这同样是一个限制，确保无人机的升力产生能力不会过低

   • gamma_dot_max = 15 / AirCraft.r2d：定义最大飞行路径角速率指令gamma_dot_max，单位是弧度每秒（rad/s）。这里，AirCraft.r2d是将弧度转换为度的转换系数

   • gamma_dot_min = -15 / AirCraft.r2d：定义最小飞行路径角速率指令gamma_dot_min，单位是弧度每秒（rad/s）

   • gamma_max = 13 / AirCraft.r2d：定义最大飞行路径角gamma_max，单位是弧度（rad）

   • gamma_min = -15 / AirCraft.r2d：定义最小飞行路径角gamma_min，单位是弧度（rad）

6. 无人机航向和倾斜角的控制参数：

   • K_phi = 0.5102：定义翻滚角控制的比例增益K_phi。这个增益用于调整无人机对翻滚角指令的响应敏感度

   • tau_phi = 0.1：定义翻滚角响应的时间常数tau_phi。这个参数用于描述无人机翻滚角动态响应的速度

   • k_psi = 0.7：定义保持航向稳定性的比例增益k_psi。这个增益通常用于航向控制回路中，以确保无人机能够准确地保持给定的航向

   • phi_max = 12 / AirCraft.r2d：定义无人机翻滚角的最大值phi_max，单位是弧度（rad）。这里，AirCraft.r2d是将弧度转换为度的转换系数。这个参数用于限制无人机的最大翻滚角，以确保飞行的稳定性

7. 无人机更高级别（如地面轨迹保持）的控制参数：

   • k_st = 0.8：定义地面轨迹保持的比例增益k_st。这个增益用于调整无人机对地面轨迹指令的响应敏感度

   • tau_m = 10：定义地面轨迹保持的时间常数tau_m。这个参数用于描述无人机在调整其地面轨迹时的动态响应速度

8. 无人机传感器测量噪声参数：

   • x_sd = 5：定义无人机位置测量在X轴方向上的标准差x_sd，单位是米（m）。这个参数用于描述X轴位置测量的噪声水平

   • y_sd = 5：定义无人机位置测量在Y轴方向上的标准差y_sd，单位是米（m）。这个参数用于描述Y轴位置测量的噪声水平

   • h_sd = 5：定义无人机高度测量的标准差h_sd，单位是米（m）。这个参数用于描述高度测量的噪声水平

   • v_sd = 2：定义无人机速度测量的标准差v_sd，单位是米每秒（m/s）。这个参数用于描述速度测量的噪声水平

   • gamma_sd = 3 / AirCraft.r2d：定义无人机飞行路径角速率测量的标准差gamma_sd，单位是弧度每秒（rad/s）。这里，AirCraft.r2d是将弧度转换为度的转换系数

   • psi_sd = 3 / AirCraft.r2d：定义无人机航向角测量的标准差psi_sd，单位是弧度（rad）

   • phi_sd = 3 / AirCraft.r2d：定义无人机翻滚角测量的标准差phi_sd，单位是弧度（rad）

   • omega_sd = 3 / AirCraft.r2d：定义无人机角速度（通常是俯仰、翻滚和偏航的角速度）测量的标准差omega_sd，单位是弧度每秒（rad/s）

---

 定义结构体属性，存储无人机的状态信息和其他相关数据：

1. properties (SetAccess = private , GetAccess = public；定义属性块，其中SetAccess = private表示只有当前类内部可以修改这些属性的值，而GetAccess = public表示这些属性的值可以从类的外部访问

2. state：声明一个名为state的结构体属性，用于存储无人机的当前状态

3. measured_state：声明一个名为measured_state的结构体属性，用于存储无人机的测量状态。注释中提到state和measured_state具有相同的结构，这意味着它们包含相同类型的字段

4. hb = struct('time',[] , 'pos',[] , 'speed',[] , 'hdg',[] , 'fp',[])：定义名为hb的结构体变量，用于存储无人机的“心跳”信息。这些字段在初始化时都是空的，稍后会被填充具体的数据。这个结构体包含以下字段：

   • time: 存储时间信息
   • pos: 存储位置信息
   • speed: 存储速度信息
   • hdg: 存储航向信息
   • fp: 存储飞行路径角信息

---

定义名为cmd的属性，它用于存储无人机的控制命令。这个属性被设置为public，意味着它可以从类的外部被访问和修改。

1. % cmd.type = 'primitive','wayPt','orbit'：注释说明cmd结构体可以有一个type字段，其值可以是'primitive'、'wayPt'或'orbit'。这些值指示了无人机的控制类型

2. % cmd.primitive = [airspd;alt;yaw]; (m/s;m;deg) yaw is measured clockwise from North：注释描述了cmd.primitive字段的结构和内容。cmd.primitive是一个包含三个元素的向量：

   • airspd：无人机的空速（单位：米/秒）。
   • alt：无人机的高度（单位：米）。
   • yaw：无人机的偏航角（单位：度），从北方向开始顺时针测量。

3. % cmd.wayPt = [E; N; U; airspd]; (m/s) Note: UAV will fly to way ENU at airspd

   ：注释描述cmd.wayPt字段的结构和内容。cmd.wayPt是一个包含四个元素的向量：

   • E：无人机需要飞到的东坐标（单位：米）。
   • N：无人机需要飞到的北坐标（单位：米）。
   • U：无人机需要飞到的上坐标（单位：米），通常是高度。
   • airspd：无人机飞往目标点的空速（单位：米/秒）。

   注释还指出，无人机将以指定的空速飞到指定的ENU（东、北、上）坐标。

4. % cmd.orbit = [E; N; U; fly2orbAirspd; orbAirspd; radius; direction]; clockwise if 1; anti-clockwise if -1：注释描述cmd.orbit字段的结构和内容。cmd.orbit是一个包含七个元素的向量：

   • E 和 N：轨道中心的东和北坐标（单位：米）。
   • U：轨道的高度或上坐标（单位：米）。
   • fly2orbAirspd：无人机飞往轨道中心的空速（单位：米/秒）。
   • orbAirspd：无人机在轨道上飞行的空速（单位：米/秒）。
   • radius：轨道的半径（单位：米）。
   • direction：轨道的飞行方向，如果为1则是顺时针，如果为-1则是逆时针。

---

接下来是methods：

一、首先定义名为 AirCraft 的构造函数，用于初始化一个名为 AirCraft 的对象。构造函数是一种特殊的方法，用于在创建对象时设置其初始状态。在这个例子中，AirCraft 构造函数接受一个参数 AC_ID_number，这个参数用于设置对象的 AC_ID 属性。将输入参数 AC_ID_number 的值赋给 obj 对象的 AC_ID 属性。这样，当创建一个新的 AirCraft 对象时，它的 AC_ID 属性就会被设置为传递给构造函数的 AC_ID_number 参数的值。当 AC_ID_number可以等于1，2，3，4，5，6，分别有不同的属性。

二、之后定义名为 move 的公共函数，该函数用于根据 sample_AirCraft.cmd 中的数据移动 AirCraft 对象。函数接受两个参数：obj（代表一个 AirCraft 对象）和 sample_arena（可能是一个代表飞行环境的结构体或其他数据类型）。

1. function move(obj,sample_arena)：定义名为 move 的函数，该函数接受两个参数：obj（一个 AirCraft 对象）和 sample_arena（可能是一个代表飞行环境的数据结构）

2. step_no = obj.step_number：从 obj（即 AirCraft 对象）中获取 step_number 属性的值，并将其存储在变量 step_no 中。这个 step_number 可能代表飞行过程中的当前步骤或时间步

3. switch obj.cmd.type：开始一个 switch 语句，根据 obj.cmd.type 的值选择执行不同的代码块。obj.cmd.type 是一个字符串，它的值可以是 'primitive'、'wayPt'、'orbit' 或 'onGrd'

4. case 'primitive'：如果 obj.cmd.type 的值为 'primitive'，则执行[obj.state,obj.measured_state] = DoPrimitive(obj,[],[],sample_arena)，调用 DoPrimitive 函数，并将 obj 和 sample_arena 作为参数传递。该函数可能会根据原始的飞行指令更新 obj 的 state 和 measured_state 属性。

5. case 'wayPt'：如果 obj.cmd.type 的值为 'wayPt'，则执行[obj.state,obj.measured_state] = DoWayPt(obj, sample_arena ,... obj.cmd,sample_arena.disturbance)，调用 DoWayPt 函数，并传递多个参数，包括 obj、sample_arena、obj.cmd 和 sample_arena.disturbance。这个函数可能会根据飞往指定航点的指令更新 obj 的状态。

6. case 'orbit'如果 obj.cmd.type 的值为 'orbit'，则执行[obj.state,obj.measured_state] = DoOrbit(obj, sample_arena)，调用 DoOrbit 函数，并传递 obj 和 sample_arena 作为参数。这个函数可能会根据在轨道上飞行的指令更新 obj 的状态。

7. case 'onGrd'：如果 obj.cmd.type 的值为 'onGrd'，则执行以下代码块[obj.state,obj.measured_state] = DoOnGrd(obj )，调用 DoOnGrd 函数，并仅传递 obj 作为参数。这个函数可能会根据在地面上的指令更新 obj 的状态。

之后是飞行器的日志记录功能，用于在每一步或每一时间步记录飞行器的状态信息。 

1. obj.vehical_log(step_no).x = obj.state.x;

   这行代码将飞行器的当前x坐标（代表东向位置）从obj.state.x复制到obj.vehical_log结构体中对应step_no步骤的x字段。

2. obj.vehical_log(step_no).y = obj.state.y;

   这行代码将飞行器的当前y坐标（代表北向位置）从obj.state.y复制到obj.vehical_log结构体中对应step_no步骤的y字段。

3. obj.vehical_log(step_no).h = obj.state.h;

   这行代码将飞行器的当前高度从obj.state.h复制到obj.vehical_log结构体中对应step_no步骤的h字段。

4. obj.vehical_log(step_no).v = obj.state.v;

   这行代码将飞行器的当前空速（速度大小）从obj.state.v复制到obj.vehical_log结构体中对应step_no步骤的v字段。注释中提到空速的范围是12.4 m/s到16.5 m/s。

5. obj.vehical_log(step_no).gamma = obj.state.gamma;

   这行代码将飞行器的当前飞行路径角（即飞行器速度与水平面的夹角）从obj.state.gamma复制到obj.vehical_log结构体中对应step_no步骤的gamma字段。

6. obj.vehical_log(step_no).psi = obj.state.psi;

   这行代码将飞行器的当前偏航角（yaw angle，即飞行器在水平面上的方向，顺时针从北向开始计算）从obj.state.psi复制到obj.vehical_log结构体中对应step_no步骤的psi字段。

7. obj.vehical_log(step_no).phi = obj.state.phi;

   这行代码将飞行器的当前坡度角（bank angle，即飞行器绕其垂直轴的旋转角度，正值表示向右倾斜）从obj.state.phi复制到obj.vehical_log结构体中对应step_no步骤的phi字段。

三、接着日志记录部分，根据飞行指令的类型（primitive、wayPt或orbit），从相应的指令结构体中提取空速和高度指令，并将这些值记录到飞行器的日志中。

1. 如果obj.cmd.type的值是'primitive'，则执行这个case块。这里假设obj.cmd.primitive是一个数组或结构体，其中包含至少两个元素或字段。第一行代码将obj.cmd.primitive的第一个元素（索引为1）赋值给日志中的cmdAirspd字段，代表空速指令。第二行代码将obj.cmd.primitive的第二个元素（索引为2）赋值给日志中的cmdAlt字段，代表高度指令。
2. 如果obj.cmd.type的值是'wayPt'，则执行这个case块。这里假设obj.cmd.wayPt也是一个数组或结构体，包含至少四个元素或字段。第一行代码将obj.cmd.wayPt的第四个元素（索引为4）赋值给日志中的cmdAirspd字段。第二行代码将obj.cmd.wayPt的第三个元素（索引为3）赋值给日志中的cmdAlt字段。
3. 如果obj.cmd.type的值是'orbit'，则执行这个case块。这里假设obj.cmd.orbit也是一个数组或结构体，包含至少五个元素或字段。第一行代码将obj.cmd.orbit的第五个元素（索引为5）赋值给日志中的cmdAirspd字段。第二行代码将obj.cmd.orbit的第三个元素（索引为3）赋值给日志中的cmdAlt字段。

四、更新无人机的“心跳”信息，“心跳”很形象，是一个定期发送的状态更新，用于通知其他系统或组件无人机的当前状态：

1. obj.hb.time = step_no：将无人机的“心跳”时间设置为当前的步骤号（step_no）。这通常用于跟踪无人机在模拟或实际飞行中的时间进度
2. obj.hb.pos.x = obj.state.x; obj.hb.pos.y = obj.state.y：将无人机的当前位置（x和y坐标）从其状态信息（obj.state）复制到“心跳”信息中的位置字段（obj.hb.pos）
3. obj.hb.pos.h = obj.state.y：将无人机的位置高度（h）设置为其y坐标（obj.state.y）。这看起来可能是一个错误，因为通常无人机的高度会由另一个字段（如obj.state.h）表示。除非在这个特定的上下文中，y坐标确实代表高度
4. obj.hb.speed = obj.state.v：将无人机的当前速度（obj.state.v）复制到“心跳”信息中的速度字段（obj.hb.speed）
5. obj.hb.hdg = obj.state.psi：将无人机的当前偏航角（obj.state.psi）复制到“心跳”信息中的偏航角字段（obj.hb.hdg）。偏航角通常表示无人机在水平面上的方向
6. obj.hb.fp = obj.state.gamma：将无人机的当前飞行路径角（obj.state.gamma）复制到“心跳”信息中的飞行路径角字段（obj.hb.fp）。飞行路径角是无人机速度与水平面的夹角
7. obj.step_number = step_no + 1：将步骤号加1，然后将其存储在obj.step_number中。这通常用于跟踪模拟或实际飞行中的当前步骤或迭代次数

---

接下来定义名为controller函数，它接受四个输入参数：obj、sample_arena、state和cont_outer，并返回一个输出control。

一、首先是“命令饱和”，其实也就是确定状态在范围内，不大于最大值，也不小于最小值。

二、计算变量：

1. 计算质量（m）：m=obj.W/sample_arena.g，obj.W 是飞行器的重量，而 sample_arena.g 是重力加速度。这个公式用于将飞行器的重量转换为质量。
2. 计算翼面积比（AR）：=obj.span^2/obj.S;飞行器翼型特性的参数。obj.span 是翼展（即翼尖到翼尖的距离），而 obj.S 是翼面积。这个公式计算了翼展的平方与翼面积的比值，通常用于评估翼型的效率或性能。
3. 计算贝塔（beta）：delta可能是飞行器的一个设计参数或调整因子。这个公式将(1.0 + obj.delta)除以(pi * AR)` 来计算贝塔值。这个值可能用于后续的飞行控制或性能评估

4. 动态压力（q_ref）的计算：q_ref=sample_arena.rho*state.v^2/2;动态压力（(q)）是流体力学中的一个重要参数，用于描述流体对物体表面的压力。这里，sample_arena.rho 是空气密度，state.v 是飞行器的速度。公式 q = 0.5 * ρ * v^2 用于计算动态压力。

5. 升力系数参考值（Cl_ref）的计算：升力系数（(C_L)）是一个无量纲数，描述了飞行器在给定动态压力下的升力与参考面积（通常是翼面积）和动态压力的乘积之比。这里，obj.S 是翼面积，state.phi 是飞行器的俯仰角，cont_outer.gamma_dot 可能是飞行路径角的变化率。公式用于计算升力系数的参考值。

6. 阻力系数（Cd）的计算：阻力系数（(C_D)）描述了飞行器在飞行中受到的阻力。这里，obj.Cdo 是基础阻力系数，而 Cl_ref^2 * beta 可能是一个由于升力产生的阻力项。

7. 推力参考值（T_ref）的计算：推力参考值（(T_{ref})）是飞行器为了保持其当前状态（如速度、高度等）所需的推力。这里，cont_outer.v_dot 可能是飞行器速度的变化率，Cd * q_ref * obj.S 是由于阻力产生的推力需求，而 obj.W * sin(state.gamma) 是由于重力产生的推力需求（当飞行器不是水平飞行时）。

8. 速度变化率（(v_{dot})）的计算：这个公式计算了从上一个状态（state.vPrev）到当前状态（state.v）的速度变化率。sample_arena.dt 是时间步长，即两次状态更新之间的时间间隔。速度变化率用于控制算法中，以确定如何调整飞行器的推力以维持或改变其速度。
9. 飞行路径角变化率（(\gamma_{dot})）的计算：这个公式计算了飞行路径角（gamma）从上一个状态（state.gammaPrev）到当前状态（state.gamma）的变化率。飞行路径角是飞行器速度向量与水平面之间的夹角。飞行路径角变化率用于飞行控制，特别是当飞行器需要改变爬升或下降率时。

三、计算控制输入：

1. 计算控制升力系数（Cl_cont）：Cl_cont 是计算出的控制升力系数。Cl_ref 是之前计算出的升力系数参考值。obj.kp_Cl 和 obj.kd_Cl 是控制增益，分别代表比例增益和微分增益。cont_outer.gamma 和 state.gamma 分别是期望的飞行路径角和实际的飞行路径角，而 cont_outer.gamma_dot 和 gamma_dot 分别是它们的变化率。通过比较期望和实际的飞行路径角及其变化率，可以计算出必要的控制升力系数来调整飞行器的飞行状态。
2. 计算控制推力（T_cont）：T_cont 是计算出的控制推力。T_ref 是之前计算出的推力参考值。obj.kp_T 和 obj.kd_T 是推力控制的比例和微分增益。cont_outer.v 和 state.v 分别是期望的速度和实际的速度，而 cont_outer.v_dot 和 v_dot 是它们的变化率。通过比较期望和实际的速度及其变化率，可以计算出必要的控制推力来调整飞行器的飞行状态。
3. 设置俯仰角控制（phi_cont）：phi_cont 是期望的俯仰角，它直接取自外部控制信号 cont_outer.phi。这意味着飞行器的俯仰角控制可能是由外部控制逻辑或操作员直接设定的。

四、同样再做一个饱和处理，确定状态在范围内，不大于最大值，也不小于最小值。

五、最后将之前计算出的控制输入组合成一个控制向量 control。具体来说，它将升力系数控制输入 Cl_cont、推力控制输入 T_cont 和俯仰角控制输入 phi_cont 组合成一个三维列向量。这种格式通常用于将多个控制输入传递给飞行器的控制系统，以便飞行器能够根据这些输入调整其飞行状态。

---

创建函数DoOnGrd ，是一个在地面（on-ground）处理状态的函数，主要用于模拟在地面上的测量过程。它接受一个对象 obj 作为输入，该对象包含飞行器的当前状态和其他相关参数。函数的主要目的是生成一个带有噪声的测量状态 meas_state，并将该测量状态赋值给对象的状态 state。

带有噪声的测量状态 meas_state，对于每个状态变量（如位置、速度、角度等），它都向真实状态值添加了一个随机噪声，该噪声是从正态分布中抽取的，标准差由对象 obj 的相应属性（如 x_sd、y_sd 等）定义。

---

创建函数DoOrbit，是一个模拟在轨道上飞行的函数。它基于给定的命令和扰动来更新飞行器的状态，并生成一个带有噪声的测量状态，根据飞行器与轨道中心的距离来决定是沿切线飞行还是开始轨道飞行，并更新飞行器的状态，接受两个参数：一个对象obj，它包含飞行器的当前状态和其他相关参数；和一个sample_arena对象，它可能包含有关飞行环境的信息，如扰动等。

函数从输入对象obj中获取之前的飞行器状态statePrev和命令cmd。同时，从sample_arena中获取扰动信息disturbance。从命令cmd中提取轨道的相关参数，如轨道中心位置orbCen、轨道半径orbRad、轨道速度orbSpd、轨道高度orbAlt、轨道方向orbDir和飞往轨道的速度fly2orbSpd。

计算飞行器当前位置到轨道中心的向量，然后将其单位化，并计算到轨道中心的距离。

计算飞行器应该沿着的切线方向，以便在轨道上飞行。切线点tangent_pt是根据飞行器到轨道中心的距离和轨道半径计算得出的。

检查飞行器是否足够接近轨道中心以开始轨道飞行。如果是，则设置命令以开始轨道飞行；否则，设置命令以沿切线方向飞行。在这两种情况下，都调用DoWayPt函数来更新飞行器的状态和生成带有噪声的测量状态。

---

创建函数DoPrimitive ，用于控制一个飞行器的飞行器控制流程：从提取飞行参数开始，设置飞行命令，通过外部控制器处理，最后更新飞行器的状态。这个过程中还考虑了扰动的影响。

1. 首先提取基本飞行参数，从 obj.cmd.primitive 数组中提取了三个基本飞行参数：速度（speed）、高度（alt）和航向（hdg）。
2. 其次获取扰动信息，从 sample_arena 对象中提取扰动信息。扰动可能包括风、大气条件等因素，它们会影响飞行器的动态行为。
3. 再获取飞行器的前一个状态（statePrev），以便与当前的控制命令一起使用。
4. 设置飞行命令，首先，设置飞行速度。然后，根据 alt 和 flt_path_ang 是否为空，决定使用哪一个作为高度控制参数，并设置相应的高度控制模式。同样，根据 roll 和 hdg 是否为空，决定使用哪一个作为航向或翻滚控制参数，并设置相应的控制模式。
5. 进行外部控制，调用 primitive_control 函数，使用飞行器的当前状态和前述设置的飞行命令，生成外部控制信号。
6. 进行控制器处理，使用 controller 函数处理外部控制信号，可能包括飞行器的动态模型、约束条件等，生成最终的控制信号。
7. 进行动态积分，使用 VehDynInt 函数（可能是飞行器动态积分的缩写）更新飞行器的状态。这个函数可能会考虑控制信号、扰动和前一个状态，来计算飞行器的下一个状态。
8. 模拟一个带有噪声的测量过程，每个测量状态（meas_state）的变量都是从其对应的真实状态（state）中计算出来的，加上一个随机噪声。这个随机噪声是通过乘以一个标准差（obj.x_sd, obj.y_sd, obj.h_sd 等）并从标准正态分布（randn(1)）中抽取一个随机数来生成的。

---

建立函数DoWayPt ，该函数用于根据给定的命令（cmd）和扰动（disturbance）来控制一个飞行器（或其他动态系统）的状态，并模拟带有噪声的测量状态。

1. 获取前一个状态：从 obj 中获取飞行器的前一个状态 statePrev。

2. 计算目标点航向：根据当前飞行器位置和目标点位置，计算从当前位置到目标点的航向角 hdg2wayPt。

3. 设置控制命令：根据目标点的信息（位置、高度、速度）和计算出的航向角，设置飞行器的控制命令 command。

4. 外部控制：调用 primitive_control 函数，使用当前状态和控制命令生成外部控制信号 cont_outer。

5. 控制器处理：调用 controller 函数，使用外部控制信号和其他相关信息（如飞行器模型、约束等）生成最终的控制信号 control。

6. 动态积分：调用 VehDynInt 函数（可能是飞行器动态积分的缩写），使用控制信号、扰动和前一个状态来更新飞行器的状态 state。

7. 模拟带噪声的测量状态：在更新后的状态 state 上添加随机噪声，以模拟实际测量中可能存在的误差，生成 meas_state。

---

函数 place_angle 的目的是将一个角度值规范化到 [-pi, pi] 的范围内。这通常用于处理周期性角度，比如圆的角度或方向角，以确保它们在合理的范围内。

---

建立函数primitive_control，该函数用于计算飞行器的外部控制指令。根据给定的状态（state）和命令（command），该函数生成速度（cont_outer.v）、飞行路径角（cont_outer.gamma）和滚转角（cont_outer.phi）等控制指令。

速度通道（Speed Channel）

• cont_outer.v 直接设置为速度指令 command.speed。

高度通道（Altitude Channel）

• 如果高度模式 command.alt.mode 为 0，则保持飞行路径角不变，cont_outer.gamma 设置为 command.alt.gamma（经过角度规范化处理）。
• 如果高度模式为 1，则进行高度保持，cont_outer.gamma 根据高度误差（command.alt.alt - state.h）和增益 obj.k_gamma 计算。

滚转通道（Roll Channel）

• 如果航向模式 command.lat.mode 为 0，则保持滚转角不变，cont_outer.phi 设置为 command.lat.roll（经过角度规范化处理）。
• 如果航向模式为 1，则进行航向保持，cont_outer.phi 根据航向误差（command.lat.hdg - state.psi）和增益 obj.k_psi 计算。
• 如果航向模式为 2，则进行地面轨迹控制，此时会计算一个参考的航向变化率 dpsi_ref，然后基于这个变化率和当前航向误差计算 cont_outer.phi。这里涉及到一些向量运算和反正弦函数 asin 的使用。

---

接下来使用了四阶龙格-库塔（Runge-Kutta）方法，计算不同的斜率（k1, k2, k3, k4），这些斜率用于估算下一个时间步长的状态。

---

创建VehDyn的函数，它用于计算无人机的动力学状态导数。这些导数描述了状态（如位置、速度、角度等）如何随时间变化。

---

Arena.m

这个文件是用于模拟无人机（UAV）的飞行环境。

1. 属性:

   • dt: 模拟的时间步长，单位为秒。
   • rho: 空气的密度。
   • g: 重力加速度，单位为米每秒平方。
   • disturbance: 表示风的影响，包括风速和风加速度。
   • safety_distance: 安全距离。
   • static_object: 静态物体的位置。
   • AO_waypoint: 一个目标点，所有无人机在起飞后都会接近这个点。

2. 方法:

   • Arena(): 这是Arena类的构造函数，用于初始化Arena对象。在这个函数中，静态物体的位置和AO_waypoint被初始化。

mission_planner.m

首先定义大类“mission_planner<handle”，主要功能是用于生成对对象AirCraft的命令指令。来自该类的对象将负责控制，而信使则嵌入在飞机本身中。你可以将其视为模拟器的游戏规划师/设计师。

---

接下来在大类中定义了一个类，并设置其访问级别为“private”，意味着这些属性只能在类的内部被访问和修改，而不能从类的外部直接访问。下面是各个属性的介绍。

1. Agents：用于保存任务中的所有UAV（无人驾驶飞行器）。

2. Arena：用于保存竞技场或环境信息。

3. running_on：是一个数组或矩阵，用于跟踪哪些UAV正在运行或正在为哪个UAV规划路线点。例如，如果running_on(1) = 1，则表示第一个UAV正在运行。

4. takeoff_order：是一个数组，保存了UAV的起飞顺序。例如，takeoff_order(1)可能是第一个UAV的ID，takeoff_order(2)是第二个UAV的ID，以此类推。

5. take_off_orbit：用于保存UAV的起飞轨道或路径。

6. step_number = 1：保存了主要的迭代次数或步骤数。从这里看，它被初始化为1。

这个类是为了管理一个包含多个UAV的飞行任务。每个UAV都有自己的起飞顺序和可能的任务规划。这个类提供了对这些信息的管理和存储。

---

然后定义了一个名为“Constant”的类，并为其设置了值，并设置其访问级别为“Constant”，意味着这个属性的值在初始化后不能被修改。

1. Delta=10000：定义了一个名为“Delta”的属性，并为其赋值10000。注释“Used in the takeoff mode”表明这个值可能在起飞模式中被使用。

---

之后又定义了一个类，并设置其访问级别为“private”，意味着这些属性只能在类的内部被访问和修改，而不能从类的外部直接访问。

1. working_mode：用于设置飞机的运行模式。注释提到了四种工作模式：起飞模式、飞往AO模式、搜索模式和跟踪模式。注释中解释了working_mode属性的用途和工作模式的选择。（for the takeoff mode：工作模式1，表示这是起飞模式。for the fly to AO mode：工作模式2，表示这是飞往某个目标（可能是机场或着陆点）的模式。for the search mode：工作模式3，表示这是搜索模式。for the trackm mode：工作模式4，表示这是跟踪模式。）

2. arc_number：为其赋值了一个包含四个1的数组。根据注释和上下文，这个属性可能与飞机的工作模式相关，例如用于表示每种模式下飞机应该执行的任务或行为。

总的来说，这个类是用来管理飞机的各种工作模式的，每种模式都有不同的行为和任务。通过设置working_mode属性，可以控制飞机在不同的工作模式之间切换，而arc_number属性可能用于进一步定义或调整每种模式下飞机的具体行为。

---

然后又定义了一个类，并设置其访问级别为“public”，意味着这些属性可以从类的外部直接访问和修改。

1. takeoff_parameters：是一个结构体，用于存储起飞模式的参数。注释中提到了几个字段，如飞机在起飞模式下的飞行高度(AC_current_level)、起飞标志(ready_flag)、飞机在协调过程中的状态(AC_CP_states)等。

2. WayPointArcs：是一个结构体，用于存储无人机需要遵循的弧线的数据。注释中提到了弧线的端点、角度和方向等信息。

3. WayPoint_level：用于表示无人机当前所在的位置或状态。如果无人机位于两个弧线之间并正在移动到下一个弧线，则其值为0；如果无人机正在跟随弧线，则其值为1。

4. NTimer：表示等待时间。注释中提到，如果在给定的等待时间内没有收到来自上一级的其他无人机的通信，无人机将移动到另一个位置。

5. ICA：表示在起飞模式中使用的中间高度或水平面。

这个类是用于管理无人机的起飞模式和相关参数的。通过设置和修改这些属性，可以控制无人机的起飞行为和路径规划。

---

定义函数“mission_planner”，该函数接受两个输入参数：UAVs（无人机的集合）和sample_arena（样本竞技场），主要功能是初始化一个对象，并设置该对象的多个属性。以下是对代码的详细解释：

1. 对象属性的初始化:

   • obj.Agents = UAVs: 将输入参数UAVs赋值给对象的Agents属性，表示无人机的集合。
   • obj.Arena = sample_arena: 将输入参数sample_arena赋值给对象的Arena属性，表示样本竞技场。

2. 工作模式的初始化:

   • obj.working_mode = ones(1,numel(UAVs)): 创建一个1xN的列向量，其中N是无人机的数量。这个向量被初始化为全1，意味着所有无人机的工作模式都被初始化为起飞模式。

3. 起飞顺序的初始化:

   • obj.takeoff_order = transpose(linspace(1,numel(UAVs))): 使用linspace函数创建一个向量，从1到无人机的数量。然后使用transpose转置该向量，得到一个行向量作为起飞顺序。

4. 计时器的初始化:

   • obj.NTimer = 20*ones(numel(UAVs)): 创建一个与无人机数量相同的列向量，并初始化为20。这可能是每个无人机的等待时间或计时器。

5. WayPoint_level的初始化:

   • obj.WayPoint_level = zeros(numel(UAVs)): 创建一个与无人机数量相同的列向量，并初始化为0。这可能是无人机在飞行过程中的水平点或高度点。

6. 无人机起飞模式的参数初始化:
   对于每一个无人机（由循环实现），其起飞模式的参数被初始化为：

   • AC_CP_states = 0: 表示无人机的状态在协调过程中为0。
   • ready_flag = 0: 表示无人机的准备标志为0。
   • AC_current_level = 0: 表示无人机当前的飞行高度或水平点为0。
   • jump_in_ready = 0: 表示无人机在准备跳跃时的状态为0。

---

接着定义了一个名为 get.take_off_orbit 的 函数，该函数用于获取无人机的起飞轨道信息。根据无人机的当前运行状态（obj.running_on），该函数返回不同的起飞轨道坐标和半径。

• 如果 obj.running_on 的值小于或等于 4，函数将设置输出 output 的 x、y、z 和 rad 属性，分别表示起飞轨道的 x 坐标、y 坐标、z 坐标和半径。在这种情况下，x 坐标为 1000，y 坐标为 0，z 坐标为 300，半径为 300。
• 如果 obj.running_on 的值大于 4，函数将设置输出 output 的 x、y、z 和 rad 属性为不同的值。在这种情况下，x 坐标为 1000，y 坐标为 0，z 坐标为 200，半径仍为 300。

此外，output 应该是一个结构体，用于存储轨道信息。

这段代码的目的是根据无人机的运行状态来确定无人机的起飞轨道，并将轨道信息以结构体的形式返回。

---

定义函数“get.ICA”（ICA是起飞模式中使用的中间高度或水平面），这个函数根据obj.running_on的值来设置output数组的内容。output是一个包含多个结构体元素的数组，每个结构体元素都有x、y、z、rad和ready这几个字段。

• 如果obj.running_on的值小于或等于4，则：
  • output(1)表示第一个ICA的位置和状态，其中x和y坐标为0，z坐标为100，半径rad为200，且ready字段为0，表示这个级别不是UAVi的跳入级别。
  • output(2)和output(3)表示第二和第三个ICA，它们的位置和rad值与output(1)相同，但ready字段为1，表示这两个级别是UAVi的跳入级别。
• 如果obj.running_on的值大于4，则：
  • output(1)和output(2)的位置和状态与obj.running_on小于或等于4时相同。
  • output(3)的位置发生了变化，z坐标变为300，其余字段值保持不变。

这个函数是用来生成ICA，根据无人机的运行状态（running_on），函数会生成不同的ICA配置。

---

定义get.WayPointArcs函数，用来获取无人机飞行路径上关键点的信息，包括每个点的x和y坐标、角度和朝向。根据obj.running_on的值，函数会返回不同的关键点信息。

• 当obj.running_on等于1时，函数定义了四个关键点（output(1)到output(4)），每个关键点都有一个x坐标范围、一个y坐标范围、一个角度（以弧度为单位）和一个朝向。这些点代表了无人机在特定运行阶段应该遵循的路径或航迹。

• 当obj.running_on等于2时，同样定义了四个关键点，但每个关键点的x、y坐标、角度和朝向都与obj.running_on等于1时不同。这意味着无人机在不同的运行阶段会有不同的飞行路径。

• 当obj.running_on等于3时，只定义了两个关键点（output(1)和output(2)），可能是因为在这个运行阶段，无人机只需要遵循两个关键点的路径。函数在这一点被截断了，所以无法看到当obj.running_on大于3时的情况。

每个关键点都有一个x字段和一个y字段，它们都是包含两个元素的数组。这可能意味着每个关键点定义了一个线段或一个区域，而不是一个具体的点。angle字段表示该关键点处的转向角度（以弧度为单位），而orientation字段可能表示无人机在该关键点处的朝向或飞行方向（正值为顺时针方向，负值为逆时针方向）。

---

定义函数plan_mission( obj ,k )，该函数接受两个输入参数：obj和k。其中，obj可能是一个包含无人机任务相关信息的对象，而k可能是用来指定哪一架无人机正在被规划任务的索引或标识。

1. obj.running_on = k：将obj对象的running_on属性设置为k的值，表示当前正在规划哪一架无人机的任务。

2. UAVs = obj.Agents：从obj对象中获取Agents属性，并将其值赋给变量UAVs。Agents是一个包含所有无人机信息的数组或列表。

3. AC=UAVs(obj.running_on)：使用obj.running_on作为索引从UAVs数组或列表中取出正在被规划任务的那架无人机的信息，并将其存储在变量AC中。

4. arena = obj.Arena：从obj对象中获取Arena属性，并将其值赋给变量arena。Arena可能表示无人机的操作环境或模拟空间。

5. switch obj.working_mode(k)：使用switch语句根据obj对象的working_mode属性在索引k处的值来决定执行哪个case。working_mode可能是一个表示无人机工作模式的数组或列表。

接下来的几个case语句根据无人机的不同工作模式来执行不同的任务：

• case 1：如果无人机的工作模式是1（起飞模式），则调用takeoff_mode函数来规划起飞任务，并将返回的命令存储在AC.cmd中。
• case 2：如果无人机的工作模式是2（飞向指定点模式），则调用Fly_to_AO_mode函数来规划飞向指定点的任务，并将返回的命令存储在AC.cmd中。
• case 3：如果无人机的工作模式是3（跟随任务路径模式），则调用follow_path函数来规划跟随任务路径的任务，并将返回的命令存储在AC.cmd中。

1. obj.step_number = obj.Agents(1).step_number：将obj对象的step_number属性设置为第一架无人机的step_number属性的值。这是为了确保所有无人机的任务步骤都是同步的。

该函数用于根据无人机的当前工作模式和其他相关信息来规划其下一个任务或行动。通过这个函数，无人机可以知道在给定的工作模式和环境条件下应该执行什么操作。

---

定义函数function cmd=takeoff_mode(obj , AC , arena , UAVs )，用于规划无人机的起飞模式。它根据无人机的当前状态和其他相关信息来确定如何执行起飞任务。它接受四个输入参数：obj（可能是一个包含任务信息的对象），AC（当前正在规划的无人机的信息），arena（无人机的操作环境），和UAVs（所有无人机的列表）。函数将返回一个命令（cmd），用于指导无人机的起飞。

1. ID = obj.running_on：从obj对象中获取当前正在规划的无人机的标识符或索引，并将其存储在变量ID中。

2. ICA = obj.ICA：从obj对象中获取ICA属性的值，并将其存储在变量ICA中。ICA的具体含义不清楚，但可能是与无人机起飞相关的某种参数或配置。

3. state = AC.state：从AC对象中获取state属性的值，并将其存储在变量state中。state可能表示无人机的当前状态（如起飞前、飞行中、降落中等）。

4. level = obj.takeoff_parameters(ID).AC_current_level：从obj对象的takeoff_parameters属性中，使用ID作为索引，获取AC_current_level属性的值，并将其存储在变量level中。这可能表示无人机当前的飞行高度或层级。

5. take_off_orbit = obj.take_off_orbit：从obj对象中获取take_off_orbit属性的值，并将其存储在变量take_off_orbit中。这可能表示无人机的起飞轨道或路径。

6. n = obj.step_number：从obj对象中获取step_number属性的值，并将其存储在变量n中。这可能表示任务的当前步骤或迭代次数。

switch obj.takeoff_parameters(ID).AC_CP_states：使用switch语句根据obj.takeoff_parameters(ID).AC_CP_states的值来执行不同的操作。AC_CP_states可能表示无人机的起飞控制状态或阶段。

case 0

用于处理无人机（UAV）起飞模式的逻辑。当无人机处于初始状态时，它会检查其他已经起飞的无人机是否位于其目标轨道之上，并根据这个检查来决定自己的下一步行动。

如果AC_CP_states的值为0，表示无人机处于起飞模式的初始状态，则执行以下操作：

• obj.takeoff_parameters(ID).ready_flag = 1;: 初始化ready_flag为1，表示无人机已经准备好起飞。如果ready_flag为0，则意味着之前起飞的某些无人机还没有达到目标轨道高度。
• 使用每个之前起飞的无人机的最后一个接收到的坐标来进行计算。这里假设在起飞模式下，每个无人机不会降低其高度。因此，使用它们的历史数据是有效的。
• 接下来的for循环遍历所有无人机：
• • for jj=1:numel(UAVs): 遍历所有无人机。
  • if jj < ID && (ICA(obj.takeoff_parameters(jj).AC_current_level+1).z - UAVs(jj).state.h <= 3): 这个条件检查是否有其他在ID之前的无人机（即之前起飞的无人机）位于当前无人机目标轨道的下方不超过3个单位的高度。如果是，则ready_flag保持为1，表示当前无人机可以起飞。

• if obj.takeoff_parameters(ID).ready_flag == 1: 如果ready_flag为1，即没有其他无人机阻挡，那么执行以下操作：

  • obj.takeoff_parameters(ID).AC_CP_states = 1;: 将无人机的控制状态设置为1，表示它现在处于起飞过程中的下一个阶段。
  • level = level + 1;: 将无人机的目标轨道高度增加1。
  • obj.takeoff_parameters(ID).AC_current_level = level;: 更新无人机的当前轨道高度。
  • cmd.type = 'orbit';: 设置命令类型为'orbit'，表示无人机将飞到新的轨道上。
  • cmd.orbit = [ICA(level).x;ICA(level).y;ICA(level).z;12;15;ICA(level).rad;1];: 设置轨道的具体参数，包括x、y、z坐标以及可能的其他参数（如速度和半径）。

• else: 如果有其他无人机阻挡：

  • 如果无人机当前在地面上（obj.takeoff_parameters(ID).AC_current_level == 0），则保持其在地面上的状态（cmd.type = 'onGrd';）。
  • 如果无人机不在地面上，则仍然设置其命令类型为'orbit'，但轨道参数保持不变（即保持在当前的轨道上）。

这段代码的目的是确保无人机在起飞前检查是否有其他无人机阻挡其路径。如果没有阻挡，它会设置一个新的轨道并命令无人机飞到那个轨道上。如果有阻挡，它会根据无人机的当前状态来决定是保持在地面上还是保持在当前的轨道上。

case 1 

是无人机在上升过程中判断是否达到其目标轨道水平，当前处于状态1，这意味着它正在向目标轨道水平上升。

首先计算与目标轨道的距离：a = abs(((state.x-ICA(level).x)^2+(state.y-ICA(level).y)^2+(state.h-ICA(level).z)^2)-(ICA(level).rad)^2);

•  `state.x`, `state.y`, 和 `state.h` 分别表示无人机的当前x坐标、y坐标和高度。
•  `ICA(level).x`, `ICA(level).y`, 和 `ICA(level).z` 分别表示目标轨道中心的x坐标、y坐标和z坐标。`ICA` 可能是一个存储轨道信息的数组或结构体，而 `level` 是一个索引或标识符，用于确定我们当前考虑的是哪个轨道。
•  `(state.x-ICA(level).x)^2`、`(state.y-ICA(level).y)^2` 和 `(state.h-ICA(level).z)^2` 是计算无人机与目标轨道中心在x、y和z方向上的距离的平方。
•  这些平方距离被加起来，然后减去目标轨道的半径的平方（`(ICA(level).rad)^2`）。这实际上是计算无人机到目标轨道的最近点的距离的平方。如果这个距离小于轨道半径，那么无人机就已经在轨道上了。
•  `abs` 函数用于取绝对值，确保结果始终为正数。

接着判断无人机是否到达目标轨道：

这里有两个 case 分支：

1. case 0：当 a 小于 obj.Delta 时执行。

   这个分支意味着无人机距离轨道中心的距离大于预定义的阈值 Delta。这表示无人机还没有达到指定的目标轨道水平。因此，无人机会继续爬升。

   命令 cmd.type 被设置为 'orbit'，意味着无人机将继续执行轨道相关的动作。cmd.orbit 被赋予了一个包含轨道信息的数组，其中包括轨道中心的 x、y、z 坐标，以及一些可能是轨道参数的值（如高度、速度、轨道半径等）。

2. case 1：当 a 大于或等于 obj.Delta 时执行。

   这个分支意味着无人机距离轨道中心的距离小于或等于预定义的阈值 Delta，即无人机已经达到了指定的目标轨道水平。

   这里提到了两种可能性：

   • 第一种情况是无人机已经达到了其预定义的保持模式（holding pattern）所在的同一水平面，这由 ICA(ii,UAV_current_levels(ii)).ready == 1 判断。在这种情况下，无人机将决定融入保持模式。

   • 第二种情况是无人机虽然达到了目标轨道水平，但还没有达到其预定义的保持模式所在的水平面。在这种情况下，无人机会继续爬升，返回到状态 0。

首先是ICA(level).ready == 1这种情况，同样是两个分支：

1.在 case 0 中：

• obj.takeoff_parameters(ID).AC_CP_states 被设置为 0，这表示无人机将返回到状态 0，并继续爬升到下一个中间级别。
• cmd.type 被设置为 'orbit'，指示无人机应继续执行与轨道相关的动作。
• cmd.orbit 被赋予了一个数组，其中包含轨道的相关信息，如轨道中心的 x、y、z 坐标，以及其他可能的轨道参数（如高度、速度、轨道半径等）。

2.在case 1 中：

它需要在保持模式中找到一个空隙并加入。这个代码段假设所有无人机都需要从当前中间轨道的第一象限跳入保持模式。

• 确保无人机位于第一象限，即 x 和 y 坐标都是非负的。这是跳入保持模式的预设条件。
• 计算跳入坐标：计算了从无人机当前位置到保持模式的最短距离的点 (u, v)。它使用了欧几里得距离公式，并假设无人机应该沿着保持模式的半径方向移动。
• 计算到达时间：计算了无人机从其当前位置到达 (u, v) 点所需的时间。它假设无人机以恒定的速度（在这里是 12 单位/时间步）移动，并且每个时间步长是 arena.dt。
• 设置跳入准备标志：将无人机的跳入准备标志设置为 1，表示该无人机已经准备好跳入保持模式。

然后要处理无人机在保持模式中的跳入逻辑，遍历所有的无人机，检查是否有其他无人机与当前无人机（由ID标识）在同一个保持模式中，并且已经在这个保持模式中。如果是这种情况，它会计算其他无人机到达某个特定点（u, v）的时间，并与当前无人机到达这个点的时间进行比较。如果两个时间非常接近（小于30秒），那么当前无人机就不能跳入这个保持模式，因为可能会与其他无人机发生冲突。

• 当jump_in_ready标志被设置为1，这意味着当前无人机与保持模式中的其他无人机之间的时间差大于30秒，因此它是准备好跳入保持模式的。设置无人机的命令类型为'orbit'，即保持模式。cmd.orbit是一个包含保持模式轨道参数的数组。这些参数包括轨道的中心坐标（x, y, z）、飞行高度（12）、飞行速度（15）、轨道半径（take_off_orbit.rad）和一个状态标志（1）。将无人机的某个状态标志（AC_CP_states）设置为2，这可能是表示无人机正在进入保持模式或已经处于保持模式中。
• 当jump_in_ready标志不被设置为1，设置了无人机的命令类型为'orbit'，但是使用了不同的轨道参数。这些参数来自ICA(level)，可能表示另一个预定义的轨道或初始爬升航道（Initial Climb Airway）。同样，这些参数包括轨道的中心坐标、飞行高度、飞行速度、轨道半径和一个状态标志。

case 2 

当无人机处于状态2时，代码计算无人机与保持模式轨道中心的距离，并检查无人机是否已到达保持模式轨道。如果无人机到达轨道，其状态将更新为3，并设置相应的轨道命令。

case 3 

当无人机处于状态3时，它已经在保持模式轨道上，并开始检查其他无人机是否也在它们的保持模式轨道上。无人机状态更新为4，并开始一个倒计时（这里设置为20）。同时，保持轨道命令不变。

case 4 

当无人机处于状态4时，代码执行倒计时。如果倒计时到达零，无人机的工作模式将切换到2（可能是飞向某个特定点或执行某种任务）。同时，保持轨道命令仍然不变。

---

定义Fly_to_AO_mode函数，目标是使无人机飞向给定的AO（可能是某个特定区域或目标点）的重心。这个函数接受两个输入参数：obj和arena。

设定无人机飞行指令，无论无人机的当前运行状态如何，函数都会设置无人机的飞行指令类型为wayPt（即飞向一个特定的点）。

设置飞行目标点，无人机将飞向arena.AO_waypoint定义的目标点。这个目标点由x、y和z坐标组成，并附加了一个高度或层信息。这里的index取决于无人机的running_on状态。如果running_on是1，那么index就是1；如果是2、3或4，那么index就是2。

状态检查和模式更改：如果无人机的x坐标大于某个阈值（对于running_on为1和2的无人机是500，对于running_on为3和4的无人机是1500），那么无人机的工作模式将被设置为3，并且会打印一条消息，表明无人机现在正在按照定义的路径飞行。

---

创建函数follow_path，根据无人机的当前状态和预设的路径点。接受一个输入参数 obj，返回一个飞行指令 cmd。

1. 获取当前活动的无人机：从 obj 中提取当前活动的无人机（可能由 obj.running_on 指定）的状态和配置信息，并将其存储在变量 AC 中。
2. 检查是否还有路径点要跟随：obj.arc_number(obj.running_on) 表示当前无人机要跟随的路径点的索引，而 numel(obj.WayPointArcs) 表示路径点数组 obj.WayPointArcs 中的元素数量。如果当前无人机的路径点索引小于或等于路径点数组的长度，说明还有路径点要跟随。
3. 获取新的路径点：调用 GetNewWayPoint 函数来获取基于当前情况的新路径点指令，并将其存储在 NewWayPoint 变量中。
4. 根据新路径点的类型生成飞行指令：根据 NewWayPoint.type 的值（可能是 'wayPoint'、'orbit' 或 'primitive'）来生成相应的飞行指令。如果 `NewWayPoint.type` 是 'wayPoint'，则设置 `cmd.type` 为 'wayPt'，并设置目标点的 `x`、`y`、`z` 坐标和高度为 `14`。  如果 `NewWayPoint.type` 是 'orbit'，则设置 `cmd.type` 为 'orbit'，并设置轨道的中心点坐标、高度、速度、高度变化率、半径和方向。 如果 `NewWayPoint.type` 是 'primitive'，则设置 `cmd.type` 为 'primitive'，并设置无人机的空速、高度和偏航角。
5. 如果没有更多路径点要跟随：如果没有更多的路径点要跟随（即 obj.arc_number(obj.running_on) 大于 numel(obj.WayPointArcs)），则无人机将飞向一个预设的目标点 (8000, 0)，高度为 200，高度层为 14。

---

上文中提到的GetNewWayPoint 的函数就在这里，接受参数 obj，首先根据无人机的当前状态（是否在两个路径点之间还是在沿着某个路径点飞行）来确定下一个路径点的类型。然后，它根据这个信息来计算下一个路径点的坐标以及其他相关的飞行参数。

1. 获取无人机当前的状态和路径点信息：

   • level：表示无人机当前的状态，0 表示无人机在两个路径点之间飞行，1 表示无人机正在沿着某个路径点飞行。
   • arc_number：表示无人机当前正在跟随的路径点的索引。
   • arc：表示无人机当前正在跟随的路径点的详细信息，包括端点坐标、角度和方向。
   • AC：表示无人机的当前状态，包括其位置、高度等信息。

2. 计算下一个接近点（Next Point of Approach, POA）：

   • POA 的坐标被设置为路径点的第一个端点的 x 和 y 坐标，高度设置为无人机当前的高度。
   • yaw 是无人机需要达到的下一个偏航角，由 next_yaw 函数计算得出。

3. 确定下一个路径点的类型：

   • 如果 level 为 0，表示无人机在两个路径点之间飞行，那么下一个路径点的类型为 'primitive'，并且如果无人机距离 POA 的距离小于 10，则更新 level 为 1，表示无人机将开始沿着路径点飞行。
   • 如果 level 不为 0（即 level 为 1），表示无人机正在沿着某个路径点飞行，那么下一个路径点的类型为 'orbit'。在这种情况下，函数会计算路径点的半径、中心坐标等信息。

4. 更新路径点信息：

   • 如果无人机距离当前路径点的第二个端点的距离小于 20，则更新 level 为 0，表示无人机将开始飞行到下一个路径点，并更新 arc_number，表示无人机将开始跟随下一个路径点。
5. 设置NewWayPoint 的属性：

   • 当 cmd_type 的值为 'orbit' 时，执行相应的 case 分支。这个分支设置了 NewWayPoint 的类型为 'orbit'，并设置了其 x、y、radius（半径）和 orientation（方向）属性。这些属性是无人机在沿圆弧路径飞行时所需要的。

   • 当 cmd_type 的值为 'primitive' 时，执行这个 case 分支。这个分支设置了 NewWayPoint 的类型为 'primitive'，并设置了其 yaw 属性。yaw 是无人机的偏航角，表示无人机需要转向的方向，以便它可以到达下一个路径点。

---

定义函数next_yaw，计算无人机的下一个偏航角，以确保它能够安全地避免障碍物并到达一个预定的点。

1. 变量初始化:

   • max_tcpa = 60: 设置一个最大碰撞时间避免（TCPA）为60秒。这意味着如果无人机与障碍物的碰撞时间少于1分钟，无人机将不会做出反应。
   • update_frequency = 20: 设置更新频率为20。这意味着每20步，无人机将更新其目标点。
   • update_flag = mod(obj.step_number, update_frequency) == 0: 这是一个标志，用于检查是否应该更新目标点。当步数 obj.step_number 是 update_frequency 的倍数时，标志将为真（true），表示需要更新目标点。

2. 无人机信息:

   • mm = obj.running_on: 获取正在运行此函数的无人机的ID。
   • AC = obj.Agents(mm): 根据无人机的ID从 obj.Agents 数组中获取无人机的当前状态和其他相关信息。
   • safety_distance = obj.Arena.safety_distance: 获取安全距离，即无人机与障碍物之间应保持的最小距离。

3. 无人机的当前状态:

   • current_yaw = find_heading(AC): 调用 find_heading 函数来获取无人机的当前偏航角（heading）。
   • my_speed = AC.state.v: 获取无人机的当前速度。
   • my_position = [AC.state.x, AC.state.y]: 获取无人机的当前位置（x和y坐标）。
   • my_velocity = [my_speed * sin(current_yaw), my_speed * cos(current_yaw)]: 计算无人机的速度向量，基于其当前速度和偏航角。
   • my_heading = current_yaw: 设置无人机的当前偏航角为 my_heading，这里似乎是重复了 current_yaw 的值。

4. 障碍物准备:

   • 代码中的注释部分提到了准备障碍物列表。每个障碍物应该是一个结构体，包含位置（x和y坐标）、速度（x和y分量）和偏航角（从北方向测量的角度）。
   • kk是一个计数器，用于跟踪obstecals 数组中当前障碍物的索引。obstecals 是一个预定义的数组，用于存储有关场景中障碍物（包括移动和静态障碍物）的信息。

   • 处理移动目标：

     • 循环遍历 obj.Agents 数组中的每个元素（即每个无人机）。
     • 如果当前无人机的 ID (i) 不等于正在运行的无人机的 ID (mm)，则将其视为障碍物。
     • 对于每个障碍物无人机，提取其位置（x 和 y 坐标）、速度（通过计算速度和方向得出）和偏航角（heading）。
     • 将这些信息存储在 obstecals 数组的当前位置（由 kk 索引指定）。
     • 更新 kk 以指向 obstecals 数组中的下一个位置。

   • 处理静态目标：

     • 循环遍历 obj.Arena.static_object 数组中的每个元素（即每个静态物体）。
     • 对于每个静态物体，提取其位置（x 和 y 坐标），并将速度设置为 [0 0]（表示静态，没有移动）。
     • 设置偏航角为 0。
     • 将这些信息存储在 obstecals 数组的当前位置（由 kk 索引指定）。
     • 更新 kk 以指向 obstecals 数组中的下一个位置。

   • 最后，nn = kk-1; 这行代码记录了 obstecals 数组中实际障碍物的数量（因为 kk 在循环结束后会多增加一次）。nn 将用于后续处理，例如计算无人机与障碍物之间的距离和角度，以决定无人机的下一步动作。

5. 可视化无人机和障碍物的位置和动态：

   • 当mm等于1且update_flag也等于1时，创建一个编号为2的图形窗口，并清除其内容。使用hold on保持图形，以便在同一窗口中绘制多个元素。这个循环遍历前四个无人机（由i <= 4确定），并对它们进行可视化。对于每个无人机，代码提取其位置信息，并根据无人机的索引为其轨迹选择不同的颜色。同时，还绘制了无人机的当前位置（红色星号）和安全距离圆对于每个障碍物，代码计算了一个表示其朝向的向量（基于其位置和速度），并将其绘制为绿色线条。同时，也绘制了障碍物的当前位置（红色星号）和安全距离圆。此外，代码还绘制了一个五边形标记（代表无人机的下一个接近点，即POA），其位置由POA.x和POA.y确定。
   • 当mm等于2且update_flag等于1且obj.Agents(1).state.x的值大于1900时，将焦点切换到编号为2的图形窗口，保持当前图形窗口的内容，以便在相同的图形上添加新的图形元素，在当前图形窗口上绘制一个标记为五角星的点，其x坐标为POA.x，y坐标为POA.y。这个五角星的颜色是黑色（由[0 0 0]指定）。
   • 当mm等于3且update_flag等于1且obj.Agents(1).state.x、obj.Agents(2).state.x的值大于1900时，切换到编号为2的图形窗口，保持当前内容，并在相同的图形上绘制一个黑色的五角星标记。

6. 定义常量hmin和hmax：它们分别表示最小和最大的角度值，以弧度为单位。

   • hmin = 10*pi/180;：这行代码计算了hmin的值。它将10度转换为弧度。转换公式是弧度 = 角度 * pi / 180。因此，hmin的值是10度对应的弧度值。
   • hmax = 180*pi/180;：这行代码计算了hmax的值。它将180度转换为弧度。由于180度是π弧度，所以hmax的值是π。

7. 检测无人机与障碍物之间的潜在碰撞:

   • 初始化变量jj=1。
   • 找到时间和CPA（Closest Point of Approach，最近接近点）分离：
     • 计算无人机与第i个障碍物之间的相对位置向量vec1
     • 计算无人机与第i个障碍物之间的相对速度向量vec1
     • 计算相对速度（mod_v_rel）的模（即大小或长度）
     • 如果mod_v_rel大于5，则计算TCPA（Time to Closest Point of Approach）和PCPA（Position at Closest Point of Approach）、distance_CPA（两个PCPA之间的欧几里得距离）
     • 当distance_CPA小于两倍的安全距离、tcpa在max_tcpa之内、tcpa大于5且大于0、AC.state.x大于1900时，
     1. 如果obj.step_number是200的倍数，使用fprintf函数输出一条消息，指出无人机和障碍物将在多少秒内发生碰撞，以及碰撞时的距离。
     2. 为了找到所需的机动操作，计算无人机与第i个障碍物之间的相对航向角。这里使用abs函数来确保相对航向角是非负的。if obstecals(i).heading~=0
        • 检查第i个障碍物的航向是否不为0。这意味着障碍物是在移动的（因为静态障碍物的航向被假设为0）。
        • if relative_heading<pi
          • 如果相对航向角小于π（即180度），则进入此条件。
          • if relative_heading<=hmin，如果相对航向角小于或等于hmin（即10度），则执行以下操作：
            • 如果无人机的航向小于障碍物的航向，则保持当前方向（mode = 0）。
            • 否则，从右侧交叉障碍物（mode = 1）。
          • elseif relative_heading<=hmax && relative_heading>=hmin，如果相对航向角在hmin和hmax（即10度到180度）之间，则执行以下操作：
            • 如果无人机的航向小于障碍物的航向，则保持当前方向（mode = 0）。
            • 否则，从右侧交叉障碍物（mode = 1）。
          • 如果相对航向角大于π（即180度），则执行以下操作：
            • 如果无人机的航向大于障碍物的航向，则保持当前方向（mode = 0）。
            • 否则，从右侧交叉障碍物（mode = 1）。
          • 如果障碍物的航向为0，则假设障碍物是静态的，并从右侧交叉障碍物（mode = 1）。
        • 最终mode变量将包含无人机在面对障碍物时应采取的机动模式的值。无人机可以选择保持当前航向（mode == 0）或者改变航向以避开障碍物（mode == 1）

          • 当mode == 0时：无人机不需要改变航向，因此其可选择的航向角范围是完整的360度。available_angles(jj).data被设置为从0度到360度的所有角度，即0:pi/180:2*pi

          • 当mode == 1时：无人机需要改变航向以避开障碍物。xp和yp似乎是障碍物预测碰撞点的坐标。temp_angle1和temp_angle2的计算涉及到无人机与障碍物之间的相对位置和避障的安全距离。temp_angle1是基于预测碰撞点的距离计算的，而temp_angle2则是基于安全距离计算的。temp_available_start_at计算了无人机可以开始改变航向的起始角度。这是通过从无人机的当前航向中减去temp_angle1并加上2*temp_angle2来得到的。available_angles(jj).data被设置为从temp_available_start_at开始，到无人机的当前航向加上π（即180度）的范围内的所有角度。这意味着无人机可以选择在这个范围内改变其航向以避开障碍物。

   • 最后，jj变量增加1，以便在下一个障碍物上执行相同的逻辑。

8. 绘制无人机在面对障碍物时可选择的航向角范围的图形表示：

   • 由于jj变量在之前的代码中用于迭代障碍物并填充available_angles数组，因此jj-1给出了数组中有效元素的索引范围。
   • 当jj不等于1（这意味着至少有一个障碍物被考虑并且available_angles数组中有至少一个有效元素）且update_flag等于1时，根据当前的迭代次数i，并据此选择不同的颜色和半径来绘制圆弧。例如，如果i等于1，则使用绿色（'g'）和半径400来绘制圆弧；如果i等于2，则使用蓝色（'b'）和半径500；如果i等于3，则使用红色（'r'）和半径600。

9. 找到所有可行航向角（yaw angles）的交集：即无人机在面对多个障碍物时可以同时满足的航向角范围

   • 如果num_of_angle_array等于3（即有三个障碍物）：

     • 代码首先计算每个available_angles数组中第一个元素的度数表示（将其从弧度转换为度）。
     • 然后，它找到这些角度范围中的最小起始角度和最大结束角度。
     • 最后，它创建一个新的数组final_available_angles，表示这些最小起始角度和最大结束角度之间的所有角度（以弧度为单位）。

   • 如果num_of_angle_array等于2（即有两个障碍物）：

     • 代码将每个available_angles数组中的角度从弧度转换为度，并存储在array1和array2中。
     • 如果array1的第一个元素大于array2的第一个元素，则交换这两个数组，以确保array1的第一个元素是最小的起始角度。
     • 然后，它使用两个嵌套的for循环来找到array1和array2中共同的角度，并将这些角度（以弧度为单位）存储在final_available_angles数组中。

   • 如果num_of_angle_array等于1（即只有一个障碍物）：final_available_angles直接设置为该障碍物的可行航向角范围。

   • 如果num_of_angle_array小于1或大于3：这意味着没有障碍物或存在超过三个障碍物。在这种情况下，代码将final_available_angles设置为表示所有可能航向角的范围，即从0度到360度（以弧度为单位）。

10. 计算从无人机的当前位置到一个指定点（POA，可能是目标点或障碍物点）的向量与某个参考单位向量（这里是 [0 1]，表示正北方向）之间的夹角，并找出这个夹角与一组给定的可行航向角（final_available_angles）中哪一个最接近：

    • 计算向量和夹角：使用了点积和向量的模长来计算夹角

    • 查找最接近的可行航向角

11. 设置无人机偏航角：

    • 检查更新标志：update_flag如果等于1，这意味着有新的航向角信息需要被应用。

    • 应用新的航向角：如果update_flag为1，代码将从final_available_angles数组中获取与temp_head最接近的航向角（通过which索引指示）。然后，它将这个航向角从弧度转换为度（乘以180/pi），并将结果存储在yaw变量中。

    • 没有新的航向角信息：如果update_flag不是1（即等于0），这意味着没有新的航向角信息需要被应用。

    • 获取当前航向角：从AC.cmd.primitive(3)获取当前的航向角。这看起来像是从无人机的控制命令或状态中获取的信息。如果成功获取了航向角，它将被存储在yaw变量中。

    • 捕获异常：catch块将被执行。在这种情况下，代码将使用之前计算的temp_head（与unit_vec的夹角）作为航向角，并将其从弧度转换为度（乘以180/pi），然后将结果存储在yaw变量中。

---

定义find_heading的函数，该函数计算并返回无人机的当前航向角。航向角是无人机当前方向与正北方向之间的夹角。代码使用了无人机在连续两个时间步的位置信息来估计航向角。

1. 获取前一个和当前位置:从 AC 中提取前一个和当前的位置信息。AC.vehical_log 可能是一个存储无人机历史位置的数组或日志，而 AC.step_number 是当前时间步的索引。
2. 计算航向向量:计算从前一个位置到当前位置的向量 heading_vec。这个向量代表了无人机的移动方向。
3. 定义单位向量:定义了一个指向正北方向的单位向量 unit_vec。
4. 计算航向角:使用点积和向量模长来计算 heading_vec 与 unit_vec 之间的夹角。结果是一个弧度值。
5. 调整航向角范围:如果 heading_vec 的 x 分量（即东西方向的分量）小于 0，说明无人机是向西移动的。在这种情况下，需要将航向角调整为 [pi, 2*pi) 的范围内。