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

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

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

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

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

---

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编译环境概述：

1. 操作系统: Windows 10专业版 19042.685

2. 主要IDE: Microsoft Visual Studio 2019 + Qt Creator 4.11.1(Community)

3. 程序语言：C++ 11

4. 其他实现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 function
    Matrix3d EA2DCM();	// 欧拉角 --> 方向余弦阵
    Vector4d EA2QV();	// 欧拉角 --> 四元数
    Vector3d EA2ERV();	// 欧拉角 --> 等效旋转矢量
    // caculate function
    Matrix3d EA2ssm();	//Skew-symmetric matrix	欧拉角 --> 反对称阵ssm
    // Basic function
    void 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 function
    Vector3d DCM2EA();	// 方向余弦阵 --> 欧拉角
    Vector4d DCM2QV();	// 方向余弦阵 --> 四元数
    Vector3d DCM2ERV();	// 方向余弦阵 --> 等效旋转矢量
    // Basic function
    void 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 function
    Vector3d QV2EA();	//四元数 --> 欧拉角
    Matrix3d QV2DCM();	//四元数 --> 方向余弦阵
    Vector3d QV2ERV();	//四元数 --> 等效旋转矢量
    // Basic function
    void 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 function
    Vector4d ERV2QV();	//等效旋转矢量 --> 四元数
    Matrix3d ERV2DCM();	//等效旋转矢量 --> 方向余弦阵
    Vector3d ERV2EA();	//等效旋转矢量 --> 欧拉角
    // Basic function
    void 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^2
    int32_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 mm
    int32_t lYoffset;				// 杠杆臂Y mm
    int32_t lZoffset;				// 杠杆臂Z mm
    int8_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 概述

1. 操作系统：Windows 10

2. IDE：Microsoft Visual Studio 2015

3. 程序语言：C++ 11

4. 主程序文件构成：GPSsolving.h GPSsolving.cpp question.cpp

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

4.2软件功能概述

软件功能如下图所示：

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

1. 在数据解算方面，软件实现了IMR数据文件的解算读取、初始姿态数据的保存、惯导数据的解算（其中包括了欧拉角、方形余弦阵、四元数、等效旋转矢量的转换、大地坐标与地心直角坐标相互转换、地理坐标系的姿态更新、速度更新、位置更新、解析粗对准等功能）。

2. 在数据的存储方面、主要实现了解算数据的存储、打开、绘图功能、绘图结果的存储、软件日志的存储。

3. 在软件功能方面，软件实现了日志功能、撰写了相应的帮助文档、通过邮件联系作者以及切换软件界面视图等功能。

5 程序运行介绍

1. IMR文件读取

2. 初始解算数据保存
   

3. 进行粗对准

4. 解算惯导数据

5. 绘图显示

6. 软件初始化

7. 数据保存

8. 数据导入

9. 保存图片显示与程序日志

10. 程序帮助文档与界面切换