CN103984343A - 一种多无人移动平台网络化协同控制方法 - Google Patents

Abstract

本发明提供一种多无人移动平台网络化协同控制方法，针对单个机器人不能完成的作业任务，建立多机器人试验平台，进行编队、避障等基本行为的实验。第一步：采用里程计的定位方式搭建基于WIFI的UDP模式进行协调通信,实时掌握车位置与姿态；第二步：获取的位置和姿态建立差动独轮小车运动模型；第三步：分别将车与目标点看作相互吸引的正负电荷，应用之间产生的电场力作用于智能车，完成固定队形编队；第四步：编队过程中，通过第一步中相互通信过来的位置信息,判断是否将要发生碰撞,对于有碰撞可能的车,引入使它们相斥的势场来产生相互之间的斥力,避免车与车之间的碰撞；第五步:根据获取的全局信息和对小车目前位置的计算,使用相斥的势场力,来避免车与障碍物之间的碰撞。

Description

一种多无人移动平台网络化协同控制方法

技术领域

本发明属于协同控制技术领域，涉及一种多无人移动平台网络化协同控制方法。

背景技术

多无人移动平台是由多个移动平台组成的集合，平台之间以及平台与环境之间通过通讯、协商与协作来共同完成单个平台所不能解决的问题.多个平台的使用比单个平台有更多的优点。多个平台相互合作能够完成超出它们各自能力范围的任务，使得多无人移动平台的整体能力大于个体能力之和。总体上说，多无人移动平台具有更广泛的任务领域、更高的效率、改良的***性能、错误容忍、鲁棒性、分布式的感知与作用、内在的并行性、对社会和生命科学的观察等显著特性。

多无人移动平台的网络化协同控制是当前多无人移动平台的一个重要发展方向。随着机器人应用范围的扩大，对其能力的要求越来越高，多个机器人协作可以完成单个机器人无法完成的复杂任务。编队控制是一个具有典型和通用性的多机器人协调问题，同时也是多机器人协调问题的基础，对多机器人协作***的研究有巨大的推动和促进作用。如果编队过程中存在障碍，则可能会导致智能多机器人碰撞，损失会很严重，也不能很好的完成期望的动作或达到预定的编队效果，所以相关的避障策略也是在编队过程中需要考虑的重要问题。

发明内容

本发明针对单个机器人不能完成的作业任务，使用多机器人***代替单个机器人执行复杂任务的需求，建立多机器人试验平台，进行编队、避障等基本行为的实验，以满足复杂环境下的任务需求。

本发明的技术方案如下：

一种多无人移动平台网络化协同控制方法，包括以下步骤：

第一步：采用里程计的定位方式搭建基于WIFI的UDP模式进行协调通信,实时掌握车位置与姿态；

第二步：根据第一步中获取的位置和姿态建立差动独轮小车运动模型；

第三步：根据第二步中小车的运动模型,使用人工势能场方法进行编队，分别将车与目标点看作相互吸引的正负电荷，应用之间产生的电场力作用于智能车，完成固定队形编队；

第四步：编队过程中，通过第一步中相互通信过来的位置信息,判断是否将要发生碰撞,对于有碰撞可能的车,引入使它们相斥的势场来产生相互之间的斥力,避免车与车之间的碰撞；

第五步:根据获取的全局信息和对小车目前位置的计算,使用相斥的势场力,来避免车与障碍物之间的碰撞。

本发明的有益效果：

本发明实验结果误差范围控制在0.2mm以内，成功完成了机器人之间的互相避碰，并达到指定位置。

附图说明

图1机器人转向分析坐标图；

图2坐标系中的位置坐标图；

图3实施例中基于MPLAB的环境对机器人编程流程图；

图4UDP广播模式建立通信网络原理图；

图5一号车的初始状态示意图；

图6一号车躲避二号车的状态示意图；

图7一号车躲避零号车的状态示意图；

图8一号车躲过所有障碍以后继续行驶示意图；

图9所有车到达指定的目标位置示意图。

具体实施方式

下面通过附图和实例对本发明的技术方案做进一步的详细描述。

1.里程计定位***的描述

本实施例用odometry定位方式。这种定位方式是基于光电编码器的准确数据，随着时间的推移整合位置的变化的定位方式。假设机器人按照图1运动，其中r_k为运动弧线半径，Δθ_k为转角，b为机器人轴间距，ΔU_l,k和ΔU_r,k为机器人左右轮运动轨迹，c为机器人中心，Δρ_k为机器人运动直线距离：

r_kΔθ_k＝ΔU_l，k

(r_k+b)Δθ_k＝ΔU_r，k

可以得到：

${\mathrm{\Δ\θ}}_k=\frac{{\mathrm{\ΔU}}_{r,k}-{\mathrm{\ΔU}}_{l,k}}{b}$

而

${\mathrm{\Δ\ρ}}_k=\frac{{\mathrm{\ΔU}}_{r,k}+{\mathrm{\ΔU}}_{l,k}}{2}$

在k-1时刻，机器人的位置为：

x_k-1＝[x_k-1y_k-1θ_k-1]^T

其中，x_k-1，y_k-1为笛卡尔坐标系，θ_k-1为参考全局坐标系的方向角。

按照图2，按照旋转角Δθ_k和变化距离Δρ_k使得机器人运动到一个新的位置x_k，

$x_k=\left[\begin{array}{c}{x}_{k-1}+{\mathrm{\Δ\ρ}}_k\cos \left({\mathrm{\α}}_k\right)\\ y_{k-1}+{\mathrm{\Δ\ρ}}_k\sin \left({\mathrm{\α}}_k\right)\\ {\mathrm{\α}}_k+{\mathrm{\δ}}_k\end{array}\right]$

在图1和图2中，我们将机器人中心的弧线运动近似为弦，将这些公式带入上式，可以得到更新坐标方程：

$x_k=x_{k-1}+\left[\begin{array}{c}{\mathrm{\Δ\ρ}}_k\cos ({\mathrm{\θ}}_{k-1}+\frac{{\mathrm{\Δ\θ}}_k}{2})\\ {\mathrm{\Δ\ρ}}_k\sin ({\mathrm{\θ}}_{k-1}+\frac{{\mathrm{\Δ\θ}}_k}{2})\\ {\mathrm{\Δ\θ}}_k\end{array}\right]$

基于以上原理，编写了定位程序，建立了多无人移动平台的定位***。

2.基于WIFI的通讯***的描述：

本实施例中wifi模块采用海凌科(香港)有限公司推出的HLK-WIFI-M03模块。Uart-Wifi是基于Uart接口的符合wifi无线网络标准的嵌入式模块，内置无线网络协议IEEE802.11协议栈以及TCP/IP协议栈，能够实现用户串口数据到无线网络之间的转换。多智能车***采用UDP广播模式建立通信网络，原理图如图3：

各个模块的具体配置：

网络设置：

IP地址：192.168.0.Y

子网掩码：255.255.255.0

网关地址：192.168.0.1

DNS地址：192.168.0.1

工作模式设置：

启用自动工作模式

协议类型：UDP

C/S模式：广播

端口号：6000

连接超时：0

配置好后重启模块，模块将自动连接网络，每个智能车发送的数据，将以广播形式发送给其它所有智能车，实现交互通讯。

3.基于人工势能场方法的编队和避障实验：

在物理学里面，我们把一定空间内物理量之间的影响称作场，包括力场和电场等。同时，我们用位姿信息作为这些量的物理属性，比如引力场中，我们用两个物体之间的距离计算引力的大小。人工势能场与之前的描述相同，我们将机器人所处的环境作为一个人工抽象出来的力场，这中间包括了机器人自身的场和障碍物、目标点的势能场。目标点对机器人产生吸引力，作为引力场，而障碍物对机器人产生排斥力，作为斥力场，计算引力和斥力的合力，然后控制机器人的行动。这样，机器人就可以由合力驱动来编队和避障了。

本发明提出一系列基于电势能场概念的人工势能场函数，将机器人当作电场中的负电荷，将障碍物当作静电场中具有几何外形的均匀导体，将目标位置当作正电荷，目标位置对机器人产生吸引力，然后将整个***当作一个电势能场来处理。其中的人工势能场函数主要有三种，机器人之间的势能场函数，机器人和障碍物之间的势能场函数，以及机器人和目标位置之间的势能场函数。

根据相关电场知识，我们将机器人之间的势场定义为：

$V_I=\left\{\begin{array}{c}{\mathrm{\α}}_I(\ln \left(\right|\left|p_{\mathrm{ij}}\right|\left|\right)+\frac{d_o}{\left|\right|p_{\mathrm{ij}}\left|\right|}),0\≤\left|\right|p_{\mathrm{ij}}\left|\right|\≤d_1\\ K\left|\right|p_{\mathrm{ij}}\left|\right|,\left|\right|p_{\mathrm{ij}}\left|\right|\≥d_1\end{array}\right.$

对应机器人之间的力为：

$F_i=\left\{\begin{array}{c}{\▿}_{p_{\mathrm{ij}}}{V}_I={\mathrm{\α}}_I(\frac{1}{p_{\mathrm{ij}}}-\frac{d_0}{{\left|\right|p_{\mathrm{ij}}\left|\right|}^2}),0\≤\left|\right|p_{\mathrm{ij}}\left|\right|\≤d_1\\ K,\left|\right|p_{\mathrm{ij}}\left|\right|\≤d_1\end{array}\right.$

其中V_I为机器人i和机器人j之间的势场，α_I和K为增益，P_ij为机器人i和机器人j之间的距离，d₀为机器人之间期望的距离，当||P_ij||≤d_I时，假定机器人之间的作用为固定的，显然可见，d₀≤d_l；

如果将障碍物看作一个质点，那么机器人与障碍物之间在电场中的影响可以表示为：

$V_o\left(r_{\mathrm{il}}\right)=k_o\frac{Q}{\left|\right|r_{\mathrm{il}}\left|\right|}$

其中V_o(r_il)表示机器人i与障碍物l之间的势场,k_o是一个正的参数，Q表示总电量，规定为r_il＝p_i-o_l，p_i表示机器人i的位置，o_l表示障碍物l的位置。根据电磁学原理，机器人和障碍物上这一点之间产生的力为：

$f_o\left(r_{\mathrm{il}}\right)={-\▿}_{r_{\mathrm{il}}}{V}_o\left(r_{\mathrm{il}}\right)$

f_o为V_o的梯度函数，根据上式可得：

$f_o\left(r_{\mathrm{il}}\right)=k_o\frac{Q}{{\left|\right|r_{\mathrm{il}}\left|\right|}^2}$

机器人的期望位置对智能体表现为引力。但是这种引力与电场中异性电荷之间的引力不同。当机器人与期望位置之间的距离越远时，我们希望引力越大，这样有利于机器人驶向目标位置。而电场中的异性电荷之间引力刚好相反，当距离越大时，电荷之间的力越小。为了方便，我们假设机器人与目标位置之间的力与它们之间的距离成正比。根据编队的要求，我们选择势场函数为：

$V_f=k_1\frac{d_i^2}{2}$

其中，d_i为机器人i的位置p_i与期望位置p_id之间的距离，k_l为正常数。那么机器人与目标位置之间的力为：

$F_f={\▿}_{p_i}{V}_f=k_1{d}_i$

实验采用上文中提到的模型和控制器，基于MPLAB的环境对机器人进行编程，软件流程图如图3。

根据以上流程，在修改了通讯的驱动程序以后，进行了实验。实验中，采用三个机器人为例，分别为零一二号车。零号车从(0；0)点位置出发，目标位置为(0.44；0.75)；一号车从(0.5；0)位置出发，目标位置为(0；1)；二号车从(1；0)位置出发，目标位置为(0；0.5)。其中，单个机器人的数据以一号车为例，数据采集如下。

图5为一号车的初始状态。左图为一号车的位置信息，包括x、y轴的坐标和朝向角，同时还显示了零号车和二号车的初始位置信息；右图为一号车的受力状况，其中，F为机器人收到的合力，F_id为各别的机器人对二号车产生的力，F_i为机器人之间势能场的合力，F_f为机器人受到目标的力。其中合力F与别的力呈线性关系。可以看到，一号车在初始位置时只受到目标位置对一号车的吸引力。

图6为一号车躲避二号车的状态。图中可以看到，三辆车互相发送位置信息，进行控制。而右图中则显示二号车进入一号车0.3m的避障范围内，使得一号车开始躲避二号车。

图7为一号车躲避零号车的状态。图中可以看到，三辆车继续互相发送位置信息，进行控制。而右图中则显示零号车进入一号车的避障范围内，使得一号车开始躲避零号车。

图8为一号车躲过所有障碍以后继续行驶。图中可以看到，三辆车继续互相发送位置信息，进行控制。而右图中则显示一号车已经躲过所有障碍，Fi为零。

图9为所有车到达指定的目标位置。从一号车的受力和坐标上来看，所有车都已经基本到达指定位置。

Claims (4)

1.一种多无人移动平台网络化协同控制方法，其特征在于，包括以下步骤：

第一步：采用里程计的定位方式搭建基于WIFI的UDP模式进行协调通信,实时掌握车位置与姿态；

第二步：根据第一步中获取的位置和姿态建立差动独轮小车运动模型；

第三步：根据第二步中小车的运动模型,使用人工势能场方法进行编队，分别将车与目标点看作相互吸引的正负电荷，应用之间产生的电场力作用于智能车，完成固定队形编队；

第四步：编队过程中，通过第一步中相互通信过来的位置信息,判断是否将要发生碰撞,对于有碰撞可能的车,引入使它们相斥的势场来产生相互之间的斥力,避免车与车之间的碰撞；

第五步:根据获取的全局信息和对小车目前位置的计算,使用相斥的势场力,来避免车与障碍物之间的碰撞。

2.如权利要求1所述的一种多无人移动平台网络化协同控制方法，其特征在于，其中所述的定位方式是基于光电编码器的准确数据随着时间的推移整合位置的变化的定位方式；其中r_k为运动弧线半径，Δθ_k为转角，b为机器人轴间距，ΔU_l,k和ΔU_r,k为机器人左右轮运动轨迹，c为机器人中心，Δρ_k为机器人运动直线距离：

r_kΔθ_k＝ΔU_l，k

(r_k+b)Δθ_k＝ΔU_r，k

得到：

${\mathrm{\Δ\θ}}_k=\frac{{\mathrm{\ΔU}}_{r,k}-{\mathrm{\ΔU}}_{l,k}}{b}$

而

${\mathrm{\Δ\ρ}}_k=\frac{{\mathrm{\ΔU}}_{r,k}+{\mathrm{\ΔU}}_{l,k}}{2}$

在k-1时刻，机器人的位置为：

x_k-1＝[x_k-1y_k-1θ_k-1]^T

其中，x_k-1，y_k-1为笛卡尔坐标系，θ_k-1为参考全局坐标系的方向角；按照旋转角Δθ_k和变化距离Δρ_k使得机器人运动到一个新的位置x_k，

$x_k=\left[\begin{array}{c}{x}_{k-1}+{\mathrm{\Δ\ρ}}_k\cos \left({\mathrm{\α}}_k\right)\\ y_{k-1}+{\mathrm{\Δ\ρ}}_k\sin \left({\mathrm{\α}}_k\right)\\ {\mathrm{\α}}_k+{\mathrm{\δ}}_k\end{array}\right]$

将机器人中心的弧线运动近似为弦，则得到其中 ${\mathrm{\γ}}_k=\frac{\mathrm{\π}}{2}-\frac{{\mathrm{\Δ\θ}}_k}{2};$ 因此 ${\mathrm{\α}}_k={\mathrm{\θ}}_{k-1}+\frac{\mathrm{\Δ\θ}}{2};$ 此外θ_k＝α_k+δ_k，其中 ${\mathrm{\δ}}_k=\frac{\mathrm{\π}}{2}-{\mathrm{\γ}}_k,$ 带入上式得到更新坐标方程：

$x_k=x_{k-1}+\left[\begin{array}{c}{\mathrm{\Δ\ρ}}_k\cos ({\mathrm{\θ}}_{k-1}+\frac{{\mathrm{\Δ\θ}}_k}{2})\\ {\mathrm{\Δ\ρ}}_k\sin ({\mathrm{\θ}}_{k-1}+\frac{{\mathrm{\Δ\θ}}_k}{2})\\ {\mathrm{\Δ\θ}}_k\end{array}\right]$

至此建立了多无人移动平台的定位模式。

3.如权利要求1或2所述的一种多无人移动平台网络化协同控制方法，其特征在于，所述的Wifi是基于Uart接口的符合wifi无线网络标准的嵌入式模块，内置无线网络协议IEEE802.11协议栈以及TCP/IP协议栈，能够实现用户串口数据到无线网络之间的转换。

4.如权利要求1或2所述的一种多无人移动平台网络化协同控制方法，其特征在于，所述的第三步中将机器人当作电场中的负电荷，将障碍物当作静电场中具有几何外形的均匀导体，将目标位置当作正电荷，目标位置对机器人产生吸引力，然后将整个***当作一个电势能场来处理，其中的人工势能场函数包括机器人之间的势能场函数、机器人和障碍物之间的势能场函数、以及机器人和目标位置之间的势能场函数。