当前位置：首页 > news >正文

mujoco 仿真导纳控制律

news 2025/10/26 15:44:16

mujoco 模型文件 admittance_sim.xml

<mujoco><option timestep="0.01" /><compiler angle="radian"/><worldbody><light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/><geom type="plane" size="1 1 0.1" rgba=".9 .9 .9 1" pos= "0 0 -0.1"/><body name="admittance_obj"><joint name="admittance_joint" type="slide" axis="1 0 0" damping="5" stiffness="10" range="-2 2"/><geom type="box" size="0.1 0.1 0.1" mass="1.0"/></body></worldbody><actuator><!-- 使用力控制器以便更好地实现导纳控制 --><motor name="actuator1" joint="admittance_joint" ctrlrange="-100 100" gear="1"/></actuator>
</mujoco>

导纳控制算法模拟

标准导纳控制模型实现了二阶导纳系统：Ma + Bv + K*x = F_ext
M_adm (导纳质量): 控制系统对外力的响应速度
B_adm (导纳阻尼): 抑制振荡，提供阻尼效果
K_adm (导纳刚度): 使系统回到期望位置的恢复力

双层控制
外环: 导纳控制器根据外力计算期望轨迹 (X_adm, V_adm)
内环: PID控制器跟踪导纳轨迹，驱动实际滑块运动

导纳控制使滑块表现出虚拟的质量-阻尼-弹簧特性：
受到外力时，滑块会"柔顺"地移动
外力消失后，滑块会平滑地返回期望位置
通过调整 M_adm、B_adm、K_adm 可以改变柔顺性

#coding=utf-8import mujoco
import numpy as np
import glfw
import time
import matplotlib.pyplot as pltMODEL_PATH = "admittance_sim.xml"def create_model():return mujoco.MjModel.from_xml_path(MODEL_PATH)def init_simulation():model = create_model()data = mujoco.MjData(model)data.qpos[0] = 0.0  # 初始位置return model, data# 导纳控制律 - 二阶系统模型
# 导纳模型: M*a + B*v + K*x = F_ext
# 其中 x 是相对于期望位置的偏差
def admittance_control(F_ext, X_des, X_adm_prev, V_adm_prev, M_adm, B_adm, K_adm, dt):"""导纳控制器参数:F_ext: 外部力X_des: 期望位置X_adm_prev: 上一时刻的导纳位置V_adm_prev: 上一时刻的导纳速度M_adm: 导纳质量B_adm: 导纳阻尼K_adm: 导纳刚度dt: 时间步长返回:X_adm: 新的导纳位置V_adm: 新的导纳速度"""# 计算位置偏差（相对于期望位置）x_error = X_adm_prev - X_des# 导纳动力学方程: M*a = F_ext - B*v - K*xacc = (F_ext - B_adm * V_adm_prev - K_adm * x_error) / M_adm# 积分更新速度和位置V_adm = V_adm_prev + acc * dtX_adm = X_adm_prev + V_adm * dtreturn X_adm, V_adm# 可视化初始化（保持不变）
def init_visualization(model):glfw.init()window = glfw.create_window(800, 600, "Admittance Control 1D", None, None)glfw.make_context_current(window)context = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_150.value)viewport = mujoco.MjrRect(0, 0, 800, 600)scene = mujoco.MjvScene(model, maxgeom=100)camera = mujoco.MjvCamera()camera.azimuth = 90.0camera.elevation = -30.0camera.distance = 2.5camera.lookat = np.array([0.0, 0.0, 0.0])return window, context, scene, camera, viewportdef force_fn(t):"""外部力函数 - 模拟不同阶段的外力输入"""if t < 1:return 0.0elif t < 2:return 15.0  # 正向推力elif t < 3:return 0.0elif t < 4:return -15.0  # 反向推力else:return 0.0def main():model, data = init_simulation()# 导纳模型参数 (虚拟质量-阻尼-弹簧系统)# ===== 参数调整建议 =====# 快速响应配置1 (推荐): M=0.8, B=15, K=150  -> 快速且稳定# 快速响应配置2 (激进): M=0.5, B=10, K=200  -> 最快但可能振荡# 平衡配置 (当前):      M=1.5, B=30, K=100  -> 稳定但较慢# 柔顺配置:             M=2.0, B=40, K=50   -> 柔软但很慢M_adm = 1.0      # 导纳质量 (↓减小加快响应)K_adm = 200.0    # 导纳刚度 (↑增大加快收敛)B_adm = 10 + 2 * np.sqrt(M_adm * K_adm)     # 导纳阻尼 (适中，临界阻尼≈2√(M*K)≈22)total_time = 6.0 # 总仿真时间dt = model.opt.timestepsteps = int(total_time / dt)X_des = 0.3  # 期望位置# 初始化导纳状态 - 从实际位置开始，避免初始振荡X_adm = data.qpos[0]  # 导纳位置初始化为当前实际位置（而非期望位置）V_adm = data.qvel[0]  # 导纳速度初始化为当前实际速度# PID控制器积分项integral_error = 0.0# 记录数据pos_history = []adm_history = []vel_history = []adm_vel_history = []force_history = []time_history = []window, context, scene, camera, viewport = init_visualization(model)print("=" * 60)print("启动导纳控制仿真...")print(f"导纳参数: M={M_adm}, B={B_adm}, K={K_adm}")print(f"期望位置: {X_des} m")print("=" * 60)# PID控制器参数 - 用于跟踪导纳轨迹kp_base = 500.0  # 基础位置增益ki_base = 100.0  # 基础积分增益kd_base = 1.0    # 基础速度增益for step in range(steps):if glfw.window_should_close(window):breakt = step * dt# 获取当前实际状态X_curr = data.qpos[0]V_curr = data.qvel[0]# 外部力输入F_ext = force_fn(t)# 导纳控制律 - 计算期望的导纳轨迹X_adm, V_adm = admittance_control(F_ext, X_des, X_adm, V_adm, M_adm, B_adm, K_adm, dt)# 增益渐变 - 前0.5秒逐渐增加增益，避免初始冲击ramp_time = 0.5  # 渐变时间if t < ramp_time:gain_factor = t / ramp_time  # 从0线性增加到1else:gain_factor = 1.0kp = kp_base * gain_factorki = ki_base * gain_factorkd = kd_base# 内环PID控制器 - 跟踪导纳轨迹# 控制力 = kp*e + ki*∫e + kd*ėpos_error = X_adm - X_currvel_error = V_adm - V_currintegral_error += pos_error * dt# 积分饱和限制，防止积分饱和integral_error = np.clip(integral_error, -3.0, 3.0)data.ctrl[0] = kp * pos_error + ki * integral_error + kd * vel_error# 打印关键信息if step % 50 == 0:print(f"t={t:.2f}s | F_ext={F_ext:6.2f}N | X_des={X_des:.3f}m | "f"X_adm={X_adm:.3f}m | X_curr={X_curr:.3f}m | "f"err={pos_error:.4f}m | gain={gain_factor:.2f}")# 仿真步进mujoco.mj_step(model, data)# 记录数据pos_history.append(X_curr)adm_history.append(X_adm)vel_history.append(V_curr)adm_vel_history.append(V_adm)force_history.append(F_ext * 0.02)  # 缩放以便显示time_history.append(t)# 更新可视化mujoco.mjv_updateScene(model, data, mujoco.MjvOption(), None, camera, mujoco.mjtCatBit.mjCAT_ALL, scene)mujoco.mjr_render(viewport, scene, context)glfw.swap_buffers(window)glfw.poll_events()time.sleep(dt / 1.0)glfw.terminate()print("=" * 60)print("仿真完成！正在生成结果图表...")print("=" * 60)# 绘制位置和力的图表fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 8))# 子图1: 位置跟踪ax1.plot(time_history, pos_history, 'b-', linewidth=2, label='actual position (X_curr)')ax1.plot(time_history, adm_history, 'r--', linewidth=2, label='admittance target (X_adm)')ax1.plot(time_history, [X_des]*len(time_history), 'g:', linewidth=2, label='desired position (X_des)')ax1.axhline(y=0, color='k', linestyle='-', alpha=0.3, linewidth=0.5)ax1.set_ylabel('position (m)', fontsize=12)ax1.set_title('admittance control - position tracking', fontsize=14, fontweight='bold')ax1.legend(loc='best', fontsize=10)ax1.grid(True, alpha=0.3)# 子图2: 速度和外力ax2_twin = ax2.twinx()ax2.plot(time_history, vel_history, 'b-', linewidth=2, label='actual velocity (V_curr)')ax2.plot(time_history, adm_vel_history, 'r--', linewidth=2, label='admittance velocity (V_adm)')ax2_twin.plot(time_history, [f/0.02 for f in force_history], 'orange', linewidth=2, label='external force (F_ext)')ax2.axhline(y=0, color='k', linestyle='-', alpha=0.3, linewidth=0.5)ax2.set_xlabel('time (s)', fontsize=12)ax2.set_ylabel('velocity (m/s)', fontsize=12, color='b')ax2_twin.set_ylabel('force (N)', fontsize=12, color='orange')ax2.tick_params(axis='y', labelcolor='b')ax2_twin.tick_params(axis='y', labelcolor='orange')ax2.set_title('velocity response and external force', fontsize=14, fontweight='bold')ax2.legend(loc='upper left', fontsize=10)ax2_twin.legend(loc='upper right', fontsize=10)ax2.grid(True, alpha=0.3)plt.tight_layout()plt.savefig('admittance_control_result.png', dpi=150)print("结果已保存至: admittance_control_result.png")plt.show()print("仿真成功完成！")if __name__ == "__main__":main()