【现代机器人学——螺旋理论】UR5机器人正运动学求解

某先森不吃鱼

已于 2022-01-24 01:28:12 修改

阅读量6.2k

点赞数 29

分类专栏：现代机器人学——螺旋理论文章标签：线性代数几何学算法

于 2022-01-23 23:15:28 首次发布

本文链接：https://blog.csdn.net/qq_23297513/article/details/122655805

版权

现代机器人学——螺旋理论专栏收录该内容

4 篇文章 2 订阅

订阅专栏

系列文章目录

【现代机器人学——螺旋理论】UR5机器人正运动学求解
 【现代机器人学——螺旋理论】UR5机器人逆运动学求解

提示：本文介绍基于螺旋理论分析的UR5机器人正运动学求解思路，仅供参考。

前言

”让经典旋量理论惠及更多的人群’这—突破发生在20世纪80年代初°哈佛大学的RogerBrockett基于刚体运动的李群结构向读者展示了如何以数学方式来描述运动链（Brockett, l983b）。该发现的重要性在于’只是简单地通过线性代数与线性微分方程的基本理论来重构旋量理论。有了“现代旋量理论’’这一现代微分几何领域的强大工具’就可以涉及范围更为广泛的机器人问题’其中部分内容将展现在我们这本教材中’其他更优秀，更高级的内容可参考Murray等（l994）的教科书。“
——摘自《MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL》

简单说就是把课程大作业顺便整理发了

参考文章：
《MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL》
《UR机械臂正逆运动学求解》与此文章理论方法、坐标系以及参数定义不太相同，可将xy方向数据取反验证。

一、参考机器人坐标

在这里插入图片描述

变量	数值
$L_1$	0.425
$L_2$	0.3922
$W_1$	0.1333
$W_2$	0.0996
$H_1$	0.1625
$H_2$	0.0997

二、正运动学

1.定义工作末端

首先我们针对旋量理论支撑的运动模型末端进行定义
${g_{st}}\left( 0 \right) = \left[ {\begin{array}{ccc} {{L_1} + {L_2}}\\ {{W_1} + {W_2}}\\ {{H_1} - {H_2}}\\ 1 \end{array}} \right]$

其矩阵形式表示为
$\left[ {\begin{array}{ccc} R&{\left[ {\begin{array}{ccc} {{L_1} + {L_2}}\\ {{W_1} + {W_2}}\\ {{H_1} - {H_2}} \end{array}} \right]}\\ 0&1 \end{array}} \right] = \left[ {\begin{array}{ccc} { - 1}&0&0&{{L_1} + {L_2}}\\ 0&0&1&{{W_1} + {W_2}}\\ 0&1&0&{{H_1} - {H_2}}\\ 0&0&0&1 \end{array}} \right]$

2.定义运动轴

UR5机器人共拥有6个旋转轴，分辨根据其参数进行定义

螺旋方向
${\xi _1} = \left[ {\begin{array}{ccc} 0\\ 0\\ 1 \end{array}} \right],{\xi _2} = \left[ {\begin{array}{ccc} 0\\ 1\\ 0 \end{array}} \right],{\xi _3} = \left[ {\begin{array}{ccc} 0\\ 1\\ 0 \end{array}} \right],{\xi _4} = \left[ {\begin{array}{ccc} 0\\ 1\\ 0 \end{array}} \right],{\xi _5} = \left[ {\begin{array}{ccc} 0\\ 0\\ { - 1} \end{array}} \right],{\xi _6} = \left[ {\begin{array}{ccc} 0\\ 1\\ 0 \end{array}} \right]$

螺旋方向
${q_1} = \left[ {\begin{array}{ccc} 0\\ 0\\ {{H_1}} \end{array}} \right]\;,{q_2} = \left[ {\begin{array}{ccc} 0\\ 0\\ {{H_1}} \end{array}} \right]\;,{q_3} = \left[ {\begin{array}{ccc} {{L_1}}\\ 0\\ {{H_1}} \end{array}} \right]\;,{q_4} = \left[ {\begin{array}{ccc} {{L_1} + {L_2}}\\ 0\\ {{H_1}} \end{array}} \right]\;,{q_5} = \left[ {\begin{array}{ccc} {{L_1} + {L_2}}\\ {{W_1}}\\ {{H_1}} \end{array}} \right]\;,{q_6} = \left[ {\begin{array}{ccc} {{L_1} + {L_2}}\\ {{W_1}}\\ {{H_1} - {H_2}} \end{array}} \right]$

螺旋方向
${v_1} = \left[ {\begin{array}{ccc} 0\\ 0\\ 0 \end{array}} \right]\;,{v_2} = \left[ {\begin{array}{ccc} { - {H_1}}\\ 0\\ 0 \end{array}} \right]\;,{v_3} = \left[ {\begin{array}{ccc} { - {H_1}}\\ 0\\ {{L_1}} \end{array}} \right]\;,{v_4} = \left[ {\begin{array}{ccc} { - {H_1}}\\ 0\\ {{L_1} + {L_2}} \end{array}} \right]\;,{v_5} = \left[ {\begin{array}{ccc} { - {W_1}}\\ {{L_1} + {L_2}}\\ 0 \end{array}} \right]\;,{v_6} = \left[ {\begin{array}{ccc} {{H_2} - {H_1}}\\ 0\\ {{L_1} + {L_2}} \end{array}} \right]$

3.定义螺旋变换矩阵

${e^{\left[ {{S_1}} \right]{\theta _1}}} = \left[ {\begin{array}{ccc} {{c_1}}&{ - {s_1}}&0&0\\ {{s_1}}&{{c_1}}&0&0\\ 0&0&1&0\\ 0&0&0&1 \end{array}} \right]$

${e^{\left[ {{S_2}} \right]{\theta _2}}} = \left[ {\begin{array}{ccc} {{c_2}}&0&{{s_2}}&{ - {H_1}{s_2}}\\ 0&1&0&0\\ { - {s_2}}&0&{{c_2}}&{{H_1}(1 - {c_2})}\\ 0&0&0&1 \end{array}} \right]$

${e^{\left[ {{S_3}} \right]{\theta _3}}} = \left[ {\begin{array}{ccc} {{c_3}}&0&{{s_3}}&{{L_1}\left( {1 - {c_3}} \right) - {H_1}{s_3}}\\ 0&1&0&0\\ { - {s_3}}&0&{{c_3}}&{{H_1}\left( {1 - {c_3}} \right) + {L_1}{s_3}}\\ 0&0&0&1 \end{array}} \right]$

${e^{\left[ {{S_4}} \right]{\theta _4}}} = \left[ {\begin{array}{ccc} {{c_4}}&0&{{s_4}}&{({L_1} + {L_2})\left( {1 - {c_4}} \right) - {H_1}{s_4}}\\ 0&1&0&0\\ { - {s_4}}&0&{{c_4}}&{{H_1}\left( {1 - {c_4}} \right) + ({L_1} + {L_2}){s_4}}\\ 0&0&0&1 \end{array}} \right]$

${e^{\left[ {{S_5}} \right]{\theta _5}}} = \left[ {\begin{array}{ccc} {{c_5}}&{{s_5}}&0&{\left( {{L_1} + {L_2}} \right)\left( {1 - {c_5}} \right) - {W_1}{s_5}}\\ { - {s_5}}&{{c_5}}&0&{{W_1}\left( {1 - {c_5}} \right) + \left( {{L_1} + {L_2}} \right){s_5}}\\ 0&0&1&0\\ 0&0&0&1 \end{array}} \right]$

${e^{\left[ {{S_6}} \right]{\theta _6}}} = \left[ {\begin{array}{ccc} {{c_6}}&0&{{s_6}}&{({L_1} + {L_2})\left( {1 - {c_6}} \right) - ({H_1} - {H_2}){s_6}}\\ 0&1&0&0\\ { - {s_6}}&0&{{c_6}}&{({H_1} - {H_2})\left( {1 - {c_6}} \right) + ({L_1} + {L_2}){s_6}}\\ 0&0&0&1 \end{array}} \right]$

4.基于螺旋理论的UR5机器人正向运动学公式

$T(\theta ) = {e^{\left[ {{S_1}} \right]{\theta _1}}}{e^{\left[ {{S_2}} \right]{\theta _2}}}{e^{\left[ {{S_3}} \right]{\theta _3}}}{e^{\left[ {{S_4}} \right]{\theta _4}}}{e^{\left[ {{S_5}} \right]{\theta _5}}}{e^{\left[ {{S_6}} \right]{\theta _6}}}M$

完全表达式（可能有误，应该不会有人用这个吧，不会吧不会吧）
$\left[ {\begin{array}{ccc} {{r_{11}}}&{{r_{12}}}&{{r_{13}}}&{{p_1}}\\ {{r_{21}}}&{{r_{22}}}&{{r_{23}}}&{{p_2}}\\ {{r_{31}}}&{{r_{32}}}&{{r_{33}}}&{{p_3}}\\ 0&0&0&1 \end{array}} \right] = \left[ {\begin{array}{ccc} { - {c_1}{c_{234}}{c_5}{c_6} + {c_1}{s_{234}}{s_6} + {s_1}{s_5}{c_6}}&{ - {c_1}{c_{234}}{c_5}{s_6} + {c_1}{s_{234}}{c_6} + {s_1}{s_5}{s_6}}&{{c_1}{c_{234}}{s_5} + {s_1}{c_5}}&{{L_2}{c_1}{c_{23}} + {L_1}{c_1}{c_2} - {H_2}{c_1}{s_{234}} + {W_2}{c_1}{c_{234}}{s_5} + {W_1}{s_1} + {W_2}{s_1}{c_5}}\\ {{s_1}{c_{234}}{c_5}{c_6} - {s_1}{s_{234}}{s_6} + {c_1}{s_5}{c_6}}&{{s_1}{c_{234}}{c_5}{s_6} - {s_1}{s_{234}}{c_6} + {c_1}{s_5}{s_6}}&{ - {s_1}{c_{234}}{s_5} + {c_1}{c_5}}&{ - {L_2}{s_1}{c_{23}} - {L_1}{s_1}{c_2} + {H_2}{s_1}{s_{234}} - {W_2}{s_1}{c_{234}}{s_5} + {W_1}{c_1} + {W_2}{c_1}{c_5}}\\ {{s_{234}}{c_5}{c_6} + {c_{234}}{s_6}}&{{s_{234}}{c_5}{s_6} + {c_{234}}{c_6}}&{ - {s_{234}}{s_5}}&{{H_1} - {H_2}{c_{234}} - {L_2}{s_{23}} - {L_1}{s_2} - {W_2}{s_{234}}{s_5}}\\ 0&0&0&1 \end{array}} \right]$

三、简单的代码实现

临时写的测试小代码，仅供参考。
这里我将所有关节旋转 $\frac{\pi}{2}$ 来校验是否正确，大抵正确。
时间原因，没有设计完整功能的函数，据称MR机器人官网有完整旋量计算库，感兴趣的同学欢迎交流！

#include <iostream>
#include "math.h"

using namespace std;

#define M_PI_2 3.1415926535 / 2

#define L_1 0.425
#define L_2 0.3922
#define W_1 0.1333
#define W_2 0.0996
#define H_1 0.1625
#define H_2 0.0997

#define Trans_Mat_1(theta) Transformation_Matrices(Trans_Mat[0], 2, theta, 0, 0, H_1);
#define Trans_Mat_2(theta) Transformation_Matrices(Trans_Mat[1], 1, theta, 0, 0, H_1);
#define Trans_Mat_3(theta) Transformation_Matrices(Trans_Mat[2], 1, theta, L_1, 0, H_1);
#define Trans_Mat_4(theta) Transformation_Matrices(Trans_Mat[3], 1, theta, L_1 + L_2, 0, H_1);
#define Trans_Mat_5(theta) Transformation_Matrices(Trans_Mat[4], 2, -theta, L_1 + L_2, W_1, H_1);
#define Trans_Mat_6(theta) Transformation_Matrices(Trans_Mat[5], 1, theta, L_1 + L_2, W_1, H_1 - H_2);

float Trans_Mat[6][4][4] = {0};


float M[4][4] = { {-1, 0, 0, L_1 + L_2},
					{ 0, 0, 1, W_1 + W_2 },
					{ 0, 1, 0, H_1 - H_2 },
					{ 0, 0, 0, 1 } };

void Transformation_Matrices(float T[4][4], int axis, float theta, float q_x, float q_y, float q_z)//仅接受与坐标轴平行的旋转计算，其他的我还在学，0 1 2分别代表x y z
{
	int i = 0;
	memset(T, 0, sizeof(T));
	float temp_p[4] = { 0 };
	float temp_sin, temp_cos = 0;
	temp_cos = cos(theta);
	temp_sin = sin(theta);
	T[axis % 3][axis % 3] = 1;
	T[(axis + 1) % 3][(axis + 1) % 3] = temp_cos;
	T[(axis + 2) % 3][(axis + 2) % 3] = temp_cos;
	T[(axis + 1) % 3][(axis + 2) % 3] = -temp_sin;
	T[(axis + 2) % 3][(axis + 1) % 3] = temp_sin;
	
	T[3][3] = 1;

	T[0][3] = q_x - T[0][0] * q_x - T[0][1] * q_y - T[0][2] * q_z;
	T[1][3] = q_y - T[1][0] * q_x - T[1][1] * q_y - T[1][2] * q_z;
	T[2][3] = q_z - T[2][0] * q_x - T[2][1] * q_y - T[2][2] * q_z;

	return;
}

void ArrMul_4(float dst[4][4], float arr1[4][4], float arr2[4][4])
{
	float temp[4][4] = {0};
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			for (int k = 0; k < 4; k++)
			{
				temp[i][j] += arr1[i][k] * arr2[k][j];
			}
		}

	}
	memcpy(dst, temp, sizeof(temp));
	return;
}

void Forwad_Kinematics(float T[4][4], float theta[6])
{
	float temp = 0;
	Trans_Mat_1(theta[0]);
	Trans_Mat_2(theta[1]);
	Trans_Mat_3(theta[2]);
	Trans_Mat_4(theta[3]);
	Trans_Mat_5(theta[4]);
	Trans_Mat_6(theta[5]);

	memcpy(T, M, sizeof(M));

	for (int k = 0; k < 6; k++)
		ArrMul_4(T, Trans_Mat[5-k], T);
}

int main()
{
	float T[4][4];
	float theta[6] = { M_PI_2, M_PI_2, M_PI_2, M_PI_2, M_PI_2, M_PI_2 };
	Forwad_Kinematics(T, theta);
	for (int k = 0; k < 6; k++)
	{
		printf("第%d个变换矩阵\r\n", k+1);
		for (int i = 0; i < 4; i++)
		{
			for (int j = 0; j < 4; j++)
			{
				printf("%3.3f ", Trans_Mat[k][i][j]);
			}
			printf("\r\n");
		}
	}

	printf("末端矩阵\r\n");
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			printf("%.3f ", T[i][j]);
		}
		printf("\r\n");
	}
}