CN117263032A - 工程机械及其安全控制方法、装置和介质 - Google Patents

Abstract

本申请公开了一种工程机械及其安全控制方法、装置和介质，属于工程机械技术领域。所述方法包括：获取基于预设坐标系形成的方程，其中第一方程描述铰接点A相对于大臂与钢丝绳的铰接点D之间的直线AD，第二方程描述液压缸与车身的铰接点B相对于该液压缸与大臂的铰接点C之间的直线BC，第三方程描述吊载物摆动轨迹；调节大臂与水平方向之间的夹角，以满足三个方程无交点。本申请利用预设坐标系来分析吊载物摆动轨迹、大臂与液压缸之间的空间关系，再通过调节大臂与水平方向之间的夹角来避免吊载物与大臂和液压缸发生碰撞。

Description

工程机械及其安全控制方法、装置和介质

技术领域

本申请涉及工程机械技术领域，具体地涉及一种工程机械及其安全控制方法、装置和介质。

背景技术

当前的吊臂式工程机械，例如轮胎吊起重机，经常需要吊装1吨以上的重物低速行驶至指定位置。但是，由于吊臂末端使用钢丝绳吊装，为柔性材料，因此当吊装物体时，工程机械整体无法成为一个刚体，吊载物会摆动，而工程机械行驶的加速度变化越大，吊载物摆动幅度也越大。当吊载物摆动幅度过大时，可能会造成吊载物与周围物体的碰撞，带来安全问题。

对此，目前在工地现场主要依靠人工根据现场情况谨慎操作，也有在平缓路面上控制工程机械加速度和脉冲输入整形的方式来减少吊载物晃动，但前者不够智能，后者控制难度较大。

发明内容

本申请实施例的目的是提供一种工程机械及其安全控制方法、装置和介质，用以至少部分地解决上述技术问题。

为了实现上述目的，本申请第一方面提供一种工程机械的安全控制方法，包括获取基于预设坐标系形成的以下方程：第一方程，用于描述大臂与车身的铰接点A相对于所述大臂与钢丝绳的铰接点D之间的直线AD；第二方程，用于描述液压缸与车身的铰接点B相对于该液压缸与所述大臂的铰接点C之间的直线BC；以及第三方程，用于描述吊载物摆动轨迹。该安全控制方法还包括：调节所述大臂与水平方向之间的夹角γ，以满足所述第一方程与所述第三方程之间、所述第二方程和所述第三方程之间均无交点。

可选地，在本申请实施例中，所述预设坐标系是车身坐标系或臂架坐标系，其中所述臂架坐标系被配置为在臂架无回转状态下分别以所述铰接点A、车身平行方向和车身垂直方向为原点、X轴和Y轴。

可选地，在本申请实施例中，针对所述臂架坐标系，描述所述直线AD的所述第一方程为：

其中，[x_AD y_AD]为所述直线AD在所述臂架坐标系下的坐标，l₁的取值范围为0～lz，而lz为大臂长度，θ为所述大臂与所述X轴的夹角。

可选地，在本申请实施例中，针对所述臂架坐标系，描述所述直线BC的所述第二方程为：

其中，[x_BC y_BC]为所述直线BC在所述臂架坐标系下的坐标，[x_B y_B]为铰接点B在所述臂架坐标系下的坐标，l₂的取值范围为0～所述直线BC的长度，∠CBA表示以铰接点B为顶点的直线BA和直线BC之间的夹角。

可选地，在本申请实施例中，针对所述臂架坐标系，描述所述吊载物摆动轨迹的所述第三方程为：

其中，[x_h y_h]为所述吊载物在所述臂架坐标系下的坐标，[x_D y_D]为铰接点D在所述臂架坐标系下的坐标，r为所述吊载物的摇摆半径，σ为所述吊载物的摇摆角度。

可选地，在本申请实施例中，针对所述夹角γ的所述调节基于以下的约束条件被执行：

其中，γ_min和γ_max分别为在所述工程机械不发生倾翻的情况下的所述大臂与水平方向的可调最小夹角和可调最大夹角，h是所述吊载物的高度，z_g和z_s分别是吊钩和车身相对于地面的高度。

可选地，在本申请实施例中，所述安全控制方法还包括：规划行驶路径。其中，针对所述行驶路径的所述规划基于以下至少一条约束条件被执行：控制前轮不能转向，以及限制后轮的转向角度；所述工程机械的转弯半径大于最小转弯半径；所述工程机械的直行宽度大于或等于直线行驶最小道路宽度；避让障碍物，所述障碍物包括根据针对车身周围环境的扫描数据确定的高度差超过预设值的路面。

可选地，在本申请实施例中，所述安全控制方法还包括：将所规划的行驶路径划分为若干直线段和圆弧段；以及针对所划分的每一直线段和圆弧段，控制所述工程机械依次行驶于加速启动阶段、正常行驶阶段和减速停止阶段，并针对每一阶段进行行驶控制和/或力矩控制。

可选地，在本申请实施例中，进行所述行驶控制包括：针对所述圆弧段，先将所述工程机械的后轮转向角度调整为角度α_t，再控制所述工程机械行驶。其中，所述角度α_t通过下式被确定：

其中，γ_t为当前圆弧段的圆弧半径，L为车身长度。

可选地，在本申请实施例中，进行所述力矩控制包括：

1)在加速启动阶段，顺次执行如下的力矩控制步骤：初始控制所述工程机械的底盘维持基准力矩；每隔预定时间间隔，控制所述底盘在上一个间隔对应的力矩的基础上增加第一预设力矩；以及在所述工程机械的当前加速度大于预设最大加减速度时，控制所述底盘在上一个间隔对应的力矩的基础上减去所述第一预设力矩，直到达到规划的行驶速度或者行驶到起始加速路径的终点。

2)在正常行驶阶段，先控制所述底盘维持所述加速启动阶段的输出力矩，再调整该输出力矩以使得所述工程机械保持所述行驶速度。

3)在减速停止阶段，顺次执行如下的力矩控制步骤：初始控制所述底盘维持所述正常行驶阶段的输出力矩；每隔预定时间间隔，控制所述底盘在上一个间隔对应的力矩的基础上减去第二预设力矩；以及在所述工程机械的当前加速度大于所述预设最大加减速度时，控制所述底盘在上一个间隔对应的力矩的基础上增加所述第二预设力矩，直到所述工程机械的速度降到0或行驶至所述停止减速路径的终点。

本申请第二方面提供一种工程机械的安全控制装置，包括：存储器，被配置成存储指令；以及处理器，被配置成从所述存储器调用所述指令以及在执行所述指令时能够实现上述任意的安全控制方法。

本申请第三方面提供一种工程机械，包括上述的安全控制装置。

可选地，在本申请实施例中，该工程机械还包括：分别安装在所述工程机械的吊钩和车身上的第一RTK传感器和第二RTK传感器，其中所述第一RTK传感器用于测量吊钩三维空间坐标，所述第二RTK传感器用于测量车身三维空间坐标；分别安装在所述工程机械的底盘的前侧、后侧、左侧和右侧的激光雷达，用于获取关于相应区域环境的扫描数据；以及安装在所述工程机械的大臂上的倾角传感器和若干拉线传感器，其中所述倾角传感器用于测量所述工程机械的大臂与水平方向的夹角γ，以及所述拉线传感器用于测量大臂长度。

本申请第四方面提供一种机器可读存储介质，该机器可读存储介质上存储有指令，该指令用于使得机器执行上述任意的安全控制方法。

通过上述技术方案，本申请实施例利用预设坐标系来分析吊载物摆动轨迹、大臂直线与液压缸直线之间的空间关系，再通过调节所述大臂与水平方向之间的夹角来使得吊载物摆动轨迹与两条直线不相交，进而避免了晃动的吊载物与大臂和液压缸之间的碰撞。这一控制方案易于实现，智能性高，有助于减少人工操作。

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

附图说明

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

图1示意性示出了根据本申请实施例的一种工程机械的安全控制方法的流程示意图；

图2示意性示出了根据本申请实施例的示例的轮胎吊起重机的传感器布局示意图；

图3示意性示出了根据本申请实施例的针对图2的起重机构建的臂架坐标系；

图4示意性示出了根据本申请实施例的图3的臂架坐标系的简洁示意图；

图5示意性示出了根据本申请实施例的安全控制方法的示例应用的流程图；以及

图6示意性示出了根据本申请实施例的一种工程机械的安全控制装置的结构示意图。

附图标记说明

201 第一RTX传感器 202 第二RTK传感器

203 倾角传感器 204 拉线传感器

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，应当理解的是，此处所描述的具体实施方式仅用于说明和解释本申请实施例，并不用于限制本申请实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

需要说明，若本申请实施例中有涉及方向性指示(诸如上、下、左、右、前、后……)，则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

另外，若本申请实施例中有涉及“第一”、“第二”等的描述，则该“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本申请要求的保护范围之内。

图1示意性示出了根据本申请实施例的一种工程机械的安全控制方法的流程示意图。需说明的是，为了更好地解释图1所示的本申请实施例的安全控制方法的部分数据来源，在此先以图2所示的轮胎吊起重机及其传感器布局为例，介绍本申请实施例所涉及的工程机械的传感器布局。

如图2所示，轮胎吊起重机包括上装和底盘两部分，其实质是一种利用轮胎式底盘行走的动臂旋转起重机，或者可理解为是一种将起重机构安装在加重型轮胎和轮轴组成的特制底盘上的一种全回转式起重机。该轮胎吊起重机的上装构造与履带式起重机基本相同，且为了保证安装作业时机身的稳定性，起重机设有四个可伸缩的支腿，进而在较为平坦地面上可不用支腿进行小的起重量吊装及吊物低速行驶。本申请实施例主要旨在解决如轮胎吊起重机在行驶过程中的吊载物晃动而与大臂或液压缸等产生碰撞的问题。

如图2所示，本申请实施例所适用的传感器布局可以包括：分别安装在所述工程机械的吊钩和车身上的第一RTK传感器201和第二RTK传感器202，其中所述第一RTK传感器201用于测量吊钩三维空间坐标，所述第二RTK传感器202用于测量车身三维空间坐标；分别安装在所述工程机械的底盘的前侧、后侧、左侧和右侧的激光雷达(图2中未示出)，用于获取关于相应区域环境的扫描数据；以及安装在所述工程机械的大臂上的倾角传感器203和若干拉线传感器204，其中所述倾角传感器203用于测量所述工程机械的大臂与水平方向的夹角(该夹角将在其他附图中标记为γ)，以及所述拉线传感器204用于测量大臂长度。其中，RTK传感器是当前常用的一种高精度定位传感器。

基于如图2所示的该传感器布局，图1的工程机械的安全控制方法可以获取基于该传感器布局采集的关联于工程机械的安全控制的数据，进而用来进行坐标构建、方程构建以及参数调节等。

据此，回到图1，本申请实施例提供一种工程机械的安全控制方法，该方法可以包括下列步骤。

步骤S100，获取基于预设坐标系形成的第一方程、第二方程和第三方程。

其中，所述预设坐标系是车身坐标系或臂架坐标系。其中，所述车身坐标系是常规的用来描述车辆运动的坐标系，例如常选取车辆质心为原点，X轴平行于地面指向驾驶员视野前方，Z轴指向上方，Y轴指向驾驶员左手边。

其中，所述臂架坐标系例如被配置为在臂架无回转状态下分别以所述铰接点A、车身平行方向和车身垂直方向为原点、X轴和Y轴。举例而言，图3是针对图2的起重机而构建的臂架坐标系，该臂架坐标系分别以大臂与车身的铰接点A、车身平行方向和车身垂直方向为原点、X轴和Y轴。在图3中，还进一步示出了下文将要用到的其他铰接点：工程机械的液压缸与车身的铰接点B；液压缸与大臂的铰接点C；大臂与钢丝绳的铰接点D；钢丝绳与吊钩的铰接点E。并且，出于描述清楚的目的，针对图3示出的臂架坐标系和各个铰接点，图4给出了更为简洁的臂架坐标系的示意图，以便于能够更为清楚地描述各个铰接点及它们相对于臂架坐标系形成的直线和夹角的相对关系。

回到步骤S100，参考图4，所述第一方程用于描述所述铰接点A相对于所述大臂与钢丝绳的铰接点D之间的直线AD，所述第二方程用于描述液压缸与车身的铰接点B相对于该液压缸与所述大臂的铰接点C之间的直线BC，而所述第三方程用于描述吊载物摆动轨迹。

如此，根据三个方程的定义，易知若第一方程与第三方程之间以及第二方程和第三方程之间均无交点，则表明直线AD、直线BC和吊载物摆动轨迹不会相交，进而表明不会发生吊载物与大臂和液压缸的碰撞。据此，回到图1，在步骤S100的基础上，本申请实施例的安全控制方法进一步包括：

步骤S200，调节所述大臂与水平方向之间的夹角，以满足所述第一方程与所述第三方程之间、所述第二方程和所述第三方程之间均无交点。

举例而言，工程机械的控制器可实时获取如图2中的倾角传感器203采集的大臂与水平方向之间的夹角γ，进而发出控制指令以减少或增大该夹角γ来使得所述第一方程和第二方程相对于第三方程均无交点，进而避免了晃动的吊载物与大臂和液压缸发生碰撞，确保了工程机械的行车安全。

如此，通过步骤S100和步骤S200，本申请实施例利用预设坐标系，考虑了吊载物摆动轨迹、大臂与液压缸之间的空间关系，通过调节所述大臂与水平方向之间的夹角来调整大臂变幅角度，减少吊载物碰撞风险。

在优选的实施例中，除考虑吊载物碰撞风险之外，还考虑了其他影响工程机械行驶安全的因素，例如凹凸不平的路面、无人驾驶工况等。对此，本申请优选的实施例在上述步骤S100和步骤S200的基础上，还进一步提出了：

步骤S300(图中未示出)，规划行驶路径。

其中，该规划基于以下至少一条约束条件被执行：控制前轮不能转向，以及限制后轮的转向角度；所述工程机械的转弯半径大于最小转弯半径；所述工程机械的直行宽度大于或等于直线行驶最小道路宽度；避让障碍物，所述障碍物包括根据针对车身周围环境的扫描数据确定的高度差超过预设值的路面。这一约束条件的设置有助于避免车辆在行驶中发生倾翻以及撞上障碍物。

优选地，规划避让所述障碍物的行驶路径包括：针对所述工程机械预期的行驶起点和行驶终点、所确定的障碍物的位置以及上述任意的约束条件，进行路径规划。在上述约束条件中，关于最小转弯半径和直线行驶最小道路宽度的确定可结合车身参数和路况进行，且将在下文结合具体示例进行解释，故而在此不再进行赘述。

更为优选地，还将所规划的行驶路径划分为若干直线段和圆弧段，并针对所划分的每一直线段和圆弧段，控制所述工程机械依次行驶于加速启动阶段、正常行驶阶段和减速停止阶段，并针对每一阶段进行行驶控制和/或力矩控制。

其中，关于行驶控制主要是控制经过圆弧段时的后轮转向角度，这还将在下文结合示例进行介绍，而进行力矩控制适应于不同行驶阶段而可以包括以下三种情形：

1、加速启动阶段。

顺次执行如下的力矩控制步骤：初始控制底盘维持基准力矩；每隔预定时间间隔，控制所述底盘在上一个间隔对应的力矩的基础上增加第一预设力矩；以及在所述工程机械的当前加速度大于预设最大加减速度时，控制所述底盘在上一个间隔对应的力矩的基础上减去所述第一预设力矩，直到达到规划的行驶速度或者行驶到起始加速路径的终点；

2、正常行驶阶段。

先控制所述底盘维持所述加速启动阶段的输出力矩，再调整输出力矩以使得所述工程机械保持在所述行驶速度。

3、减速停止阶段。

顺次执行如下的力矩控制步骤；初始控制所述底盘维持所述正常行驶阶段的输出力矩；每隔预定时间间隔，控制所述底盘在上一个间隔对应的力矩的基础上减去第二预设力矩；以及在所述工程机械的当前加速度大于所述预设最大加减速度时，控制所述底盘在上一个间隔对应的力矩的基础上增加所述第二预设力矩，直到所述工程机械的行驶速度降到0或行驶至所述停止减速路径的终点。

据此，重复上述针对直线段和圆弧段的分阶段行驶控制和力矩控制，基于所规划的行驶路径，可实现工程机械的无人驾驶，即能够使得例如无人驾驶起重机行驶至上述规划的路径的终点，且防止行驶过程中发生车辆倾翻。

图5是本申请实施例的安全控制方法的示例应用的流程图，下面则通过该示例应用来更为详细地介绍上述工程机械的安全控制方法的实现。该示例应用是在凹凸不平路面上的轮胎吊起重机的无人驾驶方法，其旨在减少吊载物的晃动，且采用如图2所示的传感器布局以及图3和图4所示的臂架坐标系。如图5所示，该无人驾驶方法包括以下的步骤S1-S9。

步骤S1，进行传感器布局。

举例而言，参考图2进行传感器布局，其中激光雷达的数量可根据环境检测需要进行设置，而拉线传感器204的数量可根据起重机的臂长情况确定，其测量的大臂长度记为lz。

步骤S2，复位吊载物和起重机上装。

举例而言，基于如图2安装的两个RTK，当起重机吊装吊载物时，由第一RTK传感器反馈吊钩三维空间坐标，再参照专家知识库将不同高度尺寸的吊载物上升至一定高度，然后将起重机上装旋转至0度。

步骤S3，建立臂架坐标系，确定描述相关直线和吊载物摆轨迹的方程。

举例而言，在步骤S2中的起重机上装旋转至0度基础上，建立如图3和图4所示的臂架坐标系。然后，获取以下参数：使用全站仪测量直线BA与X轴的夹角β、直线AC的长度(记为长度AC)和直线AB的长度(记为长度AB)；使用倾角传感器测量起重机大臂与水平方向的夹角γ；使用第二RTK传感器测量车身三维空间坐标[x_s y_s z_s]以及整体车身与水平方向的夹角∈；以及使用第一RTK传感器测量起重机吊钩三维空间坐标[x_g y_g z_g]。然后，计算大臂与X轴的夹角θ、铰接点B、C、D各自的位置坐标[x_B y_B]、[x_C y_C]、[x_D y_D]以及吊载重物的摇摆半径r，而涉及的计算公式如下：

θ＝π-(γ-∈) (1)

r＝y_D-(z_g-z_s)+h (5)

结合图4理解涉及的各个角度参数和长度参数，且其中lz是大臂长度，h是吊载物高度。

据此，上述步骤S100中涉及的描述所述直线AD的第一方程为：

其中，[x_AD y_AD]为所述直线AD在所述臂架坐标系下的坐标，l₁的取值范围为0～lz。

进一步地，描述所述直线BC的第二方程为：

其中，[x_BC y_BC]为所述直线BC在所述臂架坐标系下的坐标，l₂的取值范围为0～所述直线BC的长度，∠CBA表示以铰接点B为顶点的直线BA和直线BC之间的夹角。其中：

∠CAB＝β+γ-∈ (8)

通过式(8)和式(9)，可得：

更进一步地，描述所述吊载物摆动轨迹的所述第三方程为：

其中，[x_h y_h]为所述吊载物在所述臂架坐标系下的坐标，r为可通过式(5)得到的所述吊载物的摇摆半径，σ为所述吊载物的摇摆角度，一般取步骤S4，调节起重机大臂变幅角度。

在此，该起重机大臂变幅角度表现为所述大臂与水平方向之间的夹角γ。针对步骤S4，举例而言，基于S3步骤中直线AD、直线BC和吊载物摇摆轨迹在臂架坐标系中的三个方程，调节起重机大臂与水平方向的夹角γ，使得三个方程没有交点，从而即使吊载物在起重机行驶过程中发生摆动，也可以在一定程度上保证吊载物不与起重机大臂以及液压缸发生碰撞。其中，调节夹角γ的约束条件主要为如下所示：

其中，γ_min和γ_max分别为在所述工程机械不发生倾翻的情况下的所述大臂与水平方向的可调最小夹角和可调最大夹角，z_g和z_s分别是吊钩和车身相对于地面的高度，且参考上文，z_s可从使用第二RTK传感器测量的车身三维空间坐标[x_s y_s z_s]中得到，而z_g可从使用第一RTK传感器测量的吊钩三维空间坐标[x_g y_g z_g]中得到。

步骤S5，执行防倾翻控制策略，包括约束前后轮转向、确定最小转弯半径、确定直线行驶最小道路宽度等。

举例而言，对应上述步骤S300，由于起重机负载在车子后方，为减少由于起重机吊载重物运动加减速度过大且前轮同时转向而导致的车身倾翻风险，使用定位仪将前轮在转向至0度处，然后进行固定，使得前轮不能转向。进一步针对后轮，根据专家知识库将后轮转向角度α进行限制，一般设置为α≤10°。然后，根据车身长L和宽W计算起重机最小转弯半径R，然后计算直线行驶最小道路宽度wid，如下面的公式所示：

wid＝W+2Δw (14)

其中ΔR为转弯安全车距，Δw为直线安全车距。

步骤S6，进行路径规划。

举例而言，在步骤S2起重机上装旋转至0度基础上，同时结合步骤S1中安装的第二RTK传感器，分别记录起重机的起点位置坐标[x_s y_s z_s]和终点空间位置坐标[x_e y_e z_e]，然后基于车身的前后左右的激光雷达扫描周围环境和障碍物，将高度差超过Δh的凹凸不平地面归类为障碍物，然后基于扫描数据进行起重机起点[x_s y_s z_s]到终点[x_e y_e z_e]的路径规划。其中，规划路径的约束为：转弯半径大于式(13)中的最小转弯半径R，直行宽度大于或等于式(14)中的直线行驶最小道路宽度wid，同时尽量避开障碍物。其中Δh的计算方法如下所示：

其中ε调节范围为0<ε≤5°，一般吊载物越重，吊载物高度越高，ε越小。

步骤S7，适应于规划的路径进行行驶速度控制。

举例而言，根据起重机运动专家库，设置起重机底盘行驶最大加减速度为a_max，该最大加减速度表示最大加速度和最大减速度，两者相等，故而可用最大加减速度统一表示。然后，根据步骤S6中的规划路径计算起始最大行驶速度v_s和停止最大行驶速度v_e，最后计算整段路径中的行驶速度v，如下所示：

v_s＝a_maxt_s (18)

v_e＝a_maxt_e (19)

v＝min(v_s v_e) (20)

其中S_s和S_e分别为规划的起始加速路径的长度和停止减速路径的长度。

步骤S8，针对规划的路径，执行分阶段行驶控制。

举例而言，基于步骤S6中规划的路径，将路径分为若干个直线和圆弧段，每个直线段和圆弧段均将起重机的行驶分为“加速启动”、“正常行驶”和“减速停止”三个阶段，其中在“正常行驶”阶段，以步骤S7所确定的行驶速度v进行行驶。另外，在经过圆弧段时，根据当前规划圆弧半径γ_t，将后轮转向角度先转成相应的角度α_t，然后行驶。其中α_t计算方式如下所示：

步骤S9，适应于所划分的行驶阶段，进行力矩控制。

举例而言，基于S8中的行驶方式和S6中的规划路径，设置起重机底盘行驶为“力矩模式”，由第二RTK传感器反馈当前车身行驶速度v_t和当前位置信息，然后计算当前加速度α_t，如下所示：

其中Δt为RTK传感器刷新时间周期，v_l为RTK上一个时间周期Δt测量的行驶速度。

当起重机刚刚启动时，位于“加速启动”阶段，由控制器输出底盘基准力矩m_base，然后每隔0.5s，控制其在上一个间隔力矩基础上加上一个固定持续力矩m_t，若当前加速度a_t>a_max，则控制其在上一个间隔力矩基础上减去一个固定持续力矩m_t，直至达到S7中的行驶速度v或行驶到规划起始加速路径S_s的终点，并记录此时的控制器的输出力矩为m_s。

当经过“加速启动”阶段后，进入“正常行驶”阶段，基于前一阶段的输出力矩m_s，该阶段主要使用PID的方法控制输出至底盘的控制力矩能够使得车辆保持行驶速度v，并记录此时的控制器的输出力矩为m_m：

m_m＝m_s+k₁(v_t-v)+k₂(2v_t-v-v_l) (23)

其中k₁，k₂分为相应的比例系数。

当经过“正常行驶”阶段，进入“减速停止”阶段，基于前一阶段的输出力矩m_m，每隔0.5s，控制其在上一个间隔力矩基础上减去一个固定持续力矩m_t，若当前减速度a_dt>a_max，则控制其在上一个间隔力矩基础上加上一个固定持续力矩m_t，直至速度降为0或行驶到规划停止减速路径S_e。

步骤S9，重复行驶，直到运动至规划终点。

举例而言，重复上述在路径规划之后执行的行驶速度控制、分阶段控制和力矩控制，直至起重机在无人驾驶的情况下行驶至规划的路径终点[x_e y_e z_e]。

因此，通过该示例，可知本申请实施例的安全控制方法应用于起重机无人行驶时，至少具有以下方面的优势：

1)提供了整机美观且不干涉起重机运动的传感器布局，降低了后续进行起重机建模的成本，在其他示例中，还能利用该传感器布局来动态反馈起重机的运动姿态。

2)提供了在起重机吊载重物情况下，利用例如臂架坐标系确定大臂变幅角度的方案，降低了在起重机吊载重物行驶过程中，由于吊载物晃动而导致的其与大臂和液压缸碰撞的风险。

3)提供了在起重机吊载重物情况下，限制底盘前后轮转向和/或规划路径的方案，可避开在凹凸不平的地面上行驶，降低了起重机吊载重物行驶过程中倾翻的风险。

4)提供了在起重机吊载重物情况下，将路径分为若干个直线段和圆弧段，又针对每个直线段和圆弧段将起重机分为“加速启动”、“正常行驶”和“减速停止”三个阶段行驶的方法，减少了吊载物摇摆的幅度。

5)提供了一种二维的臂架坐标系，有利于减少控制方案的复杂度，易于实现。

图6示意性示出了根据本申请实施例的一种工程机械的安全控制装置的结构框图。如图6所示，该安全控制装置可以包括：存储器，被配置成存储指令；以及处理器，被配置成从所述存储器调用所述指令以及在执行所述指令时能够实现上述的安全控制方法。

其中，所述安全控制装置可以利用常规的具有计算能力和数据传输能力的控制器来实现，例如起重机自带的控制器。

处理器中包含内核，由内核去存储器中调取相应的程序单元。内核可以设置一个或以上，通过调整内核参数来实现本申请实施例涉及的安全控制方法。

存储器可以包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。存储器可以包括至少一个存储芯片。

本申请实施例还提供一种工程机械，其包括上述的安全控制装置。该工程机械可以是无人驾驶工程机械，例如上述的无人驾驶起重机。或者，工程机械还可以是强夯机等等。优选地，该工程机械还可以包括如图2所示出的传感器布局。

本申请实施例还提供一种机器可读存储介质，该机器可读存储介质上存储有指令，该指令用于使得机器执行上述的安全控制方法。

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

本申请是参照根据本申请实施例的方法、设备(系统)、计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

存储器可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。存储器是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

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

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

Claims (14)

1.一种工程机械的安全控制方法，其特征在于，包括：

获取基于预设坐标系形成的以下方程：

第一方程，用于描述大臂与车身的铰接点A相对于所述大臂与钢丝绳的铰接点D之间的直线AD；

第二方程，用于描述液压缸与车身的铰接点B相对于该液压缸与所述大臂的铰接点C之间的直线BC；以及

第三方程，用于描述吊载物摆动轨迹；

调节所述大臂与水平方向之间的夹角γ，以满足所述第一方程与所述第三方程之间、所述第二方程和所述第三方程之间均无交点。

2.根据权利要求1所述的安全控制方法，其特征在于，所述预设坐标系是车身坐标系或臂架坐标系，其中所述臂架坐标系被配置为在臂架无回转状态下分别以所述铰接点A、车身平行方向和车身垂直方向为原点、X轴和Y轴。

3.根据权利要求2所述的安全控制方法，其特征在于，针对所述臂架坐标系，描述所述直线AD的所述第一方程为：

其中，[x_AD y_AD]为所述直线AD在所述臂架坐标系下的坐标，l₁的取值范围为0～lz，而lz为大臂长度，θ为所述大臂与所述X轴的夹角。

4.根据权利要求2所述的安全控制方法，其特征在于，针对所述臂架坐标系，描述所述直线BC的所述第二方程为：

其中，[x_BC y_BC]为所述直线BC在所述臂架坐标系下的坐标，[x_B y_B]为铰接点B在所述臂架坐标系下的坐标，l₂的取值范围为0～所述直线BC的长度，∠CBA表示以铰接点B为顶点的直线BA和直线BC之间的夹角。

5.根据权利要求2所述的安全控制方法，其特征在于，针对所述臂架坐标系，描述所述吊载物摆动轨迹的所述第三方程为：

其中，[x_h y_h]为所述吊载物在所述臂架坐标系下的坐标，[x_D y_D]为铰接点D在所述臂架坐标系下的坐标，r为所述吊载物的摇摆半径，σ为所述吊载物的摇摆角度。

6.根据权利要求1所述的安全控制方法，其特征在于，针对所述夹角γ的所述调节基于以下的约束条件被执行：

其中，γ_min和γ_max分别为在所述工程机械不发生倾翻的情况下的所述大臂与水平方向的可调最小夹角和可调最大夹角，h是所述吊载物的高度，z_g和z_s分别是吊钩和车身相对于地面的高度。

7.根据权利要求1所述的安全控制方法，其特征在于，该安全控制方法还包括：

规划行驶路径，针对所述行驶路径的所述规划基于以下至少一条约束条件被执行：

控制前轮不能转向，以及限制后轮的转向角度；

所述工程机械的转弯半径大于最小转弯半径；

所述工程机械的直行宽度大于或等于直线行驶最小道路宽度；

避让障碍物，所述障碍物包括根据针对车身周围环境的扫描数据确定的高度差超过预设值的路面。

8.根据权利要求7所述的安全控制方法，其特征在于，该安全控制方法还包括：

将所规划的行驶路径划分为若干直线段和圆弧段；以及

针对所划分的每一直线段和圆弧段，控制所述工程机械依次行驶于加速启动阶段、正常行驶阶段和减速停止阶段，并针对每一阶段进行行驶控制和/或力矩控制。

9.根据权利要求8所述的安全控制方法，其特征在于，进行所述行驶控制包括：

针对所述圆弧段，先将所述工程机械的后轮转向角度调整为角度α_t，再控制所述工程机械行驶；

其中，所述角度α_t通过下式被确定：

其中，γ_t为当前圆弧段的圆弧半径，L为车身长度。

10.根据权利要求8所述的安全控制方法，其特征在于，进行所述力矩控制包括：

在加速启动阶段，顺次执行如下的力矩控制步骤：初始控制所述工程机械的底盘维持基准力矩；每隔预定时间间隔，控制所述底盘在上一个间隔对应的力矩的基础上增加第一预设力矩；以及在所述工程机械的当前加速度大于预设最大加减速度时，控制所述底盘在上一个间隔对应的力矩的基础上减去所述第一预设力矩，直到达到规划的行驶速度或者行驶到起始加速路径的终点；

在正常行驶阶段，先控制所述底盘维持所述加速启动阶段的输出力矩，再调整该输出力矩以使得所述工程机械保持所述行驶速度；

在减速停止阶段，顺次执行如下的力矩控制步骤：初始控制所述底盘维持所述正常行驶阶段的输出力矩；每隔预定时间间隔，控制所述底盘在上一个间隔对应的力矩的基础上减去第二预设力矩；以及在所述工程机械的当前加速度大于所述预设最大加减速度时，控制所述底盘在上一个间隔对应的力矩的基础上增加所述第二预设力矩，直到所述工程机械的速度降到0或行驶至所述停止减速路径的终点。

11.一种工程机械的安全控制装置，其特征在于，包括：

存储器，被配置成存储指令；以及

处理器，被配置成从所述存储器调用所述指令以及在执行所述指令时能够实现根据权利要求1至10中任一项所述的安全控制方法。

12.一种工程机械，其特征在于，包括权利要求11的安全控制装置。

13.根据权利要求12所述的工程机械，其特征在于，该工程机械还包括：

分别安装在所述工程机械的吊钩和车身上的第一RTK传感器和第二RTK传感器，其中所述第一RTK传感器用于测量吊钩三维空间坐标，所述第二RTK传感器用于测量车身三维空间坐标；

分别安装在所述工程机械的底盘的前侧、后侧、左侧和右侧的激光雷达，用于获取关于相应区域环境的扫描数据；以及

安装在所述工程机械的大臂上的倾角传感器和若干拉线传感器，其中所述倾角传感器用于测量所述工程机械的大臂与水平方向的夹角γ，以及所述拉线传感器用于测量大臂长度。

14.一种机器可读存储介质，其特征在于，该机器可读存储介质上存储有指令，该指令用于使得机器执行根据权利要求1至10中任一项所述的安全控制方法。