[发明专利]基于视线和力反馈的移动机器人编队导航半自主控制方法

权利要求书

1.基于视线和力反馈的移动机器人编队导航半自主控制方法，其特征在于：由主端、从端和通讯环节组成；所述主端包括操作员、视觉跟踪设备、力反馈人机接口设备和控制计算机；所述从端包括多移动机器人系统、摄像头和工作环境组成；所述通讯环节采用WiFi或其他无线通讯方式；

所述主端的操作员通过视觉跟踪设备和力反馈人机接口设备进行交互，将控制命令通过通讯环节发送给从端的多移动机器人系统；

所述多移动机器人系统包括由n个移动机器人组成的多移动平台系统；

所述视觉跟踪设备用于捕捉操作员的眼动信息，并将这种信息转换为从端多移动机器人的目标队形控制指令；从端的多移动机器人系统根据收到的指令形成预期队形；

所述力反馈人机接口设备具有三自由度的输出反馈，用于将从端多机器人系统的状态以力信号的方式反馈给主端；

所述多移动机器人系统的速度、位置信息通过控制计算机以文本或者视频的形式实时反馈给主端；所述控制计算机利用可视化界面对从端多移动平台的目标队形进行预定义，并以图片的形式显示在界面上，利用视觉跟踪设备捕捉操作员的眼动信息，根据操作员所注视的图形，形成对应的点击事件；所述摄像头用于将多机器人的状态以及从端环境以视频或图片的方式反馈给主端；根据操作员所注视的图形，所述从端的多移动机器人系统采用虚拟刚体算法，将从端的多机器人系统看作一个整体，其中每个机器人自动实现队形保持与自动避障；所述多机器人的状态在每个机器人与相邻机器人之间的位姿是一定的，不会随着虚拟刚体的运动而发生变化，当虚拟刚体察觉到障碍物或者接收到队形切换的指令时，虚拟刚体中的矢量间的相对位姿就会发生相应的变化重新以另外一种刚体出现；用这种方式来自动实现队形保持与自动避障；所述力反馈人机接口设备具有三自由度的输出反馈，其中x方向和z方向反映机器人与障碍物之间的距离和角度信息，y方向的输出量则反应整个多移动平台系统的队形大小；所述力反馈人机接口设备采用手控器；所述视觉跟踪设备采用眼动仪；

利用可视化界面，根据从端多移动平台可能面临的工作环境事先设定好需要的队形；眼动追踪器通过捕捉操作者的视觉信息，确定操作者所注视的区域，由此获取点击刺激；得到目标队形命令，再结合力反馈接口设备，最终确定主端的高级控制命令；

对于手控器的末端控制器的位置P(x,y,z)，将x坐标轴代表虚拟刚体的角速度，x轴正方向为右转，x轴负方向为左转；z坐标轴代表虚拟刚体的线速度，z轴正方向为后退，z轴负方向为前进；y坐标轴用于控制虚拟刚体的大小；

对移动机器人的预期队形定义如下：

T＝[L^d,Φ^d]

其中

L^d和Φ^d分别代表各个移动机器人之间的相对距离和相对角度矩阵，两者共同决定了多移动机器人系统的队形共有m种队形，队形的大小由人机接口设备末端控制器的y坐标轴定义，将y分割区域[y_M1,y_M2,…,y_Mm]；则主端的眼动仪和力反馈人机接口设备与从端多移动平台系统之间的对应关系为：

其中v和w分别代表从端多移动机器人系统中虚拟刚体VRB的线速度和角速度，队形面积；k_v，k_ω，k_T和k_S则分别为线速度，角速度，队形和队形大小的增益系数；[q_x,q_y,q_z]^T代表力反馈人机接口设备末端控制器的位置坐标[x_M,y_M,z_M]，而q_S则是根据由眼动跟踪仪捕捉操作员视线所获得的目标队形的点击刺激得到；

虚拟刚体算法有如下定义；

定义1：对于{1,2，...，N}标记为N的移动机器人组，用F_i表示机器人i的局部参考坐标系，用r表示位置，同时R_i(t)∈SO(3)表示在时间t内机器人i相对于F_w的位置；

定义2：虚拟刚体由一组数量为N的移动机器人组和一个局部参考系坐标系F_v组成；其中机器人的局部位置由一组时变矢量{r₁(t)，r₁(t)，...，R_N(t)}所指定；

定义3：形成Π是虚拟刚体，对于一组大小为N的机器人组，在F_v中具有恒定的局部位置{r₁，r₂，...，r_N}，持续时间T_Π0；

定义4：变换Φ是一个虚拟刚体，具有相对于局部参考系F_v的时变位置{r₁(t)，r₁(t)，...，R_N(t)}，这样一组数量为N的移动机器人组，在持续时间T_Φ0中，P_v(t)∈R₃和R_v(t)∈SO(3)分别表示在时间t的F_w中F_v原点的位置和方向；机器人i的p_i(t)和r_i(t)之间的关系为p_i＝p_v+R_V*r_i，i∈{1,2，...，N}；

在以上定义的基础上，在二维坐标p处定义了从障碍物的位置指向VRB的排斥矢量，其大小是与该障碍物的位置和半径相关的高斯函数，假设环境中存在数量为n的障碍物组，将障碍物k在全局坐标系F_w中的水平位置表示为o_w，k，那么从二维坐标p处的n障碍物生成的矢量场中的整体排斥矢量为：

其中

其中B_k是与障碍物k的半径r_k相关的正标量参数；选择B_k的值，使得VRB的命令速度v_v能够克服最大的排斥矢量，因为VRB是虚拟的，2×2矩阵Σ是正定的，它通过以下方式定义动态类高斯函数的长轴和短轴；

虚拟刚体算法为单个移动机器人定义了比VRB更强的矢量场，这样当移动机器人接近障碍物时，排斥矢量能够将其“推开”；从而远离障碍物；

上式是在全局坐标系F_w中表示的，而在局部坐标系F_v中将它们表达为