首页 > 编程语言> > 【足式机器人控制算法】（5.1）分解式虚拟模型VMC解耦思想 +阻抗控制（弹簧-阻尼件）规划反作用力 +运动学雅可比+虚功原理规划关节扭矩方法

【足式机器人控制算法】（5.1）分解式虚拟模型VMC解耦思想 +阻抗控制（弹簧-阻尼件）规划反作用力 +运动学雅可比+虚功原理规划关节扭矩方法

2021-12-22 14:59:26 作者：互联网

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

文章目录

系列文章目录
前言
一、分解式VMC 的实现原理
二、分解式VMC在四足机器人上的应用
总结

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

本文先对分解式虚拟模型VMC解耦思想+阻抗控制（弹簧-阻尼件）规划反作用力+运动学雅可比+虚功原理规划关节扭矩方法做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

提示：以下是本篇文章正文内容

一、分解式VMC 的实现原理

1.虚拟力的定义

虚拟力不是实际存在的作用力或力矩，机器人的运动最终是通过关节力矩实现的，但是总的关节力矩合成等效为一个虚拟力
.
.

2.虚拟力分配方法

虚拟腿力分配即仅考虑静力学下的力平衡问题，当机身负载不均匀时为保证姿态稳定，基于力和力臂关系可知离质心更近的腿需要产生更多的力
在这里插入图片描述
对Trot步态来说虚拟腿构建于两支撑腿连线中点
对Pace步态虚拟腿构建在两侧腿的中间
对Bound步态在前后量侧腿的中间。在Bound步态虚拟腿无法在质心处提供支撑，足端很可能由于机械尺寸约束无法靠近质心，因此可能无法简单地将Bound归入Trot和Pace统一的控制框架中，其对俯仰轴的控制是一个十分重要的研究内容

3.在每个需要控制的自由度上构造恰当的虚拟构件以产生合适的虚拟力

（1）计算虚拟力的方法

1）机械上的方法

在机器人腿部安装机械参数合适的弹簧（比例增益K1）和阻尼器（比例增益K2）

缺点：成本高、机械参数因为磨损、疲劳和安装位置不同难以确定，维护成本极高
.
.

2）控制的方法（阻抗控制）

使用阻抗控制的方法虚拟构件主要是弹簧与阻尼器

反馈控制：
阻抗控制的本质是用两个输入量的P控制器实现的，控制率为：
在这里插入图片描述
反馈控制：
使用阻抗控制的方法虚拟构件主要是弹簧与阻尼器
阻抗控制的本质是用两个输入量的P控制器实现的，控制率为：

其中：
（1）F为虚拟构件产生的虚拟力
（2）K 和 B分别为弹性和阻尼系数
（3）u和u’分别为相应广义坐标的实时位移和速度
（4）下标d表示期望值
虚拟力常常施加在机器人机体质心处，以控制机器人机体实现期望运动。
如果要控制机器人某一点保持在期望的位置(例如质心高度控制)，则同时施加弹簧和阻尼构件；
若要控制机器人达到某期望速度(例如水平移动速度控制)，则只施加阻尼构件(令 )

前馈控制：
还可以在最右端加上一个前馈项，用来提前补偿相应方向上的力或者力矩，例如在竖直方向加上重力补偿项

（2）虚拟力的向量三维表达形式

虚拟力的向量表达形式能表示坐标轴三个方向上不同K、B参数的阻抗P控制率
在这里插入图片描述
其中：
（1）m 个机体自由度对应的位置向量
（2）对应的虚拟力列向量
（3）K和B分别是弹性和阻尼系数矩阵(对角矩阵)，且有：

.
.

4.在不同的相位状态利用相应的 Jacobian矩阵计算期望的关节力矩

VMC原理控制关节扭矩的过程使用了运动学模型中的雅可比矩阵和虚功原理

（1）计算雅可比矩阵方法

为了将工作空间(Task Space)的力或力矩映射成关节空间(Joint Space)的关节力矩，需要先求得这两个空间的位置映射关系，即正运动学模型
在这里插入图片描述
其中：
机体坐标系相对于地面坐标系的个自由度对应的位姿向量为

n个关节变量的位置向量为在这里插入图片描述

进一步求x分别对的偏导数得到：
在这里插入图片描述
即

写成以下形式

即得到 Jacobian 矩阵，它将关节速度映射成机体位姿速度

.
.

（2）计算虚拟力（外部作用力）和关节力矩之间的关系方法

根据虚功原理，可以得到：
在这里插入图片描述
其中
（1）关节力矩列向量

（2）外部作用力在这里插入图片描述

根据雅可比矩阵速度关系和虚功原理即：
在这里插入图片描述

可以得到外部作用力和关节力矩之间的关系：

在这里插入图片描述
（1）包含 n 个关节的期望力矩
（2）为作用坐标系与反作用坐标系之间的关联 Jacobian 矩阵的转置矩阵

在这个公式中，Jacobian 矩阵随着关节角度变化而变化。当J为奇异矩阵时，在某些特定方向上J可以增大或者减小却不会影响F的计算结果。也就是说，在奇异构型附近，很小的关节力矩就能够在某些方向上产生很大作用力。这也解释了为什么行走类四足动物在承载能力上具有较大优势，即当其腿结构在尽量伸展以后接近了奇异构型，只需要较小的关节力矩能够产生较大的竖直支撑力

.
.

二、分解式VMC在四足机器人上的应用

1.分解式 VMC 原理框图

在这里插入图片描述

.
.

2.控制原理

将总体控制目标所施加的虚拟模型按照一定的原则分解到各个支撑腿上，利用简单的单Jacobian 矩阵计算得到相应的关节力矩，并且构造单腿摆动相的虚拟模型进行摆动相 VMC 控制，根据步态规则及触地信号进行模型切换，各支撑腿相互协作，从而避免了复杂的全局 Jacobian 矩阵的推导并可以减小模型切换引入的扰动，同时在支撑相和摆动相均采用 VMC 方法进行控制，使控制算法进一步简化和统一。以实现机器人的机体的控制目标

对角步态中，，四足机器人机体的 6 个自由度控制只能依赖于两条对角支撑腿。换句话说：我们要控制的对象是机体的两个方向的速度(Vx,Vy)、机体高度z以及 3 个姿态角rpy，一共六个量

.
.

3.构造虚拟构件

二维空间四足机器人的虚拟构件图
在这里插入图片描述

原理如下：
对于二维空间的四足机器人，需要控制的机体自由度同样是(工作空间中)矢状平面内的三个自由度，即前进速度Vx 、机体高度z和前后倾角0

（1）对于控制前进速度Vx的方法：一个水平方向的阻尼器用来“牵引”机器人以调节前进速度
（2）对于控制机体高度z的方法：一个竖直方向的直线弹簧-阻尼器用来“支撑”机体以维持期望高度
（3）对于控制前后倾角0的方法：一个绕机体中心的旋转弹簧-阻尼器用来“扭转”机体以调整前后倾角大小

.
.

4.计算虚拟力和力矩

设虚拟构件变量为
在这里插入图片描述
二维平面三个方向虚拟力和力矩的计算公式为：

其中
（1）水平方向只有阻尼器，故Kx=0
（2）通常情况下不希望z和0两个方向具有期望速度，设定

（3）机器人的实时前进速度和倾角计算：通过机载惯性测量单元(Inertial Measurement Unit, IMU)测量计算得到
（4）机体高度z：可以利用支撑腿的正运动学计算得到(膝关节和髋关节角度大小可由关节角度传感器测量得到)
.
.

5.XYZ三个方向上的平动控制方法

机器人的机体平移控制包括了两个方向的速度(Vx,Vy)控制、机体高度z控制

（1）XYZ三个方向的虚拟力计算【阻抗控制原理】

作用在每个支撑腿髋部的虚拟构件为沿着和方向的阻尼构件(期望速度分别为x’d和y’d)以及沿着z方向的弹簧-阻尼构件(期望长度为zd)，它们产生的虚拟力用矩阵形式表达为：
在这里插入图片描述
式中下标i为腿的代号，kz、bz、by、bz为控制参数，机体高度方向的期望速度zi’d一般设为零
注意这里计算得到的虚拟力同样需要进行极值的约束，以防止期望关节力矩超过关节驱动器的极限并防止机器人与地面发生打滑

（2）关节期望力矩的计算

（1）计算雅可比矩阵J

（1）根据 DH 法建立变换坐标系
在这里插入图片描述
（2）测量运动学模型的 DH 参数

（3）相邻坐标之间的变换矩阵为：

将 DH 参数分别代入上式求得各相邻坐标的转换矩阵

（4）依次相乘得到，足端 Q 点坐标系相对髋部坐标系的变换矩阵为：
在这里插入图片描述
其中：
（1）R为足端坐标系相对髋部坐标系的旋转矩阵
（2）P为足端在髋部坐标系的位置坐标

（5）对位置坐标P分别对各关节角度求偏导数，得到 Jacobian 矩阵

式中，下标i为腿的编号，S、C分别表示

其中的关节角度为腿的相应关节角

（2）计算关节期望力矩

上面计算得到虚拟力（反作用力）:
在这里插入图片描述
上面计算得到Jacobian 矩阵：

为了将其转换为关节期望力矩，需要求机器人的单腿Jacobian 矩阵

在这里插入图片描述
其中

.
.

6.RPY三个方向上的姿态控制方法

机器人的机体姿态控制包括了俯仰控制、横滚控制和偏航控制

（1）俯仰角控制

通过前后支撑腿的高度控制来调节俯仰角，即通过调节腿 1 和 3 支撑时的高度，或者调节腿 2 和 4 支撑时的高度来调整俯仰角的大小,根据俯仰角与期望值的差，直觉地抬高或者降低相应髋部的期望高度值：
定义的高度均为正值，，转换为髋部坐标下，的坐标值为z=-h ，于是得到：
在这里插入图片描述

其中
（1）对应机体期望高度zd=-hd
（2）k为调节参数，且根据几何关系可以在初步设定k=L/2（L为机体长度)的基础上进行试验整定

.
.

（2）横滚角控制

β横滚角主要是由于机体翻转产生的，是三维空间运动区别于二维空间运动的主要特征之一，也是三维空间控制的一个难点。一般在动步态的时候设置横滚角β=0
在横滚角β方向上施加旋转式的虚拟弹簧-阻尼构件来抑制β的变化，产生的虚拟力矩 Mβ的大小为：
在这里插入图片描述
式中，Kβ和Bβ分别为弹簧刚度和阻尼系数

在这里插入图片描述为横滚控制目标
β和β’分别是机体实时的横滚角度和角速度，可由机载 IMU 测量计算得到

Mβ通过两个支撑腿的侧摆关节产生，于是直接将Mβ平分到这两个支撑腿，得到每条腿的侧摆关节需要对机体产生的额外作用力矩为：
在这里插入图片描述
即

其中：

根据作用和反作用力的关系，侧摆关节驱动器输出作用到腿部上的额外力矩应该为

通过在两条支撑腿的侧摆关节分别施加额外的关节力矩在这里插入图片描述，可以产生和相同的作用效果，从而实现了机体的横滚控制
.
.

（3）偏航角控制

四足机器人的运动前进方向通过控制偏航角来实现。施加绕机体坐标系Z轴的虚拟弹簧-阻尼构件，产生一个虚拟力矩Mγ来调节的大小，得到：
在这里插入图片描述
式中，K和B分别为弹簧刚度和阻尼系数；γ为期望偏航角度，由上层控制器给定；γd’ 一般设为零；偏航角度及其角速率γd’均由机载 IMU 实时测量计算得到
若选择用侧向力fx来产生偏航(转向)力矩【和横滚角β控制冲突，合成的化会产生耦合现象】