一种定位传感器节点的移动机器人滚动时序控制方法与流程

本发明属于自动化技术领域，涉及一种定位传感器节点的移动机器人滚动时序控制方法。

背景技术

无线传感器网络是由布置在监测区域内大量的传感器节点组成，通过无线通信方式形成的一个多跳的自组织网络系统，其目的是协作地感知、采集和处理网络覆盖区域中对象的信息。它已被应用于许多领域，如国防领域、农业领域和医学领域等。然而，从另一方面来讲，如何找到并定位无线传感器网络中的传感器节点，也具有非常现实的意义。例如：找到敌军用来侦察我方部队行动情况的传感器网络，并摧毁传感器节点，保护我方部队等等。通常的情况，可以采用概率的方法构建传感器节点的分布地图，并控制机器人依次访问概率最大的区域，找到传感器节点。然而，实际的搜索环境是非常大的，并且了充满障碍物。此外，传感器信号的传播容易受扰动影响，从而使概率地图不准确。在此背景下，本发明弥补了现有技术的不足。

技术实现要素：

本发明的目标是针对现有技术的不足之处，提出一种定位传感器节点的移动机器人滚动时序控制方法。本发明首先根据传感器节点的信号强度模型，开发了基于粒子滤波原理的传感器节点位置评估方法，通过该方法能够有效处理噪声影响并预估传感器节点的位置；然后将定位传感器节点的任务采用线性时序逻辑方法表示，并转换成机器人运动轨迹的约束，结合机器人的避障要求和传感器节点的预估位置，建立机器人的滚动优化时序控制问题，求解该问题，产生机器人实际最优的控制序列，并采用实际最优控制序列中第一个控制输入到机器人中，控制机器人的运动。本发明弥补了传统控制的不足，设计的滚动时序控制方法，能控制机器人有序地搜索多个环境区域，并定位每一个环境区域中的传感器节点，同时在搜索过程中能够有效的避免与障碍物相撞。

本发明方法的步骤包括：

第一步：预测传感器节点可能的位置，具体步骤如下：

a)在第k时刻，假设第m(m＝1，2，...，n)个粒子是传感器节点，则机器人在当前位置上评估的信号强度zm(k)，如(1)式所示。

其中：zm(k)是机器人在第k时刻，接收到的第m个粒子发射的信号强度；xm(k)是第m个粒子在第k时刻的位置；p(k)是机器人在第k时刻的位置；f(·)是传感器节点的信号强度模型；rand是[0,1]间均匀分布的随机数；r是观测噪声的方差；n是粒子的个数。

b)根据(1)式，计算第m个粒子的权重。

其中：wm是第m个粒子的权重；z(k)是机器人在第k时刻实际接收到的信号强度；exp(·)是指数函数。

c)归一化权重，如(3)式所示。

d)对于n个粒子，根据归一化权重w′m(m＝1，2，...，n)，并采用轮盘赌方法重新产生n个粒子(此时第m个粒子的位置用x′m(k)表示)。

e)根据新的粒子群体，计算传感器节点可能的位置，如(4)所示。

其中：of(k)是机器人在第k时刻评估的传感器节点位置。

第二步：基于传感器节点可能的位置和避障要求，建立滚动优化控制问题，具体步骤如下：

a)建立滚动优化控制模型，如(5)式所示：

其中：n是预测长度；m是障碍物的个数；α，β，γ，d是四个参数；u(k)是k时刻的控制序列：u(0|k)，u(1|k)，...，u(n-1|k)；min表示取最小值；s.t.表示约束条件；p(l|k)是机器人在k时刻预测的在第k+l时刻的位置；是机器人在k时刻预测的在第k+l时刻，第j个障碍物智能体的位置；q(l|k)是机器人在k时刻预测的在第k+l时刻的速度；是机器人在k时刻预测的在第k+l时刻，第j个障碍物智能体的速度；u(l|k)是机器人在k时刻需要求得的在第k+l时刻的控制输入；||·||2表示2泛数；h(·)和g(·)是机器人运动学模型；是允许控制集。

b)建立障碍物智能体的运动模型：

其中：是投影矩阵；i是单位矩阵；ak是过点sk且垂直于障碍物边缘的单位向量；sk是障碍物边缘上的任意点；t表示矩阵转置。

第三步：建立滚动时序控制器，产生最优实际运动轨迹，具体步骤如下：

a)根据机器人的运动学，划分搜索环境为多个方格，方格的尺寸可以根据实际需要确定，每个方格用表示，是一个有限的整数集合是一个序号。定义一个映射o，使得o(p(k))＝λk。

b)基于搜索环境划分的方格和机器人的运动学，建立有限状态确定转移系统：

其中：λ0＝o(p(0))是初始状态；是一个转移集合；是一个权重函数；π是一个观测集合；是一个观测映射；2^π是观测集合的幂集。对于转移系统的一个有限状态序列是一个序号。定义一个距离函数

c)我们用线性时序逻辑表达式φ来表达搜索传感器节点任务中时序和逻辑部分，根据φ，结合有限状态确定转移系统可以设定它是可接受状态集合。对于每一个状态找到一个最短的状态转移路径λ，...，λδ。定义e(λ，λδ)：e(λ，λδ)＝mindist(λ，λδ)。定义传感器节点位置集合δa1，δa2，δa3，…，δah，其中ah是环境区域划分的序号，每一个传感器节点预估的位置放到相应的集合中。

d)在任意时刻k＞0，对于一个位置序列p(1|k)，...，p(n|k)，对应于状态转移系统中的一个状态转移序列λ1|k，λ2|k，...，λn|k，需要说明的是，对于每一个j∈{1，...，n}，存在一个l∈{1，...，n}，使得o(p(l|k))＝λj|k。根据不同的条件要求，采用下列不同的控制器，首先考虑传感器节点位置集合δa1：

①并且l∈{1，...，n}，并且(8)式产生位置序列满足

②如果并且，存在一个最小j∈{1，...，n}，有并且(9)式产生位置序列满足那么：

③如果表明of(k)是δa1中已经到达的有效位置，从δa1取出还未到达过的有效节点的位置o′f(k)，如并且(10)式产生位置序列满足那么：

其中：u^*(k)是最优控制序列u^*(0|k)，...，u^*(n-1|k)。如果δa1中仍存在没有到达的位置，则取出该位置代替of(k)。如果θa1中的所有有效节点都到达过后，将θa1依次替换为δa2，δa3，…，θah。

第四步：将实际机器人在第k时刻获得的最优的控制序列u^*(0|k)，...，u^*(n-1|k)第一个最优控制信号u^*(0|k)施加给实际机器人。

第五步：如果终止条件满足，例如：给定的最大搜索时间已经达到，则机器人停止运行，并将最终传感器节点位置输出；如果终止条件没有满足，则返回第一步继续执行。

本发明提出的一种定位传感器节点的移动机器人滚动时序控制方法，能控制机器人有序地搜索多个环境区域，并定位每一个环境区域中的传感器节点，同时在搜索过程中能够有效的避免与障碍物相撞，弥补了传统方法的不足。

具体实施方式

以野外搜索无限传感器网络中传感器节点为例。搜索环境长为200米，宽为200米，建立坐标系统，可表示成为[-100，100]×[-100，100]。采用一种定位传感器节点的移动机器人滚动时序控制方法，控制机器人最终定位传感器节点。

具体实施步骤如下：

第一步：初始化机器人的参数，包括：机器人的初始位置，机器人的初始速度设为0，最大速度限制则根据实际使用的机器人类型设定。

第二步：预测传感器节点可能的位置，具体步骤如下：

a)在第k时刻，假设第m(m＝1，2，...，n)个粒子是传感器节点，则机器人在当前位置上评估的信号强度zm(k)，如(1)式所示。

其中：zm(k)是机器人在第k时刻，接收到的第m个粒子发射的信号强度；xm(k)是第m个粒子在第k时刻的位置；p(k)是机器人在第k时刻的位置；f(·)是传感器节点的信号强度模型；rand是[0,1]间均匀分布的随机数；r是观测噪声的方差，在实施中r＝8；n是粒子的个数，n＝10000。

此外，传感器节点的信号强度模型f(xm(k)，p(k))是：

f(xm(k)，p(k))＝10log(0.001)-1.96log(||xm(k)-p(k)||)(2)

b)根据(1)式，计算第m个粒子的权重。

其中：wm是第m个粒子的权重；z(k)是机器人在第k时刻实际接收到的信号强度；exp(·)是指数函数。

c)归一化权重，如(4)式所示。

d)对于n个粒子，根据归一化权重w′m(m＝1，2，...，n))，并采用轮盘赌方法重新产生n个粒子(此时第m个粒子的位置用x′m(k)表示)。

e)根据新的粒子群体，计算传感器节点可能的位置，如(5)所示。

其中：of(k)是机器人在第k时刻评估的传感器节点位置。

第三步：基于传感器可能位置和避障要求，建立滚动优化控制问题，具体步骤如下：

a)建立滚动优化控制模型，如(6)式所示：

其中：n是预测长度，n＝6；m是障碍物的个数，根据传感器检测的数量确定；α＝2，β＝1，γ＝3，d＝0.3是四个参数；u(k)是k时刻的控制序列：u(0|k)，u(1|k)，...，u(n-1|k)；min表示取最小值；s.t.表示约束条件；p(l|k)是机器人在k时刻预测的在第k+l时刻的位置；是机器人在k时刻预测的在第k+l时刻，第j个障碍物智能体的位置；q(l|k)是机器人在k时刻预测的在第k+l时刻的速度；是机器人在k时刻预测的在第k+l时刻，第j个障碍物智能体的速度；u(l|k)是机器人在k时刻需要求得的在第k+l时刻的控制输入；||·||2表示2泛数；是允许控制集，即-2＜u(l|k)＜2，单位是米/秒。

b)建立障碍物智能体的运动模型：

其中：是投影矩阵；i是单位矩阵；ak是过点sk且垂直于障碍物边缘的单位向量；sk是障碍物边缘上的任意点；t表示矩阵转置。

第四步：建立滚动时序控制器，产生最优实际运动轨迹，具体步骤如下：

a)根据机器人的运动学，划分搜索环境为多个方格，方格的尺寸为5×5(单位：米)，每个方格用表示，是一个有限的整数集合{1，2，...，1600}，定义一个映射o，使得o(p(k))＝λk。将整个搜索环境分成4个区域，分别是a区[-100，0]×[0，100]，b区[0，100]×[0，100]，c区[0，100]×[-100，0]，d区[-100，0]×[-100，0]。

b)基于搜索环境划分的方格和机器人的运动学，建立有限状态确定转移系统：

其中：λ0＝o(p(0))是初始状态；是一个转移集合；是一个权重函数，如果λ1和λ2存在一个直接的转移路径，则否则π是一个观测集合；是一个观测映射；2^π是观测集合的幂集。对于转移系统的一个有限状态序列是一个序号。定义一个距离函数

c)我们用线性时序逻辑表达式φ来表达搜索传感器节点任务中时序和逻辑部分：

其中：，□是时序逻辑符号，表示“永远”；是时序逻辑符号，表示“蕴含”；u是时序逻辑符号，表示“直到”。根据φ，结合有限状态确定转移系统可以设定它是可接受状态集合。对于每一个状态找到一个最短的状态转移路径λ，...，λδ。定义e(λ，λδ)：e(λ，λδ)＝mindist(λ，λδ)。定义传感器节点位置集合δa，δb，δc，δd，每一个传感器节点预估的位置放到相应的集合中。

d)在任意时刻k＞0，对于一个位置序列p(1|k)，...，p(n|k)，对应于状态转移系统中的一个状态转移序列λ1|k，λ2|k，...，λn|k，需要说明的是，对于每一个j∈{1，...，n}，存在一个l∈{1，...，n}，使得o(p(l|k))＝λj|k。根据不同的条件要求，采用下列不同的控制器：

①并且l∈{1，...，n}，并且(10)式产生位置序列满足

②如果并且，存在一个最小j∈{1，...，n}，有并且(11)式产生位置序列满足那么：

③如果表明of(k)是δa中已经到达的有效位置，从δa取出未到达过的有效节点的位置o′f(k)，并且(12)式产生位置序列满足那么：

其中：u^*(k)是最优控制序列u^*(0|k)，...，u^*(n-1|k)；如果δa中仍存在没有到达的位置，则取出该位置代替of(k)。当δa中的所有有效节点都到达过后，将δa依次替换为δb，δc，δd，

第五步：将实际机器人在第k时刻获得的最优的控制序列u^*(0|k)，...，u^*(n-1|k)第一个最优控制信号u^*(0|k)施加给实际机器人。

第六步：如果终止条件满足，例如：给定的最大搜索时间已经达到，则机器人停止运行，并将最终传感器节点位置输出；如果终止条件没有满足，则返回第二步继续执行。