一种基于PID算法的原子自旋陀螺仪全自动磁补偿方法

摘要

本发明涉及一种基于PID算法的SERF原子自旋陀螺仪全自动磁补偿方法。该方法全自动顺序进行三轴磁补偿，首先在扫描轴线圈加载方波扫描信号，采集由扫描轴线圈方波高低电平所引起的两个陀螺仪输出信号的稳态值并进行滤波处理，后将两稳态值做差，针对差值应用PID算法，根据差值大小自动分配PID参数，将这一时刻差值和之前时刻差值代入PID算法中进行计算得出一个步进量电压，并将步进量加载到补偿轴上，重复以上步骤直至陀螺输出稳态差值趋近为零进入误差带，则该补偿轴线圈磁场达到补偿点，单轴补偿结束，开始下一个轴补偿。该方法是由算法全自动执行进行三轴磁场补偿，各轴均可补偿至零磁补偿点，速度与手动磁补偿相比更快，精度优于0.1nT，操作简单，体积更小。

权利要求

1.一种基于PID算法的SERF原子自旋陀螺仪全自动磁补偿方法，其特征在于，包括如下步骤：步骤一、定义Z轴为陀螺仪抽运光方向，X轴为与之正交的检测光方向，与检测光和抽运光垂直的方向为Y轴，首先把SERF原子自旋陀螺仪调整到正常工作状态，保持抽运激光以Z轴方向进入碱金属气室，检测激光以X轴方向进入碱金属气室，而Y轴保持与抽运激光和检测激光正交，检测系统将经过碱金属气室的检测光信号转换为电信号输出；

步骤二、在Y轴线圈加载方波扫描信号，并在补偿轴Z轴线圈加载一个补偿磁场Bz，在方波扫描信号处于高电平时，等待陀螺仪输出信号稳定后采集陀螺仪此时的输出信号稳态值，将其进行滤波处理后记录为 在方波扫描信号变为低电平时，等待陀螺仪输出信号稳定后采集陀螺仪此时的输出信号稳态值，将其进行滤波处理后记录为 将两次采样值 和 作差，得到差值步骤三、将步骤二中求得的陀螺仪稳态输出差值Err代入PID算法，根据Err值的大小分配合适的PID参数，PID算法根据这一时刻和之前时刻的Err计算出补偿轴Z轴线圈补偿磁场Bz需要调整的步进量del_z电压值；

步骤四、将步进量del_z加载到补偿轴Z轴线圈磁场Bz上，重复以上步骤直到陀螺仪稳态输出差值Err进入误差带，最后记录此时的Z轴线圈磁场值Bz；

步骤五、Z轴磁场补偿完毕，去掉Y轴线圈的方波扫描信号，对其余两轴Y轴、X轴依次进行与步骤二、步骤三相同的操作；其中对Y轴进行补偿时，在Z轴线圈以Bz为基础加载方波扫描信号，并在补偿轴Y轴线圈加载一个补偿磁场By，PID计算出调整Y轴线圈磁场By所需的步进量del_y电压值，并将步进量del_y加载到补偿轴Y轴线圈上，重复上述补偿操作直至Y轴线圈磁场By达到补偿点；其中对X轴进行补偿时，在Z轴线圈以Bz为基础加载新的方波扫描信号，并在补偿轴X轴线圈加载一个补偿磁场Bx，PID计算出调整X轴线圈磁场Bx所需的步进量del_x电压值，并将步进量del_x加载到补偿轴X轴线圈上，重复上述操作直至X轴线圈磁场Bx达到补偿点；

步骤六、全自动磁补偿完成，去除Z轴线圈的方波扫描信号，记录此时三轴磁场值Bz、By、Bx；

通过上述六个步骤，实现对SERF原子自旋陀螺仪进行快速全自动三轴磁补偿。

2.根据权利要求1所述的一种基于PID算法的SERF原子自旋陀螺仪全自动磁补偿方法，其特征在于，所述的步骤二中，将陀螺仪输出稳态值进行滤波处理，在陀螺仪输出稳定后采集n个值，采用冒泡排序算法将n个值进行排序，然后截取n个值的中间一段进行平均处理，最终得到稳态值

3.根据权利要求1所述的一种基于PID算法的SERF原子自旋陀螺仪全自动磁补偿方法，其特征在于，所述的步骤三中，根据Err的值分配合适的PID参数，PID参数根据这一时刻和之前时刻Err计算，采用分段式位置式PID：其中T为积分时间常数，TD为微分时间常数；

将PID进行离散化：

del＝kp·Err1+ki·SUM+kd·(Err1‑Err2)

其中del为步进量电压值，SUM为之前时刻差值Err的累和，Err1代表这一时刻差值，Err2代表上一时刻差值，kp、ki、kd分别为PID控制算法的比例、积分、微分参数，根据陀螺仪两次稳态输出的差值Err对补偿轴线圈磁场步进量的响应是非线性的特点，设计根据Err差值的大小应用不同的kp、ki、kd参数，以实现Err的最快下降梯度，kp、ki、kd更新方式如下式：其中αp、αi、αd分别为kp、ki、kd所对应的权重系数。

4.根据权利 要求1所述的一种基于PID算法的SERF原子自旋陀螺仪全自动磁补偿方法，其特征在于，步骤中，将步进量del_z加载到补偿轴Z轴线圈磁场Bz上：将步进量del_y加载到补偿轴Y轴线圈磁场By上：

将步进量del_x加载到补偿轴X轴线圈磁场Bx上：

说明书

一种基于PID算法的原子自旋陀螺仪全自动磁补偿方法

技术领域

[0001] 本发明涉及一种基于PID算法的SERF(Spin‑Exchange‑Relaxation‑Free)原子自旋陀螺仪全自动磁补偿方法，应用于SERF原子自旋陀螺仪主动磁补偿领域，可以以全自动的方式快速将SERF原子自旋陀螺仪的磁场补偿到零磁状态，为SERF原子自旋陀螺仪的正常工作提供了必要的条件。

背景技术

[0002] 航空、航天、自动驾驶等领域对高精度惯性导航有着迫切的需要，而陀螺仪则是惯性导航系统的核心部件，陀螺仪的性能直接决定惯导系统的总体性能。原子陀螺仪被认为是新一代超高精度陀螺仪的重要发展方向，而SERF原子自旋陀螺仪被验证理论零偏稳定性‑8可达10 °/h，其具有体积小、精度超高的优点。

[0003] SERF原子自旋陀螺仪对工作环境要求苛刻，其中重要一点是需要工作在极弱磁环境下，极弱磁环境是SERF原子自旋陀螺仪实现原子无自旋交换弛豫态(SERF态)的必要条件，仅依靠磁屏蔽桶的被动磁补偿难以实现极弱磁条件，因此需要利用核子自旋自补偿的原理设计一种主动磁补偿系统。目前主要采用交叉调制三轴磁补偿的方法进行手动磁补偿，此方法需要两台信号发生器作为基础，通过示波器观察两次陀螺仪稳态输出波形，并粗略作差，根据差值大小凭经验给出步进量来调整补偿轴磁场，以此进行手动磁补偿工作，通过信号发生器、示波器等仪器观察陀螺仪稳态输出，凭经验给出步进量调整补偿轴磁场，这种方法存在磁场补偿精度低、操作繁琐、补偿时间长的问题。因此，研究一种可以解放人力，全自动采集陀螺仪输出稳态值，准确计算出补偿轴磁场所需步进量，自动进行三轴磁补偿，从而提高补偿精度、缩短补偿时间的方法具有重要意义。

发明内容

[0004] 本发明要解决的技术问题是：解决传统SERF原子自旋陀螺仪手动磁补偿方法需要通过信号发生器、示波器等仪器观察陀螺仪稳态输出，凭经验给出步进量调整补偿轴磁场，导致精度低、操作繁琐、补偿时间长的问题。

[0005] 本发明解决上述技术问题采用的技术方案为：针对上述问题，本专利设计的一种基于PID算法的SERF原子自旋陀螺仪全自动磁补偿方法。无需信号发生器和示波器，基于DSP+FPGA(可编程逻辑门阵列)控制模块、AD数据采集模块、DA数据输出模块的硬件系统，全自动顺序进行三轴磁补偿，首先在扫描轴线圈加载方波扫描信号，采集由扫描轴线圈方波高低电平所引起的两个陀螺仪输出信号的稳态值并进行滤波处理，后将两稳态值做差，针对差值应用PID算法，根据差值大小自动分配PID参数，将这一时刻差值和之前时刻差值代入PID算法中进行计算得出一个步进量电压，并将步进量加载到补偿轴上，重复以上步骤直至陀螺输出稳态差值趋近为零进入误差带，则该补偿轴线圈磁场达到补偿点，单轴补偿结束，开始下一个轴补偿。该方法是全自动进行三轴磁场补偿，各轴均可补偿至零磁补偿点，与传统手动磁补偿相比完全解放了人力，补偿速度更快，补偿精度更高，优于0.1nT，操作简单，体积更小。

[0006] 在无人工操作的前提下，系统在扫描轴线圈加载方波扫描信号，自动采集由扫描轴线圈方波高低电平所引起的两个陀螺仪稳态输出，滤波后做差，针对差值应用PID算法，根据差值大小自动分配PID参数，将这一时刻差值和之前时刻差值代入PID算法中进行计算得出一个步进量电压，并将步进量加载到补偿轴上，重复以上步骤直至陀螺输出稳态差值趋近为零进入误差带，则该补偿轴线圈磁场达到补偿点，单轴补偿结束，开始下一个轴补偿，以此实现全自动三轴磁补偿，各轴均可补偿至零磁补偿点。

[0007] 所述方法包括以下步骤：

[0008] 步骤一、定义Z轴为陀螺仪抽运光方向，X轴为与之正交的检测光方向，与检测光和抽运光垂直的方向为Y轴。首先把SERF原子自旋陀螺仪调整到正常工作状态，保持抽运激光以Z轴方向进入碱金属气室，检测激光以X轴方向进入碱金属气室，而Y轴保持与抽运激光和检测激光正交，检测系统将经过碱金属气室的检测光信号转换为电信号输出；

[0009] 步骤二、在Y轴线圈加载方波扫描信号，并在补偿轴Z轴线圈加载一个补偿磁场Bz，在方波扫描信号处于高电平时，等待陀螺仪输出信号稳定后采集陀螺仪此时的输出信号稳态值，将其进行滤波处理后记录为 在方波扫描信号变为低电平时，等待陀螺仪输出信号稳定后采集陀螺仪此时的输出信号稳态值，将其进行滤波处理后记录为 将两次采样值 和 作差，得到差值

[0010] 步骤三、将步骤二中求得的陀螺仪稳态输出差值Err代入PID算法，根据Err值的大小分配合适的PID参数，PID算法根据这一时刻和之前时刻的Err计算出补偿轴Z轴线圈补偿磁场Bz需要调整的步进量del_z电压值；

[0011] 步骤四、将步进量del_z加载到补偿轴Z轴线圈磁场Bz上，重复以上步骤直到陀螺仪稳态输出差值Err进入误差带，最后记录此时的Z轴线圈磁场值Bz；

[0012] 步骤五、Z轴磁场补偿完毕，去掉Y轴线圈的方波扫描信号，对其余两轴Y轴、X轴依次进行与步骤二、步骤三相同的操作。其中对Y轴进行补偿时，在Z轴线圈以Bz为基础加载方波扫描信号，并在补偿轴Y轴线圈加载一个补偿磁场By，PID计算出调整Y轴线圈磁场By所需的步进量del_y电压值，并将步进量del_y加载到补偿轴Y轴线圈上，重复上述操作直至Y轴线圈磁场By达到补偿点。其中对X轴进行补偿时，在Z轴线圈以Bz为基础加载新的方波扫描信号，并在补偿轴X轴线圈加载一个补偿磁场Bx，PID计算出调整X轴线圈磁场Bx所需的步进量del_x电压值，并将步进量del_x加载到补偿轴X轴线圈上，重复上述补偿操作直至X轴线圈磁场Bx达到补偿点；

[0013] 步骤六、全自动磁补偿完成，去除Z轴线圈的方波扫描信号，记录此时三轴磁场值Bz、By、Bx；

[0014] 通过上述六个步骤，实现对SERF原子自旋陀螺仪进行快速全自动三轴磁补偿。

[0015] 本发明的原理是：根据Bloch方程组可得，SERF原子自旋陀螺仪的稳态输出对Y轴磁场By的微分为：

[0016]

[0017] 其中 为SERF原子自旋陀螺仪的稳态输出，By为Y轴线圈磁场， 为惰性气体原子感受到的碱金属原子产生的磁场， 为Z方向的电子极化率，γe为电子自旋旋磁比，Rtot为碱金属电子自旋的总弛豫率，△Bz为补偿轴Z轴线圈磁场与补偿点磁场的差值，L为光频移。

[0018] 由上式可得 的正负取决于ΔBz的正负，当ΔBz＝0(补偿轴Z轴磁场达到补偿点)时， 即ΔBz＝0时，SERF原子自旋陀螺仪的稳态输出 不随Y轴磁场By变化而变化。故补偿Z轴磁场原理就是在Y轴线圈施加一个方波扫描磁场信号ΔBy，观察陀螺仪输出变化值通过以步进量的方式微调Z轴线圈磁场Bz以调节ΔBz直至 此时即实现Z轴磁场补偿完毕。由此可得，磁补偿过程的关键就是以陀螺仪输出变化值 也就是差值Err为基础，求得步进量来调整补偿轴磁场直至达到补偿点。

[0019] 将陀螺仪输出稳态值进行滤波处理：在陀螺仪输出稳定后采集n个值，采用冒泡排序算法将n个值进行排序，然后截取n个值的中间一段进行平均处理，最终得到稳态值 滤波处理用于解决陀螺仪输出不稳定造成的采样值误差加大，导致步进量计算不准确，从而加大补偿轴磁场与补偿点偏离程度的问题。

[0020] 而微调Z轴磁场的步进量del_z则通过PID算法计算得出：

[0021] del＝kp·Err1+ki·SUM+kd·(Err1‑Err2)

[0022] 其中del为步进量电压值，SUM为之前时刻差值Err的累和，Err1代表这一时刻差值，Err2代表上一时刻差值，kp、ki、kd分别为PID控制算法的比例、积分、微分参数。根据陀螺仪两次稳态输出的差值Err对补偿轴线圈磁场步进量的响应是非线性的特点，设计根据Err差值的大小应用不同的kp、ki、kd参数，以实现Err的最快下降梯度。kp、ki、kd更新方式如下式：

[0023]

[0024] 其中αp、αi、αd分别为kp、ki、kd所对应的权重系数。

[0025] 将步进量del_z加载到补偿轴Z轴线圈磁场Bz上：当Bz>0、Err>0、则Bz＝Bz+del_z；Bz>0、Err<0、则Bz＝Bz‑del_z；Bz<0、Err>0、则Bz＝Bz‑del_z；Bz<0、Err<0、则Bz＝Bz+del_z。总结为下式：

[0026]

[0027] 其中Bz的正负代表Z轴磁场的方向，以后的By、Bx的正负也代表方向。

[0028] SERF原子自旋陀螺仪的稳态输出 对Bz分别进行一次微分、二次微分：

[0029]

[0030]

[0031] 实现Z轴磁场补偿完成后，以Bz为基础加载一个方波磁场(Bz‑ΔBz，Bz+ΔBz)观察陀螺仪输出变化值 通过以步进量的方式微调Y轴线圈磁场By以调节ΔBy使 此时即实现Y轴磁场补偿完毕；然后在以Bz为基础加载一个新的方波磁场(Bz‑2ΔBz，Bz)观察陀螺仪输出变化值 通过以步进量的方式微调X轴线圈磁场Bx以调节ΔBx使 此时
即实现X轴磁场补偿完毕。其中微调Y轴磁场、X轴磁场的步进量del_y、del_x通过PID算法计算得出：

[0032] del＝kp·Err1+ki·SUM+kd·(Err1‑Err2)

[0033] 其中del为步进量电压值，SUM为之前时刻差值Err的累和，Err1代表这一时刻差值，Err2代表上一时刻差值，kp、ki、kd分别为PID控制算法的比例、积分、微分参数。

[0034] 根据陀螺仪两次稳态输出的差值Err对补偿轴线圈磁场步进量的响应是非线性的突然点，，设计根据Err差值的大小应用不同的kp、ki、kd参数，以实现Err的最快下降梯度。kp、ki、kd更新方式如下式：

[0035]

[0036] 其中αp、αi、αd分别为kp、ki、kd所对应的权重系数。

[0037] 将步进量del_y加载到补偿轴Y轴线圈磁场By上：当By>0、Err>0、则By＝By+del_y；By>0、Err<0、则By＝By‑del_y；By<0、Err>0、则By＝By‑del_y；By<0、Err<0、则By＝By+del_y。总结为下式：

[0038]

[0039] 将步进量del_x加载到补偿轴X轴线圈磁场Bx上：当Bx>0、Err>0、则Bx＝Bx‑del_x；Bx>0、Err<0、则Bx＝Bx+del_x；Bx<0、Err>0、则Bx＝Bx+del_x；Bx<0、Err<0、则Bx＝Bx‑del_x。总结为下式：

[0040]

[0041] 本发明与现有技术相比的优点在于：

[0042] 本发明设计的一种基于PID算法的SERF原子自旋陀螺仪全自动磁补偿方法，与传统手动磁补偿方法通过示波器观察两次陀螺仪稳态输出波形，并粗略作差，根据差值大小凭经验给出步进量来调整补偿轴磁场相比，完全解放了人力，以PID算法为核心的自动磁补偿系统，实现了自动采集两次陀螺仪稳态输出滤波后做差，无需借助信号发生器、示波器等仪器，不仅体积成倍缩小，采集数据会更精准，减少了人工读数的误差。针对差值应用PID算法，根据差值大小自动分配PID参数进行PID计算，得出补偿轴所需的步进量电压值，并将步进量加载到补偿轴上自动完成三轴磁补偿过程。调整补偿轴磁场所需的步进量不再由人工凭经验给出，而是由PID算法计算得出，解放了人力，加快了补偿速度，提高了补偿精度，实现三轴磁补偿精度优于0.1nT。

附图说明

[0043] 图1为本发明所设计的基于PID算法的全自动磁补偿方法系统框架示意图；

[0044] 图2为基于PID算法的全自动磁补偿方法的硬件系统框图；

[0045] 图3为整个系统的算法流程图。

具体实施方式

[0046] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅为本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域的普通技术人员在不付出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。

[0047] 图1为本发明的系统框架示意图，定义Z轴为陀螺仪抽运光方向，X轴为与之正交的检测光方向，与检测光和抽运光垂直的方向为Y轴。其中，1代表在Z轴方向的抽运激光，2代表X轴方向的检测激光，3代表Z轴线圈，4代表X轴线圈，5代表Y轴线圈，6代表被动磁屏蔽桶，7代表光电探测器，8代表碱金属气室。

[0048] 本发明的硬件系统框图如图2所示，采用DSP+FPGA(可编程逻辑门阵列)控制模块、AD数据采集模块、DA数据输出模块的硬件系统来代替手动补磁场所用的信号发生器和示波器。

[0049] 本发明的算法流程图如图3所示，本发明设计的一种基于PID算法的SERF原子自旋陀螺仪全自动磁补偿方法，其具体实施方式包括下列步骤：

[0050] 步骤一、首先把SERF原子自旋陀螺仪调整到正常工作状态，保持抽运激光以Z轴方向进入碱金属气室，检测激光以X轴方向进入碱金属气室，而Y轴保持与抽运激光和检测激光正交，检测系统将经过碱金属气室的检测光信号转换为电信号输出；

[0051] 步骤二、通过FPGA驱动DA数据输出模块在Y轴线圈5加载一个方波扫描信号，并在补偿轴Z轴线圈3加载一个补偿磁场Bz，在方波扫描信号处于高电平时，由FPGA驱动AD数据采集模块采集陀螺仪输出信号，FPGA实时将采集数据送至DPS芯片中处理，等待陀螺仪输出信号稳定后采集陀螺仪此时的输出信号稳态值，陀螺仪对方波信号的响应是由震荡趋于平稳的曲线信号，大致时长为20s，故设置方波半周期为25s，在方波电平末尾处设置采样点，采集陀螺仪输出信号稳态值，将其进行滤波处理后记录为 在方波扫描信号变为低电平时，等待陀螺仪输出信号稳定后采集陀螺仪此时的输出信号稳态值，将其进行滤波处理后记录为 将两次采样值 和 作差，得到差值

[0052] 步骤三、在DSP芯片中进行PID运算，将步骤二中求得的陀螺仪稳态输出差值Err代入PID算法，根据Err的值分配合适的PID参数，PID算法根据这一时刻和之前时刻的Err计算出补偿轴Z轴线圈3补偿磁场Bz需要调整的步进量del_z电压值；

[0053] 步骤四、DSP将步进量del_z加载到补偿轴Z轴线圈3上，更新磁场Bz，然后将更新后的Bz发送至FPGA，并由FPGA驱动DA数据输出模块将更新后的Bz输出到补偿轴Z轴线圈3上，重复以上步骤直到差值Err进入误差带，最后记录此时的Z轴线圈3磁场值Bz；

[0054] 步骤五、Z轴磁场补偿完毕，去掉Y轴线圈5的方波扫描信号，对其余两轴Y轴、X轴依次进行与步骤二、步骤三相同的操作。其中对Y轴进行补偿时，通过FPGA驱动DA数据输出模块在Z轴线圈3以Bz为基础加载方波扫描信号，并在补偿轴Y轴线圈5加载一个补偿磁场By，PID计算出调整Y轴线圈5磁场By所需的步进量del_y电压值，并将步进量del_y加载到补偿轴Y轴线圈5上，重复上述操作直至Y轴线圈5磁场By达到补偿点。其中对X轴进行补偿时，通过FPGA驱动DA数据输出模块在Z轴线圈3以Bz为基础加载新的方波扫描信号，并在补偿轴X轴线圈4加载一个补偿磁场Bx，PID计算出调整X轴线圈4磁场Bx所需的步进量del_x电压值，并将步进量del_x加载到补偿轴X轴线圈4上，重复上述操作直至X轴线圈4磁场Bx达到补偿点；

[0055] 步骤六、全自动磁补偿完成，去除Z轴线圈3的方波扫描信号，DSP芯片将补偿后的三轴磁场值Bz、By、Bx储存到寄存器中并发送给FPGA芯片，FPGA驱动DA数据输出模块持续输出相应值到三轴线圈之上。

[0056] 根据所述的步骤二中，将陀螺仪输出稳态值进行滤波处理：在陀螺仪输出稳定后采集n个值，采用冒泡排序算法将n个值进行排序，然后截取n个值的中间一段进行平均处理，最终得到稳态值 滤波处理用于解决陀螺仪输出不稳定造成的采样值误差加大，导致步进量计算不准确，从而加大补偿轴磁场与补偿点偏离程度的问题。

[0057] 根据所述的步骤三中，在DSP芯片中进行PID运算，针对差值Err应用PID算法，根据Err的值分配合适的PID参数，PID算法根据这一时刻和之前时刻Err计算，其特征在于：采用分段式位置式PID：

[0058]

[0059] 其中T为积分时间常数，TD为微分时间常数；

[0060] 将PID进行离散化：

[0061] del＝kp·Err1+ki·SUM+kd·(Err1‑Err2)

[0062] 其中del为步进量电压值，SUM为之前时刻差值Err的累和，Err1代表这一时刻差值，Err2代表上一时刻差值，kp、ki、kd分别为PID控制算法的比例、积分、微分参数。本发明根据陀螺仪两次稳态输出的差值Err对补偿轴线圈磁场步进量的响应是非线性的特点，设计根据Err差值的大小应用不同的kp、ki、kd参数，以实现Err的最快下降梯度。kp、ki、kd更新方式如下式：

[0063]

[0064] 其中αp、αi、αd分别为kp、ki、kd所对应的权重系数。

[0065] 根据所述的步骤中，DSP将步进量del_z加载到补偿轴Z轴线圈3上，更新磁场Bz，其特征在于：

[0066]

[0067] DSP将步进量del_y加载到补偿轴Y轴线圈5上，更新磁场By，其特征在于：

[0068]

[0069] DSP将步进量del_x加载到补偿轴X轴线圈4上，更新磁场Bx，其特征在于：

[0070]

[0071] 本发明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。