1. 旋转顺序与姿态表示基础
在无人机、机器人等运动控制系统中,姿态描述是最基础也是最重要的环节之一。不同的旋转顺序会导致完全不同的姿态表示结果,这直接关系到后续控制算法的准确性。ZXY和ZYX是两种最常见的旋转顺序约定,它们分别对应不同的应用场景和硬件平台。
1.1 旋转顺序的本质差异
旋转顺序问题源于欧拉角的内在特性——三维空间的旋转是不可交换的。这意味着先绕X轴转30°再绕Y轴转45°,与先绕Y轴转45°再绕X轴转30°,最终得到的姿态完全不同。
ZXY顺序(航向-俯仰-横滚)通常用于工程机械领域,其物理意义更符合这类设备的运动特点:
- 首先绕Z轴旋转确定航向角(Yaw)
- 接着绕X轴旋转确定俯仰角(Pitch)
- 最后绕Y轴旋转确定横滚角(Roll)
而ZYX顺序(航向-俯仰-横滚)则更常见于具身机器人系统,因为:
- 航向角(Yaw)仍然是最先确定的基准
- 俯仰角(Pitch)改为绕Y轴旋转
- 横滚角(Roll)最后绕X轴旋转
1.2 右手定则与坐标系定义
无论采用哪种旋转顺序,所有坐标系都遵循右手定则:
- 右手拇指指向坐标轴正方向
- 其余四指弯曲方向即为正旋转方向
在导航系统中通常定义:
- 导航系(n系):固定参考系,如NED(东北天)或ENU(东地北)
- 机体系(b系):随载体运动的坐标系,通常为右-前-上或前-左-上
关键提示:在实际工程中,必须明确标注使用的是n→b还是b→n的转换关系。混淆这两个方向是姿态解算中最常见的错误来源之一。
2. 四元数与旋转矩阵的互转换
2.1 四元数的核心特性
四元数作为一种超复数,在姿态表示中具有独特优势:
- 避免欧拉角的万向节死锁问题
- 插值运算更加平滑
- 计算效率高于旋转矩阵
四元数的一般形式为:
code复制q = w + xi + yj + zk = [w, x, y, z]
其中w为实部,(x,y,z)为虚部,满足i²=j²=k²=ijk=-1
单位四元数可表示为:
code复制q = [cos(θ/2), n_x·sin(θ/2), n_y·sin(θ/2), n_z·sin(θ/2)]
其中n为旋转轴单位向量,θ为旋转角度
2.2 旋转矩阵的几何意义
旋转矩阵的每一列实际上代表的是原坐标系各轴在新坐标系中的方向余弦。例如Rnb的第一列就是n系X轴在b系中的三个分量。
旋转矩阵的关键性质:
- 正交性:R⁻¹ = Rᵀ
- 行列式为1
- 保持向量长度不变
2.3 四元数与旋转矩阵的转换公式
四元数转旋转矩阵的标准公式(固定形式,与旋转顺序无关):
matlab复制function R = Quat2Rot(q)
w = q(1); x = q(2); y = q(3); z = q(4);
R = [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];
end
旋转矩阵转四元数的实现需要考虑数值稳定性:
matlab复制function q = Rot2Quat(R)
t = trace(R);
if t > 0
s = 0.5/sqrt(t+1);
w = 0.25/s;
x = (R(3,2)-R(2,3))*s;
y = (R(1,3)-R(3,1))*s;
z = (R(2,1)-R(1,2))*s;
else
if R(1,1)>R(2,2) && R(1,1)>R(3,3)
s = 2*sqrt(1+R(1,1)-R(2,2)-R(3,3));
w = (R(3,2)-R(2,3))/s;
x = 0.25*s;
y = (R(1,2)+R(2,1))/s;
z = (R(1,3)+R(3,1))/s;
elseif R(2,2)>R(3,3)
s = 2*sqrt(1+R(2,2)-R(1,1)-R(3,3));
w = (R(1,3)-R(3,1))/s;
x = (R(1,2)+R(2,1))/s;
y = 0.25*s;
z = (R(2,3)+R(3,2))/s;
else
s = 2*sqrt(1+R(3,3)-R(1,1)-R(2,2));
w = (R(2,1)-R(1,2))/s;
x = (R(1,3)+R(3,1))/s;
y = (R(2,3)+R(3,2))/s;
z = 0.25*s;
end
end
q = [w,x,y,z];
q = q/norm(q); % 归一化
end
工程经验:在实际应用中,建议优先维护四元数进行姿态更新,仅在需要时转换为旋转矩阵。这是因为:
- 四元数积分更稳定
- 避免旋转矩阵的正交性随时间漂移
- 计算效率更高
3. 欧拉角与其他表示的转换
3.1 欧拉角转旋转矩阵
对于ZXY旋转顺序:
matlab复制function R = EularAngle2Rot_ZXY(att)
Pitch = att(1); Roll = att(2); Yaw = att(3);
R = [cos(Roll)*cos(Yaw)-sin(Pitch)*sin(Roll)*sin(Yaw), cos(Roll)*sin(Yaw)+cos(Yaw)*sin(Pitch)*sin(Roll), -cos(Pitch)*sin(Roll);
-cos(Pitch)*sin(Yaw), cos(Pitch)*cos(Yaw), sin(Pitch);
cos(Yaw)*sin(Roll)+cos(Roll)*sin(Pitch)*sin(Yaw), sin(Roll)*sin(Yaw)-cos(Roll)*cos(Yaw)*sin(Pitch), cos(Pitch)*cos(Roll)];
end
对于ZYX旋转顺序:
matlab复制function R = EularAngle2Rot_ZYX(att)
Pitch = att(1); Roll = att(2); Yaw = att(3);
R = [cos(Pitch)*cos(Yaw), cos(Pitch)*sin(Yaw), -sin(Pitch);
cos(Yaw)*sin(Pitch)*sin(Roll)-cos(Roll)*sin(Yaw), cos(Roll)*cos(Yaw)+sin(Pitch)*sin(Roll)*sin(Yaw), cos(Pitch)*sin(Roll);
sin(Roll)*sin(Yaw)+cos(Roll)*cos(Yaw)*sin(Pitch), cos(Roll)*sin(Pitch)*sin(Yaw)-cos(Yaw)*sin(Roll), cos(Pitch)*cos(Roll)];
end
3.2 旋转矩阵转欧拉角
ZXY顺序的转换:
matlab复制function att = Rot2EularAngle_ZXY(R)
if R(2,3) > 0.999999
att = [pi/2, atan2(R(3,1),R(1,1)), 0];
elseif R(2,3) < -0.999999
att = [-pi/2, atan2(R(3,1),R(1,1)), 0];
else
att = [atan2(R(2,3), sqrt(R(1,3)^2+R(3,3)^2)),...
atan2(-R(1,3), R(3,3)),...
atan2(-R(2,1), R(2,2))];
end
end
ZYX顺序的转换:
matlab复制function att = Rot2EularAngle_ZYX(R)
if R(1,3) > 0.999999
att = [-pi/2, atan2(-R(2,1),-R(3,1)), 0];
elseif R(1,3) < -0.999999
att = [pi/2, atan2(R(2,1),R(3,1)), 0];
else
att = [atan2(-R(1,3), sqrt(R(2,3)^2+R(3,3)^2)),...
atan2(R(2,3), R(3,3)),...
atan2(R(1,2), R(1,1))];
end
end
3.3 加速度计解算欧拉角
加速度计在静态情况下可以测量重力方向,从而解算出俯仰和横滚角:
matlab复制function att = Acc2EularAngle(Acc, RotationSequence)
if strcmp(RotationSequence, 'ZYX')
att = [-atan2(Acc(1), sqrt(Acc(2)^2+Acc(3)^2)),...
atan2(Acc(2), Acc(3)),...
0];
else % ZXY
att = [atan2(Acc(2), sqrt(Acc(1)^2+Acc(3)^2)),...
-atan2(Acc(1), Acc(3)),...
0];
end
end
注意事项:加速度计只能解算俯仰和横滚角,航向角需要结合磁力计或陀螺仪积分获得。此外,该方法仅在静态或准静态情况下有效,动态情况下加速度计测量值包含运动加速度,不能直接用于姿态解算。
4. 工程实践中的关键问题
4.1 奇异点处理
当俯仰角接近±90°时,欧拉角表示会出现万向节死锁现象。此时需要特殊处理:
matlab复制% 在Rot2EularAngle函数中已经包含了对奇异点的处理
if R(2,3) > 0.999999 % Pitch接近90°
att = [pi/2, atan2(R(3,1),R(1,1)), 0];
elseif R(2,3) < -0.999999 % Pitch接近-90°
att = [-pi/2, atan2(R(3,1),R(1,1)), 0];
end
4.2 数值稳定性维护
四元数随时间积分可能会逐渐失去单位性,需要定期归一化:
matlab复制function q = QuatNorm(q)
q = q / norm(q);
% 或者更鲁棒的实现:
n = q(1)^2 + q(2)^2 + q(3)^2 + q(4)^2;
if n < 1e-6
q = [1,0,0,0];
else
q = q / sqrt(n);
end
end
4.3 坐标系一致性验证
定期检查旋转矩阵的正交性和四元数的单位性:
matlab复制% 检查旋转矩阵
err = norm(R*R' - eye(3));
if err > 1e-6
R = Orthogonalize(R); % 重新正交化
end
% 检查四元数
if abs(norm(q)-1) > 1e-6
q = QuatNorm(q);
end
4.4 性能优化技巧
- 避免不必要的转换:尽量在一种表示下完成所有计算
- 利用对称性:旋转矩阵求逆只需转置
- 预先计算常量:如三角函数值
- 使用查表法:对于固定角度的计算
5. 不同旋转顺序的对比与应用
5.1 ZXY顺序特点
- 适用于工程机械
- Pitch绕X轴,Roll绕Y轴
- 奇异点在Pitch=±90°
- 转换公式相对复杂
5.2 ZYX顺序特点
- 适用于具身机器人
- Pitch绕Y轴,Roll绕X轴
- 奇异点同样在Pitch=±90°
- 转换公式较为常见
5.3 选择建议
- 根据硬件平台的传统选择
- 考虑主要运动方式:如果主要俯仰运动,ZYX可能更合适
- 与上下游系统保持一致
- 文档中必须明确标注使用的旋转顺序
6. 完整转换流程示例
6.1 从加速度计到四元数
matlab复制% 输入加速度计数据(已归一化)
Acc = [0.1, 0.2, 0.975];
% 解算欧拉角(ZXY顺序)
att = Acc2EularAngle(Acc, 'ZXY');
% 转换为四元数
q = EularAngle2Quat(att, 'ZXY');
% 转换为旋转矩阵
R = Quat2Rot(q);
% 验证转换正确性
att_check = Rot2EularAngle(R, 'ZXY');
disp(norm(att - att_check)); % 应该接近0
6.2 姿态更新流程
matlab复制% 初始化
q = [1,0,0,0]; % 单位四元数
% 陀螺仪测量值(rad/s)
gyro = [0.01, 0.02, 0.005];
% 时间步长
dt = 0.01;
% 四元数更新
q = QuatUpdate(q, gyro, dt);
% 转换为欧拉角显示
R = Quat2Rot(q);
att = Rot2EularAngle(R, 'ZXY');
其中四元数更新函数:
matlab复制function q_new = QuatUpdate(q, gyro, dt)
% 构造角速度四元数
omega = [0, gyro(1), gyro(2), gyro(3)];
% 四元数微分方程
q_dot = 0.5 * QuatMultiply(q, omega);
% 前向欧拉积分
q_new = q + q_dot * dt;
% 归一化
q_new = QuatNorm(q_new);
end
7. 常见问题排查
7.1 姿态解算发散
可能原因:
- 陀螺仪偏差未补偿
- 四元数未归一化
- 时间步长不稳定
- 坐标系定义混淆
解决方案:
- 校准陀螺仪零偏
- 每次更新后强制归一化
- 使用固定时间步长或精确计时
- 检查所有转换函数的方向一致性
7.2 旋转顺序错误的表现
症状:
- 俯仰和横滚角互相影响
- 特定角度下姿态解算完全错误
- 动态响应不符合预期
验证方法:
- 在已知角度下人工计算验证
- 检查旋转矩阵的行列式(应为+1)
- 验证R*R'是否等于单位矩阵
7.3 数值不稳定问题
表现:
- 随时间推移姿态逐渐失真
- 旋转矩阵不再正交
- 四元数范数偏离1
解决方法:
- 增加正交化处理
- 提高计算精度(使用double而非float)
- 减少不必要的转换次数
在实际工程实践中,我经常遇到旋转顺序混淆导致的问题。一个实用的调试技巧是:在水平静止状态下,先验证俯仰和横滚角是否正确,再测试航向角。另外,建议在代码中加入大量的断言检查,确保中间结果的合理性。