一种多特征点位置姿态冗余解算方法
【专利摘要】本发明属于物体位置姿态测量【技术领域】，具体涉及一种多特征点位置姿态冗余解算方法。方法以特征点在视觉测量系统中的三维坐标为输入条件，获取数据后通过计算特征点到空间虚拟点的距离寻找匹配点对，若所有特征点均能找到匹配点对，则直接进行去重心化操作；若无法找到匹配点对，则该点被自动舍弃，记录匹配点对个数，动态调整后续算法数据入口大小，由剩余点解算该时刻的姿态位置信息；完成匹配后，通过去重心化实现平移信息和旋转信息的分离，单独解算旋转矩阵。本发明解决了物体空间位置姿态测量的六自由度解算问题，提高算法精度的同时使算法保持较高的实时性能。
【专利说明】一种多特征点位置姿态冗余解算方法
【技术领域】
[0001] 本发明属于物体位置姿态测量【技术领域】，具体涉及一种多特征点位置姿态冗余解 算方法。
【背景技术】
[0002] 随着航空航天领域的迅猛发展，对飞行器运动过程位置姿态测量的要求已经越来 越广泛，如航天器对接、级间分离姿态的测量等等。目前，主流方法是通过立体摄影测量技 术实现对飞行器上特征点的三维坐标动态测量，进而通过特征点的三维坐标反推物体运动 的六自由度姿态参数，这个过程即为姿态解算。
[0003] 传统解算方法，按照特征点在运动前后两个坐标系中的几何关系建立方程求取姿 态的闭式解或通过优化方法求取数值解。
[0004] 根据刚体运动原理，只需三个特征点即可解算物体的姿态参数，但由于该方法的 求解方程建立在严格准确的几何约束基础上，对噪声干扰的敏感性高，一般采用增加特征 点的方式，提高数值解对噪声干扰的免疫力。但特征点数目的增加又给不同时刻间的特征 点匹配带来了困难，极易造成解算结果的错误；同时，使用数目较多的特征点来进行迭代计 算，严重增加了计算耗时，限制了该方法的应用范围。
【发明内容】
[0005] 本发明的目的在于提供一种求解物体六自由度位置姿态的冗余解算方法，此方法 根据刚体运动学原理利用物体上多个特征点坐标作为输入条件，通过特征点匹配、去重心 化、多点平差等步骤，实现对物体位置姿态的快速精确求解。
[0006] 为达到上述目的，本发明所采取的技术方案为：
[0007] -种多特征点位置姿态冗余解算方法，根据刚体变换原理，物体姿态变化时物体 上的点坐标满足：
[0008] P/ = R*Pi+t
[0009] 其中R为旋转矩阵，t为平移矩阵；
【权利要求】
1. 一种多特征点位置姿态兀余解算方法，其特征在于： 根据刚体变换原理，物体姿态变化时物体上的点坐标满足： P/ = R*Pi+t 其中R为旋转矩阵，t为平移矩阵；
其中a，目，y分别为绕X，y，Z轴的旋转角
，x,，y,，z,分别为沿X，y，z轴 的平移量； 该方法包括如下步骤： 步骤一：获取初始点坐标；获取物体上特征点的H维坐标，特征点的个数大于3个，设 Ti时亥IJ测得的N点坐标分别为： Pi (Xi，Yi, Zi)，尺2 (X2, 72, Z2)......Pn(X。，y。，zj， T2时亥Ij测得的N点坐标分别为：
步骤二；特征点匹配；该匹配过程采用计算特征点到空间虚拟点的距离实现；设Ti时 刻空间虚拟点？。咕，y。，Z。)，取
，分别计算N个 点到P。的距离iPiPj、iPaPj......iPnPj ;设T2时刻空间虚拟点P。'（X。'，y。'，z。'），取
分别计算N个点到P。'的距离 P/P。' I、I P2'P。' I......IPn'P。' I;将 IPi'P。' I 与 IPAUIP2P」......|P"P」分别进 行比较，距离相同或相差小于误差限的点视为匹配点对，若无匹配点对，则舍弃该特征点； 将匹配好的特征点按照统一的顺序进行排列，后续计算过程依次读取即可； 步骤H ;去重也化；物体姿态变化时物体上的点坐标满足： P/ = R冲 i+t (1) 上式对上述虚拟点亦成立，则； Pc' = R*Pc+t (2) 式（1)减去式（2)得： P/ -Pc' =帖化-Pc) 做 N点联立后可得方程组：
步骤四：实现多点间的平差优化。
2. 根据权利要求1所述的多特征点位置姿态兀余解算方法，其特征在于：所述步骤四 采用奇异值分解方法求解方程（4)得到旋转矩阵R，在矩阵病态的情况下，求取最小二乘解 中存在的唯一最小范数解，实现多点间的平差优化。
3. 根据权利要求2所述的多特征点位置姿态兀余解算方法，其特征在于：所述奇异值 分解具体方法为：将矩阵b=巧,奇异值分解，则r = u*vt，u、v为奇异值分解后的两正 交矩阵，由式（1)求得平移矩阵t。
4. 根据权利要求1所述的多特征点位置姿态兀余解算方法，其特征在于：由旋转矩阵R 求解旋转角的过程通过非线性方程的优化求解实现。
【文档编号】G06F19/00GK104424382SQ201310367522
【公开日】2015年3月18日 申请日期:2013年8月21日 优先权日:2013年8月21日
【发明者】孙增玉, 梁雅军, 宋金城, 郭磊, 刘柯, 陈晓晖 申请人:北京航天计量测试技术研究所, 中国运载火箭技术研究院