CN113506326B - 铲斗三维位姿跟踪方法、装置、系统及挖掘机 - Google Patents
铲斗三维位姿跟踪方法、装置、系统及挖掘机 Download PDFInfo
- Publication number
- CN113506326B CN113506326B CN202110802817.0A CN202110802817A CN113506326B CN 113506326 B CN113506326 B CN 113506326B CN 202110802817 A CN202110802817 A CN 202110802817A CN 113506326 B CN113506326 B CN 113506326B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- bucket
- radar
- camera
- radar point
- 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 abstract description 66
- 239000011159 matrix material Substances 0.000 claims abstract description 78
- 238000013507 mapping Methods 0.000 claims abstract description 20
- 238000001914 filtration Methods 0.000 claims abstract description 12
- 238000012216 screening Methods 0.000 claims abstract description 11
- 239000013598 vector Substances 0.000 claims description 41
- 238000013519 translation Methods 0.000 claims description 39
- 238000012545 processing Methods 0.000 claims description 17
- 238000004590 computer program Methods 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 3
- 238000012544 monitoring process Methods 0.000 abstract description 4
- 230000008859 change Effects 0.000 description 7
- 238000004891 communication Methods 0.000 description 7
- 230000008569 process Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 230000004927 fusion Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 238000012423 maintenance Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000004044 response Effects 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/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/28—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/28—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets
- E02F3/36—Component parts
- E02F3/40—Dippers; Buckets ; Grab devices, e.g. manufacturing processes for buckets, form, geometry or material of buckets
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
-
- 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
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
Landscapes
- Engineering & Computer Science (AREA)
- Mechanical Engineering (AREA)
- Mining & Mineral Resources (AREA)
- Civil Engineering (AREA)
- General Engineering & Computer Science (AREA)
- Structural Engineering (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Multimedia (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明提供一种铲斗三维位姿跟踪方法、装置、系统及挖掘机，所述方法包括：根据铲斗宽度值及铲斗与激光雷达的相对运动范围，对第一雷达点云进行直通滤波获取第二雷达点云；基于相机的内参矩阵及激光雷达与相机之间的映射矩阵，将第二雷达点云投影到相机图像中，生成第三雷达点云；根据铲斗的颜色信息，从第三雷达点云中筛选出第四雷达点云；获取第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息，以根据相对位姿信息对铲斗点云模型进行渲染，生成铲斗分布点云。本发明仅需要将激光雷达和相机安装在驾驶室座舱上，而无需对斗杆和铲斗进行其他类型的传感器监测，也能够在线获取铲斗分布点云，结构简单、便于维护，且不易受外界环境影响。
Description
技术领域
本发明涉及重工机械技术领域，尤其涉及一种铲斗三维位姿跟踪方法、装置、系统及挖掘机。
背景技术
铲斗的仿真以及铲斗齿尖的跟踪，可以用于挖掘机操作的验证。
现有技术中记载了一种挖掘机施工引导系统，在挖掘机的车体和动臂上设有用于测量动力臂位姿信息的定位模块，并在挖掘机上设有测量斗杆液压油缸和/或铲斗液压油缸的伸缩长度的测量组件。其工作原理是通过测量终端接收定位模块和测量组件的测量信息，计算动力臂、斗杆以及铲斗的位姿信息；再利用软件建立施工地地形模型，并根据铲斗的位姿信息确定铲斗仿真模型的三维位姿。
另外，目前为了实现挖掘机操作的验证，还利用高速相机拍摄挖掘机的铲斗，以获取其位置移动；然后通过离线分析的方式，输出铲斗齿的移动坐标位置。
现有技术在进行铲斗三维位姿跟踪的过程中，主要是通过位置传感器或者高速相机等相关传感器，通过采集铲斗位置、液压杆长度等信息，以计算出铲斗三维位姿的变化，由于挖掘机的工作环境一般较为恶劣，往往导致实际跟踪结果偏差较大；最为关键的是，现有技术只能通过离线的方式输出铲斗以及铲斗齿的位姿变化，无法将其实时输出、反馈给驾驶室内的操作员。
发明内容
本发明提供一种铲斗三维位姿跟踪方法、装置、系统及挖掘机，用以解决现有技术中无法实时确定铲斗三维位姿变化的缺陷。
第一方面，本发明提供一种铲斗三维位姿跟踪方法，包括：
根据铲斗的宽度值，以及所述铲斗与激光雷达的相对运动范围，对第一雷达点云进行直通滤波，获取第二雷达点云；所述第一雷达点云是由所述激光雷达采集的；基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云；所述相机图像是由所述相机拍摄的；根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云；获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息，以根据所述相对位姿信息对所述铲斗点云模型进行渲染，生成铲斗分布点云。
根据本发明提供的一种铲斗三维位姿跟踪方法，在将所述第二雷达点云投影到相机图像中生成第三雷达点云之前，还包括：
利用第一旋转矩阵和第一平移向量表示所述激光雷达的世界坐标原点，并利用第二旋转矩阵和第二平移向量表示所述相机的世界坐标原点；确定所述第一旋转矩阵与所述第二旋转矩阵之间的相对旋转矩阵，并确定所述第一平移向量与所述第二平移向量之间的相对平移向量；根据所述相对旋转矩阵和所述相对平移向量，确定所述映射矩阵。
根据本发明提供的一种铲斗三维位姿跟踪方法，所述基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云，包括：
根据所述相对旋转矩阵和所述相对平移向量，对所述第二雷达点云进行位姿变换，获取第五雷达点云；基于所述内参矩阵，将所述第五雷达点云投影至所述相机图像，生成所述第三雷达点云。
根据本发明提供的一种铲斗三维位姿跟踪方法，在获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息之前，还包括：
利用激光雷达和相机对铲斗进行扫描，预先构建所述铲斗点云模型。
根据本发明提供的一种铲斗三维位姿跟踪方法，所述根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云，包括：
根据所述述铲斗的颜色信息，删除所述第三雷达点云中所有与所述铲斗的颜色信息不匹配的点，以获取所述第四雷达点云。
根据本发明提供的一种铲斗三维位姿跟踪方法，在生成铲斗分布点云之后，还包括：利用驾驶室内的显示屏，展示所述铲斗分布点云。
第二方面，本发明还提供一种铲斗三维位姿跟踪装置，包括：
第一处理单元，用于根据铲斗的宽度值，以及所述铲斗与激光雷达的相对运动范围，对第一雷达点云进行直通滤波，获取第二雷达点云；所述第一雷达点云是由所述激光雷达采集的；
第二处理单元，用于基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云；所述相机图像是由所述相机拍摄的；
第三处理单元，用于根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云；
第四处理单元，用于获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息，以根据所述相对位姿信息对所述铲斗点云模型进行渲染，生成铲斗分布点云。
第三方面，本发明还提供一种铲斗三维位姿跟踪系统，包括：
激光雷达、相机、显示屏和工控机；其中，所述工控机至少包括上述铲斗三维位姿跟踪装置。
第四方面，本发明还提供一种挖掘机，至少包括上述铲斗三维位姿跟踪系统；其中，所述激光雷达与所述相机水平装设于所述挖掘机驾驶室外，且朝向所述铲斗。
第五方面，本发明提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述任一种铲斗三维位姿跟踪方法的步骤。
第六方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如上述任一种铲斗三维位姿跟踪方法的步骤。
本发明提供的铲斗三维位姿跟踪方法、装置、系统及挖掘机，仅需要将激光雷达和相机安装在驾驶室座舱上，而无需对斗杆和铲斗进行其他类型的传感器监测，也能够在线获取铲斗分布点云，结构简单、便于维护，且不易受外界环境影响。
附图说明
为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明提供的铲斗三维位姿跟踪方法的流程示意图；
图2是本发明提供的一种激光雷达和相机安装位置的俯视图；
图3是本发明提供的一种激光雷达和相机安装位置的正视图；
图4是本发明提供的铲斗三维位姿跟踪装置的结构示意图；
图5是本发明提供的电子设备的结构示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，在本发明实施例的描述中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。术语“上”、“下”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
本申请的说明书和权利要求书中的术语“第一”、“第二”等是用于区别类似的对象，而不用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施，且“第一”、“第二”等所区分的对象通常为一类，并不限定对象的个数，例如第一对象可以是一个，也可以是多个。此外，说明书以及权利要求中“和/或”表示所连接对象的至少其中之一，字符“/”，一般表示前后关联对象是一种“或”的关系。
激光雷达，其主要构成及工作原理是由雷达发射系统发送一个雷达信号，信号抵达目标障碍物的表面会引起散射，经过反射后的雷达信号被雷达接收系统所收集，以通过测量雷达信号的运行时间以确定目标障碍物与激光雷达之间的距离。
相对于传统的机械式激光雷达，固态激光雷达通过采用相控阵原理，取消了机械结构，通过调节雷达发射阵列中每个发射单元的相位差来改变雷达信号的出射角度。相控阵一般都是通过电信号对其相位进行控制实，以现雷达信号束指向所要扫描的目标。
本发明提供的铲斗三维位姿跟踪方法，综合利用激光雷达的这一特性，并结合相机进行实时图像拍摄，以实现铲斗三维位姿的实时跟踪。
下面结合图1-图5描述本发明所提供的铲斗三维位姿跟踪方法、装置、系统及挖掘机。
图1是本发明提供的铲斗三维位姿跟踪方法的流程示意图，如图1所示，包括但不限于以下步骤：
步骤101：根据铲斗的宽度值，以及所述铲斗与激光雷达的相对运动范围，对第一雷达点云进行直通滤波，获取第二雷达点云。
其中，所述第一雷达点云是由所述激光雷达采集的。
图2是本发明提供的一种激光雷达和相机安装位置的俯视图，图3是本发明提供的一种激光雷达和相机安装位置的正视图，如图2和图3所示，可以预先将激光雷达和相机固定安装在挖掘机驾驶室的前侧。其中，激光雷达和相机之间存在一定的安装间距，两者可以安装于同一水平线上，或者安装于同一垂直线上。
其中，激光雷达可以采用固态激光雷达，以能够根据挖掘机的动力臂以及铲斗的位置变化，自动改变雷达信号的出射角度，以更为准确的实时采集包含铲斗所在位置的第一雷达点云。
由于第一雷达点云中不仅仅包括铲斗活动范围内的点云，还包括其他非铲斗活动范围内的干扰点云。本发明通过获取铲斗的宽度值，以及铲斗与激光雷达的相对运动范围，以确定铲斗活动范围。
其中，铲斗与激光雷达的相对运动范围是指铲斗相对激光雷达进行运动过程中的最大移动距离范围。一般认为铲斗的宽度所在的方向与相对运动范围所在的方向互相垂直，故两者所确定出的铲斗活动范围可以近似为一块矩形区域。
根据确定的铲斗活动范围，从激光雷达实施采集的第一雷达点云中直通滤波出铲斗活动范围所在区域的点云，作为第二雷达点云。
可选地，上述直通滤波的方法可以是：通过制定铲斗活动范围所在的边界坐标(也可以仅仅指定铲斗活动范围所在矩形区域的四个顶点坐标)，对第一雷达点云进行剪裁，以保留铲斗活动范围内的所有点，则可以获取第二雷达点云。
步骤102：基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云。
其中，所述相机图像是由所述相机拍摄的。
经过步骤101获取到第二雷达点云之后，已删除了大部分干扰点云，唯一会对铲斗三维位姿跟踪造成影响的就是动力臂等部位相关的少量点云。本发明通过借助铲斗的颜色与动力臂等部位的颜色存在差异这一特性，从第二雷达点云中剔除非铲斗部位对铲斗三维位姿跟踪的影响。
在此之前，首先通过将第二雷达点云与相机所拍摄的相机图像进行融合，由于相机图像是RGB图像，其每个像素点均具备有相应的颜色信息，故融合后所获取到的第三雷达点云也具有了颜色信息，即第三雷达点云中的每个点具有对应的颜色信息。
本发明是利用激光雷达与相机之间的映射矩阵，来表示激光雷达与相机在世界坐标系中的安装位置和相对位置关系。由于获取第二雷达点云的过程中，仅对第一雷达点云进行了剪裁而并不涉及点云位置的变化，故本发明在确定了激光雷达与相机的相对位置关系之后，通过相机自身的内参矩阵，则可以将激光雷达相关的第二雷达点云与相机图像拍摄的相机图像进行快速融合，并将融合后所获取到的带有颜色信息的点云，作为第三雷达点云。
步骤103：根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云.
在步骤103中，主要是借助铲斗的颜色，以其他非铲斗部位的颜色存在区别，以从第三雷达点云中删除掉一些干扰点，从而获取到仅包含铲斗所在位置的点云，作为第四雷达点云。
具体地，由于铲斗的颜色是灰褐色，而动力臂等其他部位的颜色不是灰褐色的，故本发明通过这一颜色信息，删除掉第三雷达点云中不具有灰褐色这一颜色信息的点，则可以获取到仅仅包括铲斗所在位置的第三雷达点云。
步骤104：获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息，以根据所述相对位姿信息对所述铲斗点云模型进行渲染，生成表示铲斗点云分布的第四雷达点云。
在步骤104中，首先将实时获取到的第四雷达点云和预先构建的铲斗点云模型进行相对位姿计算。
其中，铲斗点云模型是指采用离线的方式，根据铲斗的实际尺寸，通过预先利用固态激光雷达和相机对铲斗进行扫描，所构建的具有颜色信息的点云模型，该模型可以视作铲斗的标准参考模型。
通过将第四雷达点云与铲斗点云模型进行相对位姿计算，所获取的相对位姿信息能够用于表示铲斗在当前情况下的真实位姿。
进一步地，根据获取的相对位姿信息，再对铲斗点云模型进行渲染，仿真出铲斗的实际位姿，就能够获取带有颜色信息的铲斗分布点云。
在获取铲斗分布点云之后，则可以根据其点云的分布情况，从中确定出各铲斗齿的空间三维坐标位置。
本发明提供的铲斗三维位姿跟踪方法，仅需要将激光雷达和相机安装在驾驶室座舱上，而无需对斗杆和铲斗进行其他类型的传感器监测，也能够在线获取铲斗分布点云，结构简单、便于维护，且不易受外界环境影响。
基于上述实施例的内容，作为一种可选实施例，在将所述第二雷达点云投影到相机图像中生成第三雷达点云之前，还包括：
利用第一旋转矩阵和第一平移向量表示所述激光雷达的世界坐标原点，并利用第二旋转矩阵和第二平移向量表示所述相机的世界坐标原点；
确定所述第一旋转矩阵与所述第二旋转矩阵之间的相对旋转矩阵，并确定所述第一平移向量与所述第二平移向量之间的相对平移向量；
根据所述相对旋转矩阵和所述相对平移向量，确定所述映射矩阵。
其中，旋转矩阵描述了世界坐标系的坐标轴相对于相机坐标轴的方向；平移向量描述了在相机坐标系下空间原点的位置。
作为一种可选地实施例，可以将激光雷达的世界坐标原点用第一旋转矩阵Ro1和第一平移向量Tm1来综合表示，并将相机的世界坐标原点可以用第二旋转矩阵Ro2和第二平移向量Tm2来表示。
其中，第一旋转矩阵Ro1和第二旋转矩阵Ro2均为3*3的矩阵，第一平移向量Tm1和第二平移向量Tm2均为1*3的向量。
由此，分别计算出第一旋转矩阵Ro1和第二旋转矩阵Ro2之间的相对旋转矩阵Rw，以及第一平移向量Tm1和第二平移向量Tm2之间的相对平移向量tw。
其中，相对旋转矩阵Rw也是3*3的矩阵，相对平移向量tw也是1*3的向量。
最后，根据相对旋转矩阵Rw和相对平移向量tw，生成映射矩阵R1的计算公式为：
本发明提供的铲斗三维位姿跟踪方法，采用映射矩阵标识出激光雷达和相机之间的相对位置关系，为后期将雷达点云与相机图像进行融合提供了基础，能够有效地提高融合的精确性。
基于上述实施例的内容，作为一种可选实施例，所述基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云，包括：
根据所述相对旋转矩阵和所述相对平移向量，对所述第二雷达点云进行位姿变换，获取第五雷达点云；
基于所述内参矩阵，将所述第五雷达点云投影至所述相机图像，生成所述第三雷达点云。
第五雷达点云的表达式为：Rw·Pt+tw；
其中，Rw为相对旋转矩阵，tw为相对平移向量，Pt为第二雷达点云。
设相机的内参矩阵其中，fx、fy和cy分别为相机的内参，则第三雷达点云Pr表达式为：/>
其中，
其中，X、Y、H分别为点云投影中间变量。
本发明提供了一种将雷达点云与相机图像进行融合的方法，通过将第二雷达点云投影到相机图像中，所获取到的第三雷达点云则同时具有了第二雷达点云中的位置信息，同时具有相机图像中的颜色信息，为进一步从第二雷达点云中剔除非铲斗相关的点云提供了基础，即为实时进行铲斗三维位姿跟踪提供了数据基础，提升了模拟仿真的真实性和准确性。
基于上述实施例的内容，作为一种可选实施例，在获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息之前，还包括：利用激光雷达和相机对铲斗进行扫描，预先构建所述铲斗点云模型。
其中，铲斗点云模型是通过离线的方式预先构建的，其构建方法可以是：
利用固态激光雷达和相机对铲斗进行单独扫描(即扫描对象仅包括铲斗)，并将获取的雷达点云与相机图像进行融合，以获取带有颜色的铲斗雷达点云。
进一步地，可以采用人工处理的方式对获取的铲斗雷达点云进行干扰点去除，仅保留铲斗部分点云，最终将获取到的铲斗雷达点云作为铲斗点云模型。
本发明利用离线的方式预先构建出标准的铲斗点云模型，这样在获取到第四雷达点云之后，将其与铲斗点云模型进行比对，就可以获取到第四雷达点云与铲斗点云模型之间的相对位姿信息，再利用相对位姿信息对铲斗点云模型进行渲染，就能够获取到当前铲斗的雷达点云。
故本发明仅仅需要获取需要对铲斗点云模型进行位姿变换的相对位姿信息，就可以快速完成铲斗三维雷达点云的重建，而无需重建铲斗的全部雷达点云，有效地提高了跟踪响应速度，使得实时跟踪成为可能，且获取到的铲斗分布点云分布更清晰、更真实。
需要说明的是，在本发明所提供的铲斗三维位姿跟踪方法中，获取第四雷达点云与铲斗点云模型之间的相对位姿信息的方法，可以采用点云配准算法(Point CloudRegistration)中的迭代最近点算法(Iterative Closest Point，ICP)来实现，即计算出两者之间的相对位姿信息采用相对旋转矩阵Rk和相对平移向量tk来表示，对此本发明不作具体的限定。
基于上述实施例的内容，作为一种可选实施例，所述根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云，包括：
根据所述述铲斗的颜色信息，删除所述第三雷达点云中所有与所述铲斗的颜色信息不匹配的点，以获取所述第四雷达点云。
本发明主要是借助铲斗的颜色与其他非铲斗部位的颜色存在区别，以从第三雷达点云中删除掉其他非铲斗部位所对应位置的所有干扰点，从而获取到仅包含铲斗所在位置的点云，作为第四雷达点云。
具体地，由于铲斗的颜色是灰褐色，而动力臂等其他部位的颜色不是灰褐色的，故本发明通过这一颜色信息，删除掉第三雷达点云中不具有灰褐色这一颜色信息的点，则可以获取到仅仅包括铲斗所在位置的第三雷达点云。
可选地，可以人工将铲斗的颜色设置为较为鲜艳的颜色，以提高其与其他非铲斗部位之间的颜色区别，这样能够进一步提升位姿跟踪的精度。
本发明提供的铲斗三维位姿跟踪方法，充分利用相机图像所具有的颜色信息，以利用铲斗的外在颜色与其他非铲斗部位之间的区别性，能准确、快速的冲第三雷达点云中筛选出仅包含铲斗所在位置的第四雷达点云，大大提高了识别的精度。
基于上述实施例的内容，作为一种可选实施例，在生成铲斗分布点云之后，还包括：利用驾驶室内的显示屏，展示所述铲斗分布点云。
本发明所提供的铲斗三维位姿跟踪方法，主要是通过构建铲斗分布点云，为挖掘机驾驶员的操作提供参考，故在本发明提供的铲斗三维位姿跟踪方法中，在获取到铲斗分布点云之后，将其展示在驾驶室内的显示屏，驾驶员可以在驾驶、操控挖掘机的过程中，实时查看铲斗的位姿，以克服现有技术中驾驶员操作时仅凭主观感觉进行操作，所带来的操作精度差的缺陷。
可选地，在展示铲斗分布点云的同时，还可以在显示屏中实时输出铲斗齿的空间三维坐标位置，这样能够为操作员进一步提高操作精度做出指导。
图4是本发明提供的铲斗三维位姿跟踪装置的结构示意图，如图4所示，主要包括第一处理单元41、第二处理单元42、第三处理单元43和第四处理单元44，其中：
第一处理单元41主要用于根据铲斗的宽度值，以及所述铲斗与激光雷达的相对运动范围，对第一雷达点云进行直通滤波，获取第二雷达点云；所述第一雷达点云是由所述激光雷达采集的。
第二处理单元42主要用于基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云；所述相机图像是由所述相机拍摄的。
第三处理单元43主要用于根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云。
第四处理单元44主要用于获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息，以根据所述相对位姿信息对所述铲斗点云模型进行渲染，生成铲斗分布点云。
需要说明的是，本发明实施例提供的铲斗三维位姿跟踪装置，在具体运行时，可以执行上述任一实施例所述的铲斗三维位姿跟踪方法，对此本实施例不作赘述。
本发明提供的铲斗三维位姿跟踪装置，仅需要将激光雷达和相机安装在驾驶室座舱上，而无需对斗杆和铲斗进行其他类型的传感器监测，也能够在线获取铲斗分布点云，结构简单、便于维护，且不易受外界环境影响。
基于上述实施例的内容，作为一种可选实施例，本发明还提供一种铲斗三维位姿跟踪系统，主要包括：激光雷达、相机、显示屏和工控机；其中，所述工控机至少包括上述铲斗三维位姿跟踪装置。
需要说明的是，本发明实施例提供的铲斗三维位姿跟踪系统，其在具体运行时，可以执行上述任一实施例所述的铲斗三维位姿跟踪方法，对此本实施例不作赘述。
作为一种可选实施例，本发明还提供一种挖掘机，至少包括上述的铲斗三维位姿跟踪系统；其中，所述激光雷达与所述相机水平装设于所述挖掘机驾驶室外，且朝向所述铲斗。
图5是本发明提供的电子设备的结构示意图，如图5所示，该电子设备可以包括：处理器(processor)510、通信接口(Communications Interface)520、存储器(memory)530和通信总线540，其中，处理器510，通信接口520，存储器530通过通信总线540完成相互间的通信。处理器510可以调用存储器530中的逻辑指令，以执行铲斗三维位姿跟踪方法，该方法包括：根据铲斗的宽度值，以及所述铲斗与激光雷达的相对运动范围，对第一雷达点云进行直通滤波，获取第二雷达点云；所述第一雷达点云是由所述激光雷达采集的；基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云；所述相机图像是由所述相机拍摄的；根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云；获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息，以根据所述相对位姿信息对所述铲斗点云模型进行渲染，生成铲斗分布点云。
此外，上述的存储器530中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
另一方面，本发明还提供一种计算机程序产品，所述计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，计算机能够执行上述各方法所提供的铲斗三维位姿跟踪方法，该方法包括：根据铲斗的宽度值，以及所述铲斗与激光雷达的相对运动范围，对第一雷达点云进行直通滤波，获取第二雷达点云；所述第一雷达点云是由所述激光雷达采集的；基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云；所述相机图像是由所述相机拍摄的；根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云；获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息，以根据所述相对位姿信息对所述铲斗点云模型进行渲染，生成铲斗分布点云。
又一方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各实施例提供的铲斗三维位姿跟踪方法，该方法包括：根据铲斗的宽度值，以及所述铲斗与激光雷达的相对运动范围，对第一雷达点云进行直通滤波，获取第二雷达点云；所述第一雷达点云是由所述激光雷达采集的；基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云；所述相机图像是由所述相机拍摄的；根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云；获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息，以根据所述相对位姿信息对所述铲斗点云模型进行渲染，生成铲斗分布点云。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (9)
1.一种铲斗三维位姿跟踪方法，其特征在于，包括：
根据铲斗的宽度值，以及所述铲斗与激光雷达的相对运动范围，对第一雷达点云进行直通滤波，获取第二雷达点云；所述第一雷达点云是由所述激光雷达采集的；
基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云；所述相机图像是由所述相机拍摄的；
根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云；
获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息，以根据所述相对位姿信息对所述铲斗点云模型进行渲染，生成铲斗分布点云；
在将所述第二雷达点云投影到相机图像中生成第三雷达点云之前，还包括：
利用第一旋转矩阵和第一平移向量表示所述激光雷达的世界坐标原点，并利用第二旋转矩阵和第二平移向量表示所述相机的世界坐标原点；
确定所述第一旋转矩阵与所述第二旋转矩阵之间的相对旋转矩阵，并确定所述第一平移向量与所述第二平移向量之间的相对平移向量；
根据所述相对旋转矩阵和所述相对平移向量，确定所述映射矩阵。
2.根据权利要求1所述的铲斗三维位姿跟踪方法，其特征在于，所述基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云，包括：
根据所述相对旋转矩阵和所述相对平移向量，对所述第二雷达点云进行位姿变换，获取第五雷达点云；
基于所述内参矩阵，将所述第五雷达点云投影至所述相机图像，生成所述第三雷达点云。
3.根据权利要求1所述的铲斗三维位姿跟踪方法，其特征在于，在获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息之前，还包括：
利用激光雷达和相机对铲斗进行扫描，预先构建所述铲斗点云模型。
4.根据权利要求1所述的铲斗三维位姿跟踪方法，其特征在于，所述根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云，包括：
根据所述铲斗的颜色信息，删除所述第三雷达点云中所有与所述铲斗的颜色信息不匹配的点，以获取所述第四雷达点云。
5.根据权利要求1所述的铲斗三维位姿跟踪方法，其特征在于，在生成铲斗分布点云之后，还包括：利用驾驶室内的显示屏，展示所述铲斗分布点云。
6.一种铲斗三维位姿跟踪装置，其特征在于，包括：
第一处理单元，用于根据铲斗的宽度值，以及所述铲斗与激光雷达的相对运动范围，对第一雷达点云进行直通滤波，获取第二雷达点云；所述第一雷达点云是由所述激光雷达采集的；
第二处理单元，用于基于相机的内参矩阵，以及所述激光雷达与所述相机之间的映射矩阵，将所述第二雷达点云投影到相机图像中，生成第三雷达点云；所述相机图像是由所述相机拍摄的；
在将所述第二雷达点云投影到相机图像中生成第三雷达点云之前，还包括：
利用第一旋转矩阵和第一平移向量表示所述激光雷达的世界坐标原点，并利用第二旋转矩阵和第二平移向量表示所述相机的世界坐标原点；
确定所述第一旋转矩阵与所述第二旋转矩阵之间的相对旋转矩阵，并确定所述第一平移向量与所述第二平移向量之间的相对平移向量；
根据所述相对旋转矩阵和所述相对平移向量，确定所述映射矩阵；
第三处理单元，用于根据所述铲斗的颜色信息，从所述第三雷达点云中筛选出第四雷达点云；
第四处理单元，用于获取所述第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息，以根据所述相对位姿信息对所述铲斗点云模型进行渲染，生成铲斗分布点云。
7.一种铲斗三维位姿跟踪系统，其特征在于，包括：激光雷达、相机、显示屏和工控机；
所述工控机至少包括如权利要求6所述的铲斗三维位姿跟踪装置。
8.一种挖掘机，其特征在于，至少包括如权利要求7所述的铲斗三维位姿跟踪系统；
其中，所述激光雷达与所述相机水平装设于所述挖掘机驾驶室外，且朝向所述铲斗。
9.一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述铲斗三维位姿跟踪方法步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110802817.0A CN113506326B (zh) | 2021-07-15 | 2021-07-15 | 铲斗三维位姿跟踪方法、装置、系统及挖掘机 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110802817.0A CN113506326B (zh) | 2021-07-15 | 2021-07-15 | 铲斗三维位姿跟踪方法、装置、系统及挖掘机 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113506326A CN113506326A (zh) | 2021-10-15 |
| CN113506326B true CN113506326B (zh) | 2023-08-29 |
Family
ID=78013003
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110802817.0A Active CN113506326B (zh) | 2021-07-15 | 2021-07-15 | 铲斗三维位姿跟踪方法、装置、系统及挖掘机 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113506326B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115690203A (zh) * | 2022-09-28 | 2023-02-03 | 中南大学 | 一种铲斗位姿感知方法、系统及可存储介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2002096110A2 (en) * | 2001-05-18 | 2002-11-28 | Witten Technologies Inc. | A virtual camera on the bucket of an excavator displaying 3d images of buried pipes |
| CN102936906A (zh) * | 2007-05-17 | 2013-02-20 | 西门子工业公司 | 关于挖掘的系统、装置和/或方法 |
| CN104476548A (zh) * | 2014-10-24 | 2015-04-01 | 四川省绵阳西南自动化研究所 | 一种挖掘机器人自主作业控制系统及方法 |
| DE102016224076A1 (de) * | 2016-12-02 | 2018-06-07 | Robert Bosch Gmbh | Verfahren und Vorrichtung zum Bestimmen einer Position eines Baggerarms mittels eines an einem Bagger angeordneten LIDAR-Systems |
| CN112686206A (zh) * | 2021-01-20 | 2021-04-20 | 塞尔昂(上海)工业技术有限公司 | 一种智能斗齿监测系统 |
-
2021
- 2021-07-15 CN CN202110802817.0A patent/CN113506326B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2002096110A2 (en) * | 2001-05-18 | 2002-11-28 | Witten Technologies Inc. | A virtual camera on the bucket of an excavator displaying 3d images of buried pipes |
| CN102936906A (zh) * | 2007-05-17 | 2013-02-20 | 西门子工业公司 | 关于挖掘的系统、装置和/或方法 |
| CN104476548A (zh) * | 2014-10-24 | 2015-04-01 | 四川省绵阳西南自动化研究所 | 一种挖掘机器人自主作业控制系统及方法 |
| DE102016224076A1 (de) * | 2016-12-02 | 2018-06-07 | Robert Bosch Gmbh | Verfahren und Vorrichtung zum Bestimmen einer Position eines Baggerarms mittels eines an einem Bagger angeordneten LIDAR-Systems |
| CN112686206A (zh) * | 2021-01-20 | 2021-04-20 | 塞尔昂(上海)工业技术有限公司 | 一种智能斗齿监测系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113506326A (zh) | 2021-10-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP2187166B1 (en) | Industrial Machine | |
| EP2914975B1 (en) | Extrinsic calibration of imaging sensing devices and 2d lidars mounted on transportable apparatus | |
| CN111025283B (zh) | 雷达与球机联动的方法及装置 | |
| JP6807781B2 (ja) | 表示システム、表示方法、及び遠隔操作システム | |
| JP7285051B2 (ja) | 表示制御装置、および表示制御方法 | |
| CN111680611B (zh) | 一种道路通过性检测方法、系统及设备 | |
| DE102019115874A1 (de) | Systeme und verfahren zur verbesserten entfernungsschätzung durch eine monokamera unter verwendung von radar- und bewegungsdaten | |
| JP6966218B2 (ja) | 撮像装置の校正装置、作業機械および校正方法 | |
| EP2728376A1 (en) | Extrinsic calibration of imaging sensing devices and 2D LIDARs mounted on transportable apparatus | |
| DE60206320T2 (de) | Vorrichtung und Verfahren zur perspektivischen Projektionbilderzeugung | |
| EP2788717A1 (en) | Position and orientation determination in 6-dof | |
| CN113506326B (zh) | 铲斗三维位姿跟踪方法、装置、系统及挖掘机 | |
| US20160275359A1 (en) | Information processing apparatus, information processing method, and computer readable medium storing a program | |
| JP6350082B2 (ja) | 画像処理装置 | |
| EP3589575B1 (en) | A vehicle provided with an arrangement for determining a three dimensional representation of a movable member | |
| RU2562368C1 (ru) | Способ трёхмерного (3d) картографирования | |
| KR20220113781A (ko) | 환경의 지형을 측정하는 방법 | |
| CN112937444B (zh) | 作业机械的辅助影像生成方法、装置和作业机械 | |
| JP4565348B2 (ja) | 形状測定装置及びその方法 | |
| CN116778094B (zh) | 一种基于优选视角拍摄的建筑物变形监测方法及装置 | |
| US20160150189A1 (en) | Image processing system and method | |
| Pfeiffer et al. | Ground truth evaluation of the Stixel representation using laser scanners | |
| CN110021210B (zh) | 一种具有可扩展性虚拟空间的无人机vr训练方法 | |
| CN115100287A (zh) | 外参标定方法及机器人 | |
| CN112651405A (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 | ||
| GR01 | Patent grant |