【PSINS工具箱 MATLAB例程】生成螺旋上升航迹的例程，并自动计算AVP（姿态、速度、位置）与IMU数据（加速度计与陀螺仪，含时间戳），附下载链接

生成螺旋上升航迹的MATLAB例程，并自动计算AVP（姿态、速度、位置）与IMU数据（加速度计与陀螺仪，含时间戳）。
需要安装PSINS工具箱，工具箱开源，如果需要可以提供下载链接。
代码中有丰富的中文注释，包运行成功
程序是我原创的，作者最了解自己的程序，如有其他店铺在卖，都是翻卖，遇到问题不便解决

关于工具箱

本文所述的代码需要基于PSINS工具箱，工具箱的讲解：

• PSINS初学指导：https://blog.csdn.net/callmeup/article/details/137087932

代码解析

本程序基于 PSINS 工具箱，用于生成一个 螺旋上升航迹（spiral trajectory），并输出对应的 AVP（姿态-速度-位置）与 IMU 数据，供定位与导航算法仿真使用。

航迹建模

假设载体从初始位置 $({\phi }_0,{\lambda }_0,h_0)$（纬度、经度、高度）出发，按螺旋规律上升，其轨迹可由下式给出：

• 角度随时间变化

$$\theta (t)=\omega \cdot t$$

其中，$\omega$ 为旋转角速度（rad/s）。

• 平面位移方程（以起点为参考系）：

$$\Delta x(t)=R\cdot \sin (\theta (t)),\Delta y(t)=R\cdot (\cos (\theta (t))-1)$$

其中，$R$ 为螺旋半径。

• 高度变化方程：

$$\Delta h(t)=v_{up}\cdot t$$

其中，$v_{up}$ 为上升速度。

将平面位移转换为经纬度：

$$\Delta \phi (t)=\frac{\Delta y(t)}{R_e},\Delta \lambda (t)=\frac{\Delta x(t)}{R_e\cdot \cos ({\phi }_0)}$$

其中，$R_e$ 为地球平均半径。

最终位置为：

$$\phi (t)={\phi }_0+\Delta \phi (t),\lambda (t)={\lambda }_0+\Delta \lambda (t),h(t)=h_0+\Delta h(t)$$

速度与姿态计算

通过位置对时间的数值微分得到北东地坐标系下速度 $(v_N,v_E,v_D)$：

$$v_N=\frac{\Delta \phi \cdot R_e}{\Delta t},v_E=\frac{\Delta \lambda \cdot R_e\cdot \cos (\phi )}{\Delta t},v_D=-\frac{\Delta h}{\Delta t}$$

姿态角则由速度和运动特性确定：

• 航向角 (yaw)：

$$\psi =\arctan \left(\frac{v_E}{v_N}\right)$$

• 俯仰角 (pitch)：

$$\theta =\arctan \left(-\frac{v_D}{\sqrt{v_N^2+v_E^2}}\right)$$

• 滚转角 (roll)：由向心加速度与重力的比值近似得到：

$$\varphi =\arctan \left(\frac{a_c}{g}\right)\cdot \sin (\theta (t)),a_c={\omega }^{2}R$$

数据输出

程序最终生成：

• AVP数据：姿态-速度-位置随时间变化的序列；
• IMU数据：加速度计与陀螺仪的原始观测值，便于后续滤波器仿真；
• 轨迹可视化：三维轨迹、高度曲线、速度与姿态变化。

运行结果

三维轨迹：

高度与水平曲线：

速度与姿态曲线：

AVP绘图：

IMU数据绘图：

部分代码

%% PSINS工具箱生成螺旋上升航迹的程序，生成AVP与IMU数据
% 如果程序报错，首先考虑工具箱是否安装正常，工具箱下载链接www.psins.org.cn
% 作者：matlabfilter
% 2025-09-08/Ver1

clear; close all; clc;
rng(0);
%% 初始化PSINS工具箱
glvs;

%% 设置仿真参数
ts = 0.01;          % 仿真步长 (s)
T = 300;            % 总仿真时间 (s)
t = (0:ts:T)';      % 时间序列
len = length(t);    % 数据长度

%% 螺旋上升航迹参数设置
% 初始位置 (经纬度，高度)
lat0 = 34.2*glv.deg;    % 初始纬度 (rad)
lon0 = 108.9*glv.deg;   % 初始经度 (rad) 
h0 = 100;               % 初始高度 (m)

代码结构：

完整代码

https://download.csdn.net/download/callmeup/91901842

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者