CN104002880A - 一种带有导臂的履带式移动机器人自主上下楼梯控制方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于机器人技术领域，涉及一种针对带有导臂的履带式移动机器人在运动过程中遇到楼梯时如何自主上下楼梯的控制方法。

背景技术

移动机器人正被越来越多的应用于危险环境监测、未知区域探测、行星探测、救援搜索、排爆等各种领域，在这些应用中，常常要需要攀爬楼梯，由于楼梯结构的特殊性和攀爬楼梯过程的复杂性使得移动机器人攀爬楼梯一直是移动机器人领域的一个难点和热点。

目前对移动机器人的最主要的控制方法是遥操作控制方法，具体方法是移动机器人通过自身携带的多种传感器采集现场环境的信息，然后将现场环境信息通过有线或无线的方式反馈给远程控制端，由操作人员在远程控制端根据反馈的信息作出判断后再通过有线或者无线的方式将控制指令发送给现场的移动机器人，从而控制移动机器人的行为。在遥操作控制方式下，移动机器人的运动完全取决于操作人员根据移动机器人反馈回的现场信息进行的判断与操作，由于信息的反馈和控制指令的发出存在时延或者由于反馈的信息精度不足等各种因素，使得操作人员在远端的临场感很差；另外由于操作人员要始终观察处理移动机器人反馈回的现场信息，精神高度集中，很容易视觉疲劳，这时容易判断失误，发出错误的控制指令。

移动机器人上下楼梯是一个非常复杂且需要精确控制的过程，操作人员稍有不慎可能会出现翻车现象，为了减少由于操作人员的操作失误而造成的损失，提出一种带有导臂的履带式移动机器人自主上下楼梯的控制方法，使得移动机器人可以自主完成上下楼的复合运动，上下楼的过程中不需要操作人员干预。

发明内容

技术问题：本发明提供了一种适用于直线型楼梯或弧形楼梯，能够使得移动机器人在遇到楼梯时可以仅仅根据操作人员发出的上楼或者下楼的简单指令就能实现自主上楼或自主下楼复合运动的带有导臂的履带式移动机器人自主上下楼梯的控制方法。

技术方案：本发明的带有导臂的履带式移动机器人自主上下楼梯控制方法，首先在履带式移动机器人车体顶面的前端设置面向前进方向的双目视觉传感器，在履带式移动机器人的导臂主动轮的旋转轴两端分别设置一个用于检测导臂力矩的三轴力传感器，上扬导臂至与地面夹角成β度，其中β为移动机器人的最大爬坡度，然后进行自主上楼和下楼控制流程；

自主上楼控制流程中，双目视觉传感器始终保持水平方向并实时对视野范围内的楼梯进行三维模型重建，包括以下步骤：

步骤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：上扬导臂直到导臂履带底面与移动机器人车体履带底面平行时停止，移动机器人前进，同时不断调整左右履带转动的速度差，始终保持前进方向与离车体前端最近一级台阶的踏步前缘垂直，直至双目视觉传感器检测不到台阶的边缘线时，上扬导臂，同时保持移动机器人继续向前移动，直至导臂上扬至与地面夹角为β度，完成自主下楼梯过程。

本发明方法的优选方案中，步骤A1中，将移动机器人头部对准并驶向第一级台阶的中间处的具体方法为：首先将双目视觉传感器视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后根据该三维模型得到楼梯的第一级台阶的踏步前缘的宽度m，调整左右履带的速度差使得显示屏上的三维模型的显示区域的中垂线始终垂直于该边缘线并位于该上边缘线宽度的处，即位于处，就可实现移动机器人将头部对准并驶向第一级台阶的中间处；

步骤B1中，将移动机器人头部对准并驶向楼梯最顶一级台阶的踏步前缘的中点的具体方法为：首先将双目视觉传感器视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后根据该三维模型得到楼梯最顶一级的台阶的踏步前缘的宽度n，调整左右履带的速度差使得显示屏的三维模型的显示区域的中垂线始终垂直于该边缘线并位于边缘线宽度的处，即位于处，就可实现移动机器人将头部对准并驶向楼梯最顶一级的台阶的踏步前缘的中点处。

本发明方法的优选方案中，步骤A4中，保持前进方向与离车体前端最近一级台阶的踏步前缘垂直的具体方法为：首先将双目视觉传感器视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后通过该三维模型得到离车体前端最近一级台阶的踏步前缘与显示屏的水平轴的夹角δ，然后实时调整左右履带的转速差使得能够减少夹角δ，当δ＝0时，代表移动机器人前进方向与离车体前端最近一级台阶的踏步前缘垂直；

步骤B4中，保持前进方向始终与离车体前端最近一级台阶的踏步前缘垂直的具体方法为：首先将双目视觉传感器视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后通过该三维模型得到离车体前端最近一级台阶的踏步前缘与显示屏的水平轴的夹角η，然后实时调整左右履带的转速差使得能够减少夹角η，当η＝0时，代表移动机器人前进方向与双目视觉传感器视野范围内的离车体前端最近一级台阶的踏步前缘垂直。

有益效果：与现有技术相比，本发明具有如下优点：

本发明针对带有导臂的履带式移动机器人系统提出了一种自主上下楼控制方法，该方法使得操作人员在监控设备的显示屏上发现楼梯并想让移动机器人上楼或者下楼时，只需要首先手动遥控移动机器人运动到楼梯的底部或者顶部，然后针对上楼或者下楼任务的不同适当调整移动机器人的姿态到适合上楼的姿态或适合下楼的姿态，然后发出上楼或者下楼指令，移动机器人就可以利用自身携带的双目视觉传感器和三轴力传感器实现自动对准并驶向楼梯，然后进行自主上楼或者下楼，在上楼或者下楼的过程中无需操作人员的干预；

移动机器人在上楼或者下楼的过程中能够根据双目视觉传感器实时对视野范围内的楼梯进行三维模型重建，通过该三维模型可以得到双目视觉传感器视野范围内的离车体最近一级台阶的踏步前缘与移动机器人监控端的显示屏的水平轴的夹角，通过调整左右履带的速度差使得该夹角始终保持为零，就可以实时保证移动机器人车体前进方向始终与视野范围内的离车体最近的一级台阶的踏步前缘垂直，正是由于在上楼或者下楼过程中可以始终自动保持车体前进方向与每一级台阶的踏步前缘垂直，从而使得移动机器人在自主上楼或者自主下楼过程中始终处于楼梯的中间位置，不会偏离楼梯，也正是因为在上下楼过程中移动机器人前进方向始终与每一级楼梯都垂直，所以本发明提出的方法既适用于直线型楼梯又适用于弧形楼梯甚至是螺旋形楼梯；

另外，移动机器人在自主上楼或下楼前，通过分析楼梯的三维模型可以计算出楼梯的陡峭程度，若陡峭程度过大超过了移动机器人的最大爬坡能力，则驳回操作人员发出的上楼或者下楼指令，并发送警告信息给操作人员，若楼梯的陡峭程度在移动机器人的可攀爬范围内，则确认并执行指令，移动机器人通过上楼或者下楼前对楼梯陡峭程度的分析，减少了因为操作人员判断失误发出错误指令而造成的翻车事故。

附图说明

图1是本发明实施例所使用的带导臂的履带式移动机器人示意图。

图2是本发明实施例所使用的带导臂的履带式移动机器人的监控设备示意图。

图3是移动机器人的导臂的长度、踏步的高度、踏步的宽度及双目视觉传感器中心与楼梯第一二级踢面的水平距离示意图。

图4是移动机器人的两个前轮越过第一级台阶的踏步前缘时的示意图。

图5是移动机器人导臂履带底面和车体履带底面共面时的示意图。

图6是移动机器人的两个前轮越过最顶一级台阶的踏步前缘时的示意图。

图7是移动机器人导臂下放至与楼梯顶部平台接触时的示意图。

图8是移动机器人导臂与楼梯顶部平台脱离接触时的示意图。

图9是移动机器人前端下落使导臂再次与楼梯顶部平台接触时的示意图。

图10是移动机器人完成自主上楼时的示意图。

图11是移动机器人的双目视觉传感器中心与车体履带底面的垂直距离、双目视觉传感器中心与地面的垂直距离、踏步的高度和踏步的宽度示意图。

图12是移动机器人导臂下放至与最顶一级台阶的下一级台阶的踏步前缘接触时的示意图。

图13是移动机器人前端下落使导臂再次与最顶一级台阶的下一级台阶的踏步前缘接触时的示意图。

图14是移动机器人导臂履带底面和车体履带底面共面时的示意图。

图15是移动机器人的攀爬到楼梯的底部时的示意图。

图16是移动机器人完成自主下楼时的示意图。

图中有：车体1，双目视觉传感器2，前轮3，后轮4，车体履带5，导臂主动轮6，导臂从动轮7，导臂履带8，导臂9，三轴力传感器10。

具体实施方式

下面结合实施例和说明书附图对本发明做进一步说明。

本实施例是一种针对最大爬坡能力为45度的带有导臂9的履带式移动机器人如何自主上下楼梯而提出的一种新的控制方法，首先在履带式移动机器人车体1顶面的前端设置面向前进方向的双目视觉传感器2，在履带式移动机器人的导臂主动轮6的旋转轴两端分别设置一个用于检测导臂9力矩的三轴力传感器10，上扬导臂9至与地面夹角成45度，45度为移动机器人的最大爬坡度，然后进行自主上楼和下楼控制流程；

本实施例所使用的楼梯是坡度为45度的直线型楼梯，梯段宽度为120厘米，踏步的高度为26厘米，踏步的宽度为26厘米；本实施例所使用的小型移动机器人如图1所示，其中1为车体，2为双目视觉传感器，3为前轮，4为后轮，5为车体履带，6为导臂主动轮，7为导臂从动轮，8为导臂履带，9为导臂，10为三轴力传感器，其中双目视觉传感器2安装在履带式移动机器人车体1顶面的前端，具有上下俯仰自由度，主要功能是对视野范围内的环境进行三维模型重建，车体1的前轮3为驱动轮，后轮4为随动轮，前轮3和后轮4包覆有车体履带5，导臂主动轮6和导臂从动轮7包覆有导臂履带8，导臂主动轮6与车体1前轮3共轴转动，所以前轮3转动会带动车体履带5和导臂履带8同时转动，导臂9还可以向上扬起或向下落下且两个导臂9的上扬和下落是同步的，三轴力传感器10有两个，分别安装在前轮3中轴的左右两端，用于分别测量左右导臂9对前轮3的转动轴产生的力矩F_L和F_R，两个导臂伸向车体前方且未与任何物体碰触时，F_L和F_R的值均为负值，即F_L＝F_R<0；

另外该移动机器人还配有一个监控设备，如图2所示，监控设备配备有显示屏可以实时显示双目视觉传感器2得到前方环境的三维模型，当操作人员在监控设备的显示屏上发现楼梯并想让移动机器人上楼或者下楼时，可以通过监控设备对移动机器人发出自主上楼指令或者自主下楼指令，另外在需要手动控制移动机器人的场合，操作人员可以通过该监控设备控制移动机器人；

自主上楼控制流程中，双目视觉传感器2始终保持水平方向并实时对视野范围内的楼梯进行三维模型重建，包括以下步骤：

步骤A1：移动机器人将头部对准并驶向第一级台阶的中间处，具体方法为：首先将双目视觉传感器2视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后根据该三维模型得到楼梯的第一级台阶的踏步前缘的宽度m，因为梯段宽度为120厘米，则m＝120厘米，调整左右履带的速度差使得显示屏上的三维模型的显示区域的中垂线始终垂直于该边缘线并始终位于该边缘线宽度的处，即位于60厘米处，就可实现移动机器人将头部对准并驶向第一级台阶的中间处；

在驶向楼梯的过程中，实时检测双目视觉传感器2中心分别与第一级踢面和第二级踢面的水平距离L1、L2，满足L1≤S时，停止前进，其中S为导臂的长度，如图3所示；然后根据此时检测到的L1、L2，按照下式计算出楼梯倾斜角度θ：

θ＝(180*arctan(H/W))/π，

其中H为通过三维模型计算出的第一级台阶踏步的高度，W为踏步的宽度，W＝L2-L1；

因为楼梯的踢面的高度和踏步的宽度为26厘米，所以这里计算出的H和W均为26厘米，所以θ＝45度，将楼梯的倾斜角度θ与移动机器人的最大爬坡度45作对比，这里楼梯的倾斜角度恰好是移动机器人可以攀爬的最大坡度，所以接受并确认上楼指令，进入步骤A2；

步骤A2：上扬导臂9至与地面夹角成45度，此时F_L＝F_R<0，驱动移动机器人前进，当F_L＝F_R<0变为F_L＝F_R>0时，认为左右两个导臂9同时与第一级台阶的踏步前缘接触，此时移动机器人完全与楼梯对正，进入步骤A3；

当只有一个力矩值从负值变为正值时，移动机器人停止前进，说明此时只有一个导臂9与第一级台阶的踏步前缘接触，认为此时移动机器人没有与楼梯完全对正；这里假设F_L从负值增大为正值，即F_L>0而F_R仍然为负值，认为此时左侧导臂9已搭在第一级台阶的踏步前缘上，而右侧导臂9还未与第一级台阶的踏步前缘接触；驱动右侧的前轮3向前转动从而带动右侧的车体履带5和右侧的导臂履带8向前转动，直至F_L＝F_R>0时，前轮3停止转动，此时移动机器人完全与楼梯对正，进入步骤A3；

步骤A3：驱动移动机器人前进，直到F_L＝F_R>0变为F_L＝F_R<0时，停止前进，认为此时移动机器人的前轮3已经越过第一级台阶的踏步前缘，如图4所示；放下导臂9，当F_L＝F_R<0变为F_L＝F_R>0并且等到导臂履带8底面与车体履带5底面共面后，导臂9停止下放，如图5所示；进入步骤A4；

步骤A4：驱动移动机器人继续前进，开始上楼，在此过程中不断调整左右履带的速度差，始终保持前进方向与离车体1前端最近一级台阶的踏步前缘垂直，具体方法为：首先将双目视觉传感器2视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后通过该三维模型得到离车体1前端最近一级台阶的踏步前缘与显示屏的水平轴的夹角δ，然后实时调整左右履带的转速差使得夹角δ＝0，当δ＝0时，代表移动机器人前进方向与离车体1前端最近一级台阶的踏步前缘垂直；

当F_L＝F_R>0变为F_L＝F_R<0时，停止前进，认为移动机器人的两个前轮3已经越过楼梯最上面一级台阶的踏步前缘，如图6所示；进入步骤A5；

步骤A5：放下导臂9，当F_L＝F_R<0变为F_L＝F_R>0时，停止放下导臂9，认为此时导臂9与楼梯顶部的平台接触，如图7所示；驱动移动机器人前进，导臂9脱离与楼梯顶部平台的接触，F_L＝F_R>0变为F_L＝F_R<0，如图8所示；当F_L＝F_R<0变为F_L＝F_R>0时，认为此时车体1前端在重力作用下落回地面，使得导臂9前端再次与楼梯顶部平台接触，如图9所示，此时上扬导臂9同时保持移动机器人继续前进，直至导臂9上扬至与地面夹角为45度，完成自主上楼过程，如图10所示；

自主下楼控制流程中，双目视觉传感器2始终保持垂直朝向地面并实时对视野范围内的楼梯进行三维模型重建，包括以下步骤：

步骤B1：移动机器人下放导臂9至导臂履带8底面与车体履带5底面共面，由于导臂9与地面接触，所以导臂主动轮6的旋转轴上的两个三轴力传感器检10测到的力矩值为正值即F_L＝F_R>0；

将头部对准并驶向楼梯最顶一级的台阶的踏步前缘的中点，具体方法为：首先将双目视觉传感器2视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后根据该三维模型得到楼梯最顶一级台阶的踏步前缘的宽度n，因为梯段的宽度为120厘米，则n＝120厘米，调整左右履带的速度差使得显示屏的三维模型的显示区域的中垂线始终垂直于该边缘线并始终位于该边缘线宽度的处，即位于60厘米处，就可实现移动机器人将头部对准并驶向楼梯最顶一级台阶的踏步前缘的中点处；

在驶向楼梯最顶一级台阶的踏步前缘的中点的过程中，实时检测双目视觉传感器2中心与地面的垂直距离K，满足K>T时，停止前进，其中T为双目视觉传感器2的中心与移动机器人车体履带5底面的垂直距离，如图11所示；然后根据此时的K、T和W，按照下式计算楼梯的倾斜角度θ：

θ＝(180*arctan(H/W))/π，

其中H为通过K和T得到的踏步的高度，H＝K-T，W为根据三维模型计算出的楼梯的踏步的宽度W；因为楼梯的踏步的高度和踏步的宽度为26厘米，所以这里计算出的H和W均为26厘米，所以θ＝45度，将楼梯的倾斜角度θ与移动机器人的最大爬坡度45度作对比，这里楼梯的倾斜角度恰好是移动机器人可以攀爬的最大坡度，所以移动机器人接受并确认下楼指令，进入步骤B2；

步骤B2：驱动移动机器人前进，当F_L＝F_R>0变为F_L＝F_R<0时，停止前进，认为此时移动机器人完全与楼梯对正，且移动机器人两个前轮3均已经越过最顶一级台阶的踏步前缘，进入步骤B3；

当只有一个力矩值从正值变为负值时，移动机器人停止前进，认为只有一个前轮3越过最顶一级台阶的踏步前缘，说明此时移动机器人没有与楼梯完全对正；这里假设F_L从正值增大为负值，即F_L<0而F_R仍然为正值，认为此时左侧前轮3已经越过最顶一级台阶的踏步前缘，而右侧的前轮3还没有越过最顶一级台阶的踏步前缘；驱动右侧的前轮3向前转动从而带动右侧的车体履带5和右侧的导臂履带8向前转动，直到F_L＝F_R<0时，移动机器人停止移动，认为此时移动机器人完全与楼梯对正，且移动机器人两个前轮3均已经越过最顶一级台阶的踏步前缘，进入步骤B3；

步骤B3：放下导臂9，当F_L＝F_R<0变为F_L＝F_R>0时，导臂9停止下放，认为此时左右两个导臂9搭在最顶层台阶的下一级台阶的踏步前缘上，如图12所示；驱动移动机器人前进，导臂9脱离与踏步前缘的接触，F_L＝F_R>0变为F_L＝F_R<0；当F_L＝F_R<0变为F_L＝F_R>0时停止前进，认为此时移动机器人由于重心在前端的原因而使得车体1后端翘起从而使得左右两个导臂9再次搭在最顶层台阶的下一级台阶的踏步前缘上，如图13所示；进入步骤B4；

步骤B4：导臂9上扬直到导臂履带8底面与移动机器人车体履带5底面平行时停止，如图14所示；移动机器人前进，开始下楼，在下楼过程中不断调整左右履带转动的速度差，始终保持前进方向与离车体1前端最近一级台阶的踏步前缘垂直，具体方法为：首先将双目视觉传感器2视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后通过该三维模型得到双目视觉传感器2视野范围内的离车体1前端最近一级台阶的踏步前缘与显示屏的水平轴的夹角η，然后实时调整左右履带的转速差使得夹角η＝0，当η＝0时，代表移动机器人前进方向与双目视觉传感器2视野范围内的离车体1最近一级台阶的踏步前缘垂直；

直到双目视觉传感器2检测不到台阶的边缘线时，如图15所示，上扬导臂9同时保持移动机器人继续向前移动，直至导臂9上扬至与地面夹角为45度，完成自主下楼梯过程，如图16所示。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干可以预期的改进和等同替换，这些对本发明权利要求进行改进和等同替换后的技术方案，均落入本发明的保护范围。

Claims (3)

1.一种带有导臂的履带式移动机器人自主上下楼梯控制方法，其特征在于，该方法首先在履带式移动机器人车体顶面的前端设置面向前进方向的双目视觉传感器，在履带式移动机器人的导臂主动轮的旋转轴两端分别设置一个用于检测导臂力矩的三轴力传感器，上扬导臂至与地面夹角成β度，其中β为移动机器人的最大爬坡度，然后进行自主上楼和下楼控制流程；

所述自主上楼控制流程中，双目视觉传感器始终保持水平方向并实时对视野范围内的楼梯进行三维模型重建，包括以下步骤：

步骤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：上扬导臂直到导臂履带底面与移动机器人车体履带底面平行时停止，移动机器人前进，同时不断调整左右履带转动的速度差，始终保持前进方向与离车体前端最近一级台阶的踏步前缘垂直，直至双目视觉传感器检测不到台阶的边缘线时，上扬导臂，同时保持移动机器人继续向前移动，直至导臂上扬至与地面夹角为β度，完成自主下楼梯过程。

2.根据权利要求1所述的一种带有导臂的履带式移动机器人自主上下楼梯控制方法，其特征在于，所述步骤A1中，将移动机器人头部对准并驶向第一级台阶的中间处的具体方法为：首先将双目视觉传感器视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后根据该三维模型得到楼梯的第一级台阶的踏步前缘的宽度m，调整左右履带的速度差使得显示屏上的三维模型的显示区域的中垂线始终垂直于该边缘线并位于该上边缘线宽度的处，即位于处，就可实现移动机器人将头部对准并驶向第一级台阶的中间处；

所述步骤B1中，将移动机器人头部对准并驶向楼梯最顶一级台阶的踏步前缘的中点的具体方法为：首先将双目视觉传感器视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后根据该三维模型得到楼梯最顶一级的台阶的踏步前缘的宽度n，调整左右履带的速度差使得显示屏的三维模型的显示区域的中垂线始终垂直于该边缘线并位于边缘线宽度的处，即位于处，就可实现移动机器人将头部对准并驶向楼梯最顶一级的台阶的踏步前缘的中点处。

3.根据权利要求1所述的一种带有导臂的履带式移动机器人自主上下楼梯控制方法，其特征在于，

所述步骤A4中，保持前进方向与离车体前端最近一级台阶的踏步前缘垂直的具体方法为：首先将双目视觉传感器视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后通过该三维模型得到离车体前端最近一级台阶的踏步前缘与显示屏的水平轴的夹角δ，然后实时调整左右履带的转速差使得能够减少夹角δ，当δ＝0时，代表移动机器人前进方向与离车体前端最近一级台阶的踏步前缘垂直；

所述步骤B4中，保持前进方向始终与离车体前端最近一级台阶的踏步前缘垂直的具体方法为：首先将双目视觉传感器视野范围内的楼梯的三维模型显示在移动机器人监控端显示屏上，然后通过该三维模型得到离车体前端最近一级台阶的踏步前缘与显示屏的水平轴的夹角η，然后实时调整左右履带的转速差使得能够减少夹角η，当η＝0时，代表移动机器人前进方向与双目视觉传感器视野范围内的离车体前端最近一级台阶的踏步前缘垂直。