CN111319041A - 一种机器人位姿确定方法、装置、可读存储介质及机器人 - Google Patents

一种机器人位姿确定方法、装置、可读存储介质及机器人 Download PDF

Info

Publication number
CN111319041A
CN111319041A CN202010054418.6A CN202010054418A CN111319041A CN 111319041 A CN111319041 A CN 111319041A CN 202010054418 A CN202010054418 A CN 202010054418A CN 111319041 A CN111319041 A CN 111319041A
Authority
CN
China
Prior art keywords
robot
pose
camera
coordinate system
conversion matrix
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
Application number
CN202010054418.6A
Other languages
English (en)
Other versions
CN111319041B (zh
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.)
Ubtech Robotics Corp
Original Assignee
Ubtech Robotics Corp
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 Ubtech Robotics Corp filed Critical Ubtech Robotics Corp
Priority to CN202010054418.6A priority Critical patent/CN111319041B/zh
Publication of CN111319041A publication Critical patent/CN111319041A/zh
Application granted granted Critical
Publication of CN111319041B publication Critical patent/CN111319041B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/1612—Programme controls characterised by the hand, wrist, grip control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • 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
    • 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/1628—Programme controls characterised by the control loop
    • B25J9/1653—Programme controls characterised by the control loop parameters identification, estimation, stiffness, accuracy, error analysis
    • 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/1679—Programme controls characterised by the tasks executed
    • B25J9/1692—Calibration of manipulator
    • 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/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems

Abstract

本申请属于机器人技术领域，尤其涉及一种机器人位姿确定方法、装置、计算机可读存储介质及机器人。所述方法获取当前时刻的相机位姿，所述相机位姿为机器人的相机在视觉SLAM坐标系下的位姿；根据所述当前时刻的相机位姿计算原始的机器人位姿，所述机器人位姿为所述机器人在里程计坐标系下的位姿；将所述原始的机器人位姿投影至所述机器人的行进路径上，得到校正的机器人位姿。通过本申请，可以实时计算得到机器人在任一时刻的位姿，从而得到连续的位姿信息，使得腿式机器人也可以适用于常见的ROS局部路径规划算法。

Description

一种机器人位姿确定方法、装置、可读存储介质及机器人
技术领域
本申请属于机器人技术领域，尤其涉及一种机器人位姿确定方法、装置、计算机可读存储介质及机器人。
背景技术
腿式机器人的自主导航是其实现智能化服务的关键支撑技术，而有效的自主导航需要建立在稳定可靠定位的基础上，腿式机器人与轮式机器人在运动过程中的一个明显的区别是，轮式机器人是比较平稳顺滑的移动，而腿式机器人在行走中有比较显著的左右晃动过程，且仅在某一足落地时才更新机器人位姿，从而导致得到的位姿是离散的、非实时的，无法适用常见的ROS局部路径规划算法。
发明内容
有鉴于此，本申请实施例提供了一种机器人位姿确定方法、装置、计算机可读存储介质及机器人，以解决现有的腿式机器人的位姿确定方法得到的位姿是离散的、非实时的，无法适用常见的ROS局部路径规划算法的问题。
本申请实施例的第一方面提供了一种机器人位姿确定方法，可以包括：
获取当前时刻的相机位姿，所述相机位姿为机器人的相机在视觉SLAM坐标系下的位姿；
根据所述当前时刻的相机位姿计算原始的机器人位姿，所述机器人位姿为所述机器人在里程计坐标系下的位姿；
将所述原始的机器人位姿投影至所述机器人的行进路径上，得到校正的机器人位姿。
进一步地，所述根据所述当前时刻的相机位姿计算原始的机器人位姿包括：
获取所述当前时刻之前的各组历史位姿数据，其中，每组历史位姿数据均包括同一时刻的相机位姿和机器人位姿；
根据所述各组历史位姿数据计算第一转换矩阵，所述第一转换矩阵为从所述视觉SLAM坐标系至所述里程计坐标系的转换矩阵；
将所述当前时刻的相机位姿变换为第二转换矩阵，所述第二转换矩阵为从相机坐标系至所述视觉SLAM坐标系的转换矩阵；
根据所述第一转换矩阵、所述第二转换矩阵和预设的第三转换矩阵计算第四转换矩阵，所述第三转换矩阵为从机器人坐标系至所述相机坐标系的转换矩阵，所述第四转换矩阵为从所述机器人坐标系至所述里程计坐标系的转换矩阵；
将所述第四转换矩阵变换为所述原始的机器人位姿。
进一步地，在将所述原始的机器人位姿投影至所述机器人的行进路径上之前，还包括：
获取所述机器人在第一落脚时刻的第一中心位置和速度控制指令，所述第一落脚时刻为所述当前时刻之前的第一个落脚时刻；
根据所述第一中心位置和所述速度控制指令确定所述机器人在第二落脚时刻的第二中心位置，所述第二落脚时刻为所述当前时刻之后的第一个落脚时刻；
将由所述第一中心位置至所述第二中心位置的路径确定为所述机器人的行进路径。
进一步地，所述根据所述第一中心位置和所述速度控制指令确定所述机器人在第二落脚时刻的第二中心位置包括：
若所述速度控制指令中的速度为0，且角速度不为0时，则根据预设的原地旋转模型确定所述第二中心位置。
进一步地，所述根据所述第一中心位置和所述速度控制指令确定所述机器人在第二落脚时刻的第二中心位置包括：
若所述速度控制指令中的速度不为0，且角速度为0时，则根据预设的直线运动模型确定所述第二中心位置。
进一步地，所述根据所述第一中心位置和所述速度控制指令确定所述机器人在第二落脚时刻的第二中心位置包括：
若所述速度控制指令中的速度不为0，且角速度不为0时，则根据预设的曲线运动模型确定所述第二中心位置。
进一步地，所述将所述原始的机器人位姿投影至所述机器人的行进路径上，得到校正的机器人位姿包括：
将所述原始的机器人位姿投影至所述机器人的行进路径上，得到投影位置点；
将所述投影位置点和所述原始的机器人位姿中的姿态角组成所述校正的机器人位姿。
本申请实施例的第二方面提供了一种机器人位姿确定装置，可以包括：
相机位姿获取模块，用于获取当前时刻的相机位姿，所述相机位姿为机器人的相机在视觉SLAM坐标系下的位姿；
第一计算模块，用于根据所述当前时刻的相机位姿计算原始的机器人位姿，所述机器人位姿为所述机器人在里程计坐标系下的位姿；
第二计算模块，用于将所述原始的机器人位姿投影至所述机器人的行进路径上，得到校正的机器人位姿。
进一步地，所述第一计算模块可以包括：
历史位姿数据获取单元，用于获取所述当前时刻之前的各组历史位姿数据，其中，每组历史位姿数据均包括同一时刻的相机位姿和机器人位姿；
第一转换矩阵计算单元，用于根据所述各组历史位姿数据计算第一转换矩阵，所述第一转换矩阵为从所述视觉SLAM坐标系至所述里程计坐标系的转换矩阵；
第二转换矩阵计算单元，用于将所述当前时刻的相机位姿变换为第二转换矩阵，所述第二转换矩阵为从相机坐标系至所述视觉SLAM坐标系的转换矩阵；
第四转换矩阵计算单元，用于根据所述第一转换矩阵、所述第二转换矩阵和预设的第三转换矩阵计算第四转换矩阵，所述第三转换矩阵为从机器人坐标系至所述相机坐标系的转换矩阵，所述第四转换矩阵为从所述机器人坐标系至所述里程计坐标系的转换矩阵；
位姿变换单元，用于将所述第四转换矩阵变换为所述原始的机器人位姿。
进一步地，所述机器人位姿确定装置还可以包括：
信息获取模块，用于获取所述机器人在第一落脚时刻的第一中心位置和速度控制指令，所述第一落脚时刻为所述当前时刻之前的第一个落脚时刻；
位置确定模块，用于根据所述第一中心位置和所述速度控制指令确定所述机器人在第二落脚时刻的第二中心位置，所述第二落脚时刻为所述当前时刻之后的第一个落脚时刻；
行进路径确定模块，用于将由所述第一中心位置至所述第二中心位置的路径确定为所述机器人的行进路径。
进一步，所述位置确定模块可以包括：
第一确定单元，用于若所述速度控制指令中的速度为0，且角速度不为0时，则根据预设的原地旋转模型确定所述第二中心位置。
第二确定单元，用于若所述速度控制指令中的速度不为0，且角速度为0时，则根据预设的直线运动模型确定所述第二中心位置。
第三确定单元，用于若所述速度控制指令中的速度不为0，且角速度不为0时，则根据预设的曲线运动模型确定所述第二中心位置。
进一步地，所述第二计算模块可以包括：
投影单元，用于将所述原始的机器人位姿投影至所述机器人的行进路径上，得到投影位置点；
位姿确定单元，用于将所述投影位置点和所述原始的机器人位姿中的姿态角组成所述校正的机器人位姿。
本申请实施例的第三方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述任一种机器人位姿确定方法的步骤。
本申请实施例的第四方面提供了一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述任一种机器人位姿确定方法的步骤。
本申请实施例的第五方面提供了一种计算机程序产品，当计算机程序产品在机器人上运行时，使得机器人执行上述任一种机器人位姿确定方法的步骤。
本申请实施例与现有技术相比存在的有益效果是：本申请实施例获取当前时刻的相机位姿，所述相机位姿为机器人的相机在视觉SLAM坐标系下的位姿；根据所述当前时刻的相机位姿计算原始的机器人位姿，所述机器人位姿为所述机器人在里程计坐标系下的位姿；将所述原始的机器人位姿投影至所述机器人的行进路径上，得到校正的机器人位姿。通过本申请实施例，可以实时计算得到机器人在任一时刻的位姿，从而得到连续的位姿信息，使得腿式机器人也可以适用于常见的ROS局部路径规划算法。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。
图1为本申请实施例中一种机器人位姿确定方法的一个实施例流程图；
图2为根据当前时刻的相机位姿计算原始的机器人位姿的示意流程图；
图3为机器人行走轨迹的示意图；
图4为机器人的行进路径的确定过程的示意流程图；
图5为曲线运动模型的示意图；
图6为本申请实施例中一种机器人位姿确定装置的一个实施例结构图；
图7为本申请实施例中一种机器人的示意框图。
具体实施方式
为使得本申请的发明目的、特征、优点能够更加的明显和易懂，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本申请一部分实施例，而非全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本申请保护的范围。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在此本申请说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本申请。如在本申请说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
还应当进一步理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
如在本说明书和所附权利要求书中所使用的那样，术语“如果”可以依据上下文被解释为“当...时”或“一旦”或“响应于确定”或“响应于检测到”。类似地，短语“如果确定”或“如果检测到[所描述条件或事件]”可以依据上下文被解释为意指“一旦确定”或“响应于确定”或“一旦检测到[所描述条件或事件]”或“响应于检测到[所描述条件或事件]”。
另外，在本申请的描述中，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
请参阅图1，本申请实施例中一种机器人位姿确定方法的一个实施例可以包括：
步骤S101、获取当前时刻的相机位姿。
所述相机位姿为机器人的相机在视觉SLAM坐标系下的位姿，此处将所述视觉SLAM坐标系记为{Visual}，将所述相机位姿记为VPosecamera。如无特殊说明，本申请实施例中所提及的机器人均指腿式机器人。需要注意的是，机器人的里程计所提供的运动状态是非实时的，而视觉SLAM可以提供实时、高频的相机位姿，本申请实施例正是以这一物理量为依据，对机器人在任一时刻的位姿进行实时计算。
步骤S102、根据所述当前时刻的相机位姿计算原始的机器人位姿。
所述机器人位姿为所述机器人在里程计坐标系下的位姿，此处将所述里程计坐标系记为{Odom}，将所述机器人位姿记为OPoserobot。
如图2所示，步骤S102具体可以包括如下步骤：
步骤S1021、获取所述当前时刻之前的各组历史位姿数据。
其中，每组历史位姿数据均包括同一时刻的相机位姿和机器人位姿，此处将其记为{VPosecamera，OPoserobot}。
图3所示为机器人行走轨迹的示意图，P1为机器人右脚第一次落地时(将此时刻记为T1)的机器人位姿，P2为机器人左脚第一次落地时(将此时刻记为T2)的机器人位姿，P3为机器人右脚第二次落地时(将此时刻记为T3)的机器人位姿，P4为机器人左脚第二次落地时(将此时刻记为T4)的机器人位姿，……，以此类推。每一次机器人脚落地时均对应一组历史位姿数据，也即机器人脚落地时的相机位姿和机器人位姿。
如图3所示，机器人行走时，左右摇摆是均匀的，可以认为其为绕机器人中轴线的周期性振动，类似于倒钟摆，机器人的中心即为左右两个离散位姿Pk-1和Pk的中点M[k-1][k]，如图3所示的实线位置，也即导航使用时希望获得的机器人实时位置。
步骤S1022、根据所述各组历史位姿数据计算第一转换矩阵。
所述第一转换矩阵为从所述视觉SLAM坐标系至所述里程计坐标系的转换矩阵，此处将其记为
Figure BDA0002372313300000081
在本申请实施例中可以通过对各组历史位姿数据的校正过程，确定{Visual}与{Odom}的关系
Figure BDA0002372313300000082
具体地，首先使用各组历史位姿数据{VPosecamera，OPoserobot}计算出各组历史中点位姿数据{VMcamera，OMrobot}，其中，VMcamera为在中点处，所述相机在视觉SLAM坐标系下的位姿，OMrobot为在中点处，所述机器人在里程计坐标系下的位姿。仍以图3为例，将T1时的历史位姿数据和T2时的历史位姿数据求均值，即可得到P1和P2的中点M12所对应的一组历史中点位姿数据，将T2时的历史位姿数据和T3时的历史位姿数据求均值，即可得到P2和P3的中点M23所对应的一组历史中点位姿数据，将T3时的历史位姿数据和T4时的历史位姿数据求均值，即可得到P3和P4的中点M34所对应的一组历史中点位姿数据，……，以此类推。
由于一般情况下，相机和机器人之间的相对关系是固定的，因此可以将相机在视觉SLAM坐标系下的位姿VMcamera转换为机器人在视觉SLAM坐标系下的位姿VMrobot，从而将各组历史中点位姿数据{VMcamera，OMrobot}转换为{VMrobot，OMrobot}。最后，使用图优化方法(包括但不限于g2o和ceres等图优化方法)对各组{VMrobot，OMrobot}进行数据处理，从而得到
Figure BDA0002372313300000083
步骤S1023、将所述当前时刻的相机位姿变换为第二转换矩阵。
所述第二转换矩阵为从相机坐标系至所述视觉SLAM坐标系的转换矩阵，此处将其记为
Figure BDA0002372313300000084
需要注意的是，VPosecamera和
Figure BDA0002372313300000085
是同一物理量的两种不同表示形式，前者是用位姿的形式进行表示，后者是用矩阵的形式进行表示，两者可以进行相互变换，因此，在已知所述当前时刻的相机位姿的情况下，可以将其变换为所述第二转换矩阵。
步骤S1024、根据所述第一转换矩阵、所述第二转换矩阵和预设的第三转换矩阵计算第四转换矩阵。
所述第三转换矩阵为从机器人坐标系至所述相机坐标系的转换矩阵，此处将其记为
Figure BDA0002372313300000091
由于相机和机器人之间的相对关系是固定的，因此所述第三转换矩阵可以作为一个已知的常量进行使用。
所述第四转换矩阵为从所述机器人坐标系至所述里程计坐标系的转换矩阵，此处将其记为
Figure BDA0002372313300000092
可以根据下式计算得到：
Figure BDA0002372313300000093
步骤S1025、将所述第四转换矩阵变换为所述原始的机器人位姿。
此处将所述原始的机器人位姿记为OPose'robot，需要注意的是，OPose'robot和
Figure BDA0002372313300000094
是同一物理量的两种不同表示形式，前者是用位姿的形式进行表示，后者是用矩阵的形式进行表示，两者可以进行相互变换，因此，在已知所述第四转换矩阵的情况下，可以将其变换为所述原始的机器人位姿。
步骤S103、将所述原始的机器人位姿投影至所述机器人的行进路径上，得到校正的机器人位姿。
如图4所示，所述机器人的行进路径可以预先根据如下的步骤进行具体设置：
步骤S1031、获取所述机器人在第一落脚时刻的第一中心位置和速度控制指令。
所述第一落脚时刻为所述当前时刻之前的第一个落脚时刻，此处将其记为时刻k。仍以图3为例，若所述第一个落脚时刻为T1，则对应的第一中心位置即为P1中的位置，若所述第一个落脚时刻为T2，则对应的第一中心位置即为P2中的位置，……，以此类推，此处将所述第一中心位置记为OMk-1,k。
此处将所述速度控制指令记为VelCommand(VelX，VelAng)，其中，VelX为机器人在x方向(即机器人的行进方向)的速度，且VelX≥0，VelAng为机器人的角速度，机器人的迈步控制时间周期是
Figure BDA0002372313300000095
需要注意的是，视觉SLAM可以提供实时、高频的相机位姿VPosecamera，机器人里程计提供的机器人位姿OPoserobot在时间维度不是连续的，仅当某一足落地时发布一次，软件编程可得到同时刻的VPosecamera、OPoserobot和VelCommand。因为当前腿式机器人抬脚之后不会再接收新的速度控制指令，所以有效的速度控制指令为某一足落地至抬起之前的时间段内。
步骤S1032、根据所述第一中心位置和所述速度控制指令确定所述机器人在第二落脚时刻的第二中心位置。
所述第二落脚时刻为所述当前时刻之后的第一个落脚时刻，此处将其记为时刻k+1。
在本申请实施例中，可以根据所述速度控制指令选择对应的运动模型，运动模型有原地旋转、直线运动和曲线运动模型三种。确定运动模型后，机器人每一次迈步的时间是固定的，所以可以根据所述第一中心位置和所述速度控制指令预测出机器人下一次落脚时机器人的位置，也即所述第二中心位置，此处将所述第二中心位置记为OMk,k+1。需要注意的是，所述第一中心位置和所述第二中心位置都仅仅是位置，没有姿态朝向。
所述第二中心位置具体的计算可以包括以下三种情况：
情况一，若所述速度控制指令中的速度为0，且角速度不为0(即VelX＝0，VelAng≠0)时，则根据预设的原地旋转模型确定所述第二中心位置。
所述原地旋转模型为：
OMk,k+1＝OMk-1,k
从中可以看出，在第一种情况下，所述第二中心位置等于所述第一中心位置。
情况二，若所述速度控制指令中的速度不为0，且角速度为0(即VelX≠0，VelAng＝0)时，则根据预设的直线运动模型确定所述第二中心位置。
所述直线运动模型为：
Figure BDA0002372313300000101
Figure BDA0002372313300000102
其中，
Figure BDA0002372313300000111
为在时刻k的所述第四转换矩阵。
情况三，若所述速度控制指令中的速度不为0，且角速度不为0(即VelX≠0，VelAng≠0)时，则根据预设的曲线运动模型确定所述第二中心位置。
如图5所示，所述曲线运动模型为：
Figure BDA0002372313300000112
R＝VelX/VelAng
φ＝(π-|θ|)/2
Figure BDA0002372313300000113
Figure BDA0002372313300000114
步骤S1033、将由所述第一中心位置至所述第二中心位置的路径确定为所述机器人的行进路径。
在确定出所述机器人的行进路径之后，即可将所述原始的机器人位姿投影至所述机器人的行进路径上，得到投影位置点，然后将所述投影位置点和所述原始的机器人位姿中的姿态角组成所述校正的机器人位姿。
综上所述，本申请实施例本申请实施例获取当前时刻的相机位姿，所述相机位姿为机器人的相机在视觉SLAM坐标系下的位姿；根据所述当前时刻的相机位姿计算原始的机器人位姿，所述机器人位姿为所述机器人在里程计坐标系下的位姿；将所述原始的机器人位姿投影至所述机器人的行进路径上，得到校正的机器人位姿。通过本申请实施例，可以实时计算得到机器人在任一时刻的位姿，从而得到连续的位姿信息，使得腿式机器人也可以适用于常见的ROS局部路径规划算法。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。
对应于上文实施例所述的一种机器人位姿确定方法，图6示出了本申请实施例提供的一种机器人位姿确定装置的一个实施例结构图。
本实施例中，一种机器人位姿确定装置可以包括：
相机位姿获取模块601，用于获取当前时刻的相机位姿，所述相机位姿为机器人的相机在视觉SLAM坐标系下的位姿；
第一计算模块602，用于根据所述当前时刻的相机位姿计算原始的机器人位姿，所述机器人位姿为所述机器人在里程计坐标系下的位姿；
第二计算模块603，用于将所述原始的机器人位姿投影至所述机器人的行进路径上，得到校正的机器人位姿。
进一步地，所述第一计算模块可以包括：
历史位姿数据获取单元，用于获取所述当前时刻之前的各组历史位姿数据，其中，每组历史位姿数据均包括同一时刻的相机位姿和机器人位姿；
第一转换矩阵计算单元，用于根据所述各组历史位姿数据计算第一转换矩阵，所述第一转换矩阵为从所述视觉SLAM坐标系至所述里程计坐标系的转换矩阵；
第二转换矩阵计算单元，用于将所述当前时刻的相机位姿变换为第二转换矩阵，所述第二转换矩阵为从相机坐标系至所述视觉SLAM坐标系的转换矩阵；
第四转换矩阵计算单元，用于根据所述第一转换矩阵、所述第二转换矩阵和预设的第三转换矩阵计算第四转换矩阵，所述第三转换矩阵为从机器人坐标系至所述相机坐标系的转换矩阵，所述第四转换矩阵为从所述机器人坐标系至所述里程计坐标系的转换矩阵；
位姿变换单元，用于将所述第四转换矩阵变换为所述原始的机器人位姿。
进一步地，所述机器人位姿确定装置还可以包括：
信息获取模块，用于获取所述机器人在第一落脚时刻的第一中心位置和速度控制指令，所述第一落脚时刻为所述当前时刻之前的第一个落脚时刻；
位置确定模块，用于根据所述第一中心位置和所述速度控制指令确定所述机器人在第二落脚时刻的第二中心位置，所述第二落脚时刻为所述当前时刻之后的第一个落脚时刻；
行进路径确定模块，用于将由所述第一中心位置至所述第二中心位置的路径确定为所述机器人的行进路径。
进一步，所述位置确定模块可以包括：
第一确定单元，用于若所述速度控制指令中的速度为0，且角速度不为0时，则根据预设的原地旋转模型确定所述第二中心位置。
第二确定单元，用于若所述速度控制指令中的速度不为0，且角速度为0时，则根据预设的直线运动模型确定所述第二中心位置。
第三确定单元，用于若所述速度控制指令中的速度不为0，且角速度不为0时，则根据预设的曲线运动模型确定所述第二中心位置。
进一步地，所述第二计算模块可以包括：
投影单元，用于将所述原始的机器人位姿投影至所述机器人的行进路径上，得到投影位置点；
位姿确定单元，用于将所述投影位置点和所述原始的机器人位姿中的姿态角组成所述校正的机器人位姿。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的装置，模块和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
图7示出了本申请实施例提供的一种机器人的示意框图，为了便于说明，仅示出了与本申请实施例相关的部分。
如图7所示，该实施例的机器人7包括：处理器70、存储器71以及存储在所述存储器71中并可在所述处理器70上运行的计算机程序72。所述处理器70执行所述计算机程序72时实现上述各个机器人位姿确定方法实施例中的步骤，例如图1所示的步骤S101至步骤S103。或者，所述处理器70执行所述计算机程序72时实现上述各装置实施例中各模块/单元的功能，例如图4所示模块601至模块603的功能。
示例性的，所述计算机程序72可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器71中，并由所述处理器70执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序72在所述机器人7中的执行过程。
本领域技术人员可以理解，图7仅仅是机器人7的示例，并不构成对机器人7的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述机器人7还可以包括输入输出设备、网络接入设备、总线等。
所述处理器70可以是中央处理单元(Central Processing Unit，CPU)，还可以是其它通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器71可以是所述机器人7的内部存储单元，例如机器人7的硬盘或内存。所述存储器71也可以是所述机器人7的外部存储设备，例如所述机器人7上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器71还可以既包括所述机器人7的内部存储单元也包括外部存储设备。所述存储器71用于存储所述计算机程序以及所述机器人7所需的其它程序和数据。所述存储器71还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中，应该理解到，所揭露的装置/机器人和方法，可以通过其它的方式实现。例如，以上所描述的装置/机器人实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。

Claims (10)

1.一种机器人位姿确定方法，其特征在于，包括：
获取当前时刻的相机位姿，所述相机位姿为机器人的相机在视觉SLAM坐标系下的位姿；
根据所述当前时刻的相机位姿计算原始的机器人位姿，所述机器人位姿为所述机器人在里程计坐标系下的位姿；
将所述原始的机器人位姿投影至所述机器人的行进路径上，得到校正的机器人位姿。
2.根据权利要求1所述的机器人位姿确定方法，其特征在于，所述根据所述当前时刻的相机位姿计算原始的机器人位姿包括：
获取所述当前时刻之前的各组历史位姿数据，其中，每组历史位姿数据均包括同一时刻的相机位姿和机器人位姿；
根据所述各组历史位姿数据计算第一转换矩阵，所述第一转换矩阵为从所述视觉SLAM坐标系至所述里程计坐标系的转换矩阵；
将所述当前时刻的相机位姿变换为第二转换矩阵，所述第二转换矩阵为从相机坐标系至所述视觉SLAM坐标系的转换矩阵；
根据所述第一转换矩阵、所述第二转换矩阵和预设的第三转换矩阵计算第四转换矩阵，所述第三转换矩阵为从机器人坐标系至所述相机坐标系的转换矩阵，所述第四转换矩阵为从所述机器人坐标系至所述里程计坐标系的转换矩阵；
将所述第四转换矩阵变换为所述原始的机器人位姿。
3.根据权利要求1所述的机器人位姿确定方法，其特征在于，在将所述原始的机器人位姿投影至所述机器人的行进路径上之前，还包括：
获取所述机器人在第一落脚时刻的第一中心位置和速度控制指令，所述第一落脚时刻为所述当前时刻之前的第一个落脚时刻；
根据所述第一中心位置和所述速度控制指令确定所述机器人在第二落脚时刻的第二中心位置，所述第二落脚时刻为所述当前时刻之后的第一个落脚时刻；
将由所述第一中心位置至所述第二中心位置的路径确定为所述机器人的行进路径。
4.根据权利要求3所述的机器人位姿确定方法，其特征在于，所述根据所述第一中心位置和所述速度控制指令确定所述机器人在第二落脚时刻的第二中心位置包括：
若所述速度控制指令中的速度为0，且角速度不为0时，则根据预设的原地旋转模型确定所述第二中心位置。
5.根据权利要求3所述的机器人位姿确定方法，其特征在于，所述根据所述第一中心位置和所述速度控制指令确定所述机器人在第二落脚时刻的第二中心位置包括：
若所述速度控制指令中的速度不为0，且角速度为0时，则根据预设的直线运动模型确定所述第二中心位置。
6.根据权利要求3所述的机器人位姿确定方法，其特征在于，所述根据所述第一中心位置和所述速度控制指令确定所述机器人在第二落脚时刻的第二中心位置包括：
若所述速度控制指令中的速度不为0，且角速度不为0时，则根据预设的曲线运动模型确定所述第二中心位置。
7.根据权利要求1至6中任一项所述的机器人位姿确定方法，其特征在于，所述将所述原始的机器人位姿投影至所述机器人的行进路径上，得到校正的机器人位姿包括：
将所述原始的机器人位姿投影至所述机器人的行进路径上，得到投影位置点；
将所述投影位置点和所述原始的机器人位姿中的姿态角组成所述校正的机器人位姿。
8.一种机器人位姿确定装置，其特征在于，包括：
相机位姿获取模块，用于获取当前时刻的相机位姿，所述相机位姿为机器人的相机在视觉SLAM坐标系下的位姿；
第一计算模块，用于根据所述当前时刻的相机位姿计算原始的机器人位姿，所述机器人位姿为所述机器人在里程计坐标系下的位姿；
第二计算模块，用于将所述原始的机器人位姿投影至所述机器人的行进路径上，得到校正的机器人位姿。
9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7中任一项所述的机器人位姿确定方法的步骤。
10.一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7中任一项所述的机器人位姿确定方法的步骤。
CN202010054418.6A 2020-01-17 2020-01-17 一种机器人位姿确定方法、装置、可读存储介质及机器人 Active CN111319041B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010054418.6A CN111319041B (zh) 2020-01-17 2020-01-17 一种机器人位姿确定方法、装置、可读存储介质及机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010054418.6A CN111319041B (zh) 2020-01-17 2020-01-17 一种机器人位姿确定方法、装置、可读存储介质及机器人

Publications (2)

Publication Number Publication Date
CN111319041A true CN111319041A (zh) 2020-06-23
CN111319041B CN111319041B (zh) 2021-04-16

Family

ID=71168648

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010054418.6A Active CN111319041B (zh) 2020-01-17 2020-01-17 一种机器人位姿确定方法、装置、可读存储介质及机器人

Country Status (1)

Country Link
CN (1) CN111319041B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112506190A (zh) * 2020-11-19 2021-03-16 深圳市优必选科技股份有限公司 一种机器人定位方法、机器人定位装置及机器人
CN113568439A (zh) * 2021-07-29 2021-10-29 西门子（中国）有限公司 调姿机的控制方法、装置、控制器和计算机存储介质
WO2022088613A1 (zh) * 2020-10-26 2022-05-05 北京市商汤科技开发有限公司 一种机器人的定位方法及装置、设备、存储介质
CN115533922A (zh) * 2022-11-29 2022-12-30 北京航空航天大学杭州创新研究院 位姿关系标定方法及装置、计算机设备和可读存储介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120155775A1 (en) * 2010-12-21 2012-06-21 Samsung Electronics Co., Ltd. Walking robot and simultaneous localization and mapping method thereof
CN106949895A (zh) * 2017-04-13 2017-07-14 杭州申昊科技股份有限公司 一种适用于变电站环境下的巡检机器人定位方法
CN107085422A (zh) * 2017-01-04 2017-08-22 北京航空航天大学 一种基于Xtion设备的多功能六足机器人的远程控制系统
CN107515606A (zh) * 2017-07-20 2017-12-26 北京格灵深瞳信息技术有限公司 机器人实现方法、控制方法及机器人、电子设备
CN108563227A (zh) * 2018-04-20 2018-09-21 武汉大学 一种基于活动脚轮的2d轮式机器人的运动控制方法
CN109313417A (zh) * 2015-11-16 2019-02-05 Abb瑞士股份有限公司 帮助机器人定位
CN109976344A (zh) * 2019-03-30 2019-07-05 南京理工大学 巡检机器人姿态矫正方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120155775A1 (en) * 2010-12-21 2012-06-21 Samsung Electronics Co., Ltd. Walking robot and simultaneous localization and mapping method thereof
CN109313417A (zh) * 2015-11-16 2019-02-05 Abb瑞士股份有限公司 帮助机器人定位
CN107085422A (zh) * 2017-01-04 2017-08-22 北京航空航天大学 一种基于Xtion设备的多功能六足机器人的远程控制系统
CN106949895A (zh) * 2017-04-13 2017-07-14 杭州申昊科技股份有限公司 一种适用于变电站环境下的巡检机器人定位方法
CN107515606A (zh) * 2017-07-20 2017-12-26 北京格灵深瞳信息技术有限公司 机器人实现方法、控制方法及机器人、电子设备
CN108563227A (zh) * 2018-04-20 2018-09-21 武汉大学 一种基于活动脚轮的2d轮式机器人的运动控制方法
CN109976344A (zh) * 2019-03-30 2019-07-05 南京理工大学 巡检机器人姿态矫正方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022088613A1 (zh) * 2020-10-26 2022-05-05 北京市商汤科技开发有限公司 一种机器人的定位方法及装置、设备、存储介质
CN112506190A (zh) * 2020-11-19 2021-03-16 深圳市优必选科技股份有限公司 一种机器人定位方法、机器人定位装置及机器人
CN113568439A (zh) * 2021-07-29 2021-10-29 西门子（中国）有限公司 调姿机的控制方法、装置、控制器和计算机存储介质
CN115533922A (zh) * 2022-11-29 2022-12-30 北京航空航天大学杭州创新研究院 位姿关系标定方法及装置、计算机设备和可读存储介质

Also Published As

Publication number Publication date
CN111319041B (zh) 2021-04-16

Similar Documents

Publication Publication Date Title
CN111319041B (zh) 一种机器人位姿确定方法、装置、可读存储介质及机器人
CN109959381B (zh) 一种定位方法、装置、机器人及计算机可读存储介质
CN110471409B (zh) 机器人巡检方法、装置、计算机可读存储介质及机器人
US11331806B2 (en) Robot control method and apparatus and robot using the same
CN109434831B (zh) 机器人运行方法、装置、机器人、电子设备及可读介质
US11833692B2 (en) Method and device for controlling arm of robot
CN111231950A (zh) 规划车辆变道路径的方法、装置、设备及可读存储介质
CN112744313B (zh) 一种机器人状态估计方法、装置、可读存储介质及机器人
CN112731952B (zh) 一种机器人质心规划方法、装置、可读存储介质及机器人
CN111024082B (zh) 一种规划机器人局部路径的方法、装置及机器人
CN111857037A (zh) 一种过渡轨迹的生成方法、机器人及计算机可读存储介质
CN111168688B (zh) 一种机器人动作的回放方法及装置
CN113110423A (zh) 步态轨迹规划方法、装置、计算机可读存储介质及机器人
CN111288995B (zh) 移动机器人的路径规划方法、路径规划装置及终端设备
CN115847422A (zh) 一种用于遥操作的姿态识别方法、装置及系统
CN112597612B (zh) 机器人优化方法、装置、终端设备及计算机可读存储介质
CN111185908B (zh) 识别摩擦力的机器人控制方法、装置、机器人及存储介质
CN111015668B (zh) 一种加加速度连续的速度规划方法、装置、控制器及机器人
CN112720455A (zh) 最优关节加速度与减速度计算方法、装置及其应用
CN113927585B (zh) 一种机器人平衡控制方法、装置、可读存储介质及机器人
CN112729349B (zh) 里程计在线标定的方法、装置、电子设备及存储介质
CN112720478B (zh) 一种机器人力矩控制方法、装置、可读存储介质及机器人
CN113204892B (zh) 质心轨迹生成方法、装置、计算机可读存储介质及机器人
CN113001537B (zh) 机械臂控制方法、机械臂控制装置及终端设备
CN111223193B (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