前言

本文通过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τ)y=y0+Bcos(bτ)
\tau是时间的函数。
要求起点和终点的加速度和速度都为0，加上起点和重点的位置约束，一共有6个约束条件，使用五次曲线拟合，可以得到τ的时间方程

2,轨迹跟踪原理

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

代码

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*2options = optimoptions('fsolve','Display','off');
[n,~] = size(q);
theta = zeros(n,2);
for i = 1:nq_des = q(i,:);[theta(i,:),FVAL,EXITFLAG] = fsolve(@fwd,x0,options,parms,q_des); % 输入1*2，输出1*2x0 = [theta(end,1) theta(end,1)+0.1];if EXITFLAG == 0disp(i);disp('无解');end
end
theta1 = theta(:,1);
theta2 = theta(:,2);% 求解theta1dot和theta2dotqdot = [xdot' ydot'];% n*2
thetadot = zeros(n,2);
for i = 1:nqdot_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和theta2dotqddot = [xddot' yddot'];% n*2
thetaddot = zeros(n,2);
for i = 1:nqddot_des = qddot(i,:);theta_now = theta(i,:);thetadot_now = thetadot(i,:); % 1*2J = 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反馈控制，解的关节作用力

7,Matlab实现末端轨迹跟踪相关推荐

终端滑模matlab程序,机器人轨迹跟踪控制方法研究(含MATLAB程序)
机器人轨迹跟踪控制方法研究(含MATLAB程序)(课题申报表,任务书,开题报告,中期检查表,外文翻译,论文15300字,程序,答辩PPT) 摘要机器人是一类复杂的.具有不确定性的.多输入多输出非线 ...
LQR轨迹跟踪算法Python/Matlab算法实现_代码(2)
本文根据LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导(2)使用代码实现,进行仿真: clc clear allKp = 1.0 ; dt =0.1 ;% [s] L = ...
LQR轨迹跟踪算法Python/Matlab算法实现2
这里对上一篇LQR轨迹跟踪算法Python/Matlab算法实现进行勘误: clc clear allKp = 1.0 ; dt = 0.1 ;% [s] L = 2.9 ;% [m] wheel b ...
LQR轨迹跟踪算法Python/Matlab算法实现_LQRmatrix推导
对于文章 LQR轨迹跟踪算法Python/Matlab算法实现中的LQR推导的问题,我简单写了一下手稿,不高兴做成公式了:
基于matlab单目视觉焊缝跟踪系统,基于激光线结构光3D视觉的机器人轨迹跟踪方法与应用...
摘要: 本文利用工控机.图像采集卡.工业相机和激光器,设计了激光线结构光3D视觉系统;利用工控机.运动控制卡.伺服驱动器和六轴串联机器人组成机器人运动系统;两者共同构成基于激光线结构光的机器人轨迹跟踪 ...
空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂（双臂）轨迹跟踪控制matlab仿真程序
空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂(双臂)轨迹跟踪控制matlab仿真程序,含空间机器人动力学模型,PD控制程序,带仿真结果,可供二次开发学习 ID:672006146 ...
四旋翼飞行器轨迹跟踪仿真MATLAB simulink/simscape
四旋翼飞行器轨迹跟踪仿真MATLAB simulink/simscape ID:6929669117089277
基于动力学模型的无人驾驶车辆MPC轨迹跟踪算法及carsim+matlab联合仿真学习笔记
目录 1 模型推导及算法分析 1.1 模型推导 1.1.1 车辆动力学模型 1.1.2 线性时变预测模型推导 1.2 模型预测控制器设计 1.2.1 目标函数设计 1.2.2 约束设计 2 代码解析 ...
差速机器人的纯轨迹跟踪仿真（Matlab）
差速机器人的纯轨迹跟踪仿真(Matlab) 刚入门,有的地方不对,烦请大家指正. 目录差速机器人的纯轨迹跟踪仿真(Matlab) 1 差速机器人运动模型 1.1 运动学分析建模 1.2 差速机器人的 ...
在Matlab下编程实现二维与三维的航迹跟踪控制、路径跟踪控制和轨迹跟踪控制，实现编队集群控制与避障控制
在Matlab下编程实现二维与三维的航迹跟踪控制.路径跟踪控制和轨迹跟踪控制,实现编队集群控制与避障控制. 研究对象有空中无人机.地面机器人.水面无人艇.水下机器人以及多智能体等. ID:321006 ...

7,Matlab实现末端轨迹跟踪

轨迹跟踪

前言

理论