[发明专利]一种举高消防机器人控制方法、装置及系统

权利要求书

1.一种举高消防机器人控制方法，其特征在于，包括：

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

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

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

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

所述调节消防炮举高高度，包括：获取举高消防机器人当前关节状态初始信息并储存为当前容器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关节号元素删除；跳到第一步。

2.根据权利要求1所述的一种举高消防机器人控制方法，其特征在于，所述调节消防炮粗对准消防目标方向，包括：

获取消防目标点位置，计算得到消防目标点相对于车体位置(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为圆周率。