CN114117113B - 多特征点机动车定位方法和装置、电子设备和存储介质 - Google Patents

Abstract

本发明涉及多特征点机动车定位方法和装置、电子设备和存储介质。该方法包括：确定m个推定当前位置，所述m个推定当前位置在预定的范围内，m是大于1的正整数；利用所述相机获得图片；在所述图片中获得n个特征点，并取得各特征点在三维地图中的位置，即各特征点的地图位置，n为大于1的整数；获得匹配距离的步骤，针对所述m个推定当前位置中的各推定当前位置，基于各特征点的特征点向量，获得m个匹配距离，所述特征点向量是从相机到特征点的向量；根据所述m个匹配距离确定匹配度，将匹配度最高的推定当前位置，确定为当前位置。

Description

多特征点机动车定位方法和装置、电子设备和存储介质

技术领域

本发明涉及机动车定位。

背景技术

视觉定位是目前已知硬件成本最低的室内定位方案。目前已经在地下车库定位、地下通道定位等上得到了应用。但是视觉定位有时定位不精确，定位抖动大。

发明内容

本发明鉴于现有技术的以上问题作出，用以解决现有技术中存在的一项或更多项的问题。

根据本发明的一个方面，提供了一种多特征点机动车定位方法，所述机动车安装有相机，所述方法包括以下的步骤：确定m个推定当前位置，所述m个推定当前位置在预定的范围内，m是大于1的正整数；利用所述相机获得图片；在所述图片中获得n个特征点，并取得各特征点在三维地图中的位置，即所述各特征点的地图位置，n为大于1的整数；获得匹配距离的步骤，针对所述m个推定当前位置中的各推定当前位置，基于所述各特征点的特征点向量，获得m个匹配距离，所述特征点向量是从相机到特征点的向量；根据所述m个匹配距离确定匹配度，将匹配度最高的推定当前位置，确定为当前位置。

根据本发明的另一个方面，提供了一种多特征点机动车定位装置，用于机动车，所述机动车安装有相机，所述多特征点机动车定位装置包括：推定当前位置确定单元，确定m个推定当前位置，所述m个推定当前位置在预定的范围内，m是大于1的正整数；图片获取单元，利用所述相机获得图片；特征点获取单元，在所述图片中获得n个特征点，并取得各特征点在三维地图中的位置，即所述各特征点的地图位置，n为大于1的整数；匹配距离获取单元，针对所述m个推定当前位置中的各推定当前位置，基于所述各特征点的特征点向量，获得m个匹配距离，所述特征点向量是从相机到特征点的向量；以及当前位置确定单元，根据所述m个匹配距离确定匹配度，将匹配度最高的推定当前位置，确定为当前位置。

根据本发明的一个方面，提供了一种电子设备，包括：处理器；用于存储所述处理器可执行指令的存储器；其中，所述处理器被配置为执行所述指令，以实现本发明的方法。

根据本发明的一个方面，提供了一种计算机可读存储介质，所述可读存储介质上存储设备控制程序，当所述设备控制程序被处理器执行时，实现本发明所述的方法。

根据本发明的实施方式，能够提供定位精度和减小定位抖动。

附图说明

结合附图可以更好地理解本发明。附图只是示意性的，不是对本发明的保护范围的限制。

图1示出了依据本发明的一种实施方式的多特征点动车定位方法的示意性流程图。

图2示出了依据本发明的一种实施方式的获得推定当前位置的一种示意性方法。

图3是用于解释特征点向量的示意图。

图4示出了依据一种实施方式如何确定一个特征点到其特征点向量的距离的示意图。

图5示出了依据本发明的另一实施方式的多特征点机动车定位方法的示意性流程图。

图6示出了依据本发明的一种实施方式的多特征点机动车定位装置的示意性方框图。

具体实施方式

下面结合附图对本发明的具体实施方式进行说明。这些说明都是示例性的，旨在使本领域技术人员能够实现本发明的实施方式，不是对本发明的保护范围的限制。说明中也没有描述对于实际实施不可缺少，但是对于理解本发明无关的内容。

图1是示出了依据本发明的一种实施方式的多特征点机动车定位方法的示意性流程图。本发明的方法所涉及的机动车具有一个或多个相机，各相机能够拍摄周围环境的图像。

如图1所示，依据本发明的一种实施方式的机动车定位方法，首先在步骤S100，确定m个推定当前位置，m是大于1的正整数。所述多个推定当前位置在预定的范围内。根据一种实施方式，100<m<1000。

该m个推定的当前位置可以基于已知的初始位置，也可以基于上一周期的位置而利用EKF算法（扩展卡尔曼滤波算法）结合车辆的运动信息（ABS信号和惯导）推算出的运动轨迹位置。在启动时，不知道车辆的具体位置，但是知道车辆的大概位置，这个大概的位置即初始位置。这个大概的位置的范围就是预定的范围的示例。依据一种实施方式，启动时的车辆的停靠位置（例如车位）是已知的，并且已知该位置在地图坐标系中的坐标，可以利用这个坐标来确定该初始位置（例如图2中的作为起始点的车辆推定当前位置）。

这个初始的位置（例如人工随意停在这个位置的时候）会有一定误差。该位置误差例如大概在1-2米，航向误差大概5度。在运动时，利用EKF算法（扩展卡尔曼滤波算法）结合车辆的运动信息（ABS信号和惯导）去推算。推算出的位置也有一定的误差，该误差的范围可以根据EKF的协方差和分差来生成。根据一种实施方式，x y方向上的方差和协方差可以计算出误差椭圆。基于该初始位置或运动轨迹位置在初始位置或运动轨迹位置的误差范围内，随机地或依据一定的函数关系，生产多个推定当前位置。由于推定当前位置有多个，均在误差的范围内，因而这些推定当前位置中会包含真实的当前位置。各推定当前位置具有平面坐标和航向。这些推算出的位置及其推算误差的范围也是预定的范围的示例。

图2是示出了依据本发明的一种实施方式的获得推定当前位置的方法。如图2所示，根据本发明的一种实施方式，首先，人为设定一个起始点（A点），该起始点所在的位置范围是已知的。起始点位置包括车辆的位置和航向。如上所述，这个起始点例如是车辆启动时的车位位置等，只知道该起始点（作为起始点的推定当前位置）的位置范围，而无需知道其精确范围。但是本发明并不排除知道其具体位置的情况。

然后，在该起始点周围一定的范围内随机生成虚拟位置（推定位置），这些虚拟位置包括车辆的位置和航向。本领域的技术人员容易理解，这里虽然说是虚拟，但是由于假定的起始位置并不精确，因而这些虚拟的、推定的位置也有可能才是最真实的位置。可以将起始点位置和推定位置这些车辆位置都称为A点位置。例如预期当在初始位置的位置误差在1-2米，航向误差在5度的情况下，这里推定位置的点云的范围可以是该推定的当前位置周围2-2.5米范围，航向10度范围。或者在非初始位置，推定位置的点云的范围是误差椭圆2倍的范围。

然后，如图2所示，在一定的时间间隔之后，到达了B点，根据EKF算法或其他的算法，获知这些车辆位置（A点位置）到达B点时的位置（B点位置，既车辆运动后的位置），将这些位置设定为推定的当前位置。

然后，在步骤S200，利用所述相机获得一张图片。根据一种实施方式，EKF在启动的时候需要一个位置和航向来确定位置和航向，接下来每次收到车辆纵向（ABS脉冲信号或者车速）或转向运动信息（陀螺仪产生的Z轴角速度），会根据上一时刻的位置和航向推算车辆位置和航向。即运动跟踪装置（例如EFK计算单元）每次收到车辆纵向运动信息或转向运动信息时，相机进行拍照，从而获得图片。根据另一种实施方式，周期性的进行位置和航向的修正。一般周期为1秒1-2次。也就是在低速场景下，车辆每行驶1-3米，就进行一次更新。可以在更新时进行拍照，从而获得照片。

接着在步骤S300，在所述图片中获得多个特征点，并取得各特征点在三维地图中的位置，即特征点的地图位置。特征点是指相片中能够识别、并且在地图中具有坐标的点。可以预先在数据库中保存特征点的特征，进行对比识别。

根据一种实施方式，利用距离图片中心在预定距离范围内的像素选取特征点。根据一种实施方式，所述特征点多于三个，而且任何特征点不在其他任何两个特征点的连线或连线的延长线上。根据一种实施方式，特征点的大小大于预定值。例如，如果特征点是矩形，其宽度大于预定长度。利用这样的方式确定特征点，可以增加定位的准确性。

可以利用机器视觉的方法来识别特征点。可以人为地设置特征点，例如在墙壁或柱子的特定位置设置特定的图案或物品等。根据一种实施方式，利用环境信息识别照片中的特征点。例如在厂房的情况下，可以将设定的特征点全部按照固定的间距设置，可以利用这些环境信息，迅速在图片中确定特征点。

总之，可以利用现在已知或未来知悉的各种方法来识别特征点。

根据一种实施方式，依据推定当前位置的位置和航向，将车辆放在三维地图中，再根据车辆在三维地图中的位置和姿态，计算车辆能看到哪些三维图像中的特征点，从而可以获得各特征点在三维地图中的位置。

之后，针对第一个推定当前位置（I=1），进行步骤S400到S600的处理。

在步骤S400，针对各特征点，分别获取与之对应的特征点向量，从而得到n个特征点向量，该特征点向量是从相机（例如透镜焦点）到特征点的向量。

由于各推定当前位置具有航向，而各相机相对于该机动车的位置和方向都是已知的，因而可以获得相机的位置和朝向等信息。获得了该机动车在各推定当前位置时的相机位置和朝向，就可以很容易获得特征点向量。图3是用于解释特征点向量的示意图。图中的特征产生的向量即特征点向量。

然后，在步骤S500，计算各特征点的地图位置到基于该特征点的特征点向量的距离，从而可以获得n个距离。图4示意性地示出了如何确定一个特征点到其特征点向量的距离。

本领域技术人员应该理解，尽管理论上特征点的地图位置和相机成像空间的位置应该是一致的，但是由于地图会有误差、相机的各种参数（包括相机的畸变参数、x轴焦距、y轴焦距、安装位置、横摆角、俯仰角、自旋角等）也有误差、车身的倾斜等，这两个位置也有可能会不重合。由于特征点向量是相机到相机成像空间中的特征点的连线，因而特征点的地图位置与特征点向量之间会产生距离。对于重合的情况，距离为0。不影响本发明的实施。

随后，在步骤S600，根据该n个距离，确定该推定当前位置的匹配距离。该匹配距离例如可以是取这n个距离的平均值或和值。

针对m个推定当前位置中剩余的推定当前位置，重复以上的步骤S400-S600，从而获得m个匹配距离。

然后，当完成了所有每个推定当前位置的匹配距离的计算时（

的判断结果为“是”），在步骤S700，根据该m个匹配距离确定匹配度，将匹配度最高的推定当前位置，确定为当前位置。在该匹配距离是取上述n个距离的平均值时，匹配距离越短，则匹配度越高。

本领域的技术人员应该理解，在步骤S100确定m个推定当前位置时，可以是一次性生成全部m个推定当前位置，也可以是生成一个就进行步骤S400-S600的处理，然后在生成第二个，如此类推。这些都在本发明的这一步骤S100的范围内。

依据本发明的实施方式，不需要具有长度的特征物，降低了识别难度。无需使用多个相机，降低了成本。根据本发明的实施方式，也并不需要将二维平面的投影，简化了运算难度，可以提高运算速度。

根据本发明的一种实施方式，在步骤S400中，还包括进行特征向量筛选的步骤，剔除掉俯仰角低于预定阈值的特征点。即去除俯仰角低于预定值的特征向量。特征向量的俯仰角是指该特征点向量相对于所述机动车的水平位置平面的俯仰角。根据这一技术方案，能够剔除掉运动的特征（其他车辆）而保留固定的特征（建筑物），从而能够提高运算速度，增加定位精度。

进一步，可以将匹配度最优的推定当前位置作为起始位置，在该起始位置周围一定的范围内随机生成虚拟位置，就如结合图2所说明的那样，从而这些虚拟位置可以作为推定当前位置进行下一时刻的位置确定和轨迹生成。根据一种实施方式，可以将匹配度较差（低于预定阈值）最差的预定范围的推定当前位置去除，而保留的未去除的推定当前位置可以等到下一个相机匹配阶段开始时，根据车辆运动轨迹推断下一时刻的位置。如上所述，轨迹可以来自于ABS信号和惯导信号。

这样可以减少虚拟位置的生成数量，提高运算速度。去除时，例如去除匹配度低于平均匹配度的所有推定当前位置。

根据一种实施方式，相机有多个，假定为k个，k为大于1的正整数。在这种情况下，针对各推定当前位置，在步骤S400得到各特征点对应的特征点向量，进而在步骤S500，得到各特征点到其特征点向量的距离，从而可以得到总计k*n距离。在步骤S600，根据这k*n个距离来计算各推定当前位置的匹配距离。在一种实施方式中，各推定当前位置的匹配距离是这k*n个距离的平均值或和值。

图5示出了依据本发明的另一实施方式的多特征点机动车定位方法的示意性流程图。如图5所示，根据一种实施方式，在步骤S200（利用所述相机获得图片的步骤）在多个位置分别获得图片。该多个位置应在EKF方法进行运动轨迹跟踪的误差大于预定值的范围之内。EKF方法进行轨迹跟踪，随着运动的持续会产生误差。依据本发明的实施方式，可以依据特征点的数目和要求的定位精度之间的平衡，来确定应该进行拍照的位置的数量。然后根据EKF方法轨迹跟踪误差的形成曲线，选取各位置（拍照位置）。

所述方法还包括步骤S800。在该步骤S800，针对所述m个推定当前位置中的各推定当前位置，确定各推定当前位置的对应运动轨迹。

根据一种实施方式，利用EKF算法（扩展卡尔曼滤波算法）结合车辆的运动信息（ABS信号和惯导）推算车辆的运动轨迹位置，从而得到各推定当前位置的对应运动轨迹，并获得该运动轨迹上的多个位置。假定第一推定当前位置的车辆在第一时刻运动到第二时刻，位于C位置，对应地，第二推定当前位置的车辆在第一时刻运动到第二时刻，其处于D位置。这里C位置和D位置是有差别的，第一推定位置到C位置的轨迹可被称为第一推定位置的对应运动轨迹，第二推定位置到D位置的轨迹可被称为基于第二推定位置的对应运动轨迹。第一推定位置和C位置可以被称为第一推定位置的对应运动轨迹的位置点，第二推定位置和D位置可以被称为第二推定位置的对应运动轨迹的位置点。

针对各推定当前位置，对于其对应运动轨迹中的各位置（图中假定有p个，从第一个（a=1）开始），在所述多个中的各位置分别获得一个匹配距离，从而获得多个匹配距离。在此过程中，在S400获得特征点向量时，依据该推定当前位置的对应运动轨迹，能够依据各推定当前位置最开始时的位置而得到各拍照位置时，车辆基于该推定当前位置的相机位置，从而能够得到在各拍照位置，该推定当前位置运动轨迹上的对应的位置点出的各特征点向量。

在获得这多个匹配距离之后（a=p），在步骤S900，确定各推定当前位置的最终匹配距离。可以对所述多个匹配距离进行加权平均，从而确定各推定当前位置的最终匹配距离。根据一种实施方式，在各推定当前位置的对应运动轨迹的各位置点，均利用上述步骤取得其匹配距离。在对应轨迹上获得了p个位置点的情况下，则获得p个匹配距离。可以对匹配距离进行加权计算，最后的位置获得匹配距离给予最高的权重，从而获得最终匹配距离。在步骤S700利用该最终匹配距离确定机动车当前位置。

在进行加权计算时，可以结合轨迹误差确定各位置点所获得匹配距离的权值。

应该理解，针对以上的实施方式，也可以进行特征向量的剔除和筛选。特征向量的剔除和筛选也可以在步骤S600进行。

图6示出了依据本发明的一种实施方式的多特征点机动车定位装置的示意性方框图。

依据本发明的一种实施方式的多特征点机动车定位装置用于机动车，该机动车安装有相机。如图6所示，所述多特征点机动车定位装置包括推定当前位置确定单元100、图片获取单元200、特征点获取单元300、匹配距离获取单元400、当前位置确定单元500。

推定当前位置确定单元100确定m个推定当前位置，所述多个推定当前位置在预定的范围内，m是大于1的正整数。图片获取单元200利用机动车的相机获得图片。特征点获取单元300在所述图片中获得n个特征点，并取得各特征点在三维地图中的位置，即各特征点的地图位置，n为大于1的整数。匹配距离获取单元400针对所述m个推定当前位置中的各推定当前位置，基于各特征点的特征点向量，获得m个匹配距离，所述特征点向量是从相机到特征点的向量。当前位置确定单元500根据所述m个匹配距离确定匹配度，将匹配度最高的推定当前位置，确定为当前位置。

根据一种实施方式，匹配距离获取单元400针对所述m个推定当前位置中的各推定当前位置，如下地获得各推定当前位置的匹配距离：

针对所述各特征点，分别获取基于各特征点的特征点向量，从而得到n个特征点向量；

计算所述各特征点的地图位置到其对应的特征点向量的距离，从而获得多个距离；

根据该多个距离，确定该推定当前位置的匹配距离。

进一步，匹配距离获取单元400在取得各特征点的特征点向量之后，去除俯仰角低于预定值的特征向量，从而不计算该所述特征点的地图位置到其对应的特征点向量的距离或者在确定该推定当前位置的匹配距离时，不考虑该距离。

根据一种实施方式，匹配距离是取所述多个距离的平均值或和值。

m可以大于等于100小于等于1000。

根据一种实施方式，所述推定当前位置确定单元100如下地确定该m个推定当前位置：

设定起始点位置，所述起始点位置所在的位置范围是已知的；

在该起始点位置周围一定的范围内随机生成多个虚拟位置；

在机动车运动时，利用EKF算法推定运动后的所述起始点位置和所述多个虚拟位置；

将推定出的运动后的所述起始点位置和所述多个虚拟位置的当前位置设定为所述推定当前位置。

根据一种实施方式，所述相机有k个，k为大于1的正整数，其中，图片获得单元200利用各相机分别获得一张图片；针对各推定当前位置，匹配距离获取单元400分别在各图片中获得多个特征点，并取得各特征点的地图位置，得到各特征点对应的特征点向量；得到各特征点到其特征点向量的距离，从而可以得到总计k*n个距离，根据这k*n个距离来计算该推定当前位置的匹配距离。

根据一种实施方式，图片获得单元200利用相机在多个位置分别获得图片。所述装置还包括轨迹获取单元600，其针对所述m个推定当前位置中的各推定当前位置，确定各推定当前位置的对应运动轨迹。

匹配距离获取单元400针对各推定当前位置，在所述多个位置中的各位置分别获得一个匹配距离，从而获得多个匹配距离，对所述多个匹配距离进行加权平均，确定各推定当前位置的最终匹配距离。

本领域技术人员容易理解，可以利用以上对方法的说明来理解本发明的装置。本领域技术人员容易理解，可以利用以上对装置的说明来理解本发明的方法步骤。

本领域技术人员容易理解，本发明的方法还可以包括与本发明的装置所完成的功能对应的其他步骤。以上的这些步骤也可以进行精简。

本发明对单元和步骤的标号仅仅是为了说明的方便，除非上下文中有相反的说明，否则不代表其执行的顺序。

本领域的技术人员应该理解，上述的各单元可以由软件或专门的硬件来实现，例如现场可编程门阵列、单片机、或微芯片等，或者也可以通过软件结合硬件的方式来实现。

本发明还提供了一种电子设备，包括：处理器；用于存储所述处理器可执行指令的存储器；其中，所述处理器被配置为执行所述指令，以实现本发明的方法。

本发明还涉及一种计算机软件，该计算机软件在被计算装置（例如单片机、电脑、CPU等）执行时，可以实现本发明的方法。

本发明还涉及一种计算机软件存储装置，例如硬盘、软盘、闪存等，该计算机软件存储装置存储有上述的计算机软件。

本发明对方法或步骤的说明可以用于理解对单元或装置的说明，对单元或装置的说明也可以用于理解本发明的方法或步骤。

以上说明仅仅是示意性的，不是对本发明的保护范围的限制，在本发明的权利要求范围内的任何改变、替换均在本发明的保护范围内。

Claims (9)

1.一种多特征点机动车定位方法，所述机动车安装有相机，其特征在于，所述方法包括以下的步骤：

确定m个推定当前位置，所述m个推定当前位置在预定的范围内，m是大于1的正整数；

利用所述相机获得图片；

在所述图片中获得n个特征点，并取得各特征点在三维地图中的位置，即所述各特征点的地图位置，n为大于1的整数；

获得匹配距离的步骤，针对所述m个推定当前位置中的各推定当前位置，基于所述各特征点的特征点向量，获得m个匹配距离，所述特征点向量是从相机到特征点的向量；

根据所述m个匹配距离确定匹配度，将匹配度最高的推定当前位置，确定为当前位置，

其中，所述确定m个推定当前位置的步骤包括：

设定起始点位置，所述起始点位置所在的位置范围是已知的；

在该起始点位置周围一定的范围内随机生成多个虚拟位置；

在机动车运动时，利用扩展卡尔曼滤波算法推定运动后的所述起始点位置和所述多个虚拟位置；

将推定出的运动后的所述起始点位置和所述多个虚拟位置的当前位置设定为所述推定当前位置。

2.根据权利要求1所述的方法，其特征在于，针对所述m个推定当前位置中的各推定当前位置，如下地获得各推定当前位置的匹配距离：

针对所述各特征点，分别获取基于各特征点的特征点向量，从而得到n个特征点向量；

计算所述各特征点的地图位置到其对应的特征点向量的距离，从而获得多个距离；

根据该多个距离，确定该推定当前位置的匹配距离。

3.根据权利要求2所述的方法，其特征在于，所述方法还包括特征筛选的步骤，去除俯仰角低于预定值的特征向量；其中，所述匹配距离是取所述多个距离的平均值或和值，其中m大于等于100小于等于1000。

4.根据权利要求2所述的方法，其特征在于，在所述图片中获得n个特征点的步骤中，利用距离图片中心在预定距离范围内的像素选取特征点；

其中，所述特征点多于三个，而且任何特征点不在其他任何两个特征点的连线或连线的延长线上；

其中，所述各特征点的大小大于预定值。

5.根据权利要求1所述的方法，其特征在于，利用所述相机获得图片的步骤在多个位置分别获得图片，

所述方法还包括针对所述m个推定当前位置中的各推定当前位置，确定各推定当前位置的对应运动轨迹，

在所述获得匹配距离的步骤，针对各推定当前位置，利用所述运动轨迹，在所述多个位置中的各位置分别获得一个匹配距离，从而获得多个匹配距离，对所述多个匹配距离进行加权平均，确定各推定当前位置的最终匹配距离。

6.一种多特征点机动车定位装置，用于机动车，所述机动车安装有相机，其特征在于，所述多特征点机动车定位装置包括：

推定当前位置确定单元，确定m个推定当前位置，所述m 个推定当前位置在预定的范围内，m是大于1的正整数；

图片获取单元，利用所述相机获得图片；

特征点获取单元，在所述图片中获得n个特征点，并取得各特征点在三维地图中的位置，即所述各特征点的地图位置，n为大于1的整数；

匹配距离获取单元，针对所述m个推定当前位置中的各推定当前位置，基于所述各特征点的特征点向量，获得m个匹配距离，所述特征点向量是从相机到特征点的向量；以及

当前位置确定单元，根据所述m个匹配距离确定匹配度，将匹配度最高的推定当前位置，确定为当前位置，

其中，所述推定当前位置确定单元如下地确定该m个推定当前位置：

设定起始点位置，所述起始点位置所在的位置范围是已知的；

在该起始点位置周围一定的范围内随机生成多个虚拟位置；

在机动车运动时，利用扩展卡尔曼滤波算法推定运动后的所述起始点位置和所述多个虚拟位置；

将推定出的运动后的所述起始点位置和所述多个虚拟位置的当前位置设定为所述推定当前位置。

7.根据权利要求6所述的多特征点机动车定位装置，其特征在于，所述匹配距离获取单元针对所述m个推定当前位置中的各推定当前位置，如下地获得各推定当前位置的匹配距离：

针对所述各特征点，分别获取基于所述各特征点的特征点向量，从而得到n个特征点向量；

计算所述各特征点的地图位置到其对应的特征点向量的距离，从而获得多个距离；

根据该多个距离，确定该推定当前位置的匹配距离。

8.一种电子设备，其特征在于，包括：

处理器；

用于存储所述处理器可执行指令的存储器；

其中，所述处理器被配置为执行所述指令，以实现如权利要求1至5中任一项所述的方法。

9.一种计算机可读存储介质，其特征在于，所述可读存储介质上存储设备控制程序，当所述设备控制程序被处理器执行时，实现如权利要求1至5中任一项所述的方法。

Images