买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

龙图腾网获悉大连理工大学申请的专利一种光源无约束排布的双目定位方法获国家发明授权专利权，本发明授权专利权由国家知识产权局授予，授权公告号为：CN116518963B 。

龙图腾网通过国家知识产权局官网在2025-09-02发布的发明授权授权公告中获悉：该发明授权的专利申请号/专利号为：202310491479.2，技术领域涉及：G01C21/16；该发明授权一种光源无约束排布的双目定位方法是由谢心月;邹德岳;赵楠设计研发完成，并于2023-05-05向国家知识产权局提交的专利申请。

本一种光源无约束排布的双目定位方法在说明书摘要公布了：本发明提供一种光源无约束排布的双目定位方法，属于计算机视觉定位领域。是一种使用无迹卡尔曼滤波器将惯性传感器的数据和双目相机数据融合的可见光定位方法。本发明首先在无迹卡尔曼滤波算法框架中，建立过程方程来预测目标物体在三维空间的位置和姿态角，然后根据双目相机的像素图像建立测量方程来修正过程方程中计算的目标物体的可见光特征点位置和航向角。本发明计算的目标物体的可见光特征点的位置和姿态角即使在六轴惯性传感器的数据精度低、可见光特征点在像素图像中的位置识别噪声大的情况下也能保持高精度；适用性广，稳定性强。

本发明授权一种光源无约束排布的双目定位方法在权利要求书中公布了：1.一种光源无约束排布的双目定位方法，其特征在于，所述方法为一种使用无迹卡尔曼滤波器将惯性传感器的数据和双目相机数据融合的可见光定位方法；首先在无迹卡尔曼滤波算法框架中，建立过程方程来预测目标物体在三维空间的位置和姿态角，然后根据双目相机的像素图像建立测量方程来修正过程方程中计算的目标物体的可见光特征点位置和航向角；包括以下步骤： 步骤1.初始化 步骤1.1相机标定 令双目相机左右两个相机分别为相机1，相机2；对相机1和相机2进行双目相机标定，分别获得两个相机的内参和外参；所述内参包括焦距f，像素图像中心点坐标为u0,v0，像元尺寸为dx、dy，相机1和相机2的内参一致；所述外参包括相机2相对相机1的旋转矩阵RCCS，CCS表示相机坐标系，平移矩阵TCCS； 步骤1.2建立无迹卡尔曼滤波器的状态向量 以相机1的镜头的光学中心为原点，镜头向外的方向为z轴正方向建立一个右手的三维坐标系，该坐标系为相机坐标系1，定义为CCS1，同理在相机2上建立相机坐标系2定义为CCS2，令CCS1为主坐标系；令计算周期的第k个周期的预测状态χk＝[P1,kP2,kP3,kTkβk]T，第k个周期以下称k时刻，状态向量以下称状态，其中，Pi,ki＝1,2,3为第k时刻时第i个可见光特征点随目标物体运动中的CCS1下的坐标；Tk记为k时刻的平移向量；βk向为k时刻的航向角； 当k＝0时为无迹卡尔曼滤波器的初始状态，根据当前双目相机与目标物体的距离和相对姿态设置即可； 步骤1.3建立无迹卡尔曼滤波器的过程方程 首先，对k时刻的状态向量χk建立如下等式： 等式左端为k时刻的状态；等式右端为使用k-1时刻的状态计算k时刻状态的等式，Pi,k-1为k-1时刻的CCS1下的第i个可见光特征点随目标物体运动的三维空间中的坐标；Tk-1记为k-1时刻的平移向量；Rk记为k时刻的旋转矩阵，通过k时刻的俯仰角αk和翻滚角γk以及k-1时刻的俯仰角计算得到，计算公式为： 该公式为欧拉-罗德里格斯公式；使用计算k时刻的前m个周期的平移向量输出的平均值，用以计算Tk-1；同理，使用计算k时刻的前m个周期的航向角输出的平均值，用以计算βk-1；wk-1则为k-1时刻预测状态的噪声，由无迹卡尔曼滤波算法自动估计； 上述的等式统称为过程方程χk＝Fχk-1,αk,γk，该方程描述了如何由k-1时刻状态χk-1预测出k时刻的状态χk； 步骤1.4建立无迹卡尔曼滤波器的测量方程 相机将三维空间的任意点成像到相机的像素图像上，如果能获得相机对三维空间的任意点坐标与相机像素图像的位置关系，那么在已知该点在相机的像素图像上的坐标之后，就能得到该点三维空间的坐标的部分信息，利用该信息和卡尔曼滤波算法对步骤1.2中预测状态χk进行矫正，该矫正过程称为测量； 在步骤1.1中对双目相机已经进行了标定，令： 该矩阵A称为内参矩阵； 对于第k时刻，在CCS1下的第i个特征点坐标Pi,k改为增广形式Pi,k＝xi,k,yi,k,zi,k,1T，该点在相机1图像的像素坐标其中上标p1为相机1的像素图像，为该点在像素图像的横纵坐标；上述i＝1,2,3，则Pi,k和的关系为： 将该等式记为：其中E3为三阶单位矩阵；同时，还需获得Pi,k在相机2的像素图像上的坐标上标p2为相机2的像素图像，先计算Pi,k在CCS2下的坐标如下，其中i＝1,2,3： 则为： 将该式记为： 最后，将等式和合并为： 该等式即为测量方程，测量方程将CCS1下的点坐标Pi,k转化为双目相机的相机1和相机2的像素图像坐标，也就是可以从步骤1.2中的预测状态χk中的Pi,k计算出相机1和相机2的像素图像坐标；从双目相机中获得以及从双目相机标定结果获得RCCS、TCCS后，使用无迹卡尔曼滤波器对Pi,k进行修正； 步骤1.5无迹卡尔曼滤波器的初始化参数 在步骤1.2、步骤1.3、步骤1.4中得到状态、过程方程、测量方程的卡尔曼滤波器，还需确定无迹卡尔曼滤波器的初值，该初值为目标物体的起始位置和姿态；需针对过程方程对实际目标物体运动时的三个特征点坐标变化和姿态角变化描述的准确性设置过程噪声协方差矩阵S；对应的，针对测量方程对相机成像描述的准确性设置测量噪声协方差矩阵L； 步骤2.无迹卡尔曼滤波器迭代 步骤2.1俯仰角和翻滚角获取 对于带有三个无约束排布可见光特征点的目标物体，安装六轴惯性传感器以获得在三维空间运动过程中三个实时变化的姿态角，分别为：俯仰角，航向角，翻滚角，任意的第k时刻的这三个姿态角分别记为：αk、βk、γk； 步骤2.2图像获取 对于带有三个无约束排布可见光特征点的目标物体，使用双目相机捕捉在其运动过程中的三个可见光特征点在相机像素图像中的位置，任意的k时刻的第i个可见光特征点在第j个相机的像素图像的坐标记为Pi pj，i＝1,2,3；j＝1,2；上标pj为第j个相机的像素图像； 步骤2.3计算k-1时刻状态的sigma点集 将k-1时刻的最优状态Xk-1的按照以下规则分散成2n+1个状态sigma点集： χl,k-1则为k-1时刻的最优状态Xk-1的sigma点集，即k-1时刻状态的sigma点集，其中l＝0,1,...,2n；n为Xk-1的成员数量；λ为对角线因子，为常数，计算公式为λ＝ε2n+κ-n，ε计为分布范围因子，为常数，κ为分布因子，为常数；Qk-1为k-1时刻的最优状态协方差矩阵，由无迹卡尔曼滤波器自动估计； 步骤2.4计算k时刻过程的sigma点集 在步骤1.3中建立了过程方程χk＝Fχk-1,αk,γk，将步骤2.3中的k-1时刻状态的sigma点集χl,k-1带入该方程计算状态的sigma点集在过程方程中的更新，即为k时刻过程的sigma点集χl,k，其中l＝0,1,...,2n： 其中Wl m为均值权重向量，其中l＝0,1,...,2n，计算公式为： 为过程的sigma点集χl,k在均值权重向量Wl m下的过程的sigma点集的均值； 步骤2.5计算k时刻的状态协方差矩阵 为k时刻的状态协方差矩阵；Wl c为方差权重向量，其中l＝0,1,...,2n，计算公式为： S为过程噪声协方差矩阵，在步骤1.5中设置； 步骤2.6计算k时刻的测量的sigma点集 在步骤1.4中建立测量方程HA,Pi,k,RCCS,TCCS，将步骤2.4中的k时刻过程的sigma点集χl,k带入该方程计算该点集在测量方程中的更新，得到k时刻测量的sigma点集，其中l＝0，2，...，2n： Zl,k为k时刻的测量sigma点集，为k时刻在均值权重向量Wl m下的过程sigma点集均值，其中l＝0,1,...,2n； 步骤2.7计算k时刻的测量协方差矩阵 为k时刻的测量协方差矩阵；L为测量噪声协方差矩阵，在步骤1.5中设置； 步骤2.8计算k时刻的卡尔曼增益 为k时刻的状态-测量联合协方差矩阵；Kk为k时刻的卡尔曼增益； 步骤2.9计算k时刻的最优状态和重复迭代 步骤2.9.1计算k时刻的最优状态 Xk为k时刻的最优状态；zk为从步骤2.2中获得的k时刻的可见光特征点的像素图像的坐标其中i＝1,2,3；j＝1,2；Qk为k时刻的最优状态的协方差矩阵； 步骤2.9.2输出和重复迭代 在步骤2.9.1中计算出k时刻的最优状态和k时刻的最优状态协方差之后，k时刻的迭代完成，Xk即是目标物体的三个可见光特征点在CCS1下的坐标和姿态角；令此k时刻为k-1时刻，下一时刻为k时刻，返回步骤2.1重新计算k时刻的最优状态和k时刻的最优状态协方差。

如需购买、转让、实施、许可或投资类似专利技术，可联系本专利的申请人或专利权人大连理工大学，其通讯地址为：116024 辽宁省大连市甘井子区凌工路２号；或者联系龙图腾网官方客服，联系龙图腾网可拨打电话0551-65771310或微信搜索“龙图腾网”。