[发明专利]一种基于速度规划的受约束多智能体编队方法

权利要求书

1.一种基于速度规划的受约束多智能体编队方法，其特征在于步骤如下：

步骤1：中央主机接收到既定任务轨迹L(t)、需求阵型d(t)，虚拟leader轨迹设定为既定任务轨迹L(t)，得到各个follower智能飞行器的轨迹l_i(t)：

l_i(t)＝L(t)+d_i(t)

d(t)＝[d₁(t) d₂(t)…d_n(t)]^T

在任意时刻，中央主机都会生成一刚性阵型，而各个智能飞行器会去跟踪自己所对应的位置；

步骤2：将任务确定的每个智能飞行器的轨迹分解为多段，并对每段轨迹进行速度规划，使得在轨迹末端速度达到指定值且恰好能够平滑地连接下一段轨迹；

将轨迹分解的主要分解原则：根据轨迹形状分为圆弧型、直线型；

速度规划：

直线型：

t₁＝t_s+|v_m-v_s|

其中t_s～t₁时间内为一次函数型加速，t₁～t₂时间内为保持常值最大速度，t₂～t_e时间内为2次函数型减速；其中s为直线方向上路程，v_s为轨迹初始端点速度，v_m为轨迹段最大速度，v_e为轨迹结束端点速度；

圆弧型规划：

S₂＝v_m(t₂-t₁)t₁＝t_s+|v_m-v_s|

其中t_s～t₁时间内轨迹和速度为一次函数型加速，t₁～t₂时间内轨迹和速度保持常值最大速度，t₂～t_e时间内轨迹和速度为2次函数型减速；其中s为轨迹路程，v_s为轨迹初始端点速度，v_m为轨迹段最大速度，v_e为轨迹结束端点速度，θ_s为轨迹初始时速度朝向，定义x轴正方向为θ_s＝0，逆时针增大，θ_e为轨迹终止时的速度朝向，R为圆轨迹半径；

速度连接：分解而成的多段轨迹，若轨迹衔接处速度方向不一致，则要求前段轨迹末速度和后段轨迹初速度均为0；若轨迹衔接处速度方向一致，前段轨迹末速度和后段轨迹初速度不为0但必须相等；

步骤3：系统采用WIFI无线网络作为通讯手段，中央主机和每个智能飞行器双端检测通讯链路是否正常；

中央主机不断发送轨迹控制指令给各个智能飞行器，各个智能飞行器不断发送心跳包给中央主机，若主机在2s内未收到某一智能飞行器的任何心跳包，中央主机即认为该智能飞行器失联，切换至紧急模式，将所有其他受控智能飞行器停止运动，同时若某一智能飞行器2s内未接收到任何控制指令，即认为本机与主机失联，将立即停止运动。