⭐️ 卡尔曼滤波
卡尔曼滤波是一种递归算法，用于从具有噪声的观测中估计系统状态。它特别适合用于线性、高斯动态系统。
笔者之前写过一篇博文介绍卡尔曼滤波器《boss:整个卡尔曼滤波器的简单案例------估计机器人位置》，本文手动实现一个卡尔曼滤波器并结合pybullet实现一个机器人位置估计的案例。
卡尔曼滤波的工作流程
-
预测步骤（Predict）：
- 根据当前的状态估计值和状态转移模型，预测下一个时刻的状态。
- 预测状态的协方差矩阵。
-
更新步骤（Update）：
- 利用新观测数据更新预测值，得到更精确的状态估计。
- 更新状态的协方差矩阵。
公式如下：
-
状态预测 ：
x ^ k − = F x ^ k − 1 + B u k \hat{x}k^- = F \hat{x}{k-1} + B u_k x^k−=Fx^k−1+Buk
P k − = F P k − 1 F T + Q P_k^- = F P_{k-1} F^T + Q Pk−=FPk−1FT+Q -
更新观测 ：
K k = P k − H T ( H P k − H T + R ) − 1 K_k = P_k^- H^T (H P_k^- H^T + R)^{-1} Kk=Pk−HT(HPk−HT+R)−1
x ^ k = x ^ k − + K k ( z k − H x ^ k − ) \hat{x}_k = \hat{x}_k^- + K_k (z_k - H \hat{x}_k^-) x^k=x^k−+Kk(zk−Hx^k−)
P k = ( I − K k H ) P k − P_k = (I - K_k H) P_k^- Pk=(I−KkH)Pk−
准备数据和参数
F: 状态转移矩阵H: 观测矩阵Q: 过程噪声协方差R: 测量噪声协方差x: 状态向量P: 状态协方差矩阵z: 观测值
⭐️ 案例
-
设置 PyBullet 模拟环境
在这个例子中，我们将模拟一个简单的机器人（如移动小车）在一个二维环境中的运动，机器人会随机移动，并且我们会使用卡尔曼滤波来修正机器人的位置估计。我们的目标是实现路径规划并利用卡尔曼滤波修正运动中的噪声。
-
路径规划
我们使用一个非常简单的路径规划方案，例如通过设置目标位置并计算机器人当前位置到目标位置的距离，然后让机器人朝目标移动。
python
import pybullet as p
import time
import numpy as np
import matplotlib.pyplot as plt
import pybullet_data
# --- 1. 设置环境 ---
# 启动 PyBullet 环境
# p.connect(p.DIRECT) # 使用非图形界面进行模拟
p.connect(p.GUI) # 使用非图形界面进行模拟
# 修改窗口位置和大小（Windows/Linux）
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1) # 确保 GUI 启用
p.resetDebugVisualizerCamera(
cameraDistance=7, # 调整距离
cameraYaw=50, # 调整视角
cameraPitch=-35,
cameraTargetPosition=[0, 0, 0]
)
# 加载 PyBullet 物理引擎
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # 设置路径
# 设置场景
p.setGravity(0, 0, -9.81)
p.loadURDF("plane.urdf") # 加载地面
# 加载机器人（这里假设一个简单的箱形机器人）
robot = p.loadURDF("r2d2.urdf", basePosition=[0, 0, 0.1]) # 可替换为任何简单的 URDF 文件
class KalmanFilter:
def __init__(self, F, H, Q, R, x0, P0):
"""
:param F: 状态转移矩阵
:param H: 观测矩阵
:param Q: 过程噪声协方差
:param R: 观测噪声协方差
:param x0: 初始状态
:param P0: 初始协方差矩阵
"""
self.F = F # 状态转移矩阵
self.H = H # 观测矩阵
self.Q = Q # 过程噪声协方差
self.R = R # 观测噪声协方差
self.x = x0 # 初始状态
self.P = P0 # 初始协方差矩阵
def predict(self):
"""预测步骤"""
# 预测状态
self.x = np.dot(self.F, self.x)
# 预测协方差
self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
def update(self, z):
"""更新步骤"""
# 计算卡尔曼增益
S = np.dot(np.dot(self.H, self.P), self.H.T) + self.R
K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
# 更新状态估计
y = z - np.dot(self.H, self.x)
self.x = self.x + np.dot(K, y)
# 更新协方差矩阵
self.P = self.P - np.dot(np.dot(K, self.H), self.P)
def get_estimate(self):
"""获取当前状态估计"""
return self.x
# 初始状态：机器人位置 (x, y, theta)
x0 = np.array([0, 0, 0]) # 初始位置和角度
P0 = np.eye(3) # 初始协方差矩阵
# 过程噪声协方差和观测噪声协方差
Q = np.array([[0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.1]])
R = np.array([[0.5, 0], [0, 0.5]])
# 状态转移矩阵和观测矩阵（简化模型）
F = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # 简单的线性状态转移
H = np.array([[1, 0, 0], [0, 1, 0]]) # 观察模型（x, y）
# 创建卡尔曼滤波器对象
ekf = KalmanFilter(F=F, H=H, Q=Q, R=R, x0=x0, P0=P0)
# --- 3. 路径规划算法 ---
# 设置目标位置
target_position = np.array([3, 2]) # 目标位置
# 机器人当前状态和路径记录
path = [x0[:2]]
# 设定移动的步长
step_size = 0.1
# 模拟机器人的运动
# for _ in range(100):
while True:
# 获取当前机器人状态（位置）
current_position = p.getBasePositionAndOrientation(robot)[0]
# 添加噪声
noisy_position = current_position[:2] + np.random.normal(0, 0.1, size=2)
# 执行卡尔曼滤波
ekf.predict() # 预测
ekf.update(noisy_position) # 更新
# 获取过滤后的估计位置
estimated_position = ekf.get_estimate()[:2]
# 根据当前估计位置计算控制输入（简单的前进和旋转到目标）
direction = target_position - estimated_position
distance = np.linalg.norm(direction)
if distance < step_size:
break # 到达目标
direction = direction / distance # 单位方向向量
# 更新机器人状态（简单地模拟机器人运动）
new_position = estimated_position + direction * step_size
p.resetBasePositionAndOrientation(robot, np.array(new_position.tolist() + [0]), [0, 0, 0, 1])
# 记录路径
path.append(estimated_position)
# 显示更新
time.sleep(0.1)
# --- 4. 绘制路径 ---
path = np.array(path)
plt.plot(path[:, 0], path[:, 1], label='Path')
plt.scatter(target_position[0], target_position[1], color='red', label='Target')
plt.title("Robot Path with Kalman Filter")
plt.xlabel("X Position")
plt.ylabel("Y Position")
plt.legend()
plt.show()
# 断开连接
p.disconnect()运行结果
卡尔曼位置估计
笔者水平有限，若有不对的地方欢迎评论指正！