CN118154677A - 一种非合作目标位姿估计方法、系统及存储介质 - Google Patents
一种非合作目标位姿估计方法、系统及存储介质 Download PDFInfo
- Publication number
- CN118154677A CN118154677A CN202410580555.1A CN202410580555A CN118154677A CN 118154677 A CN118154677 A CN 118154677A CN 202410580555 A CN202410580555 A CN 202410580555A CN 118154677 A CN118154677 A CN 118154677A
- Authority
- CN
- China
- Prior art keywords
- point
- target
- points
- point cloud
- interference
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 67
- 238000003860 storage Methods 0.000 title claims abstract description 16
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 37
- 238000009826 distribution Methods 0.000 claims abstract description 25
- 230000008859 change Effects 0.000 claims abstract description 19
- 238000012804 iterative process Methods 0.000 claims abstract description 9
- 230000006870 function Effects 0.000 claims description 39
- 238000001514 detection method Methods 0.000 claims description 33
- 238000013519 translation Methods 0.000 claims description 21
- 230000009466 transformation Effects 0.000 claims description 19
- 239000011159 matrix material Substances 0.000 claims description 15
- 230000004913 activation Effects 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 9
- 238000013135 deep learning Methods 0.000 claims description 8
- 238000000605 extraction Methods 0.000 claims description 8
- 230000000007 visual effect Effects 0.000 claims description 8
- 230000004927 fusion Effects 0.000 claims description 7
- 230000001419 dependent effect Effects 0.000 claims description 6
- 238000005315 distribution function Methods 0.000 claims description 6
- 238000004590 computer program Methods 0.000 claims description 4
- 238000000354 decomposition reaction Methods 0.000 claims description 4
- 238000003384 imaging method Methods 0.000 claims description 4
- 230000003993 interaction Effects 0.000 claims description 4
- 230000003313 weakening effect Effects 0.000 abstract description 2
- 238000012546 transfer Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 238000005259 measurement Methods 0.000 description 4
- 238000012549 training Methods 0.000 description 3
- 238000007796 conventional method Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000011176 pooling Methods 0.000 description 2
- 238000012216 screening Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000013100 final test Methods 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 238000011423 initialization method Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/20—Image preprocessing
- G06V10/24—Aligning, centring, orientation detection or correction of the image
- G06V10/245—Aligning, centring, orientation detection or correction of the image by locating a pattern; Special marks for positioning
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/20—Image preprocessing
- G06V10/25—Determination of region of interest [ROI] or a volume of interest [VOI]
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/82—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using neural networks
-
- 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/20—Special algorithmic details
- G06T2207/20084—Artificial neural networks [ANN]
-
- 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/20—Special algorithmic details
- G06T2207/20092—Interactive image processing based on input by user
- G06T2207/20104—Interactive definition of region of interest [ROI]
-
- 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/30244—Camera pose
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Physics & Mathematics (AREA)
- Multimedia (AREA)
- Evolutionary Computation (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Databases & Information Systems (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- General Health & Medical Sciences (AREA)
- Computing Systems (AREA)
- Artificial Intelligence (AREA)
- Health & Medical Sciences (AREA)
- Image Processing (AREA)
Abstract
本发明公开了一种非合作目标位姿估计方法、系统及存储介质，目标位姿估计方法包括：对非合作目标点云进行三维重建；获取目标ROI区域；获取目标点云的干扰点，将干扰点分成远干扰点与近干扰点，对于远干扰点，在配准前运用离群点剔除算法进行分离与剔除；对于近干扰点，在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变，并将其带入到迭代过程的运算中，完成对目标位姿的估计。基于质心距变条件下高斯分布权值的迭代最近点方法，以此起到对近干扰点的弱化，提高对仓储机器人工作环境中非合作目标位姿估计的精确性与鲁棒性。
Description
技术领域
本发明属于机器人视觉检测与定位技术领域，本发明涉及一种非合作目标位姿估计方法、系统及存储介质。
背景技术
随着智能元素不断融入到工业产线中，智能仓储、智能搬运成为及其重要的环节。在仓储搬运机器人(Warehouse Conveying Robots，WCR)工作过程中，首先要完成待搬运目标的定位功能。一些学者采用基于特征信息融合的非合作目标相对位姿测量方法，运用椭圆轮廓进行识别和定位。此外，部分研究人员采用多视图初始化方法，通过回环检测技术提高机器人位姿测量精度。在此基础上，通过极线约束与光流法辅助来跟踪目标上的点特征，最后通过三维重建得到相对位姿信息。然而被搬运对象的形状、特征、体积等各不相同，难以保证特征信息的有效性和多样性。为了实现非合作目标模式下的视觉定位功能，常用的迭代最近点算法可以配准点云并完成三维重建，然而搬运机器人工作场景存在的干扰点会影响配准与重建的精度。
发明内容
针对仓储搬运机器人导航中对非合作目标点云位姿估计因干扰点导致的配准误差大、位姿求取精度差等问题，本发明的目的在于提供一种非合作目标位姿估计方法、系统及存储介质，基于质心距变条件下高斯分布权值的迭代最近点方法，以此提高对仓储机器人工作环境中非合作目标位姿估计的精确性与鲁棒性。
实现本发明目的的技术解决方案为：
一种非合作目标位姿估计方法，包括以下步骤：
S01：对非合作目标点云进行三维重建；
S02：获取目标ROI区域；
S03：获取目标点云的干扰点，将干扰点分成远干扰点与近干扰点，对于远干扰点，在配准前运用离群点剔除算法进行分离与剔除；对于近干扰点，在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变，并将其带入到迭代过程的运算中，完成对目标位姿的估计。
优选的技术方案中，所述步骤S01中对非合作目标点云进行三维重建的方法包括：
S11：通过特征点提取与匹配算法得到图像特征点对，当空间某一点在多个相机视角中出现时，采用三角测量法重建出空间点的三维坐标；
S12：双目视觉系统中的两个相机从两个不同视角采集图像，世界坐标系下的三维点通过投影变换得到二维像素坐标，根据相机成像模型列出方程如下：
其中，与分别为空间点通过左右相机成像后的二维像素坐标，、分别为左、右相机；为相机内参，通过相机标定求取；为空间点在世界坐标系下的待求坐标；为相机外参；与分别为左右相机采集此点的深度信息；
步骤13：令投影矩阵，将世界坐标系设定为第一幅图像的相机坐标系，得：
其中，是第一幅图像相对于世界坐标系的投影变换；是第二幅图像相对于世界坐标系的投影变换；是单位矩阵；，分别为第二幅图相对于世界坐标系的运动参数。
优选的技术方案中，所述步骤S02中运用改进的深度学习检测模型获取非合作目标ROI区域；
所述改进的深度学习检测模型在YOLOv7_tiny的基础上进行改进，模型中的卷积部分均采用深度可分离卷积实现，深度可分离卷积由可分离卷积和点卷积两部分组成，其中可分离卷积实现逐通道卷积，点卷积则实现不同通道间的特征交互，卷积层后的激活函数采用ELU激活函数；ELAN中将其中一条通道上的卷积数量由3个降低至2个，在YOLO模型的Neck部分包含3输出通道间的特征融合，删除中层输出特征向底层特征的下采样，仅保留三通道间的上采样和高层特征向中层特征的下采样。
优选的技术方案中，所述步骤S03中将干扰点分成远干扰点与近干扰点的方法包括：
S31：点云集在三维坐标系下，表示为：
其中，为点云集在笛卡尔坐标系下的三维坐标；
S32：对于每一个三维点，搜索其邻域类最临近的若干组点并计算欧式距离，并计算此点与临近点距离的标准差，通过下式判断是否属于离群点：
其中，表示第个点的统计距离；表示选取的临近点数目，表示阈值系数；表示点与临近个点的标准差；
对于点，若临近点与其欧式距离大于则被认定为远干扰点，反之则为近干扰点。
优选的技术方案中，所述在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变包括：
S33：在代价函数中加入权重得到：
其中，是目标点集中点的数量；是目标点集中的第个点；为权重系数且，表示对不同对应点的重视程度；
S34：通过近干扰点与质心的距离来构造计算权重的方程，对于第组点对，分别求取其与原始点云质心距离，以及与目标点云质心之间的距离：
其中，与分别为原始点云与目标点云的质心；
S35：将与当作自变量，作为因变量，运用二维高斯分布函数表征每个位置权值的特性：
由于，标准差的值为，因此进一步表示为：
S36：得到基于质心距变条件下高斯分布权值的代价函数为：
。
优选的技术方案中，在求解过程中，将点云集的变换转化为平移与旋转两个步骤，即视作原始点云绕质心的旋转以及质心的平移，增加质心重合的约束项以简化运算，定义两组点云集的质心、：
从原始点云集与目标点云集中剔除掉质心，得：
其中，且；
进一步化简得到：
将对目标的求解转化为先求后求的过程：
其中，表示返回变换后复数的辐角；和是机器人相对于目标的最优旋转矩阵和平移向量。
优选的技术方案中，得到机器人相对于目标的最优旋转矩阵和平移向量和后，通过SVD分解完成对目标位姿的求解，求得待测对象相对于机器人的方位角和距离信息。
本发明还公开了一种非合作目标位姿估计系统，包括：
点云三维重建模块，对非合作目标点云进行三维重建；
目标ROI区域获取模块，获取目标ROI区域；
目标位姿估计模块，获取目标点云的干扰点，将干扰点分成远干扰点与近干扰点，对于远干扰点，在配准前运用离群点剔除算法进行分离与剔除；对于近干扰点，在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变，并将其带入到迭代过程的运算中，完成对目标位姿的估计。
优选的技术方案中，所述在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变包括：
S33：在代价函数中加入权重得到：
其中，是目标点云集中点的数量；是目标点云集中的第个点；为权重系数且，表示对不同对应点的重视程度；
S34：通过近干扰点与质心的距离来构造计算权重的方程，对于第组点对，分别求取其与原始点云质心距离，以及与目标点云质心之间的距离：
其中，与分别为原始点云与目标点云的质心；
S35：将与当作自变量，作为因变量，运用二维高斯分布函数表征每个位置权值的特性：
由于，标准差的值为，因此进一步表示为：
S36：得到基于质心距变条件下高斯分布权值的代价函数为：
。
本发明又公开了一种计算机存储介质，其上存储有计算机程序，所述计算机程序被执行时实现上述的非合作目标位姿估计方法。
本发明与现有技术相比，其显著优点为：
针对仓储搬运机器人导航中对非合作目标点云位姿估计因干扰点导致的配准误差大、位姿求取精度差等问题，提出一种基于质心距变条件下高斯分布权值的迭代最近点方法，以此提高对仓储机器人工作环境中非合作目标位姿估计的精确性与鲁棒性。在运用改进的深度学习检测模型实现对非合作目标ROI区域获取的基础上，将干扰点分类成远干扰点和近干扰点，对于远干扰点运用统计离群点剔除算法进行点云的预处理。对于存在于目标边缘的近干扰点，在传统迭代最近点算法中加入高斯分布权重，权值随点到质心距离的变化而改变，以此起到对近干扰点的弱化。所提方法在仓储搬运机器人面向大视场非合作目标位姿求取时具有一定的价值与实用意义。
附图说明
图1为较佳实施例的非合作目标位姿估计方法的流程图；
图2为三角测量原理示意图；
图3为改进后的目标检测模型架构示意图；
图4为D_CBE架构示意图；
图5为ELAN架构示意图；
图6为D_SPP架构示意图；
图7为离群点剔除算法示意图；
图8为对应点平均重构误差随干扰点增加变化趋势。
具体实施方式
本发明的原理是：本发明充分考虑机器人大视场工作环境，提出一种基于检测与量测耦合，通过构建质心距变条件下的迭代最近点算法来提高机器人对非合作目标位姿测量的精确性与鲁棒性。方法的整体流程是首先运用改进的深度学习检测模型实现对目标ROI区域的获取，然后通过运动恢复结构(Structure from Motion, SFM)进行目标点云的三维重建，重建出被测目标前后两次的三维点云数据。考虑到仓储搬运机器人“低空”工作环境下存在的干扰因素导致配准的点云中出现大量干扰点，本发明将干扰点分类成远干扰点与近干扰点。对于远干扰点，在配准前运用离群点剔除算法进行分离与剔除；对于近干扰点，在传统迭代最近点算法的代价函数中加入质心距变条件下的高斯分布权值，并将其带入到迭代过程的运算中，基于此可以通过SVD分解完成对目标位姿的高精度求解。
实施例1：
如图1所示，一种非合作目标位姿估计方法，包括以下步骤：
S01：对非合作目标点云进行三维重建；
S02：获取目标ROI区域；
S03：获取目标点云的干扰点，将干扰点分成远干扰点与近干扰点，对于远干扰点，在配准前运用离群点剔除算法进行分离与剔除；对于近干扰点，在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变，并将其带入到迭代过程的运算中，完成对目标位姿的估计。
一较佳的实施例中，步骤S01中对非合作目标点云进行三维重建的方法包括：
S11：通过特征点提取与匹配算法得到图像特征点对，当空间某一点在多个相机视角中出现时，采用三角测量法重建出空间点的三维坐标，如图2所示；
S12：双目视觉系统中的两个相机从两个不同视角采集图像，世界坐标系下的三维点通过投影变换得到二维像素坐标，根据相机成像模型列出方程如下：
其中，与分别为空间点通过左右相机成像后的二维像素坐标，、分别为左、右相机为相机内参，通过相机标定求取；为空间点在世界坐标系下的待求坐标；为相机外参；与分别为左右相机采集此点的深度信息；矩阵最右边为齐次坐标中的数字1；
步骤13：令投影矩阵，将世界坐标系设定为第一幅图像的相机坐标系，得：
其中，是第一幅图像相对于世界坐标系的投影变换；是第二幅图像相对于世界坐标系的投影变换；是单位矩阵；，分别为第二幅图相对于世界坐标系的运动参数。
一较佳的实施例中，步骤S02中运用改进的深度学习检测模型获取非合作目标ROI区域；
所述改进的深度学习检测模型在YOLOv7_tiny的基础上进行改进，模型中的卷积部分均采用深度可分离卷积实现，深度可分离卷积由可分离卷积和点卷积两部分组成，其中可分离卷积实现逐通道卷积，点卷积则实现不同通道间的特征交互，卷积层后的激活函数采用ELU激活函数；ELAN中将其中一条通道上的卷积数量由3个降低至2个，在YOLO模型的Neck部分包含3输出通道间的特征融合，删除中层输出特征向底层特征的下采样，仅保留三通道间的上采样和高层特征向中层特征的下采样。
一较佳的实施例中，步骤S03中将干扰点分成远干扰点与近干扰点的方法包括：
S31：点云集在三维坐标系下，表示为：
其中，为点云集在笛卡尔坐标系下的三维坐标；
S32：对于每一个三维点，搜索其邻域类最临近的若干组点并计算欧式距离，并计算此点与临近点距离的标准差，通过下式判断是否属于离群点：
其中，表示第个点的统计距离；表示选取的临近点数目，表示阈值系数；表示点与临近个点的标准差；
对于点，若临近点与其欧式距离大于则被认定为远干扰点，反之则为近干扰点。
一较佳的实施例中，在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变包括：
S33：在代价函数中加入权重得到：
其中，是目标点云集中点的数量；是目标点云集中的第个点； 为权重系数且，表示对不同对应点的重视程度；
S34：通过近干扰点与质心的距离来构造计算权重的方程，对于第组点对，分别求取其与原始点云质心距离，以及与目标点云质心之间的距离：
其中，与分别为原始点云与目标点云的质心；
S35：将与当作自变量，作为因变量，运用二维高斯分布函数表征每个位置权值的特性：
由于，标准差的值为，因此进一步表示为：
S36：得到基于质心距变条件下高斯分布权值的代价函数为：
。
一较佳的实施例中，在求解过程中，将点云集的变换转化为平移与旋转两个步骤，即视作原始点云绕质心的旋转以及质心的平移，增加质心重合的约束项以简化运算，定义两组点云集的质心、：
从原始点云集与目标点云集中剔除掉质心，得：
其中，且；
进一步化简得到：
将对目标的求解转化为先求后求的过程：
其中，表示返回变换后复数的辐角；和是机器人相对于目标的最优旋转矩阵和平移向量。
一较佳的实施例中，得到机器人相对于目标的最优旋转矩阵和平移向量和后，通过SVD分解完成对目标位姿的求解，求得待测对象相对于机器人的方位角和距离信息。
另一实施例中，一种计算机存储介质，其上存储有计算机程序，所述计算机程序被执行时实现上述的非合作目标位姿估计方法。
采用上面的任意一种位姿估计方法，此处不再赘述。
另一实施例中，一种非合作目标位姿估计系统，包括：
点云三维重建模块，对非合作目标点云进行三维重建；
目标ROI区域获取模块，获取目标ROI区域；
目标位姿估计模块，获取目标点云的干扰点，将干扰点分成远干扰点与近干扰点，对于远干扰点，在配准前运用离群点剔除算法进行分离与剔除；对于近干扰点，在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变，并将其带入到迭代过程的运算中，完成对目标位姿的估计。
具体的，下面以一较佳的实施例为例对非合作目标位姿估计系统的工作流程说明如下：
步骤一：基于SFM算法的非合作目标点云三维重建
步骤11：通过特征点提取与匹配算法初步得到图像特征点对。接着运用RANSAC算法去除目标点云的误匹配点，得到高精度的特征点对。当空间某一点在多个相机视角中出现时，可以采用三角测量法重建出空间点的三维坐标。
步骤12：图2中，双目视觉系统中的两个相机从两个不同视角采集图像，世界坐标系下的三维点通过投影变换得到二维像素坐标，因此根据相机成像模型可以列出方程如下：
(14)
其中，与分别为空间点通过左右相机成像后的二维像素坐标；为相机内参，通过相机标定求取；为空间点在世界坐标系下的待求坐标；为相机外参；与分别为左右相机采集此点的深度信息。
步骤13：在式(14)中，令投影矩阵，为了方便计算，将世界坐标系设定为第一幅图像的相机坐标系，可得：
其中，是第一幅图像相对于世界坐标系的投影变换；是第二幅图像相对于世界坐标系的投影变换；，分别为第二幅图相对于世界坐标系（即第一幅图的相机坐标系）的运动参数。
步骤二：YOLO_WCR目标检测模型构建
在仓储搬运机器人末端检测识别和姿态角解算中，可以先检测目标在视场中的某一区域位置，即实现ROI区域的锁定，这样可以减小点云的计算量，提高算法的时效性。为此，本发明在YOLOv7_tiny的基础上提出YOLO_WCR检测模型，模型结构如图3所示。模型由骨干网络和检测网络两部分组成，其中骨干网络输入图像的大小为640x640，由两个D_CBE模块、4个ELAN模块构成。同时除了最后一个ELAN模块，在其它ELAN模块之后均使用一个最大池化层Maxpool进行处理。图片在经过骨干网络提取特征后，分别将第2、3、4个ELAN模块输出的特征数据，输入检测网络部分进行检测，特征数据的维度分别为80×80×512、40×40×1024、20×20×1024。检测网络部分主要由D_CBE模块、D_SPP模块、上采样层UP模块、拼接模块Concat以及ELAN模块等组成，检测头部分共有3个检测分支，分别对骨干网络中的3中不同分别率的特征进行检测。骨干网络和检测网络首先在横向上进行链接，即经过骨干网络处理得到的3种特征数据分别输入相应的检测分支，而检测网络中，每个检测分支的处理结果通过Concat模块与前一特征数据在通道维度上进行拼接，然后将融合后的数据输入下一个检测分支中进行处理。同时，在第4个ELAN模块和对应的检测分支之间，加入了D_CBE和D_SPP两个并行的模块，并通过一个Concat模块进行数据融合后输入相应的预测分支，以增强对小目标特征提取的能力。最终通过3个预测（YOLO）输出得到的目标类别和位置信息。
步骤21：模型中的卷积部分（D_CBE），如图4所示，包含了深度方向可分离卷积模块、批正则化模块和ELU激活函数模块。均采用深度可分离卷积实现，深度可分离卷积由可分离卷积和点卷积两部分组成。其中可分离卷积实现逐通道卷积，即一个通道只被一个卷积核卷积，这种卷积对输入层的每个通道独立进行卷积运算，可以在很大程度上降低参数量，但是无法有效利用不同通道间的特征信息。点卷积的卷积核大小为1×1×M，其中M为上一深度方向卷积层的通道数，通过点卷积运算会将上一步的特征图在深度方向上进行组合，生成新的特征图，从而实现不同通道间的特征交互，保证输出特征的丰富性。深度方向卷积和点卷积两部分共同作用，在保证模型特征提取性能的前提下降低模型参数量。此外，卷积层后的激活函数采用ELU激活函数，该函数有负值，这保证激活的平均值接近零，梯度更接近自然梯度，可以提高模型的学习速度。与此同时，该激活函数对噪声具有较高的鲁棒性，更适合自主搬运场景下的应用。
ELAN部分的结构如图5所示，经过D_CBE组合模块运算后，通过Concat实现特征融合，再进行D_CBE模块的运算，D_CBE组合模块运算包括第一D_CBE模块、第二D_CBE模块和第三D_CBE模块，第一D_CBE模块运算结果、第二D_CBE模块运算结果与第二D_CBE模块和第三D_CBE模块组合运算结果通过Concat实现特征融合。该部分不同于传统检测模型中使用分组卷积来扩展网络模块的通道和基数，YOLO_WCR使用深度可分离卷积（D_CBE）取代了原结构中的多层分组卷积运算，从而降低了网络的参数量，提高了模型的运算效率，同时有助于进一步提高网络的深度，提升网络的检测能力，能够让深度网络进行更加有效地学习和收敛，解决多尺度的深度网络在收敛时模型逐步变差的问题。另外在网络中保持输入输出特征的尺度不变，通道数则仅在两个通道中的首个D_CBE处产生变化，后续结构的输入和输出通道数量保持一致，得到的输出特征图在通道维度上进行堆叠。通常情况下为了保证模型的泛化能力，在YOLO模型的Neck部分包含3输出通道间的特征融合，而考虑平面标定板的特征目标较小，因此删除中层输出特征向底层特征的下采样，仅保留三通道间的上采样和高层特征向中层特征的下采样，在保证对目标检测能力的基础上最大程度上实现模型的轻量化。
D_SPP结构如图6所示，同样使用D_CBE模块进行改进，经过D_CBE模块运算后，分别通过三个MP模块并行处理，MP模块为最大池化层，三个MP模块并行处理后与D_CBE模块运算结果通过Concat实现特征融合，再进行D_CBE模块的运算。
一方面解决了SPPCSPC模块参数量较大的问题，另一方面其与一个D_CBE模块构成了并行的双通道网络，两个模块的输出特征图进行堆叠后输入后续网络中进行检测，能够有效提高网络的特征提取能力。
步骤三：基于质心距变条件下高斯分布权值的迭代最近点算法
由于对搬运目标点云提取时存在大量的干扰点，导致点云配准算法在求取质心上发生偏移从而使得点云无法高精度对齐。本发明将干扰点分为两大类，第一类为远干扰点，这些点的位置与实际目标距离较远，对点云集质心的影响较大，因此需要在配准前进行筛选与剔除；第二类为近干扰点，这类点与目标临近甚至接触，对质心影响不大但对点云整体结构存在一定影响并且难以在配准前剔除，因此可以通过对算法的优化在迭代过程中进行处理与更新。
步骤31：通过对离群点进行统计滤波实现干扰点剔除，以此完成对原始点云与目标点云的初步处理。以原始点云为例，点云集在三维坐标系下，可表示为：
(16)
其中，为点云集在笛卡尔坐标系下的三维坐标，点云集中的点是无序排列的。
步骤32：对于每一个三维点，搜索其邻域类最临近的若干组点并计算欧式距离，并计算此点与临近点距离的标准差，通过式(17)来判断是否属于离群点。
(17)
其中，表示第个点的统计距离；表示选取的临近点数目，可取值为20；可表示为阈值系数，来控制筛选点的数目；则表示为点与临近个点的标准差。
对于点，若临近点与其欧式距离大于则被认定为离群点（即远干扰点）并给与剔除，反之则判断为近干扰点，如图7所示。
步骤33：针对点云近干扰点的弱化，本发明在代价函数中加入权重得到：
(18)
其中，为权重系数且，它表示对不同对应点的重视程度，此值越大代表此组点对对代价方程的影响越大，在求解此非线性方程最优解的过程中，通过迭代能快速优化此组点对的误差。当时，对此点对的求解与原始算法相同，表示完全考虑此点的位置与作用；相反，当的值越小，通过此组点对计算得到的误差也就越小，越易于被弱化与忽略。当时，对此对应点的求解结果为0，表示无论此点位置情况如何都完全不会考虑此点，也不会参与到运算。
步骤34：近干扰点一般存在于目标物体的边缘，因此可以通过其与质心的距离来构造计算权重的方程。对于第组点对，首先分别求取其与原始点云质心以及目标点云质心之间的距离如式(19)所示：
(19)
其中，与分别为原始点云与目标点云的质心。
步骤35：将与当作自变量，作为因变量，运用二维高斯分布函数来表征每个位置权值的特性如式(20)所示。高斯函数为单值函数，每一邻域点的权值是随该点与质心点的距离单调增减的。
(20)
由于，计算出标准差的值为，因此可表示如式(21)所示：
(21)
步骤36：将式(21)带入到式(20)中得到基于质心距变条件下高斯分布权值的代价函数，如式(22)所示：
(22)
由式(22)可知，对于求解此非线性方程最优解有6个待求解量，分别为3个欧拉角以及3个平移量。为了简化运算，将点云集的变换转化为平移与旋转两个步骤进行。即视作原始点云绕质心的旋转以及质心的平移，增加质心重合的约束项以简化运算。因此定义两组点云集的质心如(23)式所示：
(23)
结合式(22)与式(23)，从原始点云集与目标点云集中剔除掉质心，可得：
(24)
其中，且。观察式子(24)最后一项有：成立，因此可将式(24)化简为式(25)所示：
(25)
由式(25)可将对目标的求解转化为先求后求的过程：
(26)
其中，表示返回变换后复数的辐角；和是机器人相对于目标的最优旋转矩阵和平移向量，接着通过SVD分解完成对目标位姿的高精度求解，即可求得待测对象相对于机器人的方位角和距离信息。
为了验证所提方法的有效性，设计仿真实验如下。首先在目标识别验证实验中，将本发明构建的模型与目前主流的YOLOv7_x, YOLOv7, YOLOv7_tiny, YOLOv5_s等模型进行对比。模型训练周期为500，均不加载预训练权重，单次传递给程序用以训练的数据(样本)个数设置为16。运行的计算机配置为Windows 10 + 2.4GHz CPU + 24GB RAM+RTX 4080。如表1所示，从精度（Precision），召回率（Recall），FPS，Parameter以及FLOPS等方面对模型性能进行评估。
表1 模型测试结果对比
根据最终的测试情况而言，本发明所构建的模型在测试精度，FPS，参数量以及FLOPS方面均有优势。其中，YOLO_WCR模型的参数量仅为YOLOv7_tiny的36.72%，FLOPS相较YOLOv7_tiny降低62.31%，并且在保证检测精度的同时检测速度（FPS）也有一定的提升。
接着模拟构建出待测目标的点云数据，此数据描述的目标信息如下：最大长度为1000厘米，最大宽度为500厘米，最大高度为500厘米。将其进行一定角度的旋转和一定距离的平移，作为目标点云。在两组点云中分别加入位置随机的一定数量的干扰点。在以点云质心为圆心，为半径的球面上加入个点来作为干扰点。其中的取值设置在600厘米到700厘米之间并每隔10厘米取值一次。因此对于每一片点云有个干扰点。
然后加入干扰点，设置旋转角度为且平移量为厘米。通过不断增大的取值来增加干扰点的数目。接着分别运用传统方法与本发明方法对两组点云进行配准。由于每次加入的干扰点随机，因此重复10次实验并取平均值。最后利用配准前后未加任何干扰点时对应点的平均误差作为判断精度的标准。将平均误差作为纵坐标，远干扰点的数目作为横坐标，得到的实验结果如图6所示。
从图8可以看出，随着干扰点的增加，传统方法配准得到的误差也随之增大，直到最后无法完成配准。而本发明方法不会受到干扰点的影响，误差始终保持在很小的范围内。这是因为本发明方法在处理点云集时进行了离群点的剔除，智能地删除了原始点云与目标点云中的干扰点。
接下来对比两种方法的位姿估计情况，取，将两种方法解算出的旋转角与平移向量分别于预设的理想值进行比较得到结果如表2所示。
表2 位姿估计结果对比
由表2可以看出，本发明方法对于旋转参数的解算误差平均值为0.3820°，对于平移参数的解算误差平均值为1.4770厘米；而传统方法对于旋转参数的解算误差平均值为1.0735°，对于平移参数的解算误差平均值为2.2928厘米。综上所述，本发明方法能很好的弱化干扰点对测量精度的影响，能够满足仓储搬运机器人的实际使用需求。
上述实施例为本发明优选地实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
Claims (10)
1.一种非合作目标位姿估计方法，其特征在于，包括以下步骤：
S01：对非合作目标点云进行三维重建；
S02：获取目标ROI区域；
S03：获取目标点云的干扰点，将干扰点分成远干扰点与近干扰点，对于远干扰点，在配准前运用离群点剔除算法进行分离与剔除；对于近干扰点，在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变，并将其带入到迭代过程的运算中，完成对目标位姿的估计。
2.根据权利要求1所述的非合作目标位姿估计方法，其特征在于，所述步骤S01中对非合作目标点云进行三维重建的方法包括：
S11：通过特征点提取与匹配算法得到图像特征点对，当空间某一点在多个相机视角中出现时，采用三角测量法重建出空间点的三维坐标；
S12：双目视觉系统中的两个相机从两个不同视角采集图像，世界坐标系下的三维点通过投影变换得到二维像素坐标，根据相机成像模型列出方程如下：
，
其中，与分别为空间点通过左右相机成像后的二维像素坐标，、分别为左、右相机；为相机内参，通过相机标定求取；为空间点在世界坐标系下的待求坐标；为相机外参；与分别为左右相机采集此点的深度信息；
步骤13：令投影矩阵，将世界坐标系设定为第一幅图像的相机坐标系，得：
，
其中，是第一幅图像相对于世界坐标系的投影变换；是第二幅图像相对于世界坐标系的投影变换；是单位矩阵；，分别为第二幅图相对于世界坐标系的运动参数。
3.根据权利要求1所述的非合作目标位姿估计方法，其特征在于，所述步骤S02中运用改进的深度学习检测模型获取非合作目标ROI区域；
所述改进的深度学习检测模型在YOLOv7_tiny的基础上进行改进，模型中的卷积部分均采用深度可分离卷积实现，深度可分离卷积由可分离卷积和点卷积两部分组成，其中可分离卷积实现逐通道卷积，点卷积则实现不同通道间的特征交互，卷积层后的激活函数采用ELU激活函数；ELAN中将其中一条通道上的卷积数量由3个降低至2个，在YOLO模型的Neck部分包含3输出通道间的特征融合，删除中层输出特征向底层特征的下采样，仅保留三通道间的上采样和高层特征向中层特征的下采样。
4.根据权利要求1所述的非合作目标位姿估计方法，其特征在于，所述步骤S03中将干扰点分成远干扰点与近干扰点的方法包括：
S31：点云集在三维坐标系下，表示为：
，
其中，为点云集在笛卡尔坐标系下的三维坐标；
S32：对于每一个三维点，搜索其邻域类最临近的若干组点并计算欧式距离，并计算此点与临近点距离的标准差，通过下式判断是否属于离群点：
，
其中，表示第个点的统计距离；表示选取的临近点数目，表示阈值系数；表示点与临近个点的标准差；
对于点，若临近点与其欧式距离大于则被认定为远干扰点，反之则为近干扰点。
5.根据权利要求4所述的非合作目标位姿估计方法，其特征在于，所述在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变包括：
S33：在代价函数中加入权重得到：
，
其中，是目标点云集中点的数量；是目标点云集中的第个点；为权重系数且，表示对不同对应点的重视程度；
S34：通过近干扰点与质心的距离来构造计算权重的方程，对于第组点对，分别求取其与原始点云质心距离，以及与目标点云质心之间的距离：
，
其中，与分别为原始点云与目标点云的质心；
S35：将与当作自变量，作为因变量，运用二维高斯分布函数表征每个位置权值的特性：
，
由于，标准差的值为，因此进一步表示为：
，
S36：得到基于质心距变条件下高斯分布权值的代价函数为：
。
6.根据权利要求5所述的非合作目标位姿估计方法，其特征在于，在求解过程中，将点云集的变换转化为平移与旋转两个步骤，即视作原始点云绕质心的旋转以及质心的平移，增加质心重合的约束项以简化运算，定义两组点云集的质心、：
，
从原始点云集与目标点云集中剔除掉质心，得：
其中，且；
进一步化简得到：
，
将对目标的求解转化为先求后求的过程：
，
其中，表示返回变换后复数的辐角；和是机器人相对于目标的最优旋转矩阵和平移向量。
7.根据权利要求6所述的非合作目标位姿估计方法，其特征在于，得到机器人相对于目标的最优旋转矩阵和平移向量和后，通过SVD分解完成对目标位姿的求解，求得待测对象相对于机器人的方位角和距离信息。
8.一种非合作目标位姿估计系统，其特征在于，包括：
点云三维重建模块，对非合作目标点云进行三维重建；
目标ROI区域获取模块，获取目标ROI区域；
目标位姿估计模块，获取目标点云的干扰点，将干扰点分成远干扰点与近干扰点，对于远干扰点，在配准前运用离群点剔除算法进行分离与剔除；对于近干扰点，在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变，并将其带入到迭代过程的运算中，完成对目标位姿的估计。
9.根据权利要求8所述的非合作目标位姿估计系统，其特征在于，所述在传统迭代最近点算法的代价函数中加入高斯分布权重，权值随点到质心距离的变化而改变包括：
S33：在代价函数中加入权重得到：
，
其中，是目标点云集中点的数量；是目标点云集中的第个点；为点云集在笛卡尔坐标系下的三维坐标；为权重系数且，表示对不同对应点的重视程度；
S34：通过近干扰点与质心的距离来构造计算权重的方程，对于第组点对，分别求取其与原始点云质心距离，以及与目标点云质心之间的距离：
，
其中，与分别为原始点云与目标点云的质心；
S35：将与当作自变量，作为因变量，运用二维高斯分布函数表征每个位置权值的特性：
，
由于，标准差的值为，因此进一步表示为：
，
S36：得到基于质心距变条件下高斯分布权值的代价函数为：
。
10.一种计算机存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被执行时实现权利要求1-7任一项所述的非合作目标位姿估计方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410580555.1A CN118154677B (zh) | 2024-05-11 | 2024-05-11 | 一种非合作目标位姿估计方法、系统及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410580555.1A CN118154677B (zh) | 2024-05-11 | 2024-05-11 | 一种非合作目标位姿估计方法、系统及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN118154677A true CN118154677A (zh) | 2024-06-07 |
| CN118154677B CN118154677B (zh) | 2024-09-13 |
Family
ID=91297199
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410580555.1A Active CN118154677B (zh) | 2024-05-11 | 2024-05-11 | 一种非合作目标位姿估计方法、系统及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118154677B (zh) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105976353A (zh) * | 2016-04-14 | 2016-09-28 | 南京理工大学 | 基于模型和点云全局匹配的空间非合作目标位姿估计方法 |
| WO2023045455A1 (zh) * | 2021-09-21 | 2023-03-30 | 西北工业大学 | 基于分支重构配准的非合作目标三维重建方法 |
| CN117197333A (zh) * | 2023-08-02 | 2023-12-08 | 哈尔滨工业大学 | 基于多目视觉的空间目标重构与位姿估计方法及系统 |
| US20240005759A1 (en) * | 2022-09-09 | 2024-01-04 | Nanjing University Of Posts And Telecommunications | Lightweight fire smoke detection method, terminal device, and storage medium |
| CN117496129A (zh) * | 2023-11-15 | 2024-02-02 | 南京工业大学 | 一种基于YOLOv7改进的工厂安全穿戴目标检测方法 |
| CN117541614A (zh) * | 2024-01-09 | 2024-02-09 | 南京航空航天大学 | 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 |
| CN117593794A (zh) * | 2023-11-28 | 2024-02-23 | 广东工业大学 | 改进的YOLOv7-tiny模型及基于该模型的人手脸检测方法及系统 |
-
2024
- 2024-05-11 CN CN202410580555.1A patent/CN118154677B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105976353A (zh) * | 2016-04-14 | 2016-09-28 | 南京理工大学 | 基于模型和点云全局匹配的空间非合作目标位姿估计方法 |
| WO2023045455A1 (zh) * | 2021-09-21 | 2023-03-30 | 西北工业大学 | 基于分支重构配准的非合作目标三维重建方法 |
| US20240005759A1 (en) * | 2022-09-09 | 2024-01-04 | Nanjing University Of Posts And Telecommunications | Lightweight fire smoke detection method, terminal device, and storage medium |
| CN117197333A (zh) * | 2023-08-02 | 2023-12-08 | 哈尔滨工业大学 | 基于多目视觉的空间目标重构与位姿估计方法及系统 |
| CN117496129A (zh) * | 2023-11-15 | 2024-02-02 | 南京工业大学 | 一种基于YOLOv7改进的工厂安全穿戴目标检测方法 |
| CN117593794A (zh) * | 2023-11-28 | 2024-02-23 | 广东工业大学 | 改进的YOLOv7-tiny模型及基于该模型的人手脸检测方法及系统 |
| CN117541614A (zh) * | 2024-01-09 | 2024-02-09 | 南京航空航天大学 | 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 |
Non-Patent Citations (1)
| Title |
|---|
| ZHU ZIJIAN等: "Non-cooperative target pose estimation based on improved iterative closest point algorithm", JOURNAL OF SYSTEMS ENGINEERING AND ELECTRONICS, vol. 33, no. 1, 28 February 2022 (2022-02-28), pages 1 - 10 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN118154677B (zh) | 2024-09-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109410321B (zh) | 基于卷积神经网络的三维重建方法 | |
| CN110223348B (zh) | 基于rgb-d相机的机器人场景自适应位姿估计方法 | |
| CN111563415B (zh) | 一种基于双目视觉的三维目标检测系统及方法 | |
| CN108491880B (zh) | 基于神经网络的物体分类和位姿估计方法 | |
| CN112270249A (zh) | 一种融合rgb-d视觉特征的目标位姿估计方法 | |
| CN107705322A (zh) | 运动目标识别跟踪方法和系统 | |
| CN113628263A (zh) | 一种基于局部曲率及其近邻特征的点云配准方法 | |
| CN111998862B (zh) | 一种基于bnn的稠密双目slam方法 | |
| CN113989797A (zh) | 一种基于体素点云融合的三维动态目标检测方法及装置 | |
| Rubio et al. | Efficient monocular pose estimation for complex 3D models | |
| CN112712589A (zh) | 一种基于激光雷达和深度学习的植株3d建模的方法和系统 | |
| CN111046767A (zh) | 一种基于单目图像的3d目标检测方法 | |
| CN115032648B (zh) | 一种基于激光雷达密集点云的三维目标识别与定位方法 | |
| CN117197333A (zh) | 基于多目视觉的空间目标重构与位姿估计方法及系统 | |
| CN107818598A (zh) | 一种基于视觉矫正的三维点云地图融合方法 | |
| CN116246119A (zh) | 3d目标检测方法、电子设备及存储介质 | |
| CN104463953A (zh) | 基于惯性测量单元与rgb-d传感器的三维重建方法 | |
| CN114299405A (zh) | 一种无人机图像实时目标检测方法 | |
| CN116863371A (zh) | 一种基于深度学习的agv叉车货物托盘位姿识别方法 | |
| CN109934859B (zh) | 一种基于特征增强多维权重描述子的icp配准方法 | |
| CN115830375A (zh) | 点云分类方法及装置 | |
| CN114494644A (zh) | 一种基于双目立体匹配的空间非合作目标位姿估计与三维重建方法及系统 | |
| CN118154677B (zh) | 一种非合作目标位姿估计方法、系统及存储介质 | |
| Hu et al. | R-CNN based 3D object detection for autonomous driving | |
| CN113792593A (zh) | 一种基于深度融合的水下近距离目标识别跟踪方法及系统 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |