基于四元数与滑模控制的卫星姿态控制
一、基本原理
1. 四元数姿态表示
四元数运动学方程：
3. 滑模控制设计
二、代码实现（Python）
1. 四元数运算工具
import numpy as npdef quat_mult(q1, q2):"""四元数乘法"""w1, x1, y1, z1 = q1w2, x2, y2, z2 = q2w = w1*w2 - x1*x2 - y1*y2 - z1*z2x = w1*x2 + x1*w2 + y1*z2 - z1*y2y = w1*y2 - x1*z2 + y1*w2 + z1*x2z = w1*z2 + x1*y2 - y1*x2 + z1*w2return np.array([w, x, y, z])def quat_conj(q):"""四元数共轭"""return np.array([q[0], -q[1], -q[2], -q[3]])def quat_error(q_current, q_desired):"""计算误差四元数 q_e = q_desired ⊗ q_current^{-1}"""q_conj = quat_conj(q_current)return quat_mult(q_desired, q_conj)
2. 滑模控制器
class QuaternionSMC:def __init__(self, J, lambda_, K, dt):self.J = J # 转动惯量矩阵 (3x3)self.lambda_ = lambda_self.K = K # 切换增益self.dt = dtdef compute_control(self, q, omega, q_d, omega_d):# 计算误差四元数q_e = quat_error(q, q_d)q_e_v = q_e[1:] # 提取向量部分 [q1, q2, q3]# 角速度误差omega_e = omega - omega_d# 滑模面 s = omega_e + lambda * q_e_vs = omega_e + self.lambda_ * q_e_v# 等效控制 u_eq = J*(d(omega_d)/dt - lambda*0.5*omega × q_e_v) + omega × J omega# 假设目标角加速度为0（omega_d为常数）u_eq = np.dot(self.J, -self.lambda_ * 0.5 * np.cross(omega, q_e_v)) \+ np.cross(omega, np.dot(self.J, omega))# 切换控制 u_sw = -K * sign(s)u_sw = -self.K * np.sign(s)# 总控制力矩u = u_eq + u_swreturn u
3. 卫星姿态动力学仿真
def simulate_satellite_attitude():# 参数初始化J = np.diag([100, 100, 80]) # 转动惯量 (kg·m²)lambda_ = 0.5K = 20.0dt = 0.01# 初始状态q = np.array([1.0, 0.0, 0.0, 0.0]) # 初始姿态（无旋转）omega = np.array([0.0, 0.0, 0.0]) # 初始角速度q_d = np.array([0.707, 0.0, 0.707, 0.0]) # 目标姿态（绕x轴旋转90度）omega_d = np.array([0.0, 0.0, 0.0]) # 目标角速度controller = QuaternionSMC(J, lambda_, K, dt)# 记录数据time = np.arange(0, 10, dt)q_history = []omega_history = []for t in time:# 计算控制力矩u = controller.compute_control(q, omega, q_d, omega_d)# 加入扰动（模拟太阳辐射压力）disturbance = np.array([0.1*np.sin(t), 0.1*np.cos(t), 0.05*np.sin(2*t)])u += disturbance# 更新角速度（欧拉积分）omega_dot = np.linalg.inv(J).dot(-np.cross(omega, J.dot(omega)) + u/J.diagonal()omega = omega + omega_dot * dt# 更新四元数（四阶龙格-库塔法）k1 = 0.5 * quat_mult(q, np.array([0, omega[0], omega[1], omega[2]]))k2 = 0.5 * quat_mult(q + 0.5*dt*k1, np.array([0, omega[0], omega[1], omega[2]]))k3 = 0.5 * quat_mult(q + 0.5*dt*k2, np.array([0, omega[0], omega[1], omega[2]]))k4 = 0.5 * quat_mult(q + dt*k3, np.array([0, omega[0], omega[1], omega[2]]))q = q + (dt/6)*(k1 + 2*k2 + 2*k3 + k4)q /= np.linalg.norm(q) # 归一化# 记录数据q_history.append(q)omega_history.append(omega)return time, np.array(q_history), np.array(omega_history)
三、使用示例
可视化代码
import matplotlib.pyplot as plttime, q_history, omega_history = simulate_satellite_attitude()# 绘制四元数误差
plt.figure()
plt.plot(time, q_history[:, 0], label='q0')
plt.plot(time, q_history[:, 1], label='q1')
plt.plot(time, q_history[:, 2], label='q2')
plt.plot(time, q_history[:, 3], label='q3')
plt.xlabel('Time (s)')
plt.ylabel('Quaternion')
plt.legend()
plt.title('Attitude Quaternion')# 绘制角速度
plt.figure()
plt.plot(time, omega_history[:, 0], label='ω_x')
plt.plot(time, omega_history[:, 1], label='ω_y')
plt.plot(time, omega_history[:, 2], label='ω_z')
plt.xlabel('Time (s)')
plt.ylabel('Angular Velocity (rad/s)')
plt.legend()
plt.title('Angular Velocity')
plt.show()
四、改进与注意事项
- 自适应增益：
- 根据扰动估计动态调整 ( K )，降低保守性。
- 执行器饱和：
- 在控制律中增加力矩限幅，避免超出执行器能力。
五、总结
- 优势：
- 四元数避免奇异性，滑模控制提供强鲁棒性，适合存在扰动和模型不确定性的卫星姿态控制。
- 挑战：
- 抖振可能影响执行器寿命，需结合高阶滑模或滤波技术。
- 应用扩展：
- 可结合姿态确定系统（如星敏感器、陀螺仪）实现闭环控制。