0 引言

末端柔顺控制必然牵扯到机械臂逆运动学求解。

注：由于本文涉及到矩阵运算，需要事先下载好Eigen矩阵运算库，可自行搜索如何安装。

1 工程目录

RoboticsLibbin(存放可执行文件)include(存放头文件)Commom.hDH_config.hFrameTransform.hKinematics.hsrc(存放源代码)FrameTransform.cppKinematics.cpptest.cpplib(存放生成的库文件)build(编译生成的中间文件)CMakeLists.txt

2 头文件

（1）Commom.h

#pragma once
#include <Eigen/Core>typedef Eigen::Matrix<double, 6, 1> JointsState;
typedef Eigen::Matrix<double, 3, 1> Points;
typedef Eigen::Matrix<double, 4, 4> TransMat;
typedef Eigen::Matrix<double, 3, 3> RotMat;

主要给后续需要用到的一些矩阵取别名。

JointState: 表示关节状态，一共6个关节，为6行1列矩阵；

Points：表示一个点，3行1列矩阵；

TransMat: 坐标系变换齐次矩阵，4行4列矩阵；

RotMat: 坐标系变换的旋转矩阵，3行3列。

上述定义的矩阵后续不一定全部被使用。

（2）DH_config.h

#pragma once
#include <vector>
#include <cmath>namespace UR5_DH
{std::vector<double> a = {0, -0.425, -0.39225, 0, 0, 0};std::vector<double> alpha = {M_PI / 2, 0, 0, M_PI / 2, -1 * M_PI / 2, 0};std::vector<double> d = {0.089459, 0, 0, 0.10915, 0.09465, 0.0823};
}

本文档定义了UR5机器人的DH参数，后续如有需要，与UR系列相同构型的机械臂的DH参数也可以在此定义，后续解算运动学的时候可直接传入DH参数。

（3）FrameTransform.h

/* The API of Frame transformation */
#pragma once
#include <Eigen/Core>
#include "Common.h"namespace FT
{TransMat rotx(double angle);TransMat roty(double angle);TransMat rotz(double angle);TransMat trans(double x, double y, double z);
}

声明了几个函数，主要是求解沿着不同坐标轴旋转或平移时的齐次变换矩阵。

（4）Kinematics.h

#pragma once
#include <vector>
#include "Common.h"class Kinematics
{
public:/* a, alpha and d are the DH parameters */Kinematics(std::vector<double> a, std::vector<double> alpha, std::vector<double> d);public:std::vector<double> a;std::vector<double> alpha;std::vector<double> d;public:bool FK(const JointsState &joints_angle, TransMat &mat);bool IK(const TransMat &mat, std::vector<JointsState> &inverse_solutions);
};

声明了一个运动学类，结构比较简单，成员变量为DH参数，两个成员函数分别计算运动学正解和逆解。

注意：两个成员函数都是返回bool值，计算成功为true，正解FK函数计算结果保存在TransMat &mat变量中，逆解IK计算结果保存在std::vector<JointsState> &inverse_solutions变量中。

此外，逆解一共有8组解，每一组解均为JointState，即6行1列的矩阵，因此使用std::vector<JointsState>来保存结果。

3 源代码

（1）FrameTransform.cpp

对应上述的FrameTransform.h

#include "FrameTransform.h"
#include <cmath>TransMat FT::rotx(double angle)
{TransMat mat;mat << 1, 0, 0, 0,0, cos(angle), -1 * sin(angle), 0,0, sin(angle), cos(angle), 0,0, 0, 0, 1;return mat;
}TransMat FT::roty(double angle)
{TransMat mat;mat << cos(angle), 0, sin(angle), 0,0, 1, 0, 0,-1 * sin(angle), 0, cos(angle), 0,0, 0, 0, 1;return mat;
}TransMat FT::rotz(double angle)
{TransMat mat;mat << cos(angle), -1 * sin(angle), 0, 0,sin(angle), cos(angle), 0, 0,0, 0, 1, 0,0, 0, 0, 1;return mat;
}TransMat FT::trans(double x, double y, double z)
{TransMat mat;mat << 1, 0, 0, x,0, 1, 0, y,0, 0, 1, z,0, 0, 0, 1;return mat;
}

比较简单，求解就是坐标系不同的变换相应的齐次变换矩阵。

（2）Kinematics.cpp

#include "Kinematics.h"
#include "FrameTransform.h"
#include <iostream>
#include <cmath>void limit_angle(Eigen::Matrix<double, 8, 1> &angles)
{for (int i = 0; i <= 7;i++){while(true){if(angles[i] < -1 * M_PI){angles[i] += 2 * M_PI;}else if(angles[i] > M_PI){angles[i] -= 2 * M_PI;}else{break;}}}
}Kinematics::Kinematics(std::vector<double> a, std::vector<double> alpha, std::vector<double> d)
{this->a = a;this->alpha = alpha;this->d = d;
}bool Kinematics::FK(const JointsState &angles, TransMat &mat)
{try{TransMat T12 = FT::rotz(angles[0]) * FT::trans(0, 0, this->d[0]) * FT::rotx(this->alpha[0]) * FT::trans(this->a[0], 0, 0);TransMat T23 = FT::rotz(angles[1]) * FT::trans(0, 0, this->d[1]) * FT::rotx(this->alpha[1]) * FT::trans(this->a[1], 0, 0);TransMat T34 = FT::rotz(angles[2]) * FT::trans(0, 0, this->d[2]) * FT::rotx(this->alpha[2]) * FT::trans(this->a[2], 0, 0);TransMat T45 = FT::rotz(angles[3]) * FT::trans(0, 0, this->d[3]) * FT::rotx(this->alpha[3]) * FT::trans(this->a[3], 0, 0);TransMat T56 = FT::rotz(angles[4]) * FT::trans(0, 0, this->d[4]) * FT::rotx(this->alpha[4]) * FT::trans(this->a[4], 0, 0);TransMat T67 = FT::rotz(angles[5]) * FT::trans(0, 0, this->d[5]) * FT::rotx(this->alpha[5]) * FT::trans(this->a[5], 0, 0);mat = T12 * T23 * T34 * T45 * T56 * T67;return true;}catch(...){std::cerr << "Forward kinematics solution failed"<< "\n";return false;}
}bool Kinematics::IK(const TransMat &mat, std::vector<JointsState> &inverse_solutions)
{try{double nx = mat(0, 0);double ny = mat(1, 0);double nz = mat(2, 0);double ox = mat(0, 1);double oy = mat(1, 1);double oz = mat(2, 1);double ax = mat(0, 2);double ay = mat(1, 2);double az = mat(2, 2);double px = mat(0, 3);double py = mat(1, 3);double pz = mat(2, 3);Eigen::Matrix<double, 8, 1> theta1, theta2, theta3, theta4, theta5, theta6;/* ----------theta1---------- */double A1 = px - ax * this->d[5];double B1 = py - ay * this->d[5];double X = sqrt(pow(A1, 2) + pow(B1, 2) - pow(this->d[3], 2));theta1[0] = atan2(this->d[3], X) + atan2(B1, A1);theta1[1] = theta1[2] = theta1[3] = theta1[0];theta1[4] = atan2(this->d[3], -1 * X) + atan2(B1, A1);theta1[5] = theta1[6] = theta1[7] = theta1[4];/* ----------theta6---------- */double A6 = ny * cos(theta1[0]) - nx * sin(theta1[0]);double B6 = oy * cos(theta1[0]) - ox * sin(theta1[0]);theta6[0] = -1 * atan2(B6, A6);theta6[1] = theta6[0];theta6[2] = theta6[3] = M_PI + theta6[0];A6 = ny * cos(theta1[4]) - nx * sin(theta1[4]);B6 = oy * cos(theta1[4]) - ox * sin(theta1[4]);theta6[4] = -1 * atan2(B6, A6);theta6[5] = theta6[4];theta6[6] = theta6[7] = M_PI + theta6[4];/* ----------theta5---------- */theta5[0] = -1 * acos(ax * sin(theta1[0]) - ay * cos(theta1[0]));theta5[1] = theta5[0];theta5[2] = -1 * theta5[0];theta5[3] = theta5[2];theta5[4] = -1 * acos(ax * sin(theta1[4]) - ay * cos(theta1[4]));theta5[5] = theta5[4];theta5[6] = -1 * theta5[4];theta5[7] = theta5[6];/* ----------theta3---------- */std::vector<double> m(8), n(8);double record1, record2;for (int i = 0; i <= 7; i++){m[i] = this->d[4] * (sin(theta6[i]) * (nx * cos(theta1[i]) + ny * sin(theta1[i])) + cos(theta6[i]) * (ox * cos(theta1[i]) + oy * sin(theta1[i]))) - this->d[5] * (ax * cos(theta1[i]) + ay * sin(theta1[i])) + px * cos(theta1[i]) + py * sin(theta1[i]);n[i] = pz - this->d[0] - az * this->d[5] + this->d[4] * (oz * cos(theta6[i]) + nz * sin(theta6[i]));record1 = pow(m[i], 2) + pow(n[i], 2) - pow(this->a[1], 2) - pow(this->a[2], 2);record2 = 2 * this->a[1] * this->a[2];if(i % 2 == 0){if(record1 / record2 <= 1.0001 && record1 / record2 > 1){theta3[i] = 0;}else{theta3[i] = acos(record1 / record2);}}else{if(record1 / record2 <= 1.0001 && record1 / record2 > 1){theta3[i] = 0;}else{theta3[i] = -1 * acos(record1 / record2);}}}/* ----------theta2---------- */std::vector<double> s2(8), c2(8);for (int i = 0; i <= 7;i++){s2[i] = ((this->a[2] * cos(theta3[i]) + this->a[1]) * n[i] - this->a[2] * sin(theta3[i]) * m[i]) / (pow(this->a[1], 2) + pow(this->a[2], 2) + 2 * this->a[1] * this->a[2] * cos(theta3[i]));c2[i] = (m[i] + this->a[2] * sin(theta3[i]) * s2[i]) / (this->a[2] * cos(theta3[i]) + this->a[1]);theta2[i] = atan2(s2[i], c2[i]);}/* ----------theta4---------- */for (int i = 0; i <= 7;i++){theta4[i] = atan2(-1 * sin(theta6[i]) * (nx * cos(theta1[i]) + ny * sin(theta1[i])) - cos(theta6[i]) * (ox * cos(theta1[i]) + oy * sin(theta1[i])), oz * cos(theta6[i]) + nz * sin(theta6[i])) - theta2[i] - theta3[i];}limit_angle(theta1);limit_angle(theta2);limit_angle(theta3);limit_angle(theta4);limit_angle(theta5);limit_angle(theta6);JointsState angles;inverse_solutions.clear();for (int i = 0; i <= 7; i++){angles << theta1[i], theta2[i], theta3[i], theta4[i], theta5[i], theta6[i];inverse_solutions.push_back(angles);}return true;}catch(...){std::cerr << "Inverse kinematics solution failed"<< "\n";return false;}
}

这一部分不必细说，就是本文最开头提到的那篇论文的复现。

需要注意的是，因为逆解很容易因为各种奇异构型而求解失败，为了不使程序崩溃报错，最好使用try catch语句。

（3）test.cpp

#include <Eigen/Core>
#include <iostream>
#include "Kinematics.h"
#include "DH_config.h"
#include "Common.h"int main()
{Kinematics K(UR5_DH::a, UR5_DH::alpha, UR5_DH::d);TransMat mat;JointsState angles;angles << 1, 1, 1, 1, 1, 1;K.FK(angles, mat);std::vector<JointsState> solution;K.IK(mat, solution);for (int i = 0; i <= 7;i++){std::cout << solution[i] << "\n"<< "\n";}return 0;
}

基本思路是，先用UR5的DH参数初始化一个Kinematics类的K变量，接着创建一个JointState变量来表示当前关节状态，6个关节的角度均为1弧度，然后调用K.FK方法求解当前关节状态的正解，结果存储在mat变量中，最后调用K.IK方法求解mat变量的正解。

4 CMakeList.txt

cmake_minimum_required(VERSION 3.5.0)project(RoboticsLib)set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)file(GLOB ${PROJECT_NAME}_SOURCES "./src/*.cpp")
file(GLOB ${PROJECT_NAME}_HEADERS "./include/*.h")include_directories(include)
include_directories(你的Eigen路径)add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES})add_executable(test ${${PROJECT_NAME}_SOURCES})

各语句含义在3.1写一个简单的ROS通讯库中有过简单介绍。

注意：Eigen库没有库文件，只有头文件，因此只需定义好其头文件搜索路径即可。

后续进入build目录，依次执行cmake ..和make命令，即可在lib和bin下发现生成了相应文件。

3.2 写一个UR机器人运动学库相关推荐

[译] 如何用 Python 写一个 Discord 机器人
原文地址:How to write a Discord bot in Python 原文作者:Junpei Shimotsu 译文出自:掘金翻译计划本文永久链接:github.com/xitu/go ...
如何用Java写一个聊天机器人
文章目录建议结合新版教程看写在前面的的话免责声明你需要提前会的东西我们要使用的框架首先我们先下载一个Demo 文件配置 Demo里面的的目录结构在配置文件中加上你小号的QQ名字和密码我 ...
利用itchat写一个聊天机器人
利用itchat写一个聊天机器人聊天机器人图灵机器人需要的库 **自动回复私聊消息** **自动回复群聊消息** 结语: 聊天机器人偶然在CSDN上看到大佬用20行教你写一个聊天机器人,觉得甚 ...
2020年如何写一个现代的JavaScript库
摘要: 最实用的JS库开发指南- 原文:2020年如何写一个现代的JavaScript库作者:颜海镜 Fundebug经授权转载,版权归原作者所有. 我写过一些开源项目,在开源方面有一些经验,最近开 ...
吕文翰 php,自己动手写一个 iOS 网络请求库（三）——降低耦合
自己动手写一个 iOS 网络请求库(三)--降低耦合 2015-5-22 / 阅读数:16112 / 分类: iOS & Swift 本文中,我们将一起降低之前代码的耦合度,并使用适配器模式实 ...
c语言编写对答机器人_来，你也可以用 C 语言写一个聊天机器人
来,你也可以用 C 语言写一个聊天机器人你是不是一直在面对着枯燥的 C 语言特性.摸索着前人写过的各种算法,不是因为自己的兴趣,而是依靠自身的毅力,学得很苦吧. 好吧,我们找一个好玩一点的东西,一起 ...
基于WebQQ3.0协议写一个QQ机器人
最近公司需要做个qq机器人获取qq好友列表,并且能够自动向选定的qq好友定时发送消息.没有头绪,硬着头皮上甘甜的心情瞬间变得苦涩了哇多捞吆 1.WEBQQ3.0登陆协议进入WEBQQ, htt ...
教你写一个 React 状态管理库
自从 React Hooks 推行后,Redux 作为状态管理方案就显得格格不入了.Dan Abramov 很早就提到过 "You might not need Redux",开发 ...
教你用javascript写一个QQ机器人
先放项目地址:https://github.com/sunft1996/qqRobot.js 目前的qq机器人基本上都依赖于smartqq协议,在PC上跑程序,那有没有简单一点的方法呢?正好在前段时间 ...

3.2 写一个UR机器人运动学库