本发明专利技术涉及的是一种自主水下航行器自主导航定位方法，特别是涉及一种基于强跟踪容积卡尔曼滤波的自主水下航行器自主导航定位方法。本发明专利技术包括如下步骤：选取自主水下航行器运动模型；选取自主水下航行器测量模型；通过自主水下航行器运动模型和测量模型构建动态系统；滤波器参数初始化；选取渐消因子；更新滤波器时间；更新滤波器量测数据；由当前时刻更新到下一时刻，重复执行步骤(5)至步骤(7)，直到到达终止时刻，结束计算，输出结果。通过引入渐消因子实时调整滤波增益矩阵，强迫输出残差序列保持相互正交，以此来减小老数据的权值，相对地增加新数据的权值，提高了滤波器的估计精度和稳定性。
【技术实现步骤摘要】
本专利技术涉及的是，特别是涉及一种基于强跟踪容积卡尔曼滤波(Strong Tracking Cubature KalmanFilter, STCKF)的自主水下航行器自主导航定位方法。
技术介绍
自主水下航行器(Autonomous Underwater Vehicle,AUV)作为一种可以在水下移动的装置，具有自带动力、活动自主的特点，可以进行海洋环境监测、海底搜索、打捞等水下作业，已被广泛应用于海洋勘察与资源开发等领域。但水下导航问题即如何确定AUV在水下环境中的位置以便其采取正确的行动成功的完成任务，依然是AUV设计所面临的主要关键技术之一。目前国内外试图寻求一种有效的手段用于AUV自主导航定位。当AUV处于实际运行环境中时，受外部应用环境不确定性因素的影响，AUV自主导航系统的实际数学模型与理论模型不能完全匹配，即AUV自主导航系统具有模型不确定性。研究表明，当理论模型与实际系统完全匹配时，卡尔曼滤波器输出残差序列是互不相关的高斯白噪声序列。针对上述问题，周东华等人在文献《非线性系统带次优渐消因子的扩展卡尔曼滤波》(控制与决策，1990，5 (5) :1-6)及文献《一种带多重次优渐消因子的扩展卡尔曼滤波器》(自动化学报，1991，17(6) :689-695)中基于扩展卡尔曼滤波(Extended KalmanFilter, EKF)提出了强跟踪滤波器(Strong TrackingFilter, STF)，其通过在EKF状态预测协方差阵中引入渐消因子，在线实时调整增益矩阵，强迫输出残差序列保持相互正交。这样，STF 在AUV自主导航系统模型不确定时仍然能保持对系统状态的跟踪能力，有效解决了 EKF关于模型不确定的鲁棒性差、滤波发散等问题。但STF仍然存在对非线性状态后验分布的一阶线性化近似精度偏低，及需要计算非线性函数的雅克比矩阵等自身无法克服的理论局限性。加拿大学者Arasaratnam 在文献《Cubature KalmanFilters)) (IEEE TransactionsonAutomatic Control, 2009, 54(6) : 1254-1269)中提出了容积卡尔曼滤波(Cubature KalmanFilter, CKF)方法。CKF根据容积准则，通过一组具有相同权重的点经过非线性系统方程转换后产生新的点来给出下一时刻系统状态的预测，避免了对非线性模型的线性化处理，其精度达三阶。文献《Nonlinear estimation for spacecraft attitude using decentralizedunscented information filter)) (International Conference on Control Automation andSyatems, 2010:1562-1566)及文献《GaussHermite quadraturefiIter with applicationto spacecraft attitude estimation)) (Journal of Guidance and Dynamics, 2011，34 (2) : 367-379)均基于 CKF 进行飞行器姿态估计；文献《A CKF based GNSS/INS train integratedpositioning method》 (International Conference on Mechatronics and Automation, 2010:1686-1689)米用 CKF 与 GNSS/INS 协同进行火车定位。
技术实现思路
位方法。 本专利技术的目的在于提供一种精度更高、稳定性更强的自主水下航行器自主导航定本专利技术的目的是这样实现的本专利技术包括如下步骤(I)建立自主水下航行器运动模型权利要求1.，其特征在于，包括如下步骤(1)建立自主水下航行器运动模型全文摘要本专利技术涉及的是，特别是涉及一种基于强跟踪容积卡尔曼滤波的自主水下航行器自主导航定位方法。本专利技术包括如下步骤选取自主水下航行器运动模型；选取自主水下航行器测量模型；通过自主水下航行器运动模型和测量模型构建动态系统；滤波器参数初始化；选取渐消因子；更新滤波器时间；更新滤波器量测数据；由当前时刻更新到下一时刻，重复执行步骤(5)至步骤(7)，直到到达终止时刻，结束计算，输出结果。通过引入渐消因子实时调整滤波增益矩阵，强迫输出残差序列保持相互正交，以此来减小老数据的权值，相对地增加新数据的权值，提高了滤波器的估计精度和稳定性。文档编号G01C21/20GK102980579SQ20121046035公开日2013年3月20日 申请日期2012年11月15日 优先权日2012年11月15日专利技术者王宏健, 傅桂霞, 严浙平, 边信黔, 李娟 , 刘向波 申请人:哈尔滨工程大学本文档来自技高网...
【技术保护点】
一种自主水下航行器自主导航定位方法，其特征在于，包括如下步骤：（1）建立自主水下航行器运动模型：xyzψuvwrk=x+uTcos(ψ)-vTsin(ψ)y+uTsin(ψ)+vTcos(ψ)z+wTψ+rTuvwrk-1+wk-1式中，[x,y,z,Ψ]表示AUV在L中的位置和艏向；[u,v,w,r]表示AUV在V中相应的线速度和角速度；k表示任意采样时刻；T为航位推算传感器的采样时间间隔；wk?1为随机系统噪声；（2）建立自主水下航行器测量模型：zk＝Hxk|k?1+vk式中，zk是观测向量，xk=xyzψuvwrkT,vk是观测噪声，测量矩阵H为：H=0010000000010000000010000000010000000010;（3）通过自主水下航行器运动模型和测量模型构建动态系统：xk=f(xk-1)+wk-1zk=h(xk)+vk,式中，xk=xyzψuvwrkT,zk是观测向量；随机系统噪声wk~N(0,Qk)，随机观测噪声vk~N(0,Rk)，系统初始状态为x0，x0与wk，vk统计独立；非线性函数f(xk?1)和h(xk)为关于状态的一阶连续偏导数：f(xk-1)x+uTcos(ψ)-vTsin(ψ)y+uTsin(ψ)+vTcos(ψ)z+wTψ+rTuvwrk-1,h(xk)＝Hxk|k?1；（4）滤波器参数初始化：自主水下航行器初始时刻的状态x0＝[0]8×1，初始时刻的状态误差协方差P0，系统噪声Q，观测噪声R，用于求取渐消因子的C0,k的初始值C0,0及遗忘因子ρ；（5）选取渐消因子：λk=tr(C0,k-Rk)tr(1mΣi=1mzi,k|k-1zi,k|k-1T-z^k|k-1z^k|k-1T)其中，λk为渐消因子，tr(·)为求矩阵的迹，m＝2n是容积点的个数，n为AUV状态向量的维数，zi，k|k?1为通过观测方程传播后的容积点，为k时刻的观测预测值，Rk为k时刻的观测噪声，C0,k=γkγkTk=1ρC0,k-1+γkγkT1+ρk>1,γk是k时刻的残差，0＜ρ≤1为遗忘因子；（6）估计k时刻自主水下航行器的状态：x^k|k=x^k|k-1+Kk(zk-z^k|k-1)式中，为k时刻的状态估计值，为k时刻的状态预测值，Kk为卡尔曼滤波增益，zk为观测向量，为k时刻的观测预测值；（7）估计k时刻自主水下航行器的状态协方差：Pk|k=Pk|k-1-KkPzz,k|k-1KkT式中，Pk|k为k时刻的状态误差协方差估计值，Pk|k?1为k时刻的状态协方差预测值，Kk为卡尔曼滤波增益，Pzz，k|k?1为自相关协方差矩阵；（8）由当前时刻更新到下一时刻，重复执行步骤（5）至步骤（7），直到到达终止时刻，结束计算，输出结果。FDA00002409309600023.jpg,FDA00002409309600026.jpg,FDA00002409309600027.jpg,FDA00002409309600028.jpg...
【技术特征摘要】
【专利技术属性】
技术研发人员：王宏健，傅桂霞，严浙平，边信黔，李娟，刘向波，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。