基于立体视觉的激光焊接机器人轨迹跟踪研究
合集下载
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
[2] 汪建华, 赵晓光, 谭民. 一种 5 自由度视觉伺服机器人的跟踪控制研究[J]. 机器人. 2005, 27(3): 215-219 [3] MEZOUAR Y, CHAUMETTE F. Path planning for robust image-based control [J]. IEEE Trans on Robotics and Automation,
1 本课题得到教育部高等学校博士点基金（No. 20020003053）资助 -1-
http://www.paper.edu.cn
图 1 硬件组成
3.图像特征提取
图 2 视觉控制系统结构
3.1 激光焦点特征提取
由于激光焦点在图像上呈现高灰度值的连通区域，利用预设的灰度阈值可以将激光焦点与背景图像区 分开来，见式(1)。
n——机器人的关节数
T J ——{T} 下的雅克比矩阵
6.实验与分析
用所研制的基于立体视觉的焊接机器人轨迹测控系统开展各种实验，实验结果表明本系统可以实现轨 迹识别、检测与自动跟踪。实时轨迹跟踪的误差小于 0.5mm。
以不开坡口的平板钢板紧密对接焊缝的自动检测与跟踪实验为例进行说明，机器人末端跟踪速度 20mm/s，机器人事先未知轨迹位置和姿态。开始跟踪前的图像如图 5a 所示，图上的两条短白线为检测子 窗口，长白线为视觉检测出的焊缝接头线，白色小圆点为激光点，黑色小方块点为通过双目立体视觉计算 出的激光点到焊缝的垂足。跟踪后图像如图 5b 所示。此时激光点被控制移动到焊缝上的垂足上，之后继 续控制机器人使激光点沿着焊缝接头移动。
http://www.paper.edu.cn
基于立体视觉的激光焊接机器人轨迹跟踪研究1
张文增，张国贤，陈强，孙振国
（清华大学机械工程系 先进成形制造重点实验室，北京 100084）
E-mail: wenzeng@tsinghua.edu.cn
摘 要：为改善激光焊接机器人的运动轨迹精度，提出一种基于视觉的机器人轨迹跟踪控制方法。该方法 将摄像机安装于工业机器人末端构成视觉反馈的控制系统。其中，采用阈值分割方法检测出激光焦点，利 用 GPI 方法快速检测出焊缝线，然后以线－线匹配和双目立体视觉技术计算出激光焦点和焊缝线的空间位 置，得到激光焦点相对于焊缝线的误差，再结合机器人运动学原理控制机器人实时运动以消除此误差。实 验结果表明该方法能够有效提高机器人轨迹精度。 关键词：机器人技术，工业机器人，轨迹精度，视觉反馈
⎧gray(i, j) = 255, gray(i, j) ≥ Threshold1 ⎨⎩gray(i, j) = 0, gray(i, j) < Threshold1
(1)
其中，gray(i, j)——图像中(i, j)处的灰度值
Threshold1——灰度阈值
再采用形心计算方法可以获得该图像上激光焦点的位置，见式(2)。
1.引言
现有商售的关节型机器人只提供定位精度指标，而不提供也不保证运动轨迹精度参数。然而，随着加 工生产应用技术的不断提高，高速、高精度作业，诸如激光焊接、切割，要求非常高的轨迹精度。另外， 目前工业机器人仅能在严格结构化的环境中执行相当预定指令动作，还缺乏对环境的感知与应变能力，这 极大地限制了机器人的应用。多数的研究工作围绕着如何改善工业机器人的环境感知能力上。视觉作为一 种重要的传感方法，在扩展机器人系统的感知与应对环境变化能力方面具有重要作用。文[1]构建了基于 位置的工业机器人视觉伺服系统，提出一种在已知多个特征点间的距离时，使用单个摄像机估算目标位姿 的算法。文[2]采用基于图像的眼在手上视觉伺服结构实现了“眼注视”方式跟踪平面运动目标。文[3]通 过在图像空间的路径规划和基于图像的控制方法，解决了机器人末端的实际位置与期望位置相距较远的问 题。文[4]研制了基于视觉的工业火焰切割二维图纸线条跟踪系统。
本文为改善激光焊接工业机器人的运动轨迹精度，提出一种机器人激光焊接轨迹跟踪控制方法。该方 法将摄像机安装于工业机器人末端构成视觉反馈的控制系统。其中，采用阈值分割方法检测出激光焦点， 利用 GPI 方法快速检测出焊缝线，然后以线－线匹配和双目立体视觉技术计算出激光焦点和焊缝线的空间 位置，得到激光焦点相对于焊缝线的误差，再结合机器人运动学原理控制机器人实时运动以消除此误差， 从而达到较高的轨迹精度。
2002, 18(4): 534549. [4] 徐侃烈, 裴仁清, 汪晓刚, 等. 二维图纸线条跟踪系统研制. 上海大学学报. 1999, 5(4): 348-352 [5] Shiu Y C, Ahmad S. Calibration of wrist-mounted robotic sensors by solving homogeneous transform equations of the form
au + bv + c = 0
(3)
其中， u, v ——直线上的任意点的图像坐标, pixel
a, b, c——系数
在前一幅图像中检测出的直线的斜率附近的小角度范围可以作为当前直线检索的范围，并综合考虑激
光焦点的当前位置和焊接前进的方向，一般可以快速实现直线段的检测。若在上述范围内没有找到直线，
则说明曲率变化较大，扩大查找斜率范围进行检索。
nm
nm
∑ ∑ui gray(i, j) ∑ ∑ vi gray(i, j)
u = i=1 j=1
c
nm
, vc
=
i=1 n
j =1 m
(2)
∑ ∑ gray(i, j)
∑ ∑ gray(i, j)
i=1 j=1
i=1 j=1
其中， ui , vi ——图像坐标，pixel
uc , vc ——激光焦点（形心）图像坐标, pixel
(4)
CLl
CL m
CL n
其中， ( CL X 0 , CLY0 , CLZ0 ) ——检测出的直线上某点在{CL} 坐标系下的坐标，mm
( CLl, CLm, CLn) ——检测出的直线在{CL} 坐标系下的方向矢量，mm
4.2 垂足计算
系统的控制目标是移动机器人末端，使激光焦点到达轨迹线上，为此需要计算出激光焦点在轨迹线上
跟踪过程中机器人末端坐标系下的 X、Y、Z 各轴方向上误差以及距离误差分别如图 6 所示。实验结果 表明，X、Y、Z 各轴方向上的误差均在 0.3mm 以内，三维跟踪时距离误差小于 0.4mm，机器人不仅能对焊 缝接头自动视觉识别，同时加工轨迹的实时检测与控制精度也均满足焊接要求。
∆X \ mm ∆Y \ mm ∆Z \ mm ∆d \ mm
2.激光焊接机器人系统
建立的机器人视觉伺服系统硬件如图 1 所示，在关节型机器人末端安装激光加工工具、双摄像机，使 激光焦点和轨迹线成像于同一图像中。系统的控制结构如图 2 所示。
摄像机内外参数标定[1]、立体视觉坐标系、机器人手－眼关系[5]、机器人手－工具关系均预先标定。 加工时，由摄像机拍摄激光焦点和曲线轨迹图像，通过图像处理获取激光焦点和曲线的位置，经眼－ 手坐标变换和手－工具坐标变换得到工具坐标系下的激光焦点和曲线的位置，控制目标是使得激光焦点达 到曲线上，同时满足激光焦点沿轨迹线的运动速度为预定的焊接速度。
ETT
。
采用固定姿态的轨迹跟踪方法，利用{T} 下的雅克比矩阵计算关节空间中运动量（即驱动角），见式
（9）：
[ ] ( ) q1
⋅⋅⋅
qi
⋅⋅⋅
qn T = n×1
TJ
n×6
⎡ ⎢ ⎣
T pMN 0
⎤ ⎥ ⎦ 6×1
(9)
其中， qi , (i = 1, 2,⋅⋅⋅, n) ——机器人第 i 关节运动量
参考文献
[1] MANA Saedan, MARCELO H, 3D vision-based control on an industrial robot. Proc of IASTED Intel Conf on Robotics and Applications. Florida, USA. Nov, 2001: 152-157
在前一幅图像中检测出的激光焦点位置附近开设较小的子窗口，缩小搜索范围，可以提高图像上激光
焦点位置的检测速度。
3.2 焊缝曲线特征提取
由于焊缝曲线的曲率较小，可以将曲线分成若干段，每段以直线段近似，在图像中检测曲线特征的任
务就转换为检测直线段。采用灰度投影积分法[6]实现了直线目标的快速检测，见式(3)
{T} 坐标系下的运动量 T pMN 由式(8)计算：
T pMN = TTE ETCL (CL N −CL M )
(8)
{T} 下的雅克比矩阵与{E} 下的雅克比矩阵求解方法相似，只是将最后一个连杆坐标系(即{E} )相对
于倒数第
2
个连杆坐标系之间的变换矩阵关系 n−1An
改变为：
B n−1 n
=
A n−1 n
5.运动量计算
将{CL} 坐标系下激光焦点和垂足转换到工具坐标系{T} 下，再应用{T} 下的雅克比矩阵计算机器人 关节空间的运动量。采用如下方法建立工具坐标系{T} ：
（1）原点 OT 与激光焦点重合；
（2）ZT 轴与激光束中心重合，从光纤头指向工件；
（3）XT 轴单位矢量由式(6)确定：
E xT = E zT × E xCL / E zT × E xCL
(6)
ห้องสมุดไป่ตู้
（4）YT 轴单位矢量由式(7)得到：
E yT = E zT × E xT
(7)
其中， E xCL ——{CL}坐标系的 X 轴单位矢量在{E}坐标系下的描述
E xT , E yT , E zT ——{T}坐标系的各轴单位矢量分别在{E}坐标系下的描述。
-3-
http://www.paper.edu.cn
（a）初始图像
（b）跟踪后图像
图 5 左摄像机下跟踪前后激光点与直线图像
s\ mm (a)X 方向误差
s \ mm
(b)Y 方向误差
s\ mm
(c)Z 方向误差 图 6 跟踪误差
-4-
s\ mm (c)三维距离误差
http://www.paper.edu.cn
7.结论
本文为改善激光焊接工业机器人的运动轨迹精度，提出一种基于视觉反馈的激光焊接机器人轨迹跟踪 控制方法。该方法以双目立体视觉观察激光焦点和焊缝线，采用阈值分割方法检测出激光焦点，采用 GPI 方法检测出焊缝线，通过线－线匹配和立体视觉技术计算出激光焦点相对于焊缝线的误差，再结合机器人 运动学原理控制机器人实时运动以消除此误差，从而达到较高的轨迹精度。
)+
CL m(Y
−
CLYM
)+
CLn(Z
−
CL Z M
)
=
0
⎨ ⎪⎩
X
− CL X 0 CLl
=
Y
− CLY0 CL m
=
Z
− CLZ0 CL n
(5)
其中， ( CL X M , CLYM , CLZM ) ——激光焦点在{CL} 坐标系下的坐标，mm
通过求解式(4)得到{CL} 坐标系下的垂足 ( CL X N , CLYN , CLZN ) 。
在不同的环境光照条件下，直线与背景的灰度区别程度不同，会导致直线检索失败，为克服此问题，
应在实验前对加工区域的光照条件进行一定的实验，得出合适的直线检索阈值，并在实验过程中自动修正
阈值以适应环境光照的变化。
-2-
http://www.paper.edu.cn
4.点、直线三维恢复和垂足计算
4.1 点、直线三维恢复
导公式的前提是小孔成像模型，其摄像机距离目标较远，近似认为 ZCL − f ≈ ZCL 成立。而在本系统中，为了 提高误差的视觉分辨率，不宜将摄像机放置过远处。以此为基础，提出了考虑 ZCL − f ≠ ZCL 的直线－直线匹 配修正方法。
于是求得空间直线在{CL}下的直线方程：
X − CL X 0 = Y − CLY0 = Z − CLZ0
采用上述方法获得了左、右两只摄像机所拍摄图像上的激光焦点位置后，由于它们是匹配的，且已知
双摄像机之间关系，可以利用立体视觉原理恢复计算出激光焦点在左摄像机坐标系中的位置。
文[7]提出采用直线－直线匹配方法从左、右眼图像上的平面直线恢复计算出三维空间中的真实轨迹
线。研究发现，此方法会产生一定误差，且当摄像机适当远离目标，误差有减小倾向，这是因为文[7]所推
的垂足，以当前激光焦点为起点，以计算出的垂足为终点，控制机器人移动。
在轨迹线分段后，用直线段近似，相应曲线垂足算法转化为求激光焦点 M 与直线段的垂足 N 问题。
利用两点间的距离公式，先求出过激光焦点 M 并且与轨迹线垂直的平面，该平面与轨迹线的交点即为垂
足。见式（5）：
⎧ CLl( X ⎪
−
CL X M
1 本课题得到教育部高等学校博士点基金（No. 20020003053）资助 -1-
http://www.paper.edu.cn
图 1 硬件组成
3.图像特征提取
图 2 视觉控制系统结构
3.1 激光焦点特征提取
由于激光焦点在图像上呈现高灰度值的连通区域，利用预设的灰度阈值可以将激光焦点与背景图像区 分开来，见式(1)。
n——机器人的关节数
T J ——{T} 下的雅克比矩阵
6.实验与分析
用所研制的基于立体视觉的焊接机器人轨迹测控系统开展各种实验，实验结果表明本系统可以实现轨 迹识别、检测与自动跟踪。实时轨迹跟踪的误差小于 0.5mm。
以不开坡口的平板钢板紧密对接焊缝的自动检测与跟踪实验为例进行说明，机器人末端跟踪速度 20mm/s，机器人事先未知轨迹位置和姿态。开始跟踪前的图像如图 5a 所示，图上的两条短白线为检测子 窗口，长白线为视觉检测出的焊缝接头线，白色小圆点为激光点，黑色小方块点为通过双目立体视觉计算 出的激光点到焊缝的垂足。跟踪后图像如图 5b 所示。此时激光点被控制移动到焊缝上的垂足上，之后继 续控制机器人使激光点沿着焊缝接头移动。
http://www.paper.edu.cn
基于立体视觉的激光焊接机器人轨迹跟踪研究1
张文增，张国贤，陈强，孙振国
（清华大学机械工程系 先进成形制造重点实验室，北京 100084）
E-mail: wenzeng@tsinghua.edu.cn
摘 要：为改善激光焊接机器人的运动轨迹精度，提出一种基于视觉的机器人轨迹跟踪控制方法。该方法 将摄像机安装于工业机器人末端构成视觉反馈的控制系统。其中，采用阈值分割方法检测出激光焦点，利 用 GPI 方法快速检测出焊缝线，然后以线－线匹配和双目立体视觉技术计算出激光焦点和焊缝线的空间位 置，得到激光焦点相对于焊缝线的误差，再结合机器人运动学原理控制机器人实时运动以消除此误差。实 验结果表明该方法能够有效提高机器人轨迹精度。 关键词：机器人技术，工业机器人，轨迹精度，视觉反馈
⎧gray(i, j) = 255, gray(i, j) ≥ Threshold1 ⎨⎩gray(i, j) = 0, gray(i, j) < Threshold1
(1)
其中，gray(i, j)——图像中(i, j)处的灰度值
Threshold1——灰度阈值
再采用形心计算方法可以获得该图像上激光焦点的位置，见式(2)。
1.引言
现有商售的关节型机器人只提供定位精度指标，而不提供也不保证运动轨迹精度参数。然而，随着加 工生产应用技术的不断提高，高速、高精度作业，诸如激光焊接、切割，要求非常高的轨迹精度。另外， 目前工业机器人仅能在严格结构化的环境中执行相当预定指令动作，还缺乏对环境的感知与应变能力，这 极大地限制了机器人的应用。多数的研究工作围绕着如何改善工业机器人的环境感知能力上。视觉作为一 种重要的传感方法，在扩展机器人系统的感知与应对环境变化能力方面具有重要作用。文[1]构建了基于 位置的工业机器人视觉伺服系统，提出一种在已知多个特征点间的距离时，使用单个摄像机估算目标位姿 的算法。文[2]采用基于图像的眼在手上视觉伺服结构实现了“眼注视”方式跟踪平面运动目标。文[3]通 过在图像空间的路径规划和基于图像的控制方法，解决了机器人末端的实际位置与期望位置相距较远的问 题。文[4]研制了基于视觉的工业火焰切割二维图纸线条跟踪系统。
本文为改善激光焊接工业机器人的运动轨迹精度，提出一种机器人激光焊接轨迹跟踪控制方法。该方 法将摄像机安装于工业机器人末端构成视觉反馈的控制系统。其中，采用阈值分割方法检测出激光焦点， 利用 GPI 方法快速检测出焊缝线，然后以线－线匹配和双目立体视觉技术计算出激光焦点和焊缝线的空间 位置，得到激光焦点相对于焊缝线的误差，再结合机器人运动学原理控制机器人实时运动以消除此误差， 从而达到较高的轨迹精度。
2002, 18(4): 534549. [4] 徐侃烈, 裴仁清, 汪晓刚, 等. 二维图纸线条跟踪系统研制. 上海大学学报. 1999, 5(4): 348-352 [5] Shiu Y C, Ahmad S. Calibration of wrist-mounted robotic sensors by solving homogeneous transform equations of the form
au + bv + c = 0
(3)
其中， u, v ——直线上的任意点的图像坐标, pixel
a, b, c——系数
在前一幅图像中检测出的直线的斜率附近的小角度范围可以作为当前直线检索的范围，并综合考虑激
光焦点的当前位置和焊接前进的方向，一般可以快速实现直线段的检测。若在上述范围内没有找到直线，
则说明曲率变化较大，扩大查找斜率范围进行检索。
nm
nm
∑ ∑ui gray(i, j) ∑ ∑ vi gray(i, j)
u = i=1 j=1
c
nm
, vc
=
i=1 n
j =1 m
(2)
∑ ∑ gray(i, j)
∑ ∑ gray(i, j)
i=1 j=1
i=1 j=1
其中， ui , vi ——图像坐标，pixel
uc , vc ——激光焦点（形心）图像坐标, pixel
(4)
CLl
CL m
CL n
其中， ( CL X 0 , CLY0 , CLZ0 ) ——检测出的直线上某点在{CL} 坐标系下的坐标，mm
( CLl, CLm, CLn) ——检测出的直线在{CL} 坐标系下的方向矢量，mm
4.2 垂足计算
系统的控制目标是移动机器人末端，使激光焦点到达轨迹线上，为此需要计算出激光焦点在轨迹线上
跟踪过程中机器人末端坐标系下的 X、Y、Z 各轴方向上误差以及距离误差分别如图 6 所示。实验结果 表明，X、Y、Z 各轴方向上的误差均在 0.3mm 以内，三维跟踪时距离误差小于 0.4mm，机器人不仅能对焊 缝接头自动视觉识别，同时加工轨迹的实时检测与控制精度也均满足焊接要求。
∆X \ mm ∆Y \ mm ∆Z \ mm ∆d \ mm
2.激光焊接机器人系统
建立的机器人视觉伺服系统硬件如图 1 所示，在关节型机器人末端安装激光加工工具、双摄像机，使 激光焦点和轨迹线成像于同一图像中。系统的控制结构如图 2 所示。
摄像机内外参数标定[1]、立体视觉坐标系、机器人手－眼关系[5]、机器人手－工具关系均预先标定。 加工时，由摄像机拍摄激光焦点和曲线轨迹图像，通过图像处理获取激光焦点和曲线的位置，经眼－ 手坐标变换和手－工具坐标变换得到工具坐标系下的激光焦点和曲线的位置，控制目标是使得激光焦点达 到曲线上，同时满足激光焦点沿轨迹线的运动速度为预定的焊接速度。
ETT
。
采用固定姿态的轨迹跟踪方法，利用{T} 下的雅克比矩阵计算关节空间中运动量（即驱动角），见式
（9）：
[ ] ( ) q1
⋅⋅⋅
qi
⋅⋅⋅
qn T = n×1
TJ
n×6
⎡ ⎢ ⎣
T pMN 0
⎤ ⎥ ⎦ 6×1
(9)
其中， qi , (i = 1, 2,⋅⋅⋅, n) ——机器人第 i 关节运动量
参考文献
[1] MANA Saedan, MARCELO H, 3D vision-based control on an industrial robot. Proc of IASTED Intel Conf on Robotics and Applications. Florida, USA. Nov, 2001: 152-157
在前一幅图像中检测出的激光焦点位置附近开设较小的子窗口，缩小搜索范围，可以提高图像上激光
焦点位置的检测速度。
3.2 焊缝曲线特征提取
由于焊缝曲线的曲率较小，可以将曲线分成若干段，每段以直线段近似，在图像中检测曲线特征的任
务就转换为检测直线段。采用灰度投影积分法[6]实现了直线目标的快速检测，见式(3)
{T} 坐标系下的运动量 T pMN 由式(8)计算：
T pMN = TTE ETCL (CL N −CL M )
(8)
{T} 下的雅克比矩阵与{E} 下的雅克比矩阵求解方法相似，只是将最后一个连杆坐标系(即{E} )相对
于倒数第
2
个连杆坐标系之间的变换矩阵关系 n−1An
改变为：
B n−1 n
=
A n−1 n
5.运动量计算
将{CL} 坐标系下激光焦点和垂足转换到工具坐标系{T} 下，再应用{T} 下的雅克比矩阵计算机器人 关节空间的运动量。采用如下方法建立工具坐标系{T} ：
（1）原点 OT 与激光焦点重合；
（2）ZT 轴与激光束中心重合，从光纤头指向工件；
（3）XT 轴单位矢量由式(6)确定：
E xT = E zT × E xCL / E zT × E xCL
(6)
ห้องสมุดไป่ตู้
（4）YT 轴单位矢量由式(7)得到：
E yT = E zT × E xT
(7)
其中， E xCL ——{CL}坐标系的 X 轴单位矢量在{E}坐标系下的描述
E xT , E yT , E zT ——{T}坐标系的各轴单位矢量分别在{E}坐标系下的描述。
-3-
http://www.paper.edu.cn
（a）初始图像
（b）跟踪后图像
图 5 左摄像机下跟踪前后激光点与直线图像
s\ mm (a)X 方向误差
s \ mm
(b)Y 方向误差
s\ mm
(c)Z 方向误差 图 6 跟踪误差
-4-
s\ mm (c)三维距离误差
http://www.paper.edu.cn
7.结论
本文为改善激光焊接工业机器人的运动轨迹精度，提出一种基于视觉反馈的激光焊接机器人轨迹跟踪 控制方法。该方法以双目立体视觉观察激光焦点和焊缝线，采用阈值分割方法检测出激光焦点，采用 GPI 方法检测出焊缝线，通过线－线匹配和立体视觉技术计算出激光焦点相对于焊缝线的误差，再结合机器人 运动学原理控制机器人实时运动以消除此误差，从而达到较高的轨迹精度。
)+
CL m(Y
−
CLYM
)+
CLn(Z
−
CL Z M
)
=
0
⎨ ⎪⎩
X
− CL X 0 CLl
=
Y
− CLY0 CL m
=
Z
− CLZ0 CL n
(5)
其中， ( CL X M , CLYM , CLZM ) ——激光焦点在{CL} 坐标系下的坐标，mm
通过求解式(4)得到{CL} 坐标系下的垂足 ( CL X N , CLYN , CLZN ) 。
在不同的环境光照条件下，直线与背景的灰度区别程度不同，会导致直线检索失败，为克服此问题，
应在实验前对加工区域的光照条件进行一定的实验，得出合适的直线检索阈值，并在实验过程中自动修正
阈值以适应环境光照的变化。
-2-
http://www.paper.edu.cn
4.点、直线三维恢复和垂足计算
4.1 点、直线三维恢复
导公式的前提是小孔成像模型，其摄像机距离目标较远，近似认为 ZCL − f ≈ ZCL 成立。而在本系统中，为了 提高误差的视觉分辨率，不宜将摄像机放置过远处。以此为基础，提出了考虑 ZCL − f ≠ ZCL 的直线－直线匹 配修正方法。
于是求得空间直线在{CL}下的直线方程：
X − CL X 0 = Y − CLY0 = Z − CLZ0
采用上述方法获得了左、右两只摄像机所拍摄图像上的激光焦点位置后，由于它们是匹配的，且已知
双摄像机之间关系，可以利用立体视觉原理恢复计算出激光焦点在左摄像机坐标系中的位置。
文[7]提出采用直线－直线匹配方法从左、右眼图像上的平面直线恢复计算出三维空间中的真实轨迹
线。研究发现，此方法会产生一定误差，且当摄像机适当远离目标，误差有减小倾向，这是因为文[7]所推
的垂足，以当前激光焦点为起点，以计算出的垂足为终点，控制机器人移动。
在轨迹线分段后，用直线段近似，相应曲线垂足算法转化为求激光焦点 M 与直线段的垂足 N 问题。
利用两点间的距离公式，先求出过激光焦点 M 并且与轨迹线垂直的平面，该平面与轨迹线的交点即为垂
足。见式（5）：
⎧ CLl( X ⎪
−
CL X M