最近有个任务：求解UR机械臂正逆运动学，在网上参考了一下大家的求解办法，众说纷纭，其中有些朋友求解过程非常常规，但是最后求解的8组解，只有4组可用。在这里我介绍一个可以求解8组解析解的方法，供大家参考。

以UR5机械臂结构和尺寸参数为例进行正逆运动学求解，下图分别是UR5结构图和标准DH系参数：

1. 正运动学求解

正运动学是已知关节六个角度求变换矩阵T

其中：

整理得：

带入DH参数，求解：

最终变换矩阵：

正运动学求解完毕。

2. 逆运动学求解

逆运动学是已知变换矩阵T，求六个关节角度。逆运动学求解有解析法，几何法，迭代法，这里采用解析法求解。

2.1 两个简单的数学方法

2.1.1 求角度

这个逆运动学算法求解的角度范围是

因为标准的反正切arctan的值域是

所以不能使用，这里介绍一个改进的反正切求法 Atan2(y, x)（Matlab里有这个函数）,它的值域可以满足要求。

2.1.2 解方程

首先进行三角恒等变换，令

其中：

然后带入原方程：

则

2.2 约定

为了简化书写，约定：

2.3 求解1，5，6关节角度

已知：

则

其中：

等式左边：

等式右边：

2.3.1 求关节角1

利用等式左右两边第3行，第4列对应相等求关节角1。

整理得：

设：

则

根据前面介绍的解方程的方法：

2.3.2 求关节角5

利用等式左右两边第3行，第3列对应相等求关节角5。

解得：

2.3.3 求关节角6

利用等式左右两边第3行，第1列对应相等求关节角6。

设：

则

根据前面介绍的方法：

其实可以通过化简得到式中

则

2.4 求解2，3，4关节角度

已知：

则

其中：

等式左边等于

等式右边等于

2.4.1 求解关节角3

利用等式左右两边第1行，第4列对应相等，第2行，第4列对应相等，求关节角3。

为了简化，设：

将m,n带入上式得

式子③④平方和为

因为

所以

2.4.2 求解关节角2

将③④展开得：

将关节角3带入⑤⑥，求关节角2得

则

2.4.3 求解关节角4

用的第2行第2列，第1行第2列求

则

2.5 总结

2.5.1 求解公式

2.5.2 奇异位置

1.肩关节奇异位置

此时末端执行器参考点O6位于轴线z1和z2构成的平面内，关节角1无法求解。

2.肘关节奇异位置

此时关节角2无法求解。

3.腕关节奇异位置

此时轴线z4和z6平行，关节角6无法求解。

2.6 实例

利用Matlab机器人库 ur5 DH参数：

alpha1 = pi/2; a1=0; d1=89.459;

alpha2 = 0; a2=-425; d2=0;

alpha3 = 0; a3=-392.25; d3=0;

alpha4 = pi/2; a4=0; d4=109.15;

alpha5 = -pi/2; a5=0; d5=94.65;

alpha6 = 0; a6=0; d6=82.3;

取 theta1 = 1; theta2 = 1; theta3 = 1; theta4 = 1; theta5 = 1; theta6 = 1; (不要纠结theta选这6个数值是否有实际意义，这里只验证算法的有效性)

将theta带入正运动学公式，求T:

将T带入逆运动学公式, 反求theta

theta =

再将8个theta带入正运动学公式，反求8个T:

8个T均等于

验证了算法的有效性

UR机械臂正逆运动学求解相关推荐

修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）
如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码.但逆解部分在不同版本的Matlab中运行有错误. 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原 ...
matlab欧拉迭代,matlab机械臂正逆运动学求解问题，使用牛顿-欧拉迭代算法
代码复制的有问题,详细见本楼,谢谢. clc;clear; DR=pi/180; %time j = 1; for i = 0 : 0.1 : 2 %input theta= 45 * DR *(1+ ...
UR5构型机械臂正逆运动学
前言整理之前的一个项目,当时看着一个博客硬生生计算了差不多一个星期.尝试用MatLab符号推导工具箱化简一部分工作.我使用的大象机器人一款开源入门级协作机器人产品myCobot,开发文档十分完善,但 ...
机械臂正逆运动学-----数值解
机械臂正逆运动学-----数值解建立DH坐标系求正运动学单关节齐次传递矩阵正运动学:返回齐次矩阵正运动学:返回欧拉角向量求雅可比矩阵求机械臂逆运动学合成通用运动学类机械臂的运动学包括 ...
实验一机械臂正逆运动学
实验一机械臂正逆运动学一.实验目的 1.巩固正逆运动学基础概念. 2.了解正逆运动学在机械臂控制中的实际用途. 二.实验内容 1.机械臂模型DH参数的计算. 2.机械臂正运动学的计算. 3.机械臂 ...
五自由度机械臂正逆运动学算法（Ｃ语言+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自由度拟人机械臂的逆运动学解
这篇博客会讨论一下使用解析法求解3自由度拟人机械臂的逆解及分析. 一.机械臂的逆解机械臂的逆运动学问题就是由给定的末端执行器位置和方向,确定机械臂各个关节变量的值.机械臂的求解方法可以分为两大类:数 ...

UR机械臂正逆运动学求解