【基于机械臂触觉伺服的物体操控研究】UR5e运动学建模及代码实现

煜个头头

发布时间 2023.04.08阅读数 2022 评论数 0

在这里插入图片描述
我的毕设题目定为《基于机械臂触觉伺服的物体操控研究》，这个系列主要用于记录做毕设的过程。

前言：UR系列是优傲公司的代表产品，也是目前比较通用的产品级机械臂。所以我打算用该机械臂进行毕设的仿真实现。关于其运动学建模，网上有很多参考的博客及文献，但是很多都与《机器人学导论》第四版中的建模方式有所出入（不是指建模有问题，只是坐标系定义不太一致）。所以我按照自己的理解进行了运动学模型搭建，并使用webots进行了仿真验证。

1. 正运动学

模型的坐标系建立如下图所示。
在这里插入图片描述

DH参数表

注：列表的长度参考了webots中的模型，可能跟实体的参数有所出入。

a	$\alpha$	d
0	0	0.1625
0		0
0.425	0	0
0.3922	0	0.127
0		0.1
0		0.1

求解

将DH参数列表转为变换矩阵
$[\begin{matrix} 1 & 0 & 0 & a \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$

[\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & c o s (α) & - s i n (α) & 0 \\ 0 & s i n (α) & c o s (α) & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

[\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d \\ 0 & 0 & 0 & 1 \end{matrix}]

[\begin{matrix} c o s (θ) & - s i n (θ) & 0 & 0 \\ s i n (θ) & c o s (θ) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

坐标系变换

代码实现

#include <vector>
#include <eigen3/Eigen/Dense>

std::vector<Vec4<T>> getDHparams(void){
	/* define link length */
	_worldBaseDistance = 0.1625;
	_shoulderLinkLength = 0.138;  
	_upperarmLinkLength = 0.425; 
	_forearmLinkLength = 0.3922;   
	_wrist1LinkLength = 0.127; 
	_wrist2LinkLength = 0.1;   
	_wrist3LinkLength = 0.1;
	    
	Vec4<T> dh_param;
	std::vector<Vec4<T>> DH_params;
	DH_params.clear();
	dh_param << 0,0, _worldBaseDistance, 0;                         DH_params.push_back(dh_param);
	dh_param << 0, -M_PI_2, 0, -M_PI_2;                             DH_params.push_back(dh_param);
	dh_param << _upperarmLinkLength, 0, 0, 0;                       DH_params.push_back(dh_param);
	dh_param << _forearmLinkLength, 0, _wrist1LinkLength, M_PI_2;   DH_params.push_back(dh_param);
	dh_param << 0, M_PI_2, _wrist2LinkLength, 0;                    DH_params.push_back(dh_param);
	dh_param << 0, -M_PI_2, _wrist3LinkLength, 0;                   DH_params.push_back(dh_param);
}

/**
 * @brief forward kinematics
 * @param q joints angle 1 - 6
 * @param p data order is x, y, z, pitch, roll, yaw
 */
template <typename T>
void forwardKinematics(const Vec6<T>& q, Vec6<T>* p){
	std::vector<Vec4<T>> DH_params = getDHparams();
    Mat4<T> Pro_Mat[4];
    Mat4<T> Trans_Mat = Mat4<T>::Identity();
    for(int i = 0; i < JOINTS_NUM; i ++){
        /* init */
        for(int j = 0; j < 4; j ++){
            Pro_Mat[j] = Mat4<T>::Identity();
        }
        /* create transform matrix refer to DH params */
        Mat2<T> rot;

        /* a trans */
        T a = DH_params[i][0];
        Pro_Mat[0](0, 3) = a;

        /* alpha trans */
        T alpha = DH_params[i][1];
        rot << cos(alpha), -sin(alpha), sin(alpha), cos(alpha);
        Pro_Mat[1].block(1, 1, 2, 2) = rot;

        /* d trans */
        T d = DH_params[i][2];
        Pro_Mat[2](2, 3) = d;

        /* theta trans */
        T theta = q[i] + DH_params[i][3];
        rot << cos(theta), -sin(theta), sin(theta), cos(theta);
        Pro_Mat[3].block(0, 0, 2, 2) = rot;
        
        /* combine trans matrix */
        Trans_Mat =  Trans_Mat * Pro_Mat[0] * Pro_Mat[1] * Pro_Mat[2] * Pro_Mat[3];

    }

    /* xyz */
    p->operator()(0) = Trans_Mat(0, 3);
    p->operator()(1) = Trans_Mat(1, 3);
    p->operator()(2) = Trans_Mat(2, 3);

    /* rpy */
    Mat3<T> rot = Trans_Mat.block(0,0,3,3);
    Vec3<T> rpy = ori::rotationMatrixToRPY(rot);
    p->operator()(3) = rpy(0);
    p->operator()(4) = rpy(1);
    p->operator()(5) = rpy(2);
}

2. 逆运动学

因为UR5e满足Pieper准则（末端三个关节轴相交于一点），所以其逆运动学具有封闭解。这里采用解析法进行求解。求解过程参考如下的文章，因为建模方式不同，求解的公式也有一定的区别，但思路一致。
UR机械臂正逆运动学求解

角度求解用到的公式(推导过程参考上述文章)
已知：

可求得：

参数定义
$[\begin{matrix} r_{x} & o_{x} & l_{x} & p_{x} \\ r_{y} & o_{y} & l_{y} & p_{y} \\ r_{z} & o_{z} & l_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{matrix}]$

以下的推导公式中， $c_1$ 表示，表示

求解 $\theta_1$ , $\theta_5$ , $\theta_6$

根据变换矩阵公式可得以下关系

等式左边
$[\begin{matrix} r_{x} c_{1} c_{6} - o_{x} c_{1} s_{6} + r_{y} c_{6} s_{1} - o_{y} s_{1} s_{6} & l_{x} c_{1} + l_{y} s_{1} & - o_{x} c_{1} c_{6} - o_{y} c_{6} s_{1} - r_{x} c_{1} s_{6} - r_{y} s_{1} s_{6} & p_{x} c_{1} + p_{y} s_{1} - d_{6} l_{x} c_{1} - d_{6} l_{y} s_{1} \\ r_{y} c_{1} c_{6} - o_{y} c_{1} s_{6} - r_{x} c_{6} s_{1} + o_{x} s_{1} s_{6} & l_{y} c_{1} - l_{x} s_{1} & o_{x} c_{1} s_{6} - o_{y} c_{1} c_{6} - r_{y} c_{1} s_{6} + r_{x} s_{1} s_{6} & p_{y} c_{1} - p_{x} s_{1} - d_{6} l_{y} c_{1} + d_{6} l_{x} s_{1} \\ r_{z} c_{6} - o_{z} s_{6} & l_{z} & - o_{z} c_{6} - r_{z} s_{6} & p_{z} - d_{1} - d_{6} l_{z} \\ 0 & 0 & 0 & 1 \end{matrix}]$

等式右边
$[\begin{matrix} c_{234} c_{5} & - c_{234} s_{5} & s_{234} & a_{4} s_{23} + a_{3} s_{2} + d_{5} s_{234} \\ s_{5} & c_{5} & 0 & d_{4} \\ - s_{234} c_{5} & s_{234} s_{5} & c_{234} & a_{4} c_{23} + a_{3} c_{2} + d_{5} c_{234} \\ 0 & 0 & 0 & 1 \end{matrix}]$

求解

利用等式左右两边第二行，第四列对应相等进行求解。

求解

利用等式左右两边第二行，第二列对应相等进行求解。

求解

利用等式左右两边第二行，第一列对应相等进行求解。

求解 , ,

根据变换矩阵公式可得以下关系

等式左边
$[\begin{matrix} r_{x} c_{1} c_{5} c_{6} - l_{y} s_{1} s_{5} - l_{x} c_{1} s_{5} - o_{x} c_{1} c_{5} s_{6} + r_{y} c_{5} c_{6} s_{1} - o_{y} c_{5} s_{1} s_{6} & o_{x} c_{1} c_{6} + o_{y} c_{6} s_{1} + r_{x} c_{1} s_{6} + r_{y} s_{1} s_{6} & l_{x} c_{1} c_{5} + l_{y} c_{5} s_{1} + r_{x} c_{1} c_{6} s_{5} - o_{x} c_{1} s_{5} s_{6} + r_{y} c_{6} s_{1} s_{5} - o_{y} 1_{1} s_{5} s_{6} & p_{x} c_{1} + p_{y} s_{1} - d_{6} l_{x} c_{1} - d_{6} l_{y} s_{1} + d_{5} r_{y} s_{1} s_{6} + d_{5} o_{x} c_{1} c_{6} + d_{5} o_{y} c_{6} s_{1} + d_{5} r_{x} c_{1} s_{6} \\ r_{y} c_{1} c_{5} c_{6} + l_{x} s_{1} s_{5} - l_{y} c_{1} s_{5} - o_{y} c_{1} c_{5} s_{6} - r_{x} c_{5} c_{6} s_{1} + o_{x} c_{5} s_{1} s_{6} & o_{y} c_{1} c_{6} - o_{x} c_{6} s_{1} + r_{y} c_{1} s_{6} - r_{x} s_{1} s_{6} & l_{y} c_{1} c_{5} - l_{x} c_{5} s_{1} + r_{y} c_{1} c_{6} s_{5} - o_{y} c_{1} s_{5} s_{6} - r_{x} c_{6} s_{1} s_{5} + o_{x} 1_{1} s_{5} s_{6} & p_{y} c_{1} - p_{x} s_{1} - d_{6} l_{y} c_{1} + d_{6} l_{x} s_{1} - d_{5} r_{x} s_{1} s_{6} + d_{5} o_{y} c_{1} c_{6} - d_{5} o_{x} c_{6} s_{1} + d_{5} r_{y} c_{1} s_{6} \\ r_{z} c_{5} c_{6} - l_{z} s_{5} - o_{z} c_{5} s_{6} & o_{z} c_{6} + r_{z} s_{6} & l_{z} c_{5} + r_{z} c_{6} s_{5} - o_{z} s_{5} s_{6} & p_{z} - d_{1} - d_{6} l_{z} + d_{5} o_{z} c_{6} + d_{5} r_{z} s_{6} \\ 0 & 0 & 0 & 1 \end{matrix}]$ 等式右边
$[\begin{matrix} c_{234} & - s_{234} & 0 & a_{4} s_{23} + a_{3} s_{2} \\ 0 & 0 & 1 & d_{4} \\ - s_{234} & - c_{234} & 0 & a_{4} c_{23} + a_{3} c_{2} \\ 0 & 0 & 0 & 1 \end{matrix}]$

求解

利用等式左右两边第一行，第四列，以及第三行，第四列对应相等进行求解。

因为

则

求解

将（1）（2）式进行展开

整理得

$(a_4c_3+a_3)s_2+(a_4s_3)c_2 =m$
解得

求解

利用等式左右两边第三行，第一列，以及第三行，第二列对应相等进行求解。

则

奇异点

$θ_{5} = 0$ 时， $θ_{2} ， θ_{3} ， θ_{4} 与 θ_{6}$ 平行，此时需要自定义的值使系统可解。
时，可以为任何值。

对于解析解而言，只要找出求解公式中超出函数定义域或者值域的取值，并将其重新定义为有效值即可避免到达奇异位点。当然，前提是保证目标点位于机械臂的工作空间内，

结果筛选

因为上述推导公式可以得到8组解，所以需要筛选出最合适的那个解进行输出。筛选的方法要根据情景而定，一般是选择与当前位姿最接近的解或者与最优位姿最接近的解。
下面给出一种筛选思路。

参数定义
逆运动学求取的一组解
：参考点
$：$ 权重系数
x，y两点的距离函数

对每组解分别求取误差

选取最优解

权重系数可以设置为各个驱动关节对应的连杆长度，这样子越长的连杆权重越大，最终得到的结果会趋向于移动短连杆，节省能量。

距离函数可以选择为绝对值距离或者欧氏距离等。

代码实现

/**
 * @brief inverse kinematics
 * @param p data order is x, y, z, pitch, roll, yaw
 * @param q joints angle 1 - 6
 */
template <typename T>
bool inverseKinematics(const Vec6<T>& p, Vec6<T>* q, const Vec6<T>* ref_q){
    /* convert to rotation matrix */
    Vec3<T> rpy(p(3), p(4), p(5));
    Mat3<T> rot =  ori::rpyToRotMat(rpy);
    T rx = rot(0, 0); T ox = rot(0, 1); T lx = rot(0, 2); T px = p(0);
    T ry = rot(1, 0); T oy = rot(1, 1); T ly = rot(1, 2); T py = p(1);
    T rz = rot(2, 0); T oz = rot(2, 1); T lz = rot(2, 2); T pz = p(2);

    /* define DH params */
    T a3 = DH_params[2][0];
    T a4 = DH_params[3][0]; 
    T d1 = DH_params[0][2];
    T d4 = DH_params[3][2];
    T d5 = DH_params[4][2];
    T d6 = DH_params[5][2];

    /* consider multiple solution */
    Vec6<T> solve_angles[8];
    for(int i = 0; i < 8; i ++){
        /* change sign */
        T signs[3];
        for(int s = 0; s < 3; s ++){
            signs[s] = i & (1 << s) ? -1 : 1; 
        }
        
        /* theta1 */
        T m = py - d6 * ly; 
        T n = px - d6 * lx;
        T verify = pow(m,2) + pow(n,2) - pow(d4,2);
        if(verify < 0) {
            verify = 0;
        }
        T t1 = atan2(m, n) - atan2(d4, signs[0] * sqrt(verify));
    
        /* theta5 */
        T t5 = signs[1] * acos(ly * cos(t1) - lx * sin(t1));

        /* theta6 */
        m = ry * cos(t1) - rx * sin(t1);
        n = oy * cos(t1) - ox * sin(t1);
        T t6;
        if(t5 == 0){
            t6 = 0;
        }
        else{
            t6 = atan2(m, n) - atan2(sin(t5), 0);
        }

        /* theta3 */
        m = px*cos(t1) + py*sin(t1) - d6*lx*cos(t1) - d6*ly*sin(t1) + d5*ry*sin(t1)*sin(t6)
            + d5*ox*cos(t1)*cos(t6) + d5*oy*cos(t6)*sin(t1) + d5*rx*cos(t1)*sin(t6);
        n = pz - d1 - d6*lz + d5*oz*cos(t6) + d5*rz*sin(t6);
        verify = (pow(m,2) + pow(n,2) - pow(a3,2) - pow(a4,2)) / (2*a3*a4);
        if(verify > 1) {
            verify = 1;
        }
        else if(verify  < -1) {
            verify = -1;
        }
        T t3 = signs[2] * acos(verify);

        /* theta2 */
        T s2 = (m * (a4*cos(t3) + a3) - a4*sin(t3)*n) / (pow(a4,2) + 2*a4*a3*cos(t3) + pow(a3,2));
        T c2 = (n + a4*sin(t3)*s2) / (a4*cos(t3) + a3);
        T t2 = atan2(s2, c2);

        /* theta4 */
        T s234 = -rz*cos(t5)*cos(t6) + lz*sin(t5) + oz*cos(t5)*sin(t6);
        T c234 = -oz*cos(t6) - rz*sin(t6);
        T t4 = atan2(s234, c234) - t2 - t3;

        solve_angles[i] << t1, t2, t3, t4, t5, t6;
    }

    /* filter optimal solution */
    if(ref_q == NULL){
        *q = solve_angles[0];
    }
    else{
        uint8_t optimal_index = 0;
        T min_err = 0; 
        for(int i = 0; i < 8; i ++){
            T err = 0;
            for(int j = 0; j < JOINTS_NUM; j ++){
                err += abs(ref_q->operator()(j) - solve_angles[i][j]) * (*links_length[j]);
            }
            if(!i){
                min_err = err;
            }
            else if(err < min_err){
                min_err = err;
                optimal_index = i;
            }
        }
        *q = solve_angles[optimal_index];
    }
    return true;
}

逆运动学机械臂 C++Eigen 学习笔记

打赏 0

上一篇：【基于机械臂触觉伺服的物体操控】论文研读《Goal-Driven Robotic Pushing Using Tactile and Proprioceptive Feedback》

下一篇：【LVGL学习笔记】（一）环境搭建

【基于机械臂触觉伺服的物体操控研究】UR5e运动学建模及代码实现

煜个头头

1. 正运动学

DH参数表

求解

代码实现

2. 逆运动学

求解 θ 1 \theta_1 θ1​, θ 5 \theta_5 θ5​, θ 6 \theta_6 θ6​

求解 θ 1 ​

求解 θ 5 ​

求解 θ 6 ​

求解 θ 2 , θ 3 , θ 4 ​

求解 θ 3 ​

求解 θ 2 ​

求解 θ 4 ​

奇异点

结果筛选

代码实现

为你推荐

聊聊协作机器人中柔顺控制那点事儿

机器人路径规划之Dijkstra算法

基于Python的3R机器人运动仿真

现代机器人：力学，规划，控制（chapter2）自由度计算，拓扑学表示，三大空间

四足机器人质心准静态控制器（一）

机器人在2D平面下的位姿描述-安逸的机器人学

评论（0）

关于作者

煜个头头

15

0

150

2

精选【LVGL学习笔记】（三）控件使用

【LVGL学习笔记】（四）PlatformIO + LVGL8.3配置

并联四足机器人项目开源教程（二）--- 基于ROS搭建控制架构

相关推荐

法兰在基座下的速度描述与工具在工件下的速度描述之间关系

一起来开发一个磁条机器人（七）

精选04轨迹规划基础----正逆运动学的C++代码编写

机器人学——学习笔记14(Cubic Polynomials)

干货 | 机械臂的动力学（二）：拉格朗日法

运动规划入门 | 4. 白话RRT，从原理到Matlab实现

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

求解 $\theta_1$ , $\theta_5$ , $\theta_6$

求解

求解

求解

求解 , ,

求解

求解

求解

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板