2. 基于PD控制器的站立稳定性分析
任务目标
四足机器人站立时，关节的刚度与阻尼直接决定其抵抗外界扰动的能力。本案例将设计一个PD控制器，分别控制四条腿的髋关节与膝关节，通过调节比例增益 \(K_p\) 与微分增益 \(K_d\)，观察机器人质心在受到水平推力后的位移恢复情况，以此评估站立稳定性。
环境与依赖
使用Python 3.8+，仅依赖NumPy与Matplotlib（用于可视化）。无需仿真引擎，采用简化平面动力学模型：机器人躯干视为质点，每条腿简化为弹簧-阻尼系统，忽略腿部质量与地面滑动。
核心实现
以下代码实现了站立稳定性的核心仿真：
```python import numpy as np class QuadrupedStandingPD: def __init__(self, Kp=800.0, Kd=50.0, mass=8.0, leg_rest_len=0.3): self.Kp = Kp # 关节刚度 N/m self.Kd = Kd # 关节阻尼 N·s/m self.m = mass # 躯干质量 kg self.L0 = leg_rest_len # 腿的等效自然长度 m self.g = 9.81 # 四条腿的足端位置（躯干坐标系，前后左右对称） self.foot_pos = np.array([ [0.2, 0.15], # 左前 [0.2, -0.15], # 右前 [-0.2, 0.15], # 左后 [-0.2, -0.15] # 右后 ]) # 状态: [x, z, vx, vz] 躯干质心位置与速度 self.state = np.array([0.0, self.L0, 0.0, 0.0]) self.dt = 0.001 self.time = 0.0 def _leg_force(self, x, z): """计算四条腿对躯干的总作用力""" force = np.zeros(2) for fx, fy in self.foot_pos: # 腿向量（从足端指向躯干） dx = x - fx dz = z - fy current_len = np.hypot(dx, dz) if current_len < 1e-6: continue # 方向单位向量 dir_vec = np.array([dx, dz]) / current_len # 弹簧力（压缩为正）与阻尼力（沿腿方向速度分量） delta_len = current_len - self.L0 vel_proj = (self.state[2]*dx + self.state[3]*dz) / current_len spring_force = -self.Kp * delta_len damp_force = -self.Kd * vel_proj force += dir_vec * (spring_force + damp_force) # 加重力 force[1] -= self.m * self.g return force def step(self): """单步欧拉积分""" x, z, vx, vz = self.state fx, fz = self._leg_force(x, z) ax = fx / self.m az = fz / self.m self.state[0] += vx * self.dt self.state[1] += vz * self.dt self.state[2] += ax * self.dt self.state[3] += az * self.dt self.time += self.dt def simulate(self, duration=2.0, disturbance=50.0): """施加水平推力并仿真""" # 初始时刻施加水平冲量（推力持续0.05s） impulse_time = 0.05 while self.time < impulse_time: self.state[2] += (disturbance / self.m) * self.dt self.step() while self.time < duration: self.step() return self.state.copy() def reset(self): self.state = np.array([0.0, self.L0, 0.0, 0.0]) self.time = 0.0 def evaluate_stability(Kp, Kd, duration=2.0): """返回扰动后质心最大水平偏移与最终残余偏移""" robot = QuadrupedStandingPD(Kp=Kp, Kd=Kd) robot.reset() # 记录整个过程的水平位移 x_history = [] disturbance = 50.0 # 水平冲量 N·s impulse_time = 0.05 while robot.time < impulse_time: robot.state[2] += (disturbance / robot.m) * robot.dt robot.step() x_history.append(robot.state[0]) while robot.time < duration: robot.step() x_history.append(robot.state[0]) max_dev = max(abs(x) for x in x_history) final_dev = abs(x_history[-1]) return max_dev, final_dev if __name__ == "__main__": # 参数扫描示例 param_sets = [ (400, 20), # 低刚度低阻尼 (800, 50), # 中等参数 (1500, 120), # 高刚度高阻尼 (800, 5) # 高刚度低阻尼 ] print("Kp Kd 最大偏移(m) 残余偏移(m)") for kp, kd in param_sets: max_d, final_d = evaluate_stability(kp, kd) print(f"{kp:5d} {kd:5d} {max_d:.4f} {final_d:.4f}") ```运行与验证
运行方式：将上述代码保存为 standing_pd.py，在终端执行 python standing_pd.py。
验证方法：观察输出的参数扫描表。判定标准：
- 最大偏移越小，说明抵抗瞬时冲击能力越强；
- 残余偏移趋近于0（如小于0.005m），说明系统能恢复到初始站立位置；
- 对比不同 \(K_p\)、\(K_d\) 组合，可发现：\(K_p\) 过小会导致偏移过大甚至无法恢复；\(K_d\) 过小会引起振荡（残余偏移在0附近波动）；\(K_p\)、\(K_d\) 同时增大可显著减小最大偏移。
预期输出（示例）：当 \(K_p=800, K_d=50\) 时，最大偏移约0.02～0.03m，残余偏移小于0.002m；当 \(K_d=5\) 时，残余偏移会明显增大且出现振荡迹象。
参数影响分析
通过修改 param_sets 中的数值，可系统观察：
- 刚度 \(K_p\)：决定恢复力的强度。刚度过低时，重力与外力超过腿的支撑能力，机器人会倒塌；刚度过高时，微小位移即产生巨大恢复力，系统趋于刚性但可能引入数值不稳定。
- 阻尼 \(K_d\)：消耗振动能量。阻尼不足时，质心会在平衡位置附近来回振荡；阻尼过大时，恢复变慢但无超调。
判定标准：当残余偏移小于最大偏移的5%且无持续振荡时，认为该组参数满足站立稳定性要求。实际工程中，还需结合电机力矩极限与能耗选择折中参数。