【论文解读--触地检测+FSM状态切换】（猎豹 3）Contact Model Fusion for Event-Based Locomotion in Unstructured Terrains

四足机器人运动控制相关教程及博客请关注专栏：
https://blog.csdn.net/qq_35635374/category_11523325.html

足式机器人&机械臂控制合集(这个比较全面)：
https://blog.csdn.net/qq_35635374/article/details/142862871

本文先对基于事件的非结构化地形运动接触模型融合做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

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

一、简介

传统的力估计是有噪声的，带有力传感技术是脆弱的，当足式机器人在非结构化环境中时，能够可靠地管理触地状态过渡将是一项关键技能。本文介绍了一种方法可能地融合接触模型，管理不确定性在地形几何、动态建模和运动学中提高接地时接触起始的稳健性。

Kalman框架下的其他接触先验融合滤波以提高方法的稳健性。这种方法结果准确的接触检测

一个基于事件的实现了有限状态机FSM来处理突发提前或延迟的触地事件

电机关节角度读数和惯性测量的反馈单位（IMU）。这些传感器中的每一个都只显式地提供对机器人身体和腿部状态的感知，而不是外部环境。

如果没有外力传感器，我们从编码器数据、自感知力和腿部模型的动态中估计腿部的接触状态【防盗标记–盒子君hzj】。编码器将具有离散化误差，包括编码器误差在内的模型误差将导致在偶尔显示外部力。所以这些单一测量都不能告诉我们腿的状态，但我们可以从每个测量中获取信息，并使用它们来找到更好的估计，确定某只脚是否与地面接触

.
.

二、整体思路

第一步：由于机器人不能直接测量力，所以推导了基于离散时间动量的观测器GM来估计腿上的外力

第二步：这个外力估计仍然是受噪声影响，因此设计了与地面接触的高度和力简单的概率模型

第三步：使用估计腿上的外力、测量概率模型进行卡尔曼融合，【防盗标记–盒子君hzj】提高接触状态估计的准确性。与能够可靠地感知与地面的接触

第四步：采用基于事件的切换策略FSM来处理早期和最近的接触。基于事件的框架允许机器人穿越有障碍物的非结构化地形使用直接的环境感知。

（自己总结的，不喜勿喷哈）

三、基于gm的干扰观测器估计腿上的外力

本节描述了对应用现在的经典广义动量基础（基于GM）离散时间的扰动观测器

1、基于动量（gm）的扰动观测器的原理

经典扰动观测器通过广义动量的前馈，加上滤波后的动态效应，得到滤波后的扰动力

给定的力矩控制输入施加在关节，基于模型预测的轨迹结果可以与实际结果进行比较，若实际的轨迹和预测的轨迹不同则归因于外部力干扰

因为腿部没有力传感器，机器人不能直接测量腿部所受的扰动外力，所以我们推导出一个基于离散时间动量的观测器来估计腿上的扰动外力，腿上的扰动外力估计仍然受到噪声的影响，单单用这个方法无法准确的判断接触状态

当关节电机按照对应支撑状态/摆动状态的控制率输出力矩时，对基于关节动力学模型的期望动力学轨迹可以与实际动力学轨迹进行比较，通过关节动量变化观测机器人整体在运动过程中受到的外力扰动，若假定机器人所有的外部干扰力都来自于脚，即不考虑躯干受到的外力影响到腿部关节的动力学轨迹，可以得到结论：若躯干实际的轨迹和基于期望的轨迹不相符时，机器人的对不必然受到干扰外力

这个假设似乎就有问题，当干扰外力来自于躯干时，得到的观测器结果必然不准确，这种基于关节动量（GM）的检测方法就更不准了，但是当腿部动力学轨迹很短，且轨迹更新的频率很快时，所以影响并不太大
.
.

2、基于关节动量（GM）的扰动观测器的输入

（1）关节电机的质量矩阵–机械参数
（2）电机反馈的编码器速度、加速度
（3）该时刻电机输出的扭矩
（4）腿的状态–选择矩阵

.

3、推导过程

（1）建立（关节的动力学模型表达式）标准方程（推导标准运动方程）

使用基于动量（GM）观测器的原理，就要对关节进行动力学建模，这对机械设计、传动摩擦的参数要求很高，不然建出来的模型误差很大。如果关节是忽略了质量和惯性的，自然没有动量的模型了，因此这相当于完全独立的检测模块，只有他才要建立自己的关节动力学模型
关节动力学模型表达式（即标准方程）：

其中：
q是关节角
Mq是关节实际观测的扭矩（包括了控制器输出的关节扭矩+外力干扰扭矩）
M是质量矩阵
Cq是广义科里奥利力（关节旋转产生的侧向力，类似于腕力球）
g是广义的引力
S是一个关节选择器矩阵（用于选择支撑腿的，根据现在处于支撑相/腾空相状态进行选择）

τd是关节计算输出的扭矩
Sτd是支撑腿计算的关节力矩
Ji是i接触的雅克比矩阵
fi是在i接触点的外力（反作用力）
相当于干扰外力的扰动矢量
.
.

（2）计算扰动向量（将外力收集成扰动矢量）

对扰动向量τd进行滤波（滤波后的扰动矢量更准确）
方法一：

其中s是拉普拉斯变量，λ是截止频率，τd给出了扰动的估计

方法二：

其中，是广义动量，是通过科里奥项的分解得到的

通过改变变量w=λMq-τd

这个方程被认为是低通滤波器

则

.
.

4、离散时间扰动矢量τd的计算

（1）离散化引入建模误差问题

当在离散时间中计算上面的滤波方程或积分时，发现离散化过程引入了作为虚拟扰动的建模误差。但是可以通过在离散时间引入一个完全替代的推导来解决这个建模误差

（2）对扰动矢量采样使用离散时间滤波器

离散时间方程

其中z是z域变量，1>γ>0，其中γ与截止频率单调相关。让工作=（1-γ）γk滤波器的脉冲响应

尽管这个公式包含的关节加速度通过测量，可以使用离散时间计算总和按部分积分的模拟，称为按部分求和。对于一般序列，fi和gi按部分求和

代入下面的公式：

假设q_0=0，滤波器输出为：

5、估计腿部扰动外力

不管连续时间还是离散时间的推导，力的估计都是用一般的方法计算的，最终在每一时刻形成估计力为：

S是腿部i的关节选择矩阵，Ji是腿部i的雅可比矩阵
.
.
.

四、设计与地面接触的高度和力简单的概率模型

1、解决的问题

我们不能简单地假定地面是完全平坦的，由于地形崎岖或看不见的物体，可能含有意外的地面高度。所以不能相信实际的步态切换会和基于相位周期的步态切换状态一致
.
.

2、创建一个简单的足端外力正态概率模型

地面高度:在没有环境感知系统的情况下，没有外部信息,因为我们无法对地形做出假设。但是我们可以对地面高度进行概率建模。地面的高度的随机变量是来自正态高斯分布。平均地面高度设置为零，方差与地形的“糙度”大致对应。因此，我们可以使用地面高度模型来创建一个理论的高度

3、估计的足部外力接触的概率

足部外力接触的概率

实际上，我们不能简单地相信控制器将通过拉脚离开地面或及时放下。同样地，不能假定地面是完全平坦的可能包含由于粗糙而导致的意外地面高度地形或看不见的物体。没有外力传感器，【防盗标记–盒子君hzj】我们必须通过编码器估计腿部的力数据、本体感受力和腿部的动力学模型。编码器将有离散化误差和模型不准确将导致外力显示没有。这些单一的测量都不能告诉我们腿的状态，但是我们可以从然后用它们来寻找一个更好的估计或者某只脚不可能接触到地面

我们可以自然地使用概率接触模型作为先验以融合可用的数据。
当前接触状态和子相百分比被选择为

基于相位和预定状态的接触概率对于不同的差异。接近开始和结束接触阶段腿部接触的可能性较小，而腿部更可能接触到挥杆的开始和结束。因此，该模型可以用作系统的瞬时输入.

使用机器人的各个预定状态和阶段为每个腿堆叠。大致如下协方差矩阵编码我们对我们基于相位的开关模型的精度为

由于我们只是通过融合当前可用的测量值来查看瞬时接触检测，因此状态和输入矩阵定义为【防盗标记–盒子君hzj】

.
.

五、进行卡尔曼融合

A、预测：预测模型

卡尔曼滤波的标准预测方程在这里呈现为

.

B、校正：测量模型

知道这一预测会包含不可抗拒的内容在快捷的计划中，我们可以使用可用的措施正确的预测，获得更多的信息接触概率标准卡尔曼滤波器修正方程呈现为

1）地面高度测量模型
无环境感知系统，地面高度，zg，可以概率地建模。随机变量地面的高度是从一个正态高斯分布中提取出来的分布为Zg∼N（μzg；σz2g）。定义参数对于平均地面高度，μzg设置为零，【防盗标记–盒子君hzj】无外部信息，因为我们无法做出假设关于地形。类似地，方差松散地对应到“粗糙”的地形。因此，我们可以使用地面高度模型，以建立一个高度的信念，在这个高度上，我们期望在给定的位置与地面接触在垂直方向的脚，pz。可能性接触给定的脚高定义为:

.
2）接触力测量模型
唯一正确的接触指示器是脚部感受到的外力。但是，机器人目前没有直接的力量传感器。根据第三节中给出的力估算

六、基于周期相位的状态调度器

我们认为稳定状态的腿运动步态是每条腿接触和摆动状态的集合它们之间的离散、瞬时转换。腿部状态切换背后的基本机制是周期性的基于阶段的状态调度器。总相位变量φ（0，1），【防盗标记–盒子君hzj】根据当前所需步态在定义的周期内循环。在此期间，相位变量遇到相位值，中高级控制器被通知在站姿状态之间计划的更改，站立状态φ=1，摆动状态φ=0。当前，调度程序使用的阶段变量是时间的线性函数:

其中t是当前时间，t0是当前的开始时间周期，T是确定的周期周期。阶段变量φi=φ+φi。对每个脚i，阶段变量被用来确定每一个步态

.
.

七、基于事件的步态切换FSM

随着接触检测的实现，我们可以移动从定时步态调度控制转向基于事件的控制。

1、FSM状态转移图

基于相位周期计算的相位sφ仍然是步态切换的基本机制，在理想状态下，我们会希望s和sφ是相同而且同时切换的。但是，机器人的控制系统可能会出现时间延迟，这可能因为未预见的地面高度而导致其腿摆动较晚，或者由于摆动腿轨迹跟踪不准确提前或延迟接触，所以稳健控制接触切换的能力将成为一项关键技术，由于能够可靠地检测到与地面的接触状态，因此设计采用了基于事件的切换策略来处理提早和延迟的接触，而不是不顾实际的接触状态，严格遵守时间上的计划进行状态估计，步态切换器加入来自触地检测器的估计接触状态s，我们可以从定时步态时间表切换控制转向基于事件的切换控制，步态切换仅仅用到了触地检测的的布尔值结果S和基于定时器相位周期估计的的布尔值结果S，以基于相位周期估计的触地结果S为主，以触地检测的的布尔值结果S^为辅，给定步态接触状态s和当前阶段φ，步态切换器就可以提供一个估计的每条腿接触状态sφ。

阶段计划状态sφ仍然是选择步态模式的机制。但是，腿控件使用来自算法

在新的框架下，一旦机器人的腿被安排好它将继续按照所需的摆动来摆动脚的轨迹直到它检测到接触，不管它是否计划在这段时间内保持联系。然而，一旦检测到接触，【防盗标记–盒子君hzj】它将尝试保持当前脚的位置而不是跟随轨迹。如果算法在一段延迟时间后继续检测接触，t>（tdelay+t0），其中t0是检测到的初始时间联系，平衡控制器将被通知可用于平衡。故意拖延是为了防止短暂接触对机器人的步态如果简单地有轻微的擦伤或滑倒。

尽管从僵硬的基于时间的步态调度到基于事件的FSM，机器人的健壮性高度差过大的意外接触由于精确的接触检测而改进

根据机器人正常的预定切换时间、惯性延时时间和触地检测控制器估计的接触状态s^，判断足端是否遇到意外的提前或延迟接触。从而切换至三个个不同的状态，运行对应的三个状态控制器。这种故意的惯性延迟时间可以认为是一种延时消抖，是为了防止短暂的接触对机器人的步态造成的影响

.

2、状态切换条件

假设地面是刚性的不移动的，就没有提前或者延后腾空的情况

（1）切换至支撑相的条件

步态切换算法在当前时钟时间大于延迟时间后继续检测触点，其中t0为被检测触点的初始时间，平衡控制器将被通知该脚可以进行平衡
【相位周期等于定时周期t0+人为设定的延时周期t delay，此时触地检测器同时估计出接触状态，直接进入支撑状态】（从微分的角度看不可能那么理想落在这个时间戳上的，所以每次切换至少经过三个状态）

（2）切换至摆动相的条件

【相位周期大于定时周期t0+人为设定的延时周期t delay，直接切换到腾空状态】

（3）腿部提前接触状态条件

【先触地检测器提前估计到足端为接触状态（s^=1），切换至提前接触状态】+
【后相位周期大于定时周期t0+人为设定的延时周期t delay，再切换到支撑状态】

（4）腿部延迟接触状态条件

【先相位周期小于于定时周期t0+人为设定的延时周期t delay，切换至延时接触状态】+
【后触地检测器估计到足端为接触状态（s^=1），再切换到支撑状态】

八、实验效果

代码没有给实现方式

着陆高速比较

一个触地事件，没有基于事件的FSM，早期接触导致触地时的弹跳（红色圆圈突出显示）

---

总结

1）论文方向：
（1）触地检测模型：介绍了一种基于GM的扰动观测器的概率融合触地检测模型.提高触地时接触启动的鲁棒性。
（2）校正触地检测模型：在卡尔曼滤波的框架下，将概率融合触地信息与（运动学和步态相位的）接触先验信息融合矫正，增强了方法的鲁棒性。
（3）步态调度器：实现一个基于事件的有限状态机，处理意外的早期和晚期接触。

（2）难点：
（1）本体触地触觉感知不是通过一些传感器来实现的，而是通过高度透明的驱动关节角度读数和来自惯性测量单元(IMU)的反馈校正来实现的。
（2）接触力传感及其相关反馈机制将在这一过程中发挥关键作用。【防盗标记–盒子君hzj】然而，传统的力估计是有噪声的，力敏感技术是出了名的脆弱。

（3）本文贡献：
简单来说就是提供了一种方法来检测足部在地面上的接触，并在足部没有直接力传感器的情况下修改足部的控制动作。

.
.

.
.

（1）参考资料

参考论文
《Contact model fusion for event-based locomotion in unstructured terrains》

.
.

（2）其他可行的方案（自己的想法）

1、基于足端压力传感器检测的步态切换器

根据周期或触地力传感器等估计腿部的接触状态
（1）优点
容易实现

（2）缺点
成本高，检测准确不极其依赖机械安装位置

2、基于足端光电/超声波距离传感器检测的步态切换器

.
.

（3）自己的一些思考

1、基于动量观测器估计力方法与MPC估计腿部反作用力方法对比

（1）MPC估计支撑腿的反作用力用于控制，MPC估计的反作用力前提是知道腿部是在接触状态下的；

（2）基于关节动量（GM）观测器测量的腿部外力用于判断触地状态，基于关节动量（GM）观测器测量的腿部外是在未知状态下的，这里又不能用MPC估计反作用力的方法，因为在未知机器人状态是无法对机器人的动力学模型进行匹配，机器人支撑时的动力学模型和腾空使得动力学模型不一样，而MIT仅仅对支撑状态时的躯干建立了动力学模型，没有对摆动状态建立动力学模型

.
.

2、足式运动的步态本质与基于事件的状态切换的思考

（1）例子

例子一“预判平地却踩坑”
从人类的行走方式来进行理解。当一个人在正常行走的时候，如果前脚没有按预想的时间踩到地面上(例如遇到坑洼)，后脚是不会抬起的，不管此时后腿的运动时序是否已经到了特定的相位

例子二“预判下坡却踩平地”
很多人都经历过下楼梯时的“误判”，就是在还剩下一级阶梯的时候误以为已经下到了平地上，于是提前“切换”到了平地步态模式，将前腿摆动的期望落脚点高度估计为后支撑足的高度。结果是前脚运动到期望点却没有得到预期的地面支撑，身体由于重心提前前移而向前倾，前腿本能反射快速向下触地以平衡身体。在整个过程中，后支撑腿一直处于支撑状态，并不会因为到了预定抬腿时间而抬起

例子三“山羊运动”
在不规则地形上，例如观察山羊在山地地形上的行为表现，它的步幅、抬腿高度、摆动相时间、速度及停顿时间等等都是无规律的，是根据地形的实际情况进行判断和选择的最终表现结果

（2）结论

（1）足式运动的步态更符合一种直觉的逻辑判断规则，而不是严格遵循腿和腿之间运动的时间差(相位差)，这效果可以通过相位周期结合触地信号实现

（2）足式运动不仅仅只有支撑相和腾空相两个状态，在特殊地形还有一系列中间过渡状态，实现如后仰踉跄和预稳的效果，这效果通过一系列过渡状态规划实现

.

（4）基于事件的步态调度优点

（1）当环境不同时，一个接触会提前发生或者延迟发生，基于事件触发的步态切换相比于基于相位周期的机器人更加具有自适应的能力，能动态的调整站立周期和腾空周期.当某一条腿在摆动过程中提前(没走完规划的摆动轨迹)与地面发生接触(地面凸起)，则该腿自动切换为支撑相控制，下一条摆动腿将紧接着执行摆动运动。反过来，如果某一条腿走完摆动轨迹仍未触地(遇到地面凹陷)，则该腿将继续向下运动去主动寻找地面接触

（2）这样规划得到的步态与全局时间没有直接关系，腿的运动之间也不存在必然的时间差。使得每一步的抬腿运动相对独立，并且都是在全触地时刻进行换腿，不会因为上一步的不同时着地而将时序误差带入下一步的运动中
.

（5）基于事件的步态调度缺点

（1）在机器人运动过程中，有时候会出现触地不可靠的情形。比如先着地的一条腿落地以后发生打滑又离开了地面，这也会对应引入融入东的误差。在进行逻辑判断时需要对这种不合理的触地信号变化进行识别和处理，例如强制打滑后离地的腿立刻向下伸展再次进入支撑相。步态时序依赖于可靠的触地信号，在实际应用中可以通过在机器人的足底安装碰撞开关或者压电传感器等，同时要确保这些传感器对触地状态有敏感的响应并输出可靠的开关信号

（2）实现相对难一些，需要加入触地检测判断的结果，触地检测在状态估计那一部分，其实都是逻辑判断的东西