企业官网建设流程全解析

FA：Formulas and Algorithm

一、姿态变换的四元数表示

要计算从 a 旋转到 c 的四元数 q2​，已知：

- 从 b → a 的旋转四元数是 q0​ - 从 b → c 的旋转四元数是 q1​

我们目标是求 a → c 的旋转四元数q2​。

1.1、从一个坐标系（或向量）旋转到另一个的变换

若一个向量 v 在坐标系 B 中，想通过四元数 q 旋转到坐标系 A，则通常写作：

vA​=qvB​q

四元数表示的是从一个坐标系（或向量）旋转到另一个的变换。若一个向量 v 在坐标系 B 中，想通过四元数 q 旋转到坐标系 A，则通常写作：
vA​=qvB​q−1

但当我们只关心相对旋转（即“从姿态 X 到姿态 Y 的旋转”），可以把每个姿态看作是从世界坐标系（或某个公共参考系） 到该姿态的旋转。

假设所有四元数都是相对于同一个参考系（比如世界坐标系 W） 定义的：

q0​：将向量从 b 旋转到 a，即 va​=q0​vb​q0−1​ 等价于：a 的姿态 = q0​⋅b 的姿态 q1​：将向量从 b 旋转到 c，即 vc​=q1​vb​q1−1​

我们想找到 q2​，使得：
vc​=q2​va​q2−1​

二、基于轴角插值的四元数表示

四元数轴角插值（Slerp，球面线性插值）

说明：两个姿态四元数之间平滑插值，不能直接对四元数 4 个分量线性插值（Lerp），会造成姿态角速度不均匀；标准方法是Slerp 球面线性插值，本质就是基于轴角的球面插值，沿着单位四元数球面最短大圆弧过渡。

四元数约定：(q = [w, x, y, z])，单位四元数，w为实部；
两个姿态：(q_0)（起始姿态），(q_1)（终止姿态）；插值参数 (t\in[0,1])，(t=0)得到(q_0)，(t=1)得到(q_1)。

一、Slerp 公式（轴角插值原理）

1. 四元数点积：
   cos⁡θ=q0⋅q1=w0w1+x0x1+y0y1+z0z1 \cos\theta = q_0 \cdot q_1 = w_0w_1+x_0x_1+y_0y_1+z_0z_1cosθ=q0​⋅q1​=w0​w1​+x0​x1​+y0​y1​+z0​z1​
   θ\thetaθ：两个姿态之间旋转夹角。

注意：如果 (\cos\theta<0)，把 (q_1) 取反 (q_1=-q_1)。因为四元数 q 和 (-q) 代表同一个姿态，但取反可以保证走最短旋转弧。

2. Slerp 公式：
   Slerp(q0,q1,t)=sin⁡((1−t)θ)sin⁡θq0+sin⁡(tθ)sin⁡θq1 \mathrm{Slerp}(q_0,q_1,t)=\frac{\sin\big((1-t)\theta\big)}{\sin\theta} q_0 + \frac{\sin(t\theta)}{\sin\theta} q_1Slerp(q0​,q1​,t)=sinθsin((1−t)θ)​q0​+sinθsin(tθ)​q1​
   θ=arccos⁡(cos⁡θ) \theta=\arccos(\cos\theta)θ=arccos(cosθ)
3. 边界处理：
   当θ\thetaθ很小（∣cos⁡θ∣≈1|\cos\theta|\approx1∣cosθ∣≈1），sin⁡θ≈θ\sin\theta\approx\thetasinθ≈θ，公式退化为线性插值 Lerp，避免除零：
   Lerp(q0,q1,t)=(1−t)q0+tq1 \mathrm{Lerp}(q_0,q_1,t)=(1-t)q_0 + t q_1Lerp(q0​,q1​,t)=(1−t)q0​+tq1​

插值后归一化。

物理含义：沿着固定旋转轴，从初始姿态匀速旋转到目标姿态，就是你说的轴角平滑过渡。Slerp 本质就是轴角匀速插值对应的四元数表达式。

二、Python 完整代码

importnumpy as np def quat_dot(q0, q1):returnq0[0]*q1[0]+ q0[1]*q1[1]+ q0[2]*q1[2]+ q0[3]*q1[3]def quat_normalize(q): norm=np.linalg.norm(q)returnq / norm def slerp(q0, q1, t):""" Slerp球面插值 q0:[w,x,y,z]起始四元数 q1:[w,x,y,z]终止四元数 t:0~1 插值系数 return: qt[w,x,y,z]""" dot=quat_dot(q0, q1)# 保证走最短圆弧，点积负则翻转q1ifdot<0.0: q1=-q1dot=-dot# 防止数值溢出，限制[-1,1]dot=np.clip(dot, -1.0,1.0)theta=np.arccos(dot)sin_theta=np.sin(theta)# 小角度，退化为Lerpifsin_theta<1e-8: qt=(1-t)*q0 + t*q1returnquat_normalize(qt)s0=np.sin((1-t)*theta)/ sin_theta s1=np.sin(t*theta)/ sin_theta qt=s0 * q0 + s1 * q1returnqt def generate_quat_sequence(q0, q1, num_points):""" 在q0,q1之间生成num_points个插值姿态""" quat_list=[]t_arr=np.linspace(0,1, num_points)fortint_arr: qt=slerp(q0, q1, t)quat_list.append(qt)returnnp.array(quat_list)# ========== 测试示例 ==========if__name__=="__main__":# q0 [w,x,y,z]q0=np.array([1.0,0.0,0.0,0.0])# 绕Z轴旋转90度四元数q1=np.array([np.cos(np.pi/4),0,0,np.sin(np.pi/4)])N=10# 生成10个姿态点quat_seq=generate_quat_sequence(q0, q1, N)print("插值四元数序列 [w,x,y,z]:")print(quat_seq)

三、C++ 完整代码（Eigen 库，机器人常用）

依赖：Eigen3，Eigen 自带四元数，自带 slerp，这里同时给手动实现版本，方便理解原理。

#include <iostream>#include <vector>#include <Eigen/Dense>using namespace Eigen;using namespace std;// 手动实现Slerp，输入q0 q1：w,x,y,z Quaterniond slerp_manual(const Quaterniond&q0, const Quaterniond&q1, double t){double dot=q0.w()*q1.w()+ q0.x()*q1.x()+ q0.y()*q1.y()+ q0.z()*q1.z();Quaterniond q1_adj=q1;if(dot<0.0){q1_adj.coeffs()=-q1_adj.coeffs();dot=-dot;}dot=clamp(dot, -1.0,1.0);double theta=acos(dot);double sin_theta=sin(theta);if(sin_theta<1e-8){// Lerp Quaterniond qt;qt.coeffs()=(1-t)*q0.coeffs()+ t * q1_adj.coeffs();qt.normalize();returnqt;}double s0=sin((1-t)*theta)/sin_theta;double s1=sin(t*theta)/sin_theta;Quaterniond qt;qt.coeffs()=s0*q0.coeffs()+s1*q1_adj.coeffs();return qt;} vector<Quaterniond>generate_quat_sequence(const Quaterniond&q0,const Quaterniond&q1,int num_points){ vector<Quaterniond>res;for(int i=0;i<num_points;i++){ double t=static_cast<double>(i)/(num_points-1);auto qt=slerp_manual(q0,q1,t);res.push_back(qt);} return res;} int main(){//起始姿态：单位四元数 Quaterniond q0(1,0,0,0);//绕Z轴旋转90度 Quaterniond q1(cos(M_PI/4),0,0,sin(M_PI/4));int N=10;autoseq=generate_quat_sequence(q0,q1,N);for(auto&q:seq){cout<<q.w()<<", "<<q.x()<<", "<<q.y()<<", "<<q.z()<<endl;}return0;}

如果不想手写 Slerp，Eigen 内置：Quaterniond qt = q0.slerp(t, q1);一行调用。

四、关键点说明

1. 为什么是轴角插值
   Slerp 等价于：先求出两个姿态之间等效旋转轴(\boldsymbol{k})，总旋转角(\theta)；插值时旋转角取(\theta_t = t\cdot\theta)，旋转轴保持不变，再由轴角((k,\theta_t))重新构造四元数。两种数学形式完全等价。
   轴角转四元数：
   q=[cos⁡θ2, kxsin⁡θ2, kysin⁡θ2, kzsin⁡θ2] q=\left[\cos\frac{\theta}{2},\;k_x\sin\frac{\theta}{2},\;k_y\sin\frac{\theta}{2},\;k_z\sin\frac{\theta}{2}\right]q=[cos2θ​,kx​sin2θ​,ky​sin2θ​,kz​sin2θ​]
   你也可以走这条路线实现：四元数 → 轴角 → 对角度插值 → 转回四元数，效果和 Slerp 完全一致。

2. 对比 Lerp
   直接对[w,x,y,z]线性插值得到的姿态，旋转角速度不是恒定的，姿态运动 “先快后慢”，机器人 / 飞行器姿态插值一般不用。

3. 工程坑点

• 四元数双覆盖：(q,-q)同一个姿态，必须判断点积符号，否则插值会绕远路（旋转 > 180°）
• 数值稳定性：arccos 输入必须钳位[-1,1]，防止浮点误差得到|dot|>1导致 NaN
• 输出四元数必须保持单位长度