惯性导航解算程序C++实现

本文为惯性导航解算程序，为sdust本科课程《惯性导航基础》随课实验及课程设计。
在此首先感谢石波老师对小组的支持与帮助，另感谢组员对这一项目作出的贡献！

由于编者水平有限，错误在所难免，望读者指正！！！

程序开源地址：https://github.com/jhuang-86/inertial-navigation
欢迎收藏！共同交流进步！

文章目录

惯性导航解算程序C++实现
1 性质、目的和任务
- 1.1性质、目的和任务
- 1.2组织和分工情况
2 重点及内容
- 2.1 欧拉角、方向余弦阵、四元数、等效旋转矢量转换
- 2.2 大地坐标与地心直角坐标相互转换
- 2.3 IMR格式惯导数据的读取与解析
- 2.4 地理坐标系下的姿态更新
- 2.5 地理坐标系下的速度和位置更新
- 2.6 解析粗对准
3 解算程序设计思路原理
- 3.1编译环境概述：
- 3.2程序组织架构：
- 3.2代码设计：
- - 3.2.1姿态转换矩阵
  - 3.2.2坐标转换
  - 3.2.3 IMR文件的读取与解析
  - 3.2.4 姿态、速度、位置更新
4 软件概述及功能介绍
- 4.1 概述
- 4.2软件功能概述
5 程序运行介绍

1 性质、目的和任务

1.1性质、目的和任务

1.2组织和分工情况

小组中共三人，分别为hj、pzl和myj，每人协作、独立进行了惯导解算程序的设计。

小组成员	分工
hj	解算程序的算法部分、可视化界面主体功能的实现
pzl	界面换肤、图标、按钮换颜色和保存图片与日志功能、软件测试
myj	说明帮助文档的撰写、程序介绍视频、软件测试

2 重点及内容

主要包含以下几个主要内容，分别为：1. 编程实现欧拉角、方向余弦阵、四元数以及等效旋转矢量之间的相互转换；2. 编程实现大地坐标与地心直角坐标相互转换；3. 编程实现IMR格式惯导数据的读取与解析；4. 编程实现地理坐标系下的姿态更新；5.编程实现地理坐标系下的速度和位置更新；6. 编程实现解析粗对准。

2.1 欧拉角、方向余弦阵、四元数、等效旋转矢量转换

参考严恭敏老师《捷联惯导算法与组合导航原理》

2.2 大地坐标与地心直角坐标相互转换

参考严恭敏老师《捷联惯导算法与组合导航原理》

2.3 IMR格式惯导数据的读取与解析

IMR格式的数据读取即通过给定的IMR格式编写读写程序。其数据结构如下表所示：

IMR Header Struct Definition
Word	Size (bytes)	Type	Description
szHeader	8	char[8]	“$IMURAW0” – NULL terminated ASCII string
bIsIntelOrMotorola	1	int8_t	0 = Intel (Little Endian), default 1 = Motorola (Big Endian)
dVersionNumber	8	double	Inertial Explorer program version number (e.g. 8.80)
bDeltaTheta	4	int32_t	0 = Data to follow will be read as scaled angular rates 1 = (default), data to follow will be read as delta thetas, meaning angular increments (i.e. scale and multiply by dDataRateHz to get degrees/second)
bDeltaVelocity	4	int32_t	0 = Data to follow will be read as scaled accelerations 1 = (default), data to follow will be read as delta velocities, meaning velocity increments (i.e. scale and multiply by dDataRateHz to get m/s2)
dDataRateHz	8	double	The data rate of the IMU in Hz. e.g. 0.01 second data rate is 100 Hz
dGyroScaleFactor	8	double	If bDeltaTheta == 0, multiply the gyro measurements by this to get degrees/second If bDeltaTheta == 1, multiply the gyro measurements by this to get degrees, then multiply by dDataRateHz to get degrees/second
dAccelScaleFactor	8	double	If bDeltaVelocity == 0, multiply the accel measurements by this to get m/s2 If bDeltaVelocity == 1, multiply the accel measurements by this to get m/s, then multiply by dDataRateHz to get m/s2
iUtcOrGpsTime	4	int32_t	Defines the time tags as GPS or UTC seconds of the week 0 = Unknown, will default to GPS 1 = Time tags are UTC seconds of week 2 = Time tags are GPS seconds of week
iRcvTimeOrCorrTime	4	int32_t	Defines whether the time tags are on the nominal top of the second or are corrected for receiver time bias 0 = Unknown, will default to corrected time 1 = Time tags are top of the second 2 = Time tags are corrected for receiver clock bias
dTimeTagBias	8	double	If you have a known bias between your GPS and IMU time tags enter it here
szImuName	32	char[32]	Name of the IMU being used
reserved1	4	uint8_t[4]	Reserved for future use
szProgramName	32	char[32]	Name of calling program
tCreate	12	time_type	Creation time of file
bLeverArmValid	1	bool	True if lever arms from IMU to primary GNSS antenna are stored in this header
lXoffset	4	int32_t	X value of the lever arm, in millimeters
lYoffset	4	int32_t	Y value of the lever arm, in millimeters
lZoffset	4	int32_t	Z value of the lever arm, in millimeters
Reserved[354]	354	int8_t[354]	Reserved for future use

The single header, which is a total of 512 bytes long, is followed by a structure of the following type for each IMU measurement epoch:

IMR Record Struct Definition
Word	Size	Type	Description
Time	8	double	Time of the current measurement
gx	4	int32_t	Scaled gyro measurement about the IMU X-axis
gy	4	int32_t	Scaled gyro measurement about the IMU Y-axis
gz	4	int32_t	Scaled gyro measurement about the IMU Z-axis
ax	4	int32_t	Scaled accel measurement about the IMU X-axis
ay	4	int32_t	Scaled accel measurement about the IMU Y-axis
az	4	int32_t	Scaled accel measurement about the IMU Z-axis

2.4 地理坐标系下的姿态更新

参考严恭敏老师《捷联惯导算法与组合导航原理》

2.5 地理坐标系下的速度和位置更新

参考严恭敏老师《捷联惯导算法与组合导航原理》

2.6 解析粗对准

参考严恭敏老师《捷联惯导算法与组合导航原理》

3 解算程序设计思路原理

参考严恭敏老师《捷联惯导算法与组合导航原理》

3.1编译环境概述：

操作系统: Windows 10专业版 19042.685
主要IDE: Microsoft Visual Studio 2019 + Qt Creator 4.11.1(Community)
程序语言：C++ 11
其他实现IDE: PyCharm Professional 2020.3.1, MATLAB 2020a

3.2程序组织架构：

算法的主要实现结构如下图所示：

3.2代码设计：

3.2.1姿态转换矩阵

欧拉角类的类架构如下所示：

// 欧拉角
class Euler_angle
{public:// 三个参数对应// phi      航向角// theta 俯仰角// gamma 横滚角Vector3d value = Eigen::Vector3d::Zero();Euler_angle() { value = Eigen::Vector3d::Zero(); }Euler_angle(Euler_angle const &input) { value = input.value; } // 对象初始化Euler_angle(double phi, double theta, double gamma);    // 数值初始化Euler_angle(Vector3d input_vector); // Vector3d初始化//transform functionMatrix3d EA2DCM();    // 欧拉角 --> 方向余弦阵Vector4d EA2QV();    // 欧拉角 --> 四元数Vector3d EA2ERV(); // 欧拉角 --> 等效旋转矢量// caculate functionMatrix3d EA2ssm();  //Skew-symmetric matrix 欧拉角 --> 反对称阵ssm// Basic functionvoid show();void initialize() { value = Eigen::Vector3d::Zero(); };     //初始化void operator<<(Vector3d vector3);           // 重载<<，便于向量赋值void operator=(Euler_angle input);  // 重载=，便于对象间赋值//Euler_angle operator*(Euler_angle& input);  // 重载*，便于进行乘法运算Euler_angle operator+(Euler_angle& input);
};

方形余弦阵类架构的如下所示：

// 方向余弦阵
class Direct_cosine_matrix
{public:Matrix3d value = Eigen::Matrix3d::Zero();Direct_cosine_matrix() { value = Eigen::Matrix3d::Zero(); }Direct_cosine_matrix(Direct_cosine_matrix& input) { value = input.value; }    // 对象初始化Direct_cosine_matrix(Matrix3d input_matrix);//transform functionVector3d DCM2EA();  // 方向余弦阵 --> 欧拉角Vector4d DCM2QV();   // 方向余弦阵 --> 四元数Vector3d DCM2ERV();  // 方向余弦阵 --> 等效旋转矢量// Basic functionvoid show();void initialize() { value = Eigen::Matrix3d::Zero(); };     //初始化void operator<<(Matrix3d matrix3);           //重载<<，便于向量赋值void operator=(Direct_cosine_matrix input);  //重载=，便于对象间赋值
};

四元数类的架构如下所示：

// 四元数
class Quaternion_vector
{public:Vector4d value = Eigen::Vector4d::Zero();Quaternion_vector() { value = Eigen::Vector4d::Zero(); }Quaternion_vector(Quaternion_vector& input) { value = input.value; } // 对象初始化Quaternion_vector(Vector4d input);//transform functionVector3d QV2EA(); //四元数 --> 欧拉角Matrix3d QV2DCM();  //四元数 --> 方向余弦阵Vector3d QV2ERV();    //四元数 --> 等效旋转矢量// Basic functionvoid show();void initialize() { value = Eigen::Vector4d::Zero(); };        //初始化void operator<<(Vector4d& matrix3);      //重载<<，便于向量赋值void operator=(Quaternion_vector& input);//重载=，便于对象间赋值Quaternion_vector operator*(Quaternion_vector& input); //重载四元数相乘
};

等效旋转矢量类的架构如下所示：

// 等效旋转矢量
class Equivalent_rotation_vector
{public:Vector3d value = Eigen::Vector3d::Zero();Equivalent_rotation_vector() { value = Eigen::Vector3d::Zero(); }Equivalent_rotation_vector(Equivalent_rotation_vector& input) { value = input.value; }  // 对象初始化Equivalent_rotation_vector(Vector3d input);//transform functionVector4d ERV2QV();   //等效旋转矢量 --> 四元数Matrix3d ERV2DCM();  //等效旋转矢量 --> 方向余弦阵Vector3d ERV2EA(); //等效旋转矢量 --> 欧拉角// Basic functionvoid show();void initialize() { value = Eigen::Vector3d::Zero(); };        //初始化void operator<<(Vector3d& vector3);                          //重载<<，便于向量赋值void operator=(Equivalent_rotation_vector& input);       //重载=，便于对象间赋值
};

3.2.2坐标转换

坐标转化的相应函数如下所示：

Vector3d gcs2gc(double lambda, double L, double h);         // 大地坐标转换为地心直角坐标
Vector3d gc2gcs(double x, double y, double z);              // 地心直角坐标转换为大地坐标
Vector3d gcs2gc(Vector3d BLH);                              // 大地坐标转换为地心直角坐标
Vector3d gc2gcs(Vector3d XYZ);                              // 地心直角坐标转换为大地坐标
Vector3d gc2gcs(double x, double y, double z);              // 地心直角坐标转换为大地坐标
double acu_tan_l(double t, double x, double y, double z);   // 迭代计算 大地纬度L

3.2.3 IMR文件的读取与解析

IMR 文件头的组织架构

struct IMR_Header
{char szHeader[8];              // "$IMURAW\0"int8_t bIsIntelOrMotorola;      // 0 表示小字节， 1表示大字节double dVersionNumber;         // 惯性资源管理器程序版本号int32_t bDeltaTheta;         // 0 接下来的数据将以变化的角度速率读取// 1 （默认）接下来的数据将被读取为delta，即角增量int32_t bDeltaVelocity;            // 0 接下来的数据将以变化的加速度读取// 1 （默认）接下来的数据将被读取为速度，即速度增量double dDataRateHz;               // IMU 的采样率 (Hz)double dGyroScaleFactor;        // bDeltaTheta == 0 将陀螺仪的测量值乘以该值得到 度/秒// bDeltaTheta == 1 将陀螺仪的测量值乘以该值得到 度，然后乘以dDataRateHz得到 度/秒double dAccelScaleFactor;        // bDeltaVelocity == 0 将实测数据乘以该值得到 m/s^2// bDeltaVelocity == 1 将实测数据乘以该值得到 m/s， 再乘以dDataRateHz就得到 m/s^2int32_t iUtcOrGpsTime;            // 将时间标记定义为一周的GPS或UTC秒// iUtcOrGpsTime == 0 未知，默认为GPS// iUtcOrGpsTime == 1 时间标签是每周的UTC秒// iUtcOrGpsTime == 2 时间标签是每周的GPS秒int32_t iRcvTimeOrCorrTime;       // 定义时间标记是否在第二个标称的顶部或为接收机时间偏差进行校正double dTimeTagBias;           // GPS和IMU时间标签之间的偏差char szImuName[32];              // 使用的IMU名称unit8_t reserved1[4];            // 暂无，留空char szProgramName[32];          // 调用程序名称char tCreate[12];              // 创建文件时间bool bLeverArmValid;           // 如果IMU至主GNSS天线的杆臂存储在此标头中，则为此项值为真int32_t lXoffset;              // 杠杆臂X mmint32_t lYoffset;             // 杠杆臂Y mmint32_t lZoffset;             // 杠杆臂Z mmint8_t Reserved[354];         // 留作未来使用
};

IMR 数据结构：

struct IMR_Record
{double Time;       // 当前观测的时间int32_t gx;           // 关于IMU x轴的缩放陀螺测量量int32_t gy;          // 关于IMU y轴的缩放陀螺测量量int32_t gz;          // 关于IMU z轴的缩放陀螺测量量int32_t ax;          // 关于IMU x轴的缩放加速测量int32_t ay;           // 关于IMU y轴的缩放加速测量int32_t az;           // 关于IMU z轴的缩放加速测量
};struct adj_IMR_Record
{double Time;       // 当前观测的时间double gx;            // 关于IMU x轴的缩放陀螺测量量double gy;           // 关于IMU y轴的缩放陀螺测量量double gz;           // 关于IMU z轴的缩放陀螺测量量double ax;           // 关于IMU x轴的缩放加速测量double ay;            // 关于IMU y轴的缩放加速测量double az;            // 关于IMU z轴的缩放加速测量
};

IMR数据读取结构

class imr_data
{public:IMR_Header* file_header;vector<adj_IMR_Record> file_data;void read_data(char* filepath);void initial();
private:void read_imr_header(fstream& imrfile, IMR_Header* imrheader);void read_imr_data(fstream& imrfile, vector<adj_IMR_Record>& adj_data, IMR_Header imr_header);
};

3.2.4 姿态、速度、位置更新

class update
{public:// 存放m-1时刻的方向余弦阵，存放m，m-1时刻的采样数据分别于vector[2],vecotr[1]，存放m-1时刻，m-2时刻的位置数据于vector[1],vector[0]，存放m-1,m-2时刻的速度数据于vector[1],vector[0], 存放采样周期update(Direct_cosine_matrix a1, vector<Vector3d> a2, vector<Vector3d> a3, vector<Vector3d> a4, vector<Vector3d> a5, double a7);void initialize(Direct_cosine_matrix a1, vector<Vector3d> a2, vector<Vector3d> a3, vector<Vector3d> a4, vector<Vector3d> a5, double a7);    // 初始化函数input_data inputdata;   // 输入数据attitude_updating_data attitude_data;           // 姿态更新结果vector<speed_updating_data> speed_data;         // 速度更新结果，通常存放三次速度更新结果，[0] m-2 时刻的速度更新，[1]，m -1 时刻的速度更新，[2]，m 时刻的速度更新vector<position_updating_data> position_data;   // 位置更新结果，通常存放三次位置更新结果，[0] m-2 时刻的位置，[1]，m-1 时刻的位置，[2]，m 时刻的速度更新vector<position_updating_data> position_result; // 存放结果void attitude_update();     // 姿态更新void speed_update();     // 速度更新void position_update();      // 位置更新void initial_alignment();    // 初始对准// 定义通用变量
private:double sinl, cosl, tanl;double Rm;double Rn;double Rmh;double Rnh;
};

4 软件概述及功能介绍

4.1 概述

操作系统：Windows 10
IDE：Microsoft Visual Studio 2015
程序语言：C++ 11
主程序文件构成：GPSsolving.h GPSsolving.cpp question.cpp

矩阵类文件构成：Matrix.h Matrix.cpp

4.2软件功能概述

软件功能如下图所示：

本惯性导航数据解算软件主要基于Qt开发，主要实现了下述的一些功能：

在数据解算方面，软件实现了IMR数据文件的解算读取、初始姿态数据的保存、惯导数据的解算（其中包括了欧拉角、方形余弦阵、四元数、等效旋转矢量的转换、大地坐标与地心直角坐标相互转换、地理坐标系的姿态更新、速度更新、位置更新、解析粗对准等功能）。
在数据的存储方面、主要实现了解算数据的存储、打开、绘图功能、绘图结果的存储、软件日志的存储。
在软件功能方面，软件实现了日志功能、撰写了相应的帮助文档、通过邮件联系作者以及切换软件界面视图等功能。

5 程序运行介绍

IMR文件读取

初始解算数据保存
进行粗对准

解算惯导数据

绘图显示

软件初始化

数据保存

数据导入

保存图片显示与程序日志

程序帮助文档与界面切换