CN114565669A - 一种场端多相机融合定位方法 - Google Patents

一种场端多相机融合定位方法 Download PDF

Info

Publication number
CN114565669A
CN114565669A CN202111523787.6A CN202111523787A CN114565669A CN 114565669 A CN114565669 A CN 114565669A CN 202111523787 A CN202111523787 A CN 202111523787A CN 114565669 A CN114565669 A CN 114565669A
Authority
CN
China
Prior art keywords
pose
observation
observation object
kalman filter
observed
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111523787.6A
Other languages
English (en)
Inventor
戴必林
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Human Horizons Shanghai Autopilot Technology Co Ltd
Original Assignee
Human Horizons Shanghai Autopilot Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Human Horizons Shanghai Autopilot Technology Co Ltd filed Critical Human Horizons Shanghai Autopilot Technology Co Ltd
Priority to CN202111523787.6A priority Critical patent/CN114565669A/zh
Publication of CN114565669A publication Critical patent/CN114565669A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/292—Multi-camera tracking

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Image Analysis (AREA)
  • Image Processing (AREA)

Abstract

本发明公开了一种场端多相机融合定位方法，包括：获取多个不同位置的摄像头在当前时刻识别出的待定位区域图像；对多个待定位区域图像中的观测对象进行全局最近邻关联，得到同一观测对象在不同摄像头下的接地点位置和像素坐标；根据观测对象在不同摄像头下的接地点位置和像素坐标，确定观测对象的观测位姿；利用观测对象的观测位姿进行卡尔曼滤波更新，得到观测对象的估计位姿；本发明实施例对于基于路侧摄像头捕捉到的某一实际场景的待定位区域图像进行融合定位，并结合卡尔曼滤波技术进行定位更新，可以实现该待定位区域内车辆和行人的高精度定位。

Description

一种场端多相机融合定位方法
技术领域
本发明涉及多相机融合定位技术领域，尤其涉及一种场端多相机融合定位方法、设备及存储介质。
背景技术
目前常用的车辆定位技术主要分为直接定位和航位推算，其中，直接定位主要基于GPS信号的空间交汇测量及环境特征的匹配定位，航位推算则是依据车辆的加速度、角速度、速度等信息结合初始值进行积分定位。但是，对于实际场景中，例如室内、地下以及复杂环境，由于GPS信号弱或环境复杂等原因，采用传统的车辆定位技术难以确定车辆在上述场景中的准确位置。
因此，如何在各种实际场景中对车辆进行高精度定位，成为本领域亟需解决的问题。
发明内容
本发明实施例提供一种场端多相机融合定位方法，其能实现车辆的高精度定位。
本发明一实施例提供一种场端多相机融合定位方法，其包括：
获取多个不同位置的摄像头在当前时刻识别出的待定位区域图像；
对多个所述待定位区域图像中的观测对象进行全局最近邻关联，得到同一所述观测对象在不同所述摄像头下的接地点位置和像素坐标；
根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，确定所述观测对象的观测位姿；
利用所述观测对象的观测位姿进行卡尔曼滤波更新，得到所述观测对象的估计位姿。
作为上述方案的改进，所述根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，确定所述观测对象的观测位姿，包括：
根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，计算所述观测对象的中心位姿；
根据所述观测对象的中心位姿对所述观测对象进行三维投影，得到观测对象的观测位姿。
作为上述方案的改进，所述根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，计算所述观测对象的中心位姿,包括：
根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，对所述观测对象进行极线约束，得到所述观测对象在不同所述摄像头下的2D框的中心像素坐标；
其中，以所述观测对象所在的多个所述待定位区域图像的2D框的中心为极线约束的交点；所述2D框为所述观测对象在对应待定位区域图像中的外接矩形框；
根据所述观测对象在不同所述摄像头下的2D框的中心像素坐标，通过三角化求解得到所述观测对象的中心位姿。
作为上述方案的改进，当所述观测对象为车辆时，所述中心位姿包括所述车辆的中心坐标及其航向角；
则，所述根据所述观测对象的中心位姿对所述观测对象进行三维投影，得到观测对象的观测位姿，包括：
根据所述车辆的中心坐标及其航向角，通过预设的长方体投影模型进行位姿估算，得到所述车辆的观测位姿。
作为上述方案的改进，当所述观测对象为行人时，所述中心位姿包括所述行人的中心坐标；
则，所述根据所述观测对象的中心位姿对所述观测对象进行三维投影，得到观测对象的观测位姿，包括：
根据所述行人的中心坐标，通过预设的圆柱体投影模型进行位置估算，得到所述行人的观测位姿。
作为上述方案的改进，所述利用所述观测对象的观测位姿进行卡尔曼滤波更新，得到所述观测对象的估计位姿，包括：
获取卡尔曼滤波器当前时刻输出的各个所述观测对象的滤波位姿；其中，所述观测对象的滤波位姿是所述卡尔曼滤波器根据对应所述观测对象在上一时刻的滤波位姿和对应的运动模型估算得到；
对各个所述观测对象的观测位姿及各个所述观测对象的滤波位姿进行全局最近邻关联，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果；
根据各个观测对象的第一关联结果及其观测位姿，采用所述卡尔曼滤波器对各个所述观测对象进行位姿更新，得到所述观测对象的估计位姿。
作为上述方案的改进，所述对各个所述观测对象的观测位姿及各个所述观测对象的滤波位姿进行全局最近邻关联，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果，包括：
计算各个所述观测对象的观测位姿和各个所述观测对象的滤波位姿之间的第一欧式距离；
根据计算得到的第一欧式距离，构建第一代价矩阵；
根据所述第一代价矩阵进行全局最近邻关联求解，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果。
作为上述方案的改进，所述根据所述第一代价矩阵进行全局最近邻关联求解，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果，包括：
将所述第一代价矩阵中超过预设阈值的第一欧式距离更新为第一固定值；
根据更新后的第一代价矩阵进行全局最近邻关联求解，得到各个所述观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿初始的第一关联结果；
对各个所述观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿初始的第一关联结果进行过滤处理，得到最终的第一关联结果；其中，最终的第一关联结果中第一欧式距离均未超过预设阈值。
作为上述方案的改进，所述根据各个观测对象的第一关联结果及其观测位姿，采用所述卡尔曼滤波器对各个所述观测对象进行位姿更新，得到所述观测对象的估计位姿，包括：
当所述观测对象的关联结果为所述卡尔曼滤波器存在对所述观测对象的位姿跟踪，根据所述观测对象的观测位姿，采用所述卡尔曼滤波器对对应的观测对象进行位姿更新，得到所述观测对象的估计位姿；
当所述观测对象的关联结果为所述卡尔曼滤波器不存在对所述观测对象的位姿跟踪，在所述卡尔曼滤波器中添加所述观测对象，并根据所述观测对象的观测位姿初始化其在所述卡尔曼滤波器中的滤波位姿。
作为上述方案的改进，所述对多个所述待定位区域图像中的观测对象进行全局最近邻关联，得到同一所述观测对象在不同所述摄像头下的接地点位置和像素坐标，包括：
根据所述待定位区域图像中各个观测对象的像素坐标以及对应摄像头的外参，计算所述待定位区域图像中各个观测对象的接地点位置；
计算任意两个所述待定位区域图像中各个观测对象的像素坐标之间、接地点位置之间的第二欧式距离；
根据计算得到的第二欧式距离，构建第二代价矩阵；
根据所述第二代价矩阵进行全局最近邻关联求解，得到同一所述观测对象在不同所述摄像头下的接地点位置和像素坐标。
相比于现有技术，本发明实施例的有益效果在于：对于某一实际场景，通过设置路侧安装的多个摄像头捕捉同一待定位区域的待定位区域图像并识别出对应待定位区域图像中的观测对象；然对多个待定位区域图像中的观测对象进行全局最近邻关联，得到同一观测对象在不同摄像头下的接地点位置和像素坐标；根据观测对象在不同摄像头下的接地点位置和像素坐标，确定观测对象的观测位姿；利用观测对象的观测位姿进行卡尔曼滤波更新，得到观测对象的估计位姿本发明实施例基于路侧摄像头捕捉到的待定位区域图像进行融合定位，并结合卡尔曼滤波技术进行定位更新，可以实现该待定位区域内各个观测对象(例如车辆、行人)的高精度定位。
附图说明
为了更清楚地说明本发明的技术方案，下面将对实施方式中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种场端多相机融合定位方法的流程图；
图2是本发明实施例提供的摄像头布局示意图；
图3是本发明实施例提供的极线约束求解位置的示意图；
图4是本发明实施例提供的车辆三维投影示意图；
图5是本发明实施例提供的行人三维投影示意图。
图6是本发明实施例提供的定位流程示意图；
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参见图1，是本发明实施例提供的一种场端多相机融合定位方法的流程示意图，所述场端多相机融合定位方法由云端服务器执行，该方法具体包括：
S11：获取多个不同位置的摄像头在当前时刻识别出的待定位区域图像；
其中，所述待定位区域图像标识有至少一个观测对象并携带有所述观测对象的像素坐标；又或者所述待定位区域图像没有识别出观测对象。
S12：对多个所述待定位区域图像中的观测对象进行全局最近邻关联，得到同一所述观测对象在不同所述摄像头下的接地点位置和像素坐标；
示例性的，多个所述摄像头设置在同一待定位区域的路侧的不同位置，并设置在距离地面设定高度的位置，例如距离地面3米处安装摄像头，并预先标定出各个摄像头的外参和内参，用于从不同角度拍摄所述待定位区域，以得到不同视角的待定位区域图像。如图2所示，其给出了在以矩形的待定位区域的四个顶角各布局一个摄像头的示例。同一待定位区域的摄像头同步触发图像识别，所述观测对象可以是车辆和/或行人；例如各个所述摄像头分别对在同一时刻捕捉到的待定位区域图像进观测对象的识别，并标识出识别出的观测对象。例如，所述摄像头可利用神经网络对其拍摄到的待定位区域图像进行机器学习，从而标识出对应待定位区域图像中的观测对象及观测对象类别；所述观测对象类别包括车辆类别、行人类别。各个所述摄像头对根据自身的外参，以对应待定位图像的下边沿中心为接地中心点，计算出对应所述摄像头下各个所述观测对象的接地点位置；其中，根据摄像头的外参计算对应图像中目标物的位置属于现有技术，在这里不再详细说明。所述摄像头将识别出的待定位区域图像及其观测对象、观测对象类型、像素坐标、接地点位置发送到云端服务器进行后续的优化定位。
云端服务器通过全局最近邻关联对多个摄像头捕捉到的待定位区域图像中各个观测对象进行关联匹配，将不同摄像头下的各个观测对象一一匹配，从而得到同一观测对象在不同摄像头像的接地点位置和像素坐标。
S13：根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，确定所述观测对象的观测位姿；
S14：利用所述观测对象的观测位姿进行卡尔曼滤波更新，得到所述观测对象的估计位姿。
在本发明实施例中，基于路侧摄像头捕捉到的待定位区域图像进行融合定位，并结合卡尔曼滤波技术进行定位更新，可以实现该待定位区域内各个观测对象(例如车辆、行人)的高精度定位，为车辆的自动驾驶提供数据支持；同时利用路侧安装的低成本摄像头，无需在车辆加装车载摄像头，节约成本，此外还可以获得待定位车辆的全视野的图像，进一步提高定位精度。
在一种可选的实施例中，所述对多个所述待定位区域图像中的观测对象进行全局最近邻关联，得到同一所述观测对象在不同所述摄像头下的接地点位置和像素坐标，包括：
根据所述待定位区域图像中各个观测对象的像素坐标以及对应摄像头的外参，计算所述待定位区域图像中各个观测对象的接地点位置；
计算任意两个所述待定位区域图像中各个观测对象的像素坐标之间、接地点位置之间的第二欧式距离；
根据计算得到的第二欧式距离，构建第二代价矩阵；
根据所述第二代价矩阵进行全局最近邻关联求解，得到同一所述观测对象在不同所述摄像头下的接地点位置和像素坐标。
示例性，同一观测对象在不同摄像头下的全局最近邻关联求解过程如下：可以计算某一个摄像头下m个观测对象的观测位姿与另一个摄像头下m个观测对象的观测位姿的第二欧式距离sqrt((x3-x4)2+(y3-y4)2),其中，(x3，y3)表示某一个摄像头下的一个观测对象的观测位姿，(x4，y4)表示另一个摄像头下的观测对象的观测位姿；以计算出的第二欧式距离作为矩阵元素，可以得到一个m×m的第二代价矩阵；所述第二代价矩阵的第i行j列表示摄像头的观测对象i和j的第二欧式距离。其中，当需要对车辆类别的观测对象进行关联时，将该第二代价矩阵中与行人类别相关的第二欧式距离设置为最大值。其中，所述最大值可预设为100，例如将某行人与某车辆、某行人与某行人之间的第一欧式距离设置100；同理，当需要对行人类别的观测对象进行关联时，将该第二代价矩阵中与车辆类别相关的第二欧式距离设置为最大值，例如将某行人与某车辆、某车辆与某车辆之间的第二欧式距离设置100。根据上述设置得到车辆或行人的第二代价矩阵；然后对该第二代价矩阵进行阈值判断，即将所述第二代价矩阵中超过预设阈值的第二欧式距离更新为第一固定值，从而得到最终的第二代价矩阵。根据该第二代价矩阵采进行全局最近邻关联求解属于同一类别的各个摄像头下各个观测对象之间的第二关联结果，并过滤掉第二关联结果中第二欧式距离超过预设阈值的关联，得到最终的第二关联结果；例如，摄像头1下的观测对象1与摄像头2下的观测对象1关联匹配，为同一对象，摄像头1下的观测对象2与摄像头2下的观测对象2关联匹配，为同一对象，建立不同摄像头下同一观测对象的关联，可以为后续的观测对象的三维投影建模提供多视角的图像数据支持，进一步提高定位精度。
在一种可选的实施例中，所述根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，确定所述观测对象的观测位姿，包括：
步骤S131：根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，计算所述观测对象的中心位姿；
进一步，所述根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，计算所述观测对象的中心位姿,包括：
根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，对所述观测对象进行极线约束，得到所述观测对象在不同所述摄像头下的2D框的中心像素坐标；
其中，以所述观测对象所在的多个所述待定位区域图像的2D框的中心为极线约束的交点；所述2D框为所述观测对象在对应待定位区域图像中的外接矩形框；
根据所述观测对象在不同所述摄像头下的2D框的中心像素坐标，通过三角化求解得到所述观测对象的中心位姿；
如图3所述，以所述观测对象所在的多个所述待定位区域图像的2D框的中心为极线约束的交点,采用质点模型将每个观测对象在不同摄像头下的待定位区域图像中简化为一个特征点Pj，k,其中，Pj，k表示第j个观测对象在第k个摄像头下的待定位区域图像中的点，然后进行极线约束求解，得到观测对象的中心像素坐标；之后，以所述观测对象的中心像素坐标作为初始值，进行三角化求解，得到所述观测对象的中心位姿；具体的三角化求解过程如下：
设观测对象齐次坐标[x y z 1]T，则该齐次坐标[x y z 1]T到待定位区域图像的投影为：
Figure BDA0003409114120000101
公式两边同时左叉乘μ，得到μ^PX＝0，并展开后可得到：
Figure BDA0003409114120000102
其中，P1、P2、P3分别表示同一观测对象的在三个不同摄像头下的待定位区域图像的中心像素坐标；λ表示深度值；
Figure BDA0003409114120000103
表示世界坐标系到相机坐标系的变换。通过上述公式可以求解出X的值，从而得到观测对象的中心坐标；当所述观测对象为车辆时，将求解出的中心坐标和车辆的航向角作为车辆的中心位姿；当所述观测对象为行人时，将求解出的中心坐标作为行人的中心位姿。
步骤S132：根据所述观测对象的中心位姿对所述观测对象进行三维投影，得到观测对象的观测位姿。
进一步，当所述观测对象为车辆时，所述中心位姿包括所述车辆的中心坐标及其航向角；
则，所述根据所述观测对象的中心位姿对所述观测对象进行三维投影，得到观测对象的观测位姿，包括：
根据所述车辆的中心坐标及其航向角，通过预设的长方体投影模型进行位姿估算，得到所述车辆的观测位姿。
如图4所示，对通过预设的长方体投影模型，为车辆类别的观测对象进行长方体投影；所述长方体投影模型为：
Figure BDA0003409114120000111
其中，w表示所述车辆的宽度，h表示所述车辆的高度，l表示所述车辆的长度，
Figure BDA0003409114120000112
表示相机模型，
Figure BDA0003409114120000113
Figure BDA0003409114120000114
表示所述车辆的3D Bounding Box的4个顶点在车辆坐标系的坐标，例如取图4中标识出的底面3个顶点和顶面1个顶点；
Figure BDA0003409114120000115
Figure BDA0003409114120000116
表示所述车辆的对应4个顶点在相机坐标系的坐标；(xcar，ycar，zcar)表示车辆在全局坐标系的中心坐标，即上述三角化求解得到的X中的坐标值；umin表示车辆在待定位区域图像上的2DBounding Box的像素左边沿横向位置，umax表示车辆在待定位区域图像上的2D BoundingBox的像素右边沿横向位置，vmin表示车辆在待定位区域图像上的2D Bounding Box的像素下边沿纵向位置，vmax表示车辆在待定位区域图像上的2D Bounding Box的像素上边沿纵向位置，ui表示车辆顶点在待定位区域图像上的横向位置，vi表示车辆在待定位区域图像上的2D Bounding Box的像素上边沿。K表示图像坐标系到相机坐标系的变换，
Figure BDA0003409114120000117
表示相机坐标系到车辆坐标系的变换；θ表示车辆的航向角。
通过上述投影变换，可以对所述车辆的定位进行优化，得到更加准确的车辆位姿。
进一步的，当所述观测对象为行人时，所述中心位姿包括所述行人的中心坐标；
则，所述根据所述观测对象的中心位姿对所述观测对象进行三维投影，得到观测对象的观测位姿，包括：
根据所述行人的中心坐标，通过预设的圆柱体投影模型进行位置估算，得到所述行人的观测位姿。
如图5所示，对通过预设的圆柱体投影模型，为行人类别的观测对象进行圆柱体投影；所述圆柱体投影模型为：
Figure BDA0003409114120000121
其中，
Figure BDA0003409114120000122
表示行人的圆柱体投影模型的离散点在行人坐标系的坐标；r表示圆柱体投影模型的半径；(xped，yped，zped)表示车辆在全局坐标系的中心坐标，uup表示圆柱体投影模型上侧圆上的离散点的的横向像素坐标；vup表示圆柱体投影模型上侧圆上的离散点的的纵向像素坐标,udown表示圆柱体投影模型下侧圆上的离散点的的横向像素坐标,vdown表示圆柱体投影模型下侧圆上的离散点的的纵向像素坐标；amin＝min(uup，udown)，amax＝max(uup，udown)，bmin＝min(vup，vdown)，a bmin＝min(uup，udoon)，amax＝max(uup，udown)。
通过上述投影变换，可以对所述行人的定位进行优化，得到更加准确的行人位置。
在一种可选的实施例中，所述利用所述观测对象的观测位姿进行卡尔曼滤波更新，得到所述观测对象的估计位姿，包括：
获取卡尔曼滤波器当前时刻输出的各个所述观测对象的滤波位姿；其中，所述观测对象的滤波位姿是所述卡尔曼滤波器根据对应所述观测对象在上一时刻的滤波位姿和对应的运动模型估算得到；
示例性的，所述卡尔曼滤波器可根据连续帧的待定位区域图像中观测对象的位置变化，估算观测对象的移动速度，从而构建出对应的运动模型；然后根据其上一时刻输出的滤波位姿和推算出的运动模型，进一步估算所述观测对象在当前时刻的滤波位姿。
对各个所述观测对象的观测位姿及各个所述观测对象的滤波位姿进行全局最近邻关联，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果；
根据各个观测对象的第一关联结果及其观测位姿，采用所述卡尔曼滤波器对各个所述观测对象进行位姿更新，得到所述观测对象的估计位姿。
进一步，所述对各个所述观测对象的观测位姿及各个所述观测对象的滤波位姿进行全局最近邻关联，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果，包括：
计算各个所述观测对象的观测位姿和各个所述观测对象的滤波位姿之间的第一欧式距离；
根据计算得到的第一欧式距离，构建第一代价矩阵；
根据所述第一代价矩阵进行全局最近邻关联求解，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果。
进一步，所述根据所述第一代价矩阵进行全局最近邻关联求解，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果，包括：
将所述第一代价矩阵中超过预设阈值的第一欧式距离更新为第一固定值；
根据更新后的第一代价矩阵进行全局最近邻关联求解，得到各个所述观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿初始的第一关联结果；
对各个所述观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿初始的第一关联结果进行过滤处理，得到最终的第一关联结果；其中，最终的第一关联结果中第一欧式距离均未超过预设阈值；
在本发明实施例中，需要将摄像头捕捉到的观测对象的观测位姿和卡尔曼滤波器跟踪的对象滤波位姿进行关联匹配，具体的，可以计算摄像头下m个观测对象的观测位姿与卡尔曼滤波器输出的n个滤波位姿的第一欧式距离sqrt((x1-x2)2+(y1-y2)2),其中，(x1，y1)表示观测位姿，(x2，y2)表示滤波位姿；以计算出的第一欧式距离作为矩阵元素，可以得到一个m×n的第一代价矩阵；所述第一代价矩阵的第i行j列表示卡尔曼滤波器的跟踪对象i与摄像头的观测对象j的第一欧式距离。其中，当需要对车辆类别的观测对象进行关联时，将该第一代价矩阵中与行人类别相关的第一欧式距离设置为最大值，同时将偏航角超过预设角度阈值的车辆对应的第一欧式距离也设置为最大值。其中，所述最大值可预设为100，例如将某行人与某车辆、某行人与某行人之间的第一欧式距离设置100；同理，当需要对行人类别的观测对象进行关联时，将该第一代价矩阵中与车辆类别相关的第一欧式距离设置为最大值，例如将某行人与某车辆、某车辆与某车辆之间的第一欧式距离设置100。根据上述设置得到车辆或行人的第一代价矩阵；然后对该第一代价矩阵进行阈值判断，即将所述第一代价矩阵中超过预设阈值的第一欧式距离更新为第一固定值，所述第一固定值为100，从而得到最终的第一代价矩阵。根据该第一代价矩阵进行全局最近邻关联求解属于同一类别的观测对象和卡尔曼滤波器的跟踪对象之间的第一关联结果，并过滤掉第一关联结果中第一欧式距离超过预设阈值的关联，得到最终的第一关联结果；例如，观测对象1与卡尔曼滤波器的跟踪对象1关联匹配，为同一对象，观测对象2与卡尔曼滤波器的跟踪对象2关联匹配，为同一对象，观测对象3与卡尔曼滤波器的跟踪对象3关联匹配，为同一对象，后续根据关联匹配的观测对象对应的观测位姿和滤波位姿进行卡尔曼滤波更新。
在一种可选的实施例中，所述根据各个观测对象的第一关联结果及其观测位姿，采用所述卡尔曼滤波器对各个所述观测对象进行位姿更新，得到所述观测对象的估计位姿，包括：
当所述观测对象的关联结果为所述卡尔曼滤波器存在对所述观测对象的位姿跟踪，根据所述观测对象的观测位姿，采用所述卡尔曼滤波器对对应的观测对象进行位姿更新，得到所述观测对象的估计位姿；
当所述观测对象的关联结果为所述卡尔曼滤波器不存在对所述观测对象的位姿跟踪，在所述卡尔曼滤波器中添加所述观测对象，并根据所述观测对象的观测位姿初始化其在所述卡尔曼滤波器中的滤波位姿。
具体的，如图6所示，当观测对象成功关联匹配到卡尔曼滤波器跟踪的对象时，根据观测对象的观测位姿和对应的滤波位姿进行卡尔曼滤波更新；否则，将所述观测对象的观测位姿作为其在卡曼滤波器中的滤波位姿，进行初始化处理并添加该观测对象作为卡尔曼滤波器跟踪的对象。
所述卡尔曼滤波器的工作原理如下：
位姿预测：X′n+1＝A Xn+BU；
协方差预测：P′n+1＝FPnFT+Q；
卡尔曼增益计算：S＝P′n+1H(HP′n+1HT+R)-1；
位姿更新：Xn+1＝X′n+1+S(Zk-H×X′n+1)；
协方差更新：Pn+1＝(1-SH)P′n+1；
其中，X′n+1表示n+1时刻卡尔曼滤波预测更新得到的滤波位姿；Xn表示n时刻卡尔曼滤波测量更新得到的估计位姿；Xn+1表示n+1时刻卡尔曼滤波测量更新得到的估计位姿；A表示状态转移矩阵；B表示控制矩阵；U表示控制状态量；P′n+1表示n+1时刻卡尔曼滤波预测更新得到的协方差预测值；Pn+1表示n+1时刻卡尔曼滤波测量更新得到的协方差；Pn表示n时刻卡尔曼滤波测量更新得到的协方差；Q表示过程协方差；S表示卡尔曼增益；H表示观测矩阵，HT表示H的转置；R表示观测协方差；Zk表示观测到的观测对象(例如车辆、行人)世界坐标系的矩阵；F表示状态转移矩阵雅克比，FT表示F的转置。示例性的，可根据所述观测对象的观测位姿，生成所述观测矩阵H，并通过计算观测位姿的协方差，上述观测协方差R。
初始化卡尔曼滤波器：根据观测对象在多个摄像头下的观测位姿的初始滤波位姿，以及预设的卡尔曼滤波器的初始参数对卡尔曼滤波器的变量和预测更新方程和测量更新方程进行初始化；
卡尔曼滤波器的迭代：每个迭代获取估计位姿Xn+1，和对应的误差协方差Pk+1。每个迭代周期包含预测更新和测量更新；
预测更新：根据上一时刻的估计位姿Xn，用预测更新方程估计当前时刻的滤波位姿X′n+1，以及根据上一时刻的测量误差协方差Pk预测当前时刻的预测状态误差协方差P′n+1。
测量更新：基于观测对象的当前时刻的观测位姿，确定观测矩阵H和观测协方差R，根据测量更新方程更新状态X′n+1→Xn+1和状态误差协方差P′n+1→Pn+1。
通过不断迭代预测更新和测量更新，当前时刻随着时间的流逝，变成上一时刻，重新去估计新的当前时刻。
相对于现有技术，本发明实施例的有益效果在于：借助路侧安装的摄像头捕捉到的图像来对车辆和行人定位，可以降低成本；同时，基于多个摄像头下同一待定位区域的多视角的待定位区域图像，利用云端服务器的高计算能力对待定位区域图像中车辆和行人进行GNN关联、投影优化，可以实现车辆和行人的高精度定位。其次，将车辆和行人的观测位姿与卡尔曼滤波器进行关联跟踪，可以进一步提高定位精度。
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出多台改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (10)

1.一种场端多相机融合定位方法，其特征在于，包括：
获取多个不同位置的摄像头在当前时刻识别出的待定位区域图像；
对多个所述待定位区域图像中的观测对象进行全局最近邻关联，得到同一所述观测对象在不同所述摄像头下的接地点位置和像素坐标；
根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，确定所述观测对象的观测位姿；
利用所述观测对象的观测位姿进行卡尔曼滤波更新，得到所述观测对象的估计位姿。
2.如权利要求1所述的场端多相机融合定位方法，其特征在于，所述根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，确定所述观测对象的观测位姿，包括：
根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，计算所述观测对象的中心位姿；
根据所述观测对象的中心位姿对所述观测对象进行三维投影，得到观测对象的观测位姿。
3.如权利要求2所述的场端多相机融合定位方法，其特征在于，所述根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，计算所述观测对象的中心位姿,包括：
根据所述观测对象在不同所述摄像头下的接地点位置和像素坐标，对所述观测对象进行极线约束，得到所述观测对象在不同所述摄像头下的2D框的中心像素坐标；
其中，以所述观测对象所在的多个所述待定位区域图像的2D框的中心为极线约束的交点；所述2D框为所述观测对象在对应待定位区域图像中的外接矩形框；
根据所述观测对象在不同所述摄像头下的2D框的中心像素坐标，通过三角化求解得到所述观测对象的中心位姿。
4.如权利要求2所述的场端多相机融合定位方法，其特征在于，当所述观测对象为车辆时，所述中心位姿包括所述车辆的中心坐标及其航向角；
则，所述根据所述观测对象的中心位姿对所述观测对象进行三维投影，得到观测对象的观测位姿，包括：
根据所述车辆的中心坐标及其航向角，通过预设的长方体投影模型进行位姿估算，得到所述车辆的观测位姿。
5.如权利要求2所述的场端多相机融合定位方法，其特征在于，当所述观测对象为行人时，所述中心位姿包括所述行人的中心坐标；
则，所述根据所述观测对象的中心位姿对所述观测对象进行三维投影，得到观测对象的观测位姿，包括：
根据所述行人的中心坐标，通过预设的圆柱体投影模型进行位置估算，得到所述行人的观测位姿。
6.如权利要求2所述的场端多相机融合定位方法，其特征在于，所述利用所述观测对象的观测位姿进行卡尔曼滤波更新，得到所述观测对象的估计位姿，包括：
获取卡尔曼滤波器当前时刻输出的各个所述观测对象的滤波位姿；其中，所述观测对象的滤波位姿是所述卡尔曼滤波器根据对应所述观测对象在上一时刻的滤波位姿和对应的运动模型估算得到；
对各个所述观测对象的观测位姿及各个所述观测对象的滤波位姿进行全局最近邻关联，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果；
根据各个观测对象的第一关联结果及其观测位姿，采用所述卡尔曼滤波器对各个所述观测对象进行位姿更新，得到所述观测对象的估计位姿。
7.如权利要求6所述的场端多相机融合定位方法，其特征在于，所述对各个所述观测对象的观测位姿及各个所述观测对象的滤波位姿进行全局最近邻关联，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果，包括：
计算各个所述观测对象的观测位姿和各个所述观测对象的滤波位姿之间的第一欧式距离；
根据计算得到的第一欧式距离，构建第一代价矩阵；
根据所述第一代价矩阵进行全局最近邻关联求解，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果。
8.如权利要求7所述的场端多相机融合定位方法，其特征在于，所述根据所述第一代价矩阵进行全局最近邻关联求解，得到各个观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿的第一关联结果，包括：
将所述第一代价矩阵中超过预设阈值的第一欧式距离更新为第一固定值；
根据更新后的第一代价矩阵进行全局最近邻关联求解，得到各个所述观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿初始的第一关联结果；
对各个所述观测对象的观测位姿与其在所述卡尔曼滤波器中的滤波位姿初始的第一关联结果进行过滤处理，得到最终的第一关联结果；其中，最终的第一关联结果中第一欧式距离均未超过预设阈值。
9.如权利要求6所述的场端多相机融合定位方法，其特征在于，所述根据各个观测对象的第一关联结果及其观测位姿，采用所述卡尔曼滤波器对各个所述观测对象进行位姿更新，得到所述观测对象的估计位姿，包括：
当所述观测对象的关联结果为所述卡尔曼滤波器存在对所述观测对象的位姿跟踪，根据所述观测对象的观测位姿，采用所述卡尔曼滤波器对对应的观测对象进行位姿更新，得到所述观测对象的估计位姿；
当所述观测对象的关联结果为所述卡尔曼滤波器不存在对所述观测对象的位姿跟踪，在所述卡尔曼滤波器中添加所述观测对象，并根据所述观测对象的观测位姿初始化其在所述卡尔曼滤波器中的滤波位姿。
10.如权利要求1所述的场端多相机融合定位方法，其特征在于，所述对多个所述待定位区域图像中的观测对象进行全局最近邻关联，得到同一所述观测对象在不同所述摄像头下的接地点位置和像素坐标，包括：
根据所述待定位区域图像中各个观测对象的像素坐标以及对应摄像头的外参，计算所述待定位区域图像中各个观测对象的接地点位置；
计算任意两个所述待定位区域图像中各个观测对象的像素坐标之间、接地点位置之间的第二欧式距离；
根据计算得到的第二欧式距离，构建第二代价矩阵；
根据所述第二代价矩阵进行全局最近邻关联求解，得到同一所述观测对象在不同所述摄像头下的接地点位置和像素坐标。
CN202111523787.6A 2021-12-14 2021-12-14 一种场端多相机融合定位方法 Pending CN114565669A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111523787.6A CN114565669A (zh) 2021-12-14 2021-12-14 一种场端多相机融合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111523787.6A CN114565669A (zh) 2021-12-14 2021-12-14 一种场端多相机融合定位方法

Publications (1)

Publication Number Publication Date
CN114565669A true CN114565669A (zh) 2022-05-31

Family

ID=81712123

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111523787.6A Pending CN114565669A (zh) 2021-12-14 2021-12-14 一种场端多相机融合定位方法

Country Status (1)

Country Link
CN (1) CN114565669A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115900732A (zh) * 2023-01-05 2023-04-04 武汉大学 基于路侧相机和车载单元的组合导航方法及系统

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103900473A (zh) * 2014-03-31 2014-07-02 浙江大学 一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法
US9098753B1 (en) * 2014-04-25 2015-08-04 Google Inc. Methods and systems for object detection using multiple sensors
US20160162743A1 (en) * 2014-12-05 2016-06-09 Magna Electronics Inc. Vehicle vision system with situational fusion of sensor data
CN109099920A (zh) * 2018-07-20 2018-12-28 重庆长安汽车股份有限公司 基于多传感器关联的传感器目标精确定位方法
CN109490890A (zh) * 2018-11-29 2019-03-19 重庆邮电大学 一种面向智能车的毫米波雷达与单目相机信息融合方法
CN110726990A (zh) * 2019-09-23 2020-01-24 江苏大学 一种基于ds-gnn算法的多传感器融合方法
CN110782492A (zh) * 2019-10-08 2020-02-11 三星（中国）半导体有限公司 位姿跟踪方法及装置
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN111985300A (zh) * 2020-06-29 2020-11-24 魔门塔(苏州)科技有限公司 自动驾驶动态目标定位方法、装置、电子设备及存储介质
CN112150550A (zh) * 2020-09-23 2020-12-29 华人运通(上海)自动驾驶科技有限公司 一种融合定位方法及装置

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103900473A (zh) * 2014-03-31 2014-07-02 浙江大学 一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法
US9098753B1 (en) * 2014-04-25 2015-08-04 Google Inc. Methods and systems for object detection using multiple sensors
US20160162743A1 (en) * 2014-12-05 2016-06-09 Magna Electronics Inc. Vehicle vision system with situational fusion of sensor data
CN109099920A (zh) * 2018-07-20 2018-12-28 重庆长安汽车股份有限公司 基于多传感器关联的传感器目标精确定位方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109490890A (zh) * 2018-11-29 2019-03-19 重庆邮电大学 一种面向智能车的毫米波雷达与单目相机信息融合方法
CN110726990A (zh) * 2019-09-23 2020-01-24 江苏大学 一种基于ds-gnn算法的多传感器融合方法
CN110782492A (zh) * 2019-10-08 2020-02-11 三星（中国）半导体有限公司 位姿跟踪方法及装置
CN111985300A (zh) * 2020-06-29 2020-11-24 魔门塔(苏州)科技有限公司 自动驾驶动态目标定位方法、装置、电子设备及存储介质
CN112150550A (zh) * 2020-09-23 2020-12-29 华人运通(上海)自动驾驶科技有限公司 一种融合定位方法及装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
石慧: "基于多传感器融合的车辆导航系统研究与实现", 中国优秀硕士学位论文全文数据库（电子期刊）, 15 December 2011 (2011-12-15) *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115900732A (zh) * 2023-01-05 2023-04-04 武汉大学 基于路侧相机和车载单元的组合导航方法及系统

Similar Documents

Publication Publication Date Title
CN111862672B (zh) 基于顶视图的停车场车辆自定位及地图构建方法
CN110032949B (zh) 一种基于轻量化卷积神经网络的目标检测与定位方法
US11461912B2 (en) Gaussian mixture models for temporal depth fusion
CN111986506B (zh) 基于多视觉系统的机械式停车位泊车方法
Pfeiffer et al. Efficient representation of traffic scenes by means of dynamic stixels
CN113870343B (zh) 相对位姿标定方法、装置、计算机设备和存储介质
CN111862673B (zh) 基于顶视图的停车场车辆自定位及地图构建方法
Parra et al. Robust visual odometry for vehicle localization in urban environments
Zou et al. Real-time full-stack traffic scene perception for autonomous driving with roadside cameras
Lothe et al. Real-time vehicle global localisation with a single camera in dense urban areas: Exploitation of coarse 3d city models
JP2023021098A (ja) マップ構築方法、装置及び記憶媒体
CN113903011A (zh) 一种适用于室内停车场的语义地图构建及定位方法
CN113095154A (zh) 基于毫米波雷达与单目相机的三维目标检测系统及方法
Lee et al. Two-frame structure from motion using optical flow probability distributions for unmanned air vehicle obstacle avoidance
CN114419098A (zh) 基于视觉变换的运动目标轨迹预测方法及装置
CN115077519A (zh) 基于模板匹配与激光惯导松耦合的定位建图方法和装置
Hara et al. Vehicle localization based on the detection of line segments from multi-camera images
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法
Badino et al. Stereo-based free space computation in complex traffic scenarios
CN114565669A (zh) 一种场端多相机融合定位方法
CN118053299A (zh) 一种基于雷视融合的地下车库盲区显示方法和系统
Goyat et al. Tracking of vehicle trajectory by combining a camera and a laser rangefinder
Barth et al. Vehicle tracking at urban intersections using dense stereo
Pfeiffer et al. Ground truth evaluation of the Stixel representation using laser scanners
CN115830116A (zh) 一种鲁棒视觉里程计方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination