CN110378956A - 用于隧道灯清洁的隧道灯具定位方法及系统 - Google Patents

Abstract

本发明提供的一种用于隧道灯清洁的隧道灯具定位方法，包括清洁车、设置于清洁车的机械臂以及设置于机械臂执行末端的清洁装置，所述隧道灯具定位方法包括如下步骤：S1.以设置在清洁车的双目摄像机采集隧道灯具的第一图像信息，并以双目摄像机所在位置为坐标原点建立三维直角坐标系，根据第一图像信息获取隧道灯具在三维坐标系中的坐标位置；S2.采集机械臂的各关节点的实际角度，并根据各机械臂的实际角度确定出机械臂的执行末端在三维坐标系中的位置参数；S3.根据隧道灯具在三维坐标系中的位置以及机械臂的执行末端的位置参数，根据该位置参数确定出机械臂的各关节点的旋转角度，根据旋转角度控制机械臂关节点的旋转将清洁装置与隧道灯具对准，控制机械臂动作将清洁装置与隧道灯具对准。

Description

用于隧道灯清洁的隧道灯具定位方法及系统

技术领域

本发明涉及隧道灯具清洁领域，尤其涉及一种用于隧道灯清洁的隧道灯具定位方法及系统。

背景技术

公路隧道是现代公路网中即为重要的通道，能够有效缩短两地的道路通行距离，并且能够改善道路通行环境，由于隧道具有光线密闭的特性(只有进出口具有自然光，而在隧道中段则完全没有光线)，因此，在隧道中一般分布有众多的照明灯具，由于隧道空气环境，隧道灯具的表面容易积尘，造成隧道灯随着使用时间的推移因为积尘而造成亮度不足，进而造成安全隐患。

在对隧道灯具的清洁中，传统方法采用脚手架，然后有人工站在脚手架上擦洗，这种方式由于脚手架的搭建以及脚手架的移动，从而造成效率低下，而且会阻碍交通的正常通行，即使在夜间施工，在交通枢纽的隧道中仍然会造成交通拥堵，更为重要的是存在严重的安全隐患。因此，随着技术的发展，人们逐渐采用机械的方式，在车辆上安装升降机，将人运送到灯具位置处进行擦洗，这在一定程度上提高了效率，但是效率仍然地下，虽然，现有技术中也提出了一些自动清洗装置，但是这些装置在人工操控下，人工将清洁装置与灯具进行对准，在清洁过程中清洁车需要停车清洗，从而使得清洗效率仍然得不到有效提高。

因此，为了解决上述技术问题，亟需提出一种新的解决方案。

发明内容

有鉴于此，本发明的目的是提供一种用于隧道灯清洁的隧道灯具定位方法及系统，能够在清洁车不停车的条件下将清洁装置与隧道灯具位置进行对准，进而完成清洗，全程无需人工干预，能够有效提高隧道灯具的清洁效率，不会对交通通行造成大的干扰，而且能够避免传统技术中的安全隐患。

本发明提供的一种用于隧道灯清洁的隧道灯具定位方法，包括清洁车、设置于清洁车的机械臂以及设置于机械臂执行末端的清洁装置，所述隧道灯具定位方法包括如下步骤：

S1.以设置在清洁车的双目摄像机采集隧道灯具的第一图像信息，并以双目摄像机所在位置为坐标原点建立三维直角坐标系，根据第一图像信息获取隧道灯具在三维坐标系中的坐标位置；

S2.采集机械臂的各关节点的实际角度，并根据各机械臂的实际角度确定出机械臂的执行末端在三维坐标系中的位置参数；

S3.根据隧道灯具在三维坐标系中的位置以及机械臂的执行末端的位置参数，根据该位置参数确定出机械臂的各关节点的旋转角度，根据旋转角度控制机械臂关节点的旋转将清洁装置与隧道灯具对准。

进一步，步骤S2中，根据如下数学模型确定机械臂执行末端在三维坐标系的位置参数：

Xe＝m

Ye＝n

Ze＝α

β_e＝θ₂+θ₃+θ₄；

其中，Xe、Ye以及Ze分别为机械臂的执行末端在三维坐标系的X、Y和Z方向的坐标值，β_e为机械臂执行末端的姿态角，m、n和α分别为中间变量，其中：

m＝c₁(L₄c₂₃₄+L₃c₂₃+L₂c₂+L1)；

n＝s₁(L₄s₂₃₄+L₃s₂₃+L₂c₂+L1)

α＝L₄s₂₃₄+L₃s₂₃+L₂s₂；其中，c₂₃₄＝cos(α₂+θ₃+θ₄)，c₂₃＝cos(θ₂+θ₃)，c₂＝cosθ₂，s₁＝sinθ₁，s₂₃₄＝sin(α₂+θ₃+θ₄)，s₂₃＝sin(θ₂+θ₃)，c₁＝cosθ₁，s₂＝sinθ₂；其中，θ₁为机械臂的第一关节轴三维坐标系的XY平面的实际角度，θ₂为第二关节轴与第一关节轴之间的实际角度，θ₃为第二关节轴与第三关节轴之间的实际角度，θ₄为第四关节轴与第三关节轴之间的实际角度；α₂＝0。

进一步，步骤S3中，根据如下方法确定第一关节轴在三维坐标系的XOY平面上的转动角度θ′₁：

将隧道灯具以及第一关节轴垂直投影到三维直角坐标系的XOY平面上，并以第一关节点为原点，第一关节轴的投影为横坐标建立直角坐标系(x'o'y')，根据隧道灯具在直角坐标系(x'o'y')的坐标计算第一关节点的转动角度

进一步，步骤S3中，根据如下方法确定出第二关节轴的旋转角度θ′₂：

进一步，步骤S3中，根据如下方法确定出第三关节轴的旋转角度θ′₃：

θ′₃＝arctan2(n-L₃s₂,m-L₃c₂)-θ₂。

进一步，步骤S3中，根据如下方法确定出第四关节轴的旋转角度θ′₄：

θ′₄＝β_e-(θ′₂+θ′₃)。

相应地，本发明还提供了一种用于隧道灯清洁的隧道灯具定位系统，包括双目摄像机、摄像机、距离传感器以及控制器；

所述双目摄像机，设置于清洁车上，用于采集隧道灯具的第一图像信息并输出至控制器中；

所述摄像机，设置于清洁装置处，用于隧道灯具的第二图像信息；

角度传感器，设置于机械臂的关节点处，用于检测机械臂的各关节轴的关节点处，用于检测各关节轴的实际角度；

距离传感器，设置于清洁装置处，用于采集清洁装置与隧道灯具的实际距离并距离信息传输至控制器中；

所述控制器，用于接收双目摄像机、摄像机以及距离传感器输出的信息，并以双目摄像机所在位置为坐标原点为坐标系，通过第一图像信息确定灯具的位置信息，根据角度传感器输出的角度信息确定出机械臂的执行末端的位置参数，然后根据机械臂执行末端的位置参数确定出机械臂各关节点转动角度并控制机械臂各关节点转动使清洁装置到达隧道灯具位置。

进一步，所述控制器根据如下方法确定出机械臂执行末端的位置参数：

Xe＝m

Ye＝n

Ze＝α

β_e＝θ₂+θ₃+θ₄；

其中，Xe、Ye以及Ze分别为机械臂的执行末端在三维坐标系的X、Y和Z方向的坐标值，β_e为机械臂执行末端的姿态角，m、n和α分别为中间变量，其中：

m＝c₁(L₄c₂₃₄+L₃c₂₃+L₂c₂+L1)；

n＝s₁(L₄s₂₃₄+L₃s₂₃+L₂c₂+L1)

α＝L₄s₂₃₄+L₃s₂₃+L₂s₂；其中，c₂₃₄＝cos(α₂+θ₃+θ₄)，c₂₃＝cos(θ₂+θ₃)，c₂＝cosθ₂，s₁＝sinθ₁，s₂₃₄＝sin(α₂+θ₃+θ₄)，s₂₃＝sin(θ₂+θ₃)，c₁＝cosθ₁，s₂＝sinθ₂；其中，θ₁为机械臂的第一关节轴三维坐标系的XY平面的实际角度，θ₂为第二关节轴与第一关节轴之间的实际角度，θ₃为第二关节轴与第三关节轴之间的实际角度，θ₄为第四关节轴与第三关节轴之间的实际角度；α₂＝0。

进一步，控制器根据如下方法控制各机械臂关节点的转动角度：

第一关节轴在三维坐标系的XOY平面上的转动角度θ′₁：

将隧道灯具以及第一关节轴垂直投影到三维直角坐标系的XOY平面上，并以第一关节点为原点，第一关节轴的投影为横坐标建立直角坐标系(x'o'y')，根据隧道灯具在直角坐标系(x'o'y')的坐标计算第一关节点的转动角度

第二关节轴的旋转角度θ′₂：

第三关节轴的旋转角度θ′₃：

θ′₃＝arctan2(n-L₃s₂,m-L₃c₂)-θ₂；

第四关节轴的旋转角度θ′₄：

θ′₄＝β_e-(θ′₂+θ′₃)。

本发明的有益效果：通过本发明，能够在清洁车不停车的条件下将清洁装置与隧道灯具位置进行对准，进而完成清洗，全程无需人工干预，能够有效提高隧道灯具的清洁效率，不会对交通通行造成大的干扰，而且能够避免传统技术中的安全隐患。

附图说明

下面结合附图和实施例对本发明作进一步描述：

图1为本发明的结构示意图。

图2为本发明的系统示意图。

其中，1为双目摄像机，2为清洁车，3为第一关节点，4为第二关节点，5为机械臂的关节轴，7为第三关节点，6为第四关节点，8为清洁装置，9为距离传感器，10为摄像机，11为隧道灯具，12为角度传感器，13为用于安装机械臂第一关节点的舵机。

具体实施方式

以下结合说明书附图对本发明做出进一步详细说明：

本发明提供的一种用于隧道灯清洁的隧道灯具定位方法，包括清洁车2、设置于清洁车2的机械臂以及设置于机械臂执行末端的清洁装置8，所述隧道灯具定位方法包括如下步骤：

S1.以设置在清洁车的双目摄像机1采集隧道灯具11的第一图像信息，并以双目摄像机所在位置为坐标原点建立三维直角坐标系，根据第一图像信息获取隧道灯具在三维坐标系中的坐标位置；

S2.采集机械臂的各关节点的实际角度，并根据各机械臂的实际角度确定出机械臂的执行末端在三维坐标系中的位置参数；

S3.根据隧道灯具在三维坐标系中的位置以及机械臂的执行末端的位置参数，根据该位置参数确定出机械臂的各关节点的旋转角度，根据旋转角度控制机械臂关节点的旋转将清洁装置与隧道灯具对准。其中，机械臂采用如图1所示的机械臂，具有四个关节点，即机械臂的各关节轴绕着该关节点转动，在实际中，双目摄像机与机械臂的第一关节点3处于同一水平面上，保证以双目摄像机为坐标原点建立的三维坐标系的XOY平面将第一关节包含在其中，从而利于后续的处理分析，双目摄像机用于定位，另一方面，还可以对前方的障碍物进行检测，通过本发明的方法，能够在清洁车不停车的条件下将清洁装置与隧道灯具位置进行对准，进而完成清洗，全程无需人工干预，能够有效提高隧道灯具的清洁效率，不会对交通通行造成大的干扰，而且能够避免传统技术中的安全隐患，当然，清洁车上设置有舵机13，第一关节点的转动由舵机驱动，控制第一关节点在水平方向转动，第二关节点、第三关节点以及第四关节点的转动采用现有的驱动机构，比如电动气缸、电动机实现。

为了实现对隧道灯具的准确定位以及将清洁装置对准灯具，在步骤S2中，根据如下数学模型确定机械臂执行末端在三维坐标系的位置参数：

Xe＝m

Ye＝n

Ze＝α

β_e＝θ₂+θ₃+θ₄；

其中，Xe、Ye以及Ze分别为机械臂的执行末端在三维坐标系的X、Y和Z方向的坐标值，β_e为机械臂执行末端的姿态角，m、n和α分别为中间变量，其中：

m＝c₁(L₄c₂₃₄+L₃c₂₃+L₂c₂+L1)；

n＝s₁(L₄s₂₃₄+L₃s₂₃+L₂c₂+L1)

α＝L₄s₂₃₄+L₃s₂₃+L₂s₂；其中，c₂₃₄＝cos(α₂+θ₃+θ₄)，c₂₃＝cos(θ₂+θ₃)，c₂＝cosθ₂，s₁＝sinθ₁，s₂₃₄＝sin(α₂+θ₃+θ₄)，s₂₃＝sin(θ₂+θ₃)，c₁＝cosθ₁，s₂＝sinθ₂；其中，θ₁为机械臂的第一关节轴三维坐标系的XY平面的实际角度，θ₂为第二关节轴与第一关节轴之间的实际角度，θ₃为第二关节轴与第三关节轴之间的实际角度，θ₄为第四关节轴与第三关节轴之间的实际角度；α₂＝0，其中L₁为第一关节轴14的长度，L₂为第二节轴5的长度，L₃为第三关节轴15的长度，L₄为第四关节轴16的长度，通过上述的方法，能够准确地计算出机械臂的执行末端的位置参数，其中，机械臂执行末端指的是用于安装清洁装置的端部。

本实施例中，步骤S3中，根据如下方法确定第一关节轴在三维坐标系的XOY平面上的转动角度θ′₁：

将隧道灯具以及第一关节轴垂直投影到三维直角坐标系的XOY平面上，并以第一关节点为原点，第一关节轴的投影为横坐标建立直角坐标系(x'o'y')，根据隧道灯具在直角坐标系(x'o'y')的坐标计算第一关节点的转动角度

根据如下方法确定出第二关节轴的旋转角度θ′₂：

步骤S3中，根据如下方法确定出第三关节轴的旋转角度θ′₃：

θ′₃＝arctan2(n-L₃s₂,m-L₃c₂)-θ₂。

步骤S3中，根据如下方法确定出第四关节轴的旋转角度θ′₄：

θ′₄＝β_e-(θ′₂+θ′₃)，其中，θ′₂、θ′₃和θ′₄为第二关节点、第三关节点和第四关节点在数值方向的转动角，通过上述方法，能够准确得出各个关节点的转动角度，从而控制机械臂能够准确地将清洁装置送至灯具位置处。

相应地，本发明还提供了一种用于隧道灯清洁的隧道灯具定位系统，包括双目摄像机、摄像机、距离传感器以及控制器；

所述双目摄像机1，设置于清洁车上，用于采集隧道灯具的第一图像信息并输出至控制器中；

所述摄像机10，设置于清洁装置处，用于隧道灯具的第二图像信息；

角度传感器12，设置于机械臂的关节点处，用于检测机械臂的各关节轴的关节点处，用于检测各关节轴的实际角度；

距离传感器9，设置于清洁装置处，用于采集清洁装置与隧道灯具的实际距离并距离信息传输至控制器中；

所述控制器，用于接收双目摄像机、摄像机以及距离传感器输出的信息，并以双目摄像机所在位置为坐标原点为坐标系，通过第一图像信息确定灯具的位置信息，根据角度传感器输出的角度信息确定出机械臂的执行末端的位置参数，然后根据机械臂执行末端的位置参数确定出机械臂各关节点转动角度并控制机械臂各关节点转动使清洁装置到达隧道灯具位置，通过上述装置，能够在清洁车不停车的条件下将清洁装置与隧道灯具位置进行对准，进而完成清洗，全程无需人工干预，能够有效提高隧道灯具的清洁效率，不会对交通通行造成大的干扰，而且能够避免传统技术中的安全隐患。

本实施例中，所述控制器根据如下方法确定出机械臂执行末端的位置参数：

Xe＝m

Ye＝n

Ze＝α

β_e＝θ₂+θ₃+θ₄；

其中，Xe、Ye以及Ze分别为机械臂的执行末端在三维坐标系的X、Y和Z方向的坐标值，β_e为机械臂执行末端的姿态角，m、n和α分别为中间变量，其中：

m＝c₁(L₄c₂₃₄+L₃c₂₃+L₂c₂+L1)；

n＝s₁(L₄s₂₃₄+L₃s₂₃+L₂c₂+L1)

α＝L₄s₂₃₄+L₃s₂₃+L₂s₂；其中，c₂₃₄＝cos(α₂+θ₃+θ₄)，c₂₃＝cos(θ₂+θ₃)，c₂＝cosθ₂，s₁＝sinθ₁，s₂₃₄＝sin(α₂+θ₃+θ₄)，s₂₃＝sin(θ₂+θ₃)，c₁＝cosθ₁，s₂＝sinθ₂；其中，θ₁为机械臂的第一关节轴三维坐标系的XY平面的实际角度，θ₂为第二关节轴与第一关节轴之间的实际角度，θ₃为第二关节轴与第三关节轴之间的实际角度，θ₄为第四关节轴与第三关节轴之间的实际角度；α₂＝0。

具体地：控制器根据如下方法控制各机械臂关节点的转动角度：

第一关节轴在三维坐标系的XOY平面上的转动角度θ′₁：

将隧道灯具以及第一关节轴垂直投影到三维直角坐标系的XOY平面上，并以第一关节点为原点，第一关节轴的投影为横坐标建立直角坐标系(x'o'y')，根据隧道灯具在直角坐标系(x'o'y')的坐标计算第一关节点的转动角度

第二关节轴的旋转角度θ′₂：

第三关节轴的旋转角度θ′₃：

θ′₃＝arctan2(n-L₃s₂,m-L₃c₂)-θ₂；

第四关节轴的旋转角度θ′₄：

θ′₄＝β_e-(θ′₂+θ′₃)。通过上述方法，能够准确地控制第一关节点的水平转动、第二关节点、第三关节点以及第四关节点在竖直方向的转动，从而准确地控制清洁装置到达隧道的灯具位置处，并且，在车辆的行进过程中，上述的角度会一直处于动态调整中，虽然，上述装置能够准确第将清洁装置送至灯具位置处，由于隧道中的灯具表面的位姿不会完全统一，也就是说，隧道灯具有可能会倾斜设置，比如当隧道中以两侧的方式安装灯具，那么，通过第摄像机以及距离传感器感知清洁装置与灯具的进一步的位置关系，在清洁装置中设置有电驱动的转动机构，有控制器控制该转动机构实时微调，清洁装置的转动的驱动机构可以采用球铰接装置进行铰接，然后采用电动气缸或者电动机驱动球铰链转动，控制器根据距离传感器以及图像装置对清洁装置的转动控制采用现有的算法即可，在不加以赘述，从而使得清洁装置的清洁部位与灯具的表面准确贴合，其中，清洁装置可以采用现有的清洁装置，比如电动刷的方式。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (9)

1.一种用于隧道灯清洁的隧道灯具定位方法，包括清洁车、设置于清洁车的机械臂以及设置于机械臂执行末端的清洁装置，其特征在于：所述隧道灯具定位方法包括如下步骤：

S1.以设置在清洁车的双目摄像机采集隧道灯具的第一图像信息，并以双目摄像机所在位置为坐标原点建立三维直角坐标系，根据第一图像信息获取隧道灯具在三维坐标系中的坐标位置；

S2.采集机械臂的各关节点的实际角度，并根据各机械臂的实际角度确定出机械臂的执行末端在三维坐标系中的位置参数；

S3.根据隧道灯具在三维坐标系中的位置以及机械臂的执行末端的位置参数，根据该位置参数确定出机械臂的各关节点的旋转角度，根据旋转角度控制机械臂关节点的旋转将清洁装置与隧道灯具对准。

2.根据权利要求1所述用于隧道灯清洁的隧道灯具定位方法，其特征在于：步骤S2中，根据如下数学模型确定机械臂执行末端在三维坐标系的位置参数：

Xe＝m

Ye＝n

Ze＝α

β_e＝θ₂+θ₃+θ₄；

其中，Xe、Ye以及Ze分别为机械臂的执行末端在三维坐标系的X、Y和Z方向的坐标值，β_e为机械臂执行末端的姿态角，m、n和α分别为中间变量，其中：

m＝c₁(L₄c₂₃₄+L₃c₂₃+L₂c₂+L1)；

n＝s₁(L₄s₂₃₄+L₃s₂₃+L₂c₂+L1)

α＝L₄s₂₃₄+L₃s₂₃+L₂s₂；其中，c₂₃₄＝cos(α₂+θ₃+θ₄)，c₂₃＝cos(θ₂+θ₃)，c₂＝cosθ₂，s₁＝sinθ₁，s₂₃₄＝sin(α₂+θ₃+θ₄)，s₂₃＝sin(θ₂+θ₃)，c₁＝cosθ₁，s₂＝sinθ₂；其中，θ₁为机械臂的第一关节轴三维坐标系的XY平面的实际角度，θ₂为第二关节轴与第一关节轴之间的实际角度，θ₃为第二关节轴与第三关节轴之间的实际角度，θ₄为第四关节轴与第三关节轴之间的实际角度；α₂＝0。

3.根据权利要求2所述用于隧道灯清洁的隧道灯具定位方法，其特征在于：步骤S3中，根据如下方法确定第一关节轴在三维坐标系的XOY平面上的转动角度θ′₁：

将隧道灯具以及第一关节轴垂直投影到三维直角坐标系的XOY平面上，并以第一关节点为原点，第一关节轴的投影为横坐标建立直角坐标系(x'o'y')，根据隧道灯具在直角坐标系(x'o'y')的坐标计算第一关节点的转动角度

4.根据权利要求2所述用于隧道灯清洁的隧道灯具定位方法，其特征在于：步骤S3中，根据如下方法确定出第二关节轴的旋转角度θ′₂：

5.根据权利要求4所述用于隧道灯清洁的隧道灯具定位方法，其特征在于：

步骤S3中，根据如下方法确定出第三关节轴的旋转角度θ′₃：

θ′₃＝arctan 2(n-L₃s₂,m-L₃c₂)-θ₂。

6.根据权利要求5所述用于隧道灯清洁的隧道灯具定位方法，其特征在于：

步骤S3中，根据如下方法确定出第四关节轴的旋转角度θ′₄：

θ′₄＝β_e-(θ′₂+θ′₃)。

7.一种用于隧道灯清洁的隧道灯具定位系统，其特征在于：包括双目摄像机、摄像机、距离传感器以及控制器；

所述双目摄像机，设置于清洁车上，用于采集隧道灯具的第一图像信息并输出至控制器中；

所述摄像机，设置于清洁装置处，用于隧道灯具的第二图像信息；

角度传感器，设置于机械臂的关节点处，用于检测机械臂的各关节轴的关节点处，用于检测各关节轴的实际角度；

距离传感器，设置于清洁装置处，用于采集清洁装置与隧道灯具的实际距离并距离信息传输至控制器中；

所述控制器，用于接收双目摄像机、摄像机以及距离传感器输出的信息，并以双目摄像机所在位置为坐标原点为坐标系，通过第一图像信息确定灯具的位置信息，根据角度传感器输出的角度信息确定出机械臂的执行末端的位置参数，然后根据机械臂执行末端的位置参数确定出机械臂各关节点转动角度并控制机械臂各关节点转动使清洁装置到达隧道灯具位置。

8.根据权利要求7所述用于隧道灯清洁的隧道灯具定位系统，其特征在于：所述控制器根据如下方法确定出机械臂执行末端的位置参数：

Xe＝m

Ye＝n

Ze＝α

β_e＝θ₂+θ₃+θ₄；

其中，Xe、Ye以及Ze分别为机械臂的执行末端在三维坐标系的X、Y和Z方向的坐标值，β_e为机械臂执行末端的姿态角，m、n和α分别为中间变量，其中：

m＝c₁(L₄c₂₃₄+L₃c₂₃+L₂c₂+L1)；

n＝s₁(L₄s₂₃₄+L₃s₂₃+L₂c₂+L1)

α＝L₄s₂₃₄+L₃s₂₃+L₂s₂；其中，c₂₃₄＝cos(α₂+θ₃+θ₄)，c₂₃＝cos(θ₂+θ₃)，c₂＝cosθ₂，s₁＝sinθ₁，s₂₃₄＝sin(α₂+θ₃+θ₄)，s₂₃＝sin(θ₂+θ₃)，c₁＝cosθ₁，s₂＝sinθ₂；其中，θ₁为机械臂的第一关节轴三维坐标系的XY平面的实际角度，θ₂为第二关节轴与第一关节轴之间的实际角度，θ₃为第二关节轴与第三关节轴之间的实际角度，θ₄为第四关节轴与第三关节轴之间的实际角度；α₂＝0。

9.根据权利要求8所述用于隧道灯清洁的隧道灯具定位系统，其特征在于：控制器根据如下方法控制各机械臂关节点的转动角度：

第一关节轴在三维坐标系的XOY平面上的转动角度θ₁'：

将隧道灯具以及第一关节轴垂直投影到三维直角坐标系的XOY平面上，并以第一关节点为原点，第一关节轴的投影为横坐标建立直角坐标系(x'o'y')，根据隧道灯具在直角坐标系(x'o'y')的坐标计算第一关节点的转动角度

第二关节轴的旋转角度θ′₂：

第三关节轴的旋转角度θ′₃：

θ′₃＝arctan 2(n-L₃s₂,m-L₃c₂)-θ₂；

第四关节轴的旋转角度θ′₄：

θ′₄＝β_e-(θ′₂+θ′₃)。