一种基于轨迹预测的编队围捕方法

1.本发明属于机器人协同控制技术领域，具体涉及一种基于轨迹预测的编队围捕方法。


背景技术：

2.随着科技技术的不断发展，机器人的应用领域不断扩大，对机器人需要完成的任务的复杂性要求也越来越高。在面对一些复杂的任务时，单个机器人无法满足工作需求，需要多个机器人协同合作来完成任务。多机器人编队控制是多机器人研究领域一类常见的复杂协作问题，采用多个机器人组成编队具有较多优点，例如在军事侦察、搜索、排雷等应用中能够获取更多的环境信息，在飞行器、卫星编队飞行等应用中能够完成更多、更复杂的任务。
3.在多机器人编队控制的基础上，完成编队下的围捕任务具有重要的应用价值，围捕任务要求多个机器人完成对环境中的目标进行跟踪并以特定队形实现围捕，这是通过多个机器人运动至各自期望的围捕点加以实现的。在实际围捕任务中，由于环境中的干扰、机器人传感器的局限性，对目标的定位可能不精准，甚至由于视野丢失问题导致目标信息可能短时间内无法获取，这种情况极大的影响了围捕任务。


技术实现要素：

4.为了解决实际围捕中目标信息定位可能不精准以及视野丢失问题，本发明提出了一种基于轨迹预测的编队围捕方法，提高了围捕精度和围捕方法的鲁棒性。
5.本发明为了实现上述目的，提出技术方案如下：
6.一种基于轨迹预测的编队围捕方法，包括以下步骤：
7.步骤1：为解决传感器视野丢失问题，对机器人运动状态进行估计与预测，过程如下：
8.1.1通过先验知识建立目标运动轨迹的拟合函数，如式(1)所示
9.fk(t；ck)＝c0φ0(t)+c1φ1(t)+
…
+c
γ
φ
γ
(t)
ꢀꢀꢀ
(1)
10.其中，fk(t；ck)为k时刻轨迹拟合函数，{φi(t)}
i＝0,1,
…
,γ
为先验函数，根据先验知识设置为单项式或者三角函数，ck＝{ci}
i＝0,1,
…
γ
为k时刻待确定的参数，γ为拟合函数的阶数，根据实际需要选定；
11.1.2初始化{φi(t)}
i＝0,1,
…
,γ
，ck；
12.1.3机器人传感器对环境进行感知，寻找目标，若感知到目标时，其量测信息为yk，则存储k时刻量测数据yk＝yk；
13.1.4设置合适的滑动时间窗口[k-l,k]，其窗口长度为l，定义时间窗口内拟合残差函数如式(2)所示
[0014]
[0015]
并通过求解最优化问题，最小化拟合残差求解轨迹时间函数的参数估计如式(3)所示
[0016][0017]
1.5通过时刻k求解的参数估计获得最优轨迹时间函数通过fk(t)计算目标在k时刻的位置估计fk(k)与下一时刻的位置预测fk(k+1)；
[0018]
1.6令k
→
k+1，循环执行上述步骤1.3、1.4、1.5；当执行至步骤1.3时，先判断传感器是否感知到目标，若感知到目标则继续执行；若未感知到目标则执行步骤1.7；
[0019]
1.7通过上一时刻的最优轨迹时间函数f
k-1
(t)计算当前时刻的目标位置预测f
k-1
(k)，存储k时刻量测数据yk＝f
k-1
(k)，返回执行步骤1.4；
[0020]
步骤2：建立多机器人编队围捕模型，过程如下：
[0021]
2.1建立单个机器人动力学模型，如式(4)所示
[0022][0023]
其中，为k时刻机器人的位置坐标，θk为k时刻机器人的角度，vk为机器人的线速度，wk为机器人的角速度，t0为系统的时间周期；
[0024]
2.2建立多机器人系统模型，如式(5)所示
[0025]
x
i,k+1
＝x
i,k
+qu
i,k
ꢀꢀꢀ
(5)
[0026]
其中，i为机器人的编号，x
i,k
∈r3×1为k时刻机器人i的坐标与角度，u
i,k
∈r2×1为k时机器人i的控制量输入，q∈r3×2为控制量的传递函数；
[0027]
步骤3：计算多机器人系统的控制量，对目标进行编队围捕，过程如下：
[0028]
3.1初始化编队围捕方法，如选择方法1利用当前时刻目标信息进行围捕则执行步骤3.2；如选择方法2利用下一时刻目标信息进行围捕则执行步骤3.3；
[0029]
3.2选取目标在k时刻的位置估计fk(k)，作为多机器人系统的围捕目标位置，qk＝fk(k)；
[0030]
3.3选取目标下一时刻的位置预测fk(k+1)，作为多机器人系统的围捕目标位置，qk＝fk(k+1)；
[0031]
3.4计算多机器人自身的期望围捕点，以环形队形为例，如式(6)所示
[0032][0033]
其中，为机器人的期望围捕点坐标，r为圆环队形的半径，n为编队中移动机器人的数量；
[0034]
3.5计算k时刻机器人与期望围捕点的距离和角度，如式(7)所示
[0035][0036]
其中，dk为k时刻机器人与期望围捕点的距离，为k时刻机器人与期望围捕点的角度；
[0037]
3.6计算k时刻机器人的控制量，如式(8)所示
[0038][0039]
其中，λ∈r2×1为控制参数。
[0040]
首先机器人对目标位置进行感知，执行步骤1，利用最小化拟合残差算法迭代求得当前时刻特定长度时间窗口内目标运动的轨迹时间函数，通过轨迹拟合计算目标位置的状态估计，在视野丢失时利用上一时刻的轨迹时间函数计算当前时刻目标位置的状态预测值作为补偿。然后执行步骤3进行编队围捕，选择围捕方法计算多机器人围捕目标位置，根本围捕目标位置计算机器人自身的期望围捕坐标点，最后根据期望围捕坐标点计算机器人的控制量，完成编队控制。
[0041]
本发明的有益效果主要体现在：本发明方法在机器人获取目标状态时，运用轨迹拟合的方法将目标运动状态拟合成一个轨迹时间函数，当目标视野丢失时，通过轨迹时间函数计算预测值进行补偿，提高了定位精度，减少了目标视野丢失带来的影响。在进行编队围捕时，根据围捕方法计算多机器人各自的控制量，完成编队控制，提高了围捕跟踪的效率。
附图说明
[0042]
图1是本发明的机器人动力学模型的示意图。
[0043]
图2是本发明的算法的流程图。
[0044]
图3是本发明的移动机器人编队围捕效果图。
[0045]
图4是本发明的估计围捕误差示意图。
[0046]
图5是本发明的预测围捕误差示意图。
具体实施方式
[0047]
下面结合附图对本发明做进一步说明。
[0048]
参照图1～图4，一种基于轨迹预测的编队围捕方法，所述方法包括以下步骤：
[0049]
一种基于轨迹预测的编队围捕方法，包括以下步骤：
[0050]
步骤1：为解决传感器视野丢失问题，对机器人运动状态进行估计与预测，过程如下：
[0051]
1.1通过先验知识建立目标运动轨迹的拟合函数，如式(1)所示
[0052]fk
(t；ck)＝c0φ0(t)+c1φ1(t)+
…
+c
γ
φ
γ
(t)
ꢀꢀꢀ
(1)
[0053]
其中，fk(t；ck)为k时刻轨迹拟合函数，{φi(t)}
i＝0,1,
…
,γ
为先验函数，根据先验知
识设置为单项式或者三角函数，ck＝{ci}
i＝0,1,
…
γ
为k时刻待确定的参数，γ为拟合函数的阶数，根据实际需要选定；
[0054]
1.2初始化{φi(t)}
i＝0,1,
…
,γ
，ck；
[0055]
1.3机器人传感器对环境进行感知，寻找目标，若感知到目标时，其量测信息为yk，则存储k时刻量测数据yk＝yk；
[0056]
1.4设置合适的滑动时间窗口[k-l,k]，其窗口长度为l，定义时间窗口内拟合残差函数如式(2)所示
[0057][0058]
并通过求解最优化问题，最小化拟合残差求解轨迹时间函数的参数估计如式(3)所示
[0059][0060]
1.5通过时刻k求解的参数估计获得最优轨迹时间函数通过fk(t)计算目标在k时刻的位置估计fk(k)与下一时刻的位置预测fk(k+1)；
[0061]
1.6令k
→
k+1，循环执行上述步骤1.3、1.4、1.5；当执行至步骤1.3时，先判断传感器是否感知到目标，若感知到目标则继续执行；若未感知到目标则执行步骤1.7；
[0062]
1.7通过上一时刻的最优轨迹时间函数f
k-1
(t)计算当前时刻的目标位置预测f
k-1
(k)，存储k时刻量测数据yk＝f
k-1
(k)，返回执行步骤1.4；
[0063]
步骤2：建立多机器人编队围捕模型，过程如下：
[0064]
2.1建立单个机器人动力学模型，如式(4)所示
[0065][0066]
其中，为k时刻机器人的位置坐标，θk为k时刻机器人的角度，vk为机器人的线速度，wk为机器人的角速度，t0为系统的时间周期；
[0067]
2.2建立多机器人系统模型，如式(5)所示
[0068]
x
i,k+1
＝x
i,k
+qu
i,k
ꢀꢀꢀ
(5)
[0069]
其中，i为机器人的编号，x
i,k
∈r3×1为k时刻机器人i的坐标与角度，u
i,k
∈r2×1为k时机器人i的控制量输入，q∈r3×2为控制量的传递函数；
[0070]
步骤3：计算多机器人系统的控制量，对目标进行编队围捕，过程如下：
[0071]
3.1初始化编队围捕方法，如选择方法1利用当前时刻目标信息进行围捕则执行步骤3.2；如选择方法2利用下一时刻目标信息进行围捕则执行步骤3.3；
[0072]
3.2选取目标在k时刻的位置估计fk(k)，作为多机器人系统的围捕目标位置，qk＝fk(k)；
[0073]
3.3选取目标下一时刻的位置预测fk(k+1)，作为多机器人系统的围捕目标位置，qk＝fk(k+1)；
[0074]
3.4计算多机器人自身的期望围捕点，以环形队形为例，如式(6)所示
[0075][0076]
其中，为机器人的期望围捕点坐标，r为圆环队形的半径，n为编队中移动机器人的数量；
[0077]
3.5计算k时刻机器人与期望围捕点的距离和角度，如式(7)所示
[0078][0079]
其中，dk为k时刻机器人与期望围捕点的距离，为k时刻机器人与期望围捕点的角度；
[0080]
3.6计算k时刻机器人的控制量，如式(8)所示
[0081][0082]
其中，λ∈r2×1为控制参数。
[0083]
首先机器人对目标位置进行感知，执行步骤1，利用最小化拟合残差算法迭代求得当前时刻特定长度时间窗口内目标运动的轨迹时间函数，通过轨迹拟合计算目标位置的状态估计，在视野丢失时利用上一时刻的轨迹时间函数计算当前时刻目标位置的状态预测值作为补偿。然后执行步骤3进行编队围捕，选择围捕方法计算多机器人围捕目标位置，根本围捕目标位置计算机器人自身的期望围捕坐标点，最后根据期望围捕坐标点计算机器人的控制量，完成编队控制。
[0084]
为验证本发明所设计方法的有效性，采用以下实例来进行验证。
[0085]
如图1所示，移动机器人的运动模型如式(9)所示：
[0086][0087]
在本实验中，目标机器人的初始坐标与角度设置为线速度v(k)＝0.2,角速度w(k)＝0.12，时间周期t0＝0.1。
[0088]
进行编队围捕时采用3台围捕机器人，即n＝3，初始位置坐标分别设置为(-5,-2)、(-5,-6)、(-5,-1)，初始位姿角设置为0。设置编队围捕的队形半径r＝3m，设置控制系数得到移动机器人运动轨迹如图4所示。
[0089]
为了方便误差效果的分析，验证预测算法的有效性，将环形编队中心点与目标点的距离作为误差，定义系统误差如式(10)所示：
[0090][0091]
结果如图3～5所示，图3为多机器人的编队围捕效果图，图4为运用目标估计的围捕误差示意图，图5为运用目标预测的围捕误差示意图，可以看到本发明给出的方法具有良好的编队围捕效果。
[0092]
以上阐述的是本发明给出的一个实施例表现出的优良优化效果，显然本发明不只是限于上述实施例，在不偏离本发明基本精神及不超出本发明实质内容所涉及范围的前提下对其可作种种变形加以实施。