一种利用已知光源排布的双目定位方法技术

本发明专利技术提供一种利用已知光源排布的双目定位方法，属于计算机视觉定位领域。本发明专利技术通过双目相机在运动过程中捕获目标物体在相机像素图像中的三个可见光特征点的位置，以及六轴惯性传感器获取的俯仰角和滚动角，采用无迹卡尔曼滤波器将惯性传感器的数据和双目相机数据进行整合，实现可见光定位，即精确计算出目标物体在三维空间中一个可见光特征点的位置和航向角。本发明专利技术具有广泛的适用性；具有很好的稳定性，可以进行更远距离的定位，其稳定性不仅在室内环境中表现出色，对于复杂的室外环境如城市街区、山地、海洋等也同样适用。海洋等也同样适用。海洋等也同样适用。

【技术实现步骤摘要】
一种利用已知光源排布的双目定位方法


[0001]本专利技术属于计算机视觉定位领域，是一种使用双目摄像机捕捉的图像和六轴惯性传感器数据来定位的方法，该方法可以通过两个摄像头的数字图像中可见光的特征点以及惯性传感器的数据来确定目标物体的位置和姿态，实现对物体的定位或导航。

技术介绍

[0002]定位和导航常依赖于惯性传感器和双目相机这两种设备，分别通过读取惯性力和光学影像来提供关于运动和空间的信息。但是，这两种传感器各有其弊端：例如，惯性传感器可能会受到噪声和漂移的干扰，而双目相机则可能由于校准精度和光照影响而产生可见光特征点识别的像素误差。因此，在定位算法中，我们常常会整合这两种传感器的数据以提升稳定性和精确性，但这种方法需要依赖高精度的惯性传感器原始数据或目标物体的三个姿态角，对数据精度要求较高，甚至可能需要添加地磁传感器，这对环境的部署造成了大量限制。
[0003]基于此，本专利技术提出了一种新的算法。当我们已知目标物体的可见光特征点的排布条件和在双目相机图像上的位置时，这个算法能够结合低精度的惯性传感器和双目相机图像数据，对目标物体进行位姿解算。这种解算方法稳定且准确，能够进行远距离定位，并且在存在噪声干扰的远距离特征点识别情况下仍能准确计算位置和姿态。
[0004]在本课题组已申请的专利“2023104914792一种光源无约束排布的双目定位方法”(下称专利一)中，也提出了一种利用双目相机和惯性传感器对三个可见光特征点进行定位的算法，该专利针对的是三个无约束排布可见光特征点的目标物体，即三个可见光特征点的相对位置是未知的，该算法具有一定的精度和鲁棒性，但是在已知三个可见光特征点排布的应用情景下，无法利用这些排布信息，精度有待进一步提高。
[0005]本专利技术与专利一的区别之处在于：本专利技术能利用可见光特征点的位置排布信息，只解算一个可见光特征点的位置对目标物体进行定位，从而具有比专利一更高的位姿解算精度，更强的鲁棒性，更长的可定位距离。

技术实现思路

[0006]本专利技术旨在解决双目相机与惯性传感器融合定位算法存在的问题，即由于惯性传感器精度不足或特征点识别噪声导致的定位准确性较低和姿态角误差较大。本专利技术专门针对的是具有三个已知相对位置的可见光特征点的目标物体。通过双目相机在运动过程中捕获目标物体在相机像素图像中的三个可见光特征点的位置，以及六轴惯性传感器获取的俯仰角和滚动角，我们能够计算出目标物体在三维空间中一个可见光特征点的位置和航向角。
[0007]为了达到上述目的，本专利技术采用的技术方案为：
[0008]一种利用已知光源布局的双目定位方法，该方法采用无迹卡尔曼滤波器将惯性传感器的数据和双目相机数据进行整合，从而实现可见光定位。首先，在无迹卡尔曼滤波算法
的框架下，采用恒速模型(即目标物体在连续两个时刻的线速度和角速度保持不变)来建立预测目标物体在三维空间的位置和姿态角的过程方程。接下来，使用双目相机的针孔投影模型来构建测量方程，以此修正过程方程中计算目标物体可见光特征点坐标和航向角的部分。值得注意的是，本专利技术计算的目标物体的可见光特征点位置和姿态角对六轴惯性传感器数据的噪声以及双目相机特征点识别的噪声均具有抗扰动性，同时在目标物体远离相机的情况下仍然能够保持高精度和高稳定性。
[0009]测量具体包括以下步骤：
[0010]1.初始化
[0011]1.1相机标定
[0012]令双目相机左右两个相机分别为相机1，相机2；对相机1和相机2进行双目相机标定，分别获得两个相机的内参(焦距：f，像素图像中心点：(u0,v0)，像元尺寸：dx、dy)以及外参(相机2相对相机1的旋转矩阵：R
CCS
(CCS：Camera Coordinate System，相机坐标系)，平移矩阵：T
CCS
)，这里的相机1和相机2的内参一致，这些参数将在接下来的相机测量方程中使用；
[0013]1.2建立无迹卡尔曼滤波器的状态向量
[0014]以相机1的镜头的光学中心为原点，镜头向外的方向为z轴正方向建立一个右手的三维坐标系，该坐标系为相机坐标系1(CCS1)，同理在相机2上建立相机坐标系2(CCS2)，令CCS1为主坐标系，P
i,k
＝(x
i,k
,y
i,k
,z
i,k
)
T
.(i＝1,2,3)坐分别为k时刻目标物体的三个可见光特征点在CCS1下的坐标。
[0015]令计算周期的第k个周期(以下称：k时刻)的状态向量(以下称：状态)为χ
k
＝[P
1,k T
k β
k
]T
。其中，P
1,k
为第k时刻时目标物体运动中的一个可见光特征点在CCS1下的坐标(令该可见光特征点坐标等效为目标物体的坐标，以下称：目标物体坐标)；T
k
记为k时刻的平移向量；β
k
向为k时刻的航向角。建立状态为使用无迹卡尔曼滤波器的第一步，其中，当k＝0时为无迹卡尔曼滤波器的初始状态，根据当前双目相机与目标物体的距离和相对姿态设置即可；
[0016]1.3三个可见光特征点的相对位置获取
[0017]本专利技术是在已知三个可见光特征点相对位置的情况下计算目标物体的坐标和航向角，需先获取三个可见光特征点的相对位置。先建立如图2所示的目标物体坐标系。以第一个可见光特征点为原点，与CCS1的各轴平行的方向建立目标物体坐标系OCS(Object Coordinate System)。获取剩下两个特征点相对第一个特征点的位置偏移为t
12
和t
13
偏移，分别为第二个可见光特征点和第三个可见光特征点在OCS下相对第一个可见光特征点的位置偏移，有:
[0018][0019]P
1OCS
为OCS下的第一个可见光特征点坐标，和则为第二个和第三个可见光特征点在OCS下的坐标；
[0020]1.4建立无迹卡尔曼滤波器的过程方程
[0021]首先，根据恒速模型，在相邻的两个计算周期的平移向量和航向角不变，即可对k
时刻的状态向量χ
k
(以下简称状态)建立如下等式：
[0022][0023]等式左端为k时刻的状态；等式右端为使用k
‑
1时刻的状态计算k时刻状态的等式，P
1,k
‑1为k
‑
1时刻的CCS1下的目标物体运动的三维空间中的坐标；T
k
‑1记为k
‑
1时刻的平移向量；R
k
记为k时刻的旋转矩阵，通过k时刻的俯仰角α
k
和翻滚角γ
k
以及k
‑
1时刻的俯仰角计算得到，计算公式为：
[0024][0025]，该公式为欧拉
‑
罗德里格斯公式(Euler
‑...

【技术保护点】

【技术特征摘要】
1.一种利用已知光源排布的双目定位方法，针对的是具有三个已知相对位置的可见光特征点的目标物体，其特征在于，通过双目相机在运动过程中捕获目标物体在相机像素图像中的三个可见光特征点的位置，以及六轴惯性传感器获取的俯仰角和滚动角，采用无迹卡尔曼滤波器将惯性传感器的数据和双目相机数据进行整合，实现可见光定位，即精确计算出目标物体在三维空间中一个可见光特征点的位置和航向角；所述的整合过程中：在无迹卡尔曼滤波算法的框架下，采用恒速模型建立预测目标物体在三维空间的位置和姿态角的过程方程，其中恒速模型指目标物体在连续两个时刻的线速度和角速度保持不变；使用双目相机的针孔投影模型构建测量方程，以此修正过程方程中计算目标物体可见光特征点坐标和航向角的部分。2.根据权利要求1所述的一种利用已知光源排布的双目定位方法，其特征在于，具体包括以下步骤：1.初始化1.1相机标定令双目相机左右两个相机分别为相机1，相机2；对相机1和相机2进行双目相机标定，分别获得两个相机的内参以及外参；1.2建立无迹卡尔曼滤波器的状态向量1.3三个可见光特征点的相对位置获取在已知三个可见光特征点相对位置的情况下计算目标物体的坐标和航向角，需先获取三个可见光特征点的相对位置；先建立目标物体坐标系；以第一个可见光特征点为原点，与CCS1的各轴平行的方向建立目标物体坐标系OCS；获取剩下两个特征点相对第一个特征点的位置偏移为t
12
和t
13
偏移，分别为第二个可见光特征点和第三个可见光特征点在OCS下相对第一个可见光特征点的位置偏移，有:P
1OCS
为OCS下的第一个可见光特征点坐标，和则为第二个和第三个可见光特征点在OCS下的坐标；1.4建立无迹卡尔曼滤波器的过程方程首先，根据恒速模型，在相邻的两个计算周期的平移向量和航向角不变，即可对k时刻的状态向量χ
k
建立如下等式：等式左端为k时刻的状态；等式右端为使用k
‑
1时刻的状态计算k时刻状态的等式，P
1,k
‑1为k
‑
1时刻的CCS1下的目标物体运动的三维空间中的坐标；T
k
‑1记为k
‑
1时刻的平移向量；R
k
记为k时刻的旋转矩阵，通过k时刻的俯仰角α
k
和翻滚角γ
k
以及k
‑
1时刻的俯仰角计算得到，计算公式为：
w
k
则为k时刻预测状态的噪声，由无迹卡尔曼滤波算法自动估计；上述的等式统称为过程方程，写为χ
k
＝F(χ
k
‑1,α
k
,γ
k
)；建立过程方程为使用无迹卡尔曼滤波器的第二步，该方程描述了如何由k
‑
1时刻状态χ
k
‑1预测出k时刻的状态χ
k
；1.5建立无迹卡尔曼滤波器的测量方程在1.1中对双目相机已经进行了标定，获得内参矩阵；对于第k时刻，对于在CCS1下的目标物体坐标：P
1,k
，可以改为增广形式：P
1,k
＝(x
1,k
,y
1,k
,z
1,k
,1)
T
，该点在相机1图像的像素坐标其中上标p1为相机1的像素图像，为该点在像素图像的横纵坐标；P
1,k
和的关系为：这里将该等式记为：其中E3为三阶单位矩阵；同时，还需获得P
1,k
在相机2的像素图像上的坐标其中上标p2为相机2的像素图像，先计算P
1,k
在CCS2下的坐标下的坐标为，其中上标CCS2为相机坐标系2下的坐标：则为：将该式记为：最后，将等式和合并为：该等式即为双目相机的针孔投影模型下对P
1,k
的测量方程，测量方程将CCS1下的点坐标P
1,k
转化为双目相机的相机1和相机2的像素图像坐标，也就是可以从1.2中的预测状态χ
k
中的P
1,k
计算出相机1和相机2的像素图像坐标；从双目相机中获得以及从双目相机标定结果获得R
CCS
、T
CCS
后，使用无迹卡尔曼滤波器对状态中的P
1,k
进行修正；同时，在已知OCS下的点P
1OCS
、和的相对位置的情况下，可以在此观测模型下同时使用P
2,k
P
3,k
对P
1,k
进行观测；1.6无迹卡尔曼滤波器的初始化参数2.无迹卡尔曼滤波器迭代2.1俯仰角和翻滚角获取2.2图像获取对于带有三个无约束排布可见光特征点的目标物体，使用双目相机捕捉在其运动过程中的三个可见光特征点在相机像素图像中的位置，任意的k时刻的第i个可见光特征点在第j个相机的像素图像的坐标记为图像的坐标记，其中上标pj为第j个相机的像素图像；2.3计算k
‑
1时刻状态的sigma点集2.4计算k时刻过程的sigma点集2.5计算k时刻的状态协方差矩阵2.6计算k时刻过程的sigma点集下的第二个和第三个特征点坐标在2.4中计算出了k时刻过程的sigma点集χ
l,k
(l＝0,1,...,2n)，为了能将第二个和第三个特征点在相机1和相机2中的像素图像(和)引入到观测方程中，还需要计算在该过程的sigma点集下其它两个可见光特征点的坐标；2.6.1计算第二个和第三个特征点的相对位置旋转矩阵令为k时刻过程的sigma点集的航向角，为k时刻在过程的sigma点集下的其他两个可见光特征点相对第一个可见光特征点的相对位置矩阵，有：阵，有：为k
‑
1时刻的最优状态的相对位置旋转矩阵；2.6.2计算第二个和第三个特征点坐标令分别为k时刻的过程sigma点集下的第二...

【专利技术属性】
技术研发人员：谢心月，邹德岳，赵楠，
申请(专利权)人：大连理工大学，
类型：发明
国别省市：