CN110977985B - 一种定位的方法及装置 - Google Patents

一种定位的方法及装置 Download PDF

Info

Publication number
CN110977985B
CN110977985B CN201911340837.XA CN201911340837A CN110977985B CN 110977985 B CN110977985 B CN 110977985B CN 201911340837 A CN201911340837 A CN 201911340837A CN 110977985 B CN110977985 B CN 110977985B
Authority
CN
China
Prior art keywords
information
movable end
determining
mechanical arm
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.)
Active
Application number
CN201911340837.XA
Other languages
English (en)
Other versions
CN110977985A (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.)
China Unionpay Co Ltd
Original Assignee
China Unionpay 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 China Unionpay Co Ltd filed Critical China Unionpay Co Ltd
Priority to CN201911340837.XA priority Critical patent/CN110977985B/zh
Publication of CN110977985A publication Critical patent/CN110977985A/zh
Priority to PCT/CN2020/099476 priority patent/WO2021128787A1/zh
Application granted granted Critical
Publication of CN110977985B publication Critical patent/CN110977985B/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/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
    • 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
    • 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/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

本发明实施例公开了一种定位的方法及装置，方法包括：通过机器人的摄像装置获取的图像信息，确定机械臂的活动端的第一位姿信息，通过设置在机械臂的采集器获取的运动信息，确定活动端的第二位姿信息，根据第一位姿信息和第二位姿信息，确定活动端的位姿信息。本发明将第一位姿信息和第二位姿信息融合确定活动端的位姿信息，在获取视觉传感信息的同时结合机械臂运动学信息，精准定位机械臂的活动端位，从而不需要设计额外的机械结构，在不同的工作环境下均能实现精确定位。

Description

一种定位的方法及装置
技术领域
本发明涉及计算机技术领域，尤其涉及一种定位的方法及装置。
背景技术
传统服务器机房巡检运维采用人工检查的方式，不仅任务繁重，消耗大量人力物力，还常因人为疏忽等原因造成无法及时发现异常，从而影响及时修复。采用机器人来执行运维任务可以在无人值守或少人值守的机房中进行24小时不间断巡检运维，及时发现机房设备的异常情况。
当机器人在机房中执行运维任务，执行诸如重启服务器、插拔服务器硬盘等动作时，需要驱动机器人的机械臂移动到固定的位置，而服务器上的按钮尺寸及硬盘厚度均在毫米级，因而需要实时对机械臂活动端进行准确定位以确保能运动到目标位置，这样才能准确地触碰按钮或是拔出硬盘。为了解决准确定位问题，目前使用的技术方案如下：
通过设计特定的机械结构或在特定位置安装传感装置来实现定位。此方案一方面需要设计特定装置比如通过安装在机械臂上的三维激光扫描仪进行定位，增加了产品的成本，另一方面其虽然在固定位置定位精度较高，然而在其他位置的定位精度则会降低。
也就是说上述方案，需要额外设计特定的定位结构，不仅减小了机械臂的运动范围，缺乏通用性，还增加了硬件成本，不利于推广。
发明内容
本发明实施例提供一种定位的方法及装置，用以解决如何对机械臂活动端进行精准定位的问题。
第一方面，本发明实施例提供一种定位的方法，适用于设有机械臂的机器人，所述机械臂设置在所述机器人的基座上；包括：
通过所述机器人的摄像装置获取的图像信息，确定所述机械臂的活动端的第一位姿信息；
通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息；
根据所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息。
上述方案，通过采集图像信息得到第一位姿信息，通过采集运动信息获得第二位姿信息，将第一位姿信息和第二位姿信息融合确定活动端的位姿信息，在获取视觉传感信息的同时结合机械臂运动学信息，精准定位机械臂的活动端位姿。从而不需要设计额外的机械结构，在不同的工作环境下均能实现精确定位。
可选的，所述通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息，包括：
通过第一采集器获取所述机械臂的各关节的运动信息，根据所述各关节的运动信息和所述基座的位置信息，确定所述活动端的位置信息；
通过第二采集器获取所述活动端的姿态信息；
根据所述活动端的位置信息和所述活动端的姿态信息，确定所述活动端的第二位姿信息。
上述方案，通过第一采集器得到活动端的位置信息，通过第二采集器得到活动端的姿态信息，然后将二者结合得到活动端的位姿，通过结合机械臂运动学信息，减小了外界噪声的影响，提升了测量结果的鲁棒性及精度。
可选的，所述根据所述各关节的运动信息和所述基座的位置信息，确定所述活动端的位置信息，包括：
根据所述各关节的角度信息，确定自所述基座至所述活动端之间的各关节之间的第一变换矩阵；
根据所述各关节的第一变换矩阵和所述基座在世界坐标系下的第一坐标，确定所述活动端在世界坐标系下的第二坐标；
根据所述活动端的位置信息和所述活动端的姿态信息，确定所述活动端的第二位姿信息，包括：
根据所述活动端的姿态信息，确定所述活动端的旋转矩阵；
根据所述旋转矩阵和所述第二坐标，确定从所述基座转换至所述活动端的第二变换矩阵；
根据所述第二变换矩阵和所述第二坐标，确定所述活动端的第二位姿信息。
上述方案，由基座的位置信息根据各关节之间的变换矩阵得到活动端的位置信息，在不增加额外机械结构的情况下，快速准确获得机械臂活动端的位置信息。
可选的，通过公式(1)确定所述活动端在世界坐标系下的第二坐标：
公式(1)
Figure GDA0002998454370000031
其中[xa,ya,za,1]为所述基座在世界坐标系下的坐标，
Figure GDA0002998454370000033
为所述机械臂活动端在世界坐标系下的坐标，n-1Tn为各关节之间的第一变换矩阵；
通过公式(2)确定所述活动端的第二变换矩阵：
公式(2)
Figure GDA0002998454370000032
其中，Rh＝R(θroll)R(θpitch)R(θyaw)为旋转矩阵，θroll为所述机械臂活动端的横滚角，θpitch为所述机械臂活动端的俯仰角，θyaw为所述机械臂活动端的偏航角；
通过公式(3)确定所述活动端的第二位姿信息：
公式(3)
Figure GDA0002998454370000042
其中
Figure GDA0002998454370000043
为活动端的第二位姿信息，WTr为所述活动端的第二变换矩阵，Pr为所述基座在世界坐标系下的位置。
可选的，所述通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息，包括：
通过第一采集器获取的运动信息，确定所述机械臂的状态转移矩阵；
根据所述运动信息对应的控制信息，确定所述机械臂的输入转移矩阵以及关节输入矩阵；
根据上一时刻所述活动端的第二位姿信息、状态转移矩阵、输入转移矩阵以及关节输入矩阵确定所述活动端在当前的第二位姿信息。
上述方案，根据上一时刻活动端的第二位姿信息以及上一时刻机械臂内部由于惯性以及上一时刻控制信息比如电机输入的力矩对当前运动的影响对当前的位姿进行预测，提高了定位的准确度。
可选的，所述根据所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息，包括：
确定所述第一位姿信息的第一误差和所述第二位姿信息的第二误差；
根据所述第一误差、所述第二误差、所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息。
上述方案，根据第一误差、第二误差、第一位姿信息和第二位姿信息，确定活动端的位姿信息，不仅预测了活动端当前的位姿，还计算了预测值的可信程度，进一步提高了定位的精确度。
可选的，通过公式(5)确定所述活动端的位姿信息：
公式(5)
Figure GDA0002998454370000044
其中，
Figure GDA0002998454370000041
Figure GDA0002998454370000045
为第二位姿信息，
Figure GDA0002998454370000053
为第一位姿信息，Ki为增益系数，H为摄像装置的观测矩阵，
Figure GDA0002998454370000051
为第二位姿信息的第二误差，R为相机测量的不确定度。
可选的，通过公式(6)确定所述第二位姿信息的第二误差：
公式(6)
Figure GDA0002998454370000052
其中Pi-1为上一时刻所述活动端的位姿，V环境噪音，A为所述机械臂的状态转移矩阵，AT为所述机械臂的状态转移矩阵的转置。
第二方面，本发明实施例提供一种定位的装置，包括：
获取单元，用于通过所述机器人的摄像装置获取图像信息，通过设置在所述机械臂的采集器获取运动信息；
处理单元，用于通过所述机器人的摄像装置获取的图像信息，确定所述机械臂的活动端的第一位姿信息，通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息，根据所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息。
可选的，所述处理单元具体用于：
通过第一采集器获取所述机械臂的各关节的运动信息，根据所述各关节的运动信息和所述基座的位置信息，确定所述活动端的位置信息；
通过第二采集器获取所述活动端的姿态信息；
根据所述活动端的位置信息和所述活动端的姿态信息，确定所述活动端的第二位姿信息。
可选的，所述处理单元具体用于：根据所述各关节的角度信息，确定自所述基座至所述活动端之间的各关节之间的第一变换矩阵；
根据所述各关节的第一变换矩阵和所述基座在世界坐标系下的第一坐标，确定所述活动端在世界坐标系下的第二坐标；
根据所述活动端的位置信息和所述活动端的姿态信息，确定所述活动端的第二位姿信息，包括：
根据所述活动端的姿态信息，确定所述活动端的旋转矩阵；
根据所述旋转矩阵和所述第二坐标，确定从所述基座转换至所述活动端的第二变换矩阵；
根据所述第二变换矩阵和所述第二坐标，确定所述活动端的第二位姿信息。
可选的，所述处理单元具体用于：通过公式(1)确定所述活动端在世界坐标系下的第二坐标：
公式(1)
Figure GDA0002998454370000061
其中[xa,ya,za,1]为所述基座在世界坐标系下的坐标，
Figure GDA0002998454370000063
为所述机械臂活动端在世界坐标系下的坐标，n-1Tn为各关节之间的第一变换矩阵；
通过公式(2)确定所述活动端的第二变换矩阵：
公式(2)
Figure GDA0002998454370000062
其中，Rh＝R(θroll)R(θpitch)R(θyaw)为旋转矩阵，θroll为所述机械臂活动端的横滚角，θpitch为所述机械臂活动端的俯仰角，θyaw为所述机械臂活动端的偏航角；
通过公式(3)确定所述活动端的第二位姿信息：
公式(3)
Figure GDA0002998454370000064
其中
Figure GDA0002998454370000065
为活动端的第二位姿信息，WTr为所述活动端的第二变换矩阵，Pr为所述基座在世界坐标系下的位置。
可选的，所述处理单元具体用于：，所述通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息，包括：
通过第一采集器获取的运动信息，确定所述机械臂的状态转移矩阵；
根据所述运动信息对应的控制信息，确定所述机械臂的输入转移矩阵以及关节输入矩阵；
根据上一时刻所述活动端的第二位姿信息、状态转移矩阵、输入转移矩阵以及关节输入矩阵确定所述活动端在当前的第二位姿信息。
可选的，所述处理单元具体用于：通过公式(4)确定所述活动端在当前的第二位姿信息：
公式(4)
Figure GDA0002998454370000074
其中，A为所述机械臂状态转移矩阵，B为所述机械臂输入转移矩阵，ui-1为第i-1s所述机械臂的关节输入矩阵，Pw(i-1)为所述活动端在第i-1s的位姿信息。
可选的，所述处理单元具体用于：所述根据所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息，包括：
确定所述第一位姿信息的第一误差和所述第二位姿信息的第二误差；
根据所述第一误差、所述第二误差、所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息。
可选的，所述处理单元具体用于：通过公式(5)确定所述活动端的位姿信息：
公式(5)
Figure GDA0002998454370000075
其中，
Figure GDA0002998454370000071
Figure GDA0002998454370000076
为第二位姿信息，
Figure GDA0002998454370000077
为第一位姿信息，Ki为增益系数，H为摄像装置的观测矩阵，
Figure GDA0002998454370000072
为第二位姿信息的第二误差，R为相机测量的不确定度。
可选的，所述处理单元具体用于：通过公式(6)确定所述第二位姿信息的第二误差：
公式(6)
Figure GDA0002998454370000073
其中Pi-1为上一时刻所述活动端的位姿，V为环境噪音，A为所述机械臂的状态转移矩阵，AT为所述机械臂的状态转移矩阵的转置。
第三方面，本发明实施例提供一种计算机被控设备，包括：
存储器，用于存储程序指令；
处理器，用于调用所述存储器中存储的程序指令，按照获得的程序执行上述第一方面所述的方法。
第四方面，本发明实施例提供一种计算机可读非易失性存储介质，包括计算机可读指令，当计算机读取并执行所述计算机可读指令时，使得计算机执行上述第一方面所述的方法。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种系统架构的示意图；
图2为本发明实施例提供的一种定位方法的流程示意图；
图3为本发明实施例提供的一种定位方法的流程示意图；
图4为本发明实施例提供的一种定位方法的示意图；
图5为本发明实施例提供的一种定位方法的流程示意图；
图6为本发明实施例提供的一种定位方法的流程示意图；
图7为本发明实施例提供的一种定位装置的结构示意图。
具体实施方式
为了更好的理解上述技术方案，下面将结合说明书附图及具体的实施方式对上述技术方案进行详细的说明，应当理解本发明实施例以及实施例中的具体特征是对本发明技术方案的详细的说明，而不是对本发明技术方案的限定，在不冲突的情况下，本发明实施例以及实施例中的技术特征可以相互结合。
本申请实施例提供的定位方法，可以适用于设有机械臂的机器人，其中机械臂设置在机器人的基座上。图1示例性的示出了本发明实施例所适用的一种系统架构，该系统架构可以包括摄像装置100、采集器200以及机械臂300。
其中，摄像装置100用于获取机械臂300活动端的图像信息。
采集器200设置在机械臂300上，用于获取机械臂300活动端的运动信息。
需要说明的是，摄像装置100可以为单目相机、深度相机等视觉传感器，上述图1所示的结构仅是一种示例，本发明实施例对此不做限定。
为了更好的解释上述实施例，图2示例性的示出了一种定位的方法的流程，该流程可以由定位的装置执行。
如图2所示，该流程具体包括：
步骤201，通过机器人的摄像装置获取的图像信息，确定机械臂的活动端的第一位姿信息。
需要说明的是，摄像装置可以为单目相机、深度相机等视觉传感器。
步骤202，通过设置在机械臂的采集器获取的运动信息，确定活动端的第二位姿信息。
步骤203，根据第一位姿信息和第二位姿信息，确定活动端的位姿信息。
上述方案，通过采集图像信息得到第一位姿信息，通过采集运动信息获得第二位姿信息，将第一位姿信息和第二位姿信息融合确定活动端的位姿信息，在获取视觉传感信息的同时结合机械臂运动学信息，精准定位机械臂的活动端位姿。从而不需要设计额外的机械结构，在不同的工作环境下均能实现精确定位。
本申请实施例在步骤201中，具体的流程如图3所示：
步骤301，获取机械臂活动端多个点的位置。
步骤302，根据多个点的位置确定机械臂活动端的第一位姿信息。
在一种可能实现的方案中，首先完成深度相机的安装，深度相机安装在机械臂上方，随后获取机械臂活动端多个点的位置，生成位置矩阵。
举个例子，获取机械臂活动端四个点在相机坐标系下的位置矩阵Mi (i＝1...4)，由这四个坐标获取机械臂活动端上一个固定点P(这里优选的取四个点的中心)在相机坐标系下的坐标P0。
具体的，首先确定四个点是否存在遮挡，如果发生遮挡则利用剩下的点完成测量。在四个点都不存在遮挡的情况下，P0位置坐标可由下式获得：
Figure GDA0002998454370000101
需要说明的是，由于P位于四个点的中心，此处取平均即可。而在四个点存在遮挡的情况下，则不能简单地将剩余点的坐标取平均值，而是根据剩余各个点与P的位置关系，将各个点的坐标加权再求平均。举个简单的例子，比如四个点中有一个点存在遮挡，如图4所示，先取a、b两点位置坐标的平均即e 的坐标，再取b、c两点位置坐标的平均即f的坐标，再根据e的纵坐标以及f 的横坐标得到P的位置坐标。当然，此处简单的例子以二维坐标为例，实际过程中每个点取的是在空间中的坐标即三维坐标。
进一步的，机械臂活动端的姿态则由下式完成：
RO＝f(Mi)，其中f为姿态转换函数。
基于上述内容，将机械臂活动端在相机坐标系下的位姿坐标Po转换至世界坐标系下，记为
Figure GDA0002998454370000102
Figure GDA0002998454370000103
由下式得到：
Figure GDA0002998454370000104
其中，wTc为从相机坐标系变化至世界坐标系的齐次变换矩阵。
需要说明的是，上述内容以4个点举例，本申请实施例可以取任意个数的点，比如5个点、6个点等，本申请对此不做具体限定，当然，取点的个数越多，得到点P的位置越精确。
在步骤202中，具体流程如图5所示：
步骤501，通过第一采集器获取机械臂的各关节的运动信息。
步骤502，根据各关节的运动信息和基座的位置信息，确定活动端的位置信息。
步骤503，通过第二采集器获取活动端的姿态信息。
步骤504，根据活动端的位置信息和活动端的姿态信息，确定活动端的第二位姿信息。
上述步骤501中，第一采集器可以为各运动关节安装的码盘或其它可以获取运动信息的传感器等，本申请对此不做具体限定。获取的运动信息可以为关节角度、关节角速度等可以标志各关节运动参数的信息，本申请对此不做具体限定。
具体的，在步骤502中，首先根据各关节的角度信息，确定自基座至活动端之间的各关节之间的第一变换矩阵，然后根据各关节的第一变换矩阵和基座在世界坐标系下的第一坐标，确定活动端在世界坐标系下的第二坐标。
进一步的，通过公式(1)确定活动端在世界坐标系下的第二坐标：
公式(1)
Figure GDA0002998454370000111
其中，[xa,ya,za,1]为基座在世界坐标系下的坐标，
Figure GDA0002998454370000112
为机械臂活动端在世界坐标系下的坐标。
基于上述内容，举个具体的例子，比如机器人自其基座到机械臂活动端之间共有6个关节，通过各运动关节的码盘采集各关节运动信息，主要包括关节角度、关节角速度等，获得关节1至关节6的齐次变换矩阵0T1、1T2、2T3、3T4、4T5、5T6。设机器人基座在世界坐标系下的坐标为[xa,ya,za,1]，设机械臂活动端固定点P在世界坐标系下的坐标为
Figure GDA0002998454370000113
则根据运动学原理可以得到：
Figure GDA0002998454370000121
由此得到了机械臂活动端在世界坐标系下的位置信息。
进一步的，步骤503中，第二采集器可以为陀螺仪等可以采集活动端姿态信息的传感装置，姿态信息包括但不限于机械臂活动端的俯仰角θpitch、偏航角θyaw、横滚角θroll等，本申请对第二采集器以及第二采集器采集的姿态信息均不作具体限定。
本申请实施例中，步骤504具体的流程如下：
首先，根据活动端的姿态信息，确定活动端的旋转矩阵。
然后，根据旋转矩阵和第二坐标，确定从基座转换至活动端的第二变换矩阵。
最后，根据第二变换矩阵和第二坐标，确定活动端的第二位姿信息。
具体的，比如采集机械臂活动端上惯性测量仪测得的机械臂活动端姿态信息，得到机械臂活动端的俯仰角θpitch、偏航角θyaw和横滚角θroll，由此得到机械臂活动端的旋转矩阵Rh为：
Rh＝R(θroll)R(θpitch)R(θyaw)
进一步的，可以获得从机械臂基座转换至机械臂活动端的齐次变换矩阵为：
Figure GDA0002998454370000122
具体的，机械臂活动端在世界坐标系下的位姿信息可以由下式得到：
Figure GDA0002998454370000123
其中，Pr为机械臂基座在世界坐标系下的位置。
基于上述内容，下面简要的介绍齐次变换矩阵wTr的原理。
首先，在变换矩阵中加入机械臂活动端在世界坐标系下的坐标是基于纯平移变换即在空间内以不变的姿态运动的原理，在这种情况下，它的方向单位向量保持同一方向不变，所有的改变只是坐标系原点相对于参考坐标系的变换。相对于固定参考坐标系，新坐标系的位置可以用原来坐标系的原点位置向量加上表示位移的向量来表示。若用矩阵形式，新坐标系的表示可以通过坐标系左乘变换矩阵得到。即变换矩阵如下：
Figure GDA0002998454370000131
其中
Figure GDA0002998454370000132
是纯平移向量相对于参考坐标系轴的 3个分量。矩阵前3列表示没有旋转运动，最后一列表示平移运动。
基于上述内容，在矩阵前3列加入旋转矩阵Rh则得到了齐次变换矩阵。
上述方案，通过第一采集器得到活动端的位置信息，通过第二采集器得到活动端的姿态信息，然后将二者结合得到活动端的位姿，通过结合机械臂运动学信息，减小了外界噪声的影响，提升了测量结果的鲁棒性及精度。
本申请实施例中，步骤203中，首先确定第一位姿信息的第一误差和第二位姿信息的第二误差，然后根据第一误差、第二误差、第一位姿信息和第二位姿信息，确定活动端的位姿信息。
具体的流程如图6所示：
步骤601，通过第一采集器获取的运动信息，确定机械臂的状态转移矩阵。
步骤602，根据运动信息对应的控制信息，确定机械臂的输入转移矩阵以及关节输入矩阵。
步骤603，根据上一时刻活动端的第二位姿信息、状态转移矩阵、输入转移矩阵以及关节输入矩阵，确定活动端在当前的第二位姿信息。
上述方案，根据第一误差、第二误差、第一位姿信息和第二位姿信息，确定活动端的位姿信息，不仅预测了活动端当前的位姿，还计算了预测值的可信程度，进一步提高了定位的精确度。
举个具体的例子，根据第一位姿信息和第二位姿信息进行融合，获取机械臂活动端最终的位姿信息Pw。
设第i-1s机械臂活动端的位姿为Pw(i-1)，则可以获得第is机械臂活动端位姿的预测值为：
Figure GDA0002998454370000144
其中，A为机械臂状态转移矩阵，表示由关节角度、角速度及机械臂参数计算出活动端位置的函数。B为机械臂输入转移矩阵，表示由电机输入的力矩计算出活动端位置的函数。ui-1为第i-1s机械臂的关节输入矩阵，表示各个电机的输入力矩。
则第is机械臂末端位姿的预测误差协方差矩阵为：
Figure GDA0002998454370000141
其中，Pi-1为第i-1s机械臂末端位姿的实际误差协方差矩阵，v为由环境噪声或机械臂形变造成的测量的不确定度。
进一步的，第is的系统增益系数Ki可由下式获得
Figure GDA0002998454370000142
其中，H为相机的观测矩阵，表示实际物体，在这里为机械臂转换为图像信息的函数，R为相机测量的不确定度，表示算法评估机械臂活动端时计算出的误差。
于是，第is机械臂末端位姿为：
Figure GDA0002998454370000145
其中，
Figure GDA0002998454370000146
为由相机测得的第is机械臂末端的位姿。
由此更新下一秒的误差协方差矩阵为：
Figure GDA0002998454370000143
上述方案，根据上一时刻活动端的第二位姿信息以及上一时刻机械臂内部由于惯性以及上一时刻控制信息比如电机输入的力矩对当前运动的影响对当前的位姿进行预测，提高了定位的准确度。
基于相同的技术构思，图6示例性的示出了本发明实施例提供定位的装置的结构，该装置可以执行定位的方法的流程。
如图7所示，该装置可以包括：
获取单元701，用于通过所述机器人的摄像装置获取图像信息，通过设置在所述机械臂的采集器获取运动信息；
处理单元702，用于通过所述机器人的摄像装置获取的图像信息，确定所述机械臂的活动端的第一位姿信息，通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息，根据所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息。
可选的，所述处理单元702具体用于：
通过第一采集器获取所述机械臂的各关节的运动信息，根据所述各关节的运动信息和所述基座的位置信息，确定所述活动端的位置信息；
通过第二采集器获取所述活动端的姿态信息；
根据所述活动端的位置信息和所述活动端的姿态信息，确定所述活动端的第二位姿信息。
可选的，所述处理单元702具体用于：根据所述各关节的角度信息，确定自所述基座至所述活动端之间的各关节之间的第一变换矩阵；
根据所述各关节的第一变换矩阵和所述基座在世界坐标系下的第一坐标，确定所述活动端在世界坐标系下的第二坐标；
根据所述活动端的位置信息和所述活动端的姿态信息，确定所述活动端的第二位姿信息，包括：
根据所述活动端的姿态信息，确定所述活动端的旋转矩阵；
根据所述旋转矩阵和所述第二坐标，确定从所述基座转换至所述活动端的第二变换矩阵；
根据所述第二变换矩阵和所述第二坐标，确定所述活动端的第二位姿信息。
可选的，所述处理单元702具体用于：通过公式(1)确定所述活动端在世界坐标系下的第二坐标：
公式(1)
Figure GDA0002998454370000161
其中[xa,ya,za,1]为所述基座在世界坐标系下的坐标，
Figure GDA0002998454370000163
为所述机械臂活动端在世界坐标系下的坐标，n-1Tn为各关节之间的第一变换矩阵；
通过公式(2)确定所述活动端的第二变换矩阵：
公式(2)
Figure GDA0002998454370000162
其中，Rh＝R(θroll)R(θpitch)R(θyaw)为旋转矩阵，θroll为所述机械臂活动端的横滚角，θpitch为所述机械臂活动端的俯仰角，θyaw为所述机械臂活动端的偏航角；
通过公式(3)确定所述活动端的第二位姿信息：
公式(3)
Figure GDA0002998454370000164
其中
Figure GDA0002998454370000165
为活动端的第二位姿信息，WTr为所述活动端的第二变换矩阵，Pr为所述基座在世界坐标系下的位置。
可选的，所述处理单元702具体用于：，所述通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息，包括：
通过第一采集器获取的运动信息，确定所述机械臂的状态转移矩阵；
根据所述运动信息对应的控制信息，确定所述机械臂的输入转移矩阵以及关节输入矩阵；
根据上一时刻所述活动端的第二位姿信息、状态转移矩阵、输入转移矩阵以及关节输入矩阵确定所述所述活动端在当前的第二位姿信息。
可选的，所述处理单元702具体用于：通过公式(4)确定所述所述活动端在当前的第二位姿信息
公式(4)
Figure GDA0002998454370000174
其中，A为所述机械臂状态转移矩阵，B为所述机械臂输入转移矩阵，ui-1为第i-1s所述机械臂的关节输入矩阵，Pw(i-1)为所述活动端在第i-1s的位姿信息。
可选的，所述处理单元702具体用于：所述根据所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息，包括：
确定所述第一位姿信息的第一误差和所述第二位姿信息的第二误差；
根据所述第一误差、所述第二误差、所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息。
可选的，所述处理单元702具体用于：通过公式(5)确定所述所述活动端的位姿信息：
公式(5)
Figure GDA0002998454370000175
其中，
Figure GDA0002998454370000171
Figure GDA0002998454370000176
为第二位姿信息，
Figure GDA0002998454370000177
为第一位姿信息，Ki为增益系数，H为摄像装置的观测矩阵，
Figure GDA0002998454370000172
为第二位姿信息的第二误差，R为相机测量的不确定度。
可选的，所述处理单元702具体用于：通过公式(6)确定所述第二位姿信息的第二误差：
公式(6)
Figure GDA0002998454370000173
其中Pi-1为上一时刻所述活动端的位姿，V为环境噪音，A为所述机械臂的状态转移矩阵，AT为所述机械臂的状态转移矩阵的转置。
基于相同的技术构思，本发明实施例还提供了一种计算被控设备，包括：
存储器，用于存储程序指令；
处理器，用于调用所述存储器中存储的程序指令，按照获得的程序执行上述定位的方法。
基于相同的技术构思，本发明实施例还提供了一种计算机可读非易失性存储介质，包括计算机可读指令，当计算机读取并执行所述计算机可读指令时，使得计算机执行上述定位的方法。
最后应说明的是：本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明的方法、被控设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/ 或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理被控设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理被控设备的处理器执行的指令产生用于实现在流程图一个流程或多个和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理被控设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (11)

1.一种定位的方法，其特征在于，适用于设有机械臂的机器人，所述机械臂设置在所述机器人的基座上；所述方法包括：
通过所述机器人的摄像装置获取的图像信息，确定所述机械臂的活动端的第一位姿信息；
通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息；
根据所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息；
所述通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息，包括：
通过第一采集器获取所述机械臂的各关节的运动信息，根据所述各关节的运动信息和所述基座的位置信息，确定所述活动端的位置信息；
通过第二采集器获取所述活动端的姿态信息；
根据所述活动端的位置信息和所述活动端的姿态信息，确定所述活动端的第二位姿信息。
2.根据权利要求1所述的方法，其特征在于，所述根据所述各关节的运动信息和所述基座的位置信息，确定所述活动端的位置信息，包括：
根据所述各关节的角度信息，确定自所述基座至所述活动端之间的各关节之间的第一变换矩阵；
根据所述各关节的第一变换矩阵和所述基座在世界坐标系下的第一坐标，确定所述活动端在世界坐标系下的第二坐标；
根据所述活动端的位置信息和所述活动端的姿态信息，确定所述活动端的第二位姿信息，包括：
根据所述活动端的姿态信息，确定所述活动端的旋转矩阵；
根据所述旋转矩阵和所述第二坐标，确定从所述基座转换至所述活动端的第二变换矩阵；
根据所述第二变换矩阵和所述第二坐标，确定所述活动端的第二位姿信息。
3.根据权利要求2所述的方法，其特征在于，通过公式(1)确定所述活动端在世界坐标系下的第二坐标：
公式(1)
Figure FDA0002998454360000021
其中[xa,ya,za,1]为所述基座在世界坐标系下的坐标，
Figure FDA0002998454360000023
为所述机械臂活动端在世界坐标系下的坐标，n-1Tn为各关节之间的第一变换矩阵；
通过公式(2)确定所述活动端的第二变换矩阵：
公式(2)
Figure FDA0002998454360000022
其中，Rh＝R(θroll)R(θpitch)R(θyaw)为旋转矩阵，θroll为所述机械臂活动端的横滚角，θpitch为所述机械臂活动端的俯仰角，θyaw为所述机械臂活动端的偏航角；
通过公式(3)确定所述活动端的第二位姿信息：
公式(3)
Figure FDA0002998454360000024
其中
Figure FDA0002998454360000025
为活动端的第二位姿信息，WTr为所述活动端的第二变换矩阵，Pr为所述基座在世界坐标系下的位置。
4.根据权利要求1所述的方法，其特征在于，所述通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息，包括：
通过第一采集器获取的运动信息，确定所述机械臂的状态转移矩阵；
根据所述运动信息对应的控制信息，确定所述机械臂的输入转移矩阵以及关节输入矩阵；
根据上一时刻所述活动端的第二位姿信息、状态转移矩阵、输入转移矩阵以及关节输入矩阵确定所述活动端在当前的第二位姿信息。
5.根据权利要求4所述的方法，其特征在于，通过公式(4)确定所述活动端在当前的第二位姿信息
公式(4)
Figure FDA0002998454360000036
其中，A为所述机械臂的状态转移矩阵，B为所述机械臂的输入转移矩阵，ui-1为第i-1s所述机械臂的关节输入矩阵，Pw(i-1)为所述活动端在第i-1s的位姿信息。
6.根据权利要求1至5任一项所述的方法，其特征在于，所述根据所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息，包括：
确定所述第一位姿信息的第一误差和所述第二位姿信息的第二误差；
根据所述第一误差、所述第二误差、所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息。
7.根据权利要求6所述的方法，其特征在于，通过公式(5)确定所述活动端的位姿信息：
公式(5)
Figure FDA0002998454360000034
其中，
Figure FDA0002998454360000031
Figure FDA0002998454360000035
为第二位姿信息，
Figure FDA0002998454360000037
为第一位姿信息，Ki为增益系数，H为摄像装置的观测矩阵，
Figure FDA0002998454360000032
为第二位姿信息的第二误差，R为相机测量的不确定度。
8.根据权利要求6所述的方法，其特征在于，通过公式(6)确定所述第二位姿信息的第二误差：
公式(6)
Figure FDA0002998454360000033
其中Pi-1为上一时刻所述活动端的位姿，V为环境噪音，A为所述机械臂的状态转移矩阵，AT为所述机械臂的状态转移矩阵的转置。
9.一种定位装置，其特征在于，适用于设有机械臂的机器人，所述机械臂设置在所述机器人的基座上，所述装置包括：
获取单元，用于通过所述机器人的摄像装置获取图像信息，通过设置在所述机械臂的采集器获取运动信息；
处理单元，用于通过所述机器人的摄像装置获取的图像信息，确定所述机械臂的活动端的第一位姿信息，通过设置在所述机械臂的采集器获取的运动信息，确定所述活动端的第二位姿信息，根据所述第一位姿信息和所述第二位姿信息，确定所述活动端的位姿信息；
所述处理单元，具体用于：通过第一采集器获取所述机械臂的各关节的运动信息，根据所述各关节的运动信息和所述基座的位置信息，确定所述活动端的位置信息；通过第二采集器获取所述活动端的姿态信息；根据所述活动端的位置信息和所述活动端的姿态信息，确定所述活动端的第二位姿信息。
10.一种计算设备，其特征在于，包括：
存储器，用于存储程序指令；
处理器，用于调用所述存储器中存储的程序指令，按照获得的程序执行权利要求1至8任一项所述的方法。
11.一种计算机可读非易失性存储介质，其特征在于，包括计算机可读指令，当计算机读取并执行所述计算机可读指令时，使得计算机执行如权利要求1至8任一项所述的方法。
CN201911340837.XA 2019-12-23 2019-12-23 一种定位的方法及装置 Active CN110977985B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201911340837.XA CN110977985B (zh) 2019-12-23 2019-12-23 一种定位的方法及装置
PCT/CN2020/099476 WO2021128787A1 (zh) 2019-12-23 2020-06-30 一种定位的方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911340837.XA CN110977985B (zh) 2019-12-23 2019-12-23 一种定位的方法及装置

Publications (2)

Publication Number Publication Date
CN110977985A CN110977985A (zh) 2020-04-10
CN110977985B true CN110977985B (zh) 2021-10-01

Family

ID=70075805

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911340837.XA Active CN110977985B (zh) 2019-12-23 2019-12-23 一种定位的方法及装置

Country Status (2)

Country Link
CN (1) CN110977985B (zh)
WO (1) WO2021128787A1 (zh)

Families Citing this family (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110977985B (zh) * 2019-12-23 2021-10-01 中国银联股份有限公司 一种定位的方法及装置
CN111881411B (zh) * 2020-07-10 2024-02-06 广联达科技股份有限公司 机械节点位置的确定方法及确定装置
CN111923043A (zh) * 2020-07-30 2020-11-13 苏州富鑫林光电科技有限公司 一种基于多传感器融合的多机械手定位方法
CN112405526A (zh) * 2020-10-26 2021-02-26 北京市商汤科技开发有限公司 一种机器人的定位方法及装置、设备、存储介质
CN113220017A (zh) * 2021-04-16 2021-08-06 同济大学 一种地下无人机飞行方法及系统
CN114441807B (zh) * 2021-07-22 2023-07-07 荣耀终端有限公司 一种接线方法及系统
CN114274140B (zh) * 2021-09-30 2024-04-16 武汉大学 基于室外天线标定场地位置的机械臂动作规划方法及系统
CN114147717B (zh) * 2021-12-09 2024-05-24 乐聚(深圳)机器人技术有限公司 机器人运动轨迹估计方法、装置、控制器及存储介质
CN114283447B (zh) * 2021-12-13 2024-03-26 北京元客方舟科技有限公司 一种动作捕捉系统及方法
CN114305686B (zh) * 2021-12-20 2024-07-16 杭州堃博生物科技有限公司 基于磁传感器的定位处理方法、装置、设备与介质
CN114427652B (zh) * 2021-12-20 2023-10-10 哈尔滨理工大学 室内三维重建信息采集装置及装置摄像头位置获取方法
CN114536399B (zh) * 2022-01-07 2023-04-25 中国人民解放军海军军医大学第一附属医院 基于多个位姿标识的误差检测方法及机器人系统
CN114800499B (zh) * 2022-04-20 2023-08-25 北京三快在线科技有限公司 位姿调整方法、装置、计算机可读存储介质及电子设备
CN115005984B (zh) * 2022-05-07 2024-09-24 武汉市龙点睛智能科技有限公司 手术器械的标定方法及系统、手术器械的定位方法及装置
CN115805593B (zh) * 2022-12-22 2023-11-28 苏州艾利特机器人有限公司 力传感器安装信息确定方法、装置、设备和介质
CN117032027B (zh) * 2023-08-15 2024-06-14 广东美的智能科技有限公司 视觉控制系统及其控制方法、视觉运动控制器和存储介质
CN117817667B (zh) * 2024-01-26 2024-06-25 合肥工业大学 一种基于svd分解法的机械臂末端姿态调整方法

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4453085A (en) * 1981-05-11 1984-06-05 Diffracto Ltd. Electro-optical systems for control of robots, manipulator arms and co-ordinate measuring machines
US20090118864A1 (en) * 2007-11-01 2009-05-07 Bryce Eldridge Method and system for finding a tool center point for a robot using an external camera
CN106052683A (zh) * 2016-05-25 2016-10-26 速感科技（北京）有限公司 机器人运动姿态估计方法
CN106247932B (zh) * 2016-07-25 2019-03-12 天津大学 一种基于摄影系统的机器人在线误差补偿装置及方法
JP6707485B2 (ja) * 2017-03-22 2020-06-10 株式会社東芝 物体ハンドリング装置およびその較正方法
JP6564428B2 (ja) * 2017-08-03 2019-08-21 ファナック株式会社 キャリブレーションシステムおよびキャリブレーション方法
CN107314778B (zh) * 2017-08-04 2023-02-10 广东工业大学 一种相对姿态的标定方法、装置及系统
CN107498558A (zh) * 2017-09-19 2017-12-22 北京阿丘科技有限公司 全自动手眼标定方法及装置
CN109129466B (zh) * 2018-07-26 2021-07-20 清华大学 一种用于立体定向机器人的主动视觉装置及其控制方法
CN109262610A (zh) * 2018-08-30 2019-01-25 珠海格力电器股份有限公司 求解串联多自由度机器人末端位姿的方法、系统和机器人
CN110196047A (zh) * 2019-06-20 2019-09-03 东北大学 基于tof深度相机与imu的平仓机器人自主定位方法
CN110375738B (zh) * 2019-06-21 2023-03-14 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN110977985B (zh) * 2019-12-23 2021-10-01 中国银联股份有限公司 一种定位的方法及装置

Also Published As

Publication number Publication date
WO2021128787A1 (zh) 2021-07-01
CN110977985A (zh) 2020-04-10

Similar Documents

Publication Publication Date Title
CN110977985B (zh) 一种定位的方法及装置
US11833696B2 (en) Vision-based sensor system and control method for robot arms
KR20140008262A (ko) 로봇 시스템, 로봇, 로봇 제어 장치, 로봇 제어 방법 및 로봇 제어 프로그램
CN111801198A (zh) 一种手眼标定方法、系统及计算机存储介质
JP2009006410A (ja) 遠隔操作支援装置および遠隔操作支援プログラム
CN114454174B (zh) 机械臂动作捕捉方法、介质、电子设备及系统
US20200098127A1 (en) Imaging apparatus and imaging system
CN113269829B (zh) 流水生产线目标定位方法、装置、计算机设备和存储介质
CN116721166B (zh) 双目相机和imu旋转外参在线标定方法、装置及存储介质
CN111971529A (zh) 用于管理机器人系统的方法和装置
Scheuermann et al. Mobile augmented reality based annotation system: A cyber-physical human system
TWI788253B (zh) 適應性移動操作設備及方法
WO2019093299A1 (ja) 位置情報取得装置およびそれを備えたロボット制御装置
Kim et al. Vision-force guided precise robotic assembly for 2.5 D components in a semistructured environment
CN113065572A (zh) 多传感器融合的数据处理方法、定位装置及虚拟现实设备
CN114952832A (zh) 基于单目六自由度物体姿态估计的机械臂拼装方法及装置
CN113768535B (zh) 一种遥操作用超声仿型探头姿态自校准方法、系统及装置
CN113733078B (zh) 机械臂精调控制量判读方法、计算机可读存储介质
Cheng et al. “Adult” robot enabled learning process in high precision assembly automation
CN115494794A (zh) 一种动力学参数辨识方法、装置、设备及存储介质
JP2005186193A (ja) ロボットのキャリブレーション方法および三次元位置計測方法
JP2021014001A (ja) 測定装置
CN2645034Y (zh) 物体空间位姿检测装置
CN115990890B (zh) 一种机械手的标定方法、装置、计算机设备和存储介质
Skov et al. 3D Navigation by UAV using a mono-camera, for precise target tracking for contact inspection of critical infrastructures

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