使用卡尔曼滤波器估计pybullet中的机器人位置

news2024/12/25 8:53:28

⭐️ 卡尔曼滤波

卡尔曼滤波是一种递归算法，用于从具有噪声的观测中估计系统状态。它特别适合用于线性、高斯动态系统。

笔者之前写过一篇博文介绍卡尔曼滤波器《boss:整个卡尔曼滤波器的简单案例——估计机器人位置》，本文手动实现一个卡尔曼滤波器并结合pybullet实现一个机器人位置估计的案例。

卡尔曼滤波的工作流程

预测步骤（Predict）：
- 根据当前的状态估计值和状态转移模型，预测下一个时刻的状态。
- 预测状态的协方差矩阵。
更新步骤（Update）：
- 利用新观测数据更新预测值，得到更精确的状态估计。
- 更新状态的协方差矩阵。

公式如下：

状态预测：
$\hat{x}_k^- = F \hat{x}_{k-1} + B u_k$
$P_k^- = F P_{k-1} F^T + Q$
更新观测：
$K_k = P_k^- H^T (H P_k^- H^T + R)^{-1}$
$\hat{x}_k = \hat{x}_k^- + K_k (z_k - H \hat{x}_k^-)$
$P_k = (I - K_k H) P_k^-$

准备数据和参数

F: 状态转移矩阵
H: 观测矩阵
Q: 过程噪声协方差
R: 测量噪声协方差
x: 状态向量
P: 状态协方差矩阵
z: 观测值

⭐️ 案例

设置 PyBullet 模拟环境
在这个例子中，我们将模拟一个简单的机器人（如移动小车）在一个二维环境中的运动，机器人会随机移动，并且我们会使用卡尔曼滤波来修正机器人的位置估计。我们的目标是实现路径规划并利用卡尔曼滤波修正运动中的噪声。
路径规划
我们使用一个非常简单的路径规划方案，例如通过设置目标位置并计算机器人当前位置到目标位置的距离，然后让机器人朝目标移动。

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()