使用四元数规避欧拉角万向锁问题（二）
一、背景
在使用四元数解决欧拉角万向锁问题（一）一文中已经实现了基于固定空间坐标系下的滚转角改正（如下图1），规避了“ZXY”顺序内旋方式下，俯仰角为±Pi/2时的万向锁问题，这对于固定地点的视频投放已经够用了。但我们的系统获取了无人机的实时姿态，并基于此实现了无人机的视频画面的动态投放。因此希望能够根据无人机视频的实时投放姿态进行滚转角的改正（在机身坐标系下），这样会方便用户的使用。
图1 基于固定坐标系的滚转角改正效果
大体的思路是利用四元数与轴角表示法的关系，使用初始姿态+偏航俯仰改正（中间姿态）后的z轴向量作为转轴；将滚转角改正数作为转角，计算出对应的四元数，并作用于中间姿态以得到最终结果。（如下图2）
图2 基于机身坐标系的欧拉角改正方案
二、具体应用公式
1.单位四元数对应旋转作用于向量
其中，q为单位四元数，v为向量：
v
′
=
q
v
q
ˉ
v' = q v \bar{q}
v′=qvqˉ
2.轴角表示转四元数
对于单位向量[nx,ny,nz]作为旋转轴，旋转θ角对应的单位四元数：
w
=
cos
(
θ
2
)
x
=
n
x
sin
(
θ
2
)
y
=
n
y
sin
(
θ
2
)
z
=
n
z
sin
(
θ
2
)
\begin{aligned} w &= \cos\left(\frac{\theta}{2}\right) \\ x &= n_x \sin\left(\frac{\theta}{2}\right) \\ y &= n_y \sin\left(\frac{\theta}{2}\right) \\ z &= n_z \sin\left(\frac{\theta}{2}\right) \end{aligned}
wxyz=cos(2θ)=nxsin(2θ)=nysin(2θ)=nzsin(2θ)
三、代码及实验
1. python
# -*- coding: utf-8 -*-
# @Author: 27987
# @Date: 2024-7-2 14:17
# @Last Modified by: 27987
# @Last Modified time: 2024-7-2 14:17
import numpy as np
from scipy.spatial.transform import Rotation as R
import matplotlib.pyplot as plt
plt.rcParams['font.sans-serif'] = ['SimHei'] # used for chinese support
plt.rcParams['axes.unicode_minus'] = False # minus support
# 绘制旋转后的坐标轴
def plot_rotated_axes(ax, r, name=None, offset=(0, 0, 0), scale=1):
colors = ("#FF6666", "#005533", "#1199EE") # Colorblind-safe RGB
loc = np.array([offset, offset])
for i, (axis, c) in enumerate(zip((ax.xaxis, ax.yaxis, ax.zaxis),
colors)):
axlabel = axis.axis_name
axis.set_label_text(axlabel)
axis.label.set_color(c)
axis.line.set_color(c)
axis.set_tick_params(colors=c)
line = np.zeros((2, 3))
line[1, i] = scale
line_rot = r.apply(line)
line_plot = line_rot + loc
ax.plot(line_plot[:, 0], line_plot[:, 1], line_plot[:, 2], c)
text_loc = line[1]*1.2
text_loc_rot = r.apply(text_loc)
text_plot = text_loc_rot + loc[0]
ax.text(*text_plot, axlabel.upper(), color=c,
va="center", ha="center")
ax.text(*offset, name, color="k", va="center", ha="center",
bbox={"fc": "w", "alpha": 0.8, "boxstyle": "circle"})
def cal_quat_from_axis_rotate(axis, angle):
w = np.cos(angle/2)
x = np.sin(angle/2) * axis[0]
y = np.sin(angle/2) * axis[1]
z = np.sin(angle/2) * axis[2]
return [x, y, z, w]
# test effects of different operations on initial rotation (r1)
def euler_to_quaternion_intrinsic_zxy(euler_angles):
"""
Convert intrinsic ZXY Euler angles to quaternion.
Parameters:
euler_angles (tuple or list): Euler angles in ZXY order (yaw, pitch, roll) in radians.
Returns:
np.ndarray: Quaternion [x, y, z, w].
"""
print("input:(45,90,90) ZXY order: yaw, pitch, roll")
r0 = R.from_quat(np.asarray([0,0,0,1]))
# Create a rotation object from Euler angles with intrinsic ZXY order
r1 = R.from_euler('ZXY', euler_angles, degrees=False)
# Convert the rotation object to a quaternion
quaternion = r1.as_quat()
print("r1: Together:")
print(quaternion)
r2 = R.from_euler('ZXY', (euler_angles[0], 0, 0), degrees=False)
quaternion_rotate_z = r2.as_quat()
print("r2: Z:")
print(quaternion_rotate_z)
r3 = R.from_euler('ZXY', (0, euler_angles[1], 0), degrees=False)
quaternion_rotate_x = r3.as_quat()
print("r3: X:")
print(quaternion_rotate_x)
r4 = R.from_euler('ZXY', (0, 0, euler_angles[2]), degrees=False)
quaternion_rotate_y = r4.as_quat()
print("r4: Y:")
print(quaternion_rotate_y)
quaternion_rotate_all = quaternion_multiply(quaternion_rotate_z,quaternion_multiply(quaternion_rotate_x,quaternion_rotate_y))
r5 = R.from_quat(quaternion_rotate_all)
print("r5: split input euler angles, turn them into quaterion and multiply:Z*X*Y:")
print(quaternion_rotate_all)
r6 = R.from_euler('ZXY', (0, 0, np.pi/4), degrees=False)
quaternion_rotate_mod = r6.as_quat()
print("r6: roll pi/4 to quaternion:")
print(quaternion_rotate_mod)
r7 = R.from_euler('ZXY', (np.pi/2, np.pi/2, np.pi/2 + np.pi/4), degrees=False)
quaternion_rotate_plus = r7.as_quat()
print("r7: rotation based on (pi/2, pi/2, 3pi/4):")
print(quaternion_rotate_plus)
r8 = R.from_quat(quaternion_rotate_plus)
euler_plus = r8.as_euler('ZXY',degrees=True)
print("r8、r7 to euler angles:")
print(euler_plus)
r9 = R.from_quat(quaternion)
euler = r9.as_euler('ZXY', degrees=True)
after_rotate = quaternion_multiply(quaternion_rotate_mod,quaternion)
after_rotate_ = quaternion_multiply(quaternion,quaternion_rotate_mod)
r10 = R.from_quat(after_rotate)
euler_rotate = r10.as_euler('ZXY', degrees=True)
print("r9: r1 to quaternion and rotate:", euler)
print("r10: r6 to quaternion and multiply r9 from left:", euler_rotate)
print("r10: pi/4 multiply r1 from left :")
print(after_rotate)
r11 = R.from_quat(after_rotate_)
print("r11: r6 multiply r9 from right :")
print(after_rotate_)
print("r11: r11 to euler angles:")
print(r11.as_euler('ZXY', degrees=True))
relative_z = r1.apply([0,0,1])
relative_z_quat = cal_quat_from_axis_rotate(relative_z, 45*np.pi/180)
r12 = R.from_quat(relative_z_quat)
r13 = r12 * r1
r14 = r1 * r12
ax = plt.figure().add_subplot(projection="3d", proj_type="ortho")
# some test tring to feel the up and direction vector in Cesium and failed. QAQ
# up = np.zeros((2,3))
# direction = np.zeros((2,3))
# direction[1,:] = [0.4178069101631521, -0.6706103372286868, -0.6129593472832818]
# up[1,:] = [0.5775928646133864, -0.324720111520177, 0.7489615022963638]
# ax.plot(direction[:, 0], direction[:, 1], direction[:, 2])
# ax.plot(up[:, 0], up[:, 1], up[:, 2])
plot_rotated_axes(ax, r0, name="r0", offset=(0, 0, 0))
plot_rotated_axes(ax, r1, name="r1", offset=(3, 0, 0))
# plot_rotated_axes(ax, r2, name="r2", offset=(3, 0, 0))
# plot_rotated_axes(ax, r3, name="r3", offset=(6, 0, 0))
# plot_rotated_axes(ax, r4, name="r4", offset=(6, 0, 0))
# plot_rotated_axes(ax, r5, name="r5", offset=(9, 0, 0))
plot_rotated_axes(ax, r6, name="r6", offset=(6, 0, 0))
plot_rotated_axes(ax, r7, name="r7", offset=(9, 0, 0))
plot_rotated_axes(ax, r8, name="r8", offset=(12, 0, 0))
plot_rotated_axes(ax, r9, name="r9", offset=(15, 0, 0))
plot_rotated_axes(ax, r10, name="r10", offset=(18, 0, 0))
plot_rotated_axes(ax, r11, name="r11", offset=(21, 0, 0))
plot_rotated_axes(ax, r12, name="r12", offset=(24, 0, 0))
plot_rotated_axes(ax, r13, name="r13", offset=(27, 0, 0))
plot_rotated_axes(ax, r14, name="r14", offset=(30, 0, 0))
ax.set(xlim=(-1.25, 33), ylim=(-1.25, 1.25), zlim=(-1.25, 1.25))
ax.set(xticks=range(-1, 33), yticks=[-1, 0, 1], zticks=[-1, 0, 1])
ax.set_aspect("equal", adjustable="box")
plt.tight_layout()
# add comments
_ = ax.annotate(
"r0: no rotation\n"
"r1: rotation based on input\n"
# "r4: rotation based on imput roll angle\n"
# "r5: split input euler angles, turn them into quaterion and multiply:Z*X*Y:\n"
"r6: roll pi/4\n"
"r7: directly add roll pi/4 by euler angles\n"
"r8: r7 to quaterion and rotate\n"
"r9: r1 to quaternion and rotate\n"
"r10: r6*r9 quaternion\n"
"r11: r9*r6 quaternion\n"
"r12: rotate with z' for pi/4\n"
"r13: r12*r1\n"
"r14: r1*r12\n"
,
xy=(0.6, 0.7), xycoords="axes fraction", ha="left"
)
plt.show()
return None
# 四元数乘法
def quaternion_multiply(q1, q2):
"""
Multiply two quaternions.
Parameters:
q1, q2 (tuple or list): Quaternions in the form [x, y, z, w].
Returns:
np.ndarray: Resulting quaternion [x, y, z, w].
"""
x1, y1, z1, w1 = q1
x2, y2, z2, w2 = q2
w = w1*w2 - x1*x2 - y1*y2 - z1*z2
x = w1*x2 + x1*w2 + y1*z2 - z1*y2
y = w1*y2 - x1*z2 + y1*w2 + z1*x2
z = w1*z2 + x1*y2 - y1*x2 + z1*w2
return np.array([x, y, z, w])
if __name__ == "__main__":
# Example usage
euler_angles = (-17.9/180*np.pi, -np.pi/2, 0) # ZXY order: yaw, pitch, roll
# euler_angles = (np.pi/2, np.pi/2, np.pi/2) # ZXY order: yaw, pitch, roll
euler_to_quaternion_intrinsic_zxy(euler_angles)
图3 python实验结果
2. 实验结果以及分析
实验结果：
- r12 围绕 z’(r1 的 z轴)进行了45度旋转。
- r13 旋转符合预期。
- r14 旋转不知道如何解释。QAQ
四、验证
增加如下代码：
cal_quat_from_axis_rotate(axis, angle){
let w = Math.cos(angle/2),
temp = Math.sin(angle/2),
x = temp * axis[0],
y = temp * axis[1],
z = temp * axis[2]
return [x, y, z, w]
}
addModifiedRotation(yaw,pitch,roll,yaw_mod,pitch_mod,roll_mod){
yaw = yaw / 180 * Math.PI
pitch = pitch / 180 * Math.PI
roll = roll / 180 * Math.PI
yaw_mod = yaw_mod / 180 * Math.PI
pitch_mod = pitch_mod / 180 * Math.PI
roll_mod = roll_mod / 180 * Math.PI
console.log("before modified: ", yaw / Math.PI * 180, pitch / Math.PI * 180, roll / Math.PI * 180)
// yaw = 0;
// pitch = Math.PI/2;
// roll = 0;
var qua_origin = this.eulerToQuaternionIntrinsicZXY([yaw + yaw_mod,pitch + pitch_mod,roll])
console.log("try to modify: ", yaw_mod, pitch_mod, roll_mod)
//计算动轴
var relative_z = this.quaternionMultiply(this.quaternionMultiply(qua_origin, [0,0,1,0]),[-qua_origin[0], -qua_origin[1], -qua_origin[2], qua_origin[3]])
console.log("getting desired axis:", relative_z)
var relative_z_quat = this.cal_quat_from_axis_rotate(relative_z, roll_mod)
console.log("getting desired axis quat:", relative_z_quat)
// var qua_mod = this.eulerToQuaternionIntrinsicZXY([yaw_mod,pitch_mod,0])
// var qua_res = this.quaternionMultiply(qua_origin, qua_mod)
console.log("对应偏航和俯仰修正的四元数 printing qua_res:", qua_origin)
var qua_final = this.quaternionMultiply(relative_z_quat, qua_origin)
console.log("增加滚转修正", qua_final)
var euler_res = this.quaternionToEulerIntrinsicZXY(qua_final)
console.log("getting modified res:", euler_res)
// let zaxis = new Cesium.Cartesian3;
// Cesium.Cartesian3.fromElements(relative_z[0], relative_z[1], relative_z[2], zaxis)
// console.log("zaxis: ", zaxis)
return euler_res
}
//仅接受弧度制
eulerToQuaternionIntrinsicZXY(eulerAngles) {
const [yaw, pitch, roll] = eulerAngles;
// Calculate trigonometric functions
const cy = Math.cos(yaw * 0.5);
const sy = Math.sin(yaw * 0.5);
const cp = Math.cos(pitch * 0.5);
const sp = Math.sin(pitch * 0.5);
const cr = Math.cos(roll * 0.5);
const sr = Math.sin(roll * 0.5);
// Calculate quaternion
const w = cr * cp * cy - sr * sp * sy;
const x = cr * sp * cy - sr * cp * sy;
const y = cr * sp * sy + sr * cp * cy;
const z = cr * cp * sy + sr * sp * cy;
// const w = cr * cp * cy - sr * sp * cy;
// const x = cr * sp * cy + sr * cp * sy;
// const y = cr * cp * sy - cr * sp * cy;
// const z = sr * cp * cy - cr * sp * sy;
console.log("converted: ",[x, y, z, w])
return [x, y, z, w];
}
quaternionToEulerIntrinsicZXY(quaternion) {
const [x, y, z, w] = quaternion;
let precision = 30
// Calculate intermediate values
let x_x = parseFloat(x * x).toFixed(precision),
y_y = parseFloat(y * y).toFixed(precision),
z_z = parseFloat(z * z).toFixed(precision),
x_y = parseFloat(x * y).toFixed(precision),
x_z = parseFloat(x * z).toFixed(precision),
w_x = parseFloat(w * x).toFixed(precision),
w_z = parseFloat(w * z).toFixed(precision),
w_y = parseFloat(w * y).toFixed(precision),
z_y = parseFloat(z * y).toFixed(precision);
// console.log("getting xx,yy,zz,xy,xz,wx,wz,wy,zy: ",x_x,y_y,z_z,x_y,x_z,w_x,w_z,w_y,z_y)
const t12 = 2 * ((parseFloat(x_y) - parseFloat(w_z)).toFixed(precision));
const t22 = (1 - parseFloat((2 * (parseFloat(x_x) + parseFloat(z_z)).toFixed(precision)).toFixed(precision))).toFixed(precision);
const t31 = 2 * (parseFloat(x_z) - parseFloat(w_y)).toFixed(precision);
const t32 = 2 * (parseFloat(z_y) + parseFloat(w_x)).toFixed(precision);
const t33 = (1 - parseFloat(2 * (parseFloat(y_y) + parseFloat(x_x)).toFixed(precision))).toFixed(precision);
// console.log("getting -t12,t22,-t31,t32,t33",-t12,t22,-t31,t32,t33)
var pitch = 0
var yaw = 0, roll = 0
if(t32 != 1&& t32 != -1){
pitch = Math.asin(t32);
yaw = Math.atan2(-t12, t22);
roll = Math.atan2(-t31, t33);
console.log("yaw:", yaw, "pitch:", pitch, "roll", roll)
}
else{
roll = 0
const t11 = (1-parseFloat(2*(parseFloat(y_y) + parseFloat(z_z)).toFixed(precision))).toFixed(precision);
const t21 = 2*(parseFloat(x_y) + parseFloat(w_z)).toFixed(precision);
console.log("getting t11,t21", t11, t21)
if (t32 = 1){
pitch = Math.PI/2
yaw = Math.atan2(t21, t11)
}
else if (t32 = -1){
pitch = -Math.PI/2
yaw = Math.atan2(t21, t11)
}
console.log("yaw:", yaw, "pitch:", pitch, "roll", roll)
}
// pitch = pitch/Math.PI*180;
// yaw = yaw/Math.PI*180;
// roll = roll/Math.PI*180;
return [yaw, pitch, roll];
}
quaternionMultiply(q1, q2) {
const [x1, y1, z1, w1] = q1;
const [x2, y2, z2, w2] = q2;
const w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;
const x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2;
const y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2;
const z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2;
return [x, y, z, w];
}
图4 JavaScript验证结果
由图4可见无论画面如何旋转，增加滚转角后画面形变始终为等腰梯形，符合预期。
五、存在问题
根据原始计算结果，上图4中的视频画面实际上旋转了180度，最终应用的代码投放前需要在滚转角上取个负号来加以改正，为什么会有画面的反转，以及这个负号为什么好用，我在StackOverflow上提了这个问题，还希望能有大佬不吝赐教。
图5 测试数据
971
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
