CN108759834A - 一种基于全局视觉的定位方法 - Google Patents

Abstract

本发明提供一种基于视觉的定位方法，(1)获得摄像头的精确位置；(2)获得摄像头的姿态；(3)对目标成像：整个系统投入运营，对目标成像；(4)在图像中检测目标；5)计算目标射线；(6)计算目标位置；7)计算目标姿态：根据目标在图像坐标里的姿态，以及摄像头的姿态，采取视觉和IMU、OD、Geomagnetic的信息融合组合导航可以确定目标的姿态。本发明的有益效果是已经知道摄像头的位置、朝向，以及所面向地理环境的模型，就可以很容易地计算出视野范围内每个目标的位置；将视觉与GPS、IMU、OD及地磁等定位装置配合，可得到高精度的导航定位。

Description

一种基于全局视觉的定位方法

技术领域

本发明属于定位技术领域，尤其是涉及一种基于全局视觉的定位方法。

背景技术

定位是导航的前提条件，在工业、养老、医疗、会展、自动化等领域有广泛的应用。但是目前的定位技术在应用中都有短板，例如GPS容易受遮挡，在室内无法使用，在山区、树林中精度低；Wi-Fi精度低，不能穿墙；蓝牙稳定性稍差，受噪声信号干扰大；ZigBee需要密集布置信源；RFID作用距离短，一般最长为几十米，不便于整合到移动设备之中。IMU、OD可以高频测量加速度、速度和姿态角，但是受噪声影响大，长时间会有累积误差。

但是作为智能城市项目的监控摄像头，已经密集地分布在各个关键位置。如果已经知道摄像头的位置、朝向，以及所面向的地理环境，就可以很容易地计算出视野范围内的每个目标的位置。如果与GPS、IMU、OD及地磁等定位装置配合，就可以提高定位精度。

发明内容

本发明的目的是提供一种基于全局视觉的定位方法，其解决了平常定位方法中存在的定位不准确、易受干扰、安装成本高等不足，提高了定位精度，适合用在工业、自动化、医疗、会展、养老及酒店等领域的定位导航中。

本发明的技术方案是：一种基于全局视觉的定位方法，包括如下步骤：

设已知摄像头的精确位置(经度O，纬度A，高度H)和姿态(αc，βc，γc)，并且已知场地的几何模型，那么从摄像头的图像中找到目标后，就能根据目标在图像中的位置确定目标与摄像头连线的方位角(αO，βO，γO)，并根据此方位角和场地的几何模型计算出目标的位置和姿态。

(1)获得摄像头的精确位置：如果在野外，采用高精度的差分GPS装置；如果在室内，用高精度的差分GPS装置定位整个建筑物，再根据建筑物内部结构的尺寸推算摄像头的精确位置。将世界坐标系原点设置在摄像头焦距处，一个方向指向经度O方向(东方)，另一个方向指向纬度A方向(北方)，第三个方向指向高度H方向；

(2)获得摄像头的姿态：采用带水平仪和罗盘的标定模板标定相机；水平放置的标定板一个方向指向经度O方向(东方)，一个方向指向纬度A方向(北方)，与摄像头处的世界坐标系一致；设标定后，摄像头坐标系与世界坐标系的变换为R1|T，从旋转矩阵R1可以按如下公式确定出摄像头的三个姿态角(αc，βc，γc)，

(3)对目标成像：整个系统投入运营，对目标成像；

(4)在图像中检测目标：可以用目标检测的方法，也可以在目标上带预设标签的方法，确定目标在图像中的位置，目标大小为λ，相对于图像中心的偏移(r,c)，目标在图像坐标里的姿态θ；

(5)计算目标射线：因为是单目视角，无法确定目标的高度和远近；但是对于具体应用而言，目标往往在地面上，并且目标往往是某种确定的类型，例如人、车、AGV等，因此大小、高度固定。在图像中找到目标后，根据目标相对图像中心的偏移(r,c)，在修正相机变形后，可以确定目标与摄像头光轴的偏转角

并计算出目标相对于相机坐标的旋转矩阵R2，因此，可确定目标射线在世界坐标系里的角度(αO，βO，γO)；

(6)计算目标位置：已知目标射线后，有两种方式可以确定目标位置：(a)如果已知场地的几何模型：如果地面不是水平的，将场地的几何模型S向上平移目标高度后，此三维曲面与目标射线的交点就是目标位置；如果地面是水平的，不需要场地的几何模型，经过相交计算后，就可以确定目标位置；(b)根据目标大小：根据目标在图像中的大小λ，估算目标与相机的距离，从而确定目标的位置坐标。

(7)计算目标姿态：根据目标在图像坐标里的姿态θ，以及摄像头的姿态，采取视觉和IMU、OD、Geomagnetic的信息融合组合导航可以确定目标的姿态。

一种用于全局视觉定位方法中的信息融合组合导航方法，具体如下：

1)在建立系统误差方程的基础上，将位置误差方程、姿态误差方程以及惯性仪表误差方程综合在一起作为组合导航卡尔曼滤波器的观察值，可以写出INS系统误差状态方程的一般表达式如下

X(k)＝F(k-1)X(k-1)+G(k-1)W(k-1)

其中，状态变量X＝[δO,δA,δH,φ_N,φ_E,φ_D,ε_rx,ε_ry,ε_rz,△_x,△_y,△_z]，δO、δA、δH为纬度、经度和高度误差，φ_N、φ_E、φ_D为平台误差角，ε_rx、ε_ry、ε_rz为陀螺仪一阶马尔柯夫漂移，△_x、△_y、△_z为加速度计一阶马尔柯夫漂移。F为状态转换阵，G为噪声转换阵，W为系统噪声。

2)用视觉测量值和IMU、OD、Geomagnetic融合信息值的差值作为量测值，观测方程为

Z(k)＝H(k)X(k)+V(k)

其中，Z＝[δO,δA,δH,φ_N,φ_E,φ_D]^T，H为观测矩阵，V为测量噪声矩阵。

3)建立系统的状态方程和观测方程后，就可以进行卡尔曼滤波。其中状态噪声阵Q根据IMU、OD、Geomagnetic信息融合后的有关参数选取，观测噪声矩阵R根据视觉测量的性能选取。

4)经由Kalman滤波估计出惯导系统的误差，然后对惯导系统进行校正。

本发明具有的优点和积极效果是：由于采用上述技术方案，已经知道摄像头的位置、朝向，以及所面向地理环境的模型，就可以很容易地计算出视野范围内每个目标的位置；将视觉与GPS、IMU、OD及地磁等定位装置配合，可得到高精度的导航定位。

附图说明

图1是系统布局。

图2是本发明的视觉定位处理流程。

图3是本发明的视觉定位处理原理。

图4是摄像头位置、姿态及成像平面坐标。

图5是从摄像头出发的目标射线。

图6是从摄像头和目标射线计算目标位置。

图7是视觉和IMU、OD、Geomagnetic的信息融合组合导航处理流程。

图8是Kalman滤波校正惯导系统。

图9是实施例1的基于全局视觉的室内定位技术的应用示意图。

图10是实施例2的基于全局视觉的扫地机器人的应用示意图。

图中：

1、A摄像头 2、A立杆 3、摄像头A视野范围

4、B摄像头 5、B立杆 6、摄像头B视野范围

7、目标 8、C摄像头 9、摄像头C视野范围

10、C立杆

具体实施方式

如图1所示，沿道路布置有A摄像头1、B摄像头4、C摄像头8，A摄像头1、B摄像头4、C摄像头8分别设在道路上的A立杆2、B立杆5和C立杆10上，摄像头的视野范围分别为摄像头A视野范围3、摄像头B视野范围6、摄像头C视野范围9。摄像头的视野并没有全覆盖整个道路。目标7小车行驶在道路上，小车可能处于0、1、2个摄像头的视野范围内。如果小车处于0个摄像头的视野范围内时，小车依靠IMU、OD、Geomagnetic导航；如果小车处于1、2个摄像头的视野范围内时，小车依靠视觉和IMU、OD、Geomagnetic的融合信息导航。

一种基于全局视觉的定位方法，包括如下步骤：

如图2、3所示，设已知摄像头的精确位置(经度O，纬度A，高度H)和姿态(αc，βc，γc)，并且已知场地的几何模型，那么从摄像头的图像中找到目标后，就能根据目标在图像中的位置确定目标与摄像头连线的方位角(αO，βO，γO)，并根据此方位角和场地的几何模型计算出目标的位置和姿态。具体步骤如下：

(1)获得摄像头的精确位置：如果在野外，采用高精度的差分GPS装置；如果在室内，用高精度的差分GPS装置定位整个建筑物，再根据建筑物内部结构的尺寸推算摄像头的精确位置。将世界坐标系原点设置在摄像头焦距处，一个方向指向经度O方向(东方)，另一个方向指向纬度A方向(北方)，第三个方向指向高度H方向；

(2)获得摄像头的姿态：采用带水平仪和罗盘的标定模板标定相机；水平放置的标定板一个方向指向经度O方向(东方)，一个方向指向纬度A方向(北方)，与摄像头处的世界坐标系一致；设标定后，摄像头坐标系与世界坐标系的变换为R1|T，从旋转矩阵R1可以按如下公式确定出摄像头的三个姿态角(αc，βc，γc)，如图4所示；

(3)对目标成像：整个系统投入运营，对目标成像；

(4)在图像中检测目标：可以用目标检测的方法，也可以在目标上带预设标签的方法，确定目标在图像中的位置，目标大小为λ，相对于图像中心的偏移(r,c)，目标在图像坐标里的姿态θ；

(5)计算目标射线：因为是单目视角，无法确定目标的高度和远近；但是对于具体应用而言，目标往往在地面上，并且目标往往是某种确定的类型，例如人、车、AGV等，因此大小、高度固定。在图像中找到目标后，根据目标相对图像中心的偏移(r,c)，在修正相机变形后，可以确定目标与摄像头光轴的偏转角如图5所示；

并计算出目标相对于相机坐标的旋转矩阵R2，因此，可确定目标射线在世界坐标系里的角度(αO，βO，γO)；

(6)计算目标位置：已知目标射线后，有两种方式可以确定目标位置：(a)如果已知场地的几何模型：如果地面不是水平的，将场地的几何模型S向上平移目标高度后，此三维曲面与目标射线的交点就是目标位置；如果地面是水平的，不需要场地的几何模型，经过相交计算后，就可以确定目标位置；(b)根据目标大小：根据目标在图像中的大小λ，估算目标与相机的距离，从而确定目标的位置坐标，如图6所示；

(7)计算目标姿态：根据目标在图像坐标里的姿态θ，以及摄像头的姿态，采取视觉和IMU、OD、Geomagnetic的信息融合组合导航可以确定目标的姿态。

如图7所示，一种用于全局视觉定位方法中的信息融合组合导航方法，具体如下：

其中视觉、IMU、OD、Geomagnetic是AGV小车电里的几种常见传感器，可以确定目标的位置和姿态。但是这些传感器，每种都有缺陷，因此要采用信息融合的方式，综合几种传感器的信息，得到相对较为准确的位置和姿态。目前，已经有GPS和IMU、OD、Geomagnetic组合导航的方法，但是还没有视觉和IMU、OD、Geomagnetic组合导航的方法。

IMU、OD、Geomagnetic信息融合已有现成的方法，此处不再赘述。

1)在建立系统误差方程的基础上，将位置误差方程、姿态误差方程以及惯性仪表误差方程综合在一起作为组合导航卡尔曼滤波器的观察值，可以写出INS系统误差状态方程的一般表达式如下

X(k)＝F(k-1)X(k-1)+G(k-1)W(k-1)

其中，状态变量X＝[δO,δA,δH,φN,φE,φD,εrx,εry,εrz,△x,△y,△z]，δO、δA、δH为纬度、经度和高度误差，φN、φE、φD为平台误差角，εrx、εry、εrz为陀螺仪一阶马尔柯夫漂移，△x、△y、△z为加速度计一阶马尔柯夫漂移。F为状态转换阵，G为噪声转换阵，W为系统噪声。

2)用视觉测量值和IMU、OD、Geomagnetic融合信息值的差值作为量测值，观测方程为

Z(k)＝H(k)X(k)+V(k)

其中，Z＝[δO,δA,δH,φN,φE,φD]T，H为观测矩阵，V为测量噪声矩阵。

3)建立系统的状态方程和观测方程后，就可以进行卡尔曼滤波。其中状态噪声阵Q根据IMU、OD、Geomagnetic信息融合后的有关参数选取，观测噪声矩阵R根据视觉测量的性能选取，如图8所示；

4)经由Kalman滤波估计出惯导系统的误差，然后对惯导系统进行校正。

实施例1基于全局视觉的室内定位技术

采取本发明的全局视觉的定位方法，应用在室内定位技术中。如图9所示，室内定位有着重要的价值，但是目前的技术水平已经成为阻碍应用的瓶颈。如果采用全局视觉，目标发出可视化的定位请求信号，室内定位系统向目标提供精确的位置信息服务，解决目前的室内定位难题。

全局视觉：是指俯视的、能够看到大幅度范围的摄像头。

可视化的定位请求信号：摄像头能够检测到的可视化信号，例如闪烁的灯光。作用：(1)告诉摄像头检测目标的位置；(2)告诉摄像头检测目标是谁；(3)同步摄像头和目标上的时间。

步骤：

(1)目标发出可视化的定位请求信号；

(2)检测目标位置、姿态；

(3)识别目标；

(4)摄像头与目标建立无线通讯链路；

(5)摄像头通过无线通讯链路通知目标位置、姿态。

实施例2基于全局视觉的扫地机器人

采取本发明的全局视觉的定位方法，应用在扫地机器人中。如图10所示，由于没有对整个环境的认知，扫地机器人无法建立优化的巡航策略；更重要的是，没有对扫地效果的反馈，扫地机器人无法知道哪些地方需要打扫、哪些地方不需要打扫。即使有能力对环境建模的扫地机器人，也无法对整个环境建立准确的模型，尤其是动态变化的环境。

全局视觉是指俯视的、能够看到大幅度范围的摄像头。这种摄像头有两个作用：(1)对整个环境建立准确的模型，以方便扫地机器人巡航；(2)能够检测出哪儿脏，哪儿需要打扫，向扫地机器人布置清理任务；(3)检测扫地机器人的打扫效果，调整扫地机器人的打扫参数，提高清理效果。但是全局摄像头只能从上向下看，看不到遮挡的地方。

因此，基于全局视觉的扫地机器人，能够对整个环境建立完整模型，也能够通过扫地机器人的激光传感器建立行走平面的局部模型，尤其是全局摄像头被遮挡地方的局部模型。而更为重要的是，通过全局视觉，摄像头可以通过无线通讯通知扫地机器人哪儿打扫、哪儿不需要打扫，向扫地机器人布置清理任务，以及检测扫地机器人的打扫效果，调整扫地机器人的打扫参数，提高清理效果。

以上对本发明的一个实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明申请范围所作的均等变化与改进等，均应仍归属于本发明的专利涵盖范围之内。

Claims (7)

1.一种基于视觉的定位方法，其特征在于：

(1)获得摄像头的精确位置；

(2)获得摄像头的姿态；

(3)对目标成像：整个系统投入运营，对目标成像；

(4)在图像中检测目标；

(5)计算目标射线；

(6)计算目标位置；

(7)计算目标姿态：根据目标在图像坐标里的姿态，以及摄像头的姿态，采取视觉和IMU、OD、Geomagnetic的信息融合组合导航可以确定目标的姿态。

2.根据权利要求1所述的一种基于视觉的定位方法，其特征在于：步骤1)中，如果在野外，采用高精度的差分GPS装置；如果在室内，用高精度的差分GPS装置定位整个建筑物，再根据建筑物内部结构的尺寸推算摄像头的精确位置；将世界坐标系原点设置在摄像头焦距处，一个方向指向经度O方向(东方)，另一个方向指向纬度A方向(北方)，第三个方向指向高度H方向。

3.根据权利要求1所述的一种基于视觉的定位方法，其特征在于：步骤2)中，采用带水平仪和罗盘的标定模板标定相机；水平放置的标定板一个方向指向经度O方向(东方)，一个方向指向纬度A方向(北方)，与摄像头处的世界坐标系一致；设标定后，摄像头坐标系与世界坐标系的变换为R1|T，从旋转矩阵R1可以按如下公式确定出摄像头的三个姿态角(αc，βc，γc)。

4.根据权利要求1所述的一种基于视觉的定位方法，其特征在于：步骤4)中，可以用目标检测的方法，也可以在目标上带预设标签的方法，确定目标在图像中的位置，目标大小为λ，相对于图像中心的偏移(r,c)，目标在图像坐标里的姿态θ。

5.根据权利要求1所述的一种基于视觉的定位方法，其特征在于：步骤5)中，因为是单目视角，无法确定目标的高度和远近；但是对于具体应用而言，目标往往在地面上，并且目标往往是确定的类型，大小、高度固定；在图像中找到目标后，根据目标相对图像中心的偏移(r,c)，在修正相机变形后，可以确定目标与摄像头光轴的偏转角

并计算出目标相对于相机坐标的旋转矩阵R2，因此可确定目标射线在世界坐标系里的角度(αO，βO，γO)。

6.根据权利要求1所述的一种基于视觉的定位方法，其特征在于：步骤6)中，已知目标射线后，有两种方式可以确定目标位置：(a)如果已知场地的几何模型：如果地面不是水平的，将场地的几何模型S向上平移目标高度后，此三维曲面与目标射线的交点就是目标位置；如果地面是水平的，不需要场地的几何模型，经过相交计算后，就可以确定目标位置；(b)根据目标大小：根据目标在图像中的大小λ，估算目标与相机的距离，从而确定目标的位置坐标。

7.一种用于权利要求1所述的全局视觉定位方法中的信息融合组合导航方法，其特征在于：具体如下：

1)在建立系统误差方程的基础上，将位置误差方程、姿态误差方程以及惯性仪表误差方程综合在一起作为组合导航卡尔曼滤波器的观察值，可以写出INS系统误差状态方程的一般表达式如下

X(k)＝F(k-1)X(k-1)+G(k-1)W(k-1)

其中，状态变量X＝[δO,δA,δH,φN,φE,φD,εrx,εry,εrz,△x,△y,△z]，δO、δA、δH为纬度、经度和高度误差，φN、φE、φD为平台误差角，εrx、εry、εrz为陀螺仪一阶马尔柯夫漂移，△x、△y、△z为加速度计一阶马尔柯夫漂移，F为状态转换阵，G为噪声转换阵，W为系统噪声。

2)用视觉测量值和IMU、OD、Geomagnetic融合信息值的差值作为量测值，观测方程为

Z(k)＝H(k)X(k)+V(k)

其中，Z＝[δO,δA,δH,φN,φE,φD]T，H为观测矩阵，V为测量噪声矩阵。

3)建立系统的状态方程和观测方程后，就可以进行卡尔曼滤波。其中状态噪声阵Q根据IMU、OD、Geomagnetic信息融合后的有关参数选取，观测噪声矩阵R根据视觉测量的性能选取。

4)经由Kalman滤波估计出惯导系统的误差，然后对惯导系统进行校正。