[发明专利]一种基于蓝牙通信的多机器人编队控制路径跟踪方法

权利要求书

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)之间的关系为：

vω=r2r2rw-rwωrωl;]]>

（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

vir=|ωr|pxi2+(r-pyi)2]]>其中，r=xc2+yc2.]]>

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

R(θ)=cosθsinθ0-sinθcosθ0001]]>

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

xeyeθe=R(θ)xr-xyr-yθr-θ]]>

其动态特性为，

xeyeθe=ωye-v+vr(t)cosθe-ωxe+vr(t)sinθeωr(t)-ω;]]>

（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）跟踪系统完成任务后，机器人停止运行，上位机上显示机器人跟踪的实际曲线效果，实现基于蓝牙通信的多机器人编队控制路径跟踪。