CN103901889A - 一种基于蓝牙通信的多机器人编队控制路径跟踪方法 - Google Patents

Abstract

本发明公开了一种基于蓝牙通信的多机器人编队控制路径跟踪方法，机器人摆放于iSpace实验平台上；摄像头将捕捉到的各机器人位置姿态数据发送给上位机；上位机对其进行解析，保存每个机器人的位置和姿态信息；根据预定的跟踪轨迹得到多机器人构成的几何中心的位置，角度，移动线速度和角速度；以几何中心为坐标原点，建立编队***坐标系；根据机器人实际信息和参考信息计算出机器人的位置和姿态偏差；再计算出每个机器人下一个控制周期的线速度和角速度，将控制参数转换为机器人左右轮的转速，并通过蓝牙发送给机器人；本发明跟踪***精度较高，跟踪曲线适用范围广，配合编队控制功能，适用于多机器人研究的相关领域，具有良好的发展潜力。

Description

一种基于蓝牙通信的多机器人编队控制路径跟踪方法

技术领域

本发明涉及一种多机器人的路径跟踪方法，尤其涉及一种基于蓝牙通信的多机器人编队控制的路径跟踪方法。

背景技术

多机器人技术作为机器人技术领域的一个重要分支，近十几年发展非常迅猛，主要是因为与单机器人***相比，它能更出色地完成任务，同时节省成本。多机器人技术中，编队控制，通信调度以及路径跟踪等领域都成为热点研究对象，对于提升多机器人协同作业的效率有重要意义。

特别地，随着路径跟踪和编队控制以及通信协议的进一步发展，将编队控制和路径跟踪结合起来，进行目标跟踪或协同运作，已经打破以前仿真结果良好，实际效果较差的局面。对于多机器人编队控制路径跟踪***的实现问题，随着计算机性能的不断提升，采用集中式的控制方法，减轻现场作业智能体的负荷，并且稳定性也可以得到一定保证。集中控制多机器人路径跟踪一般由四部分组成，上位机，多智能体，位置捕捉***，通信***；其中通信***一般采用无线传感器网络相关领域的成熟协议，如蓝牙，ZigBee等，位置捕捉***可采用GPS自定位，也可采用位置估计等方法，上位机负责多智能体的调度和控制，各个智能体目标则在于完成特定任务。

传统的目标跟踪往往是单机器人的目标跟踪。由于单机器人一般会存在视觉范围有限，运动自由度较低等特点，若跟踪目标变化过快，或存在某些干扰等，可能会出现跟踪失误，甚至失去跟踪目标等现象。为了提高跟踪的鲁棒性，将单机器人跟踪适当改为多机器人协同跟踪，以一定的编队控制算法控制***的队形，如虚拟结构法可将跟踪目标设为结构中心，则将大大提高跟踪的鲁棒性和准确性，改善单机器人跟踪的不足。

目前，多机器人路径跟踪方法有所发展，但仍存在不足。如跟踪精度有限，初始误差较大，可移植性不强,稳定性较差等。在该方面的代表算法有两种，一是二次曲线算法，二是实时路径跟踪算法。二次曲线算法将路径跟踪问题转化为离散参考点的跟踪问题，通过逐次调整参考点，以一定的方式跟踪参考点达到路径跟踪的目的。其核心问题在于参考点的移动速度选取，及参考点与机器人距离的选择问题，该问题是制约该算法实际运用的主要问题，若距离选取过大，则跟踪精度较差；若距离选取过小，则机器人移动速度较慢，且抖振现象严重。实时路径跟踪算法在一定程度上弥补了二次曲线跟踪算法，但其核心问题在于不能跟踪直线，因此也限制了其向实际应用的转化。

发明内容

本发明的目的是针对现有技术的不足，提供了一种基于蓝牙通信的多机器人编队控制路径跟踪方法。

本发明的目的是通过以下技术方案来实现的：

一种基于蓝牙通信的多机器人编队控制路径跟踪方法，该方法由跟踪***实现，所述跟踪***包括：若干个双轮差速驱动机器人、iSpace位置捕捉***、上位机和蓝牙通信模块；其中，所述双轮差速驱动机器人置于由6个摄像头组成的覆盖整个机器人运动平台的iSpace位置捕捉***上，iSpace位置捕捉***通过电缆线与上位机相连，蓝牙通信模块与上位机相连，双轮差速驱动机器人通过蓝牙通信模块与上位机进行蓝牙通信；该方法包括以下步骤：

（1）搭建实验平台：机器人的初始位置和姿态要求按照既定的几何图形摆放于iSpace位置捕捉***的实验平台上。启动机器人和上位机后，机器人和上位机通过蓝牙模块相互连接并进行通讯。摄像头以每秒50帧的速率将捕捉到的各机器人位置姿态帧数据发送给上位机。

（2）上位机收到数据帧后对其进行解析，保存每个机器人的位置和姿态信息。

（3）根据预定的跟踪轨迹，结合前述步骤2所得的位置和姿态信息计算出多机器人构成的几何中心的位置，角度，移动线速度和角速度，以(x_c,y_c)表示几何中心的参考位置，θ_c表示几何中心的参考角度，(v_r,ω_r)表示其中心的参考线速度和角速度。在驱动轮的作用下，机器人会以一定的线速度v和角速度ω行驶，假定机器人轮子半径为r，两轮之间的距离为w，其与左右轮转速(ω_l,w_r)之间的关系为：

$\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right]=\left[\begin{array}{cc}\frac{r}{2}& \frac{r}{2}\\ \frac{r}{w}& -\frac{r}{w}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_r\\ {\mathrm{\ω}}_l\end{array}\right].$

（4）为完成控制目标，需将几何中心的参数映射到每个机器人的参数。以几何中心为坐标原点，建立编队***坐标系，第i个机器人在编队***坐标系的坐标为(p_xi,p_yi)，则可根据步骤3的几何中心的参数和几何队形的坐标算出第i个机器人的参考位置(x_ir,y_ir)，参考角度θ_ir参考线速度v_ir和角速度ω_ir，计算方法如下：

θ_ir=θ_c

x_ir=x_c+p_xicosθ_ir-p_yisinθ_ir

y_ir=y_c+p_xisinθ_ir+p_yicosθ_ir

ω_ir=ω_r

$v_{\mathrm{ir}}=\left|{\mathrm{\ω}}_r\right|\sqrt{p_{\mathrm{xi}}^2+{(r-p_{\mathrm{yi}})}^2}$ 其中， $r=\sqrt{x_c^2+y_c^2}.$

（5）根据步骤2得到的机器人实际信息和步骤4得到的参考信息，可以计算出机器人的位置和姿态偏差。机器人自身作为刚体，自身的坐标系称之为机器人坐标系。机器人运行的地图或平台称为世界坐标系。从世界坐标系到机器人坐标系的旋转矩阵为：

$R\left(\mathrm{\θ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]$

并根据所述坐标转换关系，将偏差转换为机器人坐标系的偏差(x_ie,y_ie)和θ_ie。在机器人坐标系中定义的机器人轨迹跟踪误差为：

$\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=R\left(\mathrm{\θ}\right)\left[\begin{array}{c}{x}_r-x\\ y_r-y\\ {\mathrm{\θ}}_r-\mathrm{\θ}\end{array}\right]$

其动态特性为，

$\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ωy}}_e-v+v_r\left(t\right)\cos {\mathrm{\θ}}_e\\ -{\mathrm{\ωx}}_e+v_r\left(t\right)\sin {\mathrm{\θ}}_e\\ {\mathrm{\ω}}_r\left(t\right)-\mathrm{\ω}\end{array}\right].$

（6）给定每个机器人3个大于0的控制参数{k_i1,k_i2,k_i3}，根据步骤5中的偏差，可计算出每个机器人下一个控制周期的线速度和角速度，计算方法如下：

ω_i=ω_ir+k_i2(θ_ie-α_i)+v_irη_iy_ie+v_ir ²k_i1sinθ_ie/(1+(v_irk_i1y_ie)²)

v_i=ω_iy_ie+v_ircosθ_ie+k_i3x_ie

其中α_i=-arctan(v_irk_i1y_ie),η_i=(sinθ_ie-sinα_i)/(θ_ie-α_i)

{k_i1,k_i2,k_i3}为经验值。

（7）将步骤6中得到的控制参数转换为机器人左右轮的转速(ω_iL,ω_iR)，转换公式为：

ω_iL=1992(2v_i-Lω_i)/2+8

ω_iR=1992(2v_i+Lω_i)/2+8

其中1992为拟合得到的参数，L为机器人左右轮之间的距离。

（8）将步骤7中得到的机器人左右轮的转速通过nxt提供的应用编程接口API利用蓝牙发送出去，各个机器人根据ID配对接收属于自己的控制信息。

（9）跟踪***完成任务后，机器人停止运行，上位机上显示机器人跟踪的实际曲线效果，实现基于蓝牙通信的多机器人编队控制路径跟踪。

本发明的效益在于：

1、通过加入编队控制改变了原始的单机器人跟踪带来的不确定性和丢失性，提高了目标跟踪的冗余性和鲁棒性。通过采用反演设计方法，在跟踪精度上，也能达到工业作业的要求；在收敛速度上，相对二次曲线跟踪算法和实时路径跟踪算法，可以达到更快的收敛速度。

2、通信方式可以是蓝牙，ZigBee,WiFi等无线通信协议。

3、跟踪***精度较高，跟踪曲线适用范围广，配合编队控制功能，适用于多机器人研究的相关领域，具有良好的发展潜力。

附图说明

图1是跟踪***实施整体示意图；

图2是机器人端程序流程图；

图3是多机器人编队场景图

图4是三机器人编队队形示意图；

图5是机器人坐标变换示意图；

图6是上位机的工作流程图；

图中，摄像头1、双轮差速驱动机器人2、上位机3。

具体实施方式

下面结合附图说明跟踪***的具体实施方式。

图1描述了跟踪***的整体结构示意图，跟踪***主要由下列部件组成：若干个双轮差速驱动机器人、iSpace位置捕捉***、上位机和蓝牙通信模块。所述双轮差速驱动机器人置于由6个摄像头组成的覆盖整个机器人运动平台的iSpace位置捕捉***上，iSpace位置捕捉***通过电缆线与上位机相连，蓝牙通信模块与上位机相连，双轮差速驱动机器人通过蓝牙通信模块与上位机进行蓝牙通信。

双轮差速驱动机器人可以选用LEGO公司的nxt双轮差速驱动机器人，每个机器人都具有相同的物理结构和相似的机械特性。其包括两个电机作为驱动轮的动力机构，两个驱动轮的控制相互独立。nxt机器人有内置可充电电池，并具有微处理器，可编程器件，USB连接线和蓝牙通信模块。其中驱动轮与微处理器***扩展电路相连，微处理器程序可通过蓝牙远程下载，也可通过USB连接线连接上位机下载。乐高公司为nxt机器人提供了许多方便的编程语言，本跟踪***使用的是其基于Java编程语言的类库，这些类库封装了其底层的实现，减少了研究人员对底层代码的开发，方便研究人员进行更高层的实验和理论研究，符合初期的设计目标。nxt机器人的使用方法是先对其充电，待电量充满后，烧写事先编好的和上位机配套的机器人上的程序（程序流程图见图2）。其中，驱动蜂鸣器，解析控制信号，读取左右轮和驱动左右轮，LEGO公司都提供了专门的接口，延时时间一般为1000毫秒。烧写程序既可通过USB进行有线串口传送，也可利用蓝牙模块远程传送。上位机也可采用eclipse集成开发环境进行开发，烧写程序时，其会默认选择通过USB连接，若找寻不到目标，则会自动转换为蓝牙连接。

iSpace位置捕捉***是一个为多机器人控制和分布式智能应用的实验平台，例如智能交通***，分布式能量管理和大规模资源分配。该***主要由四部分组成：视觉捕捉***、多LEGO机器人、图形用户界面、监控和本地控制器。视觉捕捉***主要包括由6个摄像头及与上位机的连接电缆。每个机器人要求至少同时有3个摄像头覆盖，才能正确捕捉到全局的位置信息(x,y)和角度信息，其位置信息根据提前设定好的参考原点获得，而角度信息以初始化时刻的角度为参考角度，视觉***根据每个机器人自身携带的三个标志点mark组成的三角形形状识别每个机器人，因此各个机器人的mark三角形形状彼此不能相同。

iSpace平台的摄像头探测到小车的路径信息，经过变送器处理成上位机能够识别的信号传递给上位机。装有Java语言开发能力的上位机作为基站将其与给定的期望路径进行比较，并通过程序运算之后担当控制、通信和计算决策等任务，上位机通过蓝牙接口发送指令信号给LEGO NXT机器人进行通信，实现编队和跟踪过程控制。

上位机可以是任何具有Java语言开发能力的台式机或笔记本，使用方便。***运行时，摄像头需与上位机始终保持连接，并且上位机需在摄像头通电的前提下，安装好配套的驱动程序和软件。如图6所示，上位机可以对摄像头进行控制，如摄像头的启动关闭，位置原点的定标和精度等级的更新等。随着时间的推移，受震动，微小移位等影响，摄像头定位精度可能随之降低，利用上位机的自定位***可以重新标定坐标，提高定位精度，因此推荐定期进行标定。上位机需配置相应的通信模块，本跟踪***采用蓝牙通信模块，***运行要求上位机和机器人的蓝牙全都开启。

（4）蓝牙通信模块可以采用一般的通信模块。蓝牙标准数据包格式如下：蓝牙规范的标准数据包

ACCESS CODE[72]

HEADER[54]

PAYLOAD[0-2745]

单位：bit

ACCESS CODE	Access Code（访问码）用于数据同步、DC偏移补偿和数据包的标识。
		HEADER	Header（报头）包含了链路控制（LC）信息。
PAYLOAD	Payload（数据）携带上层的语音和数据字段。

总之，各个机器人可以和上位机交换信息，iSpace位置捕捉***将各个机器人的位置信息传送给上位机，上位机可以解析出位置信息控制机器人的相关动作。

本发明基于蓝牙通信的多机器人编队控制路径跟踪方法，包括以下步骤：

1、搭建实验平台：机器人的初始位置和姿态要求按照既定的几何图形摆放于iSpace位置捕捉***的实验平台上。启动机器人和上位机后，机器人和上位机通过蓝牙模块相互连接并进行通讯。摄像头以每秒50帧的速率将捕捉到的各机器人位置姿态帧数据发送给上位机。

2、上位机收到数据帧后对其进行解析，保存每个机器人的位置和姿态信息。例如机器人的绝对坐标(x,y)和机器人的相对位置等等。

3、根据预定的跟踪轨迹，结合前述步骤2所得的位置和姿态信息计算出多机器人构成的几何中心的位置，角度，移动线速度和角速度，以(x_c,y_c)表示几何中心的参考位置，θ_c表示几何中心的参考角度，(v_r,ω_r)表示其中心的参考线速度和角速度。

在驱动轮的作用下，机器人会以一定的线速度v和角速度ω行驶，假定机器人轮子半径为r，两轮之间的距离为w，其与左右轮转速(ω_l,w_r)之间的关系为：

$\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right]=\left[\begin{array}{cc}\frac{r}{2}& \frac{r}{2}\\ \frac{r}{w}& -\frac{r}{w}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_r\\ {\mathrm{\ω}}_l\end{array}\right]$

以上坐标和参数都是基于世界坐标系，即如附图3所示的X_wOY_w坐标系。

4、为完成控制目标，需将几何中心的参数映射到每个机器人的参数。附图3是以三个机器人组成的三角形队形为例的多机器人编队场景，以虚拟中心为参考原点，则编队控制坐标系如附图4所示。以几何中心为坐标原点，可建立编队***坐标系，第i个机器人在编队***坐标系的坐标为(p_xi,p_yi)，则可根据步骤3的几何中心的参数和如附图4所示的几何队形的坐标算出第i个机器人的参考位置(x_ir,y_ir)，参考角度θ_ir参考线速度v_ir和角速度ω_ir，计算方法如下：

θ_ir=θ_c

x_ir=x_c+p_xicosθ_ir-p_yisinθ_ir

y_ir=y_c+p_xisinθ_ir+p_yicosθ_ir

ω_ir=ω_r

$v_{\mathrm{ir}}=\left|{\mathrm{\ω}}_r\right|\sqrt{p_{\mathrm{xi}}^2+{(r-p_{\mathrm{yi}})}^2}$ 其中， $r=\sqrt{x_c^2+y_c^2}.$

5、根据步骤2得到的机器人实际信息和步骤4得到的参考信息，可以计算出机器人的位置和姿态偏差。机器人自身作为刚体，自身的坐标系称之为机器人坐标系，如图5所示的X^roY^r坐标系。机器人运行的地图或平台称为世界坐标系如右图所示的X^woY^w坐标系。从世界坐标系到机器人坐标系的旋转矩阵为：

$R\left(\mathrm{\θ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]$

并根据所述坐标转换关系，将偏差转换为机器人坐标系的偏差(x_ie,y_ie)和θ_ie。在机器人坐标系中定义的机器人轨迹跟踪误差为：

$\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=R\left(\mathrm{\θ}\right)\left[\begin{array}{c}{x}_r-x\\ y_r-y\\ {\mathrm{\θ}}_r-\mathrm{\θ}\end{array}\right]$

其动态特性为，

$\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ωy}}_e-v+v_r\left(t\right)\cos {\mathrm{\θ}}_e\\ -{\mathrm{\ωx}}_e+v_r\left(t\right)\sin {\mathrm{\θ}}_e\\ {\mathrm{\ω}}_r\left(t\right)-\mathrm{\ω}\end{array}\right]$

从动态特性可看出，选择ω控制θ_e，选择v和控制x_e，则y_e的控制只能选择由x_e或θ_e间接控制，若选择x_e，则需保证ω不为0，即不能跟踪直线，因此可选择θ_e间接控制y_e。

6、给定每个机器人3个大于0的控制参数{k_i1,k_i2,k_i3}，根据步骤5中的偏差，可计算出每个机器人下一个控制周期的线速度和角速度，计算方法如下：

ω_i=ω_ir+k_i2(θ_ie-α_i)+v_irη_iy_ie+v_ir ²k_i1sinθ_ie/(1+(v_irk_i1y_ie)²)

v_i=ω_iy_ie+v_ircosθ_ie+k_i3x_ie

其中α_i=-arctan(v_irk_i1y_ie),η_i=(sinθ_ie-sinα_i)/(θ_ie-α_i)

{k_i1,k_i2,k_i3}为经验值，需通过实验确定其合适的取值，在本跟踪***中，k_i1=10,k_i2=5,k_i3=1.8。

7、将步骤6中得到的控制参数转换为机器人左右轮的转速(ω_iL,ω_iR)，转换公式可由实验测出。实验方法为：给定nxt机器人左轮和右轮相同的转速，测量在该转速下机器人的前进速度，全量程范围内均匀取点，实验完成后进行拟合即可得到。根据实验本跟踪***的转换公式为：

ω_iL=1992(2v_i-Lω_i)/2+8

ω_iR=1992(2v_i+Lω_i)/2+8

其中1992为拟合得到的参数，L为机器人左右轮之间的距离。

8、将步骤7中得到的机器人左右轮的转速通过nxt提供的应用编程接口API利用蓝牙发送出去，各个机器人根据ID配对接收属于自己的控制信息。

9、跟踪***完成任务后，机器人停止运行，上位机上显示机器人跟踪的实际曲线效果，实现基于蓝牙通信的多机器人编队控制路径跟踪。

本跟踪***对每个机器人都有3个可调参数，增加的3个自由度可以调节使得机器人跟踪的动态特性和稳态特性都得到改善，并且可克服某些方法不能适用于直线轨迹跟踪的缺陷，并摆脱对对象的过渡依赖性，即对步骤7所得到的对象模型精度要求并不高，同时封装了ID匹配等底层代码，具有应用方便，收敛速度快，跟踪精度高，队形保持稳定的特点。

Claims (1)

1.一种基于蓝牙通信的多机器人编队控制路径跟踪方法，该方法由跟踪***实现，所述跟踪***包括：若干个双轮差速驱动机器人、iSpace位置捕捉***、上位机和蓝牙通信模块；其中，所述双轮差速驱动机器人置于由6个摄像头组成的覆盖整个机器人运动平台的iSpace位置捕捉***上，iSpace位置捕捉***通过电缆线与上位机相连，蓝牙通信模块与上位机相连，双轮差速驱动机器人通过蓝牙通信模块与上位机进行蓝牙通信；其特征在于，该方法包括以下步骤：

（1）搭建实验平台：机器人的初始位置和姿态要求按照既定的几何图形摆放于iSpace位置捕捉***的实验平台上；启动机器人和上位机后，机器人和上位机通过蓝牙模块相互连接并进行通讯；摄像头以每秒50帧的速率将捕捉到的各机器人位置姿态帧数据发送给上位机；

（2）上位机收到数据帧后对其进行解析，保存每个机器人的位置和姿态信息；

（3）根据预定的跟踪轨迹，结合前述步骤2所得的位置和姿态信息计算出多机器人构成的几何中心的位置，角度，移动线速度和角速度，以(x_c,y_c)表示几何中心的参考位置，θ_c表示几何中心的参考角度，(v_r,ω_r)表示其中心的参考线速度和角速度；在驱动轮的作用下，机器人会以一定的线速度v和角速度ω行驶，假定机器人轮子半径为r，两轮之间的距离为w，其与左右轮转速(ω_l,w_r)之间的关系为：

$\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right]=\left[\begin{array}{cc}\frac{r}{2}& \frac{r}{2}\\ \frac{r}{w}& -\frac{r}{w}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_r\\ {\mathrm{\ω}}_l\end{array}\right];$

（4）为完成控制目标，需将几何中心的参数映射到每个机器人的参数；以几何中心为坐标原点，建立编队***坐标系，第i个机器人在编队***坐标系的坐标为(p_xi,p_yi)，则可根据步骤3的几何中心的参数和几何队形的坐标算出第i个机器人的参考位置(x_ir,y_ir)，参考角度θ_ir参考线速度v_ir和角速度ω_ir，计算方法如下：

θ_ir=θ_c

x_ir=x_c+p_xicosθ_ir-p_yisinθ_ir

y_ir=y_c+p_xisinθ_ir+p_yicosθ_ir

ω_ir=ω_r

$v_{\mathrm{ir}}=\left|{\mathrm{\ω}}_r\right|\sqrt{p_{\mathrm{xi}}^2+{(r-p_{\mathrm{yi}})}^2}$ 其中， $r=\sqrt{x_c^2+y_c^2}.$

（5）根据步骤2得到的机器人实际信息和步骤4得到的参考信息，可以计算出机器人的位置和姿态偏差；机器人自身作为刚体，自身的坐标系称之为机器人坐标系；机器人运行的地图或平台称为世界坐标系；从世界坐标系到机器人坐标系的旋转矩阵为：

$R\left(\mathrm{\θ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]$

并根据所述坐标转换关系，将偏差转换为机器人坐标系的偏差(x_ie,y_ie)和θ_ie；在机器人坐标系中定义的机器人轨迹跟踪误差为：

$\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=R\left(\mathrm{\θ}\right)\left[\begin{array}{c}{x}_r-x\\ y_r-y\\ {\mathrm{\θ}}_r-\mathrm{\θ}\end{array}\right]$

其动态特性为，

$\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ωy}}_e-v+v_r\left(t\right)\cos {\mathrm{\θ}}_e\\ -{\mathrm{\ωx}}_e+v_r\left(t\right)\sin {\mathrm{\θ}}_e\\ {\mathrm{\ω}}_r\left(t\right)-\mathrm{\ω}\end{array}\right];$

（6）给定每个机器人3个大于0的控制参数{k_i1,k_i2,k_i3}，根据步骤5中的偏差，可计算出每个机器人下一个控制周期的线速度和角速度，计算方法如下：

ω_i=ω_ir+k_i2(θ_ie-α_i)+v_irη_iy_ie+v_ir ²k_i1sinθ_ie/(1+(v_irk_i1y_ie)²)

v_i=ω_iy_ie+v_ircosθ_ie+k_i3x_ie

其中α_i=-arctan(v_irk_i1y_ie),η_i=(sinθ_ie-sinα_i)/(θ_ie-α_i)

{k_i1,k_i2,k_i3}为经验值；

（7）将步骤6中得到的控制参数转换为机器人左右轮的转速(ω_iL,ω_iR)，转换公式为：

ω_iL=1992(2v_i-Lω_i)/2+8

ω_iR=1992(2v_i+Lω_i)/2+8

其中1992为拟合得到的参数，L为机器人左右轮之间的距离；

（8）将步骤7中得到的机器人左右轮的转速通过nxt提供的应用编程接口API利用蓝牙发送出去，各个机器人根据ID配对接收属于自己的控制信息；

（9）跟踪***完成任务后，机器人停止运行，上位机上显示机器人跟踪的实际曲线效果，实现基于蓝牙通信的多机器人编队控制路径跟踪。

Images