[发明专利]一种带有导臂的履带式移动机器人自主上下楼梯控制方法

摘要

本发明公开了一种带有导臂的履带式移动机器人自主上下楼梯控制方法，该方法可以让移动机器人在自主上楼前或自主下楼前利用自身携带的双目视觉传感器和三轴力传感器实现自动对准楼梯并且在自主上楼或者自主下楼的过程中不会偏离楼梯，本发明提出的方法既适用于直线型楼梯又适用于弧形楼梯和螺旋形楼梯；另外，移动机器人在自主上楼或自主下楼前，还能够利用自身携带的双目视觉传感器计算出楼梯的陡峭程度，若陡峭程度过大超过了移动机器人的最大爬坡能力，则驳回操作人员发出的上楼或者下楼指令，并发送警告信息给操作人员，移动机器人通过对楼梯陡峭程度的分析，减少了因为操作人员判断失误发出错误指令而造成的翻车事故。

主权项

一种带有导臂的履带式移动机器人自主上下楼梯控制方法，其特征在于，该方法首先在履带式移动机器人车体顶面的前端设置面向前进方向的双目视觉传感器，在履带式移动机器人的导臂主动轮的旋转轴两端分别设置一个用于检测导臂力矩的三轴力传感器，上扬导臂至与地面夹角成β度，其中β为移动机器人的最大爬坡度，然后进行自主上楼和下楼控制流程；所述自主上楼控制流程中，双目视觉传感器始终保持水平方向并实时对视野范围内的楼梯进行三维模型重建，包括以下步骤：步骤A1：移动机器人将头部对准并驶向第一级台阶的中间处，实时检测双目视觉传感器中心分别与第一级踢面和第二级踢面的水平距离L1、L2，满足L1≤S时，移动机器人停止前进，其中S为导臂的长度，然后根据此时检测到的L1、L2，按照下式计算出楼梯倾斜角度θ：θ＝(180*arctan(H/W))/π，其中H为通过三维模型计算出的第一级台阶踏步的高度，W为踏步的宽度，W＝L2‑L1；将楼梯的倾斜角度θ与移动机器人的最大爬坡度β作对比，如果θ≤β，则接受并确认上楼指令，进入步骤A2；否则驳回上楼指令，并发送警告信息给监控端的操作人员后结束自主上楼控制流程；步骤A2：上扬导臂至与地面夹角成θ度，驱动移动机器人前进，实时检测两个三轴力传感器的力矩值，当两个力矩值的正负号同时发生反转且两个力矩值相等时，认为移动机器人完全与楼梯对正，进入步骤A3；当只有一个三轴力传感器检测到力矩值正负号发生反转时，停止前进，然后驱动正负号未发生反转的一侧的履带向前转动，直至两个力矩值相等时履带停止转动，进入步骤A3；步骤A3：驱动移动机器人前进，直到两个三轴力传感器检测到的力矩值的正负号同时发生反转时，停止前进，放下导臂，当两个力矩值的正负号再次同时发生反转并且等到导臂履带底面与车体履带底面共面后，导臂停止下放，进入步骤A4；步骤A4：驱动移动机器人继续前进，同时不断调整左右履带的速度差，始终保持前进方向与离车体前端最近一级台阶的踏步前缘垂直，当两个三轴力传感器检测到的力矩值正负号同时反转时，停止前进，进入步骤A5；步骤A5：放下导臂，当两个三轴力传感器检测到的力矩值的正负号同时反转时，停止放下导臂，驱动移动机器人前进，直至两个三轴力传感器检测到力矩值的正负号连续两次同时发生反转时，上扬导臂，同时保持移动机器人继续前进，直至导臂上扬至与地面夹角为β度，完成自主上楼过程；所述自主下楼控制流程中，双目视觉传感器始终保持垂直朝向地面并实时对视野范围内的楼梯进行三维模型重建，包括以下步骤：步骤B1：移动机器人下放导臂至导臂履带底面与车体履带底面共面，将头部对准并驶向楼梯最顶一级台阶的踏步前缘的中点，实时检测双目视觉传感器中心与地面的垂直距离K，满足K>T时，停止前进，其中T为双目视觉传感器的中心与移动机器人车体履带底面的垂直距离；然后根据此时的K、T和W，按照下式计算楼梯的倾斜角度θ：θ＝(180*arctan(H/W))/π，其中H为通过K和T得到的踏步的高度，H＝K‑T，W为根据三维模型计算出的楼梯的踏步的宽度W；将楼梯的倾斜角度θ与移动机器人的最大爬坡度β作对比，如果θ≤β，则接受并确认下楼指令，进入步骤B2；否则驳回下楼指令，并发送警告信息给监控端的操作人员后结束自主下楼控制流程；步骤B2：驱动移动机器人前进，当两个三轴力传感器检测到力矩值的正负号同时反转且两个力矩值相等时，停止前进，进入步骤B3；当只有一个三轴力传感器检测到的力矩值的正负号反转时，移动机器人停止前进，驱动力矩值的正负号未发生反转的一侧的履带向前转动，直到两个三轴力传感器检测到的力矩值相等时，移动机器人停止移动，进入步骤B3；步骤B3：放下导臂，当两个三轴力传感器均检测到力矩值的正负号同时发生反转时，导臂停止下放，驱动移动机器人前进，当两个三轴力传感器检测到力矩值正负号连续两次同时发生反转时停止前进，进入步骤B4；步骤B4：上扬导臂直到导臂履带底面与移动机器人车体履带底面平行时停止，移动机器人前进，同时不断调整左右履带转动的速度差，始终保持前进方向与离车体前端最近一级台阶的踏步前缘垂直，直至双目视觉传感器检测不到台阶的边缘线时，上扬导臂，同时保持移动机器人继续向前移动，直至导臂上扬至与地面夹角为β度，完成自主下楼梯过程。