正向运动学和反向运动学

目录

1. 2D中的旋转矩阵
2. sympy包
3. 旋转的合成
4. 旋转矩阵中的欧拉角
5. 平移
6. 齐次变换及其逆变换
7. 齐次变换的合成
8. Denavit-Hartenberg 参数
9. DH参数分配算法
10. 正向运动学
11. 反向运动学
12. 反向运动学举例

7.齐次变换的合成

齐次变换的合成与旋转的合成遵循着相同的逻辑。



假设从坐标系C到坐标系B的变换是已知的，从坐标系B到坐标系a的变换也是已知的。

${}^C\mathbf{r}P/Co$关于坐标系_$A$首先将它转化为坐标系$B$，



(6-1)

然后通过变换到A坐标系，



(6-2)

(6-1)(6-2)式可以组合为:



为了完整起见，把A和C之间的变换展开成已知的分量，

(6-3)

（6-4）

式(6-4)中的左上3x3子矩阵看起来应该很熟悉;它是a和C之间旋转的组合。

${}_B^A{R}^{B}rCo/Bo$是向量从$B_o$到$Co$在_A坐标系中的表示。另一项，${}^A{\mathbf{r}}_{Bo/Ao}$是向量从$A_o$到$B_o$在_A坐标系中的表示。

为了帮助巩固变换、逆变换和变换组合的概念，再考虑一个例子。这里是坐标系A到E。

假设我们想要创建坐标系A和E之间的变换;显然，有两条路。一条路径有变换，



（6-5）

第二个是：



（6-6)

两者的结果相同



（6-7)

下面程序是证明（7）式，经过两种途径， 实现坐标系从A到E的变换

从A系到B系到E系:

坐标系A:位于[0,0,0]

坐标系B:将坐标系A旋转大约a_y -90度。翻译A by [- 2,2,4]

坐标系E:将坐标系B旋转约b_x 90度。将B平移[0,2,0]

从A系到C系到D系到E系:

坐标系C:平移A到[4,4,0]

坐标系D:围绕c_x旋转坐标系C 90度。翻译C by [- 3,3, 2]

坐标系E:围绕d_Z旋转坐标系D 90度。将D平移[- 3,2,3]

from sympy import symbols, cos, sin, pi, sqrt, simplify
from sympy.matrices import Matrix# 为联合变量创建符号
# 数字1到4按指定的顺序对应于每个旋转。
q1, q2, q3, q4 = symbols('q1:5')# 定义关于给定特定角度的x、y和z的旋转矩阵的函数。def rot_x(q):R_x = Matrix([[1, 0, 0],[0, cos(q), -sin(q)],[0, sin(q), cos(q)]])return R_xdef rot_y(q):R_y = Matrix([[cos(q), 0, sin(q)],[0, 1, 0],[-sin(q), 0, cos(q)]])return R_ydef rot_z(q):R_z = Matrix([[cos(q), -sin(q), 0],[sin(q), cos(q), 0],[0, 0, 1]])return R_z# 定义坐标系之间的旋转# 坐标系A的初始旋转矩阵
Ra = Matrix([[1, 0, 0],[0, 1, 0],[0, 0, 1]])# 在A->B->E坐标系上进行旋转
Rb_a = rot_y(q1)
Re_b = rot_x(q2)# 在A->C->D->E坐标系上进行旋转
Rc_a = Ra
Rd_c = rot_x(q3)
Re_d = rot_z(q4)# 定义坐标系之间的转换tb_a = Matrix([[-2], [2], [4]])
te_b = Matrix([[0], [2], [0]])
tc_a = Matrix([[4], [4], [0]])
td_c = Matrix([[-3], [3], [2]])
te_d = Matrix([[-3], [2], [3]])# 定义齐次变换矩阵
Ta = Ra.row_join(Matrix([[0], [0], [0]]))
Ta = Ta.col_join(Matrix([[0, 0, 0, 1]]))Tb_a = Rb_a.row_join(tb_a)
Tb_a = Tb_a.col_join(Matrix([[0, 0, 0, 1]]))Te_b = Re_b.row_join(te_b)
Te_b = Te_b.col_join(Matrix([[0, 0, 0, 1]]))Tc_a = Rc_a.row_join(tc_a)
Tc_a = Tc_a.col_join(Matrix([[0, 0, 0, 1]]))Td_c = Rd_c.row_join(td_c)
Td_c = Td_c.col_join(Matrix([[0, 0, 0, 1]]))Te_d = Re_d.row_join(te_d)
Te_d = Te_d.col_join(Matrix([[0, 0, 0, 1]]))# 化简公式
Te_a_1 = simplify(Ta * Tb_a * Te_b)Te_a_2 = simplify(Ta * Tc_a * Td_c * Te_d)# 计算E的方向和位置
E_1 = Te_a_1.evalf(subs={ q1: -pi / 2, q2: pi / 2}, chop=True)E_2 = Te_a_2.evalf(subs={ q3: pi / 2, q4: pi / 2}, chop=True)print('E_1 :',E_1)
print('E_2 :',E_2)

有关更多sympy表达式化简的在这里。

输出如下：

E_1 : Matrix([[0, -1.00000000000000, 0, -2.00000000000000], [0, 0, -1.00000000000000, 4.00000000000000], [1.00000000000000, 0, 0, 4.00000000000000], [0, 0, 0, 1.00000000000000]])
E_2 : Matrix([[0, -1.00000000000000, 0, -2.00000000000000], [0, 0, -1.00000000000000, 4.00000000000000], [1.00000000000000, 0, 0, 4.00000000000000], [0, 0, 0, 1.00000000000000]])

由此看出，经过两个变换，两者得出来的结果是相同的。

8.Denavit-Hartenberg 参数

在前面的课程中，介绍了旋转、平移和齐次变换的概念。所有这些概念对于理解机械手正运动学问题都是必不可少的，即给定关节变量，计算末端执行器的位置。求解过程包括在机械手的每个连杆上附加一个参照系，并写出从固定基座连杆到连杆1、从连杆1到连杆2的齐次变换，一直到末端执行器。



（7-1）

通常，每个变换需要6个独立的参数来描述相对于坐标系$i-1$的坐标系$i$, 3个参数表示位置，3个参数表示方向。

1955年，雅克·德纳维特(Jacques Denavit)和理查德·哈滕伯格(Richard Hartenberg)提出了一种将参照系附加到机械手连杆上的系统方法，简化了齐次变换。他们的方法只需要四个参数来描述相邻参考系的位置和方向。在机器人技术的早期，更紧凑的正运动学方程提供了巨大的好处，因为计算通常是人工或在处理能力有限的计算机上进行的。因此，用于描述机械手运动学的Denavit-Hartenberg (DH)方法现在是普遍存在的。

自最初的描述以来，DH方法已经做了一些修改，主要是关于每个参考系原点的编号和位置，所以在比较来自不同来源的工作时，必须小心确定使用的是哪种约定。以下是五个最常见的来源，

• Waldron, KJ. A study of overconstrained linkage geometry by solution of closure equations, Part I: A method of study (1973). Mech. Mach. Theory 8(1):95-104.

• Paul, R. (1982). Robot Manipulators: Mathematics, Programming and Control (MIT Press, Cambridge, MA)

• Craig, JJ. (2005). Introduction to Robotics: Mechanics and Control, 3rd Ed (Pearson Education, Inc., NJ)(机器人学概论:机械与控制，第三版)

• Khalil, W and Dombre, E. (2002). Modeling, Identification and Control of Robots (Taylor Francis, NY)

• M. Spong and M. Vidyasagar, Robot Modeling and Control, Wiley, 2005

这里使用John J Craig书中描述的约定。

这些惯例上的差异使得比较结果更加困难。重要的是始终检查齐次变换序列是如何执行的，以关联相邻的连接。下面描述的约定与Craig(2005)一致。涉及的参数 α，a，d和θ。

参数名称和定义总结如下:

• ${\alpha }_{i-1}$(扭转角) = 沿${\hat{X}}_{i-1}$以右手法则观测的 ${\hat{Z}}_{i-1}$和${\hat{Z}}_i$之间的夹角。
• $a$