一种基于仿射因子补偿的改进ICCP方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN114111798A(43)申请公布日2022.03.01(21)申请号202111488055.8(22)申请日2021.12.07(71)申请人东南大学地址210096江苏省南京市玄武区四牌楼2号(72)发明人程向红王依能(74)专利代理机构南京众联专利代理有限公司32206代理人许小莉(51)Int.Cl.G01C21/20(2006.01)G01C21/16(2006.01)权利要求书3页说明书8页附图3页(54)发明名称一种基于仿射因子补偿的改进ICCP方法(57)摘要本发明公开一种基于仿射因子补偿的改进ICCP方法，属于地球物理场辅助惯性导航领域。ICCP(IterativeClosestContourPoint)算法是目前地形匹配导航技术最为重要的匹配算法之一，但其对惯导航迹所作的修正仅仅是旋转和平移，没有对航迹进行缩放，因而不能修正辅助定位过程中惯导指示航迹相对于实际航迹发生的形变所造成的误差。已有的引入仿射修正以提高ICCP精度的算法，大多利用模拟退火、最小二乘等方法，计算量大且仿射因子精度有限，本发明利用单位四元数表示ICCP中的旋转，求得仿射因子的解析解，提高了ICCP算法精度。CN114111798ACN114111798A权利要求书1/3页1.一种基于仿射因子补偿的改进ICCP方法，其特征在于，该方法包括如下步骤：(1)当运载体进入地球物理场适配区后，通过运载体携带的物理场传感器测量并存储航行轨迹点对应的物理场值得到实测物理场值，根据运载体携带的惯性导航系统提供的位置信息提取所在区域的物理场参考数据库；(2)通过所在区域的物理场参考数据库与惯性导航系统提供的位置信息得到惯导指示航迹点，根据实测物理场值提取惯导指示航迹点附近的等值线，并搜索惯导指示航迹点到附近等值线上的对应最邻近参考点；(3)根据最邻近参考点与惯导指示航迹点确定旋转矩阵R、仿射因子s和平移向量t；(4)利用步骤(3)得到的旋转矩阵R、仿射因子s和平移向量t更新惯导指示航迹点的位置，得到当前匹配航迹，判断迭代次数是否超限或满足局部最优条件，如果没有达到迭代次数或没有满足收敛条件，则转到步骤(2)，否则进行下一步骤；(5)对最后得到的匹配航迹进行精度评估，判断结果是否可靠，如果可靠则将结果用以校正惯导指示航迹点组成的航迹，否则认为匹配失败。2.根据权利要求1所述的一种基于仿射因子补偿的改进ICCP方法，其特征在于，步骤(2)的具体方法是：根据实测物理场值hi，在物理场参考数据库中提取惯导指示航迹点pi附近的等值线ci，并在等值线ci上搜索到pi的最邻近参考点yi；其中，i＝1,2,…,N表示第i个惯导指示航迹点序号，N为惯导指示航迹点的数量，hi表示第i个惯导指示航迹点所对应的实测物理场值，pi表示第i个惯导指示航迹点所对应的二维位置坐标，ci表示第i个惯导指示航迹点附近对应实测物理场值hi的等值线，yi表示第i个惯导指示航迹点到等值线ci上的最邻近参考点的二维位置坐标。3.根据权利要求1所述的一种基于仿射因子补偿的改进ICCP方法，其特征在于，步骤(3)的具体方法是：求解旋转矩阵R、仿射因子s和平移向量t，使得最邻近参考点集合Y与惯导指示航迹点集合P之间的欧几里得距离平方和最小，公式如下：式中，Y＝{yi,i＝1,2,…,N}表示最邻近参考点集合，P＝{pi,i＝1,2,…,N}表示惯导指示航迹点集合，d表示集合Y和集合P的欧几里得距离平方和，||X||表示向量X的2范数；利用矩阵的特征值特征向量分解方法得到旋转矩阵R：首先对惯导指示航迹点pi和最邻近参考点yi作一个变换，即令：式中，和分别为惯导指示航迹点和最邻近参考点的点集质心，Δpi和Δyi分别为惯导指示航迹点和最邻近参考点相对它们各自点集质心的位置坐标，且与均为0；然后根据相对二维位置坐标Δpi和Δyi构造一个2×2矩阵M，即：2CN114111798A权利要求书2/3页式中，M是由坐标变换后的惯导指示航迹点Δpi和最邻近参考点Δyi构成的2阶方阵，右上角标T表示矩阵的转置；T接着求解对称正定矩阵MM的正特征值λj和对应λj的单位特征向量uj(j＝1,2)，使其满足下式：根据2阶矩阵M和关于MTM的特征值、特征向量，最后得到旋转矩阵R为：接着求解仿射因子s：最邻近参考点集合Y与惯导指示航迹点集合P之间的欧几里得距离平方和d(Y,sRP+t)，也即集合Y与集合P之间误差ei的平方和：其中由此得到由于与均为0，那么为0，易得Δt也为0时式(8)可达到最小值，此时根据刚性变换不改变长度模值的特性：22||RΔpi||＝||Δpi||(11)将式(10)改写为：3CN114111798A权利要求书3/3页其中，Y