群组式协同消防机器人火场内编组队列行驶控制方法与流程

1.本发明涉及消防机器人技术领域，具体涉及一种群组式协同消防机器人火场内编组队列行驶控制方法。


背景技术：

2.对于消防机器人的研发历经了大致三个阶段，同时也形成了三代不同的消防机器人。第一个阶段主要依靠远程操作控制系统，在此基础上形成的消防机器人叫做程序控制型消防机器人，这也是世界上第一代消防机器人。在第二个阶段中，主要通过传感器来开发其性能，这一阶段的消防机器人被称为功能型消防机器人，也是世界上第二代消防机器人。在第三个阶段中，消防机器人的研究方向开始向智能化、协同化靠近，消防机器人不再单单局限于某种功能，而是更全面的智能化功能，这一阶段形成的消防机器人被称为智能型、协同型消防机器人，这也是世界上第三代消防机器人。
3.未来消防机器人的研发将重点瞄准由单一功能的灭火机器人、火场侦察机器人向多种功能组合的消防机器人发展。控制方式从有线、无线的远距离程序控制，逐步向智能型群组型、协作型消防机器人发展。交互方式可先从简单的传感器交互着手，逐步融入语音交互、动作交互、姿势交互、语义交互等交互模式，为下一步达成各机器人之间的协同作战，实现“无人化”救援现场提供可能。
4.目前，传统无人系统技术存在着自主性低、协调性差等问题，无法有效解决协同内部时间、空间和任务层面上的矛盾，难以实现复杂环境下的消防任务。面向强干扰、高动态等复杂火灾环境下的消防需求，为实现无人系统的任务能力扩展以及整体消防效能提升，采取多智能体协同作业的模式，实现能力互补和行动协调，已成为未来消防应用的主要发展方向。
5.相关技术中，公布号为cn110101996a的中国发明专利文献记载了一种复杂环境下消防机器人协同定位与自主作业方法，以及公布号为cn110201333a的中国发明专利文献记载了一种消防机器人全自动协同侦察和灭火作业方法，等等，这些方案所采用的这种集中式控制需要一个主控单元即主操控台，所有的运算都在主控单元进行，机器人必须通过主控单元下发指令才能执行相应操作，容易造成主控单元运算量负荷、实时性大打折扣，且队形呈分散状，不易控制，导致消防机器人行走至火灾现场所需时间和能耗较高，不利于及时遏制火情。
6.相关技术中，公布号为cn112286179a的中国发明专利文献记载了一种协同运动控制方法、系统、计算机设备、机器人，该方案在机器人控制中采用实时分布式的控制方式，具体机器人的控制算法为模型预测控制加数学规划方法，研究在静态环境下机器人群组队形保持优先或速度优先的控制策略，以及在动态环境中，保证多机器人成功避障的前提下，尽可能的维持原来队形的控制策略。
7.由于火灾现场具有高动态非结构特征，浓烟环境和障碍物会阻碍机器人之间通讯和视野，尤其考虑水带拖拽过程中，防止水带和水带之间、水带和障碍物之间发生交叉缠绕
问题。另外，水带充水状态其负载很大，对机器人的运动控制和稳定性影响很大，而传统的编队协同控制算法都未考虑这些因素，需针对特定条件和因素采取对应的协同控制算法。


技术实现要素：

8.本发明所要解决的技术问题在于如何控制消防机器人快速到达火灾现场，节省行走时间和能耗。
9.本发明通过以下技术手段实现解决上述技术问题的：
10.本发明提出了一种群组式协同消防机器人火场内编组队列行驶控制方法，所述消防机器人包括巡检机器人和灭火机器人，消防机器人组成的队列呈链式结构，且巡检机器人位于队头，灭火机器人跟随在巡检机器人之后，每台消防机器人装载有激光雷达和摄像机，对于每台所述消防机器人，所述方法包括以下步骤：
11.获取与其前方距离最近消防机器人的相对距离和相对角度；
12.基于所述相对距离和相对角度，计算距离误差和角度误差；
13.基于约束条件、所述距离误差和所述角度误差，建立距离性能函数和角度性能函数；
14.通过距离控制增益，调节所述距离误差与所述距离性能函数之间的差值，得到速度控制信号；
15.通过角度控制增益，调节所述角度误差与所述角度性能函数之间的差值，得到转向角控制信号；
16.基于所述速度控制信号和所述转向角控制信号，控制机器人自身行驶。
17.本发明针对链式结构编队跟随任务，设置分布式控制方式，仅仅利用了机器人的局部信息进行交互，在每个车体的局部坐标系下得到前方机器人的距离和角度值以及障碍物的距离，根据机器人距离误差和距离性能函数之间的差值通过距离控制增益调节，使得距离误差能够保证在距离性能函数的上限和下限范围内；同样，根据机器人角度误差和角度性能函数之间的差值通过角度控制增益调节，使得角度误差能够保证在角度性能函数的上限和下限范围内；以得到相应的速度和转向角来控制车体运动和转向，从而保证每台车都能跟踪上前方车辆，无需进行队形变化就能够一直保持队形，能快速到达火灾现场，节省行走时间和能耗。
18.进一步地，所述消防机器人的运动学模型为：
[0019][0020]
式中：对于i＝1，...，n，其中，xi,yi,θi表示第i台机器人的位置和方向；ui,γi,α分别表示其线速度、转向角和长度；分别表示xi,yi,θi的一阶导数。
[0021]
进一步地，所述基于所述相对距离和相对角度，计算距离误差和角度误差的公式表示为：
[0022][0023]
式中：表示距离误差；表示角度误差；di
(t)和βi(t)分别表示两台连续消防机器人之间的相对距离和相对角度；d
i,des
表示事先设定的第i台消防机器人与前方第i-1台消防机器人之间的距离。
[0024]
进一步地，所述基于约束条件、所述距离误差和所述角度误差，建立距离性能函数和角度性能函数分别为：
[0025]
分别对所述距离误差和所述角度误差进行微分处理，分别得到距离误差动态方程和角度误差动态方程；
[0026]
设定约束条件，所述约束条件包括安全约束条件和初始约束条件，其中，所述安全约束条件为d
col
《di(t)《d
con
和|βi(t)|《β
con
，所述初始约束条件为和d
con
，β
con
分别表示发生连接中断的距离和角度限制，d
col
表示两个连续机器人之间的最小安全距离，d
i，des
为事先设定的第i台消防机器人与前方第i-1台消防机器人之间的距离；
[0027]
基于所述距离误差动态方程、所述角度误差动态方程和所述约束条件，分别建立所述距离性能函数和所述角度性能函数。
[0028]
进一步地，所述距离性能函数和所述角度性能函数的公式表示如下：
[0029][0030]
式中：cu，ld，l
β
，分别是预定义的正常数；参数cu为由项引起的性能函数的减少，ld、l
β
、分别包含所需的瞬态和稳态性能规范；sw1，sw2，sw
1，2
，swu为切换函数；表示左障碍物与领导者和跟随者的连线的最小距离；表示右障碍物与领导者和跟随者的连线的最小距离；ui表示线速度；表示距离误差的性能函数的下限值；表示距离误差的性能函数的上限值；表示角度误差的性能函数的上限值；表示角度误差的性能函数的下限值；分别对应的连续可微的函数。
[0031]
进一步地，所述切换函数为：
[0032]
swu＝sw(ui，0，δu)，
[0033]
sw1＝sw(λ1+δ
λ
，0，δ
λ
)-sw(λ1，1，δ
λ
)，
[0034]
sw2＝sw(λ2+δ
λ
，0，δ
λ
)-sw(λ2，1，δ
λ
)，
[0035][0036]
式中：δu，δ
λ
和δ
1，2
为预定义的正常数；λ1，λ2分别表示距离为直线和距离为的直线最近点到障碍物的直线参数值分别在右边和左边。
[0037]
进一步地，所述通过距离控制增益，调节所述距离误差与所述距离性能函数之间的差值，得到速度控制信号，包括：
[0038]
根据速度的分布控制协议，控制所述距离误差在所述距离性能函数的边界范围内，公式表示为：
[0039][0040]
式中：表示速度控制信号；表示正控制增益；表示正控制增益；cu表示参数；表示距离误差；。
[0041]
进一步地，所述通过角度控制增益，调节所述角度误差与所述角度性能函数之间的差值，得到转向角控制信号，包括：
[0042]
根据角度的分布控制协议，控制所述角度误差在所述角度性能函数的边界范围内，公式表示为：
[0043][0044]
式中：表示转向角控制信号；表示正控制增益；其中，表示角度误差；分别表示和的一阶导数。
[0045]
进一步地，在所述巡检机器人探测到火源时，所述方法还包括：
[0046]
利用所述巡检机器人对所述火源周围以圆形轨迹进行巡航，并通过自身携带的风
速风向变送器进行风速风向侦察，确定火势蔓延方向及速度；
[0047]
利用所述巡检机器人通过自身搭载的激光雷达对所述火源周边以及所述灭火机器人行进路线周边进行三维地图建设，并为所述三维地图进行三维坐标赋值，确定所述火源在所述三维地图上的坐标位置；
[0048]
控制所述灭火机器人根据所述三维地图及所述火源在所述三维地图上的坐标位置进行路径规划，并沿规划路径运动对所述火源进行扑救。
[0049]
此外，本发明还提出了一种群组式协同消防机器人火场内编组队列行驶控制系统，所述系统包括一个巡检机器人和至少一个灭火机器人，所述巡检机器人和所述灭火机器人组成呈链式结构的队列且所述巡检机器人位于队头，所述巡检机器人和所述灭火机器人均装载有激光雷达、摄像机和分布式控制器，其中：所述分布式式控制器，包括误差计算模块、性能函数建立模块、速度控制模块、角度控制模块和行驶控制模块；
[0050]
所述激光雷达和所述摄像机分别用于获取与其前方距离最近消防机器人的相对距离和相对角度；
[0051]
所述误差计算模块，用于基于所述相对距离和相对角度，计算距离误差和角度误差；
[0052]
所述性能函数建立模块，用于基于约束条件、所述距离误差和所述角度误差，建立距离性能函数和角度性能函数；
[0053]
所述速度控制模块，用于通过距离控制增益，调节所述距离误差与所述距离性能函数之间的差值，得到速度控制信号；
[0054]
所述角度控制模块，用于通过角度控制增益，调节所述角度误差与所述角度性能函数之间的差值，得到转向角控制信号；
[0055]
所述行驶控制模块，用于基于所述速度控制信号和所述转向角控制信号，控制机器人自身行驶。
[0056]
本发明的优点在于：
[0057]
(1)本发明针对链式结构编队跟随任务，设置分布式控制方式，仅仅利用了机器人的局部信息进行交互，在每个车体的局部坐标系下得到前方机器人的距离和角度值以及障碍物的距离，根据机器人距离误差和距离性能函数之间的差值通过距离控制增益调节，使得距离误差能够保证在距离性能函数的上限和下限范围内；同样，根据机器人角度误差和角度性能函数之间的差值通过角度控制增益调节，使得角度误差能够保证在角度性能函数的上限和下限范围内；以得到相应的速度和转向角来控制车体运动和转向，从而保证每台车都能跟踪上前方车辆，无需进行队形变化就能够一直保持队形，能快速到达火灾现场，节省行走时间和能耗。
[0058]
(2)本发明通过设计分布式控制，仅利用较少的信息进行交互，相比于集中式控制运算量更小；对具有通讯约束、安全区域和视线范围均有约束的消防机器人编队设计的分布式控制器，使得距离和角度误差均满足预设暂态(调节时间)和稳态性能(收敛速度、稳态误差)；引入了预设性能控制技术，使得误差能够有较快地收敛速度且始终在一个给定的范围内变动；即通讯保持、碰撞避免和方位角受限的问题都能够同时解决。
[0059]
(3)所设计的性能函数是关于时间指数衰减的函数，保证误差始终在设计的性能函数的边界内。这样的话，距离误差和角度误差的暂态性能和稳态性能就可以被预设。
[0060]
(4)根据不同的场景有针对性的选择不同的切换函数，可以解决实际当中多种行驶场景。
[0061]
本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
[0062]
图1是本发明一实施例中群组式协同消防机器人火场内编组队列行驶控制方法的流程示意图；
[0063]
图2是本发明中消防机器人组成的队列呈链式结构示意图；
[0064]
图3是本发明中群组式消防救援示意图；
[0065]
图4是本发明另一实施例中群组式协同消防机器人火场内编组队列行驶控制系统的结构示意图。
具体实施方式
[0066]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0067]
本发明提出了一种群组式协同消防机器人火场内编组队列行驶控制方法，所述消防机器人包括巡检机器人和灭火机器人，消防机器人组成的队列呈链式结构，且巡检机器人位于队头，灭火机器人跟随在巡检机器人之后，每台消防机器人装载有激光雷达和摄像机，对于每台所述消防机器人，所述方法包括以下步骤：
[0068]
s10、获取与其前方距离最近消防机器人的相对距离和相对角度；
[0069]
需要说明的是，消防机器人i利用车载摄像头检测其前方机器人i-1的相对距离和相对角度，同时利用激光雷达扫描测量障碍区的位置。
[0070]
s20、基于所述相对距离和相对角度，计算距离误差和角度误差；
[0071]
需要说明的是，将实测的相对距离和相对角度与实现设定的期望值进行比较，得到距离误差和角度误差。
[0072]
s30、基于约束条件、所述距离误差和所述角度误差，建立距离性能函数和角度性能函数；
[0073]
s40、通过距离控制增益，调节所述距离误差与所述距离性能函数之间的差值，得到速度控制信号；
[0074]
s50、通过角度控制增益，调节所述角度误差与所述角度性能函数之间的差值，得到转向角控制信号；
[0075]
s60、基于所述速度控制信号和所述转向角控制信号，控制机器人自身行驶。
[0076]
需要说明的是，所述巡检机器人装载差分gps、陀螺仪、激光雷达及摄像机云台，可以单独完成全局地图的导航，用于跟踪一条可行且无障碍的轨迹；所述灭火机器人装载激光雷达及摄像机云台，难以单独实现全局地图导航，在巡检机器人具备全局地图导航的能力下，灭火机器人利用雷达及车载云台有限的资源作为跟随，实现火场中的运动。
[0077]
本实施例针对链式结构编队跟随任务，根据机器人距离误差和距离性能函数之间的差值通过距离控制增益调节，使得距离误差能够保证在距离性能函数的上限和下限范围内；同样，根据机器人角度误差和角度性能函数之间的差值通过角度控制增益调节，使得角度误差能够保证在角度性能函数的上限和下限范围内；通过设计分布式控制，仅利用较少的信息进行交互，相比于集中式控制运算量更小，且无需进行队形变化就能够一直保持队形，能快速到达火灾现场，节省行走时间和能耗。
[0078]
在一实施例中，如图2所示，考虑一个由n台消防机器人组成的一个排列，即构成一个链式结构，后方机器人紧跟随前方机器人向前运动。其中消防机器人的运动学模型如下：
[0079][0080]
式中：对于i＝1，...，n，其中，xi,yi,θi表示第i台机器人的位置和方向；ui,γi,α分别表示其线速度、转向角和长度；分别表示xi,yi,θi的一阶导数。
[0081]
在一实施例中，所述步骤s20中，距离误差和角度误差的计算公式如下：
[0082][0083]
式中：表示距离误差；表示角度误差；di(t)和βi(t)分别表示两台连续消防机器人之间的相对距离和相对角度；d
i,des
表示事先设定的第i台消防机器人与前方第i-1台消防机器人之间的距离。
[0084]
在一实施例中，所述步骤s30：基于约束条件、所述距离误差和所述角度误差，建立距离性能函数和角度性能函数，包括以下步骤：
[0085]
s31、分别对所述距离误差和所述角度误差进行微分处理，分别得到距离误差动态方程和角度误差动态方程；
[0086]
需要说明的是，对距离误差和角度误差的时间进行微分，并将机器人运动学模型代入，得到距离误差动态方程和角度误差动态方程如下：
[0087][0088][0089]
式中：对于i＝1,
…
,n，其中，φi＝θ
i-θ
i-1
。
[0090]
s32、设定约束条件，所述约束条件包括安全约束条件和初始约束条件，其中，所述安全约束条件为d
col
《di(t)《d
con
和|βi(t)|《β
con
，所述初始约束条件为，所述初始约束条件为和d
con
，β
con
分别表示发生连接中断的距离和角度限制，d
col
表示两个连续机器人之间的最小安全距离，d
i,des
为事先设定的第i台消防机器人与前方第i-1台消防机器人之间的距离。
[0091]
需要说明的是，根据安全约束条件，得出结论：我们应该始终保持性能函数的值有界，其关系如下：
[0092][0093]
根据初始约束条件，可得到：
[0094]
和
[0095]
s32、基于所述距离误差动态方程、所述角度误差动态方程和所述约束条件，分别建立所述距离性能函数和所述角度性能函数。
[0096]
需要说明的是，设置误差的性能函数的目的是为了约束误差的范围，使得误差在性能函数的作用范围内渐进收敛。此外，性能函数不应超过某些值，否则视觉连接中断或机器人之间发生碰撞。
[0097]
需要说明的是，如图2所示，假设每台消防机器人可获得唯一可用的反馈信号是：di(t)和βi(t)，分别为由后方机器人通过车载云台相机检测其前方机器人的相对距离和相对角度，以及由激光扫描仪检测和提取障碍物相对于机器人的位置。控制目标是设计一个完全分布的控制协议，使每台机器人都跟踪其前方机器人，即di(t)
→di,des
和βi(t)
→
0，其中，d
i,des
为事先设定的第i台机器人与前方第i-1台机器人之间的距离。此外，应避免机器人之间发生碰撞和视野连接中断。因此，可用d
con
，β
con
分别表示发生连接中断的距离和角度限制，用d
col
表示两个连续机器人之间的最小安全距离，另一个控制目标是将di(t)和βi(t)限制在d
col
《di(t)《d
con
和|βi(t)|《β
con
。最后，每台机器人都应避免与其路径的任何静态障碍物发生碰撞，同时将其前方机器人保持在其相机的视野内。
[0098]
假设该排的领头巡检机器人执行一条可行且无障碍的轨迹，其线速度和转向角为有界连续函数。特别是假设前方机器人转向角的绝对值小于π/2rad的合理值。此外，假设和β
con
》π/4，这符合典型相机的标准规格，其中，γ
max
是最大转向角，w表示车辆宽度，α为机器人长度。也很容易验证这些是跟随者在执行最小转弯半径时能够跟踪其前方机器人的最低要求。示例：其中，一辆普通机器人的α＝3m,w＝2m和γ
max
＝0.183π。因此，要求d
con
》7.97m。
[0099]
在一实施例中，所述步骤s32中所建立的距离性能函数和角度性能函数如下：
[0100][0101]
式中：cu,ld,l
β
,分别是预定义的
正常数；参数cu为由项引起的性能函数的减少，ld、l
β
、分别包含所需的瞬态和稳态性能规范；sw1，sw2，sw
1,2
，swu为切换函数；表示左障碍物与领导者和跟随者的连线的最小距离；表示右障碍物与领导者和跟随者的连线的最小距离；ui表示线速度；表示距离误差的性能函数的下限值；表示距离误差的性能函数的上限值；表示角度误差的性能函数的上限值；表示角度误差的性能函数的下限值；分别对应的连续可微的函数。
[0102]
需要说明的是，设计的性能函数是关于时间指数衰减的函数，保证误差始终在设计的性能函数的边界内。这样的话，距离误差和角度误差的暂态性能和稳态性能就可以被预设。性能函数的作用是为了能保证距离误差和角度误差能在一个有界范围内收敛，也就是对误差范围进行约束，迫使误差最后能收敛到有界范围。
[0103]
进一步地，此外，为了定义切换函数swi，引入函数其中，如果x≤0；ii)如果x》0。因此，sw(x,∈,δ)是一个c1切换函数，对于所有x∈(-∞,∈]等于0，对于所有x∈[∈+δ,+∞)等于1，并且它是一条递增、连续且可微的曲线，对于x∈(∈,∈+δ)，取值从0到1。
[0104]
本实施例中，将性能函数中的切换项定义为：
[0105]
swu＝sw(ui，o，δu)，
[0106]
sw
l
＝sw(λ1+δ
λ
，0，δ
λ
)-sw(λ1，1，δ
λ
)，
[0107]
sw2＝sw(λ2+δ
λ
，0，δ
λ
)-sw(λ2，1，δ
λ
)，
[0108][0109]
其中，δu,δ
λ
和δ
1,2
都很小预定义的正常数；表示左右障碍物与领导者和跟随者的连接直线的最小距离，λ1,λ2表示上述直线最近点到障碍物的直线参数值分别在右边和左边。因此，如果λi∈(0,1)，那么最近点位于跟随者和领导者之间。换言之，相应的障碍物可能会在它们之间产生干扰。因此，本实施例使用λi来确定障碍物是否会导致可能的碰撞或连接中断。
[0110]
在消防机器人队列行驶过程中，通常，将性能函数驱近于零，以保持相应的误差接近原点。但是，在某些情况下，必须改变误差以达到安全规范(即避免碰撞和连接维护)。
[0111]
案例(1)：当机器人速度为零(u＝0)时，由于模型的非完整约束，它不能执行转弯。例如，在灭火现场，这可能发生在机器人(i-1)围绕其后方机器人执行半径为d
i,des
的圆形轨迹时。然后，后方机器人为了与前方机器人保持所需的距离，后方的跟随者需要停下来(u＝0)。但是，由于无法转弯，因此无法跟踪前方机器人。因此，当u
→
0时，控制协议应强制后方机器人增加其速度。采取的方法是通过将距离误差的两个性能函数驱动到零以下，这导致换言之，后面的机器人朝向它的前面的机器人移动。
[0112]
案例(2)：当障碍物处于两台连续机器人之间时，需要将航向误差的性能函数偏离
零。特别是，如果障碍物干扰了两台连续机器人，则可能会发生连接中断和/或与障碍物发生碰撞。因此，后车必须避开障碍物，同时需要跟踪其前车以保持连通性。在这种情况下，选择偏离航向误差的性能函数。例如，考虑在领导者和追随者的右侧都有障碍物的情况，该障碍物往往会干扰他们。在这种情况下，我们选择减小性能函数的值，最终导致βi《0。因此，后面的机器人执行左转并远离障碍物，从而在保持与领先机器人的视觉连接，同时避开其它机器人。或者，如果左侧出现障碍物，选择增加相应性能函数的值，从而允许跟随者进行右转。这样，既不会发生连接中断，也不会发生碰撞。
[0113]
案例(3)：第三种情况可能出现在同时存在两个障碍物的情况下，一个在后面的机器人的左侧，一个在其右侧，这时必须修改性能函数。例如，后面的车辆可能需要同时左转以保持与前方机器人的连接，因为右侧有障碍物，同时右转以避免由于右侧障碍物而发生碰撞左边。请注意，在这种情况下，案例b中提出的解决方案会导致死锁。因此，为了解决这类问题，这里选择了我案例(1)提出的方法。这样，追随者就接近了领导者，并远离了这个矛盾事件发生的位置。
[0114]
此外，性能函数不应超过某些值，以防止视觉连接中断或车辆之间发生碰撞。
[0115]
具体到本实施例中，在自动灭火场景下，首先前后两消防机器人的相对距离和相对角度误是通过相机测得，与障碍物之间距离通过激光扫描仪得到，同时对3种特殊案例进行了考虑；然后，利用了利普希茨连续投影算子得到性能函数的动态方程(性能函数微分形式，显示性能函数变化趋势)，进一步得到预设的性能函数。再通过切换函数选择相应的函数项可以解决上述3种案例问题，切换函数用于修改性能函数的正常指数行为，进一步来说：
[0116]
(1)项仅在ui《δu并且随着ui→
0变为-∞时有效，因为1-swu在ui从δu变为0时平滑地从0变为1。换句话说，如ui减小，此项会导致距离误差的性能函数减小并降至零以下。因此，追随者接近领导者，从而适用案例(1)的情况。
[0117]
(2)项仅在λ1∈[0,1]时有效，即当右侧的障碍物干扰跟随者和领导者之间时，因为sw1＝1对于λ1∈[0,1]，并迅速下降，当λ1从0变为-δ
λ
或从1变为1+δ
λ
时变为0。当连接领导者和跟随者的直线接近障碍物时，碰撞和/或连接中断的危险越来越大。因此，该项变为-∞，导致航向误差的性能函数减小，并且后面的消防机器人执行左转。类似地，术语适用于左侧障碍物的情况。
[0118]
(3)项仅当上述两项几乎相反时才有效，这是由于1-sw
1,2
的行为。当后方机器人遇到矛盾事件并导致距离误差的性能函数减小并降至零以下时，它对应于案例(3)情况。因此，追随者接近领导者，实现了我们之前提出的方法。
[0119]
需要说明的是，由于本实施例引入的切换函数是平滑切换，即使性能函数的正常行为由于上述情况下的切换而被修改，性能函数仍然是连续可微的函数。
[0120]
需要说明的是，火灾现场的消防机器人具有以下特点：1)高负载，其自重几百公斤，考虑拖拽水带和水炮喷射需求，其负载能力几百公斤；2)机动性能高，消防对时间要求严格，需要消防机器人具有高速移动特点；3)适用全地形，采用履带结构，适应于马路、室
内、泥泞道路，石子路面，台阶路面各种地形；4)爬坡和越障能力高，45度斜坡和20厘米障碍物；5)需具有自动避障、寻找火源、采集数据、图像传输、双向语音、摄像实时回传的功能。可以通过消防机器人传输的数据确定火源位置，了解火灾现场，制定合适的营救方案。
[0121]
且消防机器人在仿真模型中可以用带方向的质点表示其运动轨迹和实时位置，但是在避障时需要考虑机器人的实际尺寸和机器人之间安全距离等因素。如果需要更加符合实际情况，模型可以按照1:1尺寸进行仿真。
[0122]
另外，火灾现场具有高动态非结构特征，浓烟环境和障碍物会阻碍机器人之间通讯和视野，尤其考虑水带拖拽过程中，防止水带和水带之间、水带和障碍物之间发生交叉缠绕问题。另外，水带充水状态其负载很大，对机器人的运动控制和稳定性影响很大，而传统的编队协同控制算法都未考虑这些因素，需针对特定条件和因素采取对应的协同控制算法。
[0123]
本实施例考虑火灾现场的环境因素更全面，并通过设置切换函数用于修改性能函数的正常指数行为，用于修订履带机器人，地面多路况形式的通过及避障问题，以及水带牵引的问题，以使消防机器人快速到达火灾现场，及时遏制火情。
[0124]
在一实施例中，所述步骤s40：通过距离控制增益，调节所述距离误差与所述距离性能函数之间的差值，得到速度控制信号，包括以下步骤：
[0125]
根据速度的分布控制协议，控制所述距离误差在所述距离性能函数的边界范围内，公式表示为：
[0126][0127]
式中：表示速度控制信号；表示正控制增益；表示正控制增益；；cu表示参数；表示距离误差。
[0128]
在一实施例中，所述步骤s50：通过角度控制增益，调节所述角度误差与所述角度性能函数之间的差值，得到转向角控制信号，包括以下步骤：
[0129]
根据角度的分布控制协议，控制所述角度误差在所述角度性能函数的边界范围内，公式表示为：
[0130][0131]
式中：表示转向角控制信号；表示正控制增益；
其中，表示角度误差；分别表示和的一阶导数。
[0132]
本实施例设计的分布式控制协议，包括速度控制和角度控制，速度控制是根据机器人距离误差和距离性能函数之间的差值通过距离控制增益调节，使得距离误差能够保证在距离性能函数的上限和下限范围内。同样，转向角控制是根据机器人角度误差和角度性能函数之间的差值通过角度控制增益调节，使得角度误差能够保证在角度性能函数的上限和下限范围内，同时引入切换函数来对3种特殊案例进行控制器修改，满足实际场景需要。
[0133]
需要说明的是，系统期望的瞬态和稳态性能以及避免碰撞和连接性保持规范仅通过选择正确的性能函数来实现，简化了控制增益的选择。
[0134]
特别地，控制增益和的选择会影响相应性能函数包络在内的误差和的范围以及控制输入的特性(例如，降低增益值会导致增加在规定的性能范围内的振荡行为，当采用更高的值时会得到改善，但是会加大控制器执行的幅度和速率)。因此，在实时场景中可能需要额外的微调，以将所需的控制输入信号保持在执行器实现的可行范围内。
[0135]
由于控制增益和取值会影响距离误差和方位角误差的收敛速度，以及控制器的输入，一般在仿真实验中可以通过固定一个参数，从小到大调节另外一个参数，观察性能曲线变化情况，以此选择合适的参数。而在实际应用中，影响因素较为多，比如消防机器人本体机械结构、扰动、环境因素等都会影响机器人运动情况，从而无法和仿真实验完全匹配，但是参数变化对性能的影响效果大致相同，只能不断地调试参数，从而找到较为合适的一对参数。
[0136]
在一实施例中，消防机器人在仿真模型中可以用带方向的质点表示其运动轨迹和实时位置，但是在避障时需要考虑机器人的实际尺寸和机器人之间安全距离等因素。如果需要更加符合实际情况，模型可以按照1:1尺寸进行仿真。
[0137]
在一实施例中，如图3所示，在所述巡检机器人探测到火源时，所述方法还包括：
[0138]
利用所述巡检机器人对所述火源周围以圆形轨迹进行巡航，并通过自身携带的风速风向变送器进行风速风向侦察，确定火势蔓延方向及速度；
[0139]
利用所述巡检机器人通过自身搭载的激光雷达对所述火源周边以及所述灭火机器人行进路线周边进行三维地图建设，并为所述三维地图进行三维坐标赋值，确定所述火源在所述三维地图上的坐标位置；
[0140]
控制所述灭火机器人根据所述三维地图及所述火源在所述三维地图上的坐标位置进行路径规划，并沿规划路径运动对所述火源进行扑救。
[0141]
需要说明的是，群组机器人具有典型分布式系统的特征，通过有限个能力局限的机器人交互、协调等动作来完成复杂的任务，协调等动作来完成复杂的任务。与传统智能机器人相比，群组机器人在灵活性、成本控制、稳定性等方面有着绝对的优势。群组机器人其成本优势在于，机器人进行分工协作，从而将各个功能模块分配到群组中各个机器人中。具体而言：群组式消防救援机器人一般配备：一个巡检机器人、两台灭火机器人、一台排烟机器人。
[0142]
在发生火灾后，群组式消防机器人接收到报警信号；需要同时出动不小于2台灭火机器人，机器人种类包含：灭火机器人、排烟机器人及灭火机器人。机器人在向火源接近，自动行驶过程中，形成一个作战分队。
[0143]
本实施例中，针对灾现场消防机器人特点：1)高负载，其自重几百公斤，考虑拖拽水带和水炮喷射需求，其负载能力几百公斤；2)机动性能高，消防对时间要求严格，需要消防机器人具有高速移动特点；3)适用全地形，采用履带结构，适应于马路、室内、泥泞道路，石子路面，台阶路面各种地形；4)爬坡和越障能力高，45度斜坡和20厘米障碍物；5)具有自动避障、寻找火源、采集数据、图像传输、双向语音、摄像实时回传的功能。可以通过消防机器人传输的数据确定火源位置，了解火灾现场，制定合适的营救方案。
[0144]
针对火灾现场具有高动态非结构特征，浓烟环境和障碍物会阻碍机器人之间通讯和视野，尤其考虑水带拖拽过程中，防止水带和水带之间、水带和障碍物之间发生交叉缠绕问题。另外，水带充水状态其负载很大，对机器人的运动控制和稳定性影响很大，而传统的编队协同控制算法都未考虑这些因素。
[0145]
本实施例设计的分布式控制协议主要是针对链式结构编队，不需要进行队形变化就能够一直保持队形，组成链状结构能快速到达火灾现场，节省行走时间和能耗。设计的分布式控制协议仅利用较少的信息进行交互，相比于集中式控制运算量更小；对具有通讯约束、安全区域和视线范围均有约束的消防机器人编队设计的分布式控制器，使得距离和角度误差均满足预设暂态(调节时间)和稳态性能(收敛速度、稳态误差)。简言之，通讯保持、碰撞避免和方位角受限的问题都能够同时解决。
[0146]
此外，如图4所示，本发明第二实施例提出了一种群组式协同消防机器人火场内编组队列行驶控制系统，所述系统包括一个巡检机器人和至少一个灭火机器人，所述巡检机器人和所述灭火机器人组成呈链式结构的队列且所述巡检机器人位于队头，所述巡检机器人和所述灭火机器人均装载有激光雷达、摄像机和分布式控制器，其中：所述分布式式控制器，包括误差计算模块10、性能函数建立模块20、速度控制模块30、角度控制模块40和行驶控制模块50；
[0147]
所述激光雷达和所述摄像机分别用于获取与其前方距离最近消防机器人的相对距离和相对角度；
[0148]
所述误差计算模块10，用于基于所述相对距离和相对角度，计算距离误差和角度误差；
[0149]
所述性能函数建立模块20，用于基于约束条件、所述距离误差和所述角度误差，建立距离性能函数和角度性能函数；
[0150]
所述速度控制模块30，用于通过距离控制增益，调节所述距离误差与所述距离性能函数之间的差值，得到速度控制信号；
[0151]
所述角度控制模块40，用于通过角度控制增益，调节所述角度误差与所述角度性能函数之间的差值，得到转向角控制信号；
[0152]
所述行驶控制模块50，用于基于所述速度控制信号和所述转向角控制信号，控制机器人自身行驶。
[0153]
在一实施例中，所述性能函数建立模块建立的距离性能函数和所述角度性能函数的公式表示如下：
[0154][0155]
式中：cu，ld，l
β
，分别是预定义的正常数；参数cu为由项引起的性能函数的减少，ld、l
β
、分别包含所需的瞬态和稳态性能规范；sw1，sw2，sw
1，2
，swu为切换函数；表示左障碍物与领导者和跟随者的连线的最小距离；表示右障碍物与领导者和跟随者的连线的最小距离；ui表示线速度；表示距离误差的性能函数的下限值；表示距离误差的性能函数的上限值；表示角度误差的性能函数的上限值；表示角度误差的性能函数的下限值；分别对应的连续可微的函数。
[0156]
在一实施例中，所述切换函数为：
[0157]
swu＝sw(ui，0，δu)，
[0158]
sw1＝sw(λ1+δ
λ
，0，δ
λ
)-sw(λ1，1，δ
λ
)，
[0159]
sw2＝sw(λ2+δ
λ
，0，δ
λ
)-sw(λ2，1，δ
λ
)，
[0160][0161]
式中：δu，δ
λ
和δ
1，2
为预定义的正常数；λ1，λ2分别表示距离为直线和距离为的直线最近点到障碍物的直线参数值分别在右边和左边。
[0162]
在一实施例中，所述速度控制模块采用的分布式控协议为：
[0163][0164]
式中：表示速度控制信号；表示正控制增益；cu
表示参数；表示距离误差。
[0165]
在一实施例中，所述角度控制模块采用的分布式控制协议为：
[0166][0167]
式中：表示转向角控制信号；表示正控制增益；其中，表示角度误差；分别表示和的一阶导数。
[0168]
需要说明的是，本发明所述群组式协同消防机器人火场内编组队列行驶控制系统的其他实施例或具有实现方法可参照上述各方法实施例，此处不再赘余。
[0169]
需要说明的是，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(ram)，只读存储器(rom)，可擦除可编辑只读存储器(eprom或闪速存储器)，光纤装置，以及便携式光盘只读存储器(cdrom)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
[0170]
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(pga)，现场可编程门阵列(fpga)等。
[0171]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不
一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0172]
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
[0173]
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。