六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）

代码逻辑参考https://blog.csdn.net/fengyu19930920/article/details/81144042#comments

背景介绍：

本人想用 MATLAB+V-REP 仿真机器人，对机器人的正逆运动学简单写了下代码实现部分。

首先是正运动学求解代码（所用DH参数表为UR5）

function T = zhengyundongxue(theta)%已知关节角求变换矩阵a=[0,-0.425,-0.39225,0,0,0];d=[0.89159,0,0,0.10915,0.09465,0.08230];alpha=[pi/2,0,0,pi/2,-pi/2,0];T01=T_para(theta(1),d(1),a(1),alpha(1));T12=T_para(theta(2),d(2),a(2),alpha(2));T23=T_para(theta(3),d(3),a(3),alpha(3));T34=T_para(theta(4),d(4),a(4),alpha(4));T45=T_para(theta(5),d(5),a(5),alpha(5));T56=T_para(theta(6),d(6),a(6),alpha(6));T=T01*T12*T23*T34*T45*T56;
end

function T = T_para(theta,d,a,alpha)T=[ccc(theta),-sss(theta)*ccc(alpha),sss(theta)*sss(alpha),a*ccc(theta);sss(theta),ccc(theta)*ccc(alpha),-ccc(theta)*sss(alpha),a*sss(theta);0,sss(alpha),ccc(alpha),d;0,0,0,1];
end

好了，问题来了，这个ccc和sss函数就是cos和sin，我承认我SB了。。。

function ccc=ccc(a)
%     ccc=cos(a/180*pi);ccc=cos(a);
end

function sss=sss(a)
%     sss=sin(a/180*pi);sss=sin(a);
end

至此，正运动学代码部分贴完了，经验证，和机器人工具箱所算结果一样。

其次，是逆运动学代码部分

function theta=niyundongxue(T)%变换矩阵T已知%SDH:标准DH参数表求逆解（解析解）%部分DH参数表如下，需要求解theta信息a=[0,-0.425,-0.39225,0,0,0];d=[0.89159,0,0,0.10915,0.09465,0.08230];alpha=[pi/2,0,0,pi/2,-pi/2,0];nx=T(1,1);ny=T(2,1);nz=T(3,1);ox=T(1,2);oy=T(2,2);oz=T(3,2);ax=T(1,3);ay=T(2,3);az=T(3,3);px=T(1,4);py=T(2,4);pz=T(3,4);%求解关节角1m=d(6)*ay-py;  n=ax*d(6)-px; theta1(1,1)=atan2(m,n)-atan2(d(4),sqrt(m^2+n^2-(d(4))^2));theta1(1,2)=atan2(m,n)-atan2(d(4),-sqrt(m^2+n^2-(d(4))^2));%求解关节角5theta5(1,1:2)=acos(ax*sin(theta1)-ay*cos(theta1));theta5(2,1:2)=-acos(ax*sin(theta1)-ay*cos(theta1));      %求解关节角6mm=nx*sin(theta1)-ny*cos(theta1); nn=ox*sin(theta1)-oy*cos(theta1);theta6=atan2(mm,nn)-atan2(sin(theta5),0);%求解关节角3mmm=d(5)*(sin(theta6).*(nx*cos(theta1)+ny*sin(theta1))+cos(theta6).*(ox*cos(theta1)+oy*sin(theta1))) ...-d(6)*(ax*cos(theta1)+ay*sin(theta1))+px*cos(theta1)+py*sin(theta1);nnn=pz-d(1)-az*d(6)+d(5)*(oz*cos(theta6)+nz*sin(theta6));theta3(1:2,:)=acos((mmm.^2+nnn.^2-(a(2))^2-(a(3))^2)/(2*a(2)*a(3)));theta3(3:4,:)=-acos((mmm.^2+nnn.^2-(a(2))^2-(a(3))^2)/(2*a(2)*a(3)));%求解关节角2mmm_s2(1:2,:)=mmm;mmm_s2(3:4,:)=mmm;nnn_s2(1:2,:)=nnn;nnn_s2(3:4,:)=nnn;s2=((a(3)*cos(theta3)+a(2)).*nnn_s2-a(3)*sin(theta3).*mmm_s2)./ ...((a(2))^2+(a(3))^2+2*a(2)*a(3)*cos(theta3));c2=(mmm_s2+a(3)*sin(theta3).*s2)./(a(3)*cos(theta3)+a(2));theta2=atan2(s2,c2);   %整理关节角1 5 6 3 2theta(1:4,1)=theta1(1,1);theta(5:8,1)=theta1(1,2);theta(:,2)=[theta2(1,1),theta2(3,1),theta2(2,1),theta2(4,1),theta2(1,2),theta2(3,2),theta2(2,2),theta2(4,2)]';theta(:,3)=[theta3(1,1),theta3(3,1),theta3(2,1),theta3(4,1),theta3(1,2),theta3(3,2),theta3(2,2),theta3(4,2)]';theta(1:2,5)=theta5(1,1);theta(3:4,5)=theta5(2,1);theta(5:6,5)=theta5(1,2);theta(7:8,5)=theta5(2,2);theta(1:2,6)=theta6(1,1);theta(3:4,6)=theta6(2,1);theta(5:6,6)=theta6(1,2);theta(7:8,6)=theta6(2,2); %求解关节角4theta(:,4)=atan2(-sin(theta(:,6)).*(nx*cos(theta(:,1))+ny*sin(theta(:,1)))-cos(theta(:,6)).* ...(ox*cos(theta(:,1))+oy*sin(theta(:,1))),oz*cos(theta(:,6))+nz*sin(theta(:,6)))-theta(:,2)-theta(:,3);  end

在这里插入代码片

Tip：

编写代码时，不要参考原博主https://blog.csdn.net/fengyu19930920/article/details/81144042#comments的结论部分，有错误，比如第一个m和n的定义与前述不同。
关于奇异位置，未作说明。
代码有没有硬核？名字无所谓，瞎起的，代码的逻辑不知道别人能不能看懂，能用就完事了。
改进DH参数表求逆运动学，请参考https://blog.csdn.net/jldemanman/article/details/80785075，本人未验证她的代码是否可用（明天周五了，想了想这周进度，又是深深的难过。。。）

六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）相关推荐

修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）
如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码.但逆解部分在不同版本的Matlab中运行有错误. 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原 ...
UR机械臂正逆运动学求解
最近有个任务:求解UR机械臂正逆运动学,在网上参考了一下大家的求解办法,众说纷纭,其中有些朋友求解过程非常常规,但是最后求解的8组解,只有4组可用.在这里我介绍一个可以求解8组解析解的方法,供大家参考 ...
matlab欧拉迭代,matlab机械臂正逆运动学求解问题，使用牛顿-欧拉迭代算法
代码复制的有问题,详细见本楼,谢谢. clc;clear; DR=pi/180; %time j = 1; for i = 0 : 0.1 : 2 %input theta= 45 * DR *(1+ ...
UR5构型机械臂正逆运动学
前言整理之前的一个项目,当时看着一个博客硬生生计算了差不多一个星期.尝试用MatLab符号推导工具箱化简一部分工作.我使用的大象机器人一款开源入门级协作机器人产品myCobot,开发文档十分完善,但 ...
实验一机械臂正逆运动学
实验一机械臂正逆运动学一.实验目的 1.巩固正逆运动学基础概念. 2.了解正逆运动学在机械臂控制中的实际用途. 二.实验内容 1.机械臂模型DH参数的计算. 2.机械臂正运动学的计算. 3.机械臂 ...
机械臂正逆运动学-----数值解
机械臂正逆运动学-----数值解建立DH坐标系求正运动学单关节齐次传递矩阵正运动学:返回齐次矩阵正运动学:返回欧拉角向量求雅可比矩阵求机械臂逆运动学合成通用运动学类机械臂的运动学包括 ...
五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
五自由度机械臂建模学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 机械臂还是原来的机械臂,之前用ROS做底层驱动,不需要写正逆运动学和相关算法就能得到一些简单的 ...
ROS机械臂正逆运动学
这里做一个六轴机械臂用于正逆运动学实验. 这里其实一共只有3轴,只有3轴位置没有姿态.所以urdf文件里我在末端做了3个虚拟关节,以便将kdl的frame能够填满,使得齐次坐标变换是规则的. 1.ur ...
用matlab实现机械臂正逆运动学控制
设计要求: 1.建立一个三自由度的机器人 2.建立坐标系,给出 D-H 参数表: 3.推导正运动学,并写出机器人的齐次变换矩阵: 4.推导逆运动学,并让机器人完成按要求绘制给定图形. 5.MATLAB ...
【机器人原理与实践（三）】六轴机械臂正逆解控制
文章目录 3.1 空间转换矩阵的理解 3.1.1平移变换 3.1.2旋转变换 3.2 D-H参数法 3.3 建立机械臂模型 3.3.1 机械臂模型介绍 3.3.2 使用Matlab进行示教仿真 3.4 ...

六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）

六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）相关推荐

最新文章

热门文章