CN111230866B - 一种六轴机器人末端跟随目标物实时位姿的计算方法 - Google Patents

一种六轴机器人末端跟随目标物实时位姿的计算方法 Download PDF

Info

Publication number
CN111230866B
CN111230866B CN202010044804.7A CN202010044804A CN111230866B CN 111230866 B CN111230866 B CN 111230866B CN 202010044804 A CN202010044804 A CN 202010044804A CN 111230866 B CN111230866 B CN 111230866B
Authority
CN
China
Prior art keywords
coordinate system
robot
target object
axis
following
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
CN202010044804.7A
Other languages
English (en)
Other versions
CN111230866A (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.)
Shanxi Wanhe Intelligent Technology Co ltd
Original Assignee
Shanxi Wanhe Intelligent Technology 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 Shanxi Wanhe Intelligent Technology Co ltd filed Critical Shanxi Wanhe Intelligent Technology Co ltd
Priority to CN202010044804.7A priority Critical patent/CN111230866B/zh
Publication of CN111230866A publication Critical patent/CN111230866A/zh
Application granted granted Critical
Publication of CN111230866B publication Critical patent/CN111230866B/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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses

Landscapes

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

Abstract

本发明属于位姿计算技术领域，具体涉及一种六轴机器人末端跟随目标物实时位姿的计算方法，一种六轴机器人末端跟随目标物实时位姿的计算方法，包括下列步骤：通过获取运动的目标物的参数，经过位姿转换、矩阵运算可以得到目标物的实时位姿，并推算出机器人末端应该调整的位姿。本发明实现了六轴机器人末端实时跟随目标物，能够实时的求解追踪与机器人没有任何连接关系的目标物，并推算出机器人末端应该调整的位姿；而且通过该算法使得机器人末端始终与工作台保持水平。本发明用于机器人末端跟随目标物实时位姿的计算。

Description

一种六轴机器人末端跟随目标物实时位姿的计算方法
技术领域
本发明属于位姿计算技术领域，具体涉及一种六轴机器人末端跟随目标物实时位姿的计算方法。
背景技术
机器人的运动学是在不考虑引起运动的力和力矩的情况下，描述机械臂的运动。机器人正运动学原理根据给定的机器人关节变量确定末端执行器的位置和姿态；机器人逆运动学原理则是根据给定的末端执行器的位姿确定机器人关节变量的取值。
正运动学与逆运动学只是解决了机器人上固连在一起的连杆与关节的位姿问题，无法求解追踪与机器人没有任何连接关系的目标物。
发明内容
针对上述正运动学与逆运动学无法求解追踪与机器人没有连接关系的目标物的技术问题，本发明提供了一种六轴机器人末端跟随目标物实时位姿的计算方法。
为了解决上述技术问题，本发明采用的技术方案为：
一种六轴机器人末端跟随目标物实时位姿的计算方法，包括下列步骤：
S1、以六轴机器人第一轴与工作台的交点为原点建立世界坐标系；
S2、在各轴、机器人末端以及目标物依次建立各自坐标系；
S3、依据机器人正运动学原理得到各个关节之间的齐次变换矩阵，将矩阵相乘得到当前状态下机器人末端相对于世界坐标系的位姿
Figure BDA0002368965170000021
S4、计算目标物坐标系相对于机器人末端坐标系的齐次变换矩阵记为B；
S5、根据目标物的参数求解目标物相对于目标物自身的初始坐标系的齐次变换矩阵为C；
S6、求解当前目标物在世界坐标系的位姿：T＝A*B*C；
S7、为了使机器人末端始终正对着要跟随的目标物体并保持一定距离，那么新的状态下机械臂末端相对于世界坐标系的矩阵记为
Figure BDA0002368965170000022
S8、计算目标物坐标系相对于机器人末端坐标系的变换矩阵记为B'；
S9、计算目标物相对于目标物自身坐标系的旋转矩阵C'；
S10、求解目标物在世界坐标系的位姿T＝A'*B'*C'；
S11、求解机器人末端相对于世界坐标系新的位姿A'；
S12、为了使机器人末端的水平轴能够始终平行于工作台，将A'矩阵与绕末端轴轴线的旋转的齐次矩阵Rot相乘，得到新的矩阵记为H
Figure BDA0002368965170000023
Figure BDA0002368965170000024
当机器人末端的水平轴是x轴，那么经过矩阵A'与Rot相乘x轴在新坐标系下的z轴的向量分量nz,H应为0，即：
nz,H＝nz'*nx,R+sz'*ny,R+az'*nz,R+pz'*0＝0
根据上述等式求出末端轴的偏差角，再通过机器人逆运动学可得到各关节应转动的角度θ1、θ2、θ3、θ4、θ5、θ6，从而得到六轴需要转动的角度，便可以使得机器人末端水平轴始终平行于工作台；
S13、重复S3，通过不断更新位姿，保持实时的追踪。
所述S4中计算机器人末端坐标系的齐次变换矩B的方法为：相机在末端坐标系的安装位置为(a，b，c)，相机采集到目标物相对于相机的位置坐标(x，y，z)，那么目标物在末端坐标系的位置坐标为(a+x，b+y，c+z)，目标物坐标系相对于机器人末端坐标系先绕x轴旋转90度，再绕z轴旋转 90度，计算目标物坐标系相对于机器人末端坐标系的变换矩阵
Figure BDA0002368965170000031
所述S5中求解目标物相对于目标物自身的初始坐标系的齐次变换矩阵C的方法为：设目标物在坐标系下的滚动角为R、俯仰角为P、偏航角为Y，计算其旋转矩阵为
Figure BDA0002368965170000032
所述S8中机器人末端坐标系的变换矩阵记B'的求解方法为：设始终保持的距离为Z，坐标系先绕x轴旋转90度，再绕z轴旋转90度，计算目标物相对于机器人末端的转换矩阵为
Figure BDA0002368965170000041
所述S9中目标物相对于目标物自身坐标系的旋转矩阵C'的求解方法为：由于机器人末端正对着目标物，因此目标物在坐标系下滚动角、俯仰角、偏航角都为0，即目标物相对于目标物自身坐标系的旋转矩阵为单位矩阵
Figure BDA0002368965170000042
所述S11中机器人末端相对于世界坐标系新的位姿A'的求解方法为：根据 T＝A*B*C和T＝A'*B'*C'两个等式，可得到新的状态下机械臂末端相对于世界坐标系的矩阵A'。
所述S12中偏差角的计算方法为：β＝-arctan(nz'/sz')。
所述S12中六轴需要转动的角度的计算方法为：在末端轴逆解的基础上再减去偏差角，得出六轴需要转动的角度分别为θ1、θ2、θ3、θ4、θ5、θ6－β。
本发明与现有技术相比，具有的有益效果是：
本发明实现了六轴机器人末端实时跟随目标物，能够实时的求解追踪与机器人没有任何连接关系的目标物，并推算出机器人末端应该调整的位姿；而且通过该算法使得机器人末端始终与工作台保持水平。
附图说明
图1为本发明的位置坐标示意图；
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
一种六轴机器人末端跟随目标物实时位姿的计算方法，包括下列步骤：
S1、如图1所示，以六轴机器人第一轴与工作台的交点O0为原点建立世界坐标系X0Y0Z0。
S2、在各个关节轴建立坐标系、机器人末端建立坐标系X6Y6Z6以及在目标物建立坐标系XMYMZM。
S3、依据机器人正运动学原理得到各个关节之间的齐次变换矩阵，将矩阵相乘得到当前状态下机器人末端相对于世界坐标系的位姿
Figure BDA0002368965170000051
S4、相机在末端坐标系的安装位置为(a，b，c)，相机采集到目标物相对于相机的位置坐标(x，y，z)，那么目标物在末端坐标系的位置坐标为(a+x， b+y，c+z)，目标物坐标系相对于机器人末端坐标系先绕x轴旋转90度，再绕 z轴旋转90度，计算目标物坐标系相对于机器人末端坐标系的变换矩阵
Figure BDA0002368965170000052
S5、设目标物在坐标系下的滚动角为R、俯仰角为P、偏航角为Y，计算目标物相对于目标物自身的初始坐标系的齐次变换矩
Figure BDA0002368965170000061
S6、求解当前目标物在世界坐标系的位姿：T＝A*B*C。
S7、为了使机器人末端始终正对着要跟随的目标物体并保持一定距离，那么新的状态下机械臂末端相对于世界坐标系的矩阵记为
Figure BDA0002368965170000062
S8、设始终保持的距离为Z，坐标系先绕x轴旋转90度，再绕z轴旋转90 度，计算目标物相对于机器人末端的转换矩阵为
Figure BDA0002368965170000063
S9、由于机器人末端正对着目标物，因此目标物在坐标系下滚动角、俯仰角、偏航角都为0，即目标物相对于目标物自身坐标系的旋转矩阵为单位矩阵
Figure BDA0002368965170000064
S10、求解目标物在世界坐标系的位姿T＝A'*B'*C'。
S11、求解机器人末端相对于世界坐标系新的位姿A'，根据T＝A*B*C和 T＝A'*B'*C'两个等式，可得到新的状态下机械臂末端相对于世界坐标系的矩阵 A'。
S12、为了使机器人末端的水平轴能够始终平行于工作台，将A'矩阵与绕末端轴轴线的旋转的齐次矩阵Rot相乘，得到新的矩阵记为H
Figure BDA0002368965170000071
Figure BDA0002368965170000072
当机器人末端的水平轴是x轴，那么经过矩阵A'与Rot相乘x轴在新坐标系下的z轴的向量分量nz,H应为0，即：
nz,H＝nz'*nx,R+sz'*ny,R+az'*nz,R+pz'*0＝0
根据上述等式求出末端轴的偏差角，偏差角的计算方法为：β＝-arctan(nz'/sz')，再通过机器人逆运动学可得到各关节应转动的角度θ1、θ2、θ3、θ4、θ5、θ6，在末端轴逆解的基础上再减去偏差角，得出六轴需要转动的角度分别为θ1、θ2、θ3、θ4、θ5、θ6－β，便可以使得机器人末端水平轴始终平行于工作台。
S13、重复S3，通过不断更新位姿，保持实时的追踪。
上面仅对本发明的较佳实施例作了详细说明，但是本发明并不限于上述实施例，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化，各种变化均应包含在本发明的保护范围之内。

Claims (7)

1.一种六轴机器人末端跟随目标物实时位姿的计算方法，其特征在于：包括下列步骤：
S1、以六轴机器人第一轴与工作台的交点为原点建立世界坐标系；
S2、在各轴、机器人末端以及目标物依次建立各自坐标系；
S3、依据机器人正运动学原理得到各个关节之间的齐次变换矩阵，将矩阵相乘得到当前状态下机器人末端相对于世界坐标系的位姿
Figure FDA0003331686710000011
S4、计算目标物坐标系相对于机器人末端坐标系的齐次变换矩阵记为B；
S5、根据目标物的参数求解目标物相对于目标物自身的初始坐标系的齐次变换矩阵为C；
S6、求解当前目标物在世界坐标系的位姿：T＝A*B*C；
S7、为了使机器人末端始终正对着要跟随的目标物体并保持一定距离，那么新的状态下机械臂末端相对于世界坐标系的矩阵记为
Figure FDA0003331686710000012
S8、计算目标物坐标系相对于机器人末端坐标系的变换矩阵记为B'；
S9、计算目标物相对于目标物自身坐标系的旋转矩阵C'；
所述目标物相对于目标物自身坐标系的旋转矩阵C'的求解方法为：由于机器人末端正对着目标物，因此目标物在坐标系下滚动角、俯仰角、偏航角都为0，即目标物相对于目标物自身坐标系的旋转矩阵为单位矩阵
Figure FDA0003331686710000021
S10、求解目标物在世界坐标系的位姿T＝A'*B'*C'；
S11、求解机器人末端相对于世界坐标系新的位姿A'；
S12、为了使机器人末端的水平轴能够始终平行于工作台，将A'矩阵与绕末端轴轴线的旋转的齐次矩阵Rot相乘，得到新的矩阵记为H
Figure FDA0003331686710000022
Figure FDA0003331686710000023
当机器人末端的水平轴是x轴，那么经过矩阵A'与Rot相乘x轴在新坐标系下的z轴的向量分量nz,H应为0，即：
nz,H＝nz'*nx,R+sz'*ny,R+az'*nz,R+pz'*0＝0
根据上述等式求出末端轴的偏差角，再通过机器人逆运动学可得到各关节应转动的角度θ1、θ2、θ3、θ4、θ5、θ6，从而得到六轴需要转动的角度，便可以使得机器人末端水平轴始终平行于工作台；
S13、重复S3，通过不断更新位姿，保持实时的追踪。
2.根据权利要求1所述的一种六轴机器人末端跟随目标物实时位姿的计算方法，其特征在于：所述S4中计算机器人末端坐标系的齐次变换矩B的方法为：相机在末端坐标系的安装位置为(a，b，c)，相机采集到目标物相对于相机的位置坐标(x，y，z)，那么目标物在末端坐标系的位置坐标为(a+x，b+y，c+z)，目标物坐标系相对于机器人末端坐标系先绕x轴旋转90度，再绕z轴旋转90度，计算目标物坐标系相对于机器人末端坐标系的变换矩阵
Figure FDA0003331686710000031
3.根据权利要求1所述的一种六轴机器人末端跟随目标物实时位姿的计算方法，其特征在于：所述S5中求解目标物相对于目标物自身的初始坐标系的齐次变换矩阵C的方法为：设目标物在坐标系下的滚动角为R、俯仰角为P、偏航角为Y，那么计算其旋转矩阵为
Figure FDA0003331686710000032
4.根据权利要求1所述的一种六轴机器人末端跟随目标物实时位姿的计算方法，其特征在于：所述S8中机器人末端坐标系的变换矩阵记B'的求解方法为：设始终保持的距离为Z，坐标系先绕x轴旋转90度，再绕z轴旋转90度，计算目标物相对于机器人末端的转换矩阵为
Figure FDA0003331686710000033
5.根据权利要求1所述的一种六轴机器人末端跟随目标物实时位姿的计算方法，其特征在于：所述S11中机器人末端相对于世界坐标系新的位姿A'的求解方法为：根据T＝A*B*C和T＝A'*B'*C'两个等式，可得到新的状态下机械臂末端相对于世界坐标系的矩阵A'。
6.根据权利要求1所述的一种六轴机器人末端跟随目标物实时位姿的计算方法，其特征在于：所述S12中偏差角的计算方法为：β＝-arctan(nz'/sz')。
7.根据权利要求1所述的一种六轴机器人末端跟随目标物实时位姿的计算方法，其特征在于：所述S12中六轴需要转动的角度的计算方法为：在末端轴逆解的基础上再减去偏差角，得出六轴需要转动的角度分别为θ1、θ2、θ3、θ4、θ5、θ6－β。
CN202010044804.7A 2020-01-16 2020-01-16 一种六轴机器人末端跟随目标物实时位姿的计算方法 Active CN111230866B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010044804.7A CN111230866B (zh) 2020-01-16 2020-01-16 一种六轴机器人末端跟随目标物实时位姿的计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010044804.7A CN111230866B (zh) 2020-01-16 2020-01-16 一种六轴机器人末端跟随目标物实时位姿的计算方法

Publications (2)

Publication Number Publication Date
CN111230866A CN111230866A (zh) 2020-06-05
CN111230866B true CN111230866B (zh) 2021-12-28

Family

ID=70877874

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010044804.7A Active CN111230866B (zh) 2020-01-16 2020-01-16 一种六轴机器人末端跟随目标物实时位姿的计算方法

Country Status (1)

Country Link
CN (1) CN111230866B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111905983B (zh) * 2020-07-17 2021-06-29 深圳市华成工业控制股份有限公司 基于视觉跟随的点胶轨迹修正方法、装置、系统及介质
CN113274136B (zh) * 2021-05-17 2023-02-10 上海微创医疗机器人（集团）股份有限公司 位姿调整方法、手术机器人系统和存储介质
CN114012719B (zh) * 2021-10-22 2024-04-16 上海发那科机器人有限公司 一种六轴机器人的零点标定方法及系统
CN114701372A (zh) * 2022-03-24 2022-07-05 北京启智新源科技有限公司 一种基于机械臂的自动无线充电系统及方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04116703A (ja) * 1990-09-07 1992-04-17 Komatsu Ltd ロボットの制御方法
WO2000045229A1 (en) * 1999-01-29 2000-08-03 Georgia Tech Research Corporation Uncalibrated dynamic mechanical system controller
CN101750200A (zh) * 2009-12-30 2010-06-23 航天东方红卫星有限公司 一种高分辨率小卫星颤振响应的确定方法
CN105659726B (zh) * 2009-12-01 2013-06-19 北京空间飞行器总体设计部 基于多路视觉融合的大型空间机械臂自主运动规划方法
CN109909998A (zh) * 2017-12-12 2019-06-21 北京猎户星空科技有限公司 一种控制机械臂运动的方法及装置
CN110340886A (zh) * 2019-06-03 2019-10-18 武汉中观自动化科技有限公司 一种双目跟踪实现机器人路点迁移的方法及其系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04116703A (ja) * 1990-09-07 1992-04-17 Komatsu Ltd ロボットの制御方法
WO2000045229A1 (en) * 1999-01-29 2000-08-03 Georgia Tech Research Corporation Uncalibrated dynamic mechanical system controller
CN105659726B (zh) * 2009-12-01 2013-06-19 北京空间飞行器总体设计部 基于多路视觉融合的大型空间机械臂自主运动规划方法
CN101750200A (zh) * 2009-12-30 2010-06-23 航天东方红卫星有限公司 一种高分辨率小卫星颤振响应的确定方法
CN109909998A (zh) * 2017-12-12 2019-06-21 北京猎户星空科技有限公司 一种控制机械臂运动的方法及装置
CN110340886A (zh) * 2019-06-03 2019-10-18 武汉中观自动化科技有限公司 一种双目跟踪实现机器人路点迁移的方法及其系统

Also Published As

Publication number Publication date
CN111230866A (zh) 2020-06-05

Similar Documents

Publication Publication Date Title
CN111230866B (zh) 一种六轴机器人末端跟随目标物实时位姿的计算方法
CN107685330B (zh) 一种六自由度手腕偏置串联机器人的运动学逆解求解方法
CN107901041B (zh) 一种基于图像混合矩的机器人视觉伺服控制方法
CN104965517B (zh) 一种机器人笛卡尔空间轨迹的规划方法
JP5114019B2 (ja) エフェクタの軌道を制御するための方法
CN107791248B (zh) 基于不满足Pieper准则的六自由度串联机器人的控制方法
Yu et al. An analytical solution for inverse kinematic of 7-DOF redundant manipulators with offset-wrist
CN107584474B (zh) 一种基于几何投影的球面关节双臂机器人协调运动方法
CN108908347B (zh) 一种面向冗余移动机械臂容错型重复运动规划方法
CN110712203B (zh) 一种7自由度机械臂逆运动学求解算法
CN111300425A (zh) 一种超冗余机械臂末端轨迹运动规划方法
CN109648230B (zh) 基于六自由度机器人扩展双旋转协同功能轴的摆焊方法
CN113510690B (zh) 四自由度串联机器人逆运动学求解方法和系统
CN107253191B (zh) 一种双机械臂系统及其协调控制方法
CN106844951B (zh) 基于分段几何法求解超冗余机器人逆运动学的方法及系统
CN110695988A (zh) 双机械臂协同运动方法及系统
CN106514636A (zh) 一种机器人末端位姿分析方法
CN111496783A (zh) 一种6r工业机器人逆运动学求解方法
Wang et al. A vision-based coordinated motion scheme for dual-arm robots
CN112276940A (zh) 一种六自由度非球型手腕机器人逆运动学求解方法
CN116276990A (zh) 基于神经网络训练的两自由度并联结构运动学正解方法
CN111430022A (zh) 一种穿刺手术机器人穿刺算法
CN113722864B (zh) 一种7自由度冗余机械臂逆运动学求解方法及系统
CN116330267A (zh) 一种基于工业机器人腕部奇异点计算的控制方法
JP2019093487A (ja) ロボットの制御装置及びロボットの逆変換処理方法

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