(12)发明专利申请
(10)申请公布号 CN 108839027 A(43)申请公布日 2018.11.20
(21)申请号 201811010117.2(22)申请日 2018.08.31
(71)申请人 河南工程学院
地址 451191 河南省郑州市新郑龙湖镇祥
和路1号(72)发明人 卢金燕 郭壮志 黄全振 (74)专利代理机构 郑州金成知识产权事务所
(普通合伙) 41121
代理人 郭增欣(51)Int.Cl.
B25J 9/16(2006.01)
权利要求书3页 说明书7页 附图1页
(54)发明名称
基于激光测距传感器的机器人自动对准控制方法(57)摘要
本发明公开了一种基于激光测距传感器的机器人自动对准方法，该方法利用三个一维激光测距传感器测得的目标与机器人的相对距离进行对准控制；根据三个激光测距传感器所在平面法向量当前值和期望值的偏差，得到绕X轴和绕Y轴的角度偏差；根据三个激光测距传感器所构成夹角的当前值和期望值的偏差，得到绕Z轴的角度偏差；根据机器人平移量的当前值和期望值的偏差，得到沿X轴和Y轴的位置偏差；根据目标与机器人之间的当前相对距离和期望相对距离的偏差，得到沿Z轴的位置偏差；本发明直接从距离信息得到位姿转换信息，没有采用视觉传感器繁琐的图像处理和特征提取过程，控制器设计简单，方便使用。
CN 108839027 ACN 108839027 A
权 利 要 求 书
1/3页
1.一种基于激光测距传感器的机器人自动对准方法，包括以下步骤：步骤S0，使用三个一维激光测距传感器测量目标和机器人末端之间的相对距离，且所述三个传感器呈等腰三角形布置；
步骤S1：根据所述三个激光测距传感器的测量值，离线得到所述三个激光测距传感器构成平面的期望法向量，在线得到所述平面的当前法向量；
步骤S2：将当前法向量和期望法向量都投影到与X轴垂直的平面和与Y轴垂直的平面，得到绕X轴和绕Y轴的角度偏差；
步骤S3：根据三个激光测距传感器所构成夹角的当前值和期望值的偏差，得到绕Z轴的角度偏差；
步骤S4：根据机器人平移量的当前值和期望值的偏差，得到沿X轴和Y轴的位置偏差；步骤S5：根据目标与机器人之间的当前相对距离和期望相对距离的偏差，得到沿Z轴的位置偏差；
步骤S6：根据S2的绕X轴和绕Y轴的角度偏差、S3的绕Z轴的角度偏差、S4的沿X轴和Y轴的位置偏差，以及S5的沿Z轴的位置偏差，得到机器人的运动调整量，实现六自由度的自动对准。
2.根据权利要求1所述的基于激光测距传感器的自动对准方法，其中所述三个一维激光测距传感器安装在机器人末端，且呈等腰三角形布置。
3.根据权利要求1所述的基于激光测距传感器的自动对准方法，其中步骤S1中所述的当前法向量和期望法向量计算如下：
机器人末端位姿矩阵为Te，第i个激光传感器相对于夹具的位姿矩阵Ti(i＝1,2,3)，则第i个激光传感器相对于机器人末端的位姿矩阵Tiw为：
Tiw＝TeTi(i＝1,2,3) (1)根据第i个激光测距传感器的测量值，得到第i个激光测距传感器的三维坐标(Xi,Yi,Zi)(i＝1,2,3)：
其中，value(i)为第i个激光测距传感器的测量值。
根据三个激光测距传感器的坐标，可得三条边的向量，任选其中两条边激光测距传感器构成的平面法向量如下：
和
则三个
根据公式(3),可得当前法向量离线将机器人调整到期望位姿，根据期望位姿时激光
测距传感器的测量值，利用公式(3)，得到期望法向量
4.根据权利要求1所述的基于激光测距传感器的自动对准方法，其中步骤S2中所述的绕X轴和绕Y轴的角度偏差计算如下：
将三个激光测距传感器的期望法向量和当前法向量都投影到与X轴垂直的平面和与Y
2
CN 108839027 A
权 利 要 求 书
2/3页
轴垂直的平面，记期望法向量
和
在与X轴垂直的平面和与Y轴垂直的平面上的投影分别为
和
记当前法向量在与X轴垂直的平面和与Y轴垂直的平面上的投影分别为
则可得：
其中，Δθθx和Δy分别是绕X轴的角度偏差和绕Y轴的角度偏差。
5.根据权利要求1所述的基于激光测距传感器的自动对准方法，其中步骤S3中所述的绕Z轴的角度偏差计算如下：
根据三个激光测距传感器的坐标，得到三条边的向量，任选其中两条边和
两条边的夹角如下：
和
则
根据公式(5),可得当前夹角α。离线将机器人调整到期望位姿，根据期望位姿时激光测距传感器的测量值，利用公式(5)，得到期望夹角α则绕Z轴的角度偏差如下：q。
Δθαα (6)z＝d-6.根据权利要求1所述的基于激光测距传感器的自动对准方法，其中步骤S4中所述的沿X轴和Y轴的位置偏差如下：
离线将机器人调整到期望位姿，向右平移机器人，直到左侧激光测距传感器的激光点打空，以机器人向右的平移量作为期望平移量xq。在线向右平移机器人，直到左侧激光测距传感器的激光点打空，以机器人向右的平移量作为当前平移量x,则沿X轴的位置偏差为：
Δx＝xq-x (7)同理，离线将机器人调整到期望位姿，向下平移机器人，直到上方激光测距传感器的激光点打空，以机器人向下的平移量作为期望平移量yq。在线向下平移机器人，直到上方激光测距传感器的激光点打空，以机器人向下的平移量作为当前平移量y,则沿Y轴的位置偏差为：
Δy＝yq-y (8)
7.根据权利要求1所述的基于激光测距传感器的自动对准方法，其中步骤S5中所述的沿Z轴的位置偏差如下：
根据三个激光测距传感器的测量值，得到目标与机器人之间的当前相对距离z：z＝(value(1)+value(2)+value(3))/3 (9)离线将机器人调整到期望位姿，根据期望位姿时三个激光测距传感器的测量值，利用
3
CN 108839027 A
权 利 要 求 书
3/3页
公式(9)，得到期望相对距离zq，则沿Z轴的位置偏差为：
Δz＝zq-z (10)
8.根据权利要求1所述的基于激光测距传感器的自动对准方法，其中步骤S6中所述的
T
机器人运动调整量[x,y,z,θθθx,y,z]如下：
其中，λ(△θθ△θp是调整系数，x,△y)是S2所述的绕X轴和绕Y轴的角度偏差，z是S3所述的绕Z轴的角度偏差，(△x,△y)是S4所述的沿X轴和Y轴的位置偏差，△z是S5所述的沿Z轴的位置偏差。
4
CN 108839027 A
说 明 书
基于激光测距传感器的机器人自动对准控制方法
1/7页
技术领域
[0001]本发明属于机器人控制领域，更具体地涉及一种基于激光测距传感器的机器人自动对准方法。
背景技术
[0002]随着自动化水平越来越高,机器人在各行各业占有越来越重要的地位。机器人具有定位精度高、响应速度快、载重量大等特点，能够提高作业效率，降低作业风险。目前，工业机器人已经可替代人工完成焊接、浇铸、装配、打磨、喷涂、抛光等复杂工作，被广泛应用在智能制造、工业自动化等领域。
[0003]机器人自动对准是装配制造和自动控制领域的一个重要研究方向。传统自动对准过程中，视觉传感器是最常用的外部传感器，它具有成本低、信息丰富、算法简单等优点。然而，相比于其他机器人外部传感器,视觉系统的采样频率较低,视觉处理算法的时间延迟较大,而且易受噪声、光照等影响，这严重制约了机器人控制系统的性能。[0004]在诸多外部传感器中，激光传感器因测量精度高、性能稳定、抗干扰能力强、受工作距离影响小等特点，被许多研究人员应用到机器人控制当中。Kim等使用1个视觉传感器和3个激光测距传感器搭建检测平台,通过世界坐标系、激光坐标系和相机坐标系之间的转换关系,得到目标六个自由度的位姿信息，实现大型货船的自动对准。(Kim Y.K.,Kim Y.H.,Kim K.S.,Jung Y.S..Developing a Robust Sensing System for Remote Relative 6-DOF Motion Using 1-D Laser Sensors[C].IEEE International Systems Conference,Vancouver,Canada.2012.)。该系统能够实现距离目标30米范围内的自动对准，但对准精度不高。Liu等根据激光干涉测量法标定出零件的圆柱表面、端点等几何特征,利用激光跟踪仪得到六个自由度的位姿信息，实现大尺寸、大重量零件的轴孔装配(Liu Z.,Xie Y.,Xu J.,Chen K..Laser tracker based robotic assembly system for large scale peg-hole parts.The 4th Annual International Conference on Cyber Technology in Automation,Control and Intelligent Systems.Hong Kong,China,2014,574-578)。激光跟踪仪能够很容易的获取六维位姿信息，但价格昂贵。董燕飞等利用多个激光传感器采集机器人工作环境数据，采用支持向量机对多个激光传感器信息进行融合,根据融合结果对机器人运动过程进行控制,取得了良好的控制效果(董燕飞,邢广成.基于激光传感器的机器人运动控制技术,激光杂志,2016,(37)3:25-28.)。然而，该系统功能较为简单，只能实现机器人的三维位置控制，难以对机器人的姿态进行调整。发明内容
[0005]基于上述背景，本发明提供一种基于激光测距传感器的机器人自动对准方法，该方法包括步骤如下：[0006]步骤S0，使用三个一维激光测距传感器测量目标和机器人末端之间的相对距离，且所述三个传感器呈等腰三角形布置；
5
CN 108839027 A[0007]
说 明 书
2/7页
步骤S1：根据所述三个激光测距传感器的测量值，离线得到所述三个激光测距传
感器构成平面的期望法向量，在线得到所述平面的当前法向量；[0008]步骤S2：将当前法向量和期望法向量都投影到与X轴垂直的平面和与Y轴垂直的平面，得到绕X轴和绕Y轴的角度偏差；[0009]步骤S3：根据三个激光测距传感器所构成夹角的当前值和期望值的偏差，得到绕Z轴的角度偏差；[0010]步骤S4：根据机器人平移量的当前值和期望值的偏差，得到沿X轴和Y轴的位置偏差；
[0011]步骤S5：根据目标与机器人之间的当前相对距离和期望相对距离的偏差，得到沿Z轴的位置偏差；[0012]步骤S6：根据S2的绕X轴和绕Y轴的角度偏差、S3的绕Z轴的角度偏差、S4的沿X轴和Y轴的位置偏差，以及S5的沿Z轴的位置偏差，得到机器人的运动调整量，实现六自由度的自动对准。
[0013]进一步说明，其中所述三个一维激光测距传感器安装在机器人末端，且呈等腰三角形布置。
[0014]进一步说明，其中步骤S1中所述的当前法向量和期望法向量计算如下：[0015]机器人末端位姿矩阵为Te，第i个激光传感器相对于夹具的位姿矩阵Ti(i＝1,2,3)，则第i个激光传感器相对于机器人末端的位姿矩阵Tiw为：[0016]Tiw＝TeTi(i＝1,2,3) (1)
[0017]根据第i个激光测距传感器的测量值，得到第i个激光测距传感器的三维坐标(Xi,Yi,Zi)(i＝1,2,3)：
[0018]
[0019][0020]
其中，value(i)为第i个激光测距传感器的测量值。
根据三个激光测距传感器的坐标，可得三条边的向量，任选其中两条边
和
则三个激光测距传感器构成的平面法向量如下：
[0021][0022]
根据公式(3),可得当前法向量离线将机器人调整到期望位姿，根据期望位姿
时激光测距传感器的测量值，利用公式(3)，得到期望法向量
[0023][0024]
进一步说明，其中步骤S2中所述的绕X轴和绕Y轴的角度偏差计算如下：
将三个激光测距传感器的期望法向量和当前法向量都投影到与X轴垂直的平面和
在与X轴垂直的平面和与Y轴垂直的平面上的投影分别
与Y轴垂直的平面，记期望法向量为和
和
记当前法向量在与X轴垂直的平面和与Y轴垂直的平面上的投影分别为
则可得：
6
CN 108839027 A
说 明 书
3/7页
[0025]
[0026][0027][0028]
其中，Δθθx和Δy分别是绕X轴的角度偏差和绕Y轴的角度偏差。进一步说明，其中步骤S3中所述的绕Z轴的角度偏差计算如下：根据三个激光测距传感器的坐标，得到三条边的向量，任选其中两条边两条边的夹角如下：
和
则和
[0029]
根据公式(5),可得当前夹角α。离线将机器人调整到期望位姿，根据期望位姿时激
光测距传感器的测量值，利用公式(5)，得到期望夹角α则绕Z轴的角度偏差如下：q。[0031]Δθαα (6)z＝d-[0032]进一步说明，其中步骤S4中所述的沿X轴和Y轴的位置偏差如下：[0033]离线将机器人调整到期望位姿，向右平移机器人，直到左侧激光测距传感器的激光点打空，以机器人向右的平移量作为期望平移量xq。在线向右平移机器人，直到左侧激光测距传感器的激光点打空，以机器人向右的平移量作为当前平移量x,则沿X轴的位置偏差为：
[0034]Δx＝xq-x (7)[0035]同理，离线将机器人调整到期望位姿，向下平移机器人，直到上方激光测距传感器的激光点打空，以机器人向下的平移量作为期望平移量yq。在线向下平移机器人，直到上方激光测距传感器的激光点打空，以机器人向下的平移量作为当前平移量y,则沿Y轴的位置偏差为：
[0036]Δy＝yq-y (8)[0037]进一步说明，其中步骤S5中所述的沿Z轴的位置偏差如下：[0038]根据三个激光测距传感器的测量值，得到目标与机器人之间的当前相对距离z：[0039]z＝(value(1)+value(2)+value(3))/3 (9)[0040]离线将机器人调整到期望位姿，根据期望位姿时三个激光测距传感器的测量值，利用公式(9)，得到期望相对距离zq，则沿Z轴的位置偏差为：[0041]Δz＝zq-z (10)
T[0042]进一步说明，其中步骤S6中所述的机器人运动调整量[x,y,z,θθθx,y,z]如下：
[0030]
7
CN 108839027 A
说 明 书
4/7页
[0043]
其中，λ(△θθ△θp是调整系数，x,△y)是S2所述的绕X轴和绕Y轴的角度偏差，z是S3
所述的绕Z轴的角度偏差，(△x,△y)是S4所述的沿X轴和Y轴的位置偏差，△z是S5所述的沿Z轴的位置偏差。
[0045]基于上述技术方案可知，本发明具有以下有益效果：传统的机器人自动对准多采用视觉传感器，其控制过程包括图像采集、图像处理、特征提取与分析、控制信息(如位姿、深度信息等)转换生成等，严重制约了系统的实时性。此外，由于视觉传感器视场限制以及环境噪声的影响，目标容易在视野中丢失，导致视觉控制失败。[0046]为了提高控制系统的实时性和稳定性，本发明针对机器人的自动对准，仅利用三个呈等腰三角形布置的一维激光测距传感器，测量目标与机器人之间的相对距离，根据测量结果进行机器人控制，实现目标的六自由度对准。[0047]相对于视觉传感器，激光测距传感器的测量精度更高，因此本发明的对准精度更高。此外，由于没有繁琐的图像处理和特征提取过程，本发明具有较高的实时性。本发明直接从距离信息得到位姿转换信息，控制器设计简单，方便使用，便于推广。附图说明
[0048]图1为本发明的基于激光测距传感器的机器人自动对准控制框图。
具体实施方式
[0049]下面结合附图对本发明的实施例作详细的说明：本实施例在以本发明技术方案为前提下进行实施，结合详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述实施例。
[0050]本发明公开了一种基于激光测距传感器的机器人自动对准方法，本发明仅利用三个呈等腰三角形布置的一维激光测距传感器，测量目标与机器人之间的相对距离，根据测量结果进行机器人控制，实现目标的六自由度对准。[0051]更具体地，作为本发明的一个优选实施例，如图1示出本发明的基于激光测距传感器的自动对准方法的控制框图。在控制过程中，该方法利用三个一维激光测距传感器测得的目标与机器人的相对距离进行对准控制。根据三个激光测距传感器所在平面法向量当前值和期望值的偏差，得到绕X轴和绕Y轴的角度偏差；根据三个激光测距传感器所构成夹角的当前值和期望值的偏差，得到绕Z轴的角度偏差；根据激光测距传感器平移量的当前值和期望值的偏差，得到沿X轴和Y轴的位置偏差；根据目标与机器人之间的当前相对距离和期望相对距离的偏差，得到沿Z轴的位置偏差，从而实现机器人的六自由度对准。该方法包括以下步骤：
8
[0044]
CN 108839027 A[0052]
说 明 书
5/7页
第一步：根据所述三个激光测距传感器的测量值，离线得到所述三个激光测距传
感器构成平面的期望法向量，在线得到所述平面的当前法向量；[0053]第二步：将当前法向量和期望法向量都投影到与X轴垂直的平面和与Y轴垂直的平面，得到绕X轴和绕Y轴的角度偏差；[0054]第三步：根据三个激光测距传感器所构成夹角的当前值和期望值的偏差，得到绕Z轴的角度偏差；[0055]第四步：根据机器人平移量的当前值和期望值的偏差，得到沿X轴和Y轴的位置偏差；[0056]第五步：根据目标与机器人之间的当前相对距离和期望相对距离的偏差，得到沿Z轴的位置偏差；[0057]第六步：根据S2的绕X轴和绕Y轴的角度偏差、S3的绕Z轴的角度偏差、S4的沿X轴和Y轴的位置偏差，以及S5的沿Z轴的位置偏差，得到机器人的运动调整量，实现六自由度的自动对准。
[0058]所述第一步，具体如下：
[0059]根据所述三个激光测距传感器的测量值，利用公式(3)得到所述三个激光测距传感器构成平面的期望法向量和当前法向量；[0060]其中公式(3)由以下具体步骤获得：[0061]记机器人末端位姿矩阵为Te，第i个激光传感器相对于夹具的位姿矩阵Ti(i＝1,2,3)，则第i个激光传感器相对于机器人末端的位姿矩阵Tiw为：[0062]Tiw＝TeTi(i＝1,2,3) (1)
[0063]根据第i个激光测距传感器的测量值，得到第i个激光测距传感器的三维坐标(Xi,Yi,Zi)(i＝1,2,3)：
[0064]
[0065][0066]
其中，value(i)为第i个激光测距传感器的测量值。
根据三个激光测距传感器的坐标，可得三条边的向量，任选其中两条边
和
则三个激光测距传感器构成的平面法向量如下：
[0067][0068]
根据公式(3)，可得当前法向量离线将机器人调整到期望位姿，根据期望位姿
时激光测距传感器的测量值，利用公式(3)，得到期望法向量
所述第二步，具体如下：
[0070]将当前法向量和期望法向量都投影到与X轴垂直的平面和与Y轴垂直的平面，得到式(4)所示的绕X轴和绕Y轴的角度偏差；[0071]其中公式(4)由以下具体步骤获得：
[0072]将三个激光测距传感器的期望法向量和当前法向量都投影到与X轴垂直的平面和
[0069]
9
CN 108839027 A
说 明 书
6/7页
与Y轴垂直的平面，记期望法向量为和
和
在与X轴垂直的平面和与Y轴垂直的平面上的投影分别
记当前法向量在与X轴垂直的平面和与Y轴垂直的平面上的投影分别为
则可得：
[0073]
其中，Δθθx和Δy分别是绕X轴的角度偏差和绕Y轴的角度偏差。[0075]所述第三步，具体如下：[0076]利用公式(5),计算三个激光测距传感器所构成的夹角；根据公式(6)，得到绕Z轴的角度偏差。
[0077]其中公式(5)、公式(6)由以下具体步骤获得：
[0078]
[0074]
根据三个激光测距传感器的坐标，得到三条边的向量，任选其中两条边两条边的夹角如下：
和
则和
[0079]
根据公式(5),可得当前夹角α。离线将机器人调整到期望位姿，根据期望位姿时激光测距传感器的测量值，利用公式(5)，得到期望夹角α则绕Z轴的角度偏差如下：q。[0081]Δθαα (6)z＝d-[0082]所述第四步，具体如下：
[0083]根据机器人向右的平移量的当前值和期望值的偏差，利用公式(7),得到沿X轴的位置偏差。根据机器人向下的平移量的当前值和期望值的偏差，利用公式(8),得到沿Y轴的位置偏差。
[0084]其中公式(7)、公式(8)由以下具体步骤获得：[0085]离线将机器人调整到期望位姿，向右平移机器人，直到左侧激光测距传感器的激光点打空，以机器人向右的平移量作为期望平移量xq。在线向右平移机器人，直到左侧激光测距传感器的激光点打空，以机器人向右的平移量作为当前平移量x,则沿X轴的位置偏差为：
[0086]Δx＝xq-x (7)[0087]同理，离线将机器人调整到期望位姿，向下平移机器人，直到上方激光测距传感器的激光点打空，以机器人向下的平移量作为期望平移量yq。在线向下平移机器人，直到上方激光测距传感器的激光点打空，以机器人向下的平移量作为当前平移量y,则沿Y轴的位置偏差为：
10
[0080]
CN 108839027 A[0088]
说 明 书
7/7页
Δy＝yq-y (8)
[0089]所述第五步，具体如下：
[0090]根据三个激光测距传感器的测量值，利用公式(9)，得到目标与机器人之间的相对距离。根据目标与机器人之间的当前相对距离和期望相对距离的偏差，利用公式(10)，得到沿Z轴的位置偏差。[0091]其中公式(9)、公式(10)由以下具体步骤获得：[0092]根据三个激光测距传感器的测量值，得到目标与机器人之间的当前相对距离z：[0093]z＝(value(1)+value(2)+value(3))/3 (9)[0094]离线将机器人调整到期望位姿，根据期望位姿时三个激光测距传感器的测量值，利用公式(9)，得到期望相对距离zq，则沿Z轴的位置偏差为：[0095]Δz＝zq-z (10)[0096]所述第六步，具体如下：
[0097]根据第二步的绕X轴和绕Y轴的角度偏差、第三步的绕Z轴的角度偏差、第四步的沿X轴和Y轴的位置偏差，以及第五步的沿Z轴的位置偏差，利用公式(11)，得到机器人的运动调整量，实现六自由度的自动对准。
[0098]其中公式(11)由以下具体步骤获得：
[0099]
[0100]
其中，λ(△θθ△θp是调整系数，x,△y)是S2所述的绕X轴和绕Y轴的角度偏差，z是S3
所述的绕Z轴的角度偏差，(△x,△y)是S4所述的沿X轴和Y轴的位置偏差，△z是S5所述的沿Z轴的位置偏差。
[0101]以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
11
CN 108839027 A
说 明 书 附 图
1/1页
图1
12
因篇幅问题不能全部显示，请点此查看更多更全内容