【机器人学】逆运动学
【机器人学】逆运动学
- 一、解的存在性与多重性
- 二、逆运动学的几何解法
- 三、逆运动学的代数解法
一、解的存在性与多重性
逆运动学是一个非线性的求解问题，相对于正运动学较为复杂，主要是因为可解性探究、多重解以及多重解的选择等问题。例如，形如【机器人学】正运动学详解-6.4 一个简单例子中所用的六自由度机器人，其逆运动学可以描述为：假设我们已经知道其次变换矩阵 R T H ^RT_H RTH中的16个元素，求解得到6个关节变量 θ 1 \theta_1 θ1~ θ 6 \theta_6 θ6。由于矩阵 R T H ^RT_H RTH中有4个元素为常量，因此我们可以根据矩阵相等的条件得到12个等式，但是我们要注意到这12个方程之间有着相互约束，根据旋转矩阵得到的9个元素中，实际只有3个相互独立，再结合位置矢量的3个方程，那么我们将足够得到非齐次方程的解，这就是逆运动学。
- 解的存在性
解的存在性取决于该机器人的工作空间，通俗地说，就是机器人基座固定的情况下末端执行器所能够运动到的空间范围。但是我们应该看到，对于逆运动学，不仅要求位置矢量在工作空间内，姿态也达到要求才能算作逆运动学的解。机器人的工作空间主要分成3类，包括可达工作空间、灵活工作空间和次工作空间。可达工作空间是指机器人正常运行时，末端执行器坐标系的原点能在空间活动的最大范围；灵活工作空间是指在总工作空间内，末端执行器可以任意姿态达到的点；次工作空间是指总工作空间中去除灵活工作空间后所余下的部分构成的工作空间。
当一个机器人少于6自由度时，它在三维空间内不能达到全部位姿，特别是在工作空间的边界，一般来说能达到的位姿较少。
- 多重解问题
如果逆运动学只对位置有要求或者欧拉角只有部分要求时，容易出现有多个解的情况。这时，我们首先需要将所有这些解都得到，另外，我们还需要根据一定的原则来选择其中的解，常用的选择原则有“最短路程”、“最小能量”等，“最短行程”下的解即为在关节的运动范围内选择一组使得各个关节角的变化量最小的解。根据“最短行程”的原则来选择运动学逆解时也存在多种选择方式，例如对各关节的变化量进行不同的加权，使得选择的解尽量移动靠近末端执行器的连杆。“最小能量”即选择一组使得关节角变化所需能量最小的解。
二、逆运动学的几何解法
本文将提供一个经典的几何解法案例：
一个三杆平面机械臂中，末端执行器的位姿为 X 3 X_3 X3与 X 0 X_0 X0之间的夹角 ϕ \phi ϕ，位置矢量为 [ x y 0 ] T \left[\begin{matrix}x&y&0\end{matrix}\right]^T [xy0]T，杆件1和2的长度分别为 L 1 L_1 L1和 L 2 L_2 L2，求解 θ 1 \theta_1 θ1~ θ 3 \theta_3 θ3.
根据三角形几何关系和余弦定理，我们可以得到：
x 2 + y 2 = L 1 2 + L 2 2 − 2 L 1 L 2 cos ( 180 + θ 2 ) = L 1 2 + L 2 2 + 2 L 1 L 2 cos ( θ 2 ) (1) \begin{aligned} x^2+y^2&=L_1^2+L_2^2-2L_1L_2\cos(180+\theta_2)\\ &=L_1^2+L_2^2+2L_1L_2\cos(\theta_2) \end{aligned}\tag{1} x2+y2=L12+L22−2L1L2cos(180+θ2)=L12+L22+2L1L2cos(θ2)(1)
因此，
cos θ 2 = x 2 + y 2 − L 1 2 − L 2 2 2 L 1 L 2 (2) \cos\theta_2=\frac{x^2+y^2-L_1^2-L_2^2}{2L_1L_2}\tag{2} cosθ2=2L1L2x2+y2−L12−L22(2)
如果 ∣ x 2 + y 2 − L 1 2 − L 2 2 2 L 1 L 2 ∣ > 1 |\frac{x^2+y^2-L_1^2-L_2^2}{2L_1L_2}|>1 ∣2L1L2x2+y2−L12−L22∣>1，无解；
如果 x 2 + y 2 − L 1 2 − L 2 2 2 L 1 L 2 = ± 1 \frac{x^2+y^2-L_1^2-L_2^2}{2L_1L_2}=\pm1 2L1L2x2+y2−L12−L22=±1，那么 θ 2 = π 2 ∓ π 2 \theta_2=\frac{\pi}{2}\mp\frac{\pi}{2} θ2=2π∓2π；
如果 ∣ x 2 + y 2 − L 1 2 − L 2 2 2 L 1 L 2 ∣ < 1 |\frac{x^2+y^2-L_1^2-L_2^2}{2L_1L_2}|<1 ∣2L1L2x2+y2−L12−L22∣<1，那么 θ 2 = ± arccos [ x 2 + y 2 − L 1 2 − L 2 2 2 L 1 L 2 ] \theta_2=\pm \arccos\left[\frac{x^2+y^2-L_1^2-L_2^2}{2L_1L_2}\right] θ2=±arccos[2L1L2x2+y2−L12−L22]。
根据几何关系易得：
β = arctan y x \beta=\arctan\frac{y}{x} β=arctanxy
cos ψ = x 2 + y 2 + L 1 2 − L 2 2 2 L 1 x 2 + y 2 \cos\psi=\frac{x^2+y^2+L_1^2-L_2^2}{2L_1\sqrt{x^2+y^2}} cosψ=2L1x2+y2 x2+y2+L12−L22，那么可得： ψ = arccos x 2 + y 2 + L 1 2 − L 2 2 2 L 1 x 2 + y 2 \psi=\arccos\frac{x^2+y^2+L_1^2-L_2^2}{2L_1\sqrt{x^2+y^2}} ψ=arccos2L1x2+y2 x2+y2+L12−L22。
如果 θ 2 ≤ 0 \theta_2\le0 θ2≤0，那么 θ 1 = β + ψ \theta_1=\beta+\psi θ1=β+ψ；
如果 θ 2 > 0 \theta_2>0 θ2>0，那么 θ 1 = β − ψ \theta_1=\beta-\psi θ1=β−ψ；
最后， θ 3 = ϕ − θ 1 − θ 2 \theta_3=\phi-\theta_1-\theta_2 θ3=ϕ−θ1−θ2。
三、逆运动学的代数解法
为了简化模型与学习方便，本方法将以【机器人学】正运动学详解-6.4 一个简单例子的前4个自由度形成的机械臂作为例子，结合之前所学正运动学中的修改DH法，对逆运动学的代数解法进行一个方法示例。
只看前四个节点，那么对其建系建模如下：
DH表：
| # | θ \theta θ | d d d | a a a | α \alpha α |
|---|---|---|---|---|
| 0—1 | θ 1 \theta_1 θ1 | 0 | 0 | 0° |
| 1—2 | θ 2 \theta_2 θ2 | 0 | 0 | 90° |
| 2—3 | θ 3 \theta_3 θ3 | 0 | a 2 a_2 a2 | 0° |
| 3—4 | θ 4 \theta_4 θ4 | 0 | a 3 a_3 a3 | 0° |
因此我们可以得到相邻坐标系的齐次变换矩阵：
0 T 1 = [ c θ 1 − s θ 1 0 0 s θ 1 c θ 1 0 0 0 0 1 0 0 0 0 1 ] , 1 T 2 = [ c θ 2 − s θ 2 0 0 0 0 − 1 0 s θ 2 c θ 2 0 0 0 0 0 1 ] 2 T 3 = [ c θ 3 − s θ 3 0 a 2 s θ 3 c θ 3 0 0 0 0 1 0 0 0 0 1 ] , 3 T 4 = [ c θ 4 − s θ 4 0 a 3 s θ 4 c θ 4 0 0 0 0 1 0 0 0 0 1 ] (3) \begin{aligned} &^0T_1=\left[\begin{matrix}c\theta_1&-s\theta_1&0&0\\ s\theta_1&c\theta_1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{matrix}\right],^1T_2=\left[\begin{matrix}c\theta_2&-s\theta_2&0&0\\0&0&-1&0\\ s\theta_2&c\theta_2&0&0\\0&0&0&1\end{matrix}\right]\\ &^2T_3=\left[\begin{matrix}c\theta_3&-s\theta_3&0&a_2\\ s\theta_3&c\theta_3&0&0\\ 0&0&1&0\\0&0&0&1\end{matrix}\right],^3T_4=\left[\begin{matrix}c\theta_4&-s\theta_4&0&a_3\\ s\theta_4&c\theta_4&0&0\\ 0&0&1&0\\0&0&0&1\end{matrix}\right] \end{aligned}\tag{3} 0T1=⎣ ⎡cθ1sθ100−sθ1cθ10000100001⎦ ⎤,1T2=⎣ ⎡cθ20sθ20−sθ20cθ200−1000001⎦ ⎤2T3=⎣ ⎡cθ3sθ300−sθ3cθ3000010a2001⎦ ⎤,3T4=⎣ ⎡cθ4sθ400−sθ4cθ4000010a3001⎦ ⎤(3)
因此，该机器人的手（末端执行器）所在坐标系 { H } \{H\} {H}相对于机器人基座 { R } \{R\} {R}的齐次变换矩阵 R T H ^RT_H RTH为：
R T H = 0 T 1 × 1 T 2 × 2 T 3 × 3 T 4 = [ c 1 c 234 − c 1 s 234 s 1 c 1 ( a 2 c 2 + a 3 c 23 ) s 1 c 234 − s 1 s 234 − c 1 s 1 ( a 2 c 2 + a 3 c 23 ) s 234 c 234 0 a 2 s 2 + a 3 s 23 0 0 0 1 ] = [ n x o x a x p x n y o y a y p y n z o z a z p z 0 0 0 1 ] (4) \begin{aligned} ^RT_H&=^0T_1\times^1T_2\times^2T_3\times^3T_4\\ &=\left[\begin{matrix} c_1c_{234}&-c_1s_{234}&s_1&c_1(a_2c_2+a_3c_{23})\\ s_1c_{234}&-s_1s_{234}&-c_1&s_1(a_2c_2+a_3c_{23})\\ s_{234}&c_{234}&0&a_2s_2+a_3s_{23}\\ 0&0&0&1 \end{matrix}\right]\\ &=\left[\begin{matrix} n_x&o_x&a_x&p_x\\ n_y&o_y&a_y&p_y\\ n_z&o_z&a_z&p_z\\ 0&0&0&1 \end{matrix}\right] \end{aligned}\tag{4} RTH=0T1×1T2×2T3×3T4=⎣ ⎡c1c234s1c234s2340−c1s234−s1s234c2340s1−c100c1(a2c2+a3c23)s1(a2c2+a3c23)a2s2+a3s231⎦ ⎤=⎣ ⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦ ⎤(4)
列出方程组：
{ n x = c 1 c 234 ( a ) n y = s 1 c 234 ( b ) n z = s 234 ( c ) o x = − c 1 s 234 ( d ) o y = − s 1 s 234 ( e ) o z = c 234 ( f ) a x = s 1 ( g ) a y = − c 1 ( h ) a z = 0 ( i ) p x = c 1 ( a 2 c 2 + a 3 c 23 ) ( j ) p y = s 1 ( a 2 c 2 + a 3 c 23 ) ( k ) p z = a 2 s 2 + a 3 s 23 ( l ) (5) \begin{equation} \left\{ \begin{array}{lr} n_x=c_1c_{234}&(a)\\ n_y=s_1c_{234}&(b)\\ n_z=s_{234}&(c)\\ o_x=-c_1s_{234}&(d)\\ o_y=-s_1s_{234}&(e)\\ o_z=c_{234}&(f)\\ a_x=s_1&(g)\\ a_y=-c_1&(h)\\ a_z=0&(i)\\ p_x=c_1(a_2c_2+a_3c_{23})&(j)\\ p_y=s_1(a_2c_2+a_3c_{23})&(k)\\ p_z=a_2s_2+a_3s_{23}&(l)\\ \end{array} \right. \end{equation}\tag{5} ⎩ ⎨ ⎧nx=c1c234ny=s1c234nz=s234ox=−c1s234oy=−s1s234oz=c234ax=s1ay=−c1az=0px=c1(a2c2+a3c23)py=s1(a2c2+a3c23)pz=a2s2+a3s23(a)(b)(c)(d)(e)(f)(g)(h)(i)(j)(k)(l)(5)
因此，我们可以得到：
θ 1 = { ± π 2 a y = 0 , a x = ± 1 a t a n 2 ( a x , − a y ) a y ≠ 0 (6) \theta_1= \left\{\begin{array}{lr} \pm\frac{\pi}{2}&a_y=0,a_x=\pm1\\ atan2(a_x,-a_y)&a_y\neq0 \end{array} \right. \tag{6} θ1={±2πatan2(ax,−ay)ay=0,ax=±1ay=0(6)
θ 234 = θ 2 + θ 3 + θ 4 = { ± π 2 o z = 0 , n z = ± 1 a t a n 2 ( n z , o z ) o z ≠ 0 (7) \theta_{234}=\theta_2+\theta_3+\theta_4= \left\{\begin{array}{lr} \pm\frac{\pi}{2}&o_z=0,n_z=\pm1\\ atan2(n_z,o_z)&o_z\neq0 \end{array} \right. \tag{7} θ234=θ2+θ3+θ4={±2πatan2(nz,oz)oz=0,nz=±1oz=0(7)
令 m = a 2 c 2 + a 3 c 23 = { p x − a y a y ≠ 0 p y a x a y = 0 m=a_2c_2+a_3c_{23}=\left\{\begin{array}{lr}\frac{p_x}{-a_y}&a_y\neq0\\\frac{p_y}{a_x}&a_y=0\end{array} \right. m=a2c2+a3c23={−aypxaxpyay=0ay=0， n = a 2 s 2 + a 3 s 23 = p z n=a_2s_2+a_3s_{23}=p_z n=a2s2+a3s23=pz，那么：
m 2 + n 2 = a 2 2 + a 3 2 + 2 a 2 a 3 c 2 c 23 + 2 a 2 a 3 s 2 s 23 = a 2 2 + a 3 2 + 2 a 2 a 3 c 3 (8) m^2+n^2=a_2^2+a_3^2+2a_2a_3c_2c_{23}+2a_2a_3s_2s_{23}=a_2^2+a_3^2+2a_2a_3c_3\tag{8} m2+n2=a22+a32+2a2a3c2c23+2a2a3s2s23=a22+a32+2a2a3c3(8)
θ 3 = ± arccos ( m 2 + n 2 − a 2 2 − a 3 2 2 a 2 a 3 ) (9) \theta_3=\pm\arccos\left(\frac{m^2+n^2-a_2^2-a_3^2}{2a_2a_3}\right)\tag{9} θ3=±arccos(2a2a3m2+n2−a22−a32)(9)
根据 m m m和 n n n可得：
{ c 23 = m − a 2 c 2 a 3 s 23 = n − a 2 s 2 a 3 (10) \begin{equation} \left\{\begin{array}{lr} c_{23}=\frac{m-a_2c_2}{a_3}\\\\ s_{23}=\frac{n-a_2s_2}{a_3} \end{array} \right. \end{equation}\tag{10} ⎩ ⎨ ⎧c23=a3m−a2c2s23=a3n−a2s2(10)
根据正余弦函数的规律，可知：
( m − a 2 c 2 ) 2 + ( n − a 2 s 2 ) 2 = m 2 + n 2 + a 2 2 − 2 a 2 ( m c 2 + n s 2 ） = a 3 2 (11) (m-a_2c_2)^2+(n-a_2s_2)^2=m^2+n^2+a_2^2-2a_2(mc_2+ns_2）=a_3^2\tag{11} (m−a2c2)2+(n−a2s2)2=m2+n2+a22−2a2(mc2+ns2）=a32(11)
假设 α = a t a n 2 ( m , n ) \alpha=atan2(m,n) α=atan2(m,n)，那么：
m 2 + n 2 sin ( θ 2 + α ) = m 2 + n 2 + a 2 2 − a 3 2 2 a 2 (12) \sqrt{m^2+n^2}\sin(\theta_2+\alpha)=\frac{m^2+n^2+a_2^2-a_3^2}{2a_2}\tag{12} m2+n2 sin(θ2+α)=2a2m2+n2+a22−a32(12)
θ 2 = π 2 ± ( π 2 − arcsin m 2 + n 2 + a 2 2 − a 3 2 2 a 2 m 2 + n 2 ) − α (13) \theta_2=\frac{\pi}{2}\pm\left(\frac{\pi}{2}-\arcsin\frac{m^2+n^2+a_2^2-a_3^2}{2a_2\sqrt{m^2+n^2}}\right)-\alpha\tag{13} θ2=2π±(2π−arcsin2a2m2+n2 m2+n2+a22−a32)−α(13)
最终， θ 4 \theta_4 θ4可以根据上述表达式得到：
θ 4 = θ 234 − θ 2 − θ 3 (14) \theta_4=\theta_{234}-\theta_2-\theta_3\tag{14} θ4=θ234−θ2−θ3(14)
【机器人学】逆运动学相关推荐
- 利用逆矩阵解线性方程组_机器人学导论---第四章 操作臂逆运动学（一）4.1-4.11...
第四章 操作臂逆运动学[(一)4.1-4.11] (一)概述 1.为求出要求的关节角以放置相对于工作台坐标系{S}的工具坐标系{T},可将这个问题分为两部分(1)进行坐标变换求出相对于基坐标{B}的腕 ...
- 【现代机器人学】学习笔记五：逆运动学（Inverse kinematics）
这节的内容是根据末端位姿,计算关节角度,也就是所谓的IK.IK其实是一个比较复杂的问题,远不止本节内容所述的这么简单.这节个人觉得还是偏向基本的概念了. 逆运动学解析法 6R PUMA机器人 这节文章 ...
- python计算机器人运动学分析_机器人学之逆运动学数值解法及SVD算法
机器人学之逆运动学数值解法及SVD算法 文章目录 前言 这半个月的业余时间研究了机器人逆运动学的解析解法和迭代数值解法,并用程序实现.由于解析法只适合于特定结构的机器人,不具有通用性,因此这里不讨论解 ...
- 【机器人学】使用解析法求解6轴机械臂的逆运动学解
本文是承接上一篇求3轴拟人机械臂逆解内容(链接),扩展到求6轴机械臂的逆解,研究的仍然是目前比较流行的工业机械臂构型:拟人臂+球形腕关节(如下图1和图2所示),因为这种构型的机械臂具有闭合形式的逆运动 ...
- 柔性matlab机械臂运动空间代码,中正平和的机器人学笔记——2. 机械臂逆运动学（附MATAB代码）...
0. 预备知识(别跳过呀) 上一篇我们讲了正运动学,也就是通过齐次变换矩阵,知晓关节角后求得机械臂末端的位姿,并表示在基座标系下.这一篇我们就来讲一讲逆运动学问题--已知工具坐标系{T}相对于固定坐标 ...
- 【机器人学】平面2R机器人（二）——逆运动学
前情回顾 [机器人学]平面2R机器人(一)--正运动学 解答 逆运动学求解 尽管在题目要求上并没有计算逆运动学的步骤,但是在后续的MATLAB仿真中,需要从末端执行器的笛卡尔坐标得到各关节的角度,同时 ...
- 【机器人学】使用代数法求解3自由度拟人机械臂的逆运动学解
这篇博客会讨论一下使用解析法求解3自由度拟人机械臂的逆解及分析. 一.机械臂的逆解 机械臂的逆运动学问题就是由给定的末端执行器位置和方向,确定机械臂各个关节变量的值.机械臂的求解方法可以分为两大类:数 ...
- 【现代机器人学】基于指数积的机械臂逆运动学
0 引言 基于指数积的正运动学 现代机器人学名词概念 有了以上的基础,我们现在利用指数积来对机器人的逆运动学进行求解,有一点需要注意,需要先对机器人进行指数积的正运动学建模,然后才能利 ...
- 【机器人学】基于PoE模型的串联机械臂UR5的正运动学、微分运动学和逆运动学
文章目录 基本概念 正运动学 源码 一阶运动学 基本概念 PoE(Product of Exponential)指数积公式. PoE和DH的作用都是一样的. 实际使用过程中,绝大多数在售的机器人还都是 ...
最新文章
- vim 忽略大小写查找字符串
- python实现单例模式
- Typescript + TSLint + webpack 搭建 Typescript 的开发环境
- 基于GDAL库，读取海洋风场数据（.nc格式）c++版
- 7-1 多数组排序 (12 分)三种做法（冒泡排序 ； 重写sort方法 ；利用sort和栈）
- Sql查询出数据表中所有重复的数据
- Android-Universal-Image-Loader学习笔记（4）--download
- C语言约瑟夫环（简单版）
- MySQL存储过程和函数的区别
- 1.为什么要使用token，token是什么？
- 三只松鼠2020新财报：利润下滑都是疫情的锅？
- Kubeconfig 切换上下文
- 支付宝转账系统后台或API接口，避坑
- window11无法启动您的相机,0xA00F429F＜WindowShowFailed＞（OxC00D36BB)
- Oracle数据库的数据备份
- 国外工业互联网安全现状分析（政策；标准；厂商；产品）
- WIN7修改“桌面”存储位置
- Dev g++.exe g++.exe C:\Users\meng8\Desktop\g++.exe CreateProcess: No such file or directory.
- 3D游戏人物角色建模入门第一步：了解人体的构造
- 关于“已知有两孩子，第一个孩子为女孩，求第二个孩子为女孩的概率”的个人见解