精华内容
下载资源
问答
  • 机械臂运动学逆解

    千次阅读 多人点赞 2019-07-16 16:32:49
    淘宝热卖6自由度机械运动学臂逆解(原) ...找寻了半天也没有找到现成的DH模型和运动学逆解。所有就自己使用几何法建立了模型并给出运动学逆解。最后的控制效果可以观看视频: https://www.bilibili.com/video/av58...

    机械臂正逆解


    由于贫穷的关系,只得在淘宝上购买“玩具”机械臂进行研究,这是下面这货:

    在这里插入图片描述
    相信小伙伴都在淘宝上买了这个手臂来玩耍,但是这手跟工业传统的6轴机械臂那差了不是一点半点。找寻了半天也没有找到现成的DH模型和运动学逆解。所有就自己使用几何法建立了模型并给出运动学逆解。最后的控制效果可以观看视频:

    机械臂功能演示视频

    下面是几何解过程,并附上C/C++的程序

    经过我的分析,这个机械臂可以简化成一个4自由度的机械臂(夹子和夹子上的那两个舵机对运动学逆解无关),如下图:

    在这里插入图片描述
    画出几何示意图:
    在这里插入图片描述
    这里的j0、j1、j2、j3是指4个舵机转动的角度,L1、L2、L3指三节手臂的长度。末端执行器(夹子)中心的坐标为逆解目标 (x,y,z)

    这里值得注意的一点就是该手臂与工业6自由度的手有很大区别,工业6自由度的手的逆解目标是末端执行器的姿态加坐标,即一个齐次变换矩阵。而我们这个手按照这样来几乎很多位置都是没有解的,所有我们就剔除姿态,只保留坐标,即我们只要求我们的夹子到达这个坐标,而不需要夹子是以何种姿态到达这个坐标的。所有我们的逆解目标仅仅是个坐标而已。

    这样我们容易得到正向解结果如下:

    // 根据舵机角度正向解出目标坐标
    	x = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*cos(j0);
    	y = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*sin(j0);
    	z = L1 * cos(j1) + L2 * cos(j1 + j2) + L3 * cos(j1 + j2 + j3);
    

    这里我们可以看出,我们的输入是j0、j1、j2、j3这四个舵机转动的角度,而我们的目标只是一个 三维坐标(x,y,z)
    逆解就是输入坐标(x,y,z),然后求j0、j1、j2、j3,但是我们逆向求解的时候就可能会出现多组解
    (未知数的个数4大于方程的个数3)。这样的话我们就只能先确定一个未知数的取值,然后才能对方程进行求解。

    底座舵机j0的最好算:

    j0 = atan2(y, x);
    

    这里我采取的是先确定j1的值,然后再求解方程(这里方程的推导我就不写啦,直接上代码,代码里就是我解出来的结果)。但是我们所确定的这个j1的值不一定能求出解,也可能求出几组解。所有我们就将j1舵机所能到达的角度都取一遍,保证能求出更多的解。

    	for (j1 = -90; j1 < 90; j1++)//先确定j1的值,并且把j1从-90°到90°都取一遍,算出所有有可能的解
    	{
    		j1 *= RAD2ANG;//弧度转换
    		j3 = acos((pow(a, 2) + pow(b, 2) + pow(L1, 2) - pow(L2, 2) - pow(L3, 2) - 2 * a*L1*sin(j1) - 2 * b*L1*cos(j1)) / (2 * L2*L3));
    		//确定j1的角度后,就可以求出j3的角度。
    		m = L2 * sin(j1) + L3 * sin(j1)*cos(j3) + L3 * cos(j1)*sin(j3);
    		n = L2 * cos(j1) + L3 * cos(j1)*cos(j3) - L3 * sin(j1)*sin(j3);
    		t = a - L1 * sin(j1);
    		p = pow(pow(n, 2) + pow(m, 2), 0.5);
    		q = asin(m / p);
    		j2 = asin(t / p) - q;
    		//接着就可以求出j2的角度。
    		//求出4个角度后,我们先验算一遍这4个角度求出的值(x1,y1,z1)和我们的目标坐标(x,y,z)是否一致
    		x1 = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*cos(j0);
    		y1 = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*sin(j0);
    		z1 = L1 * cos(j1) + L2 * cos(j1 + j2) + L3 * cos(j1 + j2 + j3);
    		j1 = ANG2RAD(j1);
    		j2 = ANG2RAD(j2);
    		j3 = ANG2RAD(j3);
    		//输出误差小于1cm的解,并输出该解的正向解目标
    		if(x1<(x+1) && x1 > (x-1) && y1<(y + 1) && y1 >(y - 1) && z1<(z + 1) && z1 >(z - 1))
    		{
    			printf("j1:%f,j2:%f,j3:%f,x:%f,y:%f,z:%f\r\n", j1, j2, j3, x1, y1, z1);
    		}
    	}
    

    可执行的完整C++代码如下(直接复制这个用,上面的代码是过程讲解):

    #include "pch.h"
    #include <iostream>
    #include <stdio.h>
    #include <math.h>
    
    #define RAD2ANG (3.1415926535898/180.0)
    #define ANG2RAD(N) ( (N) * (180.0/3.1415926535898) )
    void inverseKinematics(double x, double y, double z);
    
    int main()
    {
    	inverseKinematics(0, 30, 0);//逆解目标为(0,30,0)这个坐标
    
    }
    
    void inverseKinematics(double x, double y, double z)
    {
    	double a, b, c; //临时变量
    	double L1 = 10, L2 = 11, L3 = 14;//3节手臂的长度
    	double m, n, t, q, p;//临时变量
    	double j1, j2, j3, j0;//4个舵机的旋转角度
    	double x1, y1, z1;//逆解后正解算出来的值,看是否与逆解值相等
    	char i = 0;
    	j0 = atan2(y, x);
    	a = x / cos(j0);
    	if (x == 0) a = y; //如果x为0,需要交换x,y
    	b = z;
    
    	for (j1 = -90; j1 < 90; j1++)
    	{
    		j1 *= RAD2ANG;
    		j3 = acos((pow(a, 2) + pow(b, 2) + pow(L1, 2) - pow(L2, 2) - pow(L3, 2) - 2 * a*L1*sin(j1) - 2 * b*L1*cos(j1)) / (2 * L2*L3));
    		//if (abs(ANG2RAD(j3)) >= 135) { j1 = ANG2RAD(j1); continue; }
    		m = L2 * sin(j1) + L3 * sin(j1)*cos(j3) + L3 * cos(j1)*sin(j3);
    		n = L2 * cos(j1) + L3 * cos(j1)*cos(j3) - L3 * sin(j1)*sin(j3);
    		t = a - L1 * sin(j1);
    		p = pow(pow(n, 2) + pow(m, 2), 0.5);
    		q = asin(m / p);
    		j2 = asin(t / p) - q;
    		//if (abs(ANG2RAD(j2)) >= 135) { j1 = ANG2RAD(j1); continue; }
    		/***************计算正解然后与目标解对比,看解是否正确**************/
    		x1 = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*cos(j0);
    		y1 = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*sin(j0);
    		z1 = L1 * cos(j1) + L2 * cos(j1 + j2) + L3 * cos(j1 + j2 + j3);
    		j1 = ANG2RAD(j1);
    		j2 = ANG2RAD(j2);
    		j3 = ANG2RAD(j3);
    		if (x1<(x + 1) && x1 >(x - 1) && y1<(y + 1) && y1 >(y - 1) && z1<(z + 1) && z1 >(z - 1))
    		{
    			printf("j0:%f,j1:%f,j2:%f,j3:%f,x:%f,y:%f,z:%f\r\n", ANG2RAD(j0), j1, j2, j3, x1, y1, z1);
    			i = 1;
    		}
    	}
    	for (j1 = -90; j1 < 90; j1++)//这个循环是为了求解另一组解,j2 = asin(t / p) - q;j2 = -(asin(t / p) - q);多了个负号
    	{
    		j1 *= RAD2ANG;
    		j3 = acos((pow(a, 2) + pow(b, 2) + pow(L1, 2) - pow(L2, 2) - pow(L3, 2) - 2 * a*L1*sin(j1) - 2 * b*L1*cos(j1)) / (2 * L2*L3));
    		//if (abs(ANG2RAD(j3)) >= 135) { j1 = ANG2RAD(j1); continue; }
    		m = L2 * sin(j1) + L3 * sin(j1)*cos(j3) + L3 * cos(j1)*sin(j3);
    		n = L2 * cos(j1) + L3 * cos(j1)*cos(j3) - L3 * sin(j1)*sin(j3);
    		t = a - L1 * sin(j1);
    		p = pow(pow(n, 2) + pow(m, 2), 0.5);
    		q = asin(m / p);
    		j2 = -(asin(t / p) - q);
    		//if (abs(ANG2RAD(j2)) >= 135) { j1 = ANG2RAD(j1); continue; }
    		x1 = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*cos(j0);
    		y1 = (L1 * sin(j1) + L2 * sin(j1 + j2) + L3 * sin(j1 + j2 + j3))*sin(j0);
    		z1 = L1 * cos(j1) + L2 * cos(j1 + j2) + L3 * cos(j1 + j2 + j3);
    		j1 = ANG2RAD(j1);
    		j2 = ANG2RAD(j2);
    		j3 = ANG2RAD(j3);
    		if (x1<(x + 1) && x1 >(x - 1) && y1<(y + 1) && y1 >(y - 1) && z1<(z + 1) && z1 >(z - 1))
    		{
    			printf("j0:%f,j1:%f,j2:%f,j3:%f,x:%f,y:%f,z:%f\r\n", ANG2RAD(j0), j1, j2, j3, x1, y1, z1);
    			i = 1;
    		}
    	}
    
    	if (i == 0)printf("无解");
    
    }
    

    输出结果如下图:
    在这里插入图片描述

    然后我们就可以根据我们的需求选择我们的解,如果只是简单抓取地面上的东西,就选择夹子向下且最接近垂直的那个解即j3最接近90的那个解,这个解最适合夹取地面上的东西。如果是像视频中倒水一样,就得选择夹子为水平的解,具体是个什么约束关系你们可以自己推一推。反正你可以自由选择解。

    最后大家有不懂或者有更好建议的请加我

    这是我出的一个视觉机械臂的一个教程,大家看了就明白机械臂抓取的整个流程了

    QQ:965295412 WX:15310294950

    展开全文
  • 运动学逆解优化

    2014-04-03 10:14:42
    MATLAB环境下六自由度焊接机器人运动学逆解及优化
  • 部分解耦机器人的运动学逆解新方法
  • 机器人运动学逆解

    千次阅读 2019-11-24 10:44:10
    机器人运动学逆解 即根据工具坐标系相对于基坐标系的目标位姿,求解机器人各关节角。逆运动学在机器人学中占有非常重要的地位,是机器人轨迹规划和运动控制的基础,直接影响着控制的快速性与准确性。一般机器人运动...

    机器人运动学逆解

    即根据工具坐标系相对于基坐标系的目标位姿,求解机器人各关节角。逆运动学在机器人学中占有非常重要的地位,是机器人轨迹规划和运动控制的基础,直接影响着控制的快速性与准确性。一般机器人运动学逆解算法可分为以下几种: 解析法 ( 又称反变换法) 、几 何 法 和 数 值 解法。由 PAUL 等提出的反变换法求解过程直观,因而被广泛采纳,但其求解过程中需多次进行齐次变换矩阵的逆运算和 4 × 4 维矩阵的乘积运算,导致求解过程复杂耗时。

    展开全文
  • 机器人基础之运动学逆解

    千次阅读 2020-08-06 23:11:09
    机器人基础之运动学逆解概述求解腕点位置求解腕部方位*z-y-z*欧拉角具体求解算例MATLAB实现 概述 运动学逆解是指已知机器人末端位姿,求解各运动关节的位置,它是机器人运动规划和轨迹控制的基础。 以机械臂为例,其...

    概述

    运动学逆解是指已知机器人末端位姿,求解各运动关节的位置,它是机器人运动规划和轨迹控制的基础。
    以机械臂为例,其运动学逆解的求法主要有两类:数值解和解析解。
    数值解法只能求出方程的特解,不能求出所有的解。
    它的优点是计算简单,不需要进行矩阵操作;缺点是由于使用了迭代法(如牛顿迭代),实时性较差。
    这里主要介绍解析解法。
    研究发现,如果串联机械臂在结构上满足下面两个充分条件中的一个,就会有解析解。这两个充分条件也被称为Pieper准则,即:

    1. 三个相邻关节轴线交于一点
    2. 三个相邻关节轴线相互平行

    现代绝大多数工业机械臂都满足Pieper准则的第一个充分条件,即机械臂末端的三个关节的轴线相交于一点,该点通常称为腕点。
    满足第二个充分条件的机器人较少,最典型的例子是SCARA机器人。之前我所在的公司应用SCARA构型的机器人进行工件的上下料。
    假设机械臂满足Pieper准则的第一个充分条件,则机械臂的运动学方程可以写为60T=30T63T {^0_6}T={^0_3}T{^3_6}T其中, 30T\ {^0_3}T规定了腕点的位置, 63T\ {^3_6}T规定了腕部的方位。因此,运动学逆解也分为两步,先求解θ1\theta_1θ2\theta_2θ3\theta_3(腕点位置),再求解θ4\theta_4θ5\theta_5θ6\theta_6(腕部方位)。

    求解腕点位置

    将固连在机械臂腕部的三个坐标系{4}、{5}、{6}的原点设在腕点,则腕点相对于基坐标系{0}的位置为:0P6o=0P4o=10T21T32T3P4o {^0}P_6o= {^0}P_4o={^0_1}T{^1_2}T{^2_3}T {^3}P_4o其中, 3P4o\ {^3}P_4o即为变换矩阵 43T\ {^3_4}T的第4列,即0P4o=10T21T32T[a3d4sinθ3d4cosθ31](n) {^0}P_4o={^0_1}T{^1_2}T{^2_3}T \left[ \begin{matrix} a_3\\ -d_4 sin\theta_3\\ d_4 cos\theta_3\\ 1 \end{matrix} \right] \tag{n}[f1(θ3)f2(θ3)f3(θ3)1]=32T[a3d4sinθ3d4cosθ31](n) \left[ \begin{matrix} f_1(\theta_3) \\ f_2(\theta_3) \\ f_3(\theta_3) \\ 1 \end{matrix} \right] \tag{n}={^2_3}T \left[ \begin{matrix} a_3\\ -d_4 sin\theta_3\\ d_4 cos\theta_3\\ 1 \end{matrix} \right] 则有,f1(θ3)=a3cosθ3+d4sinθ3sinα3+a2f_1(\theta_3)=a_3 cos\theta_3 +d_4 sin\theta_3sin\alpha_3 +a_2 f2(θ3)=a3sinθ3cosα2d4cosθ3cosα2sinα3d4sinα2cosα3d3sinα2f_2(\theta_3)=a_3 sin\theta_3 cos\alpha_2-d_4 cos\theta_3 cos\alpha_2 sin\alpha_3 -d_4 sin\alpha_2 cos\alpha_3 -d_3 sin\alpha_2 f3(θ3)=a3sinθ3sinα2d4cosθ3sinα2sinα3+d4cosα2cosα3+d3cosα2f_3(\theta_3)=a_3 sin\theta_3 sin\alpha_2-d_4 cos\theta_3 sin\alpha_2 sin\alpha_3 +d_4 cos\alpha_2 cos\alpha_3 +d_3 cos\alpha_2 再令[g1(θ2,θ3)g2(θ2,θ3)g3(θ2,θ3)1]=21T[f1(θ3)f2(θ3)f3(θ3)1](n) \left[ \begin{matrix} g_1(\theta_2,\theta_3) \\ g_2(\theta_2,\theta_3) \\ g_3(\theta_2,\theta_3) \\ 1 \end{matrix} \right] \tag{n}={^1_2}T \left[ \begin{matrix} f_1(\theta_3) \\ f_2(\theta_3) \\ f_3(\theta_3) \\ 1 \end{matrix} \right] 则有,g1(θ2,θ3)=cosθ2f1sinθ2f2+a1g_1(\theta_2,\theta_3)=cos\theta_2 f_1-sin\theta_2 f_2 +a_1 g2(θ2,θ3)=sinθ2cosα1f1+cosθ2cosα1f2sinα1f3d2sinα1g_2(\theta_2,\theta_3)=sin\theta_2 cos\alpha_1f_1 + cos\theta_2 cos\alpha_1f_2 -sin\alpha_1 f_3 - d_2sin\alpha_1 g3(θ2,θ3)=sinθ2sinα1f1+cosθ2sinα1f2+cosα1f3+d2cosα1g_3(\theta_2,\theta_3)=sin\theta_2 sin\alpha_1f_1 + cos\theta_2 sin\alpha_1f_2 +cos\alpha_1 f_3 + d_2cos\alpha_1 因此 0P4o=10T[g1g2g31](n) {^0}P_4o={^0_1}T\left[ \begin{matrix} g_1\\ g_2\\ g_3\\ 1 \end{matrix} \right] \tag{n}由于坐标系{1}与基坐标系{0}原点重合、Z轴重合,仅存在绕Z轴的转动,因此,10T=[cosθ1sinθ100sinθ1cosθ10000100001] {^0_1}T= \left[ \begin{matrix} \cos\theta_1 & -\sin\theta_1 & 0& 0 \\ \sin\theta_1 & \cos\theta_1 & 0& 0 \\ 0 & 0 & 1 & 0\\ 0& 0& 0& 1 \end{matrix} \right]进一步可得,0P4o=[xyz1]=[cosθ1sinθ100sinθ1cosθ10000100001][g1g2g31]=[g1cosθ1g1sinθ1g1sinθ1+g1cosθ1g31] {^0}P_4o=\left[ \begin{matrix} x\\ y\\ z\\ 1 \end{matrix} \right]=\left[ \begin{matrix} \cos\theta_1 & -\sin\theta_1 & 0& 0 \\ \sin\theta_1 & \cos\theta_1 & 0& 0 \\ 0 & 0 & 1 & 0\\ 0& 0& 0& 1 \end{matrix} \right]\left[ \begin{matrix} g_1\\ g_2\\ g_3\\ 1 \end{matrix} \right] =\left[ \begin{matrix} g_1cos\theta_1- g_1sin\theta_1\\ g_1sin\theta_1+g_1cos\theta_1\\ g_3\\ 1 \end{matrix} \right]r=x2+y2+z2=g12+g22+g32r=x^2+y^2+z^2=g{^2_1}+g{^2_2}+g{^2_3} 并且z=g3z = g_3代入g1g_1g2g_2g3g_3的表达式,可得:r=(k1cosθ2+k2sinθ2)2a1+k3r=(k_1 cos\theta_2 +k_2 sin\theta_2)2a_1 +k_3 z=(k1sinθ2k2cosθ2)sinα1+k4z =(k_1 sin\theta_2 - k_2 cos\theta_2)sin\alpha_1 +k_4 其中,
    k1(θ3)=f1k_1(\theta_3)=f_1 k2(θ3)=f2k_2(\theta_3)=-f_2 k3(θ3)=f12+f22+f32+a12+d22+2d2f3k_3(\theta_3)=f{^2_1}+f{^2_2}+f{^2_3} + a{^2_1} + d{^2_2} +2d_2 f_3 k4(θ3)=f3cosα1+d2cosα1k_4(\theta_3)=f_3 cos\alpha_1 +d_2 cos\alpha_1 这里分三种情况进行讨论:

    1. a1=0a_1 =0时。r=k3(θ3)r=k_3 (\theta_3),因为rr已知,所以可以求出θ3\theta_3
    2. sinα1=0sin\alpha_1 =0时。z=k4(θ3)z=k_4 (\theta_3),因为zz已知,所以可以求出θ3\theta_3
    3. a1a_1sinα1sin\alpha_1均不为0时,有(rk3)2(2a1)2+(zk4)2sin2α1=k12+k22\frac{(r-k_3)^2}{(2a_1)^2} + \frac{(z-k_4)^2}{sin^2\alpha_1} = k{^2_1}+k{^2_2}这里所有变量均为θ3\theta_3的函数,求解关于θ3\theta_3的非线性方程即可得到θ3\theta_3。需要注意的是,更为准确地说,上述公式中所有变量均为sinθ3sin\theta_3cosθ3cos\theta_3的函数,可令u=tanθ32u=tan\frac{\theta_3}{2} cosθ=1u21+u2cos\theta=\frac{1-u^2}{1+u^2} sinθ=2u1+u2sin\theta=\frac{2u}{1+u^2} 由此,可将原非线性方程转化为关于变量uu的方程。解得uu之后再利用θ3=2arctanu\theta_3=2arctanu得到θ3\theta_3
      利用前面的公式r=(k1cosθ2+k2sinθ2)2a1+k3r=(k_1 cos\theta_2 +k_2 sin\theta_2)2a_1 +k_3 这里代入θ3\theta_3后,所有变量均为θ2\theta_2的函数,类似的,求解该非线性方程即可得到θ2\theta_2
      利用公式x=g1cosθ1g2sinθ1x=g_1 cos\theta_1 - g_2 sin\theta_1 代入θ2\theta_2θ3\theta_3,即可求得θ1\theta_1
      到此,求解得到了θ1\theta_1θ2\theta_2θ3\theta_3

    求解腕部方位

    z-y-z欧拉角

    假设某坐标系分别依次绕动坐标系的Z轴、Y轴、Z轴转动α\alphaβ\betaγ\gamma角度,则旋转矩阵为 R=[cosαsinα0sinαcosα0001][cosβ0sinβ010sinβ0cosβ][cosγsinγ0sinγcosγ0001]R=\left[ \begin{matrix} \cos\alpha& -\sin\alpha& 0 \\ \sin\alpha& \cos\alpha& 0 \\ 0 & 0 & 1 \end{matrix} \right] \left[ \begin{matrix} \cos\beta& 0 & \sin\beta\\ 0 & 1 & 0 \\ -\sin\beta& 0 & \cos\beta \end{matrix} \right]\left[ \begin{matrix} \cos\gamma& -\sin\gamma& 0 \\ \sin\gamma& \cos\gamma& 0 \\ 0 & 0 & 1 \end{matrix} \right] 具体计算得:R=[cosαcosβcosγsinαsinγcosαcosβsinγsinαcosγcosαsinβsinαcosβcosγ+cosαsinγsinαcosβsinγ+cosαcosγsinαsinβsinβcosγsinβsinγcosβ] R=\left[ \begin{matrix} \cos\alpha cos\beta cos\gamma - sin\alpha sin\gamma & -cos\alpha cos\beta sin\gamma-sin\alpha cos\gamma & cos\alpha sin\beta \\ sin\alpha cos\beta cos\gamma + cos\alpha sin\gamma & -sin\alpha cos\beta sin\gamma + cos\alpha cos\gamma & sin\alpha sin\beta \\ -sin\beta cos\gamma & sin\beta sin\gamma & cos\beta \end{matrix} \right] R=[r11r12r13r21r22r23r31r32r33] R=\left[ \begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix} \right] α\alphaβ\betaγ\gamma可由以下公式计算得到。β=atan2(r312+r322,r33)\beta=atan2(\sqrt{ r{^2_{31}} + r{^2_{32}}},r_{33}) α=atan2(r23,r13)\alpha=atan2(r_{23},r_{13}) γ=atan2(r32,r31)\gamma =atan2(r_{32},-r_{31})

    具体求解

    60T=30T63T {^0_6}T={^0_3}T{^3_6}T得到63T=30T160T {^3_6}T={^0_3}T^{-1}{^0_6}T 再由齐次变换矩阵和旋转矩阵的关系得到 63R{^3_6}R。.
    许多工业机器人腕部三关节配置成一个球面副,这种球面副可用z-y-z欧拉角的解出这三个关节角。因此利用上面介绍的方法可以求解出θ4\theta_4α\alpha)、θ5\theta_5θ\theta)、θ6\theta_6γ\gamma)。

    算例

    设六自由度机械臂的DH参数表如下。

    i a α\alpha d θ\theta
    1 0 0 0 θ1\theta_1
    2 -30 -90 0 θ2\theta_2
    3 340 0 0 θ3\theta_3
    4 -40 -90 388 θ4\theta_4
    5 0 90 0 θ5\theta_5
    6 0 -90 0 θ4\theta_4

    腕点在基坐标系{0}中的位置向量为 0P4o=[381.3151.819.5](n) {^0}P_4o= \left[ \begin{matrix} 381.3 \\ 151.8\\ 19.5 \end{matrix} \right] \tag{n} 在坐标系{3}的位置为3P4o=[403380](n) {^3}P_4o= \left[ \begin{matrix} -40\\ 338\\ 0 \end{matrix} \right] \tag{n}坐标系{6}相对于基坐标系{0}的齐次变换矩阵为:[00.57360.8192381.300.81920.5736151.810019.50001](n) \left[ \begin{matrix} 0& 0.5736& 0.8192& 381.3\\ 0& -0.8192& 0.5736& 151.8\\ 1& 0& 0& 19.5\\ 0& 0&0&1\\ \end{matrix} \right] \tag{n}

    MATLAB实现

    先封装两个函数calT和getKG。

    function Matrix_T = calT(DH_parameter, start_row, end_row)
    % 输入参数为DH参数表,参数表有四列,参数表的行数是运动轴的个数。比如六自由度机械臂的参数表为6行4列
    % 第一列是Z轴偏置;
    % 第二列是Z轴偏转角;
    % 第三列是X轴偏置;
    % 第四列是X轴偏转角;
    Matrix_T = eye(4);
    for row_index = start_row + 1 : end_row
        % 先绕X轴转theta_X, 再沿X轴平移trans_X,将两个坐标系的Z轴重合
        theta_X = DH_parameter(row_index, 2);
        trans_X = DH_parameter(row_index, 1);
        T_X = rotX(theta_X) * transX(trans_X);
        
        % 先绕Z轴转theta_Z, 再沿Z轴平移trans_Z,将两个坐标系的X轴重合
        theta_Z = DH_parameter(row_index, 4);
        trans_Z = DH_parameter(row_index, 3);
        T_Z = rotZ(theta_Z) * transZ(trans_Z);
       
        Matrix_T = Matrix_T * T_X * T_Z;
    end
    
    function [k1, k2, k3, k4, g1, g2, g3] = getKG(DH_parameter, P_3)
    % 计算k和g
    % DH_parameter: DH参数表
    % P_3:腕点坐标系3中的位置
    
    T_12 = calT(DH_parameter, 1, 2);
    T_23 = calT(DH_parameter, 2, 3);
    
    F_Matrix = T_23 * P_3;
    G_Matrix = T_12 * F_Matrix;
    
    f1 = F_Matrix(1, 1);
    f2 = F_Matrix(2, 1);
    f3 = F_Matrix(3, 1);
    
    g1 = G_Matrix(1, 1);
    g2 = G_Matrix(2, 1);
    g3 = G_Matrix(3, 1);
    
    a1 = DH_parameter(2, 1);
    alpha1 = DH_parameter(2, 2);
    d2 = DH_parameter(2, 3);
    
    k1 = f1;
    k2 = -f2;
    k3 = f1^2 + f2^2 + f3^2 + a1^2 + d2^2 + 2*d2*f3;
    k4 = f3*cos(alpha1) + d2*cos(alpha1);
    

    下面是主程序。

    syms theta1 theta2 theta3 theta4 theta5 theta6 
    
    DH_parameter = [
        0, 0, 0, theta1;
        -30, -90/180*pi, 0, theta2;
        340, 0, 0, theta3;
        -40, -90/180*pi, 338, theta4;
        0, 90/180*pi,0, theta5;
        0, -90/180*pi, 0, theta6;
        ];
        
    P_3 = [-40; 338; 0; 1];
    P_O = [381.3; 151.8; 19.5];
    x = P_O(1, 1);
    z = P_O(3, 1);
    r = P_O'* P_O;
    a1 = DH_parameter(2, 1);
    alpha1 = DH_parameter(2, 2);
    d2 = DH_parameter(2, 3);
    
    [k1, k2, k3, k4, g1, g2, g3] = getKG(DH_parameter, P_3);
    
    % 计算theta3
    s = '(r-k3)^2/4/a1^2+(z-k4)^2/sin(alpha1)^2=k1^2+k2^2'
    s = subs(s, 'r', r);
    s = subs(s, 'z', z);
    s = subs(s, 'a1', a1);
    s = subs(s, 'alpha1', alpha1);
    s = subs(s, 'k1', k1);
    s = subs(s, 'k2', k2);
    s = subs(s, 'k3', k3);
    s = subs(s, 'k4', k4);
    % 变量替换
    s = subs(s, 'cos(theta3)', '(1-u^2)/(1+u^2)');
    s = subs(s, 'sin(theta3)', '2*u/(1+u^2)');
    % 求解非线性方程
    u = solve(s, 'u');
    theta3 = double(2 * atan(u));
    

    计算得:

    theta3 =
    
        2.8628
        2.6414
        0.2646
        0.0432
    

    θ3=0.0432\theta_3=0.0432(弧度)。
    再计算θ2\theta_2

    theta3 = 0.0432;
    % 计算theta2
    s = 'r = (k1 * cos(theta2) + k2 * sin(theta2)) * 2 * a1 + k3';
    s = subs(s, 'a1', a1);
    s = subs(s, 'r', r);
    s = subs(s, 'k1', k1);
    s = subs(s, 'k2', k2);
    s = subs(s, 'k3', k3);
    s = subs(s, 'theta3', theta3);
    % 变量替换
    s = subs(s, 'cos(theta2)', '(1-u^2)/(1+u^2)');
    s = subs(s, 'sin(theta2)', '2*u/(1+u^2)');
    % 求解非线性方程
    u = solve(s, 'u');
    theta2 = double(2 * atan(u));
    

    计算得:

    theta2 =
    
       -0.9058
       -0.8272
    

    θ2=0.9058\theta_2=-0.9058(弧度)。
    再计算θ1\theta_1

    theta3 = 0.0432;
    theta2 = -0.9058;
    s = 'x = cos(theta1) * g1 - sin(theta1) * g2';
    s = subs(s, 'x', x);
    s = subs(s, 'g1', g1);
    s = subs(s, 'g2', g2);
    s = subs(s, 'theta3', theta3);
    s = subs(s, 'theta2', theta2);
    % 变量替换
    s = subs(s, 'cos(theta1)', '(1-u^2)/(1+u^2)');
    s = subs(s, 'sin(theta1)', '2*u/(1+u^2)');
    % 求解非线性方程
    u = solve(s, 'u');
    theta1 = double(2 * atan(u));
    

    计算得:

       theta1 =
    
        0.3795
       -0.3795
    

    θ1=0.3795\theta_1=0.3795(弧度)。
    下面计算θ4\theta_4θ5\theta_5θ6\theta_6
    先封装计算ZYZ欧拉角的函数

    function [alpha, beta, gamma] = calZYZEulerInv(T_Matrix)
    % 输入参数为DH参数表,参数表有四列,参数表的行数是运动轴的个数。比如六自由度机械臂的参数表为6行4列
    % T_Matrix:齐次变换矩阵;
    % alpha:第一次Z轴转角;
    % beta:Y轴转角;
    % gamma:第二次Z轴转角;
    
    R = T_Matrix(1:3, 1:3);
    
    r31 = R(3, 1);
    r32 = R(3, 2);
    r33 = R(3, 3);
    r13 = R(1, 3);
    r23 = R(2, 3);
    
    beta = double(atan2(sqrt(r31^2 + r32^2), r33));
    alpha = double(atan2(r23, r13));
    gamma = double(atan2(r32, -r31));
    

    下面是主程序。

    % 坐标系{6}相对于坐标系{0}
    T_06 = [
     0, 0.5736, 0.8192, 381.3;
     0, -0.8192, 0.5736, 151.8;
     1, 0, 0, 19.5;
     0, 0, 0,1]; 
     
    DH_parameter(1, 4) = 0.3795; % theta1
    DH_parameter(2, 4) = -0.9058; % theta2
    DH_parameter(3, 4) = 0.0432; % theta3
    
    T_03 = calT(DH_parameter, 0, 3); % 坐标系{3}相对于坐标系{0}
    T_36 = T_03 \ T_06; % 坐标系{6}相对于坐标系{3}
    [alpha, beta, gamma] = calZYZEulerInv(T_36);
    

    计算得到结果。

    alpha =
    
       0.8626
    
    
    beta =
    
       1.3394
    
    
    gamma =
    
      -1.5708
    

    至此,运动学逆解计算完成。
    六个关节角度分别为:θ1=21.7\theta_1=21.7^\circθ2=51.9\theta_2=-51.9^\circθ3=2.5\theta_3=2.5^\circθ4=49.4\theta_4=49.4^\circθ5=76.7\theta_5=76.7^\circθ6=90\theta_6=-90^\circ

    展开全文
  • 以8自由度凿岩机械臂为研究对象,提出了一种基于运动轨迹的求解多关节冗余机器人运动学逆解方法。首先构建求解机械臂逆运动学问题的BP神经网络模型,然后采用贝叶斯方法,把机械臂各关节在运动轨迹上的转动或移动...
  • 文中以研究分析基于神经网络的冗余机械臂运动学逆解相关知识为目的,通过构建专门的机械臂运动学方程,提出最佳柔顺性的相关法则,得到最优解,从而实现训练神经网络的目的,最终达到网络状态的稳定。研究结果显示,...
  • 为什么要运动学逆解 运动学逆解即根据工具坐标系相对于基坐标系的目标位姿,求解机器人各关节角。 逆运动学在机器人学中占有非常重要的地位,是机器人轨迹规划和运动控制的基础,直接影响着控制的快速性与准确性。...

    为什么要运动学逆解

    在这里插入图片描述
    运动学逆解即根据工具坐标系相对于基坐标系的目标位姿,求解机器人各关节角。
    逆运动学在机器人学中占有非常重要的地位,是机器人轨迹规划和运动控制的基础,直接影响着控制的快速性与准确性。一般机器人运动学逆解算法可分为以下几种: 解析法 ( 又称反变换法) 、几 何 法 和 数 值 解法。由 PAUL 等提出的反变换法求解过程直观,因而被广泛采纳,但其求解过程中需多次进行齐次变换矩阵的逆运算和 4 × 4 维矩阵的乘积运算,导致求解过程复杂耗时。

    在这里插入图片描述

    足端轨迹规划

    在机器人技术各个环节中,信息存在多样性,机器人作业也存在多样性。唯有机器人轨迹规划存在唯一性,机器人的使用都离不开轨迹规划与运动轨迹。随着现在机器人技术的发展,运动规划的原理已慢慢形成共识,对于关节规划逐渐存在主流技术和解决方案。主要会在避障等额外要求下做出规划需求。所以看似不起眼的机器人轨迹规划反而是所有机器人作业中绕不开的一个过程。而且轨迹规划是没有主流要求而言的。按照不同的要求需要不同的轨迹。

    在这里插入图片描述在这里插入图片描述
    在这里插入图片描述

    在这里插入图片描述

    课后题求逆解

    在这里插入图片描述

    解题方法

    import math
    
    def fun(x,y,l1,l2):
        l=math.sqrt(x*x+y*y)#求虚拟脚长度
        psail=math.asin(x/l)#脚角
        fail=math.acos((l*l+l1*l1-l2*l2)/(2*l*l1))#连杆分离角
        m1=fail-psail#右舵机弧度制
        m2=fail+psail#左舵机弧度制
        sita1=180*m1/math.pi#右舵机角度制
        sita2=180*m2/math.pi#左舵机角度制
        return(sita1,sita2)
    
    print(fun(30,100,35,80))
    

    将 L1=35,L2=80,x=30,y=100 输入算法
    求得:
    右舵机弧度制 0.37927814199991783
    左舵机弧度制 0.962191730955652
    右舵机角度制 21.731036798158822
    左舵机角度制 55.12952526614607

    南昌理工学院人工智能学院特种机器人研发中心实验室workshop项目实践平台

    展开全文
  • 为提高三分支空间机器人运动学逆解的计算精度和实时性,基于旋量指数积公式及空间雅可比矩阵,对三分支机器人运动学逆解迭代算法进行了研究,建立了通用的逆运动学求解模型。对于具有球腕结构的三分支机器人,给出了...
  • 导出了冗余度机器人基于优化力度的运动学逆解,并将优化力度函数用于冗余度机器人运动学优化控制。通过调整优化力度函数中自运动限制因子的取值,可以改变自运动速度在关节角速度中所占份额,实现人为地控制任务时间...
  • 四足机器人(二)---运动学逆解和步态规划

    千次阅读 多人点赞 2020-08-20 18:48:19
    四足机器人(二)---运动学逆解和步态规划运动学逆解步态规划MATLAB仿真 运动学逆解 其实运动学分为运动学正解和运动学逆解,二者有什么区别呢?因为在四足机器人中用的是12个舵机,所以运动学正解是已经知道运动...
  • matlab运动学逆解代码adv_robotics_homework matlab 中的高级机器人作业解决方案(2014)。 ##解决方案包括 正向运动学、逆向运动学、正向动力学(使用 Newton-Euler 算法)。 命名约定遵循 Siciliano 的书 ()。 ...
  • 计算机器人运动学逆解首先要考虑可解性(solvability),即考虑无解、多解等情况。在机器人工作空间外的目标点显然是无解的。对于多解的情况从下面的例子可以看出平面二杆机械臂(两个关节可以360°旋转)在工作空间内...
  • 其中运动学逆解直接输出给舵机,控制机器人的运动,因此运动学逆解很重要。 2 基本概念 2.1机械结构模型   对于8自由度机器人,其机械结构模型如下图所示。图中两个并联大腿L1L_1L1​杆分别由独立的舵机带动,假设...
  • 7自由度机械臂利用7个关节自由度控制末端 的6个位姿变量,具有冗余自由度,可以使一种末 端位姿对应关节角空间内的无限组解,大大提高了 操控的灵活性.冗余机械臂在实现末端位姿...解机械臂运动学逆解带来一定的困难.
  • 利用PowerCube可重构机器人模块组装了一个可变异的平面型串联机械手。基于杆组分析法对该机器人进行逆运动学分析,并...MATLAB软件的仿真结果证明了运动学逆解的合理性,为机器人的步态轨迹控制提供了依据和算法支持。
  • 用于matlab中的神经网络开发 rbf神经网络求解机器人的运动学逆解源码
  • 七自由度冗余机械臂运动学逆解与工作空间分析MATLAB实现冗余机械臂基本概念与典型结构冗余机械臂运动学逆解常用方法基于关节角参数化解析逆运动学求解基于臂型角参数化解析逆运动学求解基于臂型角参数化的冗余参数...
  • Delta并联机器人逆解程序,自己写的,正逆解完全可以对照上。
  • 2.运动学逆解:已知足端坐标,求舵机/电机转角。 二.足端轨迹规划 摆线方程: { x=r*(t-sint) y=r*(1-cost) [其中r为圆的半径,t是圆的半径所经过的弧度(滚动角),当t由0变到2π时,动点就画出了摆线的一支,称为一...
  • 运用回转变换张量法。求解了三个腕关节轴线相交于一点的6自由度喷涂机器人的运动学逆解, 并利用消元法简化了运动学逆解的求解过程。得出了较为简易的解析解。
  • 全方位移动全向轮、麦克纳姆轮底盘运动学逆解详解(内含电机输出方程) 分析不多,直接上代码,本人亲测可用
  •   今天,讲讲机器人运动学逆解中的一大“法宝”,最常用的三角方程: k1sin(θ)+k2cos(θ)=k3(1) k_1sin(\theta)+k_2cos(\theta)=k_3 \tag{1} k1​sin(θ)+k2​cos(θ)=k3​(1)   联立三角恒等式: sin2(θ)+cos...
  • 在VC6.0平台下基于C++语言编写的Delta机器人逆解,输入x y z坐标机可得到所需输入转角。
  • 采用了一种基于径向基函数(Radial Basic Function,RBF)网络的非线性拟合和系统辨识的方法,利用RBF神经网络的逼近能力较优、收敛速度快、非线性处理能力强等特点,可以有效地对冗余机械手的运动学逆问题进行求解...

空空如也

空空如也

1 2 3 4 5 ... 19
收藏数 372
精华内容 148
关键字:

运动学逆解