[发明专利]一种基于轨迹预测的编队围捕方法

权利要求书

1.一种基于轨迹预测的编队围捕方法，包括以下步骤：

步骤1：为解决传感器视野丢失问题，对机器人运动状态进行估计与预测，具体包括：

1.1通过先验知识建立目标运动轨迹的拟合函数，如式(1)所示

F_k(t；C_k)＝c₀φ₀(t)+c₁φ₁(t)+…+c_γφ_γ(t) (1)

其中，F_k(t；C_k)为k时刻轨迹拟合函数，{φ_i(t)}_{i＝0,1,…,γ}为先验函数，根据先验知识设置为单项式或者三角函数，C_k＝{c_i}_{i＝0,1,…γ}为k时刻待确定的参数，γ为拟合函数的阶数，根据实际需要选定；

1.2初始化{φ_i(t)}_{i＝0,1,…,γ}，C_k；

1.3机器人传感器对环境进行感知，寻找目标，若感知到目标时，其量测信息为Y_k，则存储k时刻量测数据y_k＝Y_k；

1.4设置合适的滑动时间窗口[k-l,k]，其窗口长度为l，定义时间窗口内拟合残差函数如式(2)所示

并通过求解最优化问题，最小化拟合残差求解轨迹时间函数的参数估计如式(3)所示

1.5通过时刻k求解的参数估计获得最优轨迹时间函数通过F_k(t)计算目标在k时刻的位置估计F_k(k)与下一时刻的位置预测F_k(k+1)；

1.6令k→k+1，循环执行上述步骤1.3、1.4、1.5；当执行至步骤1.3时，先判断传感器是否感知到目标，若感知到目标则继续执行；若未感知到目标则执行步骤1.7；

1.7通过上一时刻的最优轨迹时间函数F_k-1(t)计算当前时刻的目标位置预测F_k-1(k)，存储k时刻量测数据y_k＝F_k-1(k)，返回执行步骤1.4；

步骤2：建立多机器人编队围捕模型，具体包括：

2.1建立单个机器人动力学模型，如式(4)所示

其中，[sx_k sy_k]^T为k时刻机器人的位置坐标，θ_k为k时刻机器人的角度，v_k为机器人的线速度，w_k为机器人的角速度，T₀为系统的时间周期；

2.2建立多机器人系统模型，如式(5)所示

X_i,k+1＝X_i,k+Qu_i,k (5)

其中，i为机器人的编号，X_i,k∈R^3×1为k时刻机器人i的坐标与角度，u_i,k∈R^2×1为k时机器人i的控制量输入，Q∈R^3×2为控制量的传递函数；

步骤3：计算多机器人系统的控制量，对目标进行编队围捕，具体包括：

3.1初始化编队围捕方法，如选择方法1利用当前时刻目标信息进行围捕则执行步骤3.2；如选择方法2利用下一时刻目标信息进行围捕则执行步骤3.3；

3.2选取目标在k时刻的位置估计F_k(k)，作为多机器人系统的围捕目标位置，q_k＝F_k(k)；

3.3选取目标下一时刻的位置预测F_k(k+1)，作为多机器人系统的围捕目标位置，q_k＝F_k(k+1)；

3.4计算多机器人自身的期望围捕点，以环形队形为例，如式(6)所示

其中，为机器人的期望围捕点坐标，R为圆环队形的半径，N为编队中移动机器人的数量；

3.5计算k时刻机器人与期望围捕点的距离和角度，如式(7)所示

其中，d_k为k时刻机器人与期望围捕点的距离，为k时刻机器人与期望围捕点的角度；

3.6计算k时刻机器人的控制量，如式(8)所示

其中，λ∈R^2×1为控制参数。

2.如权利要求1所述的一种基于轨迹预测的编队方法，其特征在于，在所述步骤1中，利用最小化拟合残差算法迭代求得当前时刻特定长度时间窗口内目标运动的轨迹时间函数，通过轨迹拟合计算目标位置的状态估计，在视野丢失时利用上一时刻的轨迹时间函数计算当前时刻目标位置的状态预测值作为补偿，提高了定位精度，减少了目标视野丢失带来的影响。在步骤3执行在编队围捕时，通过对目标位置的预测值进行预测围捕，提高了围捕跟踪的效率。