摘要：以下功能算法是针对SCARA机器人的运动，为使其运动平稳有效，这里采用基于重力补偿的PID控制。这种控制办法简单有效，也是好多企业产品经常采用的核心控制功能算法。
以下功能算法是针对SCARA机器人的运动，为使其运动平稳有效，这里采用基于重力补偿的PID控制。这种控制办法简单有效，也是好多企业产品经常采用的核心控制功能算法。
这里提醒大家在具体选用时还需要根据自己的实际设备物理实体参数和功能需求进一步的完善，不能照抄照搬。
以下程序代码是基于IEC61131-3标准。
PROGRAM Main
// 物理参数
L1 : REAL := 1.0; // 连杆 1 长度
L2 : REAL := 1.0; // 连杆 2 长度
m1 : REAL := 1.0; // 连杆 1 质量
m2 : REAL := 1.0; // 连杆 2 质量
g : REAL := 9.81; // 重力加速度
// 控制参数
Kp1 : REAL := 1.0; // 关节 1 比例系数
Ki1 : REAL := 0.1; // 关节 1 积分系数
Kd1 : REAL := 0.05; // 关节 1 微分系数
Kp2 : REAL := 1.0; // 关节 2 比例系数
Ki2 : REAL := 0.1; // 关节 2 积分系数
Kd2 : REAL := 0.05; // 关节 2 微分系数
// 输入变量
Theta1 : REAL; // 关节 1 角度
Theta2 : REAL; // 关节 2 角度
DotTheta1 : REAL; // 关节 1 角速度
DotTheta2 : REAL; // 关节 2 角速度
TargetTheta1 : REAL; // 目标关节 1 角度
TargetTheta2 : REAL; // 目标关节 2 角度
// 中间变量
x1 : REAL;
y1 : REAL;
x2 : REAL;
y2 : REAL;
Dotx1 : REAL;
Doty1 : REAL;
Dotx2 : REAL;
Doty2 : REAL;
T1 : REAL;
T2 : REAL;
V : REAL;
L : REAL;
GravityCompensation1 : REAL;
GravityCompensation2 : REAL;
ErrorTheta1 : REAL; // 关节 1 角度误差
ErrorTheta2 : REAL; // 关节 2 角度误差
IntegralErrorTheta1 : REAL := 0.0; // 关节 1 角度积分误差
IntegralErrorTheta2 : REAL := 0.0; // 关节 2 角度积分误差
DerivativeErrorTheta1 : REAL := 0.0; // 关节 1 角度微分误差
DerivativeErrorTheta2 : REAL := 0.0; // 关节 2 角度微分误差
LastErrorTheta1 : REAL := 0.0; // 上一次关节 1 角度误差
LastErrorTheta2 : REAL := 0.0; // 上一次关节 2 角度误差
IsFirstCycle : BOOL := TRUE; // 标记是否为第一个扫描周期
END_VAR
// 计算连杆位置函数
FUNCTION CalcLinkPositions : VOID
x1 := L1 * COS(Theta1);
y1 := L1 * SIN(Theta1);
x2 := x1 + L2 * COS(Theta1 + Theta2);
y2 := y1 + L2 * SIN(Theta1 + Theta2);
END_FUNCTION
// 计算连杆速度函数
FUNCTION CalcLinkVelocities : VOID
Dotx1 := -L1 * SIN(Theta1) * DotTheta1;
Doty1 := L1 * COS(Theta1) * DotTheta1;
Dotx2 := Dotx1 - L2 * SIN(Theta1 + Theta2) * (DotTheta1 + DotTheta2);
Doty2 := Doty1 + L2 * COS(Theta1 + Theta2) * (DotTheta1 + DotTheta2);
END_FUNCTION
// 计算动能函数
FUNCTION CalcKineticEnergy : VOID
T1 := 0.5 * m1 * (Dotx1 * Dotx1 + Doty1 * Doty1);
T2 := 0.5 * m2 * (Dotx2 * Dotx2 + Doty2 * Doty2);
END_FUNCTION
// 计算势能函数
FUNCTION CalcPotentialEnergy : VOID
V := m1 * g * y1 + m2 * g * y2;
END_FUNCTION
// 计算拉格朗日函数
FUNCTION CalcLagrangian : VOID
L := T1 + T2 - V;
END_FUNCTION
// 计算重力补偿值函数
FUNCTION CalcGravityCompensation : VOID
GravityCompensation1 := -((m1 + m2) * g * L1 * SIN(Theta1) + m2 * g * L2 * SIN(Theta1 + Theta2));
GravityCompensation2 := -m2 * g * L2 * SIN(Theta1 + Theta2);
END_FUNCTION
// 计算角度误差函数
FUNCTION CalcErrorThetas : VOID
ErrorTheta1 := TargetTheta1 - Theta1;
ErrorTheta2 := TargetTheta2 - Theta2;
END_FUNCTION
// 计算积分误差函数
FUNCTION CalcIntegralErrors : VOID
IntegralErrorTheta1 := IntegralErrorTheta1 + ErrorTheta1;
IntegralErrorTheta2 := IntegralErrorTheta2 + ErrorTheta2;
END_FUNCTION
// 计算微分误差函数
FUNCTION CalcDerivativeErrors : VOID
IF IsFirstCycle THEN
DerivativeErrorTheta1 := 0.0;
DerivativeErrorTheta2 := 0.0;
ELSE
DerivativeErrorTheta1 := ErrorTheta1 - LastErrorTheta1;
DerivativeErrorTheta2 := ErrorTheta2 - LastErrorTheta2;
END_IF
LastErrorTheta1 := ErrorTheta1;
LastErrorTheta2 := ErrorTheta2;
IsFirstCycle := FALSE;
来源：请输入用户name