接触检测
步态规划器给出的接触序列是严格按照时间进行周期性计算的。而在实际运行当中,由于地形的不平整,又或者存在坡度等情况,腿部会发生提前或者延迟接触等情况,因此只靠步态规划器给出的接触序列来控制机器人往往是不可靠的。因此这里提出一个基于卡尔曼滤波的概率接触检测。其综合考虑了步态规划其给出的恒定接触序列,足端高度,地形的不平整性,以及通过关节编码器数据所计算出来的关节力矩,来提高接触检测的精度,同时减少了腿部由于电机力矩控制所引起的关节回弹现象。
一、预测模型
卡尔曼滤波器的标准预测方程如下:
给定一条腿的步态时间表和当前阶段,步态规划器提供了每条腿的预期接触状态,设计概率模型如下:
其中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、反作用力模型
关于反作用力计算在另外一篇文章,这里先假设已经的到反作用
事实上,唯一真实可信的接触指标是足端感觉到的外力。 然而,由于机器人目前没有配备直接力传感器,我们只能用电机所反馈的信息来对反作用力进行估计。我们同样可以为其建立一个正态分布模型,
模型中是接触状态下力传感器所测到的平均值,就是测量噪声。构建以下概率模型:
取不同参数观察其概率分布图像,横坐标为力的大小,纵坐标为发生接触的概率:
模型代码如下:
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,我们可以根据当前接触的概率得出接触状态:
目前,我们使用的卡尔曼滤波用实现,这一融合过程其实可以通过贝叶斯定律的静态似然最大化得到。
然而,上述过程是一个嵌入在标准卡尔曼更新中的过程,大概是因为在机器人学中更容易实现。
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()
复制
评论(4)
您还未登录,请登录后发表或查看评论