7,Matlab实现末端轨迹跟踪

lilili~

已于 2022-07-05 09:06:29 修改

阅读量4.3k

点赞数 3

分类专栏： matlab与足式机器人文章标签： matlab

于 2022-04-14 19:08:11 首次发布

本文链接：https://blog.csdn.net/weixin_46536094/article/details/124178197

版权

matlab与足式机器人专栏收录该内容

9 篇文章 9 订阅

订阅专栏

轨迹跟踪

前言
理论
- 1,目标轨迹规划
- 2,轨迹跟踪原理
代码
仿真结果
总结

前言

本文通过matlab，实现使用平面两自由度连杆，规定末端画如下的8字形，反解关节速度、加速度，并带入动力学进行仿真。
在这里插入图片描述

理论

1,目标轨迹规划

本例中规划轨迹的8字形的方程如下所示：
$x_{0} + Asin(a\tau) \\ y = y_{0} + Bcos(b\tau) \$
\tau是时间的函数。
要求起点和终点的加速度和速度都为0，加上起点和重点的位置约束，一共有6个约束条件，使用五次曲线拟合，可以得到τ的时间方程

2,轨迹跟踪原理

系统的动力学方程可以整理为：
$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)=\tau$
给定跟踪的位置θ，θdot，thetaddot，结合系统的动力学方程，利用反馈线性化的原理可以给定τ的方程如下所示：
$\tau = \hat{C}\dot{q}+\hat{G}(q)+\hat{M}(q)(\ddot{q}_{des}-Kd(\dot{q}-\dot{q}_{des})-Kp(q-{q}_{des}))$
Kd和Kp是对角矩阵，并且满足 $kd=2\sqrt{kp}$ ，式中都是估计值，可能与真值不同，但是通过反馈能得到不错的跟踪效果

代码

main函数

function my_main
clc
close all
clear

%1，两级倒立摆参数
%   1.1两级摆物理参数
%   1.2两级摆控制参数
%   1.3两级摆噪声参数
%2，求解关节轨迹
%   2.1给定端点轨迹 % my_figure8
%   2.2给定关节轨迹 % q2theta
%3，使用动力学求解力来使得系统跟踪需要的关节轨迹
%   3.1 定义被积函数 % dynamics
%   3.2 定义反馈控制函数
%4，画图检查跟踪效果
%5，加入传感器噪声，查看跟踪效果




%1.1两级摆物理参数
parms.m1  =  1;   
parms.I1  =  0.5; 
parms.m2  =  1;   
parms.I2  =  0.5; 
parms.g   =  10; 
parms.l1 = 1;
parms.l2 = 1;
%1.2两级摆控制参数
parms.control.Kp1 = 100;
parms.control.Kd1 = 2*sqrt(parms.control.Kp1); %0.1*parms.control.Kp1;
parms.control.Kp2 = 100;
parms.control.Kd2 = 2*sqrt(parms.control.Kp2); %0.1*parms.control.Kp2;

% 1.3噪声参数
parms.disturb.T1_mean = 0;
parms.disturb.T1_dev = 0.01;
parms.disturb.T2_mean = 0;
parms.disturb.T2_dev = 0.01;
parms.disturb.theta1_mean = 0;
parms.disturb.theta1_dev = 0.01;
parms.disturb.theta2_mean = 0;
parms.disturb.theta2_dev = 0.01;
parms.disturb.theta1dot_mean = 0;
parms.disturb.theta1dot_dev = 0.01;
parms.disturb.theta2dot_mean = 0;
parms.disturb.theta2dot_dev = 0.01;

N = 1000;
randn('seed',0); %初始化噪声
parms.disturb.T1 = parms.disturb.T1_mean + parms.disturb.T1_dev*randn(N,1);
parms.disturb.theta1 = parms.disturb.theta1_mean + parms.disturb.theta1_dev*randn(N,1);
parms.disturb.theta1dot = parms.disturb.theta1dot_mean + parms.disturb.theta1dot_dev*randn(N,1);
parms.disturb.T2 = parms.disturb.T2_mean + parms.disturb.T2_dev*randn(N,1);
parms.disturb.theta2 = parms.disturb.theta2_mean + parms.disturb.theta2_dev*randn(N,1);
parms.disturb.theta2dot = parms.disturb.theta2dot_mean + parms.disturb.theta2dot_dev*randn(N,1);

% 1.4可视化参数
parms.time_delay = 0.05; %delay between frames, will need some fine tuning for different computers
parms.framespersec = 30;

%积分区间
tspan = linspace(0,5,N);
parms.t = tspan;

x0 = 1.0; y0 = 0;

%2.1 给定端点轨迹
[x,y,xdot,ydot,xddot,yddot] = my_figure8(tspan,x0,y0);

%2.2 给定关节轨迹
[theta1,theta2,theta1dot,theta2dot,theta1ddot,theta2ddot] = q2theta(x,y,xdot,ydot,xddot,yddot,parms);


%1.2两级摆控制参数
parms.control.theta1_ref = theta1;
parms.control.theta2_ref = theta2;
parms.control.theta1dot_ref = theta1dot;
parms.control.theta2dot_ref = theta2dot;
parms.control.theta1ddot_ref = theta1ddot;
parms.control.theta2ddot_ref = theta2ddot;


%3.1 定义被积函数
x0 = [theta1(1) theta1dot(1) theta2(1) theta2dot(2)];
options = odeset('RelTol',1e-9,'AbsTol',1e-9);
[t,x] = ode45(@dynamics,tspan,x0,options,parms);  % q = [theta1 theta1dot theta2 theta2dot];


%4 画图查看结果

figure(1)  %animation
twolink_animation(t,[x(:,1), x(:,3)],parms);
% twolink_animation(t,[theta1, theta2],parms);

figure(2)
subplot(2,1,1);
plot(t,parms.control.theta1_ref,'k--'); hold on;
plot(t,x(:,1),'b');
ylabel('theta1');
subplot(2,1,2);
plot(t,parms.control.theta2_ref,'k--'); hold on;
plot(t,x(:,3),'r');
ylabel('theta2');
xlabel('time');

figure(3)
subplot(2,1,1);
plot(t,parms.control.theta1dot_ref,'k--'); hold on;
plot(t,x(:,2),'b');
ylabel('theta1dot');
subplot(2,1,2);
plot(t,parms.control.theta2dot_ref,'k--'); hold on;
plot(t,x(:,4),'r');
ylabel('theta2dot');
xlabel('time');
legend('link1','link2');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

目标追踪轨迹

输入8字形的位置和总共需要多长时间。

function [x,y,xdot,ydot,xddot,yddot] = my_figure8(t,x0,y0)

T = t(end);
A = 0.5;
B = A;
a = 2;
b = 1;

tau = 2*pi*(-15*(t/T).^4+6*(t/T).^5+10*(t/T).^3);
taudot = 2*pi*(-15*4*(1/T)*(t/T).^3+6*5*(1/T)*(t/T).^4+10*3*(1/T)*(t/T).^2);
tauddot = 2*pi*(-15*4*3*(1/T)^2*(t/T).^2 + 6*5*4*(1/T)^2*(t/T).^3+10*3*2*(1/T)^2*(t/T));

x = x0 + A*sin(a*tau);
y = y0 + B*cos(b*tau);
xdot = A*a*cos(a*tau).*taudot;
ydot = -B*b*sin(b*tau).*taudot;
xddot = -A*a*a*sin(a*tau).*taudot + A*a*cos(a*tau).*tauddot;
yddot = -B*b*b*cos(b*tau).*taudot - B*b*sin(b*tau).*tauddot;

结算关节追踪轨迹

function [theta1,theta2,theta1dot,theta2dot,theta1ddot,theta2ddot] = q2theta(x,y,xdot,ydot,xddot,yddot,parms)% 输入都是1*n的向量
% 求解theta1和theta2
x0 = [0.1 0];% 猜测解
q = [x' y']; % n*2

options = optimoptions('fsolve','Display','off');
[n,~] = size(q);
theta = zeros(n,2);
for i = 1:n
    q_des = q(i,:);
    [theta(i,:),FVAL,EXITFLAG] = fsolve(@fwd,x0,options,parms,q_des); % 输入1*2，输出1*2
    x0 = [theta(end,1) theta(end,1)+0.1];
    if EXITFLAG == 0
        disp(i);
        disp('无解');
    end
end
theta1 = theta(:,1);
theta2 = theta(:,2);

% 求解theta1dot和theta2dot

qdot = [xdot' ydot'];% n*2
thetadot = zeros(n,2);
for i = 1:n
    qdot_des = qdot(i,:);
    theta_now = theta(i,:);
    J = two_jacobian(theta_now,parms);
    thetadot(i,:) = (J\qdot_des')';
end
theta1dot = thetadot(:,1);
theta2dot = thetadot(:,2);

% 求解theta1dot和theta2dot

qddot = [xddot' yddot'];% n*2
thetaddot = zeros(n,2);
for i = 1:n
    qddot_des = qddot(i,:);
    theta_now = theta(i,:);
    thetadot_now = thetadot(i,:); % 1*2
    J = two_jacobian(theta_now,parms);
    Jdot = two_jacobiandot(theta_now,thetadot_now,parms);
    thetaddot(i,:) = (J\(qddot_des' - Jdot*thetadot_now'))';
end
theta1ddot = thetaddot(:,1);
theta2ddot = thetaddot(:,2);

function F = fwd(x0,parms,q) % x = [theta1 theta2], q = [x y] q属于1*2
% 正运动学
theta1 = x0(1); theta2 = x0(2);
c1 = cos(theta1); s1 = sin(theta1); 
c2 = cos(theta2); s2 = sin(theta2);

R01 = [c1 -s1;s1 c1]; l01 = [0;0];
H01 = [R01 l01;0 0 1];

R12 = [c2 -s2;s2 c2]; l12 = [parms.l1;0];
H12 = [R12 l12;0 0 1];

G22 = [parms.l2;0;1];
G20 = H01*H12*G22;

G20_xy = G20(1:2);

F(1) = G20_xy(1) - q(1);
F(2) = G20_xy(2) - q(2);

function J = two_jacobian(theta_now,parms)% x = [theta1 theta2];% 1*2
% 数值计算雅可比矩阵
l1 = parms.l1;
l2 = parms.l2;
theta1 = theta_now(1);
theta2 = theta_now(2);

J(1,1) = - l2*sin(theta1 + theta2) - l1*sin(theta1);
J(1,2) = -l2*sin(theta1 + theta2);
J(2,1) = l2*cos(theta1 + theta2) + l1*cos(theta1);
J(2,2) = l2*cos(theta1 + theta2);

function Jdot = two_jacobiandot(theta_now,thetadot_now,parms)
% 计算雅可比矩阵的导数
l1 = parms.l1;
l2 = parms.l2;
theta1 = theta_now(1);
theta2 = theta_now(2);
theta1dot = thetadot_now(1);
theta2dot = thetadot_now(2);

Jdot(1,1) = - theta1dot*(l2*cos(theta1 + theta2) + l1*cos(theta1)) - l2*theta2dot*cos(theta1 + theta2);
Jdot(1,2) = -l2*cos(theta1 + theta2)*(theta1dot + theta2dot);
Jdot(2,1) = - theta1dot*(l2*sin(theta1 + theta2) + l1*sin(theta1)) - l2*theta2dot*sin(theta1 + theta2);
Jdot(2,2) = -l2*sin(theta1 + theta2)*(theta1dot + theta2dot);

动力学方程与控制

function xdot = dynamics(t,x0,parms) % x0 = [theta1(1) theta1dot(1) theta2(1) theta2dot(2)];
% 控制动力学函数
theta1 = x0(1);
theta1dot = x0(2);
theta2 = x0(3);
theta2dot = x0(4);

m1 = parms.m1;
I1 = parms.I1;
m2 = parms.m2;
I2 = parms.I2;

l1 = parms.l1;
l2 = parms.l2;
g = parms.g;

% 噪声项
% parms.disturb.T1 = parms.disturb.T1_mean + parms.disturb.T1_dev*randn(N,1);
% parms.disturb.theta1 = parms.disturb.theta1_mean + parms.disturb.theta1_dev*randn(N,1);
% parms.disturb.theta1dot = parms.disturb.theta1dot_mean + parms.disturb.theta1dot_dev*randn(N,1);
% parms.disturb.T2 = parms.disturb.T2_mean + parms.disturb.T2_dev*randn(N,1);
% parms.disturb.theta2 = parms.disturb.theta2_mean + parms.disturb.theta2_dev*randn(N,1);
% parms.disturb.theta2dot = parms.disturb.theta2dot_mean + parms.disturb.theta2dot_dev*randn(N,1);

% 对噪声插值
theta1_disturb = interp1(parms.t,parms.disturb.theta1,t);
theta2_disturb = interp1(parms.t,parms.disturb.theta2,t);
theta1dot_disturb = interp1(parms.t,parms.disturb.theta1dot,t);
theta2dot_disturb = interp1(parms.t,parms.disturb.theta2dot,t);
T1_disturb = interp1(parms.t,parms.disturb.T1,t);
T2_disturb = interp1(parms.t,parms.disturb.T2,t);


theta1_sensor = theta1 + theta1_disturb;
theta2_sensor = theta2 + theta2_disturb;
theta1dot_sensor = theta1dot + theta1dot_disturb;
theta2dot_sensor = theta2dot + theta2dot_disturb;

x_sensor = [theta1_sensor theta1dot_sensor theta2_sensor theta2dot_sensor];
T = my_control(t,x_sensor,parms);
T1 = T(1) - T1_disturb; T2 = T(2) - T2_disturb;
% 系统动力学
G1 = g*m2*((l2*cos(theta1 + theta2))/2 + l1*cos(theta1)) - T1 + (g*l1*m1*cos(theta1))/2;
G2 = (g*l2*m2*cos(theta1 + theta2))/2 - T2;
C1 = -(l1*l2*m2*theta2dot*sin(theta2)*(2*theta1dot + theta2dot))/2;
C2 = (l1*l2*m2*theta1dot^2*sin(theta2))/2;
M11 = I1 + (l1^2*m1)/4 + l1^2*m2 + (l2^2*m2)/4 + I2^2 + l1*l2*m2*cos(theta2);
M12 = (l2^2*m2)/4 + I2^2 + (l1*l2*m2*cos(theta2))/2;
M21 = (l2^2*m2)/4 + I2^2 + (l1*l2*m2*cos(theta2))/2;
M22 = (l2^2*m2)/4 + I2^2;

M = [M11 M12; M21 M22]; 
C = [C1; C2];
G = [G1; G2];

qddot = M\( - G - C);
theta1ddot = qddot(1);
theta2ddot = qddot(2);

xdot = [x0(2) theta1ddot x0(4) theta2ddot]';

轨迹追踪的核心就在于以下对于控制力的代码

function T = my_control(t,x_sensor,parms)
theta1 = x_sensor(1);
theta1dot = x_sensor(2);
theta2 = x_sensor(3);
theta2dot = x_sensor(4);

m1 = parms.m1;
I1 = parms.I1;
m2 = parms.m2;
I2 = parms.I2;

l1 = parms.l1;
l2 = parms.l2;
g = parms.g;

Kp1 = parms.control.Kp1;
Kd1 = parms.control.Kd1;
Kp2 = parms.control.Kp2;
Kd2 = parms.control.Kd2;


% 系统动力学
G1 = g*m2*((l2*cos(theta1 + theta2))/2 + l1*cos(theta1));
G2 = (g*l2*m2*cos(theta1 + theta2))/2 ;
C1 = -(l1*l2*m2*theta2dot*sin(theta2)*(2*theta1dot + theta2dot))/2;
C2 = (l1*l2*m2*theta1dot^2*sin(theta2))/2;
M11 = I1 + (l1^2*m1)/4 + l1^2*m2 + (l2^2*m2)/4 + I2^2 + l1*l2*m2*cos(theta2);
M12 = (l2^2*m2)/4 + I2^2 + (l1*l2*m2*cos(theta2))/2;
M21 = (l2^2*m2)/4 + I2^2 + (l1*l2*m2*cos(theta2))/2;
M22 = (l2^2*m2)/4 + I2^2;

M = [M11 M12; M21 M22]; 
C = [C1; C2];
G = [G1; G2];

% 插值得到积分点的参考位置、速度、加速度
theta1_ref = interp1(parms.t,parms.control.theta1_ref,t);
theta1dot_ref = interp1(parms.t,parms.control.theta1dot_ref,t);
theta1ddot_ref = interp1(parms.t,parms.control.theta1ddot_ref,t);

theta2_ref = interp1(parms.t,parms.control.theta2_ref,t);
theta2dot_ref = interp1(parms.t,parms.control.theta2dot_ref,t);
theta2ddot_ref = interp1(parms.t,parms.control.theta2ddot_ref,t);

%the controller
theta = [theta1 theta2]';
theta_ref = [theta1_ref theta2_ref]';

thetadot = [theta1dot theta2dot]';
thetadot_ref = [theta1dot_ref theta2dot_ref]';

thetaddot_ref = [theta1ddot_ref theta2ddot_ref]';

Kp = [Kp1 0;0 Kp2];
Kd = [Kd1 0;0 Kd2];


T = G + C + M*(thetaddot_ref - Kp*(theta - theta_ref) - Kd*(thetadot - thetadot_ref));

仿真结果

在这里插入图片描述

总结

已知末端轨迹求关节力流程如下

求解末端轨迹的参考位置、速度、加速度
运用反运动学反解关节的参考位置、速度、加速度
使用动力学方程，使用PD反馈控制，解的关节作用力

lilili~

关注

3
点赞
踩
48

收藏

觉得还不错? 一键收藏
打赏
1
评论
7,Matlab实现末端轨迹跟踪

轨迹跟踪前言理论1,目标轨迹规划2,轨迹跟踪原理代码main函数目标追踪轨迹结算关节追踪轨迹动力学方程与控制仿真结果总结前言本文通过matlab，实现使用平面两自由度连杆，规定末端画如下的8字形，反解关节速度、加速度，并带入动力学进行仿真。理论1,目标轨迹规划本例中规划轨迹的8字形的方程如下所示：x=x0+Asin(aτ)y=y0+Bcos(bτ) x = x_{0} + Asin(a\tau) \\y = y_{0} + Bcos(b\tau) \ x=x0+Asin(aτ
复制链接

扫一扫