[发明专利]基于模糊自适应控制技术的捷联惯导非线性对准方法

权利要求书

1.一种基于模糊自适应控制技术的捷联惯导非线性对准方法，其特征在于：其步骤包括如下：

步骤1：建立DVL辅助SINS动基座对准模型，所述对准模型包括SINS的非线性误差模型、非线性滤波状态模型和非线性滤波量测模型；

所述SINS非线性误差模型建立过程如下：

步骤1.1：记AUV航行的右-前-上方建立的右手坐标系为载体坐标系b，记东-北-天当地地理坐标系为导航坐标系n，则AUV在n系下的真实姿态为真实速度为AUV真实的地理坐标为p＝[LλH]^T，SINS实际解算出的姿态为速度为地理坐标为记SINS解算的地理位置建立的坐标系为计算导航坐标系n′，定义SINS姿态误差和速度误差分别为则φ、δvⁿ的微分方程如下：

φ.=Cω-1[(I-Cnn′)ω~inn+Cnn′δωinn-Cbn′(ϵb+wgb)]]]>

δv·n=[I-(Cnn′)T]Cbn′f~b+(Cnn′)TCbn′(▿b+wab)-(2δωien+δωenn)×v~sinsn-(2ω~ien+ω~enn)×δvn]]>其中，φ＝[φ_eφ_nφ_u]^T为纵摇角、横摇角和航向角误差，δvⁿ＝[δv_eδv_nδv_u]^T为东向速度、北向速度和天向速度误差，为b系下三轴陀螺的常值误差，为b系下三轴陀螺的随机误差，为b系下三轴加速度计的常值误差，为b系下三轴加速度计的随机误差，为加速度计的实际输出，为SINS解算的速度，为计算的导航坐标系的旋转角速度；为计算的地球旋转角速度，导航坐标系相对地球的旋转角速度，为对应于的计算误差，是n系依次旋转角度φ_u、φ_e、φ_n得到n′系所形成的方向余弦矩阵，为b系到n′系的转移矩阵，即计算的姿态矩阵，为欧拉角微分方程的系数矩阵，其具体为：

Cnn′=cosφncosφu-sinφnsinφesinφucosφnsinφu+sinφnsinφecosφu-sinφncosφe-cosφesinφucosφecosφusinφesinφncosφu+cosφnsinφesinφusinφnsinφu-cosφnsinφecosφucosφncosφe]]>，Cw=cosφn0-sinφncosφe01sinφesinφn0cosφncosφe]]>上标T表示转置；

所述非线性滤波状态模型建立过程如下：

步骤1.2：选取SINS的欧拉平台误差角φ_e、φ_n、φ_u，速度误差δv_e、δv_n，陀螺常值误差加速度计常值误差组成状态量x=[φeφnφuδveδvnϵxbϵybϵzb▿xb▿yb]T,]]>则非线性滤波状态方程为：

φ·=Cω-1[(I-Cnn′)ω^inn+Cnn′δωinn-Cbn′ϵb]+wgδv·n=[I-(Cnn′)T]Cbn′f^b+(Cnn′)TCbn′▿b-(2δωien+δωenn)×v^n-(2ω^ien+ω^enn)×δvn+waϵ·b=0▿·b=0]]>

其中，只取前两维状态，并将该非线性滤波状态方程简记为且w(t)＝[w_gw_a0_1×30_1×2]^T为零均值高斯白噪声过程；

所述非线性量测模型的建立过程如下：

步骤1.3：记AUV在b系下的真实速度为DVL测得AUV在b系下的实际速度为，利用SINS解算的姿态矩阵将变换为以和中的东向速度和北向速度分量作为匹配信息源，则非线性滤波量测方程为：

z=v~sinsn-Cbn′v~dvlb=δvn-[I-(Cnn′)T]Cbn′vdvlb+v]]>

其中，取z前两维为观测值，v为零均值高斯白噪声过程，并将该非线性滤波量测方程简记为z(t)＝h(x,t)+v(t)；

步骤2：以DVL的输出周期T_dvl作为滤波周期，并以T_dvl为步长对非线性滤波模型和z(t)＝h(x,t)+v(t)进行离散化，依据得到的离散化模型在平方根容积卡尔曼滤波的框架下进行时间更新；

所述非线性滤波模型的离散化过程为：

步骤2.1：离散化为x_k＝x_k-1+[f(x_k-1,t_k-1)+w(t_k-1)]T_dvl并简记为x_k＝f(x_k-1)+w_k-1，z(t)＝h(x,t)+v(t)离散化为z_k＝h(x_k,t_k)+v(t_k)并简记为z_k＝h(x_k)+v_k；

步骤3：利用当前SINS和DVL输出计算的量测值减去相同时刻的量测预测值得到当前时刻的残差ε_k，并计算一段时间内的残差序列第1个分量和第2个分量的统计值；

所述第1个残差分量计算和统计：

步骤3.1.1：计算残差ε_k的第1个分量ε_1k，即其中z_1k/k-1为z_k/k-1的第1个分量；

步骤3.1.2：计算包括当前时刻残差在内的前20个时刻的残差第1个分量绝对值的平均值μ_1k和标准差σ_1k：

μ1k=1rΣi=k-r+1k|ϵi,1k|,σ1k=1rΣi=k-r-1k(|ϵi,1k|-μ1k)2]]>

其中，ε_i,1k表示i时刻的第1个残差分量，k代表当前时刻，r=20；

所述第2个残差分量计算和统计：

步骤3.2.1：计算残差ε_k的第2个分量ε_2k，即其中z_2k/k-1为z_k/k-1的第2个分量；

步骤3.2.2：计算包括当前时刻残差在内的前20个时刻的残差第2个分量绝对值的平均值μ_2k和标准差σ_2k：

μ2k=1rΣi=k-r+1k|ϵi,2k|,σ2k=1rΣi=k-r-1k(|ϵi,2k|-μ2k)2]]>

其中，ε_i,2k表示i时刻的第2个残差分量，r=20；

步骤4：将μ_1k和σ_1k作为模糊逻辑控制器1的输入量，μ_2k和σ_2k作为模糊逻辑控制器2的输入量，经过模糊逻辑运算，输出精确量弱化因子l_1k和l_2k，并将其组成弱化因子对角阵l_k=diag[l_1k l_2k]；

步骤5：依据强跟踪滤波原理计算次优渐消因子λ_k，然后利用λ_k修正滤波时间更新过程，最后完成滤波量测更新；

步骤6：利用当前获得的欧拉平台误差角估计值和速度估计值修正SINS解算的姿态矩阵和速度，将修正之后的值作为下一次捷联解算的初值，利用当前获得的陀螺的常值误差估计值和加速度计的常值误差估计值修正下一时刻的陀螺输出和加速度计输出具体修正公式按下式计算：

Cbn=C^n′nCbn′,vsinsn=v~sinsn-δv^kn,ωibb=ω~ibb-ϵ^kb,fb=f~b-▿^kb]]>

若姿态精度达到要求，对准结束，否则继续递推执行步骤2至步骤6，直到对准结束。