一种基于灭火机器人的消防门打开方法与流程

本发明涉及消防灭火技术领域，特别是涉及一种基于灭火机器人的消防门打开方法。

背景技术：

当发生火灾时，灭火机器人的首要工作就是打开消防门进入到火灾发生现场，因此消防门打开是灭火消防机器人能成功灭火的重要前提。当门打开时，灭火机器人必须适当地移动，给门让出一定的空间，但是由于灭火机器人的体积比较庞大，并且机械臂的长度有限，如何有效地移动灭火机器人成为了一大难点。此外，由于机械臂的旋转角度和力度有限，如何使得履带的移动与机械臂的转动相互完美配合是本领域亟需突破的难点。

技术实现要素：

本发明的目的是提供一种基于灭火机器人的消防门打开方法，以实现利用灭火机器人方便有效的打开消防门。

为实现上述目的，本发明提供了一种基于灭火机器人的消防门打开方法，所述方法为应用于灭火机器人打开消防门的方法，所述灭火机器人包括：车体，以所述灭火机器人的行进方向为前方，设于所述车体下方左右两侧的履带，设于所述车体上方左右两侧的左机械臂和右机械臂、设于车体上方前部的图像采集模块以及设于所述车体上的控制模块；所述图像采集模块位于所述左机械臂和所述右机械臂的前方；

所述方法包括：

获取所述灭火机器人的前方区域图像，对所述前方区域图像进行图像处理，得到消防门的位置信息；所述第一前方区域图像包括消防门的图像信息；所述消防门的位置信息为以所述灭火机器人的位置信息为参考的相对位置信息；

按照所述消防门的位置信息控制所述履带运动，使所述消防门位于所述灭火机器人的正前方，且距离所述灭火机器人的车体为可操作距离；

获取所述消防门的图像，根据所述消防门的图像识别门把手的三维坐标和消防门单扇宽度，根据所述门把手的三维坐标和所述左机械臂的初始位置坐标确定所述左机械臂抓取所述门把手的运动轨迹；所述消防门单扇宽度为开门时旋转半径；

控制所述左机械臂按照所述抓取所述门把手的运动轨迹运动至所述门把手，抓取所述门把手并旋扭所述门把手；

控制所述车体与所述消防门的相对位置不变、并按照由所述消防门单扇宽度确定的曲率向左做圆周运动，直至所述灭火机器人向左转过90°；控制所述履带、所述左机械臂与所述门把手的相对位置不变，控制所述车体向右转90°，再控制所述履带带动所述车体按照由所述门把手与所述车体中心之间的距离确定的曲率向左做圆周运动，直至所述灭火机器人向左转过45°；

控制所述右机械臂运动至所述消防门的背面并抵住所述消防门；控制所述左机械臂收回至初始位置；控制所述右机械臂和所述履带协同动作，直至所述车体转过135°，正对所述消防门；

控制所述右机械臂和所述履带协同运动使所述灭火机器人以特定斜率运动设定路程，控制所述右机械臂收回至初始位置，控制所述灭火机器人继续以所述特定斜率直线运动，直至通过所述消防门；所述特定斜率由左侧所述履带与左侧所述消防门的距离和所述车体与右侧所述消防门的距离确定。

可选的，所述左机械臂包括依次连接的左底座、左大臂、左小臂和左爪头，设置于所述左底座内的左大臂水平旋转舵机、设置于所述左大臂内的左大臂竖直旋转舵机、设置于所述左小臂内的左爪头收张舵机，连接于所述左大臂与所述左小臂之间的左小臂中心轴方向旋转舵机，以及连接于所述左小臂中心轴方向旋转舵机与所述左大臂之间的左小臂竖直旋转舵机；

所述左大臂水平旋转舵机用于控制所述左底座在水平方向上旋转，所述左底座带动所述左大臂在水平方向上旋转；所述左大臂竖直旋转舵机用于带动所述左大臂以与所述左底座的连接处为旋转中心，在竖直方向上旋转；所述左小臂竖直旋转舵机用于带动所述左小臂以与所述左大臂的连接处为旋转中心，在竖直方向上旋转；所述左爪头收张舵机用于控制所述左爪头的收张；所述左小臂中心轴方向旋转舵机用于控制所述左小臂和所述左爪头以所述左小臂中心轴方向旋转舵机的中心轴为旋转轴心做旋转运动。

可选的，所述右机械臂包括依次连接的右底座、右大臂、右小臂和右爪头，设置于所述右底座内的右大臂水平旋转舵机、设置于所述右大臂内的右大臂竖直旋转舵机、设置于所述右小臂内的右小臂竖直旋转舵机和右爪头收张舵机，以及连接于所述右大臂与所述右小臂之间的右小臂中心轴方向旋转舵机；

所述右大臂水平旋转舵机用于控制所述右底座在水平方向上旋转，所述右底座带动所述右大臂在水平方向上旋转；所述右大臂竖直旋转舵机用于带动所述右大臂以与所述右底座的连接处为旋转中心，在竖直方向上旋转；所述右小臂竖直旋转舵机用于带动所述右小臂以与所述右大臂的连接处为旋转中心，在竖直方向上旋转；所述右爪头收张舵机用于控制所述右爪头的收张；所述右小臂中心轴方向旋转舵机用于控制所述右小臂和所述右爪头以所述右小臂中心轴方向旋转舵机的中心轴为旋转轴心做旋转运动。

可选的，所述获取所述灭火机器人的前方区域图像，对所述前方区域图像进行图像处理，得到消防门的位置信息，具体包括：

获取第一训练数据集，所述第一训练数据集包括多个含有消防门的图像和无消防门的图像以及所述图像对应的类别；

利用所述第一训练数据集通过adaboost算法迭代训练，得到多个第一初级弱分类器；

利用多个所述第一初级弱分类器对已知类别的图片进行分类确定每一所述第一初级述弱分类器的分类准确度；

根据每一所述分类准确度修改每一所述第一初级弱分类器的弱分类阈值，得到多个第一中级弱分类器；

集合多个所述第一中级弱分类器得到第一初级强分类器；

修改所述第一初级强分类器的强分类阈值，得到第一终极强分类器；

将所述灭火机器人的前方区域图像输入至所述第一终极强分类器，识别所述消防门；

利用物体深度信息提取方法，计算所述消防门的位置信息，所述消防门的位置信息为所述消防门的三维坐标。

可选的，所述获取所述消防门的图像，根据所述消防门的图像识别门把手的三维坐标和消防门单扇宽度，具体包括：

获取第二训练数据集，所述第二训练数据集包括含有消防门和消防门把手多个图像以及所述图像对应的类别；

利用所述第二训练数据集通过adaboost算法迭代训练，得到多个第二初级弱分类器；

利用多个所述第二初级弱分类器对已知类别的图片进行分类确定每一所述第二初级述弱分类器的分类准确度；

根据每一所述分类准确度修改每一所述第二初级弱分类器的弱分类阈值，得到多个第二中级弱分类器；

集合多个所述第二中级弱分类器得到第二初级强分类器；

修改所述第二初级强分类器的强分类阈值，得到第二终极强分类器；

将所述消防门的图像输入至所述第二终极强分类器，识别所述消防门上的门把手和所述消防门；

利用物体深度信息提取方法，提取所述消防门的图像中的所述门把手的三维位置坐标和所述消防门单扇宽度。

可选的，所述控制所述左机械臂按照所述抓取所述门把手的运动轨迹运动至所述门把手，抓取所述门把手并旋扭所述门把手具体包括：

控制所述左大臂水平旋转舵机、左大臂竖直旋转舵机、左小臂竖直旋转舵机、左小臂中心轴方向旋转舵机、左爪头收张舵机协调运动，使所述左爪头成功抓取左门把手；

控制所述左小臂中心轴方向旋转舵机旋转将门锁断开。

可选的，所述控制所述车体与所述消防门的相对位置不变、并按照由所述消防门单扇宽度确定的曲率向左做圆周运动，直至所述灭火机器人向左转过90°；控制所述履带、所述左机械臂与所述门把手的相对位置不变，控制所述车体向右转90°，再控制所述履带带动所述车体按照由所述门把手与所述车体中心之间的距离确定的曲率向左做圆周运动，直至所述灭火机器人向左转过45°，具体包括：

使车体与左侧所述消防门相对位置不变，控制所述车体以曲率为100/nm^-1做圆周运动，直至所述灭火机器人向左运动转过90°；其中，n为所述消防门单扇宽度；

使所述履带、所述左机械臂与所述门把手的相对位置不变，控制所述左大臂水平旋转舵机和所述履带协同运作，控制所述车体向右转90°；

控制所述履带带动所述车体以曲率为100/lm^-1向左做圆周运动，直至所述灭火机器人向左转过45°；其中，l为所述门把手与所述车体中心之间的距离。

可选的，所述控制所述左机械臂收回至初始位置，具体包括：

控制所述左大臂水平旋转舵机、所述左大臂竖直旋转舵机、所述左小臂竖直旋转舵机、所述左小臂中心轴方向旋转舵机、所述左爪头收张舵机协同运动，使所述左爪头运动至初始位置。

可选的，所述控制所述右机械臂运动至所述消防门的背面并抵住所述消防门，具体包括：

控制所述右大臂水平旋转舵机、所述右大臂竖直旋转舵机、所述右小臂竖直旋转舵机、所述右小臂中心轴方向旋转舵机、所述右爪头收张舵机协同运动，使所述右机械臂运动至所述消防门的背面相对于所述门把手的位置的下方30cm处，将所述消防门抵住；

所述控制所述右机械臂和所述履带协同动作，直至所述车体转过135°，正对所述消防门，具体包括：

控制所述右大臂水平旋转舵机、所述右大臂竖直旋转舵机、所述右小臂竖直旋转舵机、所述右小臂中心轴方向旋转舵机、所述右爪头收张舵机和所述履带协同运动，直至所述车体转过角度135°，正对消防门。

可选的，所述控制所述右机械臂和所述履带协同运动使所述灭火机器人以特定斜率运动设定路程，控制所述右机械臂收回至初始位置，控制所述灭火机器人继续以所述特定斜率直线运动，直至通过所述消防门；所述特定斜率由左侧所述履带与左侧所述消防门的距离和所述车体与右侧所述消防门的距离确定，具体包括：

控制所述右大臂水平旋转舵机、所述右大臂竖直旋转舵机、所述右小臂竖直旋转舵机、所述右小臂中心轴方向旋转舵机、所述右爪头收张舵机和所述履带协调运动，使所述灭火机器人以斜率a/b向前运动，直至运动60cm；其中，a为左侧所述履带与左侧所述消防门的距离，b为所述车体与右侧所述消防门的距离；

控制所述右大臂水平旋转舵机、所述右大臂竖直旋转舵机、所述右小臂竖直旋转舵机、所述右小臂中心轴方向旋转舵机、所述右爪头收张舵机协同运动，使所述右爪头回到初始位置；

控制所述履带使所述灭火机器人继续以斜率a/b向前运动，直至通过所述消防门。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明提供的基于灭火机器人的消防门打开方法通过采集灭火机器人前方图像，根据图像中消防门的位置信息、门把手位置信息和消防门单扇宽度确定了灭火机器人打开消防门的运动过程和方式，保证了灭火机器人能够快速有效的打开消防门并顺利通过消防门，以确保消防灭火的顺利进行。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的基于灭火机器人的消防门打开方法的流程图图；

图2为本发明中应用的灭火机器人的立体图；

图3为本发明中应用的灭火机器人的主视图；

图4为本发明中应用的灭火机器人的俯视图；

图5为本发明中应用的灭火机器人的左机械臂或右机械臂的立体图；

图6为本发明中灭火机器人抓取阶段示意图；

图7为本发明中门完全打开的示意图；

图8为本发明中门完全打开后灭火机器人转动45°的示意图；

图9为本发明中灭火机器人转回消防门正前方的示意图；

图10为本发明中灭火机器人正通过消防门的示意图；

图11为消防门的结结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种基于灭火机器人的消防门打开方法，以实现利用灭火机器人方便有效的打开消防门。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

本实施例提供的基于灭火机器人的消防门打开方法为应用于灭火机器人打开消防门的方法，其中，如图2-6所示，灭火机器人包括：车体1，以所述灭火机器人的行进方向为前方，设于所述车体1下方左右两侧的履带2，设于所述车体1上方左右两侧的左机械臂5和右机械臂6、设于车体1上方前部的图像采集模块3以及设于所述车体1上的控制模块4；所述图像采集模块3位于所述左机械臂5和所述右机械臂6的前方；

如图6所示，所述左机械臂5包括依次连接的左底座5f、左大臂5g、左小臂5h和左爪头5i，设置于所述左底座5f内的左大臂水平旋转舵机5a、设置于所述左大臂5g内的左大臂竖直旋转舵机5b、设置于所述左小臂5h内的左爪头收张舵机5e，连接于所述左大臂5g与所述左小臂5h之间的左小臂中心轴方向旋转舵机5d，以及连接于所述左小臂中心轴方向旋转舵机5d与所述左大臂5g之间的左小臂竖直旋转舵机5c；

所述左大臂水平旋转舵机5a用于控制所述左底座5f在水平方向上旋转，所述左底座5f带动所述左大臂5g在水平方向上旋转；所述左大臂竖直旋转舵机5g用于带动所述左大臂5g以与所述左底座5f的连接处为旋转中心，在竖直方向上旋转；所述左小臂竖直旋转舵机5c用于带动所述左小臂5h以与所述左大臂5g的连接处为旋转中心，在竖直方向上旋转；所述左爪头收张舵机5e用于控制所述左爪头5i的收张；所述左小臂中心轴方向旋转舵机5d用于控制所述左小臂5h和所述左爪头5i以所述左小臂中心轴方向旋转舵机5d的中心轴为旋转轴心做旋转运动。

所述右机械臂6包括依次连接的右底座6f、右大臂6g、右小臂6h和右爪头6i，设置于所述右底座6f内的右大臂水平旋转舵机6a、设置于所述右大臂6g内的右大臂竖直旋转舵机6b、设置于所述右小臂内的右爪头收张舵机6e，连接于所述右大臂6g与所述右小臂6h之间的右小臂中心轴方向旋转舵机6d，以及连接于所述右小臂中心轴方向旋转舵机6d与所述右大臂6g之间的右小臂竖直旋转舵机6c；

所述右大臂水平旋转舵机6a用于控制所述右底座6f在水平方向上旋转，所述右底座6f带动所述右大臂6g在水平方向上旋转；所述右大臂竖直旋转舵机6b用于带动所述右大臂6g以与所述右底座6f的连接处为旋转中心，在竖直方向上旋转；所述右小臂竖直旋转舵机6c用于带动所述右小臂6h以与所述右大臂6g的连接处为旋转中心，在竖直方向上旋转；所述右爪头收张舵机6e用于控制所述右爪头6i的收张；所述右小臂中心轴方向旋转舵机6d用于控制所述右小臂6h和所述右爪头6i以所述右小臂中心轴方向旋转舵机6d的中心轴为旋转轴心做旋转运动。

其中，左、右大臂水平旋转舵机5a、6a，左、右大臂竖直旋转舵机5b、6b，左、右小臂竖直旋转舵机5c、6c，左、右爪头收张舵机5e、6e和左、右小臂中心轴方向旋转舵机5d、6d分别与控制模块4连接，由控制模块4控制，并运动。

图1所示，本实施例提供的基于灭火机器人的消防门打开方法包括：

步骤101：获取灭火机器人的前方区域图像，对所述前方区域图像进行图像处理，得到消防门的位置信息；所述第一前方区域图像包括消防门的图像信息；所述消防门的位置信息为以所述灭火机器人的位置信息为参考的相对位置信息。

具体的，该步骤101可以包括以下步骤：

获取第一训练数据集，所述第一训练数据集包括多个含有消防门的图像和无消防门的图像以及所述图像对应的类别；

利用所述第一训练数据集通过adaboost算法迭代训练，得到多个第一初级弱分类器；

利用多个所述第一初级弱分类器对已知类别的图片进行分类确定每一所述第一初级述弱分类器的分类准确度；

根据每一所述分类准确度修改每一所述第一初级弱分类器的弱分类阈值，得到多个第一中级弱分类器；

集合多个所述第一中级弱分类器得到第一初级强分类器；

修改所述第一初级强分类器的强分类阈值，得到第一终极强分类器；

将所述灭火机器人的前方区域图像输入至所述第一终极强分类器，识别所述消防门；

利用物体深度信息提取方法，计算所述消防门的位置信息，所述消防门的位置信息为所述消防门的三维坐标。

步骤102：按照所述消防门的位置信息控制所述履带2运动，使所述消防门位于所述灭火机器人的正前方，且距离所述灭火机器人的车体1为可操作距离。

步骤103：获取所述消防门的图像，根据所述消防门的图像识别门把手的三维坐标和消防门单扇宽度，根据所述门把手的三维坐标和所述左机械臂5的初始位置坐标确定所述左机械臂5抓取所述门把手的运动轨迹；所述消防门单扇宽度为开门时旋转半径。

具体的，该步骤103可以包括以下步骤：

获取第二训练数据集，所述第二训练数据集包括含有消防门和消防门把手多个图像以及所述图像对应的类别；

利用所述第二训练数据集通过adaboost算法迭代训练，得到多个第二初级弱分类器；

利用多个所述第二初级弱分类器对已知类别的图片进行分类确定每一所述第二初级述弱分类器的分类准确度；

根据每一所述分类准确度修改每一所述第二初级弱分类器的弱分类阈值，得到多个第二中级弱分类器；

集合多个所述第二中级弱分类器得到第二初级强分类器；

修改所述第二初级强分类器的强分类阈值，得到第二终极强分类器；

将所述消防门的图像输入至所述第二终极强分类器，识别所述消防门上的门把手和所述消防门；

利用物体深度信息提取方法，提取所述消防门的图像中的所述门把手的三维位置坐标和所述消防门单扇宽度。

步骤104：控制所述左机械臂5按照所述抓取所述门把手的运动轨迹运动至所述门把手，抓取所述门把手并旋扭所述门把手。

该步骤104具体包括以下步骤：

控制所述左大臂水平旋转舵机5a、左大臂竖直旋转舵机5b、左小臂竖直旋转舵机5c、左小臂中心轴方向旋转舵机5d、左爪头收张舵机5e协调运动，使所述左爪头5i成功抓取左门把手；

控制所述左小臂中心轴方向旋转舵机5d旋转将门锁断开。

步骤105：控制所述车体1与所述消防门的相对位置不变、并按照由所述消防门单扇宽度确定的曲率向左做圆周运动，直至所述灭火机器人向左转过90°；控制所述履带2、所述左机械臂5与所述门把手的相对位置不变，控制所述车体1向右转90°，再控制所述履带2带动所述车体1按照由所述门把手与所述车体1中心之间的距离确定的曲率向左做圆周运动，直至所述灭火机器人向左转过45°；

该步骤105具体包括以下步骤：

使车体1与左侧所述消防门相对位置不变，控制所述车体1以曲率为100/nm^-1做圆周运动，直至所述灭火机器人向左运动转过90°；其中，n为所述消防门单扇宽度；

使所述履带2、所述左机械臂5与所述门把手的相对位置不变，控制所述左大臂水平旋转舵机5a和所述履带2协同运作，控制所述车体1向右转90°；

控制所述履带2带动所述车体1以曲率为100/lm^-1向左做圆周运动，直至所述灭火机器人向左转过45°；其中，l为所述门把手与所述车体1中心之间的距离。

步骤106：控制所述右机械臂6运动至所述消防门的背面并抵住所述消防门；控制所述左机械臂5收回至初始位置；控制所述右机械臂6和所述履带2协同动作，直至所述车体1转过135°，正对所述消防门；

该步骤106具体包括以下步骤：

控制右大臂水平旋转舵机6a、右大臂竖直旋转舵机6b、右小臂竖直旋转舵机6c、右小臂中心轴方向旋转舵机6d、右爪头收张舵机6e协同运动，使所述右机械臂6运动至所述消防门的背面相对于所述门把手的位置的下方30cm处，将所述消防门抵住。

控制左大臂水平旋转舵机5a、左大臂竖直旋转舵机5b、左小臂竖直旋转舵机5c、左小臂中心轴方向旋转舵机5d、左爪头收张舵机5e协同运动，使所述左爪头5i运动至初始位置。

控制右大臂水平旋转舵机6a、右大臂竖直旋转舵机6b、右小臂竖直旋转舵机6c、右小臂中心轴方向旋转舵机6d、右爪头收张舵机6e、履带2协同运动，直至所述车体1转过角度135°，正对消防门。

步骤107：控制所述右机械臂6和所述履带2协同运动使所述灭火机器人以特定斜率运动设定路程，控制所述右机械臂2收回至初始位置，控制所述灭火机器人继续以所述特定斜率直线运动，直至通过所述消防门；所述特定斜率由左侧所述履带2与左侧所述消防门的距离和所述车体1与右侧所述消防门的距离确定。

该步骤107具体包括以下步骤：

控制右大臂水平旋转舵机6a、右大臂竖直旋转舵机6b、右小臂竖直旋转舵机6c、右小臂中心轴方向旋转舵机6d、右爪头收张舵机6e、履带2协调运动，使所述灭火机器人以斜率a/b向前运动，直至运动60cm；其中，a为左侧所述履带2与左侧所述消防门的距离，b为所述车体1与右侧所述消防门的距离；

控制右大臂水平旋转舵机6a、右大臂竖直旋转舵机6b、右小臂竖直旋转舵机6c、右小臂中心轴方向旋转舵机6d、右爪头收张舵机6e协同运动，使所述右爪头6i回到初始位置；

控制所述履带2使所述灭火机器人继续以斜率a/b向前运动，直至通过所述消防门。

下面以单扇宽度为90cm、门高为210cm的消防门为例，详细说明灭火机器人打开该消防门的方法：

该消防门的门把手距地面高度为106cm，矩门的旋转边缘77cm，如图11所示。

1、通过灭火机器人的图像采集模块3实时采集前方区域的图像，并将采集到的图像送入控制模块4中的数字信号处理器分析和处理。

2、数字信号处理器将处理结果送至控制模块4，控制模块4根据车体1所在地与消防门的相对位置，控制车体1移动至车体1的左侧的履带2大致与消防门的中垂线平行，且车体1的正前方到消防门的距离为50cm左右的地方。

3、通过灭火机器人的图像采集模块3实时采集前方区域的图像，并将采集到的图像送入数字信号处理器分析和处理。

4、数字信号处理器通过运行算法处理识别到门把手的三维坐标信息，并将结果送至控制模块4，控制模块4根据门把手的三维坐标信息和左爪头5i的初始位置，控制左大臂水平旋转舵机5a、左大臂竖直旋转舵机5b、左小臂竖直旋转舵机5c、左小臂中心轴方向旋转舵机5d、左爪头收张舵机5e协调运动，使左爪头5i成功抓取左门把手，如图6所示。

5、已知门的半径(消防门单扇宽度)为90cm,通过控制模块4控制车体1以曲率为10/9m^-1做圆周运动，车体1与左消防门相对位置不变，直至车体1运动转过90°。

6、通过控制模块4控制左大臂水平旋转舵机5a、履带2协同运作，使车体1整体(即除机械臂部分的灭火机器人)向右转过90°，左机械臂5与左消防门的相对位置不变。

7、门把手与车体1中心的距离为100cm，通过控制模块4控制车体1以曲率为1m^-1做圆周运动。左机械臂5与车体1相对位置不变，直至车体1运动转过角度45°，如图7所示。

8、通过控制模块4控制右大臂水平旋转舵机6a、右大臂竖直旋转舵机6b、右小臂竖直旋转舵机6c、右小臂中心轴方向旋转舵机6d、右爪头收张舵机6e协同运动，使右机械臂6运动至消防门的背面相对于门把手位置的下方30cm处，并将消防门抵住，然后通过控制模块4控制左大臂水平旋转舵机5a、左大臂竖直旋转舵机5b、左小臂竖直旋转舵机5c、左小臂中心轴方向旋转舵机5d、左爪头收张舵机5e协同运动，使左爪头5i运动至初始位置。

9、通过控制模块4控制右大臂水平旋转舵机6a、右大臂竖直旋转舵机6b、右小臂竖直旋转舵机6c、右小臂中心轴方向旋转舵机6d、右爪头收张舵机6e、履带2协同运动，直至车体1转过角度135°，正对消防门，如图9所示。

10、车体1的左侧履带2与左消防门的距离为60cm，车体1的正前方与消防门的距离为140cm，通过控制模块4控制右大臂水平旋转舵机6a、右大臂竖直旋转舵机6b、右小臂竖直旋转舵机6c、右小臂中心轴方向旋转舵机6d、右爪头收张舵机6e、履带2协调运动，使车体1整体以7/3的斜率向前运动，直至运动60cm。

11、通过控制模块4控制右大臂水平旋转舵机6a、右大臂竖直旋转舵机6b、右小臂竖直旋转舵机6c、右小臂中心轴方向旋转舵机6d、右爪头收张舵机6e协同运动，使右爪头6i回到初始位置。

12、通过控制模块4控制履带2使车体1整体继续以7/3的斜率向前运动，直至完全通过消防门，如图10所示。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。