一种机器人轨迹误差的测量方法

权利要求

1.一种机器人轨迹误差的测量方法，其特征在于所述方法通过用于轨迹发生器的旋转双棱镜系统和用于轨迹图像采集的双目视觉测量系统实现，所述方法用于对机器人全程或局部最大运动误差进行测量，其中：所述旋转双棱镜系统包括第一旋转双棱镜(1)和第二旋转双棱镜(2)，第一旋转双棱镜(1)和第二旋转双棱镜(2)同轴布置；所述双目视觉测量系统包括所第一相机(3)、第二相机(4)、第一支杆(5)、第二支杆(6)和底板(7)，第一相机(3)与第一支杆(5)一端相连，第一支杆(5)另一端固定在底板(7)上；第二相机(4)与第二支杆(6)一端相连，第二支杆(6)另一端固定在底板(7)上；第一相机(3)和第二相机(4)同时对机器人末端(9)进行拍摄，并设置同样的拍摄间隔；对拍摄的照片进行特征匹配，匹配的内容为机器人末端标记点(10)和激光点(11)；具体步骤如下：(1)在关节机器人末端粘贴一个标记点(10)；

(2)根据关节机器人各关节的运动角度以及角速度，计算出标记点(10)随时间变化的理论运动位置曲线T1(X,Y,Z)；

(3)基于旋转双棱镜逆向方法，根据步骤(2)所述的理论运动位置曲线T1(X,Y,Z)可以计算出第一旋转双棱镜(1)和第二旋转双棱镜(2)的旋转角度(θ1(t),θ2(t))和旋转速度(v1(t),v2(t))；其中：(4)控制机器人和旋转双棱镜系统同时动作，入射激光从第一旋转双棱镜(1)入射面中心垂直入射，并根据已知的旋转角度(θ1(t),θ2(t))和旋转速度(v1(t),v2(t))控制第一旋转双棱镜(1)和第二旋转双棱镜(2)旋转；使得出射激光打在机器人末端(9)上标记点(10)的理论位置，作为机器人的理论轨迹发生器；

(5)第一相机(3)和第二相机(4)对机器人末端(9)进行拍照；

(6)对同一时刻第一相机(3)和第二相机(4)拍摄的照片，进行机器人末端标记点的图像匹配，得到机器人末端标记点在第一相机(3)内的图像坐标系坐标(x1,y1)和第二相机(4)内的图像坐标系坐标(x2,y2)；

(7)对同一时刻第一相机(3)和第二相机(4)拍摄的照片，进行机器人末端上激光点的图像匹配，得到激光点在第一相机(3)内的图像坐标系坐标(x3,y3)和第二相机(4)内的图像坐标系坐标(x4,y4)；

(8)基于双目视觉测量方法，通过步骤(6)得到的标记点(10)在第一相机(3)内的图像坐标系坐标(x1,y1)，与在第二相机(4)内的图像坐标系坐标(x2,y2)，可计算出机器人运动时末端上标记点(10)的实际三维坐标(X1,Y1,Z2)；由步骤(7)得到的激光点(11)在第一相机(3)内的图像坐标系坐标(x3,y3)，与第二相机(4)内的图像坐标系坐标(x4,y4)可计算出机器人运动时末端上激光点(11)的三维坐标(X2,Y2,Z2)；

(9)根据得到的一系列标记点(10)和激光点(11)的三维坐标，分别进行机器人实际运动轨迹和理论运动轨迹拟合，得到机器人实际运动轨迹T2(X,Y,Z)和理论运动轨迹T1(X,Y,Z)；

(10)通过比较实际运动轨道和理论运动轨迹两条曲线间距离，可得到机器人全程或局部最大运动误差。

2.根据权利要求1所述的方法，其特征在于步骤(3)中所述旋转双棱镜逆向方法，具体步骤如下：(1)第一旋转双棱镜(1)和第二旋转双棱镜(2)的楔角都为α，折射率都为n，入射光 从第一旋转双棱镜(1)垂直入射，并打在第一旋转双棱镜(1)入射面中心；已知目标点P(Xp,Yp,Zp)，并设置出射点位置精度为Δ0；

(2)假设出射光在第二旋转双棱镜(2)上的出射点为第二旋转双棱镜(2)出射面中心N0(0,0)，以 为出射光向量，得到此时出射光俯仰角ρ1和方位角(3)令第一旋转双棱镜(1)旋转角度θ1＝0°且静止不动，仅第二旋转双棱镜(2)旋转；基于旋转双棱镜矢量折射定理可得：ρ＝arccos(cosδ1cosδ2-sinδ1sinδ2cos△θr)；

式中：△θr＝θ1-θ2为两棱镜夹角，δ1为第一旋转双棱镜(1)的偏向角，即光束通过第一旋转双棱镜(1)后出射光偏离入射光的角度，可求得δ1＝arcsin(n·sinα)-α，其中，n为第一旋转双棱镜(1)的折射率；δ2为第二旋转双棱镜(2)的偏向角，即光束通过第二旋转双棱镜(2)后出射光偏离入射光的角度，可求得其中，为第二旋转双棱镜(2)的等效折射率，其值为 γr＝arctan

(tanδ1·cos△θr)，βr＝arccos(sinδ1sin△θr)；将步骤(2)得到的出射光俯仰角ρ1代入公式，求出双棱镜夹角为Δθr，即第二旋转双棱镜(2)旋转角度为θ2＝-Δθr；此时，基于矢量折射定理，根据第一旋转双棱镜(1)和第二旋转双棱镜(2)的旋转角度(0°,-Δθr)，计算得到此时出射光的方位角为(4)根据步骤(2)得到的方位角 和步骤(3)得到方位角 将第一旋转双棱镜(1)和第二旋转双棱镜(2)同时增大 后，即可求得第一旋转双棱镜(1)和第二旋转双棱镜(2)旋转角度(θ1,θ2)为(5)根据步骤(4)得到的两棱镜转角(θ1,θ2)，基于旋转双棱镜矢量折射定理，求解实际打在机器人末端点 和第二旋转双棱镜2出射点位置(6)计算偏差 判断是否满足精度要求，即是否满足Δ﹤Δ0，Δ0为给

定的目标点精度；

(7)若Δ≥Δ0，回到步骤(2)中，计算以 为出射光向量时的俯仰角ρ1

和方位角 重复步骤(2)、(3)、(4)、(5)和(6)，若Δ﹤Δ0，则结束，得出旋转双棱镜转角解(θ1,θ2)。

3.根据权利要求1所述的方法，其特征在于步骤(10)中所述全程或局部最大运动误差计算方法，具体步骤如下：(1)根据得到的理论运动轨迹曲线T1(X,Y,Z)和实际运动轨迹曲线T2(X,Y,Z)，将两条运动轨迹曲线放置在同一空间坐标下；

(2)将理论运动轨迹曲线T1(X,Y,Z)等分成n1个点，对于其中的任一点Pn(Xn,Yn,Zn)，可以计算出此点处的切线方程，方程斜率为kn；

(3)将实际运动轨迹曲线T2(X,Y,Z)等分成n2个点，对于其中的任一点Pm(Xm,Ym,Zm)，可以计算出此点处的切线方程，方程斜率为km；

(4)进行对应点的匹配，设置精度为δ0，对于实际轨迹曲线上的任一点Pm(Xm,Ym,Zm)，切线方程的斜率为km，在理论轨迹曲线上找一点Pn(Xn,Yn,Zn)，同时满足|km-kn|﹤δ0、|km-1-kn-1|﹤δ0和|km+1-kn+1|﹤δ0，则认为实际曲线上的Pm点对应理论曲线上的Pn点，则机器人运动到Pm处的运动误差为(5)重复步骤(4)计算出机器人实际运动曲线上任一点的运动误差，可得到机器人全程或局部最大运动误差。

说明书

一种机器人轨迹误差的测量方法

技术领域

[0001] 本发明涉及机器人运动轨迹误差测量方法，具体涉及一种轨迹发生器和视觉测量系统相结合的机器人轨迹误差的测量方法。

背景技术

[0002] 在先技术(机械科学与技术，2011年2期，20卷，第252页，钱瑞明《挠性机器人动态误差集成激光测量与补偿研究》)提出一种基于3个激光发生器和3个激光位置检测器(PSD)的挠性构件动态误差测量方法,该法可以测量除杆长方向外的5个变形分量,且光路和测量模型简单；建立了PSD上光点位置与构件各误差分量及机器人末端执行器动态误差之间的关系，并给出其补偿控制方法。但其控制过程复杂，计算繁琐，容易出现因控制误差而导致测量结果不准确的问题。

[0003] 在先技术(机械传动，2013年5期，6卷，第50页，王良文《用于物体捕捉的四足步行机器人的运动误差模型》)提出在四足机器人的机体上，安装图像捕捉系统，用于引导机器人完成对目标物的抓取。并在对抓取状态逆运动学分析的基础上，建立精确的图像捕捉系统误差与机器人的工作臂参数误差之间的关系，给出了详细的计算公式，得到机器人运动过程中的运动误差并进行补偿。此机器人运动误差的获取是通过采集外界环境的改变的图像来逆向计算出机器人工作臂运动误差，测量精度受后期图像处理精度限制。

[0004] 在先技术(浙江大学，专利号：201010552545.5)提到一种基于扫频激光干涉的圆轨迹运动误差快速测量系统：通过总分光镜将来自扫频激光器的光信号分成X向光信号和Y向光信号，并打到安装在机床导轨的靶镜上。最后，通过X向检测机构和Y向检测机获取机床导轨的圆轨迹运动误差。此发明装置只能检测平面上机器人轨迹运动误差，应用领域较窄。

发明内容

[0005] 本发明的目的在于提供一种机器人轨迹误差的测量方法，本发明可以实现机器人全程或局部最大运动误差测量：在机器人运动过程中，通过视觉成像系统全程实时采集机器人末端实际运动轨迹，同时轨迹发生器产生机器人运动的理论轨迹，通过比较两种轨迹，可得到机器人某一段时间内或全程最大运动误差。

[0006] 本发明提出的机器人轨迹误差的测量方法，所述方法通过用于轨迹发生器的旋转双棱镜系统和用于轨迹图像采集的双目视觉测量系统实现，所述方法用于对机器人全程或局部最大运动误差进行测量，其中：

[0007] 所述旋转双棱镜系统包括第一旋转双棱镜1和第二旋转双棱镜2，第一旋转双棱镜1和第二旋转双棱镜2同轴布置；所述双目视觉测量系统包括第一相机3、第二相机4、第一支杆5、第二支杆6和底板7，第一相机3与第一支杆5一端相连，第一支杆5另一端固定于底板7上；第二相机4与第二支杆6一端相连，第二支杆6另一端固定于底板7上。第一相机3和第二相机4同时对机器人末端9进行拍摄，并设置同样的拍摄间隔；对拍摄的照片进行特征匹配，匹配的内容为机器人末端标记点10和激光点11；具体步骤如下：

[0008] (1)在关节机器人末端粘贴一个标记点10。

[0009] (2)根据关节机器人各关节的运动角度以及角速度，计算出标记点10随时间变化的理论运动位置曲线T1(X,Y,Z)；

[0010] (3)基于旋转双棱镜逆向方法，根据步骤(2)所述的理论运动位置曲线T1(X,Y,Z)可以计算出旋转双棱镜1和旋转双棱2的旋转角度(θ1(t),θ2(t))和旋转速度(v1(t),v2(t))；其中：

[0011] (4)控制机器人和旋转双棱镜系统同时动作，入射激光从第一旋转双棱镜1入射面中心垂直入射，并根据已知的旋转角度(θ1(t),θ2(t))和旋转速度(v1(t),v2(t))，控制第一旋转双棱镜1和第二旋转双棱镜2旋转；使得出射激光打在机器人末端9上标记点10的理论运动位置曲线T1(X,Y,Z)上，作为机器人的理论轨迹发生器；

[0012] (5)第一相机3和第二相机4对机器人末端9进行拍照；

[0013] (6)对同一时刻第一相机3和第二相机4拍摄的照片，进行机器人末端标记点的图像匹配，得到机器人末端标记点在第一相机3内的图像坐标系坐标(x1,y1)，在第二相机4内的图像坐标系坐标(x2,y2)；

[0014] (7)对同一时刻第一相机3和第二相机4拍摄的照片，进行机器人末端上激光点的图像匹配，得到激光点在第一相机3内的图像坐标系坐标(x3,y3)，在第二相机4内的图像坐标系坐标(x4,y4)；

[0015] (8)基于双目视觉测量方法，通过步骤(6)得到的标记点10在第一相机3内的图像坐标系坐标(x1,y1)，与在第二相机4内的图像坐标系坐标(x2,y2)，可计算出机器人运动时末端上标记点10的三维坐标(X1,Y1,Z2)；由步骤(7)得到的激光点11在第一相机3内的图像坐标系坐标(x3,y3)，与在第二相机4内的图像坐标系坐标(x4,y4)，可计算出机器人运动时末端上激光点11的三维坐标(X2,Y2,Z2)；

[0016] (9)根据得到的一系列激光点11的三维坐标，进行机器人实际运动轨迹拟合，得到机器人实际运动轨迹曲线为T2(X,Y,Z)；

[0017] (10)通过比较实际运动轨迹和理论运动轨迹两条曲线间距离，可得到机器人全程或局部最大运动误差。

[0018] 本发明中，步骤(3)中所述旋转双棱镜逆向方法，具体步骤如下：

[0019] (1)第一旋转双棱镜1和第二旋转双棱镜2的楔角为α，入射光 从第一旋转双棱镜1垂直入射，并打在第一旋转双棱镜1入射面中心。已知目标点P(Xp,Yp,Zp)，并设置出射点位置精度为Δ0；

[0020] (2)假设出射光在第二旋转双棱镜2上的出射点为第二旋转双棱镜2出射面中心N0(0,0)，以 为出射光向量，得到此时出射光俯仰角ρ1和方位角

[0021] ρ1＝arccos(zP)，

[0022] (3)令第一旋转双棱镜1旋转角度θ1＝0°且静止不动，仅第二旋转双棱镜2旋转。基于旋转双棱镜矢量折射定理可得：ρ＝arccos(cosδ1cosδ2-sinδ1sinδ2cos△θr)；

[0023] 式中：△θr＝θ1-θ2为两棱镜夹角，δ1为棱镜1的偏向角，即光束通过棱镜1后出射光偏离入射光的角度。可求得δ1＝arcsin(n·sinα)-α。δ2为棱镜2的偏向角，即光束通过棱镜2后出射光偏离入射光的角度。可求得其中，为棱镜2的等效折射率，其
值为 γr＝arctan(tanδ1·cos△θr)，βr＝arccos(sinδ1sin△
θr)。将步骤(2)得到的出射光俯仰角ρ1带入公式，求出双棱镜夹角为Δθr，即第二旋转双棱镜2旋转角度为θ2＝-Δθr。此时，基于矢量折射定理，根据第一旋转双棱镜1和第二旋转双棱镜2的旋转角度(0°,-Δθr)，计算此时出射光的方位角为

[0024] (4)根据步骤(2)得到的方位角 和步骤三得到方位角 将棱镜1和棱镜2的旋转角度同时增大 后，即可求得第一旋转双棱镜1和第二旋转双棱镜2旋转角度(θ1,θ2)为

[0025] (5)根据步骤(4)得到的两棱镜转角(θ1,θ2)，基于旋转双棱镜矢量折射定理，求解实际打在机器人末端点P1(X1p,Y1p,Z1p)和棱镜2出射点位置N1(x1n,y1n)；

[0026] (6)计算偏差 判断是否满足精度要求，即是否满足Δ﹤Δ0，Δ0为给定的目标点精度；

[0027] (7)若Δ≥δ，回到步骤(2)中，计算以 为出射光向量时的俯仰角ρ1和方位角 重复步骤(2)、(3)、(4)、(5)和(6)，若Δ﹤Δ0，则结束，得出旋转双棱镜转角解(θ1,θ2)。

[0028] 本发明中，步骤(10)中所述全程或局部最大运动误差计算方法，具体步骤如下：

[0029] (1)根据得到的理论运动轨迹曲线T1(X,Y,Z)和实际运动轨迹曲线T2(X,Y,Z)。将两条运动轨迹曲线放置在同一空间坐标下。

[0030] (2)将理论运动轨迹曲线T1(X,Y,Z)等分成n1个点，对于其中的任一点Pn(Xn,Yn,Zn)，可以计算出此点处的切线方程，方程斜率为kn；

[0031] (3)将实际运动轨迹曲线T2(X,Y,Z)等分成n2个点，对于其中的任一点Pm(Xm,Ym,Zm)，可以计算出此点处的切线方程，方程斜率为km；

[0032] (4)进行对应点的匹配，设置精度为δ0，对于实际轨迹曲线上的任一点Pm(Xm,Ym,Zm)，切线方程的斜率为km，在理论轨迹曲线上找一点Pn(Xn,Yn,Zn)，同时满足|km-kn|﹤δ0、|km-1-kn-1|﹤δ0和|km+1-kn+1|﹤δ0。则认为实际曲线上的Pm点对应理论曲线上的Pn点。则机器人运动到Pm处的运动误差为

[0033] (5)重复步骤(4)计算出机器人实际运动曲线上任一点的运动误差，可得到机器人全程或局部最大运动误差。

[0034] 本发明的有益效果在于：

[0035] 1.本方法属于非接触测量机器人运动轨迹误差方法，不需要直接接触机器人。保证了在不破坏机器人的情况下，能精确得到机器人运动轨迹误差。

[0036] 2.易于得到精准的机器人运动轨迹误差，本发明采用双目视觉测量机器人运动轨迹误差的方法，通过两个相机对机器人末端标记点和激光扫描点拍照，利用双目视觉测量原理，可以精确的得到机器人末端标记点和激光扫描点三维坐标值，通过计算标记点和扫描点的距离得到机器人运动轨迹误差，精度最高可达到微米级别。

[0037] 3.易于实时得到机器人运动轨迹误差，本发明采用循迹测量的方法，在机器人运动过程中，对机器人末端的运动轨迹进行实时误差测量。

[0038] 4.控制方便。该装置采用独立控制方式，只需要控制旋转双棱镜旋转达到指定的角度(θ1,θ2)，保证通过旋转双棱镜的激光持续打在机器人末端的理论轨迹上；对于第一相机3和第二相机4，只需使用PC机控制第一相机3和第二相机4同时拍摄照片，并设置相同的拍摄时间间隔，控制过程简单，易于实现。

附图说明

[0039] 图1是本发明一种机器人的轨迹误差的循迹测量方法的总示意图。由三个部分组成，分别为关节机器人8、双目视觉测量系统和旋转双棱镜系统。

[0040] 图2是机器人末端9标记点和激光点11示意图。

[0041] 图3为轨迹发生器产生机器人末端理论轨迹的流程图。

[0042] 图4为旋转双棱镜逆向算法示意图。

[0043] 图5为得到机器人末端9实际运动轨迹曲线和理论运动轨迹曲线流程图。

[0044] 图6为全程或局部最大运动误差计算方法流程图。

[0045] 图7为机器人局部或全程运动轨迹最大误差测量流程图。

[0046] 图8机器人理论运动轨迹仿真图。

[0047] 图9旋转双棱镜1和旋转双棱2的旋转角度(θ1(t),θ2(t))仿真图。

[0048] 图10旋转双棱镜1和旋转双棱2的旋转速度(v1(t),v2(t))仿真图。

[0049] 图11机器人理论运动轨迹和实际运动轨迹仿真图。

[0050] 图中标号：1为第一旋转双棱镜，2为第二旋转双棱镜，3为第一相机，4为第二相机，5为第一支杆，6为第二支杆，7为底板，8为机器人，9为机器人末端，10为标记点，11为激光点。

具体实施方式

[0051] 下面通过实施例结合附图进一步说明本发明。

[0052] 实施例1：

[0053] 本发明提供一种机器人轨迹误差的测量方法，可以实现机器人全程或局部最大运动误差测量：在机器人运动过程中，通过视觉成像系统全程实时采集机器人末端实际运动轨迹，同时轨迹发生器产生机器人运动的理论轨迹，通过比较两种轨迹，可得到机器人某一段时间内或全程最大运动误差。

[0054] 2.本发明的目的通过下述几个部分来实现：包括用于轨迹发生器的旋转双棱镜系统和轨迹图像采集的双目视觉测量系统。根据机器人理论运动轨迹，旋转双棱镜产生高精度的光束扫描轨迹照射在机器人末端上，同时保证该光束扫描轨迹的跟踪时间和跟踪坐标与机器人运动实际的同步性，从而高精度模拟机器人末端运动理论轨迹。后续文字我们称该扫描轨迹为机器人理论轨迹。双目视觉测量系统用于采集机器人末端上标记点的实际运动轨迹和激光点的理论运动轨迹，最后根据得到的理论运动轨迹点坐标和实际运动轨迹点坐标计算出理论运动论轨迹与实际运动轨迹的误差，得到机器人末端运动时的运动轨迹误差。

[0055] 3.结合图1、图2和图3说明旋转双棱镜产生机器人末端理论轨迹的步骤：S1：如图2所示，在关节机器人末端9中心粘贴一标记点10。S2：根据关节机器人各关节的运动角度以及角速度等信息可以计算出标记点10随时间变化的理论运动位置方程T1(X,Y,Z,t)。S3：基于旋转双棱镜逆向算法，根据标记点空间位置T1(X,Y,Z,t)可以计算出旋转双棱镜1和旋转双棱2的旋转角度(θ1(t),θ2(t))。S4：根据第一旋转双棱镜1和第二旋转双棱2的旋转角度(θ1(t),θ2(t))，可以计算得出旋转速度 S4：激光从第一旋转双棱镜1入射面中心垂直入射，第二旋转双棱镜2出射，并根据已知的(θ1(t),θ2(t))和(v1(t),v2(t))控制第一旋转双棱镜1和第二旋转双棱镜2角旋转。使得出射激光打在机器人末端9上标记点10的理论位置，作为机器人的理论轨迹发生器。

[0056] 4.结合图4和图5说明旋转双棱镜逆向算法的过程，图4中第一旋转双棱镜1和第二旋转双棱镜2的楔角为α，入射光 从第一旋转双棱镜1垂直入射，并打在第一旋转双棱镜1入射面中心。已知目标点P(Xp,Yp,Zp)，并设置出射点位置精度为Δ0。根据逆向算法算可求出第一旋转双棱镜转角(θ1,θ2)，其步骤如下：

[0057] S1：假设出射光在第二旋转双棱镜2上的出射点为第二旋转双棱镜2出射面中心N0(0,0)，以 为出射光向量，求出出射光向量的俯仰角ρ和方位角φ。

[0058] S2：令第一旋转双棱镜1不动，仅第二旋转双棱镜2旋转，根据出射光俯仰角ρ求出第二双棱镜夹角Δθr，此时的出射光方位角为φ0。根据出射光方位角φ，第一旋转双棱镜1和第二旋转双棱镜2同时旋转φ-φ0，即得到第一旋转双棱镜1和第二旋转双棱镜2转角(θ1,θ2)。或者令第二旋转双棱镜2不动，仅第一旋转双棱镜1旋转，根据出射光俯仰角ρ求出第一旋转双棱镜1和第二旋转双棱镜2夹角Δθr，此时的出射光方位角为φ0。根据出射光方位角φ，第一旋转双棱镜1和第二旋转双棱镜2同时旋转φ-φ0，即得到第一旋转双棱镜1和第二旋转双棱镜2转角(θ1,θ2)。

[0059] S3：根据第一旋转双棱镜1和第二旋转双棱镜2转角(θ1,θ2)，求解实际打在机器人末端点P1(X1p,Y1p,Z1p)和第二旋转双棱镜2出射点位置N1(x1n,y1n)。

[0060] S4：计算偏差 判断是否满足精度要求，即是否满足Δ﹤Δ0，Δ0为给定的目标点精度。

[0061] S5：若Δ≥δ，将 作为出射光向量，重复步骤(2)、(3)、(4)；若Δ﹤Δ0，则结束，得出第一旋转双棱镜1和第二旋转双棱镜2转角解(θ1,θ2)。

[0062] 5.机器人全程或局部最大运动误差计算原理如下：在得到机器人实际运动轨迹和理论运动轨迹的基础上，通过比较两条曲线间距离，可得到机器人全程或局部最大运动误差。

[0063] 根据下列步骤完成全程或局部机器人运动轨迹最大误差的测量：

[0064] S1：如附图2所示，在关节机器人末端粘贴一个标记点10。

[0065] S2：根据关节机器人各关节的运动角度以及角速度等信息可以计算出标记点随时间变化的理论运动位置方程T1(X,Y,Z,t)为 (单位cm)，参数方程为(单位cm)，其运动曲线如附图8所示，为一空间椭圆曲线。

[0066] S3：基于旋转双棱镜逆向算法，根据S2计算所得标记点空间位置T1(X,Y,Z,t)可以计算出旋转双棱镜1和旋转双棱2的旋转角度(θ1(t),θ2(t))和旋转速度(v1(t),v2(t))，如附图9和附图10所示。

[0067] S4：同时控制机器人和轨迹发生器同时动作。入射激光从旋转双棱镜1入射面中心垂直入射，并根据已知的(θ1(t),θ2(t))和(v1(t),v2(t))控制旋转双棱镜1和旋转双棱镜2旋转。使得出射激光打在机器人末端9上标记点10的理论位置，作为机器人的理论轨迹发生器。

[0068] S5：如图1所示，第一相机3和第二相机4对机器人末端9进行拍照。

[0069] S6：对同一时刻第一相机3和第二相机4拍摄的照片，进行机器人末端标记点的图像匹配，得到机器人末端标记点在第一相机3内的图像坐标系坐标(x1,y1)和第二相机4内的图像坐标系坐标(x2,y2)。

[0070] S7：针对同一时刻第一相机3和第二相机4拍摄的照片，进行机器人末端上激光点的图像匹配，得到激光点在第一相机3内的图像坐标系坐标(x3,y3)和第二相机4内的图像坐标系坐标(x4,y4)。

[0071] S8：基于双目视觉测量原理，由(x1,y1)和(x2,y2)可计算出机器人运动时末端上标记点10的实际三维坐标(X1,Y1,Z2)。由(x3,y3)和(x4,y4)可计算出机器人运动时末端上标记点的理论三维坐标(X2,Y2,Z2)即激光点三维坐标。

[0072] S9：根据得到的所有标记点10和激光点11的三维坐标，进行机器人实际运动轨迹和理论运动轨迹拟合，如附图11所示，实线为得到的机器人理论运动轨迹，虚线为得到的机器人实际运动轨迹。

[0073] S10：根据全程或局部最大运动误差计算方法，如附图11所示，在机器人实际运动轨迹上的取一点Pm(42.43,42.43,28.28)(单位cm)，可以在机器人理论运动轨迹上找到对应的点Pn(37.98,46.45,30.97)(单位cm)，得到A点的机器人运动误差为△＝6.57cm。S11：重复步骤S10，计算出机器人实际运动曲线上任一点的运动误差，可得到机器人全程或局部最大运动误差为8.98cm。