4. 策略表示与函数逼近
做机器人控制，有个很朴素的道理：你测不到的东西，你就控不了。
我记得刚入行那会儿，带我的老工程师跟我说过一句话，我一直记着——「别急着写控制器，先搞清楚你的传感器能给你什么，不能给你什么。」后来我在好几个项目里都吃了这个亏，上来就调PID，结果关节抖得跟筛子似的，查了半天才发现是速度信号噪声太大。
这一章，我们就来聊聊状态观测这件事。说白了，就是搞清楚仿真环境里到底能读到哪些数据，这些数据长什么样，怎么把它们拿到手，以及——哪些数据是直接能用的，哪些需要你动点手脚。
4.1 状态空间：你到底需要知道什么？
先问一个问题：要控制一个机械臂的关节，最少需要知道哪些信息？
你想想看，如果我只告诉你「当前关节角度是30度」，你能让这个关节稳稳停在30度吗？能，但仅限于静态。一旦要让关节动起来，比如从30度转到60度，你就需要知道它转得有多快——也就是角速度。如果再想控制得精细一点，比如让它在目标位置平稳停下来不抖动，那加速度信息也很有用。
所以，一个关节的「状态」，通常由三部分组成：
- 关节角度 (q) —— 位置信息，单位 rad
- 关节角速度 (q̇) —— 速度信息，单位 rad/s
- 关节角加速度 (q̈) —— 加速度信息，单位 rad/s²
对于一个 n 自由度的机械臂，完整的状态向量就是：
状态向量 s = [q₁, q₂, ..., qₙ, q̇₁, q̇₂, ..., q̇ₙ, q̈₁, q̈₂, ..., q̈ₙ]维度是 3n。6轴机械臂就是18维，7轴就是21维。嗯，这里要注意——维度越高，你的策略网络要学的映射关系就越复杂，训练难度也越大。我在做双臂协作项目时，14个关节42维状态，训练收敛速度明显比单臂慢了一大截。
核心原则：状态空间的设计，本质上是在「信息完整性」和「学习难度」之间做权衡。能少用一维，就少用一维。
4.2 仿真环境里能读到什么？
不同的仿真器，API接口不一样，但能拿到的数据大同小异。我用过MuJoCo、PyBullet、Isaac Gym，这里以最常见的PyBullet为例，给你看看实际怎么读数据。
4.2.1 关节状态API
PyBullet里获取关节状态，核心函数就一个：
import pybullet as p
# 假设已经连接了仿真器并加载了机器人模型
# robot_id 是机器人的唯一标识
# 获取所有关节的状态
joint_states = p.getJointStates(robot_id, range(num_joints))
# 每个关节状态是一个元组，包含：
# joint_position : 关节角度 (rad)
# joint_velocity : 关节角速度 (rad/s)
# joint_reaction_forces : 关节反作用力 (一般用不到)
# applied_joint_motor_torque : 电机施加的力矩我习惯这样拆包：
positions = []
velocities = []
for i in range(num_joints):
state = joint_states[i]
pos = state[0] # 角度
vel = state[1] # 速度
positions.append(pos)
velocities.append(vel)个人经验：别一次性把所有关节状态都塞进一个变量里。我早期写代码图省事，直接存了个大列表，结果后面调试时根本分不清哪个数据对应哪个关节。建议用字典或者命名元组，可读性会好很多。
4.2.2 加速度怎么拿？
这里有个坑——大部分仿真器不直接提供关节加速度。
PyBullet的getJointStates返回的数据里，没有加速度这一项。MuJoCo的mjData里虽然有qacc，但那是在正向动力学计算后才更新的，不是每个仿真步都默认可用。
怎么办？两种办法：
- 数值微分：用速度差分近似加速度。简单粗暴，但噪声大。
- 状态观测器：用卡尔曼滤波或龙伯格观测器估计。精度高，但实现复杂。
对于强化学习入门阶段，数值微分完全够用：
# 保存上一时刻的速度
prev_velocities = [0.0] * num_joints
def get_accelerations(current_velocities, dt):
global prev_velocities
acc = [(v - pv) / dt for v, pv in zip(current_velocities, prev_velocities)]
prev_velocities = current_velocities.copy()
return acc我曾经踩过的坑：仿真步长dt设得太小（比如1e-5秒），数值微分出来的加速度噪声极大，直接导致策略训练发散。后来我加了低通滤波才稳住。建议dt至少1e-3秒，或者对加速度做滑动平均。
4.3 状态数据的记录与可视化
光能读到数据还不够，你得能把它记下来、看出来。我见过太多同学训练了半天，策略不收敛，却拿不出任何数据来分析原因。
4.3.1 记录到CSV文件
最简单的做法，每步仿真把状态数据追加到列表，训练结束后一次性写入文件：
import csv
# 记录容器
state_log = []
def log_state(step, positions, velocities, accelerations):
row = [step] + positions + velocities + accelerations
state_log.append(row)
# 训练结束后写入
def save_log(filename="state_log.csv"):
with open(filename, 'w', newline='') as f:
writer = csv.writer(f)
# 写表头
header = ['step']
for i in range(num_joints):
header.append(f'q{i}')
for i in range(num_joints):
header.append(f'dq{i}')
for i in range(num_joints):
header.append(f'ddq{i}')
writer.writerow(header)
writer.writerows(state_log)这样记录下来的数据，直接用Excel或者Python的pandas就能打开分析。我个人习惯在训练过程中每1000步存一次中间结果，防止训练崩了前面的数据全丢。
4.3.2 实时打印调试
训练时实时打印所有关节状态会刷屏，我一般只打印关键信息：
def print_status(step, positions, velocities):
# 只打印第一个关节的状态
print(f"Step {step:6d} | q0: {positions[0]:.4f} rad | dq0: {velocities[0]:.4f} rad/s")或者打印所有关节的均值和极值，一眼就能看出有没有异常：
def print_summary(step, positions, velocities):
pos_mean = sum(positions) / len(positions)
vel_max = max(abs(v) for v in velocities)
print(f"Step {step:6d} | pos_mean: {pos_mean:.4f} | vel_max: {vel_max:.4f}")4.4 状态观测的完整流程
把上面这些串起来，一个完整的状态观测流程大概是这样的：
标准流程：
- 仿真步进，更新物理引擎
- 调用getJointStates获取关节角度和速度
- 数值微分或观测器计算加速度
- 组装状态向量（角度 + 速度 + 加速度）
- 记录到日志（可选）
- 传递给策略网络或控制器
下面这张图把这个流程画清楚了：
4.5 常见问题与避坑
| 问题 | 现象 | 原因 | 解决办法 |
|---|---|---|---|
| 角度跳变 | 角度值突然从π跳到-π | 角度周期性归零 | 使用连续角度表示，或做unwrap处理 |
| 速度噪声大 | 策略训练不稳定 | 仿真步长太小或传感器噪声 | 加低通滤波，或增大dt |
| 加速度发散 | 数值微分结果异常大 | 速度突变或dt不均匀 | 固定仿真步长，或使用观测器 |
| 数据记录丢失 | 训练崩了没日志可查 | 只在训练结束时写一次文件 | 每N步写一次中间结果 |
一个小技巧：如果你发现训练过程中状态数据突然出现NaN，别慌。先检查是不是仿真器重置后关节索引变了。我遇到过好几次，加载不同模型时关节顺序不一样，导致数据全对不上。
4.6 本章小结
状态观测这件事，说难不难，说简单也不简单。核心就三点：
- 知道要什么：角度、速度、加速度，缺哪个都不行
- 知道怎么拿：仿真API怎么调，加速度怎么补
- 知道怎么存：日志记录要可靠，调试信息要可读
把这些基础打牢了，后面写策略网络、调奖励函数的时候，你才能把精力放在真正的问题上，而不是被数据问题绊住脚。