一种基于自适应差分进化BP神经网络的姿态误差抑制方法与流程

本发明属于惯性技术领域中减小导航信息误差的方法，具体涉及一种基于自适应差分进化bp神经网络的姿态误差抑制方法。

背景技术：

惯性/星敏感器组合导航是一种以惯性导航系统(inertialnavigationsystem，ins)为主的导航系统，可为载体提供实时速度、姿态和位置信息。ins主要利用陀螺仪和加速度计测量载体角速度和线运动信息。经导航解算后得到导航信息；星敏感器通过拍摄星图并与自身星图库作对比，得到安装载体相对于惯性空间的姿态信息。惯性/星敏感器组合导航系统以星敏感器解算的导航信息为基准，对ins的发散式定位误差进行周期性重校，其不需要任何外界信息，是一种全自主导航系统。惯性/星敏感器组合导航系统具有实时性强、导航精度高、全自主等优点，被广泛应用于船舶导航中。然而，由于舰船水平摇摆使星敏感器拍摄的星图具有“拖尾”现象，影响星敏感器的解算精度，进而影响惯性/星敏感器组合导航精度。

《中国惯性技术学报》2010年18卷第4期中由杨波等人撰写的《弹载惯性/卫星/星光高精度组合导航》一文中，提出了对陀螺误差和星敏感器安装偏差角的在线自标定方法，抑制了器件偏差对导航精度的影响；《中国空间科学技术》2013年6月第3期中由张承等人撰写的《直接敏感地平的空天飞行器惯性/天文组合方法》一文中，提出了采用星敏感器和陀螺仪构造惯性基准，并在此基础上进行基于红外地平仪的天文定位解算，最后进行惯性/天文组合定位的方法，抑制了陀螺的漂移误差；公开号为cn105737858a的专利在2016年7月6日公开的《一种机载惯导系统姿态参数校准方法与装置》，该方法使用精度高的高动态星敏感器输出的姿态数据为基准完成机载ins姿态参数的动态在线校准；《ieeesensorsjournal》2017年17卷第21期中由杨延强等人撰写的《in-flightcalibrationofgyrosandstarsensorwithobservabilityanalysisforsins/cnsintegration》一文中，通过分析基于奇异值的误差状态的可观测度，提出了一种四位解耦校准方法，抑制了器件偏差对导航精度的影响，以上文献都提出了能够抑制器件偏差的方法，并且多以弹载、机载为应用背景，并没有针对船用环境下，由于舰船摇摆这一特殊应用环境造成的星图“拖尾”引起的星敏感器解算信息不精确问题进行深入研究。

技术实现要素：

本发明的目的在于提供提高导航精度，增强导航信息适用性的一种基于自适应差分进化bp神经网络的姿态误差抑制方法。

本发明的目的通过如下技术方案来实现：

基于ade-bpnn，将惯性/星敏感器组合导航系统的一段时间内组合导航信息作为输入，预测出当前t时刻载体姿态，利用卡尔曼滤波对t时刻预测载体姿态和t时刻星敏感器解算载体姿态进行数据融合，对星敏感器的导航误差进行估计、补偿，得到星敏感器高精度导航信息，与ins导航信息进行组合导航解。

一种基于自适应差分进化bp神经网络的姿态误差抑制方法，包括以下步骤：

步骤1：上电，初始化惯性/星敏感器组合导航系统。

步骤2：采集星敏感器和惯性组件输出数据。

步骤3：对步骤2中惯性组件输出数据进行导航解算，得到载体位置载体三向速度载体三轴姿态角其中，分别表示纬度、经度；分别表示东向、北向、天向速度；姿态角包括纵摇角、横摇角和航向角，分别由表示。

步骤4：由步骤2中星敏感器输出相对于惯性空间的姿态矩阵得到载体相对惯性系姿态角即

其中，b表示载体坐标系，i表示惯性系，表示载体系到惯性系转换矩阵；cbimn(m,n＝1,2,3)表示中第m行、第n列矩阵元素；arcsin为三角函数中的反正弦函数，arctan为三角函数中的反正切函数；分别表示载体相对惯性系的纵摇角、横摇角和航向角。

步骤5：利用ade-bpnn预测t时刻载体相对惯性系姿态角其中，t表示当前时刻，分别表示载体相对惯性系纵摇角、横摇角和航向角的预测值。

步骤6：构造卡尔曼滤波器1，以星敏感器测量误差δφci(i＝x,y,z)为状态量，步骤4中星敏感器解算载体姿态和步骤5中预测载体姿态之差为观测量，解算δφci(i＝x,y,z)，利用解算结果对步骤4解算结果进行补偿，得到φci(i＝x,y,z)。其中，δφcx、δφcy、δφcz分别表示星敏感器纵摇角、横摇角和航向角误差；φcx、φcy、φcz分别表示载体相对惯性系的纵摇角、横摇角和航向角。

步骤7：根据转换矩阵间的数学关系，得到地球坐标系相对准地理坐标系的转换矩阵转换过程为进一步解算得到载体的位置，即

其中，分别表示纬度和经度；e表示地球系，n表示准地理坐标系，表示地球系到准地理坐标系的转换矩阵；cenmn(m,n＝1,2,3)表示中第m行、第n列矩阵元素；表示载体系到准地理坐标系的转换矩阵；表示载体系到惯性系转换矩阵；角标t表示矩阵转置；表示地球系到惯性系转换矩阵。

步骤8：构造卡尔曼滤波器2，以步骤7解算位置与步骤3中ins解算位置之差λc-λs为观测量，ins位置误差δλs，速度误差δvsi(i＝x,y,z)，失准角φi(i＝x,y,z)，三轴陀螺常值误差εi(i＝x,y,z)，三轴加速度计零位偏置误差δai(i＝x,y,z)为状态量，解算δλs、δvsi(i＝x,y,z)、φi(i＝x,y,z)、εi(i＝x,y,z)、δai(i＝x,y,z)，对步骤3解算结果进行补偿，得到载体位置λ，速度vi(i＝x,y,z)，姿态角φi(i＝x,y,z)。其中，δλs分别表示纬度、经度误差；δvsx、δvsy、δvsz分别表示东向、北向和天向速度误差；φx、φy、φz分别表示纵摇失准角、横摇失准角、航向失准角。

步骤9：将步骤8中得到的导航信息载体位置、速度和姿态角存储并输出。

本发明的有益效果在于：

在得到舰船正常工作的组合导航信息后，结合ade-bpnn算法，预测出星敏感器输出惯性姿态，消除星图“拖尾”对星敏感器输出姿态角的影响，与ins进行组合导航解算，输出导航信息。(1)增强了在舰船出现水平摇摆时惯性/星敏感器组合导航系统的适用性，减小了因舰船摇摆导致的误差补偿不充分问题；(2)不需要任何外界辅助信息即可提高定位精度；(3)计算量小，简单易实现。

附图说明

图1为本发明的组合导航算法流程图；

图2为利用本发明进行的仿真试验结果；

图3为利用本发明进行的实测实验结果；

图4为实测实验舰船航行轨迹。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明：

如图1所示，本发明提供一种针对惯性/星敏感器的组合导航算法，具体包括如下步骤：

步骤1：上电，初始化惯性/星敏感器组合导航系统。导航初始化，需初始化系统：

(1)初始化ins初值：船体的载体位置λ0(单位均为度)，载体三向速度vi0(i＝x,y,z)(单位均为m/s)，载体三轴姿态角φi0(i＝x,y,z)(单位均为度)；初始化转换矩阵初始化四元数q0；

(2)卡尔曼滤波器1参数初值：状态变量初值δφci0(i＝x,y,z)，均方误差阵pp0，系统噪声方阵qp，量测噪声方阵rp，量测阵hp；其余初值根据实际情况设定。

(3)卡尔曼滤波器2参数初值：状态变量初值δvs0i(i＝x,y,z)、δλs0、φi0(i＝x,y,z)、εi0(i＝x,y,z)、δai0(i＝x,y,z)，均方误差阵p0，系统噪声方阵q，量测噪声方阵r，量测阵h；

其余初值根据实际情况设定。

初始化转换矩阵计算如下：

其中，n表示导航系，表示初始载体系到导航系的转移矩阵；

初始化四元数q0计算如下：

q00＝a，其中q0＝[q00q01q02q03]^t，cbnij(i,j＝1,2,3)表示中第i行、第j列矩阵元素；

导航过程中，利用该初始信息进行更新，得到任意时刻载体的位置、速度和姿态角。

步骤2：系统实时采集星敏感器输出矩阵惯性组件加速度计输出的三轴加速度和陀螺仪输出的三轴角速度其中，分别表示加速度计测量的比力在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为m/s)；分别表示陀螺仪测量的角速度在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为rad/s)。

步骤3：对步骤2中惯性组件采集数据进行导航解算：

四元数姿态矩阵更新：

设任意时刻载体系相对导航系的转动四元数为q＝[q0q1q2q3]^t，四元数的及时修正如下：

其中，分别表示qi(i＝0,1,2,3)的变化率；

将t时刻载体系相对导航系的转动四元数qi(t)(i＝0,1,2,3)代替式(4)中qi(i＝0,1,2,3)，得到t时刻转动四元数的变化率在t+1时刻载体的转动四元数为

其中式中ωi(i＝x,y,z)省略了上角标b；t为采样时间；当t＝1时，q(1)为步骤1中的载体初始四元数q0。

根据q(t+1)中元素qi(t+1)(i＝0,1,2,3)，更新捷联矩阵

其中，上式中的qi(i＝0,1,2,3)为式(5)中qi(t+1)(i＝0,1,2,3)，更新姿态信息：

利用转换关系式将加速度计沿载体系测量的比力信息投影转换到导航系，利用下列微分方程求解载体运动速度：

其中，表示的变化率；分别表示加速度计测量的比力在导航系oxn轴、oyn轴、ozn轴上的分量(单位均为m/s)；

将t时刻比力在导航系的投影fiⁿ(t)(i＝x,y,z)，代替式(8)中的fiⁿ(i＝x,y,z)，得到载体速度变化率得到t+1时刻载体速度和位置：

其中，r表示地球半径；当t＝1时，为步骤1中载体速度初始值vi0(i＝x,y,z)。

至此，根据式(7)、(9)得到ins解算的载体速度、位置和姿态角。

步骤4：由步骤2中星敏感器输出相对于惯性空间的姿态矩阵得到载体相对惯性系姿态角即

步骤5：利用ade-bpnn预测t时刻载体相对惯性系姿态角具体过程如下：

(1)初始化操作：初始化参数，包括种群规模n、基因维数d、变异因子f、交叉速率cr，以及每个基因的取值范围[umin,umax]，利用式xij＝umin+rand×(umax-umin)随机初始化种群；其中i＝1,2,...,n，j＝1,2,...,d，rand表示服从均匀分布的随机数。

(2)判断当前最小适应度值是否达到μ或迭代次数达到最大，若是则ade迭代停止，转至步骤(6)；否则进行下一步。

(3)生成后代子群：对每个目标向量进行变异操作，产生变异向量对变异向量进行交叉操作产生实验个体将目标个体与其对应实验个体竞争，比较两者适应度值，仅当的适应度值较更优时才被选为子代，否则直接将作为子代。重复此步骤产生后代子群。其中，g表示迭代次数。

(4)评估后代子群的适应值，最小适应值是当前最优值，相应的子群即为当前最优个体值。

(5)令g＝g+1，转至步骤(2)。

(6)将ade的最佳个体分配为bpnn的初始连接权值和阈值。

(7)将t时刻组合导航水平姿态信息φi(i＝x,y)与阈值θi(i＝x,y)进行比较，当水平姿态信息小于阈值时判定舰船发生摇摆，使用t-n～t-1时刻组合导航姿态信息对bpnn进行训练，得到最优模型。

(8)利用(7)中模型，预测出t时刻载体相对导航系的姿态角φpi(i＝x,y,z)，利用预测姿态角得到载体系到导航系的转换矩阵根据转换矩阵之间的关系，得到载体系相对惯性系的转换矩阵转换过程为进一步解算得到载体相对惯性系的姿态角预测值，即：

其中，表示中第m行、第n列元素；表示地球系到惯性系转换矩阵，由地球转速和导航时间得到；表示地球系到导航系的转换矩阵，由t-1时刻组合导航位置λ得到。

步骤6：构造卡尔曼滤波器1，以星敏感器测量误差δφci(i＝x,y,z)为状态量，步骤4中星敏感器解算载体姿态和步骤5中预测载体姿态之差为观测量，具体滤波过程如下：

利用星敏感器的惯性姿态误差模型，结合卡尔曼滤波，对惯性姿态进行误差修正如下：

其中，pp(t)表示t时刻估计均方误差，pp(t/t-1)表示t-1时刻到t时刻一步预测均方误差，fp(t/t-1)表示t-1时刻到t时刻的状态转移矩阵，gp(t-1)表示t-1时刻的噪声驱动阵，qp(t-1)表示t-1时刻系统噪声方差阵，hp表示量测矩阵，rp表示量测噪声方差阵，kp(t)表示t时刻滤波增益矩阵。

t时刻状态估计和估计均方误差的更新矩阵为：

其中，xp(t)表示t时刻状态估计量，xp(t)＝[δφcxδφcyδφcz]^t，当t＝1时，状态量xp(1)中元素为步骤1中卡尔曼滤波器1的状态量初值δφci0(i＝x,y,z)。根据解算结果，对步骤4解算结果进行补偿，得到惯性姿态角φci(i＝x,y,z)。

步骤7：根据转换矩阵间的数学关系，得到地球坐标系相对准地理坐标系的转换矩阵转换过程为进一步解算得到载体的位置，即

载体系到准地理系的转换矩阵由步骤3中解算的水平姿态得到，载体系到惯性系转换矩阵由步骤6中解算结果惯性姿态角得到。

步骤8：构造卡尔曼滤波器2，以步骤7解算位置与步骤3中ins解算位置之差λc-λs为观测量，ins位置误差δλs，速度误差δvsi(i＝x,y,z)，失准角φi(i＝x,y,z)，三轴陀螺常值误差εi(i＝x,y,z)，三轴加速度计零位偏置误差δai(i＝x,y,z)为状态量，具体滤波过程如下：

利用载体的速度、位置和失准角误差模型，结合卡尔曼滤波，对载体状态量进行误差修正如下：

其中，p(t)表示t时刻估计均方误差，p(t/t-1)表示t-1时刻到t时刻一步预测均方误差，f(t/t-1)表示t-1时刻到t时刻的状态转移矩阵，g(t-1)表示t-1时刻的噪声驱动阵，q(t-1)表示t-1时刻系统噪声方差阵，h表示量测矩阵，r表示量测噪声方差阵，k(t)表示t时刻滤波增益矩阵。

t时刻状态估计和估计均方误差的更新矩阵为：

其中，x(t)表示t时刻状态估计量，当t＝1时，状态量x(1)中元素为步骤1中卡尔曼滤波器2的状态量初值δvs0i(i＝x,y,z)、δλs0、φi0(i＝x,y,z)、εi0(i＝x,y,z)、δai0(i＝x,y,z)。根据解算结果，对步骤3中ins解算结果进行补偿，得到导航信息载体位置λ，速度vi(i＝x,y,z)，姿态角φi(i＝x,y,z)。

步骤9：将步骤8中得到的导航信息载体位置、速度和姿态角存储并输出。

实施例：

对本发明有益效果通过方针和实测实验得以验证：

仿真实验：

船舶航行路线：直线运动

系统初始化参数如下：

舰船运动参数：舰船速度：vx0＝0m/s，vy0＝0m/s，vz0＝0m/s

舰船姿态：

摇摆对舰船速度的影响：

均匀分布在[0,2π]内

高频率振动对舰船速度的影响：

均匀分布在[0,2π]内

当地经纬度：λ0＝126.6705°

陀螺参数：陀螺常值漂移：0.01°/h

陀螺刻度因数误差：1×10^-5

陀螺白噪声误差：0.001°/h

加速度计参数：加速度计零偏：10^-4g

加速度计刻度因数误差：1×10^-5

加速度计白噪声误差：10^-5g

ade-bpnn参数：

最佳拟合网络有20个输入神经元，10个隐藏神经元和1个输出神经元

最大迭代次数：100次

训练目标误差：0.00004

学习率：0.1

隐藏层函数：logsig

激活层函数：purelin

训练函数：trainlm

连接权值和阈值范围：[-1,1]

人口大小：n＝50

最大迭代次数：genm＝50

精度要求：μ＝0.15

交叉率：cr＝0.1

突变因子最小值：fmin＝0.2

突变因子最大值：fmax＝0.6

卡尔曼滤波器1数值设置：

pp0＝diag([0.010.010.01]²)

qp＝diag([0.010.010.01]²)

rp＝diag([0.0010.0010.001]²)

卡尔曼滤波器2数值设置：

p0＝diag([10100.00050.00050.050.050.150.010.010.0110^-5g10^-5g10^-5g]²)

q＝diag([0.5×10^-5g0.5×10^-5go1×20.5×0.010.5×0.010.5×0.01o1×6]²)

r＝diag([100r100r]²)

采样频率：0.1s

采样时间：1h

实测实验：

舰船航行路线：如图4所示。

陀螺参数：动态范围：±100°/s

零偏：≤0.01°/h

比例因子的非线性度：≤20ppm

星敏感器参数：视野角度：24°

星际观察水平：不小于7级

数据更新频率：20hz

姿态精度：5″

利用发明所述方法，得到在有风浪情况下有、无ade-bpnn算法的船体惯性/星敏感器组合导航信息误差曲线。图2表示仿真结果比较曲线，图3表示是实测实验结果比较曲线。结果表明本发明减小了船体水平摇摆时，星图“拖尾”对星敏感器解算导航信息的影响，进而较好抑制了惯性/星敏感器组合导航误差，可以满足实际需求。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。