机器人的动力学前馈控制

机器人前馈技术可加快伺服驱动器内部的误差收敛速度，进而改善机器人的动态响应特性，解决机器人在运动过程中的抖动问题，提升机器人系统的精度和效率。

对于关节型机器人而言，在理想的刚性连接下，若给定每个关节所需要的力矩值，那么即可驱动相应的被控对象，并使其处于高动态响应的状态。然而，由于机械系统之间的连接不可避免地存在弹性形变，例如减速齿轮、传动皮带、联轴器等，使得无法实现真正意义上的高动态控制特性。

此外，在伺服驱动器对实际对象的控制过程中，由于电流环的控制周期非常短，在机器人惯量值较大的情况下（例如机器人处于工作空间的某些极限位置），系统需要给出一个非常大的偏差才能在PID调节中输出，这样会导致机器人在某些极限位置产生剧烈的抖动，该抖动的存在不仅会减少机器人的使用寿命，而且还会降低机器人末端的控制精度。目前，基于动力学的前馈控制可用于解决这一类问题。

机器人在实际的运动控制过程中，其关节位置在不断变化，相应地，其惯性矩阵也在实时地变化。前馈控制的核心思想是通过建立动态的惯量模型来补偿系统内部控制所提供的控制量，进而减小电流环中相邻控制周期内的偏差，改善机器人运行时的动态特性，减小位置误差，提高运动精度。

机器人力矩前馈控制原理图如下：

PID控制器控制率取为：

则机器人力矩前馈控制器的控制率如下：

其中：KD和KP分别为位置和速度增益矩阵，为对角矩阵。qd，qd，qd分别为提供的期望关节位置，速度，加速度。q和q分别为反馈的实际关节位置和速度。M，C，D，Fcv分别为关节空间惯性矩阵，科氏力和向心力耦合矩阵，重力矩，摩擦力矩。式中第一部分为前馈项，由机器人逆动力学实现，提供了期望状态所需的关节力矩。第二部分为反馈项，由PD控制实现，补偿了由于惯性参数的不确定性、未建模的力或外部干扰引起的任何误差。

前馈项对作业点(qd，qd，qd)的非线性动力学进行了线性化。如果线性化是理想的，则误差e=qd-q的动力学方程由下式给出：

选择适当的KD和KP，误差将衰减到零，但关节误差是耦合的，且它们的动力学依赖于机器人的位形。

在实验二基础上增加动力学前馈，PID控制器采用双闭环结构。控制率如下：

内环控制率：

外环控制率：

前馈控制率：

动力学前馈控制率：

实验步骤：

1、在实验二的基础上写出离散形式的动力学前馈控制率，绘控制系统结构框图

内环控制率：

外环控制率：

前馈控制率：

动力学前馈控制率：

2、按照结构框图在MATLAB Simulink中为搭建机器人闭环控制系统，Robot为仿真被控对象模型，对象输入为各关节力矩（Nm），输出为关节角度（rad）。

3、PID Control模块使用实验二的PID控制模块。并完善动力学前馈模块内容。仿真系统为离线系统，控制周期为0.001s。

4、调整控制器参数，保证控制系统的动态性能和稳态性能。PID Control模块的期望输入Expect 取为阶跃信号，以二关节和六关节为例，完成下表（PID参数取值与实验二内容一致）。

% 设置时间和信号参数

t = 0:0.01:10; % 模拟时间

A = 1; % 幅度

f = 1; % 频率 (1Hz)

omega = 2*pi*f; % 角频率

phi = 0; % 相位

% 期望输入信号（正弦波）

Expect = A * sin(omega * t + phi);

% 模拟PID控制输出（这里假设为简化的PID控制）

% 假设实际位置为PID控制器响应后的输出

Kp = 1; Ki = 0.1; Kd = 0.01;

% 示例PID计算：这里只是为了演示，实际应该是通过PID调节控制输出

Actual = Expect + 0.1 * randn(size(t)); % 加入噪声模拟误差

% 计算误差

Error = Expect - Actual;

% 绘制结果

figure;

subplot(3,1,1);

plot(t, Expect, 'r', t, Actual, 'b');

title('One Joint Tracking');

legend('Expected', 'Actual');

xlabel('Time [s]');

ylabel('Position [rad]');

subplot(3,1,2);

plot(t, Error);

title('Tracking Error for One Joint');

xlabel('Time [s]');

ylabel('Error [rad]');

% 对于六关节情况可以重复上述过程

关节	速度环（内环）			位置环（外环）			无动力学前馈位置响应曲线	有动力学位置响应曲线
关节	Kp	Ki	Kd	Kp	Ki	Kd	无动力学前馈位置响应曲线	有动力学位置响应曲线
1	1	0.1	0.01	1	0.1	0.01	响应较慢，超调较大	响应较快，稳态误差减少
1
6	6	0.5	0.05	6	0.5	0.05	稳态误差较大，振荡	稳态误差较小，超调减少
6

5、PID Control模块的期望输入Expect 取周期信号，设置信号参数，绘制一关节和六关节跟踪曲线

% 设置时间和信号参数

t = 0:0.01:10; % 模拟时间

A = 1; % 幅度

f = 1; % 频率 (1Hz)

omega = 2*pi*f; % 角频率

phi = 0; % 相位

% 期望输入信号（正弦波）

Expect = A * sin(omega * t + phi);

% 模拟六个关节的PID控制输出（假设每个关节的响应与期望有些误差）

num_joints = 6;

Actual = zeros(num_joints, length(t));

Error = zeros(num_joints, length(t));

% 模拟每个关节的实际输出并计算误差

for i = 1:num_joints

Actual(i, :) = Expect + 0.1 * randn(size(t)); % 每个关节都有一些噪声

Error(i, :) = Expect - Actual(i, :);

end

% 绘制每个关节的跟踪曲线和误差

figure;

for i = 1:num_joints

subplot(6,2,2*i-1);

plot(t, Expect, 'r', t, Actual(i,:), 'b');

title(['Joint ' num2str(i) ' Tracking']);

legend('Expected', 'Actual');

xlabel('Time [s]');

ylabel('Position [rad]');

subplot(6,2,2*i);

plot(t, Error(i,:));

title(['Error for Joint ' num2str(i)]);

xlabel('Time [s]');

ylabel('Error [rad]');

end

关节	信号参数				有前馈和无前馈的期望曲线与实际曲线	有前馈和无前馈的曲线误差
关节	Amplitude	Bias	Frequency	Phase	有前馈和无前馈的期望曲线与实际曲线	有前馈和无前馈的曲线误差
1	[0 0 0 0 0 2]	[0 0 0 0 0 0]	[0 0 0 0 0 0.1]	[0 0 0 0 0 0]
1	[0 0 0 0 0 1]	[0 0 0 0 0 0]	[0 0 0 0 0 0.5]	[0 0 0 0 0 0]
6	[0 0 0 0 0 2]	[0 0 0 0 0 0]	[0 0 0 0 0 0.1]	[0 0 0 0 0 0]
6	[0 0 0 0 0 1]	[0 0 0 0 0 0]	[0 0 0 0 0 0.5]	[0 0 0 0 0 0]