UR3机械臂正逆运动学详解及c++完整代码

最新推荐文章于 2024-05-15 03:21:25 发布

双珵

最新推荐文章于 2024-05-15 03:21:25 发布

阅读量1.2w

点赞数 35

文章标签：矩阵 c++

本文链接：https://blog.csdn.net/u011779517/article/details/111116938

版权

D-H参数表

$i$	$\alpha_{i-1}$	$d_{i}$	$a_{i-1}$	$\theta_{i}$
$1$	$0$	$d_{1}$	$0$	$\theta_{1}$
$2$	$90$	$0$	$0$	$\theta_{2}$
$3$	$0$	$0$	$a_{2}$	$\theta_{3}$
$4$	$0$	$d_{4}$	$a_{3}$	$\theta_{4}$
$5$	$90$	$d_{5}$	$0$	$\theta_{5}$
$6$	$- 90$	$d_{6}$	$0$	$\theta_{6}$

正运动学

公式推导

这里的 $s_{i}$ 代表 $\sin(\theta_i)$ ， $c_{i}$ 代表 $\cos(\theta_i)$ ，以及下文逆运动学中出现的 $s_{ij}$ 代表 $\sin(\theta_i+\theta_j)$

由于
${}^{i-1}_{i}T= \begin{bmatrix} c_{i} & -s_{i} & 0 & a_{i-1} \\ s_{i}c_{\alpha_{i-1}} & c_{i}c_{\alpha_{i-1}} & -s_{\alpha_{i-1}} & -s_{\alpha_{i-1}}d_{i} \\ s_{i}s_{\alpha_{i-1}} & c_{i}s_{\alpha_{i-1}} & c_{\alpha_{i-1}} & c_{\alpha_{i-1}}d_{i} \\ 0 & 0& 0&1 \end{bmatrix}\tag{*}$
所以我们可以得到六个变换矩阵（这是代入 $\alpha$ 、 $d$ 与 $a$ 中的零值后的结果）
${}^{0}_{1}T(\theta_{1})= \begin{bmatrix} c_{1} & -s_{1} & 0 & 0 \\ s_{1} & c_{1} & 0&0 \\ 0& 0 & 1& d_{1} \\ 0 & 0& 0&1\tag{1} \end{bmatrix}$
${}^{1}_{2}T(\theta_{2})= \begin{bmatrix} c_{2} & -s_{2} & 0 & 0 \\ 0 & 0 & -1&0 \\ s_{2} & c_{2} & 0&0 \\ 0 & 0& 0&1\tag{2} \end{bmatrix}$
${}^{2}_{3}T(\theta_{3})= \begin{bmatrix} c_{3} & -s_{3} & 0 & a_{2} \\ s_{3} & c_{3} & 0&0 \\ 0& 0 & 1& 0 \\ 0 & 0& 0&1\tag{3} \end{bmatrix}$
${}^{3}_{4}T(\theta_{4})= \begin{bmatrix} c_{4} & -s_{4} & 0 & a_{3} \\ s_{4} & c_{4} & 0&0 \\ 0& 0 & 1& d_{4} \\ 0 & 0& 0&1\tag{4} \end{bmatrix}$
${}^{4}_{5}T(\theta_{5})= \begin{bmatrix} c_{5} & -s_{5} & 0 & 0 \\ 0& 0 & -1&-d_{5} \\ s_{5} & c_{5} & 0&0 \\ 0 & 0& 0&1\tag{5} \end{bmatrix}$
${}^{5}_{6}T(\theta_{6})= \begin{bmatrix} c_{6} & -s_{6} & 0 & 0 \\ 0& 0 & 1&d_{6} \\ -s_{6} & -c_{6} & 0&0 \\ 0 & 0& 0&1\tag{6} \end{bmatrix}$
于是
${}^{0}_{6}T= {}^{0}_{1}T{}^{1}_{2}T{}^{2}_{3}T{}^{3}_{4}T{}^{4}_{5}T{}^{5}_{6}T\tag{7}$
${}^{0}_{6}T$ 中第四列的前三行数据就是机器人末端连杆在笛卡儿坐标系里的位置

其实在编程中无需把每一个变换矩阵的表达式计算出，只要按照式子 $(*)$ 把对应的值代入计算即可，每计算一个变换矩阵，都做一次矩阵相乘。（可以看最后的代码来理解）但在下文的逆运动学中我会具体写出每一步的计算结果，上面的式子算作一个铺垫

代码

因为逆运动学要涉及反代入检验，所以看后面逆运动学求解的代码就行了

测试

红色框为仿真环境给出的数据，下面六个角度就是我输入的六个 $\theta$ 的值
橙色框为程序计算出的位置，这里由于单位问题，两组值相差了 $10^{3}$ 倍
ur3正运动学求解范例

逆运动学

准备工作

首先推得
${}^{0}_{1}T^{-1}(\theta_{1})= \begin{bmatrix} c_{1} & s_{1} & 0 & 0 \\ -s_{1} & c_{1} & 0&0 \\ 0& 0 & 1& -d_{1} \\ 0 & 0& 0&1\tag{8} \end{bmatrix}$
然后逐步计算
${}^{1}_{3}T= {}^{1}_{2}T {}^{2}_{3}T= \begin{bmatrix} c_{23} & -s_{23} & 0 & c_2a_2 \\ 0 &0 & -1&0 \\ s_{23} & c_{23} & 0 & s_2a_2 \\ 0 & 0& 0&1\tag{9} \end{bmatrix}$
${}^{1}_{4}T= {}^{1}_{3}T {}^{3}_{4}T= \begin{bmatrix} c_{234} & -s_{234} & 0 & c_{23}a_3+c_2a_2 \\ 0 &0 & -1&-d_4 \\ s_{234} & c_{234} & 0 & s_{23}a_3+s_2a_2 \\ 0 & 0& 0&1\tag{10} \end{bmatrix}$
${}^{1}_{5}T= {}^{1}_{4}T {}^{4}_{5}T= \begin{bmatrix} c_{234}c_5 & -c_{234}s_5 & s_{234} & s_{234}d_5+c_{23}a_3+c_2a_2 \\ -s_5 &-c_5 & 0&-d_4 \\ s_{234}c_5 & -s_{234}s_5 & -c_{234} & -c_{234}d_5+s_{23}a_3+s_2a_2 \\ 0 & 0& 0&1\tag{11} \end{bmatrix}$
${}^{1}_{6}T= {}^{1}_{5}T {}^{5}_{6}T= \begin{bmatrix} c_{234}c_5c_6-s_{234}s_6 & -c_{234}c_5s_6-s_{234}c_6 & -c_{234}s_5 & -c_{234}s_5d_6+s_{234}d_5+c_{23}a_3+c_2a_2 \\ -s_5c_6 &s_5s_6 & -c_5&-c_5d_6-d_4 \\ s_{234}c_5c_6+c_{234}s_6 & -s_{234}c_5s_6+c_{234}c_6 & -s_{234}s_5 & -s_{234}s_5d_6-c_{234}d_5+s_{23}a_3+s_2a_2 \\ 0 & 0& 0&1 \end{bmatrix}\tag{12}$
由于

${}^{0}_{6}T= {}^{0}_{1}T{}^{1}_{2}T{}^{2}_{3}T{}^{3}_{4}T{}^{4}_{5}T{}^{5}_{6}T= {}^{0}_{1}T{}^{1}_{6}T\tag{13}$
所以有

${}^{0}_{1}T^{-1} {}^{0}_{6}T={}^{1}_{6}T\tag{14}$
其中， ${}^{0}_{6}T$ 是我们可以根据末端位姿获得的，我们设
${}^{0}_{6}T= \begin{bmatrix} r_{11} & r_{12} & r_{13} & x \\ r_{21} & r_{22} & r_{23}&y \\ r_{31}& r_{32} & r_{33}& z \\ 0 & 0& 0&1\tag{15} \end{bmatrix}$
因此式 $(14)$ 左边矩阵相乘的结果为
${}^{0}_{1}T^{-1} {}^{0}_{6}T= \begin{bmatrix} c_1r_{11}+s_1r_{21} & c_1r_{12}+s_1r_{22} & c_1r_{13}+s_1r_{23} & c_1x+s_1y \\ -s_1r_{11}+c_1r_{21} & -s_1r_{12}+c_1r_{22} & -s_1r_{13}+c_1r_{23}&-s_1x+c_1y \\ r_{31}& r_{32} & r_{33}& z-d_1 \\ 0 & 0& 0&1\tag{16} \end{bmatrix}$
至此，求逆解的所有准备工作完成了，下面开始逐个计算 $\theta$ 的值

求逆解的总思路是：通过比对式 $(12)$ 和式 $(16)$ 两个矩阵获得等式，然后消元，最后用 $a t a n 2 ()$ 求解

求解 $\theta_{1}$

由于式 $(16)$ 中的未知数只有 $\theta_{1}$ ，因此我们选择先计算出 $\theta_{1}$ 的值。在式 $(12)$ 中，我们观察到第二行第三列（后文都简写为(2,3)这种形式）和(2,4)只有未知数 $\theta_5$ ，因此两个未知数两个方程，可解。
于是有
$\begin{cases} (2,3): \ -s_1r_{13}+c_1r_{23}=-c_5 \\ (2,4): \ -s_1x+c_1y=-c_5d_6-d_4 \tag{17} \end{cases}$
消去 $\theta_5$ 得
$-(r_{13}d_6-x)s_1+(r_{23}d_6-y)c_1=d_4\tag{18}$
为什么要写成这种形式？
因为求解过程中我们发现，这样的形式可以有一个计算公式，即
若有
${-As_1+Bc_1=C}\tag{19}$
则
$\theta_1=atan2(B,A)-atan2(C,\pm\sqrt{A^2+B^2-C^2})\tag{20}$
根据式 $(18)$ ，可以确定这里的
$\begin{aligned} &A=r_{13}d_6-x \\ &B=r_{23}d_6-y \\ &C =d_4\tag{21} \end{aligned}$
于是我们可以计算出 $\theta_1$ 的两个解

求解 $\theta_{5}$

由于前面求解 $\theta_1$ 时我们也已经找到了 $\theta_5$ 的方程，因此当 $\theta_1$ 解出来后， $\theta_5$ 也就自然而然地能解了
$\ -s_1r_{13}+c_1r_{23}=-c_5\tag{22}$
所以有
$c_5=s_1r_{13}-c_1r_{23}\tag{23}$
继而有
$s_5= \pm\sqrt{1-(s_1r_{13}-c_1r_{23})^2}\tag{24}$
因此
$\theta_5=atan2(\pm\sqrt{1-(s_1r_{13}-c_1r_{23})^2},s_1r_{13}-c_1r_{23})\tag{25}$
因为 $\theta_1$ 有两个解，所以 $\theta_5$ 有 $2\times 2=4$ 个解

求解 $\theta_{6}$

我们已经求解出 $\theta_1$ 与 $\theta_5$ ，观察式 $(12)$ 能发现， $\theta_2\theta_3\theta_4$ 牵扯在一块，与此同时(2,1)和(2,2)就显得十分简洁，因此我们第三步求解 $\theta_6$
$\begin{cases} (2,1): \ -s_1r_{11}+c_1r_{21}=-s_5c_6 \\ (2,2): \ -s_1r_{12}+c_1r_{22}=s_5s_6 \tag{26} \end{cases}$
整理得
$\begin{aligned} &s_6=\frac{-s_1r_{12}+c_1r_{22}}{s_5} \\\tag{27} &c_6=\frac{s_1r_{11}-c_1r_{21}}{s_5} \end{aligned}$
因此
$\theta_6=atan2(\frac{-s_1r_{12}+c_1r_{22}}{s_5} ,\frac{s_1r_{11}-c_1r_{21}}{s_5})\tag{28}$
计算 $\theta_6$ 时只需要代入同一组的 $\theta_1$ 与 $\theta_5$ 就行，这边不会产生额外的解

其实因为我们找到了两个方程，所以可以通过消去 $\theta_5$ 的方式化作上面的式 $(19)$ 用公式计算，这说明当 $\theta_1$ 解出来后， $\theta_5$ 与 $\theta_6$ 应该是同时可以解出来的，不分先后顺序

求解 $\theta_2$ 、 $\theta_3$ 、 $\theta_4$

先求解 $\theta_{234}$

观察矩阵可以看到， $\theta_2$ 、 $\theta_3$ 、 $\theta_4$ 基本都在一起，所以这三个要用整体的思想去看待与求解，我们这里先求解 $\theta_{234}$ ，即 $\theta_2+\theta_3 +\theta_4$
由
$\begin{cases} (1,3): \ c_1r_{13}+s_1r_{23}=-c_{234}s_5 \\ (3,3): \ r_{33}=-s_{234}s_5 \tag{29} \end{cases}$
可得
$\begin{aligned} &s_{234}=-\frac{r_{33}}{s_5} \\\tag{30} &c_{234}=-\frac{c_1r_{13}+s_1r_{23}}{s_5} \end{aligned}$
因此
$\theta_2+\theta_3 +\theta_4=atan2(-\frac{r_{33}}{s_5},-\frac{c_1r_{13}+s_1r_{23}}{s_5})\tag{31}$
这边也只需要代入相同组的解，不会产生额外的解
在编程计算中，这里算出来的结果和实际值相差180°，暂未找出错误

再求解 $\theta_{2}$

由
$\begin{cases} (1,4): \ -c_{234}s_5d_6+s_{234}d_5+c_{23}a_3+c_2a_2=c_1x+s_1y \\ (3,4): \ -s_{234}s_5d_6-c_{234}d_5+s_{23}a_3+s_2a_2=z-d_1 \tag{32} \end{cases}$
我们已经求解出 $\theta_{1}$ 、 $\theta_{5}$ 、 $\theta_{234}$ ，所以可以把常量拿字母替代掉，让式子变得整洁一点
设
$\begin{aligned} &A=-c_{234}s_5d_6+s_{234}d_5 \\ &B=-s_{234}s_5d_6-c_{234}d_5 \\ &C=c_1x+s_1y \\ \tag{33} &D =z-d_1 \end{aligned}$
则式 $(32)$ 可化简为
$\begin{cases} c_{23}a_3=C-A-c_2a_2 \\ s_{23}a_3=D-B-s_2a_2 \tag{34} \end{cases}$
这里再设
$\begin{aligned} &M=C-A \\ &N=D-B \tag{35} \end{aligned}$
对式 $(34)$ 两个等式平方相加消去 $\theta_{23}$ ，整理得

$-(-2Na_2)s_2+2Ma_2c_2=M^2+N^2+a_2^2-a_3^2\tag{36}$
根据式 $(19) (20)$ 设
$\begin{aligned} &E=-2Na_2 \\ &F=2Ma_2 \tag{37} \\ &G=M^2+N^2+a_2^2-a_3^2 \end{aligned}$
得
$\theta_2=atan2(F,E)-atan2(G,\pm\sqrt{E^2+F^2-G^2})\tag{38}$
这里在同一组解的情况下， $\theta_2$ 会产生额外的一个解，此时解的组数为 $4\times2=8$

然后反解 $\theta_{23}$

由式 $(34) (35)$ 得
$\begin{aligned} &s_{23}=\frac{N-s_2a_2}{a_3} \\ \tag{39} &c_{23}=\frac{M-c_2a_2}{a_3} \end{aligned}$
因此
$\theta_{23}=atan2(\frac{N-s_2a_2}{a_3} ,\frac{M-c_2a_2}{a_3})\tag{40}$

最后求解 $\theta_3$ 、 $\theta_4$

$\theta_3=\theta_{23}-\theta_{2}\tag{41}$ $\theta_4=\theta_{234}-\theta_{23}\tag{42}$
因为这里的 $\theta_3$ 、 $\theta_4$ 是直接相减得到的，因此也不会产生额外的解

完整代码

Eigen是一个方便矩阵计算（当然不止这个功能）的库，可以去官网下载：http://eigen.tuxfamily.org/

#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

//ur3数据,这是学长给我的数据，适用于我的仿真环境，大家可以根据自己的环境进行修改
const double d[6+1] = { 0,0.1519,0,0,0.11235,0.08535,0.0819 };//第0个不用
const double a[6] = { 0,0,-0.24365,-0.21325,0,0 };//a有0，没有6
const double alpha[6] = { 0,90,0,0,90,-90 };//alpha有0，没有6
double theta[8 + 1][6 + 1];//八组解，每组解六个角，第0个都不用

//正运动学，用于检验
void kinematics(double* theta_input)
{
	Eigen::Matrix4d T[6 + 1];//为了和theta对应，0不用
	for (int i = 1; i <= 6; i++)
	{
		T[i](0, 0) = cos(theta_input[i]);
		T[i](0, 1) = -sin(theta_input[i]);
		T[i](0, 2) = 0;
		T[i](0, 3) = a[i - 1];
		T[i](1, 0) = sin(theta_input[i]) * cos(alpha[i - 1] / 180 * EIGEN_PI);
		T[i](1, 1) = cos(theta_input[i]) * cos(alpha[i - 1] / 180 * EIGEN_PI);
		T[i](1, 2) = -sin(alpha[i - 1] / 180 * EIGEN_PI);
		T[i](1, 3) = -sin(alpha[i - 1] / 180 * EIGEN_PI) * d[i];
		T[i](2, 0) = sin(theta_input[i]) * sin(alpha[i - 1] / 180 * EIGEN_PI);
		T[i](2, 1) = cos(theta_input[i]) * sin(alpha[i - 1] / 180 * EIGEN_PI);
		T[i](2, 2) = cos(alpha[i - 1] / 180 * EIGEN_PI);
		T[i](2, 3) = cos(alpha[i - 1] / 180 * EIGEN_PI) * d[i];
		T[i](3, 0) = 0;
		T[i](3, 1) = 0;
		T[i](3, 2) = 0;
		T[i](3, 3) = 1;
	}
	Eigen::Matrix4d T06 = T[1] * T[2] * T[3] * T[4] * T[5] * T[6];
	cout << "检验得：X=" << T06(0, 3) * 1000 << "    Y=" << T06(1, 3) * 1000 << "     Z=" << T06(2, 3) * 1000;
}
int main()
{
	double x, y, z, RX, RY, RZ;
	printf("Please input x,y,z,RX,RY,RZ:\n");
	scanf("%lf%lf%lf%lf%lf%lf", &x, &y, &z, &RX, &RY, &RZ);
	//由于仿真软件的单位不同，这边进行转换
	x = x / 1000;
	y = y / 1000;
	z = z / 1000;

	//1.旋转向量转旋转矩阵，即求T06中的r
	Eigen::Vector3d v(RX, RY, RZ);
	double t_alpha = v.norm();//求模
	v.normalize();//标准化
	Eigen::AngleAxisd rv(t_alpha, v);//旋转向量
	Eigen::Matrix3d rm;
	rm = rv.matrix();
	
	//2.求解
	double A, B, C, D, E, F, G, M, N;//用大写字母替代常数

	//注意，由于数组下标从0开始的问题，矩阵第一行第一列的元素是(0,0)
	//theta1
	A = rm(0, 2) * d[6] - x;
	B = rm(1, 2) * d[6] - y;
	C = d[4];
	//第一个解，赋给一到四组
	theta[1][1] = atan2(B, A) - atan2(C, sqrt(A * A + B * B - C * C));
	theta[2][1] = theta[1][1];
	theta[3][1] = theta[1][1];
	theta[4][1] = theta[1][1];
	//第二个解，赋给五到八组
	theta[5][1] = atan2(B, A) - atan2(C, -sqrt(A * A + B * B - C * C));
	theta[6][1] = theta[5][1];
	theta[7][1] = theta[5][1];
	theta[8][1] = theta[5][1];

	//theta5
	//由theta[1][1]产生的第一个解，赋给一到二组
	A = sin(theta[1][1]) * rm(0, 2) - cos(theta[1][1]) * rm(1, 2);
	theta[1][5] = atan2(sqrt(1 - A * A), A);
	theta[2][5] = theta[1][5];
	//由theta[1][1]产生的第二个解，赋给三到四组
	theta[3][5] = atan2(-sqrt(1 - A * A), A);
	theta[4][5] = theta[3][5];
	//由theta[5][1]产生的第一个解，赋给五到六组
	A = sin(theta[5][1]) * rm(0, 2) - cos(theta[5][1]) * rm(1, 2);
	theta[5][5] = atan2(sqrt(1 - A * A), A);
	theta[6][5] = theta[5][5];
	//由theta[5][1]产生的第二个解，赋给七到八组
	theta[7][5] = atan2(-sqrt(1 - A * A), A);
	theta[8][5] = theta[7][5];

	//theta6
	for (int i = 1; i <= 8; i++)
	{
		A = (-sin(theta[i][1]) * rm(0, 1) + cos(theta[i][1]) * rm(1, 1)) / theta[i][5];
		B = (sin(theta[i][1]) * rm(0, 0) - cos(theta[i][1]) * rm(1, 0)) / theta[i][5];
		theta[i][6] = atan2(A, B);
	}

	//theta2、theta3、theta4
	for (int i = 1; i <= 8; i = i + 2)
	{
		//先算theta2+theta3+theta4
		double theta234[8 + 1];
		A = rm(2, 2) / sin(theta[i][5]);
		B = (cos(theta[i][1]) * rm(0, 2) + sin(theta[i][1]) * rm(1, 2)) / sin(theta[i][5]);
		theta234[i] = atan2(-A, -B) - EIGEN_PI;
		theta234[i + 1] = theta234[i];

		//消去theta2+theta3，计算theta2
		A = -cos(theta234[i]) * sin(theta[i][5]) * d[6] + sin(theta234[i]) * d[5];
		B = -sin(theta234[i]) * sin(theta[i][5]) * d[6] - cos(theta234[i]) * d[5];
		C = cos(theta[i][1]) * x + sin(theta[i][1]) * y;
		D = z - d[1];
		M = C - A;
		N = D - B;
		E = -2 * N * a[2];
		F = 2 * M * a[2];
		G = M * M + N * N + a[2] * a[2] - a[3] * a[3];
		theta[i][2] = atan2(F, E) - atan2(G, sqrt(E * E + F * F - G * G));
		theta[i + 1][2] = atan2(F, E) - atan2(G, -sqrt(E * E + F * F - G * G));

		//用theta2反求theta2+theta3
		double theta23[8 + 1];
		theta23[i] = atan2((N - sin(theta[i][2]) * a[2]) / a[3], (M - cos(theta[i][2]) * a[2]) / a[3]);
		theta23[i + 1] = atan2((N - sin(theta[i + 1][2]) * a[2]) / a[3], (M - cos(theta[i + 1][2]) * a[2]) / a[3]);

		//theta3
		theta[i][3] = theta23[i] - theta[i][2];
		theta[i + 1][3] = theta23[i + 1] - theta[i + 1][2];

		//theta4
		theta[i][4] = theta234[i] - theta23[i];
		theta[i + 1][4] = theta234[i + 1] - theta23[i + 1];
	}

	//输出并检验
	for (int i = 1; i <= 8; i++)
	{
		cout << "第" << i << "组解：" << endl;
		for (int j = 1; j <= 6; j++)
			cout << "theta" << j << "=" << theta[i][j] * 180 / EIGEN_PI << "  ";
		cout << endl;
		kinematics(theta[i]);
		cout << endl << endl;
	}
}

测试结果

(数据皆从仿真环境获得，不同的环境可能有不一样的数据配置)

测试数据1
输入： -118.43 -268.05 157.28 0.001 -3.166 -0.040
一组解为： -91.71 -98.96 -126.22 -46.29 91.39 358.22

在这里插入图片描述
测试数据2
输入： -63.78 -201.25 137.28 0.192 3.109 0.036
一组解为： -76.28 -83.49 -151.01 -36.32 91.85 20.76

不足之处

没有考虑角度为0°或90°的特殊情况，这边只是计算出了普遍的结果
一些角度计算出来和实际值相差360°，虽然不影响计算，但说明程序不够完善

代码仅供学习交流使用，如有错误或者疑问可以在评论区提出。转载请标明出处！

双珵

关注

35
点赞
踩
240

收藏

觉得还不错? 一键收藏
23
评论
UR3机械臂正逆运动学详解及c++完整代码

时间不足，这边先贴上代码，以后会完善全部计算过程与思路。博主也是刚开始学习机械臂，不断搜索网上的教程，找论文，费尽千辛万苦终于计算正确，学习之路太过坎坷Eigen是一个方便矩阵计算（当然不止这个功能）的库，可以去官网下载：http://eigen.tuxfamily.org/#include <iostream>#include <Eigen/Dense>using namespace std;using namespace Eigen;//ur3数据const d
复制链接

扫一扫