[发明专利]一种锚泊船舶姿态与位置监测方法及系统

权利要求书

1.一种锚泊船舶姿态与位置监测方法，其特征在于，包括步骤：

S1、通过设置于船舶船体外侧的多个惯导传感模块测量加速度、角速度、磁场强度、气压信息、GPS-RTK信息，GPS-RTK信息为全球定位卫星载波相位动态时差分信息；

S2、对惯导传感模块测量得到的信息进行融合处理，以分析船舶的姿态数据和位置数据；

步骤S2中包括：

S2.1、采用基于梯度下降法的两级姿态更新解算算法分析船舶的实时姿态数据；

S2.2、采用多个传感器融合的卡尔曼滤波算法分析船舶的实时位置数据；

惯导传感模块包括九轴传感器单元，所述九轴传感器单元包括三轴加速度计、三轴陀螺仪、三轴磁力计；

步骤S2.1中，具体包括：

S2.1.1、根据初始时刻的加速度数据、磁场强度数据得出惯导传感模块初始时刻的姿态数据，姿态数据包括俯仰角、横滚角以及偏航角；

S2.1.2、基于梯度下降法的两级姿态更新解算算法分析惯导传感模块的实时姿态数据，具体为：第一级，根据建立的三轴陀螺仪、三轴加速度计量测模型构建第一损失函数，并采用梯度下降法对惯导传感模块的俯仰角和横滚角进行更新；第二级，根据建立的三轴磁力计量测模型构建第二损失函数，并采用梯度下降法对惯导传感模块的偏航角进行更新；

S2.1.3、根据各惯导传感模块更新后的姿态数据计算得到船舶实时姿态数据；

步骤S2.2中，具体包括：

S2.2.1、根据初始时刻的GPS-RTK信息和气压信息分别得出各个惯导传感模块的地理位置坐标和所处海平面高度，从而计算得到惯导传感模块的初始位置；

S2.2.2、采用多个传感器融合的卡尔曼滤波算法分析惯导传感模块的实时位置数据，具体为：根据惯导传感模块的安装关系得到惯导传感模块的空间距离，作为距离观测量；由时间更新得到惯导传感模块的位置估计量以计算出惯导传感模块之间的距离估计量；由距离观测量减去距离估计量以得到惯导传感模块的位置误差值；在卡尔曼滤波的状态更新时，将得到的位置误差值乘以卡尔曼增益以得到更新后的状态量，利用更新后的状态量的误差修正每个惯导传感模块的位置估计量的累计误差，以得到惯导传感模块的最终位置；

S2.2.3、对步骤S2.2.2计算得到的多个惯导传感模块的最终位置取均值以得到船舶实时位置数据。

2.根据权利要求1所述的一种锚泊船舶姿态与位置监测方法，其特征在于，步骤S2.2.2中，所述惯导传感模块的位置估计量为通过旋转四元数算法和加速度双重积分法计算得到，具体为：

根据步骤S2.1.2计算得到的俯仰角、横滚角以及偏航角计算得到四元数，并通过旋转四元数算法得到姿态转移矩阵，通过姿态转移矩阵将惯导传感模块测得的载体坐标系的三轴加速度转移成导航坐标系的三轴加速度，对导航坐标系的三轴加速度数据进行二重积分，以得到惯导传感模块的位移，在初始位置的基础上进行位移叠加进而得到惯导传感模块的位置估计量。

3.根据权利要求2所述的一种锚泊船舶姿态与位置监测方法，其特征在于，步骤S2之后还包括：

S3、根据惯导传感模块的历史姿态数据以及历史位置数据以分别推测船舶下一时刻的姿态数据以及下一时刻的位置数据。

4.根据权利要求3所述的一种锚泊船舶姿态与位置监测方法，其特征在于，步骤S3具体包括：

S3.1、根据惯导传感模块的历史姿态数据进行卡尔曼滤波建模以推测船舶下一时刻的姿态数据；

S3.2、根据惯导传感模块的历史位置数据进行卡尔曼滤波建模，以预测下一时刻的船舶位置数据。