### 机器人位姿与齐次变换矩阵相互转换 在机器人学中，齐次变换矩阵用于描述空间中的平移和旋转操作。该矩阵能够统一表示位置和姿态信息，从而方便地处理坐标系间的转换。 #### 齐次变换矩阵到位姿的转换 给定一个4×4的齐次变换矩阵H，可以通过提取其中的旋转部分和平移部分来获得对应的位姿参数： - **旋转部分**：取前三个列向量作为旋转矩阵R。 - **平移部分**：最后一列为平移向量t。 ```python import numpy as np def homogeneous_to_pose(homogeneous_matrix): """ 将齐次变换矩阵转换成位姿(旋转矩阵, 平移向量). 参数: homogeneous_matrix (numpy.ndarray): 形状为(4, 4) 的齐次变换矩阵. 返回: tuple: 包含两个元素的元组, 第一个是形状为(3, 3) 的旋转矩阵; 第二个是长度为3的一维数组代表平移向量. """ rotation = homogeneous_matrix[:3, :3] translation = homogeneous_matrix[:-1, -1] return rotation, translation # 测试函数 if __name__ == "__main__": H_example = np.array([[0.7071, -0.7071, 0., 1.], [0.7071, 0.7071, 0., 2.], [0., 0., 1., 3.], [0., 0., 0., 1.]]) R, t = homogeneous_to_pose(H_example) print("Rotation Matrix:\n", R) print("\nTranslation Vector:", t) ``` 此代码片段展示了如何从齐次变换矩阵中分离出旋转和平移分量[^1]。 #### 位姿到齐次变换矩阵的转换 相反的过程则是构建一个新的4×4齐次变换矩阵，它由输入的位置矢量p=(px, py, pz)以及方向余弦构成的3x3正交矩阵组成: ```python def pose_to_homogeneous(rotation, translation): """ 将位姿(旋转矩阵, 平移向量) 转换成齐次变换矩阵. 参数: rotation (numpy.ndarray): 形状为(3, 3) 的旋转矩阵. translation (array-like): 长度为3的一维数组或列表形式的平移向量. 返回: numpy.ndarray: 形状为(4, 4) 的齐次变换矩阵. """ homogenous_transform = np.eye(4) # 设置旋转子矩阵 homogenous_transform[0:3, 0:3] = rotation # 设置平移子向量 homogenous_transform[0:3, 3] = translation return homogenous_transform # 继续之前的例子测试新功能 H_reconstructed = pose_to_homogeneous(R, t) print("\nReconstructed Homogeneous Transformation Matrix:") print(H_reconstructed) ``` 这段程序实现了基于已知的旋转和平移数据创建相应的齐次变换矩阵的功能。