[机器人学]机器人学中的坐标变换
旋转矩阵是一个 3×3 的正交矩阵，用于描述坐标系之间的旋转关系。它满足行列式为 1，且矩阵的每一行和每一列都是单位向量。例如，如果有一个机器人末端执行器在世界坐标系中的方向可以通过一个旋转矩阵 R 来表示，那么 R 的每一行表示末端执行器坐标系的一个基向量在世界坐标系中的方向。
目录
引言
最近在做项目的时候发现,机器人中的坐标系变来变去很是头疼,尤其是在一些机器人的控制器末端姿态的表示各不相同(四元数,ZYZ,XYZ,ZYX)等等,在做上位机算法过程中,就需要保证自己模块的输入输出的姿态和其他模块的格式是对应的,因此本文梳理一下坐标系的一些内容
机器人中的坐标系
因为博主用的机器人是眼在手上的,就以眼在手上为例,图中可以很明确的表示各个坐标系之间的关系了,一般最后需要求解的都是工件和机器人基座之间的相对位置关系,根据工件的绝对坐标解算出需要的机器人轨迹,发送给控制器执行
机器人中的位姿描述
机器人的姿态描述有很多方法,其本质都是描述一个姿态,但是需要相互转换
旋转矩阵
-
定义：旋转矩阵是一个 3×3 的正交矩阵，用于描述坐标系之间的旋转关系。它满足行列式为 1，且矩阵的每一行和每一列都是单位向量。例如，如果有一个机器人末端执行器在世界坐标系中的方向可以通过一个旋转矩阵 R 来表示，那么 R 的每一行表示末端执行器坐标系的一个基向量在世界坐标系中的方向。
-
优点：能够直观地表示坐标系之间的旋转关系，便于进行坐标变换。例如，当已知一个点在末端执行器坐标系中的坐标，可以通过旋转矩阵将其转换到世界坐标系中。此外，旋转矩阵在进行矩阵运算时非常方便，例如可以方便地计算旋转的逆旋转（即转置矩阵）。
-
缺点：需要存储 9 个元素（虽然只有 3 个独立的旋转自由度），在存储和计算上相对冗余。而且，当旋转矩阵中存在微小的误差时，可能导致矩阵不再满足正交性，需要进行归一化等操作来纠正。
四元数
-
定义：四元数是一种数学上的超复数，用于描述三维空间中的旋转。它由一个标量部分和一个向量部分组成，形式为 q=[w,x,y,z]，其中 w 是标量部分，x,y,z 是向量部分。四元数可以通过单位四元数来表示旋转，单位四元数满足模长为1。
-
优点：不存在万向锁问题，能够高效地进行旋转的复合运算。例如，当需要将两个旋转组合起来时，四元数可以直接通过四元数乘法来计算结果，而不需要像欧拉角那样考虑复杂的旋转顺序和角度叠加问题。此外，四元数在计算机图形学和机器人学中用于插值计算时非常方便，可以平滑地在两个姿态之间过渡，常用于机器人路径规划和动画制作。
-
缺点：物理意义不如欧拉角直观。四元数的四个分量没有像欧拉角那样直接对应于绕某个轴的旋转角度，因此对于非专业人员来说理解起来有一定难度。
欧拉角
-
定义：欧拉角是通过三个基本旋转来描述刚体在三维空间中的方向。常见的欧拉角顺序有“Z - Y - X”（即先绕 Z 轴旋转，再绕新坐标系的 Y 轴旋转，最后绕新坐标系的 X 轴旋转）、“Z - X - Y”等。例如，一个机器人末端执行器绕 Z 轴旋转 α 角，再绕新坐标系的 Y 轴旋转 β 角，最后绕新坐标系的 X 轴旋转 γ 角，就可以用（α，β，γ）这组欧拉角来描述其姿态。
-
优点：直观易懂，容易从物理意义上理解机器人的姿态变化。例如，当我们说一个机器人手臂绕 X 轴旋转了 30 度，绕 Y 轴旋转了 45 度，绕 Z 轴旋转了 60 度时，人们可以很容易地想象出其姿态变化的大致情况。
-
缺点：存在万向锁问题。当第二个旋转轴（如绕 Y 轴的旋转）的角度为 ±90° 时，第一个旋转轴（如绕 Z 轴的旋转）和第三个旋转轴（如绕 X 轴的旋转）会重合，导致姿态描述丢失一个自由度。例如，当绕 Y 轴旋转 90 度后，绕 Z 轴和绕 X 轴的旋转会变得等效，此时无法通过欧拉角唯一地描述机器人的姿态。
机器人中的位姿转化与调整
姿态转化
这里直接放公式和代码了
四元数转旋转矩阵
代码:
def quaternion_to_rotation_matrix(q):
"""四元数转旋转矩阵"""
w, x, y, z = q
R = np.array([
[1-2*y**2-2*z**2, 2*x*y-2*w*z, 2*x*z+2*w*y],
[2*x*y+2*w*z, 1-2*x**2-2*z**2, 2*y*z-2*w*x],
[2*x*z-2*w*y, 2*y*z+2*w*x, 1-2*x**2-2*y**2]
])
return R旋转矩阵转四元数
ZYZ欧拉角转旋转矩阵
旋转矩阵转ZYZ欧拉角
四元数转ZYZ
ZYZ转四元数
姿态调整
其实这里是本文的核心部分,就是机器人在进行喷涂,打磨,清洗等工作的时候,我们的需求是机器人的末端坐标z轴与平面法线共线即可.因此xy的理论上只要在与z轴垂直的平面内即可,然而实际环境中,如果x和y轴任意给定,会导致机械臂末端安装的相机,喷枪等走线被拉扯,因此需要调整x和y的位姿尽可能与安装时的默认位姿一致
def adjust_orientation(current_orientation, normal):
"""调整末端执行器姿态方向"""
target_z = normal / np.linalg.norm(normal)
if target_z[2] > 0:
target_z = -target_z
current_x = current_orientation[:, 0]
new_x = current_x - np.dot(current_x, target_z) * target_z
if np.linalg.norm(new_x) < 1e-6:
current_y = current_orientation[:, 1]
new_x = current_y - np.dot(current_y, target_z) * target_z
if np.linalg.norm(new_x) < 1e-6:
new_x = np.array([1, 0, 0])
new_x = new_x - np.dot(new_x, target_z) * target_z
new_x = new_x / np.linalg.norm(new_x)
new_y = np.cross(target_z, new_x)
new_y = new_y / np.linalg.norm(new_y)
return np.column_stack((new_x, new_y, target_z))算法流程详解
-
输入参数：
current_orientation：当前末端执行器的旋转矩阵（3x3），表示当前姿态。normal：目标表面的法向量（3D 向量），指示喷涂需要垂直的方向。
-
目标 Z 轴计算：
target_z = normal / np.linalg.norm(normal) # 单位化法向量 if target_z[2] > 0: # 确保Z轴朝下（工程需求） target_z = -target_z- 将法向量单位化后作为新坐标系的 Z 轴（即工具朝向）。
- 通过判断 Z 轴的 Z 分量（
target_z[2]）是否为正，强制 Z 轴方向朝下（符合喷涂工艺要求）。
-
目标 X 轴计算：
current_x = current_orientation[:, 0] # 当前姿态的X轴 new_x = current_x - np.dot(current_x, target_z) * target_z # 正交化- 从当前姿态中提取 X 轴方向。
- 使用Gram-Schmidt 正交化方法，计算与
target_z正交的新 X 轴。
-
退化情况处理：
if np.linalg.norm(new_x) < 1e-6: # 若X轴与Z轴接近平行 current_y = current_orientation[:, 1] # 尝试使用当前Y轴 new_x = current_y - np.dot(current_y, target_z) * target_z if np.linalg.norm(new_x) < 1e-6: # 若仍平行 new_x = np.array([1, 0, 0]) # 使用固定方向[1,0,0] new_x = new_x - np.dot(new_x, target_z) * target_z- 当 X 轴与 Z 轴接近平行时（正交化后向量几乎为零），算法会：
- 尝试使用当前姿态的 Y 轴重新计算。
- 若仍失败，则使用固定方向
[1, 0, 0]作为备选。
- 当 X 轴与 Z 轴接近平行时（正交化后向量几乎为零），算法会：
-
目标 Y 轴计算：
new_y = np.cross(target_z, new_x) # 叉乘生成正交Y轴 new_y = new_y / np.linalg.norm(new_y) # 单位化- 通过 Z 轴和 X 轴的叉乘生成 Y 轴，确保三轴正交。
-
返回新姿态矩阵：
将三个单位向量按列组合成新的旋转矩阵，表示调整后的姿态。return np.column_stack((new_x, new_y, target_z)) # 组合为3x3矩阵
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
29
0- 0
已为社区贡献1条内容
所有评论(0)