CN113706610B - 基于rgb-d相机的栈板位姿计算方法 - Google Patents
基于rgb-d相机的栈板位姿计算方法 Download PDFInfo
- Publication number
- CN113706610B CN113706610B CN202111030399.4A CN202111030399A CN113706610B CN 113706610 B CN113706610 B CN 113706610B CN 202111030399 A CN202111030399 A CN 202111030399A CN 113706610 B CN113706610 B CN 113706610B
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 47
- 239000011159 matrix material Substances 0.000 claims abstract description 70
- 238000012546 transfer Methods 0.000 claims abstract description 17
- 238000004364 calculation method Methods 0.000 claims abstract description 16
- 238000013519 translation Methods 0.000 claims abstract description 14
- 238000006243 chemical reaction Methods 0.000 claims abstract description 7
- 238000001914 filtration Methods 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 6
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 238000009826 distribution Methods 0.000 claims description 3
- 238000003708 edge detection Methods 0.000 claims description 3
- 230000003137 locomotive effect Effects 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 239000013598 vector Substances 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims 1
- 238000004519 manufacturing process Methods 0.000 abstract description 6
- 230000036544 posture Effects 0.000 description 22
- 230000008569 process Effects 0.000 description 8
- 230000009286 beneficial effect Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000003709 image segmentation Methods 0.000 description 3
- 230000011218 segmentation Effects 0.000 description 3
- 230000007547 defect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
Abstract
本发明提供了一种基于RGB‑D相机的栈板位姿计算方法，建立叉车坐标系和图像坐标系，获取叉车坐标系x轴和y轴在图像坐标系上的灭点，基于灭点和叉车车臂顶点获取的直线计算旋转矩阵，获取叉车坐标系的两个不同点以及图像坐标系对应的坐标，通过最小二乘、平移矩阵和旋转矩阵得到叉车坐标系到RGB‑D相机坐标系转换公式；建立点云坐标系，计算叉车坐标系相对于点云坐标系的姿态；利用三自由度ICP计算预测帧到当前帧的转移矩阵，计算当前帧点云相对于叉车坐标系的姿态；优化获取的姿态。本发明减少了计算规模并且全程在CPU上运行，无需对栈板进行任何改造，并且对栈板具有一定的包容性，使用RGB‑D相机对栈板截面进行识别，减少了生产成本。
Description
技术领域
本发明涉及智能物流技术领域，特别是涉及一种基于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，服从如下公式
其中，[u v 1]T为获取的y轴上的灭点，K为已知的相机内参矩阵，T为平移矩阵，[xy z 1]T为y轴的灭点在叉车坐标系下的齐次坐标，为[0 1 0 0]T。
优选的，步骤(1e)具体包括
通过叉车实际尺寸获取在叉车坐标系下的两个不同的点P1 P2，以及对应点在图像上的坐标p1 p2，构造最小二乘：Ax＝b
其中x＝[tx ty tz]T为平移矩阵，/> fx，fy为相机在对应方向的焦距，cx，cy为图像坐标系到像素坐标系的偏移量。
优选的，步骤(2)具体包括以下步骤
(2a)通过RGB-D相机拍摄的深度图获取点云信息，将点云由相机坐标系转到叉车坐标系，利用直通滤波器去除地面上的点以及栈板以上的点；
(2b)通过先验信息获取叉车在地图上的坐标以及栈板中心在地图上的坐标，将栈板中心坐标转移到叉车坐标系，进行范围搜索获取栈板点云，计算点云的法向量，筛选出栈板截面，将此点云作为模板点云；获取栈板截面点云中心坐标，搜索与栈板中心点在z轴方向上距离相同的点，拟合出以中心坐标为坐标原点平行于地面且过中心点在栈板截面上的x轴方程，垂直于地面为z轴建立点云坐标系，并且计算此时叉车坐标系相对于点云坐标系的姿态记为Ttemp Rtemp。
优选的，步骤(3)具体包括以下步骤
(3a)获取当前帧的上一帧预测点云，通过模板点云到上一帧的转移矩阵Rcloud，Tcloud将模板点云转换到当前帧的上一帧，服从转移公式：
其中点[xt yt zt]T为叉车坐标系下的模板点云坐标，[xp yp zp]T为预测帧点云的坐标；
(3b)预测帧到当前帧的转移矩阵分别为Rpcloud，Tpcloud，根据以下公式获取预测帧点云：
其中，
角度α，β，γ分别为点云绕x、y、z轴的夹角；
构建齐次线性最小二乘：Ax＝b，
其中，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，
(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)预测阶段：控制矩阵Δt＝1/fps，fps为帧率；
(4c)更新阶段： 其中，/>y＝[θ tx ty]T，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，服从如下公式
其中，[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
其中x＝[tx ty tz]T为平移矩阵，/> fx，fy为相机在对应方向的焦距，cx，cy为图像坐标系到像素坐标系的偏移量。
(2)从深度图中获取点云信息并将其从相机坐标系转换至叉车坐标系，获取栈板截面点云并设为模板点云，建立点云坐标系，计算叉车坐标系相对于点云坐标系的姿态；
(2a)通过RGB-D相机拍摄的深度图获取点云信息，将点云由相机坐标系转到叉车坐标系，利用直通滤波器去除地面上的点以及栈板以上的点；
(2b)通过先验信息获取叉车在地图上的坐标以及栈板中心在地图上的坐标，将栈板中心坐标转移到叉车坐标系，进行范围搜索获取栈板点云，计算点云的法向量，筛选出栈板截面，将此点云作为模板点云；获取栈板截面点云中心坐标，搜索与栈板中心点在z轴方向上距离相同的点，拟合出以中心坐标为坐标原点平行于地面且过中心点在栈板截面上的x轴方程，垂直于地面为z轴建立点云坐标系，并且计算此时叉车坐标系相对于点云坐标系的姿态记为Ttemp Rtemp。
(3)获取当前帧的上一帧预测点云，将模板点云转移到当前帧的上一帧，利用三自由度ICP计算预测帧到当前帧的转移矩阵，更新模板点云到当前帧点云姿态，计算当前帧点云相对于叉车坐标系的姿态；
(3a)获取当前帧的上一帧预测点云，通过模板点云到上一帧的转移矩阵Rcloud，Tcloud将模板点云转换到当前帧的上一帧，服从转移公式：
其中点[xt yt zt]T为叉车坐标系下的模板点云坐标，[xp yp zp]T为预测帧点云的坐标；
(3b)利用三自由度ICP计算预测帧到当前帧的转移矩阵。预测帧到当前帧的转移矩阵分别为Rpcloud，Tpcloud，根据以下公式获取预测帧点云：
其中，
角度α，β，γ分别为点云绕x、y、z轴的夹角；考虑到叉车在水平地面移动时，角α，β为0，角γ为近于0。
构建齐次线性最小二乘：Ax＝b，
其中，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，
(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)预测阶段：控制矩阵Δt＝1/fps，fps为帧率；为了减少姿态计算过程中由于ICP姿态计算过程中的匹配点的错误导致获得错误的姿态，设计线性卡尔曼滤波器，对姿态姿态参数[tx ty θ]进行线性滤波处理，将直接优化匹配点云以及姿态R,T的非线性问题转化为线性优化问题。
(4c)更新阶段： 其中，/>y＝[θ tx ty]T，K为卡尔曼增益，随着运动过程中不断更新。
本发明的有益效果在于：与通过单目或双目摄像头通过图像分割的方法进行栈板识别方法比较，分割识别的程序运行需要大量的计算，往往难以达到实时的效果，本发明减少了计算规模并且全程在CPU上运行。与通过添加人工标签的方法比如二维码或者其他具有显著特征的标志，但是这种方法需要手动对每一个待识别的栈板进行改造，人工成本比较高，本发明无需对栈板进行任何改造，并且对栈板具有一定的包容性(栈板具有一定程度的损坏影响不大)，与利用水平安放的激光雷达对栈板截面进行识别，但是使用了价格昂贵的激光雷达，本发明使用RGB-D相机，极大的减少了生产成本。
以上述依据本发明的理想实施例为启示，通过上述的说明内容，本领域技术人员完全可以在不偏离本发明技术思想的范围内，进行多样的变更以及修改。本发明的技术性范围并不局限于说明书上的内容，必须要根据权利要求书范围来确定其技术性范围。
Claims (7)
1.一种基于RGB-D相机的栈板位姿计算方法，其特征在于，具体包括以下步骤：
(1)根据叉车叉臂建立叉车坐标系，RGB-D相机获取叉车图像并建立图像坐标系，获取叉车坐标系x轴和y轴在图像坐标系上的灭点，基于灭点和叉车车臂顶点获取的直线计算旋转矩阵，获取叉车坐标系的两个不同点以及图像坐标系对应的坐标，通过最小二乘、平移矩阵和旋转矩阵得到叉车坐标系到RGB-D相机坐标系转换公式；具体包括以下步骤：
(1a)建立叉车坐标系，以叉臂中心与车头交汇处为坐标原点，叉臂指向为y轴正方向，垂直地面向上为z轴正方向，x方向由右手定则确定；RGB-D相机获取叉车图像并建立图像坐标系；
(1b)获取叉车坐标系y轴在图像坐标系上的灭点，所述灭点具体指：
通过固定在叉车上的RGB-D相机获取一张叉车叉臂图像，通过高斯滤波灰度增强二值化方法对上述图像进行预处理，采用canny边缘检测得到叉臂的轮廓图，拟合出叉臂两端的直线，通过直线方程构造最小二乘，利用SVD分解得到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相机坐标系转换公式；
(2)从深度图中获取点云信息并将其从相机坐标系转换至叉车坐标系，获取栈板截面点云并设为模板点云，建立点云坐标系，计算叉车坐标系相对于点云坐标系的姿态；
(3)获取当前帧的上一帧预测点云，将模板点云转移到当前帧的上一帧，利用三自由度ICP计算预测帧到当前帧的转移矩阵，更新模板点云到当前帧点云姿态，计算当前帧点云相对于叉车坐标系的姿态；
(4)优化步骤(3)获取的姿态。
2.根据权利要求1所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(1c)根据y轴的灭点计算旋转矩阵R＝[r1 r2 r3]的第二列r2具体指的是
根据y轴的灭点计算旋转矩阵R＝[r1 r2 r3]的第二列r2，服从如下公式
其中，[u v 1]T为获取的y轴上的灭点，K为已知的相机内参矩阵，T为平移矩阵，[x y z1]T为y轴的灭点在叉车坐标系下的齐次坐标，为[0 1 0 0]T。
3.根据权利要求1所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(1e)具体包括
通过叉车实际尺寸获取在叉车坐标系下的两个不同的点P1 P2，以及对应点在图像上的坐标p1 p2，构造最小二乘：Ax＝b
其中x＝[tx ty tz]T为平移矩阵，/> fx，fy为相机在对应方向的焦距，cx，cy为图像坐标系到像素坐标系的偏移量。
4.根据权利要求1所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(2)具体包括以下步骤
(2a)通过RGB-D相机拍摄的深度图获取点云信息，将点云由相机坐标系转到叉车坐标系，利用直通滤波器去除地面上的点以及栈板以上的点；
(2b)通过先验信息获取叉车在地图上的坐标以及栈板中心在地图上的坐标，将栈板中心坐标转移到叉车坐标系，进行范围搜索获取栈板点云，计算点云的法向量，筛选出栈板截面，将此点云作为模板点云；获取栈板截面点云中心坐标，搜索与栈板中心点在z轴方向上距离相同的点，拟合出以中心坐标为坐标原点平行于地面且过中心点在栈板截面上的x轴方程，垂直于地面为z轴建立点云坐标系，并且计算此时叉车坐标系相对于点云坐标系的姿态记为Ttemp Rtemp。
5.根据权利要求1所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(3)具体包括以下步骤
(3a)获取当前帧的上一帧预测点云，通过模板点云到上一帧的转移矩阵Rcloud，Tcloud将模板点云转换到当前帧的上一帧，服从转移公式：
其中点[xt yt zt]T为叉车坐标系下的模板点云坐标，[xp yp zp]T为预测帧点云的坐标；
(3b)预测帧到当前帧的转移矩阵分别为Rpcloud，Tpcloud，根据以下公式获取预测帧点云：
其中，角度α，β，γ分别为点云绕x、y、z轴的夹角；
构建齐次线性最小二乘：Ax＝b，
其中，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，
(3d)计算当前帧点云相对于叉车坐标系的姿态，R＝Rtemp*Rcloud，T＝Rcloud*Rtemp+Ttemp。
6.根据权利要求1所述的基于RGB-D相机的栈板位姿计算方法，其特征在于，步骤(4)具体包括以下步骤
(4)利用线性卡尔曼滤波获优化步骤(3)获取的姿态。
7.根据权利要求6所述的基于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)预测阶段：控制矩阵Δt＝1/fps，fps为帧率；
(4c)更新阶段： 其中，/>y＝[θ tx ty]T，K为卡尔曼增益。
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 CN113706610A (zh) | 2021-11-26 |
| CN113706610B true CN113706610B (zh) | 2024-06-07 |
Family
ID=
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105809706A (zh) * | 2016-05-25 | 2016-07-27 | 北京航空航天大学 | 一种分布式多像机系统的全局标定方法 |
| CN110014426A (zh) * | 2019-03-21 | 2019-07-16 | 同济大学 | 一种利用低精度深度相机高精度抓取形状对称工件的方法 |
| CN111775152A (zh) * | 2020-06-29 | 2020-10-16 | 深圳大学 | 基于三维测量引导机械臂抓取散乱堆叠工件的方法及系统 |
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105809706A (zh) * | 2016-05-25 | 2016-07-27 | 北京航空航天大学 | 一种分布式多像机系统的全局标定方法 |
| CN110014426A (zh) * | 2019-03-21 | 2019-07-16 | 同济大学 | 一种利用低精度深度相机高精度抓取形状对称工件的方法 |
| CN111775152A (zh) * | 2020-06-29 | 2020-10-16 | 深圳大学 | 基于三维测量引导机械臂抓取散乱堆叠工件的方法及系统 |
Non-Patent Citations (1)
| Title |
|---|
| Feature-based RGB-D camera pose optimization for real-time 3D reconstruction;Chao Wang et.al;Computational Visual Media;第3卷(第2期);第95–106页 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR102397508B1 (ko) | 오버헤드 라이트 기반 배치를 갖는 산업용 차량 | |
| CN110243380B (zh) | 一种基于多传感器数据与角度特征识别的地图匹配方法 | |
| CN111598952B (zh) | 一种多尺度合作靶标设计与在线检测识别方法及系统 | |
| CN111260289A (zh) | 一种基于视觉导航的微型无人机仓库盘点系统及方法 | |
| CN111767780B (zh) | 一种ai与视觉结合的智能集卡定位方法和系统 | |
| CN115609591B (zh) | 一种基于2D Marker的视觉定位方法和系统、复合机器人 | |
| CN112184765B (zh) | 一种用于水下航行器的自主跟踪方法 | |
| CN112880562A (zh) | 一种机械臂末端位姿误差测量方法及系统 | |
| CN114241269A (zh) | 一种用于岸桥自动控制的集卡视觉融合定位系统 | |
| CN116486287A (zh) | 基于环境自适应机器人视觉系统的目标检测方法及系统 | |
| CN111167731B (zh) | 一种产品分拣方法、产品分拣系统和智能分拣机器人 | |
| KR102490521B1 (ko) | 라이다 좌표계와 카메라 좌표계의 벡터 정합을 통한 자동 캘리브레이션 방법 | |
| CN110889871A (zh) | 机器人行驶方法、装置及机器人 | |
| US20240051146A1 (en) | Autonomous solar installation using artificial intelligence | |
| CN113706610B (zh) | 基于rgb-d相机的栈板位姿计算方法 | |
| CN113378701A (zh) | 一种基于无人机的地面多agv状态监测方法 | |
| CN116863371A (zh) | 一种基于深度学习的agv叉车货物托盘位姿识别方法 | |
| CN113706610A (zh) | 基于rgb-d相机的栈板位姿计算方法 | |
| CN115600118A (zh) | 基于二维激光点云的托盘腿识别方法及系统 | |
| CN115439821A (zh) | 基于多目视觉agv的定位与跟踪方法及系统 | |
| CN115586552A (zh) | 一种港口轮胎吊或桥吊下无人集卡精确二次定位方法 | |
| CN111854678B (zh) | 一种单目视觉下基于语义分割和卡尔曼滤波的位姿测量方法 | |
| CN114758163A (zh) | 一种叉车移动控制方法、装置、电子设备及存储介质 | |
| CN115082395A (zh) | 一种航空行李自动识别系统及方法 | |
| CN114237280A (zh) | 一种无人机精准降落机巢平台的方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant |