[ROS] KDL + DH 参数 + 正解

Kinematics 运动学
Dynamics 动力学

文章目录

[ROS] KDL + DH 参数 + 正解
- 1. 安装 kdl 库
- 2. DH 参数
- 3. UR5，使用 DH 参数构建关节（cpp）
- - 完整代码
- 4. 结合 DH 参数分析源码
- - 1. base + joint 1 + shoulder 部分，对应参数表
  - 2. shoulder + joint 2 + upperarm 部分
  - 3. upperarm + joint 3 + forearm 部分
  - 4. forearm + joint 4 + wrist1 部分
  - 5. wrist1 + joint 5 + wrist2 部分
  - 6. wrist2 + joint 6 + wrist3 部分
- 5. 正解（forward kinematics）
- - 补充代码
  - 运行结果
  - 仿真器验证
- *. 参考
- *. 问题解决
- *. rough

KDL
This package contains a recent version of the Kinematics and Dynamics Library (KDL), distributed by the Orocos Project. It is a meta-package that depends on orocos_kdl which contains the c++ version and python_orocos_kdl which contains the generated python bindings
http://wiki.ros.org/kdl

1. 安装 kdl 库

安装命令：sudo apt-get install liborocos-kdl-dev

创建项目kdl_demo，并修改cmakelists.txt

cmake_minimum_required(VERSION 3.17)
project(kdl_demo)set(CMAKE_CXX_STANDARD 14)# 使用find_package引入外部依赖包
find_package(orocos_kdl)# 包含头文件
# 将指定目录添加到编译器的头文件搜索路径之下，指定的目录被解释成当前源码路径的相对路径。
message("##${orocos_kdl_INCLUDE_DIRS}")
include_directories(${orocos_kdl_INCLUDE_DIRS}
)add_executable(kdl_demo main.cpp)# 该指令的作用为将目标文件与库文件进行链接
target_link_libraries(kdl_demo${orocos_kdl_LIBRARIES}
)

/home/jacob/devtools/clion-2020.2.4/bin/cmake/linux/bin/cmake -DCMAKE_BUILD_TYPE=Debug -G "CodeBlocks - Unix Makefiles" /home/jacob/ROSWS/kdl_demo
##/opt/ros/melodic/share/orocos_kdl/cmake/../../../include;;/usr/include/eigen3
-- Configuring done
-- Generating done
-- Build files have been written to: /home/jacob/ROSWS/kdl_demo/cmake-build-debug[Finished]

在main函数中引入头文件，验证安装结果

#include <iostream>
#include <kdl/chain.hpp>int main() {std::cout << "Hello, World!" << std::endl;KDL::Chain chain;return 0;
}

/home/jacob/ROSWS/kdl_demo/cmake-build-debug/kdl_demo
Hello, World!Process finished with exit code 0

2. DH 参数

DH参数就是一个用四个参数表达两对关节连杆之间位置角度关系的机械臂数学模型和坐标系确定系统。

它通过限制原点位置和X轴的方向，人为减少了两个自由度，因此它只需要用四个参数即可表达关节之间原本是六自由度的坐标变换。

它是一个很有用的通用的convention（惯例），而不是一个“知识点”。

参考1：干货 | 机械臂的坐标系与数学模型：传说中的DH参数

练习：通过 UR5 机械臂确定 DH 参数

DH 参数定义（ENG）

DH 参数定义（中文）
ai=沿着Xi轴，Zi移动到Zi+1的距离αi=围绕Xi轴，Zi旋转到Zi+1的角度di=沿着Zi轴，Xi−1移动到Xi的距离θi=围绕Zi轴，Xi−1旋转到Xi的角度a_i=\text{沿着}X_i\text{轴，}Z_i\text{移动到}Z_{i+1}\text{的距离} \\ \alpha _i=\text{围绕}X_i\text{轴，}Z_i\text{旋转到}Z_{i+1}\text{的角度} \\ d_i=\text{沿着}Z_i\text{轴，}X_{i-1}\text{移动到}X_i\text{的距离} \\ \theta _i=\text{围绕}Z_i\text{轴，}X_{i-1}\text{旋转到}X_i\text{的角度} ai=沿着Xi轴，Zi移动到Zi+1的距离αi=围绕Xi轴，Zi旋转到Zi+1的角度di=沿着Zi轴，Xi−1移动到Xi的距离θi=围绕Zi轴，Xi−1旋转到Xi的角度

注：aaa 与 alphaalphaalpha 这里使用i−1i-1i−1下标
DH 参数表

根据 UR5 官网重新排列的表格，需要跟代码结合起来，这个表仅供理解对照官方表格（下表），编写代码时参照上面的表格。

iii	αi\alpha_iαi	aia_iai	did_idi	θi\theta _iθi
0	0	0
1	909090	000	d1d_1d1	θ1\theta_1θ1
2	000	a2a_2a2	000	θ2\theta_2θ2
3	000	a3a_3a3	000	θ3\theta_3θ3
4	909090	0	d4d_4d4	θ4\theta_4θ4
5	−90-90−90	000	d5d_5d5	θ5\theta_5θ5
6			d6d_6d6	θ6\theta_6θ6

参考UR5官方DH参数：PARAMETERS FOR CALCULATIONS OF KINEMATICS AND DYNAMICS

没有太弄懂为什么官方DH参数排列为什么与手册不同。是不是他的 aaa 与 alphaalphaalpha 是 iii 下标，手册用的 i−1i-1i−1。（猜测正确）(参考上面重新排列的表格)

3. UR5，使用 DH 参数构建关节（cpp）

完整代码

#include <iostream>
#include <kdl/chain.hpp>/** global DH parameters for UR5* check the specs.*/
double a2 = -0.425;
double a3 = -0.39225;double alpha1 = KDL::PI / 2;
double alpha4 = KDL::PI / 2;
double alpha5 = -KDL::PI / 2;double d1 = 0.089159;
double d4 = 0.10915;
double d5 = 0.09465;
double d6 = 0.0823;int main() {std::cout << "Hello, World!" << std::endl;/** definition of a kinematic chain & add segments to the chain*/KDL::Chain chain;/***-------------------- base + joint 1 + shoulder --------------------***/// segment: link 或 joint// base --> shoulder/** Constructor of the segment** @param name name of the segment* @param joint joint of the segment, default:* Joint(Joint::None)* @param f_tip frame from the end of the joint to the tip of (next)* the segment, default: Frame::Identity()* @param M rigid body inertia of the segment, default: Inertia::Zero()*/chain.addSegment(KDL::Segment("base_shoulder_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Vector(0, 0, d1))));chain.addSegment(KDL::Segment("base_shoulder_joint", KDL::Joint(KDL::Joint::RotZ)));/***-------------------- shoulder + joint 2 + upperarm --------------------***/// shoulder --> upperarmchain.addSegment(KDL::Segment("shoulder_upperarm_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Rotation(KDL::Vector(1, 0, 0),KDL::Vector(0, cos(alpha1), sin(alpha1)),KDL::Vector(0, -sin(alpha1), cos(alpha1))))));chain.addSegment(KDL::Segment("shoulder_upperarm_joint", KDL::Joint(KDL::Joint::RotZ)));/***-------------------- upperarm + joint 3 + forearm --------------------***/// upperarm --> forearmchain.addSegment(KDL::Segment("upperarm_forearm_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Vector(a2, 0, 0))));chain.addSegment(KDL::Segment("upperarm_forearm_joint", KDL::Joint(KDL::Joint::RotZ)));/***-------------------- forearm + joint 4 + wrist1 --------------------***/// forearm --> wrist1chain.addSegment(KDL::Segment("forearm_wrist1_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Vector(a3, 0, d4))))chain.addSegment(KDL::Segment("forearm_wrist1_joint", KDL::Joint(KDL::Joint::RotZ)));/***-------------------- wrist1 + joint 5 + wrist2 --------------------***/// wrist1 --> wrist2chain.addSegment(KDL::Segment("wrist1_wrist2_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Rotation(KDL::Vector(1, 0, 0),KDL::Vector(0, cos(alpha4), sin(alpha4)),KDL::Vector(0, -sin(alpha4), cos(alpha4))))));chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Vector(0, 0, d5))));chain.addSegment(KDL::Segment("wrist1_wrist2_joint", KDL::Joint(KDL::Joint::RotZ)));/***-------------------- wrist2 + joint 6 + wrist3 --------------------***/// wrist2 --> wrist3chain.addSegment(KDL::Segment("wrist2_wrist3_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Rotation(KDL::Vector(1, 0, 0),KDL::Vector(0, cos(alpha5), sin(alpha5)),KDL::Vector(0, -sin(alpha5), cos(alpha5))))));chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Vector(0, 0, d6))));chain.addSegment(KDL::Segment("wrist2_wrist3_joint", KDL::Joint(KDL::Joint::RotZ)));return 0;
}

4. 结合 DH 参数分析源码

再次查看 DH 参数表

注意旋转遵守右手定则：右手大拇指指向旋转轴的正向。

自制图中使用了 shoulder + upperarm + forearm 命名

1. base + joint 1 + shoulder 部分，对应参数表

α0：绕x0轴，z0轴旋转到z1轴a0：沿x0轴，z0轴平移到z1轴θ1：绕z1轴，x0轴旋转到x1轴d1：沿z1轴，x0轴平移到x1轴\alpha _0\text{：绕}x_0\text{轴，}z_0\text{轴旋转到}z_1\text{轴} \\ a_0\text{：沿}x_0\text{轴，}z_0\text{轴平移到}z_1\text{轴} \\ \theta _1\text{：绕}z_1\text{轴，}x_0\text{轴旋转到}x_1\text{轴} \\ d_1\text{：沿}z_1\text{轴，}x_0\text{轴平移到}x_1\text{轴} α0：绕x0轴，z0轴旋转到z1轴a0：沿x0轴，z0轴平移到z1轴θ1：绕z1轴，x0轴旋转到x1轴d1：沿z1轴，x0轴平移到x1轴

    /***-------------------- base + joint 1 + shoulder --------------------***/// segment: link 或 joint// base --> shoulderchain.addSegment(KDL::Segment("base_shoulder_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Vector(0, 0, d1))));                      // d1chain.addSegment(KDL::Segment("base_shoulder_joint", KDL::Joint(KDL::Joint::RotZ))); // theta1

2. shoulder + joint 2 + upperarm 部分

α1：绕x1轴，z1轴旋转到z2轴a1：沿x1轴，z1轴平移到z2轴θ2：绕z2轴，x1轴旋转到x2轴d2：沿z2轴，x1轴平移到x2轴\alpha _1\text{：绕}x_1\text{轴，}z_1\text{轴旋转到}z_2\text{轴} \\ a_1\text{：沿}x_1\text{轴，}z_1\text{轴平移到}z_2\text{轴} \\ \theta _2\text{：绕}z_2\text{轴，}x_1\text{轴旋转到}x_2\text{轴} \\ d_2\text{：沿}z_2\text{轴，}x_1\text{轴平移到}x_2\text{轴} α1：绕x1轴，z1轴旋转到z2轴a1：沿x1轴，z1轴平移到z2轴θ2：绕z2轴，x1轴旋转到x2轴d2：沿z2轴，x1轴平移到x2轴

    /***-------------------- shoulder + joint 2 + upperarm --------------------***/// shoulder --> upperarmchain.addSegment(KDL::Segment("shoulder_upperarm_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Rotation(KDL::Vector(1, 0, 0),KDL::Vector(0, cos(alpha1), sin(alpha1)),KDL::Vector(0, -sin(alpha1), cos(alpha1))              // alpha1))));chain.addSegment(KDL::Segment("shoulder_upperarm_joint", KDL::Joint(KDL::Joint::RotZ)));    // theta2

注：这里以及之后代码用的是旋转矩阵的逆矩阵，依据还不清楚（todo）。

3. upperarm + joint 3 + forearm 部分

α2：绕x2轴，z2轴旋转到z3轴a2：沿x2轴，z2轴平移到z3轴θ3：绕z3轴，x2轴旋转到x3轴d3：沿z3轴，x2轴平移到x3轴\alpha _2\text{：绕}x_2\text{轴，}z_2\text{轴旋转到}z_3\text{轴} \\ a_2\text{：沿}x_2\text{轴，}z_2\text{轴平移到}z_3\text{轴} \\ \theta _3\text{：绕}z_3\text{轴，}x_2\text{轴旋转到}x_3\text{轴} \\ d_3\text{：沿}z_3\text{轴，}x_2\text{轴平移到}x_3\text{轴} α2：绕x2轴，z2轴旋转到z3轴a2：沿x2轴，z2轴平移到z3轴θ3：绕z3轴，x2轴旋转到x3轴d3：沿z3轴，x2轴平移到x3轴

   /***-------------------- upperarm + joint 3 + forearm --------------------***/// upperarm --> forearmchain.addSegment(KDL::Segment("upperarm_forearm_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Vector(a2, 0, 0))));                      // alpha2chain.addSegment(KDL::Segment("upperarm_forearm_joint", KDL::Joint(KDL::Joint::RotZ)));  // theta3

4. forearm + joint 4 + wrist1 部分

α3：绕x3轴，z3轴旋转到z4轴a3：沿x3轴，z3轴平移到z4轴θ4：绕z4轴，x3轴旋转到x4轴d4：沿z4轴，x3轴平移到x3轴\alpha _3\text{：绕}x_3\text{轴，}z_3\text{轴旋转到}z_4\text{轴} \\ a_3\text{：沿}x_3\text{轴，}z_3\text{轴平移到}z_4\text{轴} \\ \theta _4\text{：绕}z_4\text{轴，}x_3\text{轴旋转到}x_4\text{轴} \\ d_4\text{：沿}z_4\text{轴，}x_3\text{轴平移到}x_3\text{轴} α3：绕x3轴，z3轴旋转到z4轴a3：沿x3轴，z3轴平移到z4轴θ4：绕z4轴，x3轴旋转到x4轴d4：沿z4轴，x3轴平移到x3轴

    /***-------------------- forearm + joint 4 + wrist1 --------------------***/// forearm --> wrist1chain.addSegment(KDL::Segment("forearm_wrist1_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Vector(a3, 0, d4))))                       // a3, d4chain.addSegment(KDL::Segment("forearm_wrist1_joint", KDL::Joint(KDL::Joint::RotZ)));    // theta4

5. wrist1 + joint 5 + wrist2 部分

α4：绕x4轴，z4轴旋转到z5轴a4：沿x4轴，z4轴平移到z5轴θ5：绕z5轴，x4轴旋转到x5轴d5：沿z5轴，x4轴平移到x5轴\alpha _4\text{：绕}x_4\text{轴，}z_4\text{轴旋转到}z_5\text{轴} \\ a_4\text{：沿}x_4\text{轴，}z_4\text{轴平移到}z_5\text{轴} \\ \theta _5\text{：绕}z_5\text{轴，}x_4\text{轴旋转到}x_5\text{轴} \\ d_5\text{：沿}z_5\text{轴，}x_4\text{轴平移到}x_5\text{轴} α4：绕x4轴，z4轴旋转到z5轴a4：沿x4轴，z4轴平移到z5轴θ5：绕z5轴，x4轴旋转到x5轴d5：沿z5轴，x4轴平移到x5轴

    /***-------------------- wrist1 + joint 5 + wrist2 --------------------***/// wrist1 --> wrist2chain.addSegment(KDL::Segment("wrist1_wrist2_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Rotation(KDL::Vector(1, 0, 0),KDL::Vector(0, cos(alpha4), sin(alpha4)),KDL::Vector(0, -sin(alpha4), cos(alpha4))          // alpha4))));chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Vector(0, 0, d5))));                           // d5chain.addSegment(KDL::Segment("wrist1_wrist2_joint", KDL::Joint(KDL::Joint::RotZ))); // theta5

6. wrist2 + joint 6 + wrist3 部分

α5：绕x5轴，z5轴旋转到z6轴a5：沿x5轴，z5轴平移到z6轴θ6：绕z6轴，x5轴旋转到x6轴d6：沿z6轴，x5轴平移到x6轴\alpha _5\text{：绕}x_5\text{轴，}z_5\text{轴旋转到}z_6\text{轴} \\ a_5\text{：沿}x_5\text{轴，}z_5\text{轴平移到}z_6\text{轴} \\ \theta _6\text{：绕}z_6\text{轴，}x_5\text{轴旋转到}x_6\text{轴} \\ d_6\text{：沿}z_6\text{轴，}x_5\text{轴平移到}x_6\text{轴} α5：绕x5轴，z5轴旋转到z6轴a5：沿x5轴，z5轴平移到z6轴θ6：绕z6轴，x5轴旋转到x6轴d6：沿z6轴，x5轴平移到x6轴

    /***-------------------- wrist2 + joint 6 + wrist3 --------------------***/// wrist2 --> wrist3chain.addSegment(KDL::Segment("wrist2_wrist3_link", KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Rotation(KDL::Vector(1, 0, 0),KDL::Vector(0, cos(alpha5), sin(alpha5)),KDL::Vector(0, -sin(alpha5), cos(alpha5))          // alpha5))));chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None),KDL::Frame(KDL::Vector(0, 0, d6))));                           // d6chain.addSegment(KDL::Segment("wrist2_wrist3_joint", KDL::Joint(KDL::Joint::RotZ))); // theta6

5. 正解（forward kinematics）

输入：各个关节的旋转角度
输出：机械臂末端位姿（位置+姿态）

补充代码

    /** part2::forward kinematics solution*/unsigned int i = chain.getNrOfJoints();std::cout << "当前的关节: " << i << std::endl;// 正解工具KDL::ChainFkSolverPos_recursive fksolver(chain);// 正解： 已知关节角度，求解末端点的位置和姿态//关节角度KDL::JntArray q_in(i);// 第0个关节角的弧度值q_in(0) = KDL::deg2rad * 0;q_in(1) = KDL::deg2rad * 0;q_in(2) = KDL::deg2rad * 0;q_in(3) = KDL::deg2rad * 0;q_in(4) = KDL::deg2rad * 0;q_in(5) = KDL::deg2rad * 0;//末端点的 位置 和 姿态KDL::Frame p_out;// joint to cartesianint state = fksolver.JntToCart(q_in, p_out);if (state >= 0) {//有解// 位置 (x,y,z)std::cout << "位置信息: " << std::endl;std::cout << p_out.p << std::endl;// 姿态 euler(roll, pitch, yaw) , 四元素，旋转矩阵// matrix 旋转矩阵std::cout << p_out.M << std::endl;double roll;double pitch;double yaw;p_out.M.GetRPY(roll, pitch, yaw);std::cout << "rpy(" << roll * KDL::rad2deg << ", " << pitch * KDL::rad2deg << ", " << yaw * KDL::rad2deg << ") ";}

运行结果

/home/jacob/ROSWS/kdl_demo/cmake-build-debug/02_forward
Hello, World!
当前的关节: 6
位置信息:
[    -0.81725,     -0.0823,   -0.005491]
[           1,           0,           0;0, 6.12323e-17,          -1;0,           1, 6.12323e-17]
rpy(90, -0, 0)
Process finished with exit code 0

正解是在已知各个关节的旋转角度之后求解机械臂的末端位姿（位姿与姿态）

位置表示末端点在三维空间坐标系中的坐标

姿态表示末端点在三维空间坐标系中的角度（可以转换成 row，pitch，yaw）

仿真器验证

使用右手坐标系确定x、y、z轴，其分别对应（红，绿、蓝）

输出：末端位姿可以看出

输入：0，0，0，0，0，0 （所有角度为 0）

*. 参考

参考1：干货 | 机械臂的坐标系与数学模型：传说中的DH参数
参考2：PARAMETERS FOR CALCULATIONS OF KINEMATICS AND DYNAMICS

*. 问题解决

*. rough