4. 状态空间设计：关节角度、角速度、末端执行器位姿、接触力信息的选取与归一化

状态空间设计，说白了就是决定「机器人该看什么、怎么看」。

我刚开始做力矩控制那会儿，总觉得状态越多越好。恨不得把电机电流、温度、甚至螺丝拧紧力矩都塞进去。结果呢？训练半天不收敛，模型还特别容易过拟合。后来我才明白——状态空间不是做加法，是做减法。

这一章，我们就聊聊关节角度、角速度、末端位姿和接触力这四类核心信息。怎么选、怎么处理、怎么归一化，我会把踩过的坑都告诉你。

4.1 核心状态变量：选什么？为什么？

强化学习的状态空间，必须满足两个条件：可观测和充分性。可观测就是传感器能拿到；充分性就是这些信息能唯一确定当前系统状态。

对于力矩控制的机器人，我一般选这四类：

状态类别	典型维度	传感器来源	为什么需要
关节角度	6~7 (每个关节1维)	编码器、电位计	确定机器人构型
关节角速度	6~7 (每个关节1维)	编码器差分/陀螺仪	提供运动趋势
末端执行器位姿	6 (位置3+姿态3)	正向运动学/视觉	任务空间约束
接触力/力矩	6 (力3+力矩3)	六维力传感器	交互感知核心

4.2 关节角度与角速度：最基础也最容易出错

关节角度是状态空间的「地基」。但这里有个坑——角度周期性。

你想想看，0° 和 360° 在物理上是同一个位置，但如果直接输入网络，模型会认为它们是两个完全不同的状态。这会导致策略在边界处剧烈抖动。

正确的做法是：将角度映射到 [-π, π] 或 [0, 2π] 区间。更稳妥的方案是用 sin/cos 编码：

# 角度归一化示例
import numpy as np

def normalize_joint_angles(q):
    """
    q: 原始关节角度 (numpy array)
    返回: sin/cos 编码后的向量
    """
    q_norm = np.mod(q + np.pi, 2 * np.pi) - np.pi  # 映射到 [-π, π]
    sin_q = np.sin(q_norm)
    cos_q = np.cos(q_norm)
    return np.concatenate([sin_q, cos_q])

角速度的处理相对简单。直接用低通滤波后的差分值就行。我习惯用截止频率 5~10 Hz 的一阶低通，能有效滤掉编码器量化噪声。

4.3 末端执行器位姿：任务空间的「眼睛」

末端位姿告诉机器人「我的手在哪儿、朝哪个方向」。对于装配、打磨这类任务，这是必须的。

位置信息（x, y, z）直接用米为单位，归一化到 [-1, 1] 即可。但姿态信息就有点讲究了。

常见的姿态表示方式有三种：

• 欧拉角：直观，但有万向锁问题。我不推荐。
• 旋转矩阵：9 个参数，冗余度高，浪费维度。
• 四元数：4 个参数，无奇异性。我个人最常用。

四元数归一化也很简单：

def normalize_quaternion(q):
    """
    q: [w, x, y, z]
    返回: 单位四元数
    """
    norm = np.linalg.norm(q)
    if norm < 1e-6:
        return np.array([1.0, 0.0, 0.0, 0.0])
    return q / norm

4.4 接触力信息：力矩控制的核心「触觉」

接触力是力矩控制区别于位置控制的关键。没有力反馈，你就是在「盲操作」。

六维力传感器通常输出：Fx, Fy, Fz, Mx, My, Mz。但直接使用原始值会有两个问题：

1. 量纲差异大：力可能是几十牛，力矩可能是几牛米。直接拼在一起，网络会偏向大力值。
2. 噪声敏感：力传感器的高频噪声会误导策略。

我的处理流程是这样的：

def process_force_data(force_raw, torque_raw, force_range, torque_range):
    """
    force_raw: 原始力 (3,)
    torque_raw: 原始力矩 (3,)
    force_range: 力范围 [min, max]
    torque_range: 力矩范围 [min, max]
    """
    # 1. 低通滤波 (这里用简单移动平均示意)
    force_filtered = low_pass_filter(force_raw, cutoff=8.0)
    torque_filtered = low_pass_filter(torque_raw, cutoff=8.0)
    
    # 2. 归一化到 [-1, 1]
    force_norm = 2.0 * (force_filtered - force_range[0]) / (force_range[1] - force_range[0]) - 1.0
    torque_norm = 2.0 * (torque_filtered - torque_range[0]) / (torque_range[1] - torque_range[0]) - 1.0
    
    return np.concatenate([force_norm, torque_norm])

4.5 归一化策略：让所有状态「平起平坐」

归一化不是简单的数学变换，它直接影响训练稳定性。我总结了一套「三步归一化法」：

步骤	操作	目的
1. 范围标定	记录每个状态的实际物理范围	确定上下界
2. 线性映射	将范围映射到 [-1, 1] 或 [0, 1]	统一量纲
3. 异常截断	超出范围的值截断到边界	防止极端值破坏训练

具体到每种状态，我建议的范围如下：

• 关节角度：sin/cos 编码后自然在 [-1, 1]
• 关节角速度：[-1.2 * ω_max, 1.2 * ω_max] 映射到 [-1, 1]
• 末端位置：工作空间边界映射到 [-1, 1]
• 末端姿态：四元数本身就是单位向量，无需额外映射
• 接触力/力矩：根据传感器量程映射到 [-1, 1]

4.6 状态空间设计总览

说了这么多，我们画一张图来总结一下。这张图展示了从原始传感器数据到最终状态向量的完整流程：

这张图把整个流程串起来了。从传感器原始数据，经过预处理、归一化，最终拼成一个 20~30 维的状态向量。每一步都有讲究，少一步都可能出问题。

4.7 实战建议：从仿真到真机

最后，分享几个我在实际项目中总结的经验：

• 仿真阶段：状态空间可以稍微大一点（30 维左右），方便探索。但真机部署前，一定要做状态剪枝，去掉那些对策略贡献不大的维度。
• 归一化参数要固化：训练完成后，把归一化的 min/max 值保存下来。真机运行时直接用这些参数，不要重新计算。否则环境变了，策略就废了。
• 加一点随机噪声：训练时在状态上加少量高斯噪声（σ=0.01 左右），能提高策略的鲁棒性。真机传感器都有噪声，提前适应一下没坏处。