接触检测

步态规划器给出的接触序列是严格按照时间进行周期性计算的。而在实际运行当中,由于地形的不平整,又或者存在坡度等情况,腿部会发生提前或者延迟接触等情况,因此只靠步态规划器给出的接触序列来控制机器人往往是不可靠的。因此这里提出一个基于卡尔曼滤波的概率接触检测。其综合考虑了步态规划其给出的恒定接触序列,足端高度,地形的不平整性,以及通过关节编码器数据所计算出来的关节力矩,来提高接触检测的精度,同时减少了腿部由于电机力矩控制所引起的关节回弹现象。

一、预测模型

卡尔曼滤波器的标准预测方程如下:

给定一条腿的步态时间表和当前阶段
,步态规划器提供了每条腿的预期接触状态
,设计概率模型如下:

其中erf为误差函数,定义为:

该模型计算了,在给定当前相位,以及期望接触状态下,足端出于接触状态的概率。式中:

  • :接触状态改变时的期望子相位


  • :接触状态的方差

以横座标为当前腿的相位值,纵坐标表示处于接触状态(即)的概率,其中,摆动状态的概率为:

该模型有以下函数曲线:

当参数(

)取不同值时,图像如下所示,可以看出,
取不同值时,曲线的曲率会有所变化,当曲率比较大时,其预测结果更精确,但是许用误差范围较小,容易造成系统的不稳定;而当曲率变小时,预测模型的稳定性更强,其许用误差范围较大,但预测结果相对来说没那么精确,实际参数的选取可根据实机结果进行调整:

测试代码如下:

def prediction_model(phi, state, params):
    """
    Given the gait schedule and the current phase of a leg,
    the gait scheduler provides an expected contact state of
    each leg

    :param phi: phase
    :param state: contact state
    :param params: [mu, mu_bar, sigma, sigma_bar]
                    mu = [mu1, mu2] and so on
    :return: the probability of contact
    """
    mu0, mu1 = params[0]
    mu0_bar, mu1_bar = params[1]

    sigma0, sigma1 = params[2]
    sigma0_bar, sigma1_bar = params[3]

    a = math.erf((phi-mu0)/(sigma0*np.sqrt(2)))\
        + math.erf((mu1-phi)/(sigma1*np.sqrt(2)))

    b = 2+math.erf((mu0_bar-phi)/(sigma0_bar*np.sqrt(2)))\
        + math.erf((phi-mu1_bar)/(sigma1_bar*np.sqrt(2)))

    if state == 1:
        prob = 0.5 * (state * a)
    else:
        prob = 0.5 * (state * b)

    return prob
复制

因此,对于k个接触点,该预测模型可以作为系统的瞬时输入为:

协方差矩阵如下,该矩阵表示我们对预测精度的信赖程度

由于我们只关注瞬时接触检测(通过融合当前可用的测量),所以状态矩阵和输入矩阵被定义为如下:

二、测量模型(update模型)

预测模型中极有可能包含了规划器所产生的误差,因此,我们可以使用更多的测量模型来纠正预测值,以获得更精确的接触概率估计。 标准卡尔曼滤波修正方程如下:

1、地形高度模型

在没有环境感知系统的情况下,地面高度

可以进行概率建模。 我们可以将地面高度看作是一个遵从正态分布的随机值,概率模型为

不借用深度相机,我们无法获取地面信息,因此假设地面高度的均值,同理,方差

表示了当前所在地形的不平整度。

因此,对于给定当前足端垂直高度

,我们可以利用地面高度模型来创建一个当前接触高度的信赖模型,其接触概率模型如下:

当我们从其他传感器或历史足端轨迹中获取到相关地形信息时,可以对的值进行动态调整

同样,取不同参数观察其概率分布情况,横坐标为地形高度,纵坐标为发生接触的概率:

高度模型的代码:

def ground_height(pz, params):
    """
    The probability of contact given foot heigh

    :param pz: ground height
    :param params: [mu_z, sigma_z]
    :return: The probability of contact
    """
    mu_z, sigma_z = params
    prob_ground_height = 0.5 * (1 + math.erf((mu_z-pz) / (sigma_z*np.sqrt(2))))

    return prob_ground_height
复制

该模型提供的校正测量及其协方差矩阵如下:

2、反作用力模型

关于反作用力计算在另外一篇文章,这里先假设已经的到反作用

事实上,唯一真实可信的接触指标是足端感觉到的外力。 然而,由于机器人目前没有配备直接力传感器,我们只能用电机所反馈的信息来对反作用力进行估计。我们同样可以为其建立一个正态分布模型,

模型中是接触状态下力传感器所测到的平均值,
2
就是测量噪声。构建以下概率模型:

取不同参数观察其概率分布图像,横坐标为力的大小,纵坐标为发生接触的概率:

模型代码如下:

def contact_force(f, params):
    """
    the probability of contact given the estimated foot force

    :param f: contact force
    :param params: [mu_z, sigma_z]
    :return: The probability of contact
    """
    mu_f, sigma_f = params
    prob_force = 0.5 * (1 + math.erf((f-mu_f) / (sigma_f*np.sqrt(2))))

    return prob_force
复制

该模型提供第二项校正测量结果:

3、总体模型

将以上两组单独的测量结果叠加起来,形成卡尔曼滤波器中使用的观测向量。 同样,每个度量的协方差矩阵形成一个整体块 对角线协方差矩阵如下:

输出矩阵形成为两个按行叠加的N×N单位矩阵,其中N是脚的数量。

通过将步态规划器状态期望与测量模型概率相融合,可以更好地猜测每条腿的实际接触状态。 设定一个阈值P
c
(c)
,我们可以根据当前接触的概率得出接触状态:

目前,我们使用的卡尔曼滤波用Ak=0实现,这一融合过程其实可以通过贝叶斯定律的静态似然最大化得到。
然而,上述过程是一个嵌入在标准卡尔曼更新中的过程,大概是因为在机器人学中更容易实现。

4、完整代码

import math
import numpy as np
import matplotlib.pyplot as plt

# 设定周期为2
T = 2

# 根据相位计算当前接触状态
def get_contact_state(phi):
    if phi < 0.5*T:
        state = 1
    else:
        state = 0
    return state


# 预测模型
def prediction_model(phi, state, params):
    """
    Given the gait schedule and the current phase, φ, of a leg,
    the gait scheduler provides an expected contact state s φ of
    each leg

    :param phi: phase
    :param state: contact state
    :param params: [mu, mu_bar, sigma, sigma_bar]
                    mu = [mu1, mu2] and so on
    :return: the probability of contact
    """
    mu0, mu1 = params[0]
    mu0_bar, mu1_bar = params[1]

    sigma0, sigma1 = params[2]
    sigma0_bar, sigma1_bar = params[3]

    a = math.erf((phi-mu0)/(sigma0*np.sqrt(2)))\
        + math.erf((mu1-phi)/(sigma1*np.sqrt(2)))

    b = 2+math.erf((mu0_bar-phi)/(sigma0_bar*np.sqrt(2)))\
        + math.erf((phi-mu1_bar)/(sigma1_bar*np.sqrt(2)))

    if state == 1:
        prob = 0.5 * (state * a)
    else:
        prob = 0.5 * (state * b)

    return prob

# 测量模型-离地高度
def ground_height(pz, params):
    """
    The probability of contact given foot heigh

    :param pz: ground height
    :param params: [mu_z, sigma_z]
    :return: The probability of contact
    """
    mu_z, sigma_z = params
    prob_ground_height = 0.5 * (1 + math.erf((mu_z-pz) / (sigma_z*np.sqrt(2))))

    return prob_ground_height


# 测量模型-反作用力
def contact_force(f, params):
    """
    the probability of contact given the estimated foot force

    :param f: contact force
    :param params: [mu_z, sigma_z]
    :return: The probability of contact
    """
    mu_f, sigma_f = params
    prob_force = 0.5 * (1 + math.erf((f-mu_f) / (sigma_f*np.sqrt(2))))

    return prob_force

# 概率分布绘图
def test_predict():
    Mu = [0, 1]
    Mu_bar = [0, 1]
    Sigma = [0.025, 0.025]
    Sigma_bar = [0.025, 0.025]

    t = np.linspace(0, 0.999, 1000)
    prediction_prob = []
    prediction_prob2 = []
    prediction_prob3 = []

    for time in t:
        phi = time % T
        state = get_contact_state(phi)

        p = prediction_model(phi, state, [Mu, Mu_bar, Sigma, Sigma_bar])
        p2 = prediction_model(phi, state, [Mu, Mu_bar, [0.05, 0.05], [0.05, 0.05]])
        p3 = prediction_model(phi, state, [Mu, Mu_bar, [0.01, 0.01], [0.01, 0.01]])

        prediction_prob.append(p)
        prediction_prob2.append(p2)
        prediction_prob3.append(p3)

    fig = plt.figure()
    plt.subplot(211)
    plt.title('contact phase')
    plt.grid()
    plt.plot(t, prediction_prob, label='$\mu=[0, 1],\sigma=[0.025, 0.025]$')
    plt.plot(t, prediction_prob2, label='$\mu=[0, 1],\sigma=[0.05, 0.05]$')
    plt.plot(t, prediction_prob3, label='$\mu=[0, 1],\sigma=[0.01, 0.01]$')
    plt.legend()

    plt.subplot(212)
    plt.title('swing phase')
    plt.grid()
    plt.plot(t, 1-np.array(prediction_prob), label='$\mu=[0, 1],\sigma=[0.025, 0.025]$')
    plt.plot(t, 1-np.array(prediction_prob2), label='$\mu=[0, 1],\sigma=[0.05, 0.05]$')
    plt.plot(t, 1-np.array(prediction_prob3), label='$\mu=[0, 1],\sigma=[0.01, 0.01]$')
    plt.legend()
    fig.tight_layout()

    plt.show()


def test_ground_height():
    height = np.linspace(-0.3, 0.3, 1000)
    ground_height_prob = []
    ground_height_prob2 = []
    ground_height_prob3 = []

    params = [0, 0.025]
    params2 = [0, 0.05]
    params3 = [0, 0.1]

    for h in height:
        ground_height_prob.append(ground_height(h, params))
        ground_height_prob2.append(ground_height(h, params2))
        ground_height_prob3.append(ground_height(h, params3))

    fig2 = plt.figure()
    plt.plot(height, ground_height_prob, label='$\mu=0,\sigma=0.025$')
    plt.plot(height, ground_height_prob2, label='$\mu=0,\sigma=0.05$')
    plt.plot(height, ground_height_prob3, label='$\mu=0,\sigma=0.1$')
    fig2.tight_layout()
    plt.legend()
    plt.grid()
    plt.show()


def test_contact_force():
    force = np.linspace(-50, 200, 1000)
    contact_force_prob = []
    contact_force_prob2 = []
    contact_force_prob3 = []

    params = [35, 10]
    params2 = [35, 25]
    params3 = [35, 50]

    for f in force:
        contact_force_prob.append(contact_force(f, params))
        contact_force_prob2.append(contact_force(f, params2))
        contact_force_prob3.append(contact_force(f, params3))

    fig3 = plt.figure()

    plt.plot(force, contact_force_prob, label='$\mu=25,\sigma=10$')
    plt.plot(force, contact_force_prob2, label='$\mu=25,\sigma=25$')
    plt.plot(force, contact_force_prob3, label='$\mu=25,\sigma=50$')

    fig3.tight_layout()
    plt.grid()
    plt.legend()
    plt.show()


# test_predict()
# test_ground_height()
test_contact_force()
复制