CN117182891A - 一种机械臂主动建图方法及装置 - Google Patents
一种机械臂主动建图方法及装置 Download PDFInfo
- Publication number
- CN117182891A CN117182891A CN202310723230.XA CN202310723230A CN117182891A CN 117182891 A CN117182891 A CN 117182891A CN 202310723230 A CN202310723230 A CN 202310723230A CN 117182891 A CN117182891 A CN 117182891A
- Authority
- CN
- China
- Prior art keywords
- map
- mechanical arm
- boundary
- voxel
- information
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000000007 visual effect Effects 0.000 claims abstract description 42
- 238000005070 sampling Methods 0.000 claims abstract description 28
- 230000003287 optical effect Effects 0.000 claims abstract description 25
- 230000004438 eyesight Effects 0.000 claims description 45
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000001514 detection method Methods 0.000 claims description 12
- 238000012163 sequencing technique Methods 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 9
- 238000013507 mapping Methods 0.000 claims description 8
- 230000006870 function Effects 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 abstract description 2
- 238000010276 construction Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 8
- 230000007613 environmental effect Effects 0.000 description 6
- 230000008859 change Effects 0.000 description 4
- 238000006243 chemical reaction Methods 0.000 description 4
- 238000009826 distribution Methods 0.000 description 4
- 238000011156 evaluation Methods 0.000 description 4
- 238000004519 manufacturing process Methods 0.000 description 4
- 230000016776 visual perception Effects 0.000 description 4
- 238000012217 deletion Methods 0.000 description 3
- 230000037430 deletion Effects 0.000 description 3
- 230000014759 maintenance of location Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000000059 patterning Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Landscapes
- Image Processing (AREA)
Abstract
本申请提供一种机械臂主动建图方法及装置，属于图像处理领域。该方法包括：机械臂手眼标定后更新环境地图并进行边界体素检测，生成边界体素地图；将边界体素地图以最小分辨率融合到预设环境八叉树地图的根节点，生成三维融合地图；基于三维边界地图确定探索域，生成观测球；在观测球面上设置采样点，根据采样点位置与视觉传感器的光轴向量的旋转关系，获得机械臂末端视觉传感器观测位姿集；结合手眼标定与机械臂运动学模型求解，获得对应机械臂观测构型集；计算机械臂当前位姿到视点的观测位姿的无干涉路径，用于移动获取地图。本申请通过边界体素地图和环境八叉树地图结合，避免了路径规划中建模与规划单独解耦进行，可以有效避免潜在碰撞风险。
Description
技术领域
本申请涉及图像处理领域，尤其涉及一种机械臂主动建图方法，还涉及一种机械臂主动建图装置。
背景技术
目前，智能制造与自动化装备产业关注度日益增高，传统生产线形式也逐渐朝向柔性生产的方向发展。结合空间视觉系统，机械臂在人机协作、家居服务、医疗机器人、柔性生产等领域引起了广泛关注。这对于机械臂也提出了新的应用需求，即在未知复杂环境下，机械臂如何充分感知环境，规避潜在碰撞风险，安全高效完成指定任务。
现有技术中，针对未知复杂环境下的机械臂主动运动规划大都首先通过人为辅助建模或在周围固定单或多个视觉传感器实现环境感知，而后在环境完全已知的前提下进行机械臂运动规划，因此地图和运动规划需要单独解耦。
因此需要解决的问题是，地图和路径单独解耦的问题。
发明内容
本申请的目的在于克服现有技术中创建的地图与机械臂建图路径单独解耦导致碰撞的缺陷，提供一种机械臂主动建图方法，还涉及一种机械臂主动建图装置。
本申请提供一种机械臂主动建图方法，包括：
S1通过视觉传感器扫描并更新环境地图；
S2在所述环境地图中进行边界体素检测生成边界体素地图，将所述边界体素地图的边界体素信息存储至一个八叉树地图中，并将所述边界体素地图参考坐标系的最小分辨率与环境八叉树地图分辨率保持一致，生成三维边界地图。
可选地，将所述视觉传感器设置在机械臂末端进行连续的主动建图，包括：
在所述扫描并更新环境地图之前：
S0进行机械臂末端视觉传感器与执行器两者坐标系的手眼标定；
在所述生成三维边界地图之后：
S3结合机械臂任务场景进行空间划分，确定空间探索子区域规模；
S4基于所述三维边界地图、环境八叉树地图、区域历史探索信息，确定下一步待探索子区域；
S5根据所述探索子域与所述机械臂的位置关系和视觉传感器深度范围生成观测球；
S6在所述观测球面上均匀生成采样点，根据所述采样点空间位置与视觉传感器的光轴向量的旋转关系得到刚体变换矩阵，获得视点集；
S7结合手眼标定与机械臂运动学模型，进行基于可操作度优化的逆运动学求解，获得视点集对应的机械臂观测构型集，对所述构型集内元素进行排序；
S8利用RRT-Connect运动规划算法计算从机械臂当前构型到所述构型集中最优可行构型的无干涉路径，基于所述无干涉路劲移动所述机械臂并重复S1～到S8，形成环境地图。
可选地，所述生成边界体素地图，还包括：
S2.1识别所述环境地图中体素的占据状态相对于之前环境地图体素的占据状态发生变化的体素集；
S2.2根据在所述体素集中查询原边界体素在当前环境地图下的占据状态，对所述体素删除或保留，生成边界体素地图；
可选地，所述确定探索域，还包括：
S5.1通过多维空间点索引进行空间中探索域的信息查询，获取所述探索域信息；
S5.2在所述探索信息中引入地图信息熵，采用视觉传感器光轴向量与光心到探索域中心的向量夹角余弦作为运动代价，评价并选择探索域，包括：
单个体素x的信息熵计算公式如下：
其中，H(x)是单个体素x的信息熵，Po(x)为体素x的占据概率，ln表示自然对数，
可选地，还包括：利用斐波那契网格采样在所述观测球面上生成指定数量的采样点。
可选地，还包括：
得到视点集后，根据候选视点对应的信息增益，对应候选构型的可操作指标，机械臂当前构型与对应候选构型之间的关节空间欧式距离综合排序得到构型序列；
本申请还提供一种机械臂主动建图装置，包括：
标定模块，用于进行机械臂末端视觉传感器与执行器两者坐标系的手眼标定；获取模块，用于通过视觉传感器扫描并更新环境地图；
检测模块，用于在所述环境地图中进行边界体素检测，生成边界体素地图；
边界模块，用于根据在所述体素集中查询原边界体素在当前环境地图下的占据状态，对所述体素删除或保留，生成边界体素八叉树地图；
区域模块，用于基于所述三维边界地图，环境地图，历史探索信息，确定待探索子区域；
生成模块，用于根据所述探索域与所述机械臂的位置关系和视觉传感器深度范围生成观测球；
视点模块，用于在所述观测球面上均匀生成采样点，根据所述采样点空间位置与视觉传感器的光轴向量的旋转关系得到刚体变换矩阵，获得视点集。
构型模块，用于结合手眼标定与机械臂运动学模型，进行基于可操作度优化的逆运动学求解，获得视点集对应的机械臂观测构型集，对所述构型集内元素进行排序；
规划模块，用于利用RRT-Connect运动规划算法计算从机械臂当前构型到所述构型集中最优可行构型的无干涉路径，基于所述无干涉路劲移动所述机械臂并重复上述各个模块的功能，形成环境地图。
可选地，所述生成边界体素地图，包括：
所述检测模块，识别所述环境地图中体素的占据状态相对于之前环境地图体素的占据状态发生变化的体素集；
边界模块，用于根据在所述体素集中查询原边界体素在当前环境地图下的占据状态，对所述体素删除或保留，生成边界体素地图。
可选地，所述生成模块确定探索域，还包括：
在所述三维边界地图中，通过多维空间点索引进行空间中探索域的信息查询，获取所述探索域信息；
在所述探索信息中引入地图信息熵，采用视觉传感器光轴向量与光心到探索域中心的向量夹角余弦作为运动代价，评价并选择探索域，包括；
单个体素x的信息熵计算公式如下：
其中，H(x)是单个体素x的信息熵，Po(x)为体素x的占据概率，ln表示自然对数，
根据多个所述单个体素x的信息熵确定所述探索域的扫描时的误差，确定探索域。
可选地，所述视点模块还包括：
得到视点集后，根据视觉传感器当前位置与候选视点位置之间的欧式距离由近及远排序得到视点序列；
根据所述视点序列计算所述无干涉路径。
本申请的优点和有益效果：
本申请提供一种机械臂主动建图方法，包括：S1通过视觉传感器扫描并更新环境地图；S2在所述环境地图中进行边界体素检测，将边界体素信息存储至一个八叉树地图中，所述边界体素地图的参考坐标系的最小分辨率与环境八叉树地图分辨率保持一致，生成三维边界地图。本申请通过边界体素地图和环境八叉树地图结合，可以同时查询三维体素的占据状态与边界信息，避免路径规划过程地图和路径规划单独解耦。
附图说明
图1是本申请中机械臂路径主动规划方法流程示意图。
图2是本申请中机械臂路径主动规划逻辑示意图。
图3是本申请中机械臂工作区域示意图。
图4是本申请中覆盖式观测视点示意图。
图5是本申请中主动建图装置示意图。
具体实施方式
下面结合附图和具体实施例对本申请作进一步说明，以使本领域的技术人员可以更好地理解本申请并能予以实施。
以下内容均是为了详细说明本申请要保护的技术方案所提供的具体实施过程的示例，但是本申请还可以采用不同于此的描述的其他方式实施，本领域技术人员可以在本申请构思的指引下，采用不同的技术手段实现本申请，因此本申请不受下面具体实施例的限制。
本申请提供的一种机械臂主动建图方法，还涉及一种机械臂主动建图装置。图1和图5分别示意了本申请上方法和装置。
本申请提供一种机械臂主动建图方法，包括：S1通过视觉传感器扫描并更新环境地图；S2在所述环境地图中进行边界体素检测，将边界体素信息存储至一个八叉树地图中，所述边界体素地图的参考坐标系的最小分辨率与环境八叉树地图分辨率保持一致，生成三维边界地图。本申请通过边界体素地图和环境八叉树地图结合，可以同时查询三维体素的占据状态与边界信息，避免路径规划过程地图和路径规划单独解耦。
下面结合图1和图2对本申请中关于机械臂路径主动规划实施例进行进一步详细阐述。
S0进行机械臂末端视觉传感器与执行器两者坐标系的手眼标定。
配置机械臂视觉感知系统：例如视觉传感器。将机械臂固定于基座上，将所述视觉传感器安装在机械臂末端上，然后完成所述视觉传感器坐标系与机械臂末端执行器坐标系的手眼标定。
本申请所述的手眼标定，将末端的执行器绕机械臂基坐标系的x轴、y轴及z轴旋转，基于旋转矩阵计算对应的位姿矩阵；基于位姿矩阵计算从视觉传感器坐标系转换到机械臂基坐标系的变换矩阵。机械臂运动是基于机器人基坐标系的，而所述视觉传感器获取数据的位姿信息是相对于视觉传感器坐标系的，为了保证所述机械臂能准确无误地移动到激光线扫描视觉传感器识别到的工件位姿，求出所述视觉传感器坐标系与机器人坐标系之间的转换关系，即从视觉传感器坐标系到机器人基坐标系的转换矩阵，完成手眼标定。
所述的视觉感知系统，还可以是其他可以获取图形数据的视觉传感器，例如小型雷达，激光扫描仪等。
S1通过视觉传感器扫描并更新环境地图。
通过配置机械臂视觉感知系统，将机械臂固定于基座上，将视觉传感器安装在机械臂末端上。基于所述视觉传感器扫描获取环境地图数据，用于对之前的地图数据进行更新。
S2在所述环境地图中进行边界体素检测生成边界体素地图，将所述边界体素地图的边界体素信息存储至一个八叉树地图中，并将所述边界体素地图参考坐标系的最小分辨率与环境八叉树地图分辨率保持一致，生成三维边界地图。
S2.1识别所述环境地图中体素的占据状态相对于之前环境地图体素的占据状态发生变化的体素集。
本申请中，地图边界体素定义为与未知体素相邻的自由体素。在每次环境地图更新后，不对整张地图都进行边界体素检测，而是在占据状态相对于上一帧发生变化的体素集中进行。
S2.2根据在所述体素集中查询与原边界体素相毗邻的体素在当前环境地图下的占据状态，对所述体素删除或保留，结合新检测的边界体素，生成边界体素集。
所述边界体素地图依据当前环境表示和环境地图的最新变化进行更新，每个节点内存储的数据表示其作为地图边界的概率。
如此，可以同时查询工作区域中任意子区域内离散三维体素的占据状态与边界信息。
而后，还可以结合多维空间点索引，创建探索信息系统，实时更新环境地图边界分布与空间占据状态，记录历史探索区域与观测视点信息，并提供高效的空间子区域探索信息查询。
上述通过边界体素地图和环境八叉树地图结合，可以同时查询空间子区域内三维体素的占据状态与边界信息，为下一步探索区域选择提供依据，下面对机械臂路径主动建图实施例进行进一步详细阐述。
如图1所示，本申请提供的一种机械臂主动建图方法，避免了未知环境下环境建模与规划单独解耦进行，有效避免潜在碰撞风险。
S3结合机械臂任务场景进行空间划分，确定空间探索子区域规模。
S4基于所述三维边界地图、环境八叉树地图、区域历史探索信息，确定下一步待探索子区域。
本申请所述的工作区域划分，是将场景根据实际需要进行划分，例如结合场景需求与机械臂作业范围划定机械臂工作区域。
如图4所示，由于机械臂末端视觉传感器无法观测到其自身关节与环境干涉信息，因此在探索开始前给机械臂设定一个初始无障碍区域301，而其余空间则是完全未知的，即待探索区域302。
第一步是对环境地图进行边界体素检测。
本申请中，地图边界体素定义为：与未知体素相邻的自由体素。
本申请中，为提高计算效率，在每次环境地图更新后，并不对整张地图都进行边界检测，而是在占据状态相对于上一帧发生变化的体素集中进行，并通过查询原边界体素在当前环境地图下的占据状态决定其删除或保留操作。
详细过程如下：
本申请采用多分辨率的候选探索域生成方法，在对环境地图边界进行提取后，采用空间递归划分思想，基于八叉树结构创建三维边界地图。
具体的，不同于现有的环境八叉树地图，以视觉传感器已知位姿下对应的观测数据作为输入进行融合与更新。本申请将边界体素地图依据当前环境表示和环境地图的最新变化进行更新，每个节点内存储的数据表示其作为地图边界的概率。
通过将边界体素地图和环境八叉树地图的根节点以及最小分辨率保持一致，生成新的边界体素地图。以此，可以同时查询空间中任意离散三维体素的占据状态与边界信息。
最后，结合多维空间点索引，创建探索信息系统，实时更新环境地图边界分布与空间占据状态，记录历史探索区域与观测视点信息。有效的提高空间子区域探索信息查询速度。
S5根据所述探索子域与所述机械臂的位置关系和视觉传感器深度范围生成观测球。
具体的，以探索域中心为球心，结合视觉传感器深度范围以及球心到机械臂基坐标系z轴距离函数，确定半径，得到该探索域对应的观测球。
在对候选的探索域进行价值评估时，除边界密度外，本申请还加入观测过程中的：视觉传感器误差、环境噪声和/或移动物体干扰等，对建图准确性造成的影响的参数信息，例如：
S5.1通过多维空间点索引进行空间中探索域的信息查询，获取所述探索域信息；
S5.2在所述探索信息中引入地图信息熵，采用视觉传感器光轴向量与光心到探索域中心的向量夹角余弦作为运动代价，评价并选择探索域，包括：
单个体素x的信息熵计算公式如下：
单个体素x的信息熵计算公式如下：
其中，H(x)是单个体素x的信息熵，Po(x)为体素x的占据概率，ln表示自然对数，
S6在所述观测球面上均匀生成采样点，根据所述采样点空间位置与视觉传感器的光轴向量的旋转关系得到刚体变换矩阵，获得视点集。
如图5所示，利用斐波那契网格采样在观测球面303上生成指定数量的采样点304，采样点304的空间坐标作为候选视点位置，采样点304指向球心的向量作为视觉传感器在该位置下的光轴向量，计算视觉传感器当前光轴向量与采样位置下光轴向量之间的旋转向量，并根据罗德里格斯旋转公式得到旋转矩阵。
最后，结合坐标变换计算出相应的视觉传感器观测位姿，并对其进行有效性检查(存在无碰撞风险的运动学逆解)，生成候选视点集。
S7结合手眼标定与机械臂运动学模型，进行基于可操作度优化的逆运动学求解，获得视点集对应的机械臂观测构型集，对所述构型集内元素进行排序。
具体的，得到视点集后，根据候选视点对应的信息增益，对应候选构型的可操作指标，机械臂当前构型与对应候选构型之间的关节空间欧式距离综合排序得到构型序列。
S8利用RRT-Connect运动规划算法计算从机械臂当前构型到所述构型集中最优可行构型的无干涉路径，基于所述无干涉路劲移动所述机械臂并重复S1到S8，形成环境地图。
得到候选视点集后，根据视觉传感器当前位置与候选视点位置之间的欧式距离由近及远排序得到最佳视点序列，若检测到该探索域在探索过程中已被观测过，那么在候选视点的评估中还会综合考虑视点位置与此前观测位置之间的欧式距离，以此实现对历史探索域的多方位观测，基于所述无干涉路劲移动所述机械臂并重复S1到S8，形成环境地图。
本申请中利用RRT-Connect运动规划算法计算出一条从机械臂当前位姿至最佳观测位姿的无干涉路径。
本申请中还提供了一种机械臂主动建图装置，该主动建图装置中，包括一种环境地图创建装置，用于创建地图，以实现路径规划。
下面结合图5对本申请中关于机械臂路径主动规划实施例进行进一步详细阐述。
标定模块501，用于进行机械臂末端视觉传感器与执行器两者坐标系的手眼标定。
配置机械臂视觉感知系统：例如视觉传感器。将机械臂固定于基座上，将所述视觉传感器安装在机械臂末端上，然后完成所述视觉传感器坐标系与机械臂末端执行器坐标系的手眼标定。
本申请所述的手眼标定，将末端的执行器绕机械臂基坐标系的x轴、y轴及z轴旋转，基于旋转矩阵计算对应的位姿矩阵；基于位姿矩阵计算从视觉传感器坐标系转换到机械臂基坐标系的变换矩阵。机械臂运动是基于机器人基坐标系的，而所述视觉传感器获取数据的位姿信息是相对于视觉传感器坐标系的，为了保证所述机械臂能准确无误地移动到激光线扫描视觉传感器识别到的工件位姿，求出所述视觉传感器坐标系与机器人坐标系之间的转换关系，即从视觉传感器坐标系到机器人基坐标系的转换矩阵，完成手眼标定。
所述的视觉感知系统，还可以是其他可以获取图形数据的视觉传感器，例如小型雷达，激光扫描仪等。
获取模块502，用于通过视觉传感器扫描并更新环境地图。
通过配置机械臂视觉感知系统，将机械臂固定于基座上，将视觉传感器安装在机械臂末端上。基于所述视觉传感器扫描获取环境地图数据，用于对之前的地图数据进行更新。
检测模块503，用于在所述环境地图中进行边界体素检测，生成边界体素地图，包括：识别所述环境地图中体素的占据状态相对于之前环境地图体素的占据状态发生变化的体素集。
识别所述环境地图中体素的占据状态相对于之前环境地图体素的占据状态发生变化的体素集。
本申请中，地图边界体素定义为与未知体素相邻的自由体素。在每次环境地图更新后，不对整张地图都进行边界体素检测，而是在占据状态相对于上一帧发生变化的体素集中进行。
边界模块504，用于根据在所述体素集中查询原边界体素在当前环境地图下的占据状态，对所述体素删除或保留，生成边界体素八叉树地图。
具体的，可通过查询原边界体素在当前环境地图下的占据状态决定其删除或保留操作。例如，当所述占据状态发生变化，且当前环境地图下的占据状态为实则保留，否则删除更新的地图，或体素。
将所述边界体素地图最小分辨率融合到预设的环境八叉树地图的根节点，生成三维边界地图。
所述边界体素地图依据当前环境表示和环境地图的最新变化进行更新，每个节点内存储的数据表示其作为地图边界的概率。
通过将边界体素地图和环境八叉树地图的根节点以最小分辨率保持一致，生成新的三维边界地图。
如此，可以同时查询空间中任意离散三维体素的占据状态与边界信息。
而后，还可以结合多维空间点索引，创建探索信息系统，实时更新环境地图边界分布与空间占据状态，记录历史探索区域与观测视点信息，并提供高效的空间子区域探索信息查询。
区域模块505，用于基于所述三维边界地图，环境地图，历史探索信息，确定待探索子区域；
本申请所述的工作区域划分，是将场景根据实际需要进行划分，例如结合场景需求与机械臂作业范围划定机械臂工作区域。
如图4所示，由于机械臂末端视觉传感器无法观测到其自身关节与环境干涉信息，因此在探索开始前给机械臂设定一个初始无障碍区域301，而其余空间则是完全未知的，即待探索区域302。
第一步是对环境地图进行边界体素检测。
本申请中，地图边界体素定义为：与未知体素相邻的自由体素。
本申请中，为提高计算效率，在每次环境地图更新后，并不对整张地图都进行边界检测，而是在占据状态相对于上一帧发生变化的体素集中进行，并通过查询与原边界体素毗邻的体素在当前环境地图下的占据状态决定其删除或保留操作。
详细过程如下：
本申请采用多分辨率的候选探索域生成方法，在对环境地图边界进行提取后，采用空间递归划分思想，基于八叉树结构创建三维边界地图。
具体的，不同于现有的环境八叉树地图，以视觉传感器已知位姿下对应的观测数据作为输入进行融合与更新。本申请将边界体素地图依据当前环境表示和环境地图的最新变化进行更新，每个节点内存储的数据表示其作为地图边界的概率。
将边界体素地图和环境八叉树地图的根节点以及最小分辨率保持一致，以此，可以同时查询空间中任意离散三维体素的占据状态与边界信息。
最后，结合多维空间点索引，创建探索信息系统，实时更新环境地图边界分布与空间占据状态，记录历史探索区域与观测视点信息。有效的提高空间子区域探索信息查询速度。
生成模块506，用于将所述待探索区域中所述体素集所在区域确定为探索域，根据所述探索域与所述机械臂的位置关系和视觉传感器深度范围生成观测球。
具体的，以探索域中心为球心，结合视觉传感器深度范围以及球心到机械臂基坐标系z轴距离函数，确定半径，得到该探索域对应的观测球。
在对候选的探索域进行价值评估时，除边界密度外，本申请还加入观测过程中的：视觉传感器误差、环境噪声和/或移动物体干扰等，对建图准确性造成的影响的参数信息，例如：
S5.1通过多维空间点索引进行空间中探索域的信息查询，获取所述探索域信息；
S5.2在所述探索信息中引入地图信息熵，采用视觉传感器光轴向量与光心到探索域中心的向量夹角余弦作为运动代价，评价并选择探索域，包括：
单个体素x的信息熵计算公式如下：
其中，H(x)是单个体素x的信息熵，Po(x)为体素x的占据概率，ln表示自然对数，
S6.3根据多个所述单个体素x的信息熵确定所述探索域的扫描时的误差，确定探索域。
视点模块507，用于在所述观测球上设置多个指向观测球心的采样点，根据所述采样点与视觉传感器的光轴向量的旋转关系生成旋转矩阵，结合所述手眼标定获得视点集。
得到视点集后，根据候选视点对应的信息增益，对应候选构型的可操作指标，机械臂当前构型与对应候选构型之间的关节空间欧式距离综合排序得到构型序列。
如图5所示，利用斐波那契网格采样在观测球面303上生成指定数量的采样点304，采样点304的空间坐标作为候选视点位置，采样点304指向球心的向量作为视觉传感器在该位置下的光轴向量，计算视觉传感器当前光轴向量与采样位置下光轴向量之间的旋转向量，并根据罗德里格斯旋转公式得到旋转矩阵。
最后，结合坐标变换计算出相应的视觉传感器观测位姿，并对其进行有效性检查(存在无碰撞风险的运动学逆解)，生成候选视点集。
构型模块508，用于结合手眼标定与机械臂运动学模型，进行基于可操作度优化的逆运动学求解，获得视点集对应的机械臂观测构型集，对所述构型集内元素进行排序；
规划模块509，用于利用RRT-Connect运动规划算法计算从机械臂当前构型到所述构型集中最优可行构型的无干涉路径，基于所述无干涉路劲移动所述机械臂并重复上述各个模块的功能，形成环境地图。
得到候选视点集后，根据视觉传感器当前位置与候选视点位置之间的欧式距离由近及远排序得到最佳视点序列，若检测到该探索域在探索过程中已被观测过，那么在候选视点的评估中还会综合考虑视点位置与此前观测位置之间的欧式距离，以此实现对历史探索域的多方位观测，基于所述无干涉路劲移动所述机械臂并重复上述各个模块的功能，形成环境地图。
本申请中利用RRT-Connect运动规划算法计算出一条从机械臂当前位姿至最佳观测位姿的无干涉路径。
最后应说明的是：以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。
Claims (10)
1.一种机械臂主动建图方法，其特征在于，包括：
S1通过视觉传感器扫描并更新环境地图；
S2在所述环境地图中进行边界体素检测生成边界体素地图，将所述边界体素地图的边界体素信息存储至一个八叉树地图中，并将所述边界体素地图参考坐标系的最小分辨率与环境八叉树地图分辨率保持一致，生成三维边界地图。
2.根据权利要求1所述机械臂主动建图方法，其特征在于，将所述视觉传感器设置在机械臂末端进行连续的主动建图，包括：
在所述扫描并更新环境地图之前：
S0进行机械臂末端视觉传感器与执行器两者坐标系的手眼标定；
在所述生成三维边界地图之后：
S3结合机械臂任务场景进行空间划分，确定空间探索子区域规模；
S4基于所述三维边界地图、环境八叉树地图、区域历史探索信息，确定下一步待探索子区域；
S5根据所述探索子域与所述机械臂的位置关系和视觉传感器深度范围生成观测球；
S6在所述观测球面上均匀生成采样点，根据所述采样点空间位置与视觉传感器的光轴向量的旋转关系得到刚体变换矩阵，获得视点集；
S7结合手眼标定与机械臂运动学模型，进行基于可操作度优化的逆运动学求解，获得视点集对应的机械臂观测构型集，对所述构型集内元素进行排序；
S8利用RRT-Connect运动规划算法计算从机械臂当前构型到所述构型集中最优可行构型的无干涉路径，基于所述无干涉路劲移动所述机械臂并重复S1～到S8，形成环境地图。
3.根据权利要求1所述机械臂主动建图方法，其特征在于，所述生成边界体素地图，还包括：
S2.1识别所述环境地图中体素的占据状态相对于之前环境地图体素的占据状态发生变化的体素集；
S2.2根据在所述体素集中查询原边界体素在当前环境地图下的占据状态，对所述体素删除或保留，生成边界体素地图。
4.根据权利要求2所述机械臂主动建图方法，其特征在于，所述确定探索域，还包括：
S5.1通过多维空间点索引进行空间中探索域的信息查询，获取所述探索域信息；
S5.2在所述探索信息中引入地图信息熵，采用视觉传感器光轴向量与光心到探索域中心的向量夹角余弦作为运动代价，评价并选择探索域，包括：
单个体素x的信息熵计算公式如下：
其中，H(x)是单个体素x的信息熵，Po(x)为体素x的占据概率，ln表示自然对数，
5.根据权利要求2所述主动建图方法，其特征在于，还包括：利用斐波那契网格采样在所述观测球面上生成指定数量的采样点。
6.根据权利要求2所述机械臂主动建图方法，其特征在于，还包括：
得到视点集后，根据候选视点对应的信息增益，对应候选构型的可操作指标，机械臂当前构型与对应候选构型之间的关节空间欧式距离综合排序得到构型序列。
7.一种机械臂主动建图装置，其特征在于，包括：
标定模块，用于进行机械臂末端视觉传感器与执行器两者坐标系的手眼标定；
获取模块，用于通过视觉传感器扫描并更新环境地图；
检测模块，用于在所述环境地图中进行边界体素检测，生成边界体素地图；
边界模块，用于根据在所述体素集中查询原边界体素在当前环境地图下的占据状态，对所述体素删除或保留，生成边界体素八叉树地图；
区域模块，用于基于所述三维边界地图，环境地图，历史探索信息，确定待探索子区域；
生成模块，用于根据所述探索域与所述机械臂的位置关系和视觉传感器深度范围生成观测球；
视点模块，用于在所述观测球面上均匀生成采样点，根据所述采样点空间位置与视觉传感器的光轴向量的旋转关系得到刚体变换矩阵，获得视点集。
构型模块，用于结合手眼标定与机械臂运动学模型，进行基于可操作度优化的逆运动学求解，获得视点集对应的机械臂观测构型集，对所述构型集内元素进行排序；
规划模块，用于利用RRT-Connect运动规划算法计算从机械臂当前构型到所述构型集中最优可行构型的无干涉路径，基于所述无干涉路劲移动所述机械臂并重复上述各个模块的功能，形成环境地图。
8.根据权利要求7所述的机械臂主动建图装置，其特征在于，所述生成边界体素地图，包括：
所述检测模块，识别所述环境地图中体素的占据状态相对于之前环境地图体素的占据状态发生变化的体素集；
边界模块，用于根据在所述体素集中查询原边界体素在当前环境地图下的占据状态，对所述体素删除或保留，生成边界体素地图。
9.根据权利要求7所述机械臂主动建图装置，其特征在于，所述生成模块确定探索域，还包括：
在所述三维边界地图中，通过多维空间点索引进行空间中探索域的信息查询，获取所述探索域信息；
在所述探索信息中引入地图信息熵，采用视觉传感器光轴向量与光心到探索域中心的向量夹角余弦作为运动代价，评价并选择探索域，包括；
单个体素x的信息熵计算公式如下：
其中，H(x)是单个体素x的信息熵，Po(x)为体素x的占据概率，ln表示自然对数，
根据多个所述单个体素x的信息熵确定所述探索域的扫描时的误差，确定探索域。
10.根据权利要求7所述机械臂主动建图装置，其特征在于，所述视点模块还包括：
得到视点集后，根据视觉传感器当前位置与候选视点位置之间的欧式距离由近及远排序得到视点序列；
根据所述视点序列计算所述无干涉路径。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310723230.XA CN117182891A (zh) | 2023-06-16 | 2023-06-16 | 一种机械臂主动建图方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310723230.XA CN117182891A (zh) | 2023-06-16 | 2023-06-16 | 一种机械臂主动建图方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117182891A true CN117182891A (zh) | 2023-12-08 |
Family
ID=89000491
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310723230.XA Pending CN117182891A (zh) | 2023-06-16 | 2023-06-16 | 一种机械臂主动建图方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117182891A (zh) |
-
2023
- 2023-06-16 CN CN202310723230.XA patent/CN117182891A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5452442B2 (ja) | ロボットシステム及び地図更新方法 | |
| CN110000786B (zh) | 一种基于视觉机器人的历史地图利用方法 | |
| JP5018458B2 (ja) | 座標補正方法、座標補正プログラム、及び自律移動ロボット | |
| CN106767827B (zh) | 一种基于激光数据的移动机器人点云地图创建方法 | |
| KR101925862B1 (ko) | 3차원 라이다를 이용한 실시간 3차원 지도 생성 방법 및 장치 | |
| CN110866927B (zh) | 一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法 | |
| CN114004869A (zh) | 一种基于3d点云配准的定位方法 | |
| JP5728564B2 (ja) | ロボットシステム及び地図更新方法 | |
| CN115299245B (zh) | 一种智能水果采摘机器人的控制方法及控制系统 | |
| CN114119920A (zh) | 三维点云地图构建方法、系统 | |
| CN113778096B (zh) | 室内机器人的定位与模型构建方法及系统 | |
| CN116630403A (zh) | 一种用于割草机器人的轻量级语义地图构建方法及系统 | |
| CN108152829B (zh) | 一种加装直线导轨的二维激光雷达建图装置及其建图方法 | |
| CN112859110A (zh) | 一种基于三维激光雷达的定位导航方法 | |
| CN117182891A (zh) | 一种机械臂主动建图方法及装置 | |
| CN116659500A (zh) | 基于激光雷达扫描信息的移动机器人定位方法及系统 | |
| CN114459483B (zh) | 基于机器人导航用地标导航地图构建与应用方法、系统 | |
| JP5953393B2 (ja) | ロボットシステム及び地図更新方法 | |
| CN114565726A (zh) | 一种非结构化动态环境下的同时定位与建图方法 | |
| CN107632606A (zh) | 大区域基于Slam和Tag标签的移动机器人导航与定位方法 | |
| CN117058358B (zh) | 一种场景边界检测方法和移动平台 | |
| Yang et al. | An automated view planning method of robot 3D measurement | |
| CN116124125A (zh) | 基于激光的二维码地图自动创建方法 | |
| CN118049985A (zh) | 基于激光点云的关键帧判定方法及地图更新方法 | |
| CN116698014A (zh) | 一种基于多机器人激光slam和视觉slam地图融合与拼接方法 |
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 |