CN112717308A - 单主多从的消防机器人多模式灭火方法 - Google Patents

Abstract

本发明提供了一种单主多从的消防机器人多模式灭火方法，该方法包括：操作者控制主机器人到达目标位置，获取主机器人和从机器人位姿并计算位姿差值，针对片状着火区域的平行式灭火模式和点状着火区域的集中式灭火模式，计算从机器人水炮的期望角度，考虑因地面不平导致机器人底座出现的横滚角，计算水炮的俯仰角、偏航角需补偿角度，结合消防机器人水炮偏航角与俯仰角的阈值，获取水炮控制的角度约束，计算水炮从当前角度到期望角度的位移，进而主机器人和从机器人协作灭火。本发明借助多台消防机器人互相协作灭火，提升了灭火效率，实用性大大增强，更大程度上减小了操作者的压力。

Description

单主多从的消防机器人多模式灭火方法

技术领域

本发明属于消防技术领域，特别涉及一种单主多从的消防机器人多模式灭火方法，即涉及一种单个主机器人和多个从机器人的消防机器人多模式灭火方法。

背景技术

近年来多市化工厂、工业园区、港口等高危地区发生火灾、爆炸事件，给人民群众带来了巨大的损失；同时，操作者在抢险救灾中，常常伴有高风险，亟需更加先进智能的消防设备投入消防作业中。而现有的消防机器人由于智能化程度较低、适应场景较为有限、工作效率较低，无法很好地满足复杂多样的救灾环境需求，因此当前消防任务中，机器人的参与十分有限。

目前，消防救援任务中，配备使用的消防机器人大多为单一式控制，机器人不具有智能化，虽然在一定程度上减轻了救援压力，但救援模式依旧为一个操作者控制一个消防机器人，没有提高消防救援效率，没有充分发挥出消防机器人的真正价值。专利号201910485607.6提出了侦查机器人与消防机器人协同灭火方法，提高了消防机器人在火场环境的稳定性；专利号201910534840.9和专利号201910485349.1提出了一种无人机引导的消防机器人灭火方法，提高了消防机器人的智能性。但目前消防机器人在具体的灭火实施方案中仍存在欠缺，因此寻求一种用于多个消防机器人的兼顾协同控制和具体实施方案的单主多从多模式灭火方法是十分迫切且必要的。

发明内容

本发明针对上述现有技术中的缺陷，综合考虑，提出一种单主多从的消防机器人多模式灭火方法。该方法包括操作者控制主机器人到达目标位置，获取主机器人和从机器人位姿并计算位姿差值，针对片状着火区域的平行式灭火模式和点状着火区域的集中式灭火模式，计算从机器人水炮的期望角度，考虑因地面不平导致机器人底座出现的横滚角，计算水炮的俯仰角、偏航角需补偿角度，结合消防机器人水炮偏航角与俯仰角的阈值，获取水炮控制的角度约束，计算水炮从当前角度到期望角度的位移，进而主机器人和从机器人协作灭火。本发明借助多台消防机器人互相协作灭火，提升了灭火效率，实用性大大增强，更大程度上减小了操作者的压力。

本发明提供一种单主多从的消防机器人多模式灭火方法，所述消防机器人包括主机器人和从机器人构成的多机器人组，所述方法包括以下步骤：

S1、获取主机器人和从机器人位姿并计算位姿差值：操作者控制主机器人到达目标位置，根据消防机器人所携带传感器传输主从机器人和其水炮的绝对位姿数据，计算出主机器人和从机器人之间的位姿差值：

S2、计算从机器人水炮的期望角度：通过机器人上立体视觉摄像头获取火场着火点情况，并通过mash组网将图像数据传输回后台控制器，操作者通过控制器中视频窗口显示的视频信息，判断机器人的具体灭火模式，若为片状着火区域的平行式灭火模式则执行步骤S3，若为点状着火区域的集中式灭火模式则执行步骤S4；

S3、片状着火区域的平行式灭火模式下，仅考虑俯仰角的角度变化，从机器人水炮的期望角度为：

其中：θ_wm、Ψ_wm、Φ_wm分别表示主机器人水炮的俯仰角、偏航角、横滚角；

分别表示第i辆从机器人水炮的期望俯仰角、期望偏航角、期望横滚角；Δθ′_i表示第i个从机器人位置对其俯仰角造成的影响，且

(x_o，y_o，z_o)表示着火点坐标，l_i表示第i个从机器人到着火点的直线距离；

S4、点状着火区域的集中式灭火模式下，只考虑偏航角与俯仰角的角度变化，从机器人水炮的期望角度为：

其中：Δθ′_i表示第i个从机器人位置对其偏航角造成的影响；ΔΨ′_i表示第i个从机器人位置对其偏航角造成的影响，

S5、计算水炮的俯仰角、偏航角需补偿角度：考虑因地面不平导致机器人底座出现的横滚角，此时机器人绕y轴滚转，水炮与y轴构成一个圆锥面，分别将滚转前后的水炮向xoy平面和yoz平面投影，投影后的夹角即为横滚角向俯仰角、偏航角方向的投影，由此获得水炮的俯仰角、偏航角需补偿的角度为：

其中：Δθ″_i、ΔΨ″_i分别为第i个从机器人横滚角投影在俯仰角、偏航角上的角度；d_l表示水炮底座到炮筒构成的圆锥体底面的垂直距离且d_l＝l_wcosθ_wm；d_ri表示滚转后炮筒在xoy平面的投影到y轴的垂直距离且d_ri＝l_wsinθ_wnsinΔΦ_i；d′_ri表示滚转后炮筒在yoz平面的投影到y轴的垂直距离且d′_ri＝l_wsinθ_wmcosΔΦ_i，l_w表示机器人水炮长度，ΔΦ_i表示第i辆从机器人与主机器人横滚角的差值；

S6、计算水炮控制的角度约束：根据所得主机器人和从机器人的位姿差、水炮的俯仰角和偏航角需补偿的角度，结合消防机器人水炮偏航角与俯仰角的阈值，获得水炮控制的角度约束；

S7、计算水炮从当前角度到期望角度的位移：若水炮角度满足角度约束，则判断机器人的具体灭火模式，片状着火区域的平行式灭火模式执行步骤S71，点状着火区域的集中式灭火模式执行步骤S72，分别计算出水炮俯仰角、偏航角需要转动的角度，调整水炮位姿到期望角度；若水炮角度超过阈值时，调整机器人方向，使差值降至阈值以下，重复步骤S7；

S8、主机器人和从机器人协作灭火：由控制器发送水炮位移指令，调整水炮位姿到期望角度，并根据主机器人和从机器人所携带的深度相机，获得主机器人和从机器人与火源的距离，并在灭火过程中自动调整主机器人和从机器人位置，以保证主机器人和从机器人距着火点的距离一致。

优选的，所述步骤S1具体包括以下步骤：

S11、根据消防机器人所携带传感器传输的机器人和其水炮的绝对坐标数据，计算出主机器人和从机器人之间的坐标差值：

其中：(x_m，y_m，z_m)表示主机器人的坐标；(x_i，y_i，z_i)表示第i辆从机器人的坐标；(Δx_i，Δy_i，Δz_i)表示第i辆从机器人与主机器人坐标的差值，i＝1，2，...，n；

S12、根据消防机器人所携带传感器传输的机器人和其水炮的绝对姿态角数据，计算出主机器人和从机器人之间的姿态角差值：

其中：(θ_m、Ψ_m、Φ_m)表示主机器人的俯仰角、偏航角、横滚角；(θ_i、Ψ_i、Φ_i)表示第i辆从机器人的俯仰角、偏航角、横滚角；(Δθ_i、ΔΨ_i、ΔΦ_i)表示第i辆从机器人与主机器人俯仰角、偏航角、横滚角的差值。

进一步，所述步骤S6具体包括以下步骤：

S61、判断机器人的具体灭火模式，若为片状着火区域的平行式灭火模式则执行步骤S62，若为点状着火区域的集中式灭火模式则执行步骤S63；

S62、片状着火区域的平行式灭火模式下，水炮控制的角度约束为：

其中：Δθ_rwi表示从机器人的俯仰角与从机器人水炮的期望俯仰角的差值；ΔΨ_rwi表示从机器人的偏航角与从机器人水炮的期望偏航角的差值；

S63、点状着火区域的集中式灭火模式下，水炮控制的角度约束为：

进一步，所述步骤S7具体包括以下步骤：

S71、片状着火区域的平行式灭火模式下，水炮需移动的角度为：

其中：Δθ_wi和ΔΨ_wi分别表示第i个从机器人水炮需要移动的俯仰角、偏航角的角度；θ_wi和Ψ_wi分别表示测量到的第i个从机器人水炮的俯仰角、偏航角角度；

S72、点状着火区域的集中式灭火模式下，水炮需移动的角度为：

进一步，所述步骤S1还包括选择编码器测量机器人水炮偏航角、俯仰角的角位移。

进一步，所述步骤S1中使用RTK差分GPS实现主机器人和从机器人的定位，获取主机器人和从机器人的坐标、俯仰角、偏航角和横滚角。

进一步，所述步骤S2中假设通过SALM算法已获取到着火点的具体坐标。

与现有技术相比，本发明的技术效果为：

1、本发明通过主机器人和从机器人协同控制，一个操作者可以同时控制多台机器人，可节约人力资源，降低操作者伤亡率；针对火场环境，提出片状着火区域的平行式灭火模式和点状着火区域的集中式灭火模式两种不同的灭火模式，使消防作业更有针对性。

2、本发明设计的一种单主多从的消防机器人多模式灭火方法，多台消防机器人互相协作灭火，提升了灭火效率、提高了消防机器人的智能性，实用性大大增强，并更大程度上减小了操作者的压力。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显。

图1是本发明的单主多从的消防机器人多模式灭火方法的流程图；

图2是本发明的片状着火区域的平行式灭火模式侧视图；

图3是本发明的片状着火区域的平行式灭火模式俯视图；

图4是本发明的点状着火区域的集中式灭火模式侧视图；

图5是本发明的点状着火区域的集中式灭火模式俯视图；

图6是本发明的横滚角在俯仰角、偏航角方向的投影变化图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

图1示出了本发明的单主多从的消防机器人多模式灭火方法，该方法包括以下步骤：

S1、获取主机器人和从机器人位姿并计算位姿差值：操作者控制主机器人到达目标位置，根据消防机器人所携带传感器传输的机器人和其水炮的绝对位姿数据，计算出主机器人和从机器人之间的位姿差值。

S11、根据消防机器人所携带传感器传输主从机器人和其水炮的绝对坐标数据，计算出主机器人和从机器人之间的坐标差值：

其中：(x_m，y_m，z_m)表示主机器人的坐标；(x_i，y_i，z_i)表示第i辆从机器人的坐标；(Δx_i，Δy_i，Δz_i)表示第i辆从机器人与主机器人坐标的差值，i＝1，2，...，n。

S12、根据消防机器人所携带传感器传输的机器人和其水炮的绝对姿态角数据，计算出主机器人和从机器人之间的姿态角差值：

其中：(θ_m、Ψ_m、Φ_m)表示主机器人的俯仰角、偏航角、横滚角；(θ_i、Ψ_i、Φ_i)表示第i辆从机器人的俯仰角、偏航角、横滚角；(Δθ_i、ΔΨ_i、ΔΦ_i)表示第i辆从机器人与主机器人俯仰角、偏航角、横滚角的差值。

选择编码器测量机器人水炮偏航角、俯仰角的角位移；使用RTK差分GPS实现主机器人和从机器人的定位，获取主机器人和从机器人的坐标、俯仰角、偏航角和横滚角。

S2、计算从机器人水炮的期望角度：通过机器人上立体视觉摄像头获取火场着火点情况，并通过mash组网将图像数据传输回后台控制器，操作者通过控制器中视频窗口显示的视频信息，判断机器人的具体灭火模式，若为片状着火区域的平行式灭火模式则执行步骤S3，若为点状着火区域的集中式灭火模式则执行步骤S4。

S3、片状着火区域的平行式灭火模式下，仅考虑俯仰角的角度变化，从机器人水炮的期望角度为：

其中：θ_wm、Ψ_wm、Φ_wm分别表示主机器人水炮的俯仰角、偏航角、横滚角；

分别表示第i辆从机器人水炮的期望俯仰角、期望偏航角、期望横滚角；Δθ′_i表示第i个从机器人位置对其俯仰角造成的影响，且

(x_o，y_o，z_o)表示着火点坐标，l_i表示第i个从机器人到着火点的直线距离。

S4、点状着火区域的集中式灭火模式下，只考虑偏航角与俯仰角的角度变化，从机器人水炮的期望角度为：

其中：Δθ′_i表示第i个从机器人位置对其偏航角造成的影响；ΔΨ′_i表示第i个从机器人位置对其偏航角造成的影响，

S5、计算水炮的俯仰角、偏航角需补偿角度：考虑因地面不平导致机器人底座出现的横滚角，此时机器人绕y轴滚转，水炮与y轴构成一个圆锥面，分别将滚转前后的水炮向xoy平面和yoz平面投影，投影后的夹角即为横滚角向俯仰角、偏航角方向的投影，由此获得水炮的俯仰角、偏航角需补偿的角度为：

其中：Δθ″_i、ΔΨ″_i分别为第i个从机器人横滚角投影在俯仰角、偏航角上的角度；d_l表示水炮底座到炮筒构成的圆锥体底面的垂直距离且d_l＝l_wcosθ_wm；d_ri表示滚转后炮筒在xoy平面的投影到y轴的垂直距离且d_ri＝l_wsinθ_wmsinΔΦ_i；d′_ri表示滚转后炮筒在yoz平面的投影到y轴的垂直距离且d′_ri＝l_wsinθ_wmcosΔΦ_i，l_w表示机器人水炮长度，ΔΦ_i表示第i辆从机器人与主机器人横滚角的差值。

S6、计算水炮控制的角度约束：根据所得主机器人和从机器人的位姿差、水炮的俯仰角和偏航角需补偿的角度，结合消防机器人水炮偏航角与俯仰角的阈值，获得水炮控制的角度约束。

S61、判断机器人的具体灭火模式，若为片状着火区域的平行式灭火模式则执行步骤S62，若为点状着火区域的集中式灭火模式则执行步骤S63。

S62、片状着火区域的平行式灭火模式下，水炮控制的角度约束为：

其中：Δθ_rwi表示从机器人的俯仰角与从机器人水炮的期望俯仰角的差值；ΔΨ_rwi表示从机器人的偏航角与从机器人水炮的期望偏航角的差值。

S63、点状着火区域的集中式灭火模式下，水炮控制的角度约束为：

S7、计算水炮从当前角度到期望角度的位移：若水炮角度满足角度约束，则判断机器人的具体灭火模式，片状着火区域的平行式灭火模式执行步骤S71，点状着火区域的集中式灭火模式执行步骤S72，分别计算出水炮俯仰角、偏航角需要转动的角度；若水炮角度超过阈值时，调整机器人方向，使差值降至阈值以下，重复步骤S7。

S71、片状着火区域的平行式灭火模式下，水炮需移动的角度为：

其中：Δθ_wi和ΔΨ_wi分别表示第i个从机器人水炮需要移动的俯仰角、偏航角的角度；θ_wi和Ψ_wi分别表示测量到的第i个从机器人水炮的俯仰角、偏航角角度。

S72、点状着火区域的集中式灭火模式下，水炮需移动的角度为：

S8、主机器人和从机器人协作灭火：由控制器发送水炮位移指令，调整水炮位姿到期望角度，并根据主机器人和从机器人所携带的深度相机，获得主机器人和从机器人与火源的距离，并在灭火过程中自动调整主机器人和从机器人位置，以保证主机器人和从机器人距着火点的距离一致。

下面结合具体的案例对本发明做进一步的详细说明。

选取中信重工开诚智能消防机器人作为实施例机器人主体，搭载思岚A1激光雷达、Kinect2深度相机、立体视觉相机，以主机器人1与从机器人2、从机器人3、从机器人4为例，实现一主机、三从机的协作灭火，同时假定已通过SLAM算法获取到着火点5的坐标，进行具体实施方案说明，具体步骤如下：

S1、计算主机器人和从机器人位姿差值：根据消防机器人所携带传感器传输主从机器人和其水炮的绝对位姿数据，计算出主机器人和从机器人之间的位姿差值。

S11、本发明通过RTK差分GPS获取主从机器人高精度坐标与姿态角，并在后台控制器中计算出主从机器人的坐标差和姿态角差值，则机器人坐标为：

其中：(x_m，y_m，z_m)表示主机器人的坐标；(x_i，y_i，z_i)表示第i辆从机器人的坐标；(Δx_i，Δy_i，Δz_i)表示第i辆从机器人与主机器人坐标的差值，i＝1，2，3。

S12、机器人姿态角误差为：

其中：(θ_m、Ψ_m、Φ_m)表示主机器人的俯仰角、偏航角、横滚角；(θ_i、Ψ_i、Φ_i)表示第i辆从机器人的俯仰角、偏航角、横滚角；(Δθ_i、ΔΨ_i、ΔΦ_i)表示第i辆从机器人与主机器人俯仰角、偏航角、横滚角的差值。

S2、计算从机器人水炮的期望角度：通过机器人上立体视觉摄像头获取火场着火点情况，并通过mash组网将图像数据传输回后台控制器，操作者通过控制器中视频窗口显示的视频信息，判断机器人的具体灭火模式，根据两种模式的不同特征，分别计算水炮期望姿态角的表达式，若为片状着火区域的平行式灭火模式则执行步骤S3，若为点状着火区域的集中式灭火模式则执行步骤S4。

S3、片状着火区域的平行式灭火模式下，如图2所示，机器人位置变化会影响俯仰角，进而影响灭火作业，又如图3所示机器人位置变化不影响偏航角、横滚角，因此只考虑俯仰角的角度变化，通过图2可计算出从机器人水炮的期望角度为：

其中：θ_wm、Ψ_wm、Φ_wm分别表示主机器人水炮的俯仰角、偏航角、横滚角；

分别表示第i辆从机器人水炮的期望俯仰角、期望偏航角、期望横滚角；Δθ′_i表示第i个从机器人位置对其俯仰角造成的影响。

如图2所示，在xoz平面中，求出主机器人坐标到火焰坐标的直线方程为：

通过点到直线的距离，求出主机器人到着火点的直线距离d_θi的表达式为：

其中：(x_o，y_o，z_o)表示着火点坐标。

则满足

其中：l_i表示第i个从机器人到着火点的直线距离。

S4、点状着火区域的集中式灭火模式下，如图4、图5所示，在集中式灭火中，机器人位置的变化影响俯仰角、偏航角，并不影响横滚角，因此只考虑偏航角与俯仰角的角度变化，偏航角角度变化如图3所示，从机器人水炮的期望角度为：

其中：Δθ′_i表示第i个从机器人位置对其偏航角造成的影响；ΔΨ′_i表示第i个从机器人位置对其偏航角造成的影响。

如图4所示，在xoy平面中，求出主机器人坐标到火焰坐标的直线方程为：

通过点到直线的距离，求出主机器人到着火点的直线距离d_Ψi的表达式为：

则满足

S5、计算水炮的俯仰角、偏航角需补偿的角度：现有机器人水炮大多只有俯仰、偏航两自由度，因此当地面不平导致机器人底座出现横滚角时，需要通过调整俯仰角、偏航角来补偿横滚角造成的姿态差异。

如图6所示，当机器人绕y轴滚转时，水炮与y轴构成一个圆锥面，分别将滚转前后的水炮向xoy平面和yoz平面投影，投影后的夹角即为横滚角向俯仰角、偏航角方向的投影，由此获得水炮的俯仰角、偏航角需补偿的角度为：

其中：Δθ″_i、ΔΨ″_i分别为第i个从机器人横滚角投影在俯仰角、偏航角上的角度；d_l表示水炮底座到炮筒构成的圆锥体底面的垂直距离且d_l＝l_wcosθ_wm；d_ri表示滚转后炮筒在xoy平面的投影到y轴的垂直距离且d_ri＝l_wsinθ_wmsinΔΦ_i；d′_ri表示滚转后炮筒在yoz平面的投影到y轴的垂直距离且d′_ri＝l_wsinθ_wmcosΔΦ_i，l_w表示机器人水炮长度，ΔΦ_i表示第i辆从机器人与主机器人横滚角的差值。

S6、计算水炮控制的角度约束：机器人位置的变化并不影响横滚角，因此只考虑偏航角与俯仰角的角度约束，同时将俯仰角、偏航角补偿横滚角的角度加入约束公式中。

S61、判断机器人的具体灭火模式，若为片状着火区域的平行式灭火模式则执行步骤S62，若为点状着火区域的集中式灭火模式则执行步骤S63。

S62、片状着火区域的平行式灭火模式下，水炮控制的角度约束为：

其中：Δθ_rwi表示从机器人的俯仰角与从机器人水炮的期望俯仰角的差值；ΔΨ_rwi表示从机器人的偏航角与从机器人水炮的期望偏航角的差值。

S63、点状着火区域的集中式灭火模式下，水炮控制的角度约束为：

S7、计算水炮从当前角度到期望角度的位移：角度位移包括主从机器人水炮自身的俯仰角差值和偏航角差值、机器人位置不同造成的俯仰角差值和偏航角差值、需要俯仰角和横滚角补偿的机器人横滚角。若水炮角度满足角度约束，则判断机器人的具体灭火模式，片状着火区域的平行式灭火模式执行步骤S71，点状着火区域的集中式灭火模式执行步骤S72，分别计算出水炮俯仰角、偏航角需要转动的角度；若水炮角度超过阈值时，调整机器人方向，使差值降至阈值以下，重复步骤S7。

S71、片状着火区域的平行式灭火模式下，水炮需移动的角度为：

其中：Δθ_wi和ΔΨ_wi分别表示第i个从机器人水炮需要移动的俯仰角、偏航角的角度；θ_wi和Ψ_wi分别表示测量到的第i个从机器人水炮的俯仰角、偏航角角度。

S72、点状着火区域的集中式灭火模式下，水炮需移动的角度为：

S8、主机器人和从机器人协作灭火：由控制器发送水炮位移指令，调整水炮位姿到期望角度，并根据主机器人和从机器人所携带的深度相机，获得主机器人和从机器人与火源的距离，并在灭火过程中自动调整主机器人和从机器人位置，以保证主机器人和从机器人距着火点的距离一致。

本发明设计的一种单主多从的消防机器人多模式灭火方法，通过主机器人和从机器人协同控制，一个操作者可以同时控制多台机器人，可节约人力资源，降低操作者伤亡率；针对火场环境，提出片状着火区域的平行式灭火模式和点状着火区域的集中式灭火模式两种不同的灭火模式，使消防作业更有针对性；多台消防机器人互相协作灭火，提升了灭火效率、提高了消防机器人的智能性，实用性大大增强，并更大程度上减小了操作者的压力。

最后所应说明的是：以上实施例仅以说明而非限制本发明的技术方案，尽管参照上述实施例对本发明进行了详细说明，本领域的普通技术人员应当理解：依然可以对本发明进行修改或者等同替换，而不脱离本发明的精神和范围的任何修改或局部替换，其均应涵盖在本发明的权利要求范围当中。

Claims (7)

1.一种单主多从的消防机器人多模式灭火方法，其特征在于，所述消防机器人包括主机器人和从机器人构成的多机器人组，所述方法包括以下步骤：

S1、获取主机器人和从机器人位姿并计算位姿差值：操作者控制主机器人到达目标位置，根据消防机器人所携带传感器传输主从机器人和其水炮的绝对位姿数据，计算出主机器人和从机器人之间的位姿差值：

S2、计算从机器人水炮的期望角度：通过机器人上立体视觉摄像头获取火场着火点情况，并通过mash组网将图像数据传输回后台控制器，操作者通过控制器中视频窗口显示的视频信息，判断机器人的具体灭火模式，若为片状着火区域的平行式灭火模式则执行步骤S3，若为点状着火区域的集中式灭火模式则执行步骤S4；

S3、片状着火区域的平行式灭火模式下，仅考虑俯仰角的角度变化，从机器人水炮的期望角度为：

其中：θ_wm、Ψ_wm、Φ_wm分别表示主机器人水炮的俯仰角、偏航角、横滚角；

分别表示第i辆从机器人水炮的期望俯仰角、期望偏航角、期望横滚角；Δθ'_i表示第i个从机器人位置对其俯仰角造成的影响，且

(x_o，y_o，z_o)表示着火点坐标，l_i表示第t个从机器人到着火点的直线距离；

S4、点状着火区域的集中式灭火模式下，只考虑偏航角与俯仰角的角度变化，从机器人水炮的期望角度为：

其中：Δθ′_i表示第t个从机器人位置对其偏航角造成的影响；ΔΨ′_i表示第i个从机器人位置对其偏航角造成的影响，

S5、计算水炮的俯仰角、偏航角需补偿角度：考虑因地面不平导致机器人底座出现的横滚角，此时机器人绕y轴滚转，水炮与y轴构成一个圆锥面，分别将滚转前后的水炮向xoy平面和yoz平面投影，投影后的夹角即为横滚角向俯仰角、偏航角方向的投影，由此获得水炮的俯仰角、偏航角需补偿的角度为：

其中：Δθ″_i、ΔΨ″_i分别为第i个从机器人横滚角投影在俯仰角、偏航角上的角度；d_l表示水炮底座到炮筒构成的圆锥体底面的垂直距离且d_l＝l_wcosθ_wm；d_ri表示滚转后炮筒在xoy平面的投影到y轴的垂直距离且d_ri＝l_wsinθ_wmsinΔΦ_i；d'_ri表示滚转后炮筒在yoz平面的投影到y轴的垂直距离且d'_ri＝l_wsinθ_wmcosΔΦ_i，l_w表示机器人水炮长度，ΔΦ_i表示第i辆从机器人与主机器人横滚角的差值；

S6、计算水炮控制的角度约束：根据所得主机器人和从机器人的位姿差、水炮的俯仰角和偏航角需补偿的角度，结合消防机器人水炮偏航角与俯仰角的阈值，获得水炮控制的角度约束；

S7、计算水炮从当前角度到期望角度的位移：若水炮角度满足角度约束，则判断机器人的具体灭火模式，片状着火区域的平行式灭火模式执行步骤S71，点状着火区域的集中式灭火模式执行步骤S72，分别计算出水炮俯仰角、偏航角需要转动的角度，调整水炮位姿到期望角度；若水炮角度超过阈值时，调整机器人方向，使差值降至阈值以下，重复步骤S7；

S8、主机器人和从机器人协作灭火：由控制器发送水炮位移指令，调整水炮位姿到期望角度，并根据主机器人和从机器人所携带的深度相机，获得主机器人和从机器人与火源的距离，并在灭火过程中自动调整主机器人和从机器人位置，以保证主机器人和从机器人距着火点的距离一致。

2.根据权利要求1所述的单主多从的消防机器人多模式灭火方法，其特征在于，所述步骤S1具体包括以下步骤：

S11、根据消防机器人所携带传感器传输的机器人和其水炮的绝对坐标数据，计算出主机器人和从机器人之间的坐标差值：

其中：(x_m，y_m，z_m)表示主机器人的坐标；(x_i，y_i，z_i)表示第i辆从机器人的坐标；(Δx_i,Δy_i,Δz_i)表示第i辆从机器人与主机器人坐标的差值，i＝1，2，…，n；

S12、根据消防机器人所携带传感器传输的机器人和其水炮的绝对姿态角数据，计算出主机器人和从机器人之间的姿态角差值：

其中：(θ_m、Ψ_m、Φ_m)表示主机器人的俯仰角、偏航角和横滚角；(θ_i、Ψ_i、Φ_i)表示第i辆从机器人的俯仰角、偏航角和横滚角；(Δθ_i、ΔΨ_i、ΔΦ_i)表示第i辆从机器人与主机器人俯仰角、偏航角和横滚角的差值。

3.根据权利要求1所述的单主多从的消防机器人多模式灭火方法，其特征在于，所述步骤S6具体包括以下步骤：

S61、判断机器人的具体灭火模式，若为片状着火区域的平行式灭火模式则执行步骤S62，若为点状着火区域的集中式灭火模式则执行步骤S63；

S62、片状着火区域的平行式灭火模式下，水炮控制的角度约束为：

其中：Δθ_rwi表示从机器人的俯仰角与从机器人水炮的期望俯仰角的差值；ΔΨ_rwi表示从机器人的偏航角与从机器人水炮的期望偏航角的差值；

S63、点状着火区域的集中式灭火模式下，水炮控制的角度约束为：

。

4.根据权利要求1所述的单主多从的消防机器人多模式灭火方法，其特征在于，所述步骤S7具体包括以下步骤：

S71、片状着火区域的平行式灭火模式下，水炮需移动的角度为：

其中：Δθ_wi和ΔΨ_wi分别表示第t个从机器人水炮需要移动的俯仰角、偏航角的角度；θ_wi和Ψ_wi分别表示测量到的第i个从机器人水炮的俯仰角、偏航角角度；

S72、点状着火区域的集中式灭火模式下，水炮需移动的角度为：

。

5.根据权利要求1所述的单主多从的消防机器人多模式灭火方法，其特征在于，所述步骤S1还包括选择编码器测量机器人水炮偏航角和俯仰角的角位移。

6.根据权利要求1所述的单主多从的消防机器人多模式灭火方法，其特征在于，所述步骤S1中使用RTK差分GPS实现主机器人和从机器人的定位，获取主机器人和从机器人的坐标、俯仰角、偏航角和横滚角。

7.根据权利要求1所述的单主多从的消防机器人多模式灭火方法，其特征在于，所述步骤S2中假设通过SALM算法已获取到着火点的具体坐标。

Images