一种gps拒止环境下高精度位姿估计算法
阅读说明：本技术 一种gps拒止环境下高精度位姿估计算法 (High-precision pose estimation algorithm in GPS rejection environment ) 是由 雷旭升 王锐 付发 于 2021-09-10 设计创作，主要内容包括：一种GPS拒止环境下高精度位姿估计算法,涉及基于PI调节器的自适应互补滤波姿态算法和基于自适应模糊神经网络的高精度位置估计算法。首先在陀螺姿态解算方程的基础上,设计加计抑制函数,构建PI反馈修正回路来提升系统在高动态下的姿态测量精度；其次构建自适应模糊神经网络,在GPS有效时基于Fletcher-Reeves共轭梯度下降算法和最小二乘法组合算法优化模糊神经网络权值参数,以实现在GPS拒止环境下的高精度位置估计。本发明具有实时性好、抗扰动能力强、学习速度快、预测精度高等优点,可用于中小型无人机在GPS拒止环境下的高精度导航。(A high-precision pose estimation algorithm under a GPS rejection environment relates to a self-adaptive complementary filtering attitude algorithm based on a PI regulator and a high-precision position estimation algorithm based on a self-adaptive fuzzy neural network. Firstly, designing an addition inhibition function on the basis of a gyro attitude calculation equation, and constructing a PI feedback correction loop to improve the attitude measurement precision of a system under high dynamic conditions; and secondly, constructing an adaptive fuzzy neural network, and optimizing weight parameters of the fuzzy neural network based on a Fletcher-Reeves conjugate gradient descent algorithm and a least square method combined algorithm when the GPS is effective so as to realize high-precision position estimation in a GPS rejection environment. The method has the advantages of good real-time performance, strong disturbance resistance, high learning speed, high prediction precision and the like, and can be used for high-precision navigation of small and medium-sized unmanned aerial vehicles in a GPS rejection environment.)
技术领域
本发明涉及一种GPS拒止环境下高精度位姿估计算法，适用于动载体在GPS拒止区域的高精度导航领域。
背景技术
近年来，中小型无人机由于尺寸小、重量轻、隐蔽性好、性价比高等优点在军事和民用领域被广泛应用，由于成本和体积的限制，其导航系统一般以GPS/SINS组合导航为基础进行研究。
随着无人机应用场景的广泛，在实际飞行中可能通过密林、高楼、隧道等复杂环境，会出现GPS拒止区域，GPS无法提供持续有效的导航信息辅助INS系统。此时惯性器件产生的误差漂移、标度因子不稳定等问题因为得不到修正而不断积累，导致组合导航系统的整体性能下降。因此，研究GPS拒止环境下的高精度位姿估计导航算法具有十分重要的意义。
捷联惯导算法、Kalman滤波算法、互补滤波算法等常被用于为系统提供导航信息解算。捷联惯导算法数据更新速度快，在运行时间较短时稳定性好，但长时解算导航信息时，其误差随时间增大，位姿数据发生漂移、精度发散，难以满足长时间、高精度的导航需求。Kalman滤波算法依靠GPS作为量测值，当GPS信号出现异常，Kalman滤波过程中的量测更新将无法进行，此时位姿解算结果将由于误差累积随时间发散，不再可靠。互补滤波算法被用于迭代更新姿态信息，根据传感器的频域特性对数据进行融合处理，能够有效的抑制姿态角的漂移，但只适用于静止或低速状态，在高机动状态下将引入较大的加速度误差，系统的适应性较差。
发明内容
本发明解决的技术问题是：GPS/SINS组合导航系统在GPS信号受干扰或被遮挡时姿态和位置数据发散的问题，提出了一种GPS拒止环境下高精度位姿估计算法，基于加计的测量值作为观测量，引入PI反馈修正回路实现对陀螺输出的自适应补偿从而校正系统的姿态信息，同时构建自适应模糊神经网络，采取组合算法对无GPS下的系统位置信息进行预测估计，实现GPS拒止环境下的高精度导航。
本发明的技术解决方案为：首先在陀螺姿态解算方程的基础上，构建PI反馈修正回路，引入加计对陀螺的自适应补偿来解决姿态信息发散的问题；其次构建自适应模糊神经网络，基于Fletcher-Reeves共轭梯度下降算法和最小二乘法组合算法优化参数，在GPS有效状态下进行训练以实现GPS无效时为系统提供实时的高精度位置信息。其实现步骤如下：
(1)基于陀螺姿态解算的四元数方程，
其中，Q＝[q0 q1 q2 q3]T为由标量q0和矢量q1i+q2j+q3k组成用于描述载体转动的四元数，t为当前时刻，t-1为前一时刻，Δt为算法解算周期，为从地理系到载体系的陀螺角速度投影矢量；
构建PI反馈修正回路，基于加速计的测量值作为观测量，引入加速计抑制函数，对系统加速状态进行判断，为陀螺的角速度提供自适应补偿，减小高机动状态下加速计的误差对陀螺修正量的干扰，实现对陀螺角度漂移的抑制；
补偿后的四元数形式的姿态解算方程为:
式中δ∈R3×1为PI反馈调节器产生的滤波结果，用于补偿实测向量与预测向量的旋转误差，其表达式如下：
式中，加速计的误差向量e(ω)∈R3×1由重力向量的测量值和估计值通过向量积的方式实现，表示由陀螺仪积分得到的姿态四元数中的俯仰和横滚误差，表达式如下：
式中，fb∈R3×1为加速计测量值归一化的三轴加速度，gn∈R3×1为地理系下的重力加速度，gb∈R3×1为载体系下的重力加速度，为地理系到载体系的姿态转换矩阵；
kp为比例系数，用以调节陀螺仪与加计之间的交叉频率，ki为积分系数，用以修正陀螺输出角速度的漂移误差，p(a)为加速计抑制函数，其表达式如下:
通过引入加速计抑制函数p(a)，减小高机动状态下加速计的误差对陀螺修正量的干扰，从而实现对反馈修正量δ的自适应调整，提高水平姿态角融合算法的解算精度。
(2)构建GPS正常状态下位置训练的神经网络，选取基于Fletcher-Reeves共轭梯度下降算法和最小二乘法组合算法优化自适应模糊神经网络的权值参数，实现对无GPS环境下SINS系统解算位置误差的校正；
GPS有效时，使用Kalman滤波算法修正SINS系统的误差，补偿SINS解算的位置信息，同时以加计的三轴输出陀螺仪的三轴输出系统的三轴速度vi以及航向角θ作为输入，Kalman滤波解算出的位置误差δL、δλ作为输出构建样本集对用于预测系统位置的自适应模糊神经网络进行训练；
自适应模糊神经网络是基于T-S型模糊逻辑系统的“If-Then”规则的多层自适应网络，依次为模糊化层、模糊集运算层、归一化层、逆模糊化层和总输出层，神经网络的权值参数分别包含在模糊化层和逆模糊化层中；
在前向通道中，模糊化层权值参数固定，采用最小二乘法辨识逆模糊化层权值参数，在后向通道中，逆模糊化层权值参数固定，采用Fletcher-Reeves共轭梯度法辨识模糊化层权值参数，直至满足精度要求或达到最大的迭代次数；
Fletcher-Reeves共轭梯度法将最优化问题转化为目标函数最小化问题，目标函数表达式如下：
沿负梯度方向搜索时，单步运算公式如下：
Xk+1＝Xk+akPk
式中，k为当前时刻，k+1为下一时刻，X为待调整参数，ak为根据线搜索准则确定的学习速率，Pk为当前时刻的共轭梯度向量，代表当前时刻的搜索方向，当满足以下条件：
式中，gk为当前时刻的迭代梯度向量；
则可以采用负梯度方向继续进行搜索，Pk表达式如下：
式中，βk为共轭参数，选用Fletcher-Reeves法计算，其表达式如下：
基于GPS收星数判断GPS是否有效，当GPS无效时，使用训练好的神经网络的参数集，对系统的位置误差进行预测，并从导航系统中消除该部分的误差以修正系统的位置估计，从而校正系统的位置信息。
本发明与现有技术相比的优点在于：
(1)本发明针对无GPS的复杂环境下惯性传感器输出噪声信号增大的问题，提出了一种基于PI调节器的自适应互补滤波的姿态解算方法，设计了有害加速度的抑制函数，减小了高机动状态下加计的测量误差对测姿精度的影响，从而提高系统抗扰动能力，并可避免无GPS辅助校正下的误差累计；
(2)本发明针对无GPS环境下SINS系统解算位置误差累积的问题，提出了一种基于自适应模糊神经网络的高精度位置估计算法，通过Fletcher-Reeves共轭梯度下降算法和最小二乘法组合算法优化神经网络的权值参数，使得能够以较快的收敛速度寻优，避免陷入局部最优，提升了神经网络的学习效率和预测精度，有效抑制GPS失效时水平方向的位置信息发散。
附图说明
图1为基于PI调节器的自适应互补滤波姿态算法流程图；
图2为基于自适应模糊神经网络的高精度位置估计算法流程图；
图3为跑车实验中自研系统航向角误差变化图；
图4为跑车实验中自研系统横滚角误差变化图；
图5为跑车实验中自研系统俯仰角误差变化图；
图6为跑车实验中自研系统经度位置偏差变化图；
图7为跑车实验中自研系统纬度位置偏差变化图；
具体实施方式
本发明考虑到体积和成本，使用小体积低精度的MEMS惯性导航设备，基于GPS/SINS组合导航提供姿态和位置信息。当GPS信号干扰或丢失时，本发明采用惯性器件进行互补滤波解算姿态，根据加速状态由加速计对陀螺积分得到的角度进行自适应补偿；此外基于GPS正常状态下进行训练的神经网络预测系统的位置信息。
如图1和图2所示，本发明的具体实现如下
(1)建立姿态解算方程关系
建立四元数形式的姿态微分方程如下：
式中，Q＝[q0 q1 q2 q3]T为由标量q0和矢量q1i+q2j+q3k组成用于描述载体转动的四元数，为从地理系到载体系的角速度投影矢量，由陀螺仪输出的角速度计算得到，其表达式如下：
式中，为地理系到载体系的姿态转换矩阵，为地理系相对惯性系的转动角速度在载体系下的表示，为位置变化引起的地理系相对地球系的转动角速度，为地理系下的地球自转角速度，其表达式分别如下:
式中，vx为载体东向速度，vy为载体北向速度，RM、RN分别为卯酉圈和子午圈的半径，L为当地纬度，h为载体高度；
采用一阶龙格-库塔法对微分方程进行求解，则当前时刻的四元数Qt可以表示为：
其中Qt-1为前一时刻的四元数，Δt为算法解算周期；
利用上述四元数更新姿态转换矩阵其表达式如下：
姿态角(航向角θ、俯仰角横滚角γ)由姿态转换矩阵解算得到，其表达式分别如下：
式中，为矩阵的第i+1行第j+1列的元素；
在长时间的测量中，系统姿态角会发生偏移，因为陀螺仪的角速度误差在积分过程中不断累积，使得单独由角速度积分得到的转动角度产生漂移误差；
(2)构建PI反馈修正回路
为了校正陀螺仪角度的累积误差，提高水平姿态角的测量精度，基于加速计的测量值作为观测量，并通过PI反馈控制器实现自适应反馈校正；
校正后的四元数微分方程更新如下：
式中δ∈R3×1为PI反馈调节器产生的滤波结果，用于补偿实测向量与预测向量的旋转误差，其表达式如下：
式中，加速计的误差向量e(ω)∈R3×1由重力向量的测量值和估计值通过向量积的方式实现，表示由陀螺仪积分得到的姿态四元数中的俯仰和横滚误差，表达式如下：
式中，fb∈R3×1为加速计测量值归一化的三轴加速度，gn∈R3×1为地理系下的重力加速度，gb∈R3×1为载体系下的重力加速度；
kp为比例系数，用以调节陀螺仪与加计之间的交叉频率，ki为积分系数，用以修正陀螺输出角速度的漂移误差，p(a)为加速计抑制函数，其表达式如下:
通过引入加速计抑制函数p(a)，减小高机动状态下加速计的误差对陀螺修正量的干扰，从而实现对反馈修正量δ的自适应调整，提高水平姿态角融合算法的解算精度。
(3)基于自适应模糊神经网络的高精度位置估计算法
当GPS有效时，使用Kalman滤波算法修正SINS系统的误差，补偿SINS解算的位置信息，同时构建训练样本数据，对用于预测系统位置的自适应模糊神经网络进行训练；
建立系统模型，取15维状态变量X，其表达式如下:
式中，φx、φy、φz为三轴姿态失准角，δvx、δvy、δvz为地理系下的三轴速度误差，δL、δλ为经度和纬度误差，δh为高度误差，为陀螺漂移估计， 为加速度计零偏估计；
根据Kalman滤波模型，构建系统的状态方程如下：
式中，G(t)∈R15×6为系统的噪声系数矩阵，w(t)∈R6×1为系统的过程噪声向量，F(t)∈R15×15为系统状态转移矩阵，其表达式如下：
式中，FN(t)∈R9×9为三轴姿态、速度和位置的误差矩阵，FS(t)∈R9×6为惯性器件误差漂移的转移矩阵，FM(t)∈R6×6为陀螺仪、加速度计对应的系统误差矩阵；
根据Kalman滤波模型，构建系统的量测方程如下：
Z(t)＝H(t)X(t)+v(t)
式中，Z(t)∈R6×1为系统量测量，选取由GPS给出的位置、速度信息与惯导解算得到的载体位置、速度信息的差值，H(t)∈R6×15为量测噪声系数矩阵，v(t)∈R6×1为系统的量测噪声向量；
自适应模糊神经网络的训练输入设置为加计的三轴输出陀螺仪的三轴输出系统的三轴速度vi以及航向角θ，Kalman滤波解算出的位置误差δL、δλ作为输出构建样本集对自适应神经网络进行训练；
自适应模糊神经网络是基于T-S型模糊逻辑系统的“If-Then”规则的多层自适应网络，“If-Then”规则表达式如下：
1、if x isA1and y is B1then h1＝p1x+q1y+r1
2、if x isA2and y is B2then h2＝p2x+q2y+r2
式中，x、y为输入变量，Ai、Bi为模糊集，pi、qi、ri为神经网络的权值参数，在学习的过程中不断调整；
第一层模糊化层对输入变量进行模糊化得到对应模糊集的隶属度，其表达式如下：
式中，分别为Ai、Bi的隶属度函数，其中包含了自适应模糊神经网络的权值参数，分别为对应的隶属度函数值；
第二层模糊集运算层将第一层中的隶属度函数值相乘，得到第i条模糊规则的激励强度wi，其表达式如下：
第三层将激励强度归一化，表达式如下：
第四层逆模糊化层计算模糊规则自适应推理的结果，表达式如下：
第五层总输出层对上一层的输出进行去模糊化，计算所有输出的加权和并输出网络运算结果；
由于模糊化层采用的钟型函数包含非线性参数，逆模糊化层是线性参数，设计基于Fletcher-Reeves共轭梯度下降算法和最小二乘法组合算法优化自适应模糊神经网络的权值参数，在保证精度和快速性的需求下，实现高精度优化。
在前向通道中，模糊化层权值参数固定，采用最小二乘法辨识逆模糊化层权值参数，在后向通道中，逆模糊化层权值参数固定，采用Fletcher-Reeves共轭梯度法辨识模糊化层权值参数，直至满足精度要求或达到最大的迭代次数；
Fletcher-Reeves共轭梯度法将最优化问题转化为目标函数最小化问题，目标函数表达式如下：
反向梯度下降的单步运算公式如下：
Xk+1＝Xk-akgk
式中，k为当前时刻，k+1为下一时刻，X为待调整参数，ak为根据线搜索准则确定的学习速率，gk为当前时刻的迭代梯度向量，当沿负梯度方向搜索时，则单步运算公式可以表示为：
Xk+1＝Xk+akPk
式中，Pk为当前时刻的共轭梯度向量，代表当前时刻的搜索方向，当满足以下条件：
则可以采用负梯度方向继续进行搜索，Pk表达式如下：
式中，βk为共轭参数，选用Fletcher-Reeves法计算，其表达式如下：
通过引入Fletcher-Reeves共轭梯度法，对自适应模糊神经网络的参数进行寻优，提升了神经网络的收敛速度和位置信息的预测精度；
基于GPS收星数判断GPS是否有效，当GPS无效时，使用训练好的神经网络的参数集，对系统的位置误差进行预测，并从导航系统中消除该部分的误差以修正系统的位置估计，从而校正系统的位置信息。
(4)跑车实例
将自研导航系统与高精度光纤陀螺系统固定安装于轿车内，进行跑车实验验证。高精度光纤陀螺系统姿态精度0.02度，位置精度0.1m，以高精度光纤陀螺系统输出为基准对自研导航系统进行测试。实验过程的数据保存在记录仪中，某次实验的跑车的姿态数据结果如图3、图4、图5所示，位置数据结果如图6与图7所示。
自研导航系统实现了低成本、高精度的姿态解算和位置解算，能够适应无GPS下高机动状态的导航。在GPS丢星时间为600s时，系统航向角的均方根误差是1.1828度，横滚角的均方根误差是0.1812度，俯仰角的均方根是0.6037度。当GPS丢星时间为30s时，经度位置的均方根误差是1.83m，纬度位置的均方根误差1.39m。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种小物品放射性沾染检测仪及其操作系统