等效旋转矢量的参考坐标系

### 等效旋转矢量使用的参考坐标系

等效旋转矢量（ERV）描述的是相对于特定参考坐标系下的旋转操作。此坐标系的选择取决于具体应用场景的需求，但在惯性导航系统中，常见的有载体坐标系和地理坐标系。

对于载体坐标系而言，也被称为机体坐标系或本体坐标系，它固定于移动平台上并随平台一起运动。当讨论载体自身的姿态变化时，通常采用这种坐标系来定义等效旋转矢量[^1]。此时，等效旋转矢量的方向代表了相对于载体坐标系内某轴线发生的转角方向及其大小。

另一方面，在涉及地理位置定位的情况下，则会使用地理坐标系作为参照标准。这类坐标系一般选取地球表面某点为原点，并以其经度、纬度以及高度构建三维空间框架。如果要表达卫星轨道上航天器的姿态调整过程中的旋转情况，就可能需要用到基于此类坐标系统的等效旋转矢量表示方法[^2]。

值得注意的是，无论是哪种类型的参考坐标系，只要明确了起始状态与目标状态之间的相对位置关系后，就可以通过计算得出相应的等效旋转矢量来进行精确描述[^3]。

import numpy as np

def calculate_rotation_vector(initial_orientation, final_orientation):
    """
    计算两个四元数之间对应的等效旋转矢量
    
    参数:
        initial_orientation (numpy.ndarray): 初始朝向的单位四元数 [w,x,y,z]
        final_orientation (numpy.ndarray): 终止朝向的单位四元数 [w,x,y,z]

    返回:
        rotation_vector (numpy.ndarray): 对应的等效旋转矢量 [rx,ry,rz]
    """
    
    delta_quaternion = quaternion_multiply(quat_inverse(initial_orientation), final_orientation)
    angle = 2 * np.arccos(delta_quaternion[0])
    sin_half_angle = np.sqrt(1-delta_quaternion[0]*delta_quaternion[0])
    if abs(sin_half_angle)<1e-6:
        axis=np.zeros((3,))
    else:
        axis=delta_quaternion[1:]/sin_half_angle
        
    rotation_vector = angle*axis
    return rotation_vector


def quat_inverse(q):
    """求取给定四元数q=[w,x,y,z]的共轭"""
    w,x,y,z=q
    norm_squared=w*w+x*x+y*y+z*z
    inv_q=np.array([w,-x,-y,-z])/norm_squared
    return inv_q


def quaternion_multiply(q1,q2):
    """实现两个四元数相乘运算 q1*q2"""
    w1,x1,y1,z1=q1
    w2,x2,y2,z2=q2
    product_w=w1*w2-x1*x2-y1*y2-z1*z2
    product_x=w1*x2+x1*w2+y1*z2-z1*y2
    product_y=w1*y2-x1*z2+w2*y1+z1*x2
    product_z=w1*z2+x1*y2-y1*x2+w2*z1
    result=np.array([product_w,product_x,product_y,product_z])
    return result