CN106625629B - 隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制装置及方法 - Google Patents

Abstract

一种隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制装置及方法，该方法是作业设备的操作者通过人机交互单元，依据各类设备及其作业的实际情况，经控制单元将操作者的操控指令与传感器采集的信号相结合进行运算处理以输出控制指令，对三种自动控制功能（末端臂架朝向自动控制、末端臂架水平位置自动控制以及末端臂架尾端点与臂组支撑结构（或车身）垂直距离自动控制）进行自由组合，实现末端臂架结构的姿态多模式自动控制，极大程度上满足了隧道作业设备对臂架结构的智能操控要求，大大降低设备的操控难度，实现智能化自动控制。

Description

隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控 制装置及方法

技术领域

本发明涉及一种末端臂架姿态控制装置及方法，具体涉及一种对隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制的装置及方法。

背景技术

现有工业用智能机器手臂实现臂架的控制，主要分为两种控制方式：第一种，按照预定轨迹进行运动控制；第二种，通过图像识别、位置传感器等设备采集信号，由专用计算机软件进行运算处理，控制其运动。显然，作为隧道作业设备，其作业工况多变复杂，无法预先设定器运动轨迹，且现场实际工况条件恶劣，高精度的传感设备无法应用，也不适合采用专用计算机进行现场运算处理。

现有隧道作业设备对臂架控制，一般采用在臂架上加装倾角传感器的方式，实时测量相应臂架与水平面的夹角，以此作为臂架姿态控制的依据，基本实现了臂架末端与水平面保持固定姿态的功能。但是，隧道作业设备受其作业空间的限制，往往要求作业设备具有很好的灵活性，为此多关节、可多方向运动的臂架的结构运用得越来越广泛，在带来灵活性的同时，也造成了臂架的运动控制越来越复杂，尤其是末端姿态的控制，往往需要操作手反复调整才能接近目标位置。为此，开发一种针对多臂架、多运动方向作业设备的末端臂架姿态控制装置及方法意义重大。

发明内容

本发明所需要解决的技术问题是：针对上述现有技术的不足，提供一种隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制装置及方法。

为解决上述技术问题，本发明所采用的技术方案是：一种隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制装置，其特点是，该控制装置包括传感单元、控制单元、人机交互单元及执行单元，其中：

该传感单元，包括倾角传感器、回转角度传感器及位移传感器，该倾角传感器设于作业设备臂架组的各臂架上以测量各臂架与水平面的夹角值，该回转角度传感器设于该臂架组中各臂架的回转机构上以测量各回转机构的回转角度值，该位移传感器设于该臂架组中各臂架的伸缩机构上以测量各伸缩机构的运动位移值；该传感单元与该控制单元连接，以通过各传感器实时采集传感信号并传送至控制单元；

该人机交互单元，与该控制单元连接，用于接收操作者的操控指令并发送至控制单元；同时接受并显示控制单元发出的控制指令信息，将该臂架组的当前运行状态实时展示给操作者；其中，该操作者的操控指令包括末端臂架水平方向调节指令、末端臂架朝向调节指令以及末端臂架尾端点与支撑装置铰接点之间的垂直距离调节指令；

该控制单元，其包括工业控制器，该工业控制器用于接收各传感信号，同时接收人机交互单元传送的操控指令，并结合传感信号进行运算处理后输出控制指令，并将控制指令发送到执行机构单元，同时将控制指令的相关信息反馈至人机交互系统进行显示；

该执行机构单元，与该控制单元连接；该执行机构单元包括控制阀组，用于接收控制单元发出的控制指令，以使控制阀组执行该控制指令使该臂架组的基本臂架和末端臂架动作，实现对该臂架组末端臂架的姿态控制。

上述传感单元采集的信号包括所述臂架组的各臂架与水平面的当前夹角值；该臂架组中各回转机构的当前回转角度值；该臂架组中各伸缩机构的当前运动位移值。

本发明同时提供一种隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制方法，该方法步骤如下：

A.于该作业设备一臂架组的伸缩机构、回转机构、以及各臂架上设置对应的位移传感器、回转角度传感器、和倾角传感器；

B.人机交互单元获取操作者的操控指令并发送至控制单元，该操控指令包括末端臂架水平方向调节指令、末端臂架朝向调节指令以及末端臂架尾端点与支撑装置铰接点之间的垂直距离调节指令；同时传感单元中各回转角度传感器、倾角传感器及位移传感器采集当前传感信号并发送至控制单元；

C.控制单元接收操作者的操控指令和当前的各传感信号，经运算处理后输出相应的控制指令至执行机构单元使臂架组基本臂架和末端臂架动作，实现对末端臂架姿态的自动控制；

上述提及的控制指令的运算处理过程如下：

末端臂架水平方向自动调节控制指令：通过人机交互单元设定末端臂架与水平面的夹角值θe和操作者发出的末端臂架水平方向调节控制指令中的末端臂架实际水平调节角度即目标夹角值θm的最大允许差值为Δθ；若倾角传感器采集的末端臂架与水平面的当前夹角值和操作者发出的目标夹角值之间的差值Δθ₀大于Δθ时，则由控制单元计算出Δθ₀与Δθ之间的差值，根据该计算出的差值发出末端臂架水平方向调节控制指令至执行机构单元使末端臂架于垂直方向运动，使倾角传感器采集的末端臂架与水平面的当前夹角值和目标夹角值的差值小于等于Δθ；若倾角传感器采集的末端臂架与水平面的当前夹角值和操作者发出的目标夹角值之间的差值Δθ₀小于等于Δθ，则末端臂架不动作，从而实现末端臂架的水平方向自动调节控制；

末端臂架朝向自动调节控制指令：设定操作者发出的末端臂架朝向调节操控指令中的目标朝向调节角度为Δα，若基本臂架和末端臂架的回转角度传感器采集的该两臂架的当前回转角度之和Δα₀等于Δα，则基本臂架和末端臂架不动作；若基本臂架和末端臂架的回转角度传感器采集的两臂架的当前回转角度之和Δα₀小于或大于Δα，则控制单元计算Δα₀与Δα的差值，再根据计算出的该差值发出末端臂架朝向调节控制指令至执行机构单元使基本臂架和末端臂架中的至少一个臂架进行回转动作，直至基本臂架和末端臂架的回转角度传感器采集的该两臂架的当前回转角度之和等于Δα，从而实现末端臂架朝向的自动调节控制；

末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制指令：设定操作者发出的末端臂架尾端点与支撑装置铰接点之间的垂直距离调节指令中的目标垂直调节距离为L；

当臂架组的基本臂架不动作即不进行垂直或回转运动时，设定人机交互单元显示的当前垂直调节距离为L₀；若L₀小于或大于L，则由控制单元计算出L₀与L的差值，再根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动该计算出的差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若L₀等于L，则末端臂架的伸缩机构不动作，从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制；

当臂架组的基本臂架只进行垂直运动时，则先由控制单元根据基本臂架的倾角传感器采集的垂直运动后基本臂架与水平面的夹角，依据基本臂架的长度，计算出基本臂架垂直运动后的垂直方向值，再计算出该垂直方向值与L的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动该计算出的差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈，从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制；

当臂架组的基本臂架只进行回转方向运行时，则先由控制单元根据基本臂架的回转角度传感器采集的基本臂架回转运行前后的回转角度值，依据基本臂架的长度，计算出基本臂架回转运动后于水平方向的垂直变化值L_基，若末端臂架不进行回转和垂直运动，则直接根据控制单元计算出的基本臂架回转运动后于水平方向的垂直方向值与L进行比较，计算出差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若末端臂架进行回转而不进行垂直运动，则由控制单元根据末端臂架的回转角度传感器采集的末端臂架回转运动前后的回转角度值，依据末端臂架的长度，计算出末端臂架的回转机构回转带来的末端臂架于水平方向的垂直变化值L_末，将L_基、L_末、L₀和L相结合计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若末端臂架同时进行回转和垂直运动，则先由控制单元根据末端臂架的回转角度传感器采集的末端臂架回转前后的回转角度值以及末端臂架的倾角传感器采集的末端臂架垂直运动前后与水平面的夹角值，依据末端臂架的长度，计算出末端臂架在回转和垂直运动后于水平方向上的垂直变化值，再结合L_基、L₀和L，计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若末端臂架不回转而进行垂直运动，则先由控制单元根据末端臂架的倾角传感器采集的末端臂架垂直运动前后与水平面的夹角值，依据末端臂架的长度，计算出末端臂架在垂直运动后于水平方向上的垂直变化值，再结合L_基、L₀和L，计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制；

当臂架组的基本臂架同时进行回转和垂直方向运行时，则先由控制单元根据基本臂架的回转角度传感器采集的基本臂架回转前后的回转角度值以及基本臂架的倾角传感器采集的基本臂架垂直运动前后与水平面的夹角值，依据基本臂架的长度，计算出基本臂架在回转和垂直运动后于水平方向上的垂直变化值，再同理计算出末端臂架于水平方向上的垂直变化值，将该两垂直变化值与L₀、L相结合计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制。

如此，本发明通过在臂架组回转机构中安装回转角度传感器测量回转角度值，从而实现对末端臂架的朝向进行实时自动控制；通过在臂架上安装倾角传感器，测量臂架与水平面的夹角值，从而实现对末端臂架的水平位置进行实时自动控制；通过臂架上安装倾角传感器、回转角度传感器，可实时计算出操作者在操作臂架时，带来的末端臂架与支撑装置铰接点之间垂直距离的变化值，通过控制伸缩机构并测量其伸缩长度值，从而实现对臂架末端位置的实时自动控制；通过人机交互单元，可依据各类设备及其作业的实际情况，对上述三项自动控制功能进行自由组合，实现末端臂架的多模式姿态控制，极大程度上满足了隧道作业设备对臂架结构的智能操控要求，大大降低设备的操控难度，实现智能化自动控制。

附图说明

图1是本发明控制装置的总体组成框图。

图2是实施例所示的臂架组机构示意图。

图3是回转及垂直方向运动机构于垂直方向变化示意图。

图4是臂架在水平方向的等效长度。

图5是回转及垂直方向运动机构于回转方向变化示意图。

具体实施方式

如图1所示，本发明为一种隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制装置，其是在现有多臂架、多关节作业设备的基础上，通过于臂架组的基本臂架和活动臂架上装设回转角度传感器和倾角传感器以测量回转机构的回转角度值以及各臂架与水平面的夹角值；于各活动臂架上装设位移传感器和回转角度传感器以测量伸缩机构的伸缩长度值和回转机构的回转角度值。在各传感器采集的位移和角度信号的基础上，由控制单元结合人机交互单元传送的操作者的操控指令进行处理运算以输出控制指令，对末端臂架朝向、末端臂架水平位置以及末端臂架与支撑装置铰接点（臂架组的基本臂架与支撑装置于该铰接点铰接）之间的垂直距离进行自动控制。具体地，本控制装置包括传感单元、控制单元、人机交互单元及执行单元，其中：

该传感单元，与该控制单元连接；该传感单元包括倾角传感器、回转角度传感器及位移传感器，该倾角传感器设于一臂架组的基本臂架和各活动臂架上以测量各臂架与水平面的夹角值，该回转角度传感器设于该臂架组中各臂架（本文中的各臂架是指基本臂架和各活动臂架）的回转机构上以测量各回转机构的回转角度值，该位移传感器设于该臂架组中各活动臂架的伸缩机构上以测量各伸缩机构的运动位移值。如此，该传感单元通过各传感器采集的信号能获取以下信息：各臂架与水平面的夹角值、该臂架组中各回转机构的回转角度值、该臂架组中各伸缩机构的运动位移值，同时传感单元将采集的信号传送至控制单元。

该人机交互单元，与该控制单元连接，用于接收操作者的操控指令并发送至控制单元；同时接受并显示控制单元发出的控制指令信息，将臂架组的当前运行状态实时展示给操作者即工况显示。其中，该操控指令包括末端臂架水平方向调节指令、末端臂架朝向调节指令以及末端臂架与支撑装置铰接点之间的垂直距离调节指令。

该控制单元，其包括工业控制器，该工业控制器用于采集传感信号，同时接受当前人机交互单元传送的操控指令，并结合采集的传感信号进行运算处理后输出控制指令（该控制指令包括末端臂架水平方向调节控制指令、末端臂架朝向调节控制指令以及末端臂架与支撑装置铰接点之间的垂直距离调节控制指令），并将控制指令发送到执行机构单元以控制臂架组的运动，同时将控制指令的相关信息反馈至人机交互系统进行显示。

该执行机构单元，与该控制单元连接；该执行机构单元包括控制阀组，用于接收控制单元发出的控制指令，以使控制阀组执行该控制指令实现对臂架组末端臂架的姿态控制。

本发明的隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制方法，其具体步骤如下：

A.于该作业设备的臂架组的各伸缩机构、回转机构以及臂架上设置对应的传感器，具体地，臂架上设置倾角传感器，伸缩机构设置位移传感器，回转机构设置回转角度传感器；

B.人机交互单元获取操作者的操控指令并发送至控制单元，该操控指令包括末端臂架水平方向调节指令、末端臂架朝向调节指令以及末端臂架与支撑装置铰接点之间的垂直距离调节指令；同时传感单元中各回转角度传感器、倾角传感器及位移传感器采集各传感信号并发送至控制单元；

C.控制单元接收操作者的操控指令，并通过传感单元中各传感器实时采集的各传感信号，经运算处理后输出相应的控制指令（该控制指令包括末端臂架水平方向调节控制指令、末端臂架朝向调节控制指令以及末端臂架与支撑装置铰接点之间的垂直距离调节控制指令），实现对末端臂架的姿态自动控制。

上述提及的控制指令的具体运算处理过程如下：

末端臂架水平方向自动调节控制指令：通过人机交互单元设定末端臂架与水平面的夹角值θe，设定操作者发出的末端臂架水平方向调节控制指令中的末端臂架实际水平调节角度即目标夹角值θm，θe与θm的最大允许差值为Δθ；若倾角传感器采集的末端臂架与水平面的当前夹角值和操作者发出的目标夹角值之间的差值Δθ₀大于Δθ时，则由控制单元计算出Δθ₀与Δθ之间的差值，根据该计算出的差值发出末端臂架水平方向调节控制指令至执行机构单元使末端臂架于垂直方向运动，使倾角传感器采集的末端臂架与水平面的当前夹角值和目标夹角值的差值小于等于Δθ；若倾角传感器采集的末端臂架与水平面的当前夹角值和操作者发出的目标夹角值之间的差值Δθ₀小于等于Δθ，则末端臂架不动作，从而实现末端臂架的水平方向自动调节控制。

末端臂架朝向自动调节控制指令：设定操作者发出的末端臂架朝向调节操控指令中的目标朝向调节角度为Δα，若基本臂架和末端臂架的回转角度传感器采集的该两臂架的当前回转角度之和Δα₀等于Δα，则基本臂架和末端臂架不动作；若基本臂架和末端臂架的回转角度传感器采集的两臂架的当前回转角度之和Δα₀小于或大于Δα，则控制单元计算Δα₀与Δα的差值，再根据计算出的该差值发出末端臂架朝向调节控制指令至执行机构单元使基本臂架和末端臂架中的至少一个臂架进行回转动作，直至基本臂架和末端臂架的回转角度传感器采集的该两臂架的当前回转角度之和等于Δα，从而实现末端臂架朝向的自动调节控制。实际操作中，可依习惯设置左转为正、右转为负，或是右转为负、右转为正。

末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制指令：设定操作者发出的末端臂架尾端点与支撑装置铰接点之间的垂直距离调节指令中的目标垂直调节距离为L；

当臂架组的基本臂架不动作即不进行垂直或回转运动时，设定人机交互单元显示的当前垂直调节距离为L₀；若L₀小于或大于L，则由控制单元计算出L₀与L的差值，再根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动该计算出的差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若L₀等于L，则末端臂架的伸缩机构不动作，从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制；

当臂架组的基本臂架只进行垂直运动时，则先由控制单元根据基本臂架的倾角传感器采集的垂直运动后基本臂架与水平面的夹角，依据基本臂架的长度，计算出基本臂架垂直运动后的垂直方向值，再计算出该垂直方向值与L的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动该计算出的差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈，从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制；

当臂架组的基本臂架只进行回转方向运行时，则先由控制单元根据基本臂架的回转角度传感器采集的基本臂架回转运行前后的回转角度值，依据基本臂架的长度，计算出基本臂架回转运动后于水平方向的垂直变化值L_基，若末端臂架不进行回转和垂直运动，则直接根据控制单元计算出的基本臂架回转运动后于水平方向的垂直方向值与L进行比较，计算出差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若末端臂架进行回转而不进行垂直运动，则由控制单元根据末端臂架的回转角度传感器采集的末端臂架回转运动前后的回转角度值，依据末端臂架的长度，计算出末端臂架的回转机构回转带来的末端臂架于水平方向的垂直变化值L_末，将L_基、L_末、L₀和L相结合计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若末端臂架同时进行回转和垂直运动，则先由控制单元根据末端臂架的回转角度传感器采集的末端臂架回转前后的回转角度值以及末端臂架的倾角传感器采集的末端臂架垂直运动前后与水平面的夹角值，依据末端臂架的长度，计算出末端臂架在回转和垂直运动后于水平方向上的垂直变化值，再结合L_基、L₀和L，计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若末端臂架不回转而进行垂直运动，则先由控制单元根据末端臂架的倾角传感器采集的末端臂架垂直运动前后与水平面的夹角值，依据末端臂架的长度，计算出末端臂架在垂直运动后于水平方向上的垂直变化值，再结合L_基、L₀和L，计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制；

当臂架组的基本臂架同时进行回转和垂直方向运行时，则先由控制单元根据基本臂架的回转角度传感器采集的基本臂架回转前后的回转角度值以及基本臂架的倾角传感器采集的基本臂架垂直运动前后与水平面的夹角值，依据基本臂架的长度，计算出基本臂架在回转和垂直运动后于水平方向上的垂直变化值，再同理计算出末端臂架于水平方向上的垂直变化值，将该两垂直变化值与L₀、L相结合计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制。

下面以图2所示的臂架组来详细阐述本发明的实现方法。但实际运用并不局限于该类臂架的组合形式。

图2所示的臂架组包括基本臂架1和活动臂架（即末端臂架）2，该基本臂架1一端设于支撑装置3上并设有回转及垂直方向运动机构4，活动臂架2上设有伸缩机构5和回转机构6，该基本臂架与活动臂架连接处设有垂直方向运动机构7。该回转及垂直方向运动机构4设有回转角度传感器41及倾角传感器42，该伸缩机构5设有位移传感器51，该回转机构6设有回转角度传感器61，该活动臂架2设有倾角传感器21。上述各机构与传感器的功能如下：

回转及垂直方向运动机构4：可使整个臂架组进行回转及仰俯运动；

垂直方向运动机构7：实现活动臂架的仰俯运动；

伸缩机构5：实现活动臂架的前后伸缩运动；

回转机构6：实现活动臂架的回转动作；

末端臂架尾端点8，本发明的最终姿态控制对象；

回转角度传感器41：用于测量回转及垂直方向运动机构4当前的回转角度值；

倾角传感器42：用于测量回转及垂直方向运动机构4当前的仰俯角度值；

位移传感器51：用于测量伸缩机构5的伸缩长度值；

回转角度传感器61：用于测量回转机构6的回转角度值；

倾角传感器21：用于测量末端臂架的仰俯角度值。

l图2中标示的垂直距离L：即末端臂架尾端点8与支撑装置3的铰接点9（基本臂架1与支撑装置3于铰接点9铰接）在其平行水平线上的垂直距离。

本发明的控制实现方法如下：

步骤1：通过人机交互单元获取操作者的协助调节控制请求指令（即前面提及的操控指令），该指令包括末端臂架水平方向调节指令、末端臂架朝向调节指令以及末端臂架尾端点与支撑装置铰接点的垂直距离调节指令。

步骤2：控制单元接收操作者的操控指令，并通过传感单元中各传感器实时采集的各传感信号，经运算处理后输出相应的控制指令，实现末端臂架的姿态自动控制。对末端臂架的姿态自动控制包括以下三项控制，控制单元依据操作者的操控指令，进行组合，实现多模式自动控制。控制过程如下：

控制1. 末端臂架水平方向自动调节：

由传感单元的倾角传感器21实时反馈末端臂架（即活动臂架2）与水平面的夹角θe，操作者通过人机交互单元设定好目标夹角值θm（该目标夹角值即操作者发出的末端臂架水平方向调节控制指令中的水平调节角度），皆发送至控制单元，控制单元运算如下：

设定θm与θe的最大允许差值为Δθ，当操作者操作回转及垂直方向运动机构4垂直方向运动时，θe将发生改变，当-Δθ>θm-θe或Δθ<θm-θe时，控制单元输出对垂直方向运动机构7的运动控制指令至执行机构单元使垂直方向运动机构7运动，使θm-θe始终保持在允许差值范围内，实现末端臂架的水平方向自动调节。

控制2. 末端臂架朝向自动调节：

由传感单元的回转角度传感器41测得回转及垂直方向运动机构4的回转角度值α1，由回转角度传感器61测得回转机构6的回转角度值α2，操作者通过人机交互单元设定末端臂架的朝向角度为Δα（该朝向角度即为操作者发出的臂架末端朝向调节控制指令中的朝向调节角度），此时Δα=α1+α2为一定值。

当操作者操控回转及垂直方向运动机构4进行回转时，α1发生改变，控制单元输出对回转机构6的控制指令至执行机构单元使回转机构6运行，以使α2值发生改变，进而保持Δα值不变，实现末端臂架朝向的自动调节。

控制3. 末端臂架尾端点8与支撑装置铰接点9的垂直距离L的自动调节：

操作者操控回转及垂直方向运动机构4进行回转或垂直方向运动时，通过控制1及控制2的方法能自动调节臂架末端的水平位置及朝向，势必会带来末端臂架尾端点8与支撑装置铰接点9的垂直距离L发生改变。操作者通过人机交互单元设定好末端臂架尾端点与支撑装置铰接点之间的垂直距离L（该垂直距离L即操作者发出的末端臂架尾端点与支撑装置铰接点之间的垂直距离调节指令中的垂直调节距离L）。

当回转及垂直方向运动机构4发生垂直方向的运行时，例如向上运动时，会使L值变小，其计算方法为：见图3所示，当回转及垂直方向运动机构4发生垂直方向运动时，倾角传感器42测得的角度值由θ1变化为θ2，该基本臂架的总长度为L1，则L2=cosθ1*L1- cosθ2*L1，即为末端臂架尾端点与支撑装置铰接点在垂直距离上的变化值。如此，便可通过控制单元经运算处理后输出对调节伸缩机构5的控制指令至执行机构单元使伸缩机构5运动，并通过位移传感器51进行伸缩长度的反馈，可保持末端臂架尾端点与支撑装置铰接点之间的垂直距离L不变。

当回转及垂直方向运动机构4发生回转方向运行，致使末端臂架尾端点与支撑装置铰接点之间的垂直距离L减小时，其计算方法如下：

见图4所示，α1为回转及垂直方向运动机构4与水平面的当前夹角，即回转角度传感器41的测量值，该基本臂架的总长度为L1，当回转及垂直方向运动机构4发生回转方向运行时，该臂在水平方向的等效长度L3=L1*cosα1。

当回转及垂直方向运动机构4发生回转方向运动时，见图5所示，回转角度传感器41测得的角度值由α1变化为α2，则L4=cosα1*L1- cosα2*L1，为回转及垂直方向运动机构4回转带来的末端臂架尾端点与支撑装置铰接点在垂直距离上的变化值。同时，当回转及垂直方向运动机构4发生回转方向运动带来回转机构6发生反方向的调整运动时，按上述同样方法可计算出回转机构6回转带来的末端臂架尾端点与支撑装置铰接点在垂直距离上的变化值。两个变化值变化趋势相反，结合处理即为末端臂架尾端点与支撑装置铰接点在垂直距离上的实际变化值，通过调节伸缩机构5运动，并通过位移传感器51进行伸缩长度的反馈，能保持末端臂架尾端点与支撑装置铰接点的垂直距离L不变。

Claims (3)

1.一种隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制装置，其特征在于，该控制装置包括传感单元、控制单元、人机交互单元及执行单元，其中：

该传感单元，包括倾角传感器、回转角度传感器及位移传感器，该倾角传感器设于作业设备臂架组的各臂架上以测量各臂架与水平面的夹角值，该回转角度传感器设于该臂架组中各臂架的回转机构上以测量各回转机构的回转角度值，该位移传感器设于该臂架组中各臂架的伸缩机构上以测量各伸缩机构的运动位移值；该传感单元与该控制单元连接，以通过各传感器实时采集传感信号并传送至控制单元；

该人机交互单元，与该控制单元连接，用于接收操作者的操控指令并发送至控制单元；同时接受并显示控制单元发出的控制指令信息，将该臂架组的当前运行状态实时展示给操作者；其中，该操作者的操控指令包括末端臂架水平方向调节指令、末端臂架朝向调节指令以及末端臂架尾端点与支撑装置铰接点之间的垂直距离调节指令；

该控制单元，其包括工业控制器，该工业控制器用于接收各传感信号，同时接收人机交互单元传送的操控指令，并结合传感信号进行运算处理后输出控制指令，并将控制指令发送到执行机构单元，同时将控制指令的相关信息反馈至人机交互系统进行显示；

该执行机构单元，与该控制单元连接；该执行机构单元包括控制阀组，用于接收控制单元发出的控制指令，以使控制阀组执行该控制指令使该臂架组的基本臂架和末端臂架动作，实现对该臂架组末端臂架的姿态控制。

2.如权利要求1所述的隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制装置，其特征在于，所述传感单元采集的信号包括所述臂架组的各臂架与水平面的当前夹角值；该臂架组中各回转机构的当前回转角度值；该臂架组中各伸缩机构的当前运动位移值。

3.一种隧道多臂架、多关节作业设备的末端臂架姿态多模式自动控制方法，其特征在于，该方法步骤如下：

A.于该作业设备一臂架组的伸缩机构、回转机构、以及各臂架上设置对应的位移传感器、回转角度传感器、和倾角传感器；

B.人机交互单元获取操作者的操控指令并发送至控制单元，该操控指令包括末端臂架水平方向调节指令、末端臂架朝向调节指令以及末端臂架尾端点与支撑装置铰接点之间的垂直距离调节指令；同时传感单元中各回转角度传感器、倾角传感器及位移传感器采集当前传感信号并发送至控制单元；

C.控制单元接收操作者的操控指令和当前的各传感信号，经运算处理后输出相应的控制指令至执行机构单元使臂架组基本臂架和末端臂架动作，实现对末端臂架姿态的自动控制；

上述提及的控制指令的运算处理过程如下：

末端臂架水平方向自动调节控制指令：通过人机交互单元设定末端臂架与水平面的夹角值θe和操作者发出的末端臂架水平方向调节控制指令中的末端臂架实际水平调节角度即目标夹角值θm的最大允许差值为Δθ；若倾角传感器采集的末端臂架与水平面的当前夹角值和操作者发出的目标夹角值之间的差值Δθ₀大于Δθ时，则由控制单元计算出Δθ₀与Δθ之间的差值，根据该计算出的差值发出末端臂架水平方向调节控制指令至执行机构单元使末端臂架于垂直方向运动，使倾角传感器采集的末端臂架与水平面的当前夹角值和目标夹角值的差值小于等于Δθ；若倾角传感器采集的末端臂架与水平面的当前夹角值和操作者发出的目标夹角值之间的差值Δθ₀小于等于Δθ，则末端臂架不动作，从而实现末端臂架的水平方向自动调节控制；

末端臂架朝向自动调节控制指令：设定操作者发出的末端臂架朝向调节操控指令中的目标朝向调节角度为Δα，若基本臂架和末端臂架的回转角度传感器采集的该两臂架的当前回转角度之和Δα₀等于Δα，则基本臂架和末端臂架不动作；若基本臂架和末端臂架的回转角度传感器采集的两臂架的当前回转角度之和Δα₀小于或大于Δα，则控制单元计算Δα₀与Δα的差值，再根据计算出的该差值发出末端臂架朝向调节控制指令至执行机构单元使基本臂架和末端臂架中的至少一个臂架进行回转动作，直至基本臂架和末端臂架的回转角度传感器采集的该两臂架的当前回转角度之和等于Δα，从而实现末端臂架朝向的自动调节控制；

末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制指令：设定操作者发出的末端臂架尾端点与支撑装置铰接点之间的垂直距离调节指令中的目标垂直调节距离为L；

当臂架组的基本臂架不动作即不进行垂直或回转运动时，设定人机交互单元显示的当前垂直调节距离为L₀；若L₀小于或大于L，则由控制单元计算出L₀与L的差值，再根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动该计算出的差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若L₀等于L，则末端臂架的伸缩机构不动作，从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制；

当臂架组的基本臂架只进行垂直运动时，则先由控制单元根据基本臂架的倾角传感器采集的垂直运动后基本臂架与水平面的夹角，依据基本臂架的长度，计算出基本臂架垂直运动后的垂直方向值，再计算出该垂直方向值与L的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动该计算出的差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈，从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制；

当臂架组的基本臂架只进行回转方向运行时，则先由控制单元根据基本臂架的回转角度传感器采集的基本臂架回转运行前后的回转角度值，依据基本臂架的长度，计算出基本臂架回转运动后于水平方向的垂直变化值L_基，若末端臂架不进行回转和垂直运动，则直接根据控制单元计算出的基本臂架回转运动后于水平方向的垂直方向值与L进行比较，计算出差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若末端臂架进行回转而不进行垂直运动，则由控制单元根据末端臂架的回转角度传感器采集的末端臂架回转运动前后的回转角度值，依据末端臂架的长度，计算出末端臂架的回转机构回转带来的末端臂架于水平方向的垂直变化值L_末，将L_基、L_末、L₀和L相结合计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若末端臂架同时进行回转和垂直运动，则先由控制单元根据末端臂架的回转角度传感器采集的末端臂架回转前后的回转角度值以及末端臂架的倾角传感器采集的末端臂架垂直运动前后与水平面的夹角值，依据末端臂架的长度，计算出末端臂架在回转和垂直运动后于水平方向上的垂直变化值，再结合L_基、L₀和L，计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；若末端臂架不回转而进行垂直运动，则先由控制单元根据末端臂架的倾角传感器采集的末端臂架垂直运动前后与水平面的夹角值，依据末端臂架的长度，计算出末端臂架在垂直运动后于水平方向上的垂直变化值，再结合L_基、L₀和L，计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制；

当臂架组的基本臂架同时进行回转和垂直方向运行时，则先由控制单元根据基本臂架的回转角度传感器采集的基本臂架回转前后的回转角度值以及基本臂架的倾角传感器采集的基本臂架垂直运动前后与水平面的夹角值，依据基本臂架的长度，计算出基本臂架在回转和垂直运动后于水平方向上的垂直变化值，再同理计算出末端臂架于水平方向上的垂直变化值，将该两垂直变化值与L₀、L相结合计算出当前末端臂架尾端点与L之间的差值，然后根据该差值，发出垂直距离自动调节控制指令使末端臂架的伸缩机构运动计算出的该差值距离，并通过该伸缩机构的位移传感器进行伸缩长度的反馈；从而实现末端臂架尾端点与支撑装置铰接点之间的垂直距离的自动调节控制。