简介一份面向惯性导航初学者与嵌入式开发者的C语言轨迹发生器源码包用于根据初始位置、速度与姿态信息模拟生成陀螺仪和加速度计数据进而连续推算载体位置、速度与姿态的实时运动轨迹解决真实场景中传感器原始数据难获得、算法验证缺入口的问题。程序覆盖牛顿运动方程、四阶龙格-库塔微分方程求解、传感器误差建模、低通滤波与卡尔曼滤波等关键算法并体现时间同步与系统集成思路适合结合IMU原理学习也可作为惯导/GPS组合导航算法的辅助参考。压缩包共2个文件包含1个C核心程序与1个文本说明文件C文件实现轨迹解算主逻辑文本文件则可能提供程序出处、编译环境或依赖库指引便于快速搭建运行环境。整体仅4KB轻量且几乎无依赖可直接阅读调试尤其适合在真实项目中参考其积分递推、滤波去噪的代码实现。目前已有814人学习读者既可对照源码理解从传感器原始数据处理到轨迹输出的完整流程也可将其作为惯性导航课程设计或工程原型的起点在现有基础上扩展更高精度算法。1. 惯性导航轨迹发生器仿真里最容易被低估的输入环节做组合导航算法验证时最缺的不是滤波器而是一条物理自洽的轨迹。手工拼航迹只能得到位置点很难同时保证速度、姿态、比力和角速度互相一致而这恰恰是惯导解算和滤波器的“输入食粮”。惯性导航轨迹发生器程序要解决的就是把这个输入侧的底牌补齐——让使用者通过机动段编排生成一段六自由度的完整轨迹并同步输出陀螺仪和加速度计的量测数据。这套程序适合两类人一类是做惯导解算、组合导航滤波研究的需要反复生成不同机动特征的数据来调参另一类是做半物理仿真或硬件在环测试的需要一个能稳定输出高频IMU数据的前端。它的价值不是“画出一条平滑曲线”而是保证位置、速度、姿态、角速度、比力在数学上自洽让后端算法能真正跑起来。下面我把建模、结构、坑和验证方法逐一过一遍新手可以照着做熟手可以直接跳到避坑章节。2. 运动学建模与坐标变换先立住姿态四元数和轨迹微分方程2.1 轨迹不能“画”出来要“积”出来见过不少同行提供的仿真数据位置和速度对得上姿态角和角速度之间却差得很远。这种数据拿给滤波器模型一收紧就发散。原因在于很多人把轨迹发生器做成了曲线插值器而不是运动学积分器。一条完整的惯导轨迹应该由下面的状态向量描述位置p导航系下、速度v导航系下、姿态四元数q描述机体系相对导航系、机体系角速度ω_b、导航系加速度a_n。它们之间的约束是微分方程ṗ vv̇ a_nq̇ 0.5 · q ⊗ [0, ω_b]轨迹发生器的本质就是给定机动段指令期望的加速度、角速度对这三个微分方程做正向积分。为什么不能用多项式拟合离散点来“生成轨迹”因为位置连续不代表速度和加速度连续。位置曲线看起来平滑一阶导、二阶导可能剧烈跳变体现在IMU数据里就是比力跳变滤波器的量测一致性直接崩溃。坐标系我统一用NED北东地机体坐标系为x前、y右、z下。这样在水平静止状态下姿态四元数为单位四元数方向余弦矩阵退化为单位阵不容易产生符号混乱。2.2 四元数姿态更新实现与数值细节姿态更新是轨迹自洽的核心。欧拉角观感直观但积分时会有万向节锁和顺序问题四元数没有这些毛病代价就是代码里多写几个函数。先给出基础的四元数乘法和姿态更新函数import numpy as np def quat_multiply(q, r): 四元数乘法顺序 w, x, y, z qw, qx, qy, qz q rw, rx, ry, rz r return np.array([ qw*rw - qx*rx - qy*ry - qz*rz, qw*rx qx*rw qy*rz - qz*ry, qw*ry - qx*rz qy*rw qz*rx, qw*rz qx*ry - qy*rx qz*rw, ]) def update_attitude(q, omega_b, dt): 用机体系角速度增量更新姿态四元数角增量法 norm np.linalg.norm(omega_b) if norm 1e-12: return q / np.linalg.norm(q) half_angle 0.5 * norm * dt axis omega_b / norm delta_q np.array([ np.cos(half_angle), np.sin(half_angle) * axis[0], np.sin(half_angle) * axis[1], np.sin(half_angle) * axis[2], ]) q_new quat_multiply(q, delta_q) return q_new / np.linalg.norm(q_new)这段的逻辑把机体系角速度在dt内看作恒定值用它构造一个增量四元数再右乘到当前姿态上。角增量法假设小时间步内角速度恒定dt取0.01秒100Hz时对常规机动足够如果机动角速度超过每秒300度建议升级为双子样或三子样圆锥补偿算法否则姿态积分会出现不可忽略的漂移。注意最后一步的归一化。四元数范数偏离1超过1e-8积分几百步后姿态就会悄悄偏掉这是最容易被忽略的坑。2.3 位置与速度的梯形积分以及转弯机动的数学底子位置和速度推进我一般用梯形积分而不是简单的前向欧拉def propagate_pos_vel(p, v, a_n, dt): 梯形积分v取前后两步平均计算位置增量 v_new v a_n * dt p_new p 0.5 * (v v_new) * dt return p_new, v_new前向欧拉只有一阶精度在持续加速段会产生二次项误差累积梯形积分是二阶精度实现成本几乎为零。高动态场景可以升级到龙格库塔四阶但对轨迹生成器来说梯形积分已经能保证和IMU数据解算结果对得齐。转弯机动的参数设计经常要靠手动计算。协调转弯constant-turn是最常用的段类型给定巡航速度v和偏航角速度ψ̇有转弯半径 R v / ψ̇向心加速度 a_c v · ψ̇所需滚转角 φ atan(a_c / g)举个例子速度20米每秒偏航率每秒10度约0.1745弧度转弯半径约114.6米向心加速度约3.49米每平方秒对应滚转角约19.6度。这个滚转角数值在后面编排滚转段时直接能用不用等仿真跑完再猜。3. 六自由度轨迹生成器结构机动编排、主循环与IMU采样3.1 模块划分状态对象、机动段定义与主循环轨迹发生器不建议做成一大坨脚本我通常拆成两个东西一个机动段描述结构一个轨迹生成器类。机动段用dataclass承载类型、时长和参数状态推进和段切换由生成器类完成。from dataclasses import dataclass, field from typing import List, Dict dataclass class Segment: seg_type: str # accel, straight, coord_turn, s_turn, climb duration: float # 段持续时间单位秒 params: Dict field(default_factorydict)生成器类的核心是主循环先根据当前时刻找到所在机动段再由机动段计算出期望的导航系加速度a_n和机体系角速度ω_b然后推进位置、速度、姿态。下面是一个简化但结构完整的实现class TrajGen: def __init__(self, p0, v0, heading0, gamma0, segments, dt0.01): self.p np.array(p0, dtypefloat) self.v np.array(v0, dtypefloat) self.heading np.deg2rad(heading0) # 偏航角弧度 self.gamma np.deg2rad(gamma0) # 爬升角弧度 self.q euler_to_quat(0.0, self.gamma, self.heading) self.segments segments self.dt dt self.t 0.0 def _pick_segment(self): t_accum 0.0 for seg in self.segments: if self.t t_accum seg.duration: return seg, self.t - t_accum t_accum seg.duration return None, 0.0 def _command(self, seg, t_local): 根据机动段类型计算加速度指令和角速度指令 dt self.dt v_old self.v.copy() v_norm np.linalg.norm(v_old) a_long 0.0 heading_dot 0.0 gamma_dot 0.0 if seg.seg_type accel: a_long seg.params[a] elif seg.seg_type straight: pass elif seg.seg_type coord_turn: heading_dot np.deg2rad(seg.params[heading_rate]) elif seg.seg_type s_turn: # 正弦偏航机动偏航角速度随时间正弦变化 amp np.deg2rad(seg.params[A]) T seg.params[T] heading_dot amp * (2 * np.pi / T) * np.cos(2 * np.pi * t_local / T) elif seg.seg_type climb: gamma_dot np.deg2rad(seg.params[gamma_rate]) # 由角度增量解析更新速度矢量 self.heading heading_dot * dt self.gamma gamma_dot * dt v_new_norm v_norm a_long * dt v_new v_new_norm * np.array([ np.cos(self.gamma) * np.cos(self.heading), np.cos(self.gamma) * np.sin(self.heading), -np.sin(self.gamma), ]) self.v v_new a_n (self.v - v_old) / dt # 机体系角速度近似低动态时直接用欧拉角速率 omega_b np.array([0.0, gamma_dot, heading_dot]) return a_n, omega_b def step(self): seg, t_local self._pick_segment() v_prev self.v.copy() if seg is None: a_n np.zeros(3) omega_b np.zeros(3) else: a_n, omega_b self._command(seg, t_local) self.p 0.5 * (v_prev self.v) * self.dt self.q update_attitude(self.q, omega_b, self.dt) self.t self.dt return { t: self.t, p: self.p.copy(), v: self.v.copy(), q: self.q.copy(), a_n: a_n, omega_b: omega_b, }这段代码的要点有两个。第一速度矢量直接由速度大小、偏航角、爬升角解析生成加速度由速度差分反推这样位置、速度、加速度三者严格自洽不会出现“速度方向转了但加速度算不对”的问题。第二ω_b这里用了低动态近似把偏航角速度直接当作机体系z轴角速度俯仰角速度当作机体系y轴角速度高动态场景需要在此基础上补充滚转角速度投影。作为原型验证这个近似够用了但心里要清楚它的边界。3.2 一个完整轨迹脚本巡航、转弯、S机动的编排有了类之后生成一段轨迹只需要组织好机动段列表。下面这个脚本生成一段包含加速、匀速、协调转弯、S机动、爬升的80多秒轨迹segments [ Segment(accel, 10.0, {a: 2.0}), Segment(straight, 20.0, {}), Segment(coord_turn, 18.0, {heading_rate: 10.0}), Segment(straight, 10.0, {}), Segment(s_turn, 20.0, {A: 15.0, T: 12.0}), Segment(climb, 8.0, {gamma_rate: 3.0}), ] gen TrajGen( p0[0.0, 0.0, -100.0], v0[0.0, 0.0, 0.0], heading00.0, gamma00.0, segmentssegments, dt0.01, ) with open(track.csv, w) as f: f.write(t,x,y,z,vx,vy,vz,roll,pitch,yaw\n) while gen.t 90.0: state gen.step() roll, pitch, yaw quat_to_euler(state[q]) f.write( f{state[t]:.3f}, f{state[p][0]:.3f},{state[p][1]:.3f},{state[p][2]:.3f}, f{state[v][0]:.3f},{state[v][1]:.3f},{state[v][2]:.3f}, f{np.rad2deg(roll):.3f},{np.rad2deg(pitch):.3f},{np.rad2deg(yaw):.3f}\n )机动段参数决定了轨迹特征。这里给出我常用的推荐范围方便初次上手时参考段类型关键参数输出特征推荐范围accela切向加速度速度线性变化1~3 m/s²straight无匀速直线无coord_turnheading_rate恒定偏航率速度大小不变5~15 deg/ss_turnA, T正弦偏航机动路径呈S形A: 10~30 deg, T: 8~20 sclimbgamma_rate爬升角线性变化1~5 deg/s总时长建议控制在60到120秒。太短轨迹特征不充分滤波器还没收敛就结束了太长单个数据文件会膨胀后处理也麻烦。3.3 采样率与插值生成IMU数据时的工程选择轨迹推进步长和IMU采样率是两个容易混淆的概念。我一般让轨迹推进步长等于IMU采样间隔即用同一个dt在一个循环里同时产出真值和量测值。这样时间戳天然对齐省掉后续同步的麻烦。采样率怎么定关键看两个约束载体动态频率和后续滤波器的更新频率。旋翼机和地面车辆的角运动通常在5Hz以内100Hz采样足够高机动固定翼或测试复杂机动时建议200Hz。如果IMU需要1000Hz而轨迹推进用100Hz不要直接用零阶保持填充因为零阶保持会产生角速度台阶被滤波器当作量测跳变。常见做法是先推进出100Hz的稀疏轨迹再用线性插值补位置、用球面线性插值SLERP补四元数角速度用稀疏点的差分值保持。4. 比力与陀螺模拟把轨迹翻译成IMU量测数据4.1 比力方程拆解加速度计测的从来不是坐标加速度轨迹生成器的真正价值在于把导航系下的轨迹翻译成加速度计和陀螺的输出。这里最容易出问题的是比力方程。加速度计测的不是坐标加速度而是比力specific force即扣除重力后的非引力加速度。在NED坐标系下取重力矢量g_n [0, 0, 9.80665]比力公式为f_b C_n^b · (a_n - g_n)其中C_n^b是从导航系到机体系的旋转矩阵。写代码前先把旋转矩阵函数准备好def quat_to_dcm(q): 四元数转方向余弦矩阵返回从机体系到导航系的C_b^n w, x, y, z q return np.array([ [1 - 2*(y*y z*z), 2*(x*y - w*z), 2*(x*z w*y)], [2*(x*y w*z), 1 - 2*(x*x z*z), 2*(y*z - w*x)], [2*(x*z - w*y), 2*(y*z w*x), 1 - 2*(x*x y*y)], ]) def sample_imu(state, accel_bias, gyro_bias, noise): 由轨迹状态生成IMU量测比力和角速度 C_bn quat_to_dcm(state[q]) C_nb C_bn.T # 导航系 - 机体系 a_n state[a_n] - np.array([0.0, 0.0, 9.80665]) f_b C_nb a_n omega_b state[omega_b] f_b f_b accel_bias np.random.normal(0.0, noise[accel], 3) omega_b omega_b gyro_bias np.random.normal(0.0, noise[gyro], 3) return f_b, omega_b关于符号约定这里必须说清楚上述公式中g_n取[0,0,9.80665]那么静止水平状态下a_n 0f_b C_nb · [0,0,-9.80665] [0,0,-9.80665]。也就是说按这个约定静止时加速度计的z轴输出是负的。如果你的解算算法里约定静止输出为正就把公式里的减号换成加号。两种约定都能自洽但必须前后一致否则垂直通道一定会飘。提示拿到任何轨迹发生器先跑一段1秒静止轨迹做自检看比力输出是否符合你前端解算的约定再谈后续。4.2 陀螺角速度生成协调转弯与高动态机动的输出第三章代码里的ω_b是低动态近似适合原型。真实高动态场景下协调转弯的陀螺输出需要包含滚转建立过程。协调转弯稳定段理想情况下机体系角速度只有z轴分量等于偏航率但在进入转弯前飞机会有一个先滚转的过程这个过程的滚转角速度ω_x必须体现在陀螺输出里。常见的段编排是把一个转弯拆成三段segments [ Segment(roll_in, 2.0, {roll_rate: -9.8}), # 横滚角速度建立 Segment(coord_turn, 20.0, {heading_rate: 10.0}), Segment(roll_out, 2.0, {roll_rate: 9.8}), # 改平 ]滚转角速度的值由目标滚转角除以建立时间得到。如果目标滚转角是19.6度2秒建立滚转率大约每秒9.8度。这样编排出来的数据陀螺x轴有真实的滚转脉冲滤波器的姿态收敛行为才接近真实飞行。4.3 传感器误差模型注入与参数设定纯理想IMU数据只能用来验证解算算法本身做滤波器鲁棒性测试时必须注入传感器误差。常见做法是常值偏置加白噪声再加一阶马尔可夫过程模拟温变漂移。def add_sensor_errors(f_b, omega_b, err_state, errors, dt): 注入常值偏置、白噪声和一阶马尔可夫误差 f_b f_b errors[accel_bias] np.random.normal(0.0, errors[accel_noise], 3) omega_b omega_b errors[gyro_bias] np.random.normal(0.0, errors[gyro_noise], 3) # 一阶马尔可夫模拟偏置的缓慢温变漂移 tau errors[tau] alpha 1.0 - dt / tau err_state[accel_bias] alpha * err_state[accel_bias] \ np.random.normal(0.0, errors[accel_bias_std], 3) err_state[gyro_bias] alpha * err_state[gyro_bias] \ np.random.normal(0.0, errors[gyro_bias_std], 3) return f_b, omega_b参数怎么给才算合理下面是一组中等精度MEMS的典型值适合大多数仿真场景参数含义仿真设定参考gyro_bias陀螺常值零偏1~10 deg/hgyro_noise陀螺白噪声角度随机游走0.1~1 deg/√haccel_bias加速度计常值零偏1~10 mgaccel_noise加速度计白噪声速度随机游走50~500 μg/√Hztau马尔可夫时间常数60~300 s噪声标准差需要按采样率换算。比如加速度计白噪声500μg/√Hz采样率100Hz时噪声标准差约为500μg除以√100即50μg左右。不换算直接填大噪声数据会被淹没滤波器反而好滤波这个仿真就失去意义了。5. 轨迹发生器的避坑与常见问题四类高频翻车现场5.1 机动切换瞬间轨迹跳变滤波器直接发散现象加速段切巡航段后解算速度曲线出现一个明显的台阶滤波器状态估计在切换点附近剧烈波动。原因机动段之间参数不连续。加速段结束时速度已经达到某个值但下一个匀速段的设定速度跟实际速度不一致位置和速度在切换瞬间出现人为的不连续。上文代码里速度矢量是连续推进的但如果手动拼段时没有继承上一段结束状态就会踩这个坑。解决所有机动段的状态推进只在生成器内部完成不要由外部传入下一段的初值段概念只负责给出“指令方向”实际速度、位置始终从上一时刻继续。需要明确过渡时在段末尾加一小段斜坡过渡段例如加速度从2降到0的过程写成一段3秒的减加速段。5.2 欧拉角积分“掉向”与四元数范数漂移现象跑长轨迹时姿态角出现莫名其妙的跳变特别是俯仰角接近90度时航向角瞬间翻转180度。原因用了欧拉角做积分。欧拉角微分方程在奇异点附近数值不稳定且积分误差会累积。我是亲眼见过只跑两分钟轨迹航向角直接从0度跳到180度的。解决姿态全部用四元数推进只在输出、可视化时才转成欧拉角。同时每次更新后检查四元数范数偏离1超过1e-8就归一化一次。这个检查不要偷懒数值误差是积累的不清理几百步后姿态已经悄悄偏了。5.3 比力符号和重力约定搞反垂直通道异常现象静止段解算高度开始匀速下降或上升加速度计输出的z轴数据和理论值方向相反。原因比力公式用了a_n - g_n但前端解算包用的是a_n g_n约定或者坐标系Z轴方向理解不一致。NED下重力矢量是正的机体Z轴向下这两点组合起来的符号很容易绕晕。解决生成任何完整轨迹前先输出一段1秒的静止段检查比力是否为[0,0,-9.8]或[0,0,9.8]取决于约定。不一致就把公式里的加减号对调然后重新生成全部数据。这个自检只需要几行代码却能省掉后面排查滤波发散的一整天时间。5.4 轨迹真值与IMU时间戳错位现象把轨迹真值和IMU量测喂给离线滤波器后状态估计整体滞后一个或半个采样周期位置误差曲线像被平移过一样。原因真值和量测不是同一时钟推进的。比如轨迹用100Hz循环生成IMU数据单独用另一个时间戳数组缓存两个文件写出来时时间基准不完全对齐或者插值用了不同的参考时间。解决单线程统一时钟推进在同一个step循环里既更新轨迹状态又立即生成对应的IMU量测输出的所有字段都打上同一个tick索引。如果要后处理插值统一用tick索引作自变量不要用自己拼的绝对时间。6. 进阶轨迹自洽性验证与传感器误差注入技巧6.1 让生成器自己“验尸”解算结果与真值轨迹对比轨迹生成完不要急着喂给滤波器。我一般会先生成一个不注入误差的干净数据用几十行的“逆解算”验一遍自洽性——拿生成的IMU数据反推位置再和真实轨迹做差分def verify_track(imu_f, imu_w, gt_p, dt0.01): 用IMU量测做简单的惯性解算对比参考轨迹位置 q np.array([1.0, 0.0, 0.0, 0.0]) v np.zeros(3) p np.zeros(3) for i in range(len(imu_f)): q update_attitude(q, imu_w[i], dt) C_bn quat_to_dcm(q) a_n C_bn imu_f[i] np.array([0.0, 0.0, 9.80665]) v a_n * dt p v * dt err p - gt_p rms np.sqrt(np.mean(err**2)) return rms干净数据下100秒轨迹的RMS通常小于0.5米数值积分误差在这个量级。如果RMS到了几十米甚至几百米那说明轨迹生成器的比力、角速度或姿态更新里有系统性错误。注入误差后的数据再用这个脚本验证会发现RMS显著放大这是正常现象不要反过来误判生成器有bug。6.2 场景库与误差注入喂给滤波器之前的最后一道工序我习惯把机动段组合成若干标准场景静止校准段、直线巡航段、城市工况段、S机动段、盘旋段。每个场景是一个segment列表按需拼接几行代码就能生成一组覆盖不同动态范围的数据。误差注入也按场景配置低误差场景用于验证算法精度温变偏置场景用于验证陀螺偏置估计能力阶跃故障场景用于验证故障检测逻辑。回顾我自己踩过多次的教训每次生成新轨迹都跳过了自洽性验证结果滤波器发散后回头排查发现是轨迹数据本身的问题。从那以后我每次跑轨迹生成器都会强制先过一遍静止段自检和位置RMS验证再决定要不要把数据喂给滤波器——这个习惯帮我省掉的排查时间比写验证脚本的时间多得多。希望帮到你。本文还有配套的精品资源点击获取
