前馈+ PI控制算法产生不同的结果MATLAB

编程问答 2022-06-08

问题描述

希望你们一切都好。

我正在研究拾放机器人仿真。该任务的步骤之一是创建一个前馈+ PI Control函数。

此函数的结构：

[Vd,V,Je,u_theta_dot,Xerr,Xerr_integral] = FeedbackControl(X,Xd,Xd_next,Kp,Ki,delta_t,thetaList,Xerr_integral)

输入；

  • X->实际最终执行者配置Tse，

  • Xd->所需/参考最终执行者配置Tse_d，

  • Xd_next->下一个所需/参考最终执行器配置
    时间步长Tse_d1

  • Kp和Ki->增益

  • delta_t->配置之间的时间步长，

  • thetaList->当前实际关节角度，

  • Xerr_integral->用于更新Xerr的积分。

输出；

  • Vd->导致前馈扭曲
  • V->命令的终结者扭曲
  • Je-> jbase（theta）和Jarm（theta）合成
  • u_theta_dot->控制矢量（u和theta_dot）
  • Xerr->配置错误
  • Xerr_integral->用作下一步中的参数。

以下是必需的数学方程式：

enter image description here

enter image description here

enter image description here

[Xerr] = log（inverse（X）* Xd）->需要将其转换为向量。

快速说明：我的函数使用了Modern Robotics Library中的一些函数，这些函数可以在here上找到（在Github上）。

所有这些，这是我的函数实现：

function [Vd,Xerr_integral)

%% Arm properties.
Blist = [0 0 1 0 0.033 0; 0 -1 0 -0.5076 0 0; 0 -1 0 -0.3526 0 0; 0 -1 0 -0.2176 0 0; 0 0 1 0 0 0]';
l = 0.47/2;
w = 0.30/2;
r = 0.0475;
F = (r/4) * [-1/(l + w),1/(l + w),-1/(l + w); 1 1 1 1; -1 1 -1 1];
sizee = size(F);
m = sizee(2);
zeross = zeros(1,m);
F6 = [zeross; zeross; F; zeross];

Tb0 = [1 0 0 0.1662; 0 1 0 0; 0 0 1 0.0026; 0 0 0 1];
M0e = [1 0 0 0.033; 0 1 0 0; 0 0 1 0.6546; 0 0 0 1];
T0e = FKinBody(M0e,Blist,thetaList);
Tbe = Tb0 * T0e;
Teb = TransInv(Tbe);
jbase = Adjoint(Teb) * F6;
Jarm = JacobianBody(Blist,thetaList);
Je = [jbase,Jarm];
psInv = pinv(Je);

Xerr_bracket = MatrixLog6(TransInv(X) * Xd);
Xerr = se3ToVec(Xerr_bracket);
Xerr_integral = Xerr_integral + (delta_t * Xerr);
Xerr_integral = round(Xerr_integral,3);
Vd_bracket = (1/delta_t) * MatrixLog6(TransInv(Xd) * Xd_next);
Vd = se3ToVec(Vd_bracket);

V = (Adjoint(TransInv(X) * Xd) * Vd) + (Kp * Xerr) + Ki * Xerr_integral;


u_theta_dot = psInv * V;
u_theta_dot = round(u_theta_dot,3);
Xerr = round(Xerr,3);
V = round(V,3);
Vd = round(Vd,3);
Je = round(Je,3);

end

最后两行用于将结果四舍五入到小数点后三位。

当我使用预先计算的参数测试我的函数时，这些是预期的结果：

enter image description here

enter image description here

enter image description here

enter image description here

enter image description here

这是我的测试脚本：

clc
clear

Blist = [0 0 1 0 0.033 0; 0 -1 0 -0.5076 0 0; 0 -1 0 -0.3526 0 0; 0 -1 0 -0.2176 0 0; 0 0 1 0 0 0]';
Tb0 = [1 0 0 0.1662; 0 1 0 0; 0 0 1 0.0026; 0 0 0 1];
M = [1 0 0 0.033; 0 1 0 0; 0 0 1 0.6546; 0 0 0 1];
thetaList = [0 0 0.2 -1.6 0]';
Xd = [0 0 1 0.5; 0 1 0 0; -1 0 0 0.5; 0 0 0 1];
Xd_next = [0 0 1 0.6; 0 1 0 0; -1 0 0 0.3; 0 0 0 1];
X = [0.170 0 0.985 0.387; 0 1 0 0; -0.985 0 0.170 0.570; 0 0 0 1];

Xerr_bracket = MatrixLog6(TransInv(X) * Xd);
Xerr = se3ToVec(Xerr_bracket);

Kp = 0 * eye(6);
Ki = 0 * eye(6);

delta_t = 0.01;

[Vd,Xerr)

我的Je和Vd输出完全正确。但是，其他输出有一些十进制错误。差异与我将数字四舍五入没有任何关系。实际上，预期的结果是四舍五入的，这也是我这样做的原因。我不认为预期的结果是错误的，所以这就是为什么我对函数实现感到怀疑的原因。你能帮我吗？

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）

controllermatlabrobot