一种基于ISR-UKF的水下组合导航方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN110514203A(43)申请公布日2019.11.29(21)申请号201910822376.3G01S19/47(2010.01)(22)申请日2019.08.30G06F17/10(2006.01)(71)申请人东南大学地址210096江苏省南京市玄武区四牌楼2号(72)发明人徐晓苏潘绍华杨阳侯岚华吴贤安仲帅(74)专利代理机构南京众联专利代理有限公司32206代理人许小莉(51)Int.Cl.G01C21/16(2006.01)G01C21/00(2006.01)G01C23/00(2006.01)G01S19/45(2010.01)权利要求书8页说明书20页附图3页(54)发明名称一种基于ISR-UKF的水下组合导航方法(57)摘要本发明提供了一种基于ISR-UKF的水下组合导航方法，包括：分别建立水下潜航阶段SINS/DVL子系统和水面位置修正阶段SINS/GPS子系统的状态方程和量测方程；建立水下潜航阶段SINS/DVL子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，进行水下潜航阶段的导航；在水下潜航较长时间以后，通过控制AUV的潜行深度到达水面附近停留较短时间，获取GPS位置、速度信息辅助，建立水面位置修正阶段SINS/GPS子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，实现对AUV水下潜航阶段偏差的校正，从而实现AUV沿指定路线航行的目的。本发明能够提高SINS/DVL/GPS水下组合导航系统的滤波解算效率，在保证实时性和稳定性的前提下更加易于编程实现。CN110514203ACN110514203A权利要求书1/8页1.一种基于ISR-UKF的水下组合导航方法，其特征在于，该方法包括以下步骤：步骤1：完成导航定位系统的初始对准后，根据系统误差方程建立水下潜航阶段SINS/DVL子系统的状态方程，根据SINS解算的速度量和DVL测量的速度量之差作为量测量建立水下潜航阶段SINS/DVL子系统的量测方程；步骤2：在水面附近，不考虑天向运动速度及误差，根据误差方程建立水面位置修正阶段SINS/GPS子系统的状态方程，以SINS解算出来的位置、速度与GPS输出的位置、速度之差作为量测量建立水面位置修正阶段SINS/GPS子系统的量测方程；步骤3：根据步骤1中建立的子系统状态方程和量测方程建立水下潜航阶段SINS/DVL子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，进行水下潜航阶段的导航；步骤4：控制AUV的潜行深度到达水面附近停留，当GPS设备状态为定位有效时，获取GPS位置、速度信息辅助，根据步骤2中建立的子系统状态方程和量测方程，建立水面位置修正阶段SINS/GPS子系统的非线性滤波方程，通过迭代平方根无迹卡尔曼滤波算法，进行对AUV水下潜航阶段偏差的校正。2.根据权利要求1所述的基于ISR-UKF的水下组合导航方法，其特征在于，步骤1中所述建立水下潜航阶段SINS/DVL子系统的状态方程和量测方程，具体过程如下：步骤1.1对需要用到的坐标系进行定义：i——惯性坐标系：不随地球旋转，原点位于地球中心，zi轴指向北极，xi轴指向春分点，yi轴与xi、zi构成右手坐标系；e——地球坐标系：与地球固联，原点位于地心，xe轴穿越本初子午线与赤道交点，ze轴指向北极，ye轴xe、ze构成右手坐标系；b——载体坐标系：原点位于运载体中心，zb轴垂直运载体向上，xb指向运载体前方，yb与xb、zb构成右手坐标系；p——实际计算得出的平台坐标系；n——与东-北-天地理坐标系重合的导航坐标系；步骤1.2建立水下潜航阶段SINS/DVL子系统的状态方程，具体步骤如下：取姿态误差角(φEφNφU)、速度误差(δvEδvNδvU)位置误差(δLδλδh)、陀螺常值漂移(εbxεbyεbz)和加速度计随机常值误差作为SINS系统的状态量，记为：其中，φE是东向失准角，φN是北向失准角，φU是天向失准角；δvE是东向速度误差，δvN是北向速度误差，δvU是天向速度误差；δL是纬度误差，δλ是经度误差，δh是高度误差；εbx是x向陀螺漂移，εby是y向陀螺漂移，εbz是z向陀螺漂移；是x向加速度计随机常值误差，是y向加速度计随机常值误差，是z向加速度计随机常值误差。通过理想惯导比力方程的速度微分方程和捷联惯导系统的实际速度微分方程，推导出四元数表示的速度误差方程：2CN110514203A权利要求书2/8页其中，Vn是载体在n系下的理想速度，是n系下的地球自转角速度，为n系相对于e系的角速度在n系下的投影，fb为b系下的比力；式中，为p系到n系的转换四元数，为b系到p系的转换四元数，和分别代表p系到n系以及b系到p系的转换矩阵；