在Drake中实现旋转表示转换的最优方案（支持自动微分）

Drake提供了类型安全、原生支持自动微分的旋转表示类，直接使用这些类的构造函数或成员方法进行转换是最优方案——无需手动实现转换逻辑，同时完美兼容AutoDiff功能。

核心旋转表示类 #

Drake中处理旋转的核心类均支持double和AutoDiffScalar类型，包括：

• RollPitchYaw<T>：存储滚转-俯仰-偏航角（RPY）
• Quaternion<T>：存储四元数
• RotationMatrix<T>：存储旋转矩阵

这些类之间的转换方法已内置微分支持，自动处理AutoDiff的梯度传递。

具体转换示例 #

1. 四元数转旋转矩阵 #

通过RotationMatrix的构造函数或Quaternion的toRotationMatrix()方法实现：

// 普通double类型
drake::math::Quaternion<double> quat(1.0, 0.0, 0.0, 0.0); // 单位四元数
drake::math::RotationMatrix<double> rot_mat(quat);
// 等价写法
drake::math::RotationMatrix<double> rot_mat = quat.toRotationMatrix();

// AutoDiffScalar类型
using AutoDiff = drake::AutoDiffXd;
drake::math::Quaternion<AutoDiff> quat_autodiff(AutoDiff(1.0), AutoDiff(0.0), AutoDiff(0.0), AutoDiff(0.0));
drake::math::RotationMatrix<AutoDiff> rot_mat_autodiff(quat_autodiff);

2. 旋转矩阵转四元数 #

直接用Quaternion的构造函数接收RotationMatrix实例：

drake::math::RotationMatrix<double> rot_mat; // 假设已完成初始化
drake::math::Quaternion<double> quat(rot_mat);

// AutoDiff版本
drake::math::RotationMatrix<AutoDiff> rot_mat_autodiff;
drake::math::Quaternion<AutoDiff> quat_autodiff(rot_mat_autodiff);

3. 四元数转RPY角 #

通过RollPitchYaw的构造函数或Quaternion的ToRollPitchYaw()方法转换，可直接提取单个角度：

drake::math::Quaternion<double> quat;
drake::math::RollPitchYaw<double> rpy(quat);
// 等价写法
drake::math::RollPitchYaw<double> rpy = quat.ToRollPitchYaw();

// 获取单独角度值
double roll = rpy.roll();
double pitch = rpy.pitch();
double yaw = rpy.yaw();

// AutoDiff版本
drake::math::Quaternion<AutoDiff> quat_autodiff;
drake::math::RollPitchYaw<AutoDiff> rpy_autodiff(quat_autodiff);
AutoDiff roll_autodiff = rpy_autodiff.roll();

4. RPY角转旋转矩阵/四元数 #

利用目标类的构造函数直接接收RollPitchYaw实例：

// RPY转旋转矩阵
drake::math::RollPitchYaw<double> rpy(0.1, 0.2, 0.3);
drake::math::RotationMatrix<double> rot_mat(rpy);

// RPY转四元数
drake::math::Quaternion<double> quat(rpy);

// AutoDiff版本
drake::math::RollPitchYaw<AutoDiff> rpy_autodiff(AutoDiff(0.1), AutoDiff(0.2), AutoDiff(0.3));
drake::math::RotationMatrix<AutoDiff> rot_mat_autodiff(rpy_autodiff);
drake::math::Quaternion<AutoDiff> quat_autodiff(rpy_autodiff);

关键注意事项 #

• 所有转换方法原生支持AutoDiff，无需手动编写微分逻辑，Drake内部已处理自动微分的链式法则。
• 避免手动实现转换公式（如自行推导四元数转矩阵的计算式），这类代码易出错且无法兼容AutoDiff。
• Drake的RollPitchYaw默认采用X-Y-Z固定轴旋转顺序（先绕X滚转，再绕Y俯仰，最后绕Z偏航），若需其他旋转顺序，可通过构造参数指定（例如RollPitchYaw<double>(rpy_vals, drake::math::RotationMatrix<double>::RotationMatrixType::kZYX)）。

内容的提问来源于stack exchange，提问作者user3180