CN114789446A - 机器人位姿估计方法、装置、电子设备及存储介质 - Google Patents

机器人位姿估计方法、装置、电子设备及存储介质 Download PDF

Info

Publication number
CN114789446A
CN114789446A CN202210593947.2A CN202210593947A CN114789446A CN 114789446 A CN114789446 A CN 114789446A CN 202210593947 A CN202210593947 A CN 202210593947A CN 114789446 A CN114789446 A CN 114789446A
Authority
CN
China
Prior art keywords
dimensional
straight line
robot
line segment
dimensional straight
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
Application number
CN202210593947.2A
Other languages
English (en)
Inventor
吴绍锋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ping An Puhui Enterprise Management Co Ltd
Original Assignee
Ping An Puhui Enterprise Management Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Ping An Puhui Enterprise Management Co Ltd filed Critical Ping An Puhui Enterprise Management Co Ltd
Priority to CN202210593947.2A priority Critical patent/CN114789446A/zh
Publication of CN114789446A publication Critical patent/CN114789446A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Image Analysis (AREA)

Abstract

本发明涉及人工智能技术，揭露了一种机器人位姿估计方法，包括：获取周围环境的二维图像及三维点云，从所述二维图像中提取二维直线段以及从所述三维点云中提取三维直线段，将所述三维直线段映射在Plucker坐标系中，得到六维向量，根据所述六维向量及所述二维直线段构造机器人的初始位姿参数矩阵，通过所述初始位姿参数矩阵构建线性可导的损失函数，根据预设的损失值对所述损失函数求解，得到所述机器人的当前位姿参数矩阵，根据所述当前位姿参数矩阵获取机器人的旋转矩阵和平移向量。本发明还提出一种机器人位姿估计装置、电子设备以及存储介质。本发明可以提高机器人位姿估计精确度。

Description

机器人位姿估计方法、装置、电子设备及存储介质
技术领域
本发明涉及人工智能技术领域，尤其涉及一种机器人位姿估计方法、装置、电子设备及存储介质。
背景技术
机器人在移动的过程中可以使用相机获得环境的二维数据，利用激光雷达获得环境的三维数据，然后通过这些二维和三维数据构建损失函数，再通过深度学习优化方法来估计自身的位姿。现有的基于二维-三维数据中的直线特征和深度学习的机器人位姿估计方法重点在于损失函数的构建，而目前损失函数在构建过程中，二维数字特征与三维数字特征为非线性关系，计算过程过于繁杂，且正确率无法保证，导致机器人的位姿估计准确率较低。
发明内容
本发明提供一种机器人位姿估计方法、装置、电子设备及存储介质，其主要目的在于解决机器人的位姿估计准确率较低的问题。
为实现上述目的，本发明提供的一种机器人位姿估计方法，包括：
获取机器人周围环境的二维图像，在预构建的图像坐标系中提取所述二维图像中的二维直线段并提取所述二维直线段的首尾端点坐标；
采集所述周围环境的三维点云，在预构建的世界坐标系中提取所述三维点云中的三维直线段；
计算所述三维直线段的法向量和方向向量，由所述法向量和方向向量将所述三维直线段映射为Plucker坐标系下的六维向量；
根据所述六维向量及所述二维直线段构造所述机器人的初始位姿参数矩阵；
由所述二维直线段及三维直线段构建二维-三维直线对，提取所述二维-三维直线对的匹配误差；根据所述二维-三维直线对、二维直线段的首尾端点、二维-三维直线对的匹配误差及初始位姿参数矩阵构造损失函数；
根据预设的损失值对所述损失函数中的初始位姿参数矩阵求解，得到所述机器人当前位姿参数矩阵，由所述机器人当前位姿参数矩阵拆解得到所述机器人的旋转矩阵和平移向量。
可选地，所述的机器人位姿估计方法，其特征在于，所述在预构建的图像坐标系中提取所述二维图像中的二维直线段，包括：
利用中值滤波算法去除所述二维图像中的图像噪声，得到去噪二维图像；
利用融合Canny算子和小波变换的边缘检测方法对所述去噪二维图像进行边缘检测，获得多组图像边缘直线段；
基于边缘像素拟合的直线提取算法提取所述二维图像边缘直线段的直线特征，得到所述二维直线段。
可选地，所述的机器人位姿估计方法，其特征在于，所述在预构建的世界坐标系中提取所述三维点云中的三维直线段，包括：
将所述三维点云沿不同方向投影到二维空间，生成多张具有深度信息的二维图像；
采用非真实感渲染技术将所述深度信息转化为所述二维图像的灰度值；
在所述二维图像中利用所述图像灰度值提取二维直线支持区，将所述二维直线支持区反投影到三维空间，得到直线特征点集；
将所述直线特征点集拟合成多个LSHP结构，将所述的多个LSHP结构进行合并及扩展，得到世界坐标系下的三维直线段。
可选地，所述的机器人位姿估计方法，其特征在于，所述构造的损失函数表达式为：
Figure BDA0003666878500000021
其中，n为所述二维-三维直线对的数量，d(li,Li)表示所述二维直线li和所述三维直线Li的匹配误差，
Figure BDA0003666878500000022
为所述二维直线段的首部端点，
Figure BDA0003666878500000023
为所述二维直线段的尾部端点，L为所述六维向量，T为所述初始位姿参数矩阵。
可选地，所述的机器人位姿估计方法，其特征在于，根据预设的损失值对所述损失函数中的初始位姿参数矩阵求解，得到所述机器人当前位姿参数矩阵，包括：
求出所述初始位姿参数矩阵中各个参数值ti的梯度
Figure BDA0003666878500000024
Figure BDA0003666878500000025
更新所述初始位姿参数矩阵中的各个参数值：
Figure BDA0003666878500000031
其中，β为学习率；
根据n对二维-三维直线对计算出所述损失函数的损失值；
当所述损失值大于或者等于预设的损失值阈值时，返回上述获取机器人周围环境的二维图像的步骤；
当所述损失值小于所述预设的损失值阈值时，根据更新后参数值t′i得到所述机器人当前位姿参数矩阵。
可选地，所述的机器人位姿估计方法，其特征在于，所述计算所述三维直线段的法向量和方向向量，包括：
利用下述公式计算得到所述三维直线段的法向量uT和方向向量vT：
uT＝(L1 L2 L3)＝(ax ay az)×(bx by bz)，
vT＝(L4 L5 L6)＝bw(ax ay az)-aw(bx by bz)，
其中，L1为所述法向量uT在x轴的投影坐标，L2为所述法向量uT在y轴的投影坐标，L3为所述法向量uT在z轴的投影坐标，L4为所述方向向量vT在x轴的投影坐标，L5为所述方向向量vT在y轴的投影坐标，L6为所述方向向量vT在z轴的投影坐标，ax和bx分别为所述三维直线段中任意两点的x轴坐标，ay和by为分别为所述三维直线段中任意两点的y轴坐标，az和bz分别为所述三维直线段中任意两点的z轴坐标，aw和bw为常数。
可选地，所述的机器人位姿估计方法，其特征在于，所述由所述二维直线段及三维直线段构建二维-三维直线对，包括：
对所述二维直线和三维直线进行SURF特征提取，并对提取的特征进行特征匹配，获得特征点对；
对所述特征点对进行匹配筛选，获取相匹配的二维-三维特征点对。
为了解决上述问题，本发明还提供一种机器人位姿估计装置，所述装置包括：
信息获取模块，用于获取机器人周围环境的二维图像，在预构建的图像坐标系中提取所述二维图像中的二维直线段并提取所述二维直线段的首尾端点坐标；采集所述周围环境的三维点云，在预构建的世界坐标系中提取所述三维点云中的三维直线段；
信息处理模块，用于计算所述三维直线段的法向量和方向向量，由所述法向量和方向向量将所述三维直线段映射为Plucker坐标系下的六维向量；根据所述六维向量及所述二维直线段构造所述机器人的初始位姿参数矩阵；
损失函数构建模块，用于由所述二维直线段及三维直线段构建二维-三维直线对，提取所述二维-三维直线对的匹配误差，根据所述二维-三维直线对、二维直线段的首尾端点、二维-三维直线对的匹配误差及初始位姿参数矩阵构造损失函数；
位姿求解模块，用于根据预设的损失值所述损失函数中的初始位姿参数矩阵求解，得到所述机器人当前位姿参数矩阵，由所述机器人当前位姿参数矩阵拆解得到所述机器人的旋转矩阵和平移向量；
为了解决上述问题，本发明还提供一种电子设备，所述电子设备包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的计算机程序，所述计算机程序被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述所述的机器人位姿估计方法。
为了解决上述问题，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有至少一个计算机程序，所述至少一个计算机程序被电子设备中的处理器执行以实现上述所述的机器人位姿估计方法。
本发明实施例中，获取环境的二维-三维直线段，将所述三维直线段映射在Plucker坐标系中，得到一个六维向量，再根据所述六维向量及所述二维直线段构造所述机器人的初始位姿参数矩阵，通过所述初始位姿参数矩阵将所述六维向量线性表示为所述二维直线段，构建一个线性可导的损失函数，根据预设的损失值求得机器人当前位姿参数矩阵，进而获取机器人当前位姿。本发明实施例中找到同一个环境信息不同维度直线特征之间的线性关系，使损失函数的构建和求解更加便捷和精确，进而使机器人的位姿估计更加便捷和精确。因此，本发明提出的机器人位姿估计方法、装置、电子设备及计算机可读存储介质，可以解决机器人的位姿估计准确率较低的问题。
附图说明
图1为本发明一实施例提供的机器人位姿估计方法的流程示意图；
图2为本发明一实施例提供的提取所述二维图像中的二维直线段的流程示意图；
图3为本发明一实施例提供的提取所述三维点云中的三维直线段的流程示意图；
图4为本发明一实施例提供的机器人位姿估计装置的功能模块图；
图5为本发明一实施例提供的实现所述机器人位姿估计方法的电子设备的结构示意图。
本发明目的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本申请实施例提供一种机器人位姿估计方法。所述机器人位姿估计方法的执行主体包括但不限于服务端、终端等能够被配置为执行本申请实施例提供的该方法的电子设备中的至少一种。换言之，所述机器人位姿估计方法可以由安装在终端设备或服务端设备的软件或硬件来执行，所述软件可以是区块链平台。所述服务端包括但不限于：单台服务器、服务器集群、云端服务器或云端服务器集群等。所述服务器可以是独立的服务器，也可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、内容分发网络(Content Delivery Network，CDN)、以及大数据和人工智能平台等基础云计算服务的云服务器。
参照图1所示，为本发明一实施例提供的机器人位姿估计方法的流程示意图。在本实施例中，所述机器人位姿估计方法包括：
S1、获取机器人周围环境的二维图像，在预构建的图像坐标系中提取所述二维图像中的二维直线段并提取所述二维直线段的首尾端点坐标；
详细地，本发明实施例中可以利用机器人上安装的摄影装置获取所述机器人周围环境的二维图像，其中，所述摄影装置可以为模拟摄影机或数字摄像机；
进一步地，参阅图2所示，所述提取所述二维图像的二维直线段，包括：
S11、利用中值滤波算法去除所述二维图像中的图像噪声，得到去噪二维图像；
S12、利用融合Canny算子和小波变换的边缘检测方法对所述去噪二维图像进行边缘检测，获得多组图像边缘直线段；
S13、基于边缘像素拟合的直线提取算法提取所述二维图像边缘直线段的直线特征，得到所述二维直线段。
详细地，本发明实施例中所述二维直线段可表示为l＝(lx ly lc)，其中，l为所述二维直线段，lx为x轴的坐标集合，ly为y轴的坐标集合，lc为常数；
进一步地，所述二维直线段的首尾端点坐标包括首部端点ls＝(xs,ys,ds)及尾部端点le＝(xe,ye,de)，其中xs为所述二维直线段首部端点的x轴坐标集合，ys为所述二维直线段首部端点的y轴坐标集合，ds为所述二维直线段首部端点的常数，同样地，xe为所述二维直线段尾部端点的x轴坐标集合，ye为所述二维直线段尾部端点的y轴坐标集合,de为所述二维直线段尾部端点的常数。
S2、采集所述周围环境的三维点云，在预构建的世界坐标系中提取所述三维点云中的三维直线段；
详细地，本发明实施例中可以利用机器人上安装的三维探测器采集所述周围环境的三维点云；
其中，所述三维探测器可以为激光雷达探测器。所述激光雷达以激光作为信号源，由激光器发射出的脉冲激光，打到所述机器人的周围环境上，引起散射，一部分光波会反射到激光雷达的接收器上，所反射的激光会携带方位、距离等信息，将激光束按照某种轨迹进行扫描，边扫描边记录反射的激光点信息，由于扫描极为精细，获得到大量的激光点，形成所述周围环境的三维点云；
详细地，参阅图3所示，所述在预构建的世界坐标系中提取所述三维点云中的三维直线段，包括：
S21、将所述三维点云沿不同方向投影到二维空间，生成多张具有深度信息的二维图像；
S22、采用非真实感渲染技术将所述深度信息转化为所述二维图像的灰度值；
S23、在所述二维图像中利用所述图像灰度值提取二维直线支持区，将所述二维直线支持区反投影到三维空间，得到直线特征点集；
S24、将所述直线特征点集拟合成多个LSHP结构，将所述的多个LSHP结构进行合并及扩展，得到世界坐标系下的三维直线段；
其中，所述LSHP结构为一对侧边相接的三维矩形片。
S3、计算所述三维直线段的法向量和方向向量，由所述法向量和方向向量将所述三维直线段映射为Plucker坐标系下的六维向量；
详细地，本发明一个实施例中，根据所述三维直线段选取两个三维点A＝(ax,ay,az,aw)T，B＝(bx,by,bz,bw)T；
其中，ax为所述三维点A的x轴坐标，ay为所述三维点A的y轴坐标，az为所述三维点A的z轴坐标，aw为所述三维点A的常数，bx为所述三维点B的x轴坐标，by为所述三维点B的y轴坐标，bz为所述三维点B的z轴坐标，bw为所述三维点B的常数；
根据所述三维点，利用下述公式计算得到所述三维直线段的法向量uT和方向向量vT：
uT＝(L1 L2 L3)＝(ax ay az)×(bx by bz)，
vT＝(L4 L5 L6)＝bw(ax ay az)-aw(bx by bz)；
其中，uT＝(L1 L2 L3)为所述法向量，vT＝(L4 L5 L6)为所述方向向量，L1为所述法向量uT在x轴的投影坐标，L2为所述法向量uL在y轴的投影坐标，L3为所述法向量uT在z轴的投影坐标，L4为所述方向向量vT在x轴的投影坐标，L5为所述方向向量vT在y轴的投影坐标，L6为所述方向向量vT在z轴的投影坐标，ax为所述三维点A的x轴坐标，ay为所述三维点A的y轴坐标，az为所述三维点A的z轴坐标，aw为所述三维点A的常数，bx为所述三维点B的x轴坐标，by为所述三维点B的y轴坐标，bz为所述三维点B的z轴坐标，bw为所述三维点B的常数；
进一步地，本发明实施例由所述法向量和方向向量将所述三维直线段映射为Plucker坐标系下的六维向量，计算表达式为：L＝(uT vT)T＝(L1 L2 L3 L4 L5 L6)T。
S4、根据所述六维向量及所述二维直线段构造所述机器人的初始位姿参数矩阵；
本发明实施例中，所述初始位姿参数矩阵T使得所述六维向量L线性映射为所述二维直线段，数学表达式为：l＝TL，其中l为所述二维直线段，
Figure BDA0003666878500000081
为所述初始位姿参数矩阵，L为所述六维向量，R为大小为3×3、包含九个未知参数、代表所述机器人转向的方向和角度的矩阵，R[t]×为大小为3×3、包含九个未知参数、代表机器人运动的方向和距离的矩阵，t1-t18为十八个未知参数。
S5、由所述二维直线段及三维直线段构建二维-三维直线对，提取所述二维-三维直线对的匹配误差；根据所述二维-三维直线对、二维直线段的首尾端点、二维-三维直线对的匹配误差及初始位姿参数矩阵构造损失函数；
详细地，所述由所述二维直线段及三维直线段构建二维-三维直线对，包括：
对所述二维直线和三维直线进行SURF特征提取，并对提取的特征进行特征匹配，获得特征点对；
对所述特征点对进行匹配筛选，获取相匹配的二维-三维特征点对；
进一步地，本发明实施例通过构建求取变换矩阵的方法，根据所述二维-三维特征点对构建二维-三维直线对并提取所述二维-三维直线对的匹配误差。
本发明实施例中，假设构建出n对直线对SET＝{seti(li,Li)|i＝1,2…n}，根据所述二维-三维直线对、二维直线的首尾端点、二维-三维直线对的匹配误差、位姿参数矩阵构造的损失函数为
Figure BDA0003666878500000082
Figure BDA0003666878500000083
其中n为所述二维-三维直线对的数量，dli,Li表示所述二维直线li和所述三维直线Li的匹配误差，
Figure BDA0003666878500000084
为所述二维直线段的首部端点，
Figure BDA0003666878500000085
为所述二维直线段的尾部端点，L为所述六维向量，T为所述初始位姿参数矩阵。
S6、根据预设的损失值对所述损失函数中的初始位姿参数矩阵求解，得到所述机器人当前位姿参数矩阵，由所述机器人当前位姿参数矩阵拆解得到所述机器人的旋转矩阵和平移向量。
详细地，本发明实施例中，在所述损失函数中根据预设的损失值对所述初始位姿参数矩阵中各个参数逐一进行求解，机器人的位姿求解问题即为所述初始位姿参数矩阵中各个参数的求解，其中，使得损失值最小的表达式为：
Figure BDA0003666878500000091
进一步地，所述初始位姿参数矩阵求解可以使用牛顿法、最小二乘法、梯度下降法、AdaDelta、Ada-Grad、Adam、NAG和RMSprop等方法，本发明实施例中以梯度下降法为例进行求解：
求出所述初始位姿参数矩阵中各个参数值的梯度：
Figure BDA0003666878500000092
Figure BDA0003666878500000093
更新所述初始位姿参数矩阵中的各个参数值：
Figure BDA0003666878500000094
Figure BDA0003666878500000095
其中，β为学习率，可设为0.01，0.001等；
令t1＝t′1,t2＝t′2,……,t18＝t′18，根据n对直线对计算出此刻的损失值，如果所述此刻的损失值小于所述预设的损失值，则输出t′1、t′2……t′18，否则回到S1；
为防止梯度下降法陷于局部最小值，且最小值大于预设的损失值，无法跳出循环。可限制循环次数，如果循环达到设定值，则终止循环，输出所述局部最小值；
计算得到机器人当前位姿参数矩阵
Figure BDA0003666878500000096
由所述机器人当前位姿参数矩阵
Figure BDA0003666878500000097
分解得到机器人旋转矩阵为
Figure BDA0003666878500000098
机器人平移向量的斜对称矩阵为
Figure BDA0003666878500000099
根据所述机器人平移向量的斜对称矩阵和奇异值分解算法获取所述机器人的平移向量；
根据所述机器人的旋转矩阵及平移向量得到机器人的位姿。
如图4所示，是本发明一实施例提供的机器人位姿估计装置的功能模块图。
本发明所述机器人位姿估计装置100可以安装于电子设备中。根据实现的功能，所述机器人位姿估计装置100可以包括信息获取模块101、信息处理模块102、损失函数构建模块103、位姿求解模块104。本发明所述模块也可以称之为单元，是指一种能够被电子设备处理器所执行，并且能够完成固定功能的一系列计算机程序段，其存储在电子设备的存储器中。
在本实施例中，关于各模块/单元的功能如下：
所述信息获取模块101，用于获取机器人周围环境的二维图像，在预构建的图像坐标系中提取所述二维图像中的二维直线段并提取所述二维直线段的首尾端点坐标；采集所述周围环境的三维点云，在预构建的世界坐标系中提取所述三维点云中的三维直线段；
所述信息处理模块102，用于计算所述三维直线段的法向量和方向向量，由所述法向量和方向向量将所述三维直线段映射为Plucker坐标系下的六维向量；根据所述六维向量及所述二维直线段构造所述机器人的初始位姿参数矩阵；
所述损失函数构建模块103，用于由所述二维直线段及三维直线段构建二维-三维直线对，提取所述二维-三维直线对的匹配误差，根据所述二维-三维直线对、二维直线段的首尾端点、二维-三维直线对的匹配误差及初始位姿参数矩阵构造损失函数；
所述位姿求解模块104，用于根据预设的损失值所述损失函数中的初始位姿参数矩阵求解，得到所述机器人当前位姿参数矩阵，由所述机器人当前位姿参数矩阵拆解得到所述机器人的旋转矩阵和平移向量。
详细地，本发明实施例中所述机器人位姿估计装置100中所述的各模块在使用时采用与上述图1至图3中所述的机器人位姿估计方法一样的技术手段，并能够产生相同的技术效果，这里不再赘述。
如图5所示，是本发明一实施例提供的实现机器人位姿估计方法的电子设备的结构示意图。
所述电子设备1可以包括处理器10、存储器11、通信总线12以及通信接口13，还可以包括存储在所述存储器11中并可在所述处理器10上运行的计算机程序，如机器人位姿估计程序。
其中，所述处理器10在一些实施例中可以由集成电路组成，例如可以由单个封装的集成电路所组成，也可以是由多个相同功能或不同功能封装的集成电路所组成，包括一个或者多个中央处理器(Central Processing Unit，CPU)、微处理器、数字处理芯片、图形处理器及各种控制芯片的组合等。所述处理器10是所述电子设备的控制核心(ControlUnit)，利用各种接口和线路连接整个电子设备的各个部件，通过运行或执行存储在所述存储器11内的程序或者模块(例如执行机器人位姿估计程序等)，以及调用存储在所述存储器11内的数据，以执行电子设备的各种功能和处理数据。
所述存储器11至少包括一种类型的可读存储介质，所述可读存储介质包括闪存、移动硬盘、多媒体卡、卡型存储器(例如：SD或DX存储器等)、磁性存储器、磁盘、光盘等。所述存储器11在一些实施例中可以是电子设备的内部存储单元，例如该电子设备的移动硬盘。所述存储器11在另一些实施例中也可以是电子设备的外部存储设备，例如电子设备上配备的插接式移动硬盘、智能存储卡(Smart Media Card，SMC)、安全数字(Secure Digital，SD)卡、闪存卡(Flash Card)等。进一步地，所述存储器11还可以既包括电子设备的内部存储单元也包括外部存储设备。所述存储器11不仅可以用于存储安装于电子设备的应用软件及各类数据，例如机器人位姿估计程序的代码等，还可以用于暂时地存储已经输出或者将要输出的数据。
所述通信总线12可以是外设部件互连标准(Peripheral ComponentInterconnect，简称PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture，简称EISA)总线等。该总线可以分为地址总线、数据总线、控制总线等。所述总线被设置为实现所述存储器11以及至少一个处理器10等之间的连接通信。
所述通信接口13用于上述电子设备与其他设备之间的通信，包括网络接口和用户接口。可选地，所述网络接口可以包括有线接口和/或无线接口(如WI-FI接口、蓝牙接口等)，通常用于在该电子设备与其他电子设备之间建立通信连接。所述用户接口可以是显示器(Display)、输入单元(比如键盘(Keyboard))，可选地，用户接口还可以是标准的有线接口、无线接口。可选地，在一些实施例中，显示器可以是LED显示器、液晶显示器、触控式液晶显示器以及OLED(Organic Light-Emitting Diode，有机发光二极管)触摸器等。其中，显示器也可以适当的称为显示屏或显示单元，用于显示在电子设备中处理的信息以及用于显示可视化的用户界面。
图5仅示出了具有部件的电子设备，本领域技术人员可以理解的是，图5示出的结构并不构成对所述电子设备1的限定，可以包括比图示更少或者更多的部件，或者组合某些部件，或者不同的部件布置。
例如，尽管未示出，所述电子设备还可以包括给各个部件供电的电源(比如电池)，优选地，电源可以通过电源管理装置与所述至少一个处理器10逻辑相连，从而通过电源管理装置实现充电管理、放电管理、以及功耗管理等功能。电源还可以包括一个或一个以上的直流或交流电源、再充电装置、电源故障检测电路、电源转换器或者逆变器、电源状态指示器等任意组件。所述电子设备还可以包括多种传感器、蓝牙模块、Wi-Fi模块等，在此不再赘述。
应该了解，所述实施例仅为说明之用，在专利申请范围上并不受此结构的限制。
所述电子设备1中的所述存储器11存储的机器人位姿估计程序是多个指令的组合，在所述处理器10中运行时，可以实现：
获取机器人周围环境的二维图像，在预构建的图像坐标系中提取所述二维图像中的二维直线段并提取所述二维直线段的首尾端点坐标；
采集所述周围环境的三维点云，在预构建的世界坐标系中提取所述三维点云中的三维直线段；
计算所述三维直线段的法向量和方向向量，由所述法向量和方向向量将所述三维直线段映射为Plucker坐标系下的六维向量；
根据所述六维向量及所述二维直线段构造所述机器人的初始位姿参数矩阵；
由所述二维直线段及三维直线段构建二维-三维直线对，提取所述二维-三维直线对的匹配误差；根据所述二维-三维直线对、二维直线段的首尾端点、二维-三维直线对的匹配误差及初始位姿参数矩阵构造损失函数；
根据预设的损失值对所述损失函数中的初始位姿参数矩阵求解，得到所述机器人当前位姿参数矩阵，由所述机器人当前位姿参数矩阵拆解得到所述机器人的旋转矩阵和平移向量。
具体地，所述处理器10对上述指令的具体实现方法可参考附图对应实施例中相关步骤的描述，在此不赘述。
进一步地，所述电子设备1集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。所述计算机可读存储介质可以是易失性的，也可以是非易失性的。例如，所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)。
本发明还提供一种计算机可读存储介质，所述可读存储介质存储有计算机程序，所述计算机程序在被电子设备的处理器所执行时，可以实现：
获取机器人周围环境的二维图像，在预构建的图像坐标系中提取所述二维图像中的二维直线段并提取所述二维直线段的首尾端点坐标；
采集所述周围环境的三维点云，在预构建的世界坐标系中提取所述三维点云中的三维直线段；
计算所述三维直线段的法向量和方向向量，由所述法向量和方向向量将所述三维直线段映射为Plucker坐标系下的六维向量；
根据所述六维向量及所述二维直线段构造所述机器人的初始位姿参数矩阵；
由所述二维直线段及三维直线段构建二维-三维直线对，提取所述二维-三维直线对的匹配误差；根据所述二维-三维直线对、二维直线段的首尾端点、二维-三维直线对的匹配误差及初始位姿参数矩阵构造损失函数；
根据预设的损失值对所述损失函数中的初始位姿参数矩阵求解，得到所述机器人当前位姿参数矩阵，由所述机器人当前位姿参数矩阵拆解得到所述机器人的旋转矩阵和平移向量。
在本发明所提供的几个实施例中，应该理解到，所揭露的设备，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。
所述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能模块的形式实现。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。
因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明内。不应将权利要求中的任何附关联图标记视为限制所涉及的权利要求。
本发明所指区块链是分布式数据存储、点对点传输、共识机制、加密算法等计算机技术的新型应用模式。区块链(Blockchain)，本质上是一个去中心化的数据库，是一串使用密码学方法相关联产生的数据块，每一个数据块中包含了一批次网络交易的信息，用于验证其信息的有效性(防伪)和生成下一个区块。区块链可以包括区块链底层平台、平台产品服务层以及应用服务层等。
本申请实施例可以基于人工智能技术对相关的数据进行获取和处理。其中，人工智能(Artificial Intelligence，AI)是利用数字计算机或者数字计算机控制的机器模拟、延伸和扩展人的智能，感知环境、获取知识并使用知识获得最佳结果的理论、方法、技术及应用系统。
此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。系统权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一、第二等词语用来表示名称，而并不表示任何特定的顺序。
最后应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或等同替换，而不脱离本发明技术方案的精神和范围。

Claims (10)

1.一种机器人位姿估计方法，其特征在于，所述方法包括：
获取机器人周围环境的二维图像，在预构建的图像坐标系中提取所述二维图像中的二维直线段并提取所述二维直线段的首尾端点坐标；
采集所述周围环境的三维点云，在预构建的世界坐标系中提取所述三维点云中的三维直线段；
计算所述三维直线段的法向量和方向向量，由所述法向量和方向向量将所述三维直线段映射为Plucker坐标系下的六维向量；
根据所述六维向量及所述二维直线段构造所述机器人的初始位姿参数矩阵；
由所述二维直线段及三维直线段构建二维-三维直线对，提取所述二维-三维直线对的匹配误差，并根据所述二维-三维直线对、二维直线段的首尾端点、二维-三维直线对的匹配误差及初始位姿参数矩阵构造损失函数；
根据预设的损失值对所述损失函数中的初始位姿参数矩阵求解，得到所述机器人当前位姿参数矩阵，由所述机器人当前位姿参数矩阵拆解得到所述机器人的旋转矩阵和平移向量。
2.如权利要求1所述的机器人位姿估计方法，其特征在于，所述在预构建的图像坐标系中提取所述二维图像中的二维直线段，包括：
利用中值滤波算法去除所述二维图像中的图像噪声，得到去噪二维图像；
利用融合Canny算子和小波变换的边缘检测方法对所述去噪二维图像进行边缘检测，获得多组图像边缘直线段；
基于边缘像素拟合的直线提取算法提取所述二维图像边缘直线段的直线特征，得到所述二维直线段。
3.如权利要求1所述的机器人位姿估计方法，其特征在于，所述在预构建的世界坐标系中提取所述三维点云中的三维直线段，包括：
将所述三维点云沿不同方向投影到二维空间，生成多张具有深度信息的二维图像；
采用非真实感渲染技术将所述深度信息转化为所述二维图像的灰度值；
在所述二维图像中利用所述图像灰度值提取二维直线支持区，将所述二维直线支持区反投影到三维空间，得到直线特征点集；
将所述直线特征点集拟合成多个LSHP结构，将所述的多个LSHP结构进行合并及扩展，得到世界坐标系下的三维直线段。
4.如权利要求1所述的机器人位姿估计方法，其特征在于，所述构造的损失函数表达式为：
Figure FDA0003666878490000021
其中，n为所述二维-三维直线对的数量，d(li，Li)表示所述二维直线li和所述三维直线Li的匹配误差，
Figure FDA0003666878490000022
为所述二维直线段的首部端点，
Figure FDA0003666878490000023
为所述二维直线段的尾部端点，L为所述六维向量，T为所述初始位姿参数矩阵。
5.如权利要求4所述的机器人位姿估计方法，其特征在于，根据预设的损失值对所述损失函数中的初始位姿参数矩阵求解，得到所述机器人当前位姿参数矩阵，包括：
求出所述初始位姿参数矩阵中各个参数值ti的梯度
Figure FDA0003666878490000024
Figure FDA0003666878490000025
更新所述初始位姿参数矩阵中的各个参数值：
Figure FDA0003666878490000026
其中，β为学习率；
根据n对二维-三维直线对计算出所述损失函数的损失值；
当所述损失值大于或者等于预设的损失值阈值时，返回上述获取机器人周围环境的二维图像的步骤；
当所述损失值小于所述预设的损失值阈值时，根据更新后参数值t′i得到所述机器人当前位姿参数矩阵。
6.如权利要求1所述的机器人位姿估计方法，其特征在于，所述计算所述三维直线段的法向量和方向向量，包括：
利用下述公式计算得到所述三维直线段的法向量uT和方向向量vT：
uT＝(L1 L2 L3)＝(ax ay az)×(bx by bz)，
vT＝(L4 L5 L6)＝bw(ax ay az)-aw(bx by bz)，
其中，L1为所述法向量uT在x轴的投影坐标，L2为所述法向量uT在y轴的投影坐标，L3为所述法向量uT在z轴的投影坐标，L4为所述方向向量vT在x轴的投影坐标，L5为所述方向向量vT在y轴的投影坐标，L6为所述方向向量vT在z轴的投影坐标，ax和bx分别为所述三维直线段中任意两点的x轴坐标，ay和by为分别为所述三维直线段中任意两点的y轴坐标，az和bz分别为所述三维直线段中任意两点的z轴坐标，aw和bw为常数。
7.如权利要求1所述的机器人位姿估计方法，其特征在于，所述由所述二维直线段及三维直线段构建二维-三维直线对，包括：
对所述二维直线和三维直线进行SURF特征提取，并对提取的特征进行特征匹配，获得特征点对；
对所述特征点对进行匹配筛选，获取相匹配的二维-三维特征点对。
8.一种机器人位姿估计装置，其特征在于，所述装置包括：
信息获取模块，用于获取机器人周围环境的二维图像，在预构建的图像坐标系中提取所述二维图像中的二维直线段并提取所述二维直线段的首尾端点坐标；采集所述周围环境的三维点云，在预构建的世界坐标系中提取所述三维点云中的三维直线段；
信息处理模块，用于计算所述三维直线段的法向量和方向向量，由所述法向量和方向向量将所述三维直线段映射为Plucker坐标系下的六维向量；根据所述六维向量及所述二维直线段构造所述机器人的初始位姿参数矩阵；
损失函数构建模块，用于由所述二维直线段及三维直线段构建二维-三维直线对，提取所述二维-三维直线对的匹配误差，根据所述二维-三维直线对、二维直线段的首尾端点、二维-三维直线对的匹配误差及初始位姿参数矩阵构造损失函数；
位姿求解模块，用于根据预设的损失值所述损失函数中的初始位姿参数矩阵求解，得到所述机器人当前位姿参数矩阵，由所述机器人当前位姿参数矩阵拆解得到所述机器人的旋转矩阵和平移向量。
9.一种电子设备，其特征在于，所述电子设备包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的计算机程序，所述计算机程序被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1至7中任意一项所述的机器人位姿估计方法。
10.一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7中任意一项所述的机器人位姿估计方法。
CN202210593947.2A 2022-05-27 2022-05-27 机器人位姿估计方法、装置、电子设备及存储介质 Pending CN114789446A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210593947.2A CN114789446A (zh) 2022-05-27 2022-05-27 机器人位姿估计方法、装置、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210593947.2A CN114789446A (zh) 2022-05-27 2022-05-27 机器人位姿估计方法、装置、电子设备及存储介质

Publications (1)

Publication Number Publication Date
CN114789446A true CN114789446A (zh) 2022-07-26

Family

ID=82463742

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210593947.2A Pending CN114789446A (zh) 2022-05-27 2022-05-27 机器人位姿估计方法、装置、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN114789446A (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103942824A (zh) * 2014-05-15 2014-07-23 厦门大学 一种三维点云直线特征提取方法
CN106909877A (zh) * 2016-12-13 2017-06-30 浙江大学 一种基于点线综合特征的视觉同时建图与定位方法
CN109117757A (zh) * 2018-07-27 2019-01-01 四川大学 一种提取航拍图像中拉索的方法
CN109544577A (zh) * 2018-11-27 2019-03-29 辽宁工程技术大学 一种基于边缘点编组的改进直线提取方法
EP3680809A1 (en) * 2017-09-22 2020-07-15 Huawei Technologies Co., Ltd. Visual slam method and apparatus based on point and line characteristic
WO2021063127A1 (zh) * 2019-09-30 2021-04-08 深圳市瑞立视多媒体科技有限公司 多相机环境中主动式刚体的位姿定位方法及相关设备

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103942824A (zh) * 2014-05-15 2014-07-23 厦门大学 一种三维点云直线特征提取方法
CN106909877A (zh) * 2016-12-13 2017-06-30 浙江大学 一种基于点线综合特征的视觉同时建图与定位方法
EP3680809A1 (en) * 2017-09-22 2020-07-15 Huawei Technologies Co., Ltd. Visual slam method and apparatus based on point and line characteristic
CN109117757A (zh) * 2018-07-27 2019-01-01 四川大学 一种提取航拍图像中拉索的方法
CN109544577A (zh) * 2018-11-27 2019-03-29 辽宁工程技术大学 一种基于边缘点编组的改进直线提取方法
WO2021063127A1 (zh) * 2019-09-30 2021-04-08 深圳市瑞立视多媒体科技有限公司 多相机环境中主动式刚体的位姿定位方法及相关设备

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
李劲澎: "基于全局式运动恢复结构的无人机影像位姿估计关键技术研究", 《中国博士学位论文全文数据库》 *

Similar Documents

Publication Publication Date Title
Rambach et al. 6DoF object tracking based on 3D scans for augmented reality remote live support
CN112446919A (zh) 物体位姿估计方法、装置、电子设备及计算机存储介质
WO2014186970A1 (en) Systems and methods for generating three-dimensional models using sensed position data
CN111459269B (zh) 一种增强现实显示方法、系统及计算机可读存储介质
CN116229007B (zh) 利用bim建模的四维数字图像构建方法、装置、设备及介质
US20150066439A1 (en) Creating three dimensional models with acceleration data
CN110503718B (zh) 三维工程模型轻量化显示方法
CN111932534A (zh) 医学影像图片分析方法、装置、电子设备及可读存储介质
CN113034406A (zh) 扭曲文档恢复方法、装置、设备及介质
CN114049568A (zh) 基于图像比对的标的物形变检测方法、装置、设备及介质
CN112862703A (zh) 基于移动拍照的图像校正方法、装置、电子设备及介质
CN115294426B (zh) 介入医学设备的跟踪方法、装置、设备及存储介质
CN114627438A (zh) 目标检测模型生成方法、目标检测方法、设备及介质
CN113627394B (zh) 人脸提取方法、装置、电子设备及可读存储介质
CN114789446A (zh) 机器人位姿估计方法、装置、电子设备及存储介质
CN114882059A (zh) 基于图像分析的尺寸测量方法、装置、设备及存储介质
CN110782390A (zh) 图像的矫正处理方法及装置、电子设备
CN115880448A (zh) 基于双目成像的三维测量方法、装置、设备及存储介质
CN115588082A (zh) 空间数字模型实时展示方法、装置、设备及存储介质
CN115423852A (zh) 一种点云数据配准方法、系统、电子设备及存储介质
CN115393423A (zh) 目标检测方法和装置
CN113920269A (zh) 工程进度获取方法、装置、电子设备及介质
CN113887384A (zh) 基于多轨迹融合的行人轨迹分析方法、装置、设备及介质
Rodriguez et al. 3D pose estimation oriented to the initialization of an augmented reality system applied to cultural heritage
CN113255456A (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