一种举高消防机器人控制方法、装置及系统与流程

本发明涉及一种举高消防机器人控制方法、装置及系统，属于消防控制技术领域。

背景技术：

随着经济不断地发展，人们生活水平越来越提高，机器人应用开始出现在各行各业中，协助工作人员解决各类问题。特别是消防行业，机器人不仅能协助消防人员获取更多现场信息，还能代替消防人员进入危险领域进行消防工作，提升了消防工作人员灭火救援的能力和效率，更能大大减少工作人员作业危险系数，有效保障相关人员的生命安全。

消防机器人种类繁多，其中举高机器人是为最常见的一种。举高机器人通常在举高臂的末端安装风炮、水炮等消防设备；甚至可以在举高末端安装救援台，将救援台升到指定高度救援高处被困人员。传统的举高消防机器人是通过人员手动操纵操作杆一个个关节进行控制，使得举高臂到达目标高度，这种控制方式操作繁琐，人为误操作概率高，造成控制效率低下，控制精度低；并且需要培训专业人员操纵，培训成本高。中国专利申请号201210398019.7公开了一种举高消防车臂架控制系统及控制方法，该控制方法是根据预先设定的臂架目标位置参数，以及检测装置所检测到实时臂长度和角度，同时控制所有的执行机构完成臂架全部动作，不进行单独控制每个臂运动。虽然同时发出控制每个臂运动可以对控制流程简单化，缩短控制时间，但由于同时发出控制每个臂运动，只关注初始状态和目标状态，举高臂的中间运动姿态无法控制，中间路点无法得到控制，路径不可控容易产生自身碰撞以及与环境物体发生碰撞，造成了严重的事故,作业过程得不到安全保障。

技术实现要素：

本发明所要解决的技术问题是克服现有技术的缺陷，提供一种举高消防机器人控制方法、装置及系统，不需要人为手动控制，通过顺序控制关节运动，举高臂运动路径实现平稳可控。

为解决上述技术问题，本发明采用的技术方案如下：

本发明提供一种举高消防机器人控制系统，包括：车体、塔臂、联动臂、主臂和伸缩臂；所述车体与塔臂之间以转动关节形式链接，所述塔臂和联动臂以联动关节形式链接，所述联动臂和主臂之间以转动关节链接，所述主臂和伸缩臂之间以平移关节链接，所述消防炮台与伸缩臂之间以关节形式链接；所述消防炮台用于安装消防炮。

本发明还提供一种举高消防机器人控制方法，包括：

控制塔臂与车体关节旋转角度，调节消防炮粗对准消防目标方向；

联合控制塔臂、联动臂、主臂和伸缩臂，调节消防炮举高高度，使消防炮高度对准消防目标；

控制消防炮台与伸缩臂关节旋转角度，调节消防炮俯仰角，使消防炮最终对准消防目标。

进一步的，所述调节消防炮粗对准消防目标方向，包括：

获取消防目标点位置，计算得到消防目标点相对于车体位置(xt,yt,hc)，

其中，xt,yt,hc分别为x轴，y轴和z轴坐标，以右手定则为基础，车头方向x轴方向，垂直于车体为y轴方向，z轴为高度；

如果xt＞0，则控制塔臂相对车体转动角度为：jt0＝arctan(yt/xt)；

如果xt＜0，则控制塔臂相对车体转动角度为：jt0＝pi+arctan(yt/xt)；

其中，jt0为塔臂相对车体转动角度，pi为圆周率。

进一步的，所述调节消防炮举高高度，包括：

获取举高消防机器人当前关节状态初始信息并储存为当前容器jc，以及获取目标高度ht；jc＝(jc0,jc1,jc2,...,jcn)，其中0，1，2，…,n为关节序号；

根据当前容器jc中的元素计算举高消防机器人当前高度hc，以及根据当前容器jc的元素计算目标高度对应的举高消防机器人的初步目标关节状态并存储为目标容器jt，jt＝(jt0,jt1,jt2,...,jtn)；

根据目标高度和当前高度选择控制策略，如果目标高度大于当前高度，则选择上升控制策略，反之选择下降控制策略。

进一步的，所述上升控制策略为：

第一步：计算目标容器jt中元素与当前容器jc中元素的差值并储存为差值容器ja，ja＝(ja0,ja1,ja2,...,jan)，对差值容器ja内的元素按照从大到小进行排序；

第二步：基于排序后的差值容器ja获取最大元素的关节号i，采用目标容器jt的第i关节号元素值jti替换测试容器jb中的第i关节号元素jbi，jbi＝jti，更新预先构建的测试容器jb，并根据更新后的测试容器jb求解测试高度hb；

第三步：将测试高度hb与目标高度ht进行对比，

如果相等，则将jti以堆栈形式放入控制容器c中，再按进入堆栈控制容器c的先后顺序，发送相对应的关节状态至举高消防机器人，使举高消防机器人运动到达相应位置，高度调节结束；

如果不相等，则判断测试高度hb是否大于目标高度ht，

如果是，则设置中间变量jm＝(jti+jci)/2，把目标容器jt中的第i关节号元素替换中间变量jm，即jti＝jm；跳到第二步；

如果否，则将jti以堆栈形式放入控制容器c中；然后将差值容器ja内的第i关节号元素删除；跳到第一步；

所述下降控制策略为：

第一步：计算目标容器jt中元素与当前容器jc中元素的差值并储存为差值容器ja，ja＝(ja0,ja1,ja2,...,jan)，对差值容器ja内的元素按照从小到大进行排序；

第二步：基于排序后的差值容器ja获取最小元素的关节号i，采用目标容器jt的第i关节号元素值jti替换测试容器jb中的第i关节号元素jbi，jbi＝jti，更新预先构建的测试容器jb，并根据更新后的测试容器jb求解测试高度hb；

第三步：将测试高度hb与目标高度ht进行对比，

如果相等，则将jti以堆栈形式放入控制容器c中，再按进入堆栈控制容器c的先后顺序，发送相对应的关节状态至举高消防机器人，使举高消防机器人运动到达相应位置，高度调节结束；

如果不相等，则判断测试高度hb是否小于目标高度ht，

如果是，则设置中间变量jm＝(jti+jci)/2，把目标容器jt中的第i关节号元素替换中间变量jm，即jti＝jm；跳到第二步；

如果否，则将jti以堆栈形式放入控制容器c中；然后将差值容器ja内的第i关节号元素删除；跳到第一步。

进一步的，

所述根据当前容器jc中的元素计算举高消防机器人当前高度hc，包括：

对当前容器jc存储的关节状态进行正向运动学求解，得到举高消防机器人当前高度hc；

所述根据当前容器jc的元素计算目标高度对应的举高消防机器人的初步目标关节状态，包括：

对当前容器jc存储的关节状态进行逆向运动学求解，得到目标高度对应的举高消防机器人的初步目标关节状态。

进一步的，所述上升控制策略和所述下降控制策略还包括：

预先构建测试容器，将当前容器jc中的元素复制到测试容器jb中，对测试容器进行初步赋值。

进一步的，所述上升控制策略中，对差值容器ja内的元素按照从大到小进行排序，包括：

采用冒泡法或对比法将差值容器ja内的元素进行从大到小排序。

进一步的，所述调节消防炮俯仰角，包括：

获取消防炮的消防物喷射轨迹；

计算消防物喷射轨迹落地点与消防目标距离值；

根据距离值进行自动或人工控制消防炮台与伸缩臂关节旋转角度，调节消防炮俯仰角。

本发明还提供一种举高消防机器人控制装置，包括：

第一调节模块，用于控制塔臂与车体关节旋转角度，调节消防炮粗对准消防目标方向；

第二调节模块，用于联合控制塔臂、联动臂、主臂和伸缩臂，调节消防炮举高高度，使消防炮高度对准消防目标；

以及，

第三调节模块，用于控制消防炮台与伸缩臂关节旋转角度，调节消防炮俯仰角，使消防炮最终对准消防目标。

进一步的，所述第一调节模块具体用于，

获取消防目标点位置，计算得到消防目标点相对于车体位置(xt,yt,hc)，

其中，xt,yt,hc分别为x轴，y轴和z轴坐标，以右手定则为基础，车头方向x轴方向，垂直于车体为y轴方向，z轴为高度；

如果xt＞0，则控制塔臂相对车体转动角度为：jt0＝arctan(yt/xt)；

如果xt＜0，则控制塔臂相对车体转动角度为：jt0＝pi+arctan(yt/xt)；

其中，jt0为塔臂相对车体转动角度，pi为圆周率。

进一步的，所述第二调节模块具体用于，

获取举高消防机器人当前关节状态初始信息并储存为当前容器jc，以及获取目标高度ht；jc＝(jc0,jc1,jc2,...,jcn)，其中0，1，2，…,n为关节序号；

根据当前容器jc中的元素计算举高消防机器人当前高度hc，以及根据当前容器jc的元素计算目标高度对应的举高消防机器人的初步目标关节状态并存储为目标容器jt，jt＝(jt0,jt1,jt2,...,jtn)；

根据目标高度和当前高度选择控制策略，如果目标高度大于当前高度，则选择上升控制策略，反之选择下降控制策略。

进一步的，

所述上升控制策略为：

第一步：计算目标容器jt中元素与当前容器jc中元素的差值并储存为差值容器ja，ja＝(ja0,ja1,ja2,...,jan)，对差值容器ja内的元素按照从大到小进行排序；

第二步：基于排序后的差值容器ja获取最大元素的关节号i，采用目标容器jt的第i关节号元素值jti替换测试容器jb中的第i关节号元素jbi，jbi＝jti，更新预先构建的测试容器jb，并根据更新后的测试容器jb求解测试高度hb；

第三步：将测试高度hb与目标高度ht进行对比，

如果相等，则将jti以堆栈形式放入控制容器c中，再按进入堆栈控制容器c的先后顺序，发送相对应的关节状态至举高消防机器人，使举高消防机器人运动到达相应位置，高度调节结束；

如果不相等，则判断测试高度hb是否大于目标高度ht，

如果是，则设置中间变量jm＝(jti+jci)/2，把目标容器jt中的第i关节号元素替换中间变量jm，即jti＝jm；跳到第二步；

如果否，则将jti以堆栈形式放入控制容器c中；然后将差值容器ja内的第i关节号元素删除；跳到第一步；

所述下降控制策略为：

第一步：计算目标容器jt中元素与当前容器jc中元素的差值并储存为差值容器ja，ja＝(ja0,ja1,ja2,...,jan)，对差值容器ja内的元素按照从小到大进行排序；

第二步：基于排序后的差值容器ja获取最小元素的关节号i，采用目标容器jt的第i关节号元素值jti替换测试容器jb中的第i关节号元素jbi，jbi＝jti，更新预先构建的测试容器jb，并根据更新后的测试容器jb求解测试高度hb；

第三步：将测试高度hb与目标高度ht进行对比，

如果相等，则将jti以堆栈形式放入控制容器c中，再按进入堆栈控制容器c的先后顺序，发送相对应的关节状态至举高消防机器人，使举高消防机器人运动到达相应位置，高度调节结束；

如果不相等，则判断测试高度hb是否小于目标高度ht，

如果是，则设置中间变量jm＝(jti+jci)/2，把目标容器jt中的第i关节号元素替换中间变量jm，即jti＝jm；跳到第二步；

如果否，则将jti以堆栈形式放入控制容器c中；然后将差值容器ja内的第i关节号元素删除；跳到第一步。

进一步的，所述第三调节模块具体用于，

获取消防炮的消防物喷射轨迹；

计算消防物喷射轨迹落地点与消防目标距离值；

根据距离值进行自动或人工控制消防炮台与伸缩臂关节旋转角度，调节消防炮俯仰角。

本发明所达到的有益效果：

本发明对举高消防机器人进行调节控制，按顺序逐一对对关节进行寻优最优，能快速求解得到最优解，通过顺序控制关节运动，举高臂运动路径实现可控，解决了运动中间状态为不可控，出现有的臂需要回收有的需要伸展，举高臂末端容易出现上下振荡运动现象，运动不平稳，控制精度低等问题；本发明大大减少了对工作人员培训成本，大大降低了劳动强度，提高劳动效率和作业的规范性。

附图说明

图1为本发明的举高消防机器人控制系统示意图；

图2为本发明的举高消防机器人控制方法流程图；

图3为本发明的举高消防机器人控制方法的下降控制策略流程图；

图4为本发明的举高消防机器人控制方法的上升控制策略流程图。

具体实施方式

下面对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

本发明实施例提供一种举高消防机器人控制系统，参见图1，包括：车体1、塔臂2、联动臂3、主臂4和伸缩臂5。其中车体1与塔臂2之间以转动关节形式链接，塔臂2和联动臂3以联动关节形式链接，联动臂3随塔臂2运动而动，联动臂3和主臂4之间以转动关节链接，主臂4和伸缩臂5之间以平移关节链接，消防炮台6与伸缩臂5之间以关节形式链接，消防炮台用于安装消防炮，调节消防炮俯仰角。

通过控制塔臂与车体关节旋转角度，用于调节消防炮粗对准消防目标方向；

通过联合控制塔臂、联动臂、主臂和伸缩臂，用于调节消防炮举高高度，高度对准消防目标，实现进一步精准对准消防目标；

通过控制消防炮台与伸缩臂关节旋转角度，调节消防炮俯仰角，进一步精准对准消防目标。

本发明实施例还提供一种举高消防机器人控制方法，包括：

调节塔臂与车体关节旋转角度，使消防炮粗对准消防目标方向；

通过联合控制塔臂、联动臂、主臂和伸缩臂，调节消防炮举高高度，使消防炮高度对准消防目标；

通过控制消防炮台与伸缩臂关节旋转角度，调节消防炮俯仰角，完成消防炮调节。

具体的，调节塔臂与车体关节旋转角度，使消防炮粗对准消防目标方向，包括：

(1)通过传感器或人工获取消防目标点位置，进行数据处理分析得到消防目标点相对于车体位置(xt,yt,hc)，其中以右手定则为基础，车头方向x轴方向，垂直与车体为y轴方向，z轴为高度。

其中，传感器可以是视觉传感器、红外传感系统或者其他传感设。

(2)如果(xt＞0)，则控制塔臂相对车体转动角度为：jt0＝arctan(yt/xt)；

如果(xt＜0)，则控制塔臂相对车体转动角度为：jt0＝pi+arctan(yt/xt)；

其中pi为圆周率。

具体的，通过联合控制塔臂、联动臂、主臂和伸缩臂，调节消防炮举高高度，使消防炮高度对准消防目标，参见图2，包括：

第一步：获取初始信息。

具体的，包括：接收目标高度ht，获取举高消防机器人当前关节状态并储存为当前容器jc＝(jc0,jc1,jc2,...,jcn)，其中0，1，2，…,n为关节序号。

第二步：计算举高消防机器人当前高度hc。

具体为，通过对当前容器jc关节状态进行正向运动学求解，得到举高消防机器人当前高度hc。

第三步：计算目标高度对应的举高消防机器人的初步目标关节。

具体为，通过对当前容器jc关节状态进行逆向运动学求解，得到目标高度ht的初步目标关节，存储为目标容器jt＝(jt0,jt1,jt2,...,jtn)，其中0，1，2，…,n为关节序号。

第四步：将初步目标关节与当前关节差值进行比较。

具体为，将目标容器jt与当前容器jc进行关节状态相减，存储为差值容器ja＝(ja0,ja1,ja2,...,jan)，其中0，1，2，…,n为关节序号。

第五步：根据目标高度和当前高度选择控制策略。

具体为，如果目标高度大于当前高度，即控制举高臂上升，即选择上升控制策略；反之则选择下降控制策略。

具体的，下降控制策略参见图3，包括：

第一步：构建测试容器并进行初步赋值。

具体为，将当前关节容器jc中元素复制到测试容器jb中，对测试容器进行初步赋值。

第二步：对差值容器进行排序。

具体为，将差值容器ja内元素采用冒泡法或对比法等排列方法进行从小到大进行排序，最小元素值记为jai，其中i为最小元素的关节号。

第三步：对测试容器再次赋值，求解测试高度。

具体为，把测试容器jb中的第i关节号元素替换成目标容器jt的第i关节号元素值，即jbi＝jti；

然后对测试容器jb进行正向运动学计算得到测试高度hb。

第四步：判断测试高度hb是否等于目标高度ht。

如果相等，则记录第i关节控制状态jti以堆栈形式放入控制容器c中。再按进入堆栈控制容器c的先后顺序，发送相对应的关节控制状态使举高消防机器人运动到达相应位置。结束完成控制。

如果测试高度hb不等于目标高度ht，则跳到第五步。

第五步：判断测试高度hb是否小于目标高度ht。

如果是，则设置中间变量jm＝(jti+jci)/2，把目标容器jt中的第i关节号元素替换中间变量jm，即jti＝jm。跳到第三步。

如果测试高度hb大于目标高度ht，记录第i关节控制状态jti以堆栈形式放入控制容器c中；

然后将差值容器ja内的第i关节号元素删除。跳到第二步。

具体的，上升控制策略参见图4，包括：

第一步：构建测试容器并进行初步赋值。

具体为，将当前关节容器jc中元素复制到测试容器jb中，对测试容器进行初步赋值。

第二步：对差值容器进行排序。

具体为，将差值容器ja内元素采用冒泡法或对比法等排列方法进行从大到小进行排序，最大元素值记为jai，其中i为最大元素的关节号。

第三步：对测试容器再次赋值，求解测试高度。

具体为，把测试容器jb中的第i关节号元素替换成目标容器jt的第i关节号元素值，即jbi＝jti；

然后对测试容器jb进行正向运动学计算得到测试高度hb。

第四步：判断测试高度hb是否等于目标高度ht。

如果相等，则记录第i关节控制状态jti以堆栈形式放入控制容器c中。再按进入堆栈控制容器c的先后顺序，发送相对应的关节控制状态使举高消防机器人运动到达相应位置。结束完成控制。

如果测试高度hb不等于目标高度ht，则跳到第五步。

第五步：判断测试高度hb是否大于目标高度ht。

如果是，则设置中间变量jm＝(jti+jci)/2，把目标容器jt中的第i关节号元素替换中间变量jm，即jti＝jm。跳到第三步。

如果测试高度hb小于目标高度ht，记录第i关节控制状态jti以堆栈形式放入控制容器c中；

然后将差值容器ja内的第i关节号元素删除。跳到第二步。

具体的，通过控制消防炮台与伸缩臂关节旋转角度，调节消防炮俯仰角包括：

通过视觉传感器或其他方法获取消防炮的消防物喷射轨迹，计算喷射轨迹落地点与消防目标距离值，进行自动或人工控制消防炮台与伸缩臂关节旋转角度，调节消防炮俯仰角。

本发明实施例还提供一种举高消防机器人控制装置，包括：

第一调节模块，用于控制塔臂与车体关节旋转角度，调节消防炮粗对准消防目标方向；

第二调节模块，用于联合控制塔臂、联动臂、主臂和伸缩臂，调节消防炮举高高度，使消防炮高度对准消防目标；

以及，

第三调节模块，用于控制消防炮台与伸缩臂关节旋转角度，调节消防炮俯仰角，使消防炮最终对准消防目标。

具体的，第一调节模块具体用于，

获取消防目标点位置，计算得到消防目标点相对于车体位置(xt,yt,hc)，

其中，xt,yt,hc分别为x轴，y轴和z轴坐标，以右手定则为基础，车头方向x轴方向，垂直于车体为y轴方向，z轴为高度；

如果xt＞0，则控制塔臂相对车体转动角度为：jt0＝arctan(yt/xt)；

如果xt＜0，则控制塔臂相对车体转动角度为：jt0＝pi+arctan(yt/xt)；

其中，jt0为塔臂相对车体转动角度，pi为圆周率。

具体的，第二调节模块具体用于，

获取举高消防机器人当前关节状态初始信息并储存为当前容器jc，以及获取目标高度ht；

jc＝(jc0,jc1,jc2,...,jcn)，其中0，1，2，…,n为关节序号；

根据当前容器jc计算举高消防机器人当前高度hc；以及根据当前容器jc计算目标高度对应的举高消防机器人的初步目标关节状态存储为目标容器jt，jt＝(jt0,jt1,jt2,...,jtn)；

计算目标容器jt中元素与当前容器jc中元素的差值并储存为差值容器ja，ja＝(ja0,ja1,ja2,...,jan)；

基于差值容器ja选择控制策略；所述控制策略包括上升控制策略和下降控制策略。

具体的，上升控制策略为：

第一步：对差值容器ja内的元素按照从大到小进行排序；

第二步：基于排序后的差值容器ja获取最大元素的关节号i，采用目标容器jt的第i关节号元素值jti替换测试容器jb中的第i关节号元素jbi，更新预先构建的测试容器jb，并根据更新后的测试容器jb求解测试高度hb；

第三步：将测试高度hb与目标高度ht进行对比，

如果相等，则将jti以堆栈形式放入控制容器c中，再按进入堆栈控制容器c的先后顺序，发送相对应的关节状态至举高消防机器人，使举高消防机器人运动到达相应位置，高度调节结束；

如果不相等，则判断测试高度hb是否大于目标高度ht，

如果是，则设置中间变量jm＝(jti+jci)/2，把目标容器jt中的第i关节号元素替换中间变量jm，即jti＝jm；跳到第二步；

如果否，则将jti以堆栈形式放入控制容器c中；然后将差值容器ja内的第i关节号元素删除；跳到第一步；

具体的，下降控制策略为：

第一步：对差值容器ja内的元素按照从小到到进行排序；

第二步：基于排序后的差值容器ja获取最小元素的关节号i，采用目标容器jt的第i关节号元素值jti替换测试容器jb中的第i关节号元素jbi，更新预先构建的测试容器jb，并根据更新后的测试容器jb求解测试高度hb；

第三步：将测试高度hb与目标高度ht进行对比，

如果相等，则将jti以堆栈形式放入控制容器c中，再按进入堆栈控制容器c的先后顺序，发送相对应的关节状态至举高消防机器人，使举高消防机器人运动到达相应位置，高度调节结束；

如果不相等，则判断测试高度hb是否小于目标高度ht，

如果是，则设置中间变量jm＝(jti+jci)/2，把目标容器jt中的第i关节号元素替换中间变量jm，即jti＝jm；跳到第二步；

如果否，则将jti以堆栈形式放入控制容器c中；然后将差值容器ja内的第i关节号元素删除；跳到第一步。

具体的，第三调节模块具体用于，

获取消防炮的消防物喷射轨迹；

计算消防物喷射轨迹与消防目标距离值；

根据距离值进行自动或人工控制消防炮台与伸缩臂关节旋转角度，调节消防炮俯仰角。

值得指出的是，该装置实施例是与上述方法实施例对应的，上述方法实施例的实现方式均适用于该装置实施例中，并能达到相同或相似的技术效果，故不在此赘述。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

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