CN110053684B - 一种适于斜坡路面的爬行机器人腿部装置及控制 - Google Patents

Abstract

一种适于斜坡路面的爬行机器人腿部装置及控制属于机器人技术领域；解决了现有技术中爬行机器人在斜坡路面平衡能力差的问题；包括髋关节、膝关节、腿部机构、足部装置以及控制系统；所述腿部机构采用双列角接触球轴承，增加了一个转动自由度，提供了充足的平衡运动裕度；所述足部装置采用三维力传感器，可根据腿部末端的受力情况调节腿部位置，使得机身与斜坡平行，保持重心稳定；所述控制系统针对爬行机器人在斜坡行进时腿部各关节存在耦合以及受到摩擦力、温度变化综合扰动的影响，采用基于力反馈的抗干扰解耦控制，将腿部末端的受力大小作为抗干扰解耦控制输入端的参考信息，提高了腿部的控制精度、运动平衡性以及爬行机器人在斜坡的行进效率。

Description

一种适于斜坡路面的爬行机器人腿部装置及控制

技术领域

本发明属于机器人技术领域，尤其涉及一种适于斜坡路面的爬行机器人腿部装置及控制。

背景技术

我国地理环境大多为山地、丘陵复杂路面，爬行机器人代替人类进行救援、勘探工作时，经常在斜坡路面上行走，在现有技术中，爬行机器人在斜坡路面行进时存在平衡能力差的问题，这与爬行机器人的机构设计以及运动协调性控制方面有关。

爬行机器人在斜坡路面行进时，由于地面的冲击，机身会随着凹凸斜坡地形有所起伏，机身不平衡、步进电机能量损耗大，爬行机器人不能有效率地通过斜坡，而腿部机构的设计很多为单纯的仿生，如模拟壁虎、蜘蛛、鳄鱼等腿部机构，缺乏有效的减震机构以及腿部平衡运动裕度不充足。

对于控制系统，爬行机器人在斜坡路面行进时，腿部某一个关节的输入会受到其他关节输出的影响，同时，这个关节的输出也会受到其他关节输入的影响，各关节之间存在的耦合现象对腿部的运动协调性产生了影响，降低了爬行机器人的平衡能力，解耦控制虽能获得满意的控制效果，但在受到摩擦力、温度变化综合扰动的影响时，腿部的实际运动轨迹偏离期望轨迹，不能有效地通过斜坡。

发明内容

本发明克服了上述现有技术上的不足，提供一种适于斜坡路面的爬行机器人腿部装置及控制，在机构设计上，采用双列角接触球轴承，增加了一个旋转自由度，提供了更加充足的平衡运动裕度，同时，减震机构能够减小地面对机体的冲击；在控制上，采用基于力反馈的抗干扰解耦控制，实时估计综合扰动并予以补偿，并将腿部末端在斜坡路面行进时的受力大小作为抗干扰解耦控制输入端的参考信息，使得机身与斜坡平行，保持重心稳定，提高了腿部的控制精度以及运动平衡性。

为了解决上述问题，本发明的第一目的在于提供一种适于斜坡路面的爬行机器人腿部装置，第二目的在于提供一种适于斜坡路面的爬行机器人腿部装置的控制方法。

本发明所采取的第一技术方案是：

一种适于斜坡路面的爬行机器人腿部装置，包括髋关节、膝关节、腿部机构、足部装置以及控制系统；初始状态下，所述髋关节与膝关节水平方向连接，膝关节与腿部机构成90度连接，腿部机构与足部装置竖直方向连接，控制系统放置在髋关节上；

所述髋关节包括U型支撑架、第一步进电机、第一金属盘、髋关节支撑架；所述第一步进电机与髋关节支撑架固定连接，与U型支撑架通过第一金属盘转动连接，所述第一金属盘与U型支撑架固定连接，所述髋关节支撑架与U型支撑架转动连接；

所述膝关节包括第二步进电机、第二金属盘、膝关节第一传动板、膝关节第二传动板、膝关节第一连接板、膝关节第二连接板；所述第二步进电机与髋关节支撑架固定连接，与膝关节第一传动板通过第二金属盘转动连接，所述第二金属盘与膝关节第一传动板固定连接，所述膝关节第一传动板与膝关节第二传动板转动连接，所述膝关节第二传动板与膝关节第一连接板转动连接，所述膝关节第一连接板与膝关节第二连接板转动连接，并通过髋关节支撑架的限位槽进行限位，所述膝关节第二连接板与髋关节支撑架转动连接；

所述腿部机构包括减震机构以及转动机构；所述减震机构分别与膝关节第一连接板、膝关节第二连接板固定连接，与转动机构转动连接；

所述足部装置包括三维力传感器以及足部结构；所述三维力传感器分别与转动机构、足部结构固定连接；

所述控制系统包括主控芯片、第一步进电机驱动芯片、第二步进电机驱动芯片、三维力传感器芯片；所述控制系统放置在髋关节支撑架上，所述主控芯片分别于第一步进电机驱动芯片、第二步进电机驱动芯片、三维力传感器芯片控制连接。

进一步地，所述减震机构包括腿部连接板、轴筒、中心轴、两个圆螺母、弹簧预紧块、弹簧；所述腿部连接板分别与膝关节第一连接板、膝关节第二连接板以及轴筒固定连接，所述轴筒与中心轴同轴配合，上方由两个圆螺母限位，所述中心轴采用中空结构，所述弹簧预紧块与中心轴同轴配合，弹簧预紧块与轴筒之间配合弹簧；

进一步地，所述转动机构包括双列角接触球轴承、轴承底座、上端盖、下端盖；所述双列角接触球轴承安装在轴承底座上，内圈与中心轴同轴配合，并由中心轴进行上方限位，下方由下端盖限位，所述双列角接触球轴承外圈下方由轴承底座限位，外圈上方由上端盖限位，所述轴承底座与三维力传感器固定连接；

进一步地，所述足部结构包括足部底座、前足底、后足底、前足橡胶垫、后足橡胶垫；所述足部底座分别与三维力传感器以及前足底、后足底固定连接，所述前足底、后足底分别与前足橡胶垫、后足橡胶垫固定连接；

进一步地，所述主控芯片与步进电机驱动芯片采用共阳极接法，主控芯片的PA0、PA1引脚分别与第一步进电机驱动芯片的DIR-、PUL-引脚连接，用于第一步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PA2、PA3引脚分别与第二步进电机驱动芯片的DIR-、PUL-引脚连接，用于第二步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PB8、PB9分别与三维力传感器芯片的S-、S+引脚连接，用于信号的传递；

进一步地，所述第一步进电机驱动芯片的A+、A-、B+、B-引脚分别与第一步进电机的MOTOR1A+、MOTOR1A-、MOTOR1B+、MOTOR1B-引脚连接，用于控制第一步进电机脉冲信号和正反转；

进一步地，所述第二步进电机驱动芯片的A+、A-、B+、B-引脚分别与第二步进电机的MOTOR2A+、MOTOR2A-、MOTOR2B+、MOTOR2B-引脚连接，用于控制第二步进电机脉冲信号和正反转。

本发明所采取的第二技术方案是：

所述一种适于斜坡路面的爬行机器人腿部装置实现的控制方法，以右前腿从摆动相转为支撑相时，右前腿落地，其余三条腿处于支撑态为例，包括以下步骤：

步骤S1、爬行机器人在斜坡路面行进时，三维力传感器测得腿部末端X、Y、Z 三个方向的接触力；

步骤S2、主控芯片将接收到的接触力信息作为参考并根据斜坡坡度θ得到腿部末端实际控制位置P_p；

步骤S3、主控芯片根据腿部末端实际控制位置P_p进行单腿逆运动学得到右前腿髋关节期望角度θ_11d、右前腿膝关节期望角度θ_12d，θ_11d、θ_12d分别通过独立通道进行基于线性扩张状态观测器的线性自抗扰控制对综合扰动f_i(·)给予实时补偿得到虚拟控制量U₁₁、U₁₂，U₁₁、U₁₂独立控制右前腿髋关节角度θ₁₁、右前腿膝关节角度θ₁₂，实现基于力反馈的抗干扰解耦控制；

步骤S4、最终控制步进电机精确快速地转动一定角度，进而控制右前腿髋关节、右前腿膝关节的运动，使得爬行机器人在斜坡路面按照期望轨迹运动，并保持与斜面平行。

进一步地，所述步骤S2中根据腿部末端接触力得到腿部末端实际控制位置P_p具体包括：

S201、对接触力进行滤波；

S202、将传感器测量坐标系下的接触力转换为腿部基坐标系下的接触力；

S203、根据接触力f_tc与腿部末端位置给定误差值P_e的关系

得到P_e，其中K_d、K_s为两个系数矩阵；

S204、根据腿部末端期望位置P_d和腿部末端位置给定误差值P_e得到腿部末端实际控制位置P_p。

进一步地，得到腿部末端期望位置P_d的具体过程为：

右前腿在落地时，需要调整腿部位置，使得爬行机器人机身与斜坡坡度θ保持平行，根据

求得右前腿腿部末端期望位置P_d(P_x1、P_z1)，其中斜坡坡度θ、左前腿在伴随坐标系Z方向的位置P_z2、左前腿在伴随坐标系 X方向的位置P_x2、左后腿在伴随坐标系Z方向的位置P_z3、左后腿在伴随坐标系 X方向的位置P_x3、右后腿在伴随坐标系Z方向的位置P_z4、右后腿在伴随坐标系 X方向的位置P_x4已知，并且右前腿在伴随坐标系Z方向的位置P_z1、右前腿在伴随坐标系X方向的位置P_x1之间的关系为P_z1-P’_z1＝tan(P_x1-P’_x1)，P’_z1、P’_x1分别为右前腿在上一支撑态时在伴随坐标系Z方向、X方向的位置。

进一步地，所述步骤S3中θ_11d、θ_12d分别通过独立通道进行基于线性扩张状态观测器的线性自抗扰控制对综合扰动f_i(·)给予实时补偿得到U₁₁、U₁₂具体包括：

S301、根据机器人单腿在复杂斜坡上坡时的动力学方程，得到

简写为

其中q、

为关节角位置、速度和加速度矢量， M₀为惯性矩阵，C₀为离心力和哥氏力矩阵，G₀为重力项矢量，τ为关节控制力矩矢量，H为机器人不确定项，令U＝M₀ ^-1τ，

从而

U包括U₁₁、U₁₂；

S302、线性自抗扰控制主要包括线性扩张状态观测器、扰动补偿和PD反馈控制三个部分，设计三阶线性扩张状态观测器，以右前腿髋关节为例，对其的综合扰动f_i(·)在内的状态变量进行估计，即：

其中，z₁、z₂、z₃分别为θ₁₁、

和f_i(·)的估计值；b₀为系统控制量放大系数；β₁、β₂、β₃分别为线性扩张状态观测器增益，且满足β₁＝3ω₀，β₂＝3ω₀ ²，β₃＝ω₀ ³，ω₀为观测器带宽，通过调整增益可使z₁→θ₁₁、

z₃→f_i(·)；

S303、设计补偿控制律进行动态补偿，公式为：

式中U₀为PD反馈控制量，将

带入

可得

S304、采用PD控制器进行控制，控制算法为：

U₀＝K_P(θ_11d-Z₁)-K_DZ₂

其中K_P、K_D为控制器增益，且K_P＝ω_c ²，K_D＝2ω_c，ω_c为控制器可调参数。

本发明相对于现有技术具有以下有益效果：

1、本发明提供一种适于斜坡路面的爬行机器人腿部装置及控制，腿部装置采用三维力传感器，能够测得腿部末端在X、Y、Z三个方向的受力情况，便于后续腿部姿态的调整，足底结构能够防滑，提高了爬行机器人的抓地力，转动机构中采用双列角接触球轴承，在承受径向和双向轴向载荷的同时又能提供一个被动的转动自由度，提供更加充足的平衡运动裕度，提高了爬行机器人在斜坡路面行进时的平衡能力，并且减震机构能够有效地缓冲地面的冲击，减少能量的损耗；

2、本发明控制系统针对爬行机器人在斜坡路面行进时腿部各关节存在耦合以及会受到摩擦力、温度变化等综合扰动的影响，不能按照期望轨迹运动，采用基于力反馈的抗干扰解耦控制，实时估计综合扰动并予以补偿，并将腿部末端在斜坡路面的受力大小作为抗干扰解耦控制输入端的参考信息，使得机身与斜坡平行，保持重心稳定，提高了对腿部的控制精度以及运动协调性，进而提高了爬行机器人在斜坡路面的平衡能力。

附图说明

图1是本发明装置主视图；

图2是本发明装置轴测图；

图3是本发明髋关节与膝关节连接后视图；

图4是本发明髋关节与膝关节连接轴测图；

图5是本发明膝关节第一连接板结构图；

图6是本发明膝关节第二连接板结构图；

图7是本发明腿部机构左视图；

图8是本发明腿部机构剖视图；

图9是本发明足部结构图；

图10是本发明控制系统主控芯片电路图；

图11是本发明第一步进电机驱动芯片电路图；

图12是本发明第二步进电机驱动芯片电路图；

图13是本发明三维力传感器芯片电路图；

图14是本发明基于力反馈的抗干扰解耦控制图；

图15是本发明髋关节线性自抗扰控制图；

图中：髋关节1、膝关节2、腿部机构3、足部装置4、控制系统5、U型支撑架 1-1、第一步进电机1-2、第一金属盘1-3、髋关节支撑架1-4、第二步进电机2-1、第二金属盘2-2、膝关节第一传动板2-3、膝关节第二传动板2-4、膝关节第一连接板2-5、膝关节第二连接板2-6、减震机构3-1、转动机构3-2、三维力传感器 4-1、足部结构4-2、腿部连接板3-1A、轴筒3-1B、中心轴3-1C、两个圆螺母3- 1D、弹簧预紧块3-1E、弹簧3-1F、双列角接触球轴承3-2A、轴承底座3-2B、上端盖3-2C、下端盖3-2D、足部底座4-2A、前足底4-2B、后足底4-2C、前足橡胶垫4-2D、后足橡胶垫4-2E。

具体实施方式

以下将结合附图对本发明进行详细说明。

具体实施方式一：

一种适于斜坡路面的爬行机器人腿部装置，如图1-6、图10-13所示，包括髋关节1、膝关节2、腿部机构3、足部装置4以及控制系统5；初始状态下，所述髋关节1与膝关节2水平方向连接，膝关节2与腿部机构3成90度连接，腿部机构3与足部装置4竖直方向连接，控制系统5放置在髋关节1上；

所述髋关节1包括U型支撑架1-1、第一步进电机1-2、第一金属盘1-3、髋关节支撑架1-4；所述第一步进电机1-2与髋关节支撑架1-4固定连接，与U型支撑架1-1通过第一金属盘1-3转动连接，所述第一金属盘1-3与U型支撑架1-1固定连接，所述髋关节支撑架1-4与U型支撑架1-1转动连接；

所述膝关节2包括第二步进电机2-1、第二金属盘2-2、膝关节第一传动板2-3、膝关节第二传动板2-4、膝关节第一连接板2-5、膝关节第二连接板2-6；所述第二步进电机2-1与髋关节支撑架1-4固定连接，与膝关节第一传动板2-3通过第二金属盘2-2转动连接，所述第二金属盘2-2与膝关节第一传动板2-3固定连接，所述膝关节第一传动板2-3与膝关节第二传动板2-4转动连接，所述膝关节第二传动板2-4与膝关节第一连接板2-5转动连接，所述膝关节第一连接板2-5与膝关节第二连接板2-6转动连接，并通过髋关节支撑架1-4的限位槽进行限位，所述膝关节第二连接板2-6与髋关节支撑架1-4转动连接；

所述腿部机构3包括减震机构3-1以及转动机构3-2；所述减震机构3-1分别与膝关节第一连接板2-5、膝关节第二连接板2-6固定连接，与转动机构3-2转动连接；

所述足部装置4包括三维力传感器4-1以及足部结构4-2；所述三维力传感器4- 1分别与转动机构3-2、足部结构4-2固定连接；

所述控制系统5包括主控芯片、第一步进电机驱动芯片、第二步进电机驱动芯片、三维力传感器芯片；所述控制系统5放置在髋关节支撑架1-4上，所述主控芯片分别与第一步进电机驱动芯片、第二步进电机驱动芯片、三维力传感器芯片控制连接。

本实施方式中第一步进电机1-2为现有技术，生产厂家为TELESKY，型号为42BYGH24。

本实施方式中第二步进电机2-1为现有技术，生产厂家为TELESKY，型号为42BYGH24。

本实施方式中主控芯片为现有技术，生产厂家为意法半导体，型号为 STM32F407。

本实施方式中第一步进电机驱动芯片为现有技术，生产厂家为TELESKY，型号为TB6600。

本实施方式中第二步进电机驱动芯片为现有技术，生产厂家为TELESKY，型号为TB6600。

本实施方式中三维力传感器芯片为现有技术，生产厂家为深圳市怡冠兴科技发展有限公司，型号为YGX-XW303。

具体实施方式二：

如图1、图7-9所示，在具体实施方式一的基础上，一种适于斜坡路面的爬行机器人腿部装置包括髋关节1、膝关节2、腿部机构3、足部装置4以及控制系统 5，所述腿部机构3包括减震机构3-1以及转动机构3-2，所述足部装置4包括三维力传感器4-1以及足部结构4-2；所述减震机构3-1包括腿部连接板3-1A、轴筒3-1B、中心轴3-1C、两个圆螺母3-1D、弹簧预紧块3-1E、弹簧3-1F；所述腿部连接板3-1A分别与膝关节第一连接板2-5、膝关节第二连接板2-6以及轴筒3- 1B固定连接，所述轴筒3-1B与中心轴3-1C同轴配合，上方由两个圆螺母3-1D 限位，所述中心轴3-1C采用中空结构，所述弹簧预紧块3-1E与中心轴3-1C同轴配合，弹簧预紧块3-1E与轴筒3-1B之间配合弹簧3-1F；

所述转动机构3-2包括双列角接触球轴承3-2A、轴承底座3-2B、上端盖3-2C、下端盖3-2D；所述双列角接触球轴承3-2A安装在轴承底座3-2B上，内圈与中心轴3-1C同轴配合，并由中心轴3-1C进行上方限位，下方由下端盖3-2D限位，所述双列角接触球轴承3-2A外圈下方由轴承底座3-2B限位，外圈上方由上端盖3-2C限位，所述轴承底座3-2B与三维力传感器4-1固定连接；

所述足部结构4-2包括足部底座4-2A、前足底4-2B、后足底4-2C、前足橡胶垫 4-2D、后足橡胶垫4-2E；所述足部底座4-2分别与三维力传感器4-1以及前足底 4-2B、后足底4-2C固定连接，所述前足底4-2B、后足底4-2C分别与前足橡胶垫4-2D、后足橡胶垫4-2E固定连接；

如图10-13所示，所述主控芯片与步进电机驱动芯片采用共阳极接法，主控芯片的PA0、PA1引脚分别与第一步进电机驱动芯片的DIR-、PUL-引脚连接，用于第一步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PA2、PA3 引脚分别与第二步进电机驱动芯片的DIR-、PUL-引脚连接，用于第二步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PB8、PB9分别与三维力传感器芯片的S-、S+引脚连接，用于信号的传递；所述第一步进电机驱动芯片的 A+、A-、B+、B-引脚分别与第一步进电机的MOTOR1A+、MOTOR1A-、MOTOR1B+、MOTOR1B-引脚连接，用于控制第一步进电机脉冲信号和正反转；所述第二步进电机驱动芯片的A+、A-、B+、B-引脚分别与第二步进电机的 MOTOR2A+、MOTOR2A-、MOTOR2B+、MOTOR2B-引脚连接，用于控制第二步进电机脉冲信号和正反转。

本实施方式中双列角接触球轴承3-2A为现有技术，生产厂家为NSK，型号为3215。

本实施方式中主控芯片为现有技术，生产厂家为意法半导体，型号为 STM32F407。

本实施方式中第一步进电机驱动芯片为现有技术，生产厂家为TELESKY，型号为TB6600。

本实施方式中第二步进电机驱动芯片为现有技术，生产厂家为TELESKY，型号为TB6600。

本实施方式中三维力传感器芯片为现有技术，生产厂家为深圳市怡冠兴科技发展有限公司，型号为YGX-XW303。

本实施方式中第一步进电机1-2为现有技术，生产厂家为TELESKY，型号为42BYGH24。

本实施方式中第二步进电机2-1为现有技术，生产厂家为TELESKY，型号为42BYGH24。

工作过程：

所述三维力传感器4-1测得腿部末端在斜坡路面行进时X、Y、Z三个方向的接触力，并将信号传递给主控芯片，主控芯片经过基于力反馈的抗干扰解耦控制，对系统内外干扰进行估计补偿，将处理的控制信号传递给第一步进电机驱动芯片以及第二步进电机驱动芯片，精准稳定地控制第一步进电机1-2以及第二步进电机2-1的正向或反向转动角度，爬行机器人在斜坡路面行进时腿部能够按照期望轨迹运动，使得机身与斜坡平行，并保持重心的稳定以及腿部运动的协调性，同时，腿部装置采用减震机构3-1以及转动机构3-2，减震机构3-1能够缓冲地面的冲击，减少能量的损耗，转动机构3-2采用双列角接触球轴承3-2A，在承受径向和双向轴向载荷的同时又能提供一个被动的转动自由度，提供了更加充足的平衡运动裕度，提高了腿部在斜坡路面中的平衡能力。

具体实施方式三：

如图14、图15所示，一种适于斜坡路面的爬行机器人腿部装置的控制方法，以右前腿从摆动相转为支撑相时，右前腿落地，其余三条腿处于支撑态为例，包括以下步骤：

步骤S1、爬行机器人在斜坡路面行进时，三维力传感器测得腿部末端X、Y、Z 三个方向的接触力；

步骤S2、主控芯片将接收到的接触力信息作为参考并根据斜坡坡度θ得到腿部末端实际控制位置P_p；

步骤S3、主控芯片根据腿部末端实际控制位置P_p进行单腿逆运动学得到右前腿髋关节期望角度θ_11d、右前腿膝关节期望角度θ_12d，θ_11d、θ_12d分别通过独立通道进行基于线性扩张状态观测器的线性自抗扰控制对综合扰动f_i(·)给予实时补偿得到虚拟控制量U₁₁、U₁₂，U₁₁、U₁₂独立控制右前腿髋关节角度θ₁₁、右前腿膝关节角度θ₁₂，实现基于力反馈的抗干扰解耦控制；

步骤S4、最终控制步进电机精确快速地转动一定角度，进而控制右前腿髋关节、右前腿膝关节的运动，使得爬行机器人在斜坡路面按照期望轨迹运动，并保持与斜面平行。

进一步地，所述步骤S2中根据腿部末端接触力得到腿部末端实际控制位置P_p具体包括：

S201、对接触力进行滤波；

S202、将传感器测量坐标系下的接触力转换为腿部基坐标系下的接触力；

S203、根据接触力f_tc与腿部末端位置给定误差值P_e的关系

得到P_e，其中K_d、K_s为两个系数矩阵；

S204、根据腿部末端期望位置P_d和腿部末端位置给定误差值P_e得到腿部末端实际控制位置P_p。

进一步地，得到腿部末端期望位置P_d的具体过程为：

右前腿在落地时，需要调整腿部位置，使得爬行机器人机身与斜坡坡度θ保持平行，根据

求得右前腿腿部末端期望位置P_d(P_x1、P_z1)，其中斜坡坡度θ、左前腿在伴随坐标系Z方向的位置P_z2、左前腿在伴随坐标系 X方向的位置P_x2、左后腿在伴随坐标系Z方向的位置P_z3、左后腿在伴随坐标系 X方向的位置P_x3、右后腿在伴随坐标系Z方向的位置P_z4、右后腿在伴随坐标系 X方向的位置P_x4已知，并且右前腿在伴随坐标系Z方向的位置P_z1、右前腿在伴随坐标系X方向的位置P_x1之间的关系为P_z1-P’_z1＝tan(P_x1-P’_x1)，P’_z1、P’_x1分别为右前腿在上一支撑态时在伴随坐标系Z方向、X方向的位置。

进一步地，所述步骤S3中θ_11d、θ_12d分别通过独立通道进行基于线性扩张状态观测器的线性自抗扰控制对综合扰动f_i(·)给予实时补偿得到U₁₁、U₁₂具体包括：

S301、根据机器人单腿在复杂斜坡上坡时的动力学方程，得到

简写为

其中q、

为关节角位置、速度和加速度矢量， M₀为惯性矩阵，C₀为离心力和哥氏力矩阵，G₀为重力项矢量，τ为关节控制力矩矢量，H为机器人不确定项，令U＝M₀ ^-1τ，

从而

U包括U₁₁、U₁₂；

S302、线性自抗扰控制主要包括线性扩张状态观测器、扰动补偿和PD反馈控制三个部分，设计三阶线性扩张状态观测器，以右前腿髋关节为例，对其的综合扰动f_i(·)在内的状态变量进行估计，即：

其中，z₁、z₂、z₃分别为θ₁₁、

和f_i(·)的估计值；b₀为系统控制量放大系数；β₁、β₂、β₃分别为线性扩张状态观测器增益，且满足β₁＝3ω₀，β₂＝3ω₀ ²，β₃＝ω₀ ³，ω₀为观测器带宽，通过调整增益可使z₁→θ₁₁、

z₃→f_i(·)；

S303、设计补偿控制律进行动态补偿，公式为：

式中U₀为PD反馈控制量，将

带入

可得

S304、采用PD控制器进行控制，控制算法为：

U₀＝K_P(θ_11d-Z₁)-K_DZ₂

其中K_P、K_D为控制器增益，且K_P＝ω_c ²，K_D＝2ω_c，ω_c为控制器可调参数。

Claims (3)

1.一种适于斜坡路面的爬行机器人腿部装置，其特征在于，包括髋关节(1)、膝关节(2)、腿部机构(3)、足部装置(4)以及控制系统(5)；初始状态下，所述髋关节(1)与膝关节(2)水平方向连接，膝关节(2)与腿部机构(3)成90度连接，腿部机构(3)与足部装置(4)竖直方向连接，控制系统(5)放置在髋关节(1)上；

所述髋关节(1)包括U型支撑架(1-1)、第一步进电机(1-2)、第一金属盘(1-3)、髋关节支撑架(1-4)；所述第一步进电机(1-2)与髋关节支撑架(1-4)固定连接，与U型支撑架(1-1)通过第一金属盘(1-3)转动连接，所述第一金属盘(1-3)与U型支撑架(1-1)固定连接，所述髋关节支撑架(1-4)与U型支撑架(1-1)转动连接；

所述膝关节(2)包括第二步进电机(2-1)、第二金属盘(2-2)、膝关节第一传动板(2-3)、膝关节第二传动板(2-4)、膝关节第一连接板(2-5)、膝关节第二连接板(2-6)；所述第二步进电机(2-1)与髋关节支撑架(1-4)固定连接，与膝关节第一传动板(2-3)通过第二金属盘(2-2)转动连接，所述第二金属盘(2-2)与膝关节第一传动板(2-3)固定连接，所述膝关节第一传动板(2-3)与膝关节第二传动板(2-4)转动连接，所述膝关节第二传动板(2-4)与膝关节第一连接板(2-5)转动连接，所述膝关节第一连接板(2-5)与膝关节第二连接板(2-6)转动连接，并通过髋关节支撑架(1-4)的限位槽进行限位，所述膝关节第二连接板(2-6)与髋关节支撑架(1-4)转动连接；

所述腿部机构(3)包括减震机构(3-1)以及转动机构(3-2)；所述减震机构(3-1)分别与膝关节第一连接板(2-5)、膝关节第二连接板(2-6)固定连接，与转动机构(3-2)转动连接；

所述足部装置(4)包括三维力传感器(4-1)以及足部结构(4-2)；所述三维力传感器(4-1)分别与转动机构(3-2)、足部结构(4-2)固定连接；

所述控制系统(5)包括主控芯片、第一步进电机驱动芯片、第二步进电机驱动芯片、三维力传感器芯片；所述控制系统(5)放置在髋关节支撑架(1-4)上，所述主控芯片分别与第一步进电机驱动芯片、第二步进电机驱动芯片、三维力传感器芯片控制连接。

2.根据权利要求1所述一种适于斜坡路面的爬行机器人腿部装置，其特征在于，所述腿部机构(3)包括减震机构(3-1)以及转动机构(3-2)，所述足部装置(4)包括三维力传感器(4-1)以及足部结构(4-2)；所述减震机构(3-1)包括腿部连接板(3-1A)、轴筒、中心轴、两个圆螺母(3-1D)、弹簧预紧块(3-1E)、弹簧(3-1F)；所述腿部连接板(3-1A)分别与膝关节第一连接板(2-5)、膝关节第二连接板(2-6)以及轴筒(3-1B)固定连接，所述轴筒(3-1B)与中心轴(3-1C)同轴配合，上方由两个圆螺母(3-1D)限位，所述中心轴(3-1C)采用中空结构，所述弹簧预紧块(3-1E)与中心轴(3-1C)同轴配合，弹簧预紧块(3-1E)与轴筒(3-1B)之间配合弹簧(3-1F)；

所述转动机构(3-2)包括双列角接触球轴承(3-2A)、轴承底座(3-2B)、上端盖、下端盖；所述双列角接触球轴承(3-2A)安装在轴承底座(3-2B)上，内圈与中心轴(3-1C)同轴配合，并由中心轴(3-1C)进行上方限位，下方由下端盖(3-2D)限位，所述双列角接触球轴承(3-2A)外圈下方由轴承底座(3-2B)限位，外圈上方由上端盖(3-2C)限位，所述轴承底座(3-2B)与三维力传感器(4-1)固定连接；

所述足部结构(4-2)包括足部底座(4-2A)、前足底(4-2B)、后足底(4-2C)、前足橡胶垫(4-2D)、后足橡胶垫(4-2E)；所述足部底座(4-2A)分别与三维力传感器(4-1)以及前足底(4-2B)、后足底(4-2C)固定连接，所述前足底(4-2B)、后足底(4-2C)分别与前足橡胶垫(4-2D)、后足橡胶垫(4-2E)固定连接；

所述主控芯片与步进电机驱动芯片采用共阳极接法，主控芯片的PA0、PA1引脚分别与第一步进电机驱动芯片的DIR-、PUL-引脚连接，用于第一步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PA2、PA3引脚分别与第二步进电机驱动芯片的DIR-、PUL-引脚连接，用于第二步进电机驱动芯片脉冲信号和方向信号的输入；所述主控芯片的PB8、PB9分别与三维力传感器芯片的S-、S+引脚连接，用于信号的传递；所述第一步进电机驱动芯片的A+、A-、B+、B-引脚分别与第一步进电机的MOTOR1A+、MOTOR1A-、MOTOR1B+、MOTOR1B-引脚连接，用于控制第一步进电机脉冲信号和正反转；所述第二步进电机驱动芯片的A+、A-、B+、B-引脚分别与第二步进电机的MOTOR2A+、MOTOR2A-、MOTOR2B+、MOTOR2B-引脚连接，用于控制第二步进电机脉冲信号和正反转。

3.一种基于权利要求1至2任一所述一种适于斜坡路面的爬行机器人腿部装置的控制方法，以右前腿从摆动相转为支撑相时，右前腿落地，其余三条腿处于支撑态为例，其特征在于，包括以下步骤：

步骤S1、爬行机器人在斜坡路面行进时，三维力传感器测得腿部末端X、Y、Z三个方向的接触力；

步骤S2、主控芯片将接收到的接触力信息作为参考并根据斜坡坡度θ得到腿部末端实际控制位置P_p；

步骤S3、主控芯片根据腿部末端实际控制位置P_p进行单腿逆运动学得到右前腿髋关节期望角度θ_11d、右前腿膝关节期望角度θ_12d，θ_11d、θ_12d分别通过独立通道进行基于线性扩张状态观测器的线性自抗扰控制对综合扰动f_i(·)给予实时补偿得到虚拟控制量U₁₁、U₁₂，U₁₁、U₁₂独立控制右前腿髋关节角度θ₁₁、右前腿膝关节角度θ₁₂，实现基于力反馈的抗干扰解耦控制；

步骤S4、最终控制步进电机精确快速地转动一定角度，进而控制右前腿髋关节、右前腿膝关节的运动，使得爬行机器人在斜坡路面按照期望轨迹运动，并保持与斜面平行。

进一步地，所述步骤S2中根据腿部末端接触力得到腿部末端实际控制位置P_p具体包括：

S201、对接触力进行滤波；

S202、将传感器测量坐标系下的接触力转换为腿部基坐标系下的接触力；

S203、根据接触力f_tc与腿部末端位置给定误差值P_e的关系

得到P_e，其中K_d、K_s为两个系数矩阵；

S204、根据腿部末端期望位置P_d和腿部末端位置给定误差值P_e得到腿部末端实际控制位置P_p。

进一步地，得到腿部末端期望位置P_d的具体过程为：

右前腿在落地时，需要调整腿部位置，使得爬行机器人机身与斜坡坡度θ保持平行，根据

求得右前腿腿部末端期望位置P_d(P_x1、P_z1)，其中斜坡坡度θ、左前腿在伴随坐标系Z方向的位置P_z2、左前腿在伴随坐标系X方向的位置P_x2、左后腿在伴随坐标系Z方向的位置P_z3、左后腿在伴随坐标系X方向的位置P_x3、右后腿在伴随坐标系Z方向的位置P_z4、右后腿在伴随坐标系X方向的位置P_x4已知，并且右前腿在伴随坐标系Z方向的位置P_z1、右前腿在伴随坐标系X方向的位置P_x1之间的关系为P_z1-P′_z1＝tan(P_x1-P′_x1)，P′_z1、P′_x1分别为右前腿在上一支撑态时在伴随坐标系Z方向、X方向的位置；

进一步地，所述步骤S3中θ_11d、θ_12d分别通过独立通道进行基于线性扩张状态观测器的线性自抗扰控制对综合扰动f_i(·)给予实时补偿得到U₁₁、U₁₂具体包括：

S301、根据机器人单腿在复杂斜坡上坡时的动力学方程，得到

简写为

其中q、

为关节角位置、速度和加速度矢量，M₀为惯性矩阵，C₀为离心力和哥氏力矩阵，G₀为重力项矢量，τ为关节控制力矩矢量，H为机器人不确定项，令U＝M₀ ^-1τ，

从而

U包括U₁₁、U₁₂；

S302、线性自抗扰控制主要包括线性扩张状态观测器、扰动补偿和PD反馈控制三个部分，设计三阶线性扩张状态观测器，以右前腿髋关节为例，对其的综合扰动f_i(·)在内的状态变量进行估计，即：

其中，z₁、z₂、z₃分别为θ₁₁、

和f_i(·)的估计值；b₀为系统控制量放大系数；β₁、β₂、β₃分别为线性扩张状态观测器增益，且满足β₁＝3ω₀，β₂＝3ω₀ ²，β₃＝ω₀ ³，ω₀为观测器带宽，通过调整增益可使z₁→θ₁₁、

z₃→f_i(·)；

S303、设计补偿控制律进行动态补偿，公式为：

式中U₀为PD反馈控制量，将

带入

可得

S304、采用PD控制器进行控制，控制算法为：

U₀＝K_P(θ_11d-Z₁)-K_DZ₂

其中K_P、K_D为控制器增益，且K_P＝ω_c ²，K_D＝2ω_c，ω_c为控制器可调参数。

Images