[发明专利]基于磁梯度的运动速度估计方法及装置

权利要求书

1.基于磁梯度的运动速度估计方法，其特征在于，包括以下步骤：

S1、触发航姿参考系统并同步引发磁传感器阵列模块对载体进行测量；根据航姿参考系统返回的姿态四元数数据计算旋转矩阵，根据航姿参考系统返回的角速度数据计算角速度反对称阵；步骤S1具体为：

S11、配置MCU控制模块中的定时器，触发各磁传感器的测量动作，实现航姿参考系统的数据以固定的频率进行输出；

S12、测量动作触发外部中断引脚电平跳变，引发航姿参考系统测量动作输出引脚上的电平进行高低转换；依次触发磁传感器阵列模块中的各个磁传感器，使磁传感器阵列模块能根据外部中断输入引脚上的电平变换触发各个磁传感器进行测量动作；

S13、根据航姿参考系统测量返回的姿态四元数数据，MCU控制模块计算旋转矩阵，计算公式为：

其中q₀、q₁、q₂、q₃为姿态四元数分量；

S14、根据航姿参考系统测量返回的角速度数据，MCU控制模块计算角速度反对称阵，计算公式为：

其中ω_x、ω_y、ω_z分别为x、y、z轴的角速度量测；

S2、采集磁传感器阵列模块中各磁传感器数据，计算得到磁场梯度，同时根据航姿参考系统返回的磁场数据计算磁场矢量；S2具体为：

S21、在航姿参考系统返回数据后，判断MCU控制模块与航姿参考系统之间的SPI通信使能引脚的电平状况，当使能引脚从低电平跳变为高电平，即表明MCU控制模块与航姿参考系统之间的SPI通信数据传输完成，此时立即采集磁传感器阵列模块中各个磁传感器的测量数据；

S22、对各个磁传感器的测量数据进行滤波与补偿；整合滤波及补偿后的各磁传感器数据，分别表示为B_A、B_B、B_C和B_D：

其中，B_Ax、B_Ay、B_Az分别表示磁传感器A在x、y、z轴上的测量数据；B_Bx、B_By、B_Bz分别表示磁传感器B在x、y、z轴上的测量数据；B_Cx、B_Cy、B_Cz分别表示磁传感器C在x、y、z轴上的测量数据；B_Dx、B_Dy、B_Dz分别表示磁传感器D在x、y、z轴上的测量数据；

S23、计算得到磁梯度G，公式为：

其中，L为十字形传感器阵列的基线距离；

S24、整合航姿参考系统返回的磁场数据，得到装置中心点附近的磁场矢量B^b；

S3、将计算得到的旋转矩阵、角速度反对称阵、磁场梯度及磁场矢量数据输入MCU控制模块中进行计算，得到载体运动速度；步骤S3具体为：

S31、计算磁场矢量对时间的导数：

其中B_x、B_y、B_z为磁场矢量B^b在x、y、z轴的分量；ΔT为时间步长，由传感器测量与数据刷新频率确定；

S32、整合旋转矩阵、角速度反对称阵、磁梯度数据、磁场矢量与磁场矢量对时间的导数，通过克拉默法则求解非齐次线性方程组：

其中V为载体相对惯性空间的运动速度矢量。

2.根据权利要求1所述基于磁梯度的运动速度估计方法，其特征在于，所述测量动作输出引脚是指MCU控制模块与航姿参考系统之间互联的GPIO引脚；所述外部中断输入引脚是指MCU控制模块与航姿参考系统的测量动作引脚相连的引脚。

3.根据权利要求1所述基于磁梯度的运动速度估计方法，其特征在于，所述SPI通信使能引脚的电平通过将使能引脚与MCU控制模块外部中断输入进行连接，并将使能引脚的中断触发条件设置为上升沿触发来进行判别；所述磁传感器阵列模块上各个磁传感器数据的采集在外部中断的中断处理函数中进行。