CN117226810B - 一种绳索载重并联机器人及其避障方法、装置、存储介质 - Google Patents
一种绳索载重并联机器人及其避障方法、装置、存储介质 Download PDFInfo
- Publication number
- CN117226810B CN117226810B CN202311501018.5A CN202311501018A CN117226810B CN 117226810 B CN117226810 B CN 117226810B CN 202311501018 A CN202311501018 A CN 202311501018A CN 117226810 B CN117226810 B CN 117226810B
- Authority
- CN
- China
- Prior art keywords
- information
- state
- matrix
- detection information
- extended kalman
- 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 56
- 238000003860 storage Methods 0.000 title claims abstract description 20
- 238000001514 detection method Methods 0.000 claims abstract description 119
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 95
- 239000011159 matrix material Substances 0.000 claims abstract description 62
- 238000001914 filtration Methods 0.000 claims abstract description 56
- 238000012545 processing Methods 0.000 claims abstract description 17
- 238000013519 translation Methods 0.000 claims description 35
- 230000009466 transformation Effects 0.000 claims description 29
- 230000007704 transition Effects 0.000 claims description 24
- 238000004590 computer program Methods 0.000 claims description 22
- 230000006870 function Effects 0.000 claims description 17
- 238000006243 chemical reaction Methods 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 7
- 238000005312 nonlinear dynamic Methods 0.000 abstract description 6
- 238000010276 construction Methods 0.000 abstract 1
- 238000004891 communication Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 4
- 238000005259 measurement Methods 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000000750 progressive effect Effects 0.000 description 3
- 238000012549 training Methods 0.000 description 3
- 238000012800 visualization Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000002372 labelling Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 238000009435 building construction Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000011897 real-time detection Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Landscapes
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种绳索载重并联机器人及其避障方法、装置、存储介质，获取点云信息,并对点云信息进行处理后输入至目标检测网络,以获得多目标的检测信息,将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息;获取当前时刻的检测信息,采用马氏距离构建贪心匹配算法的成本矩阵对当前时刻的检测信息和当前时刻的状态预测信息进行关联匹配,以生成关联信息;根据关联信息预测生成下一时刻的检测信息,并基于吊装重物与其他类别物体在下一时刻的欧式距离生成用于控制执行器的控制信号。在作业时，能够对非线性动态目标进行线性化处理，进而进行有效避障。
Description
技术领域
本发明涉及自动化领域，特别涉及一种绳索载重并联机器人及其避障方法、装置、存储介质。
背景技术
吊装机械作为工程机械是在制造业领域应用最为广泛的一种，主要应用于工厂搬运重物、码头吊运、建筑施工、飞机检修等场景，能够提高工作效率、减轻人力劳动，但其自动化、智能化程度普遍较低。
绳驱并联机器人则是一种特殊的并联机器人，选择了多根轻量化、柔性的绳索而非传统的刚性结构来作为运动链，实现对执行器末端的并联控制驱动，具有着刚性并联结构高负载的构型特点，同时还具备着绳索驱动的运动惯量小、工作空间大、低成本等特点，被广泛应用于航天、制造业、医疗等领域。但由于绳索自身结构特点，动力学模型难以建立，不确定因素较多，因此将多绳驱动并联机器人运用于工业重型吊装领域的研究暂时未能有突破性进展。
在现有技术中,采用基于深度学习的目标检测算法搭配传统滤波算法（卡尔曼滤波算法KF）。KF用于跟踪目标的位置和速度，在每一帧中，利用当前帧的测量结果（目标检测）和上一帧的状态预测（卡尔曼滤波的先验估计），通过KF算法进行状态的更新和预测。KF根据测量结果和预测结果的不确定性来权衡二者的贡献，得到最优的估计结果。并以3DIoU（Intersection over Union，交集并集比值（交并比））作为度量，使用匈牙利匹配算法对当前帧中检测到的目标与前一帧已跟踪的目标进行关联匹配。然而,卡尔曼滤波算法只能针对线性模型，不适用于多变的实际作业场景；匈牙利算法是按照顺序匹配的，有可能会错过后续更大的IoU结果；针对非刚体（行人），3D IoU作为度量效果并不好，小目标物体速度过快时会导致检测框无法重叠。
有鉴于此，提出本申请。
发明内容
本发明公开了一种绳索载重并联机器人及其避障方法、装置、存储介质，旨在实际作业时，基于场景中的非线性动态目标进行线性化处理，实现未来帧的轨迹预测，进而进行有效避障。
本发明第一实施例提供了一种绳索载重并联机器人的避障方法,包括:
获取由激光雷达采集到的点云信息,并对所述点云信息进行处理后输入至目标检测网络,以获得多目标的检测信息,其中,所述检测信息包括类别、位姿、置信度;
将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息;
获取当前时刻的检测信息,采用马氏距离构建贪心匹配算法的成本矩阵对所述当前时刻的检测信息和所述当前时刻的状态预测信息进行关联匹配,以生成关联信息;
根据所述关联信息预测生成下一时刻的检测信息,并根据所述下一时刻的检测信息判断吊装重物与其他类别物体在下一时刻的欧式距离,其中,所述下一时刻的欧式距离用于和安全距离生成用于控制执行器的控制信号。
优选地,在所述将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息之前，还包括：
将所述激光雷达采集到的位置信息坐标从激光雷达坐标系下经过旋转矩阵、平移向量的变换生成在工作空间坐标系下的物体位置信息，具体变换过程为：
在雷达坐标系下的点经过旋转变换得到点/>,旋转变换公式为：/>，再经过平移变换得到点/>，平移变换公式为：；
将旋转矩阵和平移向量/>相结合，两坐标系转换关系为：，其中，旋转矩阵/>为3×3矩阵，平移向量/>为3维平移向量。
优选地,所述获取由激光雷达采集到的点云信息,并对所述点云信息进行处理后输入至目标检测网络,以获得多目标的检测信息，具体为：
对激光雷达采集到的点云信息进行直通滤波，以滤除非关注区域点云；
对滤除非关注区域点云的点云信息进行体素特征编码后输入至目标检测网络进行三维全稀疏卷积,以获得多目标的检测信息。
优选地,所述获取当前时刻的检测信息,采用马氏距离构建贪心匹配算法的成本矩阵对所述当前时刻的检测信息和所述当前时刻的状态预测信息进行关联匹配具体为：
在扩展卡尔曼滤波算法中通过雅克比矩阵将非线性函数线性化，同时搭配以马氏距离为度量的贪心匹配算法对跟踪特征前后帧进行关联匹配，其中，所述扩展卡尔曼滤波算法的预测与更新具体公式如下：
确定运动状态矩阵、状态转移矩阵/>和观测矩阵/>；
扩展卡尔曼滤波算法的状态转移方程为，扩展卡尔曼滤波算法的状态观测方程为/>；
利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在上一次的估计值处展开得：/>，再利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在本轮的状态预测值处展开得：； 其中，/>和/>分别表示函数和/>在/>和/>处的雅克比矩阵；
给出EKF的预测和更新两个步骤：
状态预测：
，/>；
状态更新：
，/>；
，/>；
其中雅克比矩阵和/>分别为：/>，/>,/>为单元矩阵。
本发明第二实施例提供了一种绳索载重并联机器人的避障装置,包括:
检测信息获取单元，用于获取由激光雷达采集到的点云信息,并对所述点云信息进行处理后输入至目标检测网络,以获得多目标的检测信息,其中,所述检测信息包括类别、位姿、置信度;
预测单元，用于将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息;
关联单元，用于获取当前时刻的检测信息,采用马氏距离构建贪心匹配算法的成本矩阵对所述当前时刻的检测信息和所述当前时刻的状态预测信息进行关联匹配,以生成关联信息;用于将当前获取的检测信息和经EKF得到的当前预测信息，采用马氏距离作为构建贪心匹配算法的成本矩阵进行关联匹配，生成关联信息；
控制单元，用于根据所述关联信息预测生成下一时刻的检测信息,并根据所述下一时刻的检测信息判断吊装重物与其他类别物体在下一时刻的欧式距离,其中,所述下一时刻的欧式距离用于和安全距离生成用于控制执行器的控制信号。
优选地,在所述将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息之前，还包括：
将所述激光雷达采集到的位置信息坐标从激光雷达坐标系下经过旋转矩阵、平移向量的变换生成在工作空间坐标系下的物体位置信息，具体变换过程为：
在雷达坐标系下的点经过旋转变换得到点/>,旋转变换公式为：/>，再经过平移变换得到点/>，平移变换公式为：；
将旋转矩阵和平移向量/>相结合，两坐标系转换关系为：，其中，旋转矩阵/>为3×3矩阵，平移向量/>为3维平移向量。
优选地,所述检测信息获取单元具体用于：
对激光雷达采集到的点云信息进行直通滤波，以滤除非关注区域点云；
对滤除非关注区域点云的点云信息进行体素特征编码后输入至目标检测网络进行三维全稀疏卷积,以获得多目标的检测信息。
优选地,所述关联单元具体用于：
在扩展卡尔曼滤波算法中通过雅克比矩阵将非线性函数线性化，同时搭配以马氏距离为度量的贪心匹配算法对跟踪特征前后帧进行关联匹配，其中，所述扩展卡尔曼滤波算法的预测与更新具体公式如下：
确定运动状态矩阵、状态转移矩阵/>和观测矩阵/>；
扩展卡尔曼滤波算法的状态转移方程为，扩展卡尔曼滤波算法的状态观测方程为/>；
利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在上一次的估计值处展开得：/>，再利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在本轮的状态预测值处展开得：； 其中，/>和/>分别表示函数和/>在/>和/>处的雅克比矩阵；
给出EKF的预测和更新两个步骤：
状态预测：
，/>；
状态更新：
，/>；
，/>；
其中雅克比矩阵和/>分别为：/>，/>,/>为单元矩阵。
本发明第三实施例提供了一种绳索载重并联机器人，包括存储器以及处理器，所述存储器内存储有计算机程序，所述计算机程序能够被所述处理器执行，以实现如上任意一项所述的一种绳索载重并联机器人的避障方法。
本发明第四实施例提供了一种可读存储介质，存储有计算机程序，所述计算机程序能够被所述计算机可读存储介质所在设备的处理器执行，以实现如上任意一项所述的一种绳索载重并联机器人的避障方法。
基于本发明提供的一种绳索载重并联机器人及其避障方法、装置、存储介质，先获取由激光雷达采集到的点云信息,并对所述点云信息进行处理后输入至目标检测网络,以获得多目标的检测信息,接着，将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息;再接着，获取当前时刻的检测信息,采用马氏距离构建贪心匹配算法的成本矩阵对所述当前时刻的检测信息和所述当前时刻的状态预测信息进行关联匹配,以生成关联信息;最后，根据所述关联信息预测生成下一时刻的检测信息,并根据所述下一时刻的检测信息判断吊装重物与其他类别物体在下一时刻的欧式距离,其中,所述下一时刻的欧式距离用于和安全距离生成用于控制执行器的控制信号。旨在实际作业时，基于场景中的非线性动态目标进行性化处理，实现未来帧的轨迹预测，进而进行有效避障。
附图说明
图1是本发明第一实施例提供了一种绳索载重并联机器人及其避障方法的流程示意图；
图2是本发明提供的一种绳索机器人的感知传感器布置的三维结构示意图；
图3是本发明提供一种绳索机器人的数据集处理流程示意图；
图4是本发明提供一种绳索机器人的整体方案流程示意图；
图5是本发明提供一种绳索机器人的感知避障系统的硬件交互流程图；
图6是本发明第二实施例提供了一种绳索载重并联机器人及其避障装置的模块示意图；
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。
应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
在本发明实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本发明。在本发明实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。
应当理解，本文中使用的术语“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。
取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”或“响应于检测”。类似地，取决于语境，短语“如果确定”或“如果检测（陈述的条件或事件）”可以被解释成为“当确定时”或“响应于确定”或“当检测（陈述的条件或事件）时”或“响应于检测（陈述的条件或事件）”。
实施例中提及的“第一\第二”仅仅是是区别类似的对象，不代表针对对象的特定排序，可以理解地，“第一\第二”在允许的情况下可以互换特定的顺序或先后次序。应该理解“第一\第二”区分的对象在适当情况下可以互换，以使这里描述的实施例能够以除了在这里图示或描述的那些以外的顺序实施。
以下结合附图对本发明的具体实施例做详细说明。
本发明公开了一种绳索载重并联机器人及其避障方法、装置、存储介质，旨在实际作业时，基于场景中的非线性动态目标进行性化处理，实现未来帧的轨迹预测，进而进行有效避障。
请参阅图1，本发明第一实施例提供了一种绳索载重并联机器人的避障方法,其可由绳索载重并联机器人（以下简称评价机器人）来执行，特别的，由所述机器人内的一个或者多个处理器来执行，以实现如下步骤：
S101，获取由激光雷达采集到的点云信息,并对所述点云信息进行处理后输入至目标检测网络,以获得多目标的检测信息,其中,所述检测信息包括类别、位姿、置信度;
需要说明的是，请参阅图2，绳索载重并联机器人的主体可以包括固定底座1、立柱2、转台轴承3、滑轮4、卷扬机5、横梁6、载物装置7、激光雷达8、边缘计算机9组成，其可以激光雷达采集到的点云信息输入给边缘计算机进行处理，其中，激光雷达可以是velodyne-128激光雷达，所述边缘计算机可以为科技T906边缘计算平台，平台部署了目标检测、目标跟踪、实时避障、轨迹及速度规划等任务，为绳索重型并联机器人实时自主规划及安全避障提供了硬件支持。
在使用所述目标检测网络之前还包括对所述目标检测网络进行网络模型训练，其可以选择不同于常规目标检测训练集的标注形式，如图3所示，在离线制作点云数据集时，针对绳索机器人工作场景较为固定的特点，利用激光雷达采集多个场景下的多目标、多角度的连续帧点云数据，并以固定间隔的抽帧方式抽取点云，再对抽取的点云进行数据标注，标注内容包括：类别、位置、包围框尺寸、朝向角、ID等信息。同时对抽取出的点云数据进行一次直通滤波的预处理，将非关注点云区域滤除，其好处是能够提高后续的模型训练效果和计算效率，保留了重要的点云信息，提高了实时性能。
请结合图4，在点云输入目标检测网络前的预处理阶段先进行了一次直通滤波的数据预处理，滤除了非关注点云区域，然后再输入网络进行体素特征编码和三维全稀疏卷积等后续操作，在检测阶段获得了多目标的类别、位姿、置信度信息。此时获得的位置信息坐标位于激光雷达坐标系下，经过旋转、平移转换矩阵的变换获得工作空间坐标系下的物体位置信息，实时检测信息可通过Rviz（Robot Visualization，机器人可视化（用于可视化机器人系统的开源工具））可视化工具实时监测。如图4感知传感器布置的三维结构示意图所示。具体变换如下：
旋转矩阵为R，是一个3×3矩阵；
平移向量为T，是一个3维平移向量；
在雷达坐标系下的点经过旋转变换得到点/>,公式如下：
再经过平移变换得到点，公式如下：
旋转矩阵和平移向量相结合，两坐标系转换关系为：
S102，将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息;
需要说明的是，在目标检测阶段获得了多目标的类别、置信度、转换后的位姿信息后。先根据检测获得的置信度，将低置信度的目标检测信息删除。请结合图4，在目标跟踪阶段，将上一时刻t-1的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，经过扩展卡尔曼滤波算法（EKF）预测获得当前t时刻的状态预测信息。
S103，获取当前时刻的检测信息,采用马氏距离构建贪心匹配算法的成本矩阵对所述当前时刻的检测信息和所述当前时刻的状态预测信息进行关联匹配,以生成关联信息;
需要说明的是，将当前t时刻的状态预测信息与t时刻检测所获得的信息进行关联匹配，本实施例中采用贪心匹配算法，考虑到目标多个信息中的量纲各不相同，因此采用马氏距离构建贪心匹配算法的成本矩阵，在关联匹配后，匹配成功的信息用作于EKF状态更新，同时输出修正后的t时刻位姿信息，此时的检测信息实现了上下帧之间的关联，可作为后续实时避障模块的输入信息，需要说明的是，在目标跟踪阶段，采用了EKF算法将实际作业中非线性模型线性化处理，进而实现轨迹预测，EKF不同于KF采用线性模型，而是使用线性化的近似方式来处理非线性系统，并通过融合测量值和模型预测来估计系统的状态。贪心匹配算法则是不同于匈牙利匹配算法通过增广路径的方式来寻找最大匹配，而是强调最优决策进而实现局部最优效果，更加适用于较为固定场景的绳索并联机器人，选择马氏距离作为构建匹配成本矩阵的元素能够解决多个检测信息之间的量纲问题。
进一步地，在本实施例中，针对非线性模型采用扩展卡尔曼滤波算法（EKF）进行状态预测与更新，主要是考虑到各类目标运动轨迹并非线性，EKF中通过雅克比矩阵将非线性函数线性化，同时搭配以马氏距离为度量的贪心匹配算法对跟踪特征前后帧进行关联匹配，实现局部最优，进而实现对非线性动态目标物的轨迹（位置、速度）预测。
所述扩展卡尔曼滤波算法的预测与更新具体公式如下：
确定运动状态矩阵、状态转移矩阵/>和观测矩阵/>；
扩展卡尔曼滤波算法的状态转移方程为，扩展卡尔曼滤波算法的状态观测方程为/>；
利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在上一次的估计值处展开得：/>，再利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在本轮的状态预测值处展开得：； 其中，/>和/>分别表示函数和/>在/>和/>处的雅克比矩阵；
给出EKF的预测和更新两个步骤：
状态预测：
，/>；
状态更新：
，/>；
，/>；
其中雅克比矩阵和/>分别为：/>，/>,/>为单元矩阵。
S104，根据所述关联信息预测生成下一时刻的检测信息,并根据所述下一时刻的检测信息判断吊装重物与其他类别物体在下一时刻的欧式距离,其中,所述下一时刻的欧式距离用于和安全距离生成用于控制执行器的控制信号。
需要说明的是，在目标跟踪阶段输出前后帧关联的位姿信息后，进一步计算当前t时刻的速度，本实施例采用线性估计的方法，根据历史关联的连续帧位置偏置和时间间隔得到t时刻的瞬时速度，由此进而预测t+1时刻的位姿与速度信息，将其输入实时避障模块。如图4所示，实时避障模块是基于感知部分所提供的目标信息进行决策，两模块之间是通过ROS（全称Robot Operating System，机器人操作系统）通讯的方式将实时感知预测出的结果传输到避障模块中。先根据不同目标类别设置不同的安全距离，再根据感知部分t+1时刻的预测结果（t+1时刻吊装重物以及各个目标的位置以及速度信息），计算吊装重物与其他类别物体t+1时刻的欧式距离Dt+1。最后再基于安全距离S，判断两距离大小关系，若Dt+1≤S，则通过can（Controller Area Network，控制器区域网络通讯）通讯发送急停报文给信号处理系统MicroLabBox，进而控制伺服驱动器驱动电机急停，若Dt+1>S，则继续按照轨迹规划部分生成的实时轨迹继续作业。同时在此基础上如图5所示，采用三维动态捕捉系统，获取执行器末端的实时位置，经信号处理系统MicroLabBox将信息传递到边缘计算平台，用以弥补目标检测对于吊装执行器末端出现的漏检误检情况，确保实现安全冗余。
更进一步地，轨迹规划部分则是通过ROS通讯实时获取t时刻目标检测获得的位置信息，进而构建障碍物地图，生成一条避开障碍物的时间最短，速度最优的路线，并发送至信号处理系统MicroLabBox中进而控制伺服驱动器驱动电机，让绳索重载并联机器人实现自主轨迹规划。
需要说明的是，ROS通讯的方式将前端感知信息传输到实时避障模块中，这种通讯方式优点是可实现分布式通讯，让规划模块和实时避障模块能够同时收到感知系统所输出的信息，同时还具有可视化工具支持，可以更加直观的监测机器人的状态以及运动轨迹和各类目标信息。选择在有了感知模块实时获取周围环境目标信息基础上，还增加了三维动态捕捉系统，用来提供执行器末端的实时位置，用以弥补目标检测对于吊装执行器末端出现的漏检误检情况，确保实现安全冗余。
请参阅图6，本发明第二实施例提供了一种绳索载重并联机器人的避障装置,包括:
检测信息获取单元201，用于获取由激光雷达采集到的点云信息,并对所述点云信息进行处理后输入至目标检测网络,以获得多目标的检测信息,其中,所述检测信息包括类别、位姿、置信度;
预测单元202，用于将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息;
关联单元203，用于获取当前时刻的检测信息,采用马氏距离构建贪心匹配算法的成本矩阵对所述当前时刻的检测信息和所述当前时刻的状态预测信息进行关联匹配,以生成关联信息;
控制单元204，用于根据所述关联信息预测生成下一时刻的检测信息,并根据所述下一时刻的检测信息判断吊装重物与其他类别物体在下一时刻的欧式距离,其中,所述下一时刻的欧式距离用于和安全距离生成用于控制执行器的控制信号。
优选地,在所述将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息之前，还包括：
将所述激光雷达采集到的位置信息坐标从激光雷达坐标系下经过旋转矩阵、平移向量的变换生成在工作空间坐标系下的物体位置信息，具体变换过程为：
在雷达坐标系下的点经过旋转变换得到点/>,旋转变换公式为：/>，再经过平移变换得到点/>，平移变换公式为：；
将旋转矩阵和平移向量/>相结合，两坐标系转换关系为：，其中，旋转矩阵/>为3×3矩阵，平移向量/>为3维平移向量。
优选地,所述检测信息获取单元具体用于：
对激光雷达采集到的点云信息进行直通滤波，以滤除非关注区域点云；
对滤除非关注区域点云的点云信息进行体素特征编码后输入至目标检测网络进行三维全稀疏卷积,以获得多目标的检测信息。
优选地,所述关联单元具体用于：
在扩展卡尔曼滤波算法中通过雅克比矩阵将非线性函数线性化，同时搭配以马氏距离为度量的贪心匹配算法对跟踪特征前后帧进行关联匹配，其中，所述扩展卡尔曼滤波算法的预测与更新具体公式如下：
确定运动状态矩阵、状态转移矩阵/>和观测矩阵/>；
扩展卡尔曼滤波算法的状态转移方程为，扩展卡尔曼滤波算法的状态观测方程为/>；
利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在上一次的估计值处展开得：/>，再利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在本轮的状态预测值处展开得：； 其中，/>和/>分别表示函数和/>在/>和/>处的雅克比矩阵；
给出EKF的预测和更新两个步骤：
状态预测：
，/>；
状态更新：
，/>；
，/>；
其中雅克比矩阵和/>分别为：/>，/>,/>为单元矩阵。
本发明第三实施例提供了一种绳索载重并联机器人，包括存储器以及处理器，所述存储器内存储有计算机程序，所述计算机程序能够被所述处理器执行，以实现如上任意一项所述的一种绳索载重并联机器人的避障方法。
本发明第四实施例提供了一种可读存储介质，存储有计算机程序，所述计算机程序能够被所述计算机可读存储介质所在设备的处理器执行，以实现如上任意一项所述的一种绳索载重并联机器人的避障方法。
基于本发明提供的一种绳索载重并联机器人及其避障方法、装置、存储介质，先获取由激光雷达采集到的点云信息,并对所述点云信息进行处理后输入至目标检测网络,以获得多目标的检测信息,接着，将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息;再接着，获取当前时刻的检测信息,采用马氏距离构建贪心匹配算法的成本矩阵对所述当前时刻的检测信息和所述当前时刻的状态预测信息进行关联匹配,以生成关联信息;最后，根据所述关联信息预测生成下一时刻的检测信息,并根据所述下一时刻的检测信息判断吊装重物与其他类别物体在下一时刻的欧式距离,其中,所述下一时刻的欧式距离用于和安全距离生成用于控制执行器的控制信号。旨在实际作业时，基于场景中的非线性动态目标进行性化处理，实现未来帧的轨迹预测，进而进行有效避障。
示例性地，本发明第三实施例和第四实施例中所述的计算机程序可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述实现一种绳索载重并联机器人的避障设备中的执行过程。例如，本发明第二实施例中所述的装置。
所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器 (Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列 (Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述一种绳索载重并联机器人的避障方法的控制中心，利用各种接口和线路连接整个所述实现对一种绳索载重并联机器人的避障设备方法的各个部分。
所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现一种绳索载重并联机器人的避障方法的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序（比如声音播放功能、文字转换功能等）等；存储数据区可存储根据手机的使用所创建的数据（比如音频数据、文字消息数据等）等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘、智能存储卡（Smart Media Card, SMC）、安全数字（Secure Digital, SD）卡、闪存卡（Flash Card）、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
其中，所述实现的模块如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一个计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
需说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。
Claims (10)
1.一种绳索载重并联机器人的避障方法,其特征在于,包括:
获取由激光雷达采集到的点云信息,并对所述点云信息进行处理后输入至目标检测网络,以获得多目标的检测信息,其中,所述检测信息包括类别、位姿、置信度;
将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息;
获取当前时刻的检测信息,采用马氏距离构建贪心匹配算法的成本矩阵对所述当前时刻的检测信息和所述当前时刻的状态预测信息进行关联匹配,以生成关联信息;
根据所述关联信息预测生成下一时刻的检测信息,并根据所述下一时刻的检测信息判断吊装重物与其他类别物体在下一时刻的欧式距离,其中,所述下一时刻的欧式距离用于和安全距离生成用于控制执行器的控制信号。
2.根据权利要求1所述的一种绳索载重并联机器人的避障方法,其特征在于,在所述将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息之前，还包括：
将所述激光雷达采集到的位置信息坐标从激光雷达坐标系下经过旋转矩阵、平移向量的变换生成在工作空间坐标系下的物体位置信息，具体变换过程为：
在雷达坐标系下的点经过旋转变换得到点/>,旋转变换公式为：/>，再经过平移变换得到点/>，平移变换公式为：；
将旋转矩阵和平移向量/>相结合，两坐标系转换关系为：，其中，旋转矩阵/>为3×3矩阵，平移向量/> 为3维平移向量。
3.根据权利要求1所述的一种绳索载重并联机器人的避障方法,其特征在于,所述获取由激光雷达采集到的点云信息,并对所述点云信息进行处理后输入至目标检测网络,以获得多目标的检测信息，具体为：
对激光雷达采集到的点云信息进行直通滤波，以滤除非关注区域点云；
对滤除非关注区域点云的点云信息进行体素特征编码后输入至目标检测网络进行三维全稀疏卷积,以获得多目标的检测信息。
4.根据权利要求1所述的一种绳索载重并联机器人的避障方法,其特征在于,所述获取当前时刻的检测信息,采用马氏距离构建贪心匹配算法的成本矩阵对所述当前时刻的检测信息和所述当前时刻的状态预测信息进行关联匹配具体为：
在扩展卡尔曼滤波算法中通过雅克比矩阵将非线性函数线性化，同时搭配以马氏距离为度量的贪心匹配算法对跟踪特征前后帧进行关联匹配，其中，所述扩展卡尔曼滤波算法的预测与更新具体公式如下：
确定运动状态矩阵、状态转移矩阵/>和观测矩阵/>；
扩展卡尔曼滤波算法的状态转移方程为，扩展卡尔曼滤波算法的状态观测方程为/>；
利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在上一次的估计值处展开得：/>，再利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在本轮的状态预测值处展开得：； 其中，/>和/>分别表示函数和/>在/>和/>处的雅克比矩阵；
给出EKF的预测和更新两个步骤：
状态预测：
，/>；
状态更新：
，/>；
，/>；
其中雅克比矩阵和/>分别为：/>，/>,/>为单元矩阵。
5.一种绳索载重并联机器人的避障装置,其特征在于,包括:
检测信息获取单元，用于获取由激光雷达采集到的点云信息,并对所述点云信息进行处理后输入至目标检测网络,以获得多目标的检测信息,其中,所述检测信息包括类别、位姿、置信度;
预测单元，用于将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息;
关联单元，用于获取当前时刻的检测信息,采用马氏距离构建贪心匹配算法的成本矩阵对所述当前时刻的检测信息和所述当前时刻的状态预测信息进行关联匹配,以生成关联信息;
控制单元，用于根据所述关联信息预测生成下一时刻的检测信息,并根据所述下一时刻的检测信息判断吊装重物与其他类别物体在下一时刻的欧式距离,其中,所述下一时刻的欧式距离用于和安全距离生成用于控制执行器的控制信号。
6.根据权利要求5所述的一种绳索载重并联机器人的避障装置,其特征在于,在所述将上一时刻的检测信息输入到扩展卡尔曼滤波算法中对状态进行初始化，并通过扩展卡尔曼滤波算法进行预测,生成当前时刻的状态预测信息之前，还包括：
将所述激光雷达采集到的位置信息坐标从激光雷达坐标系下经过旋转矩阵、平移向量的变换生成在工作空间坐标系下的物体位置信息，具体变换过程为：
在雷达坐标系下的点经过旋转变换得到点/>,旋转变换公式为：/>，再经过平移变换得到点/>，平移变换公式为：；
将旋转矩阵和平移向量/>相结合，两坐标系转换关系为：，其中，旋转矩阵/>为3×3矩阵，平移向量/> 为3维平移向量。
7.根据权利要求5所述的一种绳索载重并联机器人的避障装置,其特征在于,所述检测信息获取单元具体用于：
对激光雷达采集到的点云信息进行直通滤波，以滤除非关注区域点云；
对滤除非关注区域点云的点云信息进行体素特征编码后输入至目标检测网络进行三维全稀疏卷积,以获得多目标的检测信息。
8.根据权利要求5所述的一种绳索载重并联机器人的避障装置,其特征在于,所述关联单元具体用于：
在扩展卡尔曼滤波算法中通过雅克比矩阵将非线性函数线性化，同时搭配以马氏距离为度量的贪心匹配算法对跟踪特征前后帧进行关联匹配，其中，所述扩展卡尔曼滤波算法的预测与更新具体公式如下：
确定运动状态矩阵、状态转移矩阵/>和观测矩阵/>；
扩展卡尔曼滤波算法的状态转移方程为，扩展卡尔曼滤波算法的状态观测方程为/>；
利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在上一次的估计值处展开得：/>，再利用泰勒展开式对扩展卡尔曼滤波算法的状态转移方程在本轮的状态预测值处展开得：； 其中，/>和/>分别表示函数和/>在/>和/>处的雅克比矩阵；
给出EKF的预测和更新两个步骤：
状态预测：
，/>；
状态更新：
，/>；
，/>；
其中雅克比矩阵和/>分别为：/>，/>,/>为单元矩阵。
9.一种绳索载重并联机器人，其特征在于，包括存储器以及处理器，所述存储器内存储有计算机程序，所述计算机程序能够被所述处理器执行，以实现如权利要求1至4任意一项所述的一种绳索载重并联机器人的避障方法。
10.一种可读存储介质，其特征在于，存储有计算机程序，所述计算机程序能够被所述可读存储介质所在设备的处理器执行，以实现如权利要求1至4任意一项所述的一种绳索载重并联机器人的避障方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311501018.5A CN117226810B (zh) | 2023-11-13 | 2023-11-13 | 一种绳索载重并联机器人及其避障方法、装置、存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311501018.5A CN117226810B (zh) | 2023-11-13 | 2023-11-13 | 一种绳索载重并联机器人及其避障方法、装置、存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117226810A CN117226810A (zh) | 2023-12-15 |
| CN117226810B true CN117226810B (zh) | 2024-02-02 |
Family
ID=89082953
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311501018.5A Active CN117226810B (zh) | 2023-11-13 | 2023-11-13 | 一种绳索载重并联机器人及其避障方法、装置、存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117226810B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113076809A (zh) * | 2021-03-10 | 2021-07-06 | 青岛海纳云科技控股有限公司 | 一种基于视觉Transformer的高空坠物检测方法 |
| CN113172605A (zh) * | 2021-04-28 | 2021-07-27 | 哈尔滨工业大学（深圳） | 基于绳驱并联机器人的避障方法、系统、设备及存储介质 |
| CN115457075A (zh) * | 2022-07-31 | 2022-12-09 | 浙江工业大学 | 一种基于sac-pid的移动机器人目标跟随方法 |
| CN116576857A (zh) * | 2023-04-19 | 2023-08-11 | 东北大学 | 一种基于单线激光雷达的多障碍物预测导航避障方法 |
| CN116740146A (zh) * | 2023-08-07 | 2023-09-12 | 华侨大学 | 无人驾驶挖掘机动态目标检测跟踪方法、装置及设备 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20230054759A1 (en) * | 2021-08-23 | 2023-02-23 | Nvidia Corporation | Object tracking using lidar data for autonomous machine applications |
-
2023
- 2023-11-13 CN CN202311501018.5A patent/CN117226810B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113076809A (zh) * | 2021-03-10 | 2021-07-06 | 青岛海纳云科技控股有限公司 | 一种基于视觉Transformer的高空坠物检测方法 |
| CN113172605A (zh) * | 2021-04-28 | 2021-07-27 | 哈尔滨工业大学（深圳） | 基于绳驱并联机器人的避障方法、系统、设备及存储介质 |
| CN115457075A (zh) * | 2022-07-31 | 2022-12-09 | 浙江工业大学 | 一种基于sac-pid的移动机器人目标跟随方法 |
| CN116576857A (zh) * | 2023-04-19 | 2023-08-11 | 东北大学 | 一种基于单线激光雷达的多障碍物预测导航避障方法 |
| CN116740146A (zh) * | 2023-08-07 | 2023-09-12 | 华侨大学 | 无人驾驶挖掘机动态目标检测跟踪方法、装置及设备 |
Non-Patent Citations (1)
| Title |
|---|
| 使用贪心原理进行模式匹配;张勇;王元珍;梁俊杰;;小型微型计算机系统(第04期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117226810A (zh) | 2023-12-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110253570B (zh) | 基于视觉的工业机械臂人机安全系统 | |
| CN111397598B (zh) | 人机共融环境中移动型机器人路径规划采样方法及系统 | |
| CN110737271B (zh) | 一种水面机器人自主巡航系统及方法 | |
| CN110009718B (zh) | 一种三维高精度地图生成方法及装置 | |
| CN110705385B (zh) | 一种障碍物角度的检测方法、装置、设备及介质 | |
| EP4211651A1 (en) | Efficient three-dimensional object detection from point clouds | |
| CN113238554A (zh) | 一种基于激光与视觉融合slam技术的室内导航方法及系统 | |
| Alves et al. | Localization and navigation of a mobile robot in an office-like environment | |
| Tavakoli et al. | Cooperative multi-agent mapping of three-dimensional structures for pipeline inspection applications | |
| CN114998276A (zh) | 一种基于三维点云的机器人动态障碍物实时检测方法 | |
| CN115509231A (zh) | 一种机器人跟随避障方法、装置及存储介质 | |
| CN114815851A (zh) | 机器人跟随方法、装置、电子设备以及存储介质 | |
| CN112711255B (zh) | 移动机器人避障方法、控制设备及存储介质 | |
| CN117226810B (zh) | 一种绳索载重并联机器人及其避障方法、装置、存储介质 | |
| CN113158779A (zh) | 一种行走方法、装置和计算机存储介质 | |
| CN114200920A (zh) | 路径规划方法、装置及控制系统 | |
| Miądlicki et al. | LiDAR based system for tracking loader crane operator | |
| CN115902977A (zh) | 基于视觉和gps的变电站机器人双重定位方法及系统 | |
| CN115893201A (zh) | 一种塔吊的自动驾驶方法、装置、设备及存储介质 | |
| KR101907611B1 (ko) | 자율 주행체의 위치 정보를 획득하는 방법 | |
| CN109901589B (zh) | 移动机器人控制方法和装置 | |
| Jahoda et al. | Autonomous car chasing | |
| Parikh et al. | Autonomous mobile robot for inventory management in retail industry | |
| Kebir et al. | Smart robot navigation using rgb-d camera | |
| Haque et al. | Vision based trajectory following robot and swarm |
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 |