机械臂仿真控制实例（其二）-KR210正向运动学
我们使用kuka-kr210系列机械手作为例子，同时KR210也是用于拾取和放置项目的机械臂。
如果我们需要描述机械臂的零位置，则我们需要urdf文件。urdf文件时定义所有关节变量都为零时的机械臂姿态。
目录
- 构建DH参数
- a i a_i ai和 d i d_i di的值
- python代码与其解释
- 回顾正向运动学
- 调试Kuka arm的正向运动学
1.构建DH参数
打开关于KR210的rviz模拟器。如果在rviz中添加axis主题，我们可以发现机械臂基本上都位于x和z平面上。如果我们要计算机械臂的位置，则需要构建关于机械臂的DH参数表，在笔记（十五）中已经介绍过DH参数的定义过程。
定义过程如下：
-
标记{1，2，…，n }中的所有关节。
-
将{0，1，…，n}中的所有链接标记为固定的基本链接为0。
-
在所有关节上画线，定义关节轴。
-
分配每个框架的Z轴指向其关节轴。
-
在 Z ^ i − 1 \hat{Z}_{i-1} Z^i−1和 Z ^ i \hat{Z}_{i} Z^i之间识别每个坐标系的共同法线 。
-
“中间链接”（即，不是基础链接或末端执行器。）的端点与两个关节轴 i {i} i和 i + 1 {i + 1} i+1相关联。对于从1到n-1的 i i i，将 X ^ i \hat{X} _{i} X^i赋值为：
-
对于倾斜轴，沿法线之间 Z ^ i \hat{Z}_{i} Z^i 和 Z ^ i + 1 \hat{Z} _{i + 1} Z^i+1，并从 i {i} i指向 i + 1 {i + 1} i+1。
-
对于相交轴，垂直于包含 Z ^ i \hat{Z}_{i} Z^i 和 Z ^ i + 1 \hat{Z}_{i + 1} Z^i+1。
-
对于平行轴或重合轴，分配是任意的；寻找使其他DH参数等于零的方法。
-
-
对于基本链接，当第一个关节变量（ θ 1 \theta_1 θ1或者 d 1 d_1 d1）为0时，总是选择坐标系 0 {0} 0与坐标系 1 {1} 1重合。这将保证 α 0 {\alpha}_{0} α0 = 0。注意，如果关节1是转动的，则 连接偏移 d 1 {d}_{1} d1 =0。如果关节1是棱柱形的，则 转动角度 θ 1 = 0 {\theta}_{1} = 0 θ1=0。
-
对于末端执行器坐标系，如果关节n是转动关节。 当 θ n {\theta}_{n} θn = 0和坐标系n的原点即 d n {d}_{n} dn = 0，选择 X n {X}_{n} Xn朝向 X n − 1 {X}_{n-1} Xn−1。
其中：
夹持器坐标系（框架）:gripper_link
x i x_i xi和 z i z_i zi是关节轴。
a i a_i ai和 d i d_i di是非零连接的长度。
值得注意的是，我们在夹持器中心添加了一个额外的夹持器坐标，其严格的连接到链接6。夹持器的位置和方向已经在图中标出。它与链接6的参考系的区别仅在于沿Z轴平移了。
扭转角
α
i
{\alpha_i}
αi为
2. a i a_i ai和 d i d_i di的值
(1)urdf文件
a
i
a_i
ai和
d
i
d_i
di的值是由urdf文件来定义的。urdf的文件名是kr210.urdf.xacro ，它位于urdf文件夹中。我们需要关注joints标题下的内容，其中定义了所有活动的关节。
看一个例子：
这段代码将joint_1标识为revolute,并且围绕z轴旋转，并将base_link连接到link_one。“origin”字段定义了关节相对于父链接的位置和方向
这段代码是固定在link_6上的夹持器框架，其起点为相对于link_6在x轴方向上0.0375m的位置上。夹持器是一固定的，这意味着它和link_6之间没有相对运动。
在大多数情况下，URDF文件中的关节原点与根据DH参数约定创建的框架原点不一致,它们也没有相同的方向。且不能在RVIZ中直接显示关节参考系
如果我们关掉模型显示，那么能够看到的参考框架实际上是链接参考框架。
关于urdf文件，每个关键都是相对于其父关节定义的。
（2）DH参数
如上图所示，joint_1到joint_2，x要增加0.35m，z要增加0.42m。DH参数的每一行都表示，各个坐标系（框架）之间转换
KUKA KR210具有6个旋转关节。关节2是唯一棘手的对象。
θ i \theta_i θi测量 X i − 1 X_{i-1} Xi−1和 X i X_i Xi之间关于 Z i Z_i Zi的角度。机器人以其“零配置”绘制，即所有关节角度都假定为0。对于 i = 2 i = 2 i=2， X 1 X_1 X1和 X 2 X_2 X2之间存在-90度的恒定偏移量。
（3）夹持器坐标系的方向差异
最后，为了比较base_link和gripper_link之间的全齐次变换，我们需要考虑夹持器坐标系的方向差异
最简单的方法是将固有旋应用于python代码中的夹持器坐标系。
为了使两个坐标系对齐，可以先绕Z轴旋转180度，然后绕Y轴旋转-90度。
3.python代码与其解释
（1）代码部分
import numpy as np
from numpy import array
from sympy import symbols, cos, sin, pi, simplify, sqrt, atan2
from sympy.matrices import Matrix
# 在sympy中定义符号变量，在DH参数中调用
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # theta_1
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')
# 建立一个关于DH参数的列表
s = {alpha0: 0, a0: 0, d1: 0.75,
alpha1: -pi/2, a1: 0.35, d2: 0, q2:q2-pi/2,
alpha2: 0, a2: 1.25, d3: 0,
alpha3: - pi/2, a3:-0.054, d3: 0,
alpha4: pi/2, a4: 0, d4: 1.50,
alpha5: -pi/2, a5: 0, d5: 0,
alpha6: 0, a6: 0, d6: 0.303, q7:0}
# 建立齐次变换
T0_1= Matrix([[ cos(q1), -sin(q1), 0, a0],
[sin(q1)*cos(alpha0), cos(q1)*cos(alpha0), -sin(alpha0), -sin(alpha0)*d1],
[sin(q1)*sin(alpha0), cos(q1)*sin(alpha0), cos(alpha0), cos(alpha0)*d1],
[ 0, 0, 0, 1]])
T0_1 = T0_1.subs(s)
T1_2= Matrix([[ cos(q2), -sin(q2), 0, a1],
[sin(q2)*cos(alpha1), cos(q2)*cos(alpha1), -sin(alpha1), -sin(alpha1)*d1],
[sin(q2)*sin(alpha1), cos(q2)*sin(alpha1), cos(alpha1), cos(alpha1)*d1],
[ 0, 0, 0, 1]])
T1_2 = T1_2.subs(s)
T2_3= Matrix([[ cos(q3), -sin(q3), 0, a2],
[sin(q3)*cos(alpha2), cos(q3)*cos(alpha2), -sin(alpha2), -sin(alpha2)*d1],
[sin(q3)*sin(alpha2), cos(q3)*sin(alpha2), cos(alpha2), cos(alpha2)*d1],
[ 0, 0, 0, 1]])
T2_3 = T2_3.subs(s)
T3_4= Matrix([[ cos(q4), -sin(q4), 0, a3],
[sin(q4)*cos(alpha3), cos(q4)*cos(alpha3), -sin(alpha3), -sin(alpha3)*d1],
[sin(q4)*sin(alpha3), cos(q4)*sin(alpha3), cos(alpha3), cos(alpha3)*d1],
[ 0, 0, 0, 1]])
T3_4 = T3_4.subs(s)
T4_5= Matrix([[ cos(q5), -sin(q5), 0, a4],
[sin(q5)*cos(alpha4), cos(q5)*cos(alpha4), -sin(alpha4), -sin(alpha4)*d1],
[sin(q5)*sin(alpha4), cos(q5)*sin(alpha4), cos(alpha4), cos(alpha4)*d1],
[ 0, 0, 0, 1]])
T4_5 = T4_5.subs(s)
T5_6= Matrix([[ cos(q6), -sin(q6), 0, a5],
[sin(q6)*cos(alpha5), cos(q6)*cos(alpha5), -sin(alpha5), -sin(alpha5)*d1],
[sin(q6)*sin(alpha5), cos(q6)*sin(alpha5), cos(alpha5), cos(alpha5)*d1],
[ 0, 0, 0, 1]])
T5_6 = T5_6.subs(s)
T6_G= Matrix([[ cos(q7), -sin(q7), 0, a6],
[sin(q7)*cos(alpha6), cos(q7)*cos(alpha6), -sin(alpha6), -sin(alpha6)*d1],
[sin(q7)*sin(alpha6), cos(q7)*sin(alpha6), cos(alpha6), cos(alpha6)*d1],
[ 0, 0, 0, 1]])
T6_G = T6_G.subs(s)
# 在基本链接之间建立完整齐次变换
T0_2 = simplify(T0_1*T1_2)
T0_3 = simplify(T0_2*T2_3)
T0_4 = simplify(T0_3*T3_4)
T0_5 = simplify(T0_4*T4_5)
T0_6 = simplify(T0_5*T5_6)
T0_G = simplify(T0_6*T6_G)
# urdf中的Gripper_link定义与DH公约之间的方向差异
R_z = Matrix([[ cos(np.pi), -sin(np.pi), 0, 0],
[ sin(np.pi), cos(np.pi), 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])
R_y = Matrix([[ cos(-np.pi/2), 0, -sin(-np.pi/2), 0],
[ 0, 1, 0, 0],
[-sin(-np.pi/2), 0, cos(-np.pi/2), 0],
[ 0, 0, 0, 1]])
R_corr = simplify(R_z*R_y)
# 在Base_link和Gripper_link之间的完整的齐次变换
T_total= simplify(T0_G*R_corr)
(2)代码解释
1）DH参数字典
这里建立了一个关于DH参数的字典，方便以后的调用。其中
a
l
p
h
a
alpha
alpha表示旋转角，
a
a
a表示链接长度，
d
d
d表示链接偏移，
q
q
q表示联合变量
θ
\theta
θ。其中各个变量的数值在之前的 2.(2)DH参数 中已经给出。
2)齐次变换
我们在各个相邻的连接中中构建齐次变换，从link_1到Gripper_link。
在base_link 和gripper_link之间建立完整的齐次变换,通过逐步乘单独的齐次变换。
3）base_link与gripper_link之间的方向差异
之前说过，在urdf和DH公约之间存在gripper_link的方向差异。我们需要解决这个问题。我们借助物体的旋转来解决这个问题。首先旋转z轴然后再旋转y轴即可使两个坐标系重合。所以这个纠正就是两个旋转的集合。
然后我们就可以得出Kuka机械手的正向运动学。然后，我们可以通过评估矩阵在一些任意关节值处的值来评估 b a s e _ l i n k base\_link base_link和校正后的 g r i p p e r _ l i n k gripper\_link gripper_link之间的总变换，并检查其是否与ROS的末端执行器的运动匹配，从而对此进行检查。
4.运动学回顾
正运动学的目标是计算末端执行器的姿态给定所有的关节角度，这里是6个。使用DH参数，可以在各个链接之间创建单独的转换。最好以符号方式创建转换，即使用sympy库，然后将数值替换为非零项作为最后一步。DH参数约定使用四个单独的转换参数，
以矩阵形式，此变换为
齐次变换由旋转部分和平移部分组成，如下所示：
其中，
P
x
,
P
y
,
P
z
P_x, P_y, P_z
Px,Py,Pz为末端执行器相对于基础连杆的位置，
R
T
R_T
RT为旋转部分。
R
T
R_T
RT可以使用末端执行器的Roll-Pitch-Yaw角（由模拟器提供）来构建。
5.调试Kuka arm的正向运动学
一旦在关节上把单独的变换并完成了完整的齐次变换，就可以通过将结果与模型输出进行比较来验证它们。在终端窗口中，启动正向运动学演示：
$ roslaunch kuka_arm forward_kinematics.launch
执行roslaunch命令应在RViz中生成机械臂以及“joint state publisher window”。如果成功，可以看到：
ROS使用tf_echo命令非常容易地获得任意两个给定帧之间的转换。该命令的通用语法为：
$ rosrun tf tf_echo [reference frame] [target frame]
打开一个终端窗口，键入：
$ rosrun tf tf_echo base_link link_6
link_6相对于的位置和方向数据base_link在终端窗口中开始流式传输。可以按Ctrl + C停止流。
平移（链接原点之间的距离）以米为单位，而旋转以四元数和Roll-Pitch-Yaw角给出。
使用该tf_echo命令检查机械手链接和计算之间的变换，以验证每个单独的变换是否正确。
另外，也可以使用 joint_state_publisher窗口。在base_link和之间计算出最终的变换矩阵后gripper_link，可以用自己的值替换关节角度，以获得末端执行器的方向和位置。还可以使用 joint_state_publisher 窗口中的滑块替换这些关节角度值。然后，所得到的位置和方向（四元数）将在TF主题的框架下显示，这可以确认计算：
以上基本完成了机械臂的正向运动学的验证。在接下来章节就是机械臂反向运动学了。
海外访问CSDN是真的慢，传图片传的我心力憔悴。什么时候CSDN优化下就好了。
扫一扫
4968
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
