[发明专利]一种单轴激光寻北仪的多位置自寻北方法

摘要

一种单轴激光寻北仪的多位置自寻北方法，包括以下步骤：首先接收到寻北指令后，进行陀螺仪和加速度计的实时误差补偿；根据设定的目标角度进行电机驱动控制，到达目标角度后，进行仪表采样；利用多位置采样结果解算单轴激光寻北仪姿态和方位角；重复进行上述电机控制及解算，实现寻北仪的连续多位置自寻北。本发明的自寻北方法可靠性高，硬件成本低，而且控制流程合理高效，能够为载体提供全方位的姿态和方位信息，并实现不间断的连续自寻北模式。

主权项

1.一种单轴激光寻北仪的多位置自寻北方法，其特征在于包括以下步骤：(1)单轴激光寻北仪接收到寻北指令后读取误差补偿系数，按照误差补偿模型对陀螺仪和加速度计进行实时补偿，并控制电机锁定于当前位置，之后进入步骤(2)；(2)设置目标位置为0°，位置变量iptr置为0，并设置控制状态为转位状态，之后进入步骤(3)；(3)在每个控制周期内读取单轴激光寻北仪中编码器当前脉冲值，并将其转化成有效脉冲值；设置电机在每个控制周期内的位置增量；之后进入步骤(4)；(4)根据控制状态执行不同增益PID控制算法控制电机转动，当控制状态转换为锁定状态时，则锁定于该目标位置；之后进入步骤(5)；(5)在目标位置锁定时间t后，采样T0时间内陀螺仪和加速度计输出均值，并记录为Gyroi[iptr]和Accei[iptr]，之后进入步骤(6)；(6)将控制状态设为转位状态，重复执行三次步骤(3)-步骤(5)并分别设置三个不同的目标位置，同时将采样的T0时间内陀螺仪和加速度计输出均值记录为Gyroi[iptr]和Accei[iptr]，其中iptr＝1,2,3；之后进入步骤(7)；(7)解算单轴激光寻北仪姿态和方位角；之后，返回步骤(2)再次进入下一周期自寻北；所述的单轴激光寻北仪的误差补偿模型为：上式中，ωy为陀螺仪角速率输出，Ngy为陀螺仪采样脉冲数，ay为加速度计输出，Nay为加速度计采样脉冲数，T为采样时间，ωie为地球自转角速率，g为当地重力加速度，为当地纬度值；将编码器当前脉冲值转化成有效脉冲值具体方法为： t e m p P = CodeP N o w - A n g l e B l o c k · C P 360 CodeP 1 = [ t e m p P - C P t e m p > 0 t e m p P t e m p ≤ 0 , ]]>上式中，CodeP1为编码器有效脉冲值，tempP为中间变量，CodePNow为编码器当前脉冲值，AngleBlock为单轴激光寻北仪中限位挡钉角度值，CP为电机转动360°时编码器脉冲值。