一种行星轮式越障机器人爬楼梯控制方法与流程

本发明属于机器人控制领域，更具体地，涉及一种行星轮式越障机器人爬楼梯控制方法。

背景技术：

随着科学技术的发展，人们越来越需要机器人在许多恶劣环境下进行工作，如：星球表面探索，消防，火场探测，有毒、易燃、易爆场所探测以及无人战场等。复杂、未知、多变的非结构环境对机器人的越障能力提出了很高的要求，越障机器人应具备良好的适应性、稳定性和灵活性，所以针对越障机器人的控制研发具有广泛的应用前景。

现有越障机器人种类各有优点也各有不足：轮式越障机器人结构简单，控制方便，负重比高，能量效率高，在平坦路面负载行进时较其他种类机器人具有绝对优势，但在崎岖地形下其越障性能较差，比如重载车辆，虽然负重能力强，行进速度也快，但需要路面平坦；履带式越障机器人支撑面积大，下陷度小，地形适应能力强，但其结构复杂质量大，能量利用率低，比如坦克，越障能力强，但体积质量大，一般不用来负重；足式越障机器人运动灵活，地形适应能力强，但其结构和控制都很复杂，负重比较低，例如Boston Dynamics公司的R.Playter,M.Buehler和M.Raibert等人在《BigDog》一文中提出的BigDog机器人，自重90kg，负重50kg，自身长度1m，最大速度为0.7m/s。

华中科技大学硕士论文《一种新型越障机器人的设计与开发》里面介绍了一种越障机器人，其体积小，质量轻，采用了6个独立的行星轮组作为驱动，具有良好的越障能力，但该机器人的驱动组在偏航方向没有自由度，在爬有转弯弧度的楼梯时不能使左右两边的行星轮组同时翻转，左前行星轮组和右前行星轮组分别在上下两级台阶上，即车身的左前端和右前端处于不同高度，车身严重歪斜，容易导致翻车。因此，研究设计一种新型的爬楼梯控制方法具有重要的应用价值。

技术实现要素：

针对现有技术的以上缺陷或改进需求，本发明提供了一种机器人爬楼梯控制方法，其中结合行星轮式越障机器人自身的特点，相应设计了适用于行星轮式越障机器人的爬楼梯控制方法，不仅能适应机器人车身与楼梯成一定倾斜角度的情况，而且能适应每一级台阶为内侧窄，外侧宽且转弯半径越小每级台阶内外两侧宽度相差越大的转弯台阶等情况，具有控制操作简单，转向精度高等优点。

为实现上述目的，本发明提出了一种行星轮式越障机器人爬楼梯控制方法，包括如下步骤：

(1)初始状态下使越障机器人保持匀速直线前进，然后测量越障机器人同一驱动组中两行星轮组最前端与前方楼梯台阶的距离L₁和L₂，并测出各行星轮组的转速，根据所述L₁和L₂计算获得偏转角θ；

(2)由偏转角控制算法计算出各行星轮组的理论转速，并将计算得出的理论转速输入到各行星轮组的电机驱动器中以驱动行星轮组运动；

(3)经过一个调整周期后检测当前的L₁和L₂，并计算当前的偏转角θ；

(4)判断当前偏转角θ是否超过预设值：若是，则转入步骤(2)，若否，则控制越障机器人继续匀速直线前进；

(5)判断越障机器人驱动组中两行星轮组是否到达阶梯：若否，则继续匀速直线前进，若是，则两行星轮组一起翻转，翻越台阶；

(6)判断两行星轮组是否到达目的地：若否，则进入新的位姿调整过程，重复步骤(1)～(5)，若是，则结束运动，以此方式完成行星轮式越障机器人爬楼梯的控制。

作为进一步优选的，所述偏转角θ采用如下公式获得：

式中：l_AB表示越障机器人同一驱动组中两行星轮组的中心点之间的距离。

作为进一步优选的，所述由偏转角控制算法计算出各行星轮组的理论转速，具体包括：

(2.1)首先计算越障机器人位姿调整的周期时间T：

式中：k₁为比例修正系数，ω₁,ω₂分别为行星轮组中左右两轮的转速，r为行星轮组中单个轮子的半径；

(2.2)然后计算位姿调整的幅值A：

式中：k₂为比例系数，θ为偏转角；

(2.3)再计算转速的差值：

式中：t为一周期内位姿调整经历的时间，t∈(0,T)；

(2.4)最后计算出行星轮式越障机器人同一驱动组中两行星轮组的理论转速ω₁'、ω₂'：

ω₁'＝ω₁+Δω,ω₂'＝ω₂-Δω。

作为进一步优选的，判断越障机器人驱动组中两行星轮组是否到达阶梯，具体为：

判断L₁和L₂是否满足如下条件：L₁＝L₂＝L_min，其中，L_min为越障机器人到达台阶时L的值。

作为进一步优选的，所述行星轮式越障机器人优选为四行星轮越障机器人或六行星轮越障机器人。

作为进一步优选的，所述行星轮式越障机器人为四行星轮越障机器人时，其设有前后两组驱动组，每组驱动组中的两行星轮组均按所述步骤(1)-(6)进行控制。

作为进一步优选的，所述行星轮式越障机器人为六行星轮越障机器人时，其设有前中后三组驱动组，每组驱动组中的两行星轮组均按所述步骤(1)-(6)进行控制。

作为进一步优选的，所述楼梯优选为螺旋楼梯或弧形楼梯。

作为进一步优选的，步骤(4)中所述的预设值具体为0～3°，偏转角在该角度范围内可使得同一驱动组中左右两轮同时翻越台阶。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：

1.本发明基于行星轮式越障机器人的结构特点专门设计了一种新式的爬楼梯控制方法，其不仅能适应机器人车身与楼梯成一定倾斜角度的情况，而且能适应每一级台阶都为内侧窄，外侧宽且转弯半径越小每级台阶内外两侧的宽度相差越大的转弯台阶的情况，可实现任意给定偏转角的转向爬梯运动，提高了越障机器人的转弯灵活度和转弯精度。

2.本发明针对左右行星轮组可能存在高度差的问题，在驱动组翻越台阶时，将左右行星轮组与台阶间的距离进行比较，调整两行星轮的转速，使两行星轮同时到达阶梯，进而保证车体左右高度基本无变化，保证转弯爬梯的平稳性。

3.本发明通过实时的调整两行星轮的转速，控制左右行星轮组与台阶间的距离，对不同台阶具有很强的适应性，对于不同的台阶都能保证使两行星轮同时到达阶梯。

附图说明

图1是六行星轮越障机器人的结构示意图；

图2是六行星轮越障机器人的前驱动组结构示意图；

图3是六行星轮越障机器人的前车体和后车体铰接位置示意图；

图4是越障机器人相对阶梯的位姿俯视图；

图5(a)-(f)是越障机器人位姿调整过程的俯视图；

图6是越障机器人前驱动组控制方法流程图；

图7是越障机器人控制系统框图；

图8是四行星轮越障机器人的结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1～3所示，本发明中运用的行星轮式越障机器人，包括车体及用于支撑车体的三个驱动组，所述车体包括前车体100和后车体200，前车体100通过水平设置的铰接轴10铰接在后车体200上，所述的三个驱动组按从前至后的顺序依次为前驱动组300(前梁)、中驱动组400(中梁)以及后驱动组500(后梁)。

其中，前驱动组300包括支撑梁303、两个动力驱动装置305和306、两个超声波距离传感器307和308、两个行星轮组301和302；行星轮组301包括三个半径大小相同的轮子，轮子的半径取为r。两个行星轮组左右设置，左行星轮组301安装在左驱动装置305上，左驱动装置305用于驱动左行星轮组301移动，右行星轮组302安装在右驱动装置306上，右驱动装置306用于驱动右行星轮组302移动，超声波距离传感器307固接在左驱动装置305上，超声波距离传感器308固接在右驱动装置306上，超声波距离传感器307和308位于行星轮组301和302的中心点的连线上。左右两驱动装置305和306固定在支撑梁303上，驱动组300通过竖直设置的铰接轴304铰接在前车体上。所述中驱动组400以及后驱动组500的结构与前驱动组300的结构相同。

下面结合图4～7说明越障机器人在车身与楼梯成一定倾斜角度的情况下，翻越每一级台阶都是内侧窄、外侧宽且转弯半径越小每级台阶内外两侧的宽度相差越大的转弯台阶(例如螺旋楼梯，旋转楼梯，弧形楼梯)的控制方法。

如图5所示，图5(a)为初始状态下行星轮式越障机器人的车身与第一节台阶成一定的角度；图5(b)为前梁左右两轮转速调整使两行星轮同时到达第一级台阶；图5(c)为前梁两行星轮组同时翻越第一级台阶面；图5(d)为前梁左右两轮转速调整使两行星轮同时到达第二级台阶，同时中梁左右两轮转速调整使两行星轮同时到达第一级台阶；图5(e)为前梁两行星轮组同时翻越第二级台阶面，中梁两行星轮组同时翻越第一级台阶面，同时后梁左右两轮转速调整使两行星轮同时到达第一级台阶；图5(f)为前梁左右两轮转速调整使两行星轮同时到达第三级台阶，中梁两行星轮组转速调整使两行星轮同时到达第二级台阶，后梁两行星轮组同时翻越第一级台阶面。

对于保证左右两行星轮保持在同一高度上翻越台阶面，以前梁左右两轮转速调整为例，起始状态下行星轮式越障机器人的车身与第一节台阶成一定的角度，具体位姿调整步骤如下：

(1)在翻越第一级台阶前，初始状态下越障机器人以初始速度匀速直线前进(即同一驱动组中两行星轮以初始转速转动，并且两者的初始转速相同)，初始速度可根据实际需要进行设定，由超声波距离传感器测量越障机器人同一驱动组(此处为前驱动组)中两行星轮组最前端与前方障碍物(即前方与行星轮组距离最近的一个楼梯台阶的侧面)的距离L₁和L₂，并由转速传感器测出两行星轮组的转速ω₁、ω₂(转速ω₁、ω₂初始值相同)，根据所述L₁和L₂计算获得偏转角θ。

具体的，如图4所示，301、302、401、402、501和502分别对应越障机器人的六个行星轮组，C₁、C₂和C₃表示依次增高的三个台阶面。偏转角θ表示车身与阶梯的倾斜程度，L₁和L₂分别表示超声波距离传感器307和308测得的车身到前方最近台阶面的距离，标记中驱动组的中心为点O，标记超声波距离传感器307的位置为点A，标记超声波距离传感器308的位置为点B，A为行星轮组301的中心点，B为行星轮组302的中心点，则：

其中，l_AB表示越障机器人同一驱动组(此处为前驱动组)中两行星轮组的中心点之间的距离，也即超声波距离传感器307与超声波距离传感器308之间的距离。

(2)由偏转角控制算法计算出各行星轮组的理论转速，并将计算得出的理论转速输入到各行星轮组的电机驱动器中以驱动行星轮组运动。

如图7所示，角度PID反馈控制包括角度PID控制模块701、速度PID控制模块702和系统状态检测模块703，速度PID控制模块702由速度PID控制704、电机执行模块705和霍尔测速模块706组成。

角度PID控制模块701根据当前各轮角速度ω_i(i＝1,2)以及当前偏转角θ对理论转速进行计算，具体见式(2)～(5)。速度PID控制模块702根据角度PID模块701得出的理论转速调整电机的角速度，系统状态检测模块703对控制中用到的系统状态量进行实时检测，包括角度传根器、角速度传感器以及超声波距离传感器。

由偏转角控制算法计算出各行星轮组的理论转速，具体为：

(2.1)计算越障机器人位姿调整的周期时间T：

式中：k₁为比例修正系数，保证越障机器人到达台阶前完成一个周期的位姿调整，0<k₁<0.5，可根据具体路况调整k₁值；ω₁,ω₂分别为行星轮组中左右两轮的转速，r为行星轮组中单个轮子的半径；

(2.2)计算位姿调整的幅值A：

式中：θ为当前偏转角，k₂为比例系数，使得一个调整周期内完成越障机器人位姿的调整，k₂的取值由具体路况和越障机器人的额定速度决定；

(2.3)计算转速的差值：

式中：t为从本周期内位姿调整经历的时间，t∈(0,T)；

(2.4)计算出行星轮式越障机器人同一驱动组(即前驱动组)中左右两行星轮组的理论转速ω₁'、ω₂'：

ω₁'＝ω₁+Δω,ω₂'＝ω₂-Δω (5)

(3)经过一个调整周期的时间T后，检测当前的L₁和L₂，并计算当前的偏转角θ；

(4)判断当前偏转角θ是否超过预设值：若超过预设值，则转入步骤(2)，若没有超过预设值，则控制越障机器人前驱动组左右两轮继续以初始速度匀速直线前进，其中，预设值取0～3°，偏转角在这个角度范围内可以满足左右两轮同时翻越台阶。

(5)判断越障机器人驱动组中两行星轮组是否到达阶梯：若机器人未到达阶梯，则继续以初始速度匀速直线前进，若机器人到达阶梯，则前驱动组(即前梁)中对应的两行星轮组一起翻转，翻越台阶。

判断越障机器人驱动组中两行星轮组是否到达阶梯，具体为：

判断L₁和L₂是否满足如下条件：L₁＝L₂＝L_min，其中，L_min为越障机器人到达台阶时L的值(即越障机器人到达台阶时，行星轮组与台阶的距离)，L_min的值可由越障机器人两行星轮组前端紧挨台阶面时，测量超声波传感器到达台阶的距离得到。

(6)由操控者判断两行星轮组是否到达目的地：若没有到达目的地，则越障机器人前梁进入新的位姿调整过程，重复步骤(1)～(5)，若到达目的地，则结束运动，以此方式完成行星轮式越障机器人前梁的爬楼梯控制。

在越障机器人前梁爬楼过程中，中梁和后梁也同时爬楼，其控制方法与前梁爬楼的控制方法相同，即前梁翻越台阶后，中梁和前梁翻越台阶的步骤相同，对应的，中梁两个行星轮组重复步骤(1)～(6)；中梁翻越台阶后，后梁和前梁翻越台阶的步骤相同，对应的，后梁两个行星轮组重复步骤(1)～(6)，以此完成行星轮式越障机器人爬楼梯的控制。

本发明所涉及到的控制方法也可适用于四行星轮越障机器人，四行星轮越障机器人结构如图8所示。

图8中，100代表车体，200代表前驱动组，300代表后驱动组，前驱动组200通过竖直设置的铰接轴10铰接在车体上，驱动组300通过竖直设置的铰接轴20铰接在车体上。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。