机器人运动学、动力学基础上利用MATLAB进行PID控制仿真
2024-04-29 10:59:00
这是我的第一次写博客，不足之处还请谅解
进入正题
因为用SIMLINK做PID控制时，根据力矩反求加速度，再将得到的角度和角速度反馈回去继续重复计算，存在代数环问题仿真不出来，我一直没有找到解决办法，所以自己慢慢琢磨，只能用matlab代码直接进行PID控制仿真了，可能不是很合理，供参考
根据自己的结构推导出运动学方程和动力学方程
给定理想轨迹
x= 10*t*cos(5*t)+886;
y= 10*t*sin(5*t);
z= 20*t+700;
clc;clear;for i=0:1:900
t=i/100;
x (i+1)= 10*t*cos(5*t)+886;
y (i+1)= 10*t*sin(5*t);
z (i+1)= 20*t+700;end
plot3(x,y,z,'r-','LineWidth',1.5);
hold on;xlabel('X/mm');
ylabel('Y/mm');
zlabel('Z/mm');
grid on;
推导出的关节角与时间t的关系
关节角1：q1 =atan((10*t*sin(5*t))/(10*t*cos(5*t) + 886));关节角2：q2 =- acos(((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404)/(2800*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(1/2))) - atan((20*t + 700)/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100));关节角3：q3 =pi/2 - acos(4901/4900 - (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/960400 - (20*t + 700)^2/960400);
生成理论上的关节角与时间的变化曲线
clc;
clear;
%生成理论上的角度曲线
for i=0:1:900
t = 0.01*i
x= 10*t*cos(5*t)+886;
y= 10*t*sin(5*t);
z= 20*t+700;
OB4=[x,y,z];
q1=atan(y/x);
q2=-(acos((700^2+(sqrt(x^2+y^2)-100)^2+z^2-686^2)/(2*700*sqrt((sqrt(x^2+y^2)-100)^2+z^2)))+atan(z/(sqrt(x^2+y^2)-100)));
q3=pi/2-acos((700^2+686^2-(sqrt(x^2+y^2)-100)^2-z^2)/(2*700*686));
s1(i+1)=q1;
s2(i+1)=q2;
s3(i+1)=q3;
end
j=0:0.01:9;
plot(j,s1)
plot(j,s2)
plot(j,s3)
hold on
利用动力学公式，给定力矩，求解此时刻的角加速度
qdd=inv(M)*(t_0-H);%反求给定力矩，角度，角速度，此时的加速度是多少进而可以得到下一时刻的角速度和角度，再代会循环计算
根据PID原理，计算此时求得的角度以及角速度与理论上的角度和角速度的差，作为误差和误差变化率，输入PID控制器，输出力矩
clc;
clear;
%给初始值
t=0;
q1 =atan((10*t*sin(5*t))/(10*t*cos(5*t) + 886));
q2 =- acos(((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404)/(2800*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(1/2))) - atan((20*t + 700)/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100));
q3 =pi/2 - acos(4901/4900 - (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/960400 - (20*t + 700)^2/960400);
q1d =((10*sin(5*t))/(10*t*cos(5*t) + 886) + (50*t*cos(5*t))/(10*t*cos(5*t) + 886) - (10*t*sin(5*t)*(10*cos(5*t) - 50*t*sin(5*t)))/(10*t*cos(5*t) + 886)^2)/((100*t^2*sin(5*t)^2)/(10*t*cos(5*t) + 886)^2 + 1);
q2d =((800*t + (2*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) + 28000)/(2800*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(1/2)) - ((200*t + ((2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)) + 7000)*((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404))/(5600*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(3/2)))/(1 - ((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404)^2/(1960000*(20*t + 700)^2 + 1960000*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2))^(1/2) - (20/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100) - ((20*t + 700)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2))/((20*t + 700)^2/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 1);
q3d =-((2*t)/2401 + ((2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(480200*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)) + 10/343)/(1 - ((20*t + 700)^2/960400 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/960400 - 4901/4900)^2)^(1/2);
Ee=0;%定义一个误差的总和
for i = 0:1:900t=0.01*i;q_1 =atan((10*t*sin(5*t))/(10*t*cos(5*t) + 886));
q_2 =- acos(((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404)/(2800*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(1/2))) - atan((20*t + 700)/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100));
q_3 =pi/2 - acos(4901/4900 - (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/960400 - (20*t + 700)^2/960400);
q_1d =((10*sin(5*t))/(10*t*cos(5*t) + 886) + (50*t*cos(5*t))/(10*t*cos(5*t) + 886) - (10*t*sin(5*t)*(10*cos(5*t) - 50*t*sin(5*t)))/(10*t*cos(5*t) + 886)^2)/((100*t^2*sin(5*t)^2)/(10*t*cos(5*t) + 886)^2 + 1);
q_2d =((800*t + (2*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) + 28000)/(2800*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(1/2)) - ((200*t + ((2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)) + 7000)*((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404))/(5600*((20*t + 700)^2/4 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/4)^(3/2)))/(1 - ((20*t + 700)^2 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 19404)^2/(1960000*(20*t + 700)^2 + 1960000*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2))^(1/2) - (20/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100) - ((20*t + 700)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)*(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2))/((20*t + 700)^2/(2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2 + 1);
q_3d =-((2*t)/2401 + ((2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)*(50*t*sin(5*t)^2 + ((10*cos(5*t) - 50*t*sin(5*t))*(10*t*cos(5*t) + 886))/2 + 250*t^2*cos(5*t)*sin(5*t)))/(480200*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2)) + 10/343)/(1 - ((20*t + 700)^2/960400 + (2*((10*t*cos(5*t) + 886)^2/4 + 25*t^2*sin(5*t)^2)^(1/2) - 100)^2/960400 - 4901/4900)^2)^(1/2);e=[q_1-q1;q_2-q2;q_3-q3];
ec=[q_1d-q1d;q_2d-q2d;q_3d-q3d];
Ee=Ee+e;
KP=600;
KI=30;
KD=2;t_0=KP*e+KI*Ee+KD*ec;%PID控制输出力矩M =[(241529529139*cos(2*q2))/400000000000 - (26897521*sin(2*q2 + q3))/100000000 + (2669931*sin(2*q2))/500000000 - (85345834133*cos(2*q2 + 2*q3))/2000000000000 - (3842503*sin(q2 + q3))/50000000 + (5091931*cos(q2))/10000000 + (99*sin(q2))/25000 - (26897521*sin(q3))/100000000 + 1920346974018873/2251799813685248, 0, 0;0, 5823838324126837/4503599627370496 - (26897521*sin(q3))/50000000, 6149815468784047/72057594037927936 - (26897521*sin(q3))/100000000;0, 6149815468784047/72057594037927936 - (26897521*sin(q3))/100000000, 6149815468784047/72057594037927936];
H =[(99*q1d*q2d*cos(q2))/25000 - (26897521*q1d*q3d*cos(q3))/100000000 - (5091931*q1d*q2d*sin(q2))/10000000 - (26897521*q1d*q2d*cos(2*q2 + q3))/50000000 - (26897521*q1d*q3d*cos(2*q2 + q3))/100000000 + (2669931*q1d*q2d*cos(2*q2))/250000000 - (241529529139*q1d*q2d*sin(2*q2))/200000000000 + (85345834133*q1d*q2d*sin(2*q2 + 2*q3))/1000000000000 + (85345834133*q1d*q3d*sin(2*q2 + 2*q3))/1000000000000 - (3842503*q1d*q2d*cos(q2 + q3))/50000000 - (3842503*q1d*q3d*cos(q2 + q3))/50000000;(1263543455108301*sin(q2 + q3))/335544320000000 - (6697588617537127*cos(q2))/268435456000000 - (4851*sin(q2))/25000 - (99*q1d^2*cos(q2))/50000 - (26897521*q3d^2*cos(q3))/100000000 + (5091931*q1d^2*sin(q2))/20000000 + (26897521*q1d^2*cos(2*q2 + q3))/100000000 - (2669931*q1d^2*cos(2*q2))/500000000 + (241529529139*q1d^2*sin(2*q2))/400000000000 - (85345834133*q1d^2*sin(2*q2 + 2*q3))/2000000000000 + (3842503*q1d^2*cos(q2 + q3))/100000000 - (26897521*q2d*q3d*cos(q3))/50000000;(1263543455108301*sin(q2 + q3))/335544320000000 + (26897521*q1d^2*cos(q3))/200000000 + (26897521*q2d^2*cos(q3))/100000000 + (26897521*q1d^2*cos(2*q2 + q3))/200000000 - (85345834133*q1d^2*sin(2*q2 + 2*q3))/2000000000000 + (3842503*q1d^2*cos(q2 + q3))/100000000];qdd=inv(M)*(t_0-H);%反求给定力矩，角度，角速度，此时的加速度是多少
q1dd=qdd(1);
q2dd=qdd(2);
q3dd=qdd(3);
%得到下一时刻的角度和角速度 q1d=q1d+q1dd*0.01;q2d=q2d+q2dd*0.01;q3d=q3d+q3dd*0.01;q1=q1+q1d*0.01+q1dd*0.01*0.01;q2=q2+q2d*0.01+q2dd*0.01*0.01;q3=q3+q3d*0.01+q3dd*0.01*0.01; end通过调节PID的参数，减小轨迹跟踪误差
这里是关节2的跟踪效果，还不好，还要继续调节参数
后续，还可以利用模糊控制，进行模糊PID控制提高控制性能。
机器人运动学、动力学基础上利用MATLAB进行PID控制仿真相关推荐
- matlab下pid控制仿真,利用Matlab实现PID控制仿真
%PID Controller clear, clc, close all; ts=0.001; %采样时间=0.001s sys=tf(5.235e005,[1,87.35,1.047e004,0] ...
- matlab下pid控制仿真,基于MATLAB下PID控制仿真.PDF
维普资讯 2004年第 4期 中 国 航 海 NO．4Dec．2004 塑 垒 垒 SerialNO．61 文章编号 :1000-4653(2004)04-0077-04 基于MATLAB下的PID控 ...
- 利用matlab实现pid控制,运用Matlab实现PID控制器的设计
第 39 卷 第 6 期 2011 年 12 月 福州大学学报( 自然科学版) Journal of Fuzhou University( Natural Science Edition) Vol． ...
- 利用matlab实现h 控制,利用Matlab实现H∞控制
利用Matlab实现H∞控制 原文:H∞ Control in Matlab(点击下载) H∞控制器设计原理容易理解,难点在于编程.这里简单介绍Matlab里面几个相关函数的用法,希望能帮助你设计第一 ...
- 如何使用matlab得出pid控制参数值,基于MATLAB的PID控制器参数整定及仿真
基于MATLAB的PID控制器参数整定及仿真 摘要:PID控制器结构和算法简单,应用广泛,但参数整定比较复杂,在此我探讨利用MATLAB实现PID参数整定及其仿真的方法,并分析比较比例.比例积分.比例 ...
- 基于MATLAB的离散系统的数字PID控制仿真
基于MATLAB的离散系统的数字PID控制仿真 1 设计要求 2 方案设计 2.1 设计思路及总体方案框图 2.2 PID算法的基本原理 2.3 PID算法的离散化 3 总体方案的选择与设计 3.1 ...
- 【飞行器】基于matlab四旋翼飞行器PID控制仿真【含Matlab源码 1277期】
一.获取代码方式 获取代码方式1: 完整代码已上传我的资源:[飞行器]基于matlab四旋翼飞行器PID控制仿真[含Matlab源码 1277期] 点击上面蓝色字体,直接付费下载,即可. 获取代码方式 ...
- matlab离散系统pid控制系统,离散系统的数字PID控制仿真
内容介绍 原文档由会员 霜天盈月 发布 离散系统的数字PID控制仿真 本文共计33页,14252字: 摘 要 近年来,由于脉冲技术.数字式元部件.数字计算机,特别是微处理器的蓬勃发展,数字控制器在许多 ...
- 基于matlab的步进电机转速控制仿真,matlab步进电机转速控制仿真(课程设计).doc
matlab步进电机转速控制仿真(课程设计) 摘 要 一般电动机都是连续旋转,而步进电动却是一步一步转动的,故叫步进电动机.每输入一个冲信号,该电动机就转过一定的角度(有的步进电动机可以直接输出线位移 ...
最新文章
- 1行代码搞定Latex公式编写，这个4.6M的Python小插件，堪称论文必备神器
- Java关键字this、super使用总结
- 成功解决mnist.py:110: dense_to_one_hot (from tensorflow.contrib.learn.python.learn.datasets.mnist) is de
- 在项目中常用到的几个注解@JsonInclude、@JsonFormat、@DateTimeFormat
- bizmsg是什么文件可以删除吗_电脑C盘满了怎么清理？哪些文件可以删除？
- Javascript对象及数组用法笔记
- android按钮固定尺寸,如何在android中保持按钮的固定宽高比
- cesium模型不遮挡点线面_VueCLI3.0干货系列之集成Cesium三维地球框架
- 弱电系统集成服务器,智能化弱电系统集成(BMS系统集成工程）设计方案
- Android平板软件推荐,新人看过来 安卓平板装机必备软件推荐
- Java IO流处理 面试题汇总
- 计算机桌面美化软件,桌面美化软件哪个好_桌面美化软件排行榜【最新】-太平洋电脑网...
- python爬取公众号之 创建个人微信公众号
- JavaScript大师必须掌握的12个知识点 1
- 1,515美元的价格可让您驾驭野兽式机械套装
- 基金A类和C类的科普贴
- Dmaven.multiModuleProjectDirectory system propery is not set. Check $M2_HOME environment variable a
- erdas叠加显示_ERDAS影像融合
- Jemeter温故而知新
- Android开发者该学习哪些东西提高竞争力？成功入职腾讯