1. Gough-Stewart并联机器人概述
Gough-Stewart并联机器人是一种具有六自由度的空间并联机构，由固定平台（底座）、运动平台和连接两者的六根可伸缩支链组成。这种结构最早由V.E. Gough在1950年代提出，后来由D. Stewart在1965年应用于飞行模拟器，因此得名。
与传统的串联机器人相比，并联机器人具有以下显著特点：
- 高刚度：由于采用并联结构，力分散在多个支链上，整体刚度更高
- 高精度：误差平均效应使得末端精度优于串联结构
- 高负载能力：多个支链共同承担负载
- 快速响应：运动部件质量小，动态性能好
在工业应用中，Gough-Stewart平台常用于：
- 飞行模拟器
- 精密定位平台
- 医疗机器人
- 振动模拟台
- 天文望远镜支撑系统
2. MATLAB仿真环境搭建
2.1 Simulink/Simscape建模基础
Simscape是MATLAB中专门用于物理系统建模的工具箱，它基于物理网络方法，可以直接建立机械、电气、液压等系统的物理模型。对于并联机器人仿真，我们主要使用Simscape Multibody模块。
建模步骤：
- 创建新模型：在MATLAB命令窗口输入
simulink，选择"Blank Model" - 添加Simscape Multibody库：在Library Browser中找到Simscape > Multibody
- 搭建基本框架：
- 添加两个"Rigid Transform"模块代表底座和平台
- 添加六个"Prismatic Joint"模块代表可伸缩支链
- 使用"Solid"模块定义几何形状和质量属性
2.2 关键参数设置
matlab复制% 底座参数
base.mass = 15; % kg
base.inertia = [2 0 0; 0 2 0; 0 0 2]; % kg·m²
base.size = [1.5 1.5 0.1]; % m
% 平台参数
platform.mass = 5; % kg
platform.inertia = [0.5 0 0; 0 0.5 0; 0 0 0.5]; % kg·m²
platform.size = [0.8 0.8 0.08]; % m
% 支链参数
leg.stiffness = 1e6; % N/m
leg.damping = 1e3; % N·s/m
leg.length = [0.8 1.2]; % 最小和最大长度，m
注意：实际建模时应根据具体应用需求精确计算这些参数。惯性参数对动力学性能影响很大，建议使用CAD软件测量或通过实验辨识。
2.3 连接点配置
Gough-Stewart平台的关键几何参数是两个平台上的连接点分布。典型的配置是：
- 底座连接点均匀分布在半径为R的圆周上
- 平台连接点均匀分布在半径为r的圆周上
- 两组点呈30°交错排列
matlab复制% 底座连接点坐标（半径1m）
R = 1;
theta_base = [0 60 120 180 240 300]'; % 角度分布
base_points = R * [cosd(theta_base) sind(theta_base) zeros(6,1)];
% 平台连接点坐标（半径0.6m）
r = 0.6;
theta_plat = [30 90 150 210 270 330]'; % 与底座错开30°
plat_points = r * [cosd(theta_plat) sind(theta_plat) zeros(6,1)];
3. 逆向运动学求解
3.1 基本原理
逆向运动学是指根据末端执行器（平台）的位姿（位置和姿态），求解各支链长度的过程。对于Gough-Stewart平台，这个过程相对直接：
- 给定平台位置P=[x,y,z]和姿态（通常用欧拉角或四元数表示）
- 计算平台各连接点在全局坐标系中的坐标
- 计算各支链向量 = 平台连接点坐标 - 底座连接点坐标
- 支链长度 = 向量模长
3.2 数学推导
设平台姿态用旋转矩阵R表示，平台中心位置为P，则第i个平台连接点在全局坐标系中的位置为：
P_i = P + R * p_i
其中p_i是平台连接点在平台局部坐标系中的坐标。
则第i个支链的向量为：
L_i = P + R * p_i - b_i
其中b_i是底座连接点坐标。
支链长度为：
l_i = ||L_i||
3.3 MATLAB实现
matlab复制function lengths = stewart_ik(P, R, base_points, plat_points)
% P: 平台中心位置 [x;y;z]
% R: 旋转矩阵 3x3
% base_points: 底座连接点 nx3
% plat_points: 平台连接点 nx3
n = size(base_points, 1);
lengths = zeros(n,1);
for i = 1:n
plat_global = P + (R * plat_points(i,:)')';
vec = plat_global - base_points(i,:);
lengths(i) = norm(vec);
end
end
3.4 姿态表示方法
在机器人控制中，常用的姿态表示方法有：
-
欧拉角（Roll-Pitch-Yaw）：
matlab复制% 欧拉角转旋转矩阵 function R = euler2rot(phi, theta, psi) % ZYX顺序 Rz = [cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1]; Ry = [cos(theta) 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)]; Rx = [1 0 0; 0 cos(phi) -sin(phi); 0 sin(phi) cos(phi)]; R = Rz * Ry * Rx; end -
四元数：
matlab复制% 四元数转旋转矩阵 function R = quat2rot(q) q = q/norm(q); R = [1-2*q(3)^2-2*q(4)^2, 2*q(2)*q(3)-2*q(1)*q(4), 2*q(2)*q(4)+2*q(1)*q(3); 2*q(2)*q(3)+2*q(1)*q(4), 1-2*q(2)^2-2*q(4)^2, 2*q(3)*q(4)-2*q(1)*q(2); 2*q(2)*q(4)-2*q(1)*q(3), 2*q(3)*q(4)+2*q(1)*q(2), 1-2*q(2)^2-2*q(3)^2]; end
4. 动力学建模与PID控制
4.1 动力学方程
Gough-Stewart平台的动力学可以用拉格朗日方程描述：
M(q)q̈ + C(q,q̇)q̇ + G(q) = τ + JᵀF
其中：
- M(q)：6×6质量矩阵
- C(q,q̇)：科里奥利和向心力矩阵
- G(q)：重力向量
- τ：关节空间中的驱动力
- J：雅可比矩阵
- F：末端执行器受到的外力
4.2 雅可比矩阵计算
雅可比矩阵J建立了关节速度与末端速度之间的关系：
q̇ = J⁻¹v
其中v = [v_x, v_y, v_z, ω_x, ω_y, ω_z]ᵀ是末端速度旋量。
对于Gough-Stewart平台，雅可比矩阵可以表示为：
J = [s₁ᵀ; (b₁×s₁)ᵀ; ...; s₆ᵀ; (b₆×s₆)ᵀ]
其中s_i是第i个支链的单位方向向量，b_i是第i个支链在底座坐标系中的向量。
4.3 PID控制器设计
在关节空间中，PID控制律可以表示为：
τ = Kₚ(q_d - q) + Kᵢ∫(q_d - q)dt + K_d(q̇_d - q̇)
MATLAB实现：
matlab复制classdef StewartPID < handle
properties
Kp
Ki
Kd
prev_error
error_integral
dt
end
methods
function obj = StewartPID(Kp, Ki, Kd, dt)
obj.Kp = Kp;
obj.Ki = Ki;
obj.Kd = Kd;
obj.prev_error = zeros(6,1);
obj.error_integral = zeros(6,1);
obj.dt = dt;
end
function tau = compute(obj, q_d, q, qd_d, qd)
error = q_d - q;
obj.error_integral = obj.error_integral + error * obj.dt;
error_deriv = (error - obj.prev_error) / obj.dt;
tau = obj.Kp.*error + obj.Ki.*obj.error_integral + obj.Kd.*error_deriv;
obj.prev_error = error;
end
end
end
4.4 参数整定方法
PID参数整定是控制性能的关键。对于并联机器人，建议采用以下步骤：
-
先整定P参数：
- 将Ki和Kd设为0
- 逐渐增大Kp直到系统开始振荡
- 取振荡临界值的50-60%作为Kp
-
再整定D参数：
- 保持Ki=0
- 设置Kd为Kp的0.1-0.2倍
- 观察系统响应，调整Kd抑制超调
-
最后整定I参数：
- 从很小的Ki开始
- 逐步增大直到稳态误差消除
- 注意避免积分饱和
典型初始值范围：
- Kp：100-1000 N/m
- Ki：1-10 N/(m·s)
- Kd：10-100 N·s/m
5. 仿真实现与结果分析
5.1 轨迹规划
为了测试控制系统性能，我们需要规划平台的运动轨迹。常用的轨迹包括：
-
直线运动：
matlab复制function [P, R] = linear_trajectory(t, t_total) start_pos = [0; 0; 1]; end_pos = [0.5; 0.3; 1.2]; % 5次多项式插值 if t <= t_total s = 10*(t/t_total)^3 - 15*(t/t_total)^4 + 6*(t/t_total)^5; P = start_pos + s*(end_pos - start_pos); else P = end_pos; end R = eye(3); % 保持姿态不变 end -
圆周运动：
matlab复制function [P, R] = circular_trajectory(t, omega, radius) theta = omega * t; P = [radius*cos(theta); radius*sin(theta); 1]; % 平台始终指向圆心 yaw = theta + pi; R = euler2rot(0, 0, yaw); end
5.2 闭环控制结构
完整的仿真闭环控制流程：
- 轨迹生成器产生期望位姿X_d
- 逆向运动学计算期望关节位置q_d
- PID控制器计算控制力τ
- 动力学模型计算系统响应
- 测量实际关节位置q
- 重复步骤3-5
5.3 性能评估指标
-
位置跟踪误差：
matlab复制
pos_error = norm(P_d - P_actual); -
姿态跟踪误差（用旋转矩阵差）：
matlab复制R_error = R_d' * R_actual; angle_error = acos((trace(R_error)-1)/2); -
控制力平滑度：
matlab复制
force_variation = norm(tau - tau_prev);
5.4 典型问题与调试
-
支链长度超限：
- 现象：仿真中出现支链长度超过物理限制
- 解决方案：检查轨迹规划范围，增加机械约束条件
-
奇异位形：
- 现象：雅可比矩阵接近奇异，控制力激增
- 解决方案：避免平台与底座平行，增加奇异规避算法
-
PID振荡：
- 现象：系统响应出现持续振荡
- 解决方案：降低Kp，增加Kd，或加入低通滤波
-
稳态误差：
- 现象：最终位置与期望位置有固定偏差
- 解决方案：适当增加Ki，或检查重力补偿是否完整
6. 进阶主题与扩展
6.1 自适应控制
对于参数不确定或负载变化的情况，可以采用自适应控制：
matlab复制% 基于模型参考的自适应控制示例
function tau = mrac_control(q, qd, q_ref, qd_ref, qdd_ref, params)
% 估计参数
Y = regressor(q, qd, q_ref, qd_ref, qdd_ref);
theta_hat = update_parameter_estimator(Y, error);
% 计算控制力
tau = Y * theta_hat + K*( [qd_ref;qdd_ref] - [qd;qdd] );
end
6.2 力控制
在需要与环境交互的应用中，可以实施力控制：
- 测量或估计末端接触力F_ext
- 计算等效关节力：τ_ext = JᵀF_ext
- 在控制律中加入力补偿项：
matlab复制tau = tau_pid + tau_ext + G(q); % G(q)是重力补偿
6.3 联合仿真
将MATLAB模型与其他工具联合仿真：
-
与CAD软件（如SolidWorks）联合：
- 导出CAD模型到Simscape Multibody
- 保持几何和惯性参数一致
-
与控制系统设计工具（如Simulink Control Design）：
- 自动PID整定
- 频域分析
-
与实时系统（如Speedgoat）：
- 生成C代码
- 硬件在环测试
6.4 实际应用考虑
将仿真结果应用到实际系统时需考虑：
-
执行器动力学：
- 液压缸的流体动力学
- 电动缸的电机响应
-
传感器噪声：
- 编码器分辨率
- 力传感器噪声
-
结构柔性：
- 支链不是完全刚性
- 连接件存在微小变形
-
计算延迟：
- 控制器采样时间
- 通信延迟
在实际项目中，我通常会先进行详细的仿真验证，然后在实物平台上从小幅度运动开始逐步测试，同时准备好紧急停止机制。从仿真到实物的过渡中，控制参数通常需要重新调整，特别是增益值需要降低20-30%以应对实际系统中的不确定因素。