[ROS] KDL + DH 参数 + 正解
Kinematics 运动学
Dynamics 动力学
文章目录
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 参数定义（中文）
a i = 沿着 X i 轴， Z i 移动到 Z i + 1 的距离 α i = 围绕 X i 轴， Z i 旋转到 Z i + 1 的角度 d i = 沿着 Z i 轴， X i − 1 移动到 X i 的距离 θ i = 围绕 Z i 轴， X i − 1 旋转到 X i 的角度 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的角度
注： a a a 与 a l p h a alpha alpha 这里使用 i − 1 i-1 i−1下标
DH 参数表
根据 UR5 官网重新排列的表格，需要跟代码结合起来，这个表仅供理解对照官方表格（下表），编写代码时参照上面的表格。
| i i i | α i \alpha_i αi | a i a_i ai | d i d_i di | θ i \theta _i θi |
|---|---|---|---|---|
| 0 | 0 | 0 | ||
| 1 | 90 90 90 | 0 0 0 | d 1 d_1 d1 | θ 1 \theta_1 θ1 |
| 2 | 0 0 0 | a 2 a_2 a2 | 0 0 0 | θ 2 \theta_2 θ2 |
| 3 | 0 0 0 | a 3 a_3 a3 | 0 0 0 | θ 3 \theta_3 θ3 |
| 4 | 90 90 90 | 0 | d 4 d_4 d4 | θ 4 \theta_4 θ4 |
| 5 | − 90 -90 −90 | 0 0 0 | d 5 d_5 d5 | θ 5 \theta_5 θ5 |
| 6 | d 6 d_6 d6 | θ 6 \theta_6 θ6 |
参考UR5官方DH参数：PARAMETERS FOR CALCULATIONS OF KINEMATICS AND DYNAMICS
没有太弄懂为什么官方DH参数排列为什么与手册不同。是不是他的 a a a 与 a l p h a alpha alpha 是 i i i 下标，手册用的 i − 1 i-1 i−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 --> upperarm
chain.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 --> forearm
chain.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 --> wrist1
chain.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 --> wrist2
chain.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 --> wrist3
chain.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 ：绕 x 0 轴， z 0 轴旋转到 z 1 轴 a 0 ：沿 x 0 轴， z 0 轴平移到 z 1 轴 θ 1 ：绕 z 1 轴， x 0 轴旋转到 x 1 轴 d 1 ：沿 z 1 轴， x 0 轴平移到 x 1 轴 \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 --> shoulder
chain.addSegment(KDL::Segment("base_shoulder_link", KDL::Joint(KDL::Joint::None),
KDL::Frame(KDL::Vector(0, 0, d1)))); // d1
chain.addSegment(KDL::Segment("base_shoulder_joint", KDL::Joint(KDL::Joint::RotZ))); // theta1
2. shoulder + joint 2 + upperarm 部分
α
1
：绕
x
1
轴，
z
1
轴旋转到
z
2
轴
a
1
：沿
x
1
轴，
z
1
轴平移到
z
2
轴
θ
2
：绕
z
2
轴，
x
1
轴旋转到
x
2
轴
d
2
：沿
z
2
轴，
x
1
轴平移到
x
2
轴
\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 --> upperarm
chain.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
：绕
x
2
轴，
z
2
轴旋转到
z
3
轴
a
2
：沿
x
2
轴，
z
2
轴平移到
z
3
轴
θ
3
：绕
z
3
轴，
x
2
轴旋转到
x
3
轴
d
3
：沿
z
3
轴，
x
2
轴平移到
x
3
轴
\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 --> forearm
chain.addSegment(KDL::Segment("upperarm_forearm_link", KDL::Joint(KDL::Joint::None),
KDL::Frame(KDL::Vector(a2, 0, 0)))); // alpha2
chain.addSegment(KDL::Segment("upperarm_forearm_joint", KDL::Joint(KDL::Joint::RotZ))); // theta3
4. forearm + joint 4 + wrist1 部分
α
3
：绕
x
3
轴，
z
3
轴旋转到
z
4
轴
a
3
：沿
x
3
轴，
z
3
轴平移到
z
4
轴
θ
4
：绕
z
4
轴，
x
3
轴旋转到
x
4
轴
d
4
：沿
z
4
轴，
x
3
轴平移到
x
3
轴
\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 --> wrist1
chain.addSegment(KDL::Segment("forearm_wrist1_link", KDL::Joint(KDL::Joint::None),
KDL::Frame(KDL::Vector(a3, 0, d4)))) // a3, d4
chain.addSegment(KDL::Segment("forearm_wrist1_joint", KDL::Joint(KDL::Joint::RotZ))); // theta4
5. wrist1 + joint 5 + wrist2 部分
α
4
：绕
x
4
轴，
z
4
轴旋转到
z
5
轴
a
4
：沿
x
4
轴，
z
4
轴平移到
z
5
轴
θ
5
：绕
z
5
轴，
x
4
轴旋转到
x
5
轴
d
5
：沿
z
5
轴，
x
4
轴平移到
x
5
轴
\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 --> wrist2
chain.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)))); // d5
chain.addSegment(KDL::Segment("wrist1_wrist2_joint", KDL::Joint(KDL::Joint::RotZ))); // theta5
6. wrist2 + joint 6 + wrist3 部分
α
5
：绕
x
5
轴，
z
5
轴旋转到
z
6
轴
a
5
：沿
x
5
轴，
z
5
轴平移到
z
6
轴
θ
6
：绕
z
6
轴，
x
5
轴旋转到
x
6
轴
d
6
：沿
z
6
轴，
x
5
轴平移到
x
6
轴
\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 --> wrist3
chain.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)))); // d6
chain.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 cartesian
int 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
扫一扫
4209
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
