MuJoCo +机械臂+DRL+路径规划+避障仿真学习

本文介绍了基于MuJoCo的机器人仿真学习项目，重点讲解了机器人运动学、动力学和控制的核心算法原理。项目支持Franka Emika Panda等机器人模型，包含逆运动学求解（KDL、Pinocchio、CasADi优化）、轨迹规划（TOPPRA、RRTConnect）、动力学控制（阻抗/导纳/PID控制）以及强化学习PPO算法实现。详细推导了旋转表示（矩阵/四元数/欧拉角）、变换矩阵和雅可比矩阵

无宜

766人浏览 · 2026-04-13 10:45:52

无宜 · 2026-04-13 10:45:52 发布

MuJoCo 机械臂仿真学习 - 算法原理详解

本文档详细介绍项目中涉及的机器人学算法原理、数学基础、代码实现，以及项目结构和使用说明。

项目概述
- 1.1 支持的机器人
- 1.2 项目结构
数学基础
逆运动学
轨迹规划
- 4.1 TOPPRA 最优时间规划
- 4.2 OMPL RRTConnect
动力学与控制
强化学习
- 6.1 PPO 算法原理
- 6.2 奖励函数设计
视觉伺服
- 7.1 PBVS 基于位置的视觉伺服
- 7.2 MPC 模型预测控制
安装与使用
- 8.1 环境配置
- 8.2 快速开始
核心功能索引
工具模块说明
主要依赖
参考文献

1. 项目概述

一个全面的 MuJoCo 仿真项目，用于学习机器人学核心概念，包括逆运动学、轨迹规划、动力学控制、强化学习和视觉伺服。

1.1 支持的机器人

Franka Emika Panda - 7自由度机械臂，带夹爪
SO-ARM100 - 6自由度机械臂
Hello Robot Stretch - 移动机器人带机械臂

1.2 项目结构

mujoco-learning/
├── model/                                    # 机械臂模型文件
│   ├── franka_emika_panda/                  # Panda 机械臂 MJCF 模型（核心）
│   │   ├── panda.xml                        # Panda 基础模型（关节空间控制）
│   │   ├── panda_pos.xml                    # Panda 位置控制模型
│   │   ├── panda_vel.xml                    # Panda 速度控制模型
│   │   ├── panda_tau.xml                    # Panda 力矩控制模型
│   │   ├── panda_nohand.xml                 # Panda 无夹爪模型
│   │   ├── panda_remove_finger.xml          # Panda 移除手指模型
│   │   ├── mjx_panda.xml                    # Panda MJX 模型（支持 GPU 仿真）
│   │   ├── mjx_panda_nohand.xml             # Panda MJX 无夹爪模型
│   │   ├── hand.xml                         # 夹爪模型
│   │   ├── scene*.xml                       # 各类场景配置文件（含障碍物、Apriltag、相机、标定板等）
│   │   ├── mjx_scene.xml                    # MJX 场景配置
│   │   ├── mjx_single_cube.xml              # MJX 单立方体场景
│   │   └── assets/                          # 模型网格文件（.stl/.obj）
│   ├── franka_panda_urdf/                   # Panda 机械臂 URDF 模型
│   │   ├── robots/panda_arm.urdf            # URDF 模型文件
│   │   └── meshes/                          # 可视化与碰撞网格
│   ├── hello_robot_chassis/                 # Hello Robot Stretch 底盘模型
│   │   ├── stretch.xml                      # Stretch 模型
│   │   ├── scene.xml                        # 场景配置
│   │   └── assets/                          # 模型网格与纹理
│   └── misc/                                # 其他资源
│       ├── ball.xml                         # 球体模型
│       └── textures/                        # Apriltag 纹理图片（tag36h11 系列）
├── src/                                     # 核心工具模块
│   ├── mujoco_viewer.py                     # 自定义 MuJoCo 可视化器（CustomViewer），封装相机控制与渲染
│   ├── kdl_kinematic.py                     # KDL 运动学封装（正/逆运动学）
│   ├── pinocchio_kinematic.py               # Pinocchio 运动学封装（支持 MJCF/URDF）
│   ├── pid_controller.py                    # PID 控制器
│   ├── lowpass_filter.py                    # 低通滤波器（一阶 RC 滤波）
│   ├── key_listener.py                      # 键盘事件监听（pynput）
│   ├── solvepnp.py                          # Apriltag 检测 + SolvePnP 位姿估计
│   ├── matplot.py                           # PyQtGraph 实时数据可视化
│   └── utils.py                             # 工具函数（四元数/欧拉角转旋转矩阵等）
├── scripts/                                 # 安装脚本
│   ├── install_pykdl.sh                     # PyKDL 编译安装脚本
│   └── install_pinocchio.sh                 # Pinocchio 编译安装脚本
├── tensorboard/                             # 强化学习训练日志
│   └── panda_obstacle_avoidance/            # 避障训练 TensorBoard 日志
│       ├── PPO_1/                           # 第1次训练记录
│       │   └── events.out.tfevents.*        # TensorBoard 事件文件
│       └── PPO_2/                           # 第2次训练记录
│           └── events.out.tfevents.*        # TensorBoard 事件文件
├── assets/                                  # 训练资源
│   └── model/
│       ├── rl_reach_target_checkpoint/      # 到达目标 RL 检查点
│       │   ├── panda_ppo_reach_target.zip   # PPO 模型权重
│       │   └── README.md                    # 模型说明
│       └── rl_obstacle_avoidance_checkpoint/ # 避障 RL 检查点
│           └── panda_obstacle_avoidance_v1.zip # 避障模型权重
├── orocos_kinematics_dynamics/              # OROCOS KDL 运动学库源码
│   ├── orocos_kdl/                          # KDL C++ 核心库
│   │   ├── src/                             # 运动学/动力学求解器源码
│   │   ├── build/                           # 编译产物（liborocos-kdl.so）
│   │   └── CMakeLists.txt
│   ├── python_orocos_kdl/                   # PyKDL Python 绑定
│   │   └── pybind11/                        # pybind11 绑定工具
│   └── CHANGELOG.md / README.md
├── requirements.txt                         # Python 依赖
├── .gitignore
└── README.md                                # 项目说明

2. 数学基础

2.1 旋转表示方法

机器人学中常用的旋转表示方法有四种：旋转矩阵、欧拉角、四元数和轴角。

2.1.1 旋转矩阵（Rotation Matrix）

旋转矩阵是一个 $\times 3$ 的正交矩阵，满足 $R^T R = I$ ，且 $\det(R) = 1$ 。

绕 X 轴旋转（Roll）:
$R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{bmatrix}$

绕 Y 轴旋转（Pitch）:
$R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}$

绕 Z 轴旋转（Yaw）:
$R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}$

项目中的实现（src/utils.py）:

def euler2rotmat(roll, pitch, yaw):
    cr, sr = np.cos(roll), np.sin(roll)
    cp, sp = np.cos(pitch), np.sin(pitch)
    cy, sy = np.cos(yaw), np.sin(yaw)
    # Z-Y-X 旋转顺序
    R = np.array([
        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
        [-sp,   cp*sr,            cp*cr],
    ])
    return R

2.1.2 四元数（Quaternion）

四元数 $q = [w, x, y, z]$ 可以避免欧拉角的万向锁问题，且插值更自然。

四元数转旋转矩阵:
$\begin{bmatrix} 1-2y^2-2z^2 & 2xy-2zw & 2xz+2yw \\ 2xy+2zw & 1-2x^2-2z^2 & 2yz-2xw \\ 2xz-2yw & 2yz+2xw & 1-2x^2-2y^2 \end{bmatrix}$

项目中的实现:

def quat2rotmat(quat):
    w, x, y, z = quat
    R = np.array([
        [1 - 2*y**2 - 2*z**2, 2*x*y - 2*z*w, 2*x*z + 2*y*w],
        [2*x*y + 2*z*w, 1 - 2*x**2 - 2*z**2, 2*y*z - 2*x*w],
        [2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x**2 - 2*y**2]
    ])
    return R

2.1.3 欧拉角（Euler Angles）

欧拉角使用三个角度 $(r o l l, p i t c h, y a w)$ 表示旋转。本项目使用 Z-Y-X 顺序。

四元数转欧拉角:

def quat2euler(quat):
    w, x, y, z = map(float, quat)
    sin_pitch = 2 * (w * y - x * z)
    sin_pitch = np.clip(sin_pitch, -1.0, 1.0)

    if np.abs(sin_pitch) < 0.9999999:
        roll = np.arctan2(2 * (w * x + y * z), 1 - 2 * (x**2 + y**2))
        pitch = np.arcsin(sin_pitch)
        yaw = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
    else:  # 万向锁
        pitch = np.pi / 2 if sin_pitch > 0 else -np.pi / 2
        yaw = np.arctan2(2 * (x * y + w * z), 1 - 2 * (x**2 + z**2))
        roll = 0.0
    return roll, pitch, yaw

2.2 变换矩阵

齐次变换矩阵 $\in \mathbb{R}^{4 \times 4}$ 用于同时表示旋转和平移：

$\begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & x \\ r_{21} & r_{22} & r_{23} & y \\ r_{31} & r_{32} & r_{33} & z \\ 0 & 0 & 0 & 1 \end{bmatrix}$

项目中从欧拉角构建变换矩阵:

def transform2mat(x, y, z, roll, pitch, yaw):
    cr, sr = np.cos(roll), np.sin(roll)
    cp, sp = np.cos(pitch), np.sin(pitch)
    cy, sy = np.cos(yaw), np.sin(yaw)
    return np.array([
        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr, x],
        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr, y],
        [-sp,   cp*sr,            cp*cr,            z],
        [0,     0,                0,                1]
    ])

2.3 雅可比矩阵

雅可比矩阵 $J (q)$ 建立了关节速度与末端执行器速度之间的映射关系：

$\dot{x} = J(q) \dot{q}$

其中 $\dot{x} = [\dot{p}, \dot{\omega}]^T$ 是末端执行器的线速度和角速度， $\dot{q}$ 是关节速度。

雅可比矩阵的物理意义:

前三行：将关节速度映射到末端线速度
后三行：将关节速度映射到末端角速度

阻尼伪逆矩阵（用于逆运动学求解）:
$J^\# = J^T (J J^T + \lambda^2 I)^{-1}$

其中 $\lambda$ 是阻尼因子，用于避免矩阵奇异问题。

def dampedPinv(J, lambda_d=0.1):
    J_T = J.T
    damping = lambda_d ** 2 * np.eye(J.shape[0])
    J_pinv_damped = np.dot(J_T, np.linalg.inv(np.dot(J, J_T) + damping))
    return J_pinv_damped

3. 逆运动学

逆运动学（Inverse Kinematics, IK）是指根据期望的末端执行器位姿，反求各个关节的角度。

3.1 KDL 逆运动学

KDL（Kinematics and Dynamics Library） 是 ROS 中常用的运动学和动力学库。

算法原理:
KDL 使用基于雅可比矩阵的迭代数值解法（Newton-Raphson 方法的变体）。

给定目标变换矩阵 $T_{des}$ ，当前关节角度 $q$ ，迭代求解：

计算当前末端位姿 $T_{current}$
计算误差变换 $\Delta T = T_{current}^{-1} \cdot T_{des}$
将误差转换为6维空间速度向量 $\xi = [\delta_p, \delta_\omega]$
更新关节角度： $\Delta q = J^{-1} \xi$
重复直到收敛

项目实现（src/kdl_kinematic.py）:

def ik(self, tf, current_arm_motor_q=None):
    # 从4x4变换矩阵提取位置和旋转
    trans = PyKDL.Vector(tf[0, 3], tf[1, 3], tf[2, 3])
    rot = PyKDL.Rotation(
        tf[0, 0], tf[0, 1], tf[0, 2],
        tf[1, 0], tf[1, 1], tf[1, 2],
        tf[2, 0], tf[2, 1], tf[2, 2]
    )
    frame = PyKDL.Frame(rot, trans)

    # 使用 KDL 的迭代 IK 求解器
    status = self.ik_pos_solver.CartToJnt(q_init, frame, q_out)
    if status >= 0:
        return [q_out[i] for i in range(num_joints)], {"success": True}
    else:
        return q_init, {"success": False}

运行示例（ik_kdl_panda.py）:

class RobotController(mujoco_viewer.CustomViewer):
    def runFunc(self):
        tf = utils.transform2mat(self.x, self.y, self.z, np.pi, 0, 0)
        self.dof, info = self.arm.ik(tf, current_arm_motor_q=self.last_dof)
        if info["success"]:
            self.last_dof = self.dof
            self.data.ctrl[:7] = self.dof[:7]

3.2 Pinocchio CLIK

Pinocchio 是一个高效的 C++ 机器人库，提供 Python 接口。

CLIK（Closed-Loop Inverse Kinematics） 是一种闭环逆运动学方法，利用雅可比矩阵的伪逆直接进行实时控制。

算法原理:

计算当前末端位姿误差 $e = [e_p, e_o]$
期望末端速度 $v_{des} = K_p \cdot e$ （比例控制）
关节速度 $\dot{q} = J^{-1} \cdot v_{des}$
更新关节角度 $q_{new} = q_{old} + \dot{q} \cdot dt$

项目实现（control_ee_with_pinocchio.py）:

def inverse_kinematics(current_q, target_dir, target_pos):
    model = pinocchio.buildModelFromUrdf(urdf_filename)
    data = model.createData()
    oMdes = pinocchio.SE3(target_dir, np.array(target_pos))

    q = current_q
    eps = 1e-4
    IT_MAX = 1000
    DT = 1e-2
    damp = 1e-12

    while True:
        pinocchio.forwardKinematics(model, data, q)
        iMd = data.oMi[JOINT_ID].actInv(oMdes)
        err = pinocchio.log(iMd).vector  # 李代数误差

        if norm(err) < eps:
            success = True
            break
        if i >= IT_MAX:
            success = False
            break

        J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID)
        J = -np.dot(pinocchio.Jlog6(iMd.inverse()), J)
        v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
        q = pinocchio.integrate(model, q, v * DT)

关键点说明:

pinocchio.log(): 将 SE(3) 变换矩阵映射到 6 维李代数向量
pinocchio.Jlog6(): 计算对数映射的雅可比矩阵
阻尼最小二乘法避免矩阵奇异

3.3 CasADi 优化逆运动学

CasADi 是一个符号优化框架，可以将优化问题符号化后使用 IPOPT 等求解器求解。

算法原理:
将逆运动学问题构造成优化问题：
$\min_q \quad \|p_{des} - p(q)\|^2 + \|R_{des} - R(q)\|^2$
$\text{s.t.} \quad q_{min} \leq q \leq q_{max}$

项目实现（src/pinocchio_kinematic.py）:

class Kinematics:
    def createSolver(self):
        self.opti = casadi.Opti()
        self.var_q = self.opti.variable(self.model.nq)
        self.param_tf = self.opti.parameter(4, 4)

        # 位置误差
        self.translational_error = casadi.Function(
            "translational_error",
            [self.cq, self.cTf],
            [self.cdata.oMf[self.ee_id].translation - self.cTf[:3,3]]
        )

        # 姿态误差（使用旋转矩阵对数）
        self.rotational_error = casadi.Function(
            "rotational_error",
            [self.cq, self.cTf],
            [cpin.log3(self.cdata.oMf[self.ee_id].rotation @ self.cTf[:3,:3].T)]
        )

        # 构造优化问题
        self.opti.subject_to(self.opti.bounded(
            self.model.lowerPositionLimit, self.var_q, self.model.upperPositionLimit))
        self.opti.minimize(20.0 * translational_cost + 0.01 * rotation_cost +
                           0.005 * smooth_cost)

        self.opti.solver("ipopt", {'ipopt': {'max_iter': 1000, 'tol': 1e-6}})

    def ik(self, T, current_arm_motor_q=None):
        self.opti.set_value(self.param_tf, T)
        self.opti.set_initial(self.var_q, self.init_data)
        sol = self.opti.solve()
        sol_q = self.opti.value(self.var_q)
        self.init_data = sol_q
        return sol_q, {"success": True}

代价函数说明:

translational_cost: 位置误差（权重20.0，优先保证位置精度）
rotation_cost: 姿态误差（权重0.01）
smooth_cost: 平滑代价（权重0.005），保证关节角度变化连续

4. 轨迹规划

4.1 TOPPRA 最优时间规划

TOPPRA（Time-Optimal Path Parameterization） 是一种在不超出机器人动力学约束的前提下，计算最快轨迹的算法。

问题定义:
给定一条路径（由一系列路径点参数化），找到最优的时间参数化 $\tau(s)$ ，使得轨迹执行时间最短，同时满足：

关节速度约束： $|\dot{q}_i| \leq \dot{q}_{i,max}$
关节加速度约束： $|\ddot{q}_i| \leq \ddot{q}_{i,max}$

算法原理:

路径离散化：将路径离散为 $s_0, s_1, ..., s_N$
计算路径导数：对每个离散点计算 $\frac{dq}{ds}$
构建可行速度区间：
$\dot{s}_{max,i} = \min_j \sqrt{\frac{a_{j,max}}{|d^2q_j/ds^2|_i}}$
动态规划求解最优参数化：从终点反向传播可达速度约束

项目实现（trajectory_plan_toppra.py）:

import toppra as ta
import toppra.constraint as constraint

# 构建路径
way_pts = np.array([[q1, q2, ...], [q1, q2, ...], ...])
path_scalars = np.linspace(0, 1, len(way_pts))
path = ta.SplineInterpolator(path_scalars, way_pts)

# 速度约束
vlim = np.vstack([-robot.velocityLimit, robot.velocityLimit]).T
pc_vel = constraint.JointVelocityConstraint(vlim)

# 加速度约束
al = np.array([2,] * robot.nv)
alim = np.vstack([-al, al]).T
pc_acc = constraint.JointAccelerationConstraint(alim)

# TOPPRA 求解
instance = ta.algorithm.TOPPRA([pc_vel, pc_acc], path, solver_wrapper="seidel")
jnt_traj = instance.compute_trajectory(0, 0)

# 采样轨迹点
ts_sample = np.linspace(0, jnt_traj.get_duration(), 1000)
qs_sample = jnt_traj.eval(ts_sample)

4.2 OMPL RRTConnect

RRT（Rapidly-exploring Random Tree） 是一种基于随机采样的运动规划算法。

算法原理:

初始化：从起点 $q_{start}$ 构建树 $T$
随机采样：在配置空间中随机采样一个点 $q_{rand}$
最近邻搜索：在树中找到距离 $q_{rand}$ 最近的节点 $q_{near}$
扩展：从 $q_{near}$ 向 $q_{rand}$ 方向扩展一个步长 $\Delta q$ ，得到 $q_{new}$
连接：尝试用 RRTConnect 策略连接 $q_{new}$ 到另一棵树
重复：直到达到目标区域或最大迭代次数

RRTConnect 改进:

同时构建两棵树（起点树和目标树）
每次迭代尝试将一棵树向另一棵树扩展
当两棵树连接成功时，找到解路径

项目实现（path_plan_ompl_rrtconnect.py）:

import ompl.base as ob
import ompl.geometric as og

state_space = ob.RealVectorStateSpace(model.nq)
bounds = ob.RealVectorBounds(model.nq)
for i in range(model.nq):
    bounds.setLow(i, model.jnt_range[i, 0])
    bounds.setHigh(i, model.jnt_range[i, 1])
state_space.setBounds(bounds)

si = ob.SpaceInformation(state_space)

def is_state_valid(state):
    data.qpos[:7] = [state[i] for i in range(7)]
    mujoco.mj_step(model, data)
    return data.ncon == 0  # 无碰撞

si.setStateValidityChecker(ob.StateValidityCheckerFn(is_state_valid))

start = ob.State(state_space)
goal = ob.State(state_space)
start_js = [-2.80558, 0.575492, -0.331959, -0.0709803, 1.21621, 1.03666, 2.21675]
goal_js = [-0.40558, 0.375492, -0.231959, -0.0709803, 1.21621, 1.03666, 1.21675]
for i in range(7):
    start[i] = start_js[i]
    goal[i] = goal_js[i]

planner = og.RRTConnect(si)
planner.setProblemDefinition(pdef)
planner.solve(10.0)  # 10秒超时

碰撞检测: data.ncon == 0 表示无接触发生

5. 动力学与控制

5.1 阻抗控制

阻抗控制 是一种基于力的控制方法，使机器人表现出类似弹簧-阻尼系统的动态特性。

目标方程（笛卡尔空间）:
$M_d \ddot{e} + B_d \dot{e} + K_d e = F_{ext}$

其中：

$M_d$ : 期望惯性矩阵（对角矩阵，表示各方向的质量）
$B_d$ : 期望阻尼矩阵
$K_d$ : 期望刚度矩阵
$e = x_{des} - x_{actual}$ : 位置误差
$F_{ext}$ : 外部作用力

物理意义:

刚度 $K_d$ 越大，机器人对位置偏差的响应越强（定位越精确）
阻尼 $B_d$ 越大，系统响应越慢，振荡越小
惯性 $M_d$ 影响系统的动态响应特性

项目实现（impedance_control.py）:

class Test(mujoco_viewer.CustomViewer):
    def runBefore(self):
        self.Kp = np.diag([100] * self.model.nu)  # 刚度矩阵
        self.Kd = np.diag([10] * self.model.nu)   # 阻尼矩阵
        self.q_desired = [0.0, -0.991, 0.196, 0.662, -0.88, 0.66]

    def runFunc(self):
        q = self.data.qpos[:self.model.nu]
        qdot = self.data.qvel[:self.model.nu]
        error = self.q_desired - q
        torque = self.Kp @ error - self.Kd @ qdot  # 阻抗控制力矩
        self.data.ctrl[:] = torque

参数选择指南:

应用场景	Kp (刚度)	Kd (阻尼)	特性
精确装配	高	中	快速响应，低误差
人机协作	中	高	安全，柔顺
轨迹跟踪	中-高	中	平衡精度与平滑

5.2 导纳控制

导纳控制 与阻抗控制相反，根据测量的末端力/力矩来调整位置目标。

控制方程:
$M_d \ddot{x}_d + B_d \dot{x}_d + K_d (x_d - x_0) = F_{ext}$

其中 $x_d$ 是调整后的期望位置， $x_0$ 是原始位置。

项目实现（panda_dynamics_admittance.py）:

class PandaEnv(mujoco_viewer.CustomViewer):
    def runBefore(self):
        self.M_d = np.diag([10] * 6)  # 期望惯性
        self.B_d = np.diag([1] * 6)   # 期望阻尼
        self.K_d = np.diag([50] * 6)  # 期望刚度

    def runFunc(self):
        ee_pos_err = self.now_ee_pos - self.desired_pos
        F_e = F_meas - F_ref  # 力误差

        # 导纳控制方程: M·ddx + B·dx + K·(x_des - x) = F_e
        dd_ee = np.linalg.inv(self.M_d) @ (F_e - self.B_d @ self.now_ee_vel
                                           - self.K_d @ ee_pos_err)

        # 积分得到速度和位置修正量
        self.delta_d_ee_des += dd_ee * dt
        self.delta_ee_des += self.delta_d_ee_des * dt
        self.desired_pos[:6] = self.start_ee_pos[:6] + self.delta_ee_des[:6]

        # 使用 IK 求解新的关节角度
        tf = utils.transform2mat(...)
        self.dof, info = self.arm.ik(tf, current_arm_motor_q=self.last_dof)

典型应用场景:

拖动示教：人手动拖动机器人末端，机器人根据受力调整运动
碰撞响应：检测到碰撞力时，机器人自动避让
力跟踪：保持恒定的接触力

5.3 PID 控制

PID（比例-积分-微分）控制 是最经典的反馈控制方法。

控制律:
$K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{de(t)}{dt}$

离散实现:

class PIDController:
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.prev_error = 0
        self.integral = 0

    def update(self, error, dt):
        self.integral += error * dt
        derivative = (error - self.prev_error) / dt
        output = self.kp * error + self.ki * self.integral + self.kd * derivative
        self.prev_error = error
        return output

PID 参数调节口诀:

P（比例）: 响应速度，值越大响应越快，但可能导致振荡
I（积分）: 消除稳态误差，值越大消除越快，但可能导致超调
D（微分）: 预测误差趋势，提供阻尼，减少振荡

6. 强化学习

6.1 PPO 算法原理

PPO（Proximal Policy Optimization） 是一种策略梯度强化学习算法，通过限制策略更新幅度来提高训练稳定性。

目标函数:
$L^{CLIP}(\theta) = \mathbb{E}_t \left[ \min\left( r_t(\theta) \hat{A}_t, \text{clip}(r_t(\theta), 1-\epsilon, 1+\epsilon) \hat{A}_t \right) \right]$

其中：

$r_t(\theta) = \frac{\pi_\theta(a_t|s_t)}{\pi_{\theta_{old}}(a_t|s_t)}$ 是概率比
$\hat{A}_t$ 是优势函数估计
$\epsilon$ 通常设为 0.2

CLIP 机制:

当 $r_t$ 超出 $[1-\epsilon, 1+\epsilon]$ 时，损失被裁剪
防止策略更新过大导致性能崩溃

项目中的网络架构（rl_panda_reach_target_high_profile.py）:

POLICY_KWARGS = dict(
    activation_fn=nn.ReLU,
    net_arch=[dict(pi=[256, 128], vf=[256, 128])]  # 策略网络和价值网络
)
model = PPO(
    policy="MlpPolicy",
    env=env,
    policy_kwargs=POLICY_KWARGS,
    n_steps=2048,        # 每次更新前收集的步数
    batch_size=2048,
    n_epochs=10,         # 每次更新的epoch数
    gamma=0.99,          # 折扣因子
    learning_rate=2e-4,
)

关键参数说明:

参数	典型值	说明
gamma	0.99	远期奖励折扣因子
n_steps	2048	经验收集量
batch_size	2048	每次更新的批次大小
n_epochs	10	经验回放次数
learning_rate	2e-4	学习率

6.2 奖励函数设计

奖励函数是强化学习的核心，决定了智能体学到的行为。

项目中的奖励函数（rl_panda_reach_target_high_profile.py）:

def _calc_reward(self, ee_pos, ee_orient, joint_angles, action):
    dist_to_goal = np.linalg.norm(ee_pos - self.goal)

    # 1. 距离奖励（核心奖励）
    if dist_to_goal < self.goal_threshold:
        distance_reward = 100.0   # 成功奖励
    elif dist_to_goal < 2*threshold:
        distance_reward = 50.0
    elif dist_to_goal < 3*threshold:
        distance_reward = 10.0
    else:
        distance_reward = 1.0 / (1.0 + dist_to_goal)  # 稀疏奖励

    # 2. 直线接近奖励（塑形奖励）
    start_to_goal = self.goal - self.start_ee_pos
    start_to_current = ee_pos - self.start_ee_pos
    projection_ratio = np.dot(start_to_current, start_to_goal) / (np.linalg.norm(start_to_goal) ** 2)
    projected_point = self.start_ee_pos + projection_ratio * start_to_goal
    linearity_error = np.linalg.norm(ee_pos - projected_point)
    linearity_reward = 3.0 / (1.0 + linearity_error)  # 越接近直线奖励越高

    # 3. 姿态惩罚
    target_orient = np.array([0, 0, -1])  # 期望末端朝下
    angle_error = np.arccos(np.clip(np.dot(ee_orient_norm, target_orient), -1.0, 1.0))
    orientation_penalty = 0.3 * angle_error

    # 4. 动作平滑惩罚
    action_diff = action - self.prev_action
    smooth_penalty = 0.1 * np.linalg.norm(action_diff)

    # 5. 碰撞惩罚
    contact_penalty = 1.0 * self.data.ncon

    # 6. 关节限位惩罚
    joint_penalty = 0.0
    for i in range(7):
        if joint_angles[i] < min_angle:
            joint_penalty += 0.5 * (min_angle - joint_angles[i])
        elif joint_angles[i] > max_angle:
            joint_penalty += 0.5 * (joint_angles[i] - max_angle)

    total_reward = (distance_reward + linearity_reward
                   - contact_penalty - smooth_penalty
                   - orientation_penalty - joint_penalty)

    return total_reward, dist_to_goal, angle_error

奖励塑形（Reward Shaping）原则:

稀疏 vs 密集: 密集奖励学得更快，但可能学不到最优策略
塑形奖励: 添加额外奖励引导学习，但不能与最终目标冲突
权重平衡: 各奖励项权重需仔细调整，避免某个惩罚主导

7. 视觉伺服

7.1 PBVS 基于位置的视觉伺服

PBVS（Position-Based Visual Servoing） 通过提取图像特征估计目标的三维位姿，然后控制机器人到达目标位姿。

控制框图:

相机图像 → 特征提取 → 位姿估计(SolvePNP) → 位姿误差 → IK → 关节控制

算法步骤:

特征提取: 从图像中提取目标特征点（如 Apriltag 角点）
位姿估计: 使用 PnP 算法估计目标相对相机的 3D 位姿
$\text{SolvePnP}(image\_points, world\_points, camera\_matrix)$
误差计算:
- 位置误差: $e_p = p_{des} - p_{actual}$
- 姿态误差: $e_o = \frac{1}{2}(r_{des} \times r_{actual})$ （向量叉积近似）
速度控制率:
$v_{des} = \begin{bmatrix} K_p & 0 \\ 0 & K_o \end{bmatrix} \begin{bmatrix} e_p \\ e_o \end{bmatrix}$
逆运动学: $\dot{q} = J^{-1} v_{des}$

项目实现（panda_pbvs.py）:

class PandaPbvs:
    def runFunc(self):
        # 获取末端位姿
        ee_pos = self.data.body(self.end_effector_id).xpos.copy()
        ee_quat = self.data.body(self.end_effector_id).xquat.copy()
        R_ee = utils.quat2rotmat(ee_quat)

        # 期望位姿
        p_des = np.array([0.5, 0.0, 0.1])
        R_des = utils.euler2rotmat(np.pi, 0, 0)

        # 位置误差
        e_p = ee_pos - p_des
        v_des_lin = -k_p_lin * e_p

        # 姿态误差（旋转矩阵叉积）
        e_rot = 0.5 * (np.cross(R_des[:, 0], R_ee[:, 0]) +
                      np.cross(R_des[:, 1], R_ee[:, 1]) +
                      np.cross(R_des[:, 2], R_ee[:, 2]))
        v_des_rot = -k_p_rot * e_rot

        # 末端期望速度
        v_e_desired = np.concatenate([v_des_lin, v_des_rot])

        # 阻尼伪逆求解关节速度
        J_pinv_damped = self.dampedPinv(J, lambda_d=0.05)
        q_dot_des = J_pinv_damped @ v_e_desired

        # 限幅
        q_dot_des = np.clip(q_dot_des, -3.14, 3.14)
        self.data.ctrl[:7] = q_dot_des[:7]

PBVS 优缺点:

优点	缺点
误差在笛卡尔空间定义明确	需要相机标定和目标三维模型
收敛域大	对深度估计误差敏感
姿态控制精确	需要额外的位姿估计模块

7.2 MPC 模型预测控制

MPC（Model Predictive Control） 通过滚动优化来求解最优控制序列。

核心思想:

在每个控制周期，预测未来 $N$ 步的系统行为
求解优化问题得到最优控制序列
只执行第一个控制输入
下一个周期重复上述过程（滚动优化）

代价函数:
$\sum_{k=0}^{N-1} \left( \|e_p(k)\|_{Q_p}^2 + \|e_o(k)\|_{Q_r}^2 + \|\dot{q}(k)\|_R^2 \right)$

其中 $e_p, e_o$ 是位置和姿态误差， $Q_p, Q_r, R$ 是权重矩阵。

项目实现（pbvs_mpc.py）:

class PbvsMpc:
    def __init__(self, mjcf, frame_name, N, Ts):
        self.N = N      # 预测时域
        self.Ts = Ts   # 控制周期
        self.Q_p = 20.0 * np.eye(3)   # 位置误差权重
        self.Q_r = 5.0 * np.eye(3)    # 姿态误差权重
        self.R = 0.001 * np.eye(joints_num)  # 控制量权重

        # 构建优化问题
        self.opti = ca.Opti()
        self.var_dq = self.opti.variable(joints_num, N)  # 预测的关节速度

        for k in range(N):
            q_k = q_prev + dq_k * Ts
            e_p = translational_error(q_k, ref_tf)
            e_r = rotational_error(q_k, ref_tf)

            total_cost += ca.bilin(self.Q_p, e_p, e_p)
            total_cost += ca.bilin(self.Q_r, e_r, e_r)
            total_cost += ca.bilin(self.R, dq_k, dq_k)

            # 约束
            self.opti.subject_to(q_min <= q_k <= q_max)
            self.opti.subject_to(dq_min <= dq_k <= dq_max)

        self.opti.minimize(total_cost)
        self.opti.solver("ipopt", opts)

    def solve(self, T_object, current_q):
        self.opti.set_value(self.param_q0, current_q)
        self.opti.set_value(self.param_ref_tf, T_object)
        sol = self.opti.solve()
        dq_cmd = sol.value(self.var_dq)[:, 0]  # 只取第一个控制输入
        return dq_cmd, True

MPC 特点:

特点	说明
滚动优化	适应系统不确定性
约束处理	可直接处理输入/状态约束
计算量	随预测时域和状态维度指数增长
鲁棒性	自然处理系统延迟

8. 安装与使用

8.1 环境配置

# 创建虚拟环境
uv venv
source .venv/bin/activate  # Linux/Mac
# 或: .venv\Scripts\activate  # Windows

# 安装依赖
uv pip install -r requirements.txt

# 安装系统依赖（Linux/WSL2）
sudo apt update && sudo apt install -y \
    libgl1-mesa-glx libgl1-mesa-dri mesa-utils \
    libxcb-xinerama0 libxcb-icccm4 libxcb-image0 \
    libxcb-keysyms1 libxcb-randr0 libxcb-render-util0 \
    libxcb-shape0 libxcb-sync1 libxcb-xfixes0 \
    libxkbcommon-x11-0 libxcb-xkb1 libqt5gui5 libqt5widgets5

# 安装 PyKDL
bash install_pykdl.sh

# 安装 Pinocchio
bash install_pinocchio.sh

8.2 快速开始

# 关节位置控制
python control_joint_pos.py

# Pinocchio 末端 IK 控制
python control_ee_with_pinocchio.py

# 阻抗控制
python impedance_control.py

# RL 到达目标任务
python rl_panda_reach_target_high_profile.py

# 手柄遥操作
python joystick_sim_and_real_so100.py

9. 核心功能索引

9.1 逆运动学

文件	描述
`ik_casadi_panda.py`	基于 CasADi 的 Panda 逆运动学
`ik_kdl_panda.py`	KDL 逆运动学
`ik_pyroboplan_so_arm100.py`	PyRoboPlan SO-ARM100 逆运动学
`control_ee_with_pinocchio.py`	Pinocchio CLIK 闭环控制

9.2 轨迹规划

文件	描述
`trajectory_plan_toppra.py`	TOPPRA 最优时间轨迹规划
`path_plan_ompl_rrtconnect.py`	OMPL RRT 关节空间规划
`path_plan_pyroboplan_rrt.py`	PyRoboPlan RRT 带轨迹优化
`ik_path_paln_trajectory_pyroboplan.py`	笛卡尔空间 IK + 路径规划

9.3 动力学与控制

文件	描述
`joint_impedance_control.py`	关节空间阻抗控制
`impedance_control.py`	笛卡尔空间阻抗控制
`panda_dynamics_admittance.py`	末端导纳控制
`panda_dynamics_drag.py`	拖动示教仿真
`panda_dynamics_hold.py`	力矩控制保持位置
`pid_torque_and_get.py`	PID 力矩控制器

9.4 强化学习

文件	描述
`rl_panda_reach_target_high_profile.py`	PPO 到达目标任务
`rl_panda_obstacle_high_profile.py`	PPO 避障任务
`rl_panda.py`	基础 PPO 逆运动学控制

9.5 视觉伺服与视觉

文件	描述
`panda_pbvs.py`	PBVS 基于位置的视觉伺服
`pbvs_mpc.py`	MPC 模型预测视觉伺服
`camera_calibration.py`	棋盘格相机标定
`get_apriltag_pos.py`	Apriltag 位姿解算（SolvePNP）
`get_camera_pic.py`	相机图像获取

9.6 硬件接口

文件	描述
`joystick_sim_and_real_so100.py`	游戏手柄控制（仿真 + 实机）
`so100_real_control.py`	实体机器人 ZMQ 通信
`mocap_panda.py`	动捕接口控制

9.7 分析与工具

文件	描述
`get_workspace.py`	蒙特卡洛工作空间分析
`contact_detect.py`	碰撞检测
`get_ee_wrench.py`	末端执行器六维力获取
`null_space.py`	冗余机械臂零空间运动

10. 工具模块说明

10.1 src/utils.py - 数学工具

quat2rotmat(): 四元数转旋转矩阵
euler2rotmat(): 欧拉角转旋转矩阵
transform2mat(): 构建齐次变换矩阵
dampedPinv(): 阻尼伪逆

10.2 src/mujoco_viewer.py - 可视化基类

CustomViewer: 自定义仿真查看器基类
支持键盘/鼠标交互
提供 runBefore(), runFunc(), run_loop() 接口

10.3 src/pinocchio_kinematic.py - Pinocchio 封装

Kinematics: IK/FK 求解器
buildFromMJCF(): 从 MJCF 构建模型
ik(): 优化逆运动学求解

10.4 src/kdl_kinematic.py - KDL 封装

Kinematics: KDL 运动学求解器
fk(): 正运动学
ik(): 迭代逆运动学

10.5 src/pid_controller.py - PID 控制器

PIDController: 标准 PID 实现
update(): 根据误差计算控制量

10.6 src/lowpass_filter.py - 低通滤波

LowPassOnlineFilter: 一阶低通滤波器
滤除高频噪声

10.7 src/solvepnp.py - PnP 求解

相机位姿估计算法封装
用于视觉伺服中的目标定位

11. 主要依赖

mujoco (3.3.2) - 物理仿真
pinocchio (3.4.0) - 运动学与动力学
gymnasium (1.1.1) - RL 环境接口
stable-baselines3 (2.6.0) - PPO 算法实现
casadi - 优化与逆运动学
ompl (1.7.0) - 运动规划
toppra (0.6.3) - 轨迹优化
pyroboplan (1.2.0) - 机器人运动规划
opencv-python - 视觉处理

12. 参考文献

Featherstone, R. (2013). Rigid Body Dynamics Algorithms. Springer.
Schulman, J., et al. (2017). Proximal Policy Optimization Algorithms. arXiv:1707.06347.
Corke, P. (2017). Robotics, Vision and Control. Springer.
Kelly, A. (2017). A feedforward computed torque controller. MIT Press.
Casadio, M., et al. (2012). A comparison of vision-based and position-based visual servoing. IJARS.

全球具身智能开发者社区

立足具身智能前沿赛道，致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。

更多推荐

开源聚势·具身启智，杭州这场沙龙给出中国具身智能产业化新答案

全球具身智能开发者社区

YoMo安全机制详解：TLS v1.3如何保护你的AI代理通信

在当今AI驱动的分布式系统中，安全通信已成为不可忽视的核心需求。YoMo作为Serverless AI Agent Framework，采用TLS v1.3加密协议构建了强大的安全防护机制，确保AI代理在地理分布式边缘计算环境中的通信安全。本文将深入解析YoMo的TLS实现原理、配置方法及最佳实践，帮助开发者构建安全可靠的AI应用。## 为什么TLS v1.3是AI代理通信的理想选择TLS

全球具身智能开发者社区

InternScenes开源数据集

数据集介绍 InternScenes 是上海人工智能实验室发布的大规模、可模拟室内场景数据集，论文收录于 NeurIPS 2025。具身人工智能的发展高度依赖于具有场景多样性和逼真布局的大规模、可模拟3D场景数据集。然而，现有数据集通常存在以下不足：数据规模或多样性有限、布局经过"净化"处理导致小物体缺失，以及严重的物体碰撞问题。为解决上述问题，InternScenes 整合

全球具身智能开发者社区

所有评论(0)

查看更多评论

无宜

@qq_56908984

已为社区贡献1条内容

MuJoCo +机械臂+DRL+路径规划+避障仿真学习

无宜

MuJoCo 机械臂仿真学习 - 算法原理详解

目录

1. 项目概述

1.1 支持的机器人

1.2 项目结构

2. 数学基础

2.1 旋转表示方法

2.1.1 旋转矩阵（Rotation Matrix）

2.1.2 四元数（Quaternion）

2.1.3 欧拉角（Euler Angles）

2.2 变换矩阵

2.3 雅可比矩阵

3. 逆运动学

3.1 KDL 逆运动学

3.2 Pinocchio CLIK

3.3 CasADi 优化逆运动学

4. 轨迹规划

4.1 TOPPRA 最优时间规划

4.2 OMPL RRTConnect

5. 动力学与控制

5.1 阻抗控制

5.2 导纳控制

5.3 PID 控制

6. 强化学习

6.1 PPO 算法原理

6.2 奖励函数设计

7. 视觉伺服

7.1 PBVS 基于位置的视觉伺服

7.2 MPC 模型预测控制

8. 安装与使用

8.1 环境配置

8.2 快速开始

9. 核心功能索引

9.1 逆运动学

9.2 轨迹规划

9.3 动力学与控制

9.4 强化学习

9.5 视觉伺服与视觉

9.6 硬件接口

9.7 分析与工具

10. 工具模块说明

10.1 src/utils.py - 数学工具

10.2 src/mujoco_viewer.py - 可视化基类

10.3 src/pinocchio_kinematic.py - Pinocchio 封装

10.4 src/kdl_kinematic.py - KDL 封装

10.5 src/pid_controller.py - PID 控制器

10.6 src/lowpass_filter.py - 低通滤波

10.7 src/solvepnp.py - PnP 求解

11. 主要依赖

12. 参考文献

所有评论(0)

温馨提示：您尚未绑定手机号

无宜