一种移动机器人队列系统及路径规划、跟随方法
本发明属于多机器人路径规划和避障领域，主要涉及一种机器人队列的移动路径规划和避障方法，主要应用于医疗领域，军事行动，工业生产及灾后重建。
背景技术：
近些年在工业生产和灾后重建等一些应用中，机器人队列的高效工作引起了人们重视，机器人队列的前景变得光明起来，但是在应用过程中仍存在许多的问题。
首先，移动机器人队列在运行过程中的同步问题是研究的重点问题，同步是实现机器人队列的基础，比较常见的方法有基于行为法，领航跟随法和虚拟结构法等，例如：在cn201310402941.3中公开了一种基于跟随领航者编队的多移动机器人控制系统，包括实验环境图像采集模块、上位机定位模块、车式移动机器人组、通信模块、控制算法模块。其中所述实验环境图像采集模块，将用于采集多个移动机器人编队实验环境的视频图像；所述上位机定位模块，通过摄像头标定的坐标系，运用图像处理算法实时计算每个机器人的绝对位置信息；所述车式移动机器人组，由多个单体移动小车组成，自主决策完成编队任务；所述通信模块，通过无线通信方式进行数据交互以及信息共享；所述控制算法模块，基于领航跟随的编队算法协调和控制整个系统完成编队任务。但是该系统存在以下问题：
该系统采用工业摄像头获得机器人图像信息，以此得到机器人的位姿，应用条件苛刻，且获取的位姿信息准确度不够。该系统只能在室内特殊条件下使用，无法在室外和长距离作业情况下使用，具有很大的局限性。
其次，移动机器人在到达目标点过程中要找到最优的路径，传统的移动机器人路径规划是将移动机器人看作是一个质点来进行分析，而移动机器人队列则是多个目标的行进，需要整个机器人队列同步运动；这种方式存在的问题是：
机器人队列在运动过程中要保持队形，单个机器人的路径规划无法在运动过程中保持队形。对于体积小于队列间距的障碍物，容易引起机器人避障，机器人队列也应可以顺利通过。
最后，移动机器人队列在运行过程中面对障碍物和突发状况时，需要在避障的同时保持机器人队列，例如在专利cn201310402941.3中公开了一种面向多移动机器人系统的模糊编队及避障方法，首先领航机器人检测运行的前向区域是否有障碍物；如果无，则按原来队形继续运行；如果有，则多移动机器人系统进入避障状态，然后以广播的形式告知所有跟随机器人；最后跟随机器人根据存储的不同队形信息以及领航机器人广播的信息切换队形，并确定在编队队形中的位置；重复上述过程。本发明采用模糊编队及避障控制方法实现未知环境下的多移动机器人编队功能，并能有效的避开障碍物运行。但是该方法采用的是一种基于领航机器人方法，主要依靠一个功能齐全的机器人作为领航机器人，上位机无法对机器人队列实现有效的监测和控制。当领航机器人出现故障时就会导致队列出现误差，而当领航机器人无法工作时，整个队列也将无法工作。因此，该方法的可靠性和稳定性都较差，无法应对复杂环境和意外状况。
技术实现要素：
本发明主要解决的技术问题是提出一种用于移动机器人队列的路径规划和避障的方法，可以使移动机器人队列保持同步运动，并且每个个体的运动都不受到影响，还可以实现机器人队列的变化，同时在行进过程中可以对路径上的障碍物进行有效的避障，较少机器人的损耗。
为解决上述问题，本发明采用的技术方案是：
第一方面，针对移动机器人的队列问题，本发明提出了一种基于多传感器的移动机器人队列系统；包括一个上位机和多个机器人，每个机器人通过第一通讯模块与上位机进行通讯；各个机器人之间通过第二通讯模块进行相互通讯；所述的上位机可指定任意一个或者多个机器人作为领航机器人，其余的移动机器人作为跟随机器人；所述的领航机器人通过自身携带的感知模块和第二通讯模块获得的环境信息进行路径规划；所述的跟随机器人通过自身携带的定位模块获得自己与领航机器人的位置关系，实现跟随；所述的上位机通过设定领航机器人与跟随机械人之间的距离实现领航机器人和跟随机器人之间的队形变换。
本发明中在每个移动机器人上都安装有定位模块和无线通信模块，指定一个移动机器人作为领航机器人，其余的移动机器人作为跟随机器人，跟随机器人通过自身携带的定位模块和环境障碍物信息获得自己与领航机器人的位置关系，通过与领航机器人保持设定的跟随距离位置等对领航机器人进行跟随。同时也可以通过设定跟随机器人相对领航机器人的位置距离等参数可以实现不同的编队队形，以达到实际的需求。并且领航机器人的设定也可以不唯一，但一般情况下只需一个就可以，在复杂环境下可以通过设定多个领航机器人来提高系统的稳定性。
第二方面，本发明还提出了一种基于多传感器的移动机器人队列系统的路径规划方法，移动机器人队列工作时，首先由上位机设定编队队形并确定领航机器人，通过无线通信模块发送给移动机器人，移动机器人接收到指令后，跟随机器人到达指定位置。通过全景摄像头和超声波传感器检测环境障碍物信息，跟随机器人通过wi-fi通信模块将障碍物信息传送到领航机器人，领航机器人进行路径规划，并开始运动。包括以下步骤：
步骤1，任一机器人获取上位机指令，作为领航机器人；其余机器人获取上位机指令作为跟随机器人，跟随机器人移动到指定位置组成机器人队列；
步骤2，领航机器人采集环境障碍物信息；
步骤3，领航机器人获取跟随机器人采集的障碍物相对位置信息；
步骤4，领航机器人将所有障碍物位置信息应用到栅格地图中；
步骤5，以领航机器人的当前位置为初始位置，以最终位置为目标位置；
步骤6，在栅格地图中随机生成随机点xrand；
步骤7，在已知树的点中找到距离xrand最近的xnear；
步骤8，从xnear到xrand方向上以步长m截取点xnew；
步骤9，如果xnear到xnew之间没有障碍物，则在xnear周围以固定半径l寻找xnear的备选父节点；
步骤10，计算从起点到各个备选父节点再到xnew的路径，找出路径最短的备选父节点作为xnew的重选父节点；
步骤11，计算以xnew为圆心以l为半径内的节点，如果从起点到xnew再到周围节点的距离比从起点到xnew周围节点的距离短，则将xnew作为周围节点的父节点；
步骤12，将xnew加入到树的集合中，同时返回步骤6；
步骤13，当生长到目标点时，选择最短距离的路径作为规划的最优路径。
第三方面，本发明还提供了一种机器人跟随方法，机器人队列在运行过程中首先是由领航机器人开始工作，跟随机器人进行跟随。在每个移动机器人上都安装有可以相互通信的wi-fi通信模块，领航机器人通过gps模块检测自己的位置信息，然后将位置信息通过wi-fi通信模块传送给跟随机器人，跟随机器人利用领航机器人的位置信息进行跟随，具体步骤如下：
跟随机器人实时获取领航机器人的位置信息；
在跟随机器人内建立跟随机器人运动学模型；
在跟随机器人运动学模型内引入位姿误差，得到跟随机器人跟随运动模型；
跟随移动机器人身上的传感器实时采集当前的位移线速度、旋转角速度并反馈给跟随移动机器人的控制模块，跟随移动机器人的控制模块进行计算，得到轨迹跟踪误差，然后利用轨迹跟踪控制方法对随移动机器人的轨迹进行实时调整，实现对领航机器人的跟随。
进一步的，在机器人队列运行过程中遇到障碍物时要进行避障。当领航机器人的传感器检测到障碍物距离领航机器人为d1时，立即减速，同时跟随机器人也进行减速，当离开障碍物后，领航机器人和跟随机器人加速到原速度。当领航机器人的传感器检测到障碍物距离领航机器人为d2时，立即减速直至停止，同时跟随机器人停止到跟随位置。
移动机器人检测障碍物信息，并重新进行路径规划。
步骤3.4，当跟随机器人的传感器检测到障碍物距离领航机器人为d1时，立即减速，当离开障碍物后，加速到达跟随位置；当领航机器人的传感器检测到障碍物距离领航机器人为d2时，立即减速直至停止，发送信息给领航机器人，领航机器人停止运动，跟随机器人规划路径到达跟随位置，当跟随机器人到达后机器人队列开始运动。
步骤3.5，机器人队列停止运动后，利用gps定位装置检测当前位置信息，若没有达到目标点则返回步骤3.3。
通过在每个移动机器人的车体上安装的的超声波传感器可以实时检测周围的障碍物信息。在机器人运动时，在其周围一定距离设置停止区d2和减速区d1，减速区d1和停止区d2的设置跟移动机器人当前速度、位姿和车身尺寸有关。当车速比较高时，d1和d2的范围比较大。
当机器人队列到达目的地需要停靠或者需要变换队形时，上位机通过无线通信模块传输给领航机器人及跟随机器人，跟随机器人接收到指令后移动到新的跟随位置。
本发明的有益效果如下：
(1)本发明采用多个传感器作为机器人的环境感知系统，可以对自身运行状态和环境信息进行有效检测，提高系统在复杂环境下的适应能力。
(2)本发明采用快速扩展随机树方法进行路径规划，将每个机器人的环境信息叠加到领航机器人上进行规划，既提高了路径规划的效率，也保证了单个机器人的正常运动。
(3)本发明的上位机可以实时监测系统运行状况及对系统发出指令。每个机器人都可以作为领航机器人使用，通过上位机可以随时设置领航机器人和队形，可以应对不同的环境和需求。
附图说明
图1为移动机器人队列的示意图；
图2为机器人系统框图；
图3为移动机器人队列队形变换流程图；
图4为移动机器人队列路径规划流程图；
图5为移动机器人队列避障流程图。
具体实施方式
应该指出，以下详细说明都是例示性的，旨在对本发明提供进一步的说明。除非另有指明，本发明使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非本发明另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合；
下面结合附图对本专利进行说明：
图1所示的移动机器人队列示意图是本专利的一种应用情况，包括领航机器人及跟随机器人，采用领航机器人法对移动机器人队列进行控制具有成本低，稳定性高等优点。通过上位机对机器人队列矩阵设置可以调整跟随机器人数量及机器人队列的队形。
图2所示为机器人系统示意图，包括控制模块、驱动模块、感知模块、定位模块和通信模块；控制模块作为机器人的核心控制器，采用嵌入式系统，核心控制器通过处理感知模块，定位模块，通信模块的数据来控制驱动模块的运行。驱动模块在接收到控制模块的指令后驱动电机或减速器运行，从而实现移动机器人的加速减速以及转向动作。
进一步的，上述的感知系统是移动机器人采集环境信息和自身信息的模块，由多个传感器构成，包括超声波传感器，位姿传感器，温度传感器，电源检测传感器和视觉传感器构成；视觉传感器安装在移动机器人车体上方，扫描环境障碍物信息，发送给控制模块。超声波传感器安装在移动机器人车体的四个方向上，辅助检测环境信息，并实时检测障碍物信息发送给控制模块。温度传感器安装在机器人的电机和外部，实时检测电机温度和环境温度，并将温度信息发送给控制器。当电机或者环境温度过高时，控制模块向驱动模块发送停止指令，并通过通信模块发送给上位机。电量检测传感器安装在电源部分，可以通过上位机实时查看移动机器人电量剩余情况。位姿传感器由角度传感器，陀螺仪和加速度传感器构成，角度传感器安装在前轮转向部分，检测前轮转向角度，陀螺仪和加速度传感器安装在移动机器人内部，检测机器人当前纵向轴与x轴的夹角以及加速度。
定位模块采用gps定位装置进行，获得移动机器人中心点的位置坐标，可以确定移动机器人的位置。定位模块将采集到的位置信息发送给控制模块处理，并控制通信模块传输信息。
通信模块包括移动机器人与上位机之间的无线通信模块，以及移动机器人之间通信的wi-fi通信模块。控制模块控制通信模块发送信息，同时通信模块将接受到的信息发送给控制模块处理。通过无线通信模块，用户可以实时了解到所有机器人的运行状况，当有移动机器人发生故障不能继续工作时，或者通信出现故障时通信模块发送给上位机处理。
上述的多个传感器将采集到的信息发送给控制模块，控制模块控制驱动模块做出反应，同时通过通信模块实时地发送给上位机。
在本实施例中，设置跟随机器人的数量为2，他们的跟随位置分别为(-1,-1)和(1,-1)。根据图3的机器人队列流程示意图可得队形矩阵p1为：
首先确定跟随机器人的数量和移动机器人编队的队形，通过上位机输入队形矩阵p1。
上位机通过无线通信模块将指令传输给移动机器人，领航机器人通过gps定位模块采集自己的位置信息，将自己采集到的位置信息通过wi-fi通信模块传输给跟随机器人。
跟随机器人通过wi-fi通信模块接收到领航机器人的位置信息后，通过gps定位模块确定自己的位置，然后通过路径规划确定行走的路径。
跟随机器人通过控制模块给驱动模块发送指令，驱动模块驱动电机运行，使跟随机器人到达指定跟随位置，组成机器人队列。
图4为本发明中移动机器人队列路径规划的流程图，在本实施例中，跟随机器人通过自己的全景摄像头和超声波传感器对周围环境进行检测，将环境进行栅格化，把环境细分为一个单位的矩形，并将其进行二值化处理，将有障碍物的位置设为1，没有障碍物的位置设为0。将检测到的相对位置障碍物信息通过wi-fi通信模块传递到领航机器人的控制器中。
领航机器人通过通信模块接收跟随机器人的相对位置的障碍物信息，控制模块将跟随机器人相对位置的障碍物信息叠加到领航机器人的障碍物信息上，建立障碍物环境信息。
机器人队列路径规划步骤如下：
步骤1.1，以领航机器人的当前位置为初始点，以最终位置为目标点。
步骤1.2，在格栅地图中随机生成随机点xrand。
步骤1.3，在格栅地图的已知树的点中找到距离xrand最近的xnear。
步骤1.4，从xnear到xrand方向上以步长m截取点xnew。
步骤1.5，如果xnew到xnew之间没有障碍物，则在xnew周围以固定半径l寻找xnew的备选父节点。
步骤1.6，计算从起点到各个备选父节点再到xnew的路径，找出路径最短的备选父节点作为xnew的重选父节点。
步骤1.7，计算以xnew为圆心以l为半径内的节点，如果从起点到xnew再到周围节点的距离比从起点到xnew周围节点的距离短，则将xnew作为周围节点的父节点。
步骤1.8，将xnew加入到树的集合中。同时返回步骤1.2。
步骤1.9，当生长到目标点时，选择最短距离的路径作为规划的最优路径。
移动机器人将自身的运行状况实时地发送到上位机，用户可以通过上位机实时地监测移动机器人运行状况。
领航机器人运动后通过gps定位模块实时采集位置信息(x,y)，通过wi-fi模块发送给跟随机器人，跟随机器人的的wi-fi模块接收到信息后发送给控制模块，进行轨迹跟踪处理，使跟随机器人实时跟随领航机器人。
跟随机器人轨迹跟踪的主要步骤如下：
跟随机器人实时获取领航机器人的位置信息，即领航机器人开始工作，并将位置信息实时传送给跟随机器人
在跟随机器人内建立跟随机器人运动学模型；
在跟随机器人运动学模型内引入位姿误差，得到跟随机器人跟随运动模型；
跟随移动机器人身上的传感器实时采集当前的位移线速度、旋转角速度并反馈给跟随移动机器人的控制模块，跟随移动机器人的控制模块进行计算，得到轨迹跟踪误差，然后利用轨迹跟踪控制方法对随移动机器人的轨迹进行实时调整，实现对领航机器人的跟随。
建立跟随机器人运动学模型如下：
其中x(t)，y(t)是t时刻下跟随机器人的位置坐标，θ(t)是t时刻下跟随机器人运动方向相对x轴的偏角。
领航机器人的位置信息由xo(t)、yo(t)和θo(t)构成，是领航机器人在笛卡尔坐标下的坐标及相对x轴的偏角。通过队列的组成方式，可以得到跟随机器人相对领航机器人的位置信息为xr(t)、yr(t)和θr(t)，所以跟随机器人的目标位置为xr(t)，yr(t)和θr(t)。
xo(t)、yo(t)由gps传感器测得，θo(t)由陀螺仪测得，
在上述公式的基础上，引入位姿误差为[xe(t)ye(t)θe(t)]t，进行坐标变换如下：
其中xe(t)，ye(t)是t时刻下跟随机器人实际位置与期望距离x轴方向的误差和y轴方向的误差，θe(t)是t时刻下跟随机器人实际运动方向相对x轴的偏角与期望位置相对x轴偏角的误差。
根据gps定位模块确定当前位置信息，根据已知的地图信息和传感器实时采集到的环境信息确定当前位姿信息，计算跟随机器人期望位姿与实际位姿的误差，求出移动机器人跟随所需控制的加速度和前轮转角，由控制模块发出指令，驱动模块驱动执行机构实现跟随。
进一步的，在机器人队列运行过程中遇到障碍物时进行避障。
图5为机器人队列移动避障的流程图。
机器人队列在运行过程中设置必要的安全距离d1n和d2n，由移动机器人车体上的超声波传感器实时控制，每一个移动机器人车体的前后左右各安装一个超声波传感器。
超声波传感器的编号为h1，h2，h3和h4，分别代表前后左右四个超声波传感器，在运行过程中移动超声波传感器实时地检测周围环境状况。
当移动机器人直线运行时，h1传感器检测前方障碍物，并设定减速距离d1和停止距离d2，d1和d2的距离随着速度的增加而增加。
h2检测机器人后方障碍物，在移动机器人直行时不用设置。
h3和h4检测机器人左右方向的障碍物，在移动机器人直行时只需设置停止区d2，保证机器人安全通过。
当移动机器人转弯时，根据前轮转向角度
步骤3.1，当领航机器人的超声波传感器检测到障碍物距离领航机器人为d1时，立即减速，同时通过wi-fi通信模块发送信息给跟随机器人，跟随机器人也进行减速，当离开障碍物后，领航机器人和跟随机器人加速到原速度。
步骤3.2，当领航机器人的传感器检测到障碍物距离领航机器人为d2时，立即减速直至停止，同时跟随机器人停止到跟随位置。
步骤3.3，移动机器人检测障碍物信息，并重新进行路径规划。
步骤3.4，当跟随机器人的传感器检测到障碍物距离领航机器人为d1时，立即减速，当离开障碍物后，加速到达跟随位置；当领航机器人的传感器检测到障碍物距离领航机器人为d2时，立即减速直至停止，发送信息给领航机器人，领航机器人停止运动，跟随机器人规划路径到达跟随位置，当跟随机器人到达后机器人队列开始运动。
步骤3.5，机器人队列停止运动后，利用gps定位装置检测当前位置信息，若没有达到目标点则返回步骤3.3。
本发明采用的方法具有适用性高，成本低，稳定性强等优点，同时可以应对各种突发状况。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！