CN108051007A - 基于超声波组网和立体视觉的agv导航定位方法 - Google Patents

基于超声波组网和立体视觉的agv导航定位方法 Download PDF

Info

Publication number
CN108051007A
CN108051007A CN201711033453.4A CN201711033453A CN108051007A CN 108051007 A CN108051007 A CN 108051007A CN 201711033453 A CN201711033453 A CN 201711033453A CN 108051007 A CN108051007 A CN 108051007A
Authority
CN
China
Prior art keywords
image
point
information source
coordinate system
distance
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
CN201711033453.4A
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.)
Shanghai Tim Industrial Co Ltd
Original Assignee
Shanghai Tim Industrial 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 Shanghai Tim Industrial Co Ltd filed Critical Shanghai Tim Industrial Co Ltd
Priority to CN201711033453.4A priority Critical patent/CN108051007A/zh
Publication of CN108051007A publication Critical patent/CN108051007A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/02—Picture taking arrangements specially adapted for photogrammetry or photographic surveying, e.g. controlling overlapping of pictures
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/06—Systems determining the position data of a target
    • G01S15/08—Systems for measuring distance only

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Acoustics & Sound (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于超声波组网和立体视觉的AGV导航定位方法，该方法的步骤为：步骤1、进行超声波组网，利用测距原理，计算信源标签与信源基站之间距离；步骤2、对信源标签进行定位，将定位信息上传至上位机服务器；步骤3、控制器控制AGV的驱动装置，使AGV按照规划路径运动；步骤4、启动立体视觉模块，通过图像滤波、边沿检测、特征信息提取、立体匹配以及目标姿态和距离测量的流程，对欲搬运或卸载的货物进行相对距离和相对姿态的测量，调整AGV的转向角度及其与货架之间距离；步骤5、将货叉对准所述货架的***孔，再将货物搬起。本发明可快速高精度定位导航，便于后续的搬运或者卸载操作，适用于人机混合工作的区域。

Description

基于超声波组网和立体视觉的AGV导航定位方法
技术领域
本发明涉及导航定位技术领域，特别涉及一种基于超声波组网和立体视觉组合的AGV定位导航方法。
背景技术
在物流行业，装卸搬运货物是极其频繁的工作流程，耗费大量的人力资源与成本。在发达国家，这笔费用占到物流成本的15％～33％，在我国也占到15％左右。于是AGV(Automatic Guided Vehicle，无人搬运车)逐渐被应用到仓储业、制造业、港口等，为物流自动化的实现提供了便利的条件。
室内定位与导航主要是指实时确定AGV的方位，并指引其按照正确的路线进行运动。实现定位和导航功能包括以下几个步骤：1)环境地图的建立；2)自身定位；3)路径的规划。
现有的定位导航技术包括视觉、激光、磁力线、二维码、wifi、惯性导航等等，以上几种定位技术都各有优缺点，可以具体结合实际情况来进行合理设计和选择，
超声波是一种振动频率大于20kHz的循环声压，超出人耳所能听到的频率范围。超声波通常以纵波的方式在弹性介质中传播，是一种能量的传播方式，具有频率高、波长短、方向性较好等特点。同时也不易受到如天气条件、环境光照及障碍物阴影、表面粗糙度等外界环境条件的影响。超声波进行导航定位已经被广泛应用到各种移动机器人的感知***中。
传统的利用超声波技术的定位方案，主要采用反射式测距法。根据回波与发射波的时间差进行定位。这种方法依赖于反射物，对反射面要求较高。当反射面不光整或者有倾角时，极易丢失回波信号；当反射面较杂时，回波信号也变得难以进行。因此，通常采用具有超声发射器的基站组成网络，配合具有接收器的定位标签等，并通过控制***对采集的信号进行一定的算法处理才可以得到定位标签的位置环境信息。
在人机混合工作的仓储物流中心，使用具有自主定位导航功能的AGV实施搬运功能时，虽然超声波的定位导航***的定位精度可达厘米级，但是由于其无法了解货品的姿态和相对距离，很难后续实施较为精细的操作。这种情况在人力仍然参与物流运输时会显得尤其突出，无法实现快速高精度定位导航，制约了AGV对货物的后续操作。
发明内容
本发明的目的是提供一种基于超声波组网和立体视觉组合的AGV定位导航方法，其通过超声波组网对AGV进行定位和导航，利用立体视觉对货物的位置和姿态进行测量，以便调整AGV相对于货架的距离和朝向角，便于后续的搬运或者卸载操作，适用于人机混合工作的区域。
本发明提供了一种基于超声波组网和立体视觉的AGV导航定位方法，该方法的步骤为：
步骤1、开始进行超声波组网，并利用测距原理，计算信源标签与信源基站之间距离；
步骤2、利用三边定位原理，对信源标签进行定位，AGV将信源标签的定位信息通过无线通信网络上传至上位机服务器；
步骤3、上位机通过超声波组网得到AGV的定位信息，通过路径规划控制器控制AGV的驱动装置，使AGV按照规划的路径从起始点运动到目的地；
步骤4、启动立体视觉模块，通过图像滤波、边沿检测、特征信息提取、立体匹配以及目标姿态和距离测量的流程，对欲搬运或卸载的货物进行相对距离和相对姿态的测量，调整AGV的转向角度以及AGV与货架之间的距离；
步骤5、将货叉对准所述货架的***孔，再将货物搬起。
优选地，所述超声波组网包含信源标签、多个信源基站和射频发射站；
所述射频发射站包含射频发射模块，其发射射频同步信号给信源标签和各个信源基站；
每个信源基站含有射频接收模块和超声波发射模块，所述信源基站先接收来自射频发射站的射频信号，再发送超声波脉冲给信源标签；
所述信源标签含有射频接收模块和超声波接收模块，所述信源标签先接收所述射频发射站发送的射频信号，并记录该接收时刻，再接收各个信源基站发射的超声波脉冲信号，并记录该接收时刻。
优选地，所述信源基站的数目为四个，分别为一个第二信源基站、一个第三信源基站和两个第一信源基站；
所述信源基站和射频发射站包围在所述信源标签的外侧。
优选地，所述步骤1包含：
步骤1.1、超声波组网的射频发射站发射射频同步信号；
步骤1.2、超声波组网的信源基站接收来自射频发射站的射频信号，发送超声波脉冲给信源标签；
步骤1.3、信源标签首先接收到射频信号，并将该接收信号的时刻记录为T0，然后接收到第i个信源基站发射的超声波脉冲信号，并将此接收信号的时刻记录为Ti，所述定位标签与第i个信源基站之间距离的计算公式为：
li＝(Ti-T0)×C2 (1)
式中，C2＝340m/s为超声波脉冲在常温下的传播速度，li为计算得到的第i个信源基站与信源标签之间的距离信息。
优选地，所述步骤2包含：选定3个信源基站的坐标为Pi(xi,yi,zi)(i＝1,2,3)，o(x,y,z)为信源标签的坐标，利用三边定位法得到几何关系为：
式中，x、y和z分别为信源标签的x轴、y轴和z轴的坐标；xi、yi和zi分别为第i个信源基站的各个轴的坐标。
优选地，所述步骤4包含：
步骤4.1、图像滤波：采用中值滤波的方法分别对左右摄像头图像进行滤波，消除噪声干扰，得到平滑的左、右摄像头图像；所述步骤4.1具体方法为：先确定一个奇数像素的二维窗口W，尺寸为k*l，窗口内各像素按灰度大小排队后，用其中间位置的灰度值代替原灰度值，得到：
f(x,y)＝median{h(x-k,y-l),(k,l)∈W} (3)
式中，h(x,y)为原灰度值，f(x,y)为增强图像的灰度值，W为选定窗口大小；
步骤4.2、边沿检测：通过canny边缘检测的方法对两幅图像分别进行边沿检测，得到边沿特征信息；
步骤4.3、特征信息提取：对进行边沿检测后的图像进行角点、直线、圆或椭圆特征提取，并从中识别选定的特征点信息；
步骤4.4、立体匹配：根据识别出特征点在左、右摄像头中的二维信息，完成左右两幅不同图像上的特征点对应匹配，并去除坏的匹配点；
步骤4.5、目标位置和姿态的测量：根据立体匹配结果，构建测试对象坐标系，并计算出其相对于世界坐标的位置和姿态。
优选地，所述步骤4.2包含：
步骤4.2.1、先用高斯滤波函数G(x,y)对图像f(x,y)进行平滑滤波，高斯滤波函数G(x,y)与图像f(x,y)卷积得到平滑的图像g(x,y)，可得：
式中，G(x,y)为二维高斯分布函数，σ为正态分布的标准偏差；
步骤4.2.2、计算平滑图像g(x,y)中每个像素点(x,y)的梯度强度和方向，计算平滑图像g(x,y)中每个像素点(x,y)的梯度强度和方向，可得：
图像g(x,y)中每个像素点(x,y)的梯度强度和梯度方向θ(x,y)分别为：
式中，Ex、Ey分别为G(x,y)的一阶方向导数与图像f(x,y)的卷积；
步骤4.2.3、计算边缘点：中心边缘点为G(x,y)与f(x,y)的卷积在边缘梯度方向上的区域中的最大值点；通过在每一个梯度方向上判断此点强度是否为其领域的最大值来确定该点是否为边缘点。
优选地，所述步骤4.3包含：
步骤4.3.1、角点提取：采用基于Harris算子的点特征提取方法，角点响应函数为：
R＝detM-k(traceM)2 (7)
式中，detM＝λ1×λ2，traceM＝λ1+λ2，k为权值系数，取值0.04～0.06；gx为x方向的梯度，gy为y方向的梯度，为高斯滤波模板；设具有特征值λ1和λ2，判断条件为：
b1、λ1>>λ2或λ2>>λ1，对应点为边缘点，对应的R为负；
b2、λ1和λ2都很小，对应点为图像上的普通点，对应的|R|值小；
b3、λ1和λ2都很大，对应点为角点，对应的R值大；
步骤4.3.2、直线提取：采用Hough变换的方法对区域边界形状进行描述；设一幅图像的内容为一条线段，其上一点A的坐标为xi和yi，对应的参数空间值为ρ和θ分别为该点的极距和极角；极距和极角之间的映射关系为：
ρ＝xicosθ+yisinθ (8)
在参数空间中，极距ρ的最大值为其中，xmax和ymax为图像的最大坐标值；极角θ的变化范围为0°～180°。
优选地，所述步骤4.4采用欧式距离方法作为相似性度量的标准，给定参考图像和待匹配图像上的特征点pi和qj，欧式距离值Dij定义如下：
其中，Li(k)和Lj(k)分别为特征点pi和qj的特征描述向量；根据欧式距离的特征，当Dij值越小，表示特征点pi和qj越相似，越可能是匹配点对；设定一个阈值T，如果Dij值小于T时，特征点pi和qj被认为是匹配的；
所述步骤4.4的步骤包含：
4.4a、取标准图像中的特征点P1，在待匹配图像中找出与其欧氏距离最近的前两个特征点Pr1和Pr2，若特征点Pr1和Pr2中最近的距离Dmin除以次近的距离Ucmin小于匹配阈值，则表示标准图像中的这个特征点P1与待匹配图像中欧氏距离最近的特征点Pr1匹配；
4.4b、以待匹配图像中Pr1为特征点，重复步骤4.4a的过程，求取标准图像中Pr1的候选匹配点P1’；如果Pl和P1’是相同点，则Pl和Pr1匹配成功，否则弃之；
4.4c、循环进行，直到匹配完成。
优选地，所述步骤4.5包含：
步骤4.5.1、位置测量；设置两摄像机的投影中心的连线距离为基线距为b，摄像机坐标的原点在摄像机镜头的光心处，左右图像坐标系的原点在摄像机光轴与平面的交点O1和O2，空间中任意一点P在左图像和右图像中相应的坐标分别为P1(x1,y1)和P2(x2,y2)，点P在左摄像机坐标系中的坐标为(x,y,z)，由以下公式计算距离：
式中，Δz表示测量得出的被测点与立体视觉***之间距离的精度，z指被测点与立体视觉***的绝对距离，f指摄像机的焦距，b表示双目立体视觉***的基线距，Δd表示被测点视差精度；
步骤4.5.2、姿态测量；根据摄像机中心投影成像的过程，建立的坐标系有：
1-1、物体坐标系OO-xOyOzO，设目标上第i个特征点在此坐标系上的坐标为qi＝[xoi,yoi,zoi]T；
2-2、测量坐标系OS-xSySzS，将第一个摄像机的摄像机坐标系作为测量坐标系，设目标上第i个特征点在此坐标系上的坐标为pi＝[xSi,ySi,zSi]T；
3-3、两个摄像机所采集图像的像素坐标系o-uLvL和o-uRvR，以成像平面左上角为原点，uL，vL轴分别平行于像平面坐标系的x轴和y轴；目标上第i个特征点在两图像上的投影在其像素坐标系下的坐标分别为[uL,vL]T和[uR,vR]T；
以第一个摄像机坐标系作为测量坐标系，则两摄像机的投影矩阵为：
M1＝K[1 0] (11)
M2＝K[RC TC] (12)
其中，K为摄像机的内参数矩阵，RC和TC为第二个摄像机相对于第一个摄像机的旋转矩阵和平移向量；对于投影矩阵M1，令M11、M12、M13为对应于M1的行向量，设第一个摄像机所采集图像上的第i个特征点的齐次坐标为[uLi vLi 1]T，Pi为该特征点的测量坐标系下的齐次坐标，得到公式(13)：
对于投影矩阵M2、与该特征点在第二个摄像机所采集图像上投影的齐次坐标系有关系[uRi vRi 1]T：
联立公式(13)和公式(14)可得：
从公式(15)中可通过最小二乘原理求解出
设两摄像机所采集图像上得到的n个配对的特征点图像投影，特征点在测量坐标系下的三维坐标P＝{pi}(i＝1,…,n)经解算得到，在物体坐标系下的三维坐标Q＝{qi}(i＝1,…,n)已知，得到坐标系关系为：
P＝sRQ+T (16)
其中，s为比例系数；T为位移向量，是一个三维向量，即T＝[Tx,Ty,Tz]T；R为旋转矩阵，是一个3×3的单位正交矩阵；
若是理想的测量数据，可由所有点的信息直接求解公式(16)，得到s、R、T；由于会存在误差，则误差为：
ei＝pi-sRqi-T (17)
根据最小二乘理论，当所有测量误差的平方和最小时，能得到最合适的解；
假设该n个特征点的质心在测量坐标系和物体坐标系中的坐标分别为：
得到分别以质心为原点的坐标系下的新坐标：
误差平方和可写为：
或者为：
其中，
通过计算出比例系数和旋转向量，根据特征点质心在测量坐标系和物体坐标系中的坐标可计算出位移向量；
采用欧拉角法来表示相对姿态，采用z→y→x的旋转顺序，对应的旋转矩阵为：
R为旋转矩阵，是三个角度(α,β,θ)的三角函数组合；绕X轴旋转角α为俯仰角，绕Y轴旋转角β为偏航角，绕Z轴旋转角θ为滚动角；通过位姿求解可得到旋转矩阵为矩阵第i行第j列的元素)，则根据公式(22)得到欧拉角为：
β＝arcsin(r13) (24)
与现有技术相比，本发明的有益效果为：本发明的后续实施较为精细，可实现快速高精度定位导航，可以调整AGV相对于货架的距离和朝向角，便于后续的搬运或者卸载操作，适用于人机混合工作的区域。
附图说明
图1本发明一实施例提供的超声波组网的组成和原理示意图；
图2本发明一实施例提供的AGV定位导航方法流程图；
图3本发明的立体视觉算法的流程图；
图4本发明二维图像与目标三维信息的对应关系图。
具体实施方式
本发明提供了一种基于超声波组网和立体视觉的AGV导航定位方法，为使本发明更加明显易懂，以下结合具体实施方式与附图，对本发明做进一步说明。
如图1所示为本发明的超声波组网组成示意图，该超声波组网包含信源标签、多个信源基站和射频发射站。
其中，射频发射站包含射频发射模块并可发射射频同步信号给各个信源基站和信源标签。每个信源基站含有射频接收模块和超声波发射模块，信源基站接收到来自射频发射站的射频信号，随后发送超声波脉冲给信源标签。信源标签含有射频接收模块和超声波接收模块，信源标签首先接收到射频发射站发送的射频信号，将该时刻记录为T0，然后接收到第i个信源基站发射的超声波脉冲信号，并将此时刻记录为Ti。
本实施例中的信源基站的数目为四个，分别为一个第二信源基站2、一个第三信源基站3和两个第一信源基站1。信源基站和射频发射站包围在信源标签***。
如图2所示，本发明的AGV导航定位方法包含以下步骤：
步骤1、将具有超声波发射模块的信源基站进行组网，利用测距原理，计算信源标签与信源基站之间距离。具体步骤为：
步骤1.1、射频发射站发射射频同步信号。
步骤1.2、信源基站接收到来自射频发射站的射频信号，随后发送超声波脉冲给信源标签。
步骤1.3、信源标签首先接收到射频信号，并将该接收信号的时刻记录为T0，然后接收到第i个信源基站发射的超声波脉冲信号，并将此接收信号的时刻记录为Ti，所述定位标签与第i个信源基站之间距离的计算公式为：
li＝(Ti-T0)×C2 (1)
式中，C2＝340m/s为超声波脉冲在常温下的传播速度，li为计算得到的第i个信源基站与信源标签之间的距离信息。
步骤2、利用三边定位原理，实现信源标签的定位，AGV将该定位信息通过无线通信网络上传至上位机服务器。
其中，步骤2的具体步骤为：
选定3个信源基站的坐标为Pi(xi,yi,zi)(i＝1,2,3)，o(x,y,z)为信源标签的坐标，利用三边定位法得到几何关系为：
式中，x、y和z分别为信源标签的x轴、y轴和z轴的坐标；xi、yi和zi分别为第i个信源基站的各个轴的坐标；从公式(2)中可以解算出信源标签的坐标o(x,y,z)，从而完成定位功能。
步骤3、上位机通过超声波组网得到AGV的定位信息，通过路径规划，控制器控制AGV的驱动装置，使得AGV按照规划的路线从起始点运动到目的地。
步骤4、启动立体视觉模块，采用基于三角测量的方法，利用两个摄像机对从货架进行成像，从视差中恢复距离信息。该步骤主要是通过图像滤波、边沿检测、特征信息提取、立体匹配以及目标姿态和距离的测量的流程，完成对欲搬运或卸载的货品的相对距离和相对姿态进行测量，调整AGV的转向角度以及与货架之间的距离，以便后续的搬运或卸载动作能够顺利进行。
其中，步骤4的具体方法为：
步骤4.1、图像滤波：分别对左右摄像头图像进行滤波，消除噪声干扰，得到平滑的左、右摄像头图像。该图像滤波采用中值滤波的方法，其基本思想是用像素点邻域灰度值的中值来代替该像素点的灰度值；具体方法为首先确定一个奇数像素的二维窗口W，尺寸为k*l，窗口内各像素按灰度大小排队后，用其中间位置的灰度值代替原灰度值，可得：
f(x,y)＝median{h(x-k,y-l),(k,l)∈W} (3)
式中h(x,y)为原灰度值，f(x,y)为增强图像的灰度值，W为选定窗口大小。
步骤4.2、边沿检测：对两幅图像分别进行边沿检测，得到边沿特征信息。这里采用canny边缘检测的方法。
步骤4.2.1、首先用高斯滤波函数G(x,y)对图像f(x,y)进行平滑滤波，二者卷积可得到一个平滑的图像g(x,y)：
式中，G(x,y)为二维高斯分布函数，σ为正态分布的标准偏差。
步骤4.2.2、计算平滑图像g(x,y)中每个像素点(x,y)的梯度强度和方向，计算平滑图像g(x,y)中每个像素点(x,y)的梯度强度和方向：
则图像g(x,y)中每个像素点(x,y)的梯度强度和梯度方向θ(x,y)分别为：
式中，Ex、Ey分别为G(x,y)的一阶方向导数与图像f(x,y)的卷积。
步骤4.2.3、计算边缘点：中心边缘点为G(x,y)与f(x,y)的卷积在边缘梯度方向上的区域中的最大值点。这样，就可以在每一个梯度方向上判断此点强度是否为其领域的最大值来确定该点是否为边缘点。
当一个像素满足以下三个条件时，则被认为是图像的边缘点：
(a1)该点的边缘强度大于沿该点梯度方向的两个相邻像素点的边缘强度；
(a2)与该点梯度方向上相邻两点的方向差小于45°；
(a3)以该点为中心的3×3领域中的边缘强度极大值小于某个阈值。
此外，如果条件a1和条件a2同时被满足，则在梯度方向上的两相邻像素就从候选边缘点中剔除，条件a3相当于用区域梯度最大值组成的阈值图像与边缘点进行匹配，这一过程消除了许多虚假的边缘点。
步骤4.3、特征信息提取：对进行边沿检测后的图像进行角点、直线、圆或椭圆特征提取，并从中识别选定的特征点信息。
步骤4.3.1、角点提取：采用基于Harris算子的点特征提取方法，角点响应函数为：
R＝detM-k(traceM)2 (7)
式中，detM＝λ1×λ2，traceM＝λ1+λ2，k为权值系数，取值0.04～0.06；gx为x方向的梯度，gy为y方向的梯度，为高斯滤波模板。设具有特征值λ1和λ2，则判断条件如下：
(b1)、λ1>>λ2或λ2>>λ1，对应点为边缘点，对应的R为负；
(b2)、λ1和λ2都很小，对应点为图像上的普通点(即图像上的平坦区域)，对应的|R|值小；
(b3)、λ1和λ2都很大，对应点为角点，对应的R值大。
步骤4.3.2、直线提取：采用Hough变换的方法对区域边界形状进行描述的方法；设某幅图像的内容为一条线段，其上某点A的坐标为xi和yi，对应的参数空间值为ρ和θ分别为该点的极距和极角。
它们之间的映射关系为：
ρ＝xicosθ+yisinθ (8)
在参数空间中，极距ρ的最大值为其中，xmax和ymax为图像(数据空间)的最大坐标值。极角θ的变化范围为0°～180°。
步骤4.4、立体匹配：根据识别出特征点在左、右摄像头中的二维信息，完成左右两幅不同图像上的特征点对应匹配，并去除坏的匹配点。
欧式距离是很常用的一种相似性度量方法，给定参考图像和待匹配图像上的特征点pi和qj，采用欧式距离方法作为相似性度量的标准，欧式距离值Dij定义如下：
其中，Li(k)和Lj(k)分别为特征点pi和qj的特征描述向量。根据欧式距离的特征，当Dij值越小，表示两个点越相似，越可能是匹配点对，设定一个阈值T，如果Dij值小于T时，这两个点被认为是匹配的。具体步骤为：
a)取标准图像中的特征点P1，在待匹配图像中找出与其欧氏距离最近的前两个特征点Pr1和Pr2，如果这两个特征点中最近的距离Dmin除以次近的距离Ucmin小于匹配阈值，则表示标准图像中的这个特征点P1与待匹配图像中欧氏距离最近的特征点Pr1匹配。
b)以待匹配图像中Pr1为特征点，重复步骤a)的过程，求取标准图像中Pr1的候选匹配点P1’。
c)如果Pl和P1’是相同点，则Pl和Pr1匹配成功，否则弃之。
d)重复这个流程，直到匹配完成。
步骤4.5、目标位置和姿态的测量：根据立体匹配结果，进一步构建测试对象坐标系，并计算出其相对于世界坐标的位置和姿态。具体步骤如下：
步骤4.5.1、位置测量；如图4所示，假设两摄像机的投影中心的连线距离即基线距为b，摄像机坐标的原点在摄像机镜头的光心处，左右图像坐标系的原点在摄像机光轴与平面的交点O1和O2，空间中某点P在左图像和右图像中相应的坐标分别为P1(x1,y1)和P2(x2,y2)，点P在左摄像机坐标系中的坐标为(x,y,z)，可以由以下公式计算距离：
式中，Δz表示测量得出的被测点与立体视觉***之间距离的精度，z指被测点与立体视觉***的绝对距离，f指摄像机的焦距，b表示双目立体视觉***的基线距，Δd表示被测点视差精度。
步骤4.5.2、姿态测量；根据摄像机中心投影成像的过程，建立了以下几个坐标系：
1-1)物体坐标系OO-xOyOzO；设目标上第i个特征点在此坐标系上的坐标为qi＝[xoi,yoi,zoi]T；
2-2)测量坐标系OS-xSySzS；由于双目视觉只需考虑摄像机采集图像时所处的不同位置之间的关系，故将第一个摄像机的摄像机坐标系作为测量坐标系。设目标上第i个特征点在此坐标系上的坐标为pi＝[xSi,ySi,zSi]T。
3-3)两个摄像机所采集图像的像素坐标系o-uLvL和o-uRvR；以成像平面左上角为原点，uL，vL轴分别平行于像平面坐标系的x轴和y轴。目标第i个特征点在两图像上的投影在其像素坐标系下的坐标分别为[uL,vL]T和[uR,vR]T，坐标系之间的关系如图4所示。
由于是以第一个摄像机坐标系作为测量坐标系，两摄像机的投影矩阵为：
M1＝K[1 0] (11)
M2＝K[RC TC] (12)
其中，K为摄像机的内参数矩阵，RC和TC为第二个摄像机相对于第一个摄像机的旋转矩阵和平移向量。对于投影矩阵M1，令M11、M12、M13为对应于M1的行向量，设第一个摄像机所采集图像上的第i个特征点的齐次坐标为[uLi vLi 1]T，Pi为该特征点的测量坐标系下的齐次坐标，有如下关系：
同理，对于投影矩阵M2，与该特征点在第二个摄像机所采集图像上投影的齐次坐标系有类似关系[uRi vRi 1]T：
联立公式(13)和公式(14)可得：
从公式(15)中可以通过最小二乘原理求解出
设两摄像机所采集图像上可得到的n个配对的特征点图像投影，特征点在测量坐标系下的三维坐标P＝{pi}(i＝1,…,n)经解算得到，在物体坐标系下的三维坐标Q＝{qi}(i＝1,…,n)已知。这两个坐标系关系可表示为：
P＝sRQ+T (16)
其中，s为比例系数；T为位移向量，是一个三维向量，即T＝[Tx,Ty,Tz]T；R为旋转矩阵，是一个3×3的单位正交矩阵。
如果是理想的测量数据，则能由所有点的信息直接求解上述方程，得到s、R、T。
但是总会存在误差，可得：
ei＝pi-sRqi-T (17)
根据最小二乘理论，当所有测量误差的平方和最小时，便能得到最合适的解。
假设这n个特征点的质心在测量坐标系和物体坐标系中的坐标分别为：
则可以得到分别以质心为原点的坐标系下的新坐标：
则误差平方和可写为：
或者
其中，
通过计算出比例系数和旋转向量，根据特征点质心在测量坐标系和物体坐标系中的坐标即可计算出位移向量。
采用欧拉角法来表示相对姿态，采用z→y→x的旋转顺序，对应的旋转矩阵为：
R为旋转矩阵，是三个角度(α,β,θ)的三角函数组合。绕X轴旋转角α为俯仰角，绕Y轴旋转角β为偏航角，绕Z轴旋转角θ为滚动角。通过位姿求解可得到旋转矩阵为矩阵第i行第j列的元素)，可根据公式(22)得到欧拉角为：
β＝arcsin(r13) (24)
所以，综上所述，步骤4主要根据立体视觉测出货物相对AGV的距离信息和姿态信息，控制AGV的驱动装置进行距离移动和转向，使得AGV正对货架并保持合适距离，以便完成后续的搬运操作。
步骤5、将货叉对准货架的***孔，最后将货物搬起。
尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。

Claims (10)

1.一种基于超声波组网和立体视觉的AGV导航定位方法，其特征在于，该方法为：
步骤1、开始进行超声波组网，并利用测距原理，计算信源标签与信源基站之间距离；
步骤2、利用三边定位原理，对信源标签进行定位，AGV将信源标签的定位信息通过无线通信网络上传至上位机服务器；
步骤3、上位机通过超声波组网得到AGV的定位信息，通过路径规划控制器控制AGV的驱动装置，使AGV按照规划的路径从起始点运动到目的地；
步骤4、启动立体视觉模块，通过图像滤波、边沿检测、特征信息提取、立体匹配以及目标姿态和距离测量的流程，对欲搬运或卸载的货物进行相对距离和相对姿态的测量，调整AGV的转向角度以及AGV与货架之间的距离；
步骤5、将货叉对准所述货架的***孔，再将货物搬起。
2.如权利要求1所述的AGV导航定位方法，其特征在于，
所述超声波组网包含信源标签、多个信源基站和射频发射站；
所述射频发射站包含射频发射模块，其发射射频同步信号给信源标签和各个信源基站；
每个信源基站含有射频接收模块和超声波发射模块，所述信源基站先接收来自所述射频发射站的射频信号，再发送超声波脉冲给所述信源标签；
所述信源标签含有射频接收模块和超声波接收模块，所述信源标签的射频接收模块先接收所述射频发射站发送的射频信号，并记录该接收时刻，再接收各个信源基站发射的超声波脉冲信号，并记录该接收时刻。
3.如权利要求2所述的AGV导航定位方法，其特征在于，
所述信源基站的数目为四个，分别为一个第二信源基站、一个第三信源基站和两个第一信源基站；
所述信源基站和射频发射站包围在所述信源标签的外侧。
4.如权利要求2所述的AGV导航定位方法，其特征在于，
所述步骤1包含：
步骤1.1、超声波组网的射频发射站发射射频同步信号；
步骤1.2、超声波组网的信源基站接收来自射频发射站的射频信号，发送超声波脉冲给信源标签；
步骤1.3、信源标签首先接收到射频信号，并将该接收信号的时刻记录为T0，然后接收到第i个信源基站发射的超声波脉冲信号，并将此接收信号的时刻记录为Ti，所述定位标签与第i个信源基站之间距离的计算公式为：
li＝(Ti-T0)×C2 (1)
式中，C2＝340m/s为超声波脉冲在常温下的传播速度，li为计算得到的第i个信源基站与信源标签之间的距离信息。
5.如权利要求4所述的AGV导航定位方法，其特征在于，
所述步骤2包含：选定3个信源基站的坐标为Pi(xi,yi,zi)(i＝1,2,3)，o(x,y,z)为信源标签的坐标，利用三边定位法得到几何关系为：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msup> <mrow> <mo>(</mo> <mrow> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>y</mi> <mo>-</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>z</mi> <mo>-</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>=</mo> <msub> <mi>l</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <mrow> <mo>(</mo> <mrow> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mn>2</mn> </msub> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>y</mi> <mo>-</mo> <msub> <mi>y</mi> <mn>2</mn> </msub> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>z</mi> <mo>-</mo> <msub> <mi>z</mi> <mn>2</mn> </msub> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>=</mo> <msub> <mi>l</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <mrow> <mo>(</mo> <mrow> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mn>3</mn> </msub> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>y</mi> <mo>-</mo> <msub> <mi>y</mi> <mn>3</mn> </msub> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>z</mi> <mo>-</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> </mrow> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>=</mo> <msub> <mi>l</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
式中，x、y和z分别为信源标签的x轴、y轴和z轴的坐标；xi、yi和zi分别为第i个信源基站的各个轴的坐标。
6.如权利要求5所述的AGV导航定位方法，其特征在于，
所述步骤4包含：
步骤4.1、图像滤波：采用中值滤波的方法分别对左右摄像头图像进行滤波，消除噪声干扰，得到平滑的左、右摄像头图像；所述步骤4.1具体方法为：先确定一个奇数像素的二维窗口W，尺寸为k*l，窗口内各像素按灰度大小排队后，用其中间位置的灰度值代替原灰度值，得到：
f(x,y)＝median{h(x-k,y-l),(k,l)∈W} (3)
式中，h(x,y)为原灰度值，f(x,y)为增强图像的灰度值，W为选定窗口大小；
步骤4.2、边沿检测：通过canny边缘检测的方法对两幅图像分别进行边沿检测，得到边沿特征信息；
步骤4.3、特征信息提取：对进行边沿检测后的图像进行角点、直线、圆或椭圆特征提取，并从中识别选定的特征点信息；
步骤4.4、立体匹配：根据识别出特征点在左、右摄像头中的二维信息，完成左右两幅不同图像上的特征点对应匹配，并去除坏的匹配点；
步骤4.5、目标位置和姿态的测量：根据立体匹配结果，构建测试对象坐标系，并计算出其相对于世界坐标的位置和姿态。
7.如权利要求6所述的AGV导航定位方法，其特征在于，
所述步骤4.2包含：
步骤4.2.1、先用高斯滤波函数G(x,y)对图像f(x,y)进行平滑滤波，高斯滤波函数G(x,y)与图像f(x,y)卷积得到平滑的图像g(x,y)，可得：
<mrow> <mi>G</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>2</mn> <msup> <mi>&amp;pi;&amp;sigma;</mi> <mn>2</mn> </msup> </mrow> </mfrac> <mi>exp</mi> <mrow> <mo>(</mo> <mo>-</mo> <mfrac> <mrow> <msup> <mi>x</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>y</mi> <mn>2</mn> </msup> </mrow> <mrow> <mn>2</mn> <msup> <mi>&amp;sigma;</mi> <mn>2</mn> </msup> </mrow> </mfrac> <mo>)</mo> </mrow> </mrow>
<mrow> <mi>g</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>G</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>&amp;CircleTimes;</mo> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
式中，G(x,y)为二维高斯分布函数，σ为正态分布的标准偏差；
步骤4.2.2、计算平滑图像g(x,y)中每个像素点(x,y)的梯度强度和方向，计算平滑图像g(x,y)中每个像素点(x,y)的梯度强度和方向，可得：
<mrow> <mo>&amp;dtri;</mo> <mi>g</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>&amp;dtri;</mo> <mrow> <mo>(</mo> <mi>G</mi> <mo>(</mo> <mrow> <mi>x</mi> <mo>,</mo> <mi>y</mi> </mrow> <mo>)</mo> <mo>&amp;CircleTimes;</mo> <mi>f</mi> <mo>(</mo> <mrow> <mi>x</mi> <mo>,</mo> <mi>y</mi> </mrow> <mo>)</mo> <mo>)</mo> </mrow> <mo>=</mo> <mo>&amp;dtri;</mo> <mi>G</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>&amp;CircleTimes;</mo> <mi>f</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
图像g(x,y)中每个像素点(x,y)的梯度强度和梯度方向θ(x,y)分别为：
<mrow> <mo>&amp;dtri;</mo> <mi>g</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>=</mo> <msqrt> <mrow> <msubsup> <mi>E</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>E</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> </mrow> </msqrt> </mrow>
<mrow> <mi>&amp;theta;</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>arctan</mi> <mfrac> <mrow> <msub> <mi>E</mi> <mi>x</mi> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>E</mi> <mi>y</mi> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
式中，Ex、Ey分别为G(x,y)的一阶方向导数与图像f(x,y)的卷积；
步骤4.2.3、计算边缘点：中心边缘点为G(x,y)与f(x,y)的卷积在边缘梯度方向上的区域中的最大值点；通过在每一个梯度方向上判断此点强度是否为其领域的最大值来确定该点是否为边缘点。
8.如权利要求7所述的AGV导航定位方法，其特征在于，
所述步骤4.3包含：
步骤4.3.1、角点提取：采用基于Harris算子的点特征提取方法，角点响应函数为：
R＝detM-k(traceM)2 (7)
式中，detM＝λ1×λ2，traceM＝λ1+λ2，k为权值系数，取值0.04～0.06；gx为x方向的梯度，gy为y方向的梯度，为高斯滤波模板；设具有特征值λ1和λ2，判断条件为：
b1、λ1>>λ2或λ2>>λ1，对应点为边缘点，对应的R为负；
b2、λ1和λ2都很小，对应点为图像上的普通点，对应的|R|值小；
b3、λ1和λ2都很大，对应点为角点，对应的R值大；
步骤4.3.2、直线提取：采用Hough变换的方法对区域边界形状进行描述；设一幅图像的内容为一条线段，其上一点A的坐标为xi和yi，对应的参数空间值为ρ和θ分别为该点的极距和极角；极距和极角之间的映射关系为：
ρ＝xi cosθ+yi sinθ (8)
在参数空间中，极距ρ的最大值为其中，xmax和ymax为图像的最大坐标值；极角θ的变化范围为0°～180°。
9.如权利要求8所述的AGV导航定位方法，其特征在于，
所述步骤4.4采用欧式距离方法作为相似性度量的标准，给定参考图像和待匹配图像上的特征点pi和qj，欧式距离值Dij定义如下：
<mrow> <msub> <mi>D</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> <mo>=</mo> <msup> <mrow> <mo>(</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mo>|</mo> <msub> <mi>L</mi> <mi>i</mi> </msub> <mo>(</mo> <mi>k</mi> <mo>)</mo> <mo>-</mo> <msub> <mi>L</mi> <mi>j</mi> </msub> <mo>(</mo> <mi>k</mi> <mo>)</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>)</mo> </mrow> <mrow> <mn>1</mn> <mo>/</mo> <mn>2</mn> </mrow> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
其中，Li(k)和Lj(k)分别为特征点pi和qj的特征描述向量；根据欧式距离的特征，当Dij值越小，表示特征点pi和qj越相似，越可能是匹配点对；设定一个阈值T，如果Dij值小于T时，特征点pi和qj被认为是匹配的；
所述步骤4.4的步骤包含：
4.4a、取标准图像中的特征点P1，在待匹配图像中找出与其欧氏距离最近的前两个特征点Pr1和Pr2，若特征点Pr1和Pr2中最近的距离Dmin除以次近的距离Ucmin小于匹配阈值，则表示标准图像中的这个特征点P1与待匹配图像中欧氏距离最近的特征点Pr1匹配；
4.4b、以待匹配图像中Pr1为特征点，重复步骤4.4a的过程，求取标准图像中Pr1的候选匹配点P1’；如果Pl和P1’是相同点，则Pl和Pr1匹配成功，否则弃之；
4.4c、循环进行，直到匹配完成。
10.如权利要求9所述的AGV导航定位方法，其特征在于，
所述步骤4.5包含：
步骤4.5.1、位置测量；设置两摄像机的投影中心的连线距离为基线距为b，摄像机坐标的原点在摄像机镜头的光心处，左右图像坐标系的原点在摄像机光轴与平面的交点O1和O2，空间中任意一点P在左图像和右图像中相应的坐标分别为P1(x1,y1)和P2(x2,y2)，点P在左摄像机坐标系中的坐标为(x,y,z)，由以下公式计算距离：
<mrow> <mi>&amp;Delta;</mi> <mi>z</mi> <mo>=</mo> <mfrac> <msup> <mi>z</mi> <mn>2</mn> </msup> <mrow> <mi>f</mi> <mo>&amp;CenterDot;</mo> <mi>b</mi> </mrow> </mfrac> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>d</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
式中，Δz表示测量得出的被测点与立体视觉***之间距离的精度，z指被测点与立体视觉***的绝对距离，f指摄像机的焦距，b表示双目立体视觉***的基线距，Δd表示被测点视差精度；
步骤4.5.2、姿态测量；根据摄像机中心投影成像的过程，建立的坐标系有：
1-1、物体坐标系OO-xOyOzO，设目标上第i个特征点在此坐标系上的坐标为qi＝[xoi,yoi,zoi]T；
2-2、测量坐标系OS-xSySzS，将第一个摄像机的摄像机坐标系作为测量坐标系，设目标上第i个特征点在此坐标系上的坐标为pi＝[xSi,ySi,zSi]T；
3-3、两个摄像机所采集图像的像素坐标系o-uLvL和o-uRvR，以成像平面左上角为原点，uL，vL轴分别平行于像平面坐标系的x轴和y轴；目标上第i个特征点在两图像上的投影在其像素坐标系下的坐标分别为[uL,vL]T和[uR,vR]T；
以第一个摄像机坐标系作为测量坐标系，则两摄像机的投影矩阵为：
M1＝K[1 0] (11)
M2＝K[RC TC] (12)
其中，K为摄像机的内参数矩阵，RC和TC为第二个摄像机相对于第一个摄像机的旋转矩阵和平移向量；对于投影矩阵M1，令M11、M12、M13为对应于M1的行向量，设第一个摄像机所采集图像上的第i个特征点的齐次坐标为[uLi vLi 1]T，Pi为该特征点的测量坐标系下的齐次坐标，得到公式(13)：
<mrow> <mi>&amp;lambda;</mi> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>u</mi> <mrow> <mi>L</mi> <mi>i</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mrow> <mi>L</mi> <mi>i</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>M</mi> <mn>11</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>M</mi> <mn>12</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>M</mi> <mn>13</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mover> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>~</mo> </mover> <mo>&amp;DoubleRightArrow;</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;lambda;u</mi> <mrow> <mi>L</mi> <mi>i</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>M</mi> <mn>11</mn> </msub> <mover> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>~</mo> </mover> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;lambda;v</mi> <mrow> <mi>L</mi> <mi>i</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>M</mi> <mn>12</mn> </msub> <mover> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>~</mo> </mover> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;lambda;</mi> <mo>=</mo> <msub> <mi>M</mi> <mn>13</mn> </msub> <mover> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>~</mo> </mover> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>&amp;DoubleRightArrow;</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mn>13</mn> </msub> <mover> <mrow> <msub> <mi>P</mi> <mi>i</mi> </msub> <msub> <mi>u</mi> <mrow> <mi>L</mi> <mi>i</mi> </mrow> </msub> </mrow> <mo>~</mo> </mover> <mo>=</mo> <msub> <mi>M</mi> <mn>11</mn> </msub> <mover> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>~</mo> </mover> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mn>13</mn> </msub> <mover> <mrow> <msub> <mi>P</mi> <mi>i</mi> </msub> <msub> <mi>v</mi> <mrow> <mi>L</mi> <mi>i</mi> </mrow> </msub> </mrow> <mo>~</mo> </mover> <mo>=</mo> <msub> <mi>M</mi> <mn>12</mn> </msub> <mover> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>~</mo> </mover> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>&amp;DoubleRightArrow;</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mn>13</mn> </msub> <msub> <mi>u</mi> <mrow> <mi>L</mi> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>M</mi> <mn>11</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mn>13</mn> </msub> <msub> <mi>v</mi> <mrow> <mi>L</mi> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>M</mi> <mn>12</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mover> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>~</mo> </mover> <mo>=</mo> <mn>0</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>
对于投影矩阵M2、与该特征点在第二个摄像机所采集图像上投影的齐次坐标系有关系[uRi vRi 1]T：
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mn>23</mn> </msub> <msub> <mi>u</mi> <mrow> <mi>R</mi> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>M</mi> <mn>21</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mn>23</mn> </msub> <msub> <mi>v</mi> <mrow> <mi>R</mi> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>M</mi> <mn>22</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mover> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>~</mo> </mover> <mo>=</mo> <mn>0</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>
联立公式(13)和公式(14)可得：
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mn>13</mn> </msub> <msub> <mi>u</mi> <mrow> <mi>L</mi> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>M</mi> <mn>11</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mn>13</mn> </msub> <msub> <mi>v</mi> <mrow> <mi>L</mi> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>M</mi> <mn>12</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mn>23</mn> </msub> <msub> <mi>u</mi> <mrow> <mi>R</mi> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>M</mi> <mn>21</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mn>23</mn> </msub> <msub> <mi>v</mi> <mrow> <mi>R</mi> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>M</mi> <mn>22</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mover> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>~</mo> </mover> <mo>=</mo> <mn>0</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>
从公式(15)中可通过最小二乘原理求解出
设两摄像机所采集图像上得到的n个配对的特征点图像投影，特征点在测量坐标系下的三维坐标P＝{pi}(i＝1,…,n)经解算得到，在物体坐标系下的三维坐标Q＝{qi}(i＝1,…,n)已知，得到坐标系关系为：
P＝sRQ+T (16)
其中，s为比例系数；T为位移向量，是一个三维向量，即T＝[Tx,Ty,Tz]T；R为旋转矩阵，是一个3×3的单位正交矩阵；
若是理想的测量数据，可由所有点的信息直接求解公式(16)，得到s、R、T；由于会存在误差，则误差为：
ei＝pi-sRqi-T (17)
根据最小二乘理论，当所有测量误差的平方和最小时，能得到最合适的解；
假设该n个特征点的质心在测量坐标系和物体坐标系中的坐标分别为：
<mrow> <mtable> <mtr> <mtd> <mrow> <mover> <mi>p</mi> <mo>&amp;OverBar;</mo> </mover> <mo>=</mo> <mfrac> <mn>1</mn> <mi>n</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>p</mi> <mi>i</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mover> <mi>q</mi> <mo>&amp;OverBar;</mo> </mover> <mo>=</mo> <mfrac> <mn>1</mn> <mi>n</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>q</mi> <mi>i</mi> </msub> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>
得到分别以质心为原点的坐标系下的新坐标：
<mrow> <mtable> <mtr> <mtd> <mrow> <msup> <mi>p</mi> <mo>&amp;prime;</mo> </msup> <mo>=</mo> <msub> <mi>p</mi> <mi>i</mi> </msub> <mo>-</mo> <mover> <mi>p</mi> <mo>&amp;OverBar;</mo> </mover> </mrow> </mtd> <mtd> <mrow> <msup> <mi>q</mi> <mo>&amp;prime;</mo> </msup> <mo>=</mo> <msub> <mi>q</mi> <mi>i</mi> </msub> <mo>-</mo> <mover> <mi>q</mi> <mo>&amp;OverBar;</mo> </mover> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>
误差平方和可写为：
<mrow> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mo>|</mo> <mo>|</mo> <msub> <mi>e</mi> <mi>i</mi> </msub> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mo>|</mo> <mo>|</mo> <msub> <msup> <mi>p</mi> <mo>&amp;prime;</mo> </msup> <mi>i</mi> </msub> <mo>-</mo> <msub> <msup> <mi>sRq</mi> <mo>&amp;prime;</mo> </msup> <mi>i</mi> </msub> <mo>-</mo> <msup> <mi>T</mi> <mo>&amp;prime;</mo> </msup> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>
或者为：
<mrow> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mo>|</mo> <mo>|</mo> <msub> <mi>e</mi> <mi>i</mi> </msub> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mo>|</mo> <mo>|</mo> <msub> <msup> <mi>p</mi> <mo>&amp;prime;</mo> </msup> <mi>i</mi> </msub> <mo>-</mo> <msub> <msup> <mi>sRq</mi> <mo>&amp;prime;</mo> </msup> <mi>i</mi> </msub> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>-</mo> <mn>2</mn> <msup> <mi>T</mi> <mo>&amp;prime;</mo> </msup> <mo>&amp;CenterDot;</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <mrow> <mo>(</mo> <msub> <msup> <mi>p</mi> <mo>&amp;prime;</mo> </msup> <mi>i</mi> </msub> <mo>-</mo> <msub> <msup> <mi>sRq</mi> <mo>&amp;prime;</mo> </msup> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <mi>n</mi> <mo>|</mo> <mo>|</mo> <msup> <mi>T</mi> <mo>&amp;prime;</mo> </msup> <mo>|</mo> <msup> <mo>|</mo> <mn>2</mn> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow>
其中，
通过计算出比例系数和旋转向量，根据特征点质心在测量坐标系和物体坐标系中的坐标可计算出位移向量；
采用欧拉角法来表示相对姿态，采用z→y→x的旋转顺序，对应的旋转矩阵为：
<mrow> <mtable> <mtr> <mtd> <mrow> <mi>R</mi> <mrow> <mo>(</mo> <mrow> <mi>&amp;alpha;</mi> <mo>,</mo> <mi>&amp;beta;</mi> <mo>,</mo> <mi>&amp;theta;</mi> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>R</mi> <mi>Z</mi> </msub> <mrow> <mo>(</mo> <mi>&amp;theta;</mi> <mo>)</mo> </mrow> <msub> <mi>R</mi> <mi>Y</mi> </msub> <mrow> <mo>(</mo> <mi>&amp;beta;</mi> <mo>)</mo> </mrow> <msub> <mi>R</mi> <mi>X</mi> </msub> <mrow> <mo>(</mo> <mi>&amp;alpha;</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&amp;beta;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;beta;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>sin</mi> <mi>&amp;beta;</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;beta;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> <mi>cos</mi> <mi>&amp;beta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>cos</mi> <mi>&amp;alpha;</mi> <mo>+</mo> <mi>cos</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;beta;</mi> <mi>sin</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;alpha;</mi> <mo>-</mo> <mi>cos</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;beta;</mi> <mi>cos</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>cos</mi> <mi>&amp;beta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> <mi>cos</mi> <mi>&amp;alpha;</mi> <mo>-</mo> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;beta;</mi> <mi>sin</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;alpha;</mi> <mo>+</mo> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;beta;</mi> <mi>cos</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>sin</mi> <mi>&amp;beta;</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>cos</mi> <mi>&amp;beta;</mi> <mi>sin</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;beta;</mi> <mi>sin</mi> <mi>&amp;alpha;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>
R为旋转矩阵，是三个角度(α,β,θ)的三角函数组合；绕X轴旋转角α为俯仰角，绕Y轴旋转角β为偏航角，绕Z轴旋转角θ为滚动角；通过位姿求解可得到旋转矩阵(rij(i,j＝1,…,3)为矩阵第i行第j列的元素)，则根据公式(22)得到欧拉角为：
<mrow> <mi>&amp;alpha;</mi> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <mo>-</mo> <mfrac> <msub> <mi>r</mi> <mn>32</mn> </msub> <msub> <mi>r</mi> <mn>33</mn> </msub> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>23</mn> <mo>)</mo> </mrow> </mrow>
β＝arcsin(r13) (24)
<mrow> <mi>&amp;theta;</mi> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <mo>-</mo> <mfrac> <msub> <mi>r</mi> <mn>12</mn> </msub> <msub> <mi>r</mi> <mn>11</mn> </msub> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>25</mn> <mo>)</mo> </mrow> <mo>.</mo> </mrow>
CN201711033453.4A 2017-10-30 2017-10-30 基于超声波组网和立体视觉的agv导航定位方法 Pending CN108051007A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711033453.4A CN108051007A (zh) 2017-10-30 2017-10-30 基于超声波组网和立体视觉的agv导航定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711033453.4A CN108051007A (zh) 2017-10-30 2017-10-30 基于超声波组网和立体视觉的agv导航定位方法

Publications (1)

Publication Number Publication Date
CN108051007A true CN108051007A (zh) 2018-05-18

Family

ID=62118585

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711033453.4A Pending CN108051007A (zh) 2017-10-30 2017-10-30 基于超声波组网和立体视觉的agv导航定位方法

Country Status (1)

Country Link
CN (1) CN108051007A (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110133663A (zh) * 2019-05-08 2019-08-16 西安联丰迅声信息科技有限责任公司 一种分布式声像联合校准定位方法
WO2021004483A1 (zh) * 2019-07-11 2021-01-14 深圳市海柔创新科技有限公司 一种导航方法、移动载体及导航***
CN113110433A (zh) * 2021-04-02 2021-07-13 深圳优地科技有限公司 机器人姿态调整方法、装置、设备及存储介质
CN113490171A (zh) * 2021-08-11 2021-10-08 重庆大学 一种基于视觉标签的室内定位方法
CN113581720A (zh) * 2021-07-06 2021-11-02 浙江世仓智能仓储设备有限公司 一种用于立体穿梭库的四向穿梭车的行驶控制方法
CN113885510A (zh) * 2021-10-21 2022-01-04 齐鲁工业大学 一种四足机器人避障及领航员跟随方法及***
CN114524209A (zh) * 2021-12-21 2022-05-24 杭叉集团股份有限公司 一种基于双tof相机的agv高位堆叠方法及检测装置

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100042319A1 (en) * 2008-08-15 2010-02-18 Wu Chih-Jen Automatic ultrasonic and computer-vision navigation device and method using the same
CN101726296A (zh) * 2009-12-22 2010-06-09 哈尔滨工业大学 空间机器人视觉测量、路径规划、gnc一体化仿真***
CN104777835A (zh) * 2015-03-11 2015-07-15 武汉汉迪机器人科技有限公司 一种全向自动叉车及3d立体视觉导航定位方法
CN105841687A (zh) * 2015-01-14 2016-08-10 上海智乘网络科技有限公司 室内定位方法和***
CN106507478A (zh) * 2016-11-28 2017-03-15 成都理想境界科技有限公司 一种定位方法、定位网络和定位***

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100042319A1 (en) * 2008-08-15 2010-02-18 Wu Chih-Jen Automatic ultrasonic and computer-vision navigation device and method using the same
CN101726296A (zh) * 2009-12-22 2010-06-09 哈尔滨工业大学 空间机器人视觉测量、路径规划、gnc一体化仿真***
CN105841687A (zh) * 2015-01-14 2016-08-10 上海智乘网络科技有限公司 室内定位方法和***
CN104777835A (zh) * 2015-03-11 2015-07-15 武汉汉迪机器人科技有限公司 一种全向自动叉车及3d立体视觉导航定位方法
CN106507478A (zh) * 2016-11-28 2017-03-15 成都理想境界科技有限公司 一种定位方法、定位网络和定位***

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
胡春田: ""基于CCD图像的三维重建技术研究与实现"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
陈兴祁: ""基于超声波与视觉定位的智能小车导航技术研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110133663A (zh) * 2019-05-08 2019-08-16 西安联丰迅声信息科技有限责任公司 一种分布式声像联合校准定位方法
CN110133663B (zh) * 2019-05-08 2023-03-10 西安联丰迅声信息科技有限责任公司 一种分布式声像联合校准定位方法
WO2021004483A1 (zh) * 2019-07-11 2021-01-14 深圳市海柔创新科技有限公司 一种导航方法、移动载体及导航***
CN113110433A (zh) * 2021-04-02 2021-07-13 深圳优地科技有限公司 机器人姿态调整方法、装置、设备及存储介质
CN113110433B (zh) * 2021-04-02 2024-05-31 深圳优地科技有限公司 机器人姿态调整方法、装置、设备及存储介质
CN113581720A (zh) * 2021-07-06 2021-11-02 浙江世仓智能仓储设备有限公司 一种用于立体穿梭库的四向穿梭车的行驶控制方法
CN113490171A (zh) * 2021-08-11 2021-10-08 重庆大学 一种基于视觉标签的室内定位方法
CN113490171B (zh) * 2021-08-11 2022-05-13 重庆大学 一种基于视觉标签的室内定位方法
CN113885510A (zh) * 2021-10-21 2022-01-04 齐鲁工业大学 一种四足机器人避障及领航员跟随方法及***
CN113885510B (zh) * 2021-10-21 2023-11-10 齐鲁工业大学 一种四足机器人避障及领航员跟随方法及***
CN114524209A (zh) * 2021-12-21 2022-05-24 杭叉集团股份有限公司 一种基于双tof相机的agv高位堆叠方法及检测装置

Similar Documents

Publication Publication Date Title
CN108012325B (zh) 一种基于uwb和双目视觉的导航定位方法
CN108051007A (zh) 基于超声波组网和立体视觉的agv导航定位方法
US10809071B2 (en) Method for constructing a map while performing work
CN108571971B (zh) 一种agv视觉定位***及方法
CN109598765B (zh) 基于球形标定物的单目相机与毫米波雷达外参联合标定方法
CN103065323B (zh) 一种基于单应性变换矩阵的分段空间对准方法
WO2021026850A1 (zh) 一种基于二维码的导航定姿定位方法和***
Zhou A new minimal solution for the extrinsic calibration of a 2D LIDAR and a camera using three plane-line correspondences
EP3324209A1 (en) Methods and systems for vehicle environment map generation and updating
JP6767998B2 (ja) 画像の線からのカメラの外部パラメータ推定
CN103913162B (zh) 增强的移动平台定位
CN107980138B (zh) 一种虚警障碍物检测方法及装置
JP5992184B2 (ja) 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
US20190120934A1 (en) Three-dimensional alignment of radar and camera sensors
EP1394761A2 (en) Obstacle detection device and method therefor
KR102261791B1 (ko) 반사체 및 라이다를 기반으로 한 자율주행 도킹 시스템
KR20190134231A (ko) 차량의 위치 추정 장치, 차량의 위치 추정 방법, 및 이러한 방법을 수행하도록 프로그램된 컴퓨터 프로그램을 저장하는 컴퓨터 판독가능한 기록매체
CN114474056B (zh) 一种面向抓取操作的单目视觉高精度目标定位方法
EP3155369B1 (en) System and method for measuring a displacement of a mobile platform
CN113843798B (zh) 一种移动机器人抓取定位误差的纠正方法、***及机器人
CA3044322A1 (en) Self-calibrating sensor system for a wheeled vehicle
CN112669354A (zh) 一种基于车辆非完整约束的多相机运动状态估计方法
CN114413958A (zh) 无人物流车的单目视觉测距测速方法
Madsen et al. A robustness analysis of triangulation-based robot self-positioning
CN112611344B (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
WD01 Invention patent application deemed withdrawn after publication
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20180518