CN117572858A - 一种四足履带机器人控制平衡与修正导航方向的行进方法 - Google Patents

Abstract

本发明提供一种四足履带机器人控制平衡与修正导航方向的行进方法，具体步骤包括（1）预设行进路线，开启行进指令，驱使机器人开始行进；（2）传感器实时监测机器人是否怕偏离路线；（3）控制模块发出修正指令；（4）检测舵机一和舵机二的状态，并修正机器人的路线；本发明能够使得机器人在修正偏离路线时，能实现姿态修正，且通用性好，且方法可靠性好。

Description

一种四足履带机器人控制平衡与修正导航方向的行进方法

技术领域

本发明涉及机器人技术领域，具体涉及一种四足履带机器人控制平衡与修正导航方向的行进方法。

背景技术

当前国内甚少有能够在复杂的地形下能够进行救援以及进行补给的机器人，轮式机器人和四足式机器人是其中的两种，其中轮式机器人主要有轮子或履带等结构驱动，其动力较为强劲，实用性广泛；而四足式机器人有更强的灵活性，但行进速度相对较慢，机器人的背部可以承载一定重量的物资，在一些自然灾害中可以为提供补给援助。

而中国专利申请号为202210366509.2，公布日为2022.07.08的专利文献，其公开了一种四足机器人基于trot步态的平衡控制算法，该方法包括以下步骤：S1、设计四足机器人的步态，并给出步态时序图；S2、搭建平衡控制框图，从期望状态的输入到期望关节力矩的输出，根据四足机器人期望状态的导数和当前状态，求得期望的地面反作用力，地面反作用力包括沿x、y轴的摩擦力和沿z轴的支撑力，再根据期望的地面反作用力和四足机器人单腿的雅可比矩阵求得期望的关节力矩的大小，用以保证四足机器人最基本的平衡控制，解决了四足机器人在任一时刻下都处于自不稳定的状态的问题。

该专利文献通过搭建平衡控制框图，根据四足机器人期望状态的导数和当前状态，期望的地面反作用力和四足机器人单腿的雅可比矩阵求得期望的关节力矩的大小，用以保证四足机器人基本的平衡控制，但是该方法在机器人出现路线偏离需要修正而做出姿态调整时，但是该方法主要是通过trot算法对姿态进行调整，而对于不同的机器人其由于腿部长度的区别从而使得不同机器人能转动角度上有所区别，从而对trot算法的姿态调整具有一定的限定，并且对于不同的机器人都需要重新计算其姿态调整的限定角度，从而使得操作麻烦，通用性差，另外在利用trot算法进行姿态调整时，若机器人遇到障碍物时，若调整后的姿态使得腿部的力矩值达到一定量若再按照trot算法向障碍物方向频繁调整将会导致腿部转动的力矩过大从而导致机器的损坏，从而导致机器人的寿命差。

发明内容

本发明的目的在于提供一种四足履带机器人控制平衡与修正导航方向的行进方法，使得机器人在修正偏离路线时，能实现姿态修正，且通用性好，且方法可靠性好。

为达到上述目的，一种四足履带机器人控制平衡与修正导航方向的行走方法，四足履带机器人包括躯体和腿部结构，所述腿部结构包括舵机驱动装置、第一限位连接组件、第二限位连接组件和履带驱动装置，所述舵机驱动装置通过舵机连接件设置在所述躯体的两侧，所述舵机驱动装置包括舵机一、U形连接件、舵机固定板和舵机二，所述舵机二的输出轴端通过连接外壳与所述履带驱动装置转动连接，所述U形连接件的一端与所述舵机一的输出轴端转动连接，所述U形连接件的另一端通过舵机固定板与所述舵机二固定连接，所述履带驱动装置通过连接外壳与所述舵机驱动装置转动连接；

所述方法包括以下步骤：

（1）预设行进路线，开启行进指令，驱使机器人开始行进；

（2）传感器实时监测机器人是否偏离路线，包括（2.1）-（2.3）；

（2.1）陀螺仪实时检测加速度，获取加速度的大小和方向；

（2.2）判别加速度出现偏移现象或加速度方向与预设行进路线的方向不在同一水平线上，则识别为机器人偏离了预设的行进路线；

（2.3）若识别到机器人偏离了预设的行进路线，陀螺仪记录当前状态加速度的大小和方向，并将数据传送给控制模块；

（3）控制模块发出修正指令；

（4）检测舵机一和舵机二的状态，并修正机器人的路线；（4）具体包括（4.1）-（4.3）；

（4.1）建立XYZ空间坐标系，根据小腿长度、大腿长度、小腿与大腿之间的连接长度以及足端坐标计算舵机一转角和舵机二转角；

（4.2）将舵机一转角和舵机二转角以及Trot步态算法，进而转换为舵机一和舵机二的修正角度，同时舵机一和舵机二在向修正角度转动时，在行进的前t/4处于加速状态，在行进中的t/2处于匀速状态，在行进的后t/4处于减速状态，t为Trot步态算法中预设的一个步态周期，完成平衡控制下的行进姿态调整，实现修正路线；

（4.3）当通过舵机一和舵机二转角与Trot步态算法结合调整行进姿态时，若检测到机器人前方具有障碍物，确定腿部结构的力矩的大小，若腿部结构的力矩大于最高堵转力矩，则退出Trot步态算法调整行进姿态或使得腿部结构切换到Trot行走周期中的摆动项。

以上设置，通过设置舵机连接件将舵机驱动安装连接在躯体的两侧，能够通过腿部结构带动躯体行进；在舵机一与舵机二之间设置U形连接件，通过U形连接件将舵机一和舵机二转动连接，从而能够通过舵机一带动舵机二进行转动；而舵机驱动装置通过连接外壳与履带驱动连接，进而通过舵机二转动带动履带驱动实现仿生步态行进，并且可以灵活地转换成履带驱动，舵机驱动装置静止，同时保持履带与地面紧贴，通过驱动电机带动履带转动，从而实现机器人稳定高效行进，同时在四足行进过程中，根据腿部的长度以及足端的坐标确定大腿的驱动舵机二以及大腿与小腿之间驱动舵机一的转动角度，然后根据转动角度确定期待的姿势，并通过Trot算法得到之后的姿势修正，这样能在根据不同的机器人之后无需对机器人的转动角度的设置进行逐一设置，通用性强，另外通过确定舵机一和舵机二的转动角度之后可以确定为了实现足端坐标下确保在行进路线下进行行进方向的调整，然后结合Trot算法进行姿态调整，从而使得姿态调整更加可靠有效，另外由于在遇到障碍物时，通过计算腿部结构的力矩是否超出预设值，在超出预设值之后将不再进行步态调整或使得腿部结构切换到Trot行走周期中的摆动项，这样使得超负荷的腿部结构不再受到过多的支撑力的冲击从而损坏机器人，可靠性高。

进一步的，所述第一限位连接组件包括第一连接杆、第一限位环、第一圆盘，所述第一圆盘通过横杆与所述舵机固定板固定连接，所述第一圆盘上设有第一凸块，所述第一限位环套接在第一凸块上，第一限位环的一端与第一连接杆的一端相连，所述第一连接杆的另一端与所述U形连接件连接，所述第二限位连接组件包括第二连接杆、第二限位环、第二圆盘，所述第二圆盘通过横杆与所述舵机固定板固定连接，所述第二圆盘上设有第二凸块，所述第二限位环套接在第二凸块上，第二限位环的一端与第二连接杆的一端相连，所述第二连接杆的另一端与第二舵盘连接，所述舵机二的输出轴端通过连接外壳与所述履带驱动装置转动连接。

以上设置，当需要通过四足进行行走时，通过驱动舵机一和舵机二进行工作从而腿部姿态之间切换进而实现四足进行行走，当需要通过履带进行行进时，舵机驱动装置静止，同时保持履带与地面紧贴，由于第一限位连接组件中第一连接杆的另一端与U形连接件相连，第一连接杆的一端与第一限位环套接，第二限位连接组件具有相同结构设置，从而使得履带与底面相贴合时，受到底面向上的力时，第一凸块和第二凸块分别在第一限位环以及第二限位环移动到最顶端，从而防止躯体受到地面瞬间冲击力进而使得躯体向上运动过大从而使得躯体与U形连接件之间容易出现损坏的情况，同时当行驶到突然变高的地面时，第一凸块和第二凸块分别在第一限位环和第二限位环内移动到最低端，从而使得防止躯体受到地面高处影响使得躯体向下移动过低从而导致躯体与U形连接件之间损坏，仅通过一套腿部结构即可实现履带行走也可以实现四足行走，结构简单，且可靠性好。

进一步的，所述躯体上设置有控制模块；在所述躯体的顶部设置有用于识别和检测前方的行进路况的摄像头，摄像头与控制模块电连接；所述履带驱动装置包括履带、履带固定件和驱动电机，所述履带固定件设置在所述履带上并设有通孔，所述驱动电机的输出端穿过通孔与所述履带转动连接，用于驱动所述履带转动；

步骤（4.1）中具体还包括以机器人右侧方向为Z轴方向，以机器人后侧方向为Y轴方向，以垂直Y轴和Z轴所在平面的方向为X轴方向，建立XYZ空间坐标系，在XOY坐标系中，设置小腿长度为I₁，大腿长度为I₂，小腿与大腿之间的连接长度为，预设足端坐标为（x，y），舵机二转角/>通过公式（1）得到；

（1）；

舵机一转角θ₄通过公式（2）得到；

（2）。

以上设置，通过在躯体上设置控制模块和摄像头，能够通过摄像头识别和检测机器人行进的路况，并将路况数据传输给控制模块，从而控制模块通过路况，进而控制舵机驱动和履带驱动；舵机驱动装置通过连接外壳与履带驱动装置转动连接，进而通过舵机二转动带动履带驱动装置实现仿生步态行进，能够使得履带驱动与舵机驱动之间灵活转换，同时由于通过以机器人的侧面作为XY坐标平面，然后以垂直于XY坐标平面做Z轴，这样使得腿部运动转动主要通过大腿与小腿之间的夹角作为转角以及小腿与Y轴的夹角作为转角即可实现到达足端坐标，从而计算方便且可靠。

进一步的，所述舵机固定板包括第一舵机固定板和第二舵机固定板；所述舵机连接件上设有通孔，所述舵机连接件的一端与所述躯体连接，所述舵机连接件的另一端通过第一舵机固定板与所述舵机一固定连接；

步骤（4.3）中具体还包括通过判断腿部结构的力矩大小，实现平衡行进姿态调整，包括：

（4.3.1）通过腿部结构建立动力学模型，在XYZ空间坐标系中，根据空间坐标的惯性矩阵、耦合矩阵、速度以及加速度通过动力学方程，计算得到小腿的力矩和大腿的力矩；

（4.3.2）将小腿的力矩和大腿的力矩/>中至少一个大于舵机的最高堵转力矩进行比较，控制模块控制腿部结构的舵机一和舵机二停止转动，则退出Trot步态算法调整行进姿态或使得腿部结构切换到Trot行走周期中的摆动项。

以上设置，通过在躯体与腿部结构之间设置舵机连接件，从而使得躯体与腿部结构连接形成整体，保证了机器人能够在舵机驱动和履带驱动之间灵活转换，从而实现机器人稳定高效行进，同时通过动力学方程计算出大腿和小腿的力矩从而确定是否需要退出Trot算法，实现方法简单。

进一步的，所述U形连接件上设置有通孔，所述U形连接件的一端与所述舵机一的输出轴端转动连接，所述U形连接件的另一端通过第二舵机固定板与所述舵机二固定连接。

以上设置，通过在舵机一和舵机二之间设置U形连接件，能够将舵机一和舵机二连接，同时使得舵机一能够带动舵机二进行转动，从而使得腿部结构实现仿生步态行进。

进一步的，所述舵机二的输出轴端与第二舵盘转动连接，所述连接外壳的一端与第二舵盘连接，所述连接外壳的另一端与履带固定件连接。

以上设置，通过在舵机二和履带固定件之间设置外壳连接件，使得舵机二通过外壳连接件与履带固定件连接，从而实现舵机驱动与履带驱动进行连接。

进一步的，所述第二舵盘与所述第二连接杆的一端连接，所述第二连接杆的另一端与所述第二限位环的一端连接，所述第二限位环的另一端套接在第二凸块上。

以上设置，通过第二连接杆与第二舵盘连接，在只有履带驱动行进时，可以限制舵机二转动角度过大，从而限制机器人行进过程中躯体下降的高度。

进一步的，所述U形连接件靠近所述舵机一输出轴端的一端与所述第一连接杆的一端固定连接，舵机一的输出轴端通过第一舵机与U形连接件转动连接，所述第一连接杆的另一端与所述第一限位环的一端连接，所述第一限位环的另一端套接在第一凸块上。

以上设置，通过第一连接杆与U形连接件连接，在只有履带驱动行进时，可以限制舵机一转动角度过大，从而限制机器人行进过程中躯体下降的高度。

进一步的，第二连接件包括第二一连接杆和第二二连接杆，第二一连接杆的一端与第二限位环铰接，第二一连接杆的另一端与第二二连接杆的一端固定连接，第二二连接杆的另一端设置有第二弧形杆，第二弧形杆与第二舵盘的外周壁相贴，第二弧形杆固定在设置连接外壳上，第二一连接杆的另一端与第二二连接杆一端具有夹角。

以上设置，通过将第二二连接杆的另一端设置为第二弧形杆，然后第二弧形杆与第二舵盘的外周壁相贴，从而第二弧形杆既不会影响第二舵盘的转动，同时也能与外接外壳相连，通过第二一连接杆与第二二连接杆之间的夹角设置，使得第二凸块在第二限位环内移动更加可靠，结构简单且可靠。

进一步的，第一连接件包括第一一连接杆和第一二连接杆，第一一连接杆的一端与第一限位环铰接，第一一连接杆的另一端与第一二连接杆的一端固定连接，第一二连接杆的另一端设置有第一弧形杆，第一弧形杆与第一舵盘的外周壁相贴，第一弧形杆固定在设置U形连接件上，第一一连接杆的另一端与第一二连接杆一端具有夹角。

以上设置，通过将第一二连接杆的另一端设置为第一弧形杆，然后第一弧形杆与第一舵盘的外周壁相贴，从而第一弧形杆既不会影响第一舵盘的转动，同时也能与U形连接件相连，通过第一一连接杆与第一二连接杆之间的夹角设置，使得第一凸块在第一限位环套内移动更加可靠，结构简单且可靠。

附图说明

图1为本发明的整体结构示意图。

图2为本发明另一视角的整体结构示意图。

图3为本发明的腿部结构爆炸图。

图4为本发明的限位连接组件、U型连接件以及舵盘连接示意图。

图5为本发明在XYZ坐标系中计算舵机一和舵机二转角的示意图。

图6为本发明的方法步骤流程图。

图7为本发明的Trot步态算法中一个步态周期内的示意图。

具体实施方式

下面结合附图和具体实施方式对本发明做进一步详细说明。

如图1至图7所示，一种四足履带机器人控制平衡与修正导航方向的行走方法，四足履带机器人包括躯体1和腿部结构2，躯体1上设置有控制模块12，用于协调控制机器人；在躯体1的顶部设置有摄像头13，用于识别和检测前方的行进路况，并将路况数据传输给控制模块12，从而使得控制模块12根据路况数据控制腿部结构2，实现机器人行进，本申请中，控制模块12为现有技术在此不再累述；本结构中腿部结构2具有四个，分别在躯体1的两侧的四个位置，腿部结构2包括舵机驱动装置、第一限位连接组件21、第二限位连接组件22和履带驱动装置，舵机驱动装置包括舵机一201、U形连接件202、舵机固定板和舵机二203，其中舵机固定板包括第一舵机固定板40和第二舵机固定板41，第一限位连接组件21包括第一连接杆211、第一限位环212、第一圆盘213，第二限位连接组件22包括第二连接杆221、第二限位环222、第二圆盘223，履带驱动装置包括履带24、履带固定件25和驱动电机26。

本实施例中，如图3所示，舵机驱动装置还包括舵机连接件3，舵机连接件3上设有通孔，舵机连接件3的一端通过螺栓穿过通孔与躯体1固定连接，舵机连接件3的另一端通过螺栓穿过通孔与第一舵机固定板40固定连接，同时舵机一21安装在第一舵机固定板40上，舵机一201的输出轴端与U形连接件202的一端转动连接，本实施例中，舵机一201的输出轴端通过第一舵盘与U形连接件转动连接，而U形连接件202的另一端通过螺栓穿过通孔与第二舵机固定板41固定连接，同时舵机二203安装在第二舵机固定板41上，使得舵机一201和舵机二203通过U形连接件202进行连接，舵机一201通过第一舵机固定板40固定在躯体1上，然后通过舵机一201的输出端使得U形连接件202相对于舵机一201转动，从而实现舵机一201带动舵机二203进行转动，从而使得腿部结构2实现仿生步态行进。

本实施例中，如图3所示，第一圆盘213通过横杆4与第一舵机固定板40固定连接，第一凸块214设置在第一圆盘213上，第一限位环212套接在第一凸块214上，第一限位环212的另一端与第一连接杆211的一端铰接，第一连接杆211的另一端与U形连接件202靠近舵机一201输出轴端的一端固定连接，使得U形连接件202相对于舵机一201转动，进而带动第一连接杆211转动，从而实现第一限位环212绕着第一凸块214转动，同时形成滑动连接；

第二圆盘223通过横杆4与第二舵机固定板41固定连接，第二凸块224设置在第二圆盘223上，第二限位环222套接在第二凸块224上，第二限位环222的另一端与第二连接杆221的一端铰接，第二连接杆221另一端与第二舵盘6固定连接，同时第二舵盘6安装在连接外壳7上，本实施例中，第二连接外壳7的上端设置有缺口（图中未示意出），第二舵盘6设置在缺口内，且舵机二203的输出轴端与第二舵盘6转动连接，同时第二舵盘6与连接外壳7的一端转动连接，使得履带驱动装置相对于舵机二203进行转动，进而带动第二连接杆221转动，连接外壳7的另一端与履带固定件25固定连接，履带固定件25设置在履带24上并设有通孔，驱动电机26的输出端穿过通孔与履带24转动连接，用于驱动履带24转动；通过在舵机二203和履带固定件25之间设置连接外壳7，使得舵机二203通过连接外壳7与履带固定件25连接，从而实现舵机驱动与履带驱动进行连接，使得机器人能够实现仿生步态行进与履带驱动行进灵活转换，从而实现机器人稳定高效行进。

如图4所示，第二连接件221包括第二一连接杆2211和第二二连接杆2212，第二一连接杆2211的一端与第二限位环222铰接，第二一连接杆2211的另一端与第二二连接杆2212的一端固定连接，第二二连接杆2212的另一端设置有第二弧形杆2213，第二弧形杆2213与第二舵盘6的外周壁相贴，第二弧形杆2213固定在设置连接外壳上7，本实施例中，第二二连接杆2212上设置有固定孔，通过螺钉穿过固定孔将第二二连接杆2212固定在连接外壳7上，第二一连接杆2211的另一端与第二二连接杆2212一端具有夹角；通过将第二二连接杆2212的另一端设置为第二弧形杆2213，然后第二弧形杆2213与第二舵盘6的外周壁相贴，从而第二弧形2213杆既不会影响第二舵盘6的转动，同时也能与连接外壳7相连，通过第二一连接杆2211与第二二连接杆2212之间的夹角设置，使得第二凸块221在第二限位环222内移动更加可靠，结构简单且可靠。

如图4所示，第一连接件211包括第一一连接杆2111和第一二连接杆2112，第一一连接杆211的一端与第一限位环212铰接，第一一连接杆2111的另一端与第一二连接杆2112的一端固定连接，第一二连接杆2112的另一端设置有第一弧形杆2113，第一弧形杆2113与第一舵盘8的外周壁相贴，第一弧形杆2113固定在设置U形连接件202上，第一一连接杆211的另一端与第一二连接杆2112一端具有夹角；通过将第一二连接杆2112的另一端设置为第一弧形杆2113，然后第一弧形杆2113与第一舵盘8的外周壁相贴，从而第一弧形杆2113既不会影响第一舵盘8的转动，同时也能与U形连接件202相连，通过第一一连接杆2111与第一二连接杆2112之间的夹角设置，使得第一凸块214在第一限位环212内移动更加可靠，结构简单且可靠。

本实施例中，如图6所示，一种四足履带机器人控制平衡与修正导航方向的行进方法，具体步骤包括：

（1）预设行进路线，开启行进指令，驱使机器人开始行进。

（2）传感器实时监测机器人是否怕偏离路线，包括（2.1）-（2.3）。

（2.1）陀螺仪实时检测加速度，获取加速度的大小和方向；

（2.2）判别加速度出现偏移现象或加速度方向与预设行进路线的方向不在同一水平线上，则识别为机器人偏离了预设的行进路线；

（2.3）陀螺仪记录当前状态加速度的大小和方向，并将数据传送给控制模块。

（3）控制模块发出修正指令。

（4）检测驱动舵机的状态，并修正机器人的路线，（4）具体包括（4.1）-（4.3）；

（4.1）以机器人右侧方向为Z轴方向，以机器人后侧方向为Y轴方向，以垂直Y轴和Z轴所在平面的方向为X轴方向，建立XYZ空间坐标系，在XOY坐标系中，设置小腿长度为I₁，大腿长度为I₂，小腿与大腿之间的连接长度为，预设足端坐标为（x，y），有

，

根据余弦定理有：

，

可以得到舵机二转角

；

接着计算小腿与大腿之间连接直线与小腿之间的角度θ₃，该公式依然通过余弦定理得到：

，

而对于小腿与大腿连接直线的转角θ₁，

；

则有

，

计算得出

；

其中，舵机一转角为θ₄，舵机二转角为θ₅；

（4.2）将舵机一转角和舵机二转角作为参数加入Trot步态算法的空间坐标中，进而转换为舵机的修正角度，同时舵机在向修正角度转动时，在行进的前t/4处于加速状态，在行进中的t/2处于匀速状态，在行进的后t/4处于减速状态，t为Trot步态算法中预设的一个步态周期，完成平衡控制下的行进姿态调整，实现修正路线；本实施例中，t/4处于加速状态可以通过（2.3）陀螺仪记录当前状态加速度的大小实现，同时参看图7所示，Trot步态算法是步态周期t内调整各个腿部运动上的姿态，比如在前t/4周期时，需要右后腿向前移动，左前腿向上移动，从而可以确定右后腿以及左前腿上舵机一和舵机二的转动角度之后的预期姿态，然后根据Trot步态算法进行调整，同时舵机一和舵机二进行加速转动从而使得姿态进行调整的同时也实现偏航路线的调整，前t/4进行加速转动能使得偏航路线能尽快进行调整确保调整的可靠性。

（4.3）当通过舵机一和舵机二转角与Trot步态算法结合调整行进姿态时，若检测到机器人前方具有障碍物，确定腿部结构的力矩的大小，若腿部结构的力矩大于最高堵转力矩，则退出Trot步态算法调整行进姿态或使得腿部结构切换到Trot行走周期中的摆动项，本实施例中障碍物的检测可以通过摄像头获取前方路况的图片之后并进行识别，若物品超过一定大小则识别为障碍物，具体包括：

（4.3.1）通过腿部结构建立动力学模型，在XYZ空间坐标系中，根据空间坐标的惯性矩阵、耦合矩阵、速度、重力载荷以及加速度通过动力学方程，计算得到小腿的力矩和大腿的力矩/>，本实施例中，通过使用matlab插件Robotic toolbox可以求出对应力矩值，首先，计算重力荷载采用gravload()函数，其次，使用inertia()计算惯性矩阵M(x)之后，使用criolis()函数计算关节之间的耦合矩阵W(x,v)，最后使用payload()函数计算重力荷载G(x)，最后，使用inertia()函数求出的惯性矩阵，并用nofriction()函数求解速度v，然后根据陀螺仪测量到的加速度a，然后通过以下动力学方程计算出驱动力矩/>，/>=M（x）*a+W（x,v）+G（x），其中x表示的是关节位置，比如是大腿还是小腿。

（4.3.2）将小腿的力矩和大腿的力矩/>中至少一个大于舵机的最高堵转力矩进行比较，控制模块控制腿部结构的舵机一和舵机二停止转动，则退出Trot步态算法调整行进姿态或使得腿部结构切换到Trot行走周期中的摆动项。本实施例中，最高堵转力矩是预设好的，为/>。

本发明的工作原理：通过设置舵机连接件将舵机驱动安装连接在躯体的两侧，能够通过腿部结构带动躯体行进；在舵机一与舵机二之间设置U形连接件，通过U形连接件将舵机一和舵机二转动连接，从而能够通过舵机一带动舵机二进行转动；而舵机驱动装置通过连接外壳与履带驱动连接，进而通过舵机二转动带动履带驱动实现仿生步态行进，并且可以灵活地转换成履带驱动，舵机驱动装置静止，同时保持履带与地面紧贴，通过驱动电机带动履带转动，从而实现机器人稳定高效行进，同时在四足行进过程中，根据腿部的长度以及足端的坐标确定大腿的驱动舵机二以及大腿与小腿之间驱动舵机一的转动角度，然后根据转动角度确定期待的姿势，并通过Trot算法得到之后的姿势修正，这样能在根据不同的机器人之后无需对机器人的转动角度的设置进行逐一设置，通用性强，另外通过确定舵机一和舵机二的转动角度之后可以确定为了实现足端坐标下确保在行进路线下进行行进方向的调整，然后结合Trot算法进行姿态调整，从而使得姿态调整更加可靠有效，另外由于在遇到障碍物时，通过计算腿部结构的力矩是否超出预设值，在超出预设值之后将不再进行步态调整或使得腿部结构切换到Trot行走周期中的摆动项，这样使得超负荷的腿部结构不再受到过多的支撑力的冲击从而损坏机器人，可靠性高。

Claims (10)

1.一种四足履带机器人控制平衡与修正导航方向的行进方法，四足履带机器人包括躯体和腿部结构，其特征在于：所述腿部结构包括舵机驱动装置、第一限位连接组件、第二限位连接组件和履带驱动装置，所述舵机驱动装置通过舵机连接件设置在所述躯体的两侧，所述舵机驱动装置包括舵机一、U形连接件、舵机固定板和舵机二，所述舵机二的输出轴端通过连接外壳与所述履带驱动装置转动连接，所述U形连接件的一端与所述舵机一的输出轴端转动连接，所述U形连接件的另一端通过舵机固定板与所述舵机二固定连接，所述履带驱动装置通过连接外壳与所述舵机驱动装置转动连接；

所述方法包括以下步骤：

（1）预设行进路线，开启行进指令，驱使机器人开始行进；

（2）传感器实时监测机器人是否偏离路线，包括（2.1）-（2.3）；

（2.1）陀螺仪实时检测加速度，获取加速度的大小和方向；

（2.2）判别加速度出现偏移现象或加速度方向与预设行进路线的方向不在同一水平线上，则识别为机器人偏离了预设的行进路线；

（2.3）若识别到机器人偏离了预设的行进路线，陀螺仪记录当前状态加速度的大小和方向，并将数据传送给控制模块；

（3）控制模块发出修正指令；

（4）检测舵机一和舵机二的状态，并修正机器人的路线；（4）具体包括（4.1）-（4.3）；

（4.1）建立XYZ空间坐标系，根据小腿长度、大腿长度、小腿与大腿之间的连接长度以及足端坐标计算舵机一转角和舵机二转角；

（4.2）将舵机一转角和舵机二转角以及Trot步态算法，进而转换为舵机一和舵机二的修正角度，同时舵机一和舵机二在向修正角度转动时，在行进的前t/4处于加速状态，在行进中的t/2处于匀速状态，在行进的后t/4处于减速状态，t为Trot步态算法中预设的一个步态周期，完成平衡控制下的行进姿态调整，实现修正路线；

（4.3）当通过舵机一和舵机二转角与Trot步态算法结合调整行进姿态时，若检测到机器人前方具有障碍物，确定腿部结构的力矩的大小，若腿部结构的力矩大于最高堵转力矩，则退出Trot步态算法调整行进姿态或使得腿部结构切换到Trot行走周期中的摆动项。

2.根据权利要求1所述的一种四足履带机器人控制平衡与修正导航方向的行进方法，其特征在于：所述第一限位连接组件包括第一连接杆、第一限位环、第一圆盘，所述第一圆盘通过横杆与所述舵机固定板固定连接，所述第一圆盘上设有第一凸块，所述第一限位环套接在第一凸块上，第一限位环的一端与第一连接杆的一端相连，所述第一连接杆的另一端与所述U形连接件连接，所述第二限位连接组件包括第二连接杆、第二限位环、第二圆盘，所述第二圆盘通过横杆与所述舵机固定板固定连接，所述第二圆盘上设有第二凸块，所述第二限位环套接在第二凸块上，第二限位环的一端与第二连接杆的一端相连，所述第二连接杆的另一端与第二舵盘连接，所述舵机二的输出轴端通过连接外壳与所述履带驱动装置转动连接。

3.根据权利要求1所述的一种四足履带机器人控制平衡与修正导航方向的行进方法，其特征在于：所述躯体上设置有控制模块；在所述躯体的顶部设置有用于识别和检测前方的行进路况的摄像头，摄像头与控制模块电连接；所述履带驱动装置包括履带、履带固定件和驱动电机，所述履带固定件设置在所述履带上并设有通孔，所述驱动电机的输出端穿过通孔与所述履带转动连接，用于驱动所述履带转动；

步骤（4.1）中具体还包括以机器人右侧方向为Z轴方向，以机器人后侧方向为Y轴方向，以垂直Y轴和Z轴所在平面的方向为X轴方向，建立XYZ空间坐标系，在XOY坐标系中，设置小腿长度为I₁，大腿长度为I₂，小腿与大腿之间的连接长度为，预设足端坐标为（x，y），舵机二转角/>通过公式（1）得到；

（1）；

舵机一转角θ₄通过公式（2）得到；

（2）。

4.根据权利要求1所述的一种四足履带机器人控制平衡与修正导航方向的行进方法，其特征在于：所述舵机固定板包括第一舵机固定板和第二舵机固定板；所述舵机连接件上设有通孔，所述舵机连接件的一端与所述躯体连接，所述舵机连接件的另一端通过第一舵机固定板与所述舵机一固定连接；

步骤（4.3）中具体还包括通过判断腿部结构的力矩大小，实现平衡行进姿态调整，包括：

（4.3.1）通过腿部结构建立动力学模型，在XYZ空间坐标系中，根据空间坐标的惯性矩阵、耦合矩阵、速度以及加速度通过动力学方程，计算得到小腿的力矩和大腿的力矩/>；

（4.3.2）将小腿的力矩和大腿的力矩/>中至少一个大于舵机的最高堵转力矩进行比较，控制模块控制腿部结构的舵机一和舵机二停止转动，则退出Trot步态算法调整行进姿态或使得腿部结构切换到Trot行走周期中的摆动项。

5.根据权利要求4所述的一种四足履带机器人控制平衡与修正导航方向的行进方法，其特征在于：所述U形连接件上设置有通孔，所述U形连接件的一端与所述舵机一的输出轴端转动连接，所述U形连接件的另一端通过第二舵机固定板与所述舵机二固定连接。

6.根据权利要求5所述的一种四足履带机器人控制平衡与修正导航方向的行进方法，其特征在于：所述舵机二的输出轴端与第二舵盘转动连接，所述连接外壳的一端与第二舵盘连接，所述连接外壳的另一端与履带固定件连接。

7.根据权利要求6所述的一种四足履带机器人控制平衡与修正导航方向的行进方法，其特征在于：所述第二舵盘与所述第二连接杆的一端连接，所述第二连接杆的另一端与所述第二限位环的一端连接，所述第二限位环的另一端套接在第二凸块上。

8.根据权利要求4所述的一种四足履带机器人控制平衡与修正导航方向的行进方法，其特征在于：所述U形连接件靠近所述舵机一输出轴端的一端与所述第一连接杆的一端固定连接，舵机一的输出轴端通过第一舵机与U形连接件转动连接，所述第一连接杆的另一端与所述第一限位环的一端连接，所述第一限位环的另一端套接在第一凸块上。

9.根据权利要求4所述的一种四足履带机器人控制平衡与修正导航方向的行进方法，其特征在于：第二连接件包括第二一连接杆和第二二连接杆，第二一连接杆的一端与第二限位环铰接，第二一连接杆的另一端与第二二连接杆的一端固定连接，第二二连接杆的另一端设置有第二弧形杆，第二弧形杆与第二舵盘的外周壁相贴，第二弧形杆固定在设置连接外壳上，第二一连接杆的另一端与第二二连接杆一端具有夹角。

10.根据权利要求4所述的一种四足履带机器人控制平衡与修正导航方向的行进方法，其特征在于：第一连接件包括第一一连接杆和第一二连接杆，第一一连接杆的一端与第一限位环铰接，第一一连接杆的另一端与第一二连接杆的一端固定连接，第一二连接杆的另一端设置有第一弧形杆，第一弧形杆与第一舵盘的外周壁相贴，第一弧形杆固定在设置U形连接件上，第一一连接杆的另一端与第一二连接杆一端具有夹角。