CN113706610A - 基于rgb-d相机的栈板位姿计算方法 - Google Patents

基于rgb-d相机的栈板位姿计算方法 Download PDF

Info

Publication number
CN113706610A
CN113706610A CN202111030399.4A CN202111030399A CN113706610A CN 113706610 A CN113706610 A CN 113706610A CN 202111030399 A CN202111030399 A CN 202111030399A CN 113706610 A CN113706610 A CN 113706610A
Authority
CN
China
Prior art keywords
coordinate system
forklift
point cloud
pallet
camera
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.)
Granted
Application number
CN202111030399.4A
Other languages
English (en)
Other versions
CN113706610B (zh
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.)
Jingxin Intelligent Technology Guangzhou Co ltd
Guangzhou Institute of Technology of Xidian University
Original Assignee
Jingxin Intelligent Technology Guangzhou Co ltd
Guangzhou Institute of Technology of Xidian University
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 Jingxin Intelligent Technology Guangzhou Co ltd, Guangzhou Institute of Technology of Xidian University filed Critical Jingxin Intelligent Technology Guangzhou Co ltd
Priority to CN202111030399.4A priority Critical patent/CN113706610B/zh
Priority claimed from CN202111030399.4A external-priority patent/CN113706610B/zh
Publication of CN113706610A publication Critical patent/CN113706610A/zh
Application granted granted Critical
Publication of CN113706610B publication Critical patent/CN113706610B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/20—Finite element generation, e.g. wire-frame surface description, tesselation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/60—Rotation of whole images or parts thereof
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/70—Denoising; Smoothing
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/13—Edge detection
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10024—Color image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明提供了一种基于RGB‑D相机的栈板位姿计算方法，建立叉车坐标系和图像坐标系，获取叉车坐标系x轴和y轴在图像坐标系上的灭点，基于灭点和叉车车臂顶点获取的直线计算旋转矩阵，获取叉车坐标系的两个不同点以及图像坐标系对应的坐标，通过最小二乘、平移矩阵和旋转矩阵得到叉车坐标系到RGB‑D相机坐标系转换公式；建立点云坐标系，计算叉车坐标系相对于点云坐标系的姿态；利用三自由度ICP计算预测帧到当前帧的转移矩阵，计算当前帧点云相对于叉车坐标系的姿态；优化获取的姿态。本发明减少了计算规模并且全程在CPU上运行，无需对栈板进行任何改造，并且对栈板具有一定的包容性，使用RGB‑D相机对栈板截面进行识别，减少了生产成本。

Description

基于RGB-D相机的栈板位姿计算方法
技术领域
本发明涉及智能物流技术领域，特别是涉及一种基于RGB-D相机的栈板位姿计算。
背景技术
近年来，随着传统的物流行业的升级，自动化物流成为了现代物流技术的发展趋势，与传统的人工物流技术相比自动化物流能节省大量物力并且极大的提高运输效率。在货物运输装在中智能叉车占据极大比例，叉车除了需要具有定位导航功能外还需要具有识别放置货物的栈板，并且能成功对准栈板装载货物以实现货物叉取的能力，完成精准的货物搬运。
智能叉车对准栈板是成功取得货物的关键，但是目前在智能叉车栈板识别对准领域仍存在大量问题。目前有三种主流栈板识别定位的方法，一种方法是通过提前获取栈板的详细位置，利用激光定位实时获取AGV位置坐标，自动对准栈板进行叉取，但是栈板一旦倾斜则会对准失败；或者是利用水平安放的激光雷达对栈板截面进行识别，但是使用了价格昂贵的激光雷达，不利于降低生产成本。另一种是通过添加人工标签的方法比如二维码或者其他具有显著特征的标志，但是这种方法需要手动对每一个待识别的栈板进行改造，人工成本比较高，并且容易受到栈板自身的影响，在栈板标签处损坏，沾有污渍则难以识别和对准。还有一种则是通过单目或双目摄像头通过图像分割的方法进行栈板识别，但是该方法受到光照等环境因素的影响，并且分割识别的程序运行需要大量的计算，往往难以达到实时的效果。
与之相比，RGB-D相机是通过将一定结构的红外光线投射到物体上再通过专门的红外摄像头接收，相机根据发射和返回的红外光获取目标的三维信息。RGB-D相机能够实时获取目标的三维信息，相关技术中提出基于ToF成像系统，获取关于待识别的栈板的栈板点云数据；从所述栈板点云数据中分离出地面成分和物体成分；基于点云区域增长算法，从对应所述物体成分的点云数据中分割出栈板前端面；从预先建立的栈板数据库中获取所述点云特征对应的相邻点云特征，根据所述点云特征与所述相邻点云特征确定目标旋转矩阵与目标平移矩阵，对栈板具有一定的包容性，对于缺失或者沾有污渍等人为标签难以处理的问题也能得到形状点云，并且不需要人工对栈板进行改造降低人工生产成本，不需要使用价格昂贵的激光雷达降低了生产成本。
但上述基于区域增长的算法依赖于地面标定的结果，导致栈板识别率下降；基于模板库的方法存在栈板不适应问题。
发明内容
针对现有技术中存在的缺陷，本发明的目的在于提供一种基于RGB-D相机的栈板位姿计算，解决现有的算法依赖于地面标定结果导致栈板识别率下降以及基于模板库的方法存在栈板不适应问题的技术缺陷。
为了达到上述目的，本发明所采用的具体技术方案如下：
一种基于RGB-D相机的栈板位姿计算方法，具体包括以下步骤：
(1)根据叉车叉臂建立叉车坐标系，RGB-D相机获取叉车图像并建立图像坐标系，获取叉车坐标系x轴和y轴在图像坐标系上的灭点，基于灭点和叉车车臂顶点获取的直线计算旋转矩阵，获取叉车坐标系的两个不同点以及图像坐标系对应的坐标，通过最小二乘、平移矩阵和旋转矩阵得到叉车坐标系到RGB-D相机坐标系转换公式；
(2)从深度图中获取点云信息并将其从相机坐标系转换至叉车坐标系，获取栈板截面点云并设为模板点云，建立点云坐标系，计算叉车坐标系相对于点云坐标系的姿态；
(3)获取当前帧的上一帧预测点云，将模板点云转移到当前帧的上一帧，利用三自由度ICP计算预测帧到当前帧的转移矩阵，更新模板点云到当前帧点云姿态，计算当前帧点云相对于叉车坐标系的姿态；
(4)优化步骤(3)获取的姿态。
优选的，步骤(1)具体包括以下步骤：
(1a)建立叉车坐标系，以叉臂中心与车头交汇处为坐标原点，叉臂指向为y轴正方向，垂直地面向上为z轴正方向，x方向由右手定则确定；RGB-D相机获取叉车图像并建立图像坐标系；
(1b)获取叉车坐标系y轴在图像坐标系上的灭点；
(1c)根据y轴的灭点计算旋转矩阵R＝[r1 r2 r3]的第二列r2；
(1d)获取叉车叉臂顶点的直线，利用x方向在直线上的灭点和旋转矩阵R的第二列r2正交计算得到R的第一列r1，旋转矩阵R的第三轮r3则通过r1和r2的叉积得到；
(1e)通过叉车实际尺寸获取在叉车坐标系下的两个不同的点P1 P2，以及对应点在图像上的坐标p1 p2，通过最小二乘、平移矩阵和旋转矩阵得到叉车坐标系到RGB-D相机坐标系转换公式。
优选的，步骤(1b)获取叉车坐标系y轴在图像坐标系上的灭点具体指的是
通过固定在叉车上的RGB-D相机获取一张叉车叉臂图像，通过高斯滤波灰度增强二值化方法对上述图像进行预处理，采用canny边缘检测得到叉臂的轮廓图，拟合出叉臂两端的直线，通过直线方程构造最小二乘，利用SVD分解得到y方向上灭点坐标。
优选的，步骤(1c)根据y轴的灭点计算旋转矩阵R＝[r1 r2 r3]的第二列r2具体指的是
根据y轴的灭点计算旋转矩阵R＝[r1 r2 r3]的第二列r2，服从如下公式
Figure BDA0003244961380000031
其中，[u v 1]T为获取的y轴上的灭点，K为已知的相机内参矩阵，T为平移矩阵，[xy z 1]T为y轴的灭点在叉车坐标系下的齐次坐标，为[0 1 0 0]T。
优选的，步骤(1e)具体包括
通过叉车实际尺寸获取在叉车坐标系下的两个不同的点P1 P2，以及对应点在图像上的坐标p1 p2，构造最小二乘：Ax＝b
其中
Figure BDA0003244961380000032
x＝[tx ty tz]T为平移矩阵，
Figure BDA0003244961380000033
Figure BDA0003244961380000034
fx，fy为相机在对应方向的焦距，cx，cy为图像坐标系到像素坐标系的偏移量。
优选的，步骤(2)具体包括以下步骤
(2a)通过RGB-D相机拍摄的深度图获取点云信息，将点云由相机坐标系转到叉车坐标系，利用直通滤波器去除地面上的点以及栈板以上的点；
(2b)通过先验信息获取叉车在地图上的坐标以及栈板中心在地图上的坐标，将栈板中心坐标转移到叉车坐标系，进行范围搜索获取栈板点云，计算点云的法向量，筛选出栈板截面，将此点云作为模板点云；获取栈板截面点云中心坐标，搜索与栈板中心点在z轴方向上距离相同的点，拟合出以中心坐标为坐标原点平行于地面且过中心点在栈板截面上的x轴方程，垂直于地面为z轴建立点云坐标系，并且计算此时叉车坐标系相对于点云坐标系的姿态记为Ttemp Rtemp。
优选的，步骤(3)具体包括以下步骤
(3a)获取当前帧的上一帧预测点云，通过模板点云到上一帧的转移矩阵Rcloud，Tcloud将模板点云转换到当前帧的上一帧，服从转移公式：
Figure BDA0003244961380000041
其中点[xt yt zt]T为叉车坐标系下的模板点云坐标，[xp yp zp]T为预测帧点云的坐标；
(3b)预测帧到当前帧的转移矩阵分别为Rpcloud，Tpcloud，根据以下公式获取预测帧点云：
Figure BDA0003244961380000042
其中，
Figure BDA0003244961380000043
角度α，β，γ分别为点云绕x、y、z轴的夹角；
构建齐次线性最小二乘：Ax＝b，
其中，
Figure BDA0003244961380000044
b＝((xc-xt)(yc-yt)...)T，x(θ tx ty)T；
获取此时的[θ tx ty][θ tx ty]，计算度量误差量
Edis＝[(xc-xt)2+(yc-yt)2]1/2
Etrans＝[(θt-θc)2+(txt-txc)2+(tyc-tyt)2]1/2
误差小于设定的阈值则得到此时的转移矩阵，误差不满足要求，更新Rpcloud,Tpcloud,重新计算最小二乘；
(3c)更新模板点云到当前点云姿态Rcloud，Tpcloud，
Figure BDA0003244961380000051
Figure BDA0003244961380000052
(3d)计算当前帧点云相对于叉车坐标系的姿态，R＝Rtemp*Rcloud，T＝Rcloud*Rtemp+Ttemp。
优选的，步骤(4)具体包括以下步骤
利用线性卡尔曼滤波获优化步骤(3)获取的姿态。
优选的，步骤(4)具体包括以下步骤
(4a)构建运动学方程，状态方程：xk＝Axk-1+Buk-1+qk-1，观测方程：yk＝Hyk-1+rk；其中qk-1，rk为系统噪声和测量噪声，服从均值为0协方差矩阵为Qk-1，Rk-1的高斯分布，xk为K时刻的状态量，yk为K时候观测量A,B为控制系统矩阵，H为观测系统参数，uk为K时刻的控制矩阵；
(4b)预测阶段：
Figure BDA0003244961380000053
控制矩阵
Figure BDA0003244961380000054
Δt＝1/fps，fps为帧率；
(4c)更新阶段：
Figure BDA0003244961380000055
Figure BDA0003244961380000056
其中，
Figure BDA0003244961380000057
y＝[θ tx ty]T，
Figure BDA0003244961380000058
K为卡尔曼增益。
本发明的有益效果在于：与通过单目或双目摄像头通过图像分割的方法进行栈板识别方法比较，分割识别的程序运行需要大量的计算，往往难以达到实时的效果，本发明减少了计算规模并且全程在CPU上运行。与通过添加人工标签的方法比如二维码或者其他具有显著特征的标志，但是这种方法需要手动对每一个待识别的栈板进行改造，人工成本比较高，本发明无需对栈板进行任何改造，并且对栈板具有一定的包容性(栈板具有一定程度的损坏影响不大)，与利用水平安放的激光雷达对栈板截面进行识别，但是使用了价格昂贵的激光雷达，本发明使用RGB-D相机，极大的减少了生产成本。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明一种基于RGB-D相机的栈板位姿计算方法的流程图；
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的其他实施例，都属于本发明保护的范围。
如图1所示，本发明提出了一种基于RGB-D相机的栈板位姿计算方法，具体包括以下步骤：
(1)根据叉车叉臂建立叉车坐标系，RGB-D相机获取叉车图像并建立图像坐标系，获取叉车坐标系x轴和y轴在图像坐标系上的灭点，基于灭点和叉车车臂顶点获取的直线计算旋转矩阵，获取叉车坐标系的两个不同点以及图像坐标系对应的坐标，通过最小二乘、平移矩阵和旋转矩阵得到叉车坐标系到RGB-D相机坐标系转换公式；
步骤(1)具体包括以下步骤：
(1a)建立叉车坐标系，以叉臂中心与车头交汇处为坐标原点，叉臂指向为y轴正方向，垂直地面向上为z轴正方向，x方向由右手定则确定；RGB-D相机获取叉车图像并建立图像坐标系；
(1b)获取叉车坐标系y轴在图像坐标系上的灭点；
步骤(1b)获取叉车坐标系y轴在图像坐标系上的灭点具体指的是
通过固定在叉车上的RGB-D相机获取一张叉车叉臂图像，通过高斯滤波灰度增强二值化方法对上述图像进行预处理，采用canny边缘检测得到叉臂的轮廓图，拟合出叉臂两端的直线，通过直线方程构造最小二乘，利用SVD分解得到y方向上灭点坐标。
(1c)根据y轴的灭点计算旋转矩阵R＝[r1 r2 r3]的第二列r2；
步骤(1c)根据y轴的灭点计算旋转矩阵R＝[r1 r2 r3]的第二列r2具体指的是
根据y轴的灭点计算旋转矩阵R＝[r1 r2 r3]的第二列r2，服从如下公式
Figure BDA0003244961380000071
其中，[u v 1]T为获取的y轴上的灭点，K为已知的相机内参矩阵，T为平移矩阵，[xy z 1]T为y轴的灭点在叉车坐标系下的齐次坐标，为[0 1 0 0]T。
(1d)获取叉车叉臂顶点的直线，利用x方向在直线上的灭点和旋转矩阵R的第二列r2正交计算得到R的第一列r1，旋转矩阵R的第三轮r3则通过r1和r2的叉积得到；
(1e)通过叉车实际尺寸获取在叉车坐标系下的两个不同的点P1 P2，以及对应点在图像上的坐标p1 p2，通过最小二乘、平移矩阵和旋转矩阵得到叉车坐标系到RGB-D相机坐标系转换公式。
该步骤具体包括
通过叉车实际尺寸获取在叉车坐标系下的两个不同的点P1 P2，以及对应点在图像上的坐标p1 p2，构造最小二乘：Ax＝b
其中
Figure BDA0003244961380000072
x＝[tx ty tz]T为平移矩阵，
Figure BDA0003244961380000073
Figure BDA0003244961380000074
fx，fy为相机在对应方向的焦距，cx，cy为图像坐标系到像素坐标系的偏移量。
(2)从深度图中获取点云信息并将其从相机坐标系转换至叉车坐标系，获取栈板截面点云并设为模板点云，建立点云坐标系，计算叉车坐标系相对于点云坐标系的姿态；
(2a)通过RGB-D相机拍摄的深度图获取点云信息，将点云由相机坐标系转到叉车坐标系，利用直通滤波器去除地面上的点以及栈板以上的点；
(2b)通过先验信息获取叉车在地图上的坐标以及栈板中心在地图上的坐标，将栈板中心坐标转移到叉车坐标系，进行范围搜索获取栈板点云，计算点云的法向量，筛选出栈板截面，将此点云作为模板点云；获取栈板截面点云中心坐标，搜索与栈板中心点在z轴方向上距离相同的点，拟合出以中心坐标为坐标原点平行于地面且过中心点在栈板截面上的x轴方程，垂直于地面为z轴建立点云坐标系，并且计算此时叉车坐标系相对于点云坐标系的姿态记为Ttemp Rtemp。
(3)获取当前帧的上一帧预测点云，将模板点云转移到当前帧的上一帧，利用三自由度ICP计算预测帧到当前帧的转移矩阵，更新模板点云到当前帧点云姿态，计算当前帧点云相对于叉车坐标系的姿态；
(3a)获取当前帧的上一帧预测点云，通过模板点云到上一帧的转移矩阵Rcloud，Tcloud将模板点云转换到当前帧的上一帧，服从转移公式：
Figure BDA0003244961380000081
其中点[xt yt zt]T为叉车坐标系下的模板点云坐标，[xp yp zp]T为预测帧点云的坐标；
(3b)利用三自由度ICP计算预测帧到当前帧的转移矩阵。预测帧到当前帧的转移矩阵分别为Rpcloud，Tpcloud，根据以下公式获取预测帧点云：
Figure BDA0003244961380000082
其中，
Figure BDA0003244961380000083
角度α，β，γ分别为点云绕x、y、z轴的夹角；考虑到叉车在水平地面移动时，角α，β为0，角γ为近于0。
Figure BDA0003244961380000084
Figure BDA0003244961380000091
构建齐次线性最小二乘：Ax＝b，
其中，
Figure BDA0003244961380000092
b＝((xc-xt) (yc-yt) ...)T，x(θ tx ty)T；
获取此时的[θ tx ty][θ tx ty]，计算度量误差量
Edis＝[(xc-xt)2+(yc-yt)2]1/2
Etrans＝[(θt-θc)2+(txt-txc)2+(tyc-tyt)2]1/2
误差小于设定的阈值则得到此时的转移矩阵，误差不满足要求，更新Rpcloud,Tpcloud,重新计算最小二乘；
(3c)更新模板点云到当前点云姿态Rcloud，Tpcloud，
Figure BDA0003244961380000093
Figure BDA0003244961380000094
(3d)计算当前帧点云相对于叉车坐标系的姿态，R＝Rtemp*Rcloud，T＝Rcloud*Rtemp+Ttemp。
(4)优化步骤(3)获取的姿态。
优选的，步骤(4)利用线性卡尔曼滤波获优化步骤(3)获取的姿态。为了减少姿态计算过程中由于ICP姿态计算过程中的匹配点的错误导致获得错误的姿态，设计线性卡尔曼滤波器，对姿态姿态参数[tx ty θ]进行线性滤波处理，将直接优化匹配点云以及姿态R,T的非线性问题转化为线性优化问题。具体包括以下步骤
(4a)构建运动学方程，状态方程：xk＝Axk-1+Buk-1+qk-1，观测方程：yk＝Hyk-1+rk；其中qk-1，rk为系统噪声和测量噪声，服从均值为0协方差矩阵为Qk-1，Rk-1的高斯分布，xk为K时刻的状态量，yk为K时候观测量A,B为控制系统矩阵，H为观测系统参数，uk为K时刻的控制矩阵；
(4b)预测阶段：
Figure BDA0003244961380000095
控制矩阵
Figure BDA0003244961380000101
Δt＝1/fps，fps为帧率；为了减少姿态计算过程中由于ICP姿态计算过程中的匹配点的错误导致获得错误的姿态，设计线性卡尔曼滤波器，对姿态姿态参数[tx ty θ]进行线性滤波处理，将直接优化匹配点云以及姿态R,T的非线性问题转化为线性优化问题。
(4c)更新阶段：
Figure BDA0003244961380000102
Figure BDA0003244961380000103
其中，
Figure BDA0003244961380000104
y＝[θ tx ty]T，
Figure BDA0003244961380000105
K为卡尔曼增益，随着运动过程中不断更新。
本发明的有益效果在于：与通过单目或双目摄像头通过图像分割的方法进行栈板识别方法比较，分割识别的程序运行需要大量的计算，往往难以达到实时的效果，本发明减少了计算规模并且全程在CPU上运行。与通过添加人工标签的方法比如二维码或者其他具有显著特征的标志，但是这种方法需要手动对每一个待识别的栈板进行改造，人工成本比较高，本发明无需对栈板进行任何改造，并且对栈板具有一定的包容性(栈板具有一定程度的损坏影响不大)，与利用水平安放的激光雷达对栈板截面进行识别，但是使用了价格昂贵的激光雷达，本发明使用RGB-D相机，极大的减少了生产成本。
以上述依据本发明的理想实施例为启示，通过上述的说明内容，本领域技术人员完全可以在不偏离本发明技术思想的范围内，进行多样的变更以及修改。本发明的技术性范围并不局限于说明书上的内容，必须要根据权利要求书范围来确定其技术性范围。

Claims (9)

1.一种基于RGB-D相机的栈板位姿计算方法，其特征在于，具体包括以下步骤：
(1)根据叉车叉臂建立叉车坐标系，RGB-D相机获取叉车图像并建立图像坐标系，获取叉车坐标系x轴和y轴在图像坐标系上的灭点，基于灭点和叉车车臂顶点获取的直线计算旋转矩阵，获取叉车坐标系的两个不同点以及图像坐标系对应的坐标，通过最小二乘、平移矩阵和旋转矩阵得到叉车坐标系到RGB-D相机坐标系转换公式；
(2)从深度图中获取点云信息并将其从相机坐标系转换至叉车坐标系，获取栈板截面点云并设为模板点云，建立点云坐标系，计算叉车坐标系相对于点云坐标系的姿态；
(3)获取当前帧的上一帧预测点云，将模板点云转移到当前帧的上一帧，利用三自由度ICP计算预测帧到当前帧的转移矩阵，更新模板点云到当前帧点云姿态，计算当前帧点云相对于叉车坐标系的姿态；
(4)优化步骤(3)获取的姿态。
2.根据权利要求1所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(1)具体包括以下步骤：
(1a)建立叉车坐标系，以叉臂中心与车头交汇处为坐标原点，叉臂指向为y轴正方向，垂直地面向上为z轴正方向，x方向由右手定则确定；RGB-D相机获取叉车图像并建立图像坐标系；
(1b)获取叉车坐标系y轴在图像坐标系上的灭点；
(1c)根据y轴的灭点计算旋转矩阵R＝[r1 r2 r3]的第二列r2；
(1d)获取叉车叉臂顶点的直线，利用x方向在直线上的灭点和旋转矩阵R的第二列r2正交计算得到R的第一列r1，旋转矩阵R的第三轮r3则通过r1和r2的叉积得到；
(1e)通过叉车实际尺寸获取在叉车坐标系下的两个不同的点P1 P2，以及对应点在图像上的坐标p1 p2，通过最小二乘、平移矩阵和旋转矩阵得到叉车坐标系到RGB-D相机坐标系转换公式。
3.根据权利要求2所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(1b)获取叉车坐标系y轴在图像坐标系上的灭点具体指的是
通过固定在叉车上的RGB-D相机获取一张叉车叉臂图像，通过高斯滤波灰度增强二值化方法对上述图像进行预处理，采用canny边缘检测得到叉臂的轮廓图，拟合出叉臂两端的直线，通过直线方程构造最小二乘，利用SVD分解得到y方向上灭点坐标。
4.根据权利要求2所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(1c)根据y轴的灭点计算旋转矩阵R＝[r1 r2 r3的第二列r2具体指的是
根据y轴的灭点计算旋转矩阵R＝[r1 r2 r3]的第二列r2，服从如下公式
Figure FDA0003244961370000021
其中，[u v 1]T为获取的y轴上的灭点，K为已知的相机内参矩阵，T为平移矩阵，[x y z1]T为y轴的灭点在叉车坐标系下的齐次坐标，为[0 1 0 0]T。
5.根据权利要求2所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(1e)具体包括
通过叉车实际尺寸获取在叉车坐标系下的两个不同的点P1 P2，以及对应点在图像上的坐标p1 p2，构造最小二乘：Ax＝b
其中
Figure FDA0003244961370000022
x＝[tx ty tz]T为平移矩阵，
Figure FDA0003244961370000023
Figure FDA0003244961370000024
fx，fy为相机在对应方向的焦距，cx，cy为图像坐标系到像素坐标系的偏移量。
6.根据权利要求1所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(2)具体包括以下步骤
(2a)通过RGB-D相机拍摄的深度图获取点云信息，将点云由相机坐标系转到叉车坐标系，利用直通滤波器去除地面上的点以及栈板以上的点；
(2b)通过先验信息获取叉车在地图上的坐标以及栈板中心在地图上的坐标，将栈板中心坐标转移到叉车坐标系，进行范围搜索获取栈板点云，计算点云的法向量，筛选出栈板截面，将此点云作为模板点云；获取栈板截面点云中心坐标，搜索与栈板中心点在z轴方向上距离相同的点，拟合出以中心坐标为坐标原点平行于地面且过中心点在栈板截面上的x轴方程，垂直于地面为z轴建立点云坐标系，并且计算此时叉车坐标系相对于点云坐标系的姿态记为TtempRtemp。
7.根据权利要求1所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(3)具体包括以下步骤
(3a)获取当前帧的上一帧预测点云，通过模板点云到上一帧的转移矩阵Rcloud，Tcloud将模板点云转换到当前帧的上一帧，服从转移公式：
Figure FDA0003244961370000031
其中点[xt yt zt]T为叉车坐标系下的模板点云坐标，[xp yp zp]T为预测帧点云的坐标；
(3b)预测帧到当前帧的转移矩阵分别为Rpcloud，Tpcloud，根据以下公式获取预测帧点云：
Figure FDA0003244961370000032
其中，
Figure FDA0003244961370000033
角度α，β，γ分别为点云绕x、y、z轴的夹角；
构建齐次线性最小二乘：Ax＝b，
其中，
Figure FDA0003244961370000034
b＝((xc-xt)(yc-yt)...)T，x(θ tx ty)T；
获取此时的[θ tx ty][θ tx ty]，计算度量误差量
Edis＝[(xc-xt)2+(yc-yt)2]1/2
Etrans＝[(θt-θc)2+(txt-txc)2+(tyc-tyt)2]1/2
误差小于设定的阈值则得到此时的转移矩阵，误差不满足要求，更新Rpcloud,Tpcloud,重新计算最小二乘；
(3c)更新模板点云到当前点云姿态Rcloud，Tpcloud，
Figure FDA0003244961370000035
Figure FDA0003244961370000036
(3d)计算当前帧点云相对于叉车坐标系的姿态，R＝Rtemp*Rcloud，T＝Rcloud*Rtemp+Ttemp。
8.根据权利要求1所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(4)具体包括以下步骤
(4)利用线性卡尔曼滤波获优化步骤(3)获取的姿态。
9.根据权利要求8所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(4)具体包括以下步骤
(4a)构建运动学方程，状态方程：xk＝Axk-1+Buk-1+qk-1，观测方程：yk＝Hyk-1+rk；其中qk-1，rk为系统噪声和测量噪声，服从均值为0协方差矩阵为Qk-1，Rk-1的高斯分布，xk为K时刻的状态量，yk为K时候观测量A,B为控制系统矩阵，H为观测系统参数，uk为K时刻的控制矩阵；
(4b)预测阶段：
Figure FDA0003244961370000041
控制矩阵
Figure FDA0003244961370000042
Δt＝1/fps，fps为帧率；
(4c)更新阶段：
Figure FDA0003244961370000043
Figure FDA0003244961370000044
其中，
Figure FDA0003244961370000045
y＝[θ tx ty]T，
Figure FDA0003244961370000046
K为卡尔曼增益。
CN202111030399.4A 2021-09-03 基于rgb-d相机的栈板位姿计算方法 Active CN113706610B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111030399.4A CN113706610B (zh) 2021-09-03 基于rgb-d相机的栈板位姿计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111030399.4A CN113706610B (zh) 2021-09-03 基于rgb-d相机的栈板位姿计算方法

Publications (2)

Publication Number Publication Date
CN113706610A true CN113706610A (zh) 2021-11-26
CN113706610B CN113706610B (zh) 2024-06-07

Family

ID=

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114758163A (zh) * 2022-06-15 2022-07-15 福勤智能科技（昆山）有限公司 一种叉车移动控制方法、装置、电子设备及存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105809706A (zh) * 2016-05-25 2016-07-27 北京航空航天大学 一种分布式多像机系统的全局标定方法
US20180211399A1 (en) * 2017-01-26 2018-07-26 Samsung Electronics Co., Ltd. Modeling method and apparatus using three-dimensional (3d) point cloud
CN110014426A (zh) * 2019-03-21 2019-07-16 同济大学 一种利用低精度深度相机高精度抓取形状对称工件的方法
CN111775152A (zh) * 2020-06-29 2020-10-16 深圳大学 基于三维测量引导机械臂抓取散乱堆叠工件的方法及系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105809706A (zh) * 2016-05-25 2016-07-27 北京航空航天大学 一种分布式多像机系统的全局标定方法
US20180211399A1 (en) * 2017-01-26 2018-07-26 Samsung Electronics Co., Ltd. Modeling method and apparatus using three-dimensional (3d) point cloud
CN110014426A (zh) * 2019-03-21 2019-07-16 同济大学 一种利用低精度深度相机高精度抓取形状对称工件的方法
CN111775152A (zh) * 2020-06-29 2020-10-16 深圳大学 基于三维测量引导机械臂抓取散乱堆叠工件的方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
CHAO WANG ET.AL: "Feature-based RGB-D camera pose optimization for real-time 3D reconstruction", COMPUTATIONAL VISUAL MEDIA, vol. 3, no. 2, pages 95 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114758163A (zh) * 2022-06-15 2022-07-15 福勤智能科技（昆山）有限公司 一种叉车移动控制方法、装置、电子设备及存储介质
CN114758163B (zh) * 2022-06-15 2022-09-16 福勤智能科技（昆山）有限公司 一种叉车移动控制方法、装置、电子设备及存储介质

Similar Documents

Publication Publication Date Title
CN112258618B (zh) 基于先验激光点云与深度图融合的语义建图与定位方法
CN109345588B (zh) 一种基于Tag的六自由度姿态估计方法
CN111598952B (zh) 一种多尺度合作靶标设计与在线检测识别方法及系统
KR102397508B1 (ko) 오버헤드 라이트 기반 배치를 갖는 산업용 차량
CN111260289A (zh) 一种基于视觉导航的微型无人机仓库盘点系统及方法
CN112184765B (zh) 一种用于水下航行器的自主跟踪方法
CN112880562A (zh) 一种机械臂末端位姿误差测量方法及系统
CN115609591B (zh) 一种基于2D Marker的视觉定位方法和系统、复合机器人
CN114331986A (zh) 一种基于无人机视觉的坝体裂纹识别与测量方法
CN115546202B (zh) 一种用于无人叉车的托盘检测与定位方法
CN111784655A (zh) 一种水下机器人回收定位方法
Wang et al. Autonomous landing of multi-rotors UAV with monocular gimbaled camera on moving vehicle
CN115014371A (zh) 一种用于智慧粮库粮食转运车的定位与建图方法、装置及存储介质
CN114862301A (zh) 基于二维码辅助定位的托盘叉车agv自动装载方法
CN113267180A (zh) 一种基于3d深度视觉的agv叉车托盘定位及叉取方法
CN114241269A (zh) 一种用于岸桥自动控制的集卡视觉融合定位系统
CN116128841A (zh) 托盘位姿检测方法及装置、无人叉车、存储介质
CN116503803A (zh) 障碍物检测方法、装置、电子设备以及存储介质
KR102490521B1 (ko) 라이다 좌표계와 카메라 좌표계의 벡터 정합을 통한 자동 캘리브레이션 방법
CN112581519B (zh) 一种放射性废物包识别定位方法与装置
CN113706610A (zh) 基于rgb-d相机的栈板位姿计算方法
CN113706610B (zh) 基于rgb-d相机的栈板位姿计算方法
CN111932617A (zh) 一种对规则物体实现实时检测定位方法和系统
CN116185049A (zh) 基于视觉引导的无人直升机自主着舰方法
CN115272465A (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
GR01 Patent grant