CN103941582B - 液压驱动型仿生单腿双环控制方法 - Google Patents

Abstract

本发明公开了一种液压驱动型仿生单腿双环控制方法，由外环力/位混合控制和内环单关节力控制两个环节组成，在外环力/位混合控制环节，接收外部输入的原始期望足端位置轨迹，针对原始期望足端位置轨迹依次进行重新规划、自抗扰控制、基于扩展雅克比的冗余优化得到仿生单腿各个关节的原始关节空间控制量；在内环单关节力控制环节，采集仿生单腿各个关节的输出力信息，基于输出力信息对仿生单腿各个关节的原始关节空间控制量进行修正得到修正后的关节空间控制量，分别基于仿生单腿各个关节的关节空间控制量控制仿生单腿各个关节的液压动作器的动作。本发明具有地形适应能力好、柔顺能力好、通用性强、鲁棒性高、应用范围广的优点。

Description

液压驱动型仿生单腿双环控制方法

技术领域

本发明涉及足式仿生机器人非结构化地形适应性控制技术领域，具体涉及一种用于解决足式机器人非结构化地形适应性控制中的摆动腿精确轨迹跟踪与柔顺接触控制问题的液压驱动型仿生单腿双环控制方法。

背景技术

四足仿生机器人是以步行、奔跑和跳跃为移动方式的机器人平台，其主要特点是载荷大、重量轻、灵活性好和良好的地形适应性。在非结构化地形条件下，如丛林、丘陵和山地等环境，一般的机械化运输工具无法胜任运输任务，而四足仿生机器人具备各种地形下的高通过能力，因此具有重要的研究意义；另外，除了担负辅助运输任务外，四足机器人也可以承担仓库、营地周边巡逻安保任务，可以广泛于军警、地质和石油勘探等部门。运动控制系统作为四足仿生机器人移动任务执行的核心，其主要目标就是实现在非结构化地形环境中的稳定行走。为了实现对不同已知/未知地形的柔顺与适应，在机械结构设计过程中仿生单腿采用4自由度冗余设计，足端与地面采用点接触方式。另外，从运动控制角度来看，为了使四足仿生机器人具有较高的地面通过能力，不仅需要采用合适的运动规划方法，构建仿生单腿的柔顺能力也是影响其适应能力的重要方面。其中，对于仿生单腿的控制而言，不仅需要完成摆动相位下的精确位置控制，还需要完成接触相和支撑相的柔顺控制，其中摆动相末段与地面的柔顺接触控制对于机器人的稳定能力具有重要作用。机器人控制技术中，机器人对接触环境顺从的能力称为柔顺性。柔顺性可分为主动柔顺性和被动柔顺性，被动柔顺性指机器人借助辅助柔顺机构对环境作用力产生自然顺从，主动柔顺性指机器人根据力反馈信息采取一定的控制策略去主动控制作用力，实现对接触环境的主动顺从。仿生单腿在结构设计上已经考虑了被动柔顺机构，如踝部弹簧等，但适应能力有限。当四足仿生机器人在未知不平整地面上行走时，若仍采用传统的位置控制方法，则有可能因为足端与地面的意外碰撞而产生过大的接触力，对机器人行走稳定性造成不良影响，甚至损害足部机构、关节传动/驱动部件，因此需进一步考虑如何增强其主动柔顺能力以适应不平整地面。因此，针对包括液压驱动型四足仿生机器人在内的液压驱动型多足机器人，如何提高仿生单腿的地形适应能力和柔顺能力，已经成为一项亟待解决的关键技术问题。

发明内容

本发明要解决的技术问题是提供一种地形适应能力好、柔顺能力好、通用性强、鲁棒性高、应用范围广的液压驱动型仿生单腿双环控制方法。

为了解决上述技术问题，本发明采用的技术方案为：

一种液压驱动型仿生单腿双环控制方法，其实施步骤如下：

1)接收外部输入的原始期望足端位置轨迹；

2)采集仿生单腿的足端力信息，当足端力信息显示足端完全接触到地面时，获取仿生单腿各个关节的关节位置信息，根据所述关节位置信息进行正运动学解算得到足端实际位置，根据所述足端实际位置对原始期望足端位置轨迹进行重新规划，得到重新规划后的期望足端位置轨迹；

3)将所述期望足端位置轨迹包含的足端x、y、z三个方向的期望轨迹分别进行自抗扰控制，分别得到足端x、y、z三个方向的控制量，所述足端x、y、z三个方向的控制量构成仿生单腿的操作空间控制量；

4)利用指定的优化目标构造约束条件并建立扩展雅可比矩阵，基于扩展雅克比的冗余优化方法将所述操作空间控制量转换为仿生单腿各个关节的原始关节空间控制量；

5)采集仿生单腿各个关节的输出力信息，基于所述输出力信息对仿生单腿各个关节的原始关节空间控制量进行修正得到修正后的关节空间控制量，分别基于仿生单腿各个关节的关节空间控制量控制仿生单腿各个关节的液压动作器的动作。

进一步地，所述步骤2)中具体是指根据式(1)对原始期望足端位置轨迹进行重新规划；

$\left\{\begin{array}{c}{x}_{rd}\left(t\right)=x+\frac{\left(x_d\right(T)-x)}{T}*t\\ y_{rd}\left(t\right)=y+\frac{\left(y_d\right(T)-y)}{T}*t\\ z_{rd}\left(t\right)=z+\sin \left(\frac{2*\mathrm{\π}*t}{T}\right)+\frac{\left(z_d\right(T)-z)}{T}*t\end{array}\right.---\left(1\right)$

式(1)中，X_d:(x_d(T),y_d(T),z_d(T))表示外部输入的原始期望足端位置轨迹，其中T表示步行周期，t表示从落脚开始至今的时间，X_rd:(x_rd,y_rd,z_rd)表示重新规划后的期望足端位置轨迹，X:(x,y,z)表示足端实际位置。

进一步地，所述步骤3)中将期望足端位置轨迹包含的足端x方向的期望轨迹进行自抗扰控制的详细步骤如下：

3.1)安排过渡过程：根据所述期望足端位置轨迹包含的足端x方向的期望轨迹为自抗扰控制器安排过渡过程，得到如式(2)所示的光滑的输入信号及其微分信号；

$\left\{\begin{array}{c}{f}_h=fhan(v_{11}-x_{rd},v_{12},r_0,h)\\ v_{11}=v_{11}+{\mathrm{hv}}_{12}\\ v_{12}=v_{12}+{\mathrm{hf}}_h\end{array}\right.---\left(2\right)$

式(2)中，fhan表示最速控制综合函数，v₁₁表示安排过渡过程得到的光滑的输入信号，x_rd表示期望足端位置轨迹包含的足端x方向的期望轨迹，v₁₂表示所述输入信号的微分信号，r₀为用于决定输入信号v₁₁跟踪足端x方向的期望轨迹快慢的速度因子，h为用于滤掉输入信号v₁₁所含的噪声滤波因子；

3.2)非线性组合：根据式(3)构建的自抗扰控制器的非线性组合；

$\left\{\begin{array}{c}{e}_1=v_{11}-z_{11}\\ e_2=v_{12}-z_{12}\\ u_0=-fhan(e_1,{\mathrm{ce}}_2,r,h)\\ u_x=(u_0-z_{13})/b_0\end{array}\right.---\left(3\right)$

式(3)中，z₁₁表示反馈得到的输入信号v₁₁的观测值，z₁₂表示反馈得到的微分信号v₁₂的观测值，z₁₃为反馈得到的扰动项f的观测值，e₁表示位置误差，e₂表示误差微分信号，u₀表示对应足端x方向的非线性组合的初步输出值，fhan表示最速控制综合函数，u_x表示足端x方向的控制量，c、r和b₀分别表示抗扰控制器的非线性组合可调参数，h为用于滤掉输入信号v₁₁所含的噪声滤波因子；

3.3)扩张状态观测器：根据自抗扰控制器的最终输出值和足端x方向的控制量构建如式(4)所示的扩张状态观测器，分别将输入信号的观测值、微分信号的观测值、扰动项的观测值返回输出；

$\left\{\begin{array}{c}e=z_{11}-y\\ fe=fal(e,0.5,h)\\ {\mathrm{fe}}_1=fal(e,0.25,h)\\ z_{11}=z_{11}+h(z_{12}-{\mathrm{\β}}_{01}e)\\ z_{12}=z_{12}+h(z_{13}-{\mathrm{\β}}_{02}fe+b_0{u}_x)\\ z_{13}=z_{13}+h(-{\mathrm{\β}}_{03}{\mathrm{fe}}_1)\end{array}\right.---\left(4\right)$

式(4)中，z₁₁表示反馈输出的输入信号v₁₁的观测值，z₁₂表示反馈输出的微分信号v₁₂的观测值，z₁₃为反馈输出的扰动项f的观测值，e表示反馈输出的输入信号v₁₁的观测值和自抗扰控制器的最终输出值之间的误差，h为用于滤掉输入信号v₁₁所含的噪声滤波因子，y表示自抗扰控制器的最终输出值，u_x表示对应足端x方向的非线性组合的最终输出值，b₀表示抗扰控制器的非线性组合可调参数，β₀₁、β₀₂和β₀₃分别为扩张状态观测器的可调参数，其中fal函数的表达式如式(5)所示；

$fal(e,\mathrm{\&alpha;},h)=\{\begin{array}{c}{\left|e\right|}^{\mathrm{\&alpha;}}sign\left(e\right),\left|e\right|>h\\ \left|e\right|/h^{1-\mathrm{\&alpha;}},\left|e\right|\&le;h\end{array},0<\mathrm{\&alpha;}<1,h>0---\left(5\right).$

进一步地，所述步骤4)的详细步骤如下：

4.1)选择指定的优化目标；

4.2)根据所述优化指标生成约束条件；

4.3)基于所述约束条件建立如式(6)所示的扩展雅克比矩阵；

$J^e=\left[\begin{array}{c}{J}_{j\&times;k}\\ \&part;G/\&part;\mathrm{\&theta;}\end{array}\right]---\left(6\right)$

式(6)中，J^e表示建立的扩展雅克比矩阵；J_j×k表示仿生单腿的单腿关节空间到操作空间的雅可比矩阵，其中j表示仿生单腿的操作空间自由度数量，k表示仿生单腿的单腿关节空间自由度数量；G为关于θ_i的非线性函数且满足约束条件G(θ_i)＝0，其中θ_i中的序号i表示仿生单腿的关节序号；

4.4)根据式(7)将扩展雅克比矩阵和单腿动力学方程的惯量矩阵结合，将所述操作空间控制量转换为仿生单腿各个关节的原始关节空间控制量；

τ＝A(θ)(J^e)^-1U (7)

式(7)中，A(θ)表示单腿动力学方程的惯量矩阵，J^e表示建立的扩展雅克比矩阵；U表示操作空间控制量，τ表示仿生单腿各个关节的原始关节空间控制量组成的集合，集合τ的表达式为τ＝[τ_1d；τ_2d；τ_3d；τ_4d]，其中τ_1d、τ_2d、τ_3d、τ_4d分别表示仿生单腿各个关节的原始关节空间控制量。

进一步地，所述步骤4.1)选择指定的优化目标为能量消耗、液压油的流速、液压缸的伸缩速度、使腿运动范围最大中的一种。

本发明液压驱动型仿生单腿双环控制方法具有下述技术效果：

1、本发明主要由外环力/位混合控制、内环单关节力控制两部分组成，通过步骤1)～步骤4)实现了仿生单腿各个关节的外环力/位混合控制，外环力/位混合控制的输入信号为原始期望足端位置轨迹、闭环反馈的输入信号为仿生单腿各个关节的关节位置信息，基于外环力/位混合控制依次通过重新规划、自抗扰控制、扩展雅克比的冗余优化得到仿生单腿各个关节的原始关节空间控制量，外环力/位混合控制部分主要是实现在位置不受限的情况下实现期望轨迹的跟踪、在位置受限的情况下修改期望轨迹以减小冲击给机器人系统稳定带来的影响，主要输出信息是各关节的期望输出力矩；通过步骤5)则实现了仿生单腿各个关节的内环单关节力控制，内环单关节力控制的输入信号为原始关节空间控制量、闭环反馈的输入信号为仿生单腿各个关节的输出力信息，通过内环单关节力控制对原始关节空间控制量进行修正得到修正后的关节空间控制量，分别基于仿生单腿各个关节的关节空间控制量控制仿生单腿各个关节的液压动作器的动作，内环单关节力控制部分主要是实现对于关节期望力矩的跟踪，利用检测的关节位置和作动器输出力，解算实际关节输出力矩，与期望力矩进行比较，生成关节作动器的控制电流，驱动各关节作动器运动。本发明采用上述外环力/位混合控制、内环单关节力控制构成的双环控制方法，能够使足式机器人适应不同的地形，而不需要针对特定地形设计专门的规划轨迹，增加了机器人在运动过程中的柔顺能力，提高了机器人对于非结构化地形的适应能力，具有地形适应能力好、柔顺能力好的优点。

2、本发明的实现仅仅依赖于仿生单腿的基本传感器体系，包括用于采集各个关节的关节位置信息的关节位置传感器、用于采集仿生单腿的足端力信息的传感器、用于采集各个关节的输出力信息的力或者力矩传感器，上述传感器均为足式机器人的基本传感器体系，此外本发明的实现不依赖特定的硬件结构，因此只需要构建前述的基本传感器体系，通过基本传感器体系获取控制对象的模型参数即可实现本发明的技术方案，而且本发明基于仿生单腿控制，既可以用于四足机器人，还可以用于其它数量的多足机器人，具有通用性强的优点。

3、本发明利用指定的优化目标构造约束条件并建立扩展雅可比矩阵，基于扩展雅克比的冗余优化方法将操作空间控制量转换为仿生单腿各个关节的原始关节空间控制量，充分利用了仿生单腿的冗余设计特点。

4、本发明基于扩展雅克比的冗余优化方法将所述操作空间控制量转换为仿生单腿各个关节的原始关节空间控制量时，能够将具体的仿生单腿模型与液压作动器模型通过双环控制结构进行分离，提高了算法的实时性，同时应用自抗扰控制原理，对各种扰动在操作空间控制环进行了估计和消除，进一步提高了算法的鲁棒性。

5、本发明的结构清晰、层次分明，具有较好的理论价值和应用价值，能够应用于各类足式机器人，在工业、消防、航天等领域具有广泛的应用前景，具有应用范围广的优点。

附图说明

图1为本发明实施例双环控制方法的实施流程示意图。

图2为本发明实施例中自抗扰控制原理示意图。

图3为应用本发明实施例方法的仿生单腿的滚动关节结构示意图。

图4为应用本发明实施例方法的仿生单腿的俯仰关节结构示意图。

具体实施方式

如图1所示，本实施例液压驱动型仿生单腿双环控制方法的实施步骤如下：

1)接收外部输入的原始期望足端位置轨迹X_d:(x_d,y_d,z_d)；

2)基于接触力信息的轨迹修正：采集仿生单腿的足端力信息(F_x,F_y,F_z)，当足端力信息(F_x,F_y,F_z)显示足端完全接触到地面时，获取仿生单腿各个关节的关节位置信息(l₁,l₂,l₃,l₄)，根据关节位置信息(l₁,l₂,l₃,l₄)进行正运动学解算得到足端实际位置X:(x,y,z)，根据足端实际位置X:(x,y,z)对原始期望足端位置轨迹X_d:(x_d,y_d,z_d)进行重新规划，得到重新规划后的期望足端位置轨迹X_rd:(x_rd,y_rd,z_rd)；

3)自抗扰控制：将期望足端位置轨迹X_rd:(x_rd,y_rd,z_rd)包含的足端x、y、z三个方向的期望轨迹(x_rd,y_rd,z_rd)分别进行自抗扰控制，分别得到足端x、y、z三个方向的控制量(u_x,u_y,u_z)，足端x、y、z三个方向的控制量构成仿生单腿的操作空间控制量U:(u_x,u_y,u_z)；

4)基于扩展雅克比的冗余优化：利用指定的优化目标构造约束条件并建立扩展雅可比矩阵J^e，基于扩展雅克比的冗余优化方法将操作空间控制量U:(u_x,u_y,u_z)转换为仿生单腿各个关节的原始关节空间控制量τ:(τ_1d；τ_2d；τ_3d；τ_4d)；

5)内环单关节力控制：采集仿生单腿各个关节的输出力信息，基于输出力信息对仿生单腿各个关节的原始关节空间控制量τ:(τ_1d；τ_2d；τ_3d；τ_4d)进行修正得到修正后的关节空间控制量τ:(τ₁；τ₂；τ₃；τ₄)，分别基于仿生单腿各个关节的关节空间控制量(τ₁,τ₂,τ₃,τ₄)控制仿生单腿各个关节的液压动作器的动作。

本实施例的方法主要由外环力/位混合控制、内环单关节力控制两部分组成，通过步骤1)～步骤4)实现了仿生单腿各个关节的外环力/位混合控制，考虑了足端与环境的接触力，实现了外部柔顺(仿生单腿与环境的接触力约束)，通过步骤5)则实现了仿生单腿各个关节的内环单关节力控制，建立了内部力控制回路，实现了内部柔顺(液压作动器输出力约束)，从而实现了外部柔顺(仿生单腿与环境的接触力约束)和内部柔顺(液压作动器输出力约束)的统一，从根本上减小传统位置控制对于摆动足端落地碰撞时产生的接触力，从而降低足端与地面接触对机器人稳定性、机构造成的损害。

本实施例中，步骤2)中具体是指根据式(1)对原始期望足端位置轨迹进行重新规划；

$\left\{\begin{array}{c}{x}_{rd}\left(t\right)=x+\frac{\left(x_d\right(T)-x)}{T}*t\\ y_{rd}\left(t\right)=y+\frac{\left(y_d\right(T)-y)}{T}*t\\ z_{rd}\left(t\right)=z+\sin \left(\frac{2*\mathrm{\&pi;}*t}{T}\right)+\frac{\left(z_d\right(T)-z)}{T}*t\end{array}\right.---\left(1\right)$

式(1)中，X_d:(x_d(T),y_d(T),z_d(T))表示外部输入的原始期望足端位置轨迹，其中T表示步行周期，t表示从落脚开始至今的时间，X_rd:(x_rd,y_rd,z_rd)表示重新规划后的期望足端位置轨迹，X:(x,y,z)表示足端实际位置。

如图2所示，本实施例步骤3)中将期望足端位置轨迹X_rd:(x_rd,y_rd,z_rd)包含的足端x方向的期望轨迹x_rd进行自抗扰控制的详细步骤如下：

3.1)安排过渡过程：根据期望足端位置轨迹X_rd:(x_rd,y_rd,z_rd)包含的足端x方向的期望轨迹x_rd为自抗扰控制器安排过渡过程，得到如式(2)所示的光滑的输入信号及其微分信号；

$\left\{\begin{array}{c}{f}_h=fhan(v_{11}-x_{rd},v_{12},r_0,h)\\ v_{11}=v_{11}+{\mathrm{hv}}_{12}\\ v_{12}=v_{12}+{\mathrm{hf}}_h\end{array}\right.---\left(2\right)$

式(2)中，fhan表示最速控制综合函数，v₁₁表示安排过渡过程得到的光滑的输入信号，x_rd表示期望足端位置轨迹包含的足端x方向的期望轨迹，v₁₂表示输入信号的微分信号，r₀为用于决定输入信号v₁₁跟踪足端x方向的期望轨迹快慢的速度因子，h为用于滤掉输入信号v₁₁所含的噪声滤波因子；

3.2)非线性组合：根据式(3)构建的自抗扰控制器的非线性组合；

$\left\{\begin{array}{c}{e}_1=v_{11}-z_{11}\\ e_2=v_{12}-z_{12}\\ u_0=-fhan(e_1,{\mathrm{ce}}_2,r,h)\\ u_x=(u_0-z_{13})/b_0\end{array}\right.---\left(3\right)$

式(3)中，z₁₁表示反馈得到的输入信号v₁₁的观测值，z₁₂表示反馈得到的微分信号v₁₂的观测值，z₁₃为反馈得到的扰动项f的观测值，e₁表示位置误差，e₂表示误差微分信号，u₀表示对应足端x方向的非线性组合的初步输出值，fhan表示最速控制综合函数，u_x表示足端x方向的控制量，c、r和b₀分别表示抗扰控制器的非线性组合可调参数，h为用于滤掉输入信号v₁₁所含的噪声滤波因子；

3.3)扩张状态观测器：根据自抗扰控制器的最终输出值和足端x方向的控制量构建如式(4)所示的扩张状态观测器，分别将输入信号的观测值、微分信号的观测值、扰动项的观测值返回输出；

$\left\{\begin{array}{c}e=z_{11}-y\\ fe=fal(e,0.5,h)\\ {\mathrm{fe}}_1=fal(e,0.25,h)\\ z_{11}=z_{11}+h(z_{12}-{\mathrm{\&beta;}}_{01}e)\\ z_{12}=z_{12}+h(z_{13}-{\mathrm{\&beta;}}_{02}fe+b_0{u}_x)\\ z_{13}=z_{13}+h(-{\mathrm{\&beta;}}_{03}{\mathrm{fe}}_1)\end{array}\right.---\left(4\right)$

式(4)中，z₁₁表示反馈输出的输入信号v₁₁的观测值，z₁₂表示反馈输出的微分信号v₁₂的观测值，z₁₃为反馈输出的扰动项f的观测值，e表示反馈输出的输入信号v₁₁的观测值和自抗扰控制器的最终输出值之间的误差，h为用于滤掉输入信号v₁₁所含的噪声滤波因子，y表示自抗扰控制器的最终输出值，u_x表示对应足端x方向的非线性组合的最终输出值，b₀表示抗扰控制器的非线性组合可调参数，β₀₁、β₀₂和β₀₃分别为扩张状态观测器的可调参数，其中fal函数的表达式如式(5)所示；

$fal(e,\mathrm{\&alpha;},h)=\{\begin{array}{c}{\left|e\right|}^{\mathrm{\&alpha;}}sign\left(e\right),\left|e\right|>h\\ \left|e\right|/h^{1-\mathrm{\&alpha;}},\left|e\right|\&le;h\end{array},0<\mathrm{\&alpha;}<1,h>0---\left(5\right).$

本实施例中，获取足端y方向的控制量u_y、z方向的控制量u_z和前述的步骤3.1)～3.3)获取x方向的控制量u_x的方法相同，在此不再赘述。最终得到x方向的控制量u_x、y方向的控制量u_y、z方向的控制量u_z构成的操作空间控制量U:(u_x,u_y,u_z)。

本实施例中，步骤4)的详细步骤如下：

4.1)选择指定的优化目标；选择指定的优化目标可以根据需要选择能量消耗、液压油的流速、液压缸的伸缩速度、使腿运动范围最大中的一种。本实施例中，选择指定的优化目标为使腿运动范围最大。

4.2)根据优化指标生成约束条件G(θ_i)＝0。

4.3)基于约束条件建立如式(6)所示的扩展雅克比矩阵。

$J^e=\left[\begin{array}{c}{J}_{j\&times;k}\\ \&part;G/\&part;\mathrm{\&theta;}\end{array}\right]---\left(6\right)$

式(6)中，J^e表示建立的扩展雅克比矩阵；J_j×k表示仿生单腿的单腿关节空间到操作空间的雅可比矩阵，其中j表示仿生单腿的操作空间自由度数量，k表示仿生单腿的单腿关节空间自由度数量；G为关于θ_i的非线性函数且满足约束条件G(θ_i)＝0，其中θ_i中的序号i表示仿生单腿的关节序号，θ为仿生单腿的关节空间旋转运动量；本实施例中，由于机器人单腿关节空间有(θ₁,θ₂,θ₃,θ₄)四个自由度，操作空间只有(x,y,z)三个自由度，所以腿的运动性能存在优化空间，因此基于约束条件G(θ_i)＝0建立的扩展雅克比矩阵J^e中，J_j×k具体为J_3×4，即本实施例中，j表示的仿生单腿的操作空间自由度数量为3，k表示的仿生单腿的单腿关节空间自由度数量为4。显然，通过建立不同j、k取值的仿生单腿的单腿关节空间到操作空间的雅可比矩阵，即可将具体的仿生单腿模型与液压作动器模型通过双环控制结构进行分离。

4.4)根据式(7)将扩展雅克比矩阵和单腿动力学方程的惯量矩阵结合，将操作空间控制量转换为仿生单腿各个关节的原始关节空间控制量。

τ＝A(θ)(J^e)^-1U (7)

式(7)中，A(θ)表示单腿动力学方程的惯量矩阵，J^e表示建立的扩展雅克比矩阵；U表示操作空间控制量，τ表示仿生单腿各个关节的原始关节空间控制量组成的集合，集合τ的表达式为τ＝[τ_1d；τ_2d；τ_3d；τ_4d]，其中τ_1d、τ_2d、τ_3d、τ_4d分别表示仿生单腿各个关节的原始关节空间控制量。

本实施例中，步骤5)内环单关节力控制环节基于仿生单腿各个关节的关节空间控制量(τ₁,τ₂,τ₃,τ₄)控制仿生单腿各个关节的液压动作器的动作时，具体是基于仿生单腿各个关节的关节空间控制量(τ₁,τ₂,τ₃,τ₄)设计对应的关节力控制器，通过关节力控制器将对应关节的关节空间控制量转换得到各作动器的控制电流，从而控制对应关节的液压作动器输出力。同时，在各液压作动器输出力的作用下，液压作动器的长度发生变化，驱动仿生单腿各关节角位置发生变化，仿生单腿末端位置、与环境的作用力也发生相应变化。利用液压作动器长度通过运动学计算得到的仿生单腿末端位置，利用足端力传感器采集足端与地面接触力，将末端位置与力信息反馈到外环力/位混合控制器。

本实施例中具体应用的机器人为液压驱动型四足机器人，但是显然本实施例也可以应用于液压驱动型的其它多足机器人。对应本实施例中具体应用的机器人而言，仿生单腿主体结构包括腿部连杆、各关节液压作动器、各关节位置传感器、各关节力传感器、足端三维力传感器。仿照四足哺乳动物后腿的生理结构，仿生单腿包括髋关节、膝关节和踝关节三个主动关节，此外还包括踝部一个被动关节。其中，髋关节具有俯仰和滚动两个自由度，而膝关节和踝关节的自由度均为俯仰方向。对于足端而言，因为理论上与地面是点接触，在操作空间具有3个自由度，而腿部总共有4个自由度，因此该仿生单腿具有1个冗余自由度。在设计中，为了便于系统集成，液压作动器、关节位置传感器和力传感器采用一体化设计，三维力传感器安装在足部末端，用于检测足端与环境之间的作用力。

如图3和图4所示，应用本实施例方法的液压驱动型四足机器人的仿生单腿包括滚动关节结构和俯仰关节结构。仿生单腿的滚动关节结构中，躯干框架上设有仿生单腿的髋部滚动关节(机器人单腿关节空间的第四个自由度)，髋部滚动关节包括两个冗余的液压作动器#4，两个冗余的液压作动器#4的一端均与躯干框架相连，另一端则分别通过一个滚动连杆与髋部滚动关节相连，θ₄为髋部滚动关节角，液压作动器#4的一端通过固定长度为d₇的滚动连杆连接到滚动关节、另一端固连在躯干框架上，d₇表示滚动连杆的固定长度，d₈表示液压作动器#4与躯干框架相连的固连点与髋部滚动关节的关节转动轴之间的距离(虚拟连线的长度)，这样随着液压作动器#4运动时长度l₄的变化，实现髋部滚动关节角θ₄的变化，其变化关系如式(8)所示。仿生单腿的俯仰关节结构中，仿生单腿的主题结构包括依次相连的躯干框架、大腿连杆、小腿连杆和踝部连杆，L₁表示大腿连杆的长度，L₂表示小腿连杆的长度，L₃表示踝部连杆到足端的长度，仿生单腿的俯仰关节从上至下依次包括髋部俯仰关节(机器人单腿关节空间的第一个自由度)、膝部俯仰关节(机器人单腿关节空间的第二个自由度)、踝部俯仰关节(机器人单腿关节空间的第三个自由度)。θ₁为髋部俯仰关节角，是躯干框架和大腿连杆之间的夹角，液压作动器#1两端分别固定于躯干框架和大腿连杆，d₁表示液压作动器#1在躯干框架上两的端点和髋部俯仰关节转动轴之间的固定距离，d₂表示液压作动器#1在大腿连杆上的端点和髋部俯仰关节转动轴之间的固定距离，液压作动器#1动作时，其长度l₁变化，从而带动髋部俯仰关节角θ₁的变化，具体变化规律如式(8)所示。θ₂为膝部俯仰关节角，是小腿连杆和大腿连杆之间的夹角，液压作动器#2两端分别固定于小腿连杆和大腿连杆，d₃表示液压作动器#2在大腿连杆上的端点和膝部俯仰关节转动轴之间的固定距离，d₄表示液压作动器#2在小腿连杆上的端点和膝部俯仰关节转动轴之间的固定距离，液压作动器2动作时，其长度l₂变化，从而带动膝部俯仰关节角θ₂的变化，具体变化规律如式(8)所示。θ₃为踝部俯仰关节角，是小腿连杆和踝部连杆之间的夹角，液压作动器#3两端分别固定于小腿连杆和踝部连杆，d₅表示液压作动器#3在小腿连杆上的端点和踝部俯仰关节转动轴之间的固定距离，d₆表示液压作动器#3在踝部连杆上的端点和踝部俯仰关节转动轴之间的固定距离。液压作动器#3动作时，其长度l₃变化，从而带动踝部俯仰关节角θ₃的变化，具体变化规律如式(8)所示。

本实施例中，根据关节位置信息(l₁,l₂,l₃,l₄)进行正运动学解算得到足端实际位置的方法与现有技术相同，其输入为仿生单腿各个关节的关节位置信息(l₁,l₂,l₃,l₄)，输出为足端实际位置X。其基本原理如下：由于各个液压作动器与虚拟连杆之间的构成的三角形的另外两边的长度固定且可以通过测量得到，因此可以解算出相邻连杆之间的夹角，进而利用约束关系解算得到(θ₁,θ₂,θ₃,θ₄)如式(8)所示。

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_1=\arg \cos \left(\frac{{d_1}^2+{d_2}^2-{l_1}^2}{2d_1{d}_2}\right)\\ {\mathrm{\&theta;}}_2=\arg \cos \left(\frac{{d_3}^2+{d_4}^2-{l_2}^2}{2d_3{d}_4}\right)\\ {\mathrm{\&theta;}}_3=\arg \cos \left(\frac{{d_5}^2+{d_6}^2-{l_3}^2}{2d_5{d}_6}\right)\\ {\mathrm{\&theta;}}_4=\arg \cos \left(\frac{{d_7}^2+{d_8}^2-{l_1}^2}{2d_7{d}_8}\right)\end{array}\right.---\left(8\right)$

式(8)中，θ₁～θ₄以及d₁～d₈的含义如前文所述，在此不再赘述。在得到(θ₁,θ₂,θ₃,θ₄)的基础上，由于虚拟连杆的长度固定且已知，进而通过几何解算即可得到足端点相对于躯干与单腿连接处的相对位置如式(9)所示，该位置即为足端实际位置X:(x,y,z)。

$\{\begin{array}{c}x=2*L_1*\cos ({\mathrm{\&theta;}}_1+{\mathrm{\&theta;}}_2)+2*L_1*\cos \left({\mathrm{\&theta;}}_1\right)+2*L_3*\cos ({\mathrm{\&theta;}}_1+{\mathrm{\&theta;}}_2+{\mathrm{\&theta;}}_3)\\ y=\tan ({\mathrm{\&theta;}}_4-\mathrm{\&pi;}/4)*sqrt(x^2+z^2)\\ z=-2*L_2*\sin ({\mathrm{\&theta;}}_1+{\mathrm{\&theta;}}_2)-2*L_1*\sin \left({\mathrm{\&theta;}}_1\right)-2*L_3*\sin ({\mathrm{\&theta;}}_1+{\mathrm{\&theta;}}_2+{\mathrm{\&theta;}}_3)\end{array}---\left(9\right)$

式(9)中，x、y、z依次为足端实际位置X的x、y、z的位置，参见图3和图4，L₁表示大腿连杆的长度，L₂表示小腿连杆的长度，L₃表示踝部连杆到足端的长度，θ₁表示髋部俯仰关节角，θ₂表示膝部俯仰关节角，θ₃表示踝部俯仰关节角，θ₄表示髋部滚动关节角。需要说明的是，本实施例仅仅是以图3和图4所示的仿生单腿为例说明根据关节位置信息(l₁,l₂,l₃,l₄)进行正运动学解算得到足端实际位置X的方法，此外，本实施例解算足端实际位置的方法同样也可以适用于其它不同的仿生单腿结构，同时针对其它不同的仿生单腿结构，本实施例也可以采用其它的足端实际位置的解算方法，在此不再赘述。

与现有技术相同，本实施例中最速控制综合函数fhan的表达式如式(10)所示。

$fhan(x_1,x_2,r,h)=\left\{\begin{array}{c}d={\mathrm{rh}}^2,a_0={\mathrm{hx}}_2,y=x_1+a_0\\ a_1=\sqrt{d(d+8|y\left|\right)}\\ a_2=a_0+sign\left(y\right)(a_1-d)/2\\ s_y=\left(sign\right(y+d)-sign(y-d\left)\right)/2\\ a=(a_0+y-a_2)s_y+a_2\\ s_a=\left(sign\right(a+d)-sign(a-d\left)\right)/2\\ fhan=-r(a/d-sign(a-d\left)\right)s_a-rsign\left(a\right)\end{array}\right.---\left(10\right)$

式(9)中，x₁、x₂、r、h分别为传递给最速控制综合函数fhan的参数，sign为符号函数，变量d、a、a₀、a₁、a₂、y、s_y、s_a均为中间变量。

本实施例中，外环力/位混合控制的输入为期望位置轨迹，通过采集各关节位置信息和足端力信息，设计力/位混合控制器，得到内环单关节力控制的参考输入——各关节的期望输出力矩。内环单关节力控制通过采集作动器的输出力信息和作动器位置信息，并根据各关节的期望输出力矩，设计单作动器力控制器，得到各作动器的控制电流，控制仿生单腿各作动器的输出力。基于外环力/位混合控制和内环单关节力控制，具有下述优点，1)控制结构清晰。将液压作动器的控制、关节控制与单腿整体控制分离，减小了腿部多关节结构与液压作动器非线性特性之间的耦合，从控制回路上加以分隔，降低了控制器设计的难度，同时也降低了对控制板处理能力的要求，有利于算法的实现；2)具有较好的环境适应能力。通过足端力检测和阈值的设定，可以减小足端与地面的冲击，降低足端/地面接触对于上体稳定性带来的影响，提高机器人对于不平整地面的适应能力；3)具有系统保护功能。通过液压作动器输出力和足端接触力阈值的设定，可以在足端、液压作动器位置受限的情况下有效限制液压作动器的输出力，从而保护机器人单腿结构不受损害，同时也可以保护仿生腿传动、驱动及机器人机载各部件。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种液压驱动型仿生单腿双环控制方法，其特征在于实施步骤如下：

1)接收外部输入的原始期望足端位置轨迹；

2)采集仿生单腿的足端力信息，当足端力信息显示足端完全接触到地面时，获取仿生单腿各个关节的关节位置信息，根据所述关节位置信息进行正运动学解算得到足端实际位置，根据所述足端实际位置对原始期望足端位置轨迹进行重新规划，得到重新规划后的期望足端位置轨迹；

3)将所述期望足端位置轨迹包含的足端x、y、z三个方向的期望轨迹分别进行自抗扰控制，分别得到足端x、y、z三个方向的控制量，所述足端x、y、z三个方向的控制量构成仿生单腿的操作空间控制量；

4)利用指定的优化目标构造约束条件并建立扩展雅可比矩阵，基于扩展雅克比的冗余优化方法将所述操作空间控制量转换为仿生单腿各个关节的原始关节空间控制量；

5)采集仿生单腿各个关节的输出力信息，基于所述输出力信息对仿生单腿各个关节的原始关节空间控制量进行修正得到修正后的关节空间控制量，分别基于仿生单腿各个关节的关节空间控制量控制仿生单腿各个关节的液压动作器的动作。

2.根据权利要求1所述的液压驱动型仿生单腿双环控制方法，其特征在于：所述步骤2)中具体是指根据式(1)对原始期望足端位置轨迹进行重新规划；

$\left\{\begin{array}{c}{x}_{rd}\left(t\right)=x+\frac{\left(x_d\right(T)-x)}{T}*t\\ y_{rd}\left(t\right)=y+\frac{\left(y_d\right(T)-y)}{T}*t\\ z_{rd}\left(t\right)=z+\sin \left(\frac{2*\mathrm{\&pi;}*t}{T}\right)+\frac{\left(z_d\right(T)-z)}{T}*t\end{array}\right.---\left(1\right)$

式(1)中，X_d:(x_d(T),y_d(T),z_d(T))表示外部输入的原始期望足端位置轨迹，其中T表示步行周期，t表示从落脚开始至今的时间，X_rd:(x_rd,y_rd,z_rd)表示重新规划后的期望足端位置轨迹，X:(x,y,z)表示足端实际位置。

3.根据权利要求2所述的液压驱动型仿生单腿双环控制方法，其特征在于，所述步骤3)中将期望足端位置轨迹包含的足端x方向的期望轨迹进行自抗扰控制的详细步骤如下：

3.1)安排过渡过程：根据所述期望足端位置轨迹包含的足端x方向的期望轨迹为自抗扰控制器安排过渡过程，得到如式(2)所示的光滑的输入信号及其微分信号；

$\left\{\begin{array}{c}{f}_h=fhan(v_{11}-x_{rd},v_{12},r_0,h)\\ v_{11}=v_{11}+{\mathrm{hv}}_{12}\\ v_{12}=v_{12}+{\mathrm{hf}}_h\end{array}\right.---\left(2\right)$

式(2)中，fhan表示最速控制综合函数，v₁₁表示安排过渡过程得到的光滑的输入信号，x_rd表示期望足端位置轨迹包含的足端x方向的期望轨迹，v₁₂表示所述输入信号的微分信号，r₀为用于决定输入信号v₁₁跟踪足端x方向的期望轨迹快慢的速度因子，h为用于滤掉输入信号v₁₁所含的噪声滤波因子；

3.2)非线性组合：根据式(3)构建的自抗扰控制器的非线性组合；

$\left\{\begin{array}{c}{e}_1=v_{11}-z_{11}\\ e_2=v_{12}-z_{12}\\ u_0=-fhan\left(e_1,{\mathrm{ce}}_2,r,h\right)\\ u_x=\left(u_0-z_{13}\right)/b_0\end{array}\right.---\left(3\right)$

式(3)中，z₁₁表示反馈得到的输入信号v₁₁的观测值，z₁₂表示反馈得到的微分信号v₁₂的观测值，z₁₃为反馈得到的扰动项f的观测值，e₁表示位置误差，e₂表示误差微分信号，u₀表示对应足端x方向的非线性组合的初步输出值，fhan表示最速控制综合函数，u_x表示足端x方向的控制量，c、r和b₀分别表示抗扰控制器的非线性组合可调参数，h为用于滤掉输入信号v₁₁所含的噪声滤波因子；

3.3)扩张状态观测器：根据自抗扰控制器的最终输出值和足端x方向的控制量构建如式(4)所示的扩张状态观测器，分别将输入信号的观测值、微分信号的观测值、扰动项的观测值返回输出；

$\left\{\begin{array}{c}e=z_{11}-y\\ fe=fal\left(e,0.5,h\right)\\ {\mathrm{fe}}_1=fal\left(e,0.25,h\right)\\ z_{11}=z_{11}+h\left(z_{12}-{\mathrm{\&beta;}}_{01}e\right)\\ z_{12}=z_{12}+h\left(z_{13}-{\mathrm{\&beta;}}_{02}fe+b_0{u}_x\right)\\ z_{13}=z_{13}+h\left(-{\mathrm{\&beta;}}_{03}{\mathrm{fe}}_1\right)\end{array}\right.---\left(4\right)$

式(4)中，z₁₁表示反馈输出的输入信号v₁₁的观测值，z₁₂表示反馈输出的微分信号v₁₂的观测值，z₁₃为反馈输出的扰动项f的观测值，e表示反馈输出的输入信号v₁₁的观测值和自抗扰控制器的最终输出值之间的误差，h为用于滤掉输入信号v₁₁所含的噪声滤波因子，y表示自抗扰控制器的最终输出值，u_x表示对应足端x方向的非线性组合的最终输出值，b₀表示抗扰控制器的非线性组合可调参数，β₀₁、β₀₂和β₀₃分别为扩张状态观测器的可调参数，其中fal函数的表达式如式(5)所示；

$fal(e,\mathrm{\&alpha;},h)=\left\{\begin{array}{c}|e{|}^{\mathrm{\&alpha;}}sign\left(e\right),|e|>h\\ \left|e\right|/h^{1-\mathrm{\&alpha;}},\left|e\right|\&le;h\end{array}\right.,0<\mathrm{\&alpha;}<1,h>0---\left(5\right).$

4.根据权利要求3所述的液压驱动型仿生单腿双环控制方法，其特征在于，所述步骤4)的详细步骤如下：

4.1)选择指定的优化目标；

4.2)根据所述优化指标生成约束条件；

4.3)基于所述约束条件建立如式(6)所示的扩展雅克比矩阵；

$J^e=\left[\begin{array}{c}{J}_{j\&times;k}\\ \&part;G/\&part;\mathrm{\&theta;}\end{array}\right]---\left(6\right)$

式(6)中，J^e表示建立的扩展雅克比矩阵；J_j×k表示仿生单腿的单腿关节空间到操作空间的雅可比矩阵，其中j表示仿生单腿的操作空间自由度数量，k表示仿生单腿的单腿关节空间自由度数量；G为关于θ_i的非线性函数且满足约束条件G(θ_i)＝0，其中θ_i中的序号i表示仿生单腿的关节序号，θ为仿生单腿的关节空间旋转运动量；

4.4)根据式(7)将扩展雅克比矩阵和单腿动力学方程的惯量矩阵结合，将所述操作空间控制量转换为仿生单腿各个关节的原始关节空间控制量；

τ＝A(θ)(J^e)^-1U (7)

式(7)中，A(θ)表示单腿动力学方程的惯量矩阵，J^e表示建立的扩展雅克比矩阵；U表示操作空间控制量，τ表示仿生单腿各个关节的原始关节空间控制量组成的集合，集合τ的表达式为τ＝[τ_1d；τ_2d；τ_3d；τ_4d]，其中τ_1d、τ_2d、τ_3d、τ_4d分别表示仿生单腿各个关节的原始关节空间控制量。

5.根据权利要求4所述的液压驱动型仿生单腿双环控制方法，其特征在于：所述步骤4.1)选择指定的优化目标为能量消耗、液压油的流速、液压缸的伸缩速度、使腿运动范围最大中的一种。