CN109807933A - 一种能力图点云更新方法、装置、设备及存储介质 - Google Patents
一种能力图点云更新方法、装置、设备及存储介质 Download PDFInfo
- Publication number
- CN109807933A CN109807933A CN201910085829.9A CN201910085829A CN109807933A CN 109807933 A CN109807933 A CN 109807933A CN 201910085829 A CN201910085829 A CN 201910085829A CN 109807933 A CN109807933 A CN 109807933A
- Authority
- CN
- China
- Prior art keywords
- cloud
- robot
- hard
- change vector
- 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.)
- Granted
Links
Landscapes
- Manipulator (AREA)
Abstract
本发明公开了一种能力图点云更新方法、装置、设备及存储介质。该方法包括：初始时刻，获取机器人的静态能力图点云，所述静态能力图点云包括机器人的机械臂各关节运动范围中采样点的位置信息；获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部；根据所述相机位置的变化向量确定所述机器人的位姿变化向量；基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云。通过上述技术方案，实现了动态更新能力图，提高描述工作空间的实时性和准确性。
Description
技术领域
本发明实施例涉及机器视觉技术领域，尤其涉及一种能力图点云更新方法、装置、设备及存储介质。
背景技术
机器人与周围环境交互主要是通过末端效应器，即各自运动连杆的末端，例如手部、脚部或头部等。能力图用于描述机器人的工作空间，是其手部、脚部或头部能够到达的环境中位姿的点集，可以帮助机器人获取目标物体是否可达的信息，得到了广泛的应用。通常，在机器人进行抓取任务前，为其双臂运动链预计算一个能力图，其形状大致为以肩部为圆心的球体，并且以网格单元的形式表示，当确定目标物体的抓取位姿以后，机器人可根据网格单元规划出末端效应器的抓取路径。
现有的能力图都是通过在机器人执行任务前预先计算位姿信息而生成的静态能力图，理想条件下，机器人、目标物体和静态能力图存在唯一的映射关系。但静态能力图是在预处理阶段计算得到的常量，适用于以机器人作为参考坐标系的局部空间中抓取任务的描述，当机器人在动态环境中抓取时，涉及到在室内环境中连续运动，生成的静态能力图点云在发生位移变化时不再适用，无法实时地描述工作空间。
发明内容
本发明提供了一种能力图点云更新方法、装置、设备及存储介质，以通过动态更新能力图，提高描述工作空间的实时性和准确性。
第一方面，本发明实施例提供了一种能力图点云更新方法，包括：
初始时刻，获取机器人的静态能力图点云，所述静态能力图点云包括机器人的机械臂各关节运动范围中采样点的位置信息；
获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部；
根据所述相机位置的变化向量确定所述机器人的位姿变化向量；
基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云。
进一步的，所述初始时刻，获取机器人的静态能力图点云，包括：
对初始时刻机械臂各关节电机的运动范围进行采样；
利用六轴机械臂正解方程求解采样空间，得到机器人初始时刻的静态能力图点云，其中，所述采样空间由采样结果的笛卡尔积表示。
进一步的，所述根据所述相机位置的变化向量确定所述机器人的位姿变化向量，包括：
确定所述相机与末端效应器之间的坐标变换关系，所述末端效应器包括安装在机器人头部的效应器和安装在机器人躯干的效应器；
基于所述坐标变换关系将所述相机位置的变化向量转换为所述机器人的位姿变化向量。
进一步的，所述确定所述相机与末端效应器之间的坐标变换关系，包括：
根据机器人运动学方程求解相机坐标系到机器人头部的局部坐标系的第一变换矩阵；
根据机器人运动学链式法则和所述第一变换矩阵确定第二变换矩阵，所述第二变化矩阵为从相机坐标系到机器人躯干的参考坐标系的变换矩阵。
进一步的，所述基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云，包括：
将所述静态能力图点云中的各空间点按照所述位姿变化向量移动，移动后的各空间点构成当前时刻的能力图点云。
进一步的，所述方法还包括：
基于导航三维点云地图Octomap构建机器人周围空间的三维点云地图；
将当前时刻的能力图点云插入所述三维点云地图；
对所述当前时刻的能力图点云中的各空间点进行迭代查询，判断各空间点在所述三维网格地图中是否为占有状态；
若是，则删除所述空间点，否则保留所述空间点，保留的空间点构成当前时刻的能力图点云。
进一步的，机械臂肩关节电机的运动范围包括俯仰和翻滚两个自由度，肘关节电机的运动范围包括偏航和翻滚两个自由度；
所述采样结果的笛卡尔积S表示为：S＝Δqsp·Δqsr·Δqey·Δqer，其中，Δqsp表示肩关节电机绕x轴方向俯仰的采样结果，Δqsr表示肩关节电机绕z轴方向翻滚的采样结果，Δqey表示肘关节电机绕y轴方向偏航的采样结果，Δqer表示肘关节电机绕z轴方向翻滚的采样结果。
第二方面，本发明实施例提供了一种能力图点云更新装置，包括：
第一获取模块，用于初始时刻，获取机器人的静态能力图点云，所述静态能力图点云包括机器人的机械臂各关节运动范围中采样点的位置信息；
第二获取模块，用于获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部；
位姿变化确定模块，用于根据所述相机位置的变化向量确定所述机器人的位姿变化向量；
更新模块，用于基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云。
第三方面，本发明实施例提供了一种设备，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的能力图点云更新方法。
第四方面，本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如第一方面所述的能力图点云更新方法。
本发明实施例提供了一种能力图点云更新方法、装置、设备及存储介质，该方法包括：初始时刻，获取机器人的静态能力图点云，所述静态能力图点云包括机器人的机械臂各关节运动范围中采样点的位置信息；获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部；根据所述相机位置的变化向量确定所述机器人的位姿变化向量；基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云。通过上述技术方案，实现了动态更新能力图，提高描述工作空间的实时性和准确性。
附图说明
图1为本发明实施例一提供的一种能力图点云更新方法的流程图；
图2为本发明实施例一中的能力图点云应用场景的示意图；
图3为本发明实施例二提供的一种能力图点云更新方法的流程图；
图4为本发明实施例三提供的一种能力图点云更新方法的流程图；
图5为本发明实施例三中的基于Octomap构建三维环境地图的示意图；
图6A为本发明实施例三中的机器人在空闲区域的能力图的示意图；
图6B为本发明实施例三中的机器人在障碍区域的能力图的示意图；
图7为本发明实施例四提供的一种能力图点云更新装置的结构示意图；
图8为本发明实施例五提供的一种设备的硬件结构示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。
实施例一
图1为本发明实施例一提供的一种能力图点云更新方法的流程图，本实施例可适用于在机器人移动过程中描述实时的工作空间的情况。具体的，该能力图点云更新方法可以由能力图点云更新装置执行，该能力图点云更新装置可以通过软件和/或硬件的方式实现，并集成在设备中。进一步的，所述设备包括但不限定于：台式计算机、工控机、上位机等电子设备。
图2为本发明实施例一中的能力图点云应用场景的示意图。如图2所示，根据仿人机器人的连杆和关节结构，对机器人的几何模型进行抽象，结合运动学方程，明确机器人位姿与各关节变量的关系，最终确定机器人的手部的可达区域(大致如图2中的椭圆区域所示)。可达区域大致为以肩部为圆心的球体或椭球体，对各关节运动范围或机器人的各种位姿进行采样，得到手部可到达的空间位置的点的集合，即为能力图点云。当机器人静止时，得到的是静态能力图点云，当机器人发生移动或位姿变化，则需要更新能力图点云。需要说明的是，本实施例中的机器人为仿人机器人，具有仿人的头部、躯干和四肢。
参考图1，该能力图点云更新方法具体包括如下步骤：
S110、初始时刻，获取机器人的静态能力图点云，所述静态能力图点云包括机器人的机械臂各关节运动范围中采样点的位置信息。
具体的，初始时刻，机器人处于初始位姿，对其进行运动学建模，首先从机器人的连杆和关节结构中，对机器人的几何模型进行抽象，构建起表示连杆和关节在空间中相对位置关系的数据结构，并以此求解出机器人的运动学方程(反映机器人位姿与关节变量之间的关系)。根据机器人的连杆和关节结构，以及运动学方程可获得机器人机械臂各关节所能达到的运动范围，对各关节的运动范围进行采样，对采样到的每一种关节状态(关节在各局部坐标系中的旋转角度)，利用六轴机械臂正解方程FK方程计算出当前关节状态下机器人手部能到达的空间位置，这些空间位置的点即构成初始时刻的静态能力图点云，作为后续能力图点云的更新的基准。可选的，能力图点云的形式较为密集，将其降维后，可以Octomap网格的形式表示，节省存储空间，并且便于实际应用中根据机器人位姿规划手部的移动路径。
S120、获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部。
具体的，在机器人移动过程中，通过加速度计、陀螺仪等可测量相机在x轴、y轴、z轴方向上移动的距离和绕x轴、y轴、z轴旋转的角度等，从而获取相机的空间位置的变化向量，并据此确定机器人的位姿变化向量。
S130、根据所述相机位置的变化向量确定所述机器人的位姿变化向量。
具体的，相机处于一个局部坐标系(称为相机坐标系)中，相机位置的变化向量也是基于相机坐标系表示的，而机器人头部、手部运动链中的各个关节也分别作为各局部坐标系的原点，各关节可独立运动，相机位置的变化向量通过一定形式的变化(齐次变换)，可转换为各个关节在各自的局部坐标系下的坐标或向量。例如，已知相机坐标系和头部关节局部坐标系之间的变换矩阵，以及头部关节局部坐标系和躯干参考坐标系之间的变换矩阵，则基于此链式法则，可得到相机坐标系到躯干参考坐标系之间的变换关系，从而根据相机位置的变化向量确定机器人的位姿变化向量。需要说明的是，相机空间位置的变化向量是基于相机坐标系测量和计算的，而机器人的位姿变化向量是基于躯干参考坐标系计算的。
S140、基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云。
具体的，根据位姿变化向量可确定机器人的移动方向、距离、转动角度等，在此基础上，可重新确定当前时刻的能力图点云。对应的，初始时刻的静态能力图也发生了变化，其移动方向、距离、转动角度与机器人的移动方向、距离和转动角度一致。例如，机器人(躯干)向前移动了1m，但其连杆和关节结构、运动学方程都是一定的，则手部的可达区域相对于机器人本身的空间关系也是一定的，对应的静态能力图点云中点的空间位置也都向前移动1m。
需要说明的是，对于任意的当前时刻Tt，都可根据从初始时刻T0到当前时刻Tt的相机位置的变化向量确定位姿变化向量，并更新能力图点云；也可将前一时刻Tt-1作为初始时刻，根据从前一时刻Tt-1到当前时刻Tt的相机位置的变化向量确定位姿变化向量，并将前一时刻Tt-1对应的能力图点云作为初始时刻的静态能力图点云，在此基础上再更新得到当前时刻Tt的能力图点云。离散能力图点云的形式考虑了机器人各关节电机的物理属性和机器人运动学特性，可避免末端效应器在执行任务过程中自碰撞。
本发明实施例一提供的一种能力图点云更新方法、装置、设备及存储介质，通过获取初始时刻机器人的静态能力图点云，以及从初始时刻到当前时刻相机位置的变化向量，根据所述相机位置的变化向量确定所述机器人的位姿变化向量；基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云，实现了动态更新能力图，提高描述工作空间的实时性和准确性。
实施例二
图3为本发明实施例二提供的一种能力图点云更新方法的流程图，本实施例是在上述实施例的基础上进行优化，对相机与末端效应器间的坐标变换关系以及位姿变化向量的确定进行具体描述，需要说明的是，未在本实施例中详尽描述的技术细节可参见上述任意实施例。
具体的，参考图3，该方法具体包括如下步骤：
S210、对初始时刻机械臂各关节电机的运动范围进行采样。
具体的，根据机器人的连杆和关节结构，以及运动学方程确定机器人机械臂各关节电机的运动范围，并在所有运动范围中进行采样，得到的各种关节状态组成采样空间。
进一步的，所述运动学方程表示机器人相邻的运动坐标系i、运动坐标系j之间的相对位置关系，其形式为：pj＝pi+Ri·bj，其中，pi、pj分别表示第i和j个关节连杆在空间中的位置，Ri,Rj分别表示第i和j个关节连杆在空间中的旋转，bj是连杆之间的相对位置矢量，是在机器人几何模型和关节结构确认之后的常量，是罗德里格旋转公式，
S220、利用六轴机械臂正解方程求解采样空间，得到机器人初始时刻的静态能力图点云，其中，所述采样空间由采样结果的笛卡尔积表示。
具体的，利用六轴机械臂正解方程FK方程计算出初始时刻各种关节状态下机器人手部能够到达的空间位置，空间位置的点集即构成初始时刻的静态能力图点云C，即C＝FK(S)，其中，S为机械臂的采样空间中各关节向量组成的集合；
FK方程用于根据采样空间各样本中各个关节的转动角度，求出末端(手部)的位姿，从而确定手部到达的空间位置。笛卡尔积是指两个集合M和N的笛卡尔积(Cartesianproduct)，也称直积，集合M是指机械臂的肩关节电机的运动范围的采样样本，包括俯仰和翻滚两个自由度，集合N是指机械臂的肘关节电机的运动范围的采样样本，包括偏航和翻滚两个自由度，采样空间是对机械臂肩关节和肘关节电机运动范围采样得到的向量的集合。
S230、获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部。
S240、确定所述相机与末端效应器之间的坐标变换关系，所述末端效应器包括安装在机器人头部的效应器和安装在机器人躯干的效应器。
具体的，根据相机与末端效应器之间的坐标变换关系，可将相机位置的变化向量转换为机器人的位姿变化向量。该坐标变换关系基于链式法则，即相机到头部的坐标变换——头部到躯干的坐标变换，最终得到相机到躯干的坐标变换。
进一步的，所述确定机器人的相机与末端效应器之间的坐标变换关系，包括：根据机器人运动学方程求解相机坐标系到机器人头部的局部坐标系的第一变换矩阵；根据机器人运动学链式法则和所述第一变换矩阵确定第二变换矩阵，所述第二变化矩阵为从相机坐标系到机器人躯干的参考坐标系的变换矩阵。
具体的，通过坐标变换关系可将相机坐标系下观察到的点的坐标转换为头部的局部坐标系下的坐标，再转换为躯干的参考坐标系下的坐标。通过测量相机到头部关节的实际距离以及相机坐标系相对头部关节坐标系转过的角度，可以计算出相机坐标系和头部关节局部坐标系之间的第一变换矩阵W1，W1为齐次变换矩阵，对头部关节局部坐标系和躯干参考坐标系进行同样的操作后得到第二变换矩阵W2，W2为齐次变换矩阵，则基于链式法则将第二变换矩阵乘以第一变换矩阵即为从相机坐标系到躯干参考坐标系的变换矩阵。
示例性的，已知第一变换矩阵W1以及第二变换矩阵W2，存在：P头部＝W1P相机，P躯干＝W2P头部，则P躯干＝W2W1P相机，令W＝W2W1，所以有P躯干＝WP相机，这样即可将相机坐标系观察到的点的坐标转换到躯干的参考坐标系下，完成坐标系的统一。需要说明的是，由于机器人的移动过程中其头部与躯干之间的结构是相对固定的，相机和躯干的相对位置也是固定的，因此，坐标变换关系也是特定的，本实施例中具体有
S250、基于所述坐标变换关系将所述相机位置的变化向量转换为所述机器人的位姿变化向量。
具体的，初始时刻相机坐标系中可观察到的点(坐标记为Pc)，当相机产生位姿变化向量X1时，该点的坐标也对应地变为Pc+X1；将机器人位姿变换之前该点在躯干参考坐标系中的坐标记为Pw，位姿变化之后该点在躯干参考坐标下的坐标记为Pw'，则根据相机坐标系与躯干参考坐标系之间的齐次变换矩阵W，有如下关系：Pw'＝W(Pc+X1)＝WPc+WX1＝Pw+WX1，其中，WX1实质为机器人的位姿变化向量X2，位姿变化向量X2是由第二变化矩阵、第一变换矩阵及相机位置的变化向量相乘得到，即X2＝W2W1X1＝WX1。
S260、将所述静态能力图点云中的各空间点按照所述位姿变化向量移动，移动后的各空间点构成当前时刻的能力图点云。
具体的，静态能力图点云C中的各个点按照所述位姿变化向量移动，将各个点的坐标加上所述位姿变化向量后，即可构成当前时刻的能力图点云CT。需要说明的是，能力图点云是以躯干作为参考坐标系的，在机器人平移或旋转的过程中，能力图始终以机械臂的肩关节为中心。在当前时刻，根据机器人的位姿变化向量对离散的能力图点云中的所有空间点进行单一性映射，可更新能力图点云，即CT＝cj+X2，其中，X2是当前时刻机器人(相对于初始位姿的)位姿变化向量，cj是初始时刻通过FK方程计算得到的静态能力图点云集合中与索引j对应的空间点。当机器人发生位姿变化时，附着在机器人附近的能力图点云也应该发生相应的位移或旋转，得到的CT即为当前时刻的能力图点云。
本发明实施例二提供的一种能力图点云更新方法，在上述实施例的基础上进行优化，通过确定所述相机与末端效应器之间的坐标变换关系，基于链式法则将相机坐标系下观察到的点的坐标转换为头部的局部坐标系下的坐标，再转换为躯干的参考坐标系下的坐标，实现根据相机当前时刻的变化向量实时求解机器人的位姿变化向量，并据此更新初始时刻的静态能力图点云中的点，得到当前时刻的能力图点云，进而实现动态更新能力图，提高描述工作空间的实时性和准确性。
实施例三
图4为本发明实施例二提供的一种能力图点云更新方法的流程图，本实施例是在上述实施例的基础上，进行具体优化，基于Octomap构建机器人周围空间的三维点云地图，在更新能力图点云时考虑到障碍和噪声的占有节点，在机器人移动的过程中能够结合实际环境去除不可达的点。需要说明的是，未在本实施例中详尽描述的技术细节可参见上述任意实施例。
具体的，参考图4，该方法具体包括如下步骤：
S301、对初始时刻机械臂各关节电机的运动范围进行采样。
S302、利用六轴机械臂正解方程求解采样空间，得到机器人初始时刻的静态能力图点云，其中，所述采样空间由采样结果的笛卡尔积表示。
进一步的，机械臂肩关节电机的运动范围包括俯仰和翻滚两个自由度，肘关节电机的运动范围包括偏航和翻滚两个自由度；所述采样结果的笛卡尔积S表示为：S＝Δqsp·Δqsr·Δqey·Δqer，其中，Δqsp表示肩关节电机绕x轴方向俯仰的采样结果，Δqsr表示肩关节电机绕z轴方向翻滚的采样结果，Δqey表示肘关节电机绕y轴方向偏航的采样结果，Δqer表示肘关节电机绕z轴方向翻滚的采样结果。
S303、获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部。
S304、根据所述相机位置的变化向量确定所述机器人的位姿变化向量。
S305、将所述静态能力图点云中的各空间点按照所述位姿变化向量移动，移动后的各空间点构成当前时刻的能力图点云
S306、基于导航三维点云地图Octomap构建机器人周围空间的三维点云地图。
具体的，Octomap是一种以八叉树的数据结构存储三维环境的概率占据地图，由很多网格组成，分辨率可调。相比于密集的点云实现了降维，能够节省大量的存储空间。每个网格都通过一个数值来描述该网格是否被占据，通常用0～1之间的浮点数表示该网格被占据的概率。
图5为本发明实施例三中的基于Octomap构建三维环境地图的示意图。需要说明的是，根据SLAM构建的位姿点云地图，保留了很多冗余信息，这些冗余信息对于机器人在局部环境中完成任务并无实际作用，虽然具备更好的可视化效果，但是密集型点云将占用大量存储空间。而基于Octomap构建的空间的三维地图，以体素的三种状态表示局部环境中有障碍的区域、空闲的区域和未探索的区域，是对稠密点云的降维表示，可以保证在场景地图构建时的效率与实时性，并且能够有效防止噪声、移动物体和深度误差的干扰，以概率的方式对占有的节点进行更新。
S307、将当前时刻的能力图点云插入所述三维点云地图。
具体的，Octomap以特定的分辨率将空间划分为具备三种状态的节点：占有节点、未知(未探索)节点、空闲节点。在Octomap定义的数据结构中局部环境地图的障碍和噪声，都是八叉树的占有节点。当机器人移动到附近时，某些可达区域将变得不可达。在Octomap地图中障碍和噪声产生的占有节点构成的不可达区域由集合B来表示。在更新当前时刻的能力图点云CT时也要考虑到对集合B中的网格的不可达性，即当前时刻的能力图点云CT应为减去不可达区域后的相对补集，公式为：C'T＝CT-CT∩B，C'T为考虑到Octomap地图中的占有节点，最终得到的当前时刻的能力图点云。
S308、对所述当前时刻的能力图点云中的各空间点进行迭代查询。
具体的，将当前时刻的能力图点云插入所述三维点云地图后，对各空间点进行迭代查询，判断空间点对应的八叉树子节点在Octomap中是否为占有状态。能力图点云是以肩关节处为中心的，当能力图点云与Octomap地图中的不可达节点(如障碍物所在的空间点)形成交集时，表示机器人的能力图点云与Octomap地图中的障碍发生碰撞重叠，则在能力图点云的集合中去掉这些有交集的点云，剩余点云的空间位置都是非占有状态。
S309、判断空间点在所述三维网格地图中是否为占有状态，若是，则执行步骤S310；若否，则执行步骤S311。
S310、删除所述空间点。
S311、保留所述空间点，保留的空间点构成当前时刻的能力图点云。
图6A、6B分别为本发明实施例三中的机器人在空闲区域、障碍区域的能力图的示意图。其中，图6A为机器人在空闲区域(无障碍区域)手部可达区域完整的范围；随着机器人移动到障碍区域，能力图与障碍物形成交集时，部分网格消失(如图6B所示)，即去除了占有状态的空间点，考虑了实际环境的变化，体现了能力图在位姿上的动态性。
进一步的，初始时刻通过FK方程计算生成了静态的能力图点云，后期动态能力图点云的更新依赖于机器人位姿变化向量，即静态的能力图点云在当前时刻随着机器人的位姿变化向量而发生移动；同时要实时更新Octomap地图中的网格被占据的概率，即判断当前时刻Tt这些空间点或网格属于哪种状态(被占用、未知、空闲)。
进一步的，Octomap中对实际环境中的物体或障碍(被占有的网格)的更新形式如下：其中，zt记录了每时刻的观测状态，P(n|zt)表示在t时刻，观测状态为zt时节点占有的概率，P(n)表示先验概率。考虑到深度传感器的噪声和动态物体的影响，式中的概率并不是某个时刻的瞬时值。为了得到从初始时刻到当前时刻这一段时间的节点状态的概率累加值，以log-odds的方式将值概率空间转到实数空间，即则到当前时刻为止的节点状态公式为：L(n|z1:T)＝L(n|z1:T-1)+L(n|zT)，该公式把占有概率从[0,1]范围扩充到实数域R。当前时刻节点的状态以实数的形式累加到t-1时刻，如果是空节点(未被占有)，则默认log-odds(即L(n|z1:T))打分为-0.4，如果是占有节点则log-odds打分为0.85。最终累积得分超过0分则将该节点状态更新为占有节点，否则为空节点。
本发明实施例三提供的一种能力图点云更新方法，在上述实施例的基础上进行优化，基于Octomap构建机器人周围空间的三维点云地图，在更新能力图点云时考虑到障碍和噪声的占有节点，在机器人移动的过程中能够结合实际环境去除不可达的点，进而实现动态更新能力图，并将能力图与三维环境地图融合，能够对地图中的障碍区域进行响应，达到与实际环境自适应地实时更新，提高描述工作空间的准确性。
实施例四
图7为本发明实施例四提供的一种能力图点云更新装置的结构图。本实施例提供的能力图点云更新装置包括：
第一获取模块410，用于初始时刻，获取机器人的静态能力图点云，所述静态能力图点云包括机器人的机械臂各关节运动范围中采样点的位置信息；
第二获取模块420，用于获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部；
位姿变化确定模块430，用于根据所述相机位置的变化向量确定所述机器人的位姿变化向量；
更新模块440，用于基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云。
本发明实施例三提供的一种能力图点云更新装置，通过第一获取模块在初始时刻获取机器人的静态能力图点云，通过第二获取模块获取从初始时刻到当前时刻相机位置的变化向量，通过位姿变化确定模块根据所述相机位置的变化向量确定所述机器人的位姿变化向量，通过更新模块基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云，实现了动态更新能力图，提高描述工作空间的实时性和准确性。
在上述实施例的基础上，所述第一获取模块410，包括：
采样单元，用于对初始时刻机械臂各关节电机的运动范围进行采样；
正解单元，用于利用六轴机械臂正解方程求解采样空间，得到机器人初始时刻的静态能力图点云，其中，所述采样空间由采样结果的笛卡尔积表示。
在上述实施例的基础上，所述位姿变化确定模块430，包括：
坐标变换单元，用于确定所述相机与末端效应器之间的坐标变换关系，所述末端效应器包括安装在机器人头部的效应器和安装在机器人躯干的效应器；
位姿变化转换单元，用于基于所述坐标变换关系将所述相机位置的变化向量转换为所述机器人的位姿变化向量。
进一步的，所述坐标变换单元，包括：
第一变换子单元，用于根据机器人运动学方程求解相机坐标系到机器人头部的局部坐标系的第一变换矩阵；
第二变换子单元，用于根据机器人运动学链式法则和所述第一变换矩阵确定第二变换矩阵，所述第二变化矩阵为从相机坐标系到机器人躯干的参考坐标系的变换矩阵。
进一步的，所述更新模块440，具体用于将所述静态能力图点云中的各空间点按照所述位姿变化向量移动，移动后的各空间点构成当前时刻的能力图点云。
进一步的，所述装置还包括：
三维点云地图构建模块，用于基于导航三维点云地图Octomap构建机器人周围空间的三维点云地图；
插入模块，用于将当前时刻的能力图点云插入所述三维点云地图；
迭代查询模块，用于对所述当前时刻的能力图点云中的各空间点进行迭代查询，判断各空间点在所述三维网格地图中是否为占有状态；
若是，则删除所述空间点，否则保留所述空间点，保留的空间点构成当前时刻的能力图点云。
进一步的，机械臂肩关节电机的运动范围包括俯仰和翻滚两个自由度，肘关节电机的运动范围包括偏航和翻滚两个自由度；
所述采样结果的笛卡尔积S表示为：S＝Δqsp·Δqsr·Δqey·Δqer，其中，Δqsp表示肩关节电机绕x轴方向俯仰的采样结果，Δqsr表示肩关节电机绕z轴方向翻滚的采样结果，Δqey表示肘关节电机绕y轴方向偏航的采样结果，Δqer表示肘关节电机绕z轴方向翻滚的采样结果。
本发明实施例四提供的能力图点云更新装置可以用于执行上述任意实施例提供的能力图点云更新方法，具备相应的功能和有益效果。
实施例五
图8为本发明实施例五提供的一种设备的硬件结构示意图。如图8所示，本实施例提供的一种设备，包括：处理器510和存储装置520。该设备中的处理器可以是一个或多个，图8中以一个处理器510为例，所述设备中的处理器510和存储装置520可以通过总线或其他方式连接，图8中以通过总线连接为例。
所述一个或多个程序被所述一个或多个处理器510执行，使得所述一个或多个处理器实现上述实施例中任意所述的能力图点云更新方法。
该设备中的存储装置520作为一种计算机可读存储介质，可用于存储一个或多个程序，所述程序可以是软件程序、计算机可执行程序以及模块，如本发明实施例中能力图点云更新方法对应的程序指令/模块(例如，附图4所示的CT图像的重建装置中的模块，包括：第一获取模块410、第二获取模块420、位姿变化确定模块430以及更新模块440)。处理器510通过运行存储在存储装置520中的软件程序、指令以及模块，从而执行设备的各种功能应用以及数据处理，即实现上述方法实施例中的能力图点云更新方法。
存储装置520主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据设备的使用所创建的数据等(如上述实施例中的静态能力图点云、位姿变化向量等)。此外，存储装置520可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实例中，存储装置520可进一步包括相对于处理器510远程设置的存储器，这些远程存储器可以通过网络连接至设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
并且，当上述设备中所包括一个或者多个程序被所述一个或者多个处理器510执行时，程序进行如下操作：
初始时刻，获取机器人的静态能力图点云，所述静态能力图点云包括机器人的机械臂各关节运动范围中采样点的位置信息；获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部；根据所述相机位置的变化向量确定所述机器人的位姿变化向量；基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云。
本实施例提出的设备与上述实施例提出的能力图点云更新方法属于同一发明构思，未在本实施例中详尽描述的技术细节可参见上述任意实施例，并且本实施例具备与执行能力图点云更新方法相同的有益效果。
在上述实施例的基础上，本实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被能力图点云更新装置执行时实现本发明上述任意实施例中的能力图点云更新方法，该方法包括：
初始时刻，获取机器人的静态能力图点云，所述静态能力图点云包括机器人的机械臂各关节运动范围中采样点的位置信息；获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部；根据所述相机位置的变化向量确定所述机器人的位姿变化向量；基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云。
当然,本发明实施例所提供的一种包含计算机可执行指令的存储介质,其计算机可执行指令不限于如上所述的能力图点云更新方法操作,还可以执行本发明任意实施例所提供的能力图点云更新方法中的相关操作，且具备相应的功能和有益效果。
通过以上关于实施方式的描述，所属领域的技术人员可以清楚地了解到，本发明可借助软件及必需的通用硬件来实现，当然也可以通过硬件实现，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如计算机的软盘、只读存储器(Read-Only Memory,ROM)、随机存取存储器(RandomAccess Memory,RAM)、闪存(FLASH)、硬盘或光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的能力图点云更新方法。
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。
Claims (10)
1.一种能力图点云更新方法，其特征在于，包括：
初始时刻，获取机器人的静态能力图点云，所述静态能力图点云包括机器人的机械臂各关节运动范围中采样点的位置信息；
获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部；
根据所述相机位置的变化向量确定所述机器人的位姿变化向量；
基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云。
2.根据权利要求1所述的方法，其特征在于，所述初始时刻，获取机器人的静态能力图点云，包括：
对初始时刻机械臂各关节电机的运动范围进行采样；
利用六轴机械臂正解方程求解采样空间，得到机器人初始时刻的静态能力图点云，其中，所述采样空间由采样结果的笛卡尔积表示。
3.根据权利要求1所述的方法，其特征在于，所述根据所述相机位置的变化向量确定所述机器人的位姿变化向量，包括：
确定所述相机与末端效应器之间的坐标变换关系，所述末端效应器包括安装在机器人头部的效应器和安装在机器人躯干的效应器；
基于所述坐标变换关系将所述相机位置的变化向量转换为所述机器人的位姿变化向量。
4.根据权利要求3所述的方法，其特征在于，所述确定所述相机与末端效应器之间的坐标变换关系，包括：
根据机器人运动学方程求解相机坐标系到机器人头部的局部坐标系的第一变换矩阵；
根据机器人运动学链式法则和所述第一变换矩阵确定第二变换矩阵，所述第二变化矩阵为从相机坐标系到机器人躯干的参考坐标系的变换矩阵。
5.根据权利要求1所述的方法，其特征在于，所述基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云，包括：
将所述静态能力图点云中的各空间点按照所述位姿变化向量移动，移动后的各空间点构成当前时刻的能力图点云。
6.根据权利要求1所述的方法，其特征在于，还包括：
基于导航三维点云地图Octomap构建机器人周围空间的三维点云地图；
将当前时刻的能力图点云插入所述三维点云地图；
对所述当前时刻的能力图点云中的各空间点进行迭代查询，判断各空间点在所述三维网格地图中是否为占有状态；
若是，则删除所述空间点，否则保留所述空间点，保留的空间点构成当前时刻的能力图点云。
7.根据权利要求2所述的方法，其特征在于，机械臂肩关节电机的运动范围包括俯仰和翻滚两个自由度，肘关节电机的运动范围包括偏航和翻滚两个自由度；
所述采样结果的笛卡尔积S表示为：S＝Δqsp·Δqsr·Δqey·Δqer，其中，Δqsp表示肩关节电机绕x轴方向俯仰的采样结果，Δqsr表示肩关节电机绕z轴方向翻滚的采样结果，Δqey表示肘关节电机绕y轴方向偏航的采样结果，Δqer表示肘关节电机绕z轴方向翻滚的采样结果。
8.一种能力图点云更新装置，其特征在于，包括：
第一获取模块，用于初始时刻，获取机器人的静态能力图点云，所述静态能力图点云包括机器人的机械臂各关节运动范围中采样点的位置信息；
第二获取模块，用于获取从初始时刻到当前时刻相机位置的变化向量，所述相机固定在机器人内部；
位姿变化确定模块，用于根据所述相机位置的变化向量确定所述机器人的位姿变化向量；
更新模块，用于基于所述位姿变化向量更新所述静态能力图点云，得到当前时刻的能力图点云。
9.一种设备，其特征在于，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-7中任一所述的能力图点云更新方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一所述的能力图点云更新方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910085829.9A CN109807933B (zh) | 2019-01-29 | 2019-01-29 | 一种能力图点云更新方法、装置、设备及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910085829.9A CN109807933B (zh) | 2019-01-29 | 2019-01-29 | 一种能力图点云更新方法、装置、设备及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109807933A true CN109807933A (zh) | 2019-05-28 |
| CN109807933B CN109807933B (zh) | 2022-04-15 |
Family
ID=66605596
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910085829.9A Active CN109807933B (zh) | 2019-01-29 | 2019-01-29 | 一种能力图点云更新方法、装置、设备及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109807933B (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111222225A (zh) * | 2019-12-20 | 2020-06-02 | 浙江欣奕华智能科技有限公司 | 一种机器人中传感器位姿的确定方法及装置 |
| CN112706008A (zh) * | 2021-01-06 | 2021-04-27 | 广东博智林机器人有限公司 | 一种天花打磨方法及装置 |
| WO2021223465A1 (zh) * | 2020-05-06 | 2021-11-11 | 北京嘀嘀无限科技发展有限公司 | 一种高精度地图构建方法及系统 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101325926B1 (ko) * | 2012-05-22 | 2013-11-07 | 동국대학교 산학협력단 | 실시간 ３ｄ 데이터 송수신을 위한 ３ｄ 데이터 처리 장치 및 방법 |
| US20150073646A1 (en) * | 2010-05-20 | 2015-03-12 | Irobot Corporation | Mobile Human Interface Robot |
| CN104992074A (zh) * | 2015-07-29 | 2015-10-21 | 华南理工大学 | 机载激光扫描系统航带拼接方法与装置 |
| CN105976353A (zh) * | 2016-04-14 | 2016-09-28 | 南京理工大学 | 基于模型和点云全局匹配的空间非合作目标位姿估计方法 |
| CN108732584A (zh) * | 2017-04-17 | 2018-11-02 | 百度在线网络技术（北京）有限公司 | 用于更新地图的方法和装置 |
| CN109202912A (zh) * | 2018-11-15 | 2019-01-15 | 太原理工大学 | 一种基于单目深度传感器和机械臂配准目标轮廓点云的方法 |
-
2019
- 2019-01-29 CN CN201910085829.9A patent/CN109807933B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150073646A1 (en) * | 2010-05-20 | 2015-03-12 | Irobot Corporation | Mobile Human Interface Robot |
| KR101325926B1 (ko) * | 2012-05-22 | 2013-11-07 | 동국대학교 산학협력단 | 실시간 ３ｄ 데이터 송수신을 위한 ３ｄ 데이터 처리 장치 및 방법 |
| CN104992074A (zh) * | 2015-07-29 | 2015-10-21 | 华南理工大学 | 机载激光扫描系统航带拼接方法与装置 |
| CN105976353A (zh) * | 2016-04-14 | 2016-09-28 | 南京理工大学 | 基于模型和点云全局匹配的空间非合作目标位姿估计方法 |
| CN108732584A (zh) * | 2017-04-17 | 2018-11-02 | 百度在线网络技术（北京）有限公司 | 用于更新地图的方法和装置 |
| CN109202912A (zh) * | 2018-11-15 | 2019-01-15 | 太原理工大学 | 一种基于单目深度传感器和机械臂配准目标轮廓点云的方法 |
Non-Patent Citations (2)
| Title |
|---|
| FRANZISKA ZACHARIAS等: "Capturing Robot Workspace Structure: Representing Robot Capabilities", 《PROCEEDING OF THE 2007 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS》 * |
| YIMING YANG 等: "iDRM: Humanoid Motion Planning with Realtime End–Pose Selection in Complex Environments", 《2016 IEEE-RAS 16TH INTERNATIONAL CONFERENCE ON HUMANOID ROBOTS (HUMANOIDS)》 * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111222225A (zh) * | 2019-12-20 | 2020-06-02 | 浙江欣奕华智能科技有限公司 | 一种机器人中传感器位姿的确定方法及装置 |
| CN111222225B (zh) * | 2019-12-20 | 2023-08-29 | 浙江欣奕华智能科技有限公司 | 一种机器人中传感器位姿的确定方法及装置 |
| WO2021223465A1 (zh) * | 2020-05-06 | 2021-11-11 | 北京嘀嘀无限科技发展有限公司 | 一种高精度地图构建方法及系统 |
| CN112706008A (zh) * | 2021-01-06 | 2021-04-27 | 广东博智林机器人有限公司 | 一种天花打磨方法及装置 |
| CN112706008B (zh) * | 2021-01-06 | 2022-03-01 | 广东博智林机器人有限公司 | 一种天花打磨方法及装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109807933B (zh) | 2022-04-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109521774B (zh) | 一种基于强化学习的喷涂机器人轨迹优化方法 | |
| CN109807933A (zh) | 一种能力图点云更新方法、装置、设备及存储介质 | |
| CN113352319B (zh) | 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法 | |
| CN106584461B (zh) | 多约束条件下七自由度仿人机械臂的逆运动学拟人臂构型优化方法 | |
| CN111360824B (zh) | 一种双臂自碰撞检测方法和计算机可读存储介质 | |
| WO2023024317A1 (zh) | 一种机器人避障方法、装置和机器人 | |
| US20110093119A1 (en) | Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same | |
| CN107957684A (zh) | 一种基于虚拟速度向量场的机器人三维无碰轨迹规划方法 | |
| KR20160070006A (ko) | 간섭 회피 방법, 제어 장치 및 프로그램 | |
| CN107253191B (zh) | 一种双机械臂系统及其协调控制方法 | |
| CN113442140B (zh) | 一种基于Bezier寻优的笛卡尔空间避障规划方法 | |
| CN112828889A (zh) | 一种六轴协作机械臂路径规划方法及系统 | |
| JP3866168B2 (ja) | 多重構造を用いた動作生成システム | |
| JPH02224004A (ja) | 移動体の干渉チェック装置 | |
| Monica et al. | Humanoid robot next best view planning under occlusions using body movement primitives | |
| CN114714346A (zh) | 一种用于微波远场-近场扫描与成像任务的机械臂控制策略与优化方法 | |
| JP5766936B2 (ja) | ３次元環境復元装置、３次元環境復元方法、及びロボット | |
| Tonneau et al. | Task efficient contact configurations for arbitrary virtual creatures | |
| Tonneau et al. | Using task efficient contact configurations to animate creatures in arbitrary environments | |
| Liu et al. | A planning method for safe interaction between human arms and robot manipulators | |
| CN101308523B (zh) | 一种虚拟激光加工过程几何仿真方法 | |
| CN113146637A (zh) | 一种机器人笛卡尔空间的运动规划方法 | |
| Sithamparanathan et al. | Pose Estimation of a Robot Arm from a Single Camera | |
| Ryde et al. | Cooperative mutual 3D laser mapping and localization | |
| CN110531698B (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 | ||
| CP01 | Change in the name or title of a patent holder | ||
| CP01 | Change in the name or title of a patent holder |
Address after: No.3 workshop, No.3, Luyang Caimao Road, Zhoushi Town, Kunshan City, Suzhou City, Jiangsu Province Patentee after: Jiangsu Tongyou Vision Technology Co.,Ltd. Address before: No.3 workshop, No.3, Luyang Caimao Road, Zhoushi Town, Kunshan City, Suzhou City, Jiangsu Province Patentee before: JIANGSU KOYO VISION TECHNOLOGY Co.,Ltd. |
|
| CP01 | Change in the name or title of a patent holder | ||
| CP01 | Change in the name or title of a patent holder |
Address after: No.3 workshop, No.3, Luyang Caimao Road, Zhoushi Town, Kunshan City, Suzhou City, Jiangsu Province Patentee after: Jiangsu Tongyou Robot Technology Co.,Ltd. Address before: No.3 workshop, No.3, Luyang Caimao Road, Zhoushi Town, Kunshan City, Suzhou City, Jiangsu Province Patentee before: Jiangsu Tongyou Vision Technology Co.,Ltd. |