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

本发明涉及机器人技术领域，尤其涉及一种工具头位姿的调整方法、装置及可读存储介质。

背景技术：

随着机器人技术的发展，机器人的性能和安全性有了长足的提升，人机协作机器人的出现，更是标志着人和机器人协同工作成为切实可行的事情。近十年现代工业飞速发展，机器人的普及应用也使得机器人的成本在近两年迅速靠近消费者可负担的水平。个人护理机器人作为机器人的一种，包括安装在机械手臂末端带有护理功能(按摩，艾灸，化妆，美容等)的工具头，该工具头与被护理的人体直接接触或者保持特定的距离和姿势来进行护理作业。

目前，确定护理机器人工作时位姿(位置和姿态)的解决方案主要有机器视觉方案、接触式多轴力传感器方案、雷达(超声波或者激光等)传感器。上述解决方案都有不尽人意的地方，例如，机器视觉方案的机器人价格昂贵，机器人视觉方案需要特殊光源照射，扫描时间和坐标生成时间长，实时性差，摄像头的体积大不利于集成在工具头，并且暴露了客户的隐私部位；接触式多轴力传感器方案的机器人价格昂贵，只适用于工具头与人体接触式场景，对工具头位姿指导没有前瞻性；雷达方案的机器人价格昂贵、精度差、盲区大、容易受干扰，且长时间雷达波的放射对人体健康有潜在风险。所以，如何提供一种价格低廉、精度高、实时性强及位姿确定准确的工具头位姿确定方案成为待解决的问题。

上述内容仅用于辅助理解本发明的技术方案，并不代表承认上述内容是现有技术。

技术实现要素：

本发明的主要目的在于提供一种价格低廉、精度高、实时性强及位姿确定准确的工具头位姿确定方案。

为实现上述目的，本发明提供一种工具头位姿的调整方法，所述工具头位姿的调整方法包括：

控制激光距离传感器的激光点照射到待检测部位上，使得各所述激光点不在一条直线上，其中，所述激光距离传感器的数量大于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，其中，该处是为了计算平面综合法向量的单位法向量，从而确定该综合法向量的方向；

欧拉角旋转公式为：

预设公式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、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。