Safe Reinforcement Learning on the Constraint Manifold: Theory and Applications

在线学习的需求：

• 机器人自适应控制要求策略在部署中持续（适应环境变化或者硬件老化）
• CMDP的离线训练：训练后直接部署无法解决真实世界的动态不确定性
• 需要保证在线学习的每一步都是安全的

同时如果在仿真训练时已经满足鲁棒性约束，迁移到现实时更能保证安全

举例：在一个机械臂避障任务中，机械臂末端需要从起点运动到目标点并在途中避开障碍物，其中有安全约束为机械臂与障碍物之间的距离D(q)> d_safe.扰动是关节控制过程中的噪声（如电机误差或外部力干扰）。

• 当没有扰动（标称情况）
  此时使用ATACOM控制方法：构造约束函数；随后在构造的约束函数切空间生成安全动作，从而严格保证k(q) <= 0

• 当存在扰动
  扰动模型:控制输入受到噪声影响；定理2可以保证机械臂可能短暂进入不安全区域，但是不会完全碰撞到障碍物

通过一个 机械臂避障 的实例，结合ATACOM算法的设计步骤，说明如何从问题建模到控制器实现的全过程。假设机械臂需在避免碰撞（安全约束）的同时完成末端轨迹跟踪（任务目标）。

---

1. 问题建模
   状态与动力学

• 状态变量：

$$ \( s = [q, \dot{q}] \in \mathbb{R}^{2n} \)，其中 \( q \) 为关节角度，\( \dot{q} \) 为角速度。 $$

• 动力学模型（控制仿射形式）：

$$ \dot{s} = \underbrace{\begin{bmatrix} \dot{q} \\ -M(q)^{-1}(C(q,\dot{q})\dot{q} + g(q)) \end{bmatrix}}_{f(s)} + \underbrace{\begin{bmatrix} 0 \\ M(q)^{-1} \end{bmatrix}}_{G(s)} u_s $$

• ( M(q) )：惯性矩阵，( C(q,\dot{q}) )：科氏力矩阵，( g(q) )：重力项。
• 控制输入 ( u_s \in \mathbb{R}^n )：关节力矩。

安全约束

• 避障约束（距离函数）：

$$ k(s) = d_{\text{min}} - \|p(q) - p_{\text{obs}}\| \leq 0 $$

• ( p(q) )：机械臂末端位置，( p_{\text{obs}} )：障碍物位置，( d_{\text{min}} )：最小安全距离。
• 约束函数 ( k(s) ) 需满足 ( C^1 ) 光滑性（可通过距离场平滑化实现）。

---

2. ATACOM算法设计步骤
   步骤1：构造约束流形

• 引入松弛变量 ( \mu \geq 0 )，定义等式约束：

$$ c(s, \mu) = k(s) + \mu = 0 $$

• 约束流形：

$$ \mathcal{M} = \{(s, \mu) \in \mathbb{R}^{2n} \times [0, +\infty) : c(s, \mu) = 0\} $$

• 投影性质：若 ( (s, \mu) \in \mathcal{M} )，则 ( s ) 满足原始约束 ( k(s) \leq 0 )。

步骤2：设计松弛变量动力学

• 动态更新规则（类K函数控制）：

$$ \dot{\mu} = \alpha(\mu) u_\mu, \quad \alpha(\mu) = \mu \quad (\text{例：线性函数}) $$

• 控制输入 ( u_\mu \in \mathbb{R} )：调节松弛变量的变化速率。
• 性质：若初始 ( \mu(0) > 0 )，则 ( \mu(t) > 0 ) 对所有 ( t \geq 0 ) 成立（Lemma 1）。

步骤3：增广系统动力学

• 联合系统：

$$ \begin{bmatrix} \dot{s} \\ \dot{\mu} \end{bmatrix} = \begin{bmatrix} f(s) \\ 0 \end{bmatrix} + \begin{bmatrix} G(s) & 0 \\ 0 & \alpha(\mu) \end{bmatrix} \begin{bmatrix} u_s \\ u_\mu \end{bmatrix} $$

• 控制矩阵：

$$ J_u(s, \mu) = \nabla_s c(s, \mu) G(s) = \nabla_q k(s) \cdot J_{\text{arm}}(q) \cdot M(q)^{-1} $$

- \( J_{\text{arm}}(q) \)：机械臂末端雅可比矩阵。

步骤4：切空间投影

• 切空间基 ( B_u ) 计算：
  • 对 ( J_u ) 进行SVD分解，取右奇异向量 ( B_u \in \mathbb{R}^{(n+1) \times n} ) 满足 ( J_u B_u = 0 )。
  • 物理意义：( B_u ) 的列向量张成允许的安全控制方向（即不违反约束的力矩组合）。

步骤5：安全控制器

• 控制律（公式12）：

$$ \begin{bmatrix} u_s \\ u_\mu \end{bmatrix} = \underbrace{-J_u^\dagger \psi}_{\text{漂移补偿}} + \underbrace{B_u u}_{\text{任务控制}} - \underbrace{\lambda J_u^\dagger c}_{\text{收缩项}} $$

• 参数说明：
  • ( \psi = \nabla_s k(s) \cdot f(s) )：约束对自然动态的敏感度。
  • ( u )：RL策略输出的原始力矩（如跟踪PD控制信号）。
  • ( \lambda > 0 )：收缩速率。

---

3. 实例运行流程
4. 初始化：机械臂位于安全位置 ( s_0 )，松弛变量 ( \mu(0) = \max(-k(s_0), \text{tol}) )。
5. 在线控制循环：
   • RL策略输出 ( u )：如目标跟踪的PD控制信号。
   • ATACOM修正：
     • 计算 ( J_u ) 和 ( B_u )，投影 ( u ) 到切空间。
     • 若机械臂接近障碍物（( k(s) \to 0 )），收缩项 ( -\lambda J_u^\dagger c ) 主导，产生避障力矩。
   • 执行安全控制 ( u_s )。
6. 结果：机械臂末端绕开障碍物并收敛到目标轨迹（见下图）。

---

4. 关键设计要点
   | 设计环节 | 解决方法 | 实例中的实现 |
   |--------------------|--------------------------------------------------|------------------------------------------|
   | 约束建模 | 距离函数 + 松弛变量 | ( k(s) = d_{\text{min}} - |p - p_{\text{obs}}| ) |
   | 流形切空间计算 | SVD分解控制矩阵 ( J_u ) | 机械臂雅可比矩阵与惯性矩阵耦合 |
   | 安全控制律 | 漂移补偿 + 任务投影 + 收缩项 | 避障力矩自动生成 |
   | 动态松弛变量 | 类K函数控制的 ( \dot{\mu} ) | 保证 ( \mu(t) > 0 ) 避免数值奇点 |

---

5. 数学与物理意义对应

• 切空间投影：将关节力矩限制在“不缩短机械臂与障碍物距离”的方向。
• 收缩项：类似虚拟排斥力，强度随 ( k(s) ) 接近0而增大。
• 松弛变量：量化“允许的临时约束违反”，动态调整控制保守性。

几种RL对于约束函数的处理方法

假设约束函数$k(s) \leq 0$, 机械臂与障碍物距离 $ D ( s ) \geq d _ { s a f e }$ 可以表示为 $ k(s) = d_safe - D(s) \leq 0 $

方法对比与切空间的优越性

以下是针对同一个约束函数，不同安全强化学习方法处理方式的对比分析，结合数学形式、实现逻辑和应用场景的详细说明：

1. 问题设定
   约束函数：

$$ k(s) \leq 0 $$ $$ （例如：机械臂与障碍物距离 \( D(s) \geq d_{\text{safe}} \) 可表示为 \( k(s) = d_{\text{safe}} - D(s) \leq 0 \)） $$

目标：在强化学习训练和部署中保证 ( k(s) \leq 0 ) 始终成立。

1. 不同方法对比

(1) 惩罚函数法（Penalty Method）

• 处理方式：
  在奖励函数中加入惩罚项：

$$ R_{\text{penalized}} = R_{\text{task}} - \lambda \max(0, k(s))^2 $$

• (\lambda) 为惩罚系数，需手动调节。
• 特点：
  • 数学简单：无需修改策略结构。
  • 安全无保证：训练早期可能严重违反约束（如图1左）。
  • 调参敏感：(\lambda) 过小导致约束失效，过大抑制探索。
• 适用场景：
  仿真环境预训练，且约束违反无严重后果时。

(2) 安全滤波器（QP-Based Safety Filter）

• 处理方式：
  在线求解二次规划问题，修正动作 ( u )：

$$ \begin{aligned} \min_{u} \quad & \|u - u_{\text{RL}}\|^2 \\ \text{s.t.} \quad & \nabla k(s)^T (f(s) + G(s)u) \leq -\alpha k(s) \end{aligned} $$

• 通过CBF（Control Barrier Function）保证约束。
• 特点：
  • 即时安全：每一步动作均满足约束。
  • 计算开销大：实时求解QP问题（毫秒级延迟）。
  • 可行性风险：约束冲突时无解（如机械臂被“卡死”）。
• 适用场景：
  计算资源充足的实时控制系统（如自动驾驶）。

(3) 备份策略（Backup Policy）

• 处理方式：

$$ 预设安全策略 \( \pi_{\text{safe}} \)（如零速度指令），当 \( k(s) > \epsilon \) 时切换： $$ $$ u = \begin{cases} u_{\text{RL}} & \text{if } k(s) \leq \epsilon, \\ \pi_{\text{safe}}(s) & \text{otherwise}. \end{cases} $$

• 特点：
  • 简单可靠：硬件易实现。
  • 保守性：任务中断频繁（如图1中）。
  • 泛化性差：需为每个任务单独设计 ( \pi_{\text{safe}} )。
• 适用场景：
  已知明确安全动作的简单系统（如急停按钮）。

(4) 切空间投影（ATACOM，Tangent Space Projection）

• 处理方式：
  1. 构造约束流形 ( \mathcal{M} = {s \mid k(s) = 0} ) 的切空间 ( T_s\mathcal{M} )。
  2. 将RL动作 ( u ) 投影到切空间：

$$ u_{\text{safe}} = (I - \nabla k(s) (\nabla k(s)^T \nabla k(s))^{-1} \nabla k(s)^T) u $$

3. 动态调整松弛变量 ( \mu ) 保证渐近收敛（见原文公式12）。

• 特点：
  • 严格安全：数学保证 ( k(s) \leq 0 )（即使训练中）。
  • 高效：投影计算仅需矩阵乘法（微秒级）。
  • 自适应：无需手动设计安全策略。
• 适用场景：
  高维复杂系统（如机器人、无人机）的在线安全学习。

对比总结表

方法	数学形式	安全性保证	计算效率	是否需要调参	适用场景
惩罚函数法	修改奖励函数	❌ 无	⚡ 最高	✅ 需调λ	无风险仿真
安全滤波器	在线QP优化	✅ 严格	🐢 慢	❌ 无需	高资源实时系统
备份策略	条件策略切换	⚠️ 保守	⚡ 快	✅ 需设计策略	简单硬件系统
切空间投影	几何投影+动态松弛	✅ 严格	⚡ 快	❌ 无需	复杂在线学习

1. 实例说明（机械臂避障）

• 惩罚函数法：
  训练早期机械臂碰撞障碍物（( k(s) > 0 )），直到策略学会规避。
• 安全滤波器：
  每个时间步求解QP，保证 ( D(s) \geq 0.1 ) m，但计算延迟导致控制频率下降。
• 备份策略：
  机械臂接近障碍物时紧急停止，无法完成任务。
• 切空间投影：
  机械臂沿障碍物表面切线运动（( D(s) \equiv 0.1 ) m），既安全又完成任务。

1. 核心差异总结
2. 安全性的数学基础：
   • 切空间投影和QP滤波器通过微分几何/优化理论严格保证安全。
   • 其他方法依赖启发式设计。
3. 计算本质：
   • 切空间投影是解析解，QP是数值解，备份策略是规则匹配。
4. 适用性：
   • 切空间方法唯一同时满足实时性、严格安全和自适应学习的要求。

通过这种对比，可以清晰看出为何ATACOM的切空间方法在复杂安全关键任务中具有显著优势。

应用方向	是否适合用 RL	推荐算法
自主步态生成	✅	PPO / SAC
多地形适应	✅	PPO + Domain Rand
扰动恢复	✅	TD3 / SAC
动力学复杂、控制难	✅	RL + WBC / MPC
纯平地、稳定任务	❌（传统控制器足够）	-

强化学习在双足机器人中的问题和解决方案

问题类别	具体问题描述	典型解决方法	说明
Sim-to-Real Gap	仿真与现实存在建模差异，策略迁移失败	Domain Randomization	训练时随机化仿真参数，提高泛化能力
		Domain Adaptation / Fine-tuning	用少量真实数据微调策略
		Residual Policy Learning	用小网络学习偏差残差，补偿仿真误差
		System Identification	精准拟合物理参数，减小建模误差
样本效率低	学习步态需要大量交互，训练时间长	Imitation Learning + RL	先用专家演示加速策略初始化
		Offline RL（如 CQL）	用离线数据训练，减少实物交互
		Model-based RL / Dynamics model	引入动力学模型减少试错次数
安全性差	实机训练中容易跌倒、损伤硬件	Safe RL / Shielded RL	引入安全约束、屏蔽高风险动作
		Curriculum Learning	从简单任务逐步训练，降低跌倒风险
		仿真预训练 + 小步实机测试	仿真先学好，在现实中逐步上线
不可解释性强	策略黑箱、调试难、难验证稳定性	Hybrid Control (RL + MPC/WBC)	RL 生成参考，稳定由经典控制器保障
		可视化工具（t-SNE, Grad-CAM）	分析策略结构与决策依据
		结构化策略设计	将任务拆解成可控子模块组合训练

扩散模型在其中的作用

Sim2Real问题点	扩散模型可能解决的问题	原理与作用说明
1. 物理环境差异	生成更真实、丰富的环境扰动样本	扩散模型可以学习仿真环境和真实环境间的差异分布，生成多样化、接近真实环境的状态或扰动样本，丰富训练数据，使策略对真实环境有更好的鲁棒性。
2. 样本效率低	辅助生成高质量模拟数据，提升样本多样性和有效性	通过扩散模型生成逼真且多样化的状态转移样本，减少真实环境数据需求，提高训练效率。
3. 安全风险	辅助仿真中生成更接近真实的安全扰动数据，降低实机试错风险	利用扩散模型生成多种环境扰动场景，进行安全的仿真训练，避免实机危险。
4. 策略泛化能力弱	通过生成多样化情景样本，增强策略对未见环境的泛化	多样化生成可以让RL策略在训练时接触更广泛的状态分布，减少策略过拟合，提升现实适应能力。

双足机器人的控制方法汇总

层级	控制范式	代表方法
1⃣ 底层控制	位置/速度/力控制	PID控制、阻抗控制、力控制
2⃣ 运动学级	逆运动学、任务空间控制	IK伪逆、OSC、Stack-of-Tasks
3⃣ 动力学级	基于模型的控制	WBC、Feedback Linearization、优化控制
4⃣ 步态规划与预测控制	步态生成、预测控制	ZMP轨迹规划、MPC、Preview Control
5⃣ 混合系统与学习	稳定性优化与学习	HZD、强化学习、RL-MPC、模仿学习

方法	控制层级	优点	局限	适合场景
PID/阻抗	底层	简单鲁棒	控制力弱	力控制、人机交互
SoT / OSC	运动学	多任务清晰	不考虑动态	多目标位置控制
WBC	动力学	多任务 + 力控制	实时性要求高	全身控制、双足
MPC / Preview	预测	稳定性强	需模型、优化开销大	CoM轨迹、ZMP稳定
HZD	杂交系统	周期稳定性好	难以适应非周期	平地行走、理论研究
强化学习	数据驱动	灵活自适应	样本量大、转移难	非结构环境

杂交系统是指包含连续动态系统（例如由微分方程描述的运动）和离散事件系统（例如接触切换、碰撞、跳跃）的组合系统。

双足机器人中的杂交结构

• 系统由两种部分构成：连续阶段（Continuous Dynamics：机器人在空中摆腿、质心运动

例如：单腿支撑期、双腿支撑期的动力学模型

离散事件（Discrete Transitions）：脚与地面接触或离地引起的状态突变

一个杂交系统一般写作如下六元组：$ H = ( Q , X , f , D , R , G )$

控制流程总结图

目标行走任务（如向前走）
↓
[高层任务] 决定步长/频率
↓
[步态生成] 生成ZMP轨迹、抬脚时序
↓
[运动规划] 用ZMP规划CoM轨迹（如 Preview Control）
↓
[任务空间控制] 生成CoM和足端运动指令
↓
[关节控制] 逆解得到关节目标值（IK/ID）
↓
[底层控制] PID or 力矩控制跟踪
↓
[电机执行，机器人行走]

双足机器人中步行常用轨迹类型

轨迹类型	控制目标	怎么求得的
ZMP轨迹（零力矩点）	动态稳定性	ZMP保持在支撑面内部
质心轨迹（CoM）	平衡、重心移动	CoM 按LIPM/MPC等控制
足端轨迹（swing foot）	摆动腿落点精度	Bézier曲线、多项式轨迹
关节轨迹（joint angles）	驱动指令输入	由逆运动学或优化求得
任务空间轨迹（如髋/躯干）	姿态保持	水平/垂直速度稳定

双足机器人中OMPL的用途

• 环境中的路径规划（从A到B点）
• 多步脚印序列规划： OMPL 可用来在配置空间中搜索一条“步态合法序列”。

ESO在双足机器人中的应用

双足机器人是高度非线性、耦合、强扰动的系统：

问题	对应解决目标
地面不确定性（软硬、摩擦）	外部扰动估计
建模误差（动力学不完整）	结构性扰动估计
控制信号延迟、误差	观测状态误差补偿
状态难以精确传感（如关节加速度）	使用 ESO 估计不可测状态

双足中的欠驱动问题

欠驱动意味着控制输入的自由度少于系统的自由度，导致系统无法直接驱动所有运动变量。控制这类系统时，通常需要利用系统本身的动力学特性和非线性控制方法。

方法类别	具体方法
反馈线性化控制	利用系统的非线性动力学方程，通过反馈设计奖系统部分状态线性化，便于设计线性控制器
滑膜控制	设计滑膜面
非线性控制（如反馈线性化结合非线性轨迹规划）	- 设计非线性控制器匹配系统非线性动力学，同时结合轨迹规划确保系统按预期运动
阶段切换控制（Hybrid Control）	- 欠驱动系统多表现为混合动力学（如步态中不同相位），通过切换不同控制策略完成不同阶段的控制
反步法（Backstepping）	- 递归设计控制律，将复杂系统分解为层级子系统，逐步设计控制器，适用于非线性欠驱动系统
优化控制与MPC（Model Predictive Control）	- 利用模型预测未来状态，求解带约束的最优控制输入，特别适合处理欠驱动系统的运动规划和实时控制
学习控制与强化学习	- 通过数据驱动的方法，学习欠驱动系统的控制策略，适合模型不准确或环境复杂的情况

双足机器人中走的更快的问题

轨迹规划改进

• 动态步态生成
• 可变的步态调整

控制算法应该怎么改进

• 前馈和反馈进行结合：前馈部分用于快速响应人体和环境变化，反馈部分保证稳定性和误差校正。
• MPC：利用预测未来状态，提前计算控制输入，增强快速运动的稳定性和灵活性。
• 自适应控制：实时调整控制参数，应对人体动力学变化和不同的速度变化。

解决走路晃动幅度很大的问题

加强平衡控制：对质心和零力矩点进行控制

控制算法优化：

• 增强反馈控制增益
• 阻抗控制或者自适应阻抗控制
• 模型预测控制

MPC for Humanoid Gait Generation: Stability and Feasibility

问题：传统的ZMP方法与质心（CoM）动力学存在内在不稳定性。
创新方法：提出了一种本质稳定的模型预测控制，通过稳定性约束保证CoM轨迹相对于ZMP轨迹有界。

整体框架：

• 高层指令：接收外部驱动的参考速度（平移速度v_x, v_y和转向速度\omaga）;
• 脚步生成模块：通过QP优化生成候选脚步位置和时序T_s；
• IS-MPC模块：优化实时脚步和CoM轨迹，确定稳定性约束；
• 运动控制：跟踪生成的CoM和摆动脚的轨迹。

DreamPolicy: A Scalable Unified Policy for Versatile Humanoid Locomotion

任务的依赖性：传统的强化学习需要为每种地形设计特定的奖励函数，导致“一个任务一个策略”
数据利用不足：即使数据量增长，但是传统的强化学习难以有效利用离线数据提升泛化能力。
泛化性差：策略容易过拟合训练地形，在未见地形中表现不佳。

现有解决方案的缺陷：

• 人类运动数据集需要复杂重定向到人形机器人，而且无法直接编码地形约束。
• 策略蒸馏（将一个性能好但是计算开销高的策略压缩给一个较小较高效的策略网络）依赖在线仿真，计算成本较高。

DreamPolicy的框架创新
地形感知的的专家技能收集：

• 在六种单一地形上训练专用的RL策略，生成巨大的离线数据集
• 数据直接捕获人形机器人运动学，包含本地感知，地形高度图，历史状态等

扩散驱动的混动合成

• 使用生成模型框架（DDPM）来预测未来的状态轨迹，条件输入包含历史状态和地形嵌入。

HMI条件化统一策略

• 以扩散模型生成的轨迹为动态目标，优化RL策略。扩散模型和策略优化分离，支持通过数据扩展持续改进。
• HMI隐式编码指的是一种通过扩散模型生成的未来运动轨迹来动态替代传统手工设计奖励函数的技术。核心思想是将地形物理约束，运动风格等复杂目标嵌入到扩散模型生成的轨迹中，从而指导策略学习。

算法模块

文件名	功能简要
Eul_w_filter.cpp / Eul_w_filter.h	用于欧拉角速度滤波（姿态估计的角速度平滑）
foot_placement.cpp / foot_placement.h	足端位置规划（多用于步态规划、平衡控制）
gait_scheduler.cpp / gait_scheduler.h	步态调度器（步态状态机、步态切换、步态周期控制）
joystick_interpreter.cpp / joystick_interpreter.h	摇杆解释器（人机交互或远程控制的速度指令解析）
mpc.cpp / mpc.h	模型预测控制（用于轨迹规划、步态控制或平衡控制）
pino_kin_dyn.cpp / pino_kin_dyn.h	Pinocchio动力学/运动学接口（封装好的运动学求解器）
priority_tasks.cpp / priority_tasks.h	任务优先级模块（多任务控制，如Stack-of-Tasks等）
StateEst.cpp / StateEst.h	状态估计模块（卡尔曼滤波或其他方法估计机器人状态）
wbc_priority.cpp / wbc_priority.h	具有优先级的全身控制（Whole-Body Control）模块

jocabi矩阵的作用

四元数的形式

• 四元数通常表示为：q=w+xi+yj+zk
  w：实部（标量部分），表示旋转的角度信息。
  x,y,z：虚部（向量部分），表示旋转轴的方向。
  避免万向节锁：欧拉角在特定角度（如俯仰角±90°）时丢失自由度，而四元数无此问题。

• 四元数的规范化：
  $ | | q | | = \sqrt { w ^ { 2 } + x ^ { 2 } + y ^ { 2 } + z ^ { 2 } } = 1$

• 四元数到旋转矩阵：$ R = \begin{bmatrix} 1 - 2 y ^ { 2 } - 2 z ^ { 2 } 2 x y - 2 w z 2 x z 2 x z 2 x z \ 2 x y + 2 w z 1 - 2 x ^ { 2 } - 2 z ^ { 2 } 2 y z - 2 w x \ 2 x z - 2 w y 2 y z + 2 w x 1 - 2 x ^ { 2 } - 2 y ^ { 2 } \end{bmatrix} $

旋转矩阵的使用

• 绕x轴旋转：
  M << 1, 0, 0,
  0, cos(theta), -sin(theta),
  0, sin(theta), cos(theta);

• 绕y轴旋转：
  M << cos(theta), 0, sin(theta),
  0, 1, 0,
  -sin(theta), 0, cos(theta);

• 绕z轴旋转：
  M << cos(theta), -sin(theta), 0,
  sin(theta), cos(theta), 0,
  0, 0, 1;

贝塞尔曲线的使用

• $ B ( s ) = \sum _ { i = 0 } ^ { n } C ( n , i ) ( 1 - s ) ^ { n - i } s ^ { i } P _ { i }， 其中 s∈[0,1] $ 。
• 上面的贝塞尔曲线本身就是连续没有拐点的，同时导数也是连续的。
• 在一维（Bezier_1D）里，P 是一个向量，保存了 控制点的值。

卡尔曼滤波

卡尔曼滤波是一种递归状态估计算法，通过融合系统模型预测和传感器观测数据，在噪声干扰下得到最优状态估计。

• 预测-更新循环：交替进行模型预测和测量校正。
• 最小方差准则：通过协方差矩阵量化不确定性，追求估计误差最小。

状态方程：$ x _ { k } = A _ { k } x _ { k - 1 } + B _ { k } u _ { k } + w _ { k }$
观测方程： $z _ { k } = H _ { k } x _ { k } + v _ { k } $
以下是卡尔曼滤波（Kalman Filter）的详细过程解析，结合数学推导和实际意义说明：

1. 卡尔曼滤波五大公式及解释
   假设线性系统模型如下：

• 状态方程：( \mathbf{x}_k = \mathbf{A}k \mathbf{x}{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k )
• 观测方程：( \mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf )
  其中：
• ( \mathbf{x}_k )：系统状态（如位置、速度）。
• ( \mathbf{u}_k )：控制输入。
• ( \mathbf{w}_k \sim \mathcal{N}(0, \mathbf{Q}_k) )：过程噪声（建模误差）。
• ( \mathbf{v}_k \sim \mathcal{N}(0, \mathbf{R}_k) )：观测噪声。
• ( \mathbf{A}_k, \mathbf{B}_k, \mathbf{H}_k )：状态转移、控制输入、观测矩阵。

步骤1：状态预测（时间更新）

1. 先验状态估计：

$$ \hat{\mathbf{x}}_k^- = \mathbf{A}_k \hat{\mathbf{x}}_{k-1} + \mathbf{B}_k \mathbf{u}_k $$

• 基于上一时刻最优估计 ( \hat{\mathbf{x}}_{k-1} ) 和系统模型预测当前状态。

2. 先验协方差预测：

$$ \mathbf{P}_k^- = \mathbf{A}_k \mathbf{P}_{k-1} \mathbf{A}_k^T + \mathbf{Q}_k $$

• ( \mathbf{P}_k^- ) 表示预测状态的不确定性（含过程噪声 ( \mathbf{Q}_k )）。

步骤2：卡尔曼增益计算

$$ \mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}_k)^{-1} $$

• 物理意义：
  • 分子 ( \mathbf{P}_k^- \mathbf{H}_k^T )：预测状态与观测的协方差。
  • 分母 ( \mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}_k )：预测观测的总不确定性（含传感器噪声 ( \mathbf{R}_k )）。
• 作用：权衡模型预测与观测的信任度。若传感器噪声大（( \mathbf{R}_k ) 大），增益 ( \mathbf{K}_k ) 减小，更依赖模型预测。

步骤3：状态更新（测量校正）

$$ \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_k^-) $$

• 残差项 ( \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_k^- )：观测值与预测值的差异（即“新息”）。
• 更新逻辑：通过卡尔曼增益将残差加权修正预测值。

步骤4：协方差更新

$$ \mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^- $$

• 意义：修正后的状态不确定性降低（( \mathbf{P}_k \leq \mathbf{P}_k^- )）。

4. 扩展卡尔曼滤波（EKF）
   针对非线性系统（如机器人SLAM）：
5. 线性化：在当前估计点对非线性模型 ( f(\mathbf{x}), h(\mathbf{x}) ) 进行一阶泰勒展开。
   • 状态方程雅可比矩阵：( \mathbf{F}k = \left. \frac{\partial f}{\partial \mathbf{x}} \right|{\hat{\mathbf{x}}_{k-1}} )。
   • 观测方程雅可比矩阵：( \mathbf{J}k = \left. \frac{\partial h}{\partial \mathbf{x}} \right|{\hat{\mathbf{x}}_k^-} )。
6. 公式调整：
   • 预测：( \hat{\mathbf{x}}k^- = f(\hat{\mathbf{x}}{k-1}) )。
   • 协方差：( \mathbf{P}_k^- = \mathbf{F}k \mathbf{P}{k-1} \mathbf{F}_k^T + \mathbf{Q}_k )。
   • 增益：( \mathbf{K}_k = \mathbf{P}_k^- \mathbf{J}_k^T (\mathbf{J}_k \mathbf{P}_k^- \mathbf{J}_k^T + \mathbf{R}_k)^{-1} )。

MPC的控制过程

• 线性化动力学（Ac, Bc）
  • 当前机器人站在世界系某个角度（base_rot），所以要将惯性矩阵和其他力项转换到世界系。
• 离散变换，把连续时间Ac, Bc乘上dt得到离散时间矩阵A, B。
• 累积状态转移矩阵（Aqp, Aqp1）
• 累积输入矩阵（Bqp1, Bqp11, Bqp）
• 构建QP问题

HiLMa-Res: A General Hierarchical Framework via Residual RL for Combining Quadrupedal Locomotion and Manipulation

✅ 1. 提出了一个可泛化的 loco-manipulation 分层强化学习框架

📌 原理：

• Loco-manipulation 是 locomotion（行走）+ manipulation（操作）的组合问题，控制空间极大，策略难以学习。
• 将整体任务分层：
  • 低层：Locomotion Controller → 学习行走并追踪任意足端轨迹；
    • 用神经网络实现 PPO 策略
      输入：包含机器人当前的运动状态、历史状态、外部扰动、目标轨迹（轨迹隐含在相位变量和周期中）；
      输出：每只脚的期望接触力 / 足端位置（用于力控或位置控）；
      训练方法：PPO 强化学习算法；
      在 Isaac Gym 中高效并行训练；
      加了 domain randomization（随机地形、噪声）以增强泛化性；
      特征：一旦训练完成，可以直接部署到真实机器人，无需再训练。
  • 高层：Manipulation Planner → 规划残差轨迹，实现操纵任务。
    • 用神经网络实现轨迹规划策略
      输入：状态输入：机器人自身状态、目标信息（如目标位置、速度方向）；
      或图像输入：深度图、摄像头画面（需加 CNN）；
      输出：每条腿一条 3 阶 Bezier 曲线的控制点（一般是 3~4 个点）；
      CPG 的周期/相位调节参数（例如走路节奏）；
      训练方法：
      同样使用强化学习算法（PPO/DroQ）；
      奖励函数任务相关，比如推球的方向、推箱位置、避障是否成功；
      可接入现实数据再训练（RLPD），提升现实表现；
      特点：通用框架 + 不同任务换不同 reward 即可复用。
  • 用低层网络学“怎么走”；用高层网络学“走向哪里、怎么推、怎么跨”。

✅ 好处：

• 解耦复杂任务 → 降低学习难度；
• 低层可重用多个任务，高层策略任务特定，实现模块化和迁移能力。

✅ 2. 采用任务无关的运动控制器和任务相关的操作规划器

📌 原理：

• 低层运动控制器（Locomotion Controller）专注于足端轨迹跟踪，不关心上层任务内容。
• 高层操作规划器只需要设计足端轨迹（residual），来完成具体任务目标。

✅ 实现方式：

• 低层训练时，只用模拟中随机生成的轨迹训练它追踪；
• 高层策略负责设计这些轨迹的“形状”（比如球要往左推 → 左脚向外伸）；
• 二者通过轨迹接口（Bezier + CPG）连接，高层给轨迹，低层负责执行。
• 实现方式：
  • 低层训练时，只用模拟中随机生成的轨迹训练它追踪；
  • 高层策略负责设计这些轨迹的“形状”（比如球要往左推 → 左脚向外伸）；
  • 二者通过轨迹接口（Bezier + CPG）连接，高层给轨迹，低层负责执行。

✅ 3. 利用了残差学习（Bezier 曲线）+ CPG（中央模式发生器）来灵活生成轨迹

📌 原理：

• CPG（Central Pattern Generator）：用于生成周期性足端轨迹，如走路、跳跃；
• Bezier 曲线：高层策略学习残差，用平滑的曲线微调轨迹，实现操作动作（如推、踢、跨越）。

✅ 实现方式：

• 控制器最终执行的轨迹 = CPG生成的默认轨迹 + Bezier残差轨迹；
• Bezier 的控制点由高层网络输出，调节轨迹形状；
• 可用较少参数，生成复杂、多样的足端轨迹。

✅ 4. 在多个现实任务上验证了方法的通用性与优越性

📌 实验设计原理：

• 将高层策略迁移到多个任务上，如：

  1. 带球（dribble）
  2. 踩点避障（stepostone）
  3. 推箱（navload）

• 低层控制器不变，仅更换高层策略 → 验证了框架的模块化和通用性。

✅ 成果：

• 所有任务均能从仿真零样本直接迁移到现实机器人，性能优于多个对比基线（Reward shaping、Motion tracking、AMP 等）。

✅ 5. 支持视觉、状态、多模态输入，适配多种 RL 算法

📌 原理：

• 高层策略输入是任务相关的信息，具有很强的扩展性；
  • 可接受：
    • 状态向量（如物体位置、机器人速度等）；
    • 视觉输入（如深度图）；
    • 多模态（状态 + 视觉）；
• 可适配不同类型的 RL 算法，如 PPO、DroQ、RLPD（现实数据驱动）等。

✅ 实现方式：

• 高层策略结构灵活：可以是 MLP（状态输入）或 CNN+MLP（图像输入）；
• 训练框架可无缝切换不同策略优化器；
• 利用现实 fine-tuning（如 RLPD）提升现实成功率和样本效率。

---

王雪松, 王荣荣, 程玉虎. 安全强化学习综述. 自动化学报, 2023, 49(9): 1813−1835

---

《Safe Reinforcement Learning Using Robust Control Barrier Functions》（Emam et al., 2022）（本质上是一种策略约束方法）

提出了一种结合**鲁棒控制屏障函数（RCBF）和强化学习的安全强化学习框架，并以Soft Actor-Critic (SAC)**为基底，解决训练过程中可能进入不安全状态的问题。

论文核心结构与思路

1. 问题背景

• 强化学习需要大量探索，但这可能会引发安全问题（如机器人撞墙、无人机坠落）。
• 安全强化学习目标：在训练中就保持系统不进入不安全状态，而不是仅最终收敛到安全策略。

2. 方法结构（SAC-RCBF）

📎 系统建模：

考虑扰动控制仿射系统：

\dot{x}(t) = f(x(t)) + g(x(t))u(x(t)) + d(x(t))

• (d(x))：未知扰动，用**高斯过程（GP）**建模。
• 控制目标是：用 RL 找最优策略 ( \pi )，在执行前通过 RCBF 层进行修正，确保安全。

📎 安全模块：RCBF-QP 层

• 用**鲁棒控制屏障函数（RCBF）**构造约束：

\nabla h(x)^T(f(x) + g(x)u(x)) \geq -\alpha(h(x)) - \min \nabla h(x)^T d(x)

• 通过**二次规划（QP）**调整策略输出，最小化修改幅度，确保满足约束。

📎 强化学习部分：Soft Actor-Critic (SAC)

• 引入可微分安全层，可以反向传播梯度，增强策略学习能力。

• 提出两个优化：

  1. Differentiable RCBF Layer：允许梯度传播：使得策略能够进行自动修正。可微分安全层让“安全修正”变成了策略可以感知和优化的目标，因此是实实在在的“加速 + 提升性能”的方法。
  2. Model-based rollout：用学习到的模型生成合成数据，加速学习

• 没有可微分安全层：

问题	影响
策略网络不知道“自己动作被改了”	无法学习“避开”那些不安全动作
CBF 修正是黑箱、不可导	策略只能“撞墙后被拉回来”，而不是“提前不撞”
SAC 更新方向可能乱了	有效梯度方向被掩盖，学习效率低
高维控制系统尤其严重	RL 很难靠试错探索到“刚刚好不触碰屏障”的策略

• 有安全层：

优点	结果
策略能“感知到”安全边界的位置	不再只是靠碰撞惩罚、随机试错
CBF 修正能“教会”网络如何避开风险	把 CBF 的结构性知识反传给策略
策略更快收敛、学到更优行为	不容易陷入安全/性能折中困境

📎 模块化任务学习（Modular Learning）

• 任务 reward 与安全 constraint 解耦，使得训练出的策略具有更强的迁移能力（zero-shot transfer）。

创新点总结

创新点类别	内容说明
✅ 融合方法创新	将 RCBF 融入 SAC 中，确保训练过程安全
✅ 可微分安全层	提出 differentiable RCBF-QP，允许梯度传播
✅ 模块化训练思想	reward 与 safety 解耦，提升策略迁移性
✅ 数据效率优化	使用部分已学模型做合成 rollout，提升样本效率

可改进方向 / 研究机会

1. 更多维度扰动建模方法：GP 对高维 d(x) 效率有限，可考虑使用贝叶斯神经网络或 ensemble 模型。
2. 更通用的 CBF 设计工具：当前构造 CBF 需先验经验，可引入自动构造或学习 CBF 的方法。
3. 现实部署实验验证缺失：目前只在模拟环境中测试，若能在真实机器人上测试更具说服力。
4. 与 MPC 的融合探索：框架可拓展为 hybrid RL-MPC，进一步提升实时安全性。

---

SAC算法：

SAC 是一种最大化奖励 + 最大化动作多样性（熵）的 off-policy 深度强化学习算法，具有高效、稳定、探索性强的特点。

一、SAC 的基本思想

传统强化学习只最大化期望回报：

\max_\pi \mathbb{E}_{\pi} \left[\sum_t r(s_t, a_t)\right]

不仅希望获得高奖励，还希望策略有足够的“随机性”，保持探索能力。

引入最大熵强化学习目标：

\max_\pi \mathbb{E}_{\pi} \left[\sum_t r(s_t, a_t) + \alpha \mathcal{H}(\pi(\cdot|s_t)) \right]

• ( \mathcal{H}(\pi) = -\log \pi(a|s) ) 是策略的熵
• ( \alpha )：探索与奖励之间的平衡因子

🔍 二、SAC 的结构组成

SAC 有四个主要模块（四个网络）：

模块	作用
Actor（策略网络）	输出当前状态下的概率分布 ( \pi(a
Q1, Q2 网络（Critic）	近似动作值函数 ( Q(s, a) )，使用双 Q 避免过高估计
Value 网络（可选）	估计当前状态下的“soft value” ( V(s) )（新版可省略）

训练使用目标网络（target Q）、经验池（replay buffer）等稳定手段。

四、SAC 的优点

特点	描述
✅ off-policy	能重复使用旧数据，训练效率高
✅ 连续动作空间	可用于机器人控制等场景
✅ 探索性强	保留策略熵，鼓励更多尝试
✅ 收敛稳定	双 Q + target network 提升稳定性
✅ 可扩展	可结合可微分安全层（如前文 RCBF）

关于 “在线 / 离线”强化学习 与 “on-policy / off-policy”

比较点	在线 / 离线	On-policy / Off-policy
关注的是？	数据是否来自当前环境？	样本是否由当前策略生成？
数据是否更新？	在线更新 / 离线固定	都可以实时或离线训练
能否与环境交互？	离线不能，在线可以	无限制，on/off-policy 都可在线训练
是否用 replay buffer？	不一定（视方法而定）	Off-policy 用，On-policy 通常不用
安全应用偏好？	离线更安全	与安全性无直接关系

---

《Learning Safety in Model-Based Reinforcement Learning using MPC and Gaussian Processes》

提出了一种结合 高斯过程（GP）与模型预测控制（MPC） 的 安全强化学习（Safe RL）方法。它的研究目标是在不牺牲性能的前提下，提高策略的安全性。

我将从三个层面来解析：

一、文章的核心创新点分析

1. 使用 GP 回归学习 MPC 参数空间中的安全集

• 提出了一个 用高斯过程回归建模安全约束 的方法，不直接建模系统动力学，而是建模“哪个 MPC 参数组合是安全的”。
• 安全性不再是手工设定的硬约束，而是通过历史数据学习的概率安全区域 SD\mathcal{S}_D。

👉 创新点：这是将黑箱约束建模方法用于 MPC-RL 中安全约束建模的首个系统性应用。

2. 将 MPC 作为 RL 的函数近似器

• 与传统 RL 使用神经网络不同，该文使用 参数化的 MPC 控制器作为策略表示器。
• 学习的不是控制策略本身，而是 MPC 的参数（如模型参数、约束backoff等）。

👉 创新点：相比 DNN 更可解释，且 MPC 本身具有对约束的原生支持。

3. 提出基于 GP 的安全约束嵌入到策略更新优化问题中

• 在 RL 参数更新时，加入 GP 学习的安全约束 z(θ)≤0z(\theta) \leq 0，确保新参数落在安全区域。
• 若更新不可行，则逐步 backtrack 安全概率 β，提升可行性。

👉 创新点：结合了 概率安全约束 + 二阶 Q-learning + backtracking β，实现数据驱动的安全探索。

总结

类别	内容
核心创新	基于 GP 的安全参数集建模，嵌入 MPC-RL 训练过程
关键技术点	安全约束函数建模、LSTD Q-learning、贝叶斯优化启发式
可改进之处	GP 初期性能差、高维扩展难、参数空间学习有限
潜在创新方向	多模型融合、迁移学习、安全 BO 比对、真实机器人部署

---

《Constrained Variational Policy Optimization for Safe Reinforcement Learning》

一、研究背景与问题定义

1.1 安全强化学习的挑战

• 核心问题：在安全关键场景（如机器人控制）中，策略需在最大化奖励的同时满足安全约束（如碰撞避免）。
• 传统方法的局限：
  • 原始-对偶方法（Primal-Dual）：交替优化策略参数和对偶变量（拉格朗日乘子），存在数值不稳定性和缺乏最优性保证。 (核心思想：把约束优化问题转换为拉格朗日形式，交替优化策略参数（θ）和对偶变量（λ），通过迭代更新使策略参数和对偶变量同时收敛。)
  • 泰勒近似方法：通过低阶近似简化约束优化问题，但可能导致约束违反率较高，近似不够精确，可能“形式上满足约束”，但实际策略存在约束违反。 （核心思想：对约束函数进行一阶或二阶泰勒展开，简化约束优化的计算）
  • 样本效率低：现有方法多为在线策略（on-policy），难以利用历史数据进行高效学习。

1.2 约束马尔可夫决策过程（CMDP）

• 定义：在标准MDP基础上引入约束成本函数集合 (C)，目标为：

\pi^* = \arg \max_{\pi} J_r(\pi) \quad \text{s.t.} \quad J_c(\pi) \leq \epsilon_1

其中 (J_r(\pi)) 和 (J_c(\pi)) 分别为奖励和成本的累积折扣期望（比如摔倒、电量消耗、与人碰撞等）。

二、方法创新：CVPO算法

2.1 概率推断视角的重新建模

• 关键思想：将安全RL问题转化为概率推断问题，通过变分推断和EM算法自然融入约束。
• 概率图模型：引入最优性变量 (O)，表示轨迹 (\tau) 的“成功事件”，其似然函数与奖励相关：

p(O=1|\tau) \propto \exp\left(\sum_t \gamma^t r_t / \alpha\right)

• 证据下界（ELBO）：通过变分分布 (q(\tau)) 最大化下界，同时约束 (q) 属于安全分布族 (\Pi_{Q}^{\epsilon_1})。

2.2 算法框架：EM两步优化
E步：凸优化求解变分分布

• 优化目标：在约束下最大化奖励期望，同时限制与旧策略的KL散度：

\max_q \mathbb{E}{\rho_q} \left[ \mathbb{E}{q} [Q_r(s,a)] \right] \quad \text{s.t.} \quad \mathbb{E}{\rho_q} [\mathbb{E}q [Q_c(s,a)]] \leq \epsilon_1, \quad D{KL}(q | \pi{\theta_i}) \leq \epsilon_2

• 闭式解与强对偶性：
  • 通过拉格朗日对偶方法解析求解，得到非参数变分分布的闭式表达式：

q^*(a|s) \propto \pi_{\theta_i}(a|s) \exp\left( \frac{Q_r(s,a) - \lambda Q_c(s,a)}{\alpha} \right)

• 证明优化问题的严格凸性，确保解的唯一性和最优性。

M步：监督学习更新策略

• 目标函数：在信任域内最小化与最优变分分布的KL散度：

\min_{\theta} \mathbb{E}{\rho_q} \left[ D{KL}(q^*(\cdot|s) | \pi_{\theta}(\cdot|s)) \right]

• 实现方式：通过监督学习（如神经网络）拟合 (q^*)，支持离线策略（off-policy）数据复用。

三、理论贡献与优势

3.1 理论保证

1. 最优性与可行性：E步的凸优化确保变分分布同时满足奖励最大化和约束条件。
2. 信任域机制：M步的KL约束提供策略更新的鲁棒性保证，避免过度偏离安全区域。
3. 样本效率：离线策略训练减少与环境交互的成本，实验显示样本效率提升高达1000倍。

3.2 对现有方法的改进

• 稳定性：分解为凸优化和监督学习，避免原始-对偶方法的梯度冲突。
• 约束满足性：直接优化安全约束，而非通过惩罚项近似。
• 灵活性：非参数变分分布避免参数化策略的近似误差。

此笔记记录学习书本 机器人学导论（原书第3版）（美）HLHN J.CRAIG，著贠超等译的学习与总结笔记，其中部分内容由AI生成。

---

✅ 1. 雅可比矩阵是啥？

在多变量函数中，雅可比矩阵（Jacobian Matrix）是所有一阶偏导数组成的矩阵：

如果有函数：

$$ \mathbf{y} = f(\mathbf{x}) \quad \text{其中 } \mathbf{x} \in \mathbb{R}^n, \mathbf{y} \in \mathbb{R}^m $$

则雅可比矩阵为：

$$ J = \frac{\partial \mathbf{y}}{\partial \mathbf{x}} = \begin{bmatrix} \frac{\partial y_1}{\partial x_1} & \cdots & \frac{\partial y_1}{\partial x_n} \\ $$

\vdots & \ddots & \vdots \

$$ \frac{\partial y_m}{\partial x_1} & \cdots & \frac{\partial y_m}{\partial x_n} \end{bmatrix} $$

在机器人中：

• ( \mathbf{x} ) 是关节角度（如 ( \theta_1, \theta_2, \ldots )）
• ( \mathbf{y} ) 是末端执行器的位置或速度

---

✅ 2. 雅可比逆矩阵是啥？

当我们想从输出空间（如末端速度）反推输入空间（如关节速度）时，需要“反过来”用雅可比矩阵，这时候我们就需要**“逆”雅可比矩阵**。

• 如果雅可比是方阵（m = n） 且满秩，直接求逆：

$$ \dot{\mathbf{x}} = J^{-1} \dot{\mathbf{y}} $$

• 如果是非方阵（m ≠ n）或者不可逆，就用广义逆（伪逆）：
  • 对于 冗余系统（n > m）：

$$ \dot{\mathbf{x}} = J^\top (JJ^\top)^{-1} \dot{\mathbf{y}} $$

• 对于 欠驱动系统（n < m）：

$$ \dot{\mathbf{x}} = (J^\top J)^{-1} J^\top \dot{\mathbf{y}} $$

• 或用 Moore-Penrose 伪逆：

$$ \dot{\mathbf{x}} = J^+ \dot{\mathbf{y}} $$

---

✅ 3. 举个简单例子（机器人）

假设一个二维机械臂（2个关节）要控制末端点 ( x, y ) 的速度。

1. 正向速度：( \dot{\mathbf{y}} = J \dot{\theta} )
2. 反向求关节速度（逆问题）：( \dot{\theta} = J^{-1} \dot{\mathbf{y}} )，若不能求逆，就用广义逆！

---

雅可比矩阵和机器人运动学之间的关系非常密切，尤其是在速度运动学中，雅可比矩阵起到了桥梁作用。下面我帮你分层讲清楚这个关系。

---

✅ 一、什么是运动学？

机器人运动学分为两大类：

类型	说明
正运动学（FK）	给定关节角度，求末端位姿（位置和方向）
逆运动学（IK）	给定末端位姿，求关节角度（复杂）

还有一个细分方向叫：

🚀 速度运动学：给定关节速度，求末端速度（或者反过来）

---

✅ 二、雅可比矩阵出现在速度运动学中

设：

• ( \boldsymbol{q} )：关节变量（角度、位移等）
• ( \dot{\boldsymbol{q}} )：关节速度
• ( \mathbf{x} )：末端位姿（通常是位置）
• ( \dot{\mathbf{x}} )：末端速度（线速度 + 角速度）

通过雅可比矩阵 ( J(\boldsymbol{q}) )，有以下关系：

$$ \dot{\mathbf{x}} = J(\boldsymbol{q}) \cdot \dot{\boldsymbol{q}} $$

这就是速度映射关系：关节空间 → 笛卡尔空间。

---

✅ 三、雅可比矩阵在运动学中承担什么角色？

用法	作用
正运动学的导数	把 FK 对关节变量求导，就得到了雅可比矩阵
末端速度计算	通过雅可比矩阵从 ( \dot{q} ) 得到 ( \dot{x} )
逆运动学辅助工具	用雅可比矩阵的伪逆求解逆运动学
奇异性检测	雅可比矩阵行列式为0的地方就是“奇异点”
力-力矩传递	( \tau = J^\top \cdot F )，关节力矩 ← 末端力

---

✅ 四、简单举个例子（2关节平面机械臂）

$$ 设两段长度分别为 \( l_1, l_2 \)，角度为 \( \theta_1, \theta_2 \) $$

末端位置为：

$$ \begin{aligned} x &= l_1 \cos \theta_1 + l_2 \cos (\theta_1 + \theta_2) \\ y &= l_1 \sin \theta_1 + l_2 \sin (\theta_1 + \theta_2) \end{aligned} $$

求导得雅可比矩阵：

$$ J = \frac{\partial(x, y)}{\partial(\theta_1, \theta_2)} = \begin{bmatrix} $$

• l_1 \sin \theta_1 - l_2 \sin(\theta_1 + \theta_2) & -l_2 \sin(\theta_1 + \theta_2) \

$$ l_1 \cos \theta_1 + l_2 \cos(\theta_1 + \theta_2) & l_2 \cos(\theta_1 + \theta_2) \end{bmatrix} $$

这样，就可以实现：

$$ \dot{x}, \dot{y} = J \cdot \dot{\theta} $$

也可以反过来：

$$ \dot{\theta} = J^+ \cdot \dot{x} $$

---

• 规划方式分为两种，分别是笛卡尔空间的规划（位置规划）和关节空间的规划（角度规划）

B-Spline（Basis Spline，基样条）

是一种强大而灵活的曲线拟合方法，特别适用于轨迹规划、计算机图形学、机器人路径平滑等场景。下面我们从直观原理、数学构成、实现步骤三个角度来解释 B-Spline 是怎么实现的。

---

B-Spline 是将一段复杂曲线拆成若干小段，每段用低阶（通常是三次）多项式表示，多个小段在控制点处光滑地拼接起来。它的主要特点：

• 局部控制性强：修改一个控制点只影响局部。
• 光滑性强：曲线的低阶导数连续（例如 3 次 B-Spline 有 2 阶导数连续）。
• 比普通样条更灵活：通过节点向量控制曲线形状和连接方式。

B-Spline 曲线的数学表达形式是：

$$ C(t) = \sum_{i=0}^{n} N_{i,k}(t) \cdot P_i $$

其中：

• ( P_i )：控制点
• ( N_{i,k}(t) )：B样条基函数，阶数为 (k)（例如 (k=4) 表示三次）
• ( t )：参数
• ( N_{i,k}(t) ) 是通过递归定义的：

$$ N_{i,1}(t) = \begin{cases} $$

1, & t_i \le t < t_{i+1} \

$$ 0, & \text{otherwise} \end{cases} $$ $$ N_{i,k}(t) = \frac{t - t_i}{t_{i+k-1} - t_i} N_{i,k-1}(t) + \frac{t_{i+k} - t}{t_{i+k} - t_{i+1}} N_{i+1,k-1}(t) $$

此笔记记录学习书本**王树森，张志华，《深度强化学习（初稿》**的学习与总结笔记，其中部分内容由AI生成。

Q学习产生高估（overestimation）的原因，主要源于其最大值操作（max operator）在引入噪声时的偏差。这种现象通常称为 最大值偏差（maximization bias）。

原理解释

在 Q-learning 中，我们使用以下目标来更新 Q 值：

$$ Q(s, a) \leftarrow Q(s, a) + \alpha \left[ r + \gamma \cdot \max_{a'} Q(s', a') - Q(s, a) \right] $$

其中：

• (\max_{a’} Q(s’, a’))：从下一个状态中选择估计值最大的动作作为更新目标。

⚠️ 问题在于：
如果 Q 值本身带有估计误差（例如由于采样、函数逼近、探索策略等），
那么取最大值操作会偏向选择被高估的值，从而使得目标值系统性偏高。

---

举个例子（简化）

假设在状态 (s’) 下，我们有三个动作，真实 Q 值如下：

动作 (a’)	真正的 Q 值	网络估计值（含误差）
(a_1)	1.0	1.1（+0.1）
(a_2)	0.8	1.0（+0.2）
(a_3)	0.6	0.9（+0.3）

• 虽然 (a_1) 是最优动作（真实 Q 值最大），
• 但由于随机估计误差，(\max Q(s’, a’) = 1.1)，这个值偏高了。

这会导致我们在训练时错误地使用偏高的目标值去更新当前状态的 Q 值，从而 累计越来越大的高估偏差。

---

高估有什么危害？

• 策略不稳定：错误地偏向那些“看起来收益高”但其实不是的动作；
• 训练发散：目标 Q 值越来越不准确，导致训练无法收敛；
• 性能下降：测试表现可能远低于理论最优策略。

---

如何缓解高估？

1. Double Q-Learning：
   • 采用两个 Q 网络，分别用于选择和评估动作。
   • 更新目标变为：

$$ Q(s, a) \leftarrow r + \gamma \cdot Q_2(s', \arg\max_{a'} Q_1(s', a')) $$

• 显著降低高估。

2. Averaged DQN / Ensemble 方法：

   • 使用多个 Q 网络求平均，减少单个网络带来的噪声偏差。

3. Regularization / Uncertainty penalty：

   • 对 Q 值添加不确定性惩罚，抑制过高的估计。

---

行为策略和目标策略的区别

例1：Q-learning（离策略）

• 行为策略：ε-greedy（偶尔探索）
• 目标策略：贪婪策略（永远选最大Q值）

这是典型的 离策略学习（off-policy） —— 用一种策略去收集数据，用另一种策略去学习。

例2：SARSA（在策略）

• 行为策略：ε-greedy
• 目标策略：同样是ε-greedy

这是 在策略学习（on-policy） —— 你用什么策略学习，就用它去探索和更新。

行为策略 ≠ 目标策略：

• 你可以用 更大胆、更多样的行为策略 去探索，避免陷入局部最优
• 然后用行为数据，训练一个 更保守、更稳定的目标策略

这就是 DDPG、TD3、DQN 等算法的本质思路。

---

“双延时确定策略梯度”（Twin Delayed Deep Deterministic Policy Gradient，TD3）解决强化学习中 自举 + 最大化引起的 Q 值高估问题

---

我们分三步来讲清楚：

🔥 1. 问题背景：自举 + 最大化 导致的高估偏差

在像 DDPG、Q-learning 这类算法中，我们更新 Q 值时用的是这个形式：

$$ y = r + \gamma \cdot \max_{a'} Q(s', a') $$

但注意！

• ( Q(s’, a’) ) 是网络的估计值
• “最大化”这个估计值（选最大 Q）会放大噪声和误差！

🔍 这就像你考试估分，题做错了但还选“我觉得我做得最好的那题”，会高估总分
→ 随着训练进行，Q值越估越高，结果策略学得不靠谱！

---

🧠 2. TD3 怎么解决这个高估问题？

TD3 提出了三个关键改进，其中双Q网络是最核心的👇

---

✅ 【方法一】双Q网络（Twin Q-networks）

用两个独立的 Q 网络： ( Q_1(s, a) )、( Q_2(s, a) )

更新 TD目标时，不再用 max，而是：

$$ y = r + \gamma \cdot \min\left(Q_1(s', a'),\ Q_2(s', a')\right) $$

✅ 取较小值，就能抵消高估误差（保守估计）
✅ 这不是自举去最大，而是自举去最小，避免因误差叠加而爆炸

✅ 【方法二】延迟策略更新（Delayed Policy Update）

• 策略网络（Actor）更新得更慢，例如每 2～3 次 Critic 更新后才更新一次
• 因为 Critic（Q函数）学得更稳定时，再去学策略，效果更稳

✅ 【方法三】目标动作加噪声（Target Policy Smoothing）

• 为了防止 Q 网络过于依赖某一个精确动作值
• 在 TD目标里加入一个小的随机噪声：

$$ a' = \pi_{\text{target}}(s') + \epsilon,\quad \epsilon \sim \text{clip}(\mathcal{N}(0, \sigma), -c, c) $$

✅ 这样做可以使目标 Q 更平滑，缓解尖峰高估

---