4. 基于四元数的太阳翼姿态控制

任务定义

本案例要求设计一个基于四元数的太阳翼对日定向控制器。卫星本体姿态用四元数表示，目标姿态为使太阳翼法线指向太阳方向。控制器需利用四元数误差直接计算控制力矩，避免使用欧拉角带来的奇异性问题。输入为当前姿态四元数与目标姿态四元数，输出为三轴控制力矩，并通过仿真验证姿态收敛过程。

四元数姿态误差定义

设当前姿态四元数为 q = [q0, q1, q2, q3]^T（标量在前），目标姿态四元数为 q_d。姿态误差四元数定义为：

q_err = q_d^{-1} ⊗ q

其中 q_d^{-1} 为共轭四元数（单位四元数逆等于共轭），⊗ 为四元数乘法。当 q_err 的矢量部分趋近于零时，当前姿态收敛到目标姿态。控制器采用如下形式：

τ = -K_p · sign(q0_err) · q_vec_err - K_d · ω

其中 q_vec_err = [q1_err, q2_err, q3_err]^T，ω 为角速度，K_p、K_d 为正定增益矩阵。符号项 sign(q0_err) 保证误差始终沿最短旋转路径收敛，避免绕远路。

代码实现

import numpy as np
from scipy.integrate import solve_ivp

def quat_mult(q1, q2):
    """四元数乘法，q = [q0, q1, q2, q3]"""
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    return np.array([
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2
    ])

def quat_conj(q):
    """四元数共轭"""
    return np.array([q[0], -q[1], -q[2], -q[3]])

def quat_normalize(q):
    """归一化四元数"""
    norm = np.linalg.norm(q)
    return q / norm if norm > 0 else np.array([1.0, 0, 0, 0])

def quat_to_rotation_matrix(q):
    """四元数转旋转矩阵（用于验证）"""
    w, x, y, z = q
    return np.array([
        [1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)],
        [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)],
        [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)]
    ])

def satellite_dynamics(t, state, Kp, Kd, q_d, J):
    """卫星姿态动力学与运动学方程"""
    # state = [q0, q1, q2, q3, wx, wy, wz]
    q = state[0:4]
    omega = state[4:7]
    
    # 归一化四元数（防止数值漂移）
    q = quat_normalize(q)
    
    # 计算姿态误差四元数
    q_err = quat_mult(quat_conj(q_d), q)
    q_err = quat_normalize(q_err)
    
    # 控制器：τ = -Kp * sign(q0_err) * q_vec_err - Kd * ω
    q_vec_err = q_err[1:4]
    sign_q0 = np.sign(q_err[0])
    if sign_q0 == 0:
        sign_q0 = 1.0  # 避免零值
    tau = -Kp @ (sign_q0 * q_vec_err) - Kd @ omega
    
    # 姿态运动学（四元数微分方程）
    omega_quat = np.array([0, omega[0], omega[1], omega[2]])
    q_dot = 0.5 * quat_mult(q, omega_quat)
    
    # 刚体动力学（欧拉方程）
    omega_dot = np.linalg.inv(J) @ (tau - np.cross(omega, J @ omega))
    
    return np.concatenate([q_dot, omega_dot])

def simulate_sun_pointing():
    """仿真太阳翼对日定向控制"""
    # 卫星转动惯量 (kg·m²)
    J = np.diag([10.0, 12.0, 8.0])
    
    # 控制器增益
    Kp = np.diag([2.0, 2.0, 2.0])
    Kd = np.diag([5.0, 5.0, 5.0])
    
    # 目标姿态：太阳方向在卫星本体系中的期望指向
    # 假设太阳方向在惯性系中为 [1,0,0]，卫星初始姿态使太阳翼法线偏离
    # 目标四元数（使本体x轴指向太阳）
    q_d = np.array([1.0, 0.0, 0.0, 0.0])  # 单位四元数，零姿态
    
    # 初始状态：姿态偏离30度，初始角速度为零
    angle = np.radians(30)
    axis = np.array([0, 1, 0])  # 绕y轴旋转30度
    q_init = np.array([np.cos(angle/2), 
                       axis[0]*np.sin(angle/2),
                       axis[1]*np.sin(angle/2),
                       axis[2]*np.sin(angle/2)])
    omega_init = np.array([0.0, 0.0, 0.0])
    state0 = np.concatenate([q_init, omega_init])
    
    # 时间范围
    t_span = (0, 30)
    t_eval = np.linspace(0, 30, 300)
    
    # 求解微分方程
    sol = solve_ivp(satellite_dynamics, t_span, state0, 
                    args=(Kp, Kd, q_d, J), t_eval=t_eval,
                    rtol=1e-8, atol=1e-10)
    
    return sol

# 运行仿真
sol = simulate_sun_pointing()

# 计算姿态误差角随时间变化（用于验证）
def compute_error_angle(q, q_d):
    """计算当前姿态与目标姿态之间的旋转角"""
    q_err = quat_mult(quat_conj(q_d), q)
    q_err = quat_normalize(q_err)
    # 旋转角 = 2*arccos(|q0_err|)
    angle = 2 * np.arccos(np.clip(np.abs(q_err[0]), -1.0, 1.0))
    return np.degrees(angle)

error_angles = []
for i in range(len(sol.t)):
    q = sol.y[0:4, i]
    q = quat_normalize(q)
    error_angles.append(compute_error_angle(q, q_d))

# 输出最终误差角（预期输出，非实测）
final_error = error_angles[-1]
print(f"仿真时间: {sol.t[-1]:.1f}s")
print(f"最终姿态误差角: {final_error:.4f}°")
print(f"最大角速度: {np.max(np.abs(sol.y[4:7, :])):.4f} rad/s")

落地与验证

运行环境：Python 3.8+，需安装 numpy 和 scipy。将上述代码保存为 quat_attitude_control.py，在终端执行 python quat_attitude_control.py。

验证方法：观察程序输出的最终姿态误差角。若控制器设计正确，误差角应随时间收敛至接近零（小于0.1°）。同时可绘制误差角随时间变化曲线，应呈现单调衰减趋势。由于本案例未提供真实执行环境，上述输出为基于代码逻辑的预期结果，实际运行时应以真实输出为准。

判定标准：仿真结束时姿态误差角小于初始误差的5%（即小于1.5°），且角速度趋于零，则判定控制律有效。若误差不收敛或发散，需检查增益参数是否合理（Kp与Kd的比值应满足阻尼条件）。

关键说明

本实现采用单位四元数表示姿态，控制器直接使用四元数误差矢量部分，避免了欧拉角在90°附近出现的奇异问题。仿真中设置初始偏差30°，控制器增益取Kp=2、Kd=5（临界阻尼附近），预期在约15秒内收敛。若需调整响应速度，可按ζ=Kd/(2√Kp)关系修改增益。代码中quat_to_rotation_matrix函数可用于将结果转换为旋转矩阵以验证指向精度，但核心控制律不依赖该转换，确保全姿态范围内无奇异。