基于高斯过程回归的足式机器人触地检测研究*

刘恒力，刘清宇，郭永兴

(武汉科技大学 a.机器人与智能系统研究院；b.冶金装备及其控制教育部重点实验室；c.机械传动与制造工程湖北省重点实验室，武汉 430081)

0 引言

足式机器人由于具有良好的地形适应性和高度的灵活性，可以在轮式或履带式车辆无法到达的困难地形环境上行驶。随着足式机器人在这样的地形上运动，其与外部环境的接触检测显得尤为重要。目前，大多数足式机器人采用外部传感器检测接触碰撞和无外部传感器的接触碰撞检测算法。采用外部传感器检测接触碰撞[1-2]，文献[3]在机器人足端安装力传感器，根据反馈的信息对机器人运动节律和关节信息进行实时调节。然而采用外部传感器检测接触碰撞会增加腿部的转动惯量，额外的传感器也会增加成本和系统的复杂性，且安装在足端的传感器在受到冲击力时容易损坏，导致机器人的可靠性降低。另一种是不采用外部传感器而是通过本体传感器检测接触碰撞[4-6]。该方法通常利用足端的力的估计值来实现对触地的检测。文献[7]提出了一种本体感知驱动模式，通过低传动比的驱动、关节角度和来自惯性测量单元(IMU)的反馈来寻求接触力，利用关节处的力控制，有效地控制足部的接触作用力。文献[8]基于浮动基动力学模型，提出了一种新的残差公式估计外部接触力。文献[9]开发了一种坚固的腿测距模块，它使用内部力感测来检测脚的撞击，但是忽略了加速度测量值的误差影响。文献[10]提出了一种在融合多个先验的概率接触检测的统一框架内追求基于广义动量的扰动观测器,用来检测和处理意外的过早和过晚的接触碰撞。

随着机器学习理论和技术的成熟，从数据中学习模型已日益成为机器人建模、规划及控制的重要方法。机器人逆动力学满足关节空间到力矩空间的唯一映射关系，逆动力学的学习属于标准的回归问题。从观测数据中对隐含的函数关系进行拟合，如高斯过程回归[11-13]、支持向量机回归[14]等方法。高斯过程(Gaussian processes，GP)作为贝叶斯机器学习方法之一，以概率分布来表示先验知识，在此基础上构建预测模型。文献[15]采用高斯过程回归方法，通过 SCARA 机器人的输入与输出量对其逆向动力学模型进行估计，并用于计算力矩控制中，得到了不错的仿真结果。文献[16]基于稳定模型的高斯过程回归机器人控制，提出了一种新的计算转矩控制律，其跟踪精度方面优于传统的计算转矩方法。

本文提出采用一种免外部传感器的基于高斯过程回归的机器人接触碰撞力检测方法，首先，给出机器人的足端力模型，确立关节驱动转矩与足端力之间的关系。然后，在Simmechanics里建立双关节单腿机器人的物理模型，给定关节空间运动的角度和角速度为输入量，以关节所需的实际驱动力矩为输出量，利用训练数据通过高斯过程回归进行系统的逆动力学模型的拟合，用预测数据验证得到的逆动力学模型的准确性。最后，将得到的逆动力学模型用于足端力的检测。

1 基于高斯过程回归的碰撞检测建模

1.1 足端力建模

为研究单腿机器人的受力情况，本文将单腿机器人简化为二连杆结构。采用拉格朗日方程建立单腿机器人的动力学模型如下：

(1)

基于动力学模型的方法检测接触碰撞是通过计算估计转矩与实际转矩之间的差值：

τk=τ′-τ

(2)

式中，τk为足端力引起的各关节转矩，τ′为有足端力时各关节的实际输入转矩，τ为无足端力时各关节的计算输入转矩。关节转矩和足端力之间的关系满足：

τk=JT×Fk

(3)

式中,Fk为机器人足端力；J为机器人的速度雅克比矩阵；τk为足端力引起的关节转矩。

由式(1)～式(3)可推导出足端力：

(4)

从上式可知，求解足端力需要精确的动力学模型参数，然而动力学模型参数辨识推导复杂,受摩擦力和各关节的加速度误差的影响，辨识精度不高，导致力矩残差中含有误差：

τk+δτ=τ′-τ

(5)

δτ包括模型误差和测量误差。减小此误差将提高足端力检测的精度，本文采用基于高斯回归过程建模来学习动力学参数。

1.2 高斯过程回归建模原理

高斯过程回归方法是贝叶斯优化方法的一种，在处理小样本、维数高和非线性等回归问题上具有很好的效果。给定输入X和输出Y， 回归的任务就是学习输入和输出之间的映射关系，利用这个映射关系，预测新输入x*对应的输出量y*。可以定义出一个高斯过程来描述函数的分布，高斯过程的特征由其均值函数m(x)和协方差函数k(x,x′)来确定：

m(x)=E[f(x)]

(6)

k(x,x′)=E[(f(x)-m(x))(f(x′)-m(x′))]

(7)

GP可以定义为：

f(x)～GP[m(x),k(x,x′)]

(8)

(9)

假设训练集为(x,y)，测试集为(x*,y*)，可以得到观测值y和预测值f*的联合先验分布为：

(10)

其中，K(X*,X*)为测试点x*自身协方差矩阵，K(X,X)为训练点的协方差矩阵，K(X,X*)=K(X*,X)是测试点x*和训练集点x之间的协方差矩阵，由此可以计算出预测值f*后验分布为：

f*|X,y,x*～N(μ,Σ)

(11)

其中，

μ=K(x*,X)[K(X,X)+σ2I]-1y

(12)

Σ=K(X,x*)-K(x*,X)[K(X,X)+σ2I]-1K(X,x*)

(13)

其中，μ和Σ为测试点x*对应的预测值f*的均值和协方差。

由上述公式可以看出，高斯过程回归算法,对于预测输出值,并非单纯估算一个预估值,还会估算出预估值方差。这种方式将预测的不确定性考虑进去,更贴近实际。

选取协方差函数来对高斯过程进行训练，如平方指数协方差函数：

(14)

(15)

获得超参数后，利用式(12)、式(13)就可以得到测试点x*对应的预测值f*的均值和方差。

1.3 利用高斯过程进行足端碰撞力检测

(1)首先选择预测模型的输入变量和输出变量，输出变量为各关节的驱动转矩，影响各关节的驱动转矩的因素有各关节的角度、角速度、角加速度以及摩擦转矩，而摩擦转矩又受角速度影响，因此它对驱动转矩的影响可以归结到角速度上，由于高斯回归过程只是输入量与输出量之间的函数映射关系，而与具体的物理系统无关，这里采用前一时刻，后一时刻的角速度代替角加速度对驱动转矩的影响以减少角加速度的误差对系统模型造成的影响。因此，选择各关节角度、角速度、前一时刻角速度，后一时刻角速度作为输入变量，选择各关节驱动转矩为输出变量。

(2)利用Simulink进行建模仿真，得到无足端力和有足端力的两份数据集，再将无足端力数据集分为两部分，一部分被选为训练集作为建模数据集，另一部分被选为测试集，用来检测模型的准确性。

(3)选择先验分布，定义似然函数。

(4)选择协方差函数，根据训练样本集通过共轭梯度法迭代求解得出最优超参数。

图2 Simulink单腿机器人控制系统

(5)输入测试集的输入，根据所得的动力学模型预测输出，与预测集的真实输出比较，检查精度是否符合要求，若符合要求，则结束系统的逆动力学模型的建立，否则，重复第(3)、(4)步。

(6)将得到的系统概率动力学模型用于预测有足端力时的驱动转矩，依据前面的足端力模型，检测足端力的大小。

2 单腿机器人足端碰撞仿真实验

2.1 Simulink模型建立和配置

为了验证模型的有效性，本节搭建了单腿机器人的物理模型，设计了仿真实验。首先，基于Simulink的Simmechanics库建立了单腿机器人的物理模型，如图1所示。

图1 单腿机器人模型

该单腿机器人有两个转动关节，分别为髋关节和膝关节。机器人身体长0.6 m，宽0.3 m，高0.15 m，质量为12 kg，大腿长0.6 m，腿截面宽度为0.06 m，质量为1 kg，小腿长0.6 m，腿截面宽为0.06 m，质量为1 kg。

2.2 学习过程

首先固定身体，以身体质心为坐标原点，建立参考坐标系，对机器人足端进行轨迹规划，对水平方向的起点、终点的位置，速度和加速度进行约束，进行五次多项式拟合得到足端的运动轨迹：

x=230.4×t5-288×t4+96×t3-0.6

(16)

选择足端运动轨迹为抛物线，竖直方向轨迹规划为：

(17)

通过逆运动学得到其关节空间的规划角度，采用逆动力学模型前馈加PD反馈控制得到关节的驱动力矩(此处的逆动力学模型的参数仅为一个大致的参数)。摩擦力矩采用库伦-粘滞模型定义，将关节驱动转矩输入给单腿机器人的物理模型，使单腿机器人按照规划的轨迹运动。

Simulink系统框图如图2所示，设置仿真时间为0.5 s，步长为0.000 5 s，仿真得到膝关节和髋关节的关节角度、关节角速度，前一时刻的关节角速度，后一时刻的关节角速度，关节驱动力矩的数据，由于实际数据可能还有噪声，因此给关节驱动力矩加入了高斯噪声。将数据分为两组，由于涉及到前一时刻的关节角速度和后一时刻的关节角速度，取训练数据167组，预测数据166组，利用训练数据进行系统逆动力学模型的学习，得到腿部的系统逆动力学模型，用预测数据检测学习到的模型。检查精度是否符合要求，若不符合，则重新学习。

得到系统的逆动力学模型后，给定一个足端力，按照规划的轨迹再一次进行仿真。根据得到的模型预测所需的髋关节和膝关节的驱动力矩，即为无足端力时所需的驱动力矩，与有足端力时实际所需的驱动力矩之差，即为足端力碰撞力矩。根据足端力模型可以求解足端力大小。通过改变规划的足端轨迹的步长和步高，可以再一次进行仿真实验以进行系统动力学模型的学习。

不固定身体，让其从空中掉落，最后触地竖直跳跃。以地面坐标系作为参考坐标系，空中相时保持机器人的髋关节和膝关节的关节角度固定不变，髋关节角度为0.698 rad(相对于负z轴逆时针的旋转角度)，膝关节角度为-1.396 rad(相对于大腿顺时针旋转的角度)，当进入触地相时，假设触地瞬间足端速度迅速变为0，足端与地面保持固定不动，利用基于SLIP模型的触地相模型对机器人髋关节质心作轨迹规划，通过逆运动学得到其髋关节和膝关节空间规划角度，再对其进行四次多项式拟合得到:

qh=108.715*t4-66.66*t3-2.695*t2+3.959*t+0.698

(18)

qk=-217.429*t4+133.32*t3+5.390*t2-7.918*t-1.396

(19)

空中相时采用PID反馈控制，触地相时采用PD反馈控制关节的驱动力矩，机器人从足端距地面0.48 m处落下，0.313 s触地，触地相时间为0.306 6 s，设置仿真时间为0.933 s，步长为0.000 2 s，重复身体固定时的过程对其进行学习，取训练数据200组，预测数据120组，得到整个机器人系统的逆动力学模型。

2.3 仿真实验结果

身体固定时，关节驱动转矩预测值与真实值及其残差如图3、图4所示。

图3 身体固定的髋关节驱动转矩预测值与真实值及其残差

图4 身体固定的膝关节驱动转矩预测值与真实值及其残差

可以看出，髋关节转矩残差最大为0.985 N·m，发生在0.306 s，此时由于髋关节角速度方向改变而导致髋关节摩擦转矩突变，而导致此处附近转矩残差较大，其他位置的残差最大为0.279 N·m。膝关节转矩残差最大为0.979 N·m，发生在0.252 s，此时由于膝关节角速度方向改变而导致膝关节摩擦转矩突变，而导致此处附近残差较大，其他位置的残差最大为0.213 N·m。残差为模型误差和测量误差，此处受摩擦转矩影响较大。

当在0.22 s～0.25 s内加入一个fx=9 N，fz=9 N的足端力时，用得到的系统逆动力学模型进行预测，关节驱动转矩预测值与真实值如图5所示。

图5 有足端力时关节驱动转矩预测值与真实值

图中曲线表明，当机器人发生碰撞时，利用学习得到的系统逆动力学模型获取的驱动转矩预测值与驱动转矩的真实值之间存在一个较为明显的差值，这个差值就是由于足端发生碰撞而引起，其大小就是驱动关节碰撞力矩τk，如图6所示。

图6 碰撞力矩

从图6可以明显看出，在0.22 s～0.25 s之间，髋关节碰撞碰撞力矩迅速增加至3.6 N·m左右，膝关节碰撞力矩迅速增加至4.9 Nm左右，在之后还有一段波动值。根据得到的关节碰撞力矩，利用足端力模型可以得到检测的足端碰撞力大小如图7所示。

图7 碰撞力

当身体不固定时，让其从空中掉落，最后触地竖直跳跃。其动力学模型与身体固定时有所差异，因为相比于机器人身体固定，其动能和势能有所改变，拉格朗日动力学方程的系数有所改变。因此需要对机器人整体的动力学模型参数进行预测，机器人在0.313 s时触地，0.620 s时起跳。

由于机器人触地瞬间会有一个较大的冲击力，因此角速度会在一瞬间发生较大的变化，而高斯回归训练时对输入变量相差较大时预测结果精度较低，因此训练集的角速度参数也要设计成迅速变化，以此来提高高斯回归过程预测的精度。关节驱动转矩预测值与真实值及其残差如图8、图9所示。

图8 身体运动的髋关节驱动转矩预测值与真实值及其残差

图9 身体运动的膝关节驱动转矩预测值与真实值及其残差

经过训练得到系统的逆动力学模型后，用其来检测足端碰撞力的大小，其关节驱动转矩预测值与真实值如图10所示，足端碰撞引起的关节碰撞力矩如图11所示。

图10 触地碰撞时关节驱动转矩预测值与真实值

图11 触地碰撞时的碰撞力矩

从图中可以看出，当机器人在0.313 s触地时，由于受到地面的冲击力作用，其关节角加速度，关节角速度发生突变，导致所需的驱动转矩迅速增大，随后，冲击力迅速变小，地面对机器人的支撑力逐渐增加，真实的驱动转矩随着触地力的变化而发生相应的变化。碰撞力矩是真实的驱动转矩与预测的驱动转矩的差值，其变化趋势反映的是碰撞力的变化趋势。

实际足端碰撞力与通过系统的逆动力学模型计算出的检测力如图12、图13所示。

图12 水平方向检测碰撞力与实际碰撞力 图13 竖直方向检测碰撞力与实际碰撞力

3 分析与讨论

当身体固定时，从图5中可以看出足端碰撞引起的关节力矩在0.25 s～0.35 s之间波动较大，与实际出入较大，分析可知这是由于关节角速度方向改变导致摩擦转矩突变引起，由于训练数据中角速度方向改变的数据仅有两个，所以模型预测不够精确。增加训练数据中关于关节转速方向转变的个数，再次训练得到模型进行预测。利用修改后的逆动力学模型得到足端碰撞引起的关节力矩的检测值如图14所示。足端碰撞力检测值如图15所示。

图14 模型修改后的碰撞力矩

图15 模型修改后的碰撞力

修改后的系统逆动力学模型对足端力的检测消除了摩擦转矩突变带来的误差，更加精确。

当机器人身体不固定时，着地瞬间由于存在惯性及动能，关节速度瞬时出现尖峰值，关节角度瞬时出现较大的变化值。触地瞬间足端受到一个较大的冲击力，又迅速减小，在机器人腿部收缩过程中足端受力又逐渐增加，直到机器人腿部收缩到最底部时又达到一个极值，然后随着机器人腿部的伸展又逐渐减小，直至机器人足端离地，受力减小为0。从图11和图12可以看出，足端实际水平方向最大受力为732 N，在最底部实际水平方向最大受力为271 N，检测到的水平方向最大受力为683 N，误差为6.69%，在最底部检测到的水平方向最大受力为240 N，误差为11.4%。足端实际竖直方向最大受力为1046 N，在最底部实际竖直方向最大受力为683 N，检测到的竖直方向最大受力为952 N，误差为8.99%，在最底部检测到的竖直方向最大受力为611 N，误差为10.5%。由于触地瞬间角速度突变，摩擦转矩突变，转矩输出存在滞后等因素的影响，使检测到的足端碰撞力比实际碰撞力小，且存在一定的滞后现象。

4 结论

针对单腿机器人足端触地检测问题，提出了一种基于高斯过程回归建模获得系统的逆动力学模型的方法。通过仿真得到输入量和输出量，对模型加以训练，建立关节驱动力矩与关节角度和关节角速度之间的映射函数。根据建立的映射关系，可以得到机器人各关节的驱动力矩，当机器人足端发生碰撞时，通过驱动关节的实际力矩和预测力矩，可以检测足端碰撞力的大小。

仿真结果表明，本文提出的方法能够实现机器人足端触地的检测，避免了测量误差以及对摩擦力、驱动器进行建模以及复杂繁琐的动力学参数辨识的问题，大大减小了动力学模型与实际模型之间的误差。但是本文考虑的噪声为高斯噪声，没有考虑其他的噪声干扰，对其他噪声干扰的数据，预测的模型可能不够准确。同时，由于在计算过程中涉及到矩阵的求逆运算，计算量相对较大。

后期的工作中，将采用组合协方差函数探索研究各种噪声下的建模预测以提高预测精度，并对减少计算量，实现更快速的检测进行更进一步的研究。