CN111438689B - 工具头位姿的调整方法、装置及可读存储介质 - Google Patents

工具头位姿的调整方法、装置及可读存储介质 Download PDF

Info

Publication number
CN111438689B
CN111438689B CN202010194511.7A CN202010194511A CN111438689B CN 111438689 B CN111438689 B CN 111438689B CN 202010194511 A CN202010194511 A CN 202010194511A CN 111438689 B CN111438689 B CN 111438689B
Authority
CN
China
Prior art keywords
laser
tool head
pose
normal vector
calculating
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
CN202010194511.7A
Other languages
English (en)
Other versions
CN111438689A (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.)
Zhimei Kangmin (Zhuhai) Health Technology Co., Ltd
Original Assignee
Zhimei Kangmin Zhuhai Health 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 Zhimei Kangmin Zhuhai Health Technology Co Ltd filed Critical Zhimei Kangmin Zhuhai Health Technology Co Ltd
Priority to CN202010194511.7A priority Critical patent/CN111438689B/zh
Publication of CN111438689A publication Critical patent/CN111438689A/zh
Priority to JP2022557115A priority patent/JP7415036B2/ja
Priority to CA3174424A priority patent/CA3174424A1/en
Priority to PCT/CN2020/136749 priority patent/WO2021184859A1/zh
Priority to EP20925967.0A priority patent/EP4116042A4/en
Priority to KR1020227035574A priority patent/KR20220154189A/ko
Application granted granted Critical
Publication of CN111438689B publication Critical patent/CN111438689B/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/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
    • 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
    • B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
    • B25J13/089—Determining the position of the robot with reference to its environment
    • 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
    • B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
    • 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/022—Optical sensing devices using lasers
    • 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
    • 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
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/37—Measurements
    • G05B2219/37275—Laser, interferometer
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40613—Camera, laser scanner on end effector, hand eye manipulator, local
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40623—Track position of end effector by laser beam
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/45—Nc applications
    • G05B2219/45122—Laser skin treatment

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Optics & Photonics (AREA)
  • Human Computer Interaction (AREA)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种工具头位姿的调整方法、装置及可读存储介质，通过激光距离传感器检测到激光点的距离值，各激光点能构建至少一平面，从而确定待检测部位对应的各平面，并计算各平面的综合法向量，利用位姿表达式计算机器人工具头的位姿参数，从而调整工具头的位姿；本发明能够准确确定工具头待调整位姿的位姿参数，进而提高了艾灸仪的作业效率，并且，本发明的激光距离传感器成本低，机器人作业时，精度高、实时性强，能够适应工具头与人体接触式场景或保持人体一定距离的场景。

Description

工具头位姿的调整方法、装置及可读存储介质
技术领域
本发明涉及机器人技术领域，尤其涉及一种工具头位姿的调整方法、装置及可读存储介质。
背景技术
随着机器人技术的发展，机器人的性能和安全性有了长足的提升，人机协作机器人的出现，更是标志着人和机器人协同工作成为切实可行的事情。近十年现代工业飞速发展，机器人的普及应用也使得机器人的成本在近两年迅速靠近消费者可负担的水平。个人护理机器人作为机器人的一种，包括安装在机械手臂末端带有护理功能（按摩，艾灸，化妆，美容等）的工具头，该工具头与被护理的人体直接接触或者保持特定的距离和姿势来进行护理作业。
目前，确定护理机器人工作时位姿（位置和姿态）的解决方案主要有机器视觉方案、接触式多轴力传感器方案、雷达（超声波或者激光等）传感器。上述解决方案都有不尽人意的地方，例如，机器视觉方案的机器人价格昂贵，机器人视觉方案需要特殊光源照射，扫描时间和坐标生成时间长，实时性差，摄像头的体积大不利于集成在工具头，并且暴露了客户的隐私部位；接触式多轴力传感器方案的机器人价格昂贵，只适用于工具头与人体接触式场景，对工具头位姿指导没有前瞻性；雷达方案的机器人价格昂贵、精度差、盲区大、容易受干扰，且长时间雷达波的放射对人体健康有潜在风险。所以，如何提供一种价格低廉、精度高、实时性强及位姿确定准确的工具头位姿确定方案成为待解决的问题。
上述内容仅用于辅助理解本发明的技术方案，并不代表承认上述内容是现有技术。
发明内容
本发明的主要目的在于提供一种价格低廉、精度高、实时性强及位姿确定准确的工具头位姿确定方案。
为实现上述目的，本发明提供一种工具头位姿的调整方法，所述工具头位姿的调整方法包括：
控制激光距离传感器的激光点照射到待检测部位上，使得各所述激光点不在一条直线上，其中，所述激光距离传感器的数量大于2个；
获取各所述激光距离传感器测量的距离值、各所述激光距离传感器的初始坐标及各所述激光距离传感器的激光方向，其中，所述距离值为所述激光距离传感器与对应的所述激光点的距离；
基于所述距离值、所述初始坐标及所述激光方向计算所述待检测部位上各激光点的激光点坐标，计算各所述激光点坐标所确定的平面的综合平面法向量，其中，所述综合平面法向量由所述激光点坐标确定的平面的平面法向量求得；
基于预设的位姿表达式、所述距离值及所述综合平面法向量计算所述机器人的工具头待调整位姿的位姿参数，并基于所述位姿参数控制所述机器人的工具头调整至所述待调整位姿。
可选地，若所述激光距离传感器的数量大于3个，所述计算各所述激光点坐标所确定的平面的综合平面法向量的步骤包括：
计算所述激光点坐标所确定的对应平面的平面法向量；
利用加权平均算法计算各所述平面法向量的加权平均值，将所述加权平均值作为所述综合平面法向量。
可选地，所述位姿表达式包括欧拉角、四元数或者旋转矩阵表达式。
可选地，所述激光距离传感器的数量为3个，各所述激光距离传感器的激光束互相平行。
可选地，所述基于所述距离值、所述初始坐标及所述激光方向计算所述待检测部位上各激光点的激光点坐标的步骤包括：
设各所述激光方向的单位向量分别为Ra(rx1，ry1，rz1)、Rb(rx2，ry2，rz2)、Rc(rx3，ry3，rz3)，各所述激光距离传感器所在工具头坐标系的坐标为Pa(x1，y1，z1)、Pb(x2，y2，z2)、Pc(x3，y3，z3)，各所述距离值为d1、d2、d3，各所述激光点的坐标为Ua、Ub、Uc，则
Ua.x=x1+rx1*d1，Ua.y=y1+ry1*d1，Ua.z=z1+rz1*d1；
Ub.x=x2+rx2*d2，Ub.y=y2+ry2*d2，Ub.z=z2+rz2*d2；
Uc.x=x3+rx3*d3，Uc.y=y3+ry3*d3，Uc.z=z3+rz3*d3；
即求得各所述激光点的坐标Ua（Ua.x，Ua.y，Ua.z）、Ub（Ub.x，Uby，Ub.z）、Uc（Uc.x，Uc.y，Uc.z）。
可选地，所述基于预设的位姿表达式、所述距离值及所述综合平面法向量计算所述机器人的工具头待调整位姿的位姿参数的步骤包括：
设求得的所述综合平面法向量为V(vx，vy，vz)，约定vz>0，其中，若vz<0，则所述综合平面法向量乘以-1；
预设公式M（α，β，γ）*[0，0，1]T=Vnorm T，其中M（α，β，γ）为欧拉角旋转公式中的旋转矩阵，Vnorm表示模为1的所述综合平面法向量，α、β、γ分别表示欧拉角；
基于所述预设公式、所述距离值及欧拉角旋转公式求得所述位姿参数。
可选地，所述基于所述预设公式、所述距离值及欧拉角旋转公式求得所述位姿参数的步骤包括：
令rx=0，基于所述预设公式及所述欧拉角旋转公式联立方程组求出ry、rz，其中，rx、ry、rz分别为欧拉角α、β、γ；
基于所述距离值利用预设算法计算综合距离值，设为daver；
将所述工具头坐标系的原点在z方向加上daver，使得所述原点设置在所述待检测部位；
获取所述原点设置在所述待检测部位时，所述原点相对于机械手基座的基座坐标系的位姿参数，设为x0、y0、z0、rx0、ry0、rz0；
设所述工具头的待调整位姿的欧拉角参数为Rx、Ry、Rz，则
Rx=rx0，Ry=ry0+ry，Rz=rz0+rz，则
所述工具头的待调整位姿的位姿参数为x0、y0、z0、Rx、Ry、Rz。
可选地，所述预设算法包括平均值算法及加权平均算法。
此外，为实现上述目的，本发明还提供一种工具头位姿的调整装置，所述工具头位姿的调整装置包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的工具头位姿的调整程序，所述工具头位姿的调整程序被所述处理器执行时实现如上所述的工具头位姿的调整方法的步骤。
此外，为实现上述目的，本发明还提供一种可读存储介质，所述可读存储介质上存储有工具头位姿的调整程序，所述工具头位姿的调整程序被处理器执行时实现如上所述的工具头位姿的调整方法的步骤。
本发明提供一种工具头位姿的调整方法、装置可读存储介质，能够准确确定机器人工具头的位姿，从而提高了机器人的作业效率；本发明的激光距离传感器成本低、体积小、精度高，能够很好的集成在机器人的工具头上，从而有利于工具头位姿的确定，能够实时对激光距离器的位置进行调整，并且本发明能够适应工具头与人体接触式场景或保持人体一定距离的场景。
附图说明
图1为本发明工具头位姿的调整方法第一实施例的流程示意图；
图2为艾灸仪机器人在待检测部位进行针灸的示意图；
图3为第一实施例中3个激光距离传感器在工具头上的示意图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本申请，并不用于限定本申请。
本发明进一步提供一种工具头位姿的调整方法。参照图1，图1为本发明工具头位姿的调整方法第一实施例的流程示意图。
在本实施例中，该工具头位姿的调整方法的执行主体为工具头位姿的调整系统，该工具头位姿的调整系统包括工具头位姿的调整装置，该工具头位姿的调整装置可以是机器人设备，例如，艾灸仪、按摩仪等，当然，也可以是PC、掌上电脑等终端设备。本发明通过激光距离传感器检测到激光点的距离值，各激光点能构建至少一平面，从而确定待检测部位对应的各平面，并计算各平面的综合法向量，利用位姿表达式计算机器人工具头的位姿参数，从而调整工具头的位姿；本发明能够准确确定工具头待调整位姿的位姿参数。本实施例以艾灸仪为例进行说明。该工具头位姿的调整方法包括以下步骤：
步骤S10，控制激光距离传感器的激光点照射到待检测部位上，使得各所述激光点不在一条直线上，其中，所述激光距离传感器的数量大于2个；
在本实施例中，参照图2，图2为艾灸仪机器人在待检测部位进行针灸的示意图，该待检测部位包括患者待针灸的身体部位，该激光距离传感可以安装在针灸仪机器人的工具头上，例如，在机器人工具头具有固定联动关系的面上安装三个或三个以上的激光距离传感器2。当然，也可以安装在机器人的其它位置上，机械手1用于调整工具头的位姿。在待检测部位进入距离激光距离传感器2的量程范围时，各激光距离传感器2的激光束射到待检测部上，在待检测部位形成激光点，该激光距离传感器2的数量大于或等于3个，由于各激光点必须能够构建平面，所以，该激光点不能在同一直线上。优选地，避免该激光距离传感器2的激光束汇集于一点，该激光束互相平行，永远没有相交点，这样能够在计算激光点坐标时简化算法，从而提高数据处理效率，进而加快工具头位姿的调整效率。
步骤S20，获取各所述激光距离传感器测量的距离值、各所述激光距离传感器的初始坐标及各所述激光距离传感器的激光方向，其中，所述距离值为所述激光距离传感器与对应的所述激光点的距离；
在本实施例中，激光距离传感器2可以测量到激光点的距离，设定工具头坐标系，获取激光距离传感器2在工具头坐标系的初始坐标及激光距离传感器2的激光方向，该激光距离传感器2的激光方向可以用单位向量表示。
步骤S30，基于所述距离值、所述初始坐标及所述激光方向计算所述待检测部位上各激光点的激光点坐标，计算各所述激光点坐标所确定的平面的综合平面法向量，其中，所述综合平面法向量由所述激光点坐标确定的平面的平面法向量求得；
在本实施例中，参照图3，图3为3个激光距离传感器2在工具头上的示意图，若该激光距离传感器2为3个，则该综合平面法向量为三个激光点所在平面的平面法向量，其中，可以通过向量叉乘法计算得到该平面法向量；若该激光距离传感器2大于3个，则该综合法向量可以利用加权平均算法或者其他算法得到的法向量，例如，若各激光点能够构建四个平面，则分别通过向量叉乘法计算四个平面的法向量，然后通过加权平均算法计算四个法向量的加权平均值，从而得到综合法向量。
其中，若该激光距离传感器2数量为3个，各激光距离传感器2的激光束互相平行，设各激光方向的单位向量分别为Ra(rx1，ry1，rz1)、Rb(rx2，ry2，rz2)、Rc(rx3，ry3，rz3)，各激光距离传感器所在工具头坐标系的坐标为Pa(x1，y1，z1)、Pb(x2，y2，z2)、Pc(x3，y3，z3)，各距离值为d1、d2、d3，各激光点的坐标为Ua、Ub、Uc，则
Ua.x=x1+rx1*d1，Ua.y=y1+ry1*d1，Ua.z=z1+rz1*d1；
Ub.x=x2+rx2*d2，Ub.y=y2+ry2*d2，Ub.z=z2+rz2*d2；
Uc.x=x3+rx3*d3，Uc.y=y3+ry3*d3，Uc.z=z3+rz3*d3；
即求得各激光点的坐标Ua（Ua.x，Ua.y，Ua.z）、Ub（Ub.x，Uby，Ub.z）、Uc（Uc.x，Uc.y，Uc.z），通过向量叉乘法可以计算三个激光点的平面法向量。
步骤S40，基于预设的位姿表达式、所述距离值及所述综合平面法向量计算所述机器人的工具头待调整位姿的位姿参数，并基于所述位姿参数控制所述机器人的工具头调整至所述待调整位姿。
在本实施例中，该预设的位姿表达式包括欧拉角、四元数或者旋转矩阵等表达式，根据综合平面法向量及距离值利用位姿表达式求得机器人待调整位姿的位姿参数。
以下利用欧拉角旋转公式计算机器人待调整位姿的位姿参数。
设求得的综合平面法向量为V(vx，vy，vz)，约定vz>0，其中，若vz<0，则综合平面法向量乘以-1，其中，该处是为了计算平面综合法向量的单位法向量，从而确定该综合法向量的方向；
欧拉角旋转公式为：
Figure 577816DEST_PATH_IMAGE002
预设公式M（α，β，γ）*[0，0，1]T=Vnorm T，其中M（α，β，γ）为欧拉角旋转公式中的旋转矩阵，其中，α、β、γ分别表示欧拉角，Vnorm表示模为1的所述综合平面法向量，α、β、γ分别表示欧拉角。
令rx=0，基于所述预设公式及所述欧拉角旋转公式联立方程组求出ry、rz，其中，rx、ry、rz分别为欧拉角α、β、γ，基于距离值利用预设算法计算综合距离值，设为daver，将工具头坐标系的原点在z方向加上daver，使得该原点设置在待检测部位，然后获取该原点设置在该待检测部位时，该原点相对于机械手基座的基座坐标系的位姿参数，设为x0、y0、z0、rx0、ry0、rz0，设工具头的待调整位姿的欧拉角参数为Rx、Ry、Rz，则Rx=rx0，Ry=ry0+ry，Rz=rz0+rz，则工具头的待调整位姿的位姿参数为x0、y0、z0、Rx、Ry、Rz，其中，该预设算法包括平均值算法及加权平均算法，例如，利用平均值算法计算d1、d2、d3的平均值，得到daver。
根据待调整位姿的位姿参数调整工具头的位姿，从而机器人能够准确对待检测部位进行针灸护理。
本实施例提出逇工具头位姿的调整方法，通过控制激光距离传感器的激光点照射到待检测部位上，使得各所述激光点不在一条直线上，其中，所述激光距离传感器的数量大于2个，获取各所述激光距离传感器测量的距离值、各所述激光距离传感器的初始坐标及各所述激光距离传感器的激光方向，其中，所述距离值为所述激光距离传感器与对应的所述激光点的距离；基于所述距离值、所述初始坐标及所述激光方向计算所述待检测部位上各激光点的激光点坐标，计算各所述激光点坐标所确定的平面的综合平面法向量，其中，所述综合平面法向量由所述激光点坐标确定的平面的平面法向量求得；基于预设的位姿表达式、所述距离值及所述综合平面法向量计算所述机器人的工具头待调整位姿的位姿参数，并基于所述位姿参数控制所述机器人的工具头调整至所述待调整位姿；从而实现了准确确定工具头待调整位姿的位姿参数，进而提高了艾灸仪的作业效率，并且，本发明的激光距离传感器成本低，机器人作业时，精度高、实时性强，能够适应工具头与人体接触式场景或保持人体一定距离的场景。
此外，本发明实施例还提出一种可读存储介质。本发明可读存储介质上存储有工具头位姿的调整程序，所述工具头位姿的调整程序被处理器执行时实现如下步骤：
控制激光距离传感器的激光点照射到待检测部位上，使得各所述激光点不在一条直线上，其中，所述激光距离传感器的数量大于2个；
获取各所述激光距离传感器测量的距离值、各所述激光距离传感器的初始坐标及各所述激光距离传感器的激光方向，其中，所述距离值为所述激光距离传感器与对应的所述激光点的距离；
基于所述距离值、所述初始坐标及所述激光方向计算所述待检测部位上各激光点的激光点坐标，计算各所述激光点坐标所确定的平面的综合平面法向量，其中，所述综合平面法向量由所述激光点坐标确定的平面的平面法向量求得；
基于预设的位姿表达式、所述距离值及所述综合平面法向量计算所述机器人的工具头待调整位姿的位姿参数，并基于所述位姿参数控制所述机器人的工具头调整至所述待调整位姿。
进一步地，若所述激光距离传感器的数量大于3个，所述计算各所述激光点坐标所确定的平面的综合平面法向量的步骤包括：
计算所述激光点坐标所确定的对应平面的平面法向量；
利用加权平均算法计算各所述平面法向量的加权平均值，将所述加权平均值作为所述综合平面法向量。
进一步地，所述位姿表达式包括欧拉角、四元数或者旋转矩阵表达式。
进一步地，所述激光距离传感器的数量为3个，各所述激光距离传感器的激光束互相平行。
进一步地，所述基于所述距离值、所述初始坐标及所述激光方向计算所述待检测部位上各激光点的激光点坐标的步骤包括：
设各所述激光方向的单位向量分别为Ra(rx1，ry1，rz1)、Rb(rx2，ry2，rz2)、Rc(rx3，ry3，rz3)，各所述激光距离传感器所在工具头坐标系的坐标为Pa(x1，y1，z1)、Pb(x2，y2，z2)、Pc(x3，y3，z3)，各所述距离值为d1、d2、d3，各所述激光点的坐标为Ua、Ub、Uc，则
Ua.x=x1+rx1*d1，Ua.y=y1+ry1*d1，Ua.z=z1+rz1*d1；
Ub.x=x2+rx2*d2，Ub.y=y2+ry2*d2，Ub.z=z2+rz2*d2；
Uc.x=x3+rx3*d3，Uc.y=y3+ry3*d3，Uc.z=z3+rz3*d3；
即求得各所述激光点的坐标，其中，Pa、Pb、Pc分别为各所述激光距离传感器的坐标Ua（Ua.x，Ua.y，Ua.z）、Ub（Ub.x，Uby，Ub.z）、Uc（Uc.x，Uc.y，Uc.z）。
进一步地，所述基于预设的位姿表达式、所述距离值及所述综合平面法向量计算所述机器人的工具头待调整位姿的位姿参数的步骤包括：
设求得的所述综合平面法向量为V(vx，vy，vz)，约定vz>0，其中，若vz<0，则所述综合平面法向量乘以-1；
预设公式M（α，β，γ）*[0，0，1]T=Vnorm T，其中M（α，β，γ）为欧拉角旋转公式中的旋转矩阵；
基于所述预设公式、所述距离值及欧拉角旋转公式求得所述位姿参数，其中，Vnorm表示模为1的所述综合平面法向量，α、β、γ分别表示欧拉角。
进一步地，所述基于所述预设公式、所述距离值及欧拉角旋转公式求得所述位姿参数的步骤包括：
令rx=0，基于所述预设公式及所述欧拉角旋转公式联立方程组求出ry、rz，其中，rx、ry、rz分别为欧拉角α、β、γ；
基于所述距离值利用预设算法计算综合距离值，设为daver；
将所述工具头坐标系的原点在z方向加上daver，使得所述原点设置在所述待检测部位；
获取所述原点设置在所述待检测部位时，所述原点相对于机械手基座的基座坐标系的位姿参数，设为x0、y0、z0、rx0、ry0、rz0；
设所述工具头的待调整位姿的欧拉角参数为Rx、Ry、Rz，则
Rx=rx0，Ry=ry0+ry，Rz=rz0+rz，则
所述工具头的待调整位姿的位姿参数为x0、y0、z0、Rx、Ry、Rz。
进一步地，所述预设算法包括平均值算法及加权平均算法。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在如上所述的一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种工具头位姿的调整方法，用于艾灸仪机器人或按摩仪，其特征在于，所述工具头位姿的调整方法包括：
控制激光距离传感器的激光点照射到待检测部位上，使得各所述激光点不在一条直线上，其中，所述激光距离传感器的数量大于2个；
设定工具头坐标系，获取各所述激光距离传感器测量的距离值、各所述激光距离传感器在工具头坐标系的初始坐标及各所述激光距离传感器的激光方向，其中，所述距离值为所述激光距离传感器与对应的所述激光点的距离；
基于所述距离值、所述初始坐标及所述激光方向计算所述待检测部位上各激光点的激光点坐标，计算各所述激光点坐标所确定的平面的综合平面法向量，其中，所述综合平面法向量由所述激光点坐标确定的平面的平面法向量求得；
基于预设的位姿表达式、所述距离值及所述综合平面法向量计算所述机器人的工具头待调整位姿的位姿参数，并基于所述位姿参数控制所述机器人的工具头调整至所述待调整位姿。
2.如权利要求1所述的工具头位姿的调整方法，其特征在于，若所述激光距离传感器的数量大于3个，所述计算各所述激光点坐标所确定的平面的综合平面法向量的步骤包括：
计算所述激光点坐标所确定的对应平面的平面法向量；
利用加权平均算法计算各所述平面法向量的加权平均值，将所述加权平均值作为所述综合平面法向量。
3.如权利要求1所述的工具头位姿的调整方法，其特征在于，所述位姿表达式包括欧拉角、四元数或者旋转矩阵表达式。
4.如权利要求1所述的工具头位姿的调整方法，其特征在于，所述激光距离传感器的数量为3个，各所述激光距离传感器的激光束互相平行。
5.如权利要求4所述的工具头位姿的调整方法，其特征在于，所述基于所述距离值、所述初始坐标及所述激光方向计算所述待检测部位上各激光点的激光点坐标的步骤包括：
设各所述激光方向的单位向量分别为Ra(rx1，ry1，rz1)、Rb(rx2，ry2，rz2)、Rc(rx3，ry3，rz3)，各所述激光距离传感器所在工具头坐标系的坐标为Pa(x1，y1，z1)、Pb(x2，y2，z2)、Pc(x3，y3，z3)，各所述距离值为d1、d2、d3，各所述激光点的坐标为Ua、Ub、Uc，则
Ua.x=x1+rx1*d1，Ua.y=y1+ry1*d1，Ua.z=z1+rz1*d1；
Ub.x=x2+rx2*d2，Ub.y=y2+ry2*d2，Ub.z=z2+rz2*d2；
Uc.x=x3+rx3*d3，Uc.y=y3+ry3*d3，Uc.z=z3+rz3*d3；
即求得各所述激光点的坐标Ua（Ua.x，Ua.y，Ua.z）、Ub（Ub.x，Uby，Ub.z）、Uc（Uc.x，Uc.y，Uc.z）。
6.如权利要求3所述的工具头位姿的调整方法，其特征在于，所述基于预设的位姿表达式、所述距离值及所述综合平面法向量计算所述机器人的工具头待调整位姿的位姿参数的步骤包括：
设求得的所述综合平面法向量为V(vx，vy，vz)，约定vz>0，其中，若vz<0，则所述综合平面法向量乘以-1；
预设公式M（α，β，γ）*[0，0，1]T=Vnorm T，其中M（α，β，γ）为欧拉角旋转公式中的旋转矩阵，Vnorm表示模为1的所述综合平面法向量，α、β、γ分别表示欧拉角；
基于所述预设公式、所述距离值及欧拉角旋转公式求得所述位姿参数。
7.如权利要求6所述的工具头位姿的调整方法，其特征在于，所述基于所述预设公式、所述距离值及欧拉角旋转公式求得所述位姿参数的步骤包括：
令rx=0，基于所述预设公式及所述欧拉角旋转公式联立方程组求出ry、rz，其中，rx、ry、rz分别为欧拉角α、β、γ；
基于所述距离值利用预设算法计算综合距离值，设为daver；
将所述工具头坐标系的原点在z方向加上daver，使得所述原点设置在所述待检测部位；
获取所述原点设置在所述待检测部位时，所述原点相对于机械手基座的基座坐标系的位姿参数，设为x0、y0、z0、rx0、ry0、rz0；
设所述工具头的待调整位姿的欧拉角参数为Rx、Ry、Rz，则
Rx=rx0，Ry=ry0+ry，Rz=rz0+rz，则
所述工具头的待调整位姿的位姿参数为x0、y0、z0、Rx、Ry、Rz。
8.如权利要求7所述的工具头位姿的调整方法，其特征在于，所述预设算法包括平均值算法及加权平均算法。
9.一种工具头位姿的调整装置，其特征在于，所述工具头位姿的调整装置包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的工具头位姿的调整程序，所述工具头位姿的调整程序被所述处理器执行时实现如权利要求1至8中任一项所述的方法的步骤。
10.一种可读存储介质，其特征在于，所述可读存储介质上存储有工具头位姿的调整程序，所述工具头位姿的调整程序被处理器执行时实现如权利要求1至8中任一项所述的工具头位姿的调整方法步骤。
CN202010194511.7A 2020-03-19 2020-03-19 工具头位姿的调整方法、装置及可读存储介质 Active CN111438689B (zh)

Priority Applications (6)

Application Number Priority Date Filing Date Title
CN202010194511.7A CN111438689B (zh) 2020-03-19 2020-03-19 工具头位姿的调整方法、装置及可读存储介质
JP2022557115A JP7415036B2 (ja) 2020-03-19 2020-12-16 ツールヘッドの位置姿勢の調整方法、装置及び可読記憶媒体
CA3174424A CA3174424A1 (en) 2020-03-19 2020-12-16 Tool head posture adjustment method, apparatus and readable storage medium
PCT/CN2020/136749 WO2021184859A1 (zh) 2020-03-19 2020-12-16 工具头位姿的调整方法、装置及可读存储介质
EP20925967.0A EP4116042A4 (en) 2020-03-19 2020-12-16 METHOD AND APPARATUS FOR ADJUSTING TOOL HEAD POSITION AND READABLE STORAGE MEDIA
KR1020227035574A KR20220154189A (ko) 2020-03-19 2020-12-16 툴헤드 자세의 조정방법, 장치 및 판독 가능한 저장매체

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010194511.7A CN111438689B (zh) 2020-03-19 2020-03-19 工具头位姿的调整方法、装置及可读存储介质

Publications (2)

Publication Number Publication Date
CN111438689A CN111438689A (zh) 2020-07-24
CN111438689B true CN111438689B (zh) 2021-09-21

Family

ID=71629601

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010194511.7A Active CN111438689B (zh) 2020-03-19 2020-03-19 工具头位姿的调整方法、装置及可读存储介质

Country Status (6)

Country Link
EP (1) EP4116042A4 (zh)
JP (1) JP7415036B2 (zh)
KR (1) KR20220154189A (zh)
CN (1) CN111438689B (zh)
CA (1) CA3174424A1 (zh)
WO (1) WO2021184859A1 (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111438689B (zh) * 2020-03-19 2021-09-21 智美康民(珠海)健康科技有限公司 工具头位姿的调整方法、装置及可读存储介质
CN115781270B (zh) * 2022-11-03 2024-05-07 中国工程物理研究院激光聚变研究中心 一种光学元件位姿校正系统的装配校正方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103447877A (zh) * 2013-09-16 2013-12-18 南京航空航天大学 自主移动机构法矢检测与调姿运动方法
CN103471545A (zh) * 2013-09-25 2013-12-25 吉林大学 六自由度位姿测量系统及测量方法
CN106965180A (zh) * 2017-04-13 2017-07-21 北京理工大学 流水线上瓶子的机械臂抓取装置与方法
CN108177143A (zh) * 2017-12-05 2018-06-19 上海工程技术大学 一种基于激光视觉引导的机器人定位抓取方法及系统
CN108453743A (zh) * 2018-05-14 2018-08-28 清华大学深圳研究生院 机械臂抓取方法
CN108839027A (zh) * 2018-08-31 2018-11-20 河南工程学院 基于激光测距传感器的机器人自动对准控制方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4944840B2 (ja) 2008-06-09 2012-06-06 鹿島建設株式会社 誘導システム及び誘導方法
JP2011062786A (ja) 2009-09-18 2011-03-31 Ihi Corp レーザセンサ制御装置及びレーザセンサ制御方法
CN102284956B (zh) * 2011-05-04 2013-11-20 南京航空航天大学 一种自动钻铆机器人的法向找正方法
CN106489061A (zh) 2014-08-29 2017-03-08 日立汽车系统株式会社 部件的制造方法和使用它的制造装置、体积测定方法
CN104816307B (zh) * 2015-03-25 2016-08-17 西北工业大学 工业机器人精准制孔的四点法向调平方法
CN105091744B (zh) * 2015-05-07 2018-06-26 中国科学院自动化研究所 一种基于视觉传感器和激光测距仪的位姿检测装置与方法
KR101777618B1 (ko) * 2015-11-19 2017-09-13 대우조선해양 주식회사 레이저 거리 센서를 구비한 용접 로봇 및 그의 교정방법
CN110575386B (zh) * 2019-08-13 2023-01-13 智美康民(珠海)健康科技有限公司 一种艾灸机械手的控制方法、装置及设备
CN111438689B (zh) * 2020-03-19 2021-09-21 智美康民(珠海)健康科技有限公司 工具头位姿的调整方法、装置及可读存储介质

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103447877A (zh) * 2013-09-16 2013-12-18 南京航空航天大学 自主移动机构法矢检测与调姿运动方法
CN103471545A (zh) * 2013-09-25 2013-12-25 吉林大学 六自由度位姿测量系统及测量方法
CN106965180A (zh) * 2017-04-13 2017-07-21 北京理工大学 流水线上瓶子的机械臂抓取装置与方法
CN108177143A (zh) * 2017-12-05 2018-06-19 上海工程技术大学 一种基于激光视觉引导的机器人定位抓取方法及系统
CN108453743A (zh) * 2018-05-14 2018-08-28 清华大学深圳研究生院 机械臂抓取方法
CN108839027A (zh) * 2018-08-31 2018-11-20 河南工程学院 基于激光测距传感器的机器人自动对准控制方法

Also Published As

Publication number Publication date
KR20220154189A (ko) 2022-11-21
CN111438689A (zh) 2020-07-24
WO2021184859A1 (zh) 2021-09-23
CA3174424A1 (en) 2021-09-23
EP4116042A1 (en) 2023-01-11
JP7415036B2 (ja) 2024-01-16
EP4116042A4 (en) 2023-08-23
JP2023517395A (ja) 2023-04-25

Similar Documents

Publication Publication Date Title
CN111438689B (zh) 工具头位姿的调整方法、装置及可读存储介质
US8560122B2 (en) Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
Wang et al. A screw axis identification method for serial robot calibration based on the POE model
CN113021358B (zh) 机械臂工具坐标系原点标定方法、装置和电子设备
JP7035657B2 (ja) ロボットの制御装置、ロボット、ロボットシステム、並びに、カメラの校正方法
JP2011527053A5 (zh)
CN108466265B (zh) 机械手臂路径规划与作业方法、装置以及计算机设备
Fang et al. Robot path and end-effector orientation planning using augmented reality
JPH0768480A (ja) マニピュレータの関節角度制御方法
CN110142748A (zh) 一种适用于喷涂焊接行业的机器人快速示教系统及方法
CN112536792B (zh) 一种基于球面几何解析法的机器臂逆解方法
Shen et al. A novel robot kinematic calibration method based on common perpendicular line model
He et al. A local POE-based self-calibration method using position and distance constraints for collaborative robots
JP4244705B2 (ja) 回転機構付きレーザ加工ヘッドのキャリブレーション装置およびその方法
JPH0727408B2 (ja) 固定３次元視覚併用ロボットハンドリング装置
CN114750153B (zh) 机器人机械臂的运动控制系统、协作机器人及存储介质
CN113768535A (zh) 一种遥操作用超声仿型探头姿态自校准方法、系统及装置
CN114119684A (zh) 基于四面体结构的标记点配准方法
JP2002215211A (ja) 数値制御装置
Li et al. In-field self-calibration of robotic manipulator using stereo camera: Application to humanitarian demining robot
CN117428776A (zh) 机器人的标定方法、系统、设备及其介质
WO2022173049A1 (ja) 情報処理方法、情報処理システム、プログラム
Alberts et al. Probabilistic calibration of a manipulator and a 2D laser scanner
JPH0762613B2 (ja) 物体形状検知方法
Hwang et al. A visual servoing with collision avoidance mechanism for redundant manipulators

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
TA01 Transfer of patent application right

Effective date of registration: 20201130

Address after: Room 1001, unit 3, building 1, No. 145, Mingzhu North Road, Xiangzhou District, Zhuhai City, Guangdong Province

Applicant after: Zhimei Kangmin (Zhuhai) Health Technology Co., Ltd

Address before: Room 702-2, 7th floor, No. 18 Futian Road, Xiangzhou District, Zhuhai City, Guangdong Province

Applicant before: ZHUHAI WANNAOTE HEALTH TECHNOLOGY Co.,Ltd.

TA01 Transfer of patent application right
GR01 Patent grant
GR01 Patent grant