一种多机器人动态目标跟踪协同控制方法与流程

1.本发明涉及多机器人控制系统技术领域，具体为一种多机器人动态目标跟踪协同控制方法。


背景技术：

2.目标跟踪技术是机器人领域的一个重要的研究方向，机器人动态目标跟踪技术无论在军事领域还是在人们的日常生活中都有着广泛的应用。在军事上，可用于战场监视、执行拦截任务时对敌方目标进行定位、跟踪和打击等。只有准确的获取敌方目标的运动轨迹，才能执行后续的拦截和打击任务。在民用领域，服务机器人越来越智能化，智能移动机器人出现在越来越多的公共场所为人们提供服务，例如银行大厅里的智能机器人等，服务机器人智能化的关键环节是机器人的目标识别与跟踪。动态目标跟踪是当前机器人领域的一个热点问题，越来越多的国内外学者致力于多机器人动态目标跟踪技术的研究并取得了一些引人瞩目的成果。
3.多机器人动态目标跟踪中常用的方法是基于模型预测的方法。为避免监测区域出现监测盲点，同时考虑对动态目标的跟踪精确度，通常可以将多机器人协同控制问题建模成一个最优控制问题，即在满足约束条件下寻找最优的多机器人控制策略，使得跟踪精确度最优。此问题被认为是一个np困难问题，即找到此问题最优解的计算量将随着机器人数量的增加而呈指数增长。即使使用传统的贪婪算法，在每一时刻，通过对目标下一时刻的状态进行预测，寻求局部最优解，仍面临计算量巨大，不能够实时规划的问题。


技术实现要素：

4.为解决多机器人动态目标跟踪传统估计方法的缺点，本发明提供了一种多机器人动态目标跟踪协同控制方法，可以实现多机器人对动态目标的实时跟踪，在满足覆盖质量的要求的同时保证感知质量最小化。
5.本发明的技术方案如下：一种多机器人动态目标跟踪协同控制方法，包括动态目标g和观测动态目标g的n个机器人，其特征在于：还包括以下步骤：步骤1，建立动态目标的目标跟踪系统一般化模型,包括动态目标的运动学模型和目标跟踪系统的观测模型；通过移动机器人携带的传感器判断是否能探测到动态目标，是进入步骤2，否则转入步骤3。
6.步骤2，通过目标跟踪系统的观测模型测量各机器人与动态目标的距离，估计目标当前时刻的状态和误差协方差矩阵,使用加权平均的方法, 将n个机器人对目标的状态估计进行数据融合，作为当前时刻目标状态的最优估计，并对动态目标g进行一步预测。
7.步骤3，基于近似贪婪算法,在满足覆盖质量的情况下，求解最小感知质量。
8.步骤4，根据步骤3求解所得的最小感知质量，规划下一时刻最优的机器人位置，移动机器人使其实现对动态目标g的跟踪。
9.作为优选，步骤1中动态目标的运动学模型采用的是ct模型，描述为x（k 1）=ax（k）
w（k），其中x为状态变量，a为状态转移矩阵，w为系统噪声，k为任意时刻，且k≥0。
10.作为优选，步骤1中目标跟踪系统的观测模型为：每个机器人均携带超声波传感器，该超声波传感器能够测量该机器人与目标之间距离，描述为：z
i
（k）=h
i
（x(k)) v
i
（k），z
i
为机器人i的测量值，h
i
为动态目标与机器人i的相对距离:，其中（x
i
,y
i
）为移动机器人i的位置，(x,y)为动态目标g的位置，v
i
为机器人自身的测量噪声，x为状态变量，下标i表示第i个机器人，i=1,2，
…
n。
11.作为优选，步骤2基于运动学模型通过扩展卡尔曼滤波算法求得状态预测和误差协方差预测，以此更新目标当前时刻的状态和误差协方差矩阵。
12.作为优选，加权平均的方法为：对所有观测到目标的机器人获取的估计值赋予相同的权重，即对所有机器人的估计值取平均值作为目标状态的估计，将多个机器人对目标的状态估计进行数据融合，作为当前时刻目标状态的最优估计，并对动态目标g进行一步预测。
13.作为优选，步骤3具体为：步骤3.1，定义感知质量j
sense
为融合后的误差协方差矩阵p的行列式j
sense
=det(p)；步骤3.2，定义覆盖质量为，a
cov
为至少被一个机器人覆盖的区域面积，a
foi
为监控范围的总面积；步骤3.3，定义判断机器人i的探测范围的函数i
i
，当机器人i能够观测到时，函数i
i
为1；当机器人i不能够观测到时，函数i
i
为0，通过二重积分求面积的方法进一步求得覆盖质量j
cov
与探测范围函数i
i
之间的表达式，求出覆盖质量；步骤3.4，根据近似贪婪算法，在满足覆盖质量的前提下，求解最小感知质量。
14.作为优选，步骤3.4中对覆盖质量j
cov
的要求为j
cov
≥0.7。
15.作为优选，步骤3.4中近似贪婪算法为在任意时刻逐个规划每个机器人的最优控制策略，通过遍历搜索方法求得最小感知质量。
16.作为优选，步骤3.4中近似贪婪算法具体为：假设第1个机器人进行移动，固定其余n
‑
1个机器人的位置，求出满足覆盖质量要求下的所有可能感知质量j
sense
，选出最优感知质量所对应的第1个机器人的位置；保持第1个机器人在最优位置不动，假设第2个机器人移动，保持其他n
‑
2个机器人不动，求出所有可能的感知质量j
sense
，选出最优感知质量所对应的第2个机器人的位置；保持第1个机器人和第2个机器人在最优位置上不动，继续遍历剩余的n
‑
2个机器人，当遍历至第n个机器人时，求得该n个机器人组的最小感知质量。
17.有益效果：（1）本发明通过对动态目标的预测和最优感知质量的求解，可以实现多机器人对动态目标的实时跟踪；（2）本发明通过近似贪婪算法对动态目标进行一步预测，在任意时刻只需5n次计算，相较于传统的遍历搜索方法需要的5
n
次计算量，大幅度降低了计算量，提高了计算的速度；目标状态误差在置信区间内，保证了跟踪精度；（3）本发明通过近似贪婪算法，在任意时刻逐个规划每个机器人的最优控制策略可以实现实时规划，提高了机器人跟踪的实时性能。
附图说明
18.图1为本发明一个实施例的多机器人动态目标跟踪协同控制方法流程图；图2为本发明一个实施例的目标真实运动轨迹与机器人的初始分布图；图3为本发明一个实施例的目标真实轨迹与预测轨迹运行过程对比图；图4为本发明一个实施例的目标真实轨迹与预测轨迹对比图；图5为本发明一个实施例的x方向位置误差与置信区间图；图6为本发明一个实施例的x方向速度误差与置信区间图；图7为本发明一个实施例的y方向位置误差置信区间图；图8为本发明一个实施例的y方向速度误差与置信区间图；图9为本发明一个实施例的每一时刻覆盖质量数据图。
具体实施方式
19.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
20.本发明公开了一种新的多机器人动态目标跟踪协同控制方法，包括动态目标g和观测动态目标g的n个机器人，如图1所示，包括以下步骤：步骤1，建立动态目标的目标跟踪系统一般化模型,包括动态目标的运动学模型和目标跟踪系统的观测模型。判断动态目标是否在机器人的观测范围内，若判断为是则进入步骤2，否则转入步骤3。
21.动态目标的运动学模型采用的是ct模型即匀速转弯运动模型，描述为x（k 1）=ax（k） w（k），其中k为任意时刻，且k≥0，x为状态变量，，x和y表示笛卡尔坐标系中动态目标的位置，和表示动态目标在x方向和y方向上的速度；a为状态转移矩阵，w为系统噪声，假设系统噪声序列为均值为0、协方差矩阵为q的高斯白噪声，状态转移矩阵a为：，其中w表示转弯角速度，t为采样时间。
22.目标跟踪系统的观测模型为：每个机器人携带超声波传感器，该超声波传感器能够测量该机器人与目标之间距离，目标跟踪系统的测量模型为：z
i
（k）=h
i
（x(k)) v
i
（k），z
i
为机器人i的测量值，h
i
为动态目标与机器人i的相对距离: ，其中（x
i
,y
i
）为移动机器人i的位置，(x,y)为动态目标g的位置，v
i
为机器人自身的测量噪声，
假设测量噪声为均值为0，协方差矩阵为r的高斯白噪声；x为状态变量，下标i表示第i个机器人，i=1,2，
…
n。如图2所示，为目标真实运动轨迹与机器人的初始分布图。
23.步骤2，通过目标跟踪系统的观测模型测量各机器人与动态目标的距离，通过扩展卡尔曼滤波算法估计目标当前时刻的状态和误差协方差矩阵,使用加权平均的方法, 将n个机器人对目标的状态估计进行数据融合，作为当前时刻目标状态的最优估计，并对动态目标g进行一步预测，具体为：步骤2.1，利用扩展卡尔曼滤波算法更新目标当前时刻的状态x
i
(k|k)和误差协方差矩阵p
i
(k|k)，具体为：在任意时刻k的状态预测为；测量预测为；线性化观测方程，求解雅克比矩阵为：；协方差预测：，其中q为系统噪声w的协方差矩阵，a为状态转移矩阵；由协方差预测量p
i
、雅克比矩阵h
i
、高斯白噪声r求解卡尔曼增益k：；将卡尔曼增益代入求得状态变量x的状态更新：以及协方差更新：。
24.步骤2.2：采用数据融合中的加权平均方法，即将所有观测到目标的机器人获取的估计值赋予权重w
i
，并进行数据融合，利用数据融合的信息对动态目标g进行一步预测。本方法对所有机器人的估计值采用相同权重，即对所有机器人的估计值取平均值作为当前时刻目标状态的最优估计。
25.步骤3，基于近似贪婪算法, 在满足覆盖质量的前提下求解最小感知质量。
26.步骤3.1，感知质量定义为融合后的误差协方差矩阵p的行列式，公式为：j
sense
=det(p)。
27.步骤3.2，在设计移动机器人网络调度算法时，需要综合考虑覆盖质量与感知质量指标，在满足覆盖质量的条件下，优化感知质量，规划机器人的移动路径。覆盖质量的定义为，a
cov
为至少被一个机器人覆盖的区域面积，覆盖的区域范围与移动机器人的位置有关；如图9所示，为每一时刻覆盖质量数据图，a
foi
代表监控范围的总面积。
28.步骤3.3，假设机器人i的探测范围为r
i
，定义判断函数i
i
：，其中，为监控区域内的一点与机器人i的相对距离，结果为1则表明机器人i可以观测到点（x,y），反之，结果为0。
29.因此，基于当前所有机器人的位置，当前状态下的覆盖质量j
cov
可以表示为：
，其中i=1,2,...n表示第i个机器人， a
foi
为监控范围的总面积。
30.步骤3.4，根据近似贪婪算法，在满足覆盖质量为0.7的约束下，求出预测的下一时刻目标感知质量的最小值。具体包括：假设在任意时刻所有机器人均可以在上下左右四个方向以相同速度移动或者保持不动，因此在任意时刻，每个机器人的控制策略均有五种可能。本实施例选取机器人数量为9个，根据传统的遍历搜索方法，在任意时刻，9个机器人的控制策略有59，约195万种组合，计算量巨大，不能够实现实时规划。因此采用近似方法，在任意时刻逐个规划每个机器人的最优控制策略，具体为首先固定其余8个机器人的位置，假设第一个机器人进行移动，求出满足覆盖质量要求下的所有可能的感知质量j
sense
，选出最优感知质量所对应的第1个机器人的位置；这时第一个机器人在最优位置上保持不动，假设第2个机器人进行移动，并保持其他7个机器人不动，求出每种可能的感知质量，选出最优感知质量所对应的第2个机器人的位置；保持第1个机器人和第2个机器人在最优位置上不动，继续遍历剩余的7个机器人，最后求得最小感知质量。通过此方法，在任意时刻，共需5
×
9，即45次计算，大幅度降低了计算量。
31.步骤4，根据步骤3求解所得的最小感知质量，规划下一时刻最优的机器人位置，移动机器人实现多机器人对动态目标g的协同跟踪。
32.上述步骤为迭代运算步骤，每个时刻迭代一次。
33.本发明中的实施例具体实施过程如下：使用matlab 2019b开展仿真实验，其中监控范围是120米*120米的正方形区域，初始时刻均匀分布了9个移动机器人，每个机器人的观测范围是以20米为半径的圆形观测区域，移动速度为1m/s。动态目标的初始状态为初始位置（90，0）初始速度（120m/s，230m/s），并以w=5rad/s的速度进行匀速转弯运动。总观测时间为50s，采样时间为1s，最低覆盖质量要求为0.7。实验流程如下：步骤1，建立目标跟踪系统的一般化模型。步骤2，在当前时刻，根据能够探测到动态目标的所有机器人得到的测量数据，对目标当前状态，即目标的位置和速度进行估计，并对目标下一时刻的位置、速度进行预测。步骤3，根据对目标下一时刻的预测，顺次选择每个机器人最优的移动策略，使得下一时刻多机器人能够在满足覆盖质量为0.7的约束条件下，对目标进行最优状态估计。步骤4，根据步骤3的结果移动多机器人，并返回步骤1。
34.图2表示目标真实轨迹以及机器人初始分布，可以看出，在初始时刻只有1个机器人可以观测到移动目标。图3表示动态目标跟踪一半时间时目标真实轨迹与预测轨迹对比图，图4表示目标真实轨迹与预测轨迹在整个跟踪过程的对比图。可以看出目标真实轨迹与预测轨迹高度重合，验证了本专利的可实施性以及高吻合度。同时可以看出，由于有覆盖质量的要求，因此不会出现所有机器人均集中于目标附近的情况，所以可以通过调节覆盖质量参数，满足不同任务需求。图5
‑
图8表示对目标状态估计的误差。定义四个状态变量误差e
x
、e
vx
、e
y
、e
vy 分别表示状态变量在x、y方向上的位置误差、以及在x、y方向上的速度误差，，其中x
ekf
、x分别表示目标在x方向上位置的估计值和真实值；
，其中、分别表示表示目标在x方向上速度的估计值和真实值；，其中y
ekf
、y分别表示目标在y方向上位置的估计值和真实值；，其中、y分别表示目标在y方向上速度的估计值和真实值。通过四个状态变量误差表示目标实际状态与估计状态之间的误差置信区间是不考虑偏移时的情形。由图5
‑
图8可以看出四个状态变量误差均在置信区间范围内，验证了采用此方法可以对动态目标进行精确跟踪。
35.最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。