动手实践：用Python仿真一个简易的捷联惯导系统（SINS）-平芜编程栈

动手实践：用Python仿真一个简易的捷联惯导系统（SINS）

在自动驾驶、无人机和机器人领域，惯性导航系统（INS）扮演着至关重要的角色。它不依赖外部信号，仅通过内部传感器就能实现连续定位，这种特性使其在地下、水下或信号遮挡区域具有不可替代的优势。本文将带您用Python从零构建一个简易的捷联惯导仿真系统，通过代码实现来深入理解姿态解算、位置积分等核心概念。

1. 环境准备与基础概念

开始前需要安装必要的Python库：

pip install numpy matplotlib scipy

捷联惯导（Strapdown INS）与平台式系统的本质区别在于：它直接将惯性传感器（IMU）固连在载体上，通过数学算法替代物理稳定平台。这种设计带来了结构简化的优势，但也对计算能力提出了更高要求。

关键组件对比：

组件	平台式INS	捷联式INS
机械结构	复杂的三轴稳定平台	直接固定在载体上
计算复杂度	较低	较高
动态性能	受机械限制	更宽的工作范围
维护成本	高	低

提示：仿真时重点关注三个坐标系转换——载体坐标系(b系)、导航坐标系(n系)和惯性坐标系(i系)

2. IMU数据生成与仿真

真实的IMU输出包含噪声和误差，为简化初期验证，我们先实现理想传感器数据的模拟。创建一个IMUSimulator类来生成角速度和加速度数据：

class IMUSimulator: def __init__(self, fs=100): self.fs = fs # 采样频率(Hz) self.dt = 1/fs def generate_motion(self, duration, motion_type='turn'): t = np.arange(0, duration, self.dt) if motion_type == 'turn': # 模拟载体绕Z轴旋转90度 gyro = np.zeros((len(t), 3)) gyro[:, 2] = np.pi/2 / duration # 恒定角速度 accel = np.zeros((len(t), 3)) accel[:, 1] = 9.8 # 保持Y轴向上 return gyro, accel, t

常见运动模式仿真要点：

匀速直线运动：加速度计输出恒定重力分量
旋转运动：陀螺仪输出恒定角速度
加速运动：加速度计输出包含动态分量

3. 姿态解算算法实现

姿态解算是捷联惯导的核心，这里采用四元数法进行实现。相比欧拉角，四元数避免了万向节锁问题，计算效率也更高。

def quaternion_update(q, gyro, dt): # 四元数微分方程求解 omega = np.array([[0, -gyro[0], -gyro[1], -gyro[2]], [gyro[0], 0, gyro[2], -gyro[1]], [gyro[1], -gyro[2], 0, gyro[0]], [gyro[2], gyro[1], -gyro[0], 0]]) q = q + 0.5 * omega @ q * dt return q / np.linalg.norm(q) # 归一化

误差来源分析：

积分近似误差：特别是大机动时的不可交换性误差
传感器噪声：白噪声、随机游走等
初始对准误差：姿态初始化偏差

注意：实际应用中需要定期进行归一化处理，防止四元数发散

4. 位置与速度积分

完成姿态解算后，需要将加速度从载体坐标系转换到导航坐标系，并进行二次积分得到位置：

def update_velocity_position(q, accel, vel, pos, dt): # 坐标系转换 C_bn = quat2dcm(q) # 四元数转方向余弦阵 accel_n = C_bn @ accel - np.array([0, 0, 9.8]) # 扣除重力 # 速度位置更新 new_vel = vel + accel_n * dt new_pos = pos + (vel + new_vel) * 0.5 * dt return new_vel, new_pos

误差累积演示：

# 模拟1小时导航，比较理想轨迹与解算结果 true_pos = np.array([0, 0, 0]) calc_pos = np.array([0, 0, 0]) errors = [] for i in range(3600*100): # ...解算过程... errors.append(np.linalg.norm(true_pos - calc_pos)) plt.plot(errors) # 显示误差随时间增长

5. 可视化与结果分析

使用Matplotlib创建动态展示窗口可以直观观察误差累积过程：

def plot_trajectory_3d(true_traj, est_traj): fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.plot(true_traj[:,0], true_traj[:,1], true_traj[:,2], 'g-') ax.plot(est_traj[:,0], est_traj[:,1], est_traj[:,2], 'r--') ax.set_xlabel('X (m)'); ax.set_ylabel('Y (m)'); ax.set_zlabel('Z (m)') plt.legend(['True', 'Estimated'])

典型问题诊断：

发散速度过快：检查姿态解算周期是否匹配采样率
周期性振荡：可能存在传感器安装偏差
Z轴漂移明显：重力补偿不准确

6. 进阶改进方向

基础仿真完成后，可以考虑引入更现实的误差模型：

def add_imu_error(ideal_data): # 添加常见IMU误差 bias = 0.1 * np.random.randn() # 零偏 scale = 1 + 0.01 * np.random.randn() # 标度因数 noise = 0.05 * np.random.randn(len(ideal_data)) return scale * ideal_data + bias + noise

组合导航扩展：