CN113997293A - 一种动态视觉的感知观测跟踪控制方法、装置及设备 - Google Patents
一种动态视觉的感知观测跟踪控制方法、装置及设备 Download PDFInfo
- Publication number
- CN113997293A CN113997293A CN202111487928.3A CN202111487928A CN113997293A CN 113997293 A CN113997293 A CN 113997293A CN 202111487928 A CN202111487928 A CN 202111487928A CN 113997293 A CN113997293 A CN 113997293A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- robot
- axes
- axis
- equipment
- 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 57
- 230000008447 perception Effects 0.000 title claims abstract description 45
- 230000004438 eyesight Effects 0.000 title claims abstract description 42
- 238000013507 mapping Methods 0.000 claims description 47
- 230000016776 visual perception Effects 0.000 claims description 27
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000012545 processing Methods 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 7
- 230000000007 visual effect Effects 0.000 claims description 4
- 239000004576 sand Substances 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 abstract description 16
- 238000010586 diagram Methods 0.000 description 6
- 238000004590 computer program Methods 0.000 description 5
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000003384 imaging method Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/02—Sensing devices
- B25J19/021—Optical sensing devices
- B25J19/023—Optical sensing devices including video camera means
Abstract
本发明涉及一种动态视觉的感知观测跟踪控制方法、装置及设备，动态视觉感知系统包括VR设备和机器人，机器人的末端设置有摄像设备，该方法基于搭建了包含VR设备、机器人和摄像设备的动态视觉感知系统，并提出了针对该动态视觉感知系统采用逆运动学公式分析的异构型主从遥操作逆运动学模型，实现对目标的动态多角度可见光观测，通过逆运动学公式分析动态视觉感知系统机器人各个关节角实现机器人末端及摄像设备对VR设备的位姿跟踪，其位置跟踪平均误差不大于1.7mm，姿态跟踪平均误差小于2.36°；解决了现有遥操作技术与AR技术相结合遥控操作采用D‑H法建立遥操作机器人主从臂的运动学模型，得到的姿态跟踪精度低的技术问题。
Description
技术领域
本发明涉及机器人视觉跟踪技术领域，尤其涉及一种动态视觉的感知观测跟踪控制方法、装置及设备。
背景技术
机器人代替人工开展配网带电作业是电网运维方式发展的趋势。但是由于现有配网线路多回路类型比例高、设备密度高，传统的固定式视觉感知系统难以绕过作业空间内的障碍遮蔽为操作人员和机器人自主控制系统提供作业目标和环境的感知和定位信息。而随着VR/AR设备技术成熟、成本降低，将VR/AR设备与遥操作技术相融合，研究可跟踪操作人员头部位姿、模拟人类在多视角观测同一目标时脊柱的姿态变化、具有高临场感的作业机器人动态视觉感知系统满足现在的需求。
目前，遥操作技术与AR技术相结合的研究主要集中于将AR作为操作人员的视觉感知来源，提升遥操作的临场感。而VR技术与遥操作技术相结合的研究主要是利用虚拟图像或者虚拟力、触觉降低遥操作延时问题对操作临场感的影响。在遥操作实现方面，目前常用的方法是采用D-H法建立遥操作机器人主从臂的运动学模型，通过该运动学模型得到的定位精度低。
发明内容
本发明实施例提供了一种动态视觉的感知观测跟踪控制方法、装置及设备，用于解决现有遥操作技术与AR技术相结合遥控操作采用D-H法建立遥操作机器人主从臂的运动学模型，得到的姿态跟踪精度低的技术问题。
为了实现上述目的，本发明实施例提供如下技术方案：
一种动态视觉的感知观测跟踪控制方法，应用于动态视觉感知系统上，所述动态视觉感知系统包括VR设备和机器人，所述机器人的末端设置有摄像设备，该方法包括以下步骤：
对所述VR设备、所述机器人和所述摄像设备分别建立对应的第一坐标系、第二坐标系和第三坐标系；
实时获取所述VR设备在第一坐标系下观测目标的第一空间参数，以及实时获取所述摄像设备在第三坐标系下的第三空间参数；
通过逆运动学公式对所述第一空间参数和所述第三空间参数进行映射处理，得到机器人的关节角；
通过所述关节角控制所述机器人运行，以控制所述摄像设备实时对观测目标动态观测。
优选地，所述逆运动学公式为：[x,y,z,ψ,θ,φ]m＝f-1([x,y,z,ψ,θ,φ]s)＝f-1(g-1(q))；f为VR设备获取的第一空间参数到机器人末端的映射模型；g为机器人末端位姿到机器人各关节角大小的映射模型；q＝[q1,q2,…,q6]为机器人的关节角，[x,y,z,ψ,θ,φ]m为第一坐标系x、y、z轴对应的坐标xm、ym、zm和旋转角ψm、θm、φm；[x,y,z,ψ,θ,φ]s为第三坐标系x、y、z轴对应坐标xs、ys、zs和旋转角ψs、θs、φs。
优选地，通过逆运动学公式对所述第一空间参数和所述第三空间参数进行映射处理，得到机器人的关节角的步骤包括：
将所述第一空间参数和所述第三空间参数通过对应的坐标系三轴建立第一旋转矩阵和第三旋转矩阵；
将所述第一坐标系的a、o、n轴与所述第三坐标系的n'、o'、a'轴进行映射对应，得到所述VR设备在所述第三坐标系上的映射旋转等式；
通过所述第一旋转矩阵、所述第三旋转矩阵代入所述映射旋转等式，得到所述VR设备映射到所述第三坐标系的空间参数；
获取所述机器人在所述第二坐标系下的各个关节参数；
通过多元方程组对所述空间参数逐一与每个关节参数进行求解，得到机器人的各个关节角。
优选地，所述第一旋转矩阵为：
式中，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，oxm、oym、ozm分别为第一坐标系x、y、z轴在o轴的坐标，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，rz(φ)m为第一坐标系z轴旋转后的旋转角，ry(θ)m为第一坐标系y轴旋转后的旋转角，rx(ψ)m为第一坐标x轴旋转后的旋转角；
所述第三旋转矩阵为：
式中，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，o'xs、o'ys、o'zs分别为第三坐标系x、y、z轴在o'轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，rz(φ')s为第三坐标系z轴旋转后的旋转角，ry(θ')s为第三坐标系y轴旋转后的旋转角，rx(ψ')s为第三坐标x轴旋转后的旋转角。
优选地，所述映射旋转等式为：
式中，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，oxm、oym、ozm分别为第一坐标系x、y、z轴在o轴的坐标，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，o'xs、o'ys、o'zs分别为第三坐标系x、y、z轴在o'轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标。
优选地，所述第三坐标系的空间参数为：
ozm＝cos(θ)sin(ψ)
azm＝cos(ψ)cos(θ)
nzm＝-sin(θ)
aym＝cos(ψ)sin(θ)sin(φ)-cos(φ)sin(ψ)
axm＝cos(ψ)cos(φ)sin(θ)+sin(ψ)sin(φ)
式中，k为VR设备与机器人之间的映射比例系数，ψ、θ、φ分别为第一坐标系x、y、z轴对应的旋转角，x0、y0、z0分为为VR设备映射在第三坐标系的坐标，ψ0、θ0、φ0分别为VR设备映射在第三坐标系上与x0、y0、z0对应的角度。
优选地，该动态视觉的感知观测跟踪控制方法包括：获取观测目标在所述第一坐标系下的第一坐标位置参数；通过坐标变换关系对所述第一坐标位置参数处理，得到观测目标在第三坐标系下的第三坐标位置参数。
优选地，所述坐标变换关系为：
式中，pin_eye为第一坐标系下的第一坐标位置参数，pin_cartesian为第三坐标系下的第三坐标位置参数，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，o'xs、o'ys、o'zs分别为第三坐标系x、y、z轴在o'轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，x's、y's、z's分别为第三坐标系x、y、z轴的坐标。
本发明还提供一种动态视觉的感知观测跟踪控制系统，应用于动态视觉感知系统上，所述动态视觉感知系统包括VR设备和机器人，所述机器人的末端设置有摄像设备，该装置包括坐标建立模块、参数获取模块、映射处理模块和执行观测模块；
所述坐标建立模块，用于对所述VR设备、所述机器人和所述摄像设备分别建立对应的第一坐标系、第二坐标系和第三坐标系；
所述参数获取模块，用于实时获取所述VR设备在第一坐标系下观测目标的第一空间参数，以及实时获取所述摄像设备在第三坐标系下的第三空间参数；
所述映射处理模块，用于通过逆运动学公式对所述第一空间参数和所述第三空间参数进行映射处理，得到机器人的关节角；
所述执行观测模块，用于通过所述关节角控制所述机器人运行，以控制所述摄像设备实时对观测目标动态观测。
本发明还提供一种动态视觉的感知观测跟踪控制设备，包括处理器以及存储器；
所述存储器，用于存储程序代码，并将所述程序代码传输给所述处理器；
所述处理器，用于根据所述程序代码中的指令执行上述所述的动态视觉的感知观测跟踪控制方法。
从以上技术方案可以看出，本发明实施例具有以下优点：该动态视觉的感知观测跟踪控制方法、装置及设备，其方法包括对VR设备、机器人和摄像设备分别建立对应的第一坐标系、第二坐标系和第三坐标系；实时获取VR设备在第一坐标系下观测目标的第一空间参数，以及实时获取摄像设备在第三坐标系下的第三空间参数；通过逆运动学公式对第一空间参数和第三空间参数进行映射处理，得到机器人的关节角；通过关节角控制机器人运行，以控制摄像设备实时对观测目标动态观测。该方法基于搭建了包含VR设备、机器人和摄像设备的动态视觉感知系统，并提出了针对该动态视觉感知系统采用逆运动学公式分析的异构型主从遥操作逆运动学模型(VR设备与机器人)，实现对目标的动态多角度可见光观测，通过逆运动学公式分析动态视觉感知系统机器人各个关节角实现机器人末端及摄像设备对VR设备的位姿跟踪，其位置跟踪平均误差不大于1.7mm，姿态跟踪平均误差小于2.36°；解决了现有遥操作技术与AR技术相结合遥控操作采用D-H法建立遥操作机器人主从臂的运动学模型，得到的姿态跟踪精度低的技术问题。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。
图1为本发明实施例所述的动态视觉的感知观测跟踪控制方法的步骤流程图；
图2为本发明实施例所述的动态视觉感知系统的框架图；
图3为本发明实施例所述的动态视觉的感知观测跟踪控制方法机器人坐标的示意图；
图4a为本发明实施例所述的动态视觉的感知观测跟踪控制方法位姿跟踪的空间参数折线示意图；
图4b为本发明实施例所述的动态视觉的感知观测跟踪控制方法位姿跟踪的关节角折线示意图；
图5为本发明实施例所述的动态视觉的感知观测跟踪控制装置的框架图。
具体实施方式
为使得本发明的发明目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
本申请实施例提供了一种动态视觉的感知观测跟踪控制方法、装置及设备，应用于动态视觉感知系统上，用于解决了现有遥操作技术与AR技术相结合遥控操作采用D-H法建立遥操作机器人主从臂的运动学模型，得到的姿态跟踪精度低的技术问题。
实施例一：
图1为本发明实施例所述的动态视觉的感知观测跟踪控制方法的步骤流程图，图2为本发明实施例所述的动态视觉感知系统的框架图。
如图1和图2所示，本发明实施例提供了一种动态视觉的感知观测跟踪控制方法，应用于动态视觉感知系统上，动态视觉感知系统包括VR设备10和机器人20，机器人20的末端设置有摄像设备30。
需要说明的是，VR设备10主要用于观测待观测目标，VR设备10可以设置有头戴式。机器人20可以为6个自由度串联的机器人，摄像设备30可以为双目相机。本实施例中，动态视觉感知系统主要是由VR设备10、机器人20和摄像设备30组成。
在本发明实施例中，VR头戴设备将实时反馈双目相机采集的可见光图像；佩戴人员根据观察需要在一定范围内移动或转动头部；并且在VR设备10内置惯导(InertialMeasurement Unit,IMU)及基站，用于实时反馈的VR设备姿态与位置信息，通过该动态视觉的感知观测跟踪控制方法求解末端搭载双目相机的机器人各关节角，并通过伺服控制模式驱动机器人关节运动，实现双目相机对VR设备的空间跟踪。
该动态视觉的感知观测跟踪控制方法包括以下步骤：
S1.对VR设备、机器人和摄像设备分别建立对应的第一坐标系、第二坐标系和第三坐标系。
需要说明的是，主要是对VR设备、机器人和摄像设备分别建立笛卡尔坐标系的第一坐标系、第二坐标系和第三坐标系。
S2.实时获取VR设备在第一坐标系下观测目标的第一空间参数，以及实时获取摄像设备在第三坐标系下的第三空间参数。
需要说明的是，主要是获取采用VR设备观测目标和摄像设备在对应坐标系下的空间参数。在本实施例中，第一坐标系x、y、z轴的坐标作为VR设备获取观测目标的空间位置参数，在第一坐标系x、y、z轴对应旋转角ψm、θm、φm作为VR设备获取观测目标的姿态参数，第三坐标系x、y、z轴的坐标作为摄像设备的空间位置参数，在第三坐标系x、y、z轴对应旋转角ψs、θs、φs作为摄像设备的空间姿态参数。
S3.通过逆运动学公式对第一空间参数和第三空间参数进行映射处理，得到机器人的关节角。
需要说明的是，逆运动学公式为：[x,y,z,ψ,θ,φ]m＝f-1([x,y,z,ψ,θ,φ]s)＝f-1(g-1(q))；f为VR设备获取的第一空间参数到机器人末端(摄像设备)的映射模型；g为机器人末端(摄像设备)位姿到机器人各关节角大小的映射模型；q＝[q1,q2,…,q6]为机器人的关节角，[x,y,z,ψ,θ,φ]m为第一坐标系x、y、z轴对应的坐标xm、ym、zm和旋转角ψm、θm、φm；[x,y,z,ψ,θ,φ]s为第三坐标系x、y、z轴对应坐标xs、ys、zs和旋转角ψs、θs、φs。
在本发明实施例中，在位置映射方面，VR设备获取观测目标的空间位置与动态视觉感知系统末端摄像设备的位置映射采用比例映射，即：[x-x0,y-y0,z-z0]s＝a[-x+x0,-y+y0,z-z0]m，k为第一坐标系与第三坐标系之间位置映射的比例系数。[x0,y0,z0]m和[x0,y0,z0]s分别为VR设备和机器人初始状态空间位置。
S4.通过关节角控制机器人运行，以控制摄像设备实时对观测目标动态观测。
需要说明的是，主要是通过得到的机器人各个自由度关节角的参数，按照各个自由度关节角控制机器人移动，能够让摄像设备实现实时观测目标的动态。
本发明提供的一种动态视觉的感知观测跟踪控制方法，包括对VR设备、机器人和摄像设备分别建立对应的第一坐标系、第二坐标系和第三坐标系；实时获取VR设备在第一坐标系下观测目标的第一空间参数，以及实时获取摄像设备在第三坐标系下的第三空间参数；通过逆运动学公式对第一空间参数和第三空间参数进行映射处理，得到机器人的关节角；通过关节角控制机器人运行，以控制摄像设备实时对观测目标动态观测。该方法基于搭建了包含VR设备、机器人和摄像设备的动态视觉感知系统，并提出了针对该动态视觉感知系统采用逆运动学公式分析的异构型主从遥操作逆运动学模型，实现对目标的动态多角度可见光观测，通过逆运动学公式分析动态视觉感知系统机器人各个关节角实现机器人末端及摄像设备对VR设备的位姿跟踪，其位置跟踪平均误差不大于1.7mm，姿态跟踪平均误差小于2.36°；解决了现有遥操作技术与AR技术相结合遥控操作采用D-H法建立遥操作机器人主从臂的运动学模型，得到的姿态跟踪精度低的技术问题。
在本发明的一个实施例中，通过逆运动学公式对第一空间参数和第三空间参数进行映射处理，得到机器人的关节角的步骤包括：
将第一空间参数和第三空间参数通过对应的坐标系三轴建立第一旋转矩阵和第三旋转矩阵；
将第一坐标系的a、o、n轴与第三坐标系的n'、o'、a'轴进行映射对应，得到VR设备在第三坐标系上的映射旋转等式；
通过第一旋转矩阵、第三旋转矩阵代入映射旋转等式，得到VR设备映射到第三坐标系的空间参数；
获取机器人在第二坐标系下的各个关节参数；
通过多元方程组对空间参数逐一与每个关节参数进行求解，得到机器人的各个关节角。
在本发明实施例中，第一旋转矩阵为：
式中，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，oxm、oym、ozm分别为第一坐标系x、y、z轴在o轴的坐标，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，rz(φ)m为第一坐标系z轴旋转后的旋转角，ry(θ)m为第一坐标系y轴旋转后的旋转角，rx(ψ)m为第一坐标x轴旋转后的旋转角；
第三旋转矩阵为：
式中，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，o'xs、o'ys、o'zs分别为第三坐标系x、y、z轴在o'轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，rz(φ')s为第三坐标系z轴旋转后的旋转角，ry(θ')s为第三坐标系y轴旋转后的旋转角，rx(ψ')s为第三坐标x轴旋转后的旋转角。
在本发明实施例中，映射旋转等式为：
式中，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，oxm、oym、ozm分别为第一坐标系x、y、z轴在o轴的坐标，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，o'xs、o'ys、o'zs分别为第三坐标系x、y、z轴在o'轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标。
在本发明实施例中，第三坐标系的空间参数为：
ozm＝cos(θ)sin(ψ)
azm＝cos(ψ)cos(θ)
nzm＝-sin(θ)
aym＝cos(ψ)sin(θ)sin(φ)-cos(φ)sin(ψ)
axm＝cos(ψ)cos(φ)sin(θ)+sin(ψ)sin(φ)
式中，k为VR设备与机器人之间的映射比例系数，ψ、θ、φ分别为第一坐标系x、y、z轴对应的旋转角，x0、y0、z0分为为VR设备映射在第三坐标系的坐标，ψ0、θ0、φ0分别为VR设备映射在第三坐标系上与x0、y0、z0对应的角度。
图3为本发明实施例所述的动态视觉的感知观测跟踪控制方法机器人坐标的示意图。
在本发明实施例中，以机器人六个自由度作为案例说明，如图3所示，定义关节4回转轴线和关节5回转轴线的交点为机器人“颈”，求解的关键是利用机器人的几何约束，首先根据机器人末端(摄像设备)位姿确定“颈”部空间位置，从而将一个描述机器人末端位姿和机器人6个关节角度关系的六元方程组求解问题，转化成为分别描述机器人“颈”部位置与机器人等3个关节角度之间关系，以及描述机器人末端姿态与剩余等3个关节角度之间关系的两个三元方程组求解问题。
需要说明的是，通过两个三元方程组对空间参数逐一与每个关节参数进行求解，得到机器人的各个关节角具体求解过程，可以参考文件《Andersen,TT.Optimizing theUniversal Robots ROS driver[M]//.Technical University of Denmark,Departmentof Electrical Engineering,2015》，此处不对求解机器人各个关节角过程进行详细描述。
在本发明的一个实施例中，该动态视觉的感知观测跟踪控制方法包括：获取观测目标在第一坐标系下的第一坐标位置参数；通过坐标变换关系对第一坐标位置参数处理，得到观测目标在第三坐标系下的第三坐标位置参数。
在本发明实施例中，坐标变换关系为：
式中，pin_eye为第一坐标系下的第一坐标位置参数，pin_cartesian为第三坐标系下的第三坐标位置参数，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，o'xs、o'ys、o'zs分别为第三坐标系x、y、z轴在o'轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，x's、y's、z's分别为第三坐标系x、y、z轴的坐标。
需要说明的是，基于摄像设备所采集的视场深度信息(第三坐标系下的第三坐标位置参数)和坐标变换关系，建立了观测目标在第三坐标系与第一坐标系之间的映射关系，实现在不同观测角度下对同一固定观测目标的绝对坐标粗定位。
图4a为本发明实施例所述的动态视觉的感知观测跟踪控制方法位姿跟踪的空间参数折线示意图，图4b为本发明实施例所述的动态视觉的感知观测跟踪控制方法位姿跟踪的关节角折线示意图。
在本发明实施例中，该动态视觉的感知观测跟踪控制方法通过以下案例进行说明，若初始状态下，VR设备在第一坐标系下初始状态的坐标为[x0m,y0m,z0m]＝[0,0,0]及其旋转角为[ψ0m,θ0m,φ0m]＝[0,0,0]，而机器人末端摄像设备在第三坐标系下初始状态的坐标为[x0s,y0s,z0s]＝[0.428,-0.0068,0.3005]及其旋转角为
通过该动态视觉的感知观测跟踪控制方法分析映射得到空间参数和各个关节角，如图4a和图4b所示，通过图4a可知VR设备生成的参考信号已经换算为在第三坐标系下的表达，下标ref表示VR设备生成的末端参考位姿轨迹信号，下标act表示与机器人末端固连的摄像设备实际空间位姿信号。该动态视觉感知系统在工作空间和关节空间的跟踪平均误差和标准差统计分别如下表1和表2所示。
表1为机器人末端的误差统计表
表2为关节角的误差统计表
由上表1可知，该动态视觉的感知观测跟踪控制方法对观测目标的位置跟踪平均误差不大于1.7mm，姿态跟踪平均误差小于2.36°。在本实施例中，误差主要来源于动态视觉感知系统的时间滞后影响，主要表现在标准差远大于误差均值，如表1和表2所示；而在跟踪效果上表现为参考轨迹变化趋势发生变化后，跟踪误差会有大幅上升并逐渐回落，如图4b中虚线所示。该动态视觉感知系统滞后的主要来源是由VR设备、机器人的固有工作频率差异造成。VR设备质量轻、惯性小且一般直接驱动运动，工作频率较高；串联机器人关节质量较大且由电机驱动，工作频率较低，这导致该动态视觉感知系统本身是一个低通滤波器，机器人始终无法跟踪VR设备生成的轨迹信号中的高频成分，关节空间的跟踪始终存在滞后。
实施例二：
图5为本发明实施例所述的动态视觉的感知观测跟踪控制装置的框架图。
如图5所示，本发明实施例还提供一种动态视觉的感知观测跟踪控制装置，应用于动态视觉感知系统上，动态视觉感知系统包括VR设备和机器人，机器人的末端设置有摄像设备，该装置包括坐标建立模块10、参数获取模块20、映射处理模块30和执行观测模块40；
坐标建立模块10，用于对VR设备、机器人和摄像设备分别建立对应的第一坐标系、第二坐标系和第三坐标系；
参数获取模块20，用于实时获取VR设备在第一坐标系下观测目标的第一空间参数，以及实时获取摄像设备在第三坐标系下的第三空间参数；
映射处理模块30，用于通过逆运动学公式对第一空间参数和第三空间参数进行映射处理，得到机器人的关节角；
执行观测模块40，用于通过关节角控制机器人运行，以控制摄像设备实时对观测目标动态观测。
需要说明的是，实施例二装置中的模块对应于实施例一方法中的步骤，实施例一方法中的步骤已在实施例一中详细阐述了，在此实施例二中不再对装置中的模块内容进行详细阐述。
实施例三：
本发明实施例提供了一种动态视觉的感知观测跟踪控制设备，包括处理器以及存储器；
存储器，用于存储程序代码，并将程序代码传输给处理器；
处理器，用于根据程序代码中的指令执行上述的动态视觉的感知观测跟踪控制方法。
需要说明的是，处理器用于根据所程序代码中的指令执行上述的一种动态视觉的感知观测跟踪控制方法实施例中的步骤。或者，处理器执行计算机程序时实现上述各系统/装置实施例中各模块/单元的功能。
示例性的，计算机程序可以被分割成一个或多个模块/单元，一个或者多个模块/单元被存储在存储器中，并由处理器执行，以完成本申请。一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述计算机程序在终端设备中的执行过程。
终端设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。终端设备可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，并不构成对终端设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如终端设备还可以包括输入输出设备、网络接入设备、总线等。
所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
存储器可以是终端设备的内部存储单元，例如终端设备的硬盘或内存。存储器也可以是终端设备的外部存储设备，例如终端设备上配备的插接式硬盘，智能存储卡(SmartMedia Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，存储器还可以既包括终端设备的内部存储单元也包括外部存储设备。存储器用于存储计算机程序以及终端设备所需的其他程序和数据。存储器还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (10)
1.一种动态视觉的感知观测跟踪控制方法，应用于动态视觉感知系统上，其特征在于，所述动态视觉感知系统包括VR设备和机器人，所述机器人的末端设置有摄像设备，该方法包括以下步骤：
对所述VR设备、所述机器人和所述摄像设备分别建立对应的第一坐标系、第二坐标系和第三坐标系；
实时获取所述VR设备在第一坐标系下观测目标的第一空间参数，以及实时获取所述摄像设备在第三坐标系下的第三空间参数；
通过逆运动学公式对所述第一空间参数和所述第三空间参数进行映射处理，得到机器人的关节角；
通过所述关节角控制所述机器人运行，以控制所述摄像设备实时对观测目标动态观测。
2.根据权利要求1所述的动态视觉的感知观测跟踪控制方法，其特征在于，所述逆运动学公式为：[x,y,z,ψ,θ,φ]m＝f-1([x,y,z,ψ,θ,φ]s)＝f-1(g-1(q))；f为VR设备获取的第一空间参数到机器人末端的映射模型；g为机器人末端位姿到机器人各关节角大小的映射模型；q＝[q1,q2,…,q6]为机器人的关节角，[x,y,z,ψ,θ,φ]m为第一坐标系x、y、z轴对应的坐标xm、ym、zm和旋转角ψm、θm、φm；[x,y,z,ψ,θ,φ]s为第三坐标系x、y、z轴对应坐标xs、ys、zs和旋转角ψs、θs、φs。
3.根据权利要求1所述的动态视觉的感知观测跟踪控制方法，其特征在于，通过逆运动学公式对所述第一空间参数和所述第三空间参数进行映射处理，得到机器人的关节角的步骤包括：
将所述第一空间参数和所述第三空间参数通过对应的坐标系三轴建立第一旋转矩阵和第三旋转矩阵；
将所述第一坐标系的a、o、n轴与所述第三坐标系的n'、o'、a'轴进行映射对应，得到所述VR设备在所述第三坐标系上的映射旋转等式；
通过所述第一旋转矩阵、所述第三旋转矩阵代入所述映射旋转等式，得到所述VR设备映射到所述第三坐标系的空间参数；
获取所述机器人在所述第二坐标系下的各个关节参数；
通过多元方程组对所述空间参数逐一与每个关节参数进行求解，得到机器人的各个关节角。
4.根据权利要求3所述的动态视觉的感知观测跟踪控制方法，其特征在于，所述第一旋转矩阵为：
式中，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，oxm、oym、ozm分别为第一坐标系x、y、z轴在o轴的坐标，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，rz(φ)m为第一坐标系z轴旋转后的旋转角，ry(θ)m为第一坐标系y轴旋转后的旋转角，rx(ψ)m为第一坐标x轴旋转后的旋转角；
所述第三旋转矩阵为：
式中，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，o'xs、o'ys、o'zs分别为第三坐标系x、y、z轴在o'轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，rz(φ')s为第三坐标系z轴旋转后的旋转角，ry(θ')s为第三坐标系y轴旋转后的旋转角，rx(ψ')s为第三坐标x轴旋转后的旋转角。
5.根据权利要求3所述的动态视觉的感知观测跟踪控制方法，其特征在于，所述映射旋转等式为：
式中，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，oxm、oym、ozm分别为第一坐标系x、y、z轴在o轴的坐标，nxm、nym、nzm分别为第一坐标系x、y、z轴在n轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，o'xs、o'ys、o'zs分别为第三坐标系x、y、z轴在o'轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标。
6.根据权利要求3所述的动态视觉的感知观测跟踪控制方法，其特征在于，所述第三坐标系的空间参数为：
ozm＝cos(θ)sin(ψ)
azm＝cos(ψ)cos(θ)
nzm＝-sin(θ)
aym＝cos(ψ)sin(θ)sin(φ)-cos(φ)sin(ψ)
axm＝cos(ψ)cos(φ)sin(θ)+sin(ψ)sin(φ)
式中，k为VR设备与机器人之间的映射比例系数，ψ、θ、φ分别为第一坐标系x、y、z轴对应的旋转角，x0、y0、z0分为为VR设备映射在第三坐标系的坐标，ψ0、θ0、φ0分别为VR设备映射在第三坐标系上与x0、y0、z0对应的角度。
7.根据权利要求1所述的动态视觉的感知观测跟踪控制方法，其特征在于，包括：获取观测目标在所述第一坐标系下的第一坐标位置参数；通过坐标变换关系对所述第一坐标位置参数处理，得到观测目标在第三坐标系下的第三坐标位置参数。
8.根据权利要求7所述的动态视觉的感知观测跟踪控制方法，其特征在于，所述坐标变换关系为：
式中，pin_eye为第一坐标系下的第一坐标位置参数，pin_cartesian为第三坐标系下的第三坐标位置参数，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，o'xs、o'ys、o'zs分别为第三坐标系x、y、z轴在o'轴的坐标，n'xs、n'ys、n'zs分别为第三坐标系x、y、z轴在n'轴的坐标，x's、y's、z's分别为第三坐标系x、y、z轴的坐标。
9.一种动态视觉的感知观测跟踪控制装置，应用于动态视觉感知系统上，其特征在于，所述动态视觉感知系统包括VR设备和机器人，所述机器人的末端设置有摄像设备，该装置包括坐标建立模块、参数获取模块、映射处理模块和执行观测模块；
所述坐标建立模块，用于对所述VR设备、所述机器人和所述摄像设备分别建立对应的第一坐标系、第二坐标系和第三坐标系；
所述参数获取模块，用于实时获取所述VR设备在第一坐标系下观测目标的第一空间参数，以及实时获取所述摄像设备在第三坐标系下的第三空间参数；
所述映射处理模块，用于通过逆运动学公式对所述第一空间参数和所述第三空间参数进行映射处理，得到机器人的关节角；
所述执行观测模块，用于通过所述关节角控制所述机器人运行，以控制所述摄像设备实时对观测目标动态观测。
10.一种动态视觉的感知观测跟踪控制设备，其特征在于，包括处理器以及存储器；
所述存储器，用于存储程序代码，并将所述程序代码传输给所述处理器；
所述处理器，用于根据所述程序代码中的指令执行如权利要求1-8任意一项所述的动态视觉的感知观测跟踪控制方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111487928.3A CN113997293A (zh) | 2021-12-07 | 2021-12-07 | 一种动态视觉的感知观测跟踪控制方法、装置及设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111487928.3A CN113997293A (zh) | 2021-12-07 | 2021-12-07 | 一种动态视觉的感知观测跟踪控制方法、装置及设备 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113997293A true CN113997293A (zh) | 2022-02-01 |
Family
ID=79931505
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111487928.3A Pending CN113997293A (zh) | 2021-12-07 | 2021-12-07 | 一种动态视觉的感知观测跟踪控制方法、装置及设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113997293A (zh) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017151996A1 (en) * | 2016-03-04 | 2017-09-08 | Covidien Lp | Inverse kinematic control systems for robotic surgical system |
| CN107309882A (zh) * | 2017-08-14 | 2017-11-03 | 青岛理工大学 | 一种机器人示教编程系统及方法 |
| CN110900581A (zh) * | 2019-12-27 | 2020-03-24 | 福州大学 | 基于RealSense相机的四自由度机械臂视觉伺服控制方法及装置 |
| CN113093914A (zh) * | 2021-04-21 | 2021-07-09 | 广东电网有限责任公司电力科学研究院 | 一种基于vr的高临场视觉感知方法及装置 |
-
2021
- 2021-12-07 CN CN202111487928.3A patent/CN113997293A/zh active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017151996A1 (en) * | 2016-03-04 | 2017-09-08 | Covidien Lp | Inverse kinematic control systems for robotic surgical system |
| CN107309882A (zh) * | 2017-08-14 | 2017-11-03 | 青岛理工大学 | 一种机器人示教编程系统及方法 |
| CN110900581A (zh) * | 2019-12-27 | 2020-03-24 | 福州大学 | 基于RealSense相机的四自由度机械臂视觉伺服控制方法及装置 |
| CN113093914A (zh) * | 2021-04-21 | 2021-07-09 | 广东电网有限责任公司电力科学研究院 | 一种基于vr的高临场视觉感知方法及装置 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Asfour et al. | The karlsruhe humanoid head | |
| CN105555222B (zh) | 医用机械臂装置、医用机械臂控制系统、医用机械臂控制方法、及程序 | |
| CN111801198B (zh) | 一种手眼标定方法、系统及计算机存储介质 | |
| US8996176B2 (en) | Robot apparatus, method for controlling the same, and computer program | |
| CN111553239B (zh) | 一种机器人关节视觉伺服控制方法、终端设备及存储介质 | |
| US8744215B2 (en) | Methods and processes to aggregate multiple image feeds and dynamically select command reference frames | |
| US11833692B2 (en) | Method and device for controlling arm of robot | |
| US11331806B2 (en) | Robot control method and apparatus and robot using the same | |
| CN109079787B (zh) | 基于神经网络的非刚性机器人自动手眼标定方法 | |
| CN109785322B (zh) | 单眼人体姿态估计网络训练方法、图像处理方法和装置 | |
| CN111775145B (zh) | 一种串并联机器人的控制系统 | |
| US20060287772A1 (en) | Device and method for programming an industrial robot | |
| CN112083800A (zh) | 基于自适应手指关节规则滤波的手势识别方法及系统 | |
| CN110465946B (zh) | 一种像素坐标与机器人坐标关系标定方法 | |
| DeJong et al. | Mental transformations in human-robot interaction | |
| Hernández-Ordoñez et al. | An education application for teaching robot arm manipulator concepts using augmented reality | |
| CN116766194A (zh) | 基于双目视觉的盘类工件定位与抓取系统和方法 | |
| CN113070877B (zh) | 一种面向七轴机械臂视觉示教的可变姿态映射方法 | |
| CN113997293A (zh) | 一种动态视觉的感知观测跟踪控制方法、装置及设备 | |
| CN113084791A (zh) | 机械臂控制方法、机械臂控制装置及终端设备 | |
| Ranne et al. | Augmented reality-assisted reconfiguration and workspace visualization of malleable robots: Workspace modification through holographic guidance | |
| Nelson et al. | Vision resolvability for visually servoed manipulation | |
| Buys et al. | Haptic coupling with augmented feedback between two KUKA Light-Weight Robots and the PR2 robot arms | |
| Mei et al. | Design of intelligent 3D collaborative manufacturing platform for non-holonomic mobile industrial robots based on improved binocular vision | |
| TWI834495B (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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20220201 |
|
| RJ01 | Rejection of invention patent application after publication |