✨ 长期致力于带机械臂四足机器人、全身控制、阻抗控制、自适应阻抗控制研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》
（1）全身动力学建模与二次规划分层优化控制：
将带6自由度机械臂的12自由度四足机器人（站姿）建模为浮动基座14刚体系统。使用拉格朗日法与选择矩阵推导整体动力学方程，其中选择矩阵将支撑腿产生的力从广义力中提取。将全身控制问题构建为严格分层二次规划问题。第一层优化任务：维持基座质心高度和姿态的恒定（最高优先级），以质心动量与角动量的变化率为优化变量，等式约束为ZMP稳定裕度不等式始终满足。第二层优化：动力学一致性与接触力锥约束（摩擦锥近似为12边棱锥），优化支撑腿足底力分布，使各腿力尽可能均匀。第三层优化：机械臂末端加速度跟踪期望轨迹，期望加速度由阻抗控制器产生。使用基于加权-松弛变量法处理不可行问题，确保高优先级任务不因低优先级任务妥协。仿真结果表明，在机械臂末端施加20N外力扰动时，基座姿态波动小于0.8°，质心高度变化小于4mm，优于未分层优化方案的1.9°和11mm。
（2）自适应阻抗控制器设计与稳定性分析：
末端执行器与环境间的期望动力学关系设为Md·Δa + Bd·Δv + Kd·Δx = F_ext，其中Md、Bd、Kd为期望惯量、阻尼和刚度对角阵。设计自适应律对阻尼矩阵Bd进行实时调整：当接触力波动频率高于0.8Hz时增大Bd以抑制振荡，当力误差持续3秒保持同一方向时减小Bd以减小稳态力误差。自适应律使用模糊推理系统，输入为力误差的变化率与力误差的乘积，输出阻尼调整因子μ∈[0.5, 2.0]，模糊规则25条。用李雅普诺夫理论证明闭环系统的渐近稳定性，构造的Lyapunov函数V = (1/2)Δs^T M Δs + (1/2)Δx^T Kd Δx，其导数在自适应律下负半定。在CoppeliaSim中开展恒力打磨仿真，期望法向力15N，自适应阻抗控制下力响应的超调量为9.2%，稳态误差±0.6N，优于固定阻抗参数的14%超调和±1.2N稳态误差。
（3）MATLAB与CoppeliaSim联合仿真及实物初步测试：
搭建联合仿真平台，MATLAB负责全身控制算法解算（0.02秒控制周期），CoppeliaSim提供物理引擎和传感器反馈。二者通过TCP/IP实时交换关节力矩指令和关节状态。在联合仿真中，机器人执行桌面推拉任务：机械臂末端以15N力推动物体沿直线移动0.25米，过程中四足保持静止站姿。仿真显示末端力稳定15.3±1.1N，各腿足底力保持在摩擦锥内无滑移。将算法移植到实物四足机器人Unitree B1上的初步测试中，机械臂抓持物体时，四个足端压力传感器读数变化小于7%，基座姿态角变化<2°，验证了全身协同力控的有效性。
import numpy as np import quadprog # 分层QP求解器 def hierarchical_qp_solver(A_eq, b_eq, C_ineq, d_ineq, H_list, f_list, levels): # H_list, f_list: 每层的二次项和一次项 x_opt = None for l in range(levels): H, f = H_list[l], f_list[l] # 加上之前层的最优值作为等式约束（任务空间松弛） if x_opt is not None: A_eq = np.vstack([A_eq, np.eye(len(f))]) b_eq = np.concatenate([b_eq, x_opt]) x = quadprog.solve_qp(H, f, -C_ineq, -d_ineq, A_eq, b_eq, 0)[0] x_opt = x return x_opt # 自适应阻抗控制 class AdaptiveImpedanceController: def __init__(self, Md, Bd0, Kd, dt=0.005): self.Md = Md # 期望惯量 self.Bd = Bd0 # 初始阻尼 self.Kd = Kd self.dt = dt self.error_int = 0.0 def update_damping(self, force_error, force_error_rate): # 模糊规则简化版 prod = force_error * force_error_rate if prod > 0.05: mu = 1.5 # 增大阻尼 elif prod < -0.05: mu = 0.7 # 减小阻尼 else: mu = 1.0 self.Bd = self.Bd * (0.98) + mu * self.Bd0 * 0.02 # 平滑 return self.Bd def compute_acceleration(self, x_d, x, v_d, v, F_ext): dx = x - x_d dv = v - v_d a_des = np.linalg.inv(self.Md) @ (F_ext - self.Bd @ dv - self.Kd @ dx) return a_des
