专利名称：一种具有掘进功能的矿难救灾机器人及其控制方法
技术领域：
本发明涉及一种六足机器人及其控制领域，具体是指具有掘进功能的矿难救灾六足机器人及其控制方法。
背景技术：
煤炭行业是工业生产中伤亡事故最严重的行业。长期以来，煤矿灾难发生后的救援一直是一个热门的研究课题，由于灾害矿井具有以下特点空间狭小、矿难现场巷道结构不稳定、矿难现场内具有可燃性气体等，以上特点对于救援人员的救援作业和人身安全会造成极大的影响和危害。可移动机器人在矿难发生后能够代替人类进入矿井，并通过其上搭载的各类传感器(温度传感器、一氧化碳浓度传感器、瓦斯浓度传感器、声音探测器、定位传感器、摄像头等)完成危险的井下环境探测和辅助救援工作，提高救援效率，避免或减少救援人员的伤亡。轮式机器人在相对平坦的路面上具有良好的移动性，能量利用效率高，但是在复杂的地面下移动性受到很大限制，不适于灾害矿井的救援工作。足式机器人，尤其是六足机器人具有良好的环境适应能力，在灾害矿井的非结构环境下能够稳定行走，但是移动速度慢，能量利用效率低，不能满足快速救援和能量补给困难的要求。当前，用于矿难救灾的机器人主要以履带式机器人为主，履带式机器人具有地形适应能力强和运动速度快的特点， 但是履带式结构的机器人要实现掘进功能，需在机器人前端设计专门的掘进机构，并在机器人内部设计碎料传送结构，从而导致整个系统结构复杂，体节庞大，还会带来较大的能量消耗，不适用于矿难救灾场合使用。当前，还没有具备掘进功能的矿难救灾机器人，一旦灾难矿井被堵塞，机器人就无法进入或穿过堵塞区进行救援，影响救援效果。
发明内容
本发明是为克服上述现有技术所存在的不足之处，提供一种具有掘进功能的矿难救灾机器人，为辅助矿难救灾提供一种机器人移动平台。该机器人采用六腿结构，采用三角步态行进控制方式，环境适应能力强，在行进控制上采用轮式机器人的驱动原理，移动速度快，而且具备掘进功能，能够进入、穿越土堆、煤堆和废墟等介质，从而进入被堵塞的救援现场，克服了现有矿难救灾机器人不能穿越和进入废墟的缺点，而且行进和掘进机构复合一体，使机器人体积和质量更小，能量消耗更少，便于进入狭小空间长时间作业。本发明为解决技术问题采用如下技术方案本发明具有掘进功能的矿难救灾机器人的结构特点是包括锥形钻土头部、圆柱形的前体节和圆柱形的后体节，所述前体节和后体节通过能够在平面上转动的腰部相连接；在所述前体节和后体节的两侧对称设置有各支腿，所述各支腿包括位于前体节两侧的左前腿和右前腿、位于后体节两侧的左中腿、右中腿、左后腿和右后腿，所述各支腿通过由转动副构成的髋关节与所述前体节或后体节相连接，各支腿的末端为弧面足部；锥形钻土头部固连在前体节的前端。
本发明具有掘进功能的矿难救灾机器人的结构特点也在于所述支腿的结构设置为连接在所述髋关节上的为大腿，所述大腿通过膝关节连接小腿，所述膝关节是以所述大腿的下半部分为机架，以小腿的顶端为连杆，在所述机架与连杆之间分别连接有摇杆和曲柄，构成曲柄摇杆机构；所述曲柄摇杆机构中与大腿末端相连的曲柄是由膝关节驱动电机驱动的原动杆。所述腰部结构设置包括原动齿轮、从动齿轮、上固定板和下固定板，所述原动齿轮轴和从动齿轮轴分别通过轴承支承在上固定板和下固定板之间；所述上固定板和下固定板与后体节固定连接，从动齿轮轴与前体节固定连接，所述原动齿轮是由固定设置在上固定板上的腰部驱动电机驱动。本发明具有掘进功能的矿难救灾机器人的行进控制方法的特点是采用基于三角步态的行进控制方式，由左前腿、左后腿和右中腿组成左三角，由右前腿、右后腿和左中腿组成右三角，各支腿在髋关节驱动电机的驱动下进行转动；支腿转动至足底部与地面接触的状态为支撑态，足底部离开地面的状态为摆动态；所述左三角和右三角交替呈摆动态和支撑态；任一时刻至少有一个三角处于支撑态，同一三角中的三条支腿在同一时刻进入支撑态或摆动态。本发明具有掘进功能的矿难救灾机器人的掘进控制方法的特点是按如下过程进行a、基于三角步态运动的前向钻孔控制过程机器人以三角步态前向运动，同时钻土头部由头部驱动电机驱动旋转，当达到给定的掘进量时，停止前向运动，调整机器人的左三角和右三角均为支撑态；b、基于六腿支撑的扩孔控制过程前体节相对于后体节在腰部驱动电机的驱动下左右摆动，通过改变各支腿的角度调整机器人的俯仰角，进而使钻土头部上下运动，钻土头部的转动配合其左右摆动和上下运动，使机器人钻出截面较大的孔洞，便于机器人通过。本发明具有掘进功能的矿难救灾机器人的拨土控制方法的特点是采用五腿支撑单腿顺序轮流拨土的控制方式，在机器人掘进过程结束后，调整为五条支腿支撑状态，另外一条腿为拨土腿，拨土腿的大腿静止，小腿在膝关节驱动电机的驱动下带动弧面足往复摆动，实现向后方拨土。与已有技术相比，本发明有益效果体现在1、本发明结构简单，环境适应能力强。机器人行进和掘进机构复合一体，结构简单，体积和能耗小；六条腿在体节两侧呈对称分布，机器人发生倾覆后仍能够正常行走，适合于灾难矿井下的复杂环境。2、本发明腿部结构的功能多，控制方法简单。大腿同一方向上的连续转动驱动机器人行进，小腿的往复摆动带动足部拨土，各支腿转动角度的协调配合还能使钻土头部上下运动，进而完成掘进过程中的扩孔任务，而且能够调整腿长；通过单独控制髋关节或膝关节运动能够完成某种运动功能，无需两个关节运动的耦合控制，控制方法简单，控制误差小。3、本发明腰部结构紧凑，可靠性高。机器人腰部采用齿轮组结构，可靠性高，转动角度控制的精度高。4、本发明行进稳定性好、速度快，便于控制的实现。基于三角步态的行进控制方法在机器人行进过程中始终保持对机器人的三点支撑，类似于足式机器人的控制特点，稳定性好，能够在非结构环境下稳定行走；大腿同一方向上的连续转动驱动机器人行进，类似于轮式机器人控制的特点，行进速度快；行进过程中腿部状态仅有摆动态和支撑态两种状态， 驱动电机的运动规划简单，便于控制的实现。5、本发明采用基于三角步态运动的前向钻孔控制和基于六腿支撑的扩孔控制相结合的掘进控制方法，利用机器人钻土头部的转动，配合腿部和腰部的动作，能够实现机器人在废墟中的掘进，无需专门的掘进结构，且掘进效率高。6、本发明采用五腿支撑单腿顺序轮流拨土的控制方式，五腿支撑使机器人稳定性高，单足拨土范围可调，拨土效果好。
图1为本发明的总体结构示意图2为本发明机器人剖面图3为本发明机器人腿部组成示意图4a、图4b为本发明机器人不同腿长的调整示意图5为本发明机器人腰部结构示意图6为本发明机器人行进中髋关节转角规划图7为本发明机器人行进过程中髋关节转角控制方框图
图8为本发明机器人掘进控制流程图9为本发明机器人总体控制流程图。
具体实施例方式参见图1和图2，本实施例中矿难救灾机器人包括有锥形钻土头部1、圆柱形的前体节2、圆柱形的后体节3，其中，前体节2和后体节3通过能够在平面上转动的腰部4相连接；在前体节2和后体节3的两侧对称设置有各支腿，各支腿包括位于前体节2两侧的左前腿如和右前腿恥、位于后体节3两侧的左中腿5c、右中腿5d、左后腿k和右后腿5f，各支腿通过由转动副构成的髋关节与所述前体节2或后体节3相连接，各支腿的末端为弧面足部；锥形钻土头部1固连在前体节2的前端。机器人六腿在体节两侧呈对称分布，并且腿的长度大于体节的高度，这种形式使得机器人运动时不分“上下”和“前后”，机器人发生倾覆后，只需改变部分控制参数仍然能够继续行进。参见图3，本实施例中矿难救灾机器人的支腿结构设置为连接在髋关节上的为大腿6，大腿通过膝关节连接小腿7，膝关节是以大腿的下半部分为机架8，以小腿的顶端为连杆9，在机架8与连杆9之间分别连接有摇杆10和曲柄11，构成曲柄摇杆机构；曲柄摇杆机构中与大腿末端相连的曲柄11是由膝关节驱动电机12驱动的原动杆。曲柄摇杆机构组成的膝关节中原动杆同一方向上的连续转动带动小腿的前后往复摆动，进而带动弧面足部拨土，膝关节驱动电机的等速转动转化弧面足部在拨土过程中的非等速摆动，即随着拨土过程中足部拨土面上土量的增加，足部负载变大，此时足部摆动速度变慢，拨土力矩增大，当拨土到位后足部负载迅速减小，此时利用曲柄摇杆机构的急回特性，足部快速摆动，这种结构和运动特性允许用较小功率的膝关节驱动电机实现顺利拨土。通过调整膝关节原动杆的角度可以进行腿长调整，如图如和图4b所示，腿长指的是从髋关节中心点至足部前端的距离lt，It的大小由原动杆的角度确定，图如和图4b表示原动杆在不同角度时的腿长。参见图5，本实施例中矿难救灾机器人腰部结构设置包括原动齿轮13、从动齿轮 14、上固定板15和下固定板16，原动齿轮轴17和从动齿轮轴18分别通过轴承支承在上固定板15和下固定板16之间；上固定板15和下固定板16与后体节3固定连接，从动齿轮轴 18与前体节2固定连接，原动齿轮13是由固定设置在上固定板15上的腰部驱动电机19驱动。这一结构形式能使前体节在腰部驱动电机的驱动下在水平面内左右摆动，在腰部驱动电机锁死后，前体节和后体节之间近似为刚性连接，前后体节成为一个整体。机器人行进控制方法的基本原理是采用基于三角步态的行进控制方式，由左前腿5a、左后腿k和右中腿5d组成左三角，由右前腿5b、右后腿5f和左中腿5c组成右三角，各支腿在髋关节驱动电机20的驱动下进行转动；支腿转动至足底部与地面接触的状态为支撑态，足底部离开地面的状态为摆动态；所述左三角和右三角交替呈摆动态和支撑态； 任一时刻至少有一个三角处于支撑态，同一三角中的三条支腿在同一时刻进入支撑态或摆动态。图6为三角步态行进方式下髋关节转动角度规划图，图中粗实线为左三角腿的相角变化轨迹，粗虚线为右三角腿的相角变化轨迹。在运动过程中，每个三角存在两种状态， 即支撑态和摆动态，处于支撑态的腿起驱动机器人运动的作用。任一时刻，至少有一个三角处于支撑态，以保证机器人的稳定性。各腿的控制向量u包含四个参数t。、ts、c5s* Δ Φ3, 其中t。为任一三角处于支撑态和摆动态的总时间，ts为支撑态的时间，为支撑态时腿转过的角度，摆动态时腿转过的角度为2 π -Φ3，ΔΦ3为位于机器人左侧腿在驱动状态时转过的角度与位于机器人右侧腿在驱动状态时转过的角度之差。各参数在控制中的作用分别为t。控制机器人的运动速度，t。越小行进速度越大，反之行进速度越小；ts控制支撑态的
时间，机器人稳定行走要求G 。3控制支撑态时腿转过的角度，越大则机器人行进
过程中重心起伏越大，反之起伏越小；Δ 控制机器人行进过程中的转弯，Δ Φ3的符号和大小分别控制转弯方向和转弯幅度。通过调整各腿控制参数能够实现机器人的直线运动、 原地转弯运动和行进中转弯运动。a.直线运动。直线运动时Δ = 0，各腿转动方向相同。b.原地转弯。原地转弯时Δ = 0，位于身体左侧和右侧的腿转动方向相反。C.行进中转弯。行进中转弯时ΔΦ3#0，当ΔΦ3>0时向右转弯，反之向左转弯，转弯幅度由I Δ Φ3决定，I Δ φ3|越大转弯幅度越大，即转弯半径越小。机器人行进过程中由于受到环境的干扰，各髋关节转动的角度会与规划出的角度产生误差，随着累误差不断积累，最终会导致各腿相位和速度失调，机器人失去稳定性。为了保证机器人步态稳定，采用PD控制，具体原理是以一定的时间间隔通过角度传感器测量各髋关节的转角Φ，并与各髋关节的期望转角Od进行比较，从而得到各髋关节的转角误差Φ e，根据此误差及其微分对膝关节驱动电机进行控制。
图7为机器人行进过程中髋关节转角控制方框图，其中PD控制器的控制参数，即比例系数和微分系数通过试验的手段离线获取，也可以通过机器学习方法进行在线获取， 以进行自适应控制。参见图8，机器人掘进控制按如下过程进行a、基于三角步态运动的前向钻孔控制过程机器人以三角步态前向运动，同时钻土头部1由头部驱动电机21驱动旋转，当达到给定的掘进量时，停止前向运动，调整机器人的左三角和右三角均为支撑态；b、基于六腿支撑的扩孔控制过程前体节2相对于后体节3在腰部驱动电机19的驱动下左右摆动，通过改变各支腿的角度调整机器人的俯仰角，进而使钻土头部上下运动， 钻土头部1的转动配合其左右摆动和上下运动，使机器人钻出截面较大的孔洞，便于机器人通过。基于六腿支撑的扩孔控制有三种运动控制方式a、钻土头部上下方向上保持在某一位置上进行左右摆动，左右方向钻土，而后钻土头部在上下方向上移至另一位置后继续进行左右方向上的钻土，上述过程多次反复后实现扩孔；b、钻土头部左右方向上保持在某一位置上进行上下摆动，上下方向钻土，而后钻土头部在左右方向上移至另一位置后继续进行上下方向上的钻土，上述过程多次反复后实现扩孔；C、钻土头部上下和左右方向上同时摆动实现扩孔。前两种控制方式简单、易于实现，但扩孔效率较低；第三种控制方式运动规划复杂，对控制的实时性要求高，但扩孔效率高。本实施例中具有掘进功能的矿难救灾机器人的拨土控制方法是采用五腿支撑单腿顺序轮流拨土的控制方式，在机器人掘进过程结束后，调整为五条支腿支撑状态，另外一条腿为拨土腿，拨土腿的大腿静止，小腿7在膝关节驱动电机12的驱动下带动弧面足往复摆动，实现向后方拨土。拨土腿的轮换顺序依次为左前腿fe、右前腿恥、左中腿5c、右中腿 5d、左后腿k、右后腿5f，一个轮换周期后将身体下方的土拨至身后。机器人整体运动控制流程如图9所示，控制系统启动后，首先进行自检，自检通过后进行任务模式的选择，若选择行走模式，则机器人进入本体行走控制模式；若选择掘进模式，则机器人进入掘进控制模式，无论在何种模式下，当机器人接收到停止指令后，则电机下电。当机器人自检不通过时，则驱动电机系统立即下电。
权利要求
1.一种具有掘进功能的矿难救灾机器人，其特征在于包括锥形钻土头部(1)、圆柱形的前体节( 和圆柱形的后体节(3)，所述前体节( 和后体节C3)通过能够在平面上转动的腰部(4)相连接；在所述前体节( 和后体节( 的两侧对称设置有各支腿，所述各支腿包括位于前体节( 两侧的左前腿(5a)和右前腿( )、位于后体节C3)两侧的左中腿 (5c)、右中腿(5d)、左后腿(5e)和右后腿(5f)，所述各支腿通过由转动副构成的髋关节与所述前体节( 或后体节C3)相连接，各支腿的末端为弧面足部；锥形钻土头部(1)固连在前体节O)的前端。
2.根据权利1所述的具有掘进功能的矿难救灾机器人，其特征在于所述支腿的结构设置为连接在所述髋关节上的为大腿(6)，所述大腿通过膝关节连接小腿(7)，所述膝关节是以所述大腿的下半部分为机架(8)，以小腿的顶端为连杆(9)，在所述机架(8)与连杆(9) 之间分别连接有摇杆(10)和曲柄(11)，构成曲柄摇杆机构；所述曲柄摇杆机构中与大腿末端相连的曲柄(11)是由膝关节驱动电机(1 驱动的原动杆。
3.根据权利要求1所述的具有掘进功能的矿难救灾机器人，其特征在于所述腰部结构设置包括原动齿轮(13)、从动齿轮(14)、上固定板(1 和下固定板(16)，所述原动齿轮轴(17)和从动齿轮轴(18)分别通过轴承支承在上固定板(1 和下固定板(16)之间；所述上固定板(1 和下固定板(16)与后体节(3)固定连接，从动齿轮轴(18)与前体节O)固定连接，所述原动齿轮(1 是由固定设置在上固定板(1 上的腰部驱动电机(19)驱动。
4.一种权利要求1所述的具有掘进功能的矿难救灾机器人的行进控制方法，其特征在于采用基于三角步态的行进控制方式，由左前腿Ga)、左后腿(5e)和右中腿(5d)组成左三角，由右前腿(恥)、右后腿(5f)和左中腿(5c)组成右三角，各支腿在髋关节驱动电机 (20)的驱动下进行转动；支腿转动至足底部与地面接触的状态为支撑态，足底部离开地面的状态为摆动态；所述左三角和右三角交替呈摆动态和支撑态；任一时刻至少有一个三角处于支撑态，同一三角中的三条支腿在同一时刻进入支撑态或摆动态。
5.权利要求1所述的具有掘进功能的矿难救灾机器人的掘进控制方法，其特征在于按如下过程进行a、基于三角步态运动的前向钻孔控制过程机器人以三角步态前向运动，同时钻土头部(1)由头部驱动电机驱动旋转，当达到给定的掘进量时，停止前向运动，调整机器人的左三角和右三角均为支撑态；b、基于六腿支撑的扩孔控制过程前体节( 相对于后体节( 在腰部驱动电机(19) 的驱动下左右摆动，通过改变各支腿的角度调整机器人的俯仰角，进而使钻土头部上下运动，钻土头部的转动配合其左右摆动和上下运动，使机器人钻出截面较大的孔洞，便于机器人通过。
6.权利要求1所述的具有掘进功能的矿难救灾机器人的拨土控制方法，其特征在于 采用五腿支撑单腿顺序轮流拨土的控制方式，在机器人掘进过程结束后，调整为五条支腿支撑状态，另外一条腿为拨土腿，拨土腿的大腿静止，小腿(7)在膝关节驱动电机(1 的驱动下带动弧面足往复摆动，实现向后方拨土。
全文摘要
本发明公开了一种具有掘进功能的矿难救灾机器人，其特征在于包括锥形钻土头部、圆柱形前体节和圆柱形后体节，前体节和后体节通过能够在平面上转动的腰部相连接；在前体节和后体节的两侧对称设置有各支腿，各支腿通过由转动副构成的髋关节与前体节或后体节相连接，各支腿的末端为弧面足部；锥形钻土头部固连在前体节的前端。本发明可以根据空间大小调整腿长，环境适应能力；能够在土堆、废墟等介质中通过钻头松土和足部拨土配合实现掘进功能。该机器人能够辅助用于矿难救灾、野外探险等危险任务。
文档编号E21F11/00GK102434196SQ201110370250
公开日2012年5月2日 申请日期2011年11月21日 优先权日2011年11月21日
发明者冯勇, 孙建, 孙玉苹, 孙玉香, 张强, 曹会彬, 武遵, 王以俊, 葛运建 申请人:中国科学院合肥物质科学研究院