基于轮廓传感器的空间定位方法及系统与流程

1.本发明属于空间定位技术领域，特别涉及一种基于轮廓传感器的空间定位方法和系统。


背景技术：

2.随着工业生产自动化程度越来越高。机器人使用越来越来越普及。使得生产效率越来越高，产品质量越来越好的同时也产生了一些新的技术问题需要解决。
3.传统模式下，生产过程中会将待处理工件放到特定工装夹具上进行固定。机器人按照固定轨迹对工件进行处理。在这个过程中机器人的行走路径是固定不变的，这时就需要待处理工件在空间内的位置是固定不变的。但是在一些生产情况下无法保证待处理工件在空间内的位置的一致性。这时就需要机器人根据待处理工件在空间位置的变化情况改变机器人的运行轨迹以适应待处理工件。保证自动化生产的完成。这个过程中机器人需要知道待处理工件在空间位置的变化情况。这时产生了一个新的问题。即如何测量待处理工件在空间位置的变化情况。
4.根据测量工具的不同测量系统主要分为两种。轮廓传感器空间测量系统和相机视觉空间测量系统。
5.(1)相机获得特征点坐标的方式：
6.相机对工件进行拍照，获得工件的图像信息。根据相机的像素不同。获得图像的像素点也不同。比如5千万像素的相机，获得图像有5千万个像素点规则排列。每个像素点的光学信号也不同。再通过图像信息数据分析软件找到对应的特征点。得到特征点的像素坐标。但是这个特征点的像素坐标是二维的，还需要测得这个特征点的深度信息。通常用双目相机测得特征点的深度信息。远原理就是根据特征点分别在左目相机和右目相机的像素坐标和两相机焦距的距离用算法算得特征点的深度信息。再利用特定的方法和算法将特征点的像素坐标和深度信息转化为基准坐标系base1的三维坐标值。
7.(2)轮廓传感器获得特征点坐标的方式
8.轮廓传感器的测量原理：轮廓传感器有发射区和接收区。发射区发射一段激光线。根据传感器分辨率不同，这一段激光线含有的激光点数目也不同。以上海tda项目为例传感器发射激光线含有1200个激光点。激光线照射到工件表面上，并通过工件反射在传感器接收区接受。传感器直接测得激光点的发射位置和接受位置和从发射到接收的间隔时间和接受激光点的光学信息。传感器通过内部算法算得所有接受激光点相对于传感器本身坐标系的坐标值。由于传感器发射是一条激光线所以只能得到两个维度的坐标值。分别是垂直于发射区平面方向的坐标值和在发射区平面内平行于激光线方向上的坐标值。第三个维度坐标必须有其他方法获得。我们可以调整传感器的摆放位置使传感器发射的激光线垂直于base1的一个坐标轴。
9.这样激光线上的每一个激光点在base1该轴上的坐标值均是相同且固定的。这样我们可以通过测量传感器在base1中的位置信息得到传感器测量工件上点的第三个维度的
坐标值。在通过特定的校正方法使测量点在传感器坐标系中两个维度的坐标值转换为测量点在base1中另外两个轴的坐标值。这样我们就会得到测量点在base1中的三维坐标值。
10.这两种特征点的坐标采集方式都会存在误差。经过后续的计算会放大这个误差。从而不能准确的进行定位。


技术实现要素：

11.本发明针对现有技术中存在的问题，提出了一种能够有效降低误差的基于轮廓传感器的空间定位方法。
12.为实现上述目的，本发明提供一种基于轮廓传感器的空间定位方法，包括以下步骤：
13.设置辅具建立基准坐标系；
14.在辅具的底边、顶边和侧边周围分别设置轮廓传感器；通过轮廓传感器分别采集辅具的底边、顶边和侧边上的轮廓特征点在基准坐标系中的坐标值；
15.通过轮廓传感器分别采集待定位物体的底边、顶边和侧边上的轮廓特征点在基准坐标系中的坐标值；
16.通过辅具上的轮廓特征点的坐标值和待定位物体的轮廓特征点的坐标值计算出从辅具所在的位置移动到待定位物体所在位置的移动参数。
17.进一步，以辅具的底边为x轴，辅具的侧边为y轴，辅具的底边和为y轴的侧边的交点为坐标原点建立基准坐标系。
18.其中，在辅具底边的下方固定设置第一轮廓传感器bx1和第二轮廓传感器bx2，在辅具的顶边的上方固定设置第三轮廓传感器bx3，在辅具的一个侧边的外侧固定设置第四轮廓传感器bx4。
19.更进一步，调整第一轮廓传感器bx1和第二轮廓传感器bx2的角度，使得两个传感器发出激光线在辅具上且与辅具底边垂直，第一轮廓传感器bx1发出激光线与辅具底边交点为第一轮廓特征点，第二轮廓传感器bx2发出激光线与辅具底边交点为第二轮廓特征点；调整第三轮廓传感器bx3角度使得传感器发出激光线在辅具上与辅具顶边垂直，第三轮廓传感器bx3发出激光线与辅具顶边交点为第三轮廓特征点；调整第四轮廓传感器bx4角度使得传感器发出激光线在辅具上与辅具侧边垂直，第四轮廓传感器bx4发出激光线与辅具侧边交点为第四轮廓特征点。
20.进一步，所述辅具的底边长度与待定位物体的底边长度相同，辅具的侧边长度与带定位物体的侧边长度相同，所述辅具为长方体辅具。
21.进一步，根据以下公式获得从辅具所在的位置移动到待定位物体所在位置的移动参数：
22.x＝p4_z*(sina*sinc
‑
cosa*cosc*sinb)+d*(cosc*sina+cosa*sinb*sinc)+cosa*cosb*p4_x；
23.y＝p1_y+n*(p2_y
‑
p1_y)；
24.z＝p1_z+n*(p2_z
‑
p1_z)；
25.tana＝(p1_y
‑
p2_y)/((a
‑
b)^2+(p1_z
‑
p2_z)^2)^1/2；
26.tanb＝
‑
(p1_z
‑
p2_z)/(a
‑
b)；
27.tanc＝((p1_z
‑
p3_z)+tanb*(a
‑
c)/cosc)/(p1_y
‑
p3_y)；
28.上述公式中，x表示辅具沿x轴平移的距离，y表示辅具沿y轴平移的距离，z表示辅具沿z轴平移的距离，a表示辅具围绕z轴旋转的角度，b表示辅具围绕y轴旋转的角度，c表示辅具围绕x轴旋转的角度，n＝(x
‑
a)/(b
‑
a)；a表示第一轮廓特征点到基准坐标系中原点的距离，b表示第二轮廓特征点到基准坐标系中原点的距离，c表示第三轮廓特征点到基准坐标系中y轴的距离，d表示第四轮廓特征点到基准坐标系中原点的距离；p1_x、p1_y和p1_z分别表示第一轮廓传感器bx1观测得到的待定位物体底边上的点在基准坐标系中x轴、y轴和z轴上的值；p2_x、p2_y和p2_z分别表示第二轮廓传感器bx2观测得到的待定位物体底边上的点在基准坐标系中x轴、y轴和z轴上的值；p3_x、p3_y和p3_z分别表示第三轮廓传感器bx3观测得到的待定位物体顶边上的点在基准坐标系中x轴、y轴和z轴上的值；p4_x、p4_y和p4_z分别示第四轮廓传感器bx4观测得到的待定位物体侧边上的点在基准坐标系中x轴、y轴和z轴上的值。本发明还提供了一种基于轮廓传感器的空间定位系统，包括第一位置特征点获取模块、第二位置特征点获取模块和移动参数获取模块，其中：第一位置特征点获取模块通过轮廓传感器获得轮廓特征点在第一位置上的坐标；第二位置特征点获取模块通过轮廓传感器获得第二位置上对应的轮廓特征点的坐标；移动参数获取模块通过计算获得第一位置到第二位置上的移动参数。
29.其中，第一位置特征点获取模块包括辅具和多个轮廓传感器；所述多个轮廓传感器分别固定设置在辅具的周围，用于采集辅具的边缘上轮廓特征点的坐标。
30.进一步，第一位置特征点获取模块中包括4个轮廓传感器；在辅具底边的下方固定设置第一轮廓传感器bx1和第二轮廓传感器bx2，在辅具的顶边的上方固定设置第三轮廓传感器bx3，在辅具的一个侧边的外侧固定设置第四轮廓传感器bx4。
31.进一步，第二位置特征点获取模块包括待定位物体，所述第二位置特征点获取模块通过第一位置特征点获取模块中的轮廓传感器获取待定位物体边缘的轮廓特征点坐标。
32.工作原理：本发明主要将物体在空间内的移动关系转换成空间矩形在空间内的移动关系，运用空间矩形上的特征点的坐标变化求得空间矩形在三维坐标系内的旋转角度和平移值。
33.假设空间内有一空间矩形。这个空间矩形透明且能任意穿过，这个矩形尺寸和待定位工件的尺寸相当。带定位工件固定在这个矩形上且工件在矩形所在平面内投影在这个矩形内并且这个矩形的底边平行于地面。由于工件确定，尺寸一定。那么这样的矩形一定存在且不唯一。工件固定在这个矩形上，工件和矩形相对位置固定。工件空间内位置变换关系可等效于求这个空间矩形在空间内的位置变换关系。
34.当一个工件在基准位置放置时，它所在的空间矩形位置也确定了下来。矩形底边为l1，矩形顶边为l2，取任意侧边为l3。先以l1和l3的交点为坐标原点，l1为x轴正方形，l3为y轴正方向建立空间直角坐标系base1作为基准坐标系。对工件进行平移旋转。向x轴平移x、y轴平移y、z轴平移z、再以z轴位旋转轴旋转a、再以y轴位旋转轴旋转b、再以x轴位旋转轴旋转c得到实际工件。x,y,z,a,b,c为未知变量。这时工件所在空间也确定了下来。假设l1变换后得l4,l2变换后得l5,l3变换后得l6。在l1上取两点o1(a,0,0)，o2(b,0,0)。a，b为常量且0<a<b<l且2/3*l<(b
‑
a)。在l2取一点o3(c,h,0)，c为常量，h为矩形高，且a<c<b。在l3取一点o4(0,d,0)，d为常量，且0<d<h。变换后在l4上存在两点p1(a,p1_y,p1_z)，p2(b,p2_y,p2_
z)。a，b为常量且0<a<b<l且2/3*l<(b
‑
a)。在l5存在一点p3(c,p3_y,p3_z)，c为常量，且a<c<b。在l6存在一点p4(p4_x,d,p4_z)，d为常量，且0<d<h。a、b、c、d、l、h为常量。p1_y,p1_z,p2_y,p2_z,p3_y,p3_z,p4_x,p4_z为测量系统需要测量的测量值。
35.x,y,z,a,b,c为需要求解的未知变量。o1,o2,o3,o4,p1,p2,p3,p4的坐标值都是特征点在base1中的坐标值。
36.在空间坐标变换中一般用矩阵乘计算。平移矩阵tx代表沿x轴平移x，平移矩阵ty代表沿y轴平移y,平移矩阵tz代表沿z轴平移z。旋转矩阵tra代表以z轴旋转a，旋转矩阵trb代表以y轴旋转b，旋转矩阵trc代表以x轴旋转c。旋转矩阵taa代表以z轴旋转
‑
a，旋转矩阵tbb代表以y轴旋转
‑
b，旋转矩阵tcc代表以x轴旋转
‑
c。
37.平移矩阵：
38.tx＝[1,0,0,x；0,1,0,0；0,0,1,0；0,0,0,1]；
[0039]
ty＝[1,0,0,0；0,1,0,y；0,0,1,0；0,0,0,1]；
[0040]
tz＝[1,0,0,0；0,1,0,0；0,0,1,z；0,0,0,1]；
[0041]
旋转矩阵：
[0042]
tra＝[cosa,
‑
sina,0,0；sina,cosa,0,0；0,0,1,0；0,0,0,1]；
[0043]
trb＝[cosb,0,sinb,0；0,1,0,0；
‑
sinb,0,cosb,0；0,0,0,1]；
[0044]
trc＝[1,0,0,0；0,cosc,
‑
sinc,0；0,sinc,cosc,0；0,0,0,1]；
[0045]
taa＝[cosa,sina,0,0；
‑
sina,cosa,0,0；0,0,1,0；0,0,0,1]；
[0046]
tbb＝[cosb,0,
‑
sinb,0；0,1,0,0；sinb,0,cosb,0；0,0,0,1]；
[0047]
tcc＝[1,0,0,0；0,cosc,sinc,0；0,
‑
sinc,cosc,0；0,0,0,1]。
[0048]
base1内向量p1_p2＝p2
‑
p1,o1_o2＝o2
‑
o1根据变换公式。向量o1_o2沿z轴旋转a再沿y轴旋转b的到向量p11_p12，p11_p12＝trb*tra*o1_o2,由于在base1内向量p11_p12与向量p1_p2同向，所以：
[0049]
p11_p12＝taa*tbb*p1_p2；
[0050]
p11_p12(1,1)*p1_p2(2,1)＝p11_p12(2,1)*p1_p2(1,1)；
[0051]
p11_p12(1,1)*p1_p2(3,1)＝p11_p12(3,1)*p1_p2(1,1)；
[0052]
通过上面公式可以到：
[0053]
tanb＝
‑
(p1_z
‑
p2_z)/(a
‑
b)；
[0054]
从而可以得到sinb,cosb。
[0055]
tana＝(p1_y
‑
p2_y)*cosb/(a
‑
b)；
[0056]
tana＝(p1_y
‑
p2_y)/((a
‑
b)^2+(p1_z
‑
p2_z)^2)^1/2；
[0057]
从而可以得到sina,cosa。
[0058]
base1内向量p1_p3＝p3
‑
p1,o1_o3＝o3
‑
o1根据变换公式。向量p1_p3沿x轴旋转
‑
c再沿y轴旋转
‑
b沿z轴旋转
‑
a的到向量p11_p13，p11_p13＝taa*tbb*tcc*p1_p3,在base2内向量p11_p13在oxy平面内，从而：
[0059]
p11_p13＝taa*tbb*tcc*p1_p3；
[0060]
p11_p13(3,1)＝0；
[0061]
通过上面公式可以到：
[0062]
cosb*sinc*(p1_y
‑
p3_y)
‑
cosb*cosc*(p1_z
‑
p3_z)
‑
sinb*(a
‑
c)＝0；
[0063]
tanc＝((p1_z
‑
p3_z)+tanb*(a
‑
c)/cosc)/(p1_y
‑
p3_y)；
[0064]
从而可以得到sinc,cosc。
[0065]
点p4沿x轴旋转
‑
c再沿y轴旋转
‑
b沿z轴旋转
‑
a的到点p14，
[0066]
p14＝taa*tbb*tcc*p4,则：
[0067]
x＝p14(1,1)
[0068]
＝p4_z*(sina*sinc
‑
cosa*cosc*sinb)+d*(cosc*sina+cosa*sinb*sinc)+cosa*cosb*p4_x。
[0069]
平移路径可看作base1原点先移动到p4点在沿l4方向平移到base2原点，p4点到base2原点的平移向量n＝[0,0,0,n*(b
‑
a)；0,0,0,n*(p2_y
‑
p1_y)；0,0,0,n*(p2_z
‑
p1_z)；0,0,0,1]；因为x＝a+n*(b
‑
a)；
[0070]
x＝p14(1,1)＝p4_z*(sina*sinc
‑
cosa*cosc*sinb)+d*(cosc*sina+cosa*sinb*sinc)+cosa*cosb*p4_x；所以，
[0071]
n＝(x
‑
a)/(b
‑
a)＝(p4_z*(sina*sinc
‑
cosa*cosc*sinb)+d*(cosc*sina+cosa*sinb*sinc)+cosa*cosb*p4_x
‑
a)/(b
‑
a)
[0072]
y＝p1_y+n*(p2_y
‑
p1_y)；
[0073]
z＝p1_z+n*(p2_z
‑
p1_z)。
[0074]
上述公式中，p1_y,p1_z分别表示p1点在base1坐标系中的y轴和z轴上的值；p2_y,p2_z分别表示p2点在base1坐标系中的y轴和z轴上的值；p3_y,p3_z,分别表示p3点在base1坐标系中的y轴和z轴上的值；p4_x,p4_z分别表示p4点在base1坐标系中的x轴和z轴上的值。
[0075]
有益效果：与现有技术相比，本发明提供的方法需要特征点少，计算量小；反馈速度快，同时能有效降低基于轮廓传感器测量方法的误差。
附图说明
[0076]
图1是本发明提供的方法工作示意图。
具体实施方式
[0077]
下面将结合本发明实例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
[0078]
本实施例公开了一种基于轮廓传感器的空间定位系统，主要包括第一位置特征点获取模块、第二位置特征点获取模块和移动参数获取模块，其中：第一位置特征点获取模块主要通过轮廓传感器获得四个轮廓特征点在第一位置上的坐标；第二位置特征点获取模块通过轮廓传感器获得第二位置上对应的四个轮廓特征点的坐标；移动参数获取模块通过计算可以获得第一位置到第二位置上移动的位移和旋转的角度。
[0079]
本实施例中，第一位置是与待定位的物体大小相仿的长方体辅具所在的位置，第二位置是待定位的物体所在的位置，本实施例中以车门为例。由于本实施例中待定位的物体为车门，该车门底边长l，高为h，所以本实施例中采用的长方体辅具的长为l，高为h，厚度
为5mm。如图1所示，在长方体辅具底边的下方固定设置第一轮廓传感器bx1和第二轮廓传感器bx2，在长方体辅具的顶边的上方固定设置第三轮廓传感器bx3，在长方体辅具一侧边的外侧固定设置第四轮廓传感器bx4。其中，第一轮廓传感器bx1所在位置为第一测量点b1，第二轮廓传感器bx2所在位置为第二测量点b2，第三轮廓传感器bx3所在位置为第三测量点b3，第四轮廓传感器bx4所在位置为第四测量点b4。
[0080]
采用本实施例提供的基于轮廓传感器的空间定位系统进行空间定位时，主要包括以下步骤：
[0081]
步骤1：安装长方体辅具建立基准坐标系base1。以长方体辅具的底边为x轴，以安装第四轮廓传感器bx4的侧边为y轴，以长方体辅具底边与该条长方体辅具侧边的交点为坐标原点建立基准坐标系base1。
[0082]
步骤2：建立空间坐标测量系统。本实施例中采用wenglor轮廓传感器但是本方法并不限于wenglor轮廓传感器。
[0083]
调整第一轮廓传感器bx1和第二轮廓传感器bx2的角度，使得两个传感器发出激光线在长方体辅具上且与长方体辅具底边垂直，第一轮廓传感器bx1发出激光线与辅具底边交点为交点o1，第二轮廓传感器bx2发出激光线与辅具底边交点为交点o2。调整第三轮廓传感器bx3角度使得传感器发出激光线在长方体辅具上与长方体辅具顶边垂直，第三轮廓传感器bx3发出激光线与辅具顶边交点为交点o3。调整第四轮廓传感器bx4角度使得传感器发出激光线在长方体辅具上与长方体辅具侧边垂直，第四轮廓传感器bx4发出激光线与辅具侧边交点为交点o4。测量交点o1到原点的距离为a，交点o2到原点的距离为b，交点o3到y轴的距离为c，交点o4到原点的距离为d。
[0084]
在第一轮廓传感器bx1内建立新的直角坐标系，并旋转坐标系y轴使得坐标系的x轴与辅具上的激光线重合，平移坐标系原点使得第一轮廓传感器bx1测量值bx1_y为0，bx1_z为0，即使第一测量点b1在base1坐标系中的y轴的坐标值为0，z轴的坐标值为0。在第二轮廓传感器bx2内建立新的直角坐标系，并旋转坐标系的y轴使得坐标系的x轴与辅具上的激光线重合，平移坐标系原点使得传感器测量值bx2_y为0，bx2_z为0，即使第二测量点b2在base1坐标系中的y轴的坐标值为0，z轴的坐标值为0。在第三轮廓传感器bx3内建立新的直角坐标系，并旋转坐标系的y轴使得坐标系的x轴与辅具上的激光线重合，平移坐标系原点使得传感器测量值bx3_y为h，bx3_z为0，即使第一测量点b3在base1坐标系中的y轴的坐标值为h，z轴的坐标值为0。在第四轮廓传感器bx4内建立新的直角坐标系，并旋转坐标系的y轴使得坐标系的x轴与辅具上的激光线重合，平移坐标系原点使得传感器测量值bx4_x为0，bx4_z为0,即使第一测量点b4在base1坐标系中的x轴的坐标值为0，z轴的坐标值为0。从而得到第一测量点b1在base1中的坐标为b1(a,0,0)，第二测量点b2在base1中的坐标为b2(b,0,0)，第三测量点b3在base1中的坐标为b3(c,h,0)，第四测量点b4在base1中的坐标为b4(0,d,0)。
[0085]
步骤3：将车门放置在轮廓传感器可以观测的范围内，分别通过第一轮廓传感器bx1、第二轮廓传感器bx2、第三轮廓传感器bx3和第四轮廓传感器bx4对车门进行观测，得到车门底边上的两个交点p1和p2的坐标，车门顶边上的交点p3的坐标，车门侧边上的交点p4的坐标。在base1中的每个交点的坐标分别为p1(p1_x,p1_y,p1_z),p2(p2_x,p2_y,p2_z),p3(p3_x,p3_y,p3_z),p4(p4_x,p4_y,p4_z),其中，p1_x、p1_y和p1_z分别表示p1点在base1坐标系中x
轴、y轴和z轴上的值；p2_x、p2_y和p2_z分别表示p2点在base1坐标系中x轴、y轴和z轴上的值；p3_x、p3_y和p3_z分别表示p3点在base1坐标系中x轴、y轴和z轴上的值；p4_x、p4_y和p4_z分别表示p4点在base1坐标系中x轴、y轴和z轴上的值。其中，p1_x等于a，p2_x等于b，p3_x等于c，p3_y等于h，p4_y等于d，其他值为轮廓传感器的反馈值。
[0086]
步骤3：通过计算可以获得从长方体辅具到车门所在位置移动的位移和旋转的角度。
[0087]
其中，
[0088]
x＝p4_z*(sina*sinc
‑
cosa*cosc*sinb)+d*(cosc*sina+cosa*sinb*sinc)+cosa*cosb*p4_x；
[0089]
y＝p1_y+n*(p2_y
‑
p1_y)；
[0090]
z＝p1_z+n*(p2_z
‑
p1_z)；
[0091]
tana＝(p1_y
‑
p2_y)/((a
‑
b)^2+(p1_z
‑
p2_z)^2)^1/2；
[0092]
tanb＝
‑
(p1_z
‑
p2_z)/(a
‑
b)；
[0093]
tanc＝((p1_z
‑
p3_z)+tanb*(a
‑
c)/cosc)/(p1_y
‑
p3_y)；
[0094]
上述公式中，x表示长方体辅具沿x轴平移的距离，y表示长方体辅具沿y轴平移的距离，z表示长方体辅具沿z轴平移的距离，a表示长方体辅具围绕z轴旋转的角度，b表示长方体辅具围绕y轴旋转的角度，c表示长方体辅具围绕x轴旋转的角度，n＝(x
‑
a)/(b
‑
a)。
[0095]
利用本实施例提供的方法可以将多个轮廓传感器的测量值整合到一个坐标系内并与机器人的坐标系一致。为后续计算带来方便。采集的数据直接可用不需要额外处理。在原始数据上减少不必要的误差。受wenglor2d/3d轮廓传感器和编码器精度影响原始数据存在200μm误差。
[0096]
利用实际物体上的特征轮廓曲线上的点的坐标和空间矩形上特征点的坐标获得标定移动参数的方法，可以降低原始数据个别点扰动数据有波动造成的计算误差。比如物体表面细微凹陷和凸起的部分一致性有差异等。利用这种方法实现的定位效果良好问题误差控制在0.5mm以内。
[0097]
虽然本发明已以较佳实施例揭露如上，然其并非用以限定本发明。本发明所属技术领域中具有通常知识者，在不脱离本发明的精神和范围内，当可作各种的更动与润饰。因此，本发明的保护范围当视权利要求书所界定者为准。