关于PSINS运动轨迹仿真模块的理解和思考

本篇博客主要是对严恭敏老师PSINS工具箱中的轨迹仿真模块的一些阅读、理解和思考，同时也包含了一些疑问。运动轨迹仿真模块主要是输出载体的位置、姿态、速度，以及仿真生成惯性传感器信息的模块。一般做导航算法仿真验证的时候，都需要利用该模块生成传感器信息，验证算法的正确性和有效性。

坐标系定义

在PSINS中部分定义如下：

• 载体坐标系(b)： 右前上(RFU)
• 地理坐标系（导航系n）：东北天(ENU)
• 姿态/欧拉角向量： att=[俯仰pitch;横滚roll;方位yaw]，俯仰角范围-pi2 ~ pi/2，机头朝上为正；横滚范围-pi ~ pi，右倾为正；方位角范围-pi ~ pi，注意北偏西为正

飞行轨迹设计

1. 飞行轨迹类型

参考严恭敏老师《捷联惯导算法与组合导航原理》的第8章或者readme.doc的第10.8节可知，PSINS轨迹仿真中包含的运动类型有：

2. 相关代码

在demo/test_SINS_trj.m是一个轨迹仿真的例子。

（1） test_SINS_trj.m

• 在该文件中调用trjsegement函数对航迹进行了分段仿真
• 调用trjsimu函数生成姿态、速度、位置以及惯性器件信息
  

（2） trjsegement.m

该文件根据输出的航迹类型，持续时间，前向加速度，姿态变化率生成seg.wat。其中seg.wat中包含持续时间、机头前向速度、姿态变化率以及“t frame”中的加速度。

对于向心加速度这部分的计算，以及后面推算导航系下的加速度，我不太确定PSINS这部分计算的正确性，而且我感觉不引入t frame对我来说更容易理解，具体会在后面的思考中解释。

手册中对t frame相关的描述如下：

相关代码如下：

（3） trjsimu.m

手册中包含了计算惯性器件信息（陀螺仪输出、加速度计输出）的公式，对照公式看代码会比较容易理解。

手册中的相关描述：

相关代码：

疑问与思考

我主要不太明白的地方有两个：

1. damping是在做什么？
2. 计算导航系加速度an = Cnt*at (注释第7条)的方法是否正确？

-------------2022.11.08更新-----------------------
对于疑问1，最近对damping有一些理解，所以进行了疑问1的更新。
考虑一种情况:

• t-1时刻加速：wat1中 w(t-1)=0, a(t-1)=1m/s^2, v(t-1)=0
• t时刻减速：wat1中 w(t)=0, a(t)=-1m/s^2, v(t)=1*dt m/s；
• t+1时刻匀速：wat1中 w(t+1)=0, a(t+1)=0, v(t+1)=0；

按照代码51-59行的计算方式，t+1时刻，如果damping=0，那么当前时刻等上一时刻的速度vn1=vn=1*dt m/s, an=0。但是显然实际上t+1时刻我们希望的是速度为0，没有damping继续减速就做不到让速度为0。damping实质是在匀速段，在上一时刻的速度基础上，让它逐步加速或者减速到我们期望的匀速速度。

---------------原内容-------------------------
对于疑问2，我认为比较正确的方法是：

• 首先计算欧拉旋转矩阵，然后计算Wnbb — 机体相对导航系的角速率在机体系下的投影
  
• 叉乘计算向心力，并计算地理系加速度acceN （acceB只有机头方向有加速度，其他两个方向为0）
  

由于b系和t系y轴重合（机头前向），而且对于机体而言，只有前向有速度，因此PSINS中的算法则相当于：

用matlab计算试了试，在姿态角都不为0，且机体有速度，并且航向变化率不为0的情况下，这两种方法给出的结果是不太一致的。我没有做详细的公式推导，但是从计算结果来看，两种计算方法应该是不等价的。