用于控制折叠臂式臂架的方法、处理器、装置及工程设备

摘要

本发明涉及工程设备技术领域，公开了一种用于控制折叠臂式臂架的方法、处理器、装置及工程设备。方法包括：获取折叠臂式臂架的关节的当前转动角度，确定折叠臂式臂架的末端的当前位置；根据当前位置和输入的运动指令确定末端的期望位置；确定折叠臂式臂架的关节中的平台调平转动关节，根据期望位置和平台调平转动关节对应的当前转动角度确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度，根据平台调平转动关节对应的当前转动角度和其他关节对应的期望转动角度生成用于调节折叠臂式臂架位姿的控制指令。本发明简化了运动学求解步骤，有效地降低了确定臂架位姿控制指令时的计算难度。

权利要求

1.一种用于控制折叠臂式臂架的方法，其特征在于，所述折叠臂式臂架包括多个节臂以及连接所述多个节臂的关节，所述方法包括：获取所述关节的当前转动角度；

根据所述当前转动角度确定所述折叠臂式臂架的末端的当前位置；

根据所述当前位置和输入的运动指令确定所述折叠臂式臂架的末端的期望位置；

确定所述折叠臂式臂架的关节中用于对所述折叠臂式臂架的末端进行调节以使所述末端保持水平状态的平台调平转动关节；

根据所述期望位置和所述平台调平转动关节对应的当前转动角度确定所述折叠臂式臂架的除所述平台调平转动关节以外的其他关节对应的期望转动角度；以及根据所述平台调平转动关节对应的当前转动角度和所述其他关节对应的期望转动角度生成用于调节所述折叠臂式臂架位姿的控制指令；

所述获取所述关节的当前转动角度，包括：

响应于监测到用于控制所述折叠臂式臂架的末端移动的控制信号，获取当前时刻的所述关节的当前转动角度。

2.根据权利要求1所述的方法，其特征在于，所述根据所述期望位置和所述平台调平转动关节对应的当前转动角度确定所述折叠臂式臂架的除所述平台调平转动关节以外的其他关节对应的期望转动角度，包括：将所述期望位置和所述平台调平转动关节对应的当前转动角度输入至基于Paden‑Kahan子问题建立的逆向运动学模型，以确定所述折叠臂式臂架的除所述平台调平转动关节以外的其他关节对应的期望转动角度。

3.根据权利要求2所述的方法，其特征在于，所述将所述期望位置和所述平台调平转动关节对应的当前转动角度输入至基于Paden‑Kahan子问题建立的逆向运动学模型，以确定所述折叠臂式臂架的除所述平台调平转动关节以外的其他关节对应的期望转动角度，包括：基于所述期望位置和所述平台调平转动关节对应的当前转动角度判断所述逆向运动学模型是否存在实数解；

如果确定所述逆向运动学模型存在实数解，则判断所述实数解是否超出所述臂架的除所述平台调平转动关节以外的其他关节对应的第一预设运动范围；

如果所述实数解未超出所述第一预设运动范围，则基于所述实数解确定所述其他关节对应的期望转动角度。

4.根据权利要求3所述的方法，其特征在于，所述将所述期望位置和所述平台调平转动关节对应的当前转动角度输入至基于Paden‑Kahan子问题建立的逆向运动学模型，以确定所述折叠臂式臂架的除所述平台调平转动关节以外的其他关节对应的期望转动角度，包括：如果确定所述逆向运动学模型不存在实数解，或存在超出所述第一预设运动范围的实数解，则等待预设间隔时长，以在等待结束后重新获取所述平台调平转动关节对应的当前转动角度，直至得到未超出所述第一预设运动范围的实数解。

5.根据权利要求2所述的方法，其特征在于，所述基于Paden‑Kahan子问题建立的逆向运动学模型是通过以下步骤建立的：确定所述折叠臂式臂架的关节中用于对折叠臂式臂架整体进行调节以使所述折叠臂式臂架整体在水平面内转动的转台转动关节；

基于所述折叠臂式臂架的各个节臂以及关节之间的空间几何关系建立所述转台转动关节对应的期望转动角度的第一求解模型；

获取所述折叠臂式臂架的初始位姿信息，所述初始位姿信息包括所述关节的初始位置和所述折叠臂式臂架的末端的初始位置；

根据所述初始位姿信息、所述期望位置、所述平台调平转动关节对应的当前转动角度以及所述第一求解模型，基于Paden‑Kahan子问题建立所述折叠臂式臂架的除所述平台调平转动关节和所述转台转动关节以外的剩余关节对应的期望转动角度的第二求解模型；

根据所述第一求解模型和所述第二求解模型建立所述逆向运动学模型。

6.根据权利要求1所述的方法，其特征在于，所述根据所述当前转动角度确定所述折叠臂式臂架的末端的当前位置，包括：将所述当前转动角度输入至正向运动学模型，以确定所述折叠臂式臂架的末端的当前位置；

其中，所述正向运动学模型是基于旋量法建立的。

7.根据权利要求1至6中任意一项所述的方法，其特征在于，所述多个节臂包括塔臂、主臂以及工作平台，所述关节包括转台转动关节、塔臂转动关节、主臂转动关节以及平台调平转动关节，所述转台转动关节通过所述塔臂转动关节与所述塔臂的一端连接，所述塔臂的另一端通过所述主臂转动关节与所述主臂的一端连接，所述主臂的另一端通过所述平台调平转动关节与所述工作平台连接，所述当前转动角度包括所述转台转动关节的第一当前转动角度、所述塔臂转动关节的第二当前转动角度、所述主臂转动关节的第三当前转动角度以及所述平台调平转动关节的第四当前转动角度。

8.一种处理器，其特征在于，被配置成执行根据权利要求1至7中任意一项所述的用于折叠臂式臂架运动的方法。

9.一种用于控制折叠臂式臂架的装置，其特征在于，包括：

液压驱动系统，用于驱动折叠臂式臂架运动；

传感器，用于检测所述折叠臂式臂架的关节转动角度；

根据权利要求8所述的处理器；以及

液压伺服控制器，被配置成：

响应于接收到的控制信号，根据所述传感器检测的关节转动角度生成关节转动角度信息，并将生成的关节转动角度信息输送至所述处理器；

根据从所述处理器接收的用于调节所述折叠臂式臂架位姿的控制指令控制所述液压驱动系统驱动所述折叠臂式臂架运动。

10.根据权利要求9所述的装置，其特征在于，还包括：

遥控器，用于响应于用户操作发送所述控制信号。

11.一种工程设备，其特征在于，包括：

折叠臂式臂架，所述折叠臂式臂架包括多个节臂以及连接所述多个节臂的关节；

根据权利要求9或10所述的用于控制折叠臂式臂架的装置。

12.根据权利要求11所述的工程设备，其特征在于，所述多个节臂包括塔臂、主臂以及工作平台，所述关节包括转台转动关节、塔臂转动关节、主臂转动关节以及平台调平转动关节，所述转台转动关节通过所述塔臂转动关节与所述塔臂的一端连接，所述塔臂的另一端通过所述主臂转动关节与所述主臂的一端连接，所述主臂的另一端通过所述平台调平转动关节与所述工作平台连接。

13.一种机器可读存储介质，所述机器可读存储介质上存储有指令，其特征在于，所述指令在被处理器执行时使得所述处理器执行根据权利要求1至7中任意一项所述的用于控制折叠臂式臂架的方法。

说明书

用于控制折叠臂式臂架的方法、处理器、装置及工程设备

技术领域

[0001] 本发明涉及工程设备技术领域，具体地涉及一种用于控制折叠臂式臂架的方法、处理器、装置及工程设备。

背景技术

[0002] 工程设备的折叠臂式臂架通常为串联结构，其包括多个节臂，节臂间通过可以控制节臂运动的关节连接。随着折叠臂式臂架的工程设备的应用场合日益广泛，同时也将面临作业环境复杂、作业难度高等问题。

[0003] 包括折叠臂式臂架的工程设备的结构中，其运动副主要由转动关节组成，在控制该工程设备的末端平台进行运动时，通常需要了解该工程设备具体结构的操作手根据经验对末端平台移动位置与角度进行估算，然后通过控制手柄来单独移动各个臂节的液压驱动油缸，使末端平台达到预定的目标位置。这不仅对操作人员对折叠臂式臂架工程设备的熟悉程度提出要求，且增加了操作手的劳动强度，也为操作安全性带来隐患，为提高工程设备的作业效率，对其平台位置进行直线运动控制有着重要意义。

发明内容

[0004] 针对现有技术中的上述不足，本发明实施例的目的是提供一种用于控制折叠臂式臂架的方法、处理器、装置及工程设备。

[0005] 为了实现上述目的，本发明第一方面提供一种用于控制折叠臂式臂架的方法，折叠臂式臂架包括多个节臂以及连接多个节臂的关节，包括：

[0006] 获取关节的当前转动角度；

[0007] 根据当前转动角度确定折叠臂式臂架的末端的当前位置；

[0008] 根据当前位置和输入的运动指令确定折叠臂式臂架的末端的期望位置；

[0009] 确定折叠臂式臂架的关节中用于对折叠臂式臂架的末端进行调节以使末端保持水平状态的平台调平转动关节；

[0010] 根据期望位置和平台调平转动关节对应的当前转动角度确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度；以及

[0011] 根据平台调平转动关节对应的当前转动角度和其他关节对应的期望转动角度生成用于调节折叠臂式臂架位姿的控制指令。

[0012] 在本发明实施例中，根据期望位置和平台调平转动关节对应的当前转动角度确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度，包括：

[0013] 将期望位置和平台调平转动关节对应的当前转动角度输入至基于Paden‑Kahan子问题建立的逆向运动学模型，以确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度。

[0014] 在本发明实施例中，将期望位置和平台调平转动关节对应的当前转动角度输入至基于Paden‑Kahan子问题建立的逆向运动学模型，以确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度，包括：

[0015] 基于期望位置和平台调平转动关节对应的当前转动角度判断逆向运动学模型是否存在实数解；

[0016] 如果确定逆向运动学模型存在实数解，则判断实数解是否超出臂架的除主动调平关节以外的其他关节对应的第一预设运动范围；

[0017] 如果实数解未超出第一预设运动范围，则基于实数解确定其他关节对应的期望转动角度。

[0018] 在本发明实施例中，将期望位置和平台调平转动关节对应的当前转动角度输入至基于Paden‑Kahan子问题建立的逆向运动学模型，以确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度，包括：

[0019] 如果确定逆向运动学模型不存在实数解，或存在超出第一预设运动范围的实数解，则等待预设间隔时长，以在等待结束后重新获取平台调平转动关节对应的当前转动角度，直至得到未超出第一预设运动范围的实数解。

[0020] 在本发明实施例中，基于Paden‑Kahan子问题建立的逆向运动学模型是通过以下步骤建立的：

[0021] 确定折叠臂式臂架的关节中用于对折叠臂式臂架整体进行调节以使折叠臂式臂架整体在水平面内转动的转台转动关节；

[0022] 基于折叠式臂架的各个节臂以及关节之间的空间几何关系建立转台转动关节对应的期望转动角度的第一求解模型；

[0023] 获取折叠臂式臂架的初始位姿信息，初始位姿信息包括关节的初始位置和折叠臂式臂架的末端的初始位置；

[0024] 根据初始位姿信息、期望位置、平台调平转动关节对应的当前转动角度以及第一求解模型，基于Paden‑Kahan子问题建立折叠臂式臂架的除平台调平转动关节和转台转动关节以外的剩余关节对应的期望转动角度的第二求解模型；

[0025] 根据第一求解模型和第二求解模型建立逆向运动学模型。

[0026] 在本发明实施例中，获取关节的当前转动角度，包括：

[0027] 响应于监测到用于控制折叠臂式臂架的末端移动的控制信号，获取当前时刻的关节的当前转动角度。

[0028] 在本发明实施例中，根据当前转动角度确定折叠臂式臂架的末端的当前位置，包括：

[0029] 将当前转动角度输入至正向运动学模型，以确定折叠臂式臂架的末端的当前位置；

[0030] 其中，正向运动学模型是基于旋量法建立的。

[0031] 在本发明实施例中，多个节臂包括塔臂、主臂以及工作平台，关节包括转台转动关节、塔臂转动关节、主臂转动关节以及平台调平转动关节，

[0032] 转台转动关节通过塔臂转动关节与塔臂的一端连接，塔臂的另一端通过主臂转动关节与主臂的一端连接，主臂的另一端通过平台调平转动关节与工作平台连接，[0033] 当前转动角度包括转台转动关节的第一当前转动角度、塔臂转动关节的第二当前转动角度、主臂转动关节的第三当前转动角度以及平台调平转动关节的第四当前转动角度。

[0034] 本发明第二方面提供一种处理器，被配置成执行时实现如上的用于控制折叠臂式臂架的方法的步骤。

[0035] 本发明第三方面提供一种用于控制折叠臂式臂架的装置，包括：

[0036] 液压驱动系统，用于驱动折叠臂式臂架运动；

[0037] 传感器，用于检测折叠臂式臂架的关节转动角度；

[0038] 如上实施例所述的处理器；以及

[0039] 液压伺服控制器，被配置成：

[0040] 响应于接收到的控制信号，根据传感器检测的关节转动角度生成关节转动角度信息，并将生成的关节转动角度信息输送至处理器；

[0041] 根据从处理器接收的用于调节折叠臂式臂架位姿的控制指令控制液压驱动系统驱动折叠臂式臂架运动。

[0042] 在本发明实施例中，还包括：

[0043] 遥控器，用于响应于用户操作发送控制信号。

[0044] 本发明第四方面提供一种工程设备，包括：

[0045] 折叠臂式臂架，折叠臂式臂架包括多个节臂以及连接多个节臂的关节；

[0046] 如上实施例所述的用于控制折叠臂式臂架的装置。

[0047] 在本发明实施例中，多个节臂包括塔臂、主臂以及工作平台，关节包括转台转动关节、塔臂转动关节、主臂转动关节以及平台调平转动关节，转台转动关节通过塔臂转动关节与塔臂的一端连接，塔臂的另一端通过主臂转动关节与主臂的一端连接，主臂的另一端通过平台调平转动关节与工作平台连接。

[0048] 本发明第五方面提供一种机器可读存储介质，机器可读存储介质上存储有指令，指令在被处理器执行时使得处理器执行如上实施方式中的用于控制折叠臂式臂架的方法。

[0049] 通过上述技术方案，在确定折叠臂式臂架计算臂架控制指令时，通过划分出平台调平转动关节有效的降低了在确定用于调节折叠臂式臂架位姿的控制指令时的计算难度，并极大的加快了计算的速度，有效提升在复杂高空作业场景中的适应性和灵活性，并且，通过用于调节折叠臂式臂架位姿的控制指令，可以实现臂式高空作业平台末端位置的自动控制，无需操作人员预先熟悉工程设备臂架的结构形式，降低了高空作业平台的使用门槛，使臂式高空作业平台的操作更加简单直观、可控性更好，降低劳动强度。

[0050] 本发明的其它特征和优点将在随后的具体实施方式部分予以详细说明。

附图说明

[0051] 附图是用来提供对本发明的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明，但并不构成对本发明的限制。在附图中：

[0052] 图1为根据本发明一实施例的用于控制折叠臂式臂架的方法的流程示意图；

[0053] 图2为根据本发明一实施例的用于控制折叠臂式臂架的装置的示意框图；

[0054] 图3为根据本发明一实施例的逆向运动学模型求解的流程示意图；

[0055] 图4为根据本发明一实施例的工程设备的臂架的示意图；

[0056] 图5为根据本发明一实施例的平行关节旋转运动的示意图；

[0057] 图6为根据本发明一实施例的平行关节旋转运动的平面投影的示意图。

[0058] 附图标记说明

[0059] 100、用于控制工程设备臂架的装置；101、处理器；102、液压驱动系统；103、液压伺服控制器；104、传感器；105、遥控器；111、转台转动关节；112、塔臂转动关节；113、塔臂；114、主臂转动关节；115、主臂；116平台调平转动关节；θ1、第一期望转动角度；θ2、第二期望转动角度；θ3、第三期望转动角度；θ4、第四当前转动角度。

具体实施方式

[0060] 以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。

[0061] 图1为根据本发明一实施例的用于控制折叠臂式臂架的方法的流程示意图。如图1所示，在本发明实施例中，提供了一种用于控制折叠臂式臂架的方法，折叠臂式臂架包括多个节臂以及连接多个节臂的关节，以该方法应用于处理器为例进行说明，该方法可以包括以下步骤：

[0062] 步骤S100，获取关节的当前转动角度；

[0063] 本实施例中，需要说明的是，工程设备的折叠臂式臂架可以是串联结构，其可以包括多个节臂，节臂间通过可以控制节臂进行相对转动的关节连接。本发明实施例中，折叠臂式臂架的所有节臂可以均为不可伸缩的节臂。当前转动角度包括当前时刻关节对应的转动角度。可以通过在折叠臂式臂架的关节对应的位置安装的传感器(例如，角度传感器)来获取折叠臂式臂架的当前转动角度。

[0064] 具体地，获取关节的当前转动角度，包括：

[0065] 步骤a，响应于监测到用于控制折叠臂式臂架的末端移动的控制信号，获取当前时刻的关节的当前转动角度。

[0066] 本实施例中，需要说明的是，控制折叠臂式臂架的末端移动的控制信号可以触发对折叠臂式臂架的末端进行移动，此时需要确定如何控制折叠臂式臂架的节臂或关节进行运动，以实现折叠臂式臂架末端的位置移动。具体地，当处理器监测到用于控制折叠臂式臂架的末端移动的控制信号时，可以从传感器获取当前时刻的折叠臂式臂架的各个关节的当前转动角度。在一个示例中，控制信号可以是用户(操作员)通过操作遥控器产生的。

[0067] 步骤S200，根据当前转动角度确定折叠臂式臂架的末端的当前位置；

[0068] 本实施例中，需要说明的是，工程设备在控制折叠臂式臂架运动时，主要目的是为了控制折叠臂式臂架末端达到指定位置，例如，高空作业平台车控制末端的工作平台移动，以到达指定位置。在对折叠臂式臂架的位姿进行调整之前，需要确定折叠臂式臂架调整前的当前时刻其末端的位置，即末端的当前位置。

[0069] 具体地，根据当前转动角度定折叠臂式臂架的末端的当前位置，包括：

[0070] 步骤b，将当前转动角度输入至正向运动学模型，以确定折叠臂式臂架的末端的当前位置；其中，正向运动学模型是基于旋量法建立的。

[0071] 本实施例中，需要说明的是，控制折叠臂式臂架的末端从当前位置移动到期望位置时，需要确定当前时刻折叠臂式臂架末端的当前位置，以在确定末端的当前位置后依据运动指令确定末端的期望位置，进而根据期望位置确定控制折叠臂式臂架的各个关节进行姿态调整的期望转动角度，以实现对折叠臂式臂架的姿态进行调整，在此过程前，将建立运动学模型以实现期望转动角度的确定，而在建立运动学模型的初始时刻折叠臂式臂架的状态为初始状态，折叠臂式臂架处于初始状态下的位姿信息为折叠臂式臂架的初始位姿信息。

[0072] 本实施例中，运动学模型包括正向运动学模型和逆向运动学模型，其中，根据旋量法建立正向运动学模型。可以理解，旋量法建立正向运动学模型为根据旋量理论，将串联式折叠臂式臂架的关节运动视为各个节臂的旋量运动，且基于旋量理论中的刚体的旋转运动可由运动旋量的指数积的形式表示，进而可得到刚体转动一定角度后的位姿表达式。其具体的计算方法是所属领域技术人员所知的，此处不再赘述。

[0073] 具体地，在确定正向运动学模型时，确定在初始状态下折叠臂式臂架节臂以及关节的初始位姿信息，并确定折叠臂式臂架的末端的初始位置，以及确定在初始状态下各关节对应的单位运动旋量，最终建立正向运动学模型。将折叠臂式臂架的当前转动角度输入至该正向运动学模型后，即可确定折叠臂式臂架的末端的当前位置。

[0074] 例如，在一实施例中，正向运动学模型为： 其中，gst(θ)表示折叠臂式臂架的末端的当前位置，gst(0)表示折叠臂式臂架的末端的初始位置，θi(i＝1、2、3、4)表示折叠臂式臂架的关节对应的当前转动角度，ξi(i＝1、2、3、4)表示初始状态下关节对应的单位运动旋量。

[0075] 可以理解的是，在一实施例中，还可以采用D‑H(Denavit‑Hartenberg)参数法建立正向运动学模型。D‑H参数法中，各连杆相对于前一连杆建立坐标系，得到各个连杆相对于前一连杆的相对运动，其具体的建立折叠臂式臂架的运动学方程的方式是所属领域技术人员所知的，在此不再赘述。

[0076] 步骤S300，根据当前位置和输入的运动指令确定折叠臂式臂架的末端的期望位置；

[0077] 本实施例中，需要说明的是，在实际应用中，在操作折叠臂式臂架时可以根据实际需求向折叠臂式臂架下达对应的运动指令以使折叠臂式臂架运动，例如，往前移动5米。处理器接收到该运动指令后，需要对该运动指令进行转换，以得到在空间中基于预设基坐标系的折叠臂式臂架末端的期望位置。期望位置是指想要操作折叠臂式臂架末端所到达的位置。运动指令由操作人员输入，例如可以是通过遥控器进行输入，控制按钮进行输入等输入方式。运动指令可以包括在控制所述折叠臂式臂架的末端移动的控制信号中，或可以是在控制所述折叠臂式臂架的末端移动的控制信号输入后单独输入的。

[0078] 本实施例中，以折叠臂式臂架的转台转动关节所在位置为原点建立的坐标系作为基坐标系，处理器在确定折叠臂式臂架末端的当前位置后，根据输入的运动指令在该当前位置的基础上加上该运动指令对应的运动量，以确定折叠臂式臂架的末端在基坐标系中的期望位置。

[0079] 步骤S400，确定折叠臂式臂架的关节中用于对折叠臂式臂架的末端进行调节以使末端保持水平状态的平台调平转动关节；

[0080] 本实施例中，需要说明的是，折叠式臂架的关节可以控制节臂进行转动，折叠臂式臂架的关节中，平台调平转动关节为控制折叠臂式臂架末端保持水平的关节，该关节的转动角度无需计算，可直接通过角度传感器进行读取。

[0081] 具体地，处理器确定折叠臂式臂架的所有关节中用于对折叠臂式臂架的末端进行调节以使末端保持水平状态的平台调平转动关节。

[0082] 步骤S500，根据期望位置和平台调平转动关节对应的当前转动角度确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度；

[0083] 本实施例中，需要说明的是，其他关节是指臂架所有的关节中除了平台调平转动关节外其余的关节；期望转动角度是指使得折叠臂式臂架末端运动至期望位置时，需要对该其他关节的进行设置的参数。

[0084] 具体地，处理器根据期望位置和平台调平转动关节对应的当前转动角度来确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度。

[0085] 步骤S600，根据所述平台调平转动关节对应的当前转动角度和所述其他关节对应的期望转动角度生成用于调节所述折叠臂式臂架位姿的控制指令。

[0086] 本实施例中，需要说明的是，控制指令用于调节折叠臂式臂架位姿，具体为基于各个关节已设置的参数对折叠臂式臂架的所有的关节进行控制的指令。用于调节折叠臂式臂架位姿的控制指令包括了对折叠臂式臂架所有的关节的转动角度进行设置的指令，其中，平台调平转动关节对应设置的参数为当前转动角度，其他关节对应设置的参数为期望转动角度。通过将平台调平转动关节对应的当前转动角度作为控制该平台调平转动关节的参数，降低了确定用于调节折叠臂式臂架位姿对应的控制指令时的计算难度。

[0087] 具体地，处理器可以根据平台调平转动关节对应的当前转动角度和其他关节对应的期望转动角度生成用于调节折叠臂式臂架位姿的控制指令。

[0088] 本发明实施例中，用于控制折叠臂式臂架的装置的控制结构由液压伺服控制器与包含处理器的工控机两部分组成，分别作为下位机与上位机。下位机对液压驱动系统进行实时位置控制，上位机用于求解复杂的臂架逆解与关节空间轨迹规划，该控制结构通过分层控制的方式，工控机向液压伺服控制器定时下发的控制信号，可以一定程度补偿由于液压控制时延产生的控制误差，从而提高对折叠臂式臂架进行控制的整个折叠臂式臂架控制系统的响应速度。同时也可以避免由于逆运动学求解超时造成的折叠臂式臂架控制不稳定问题，提高了折叠臂式臂架控制系统的鲁棒性。

[0089] 参考图2，在一应用场景中，用于控制折叠臂式臂架的方法应用于用于控制折叠臂式臂架的装置100，用于控制折叠臂式臂架的装置100包括遥控器105、传感器104、液压伺服控制器103、液压驱动系统102以及处理器101。当遥控器105被触发，液压伺服控制器103根据遥控器105触发的当前时刻t，从传感器104获取当前时刻臂架的各个关节对应的当前转动角度，将该当前时刻和该当前转动角度输入至处理器101，处理器101基于获取的当前时刻和该当前转动角度确定用于控制折叠臂式臂架的位姿的控制指令，该控制指令包括对折叠臂式臂架的每一个关节对应的转动角度。具体地，处理器101通过正向运动学模型确定折叠臂式臂架末端的当前位置，输入至笛卡尔空间轨迹规划中，以得到根据该当前位置以及输入的运动指令确定臂架的末端的期望位置，从而将该期望位置输入至逆向运动学模型中，确定折叠臂式臂架的每一个关节对应的期望位置，关节空间轨迹规划通过每一个关节对应的期望位置确定每一个关节对应的期望转动角度，以生成用于调节折叠臂式臂架位姿的控制指令。液压伺服控制器103根据该控制指令控制液压驱动系统102驱动臂架的多个关节进行同步运动。其中，输入的运动指令可以由遥控器105按钮对应的控制信号进行确定。

[0090] 上述用于控制工程设备臂架的方法，通过获取折叠臂式臂架的关节的当前转动角度，以根据当前转动角度确定折叠臂式臂架的末端的当前位置，从而根据当前位置和输入的运动指令确定折叠臂式臂架的末端的期望位置，并确定折叠臂式臂架的关节中用于对折叠臂式臂架的末端进行调节以使末端保持水平状态的平台调平转动关节，以根据期望位置和平台调平转动关节对应的当前转动角度确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度，进而根据平台调平转动关节对应的当前转动角度和其他关节对应的期望转动角度生成用于调节折叠臂式臂架位姿的控制指令。针对折叠臂式臂架计算臂架控制指令时运算时间长，通过划分出平台调平转动关节有效的降低了在确定用于调节折叠臂式臂架位姿的控制指令时的计算难度，并极大的加快了计算的速度，有效提升在复杂高空作业场景中的适应性和灵活性，并且，通过用于调节折叠臂式臂架位姿的控制指令，可以实现臂式高空作业平台末端位置的自动控制，无需操作人员预先熟悉工程设备臂架的结构形式，降低了高空作业平台的使用门槛，使臂式高空作业平台的操作更加简单直观、可控性更好，降低劳动强度。

[0091] 在一个实施例中，根据期望位置和平台调平转动关节对应的当前转动角度确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度，包括：

[0092] 步骤c，将期望位置和平台调平转动关节对应的当前转动角度输入至基于Paden‑Kahan子问题建立的逆向运动学模型，以确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度。

[0093] 本实施例中，需要说明的是，逆运动学是决定要达成所需要的姿势所要设置的关节可活动对象的参数的过程，即对折叠臂式臂架的关节的期望转动角度进行设置的过程，逆向运动学模型可实现对该期望转动角度的求解。在建立逆向运动学模型时，对折叠臂式臂架的除平台调平转动关节以外的其他关节进行求解方程的确定，以在获取到期望位置和平台调平转动关节对应的当前转动角度时，将该获取到的期望位置和平台调平转动关节对应的当前转动角度作为已知量，代入确定的求解方程，从而得到该其他关节对应的期望转动角度。

[0094] 具体地，针对折叠臂式臂架的除平台调平转动关节以外的其他关节的期望转动角度进行计算时，处理器将确定的平台调平转动关节对应的当前转动角度和臂架末端的期望位置作为已知量输入至基于Paden‑Kahan子问题建立的逆向运动学模型，以在经过逆向运动学模型中的运算过程后，得到该其他关节对应的期望转动角度。

[0095] 具体地，基于Paden‑Kahan子问题建立的逆向运动学模型是通过以下步骤建立的：

[0096] 步骤c1，确定折叠臂式臂架的关节中用于对折叠臂式臂架整体进行调节以使折叠臂式臂架整体在水平面内转动的转台转动关节；

[0097] 步骤c2，基于折叠式臂架的各个节臂以及关节之间的空间几何关系建立转台转动关节对应的期望转动角度的第一求解模型；

[0098] 步骤c3，获取折叠臂式臂架的初始位姿信息，初始位姿信息包括关节的初始位置和折叠臂式臂架的末端的初始位置；

[0099] 步骤c4，根据初始位姿信息、期望位置、平台调平转动关节对应的当前转动角度以及第一求解模型，基于Paden‑Kahan子问题建立折叠臂式臂架的除平台调平转动关节和转台转动关节以外的剩余关节对应的期望转动角度的第二求解模型；

[0100] 步骤c5，根据第一求解模型和第二求解模型建立逆向运动学模型。

[0101] 本实施例中，需要说明的是，控制折叠臂式臂架的末端从当前位置移动到期望位置时，将在确定末端的当前位置后依据运动指令确定末端的期望位置，进而根据期望位置确定控制折叠臂式臂架的关节进行姿态调整的期望转动角度，以实现对折叠臂式臂架的姿态进行调整，在此过程前，将建立运动学模型以实现期望转动角度的确定，而在建立运动学模型的初始时刻折叠臂式臂架的状态为初始状态，折叠臂式臂架处于初始状态下的位姿信息为折叠臂式臂架的初始位姿信息。可以理解的是，该初始位姿信息可以是折叠臂式臂架的出厂位姿信息，还可以是折叠臂式臂架在进行一定的姿态调整后的位姿信息，其确定的依据为建立运动学模型的初始时刻，可以根据实际需求进行确定或变更。

[0102] 运动学模型包括正向运动学模型和逆向运动学模型，其中，基于末端的期望位置进行逆向运动学求解，得到臂架所有的关节的期望转动角度时，计算过程复杂且耗时长。本实施例中，通过减少需要进行计算的关节的数量，选取平台调平转动关节的当前转动角度作为定值，基于该确定的定值对其他关节进行求解，以得到使得折叠臂式臂架的末端到达期望位置时折叠臂式臂架所有的关节唯一对应的转动角度。建立逆向运动学模型时，将对折叠臂式臂架的除平台调平转动关节以外的其他关节进行求解方程的确定，在建立逆向运动学模型时，将平台调平转动关节对应的当前转动角度作为逆向运动学模型中进行逆向运动学求解的已知量，也即该平台调平转动关节对应的当前转动角度将直接作为定值，不需要再进一步进行转动角度的求解。

[0103] 具体地，在一实施例中，折叠臂式臂架的关节有4个，包括：转台转动关节、塔臂转动关节、主臂转动关节、以及平台调平转动关节。本实施例中，考虑实际工况的臂架的关节中，平台调平转动关节用于工作平台的调平，其通过角度传感器进行主动的调节，故考虑将平台调平转动关节对应的转动角度作为已知量，在折叠臂式臂架的逆向运动学模型中的运算过程中直接读取平台调平转动关节对应的当前转动角度，将其作为当前求逆的一个定值进行求解。

[0104] 转台转动关节为折叠式臂架的关节中用于对折叠臂式臂架整体进行调节以使折叠臂式臂架整体在水平面内转动的关节。在确定折叠臂式臂架末端的期望位置后，可以根据折叠臂式臂架的各个节臂以及关节之间的空间几何关系确定用于确定转台转动关节对应的期望转动角度的第一求解模型。具体地，当期望位置为(x，y，z)时，可确定用于确定转台转动关节对应的期望转动角度θ1的第一求解模型为：θ1＝atan2(y,x)。

[0105] 第二求解模型是指用于确定折叠臂式臂架的除平台调平转动关节和转台转动关节以外的剩余关节对应的期望转动角度的求解模型。剩余关节包括塔臂转动关节、主臂转动关节。根据初始位姿信息、期望位置、平台调平转动关节对应的当前转动角度以及第一求解模型，基于Paden‑Kahan子问题建立第二求解模型，可以理解的是，塔臂转动关节、主臂转动关节为互相平行的关节。具体地，图5为根据本发明一实施例的平行关节旋转运动的示意图，图6为根据本发明一实施例的平行关节旋转运动的平面投影的示意图。参考图5和图6，在本发明实施例中，假设从折叠臂式臂架的末端的初始位置点q0绕轴ξ3转动θ3到点qw1，再从点qw1绕轴ξ2转动θ2到qw，其中，qw通过末端的期望位置qt与θ1确定， 在垂直于轴ξ2、ξ3的绕轴旋转轨迹面，取轨迹面上与两轴的交点分别为r2、r3，且c是qw1在轨迹面上的投影点。

[0106] 定义矢量：

[0107] u＝qw‑r2，v＝q0‑r3；

[0108] r23＝r2‑r3，r32＝r3‑r2；

[0109] 则u与r23之间的夹角θ01为：

[0110]

[0111]

[0112] 则v与r23之间的夹角θ02为：

[0113]

[0114]

[0115] 其中，ξ2、ξ3分别表示塔臂转动关节和主臂转动关节对应的初始位姿信息系中转轴的单位运动旋量， 表示转轴的单位轴向量的转置；

[0116] θ01表示u与r23之间的夹角；θ02表示v与r23之间的夹角。

[0117] θ1表示转台转动关节对应的期望转动角度，根据第一求解模型确定；θ2表示塔臂转动关节对应的期望转动角度；θ3表示主臂转动关节对应的期望转动角度。

[0118] 基于Paden‑Kahan子问题建立折叠臂式臂架的运动学方程的方式的具体推导过程是所属领域技术人员所知的，在此不再赘述。

[0119] 在一个实施例中，将期望位置和平台调平转动关节对应的当前转动角度输入至基于Paden‑Kahan子问题建立的逆向运动学模型，以确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度，包括：

[0120] 步骤d，基于期望位置和平台调平转动关节对应的当前转动角度判断逆向运动学模型是否存在实数解；

[0121] 步骤e，如果确定逆向运动学模型存在实数解，则判断实数解是否超出折叠臂式臂架的除平台调平转动关节以外的其他关节对应的第一预设运动范围；

[0122] 步骤f，如果实数解未超出第一预设运动范围，则基于实数解确定其他关节对应的期望转动角度。

[0123] 本实施例中，需要说明的是，在通过逆向运动学模型确定其他关节对应的期望转动角度为利用逆向运动学模型中的求解方程进行计算。期望转动角度对应的值应为实数，且需要在该求解的关节所对应的运动范围内。其他关节对应的运动范围为第一预设运动范围，该第一预设运动范围表示该其他关节在出厂时可以或者被允许的运动范围。故需在将期望位置和平台调平转动关节对应的当前转动角度代入计算结束后，对逆向运动学模型是否存在实数解进行判断，当求出的解是实数解时，判断其是否在该第一预设运动范围内；只有确定的解为实数解且该实数解在第一预设运动范围内时，才可基于该实数解确定其他关节对应的期望转动角度。可以理解的是，该实数解为多个，分别对应各个其他关节，基于各个其他关节对应的实数解分别确定各个其他关节对应的期望转动角度。

[0124] 具体地，处理器将基于期望位置和平台调平转动关节对应的当前转动角度确定逆向运动学模型是否存在实数解，如果确定逆向运动学模型存在实数解，则判断该实数解是否超出臂架的除平台调平转动关节以外的其他关节对应的第一预设运动范围，若实数解未超出第一预设运动范围，则基于实数解确定其他关节对应的期望转动角度。

[0125] 在一个实施例中，将期望位置和平台调平转动关节对应的当前转动角度输入至基于Paden‑Kahan子问题建立的逆向运动学模型，以确定折叠臂式臂架的除平台调平转动关节以外的其他关节对应的期望转动角度，包括：

[0126] 步骤g，如果确定逆向运动学模型不存在实数解，或存在超出第一预设运动范围的实数解，则等待预设间隔时长，以在等待结束后重新获取平台调平转动关节对应的当前转动角度，直至得到未超出第一预设运动范围的实数解。

[0127] 本实施例中，需要说明的是，在对逆向运动学模型是否存在实数解进行判断时，当求出的解不是实数解或存在实数解但该实数解超出第一预设运动范围时，将确定基于当前的已知条件该逆向运动学模型无解。预设间隔时长是一个预设的时间范围，例如，0.02s。平台调平转动关节用于对折叠臂式臂架的末端进行调节以使末端保持水平状态，其转动角度可直接通过角度传感器读取的，当平台调平转动关节对应的当前转动角度作为已知量代入逆向运动学模型后该逆向运动学模型无解时，确定基于该平台调平转动关节对应的当前转动角度无法得出其他关节对应的期望转动角度，该平台调平转动关节对应的当前转动角度可能存在读取误差，故在确定逆向运动学模型无法得到实数解时，等待预设间隔时长，以在等待结束后重新获取平台调平转动关节对应的当前转动角度。当重新获取平台调平转动关节对应的当前转动角度后，将基于该重新获取的平台调平转动关节对应的当前转动角度重新确定折叠臂式臂架末端的期望位置，并将该重新确定的期望位置和重新获取的平台调平转动关节对应的当前转动角度输入至逆向运动学模型，直至得到未超出第一预设运动范围的实数解。

[0128] 具体地，处理器在确定逆向运动学模型不存在实数解，或存在超出第一预设运动范围的实数解时，将等待预设间隔时长，以在等待结束后重新获取平台调平转动关节对应的当前转动角度，直至得到未超出第一预设运动范围的实数解。

[0129] 在本发明实施例中，多个节臂包括塔臂、主臂以及工作平台，关节包括转台转动关节、塔臂转动关节、主臂转动关节以及平台调平转动关节，

[0130] 转台转动关节通过塔臂转动关节与塔臂的一端连接，塔臂的另一端通过主臂转动关节与主臂的一端连接，主臂的另一端通过平台调平转动关节与工作平台连接，[0131] 当前转动角度包括转台转动关节的第一当前转动角度、塔臂转动关节的第二当前转动角度、主臂转动关节的第三当前转动角度以及平台调平转动关节的第四当前转动角度；

[0132] 折叠臂式臂架的关节包括：转台转动、塔臂转动、主臂转动、平台调平转动。

[0133] 图3为根据本发明一实施例的逆向运动学模型求解的流程示意图，图4为根据本发明一实施例的工程设备的臂架的示意图。参考图3和图4，在本发明实施例中，工程设备包括多个节臂和连接多个节臂的关节。其中，多个节臂可以包括塔臂113以及主臂115，关节包括转台转动关节111、塔臂转动关节112、主臂转动关节114以及平台调平转动关节116。转台转动关节111通过塔臂转动关节112与塔臂113的一端连接，塔臂113的另一端通过主臂转动关节114与主臂115的一端连接，主臂115的另一端与平台调平转动关节116连接。关节的当前转动角度包括转台转动关节的第一当前转动角度、塔臂转动关节的第二当前转动角度、主臂转动关节的第三当前转动角度以及平台调平转动关节的第四当前转动角度θ4。在本发明实施例中，考虑实际工况，在臂架的关节中平台调平转动关节116用于工作平台的调平，其通过角度传感器进行主动的调节；故将平台调平转动关节116对应的第四当前转动角度θ4作为已知量，在折叠臂式臂架进行运动学求解的流程时，直接读取平台调平转动关节116对应的第四当前转动角度θ4对应的第四当前转动角度，将其作为当前求逆的一个定值进行求解，从而计算折叠臂式臂架的所有关节中除平台调平转动关节116之外其他关节对应的期望转动角度：转台转动关节111对应的第一期望转动角度θ1、塔臂转动关节112对应的第二期望转动角度θ2、主臂转动关节114对应的第三期望转动角度θ3；判断计算的结果是否存在实数解，若存在实数解，则判断该实数解是否超出第一预设运动范围，若该实数解未超出第一预设运动范围，则确定存在可行解，此时可以确定其他关节对应的期望转动角度；若不存在实数解，或者存在超出第一预设运动范围，则对将等待预设间隔时长，以在等待结束后重新获取平台调平转动关节116对应的第四当前转动角度θ4，并将重新获取的平台调平转动关节116对应的第四当前转动角度θ4代入计算流程中，重新执行计算，直到确定出可行解。

[0134] 现有技术中，针对折叠臂式臂架的末端位置进行控制时，采用的控制算法运算时间较长，无法达到实时控制效果。并且，当折叠臂式臂架的关节较多时，会为确定各关节的期望转动角度的逆运动学模型带来困难，而常规方式会采用比较通用的解法，计算量会随着折叠臂式臂架关节的提高而增加，求解时间无法满足实时控制的要求。而本发明实施例提供的技术方案，确定平台调平转动关节对应的当前转动角度为定值，对其余关节进行求解，极大的提升了运算速度，且求解精度高，满足实时性控制要求；并且，通过自动控制，无需操作人员预先熟悉折叠臂式臂车的结构形式，降低了高空作业平台的使用门槛，使臂式高空作业平台的操作更加简单直观、可控性更好，降低劳动强度。

[0135] 本发明实施例提供了一种处理器，被配置成执行时实现如上的用于控制折叠臂式臂架的方法的步骤。

[0136] 本发明实施例提供了一种用于控制折叠臂式臂架的装置，包括：

[0137] 液压驱动系统，用于驱动折叠臂式臂架运动；

[0138] 传感器，用于检测折叠臂式臂架的关节转动角度；

[0139] 如上实施例所述的处理器；以及

[0140] 液压伺服控制器，被配置成：

[0141] 响应于接收到的控制信号，根据传感器检测的关节转动角度生成关节转动角度信息，并将生成的关节转动角度信息输送至处理器；

[0142] 根据从处理器接收的用于调节折叠臂式臂架位姿的控制指令控制液压驱动系统驱动折叠臂式臂架运动。

[0143] 在本发明实施例中，还包括：

[0144] 遥控器，用于响应于用户操作发送控制信号。

[0145] 本发明实施例提供了一种工程设备，包括：

[0146] 折叠臂式臂架，折叠臂式臂架包括多个节臂以及连接多个节臂的关节；

[0147] 如上实施例所述的用于控制折叠臂式臂架的装置。

[0148] 在本发明实施例中，多个节臂包括塔臂、主臂以及工作平台，关节包括转台转动关节、塔臂转动关节、主臂转动关节以及平台调平转动关节，转台转动关节通过塔臂转动关节与塔臂的一端连接，塔臂的另一端通过主臂转动关节与主臂的一端连接，主臂的另一端通过平台调平转动关节与工作平台连接。

[0149] 本发明实施例提供了一种机器可读存储介质，机器可读存储介质上存储有指令，指令在被处理器执行时使得处理器执行如上实施方式中的用于控制折叠臂式臂架的方法。

[0150] 本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD‑ROM、光学存储器等)上实施的计算机程序产品的形式。

[0151] 还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、商品或者设备中还存在另外的相同要素。

[0152] 以上仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。

[0153] 以上结合附图详细描述了本发明的优选实施方式，但是，本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种简单变型，这些简单变型均属于本发明的保护范围。另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。

[0154] 此外，本发明的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明的思想，其同样应当视为本发明所公开的内容。