CN103697891A - 一种工程机械及其入场路径规划装置、系统及方法 - Google Patents

Abstract

本发明公开了一种工程机械及其入场路径规划装置、系统及方法，其中，该装置包括：接收器，接收作业场景信息、所述工程机械信息和行走指令；以及控制器，用于：根据所述作业场景信息创建作业场景；根据所述工程机械信息创建工程机械物理模型；根据所述行走指令控制所述工程机械物理模型在所述作业场景中行走，以生成入场路径；解析所述入场路径，并在该入场路径不满足所述工程机械实际行走要求的情况下，重新生成入场路径，直至所生成的入场路径满足所述工程机械实际行走要求后，保存该入场路径，从而能够减少工作量并提高路径的可行性。

Description

一种工程机械及其入场路径规划装置、系统及方法

技术领域

本发明涉及工程机械领域，具体地，涉及一种工程机械入场路径规划装置、系统及方法。

背景技术

工程机械在建筑、装卸等作业中有着极为广泛的应用。由于工程机械的体积较大、作业环境较复杂，因此，在将工程机械运送至工位（例如将起重机行驶至吊装物前）之前，需要对工程机械的入场路径进行规划，以避免运送期间被阻断或出现事故。

目前，在工程机械的作业过程中，一般由工作人员根据实地的场景，通过手动来规划工程机械的入场路径并对转弯角及特殊路径进行人为计算以判断可行性。然而，在上述路径规划过程中，需要耗费大量的人力计算时间来进行路径规划，而且，在计算过程中存在因人为因素而造成的误差，从而降低了规划路径的可行性，进而导致作业效率降低，甚至引起安全事故的发生。

发明内容

本发明的目的是提供一种工程机械入场路径规划装置、系统及方法，该装置、系统及方法能够减少工作量并提高路径的可行性。

为了实现上述目的，本发明提供一种工程机械入场路径规划装置，该装置包括：接收器，接收作业场景信息、所述工程机械信息和行走指令；以及控制器，用于：根据所述作业场景信息创建作业场景；根据所述工程机械信息创建工程机械物理模型；根据所述行走指令控制所述工程机械物理模型在所述作业场景中行走，以生成入场路径；解析所述入场路径，并在该入场路径不满足所述工程机械实际行走要求的情况下，重新生成入场路径，直至所生成的入场路径满足所述工程机械实际行走要求后，保存该入场路径。

本发明还提供一种工程机械入场路径规划系统，该系统包括：上述工程机械入场路径规划装置；输入装置，与所述接收器连接，用于输入作业场景信息、所述工程机械信息和行走指令。

此外，本发明还提供一种工程机械，该工程机械包括上述工程机械入场路径规划系统。

本发明通过控制工程机械的物理模型在创建的作业场景中行走，来生成入场路径，然后对该入场路径进行解析，在满足实际行走要求的情况下，保存该入场路径，从而能够在减少工作量的同时提高入场路径的可行性。

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

附图说明

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

图1是根据本发明的一种工程机械入场路径规划系统的框图；

图2是根据本发明一种实施方式的工程机械路径规划系统的结构图；

图3是根据本发明实施方式对作业场景中物体进行处理的示意图；

图4是根据本发明实施方式生成的入场路径的以转弯路径示意图；

图5是根据本发明实施方式的进行碰撞检测的障碍物A的包围盒与工程机械的包围盒的示意图；以及

图6是根据本发明实施方式的一种工程机械入场路径规划方法的流程图。

附图标记说明

100      路径规划系统                    110      输入装置

111      第一触摸屏                      112      遥控手柄

120      路径规划装置                    121      接收器

122      控制器                          130      输出装置

131      第二触摸屏

具体实施方式

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

图1是根据本发明的一种工程机械入场路径规划系统的框图。如图1所示，该工程机械入场路径规划系统100可以包括输入装置100以及路径规划装置110，并且可选择的还可以包括输出装置130（由图1中虚线框表示）。

通过图1可以看出，工程机械入场路径规划装置120可以包括：接收器121，该接收器121与输入装置110连接，用于接收来自输入装置100的作业场景信息、工程机械信息和行走指令；以及控制器122，该控制器122用于：根据作业场景信息创建作业场景；根据工程机械信息创建工程机械物理模型；根据行走指令控制工程机械物理模型在作业场景中行走，以生成入场路径；解析入场路径，并在该入场路径满足工程机械实际行走要求的情况下，保存该入场路径。

图2是根据本发明一种实施方式的工程机械路径规划系统的结构图。该实施方式可以是针对工程机械（如起重机）在大型吊装工程中进行入场路径规划的，如图2所示，输入装置110可以包括第一触摸屏111和遥控手柄112。其中第一触摸屏111可以用于如作业场景信息、工程机械信息等相关参数的设置，遥控手柄112可以用于发送用于驱动工程机械物理模型的行走指令。接收器121将接收到的上述信息传输至控制器122中，控制器122可以进行操作指令解析，进行路径规划，驱动三维模型运行，然后对路径解析，并将经解析的合适路径输出至输出装置130（图2中为第二触摸屏131），同时还可以将模型的运动通过第二触摸屏131来进行显示，以便于操作者进行相应操作。但本发明并不限于上述具体部件，本领域技术人员可以应用任何合适的部件来实施本发明的技术方案，如可以通过鼠标来实现遥控手柄112的功能，也可以通过同一个触摸屏来实现第一触摸屏111和第二触摸屏131的功能。

在一种实施方式中，工程机械入场路径规划系统100可以通过场景创建、工程机械物理模型创建、入场路径生成以及入场路径解析并存储来实现入场路径规划，以下将对上述几个部分进行详细描述。

场景创建

根据作业现场的施工图纸（如CAD图），进行信息数据简化和换算，得出作业现场中所有物体（包含障碍物、吊装物）相对于某个点（设置为原点）的基本信息，例如，物体的中心点坐标、方位角、长、宽、高等。由于作业现场中存在的物体并不是单一、有规则的几何体，因此可以采用矩形包围盒方式来对物体进行处理。如果作业对精度要求较高，可以将不规则的物体分解成多个小物体的叠加，然后在采用矩形包围盒进行处理。

图3根据本发明实施方式对作业场景中物体进行处理的示意图。可以根据以下方程（1）来确定物体的四个顶点的坐标：

其中，图3中的A（x_a，y_a）、B（x_b，y_b）、C（x_c，y_c）、D（x_d，y_d）分别为物体投影于平面中的四个顶点的坐标，（x₀，y₀）为物体的中心点坐标，Len、Wid分别为物体的长、宽，

为物体的方位角。在确定物体的四个顶点的投影坐标后，既可以确定物体在作业场景中所占据的空间。

如此，控制器122根据作业场景信息按照上述矩形包围盒的方式来进行物体信息的转换，生成物体四个顶点的投影坐标，并从而生成场景三维图和二维平面投影图。应该注意，本领域技术人员可以使用任何合适算法来进行物体信息的转换以实现本发明的技术方案。

因为场景中的物体坐标是基于某个原点的坐标系（XOY），但是对于不同的触摸屏，由于屏幕大小存在区别，屏幕以左上角为原点（0,0）。因此为了使得显示更加美观并适合观察，需要将作业场景中的物体尺寸和坐标进行坐标变换。假设屏幕坐标系为（XOY），原场景坐标系为（X′OY′），而物体在作业场景中的坐标为（x₀，y₀），则可以根据方程（2）将作业场景中的坐标转化为屏幕坐标。

$\left\{\begin{array}{c}X=(X^{\′}-X_0)*\mathrm{SIZE}\\ Y=(-Y^{\′}+Y_0)*\mathrm{SIZE}\end{array}\right.---\left(2\right)$

其中，X、Y为X₀、Y₀在坐标系（XOY）中的坐标，X′、Y′为X₀、Y₀在坐标系（X′OY′）中的坐标，SIZE为屏幕尺寸。

工程机械物理模型创建

在某些情况下，在进行入场路径规划前，需要选择能够满足作业要求的工程机械，如型号、工况等。根据作业场景信息（吊装物信息，如吊装物大小、吊装起点、目标点、吊重等），由操作人员输入或由系统自动选择选择满足吊装的工程机械的型号和工况，控制器122根据工程机械型号进行尺寸变换和组装以创建工程机械物理模型。

而且，在进行入场路径规划前，需要对工程机械物理模型的初始位置、方位角θ、行走步长Δstep进行设置，并初始化驱动建，如驱动激活键、停止及结束键等。

入场路径生成

在自动生成的三维作业场景中，通过鼠标拖拽或遥控手柄控制的方式，驱动工程机械物理模型运动。操作者可以选择以“驾驶员视角”或者以“旁观者视角”控制工程机械物理模型行走，以呈现真实操作的感觉。在驱动工程机械物理模型时，光标所在位置代表工程机械物理模型的回转中心点。

工程机械物理模型在行走过程中，存在直线行走和转弯两种模式。根据这两种情况来计算工程机械物理模型的回转中心点的轨迹线。

在入场路径生成时，在工程机械物理模型直线行走的过程中，控制器122获取工程机械物理模型的回转中心点的轨迹点坐标。将这些轨迹点坐标和方位角按照先后顺序存储在向量Vector[(x₀,y₀,θ₀),(x₁,y₁,θ₁),(x₂,y₂,θ₂).....(x_n,y_n,θ_n)]中。当工程机械物理模型的坐标位置与吊装目标之间的距离满足设定阈值时，表明工程机械物理模型已经到达站位点，将轨迹点记载为(x_i,y_i,θ_i,k)，x_i、y_i表示第i点的xy坐标，θ_i方位角，且当直线行走时k为1，转弯时k为0，之后可以对已规划的入场路径进行解析。

入场路径解析

入场路径解析可以包括直线路径解析、转弯路径解析、和/或碰撞解析，以判断所述入场路径是否满足所述工程机械实际行走要求。以下将通过具体实施方式分别对其进行详细描述。

首先将进行直线路径解析的详细描述。

计算每两个相邻轨迹点之间的距离ΔL，及如果ΔL≤Δstep，则删除后一个轨迹点，并更新所述向量，确保所述向量内的每两个相邻轨迹点之间的距离大于所述行走步长Δstep。

例如，轨迹点(x₀,y₀,θ₀),(x₁,y₁,θ₀)，因为是直线行走，所以轨迹点的方位角相同，根据方程（3）计算二者之间的距离：

$\mathrm{\ΔL}=\sqrt{{(x_1-x_0)}^2+{(y_1-y_0)}^2}---\left(3\right)$

如果ΔL≤Δstep，则删除轨迹点(x₁,y₁,θ₀)，并将向量更新为Vector[(x₀,y₀,θ₀),(x₂,y₂,θ₀).....(x_n,y_n,θ₀)]，否则将轨迹点(x₁,y₁,θ₀)添加到向量Vector中。之后，进行下一个轨迹点的判断。最终，更新后的工程机械物理模型的轨迹点为NewVector[(x₀,y₀,θ₀),(x_i,y_i,θ₀),(x_i+1,y_i+1,θ₀).....(x_i+m,y_i+m,θ₀)]。应注意，该直线轨迹点的确定也可以在入场路径生成后，在解析过程中进行。

下面将进行转弯路径解析的详细描述。

工程机械在转弯时，采用第一个轮胎转弯角为整车转弯角。根据设定的步长将工程机械物理模型的入场路径的一转弯路径划分为多条线段；计算第一条线段和最后一条线段的方位角θ₁和θ₂（可以沿工程机械物理模型的行走方向确定第一条线段和最后一条线段）；计算转弯角

将转弯角与工程机械物理模型的最大转弯角相比较；及如果转弯角大于最大转弯角，则确定入场路径不满足工程机械实际行走要求并重新规划入场路径。

例如，图4中的一转弯路径，根据设定的步长，将该转弯路径划分为三个线段，并获得l₁(x₁,y₁)、l₂(x₂,y₂)、l₃(x₁′,y₁′)和l₄(x₂′,y₂′)四个轨迹点。分别计算l₁l₂、l₃l₄线段的方位角。根据方程（4）计算出方位角的绝对值θ₁′和θ₂′。

θ₁'＝tan^-1（|(y₂-y₁)/(x₂-x₁)|）

                                （4）

θ₂'＝tan^-1（|(y₂'-y₁')/(x₂'-x₁')|）

例如，对于方位角θ₁，根据方程（5）、（6）、（7）、（8）判断方位角位于哪个象限，并确定θ₁的实际值。

$\left\{\begin{array}{c}(y_2-y_1)>0\\ (x_2-x_1)>0\end{array}\right.$ θ₁=θ₁′；（第一象限）    （5）

$\left\{\begin{array}{c}(y_2-y_1)>0\\ (x_2-x_1)<0\end{array}\right.$ θ₁=π－θ₁′；（第二象限）    （6）

$\left\{\begin{array}{c}(y_2-y_1)<0\\ (x_2-x_1)<0\end{array}\right.$ θ₁=θ₁′－π；（第三象限）    （7）

$\left\{\begin{array}{c}(y_1-y_2)<0\\ (x_2-x_1)>0\end{array}\right.$ θ₁=－θ₁′（第四象限）    （8）

同样，可以计算方位角θ₂，工程机械物理模型的转弯角如果转弯角大于该工程机械的最大转弯角，则确定所述入场路径不满足所述工程机械实际行走要求并重新规划入场路径。同样，本领域技术人员根据上述描述可以通过依据最小转弯半径来实现转弯计算。

以下将通过具体实施方式对碰撞检测进行详细描述。

根据AABB算法初步检测工程机械物理模型附近是否有障碍物。如图5所示，A代表障碍物，B代表工程机械物理模型。通过AABB包围盒算法已经检测出障碍物A的包围盒与工程机械物理模型B的包围盒相交（AABB包围盒算法为已知算法，因此在此省略详细描述），但是在工程机械物理模型的行走方向上，障碍物A并没有对工程机械物理模型B造成威胁。因此，本发明提供一种以工程机械车身的边缘是否与障碍物发生碰撞为检测原则的方法，来进一步判断障碍物与工程机械物理模型是否会发生碰撞的碰撞检测，以确定障碍物是否会对工程机械物理模型的前进造成影响。

已知工程机械物理模型B的长、宽为(l_b,w_b)，回转中心点的轨迹点坐标为(x_b,y_b)，与X轴坐标的夹角（方位角）为θ_b；障碍物A的长、宽为(l_a w_a)，中心点坐标为(x_a,y_a)，与X轴坐标的夹角为θ_a。

将障碍物A中心的X坐标x_a代入到计算沿工程机械物理模型中心线的直线L的方程（9）中，得到y′_a。

y＝tanθ_b*x+（y_b-tanθ_b*x_b）    （9）

将y′_a与障碍物A的中心点的Y坐标y_a进行比较；如果y_a＞y′_a，则确定障碍物A在工程机械物理模型B的上方，如果y_a＜y′_a，则确定障碍物A在工程机械物理模型B的下方。

由于工程机械在实际中车身较大，因此不能简单的将其简化为以回转中心为代表的一个点，在计算过程中，应以工程机械物理模型B的边缘是否与障碍物A发生碰撞为检测原则。因此，本发明给出如下进一步判断碰撞的方法。

在上述过程中，如果确定障碍物A在工程机械物理模型B的上方，则通过方程（10）计算d′。

$d^{\&prime;}=(y_b-\tan {\mathrm{\&theta;}}_b*x_b+|\frac{d}{\cos \mathrm{\&theta;}}\left|\right)---\left(10\right)$

如果确定障碍物A在工程机械物理模型B的下方，则通过方程（11）计算d′。

$d^{\&prime;}=(y_b-{\tan \mathrm{\&theta;}}_b*x_b-|\frac{d}{\cos {\mathrm{\&theta;}}_b}\left|\right)---\left(11\right)$

其中，d=w_b+Δ，Δ为常数。

然后，将障碍物A的四个顶点的X坐标x_a1、x_a2、x_a3、x_a4及d′分别代入工程机械边缘线L’的方程（12）中，分别得出y′_a1、y′_a2、y′_a3、y′_a4，然后将y′_a1、y′_a2、y′_a3、y′_a4分别与所述障碍物四个顶点的Y坐标y_a1、y_a2、y_a3、y_a4进行比较。

y＝tanθ_b*x+d'    （12）

在障碍物A在工程机械物理模型B上方的情况下，如果满足((y_a1＜y'_a1)||(y_a2＜y'_a2)||(y_a3＜y'_a3)||(y_a4＜y'_a4))，则表明障碍物A能与行走的工程机械的边缘发生碰撞，如此则确定入场路径不满足工程机械物理模型B的实际行走要求，障碍物A对工程机械物理模型B的行驶构成威胁，此时工程机械物理模型B应该绕开障碍物A，因此需要重新规划入场路径；在障碍物A在工程机械物理模型B下方的情况下，如果满足((y_a1＞y'_a1)||(y_a2＞y'_a2)||(y_a3＞y'_a3)||(y_a4＞y'_a4))，则表明障碍物A能与行走的工程机械的边缘发生碰撞，如此则确定入场路径不满足工程机械的实际行走要求，障碍物A对工程机械B的行驶构成威胁，此时工程机械物理模型B应该绕开障碍物A，因此需要重新规划入场路径。

在确定入场路径符合工程机械实际要求的情况下，保存该入场路径，并可以通过输出装置（如图2中的第二触摸屏131）来输出该路径参数。

应该注意，尽管在上述实施方式中，直线路径解析、转弯路径解析和碰撞解析是在入场路径生成之后进行的，但是，也可以在生成入场路径的同时执行，如，在直线行走时进行直线路径解析，在发生转弯时进行转弯计算，在存在障碍物时进行碰撞检测。

图6示出了根据本发明的一种工程机械入场路径规划方法的流程图。如图6所示，在步骤601处，接收器121接收来相关参数，如作业场景信息、工程机械信息和行走指令。在步骤602处，控制器122根据作业场景信息创建作业场景。在步骤603处，控制器122根据工程机械信息创建工程机械物理模型。在步骤604处，控制器122根据行走指令控制工程机械物理模型在作业场景中行走，以生成入场路径。在步骤605处，控制器122解析入场路径，判断该入场路径是否满足工程机械实际行走要求；如果满足，则在步骤606处保存该入场路径。如果不满足，则返回至步骤604，以便控制器122根据输入装置的指令重新进行路径规划。

其中，在生成路径的过程中或在解析过程中，控制器122可以执行直线路径解析、转弯路径解析和碰撞解析，这已经在上文中进行了详细描述，于此不再赘述。

此外，本发明还提供一种工程机械，该工程机械包括上述工程机械入场路径规划系统。

通过本发明所提的工程机械入场路径规划装置、系统和方法，利用创建的作业场景和工程机械物理模型，使得操作者能够直观形象地观察入场路径创建过程中可能出现的情况，并且利用计算机及图形学通过转弯计算、碰撞检测等安全计算，减少了人工计算所产生的误差和工作量，从而提高了入场路径的可行性及作业效率。

以上结合附图详细描述了本发明的优选实施方式，但是，本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种简单变型，这些简单变型均属于本发明的保护范围。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合，为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。

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

Claims (26)

1.一种工程机械入场路径规划装置，其特征在于，该装置包括：

接收器，接收作业场景信息、所述工程机械信息和行走指令；以及

控制器，用于：

根据所述作业场景信息创建作业场景；

根据所述工程机械信息创建工程机械物理模型；

根据所述行走指令控制所述工程机械物理模型在所述作业场景中行走，以生成入场路径；

解析所述入场路径，并在该入场路径不满足所述工程机械实际行走要求的情况下，重新生成入场路径，直至所生成的入场路径满足所述工程机械实际行走要求后，保存该入场路径。

2.根据权利要求1所述的工程机械入场路径规划装置，其特征在于，所述控制器根据所述作业场景信息采用矩形包围盒的方式来确定物体在所述作业场景中所占据的空间，以用于创建所述作业场景。

3.根据权利要求2所述的工程机械入场路径规划装置，其特征在于，所述作业场景信息包括：所述作业场景中物体的中心点坐标、方位角、长度、宽度，所述矩形包围盒的方式通过以下公式计算所述物体的四个顶点投影在所述作业场景中的坐标A(x_a,y_a)、B(x_a,y_a)、C(x_a,y_a)、D(x_a,y_a)：

$A(x_a,y_a)\left\{\begin{array}{c}{x}_a=x_0-(1/2\cos \&PartialD;*\mathrm{Len})-(1/2\sin \&PartialD;*\mathrm{Wid})\\ y_a=y_0-(1/2\sin \&PartialD;*\mathrm{Len})+(1/2\cos \&PartialD;*\mathrm{Wid})\end{array}\right.$ $B(x_b,y_b)\left\{\begin{array}{c}{x}_b=x_0+(1/2\cos \&PartialD;*\mathrm{Len})-(1/2\sin \&PartialD;*\mathrm{Wid})\\ y_b=y_0+(1/2\sin \&PartialD;*\mathrm{Len})+(1/2\cos \&PartialD;*\mathrm{Wid})\end{array}\right.$

$C(x_c,y_c)\left\{\begin{array}{c}{x}_c=x_0+(1/2\cos \&PartialD;*\mathrm{Len})+(1/2\sin \&PartialD;*\mathrm{Wid})\\ y_c=y_0+(1/2\sin \&PartialD;*\mathrm{Len})-(1/2\cos \&PartialD;*\mathrm{Wid})\end{array}\right.$

$D(x_d,y_d)\left\{\begin{array}{c}{x}_d=x_0-(1/2\cos \&PartialD;*\mathrm{Len})+(1/2\sin \&PartialD;*\mathrm{Wid})\\ y_d=y_0-(1/2\sin \&PartialD;*\mathrm{Len})-(1/2\cos \&PartialD;*\mathrm{Wid})\end{array}\right.$

其中，(x₀,y₀)为所述物体的中心点坐标，Len和Wid分别为所述物体的长度和宽度，

为所述物体的方位角。

4.根据权利要求1所述的工程机械入场路径规划装置，其特征在于，在生成所述入场路径过程中，当所述工程机械物理模型的坐标位置与作业目标之间的距离满足设定阈值时，所述控制器确定所述工程机械物理模型已经到达站位点，已完成所述入场路径的生成。

5.根据权利要求3所述的工程机械入场路径规划装置，其特征在于，在生成所述入场路径的过程中，所述控制器具体用于：

获取所述工程机械物理模型的回转中心点的轨迹点坐标；

按照轨迹点的先后顺序将各个轨迹点坐标和方位角存储在向量Vector[(x₀,y₀,θ₀),(x₁,y₁,θ₁),(x₂,y₂,θ₂).....(x_n,y_n,θ_n)]中，以形成所述入场路径。

6.根据权利要求5所述的工程机械入场路径规划装置，其特征在于，所述入场路径解析包括直线路径解析、转弯路径解析、和/或碰撞解析。

7.根据权利要求6所述的工程机械入场路径规划装置，其特征在于，所述工程机械信息包括所述工程机械物理模型的行走步长Δstep，在所述直线路径解析的过程中，所述控制器具体用于：

计算每两个相邻轨迹点之间的距离ΔL；及

如果ΔL≤Δstep，确定该直线路径不满足所述工程机械实际行走要求。

8.根据权利要求7所述的工程机械入场路径规划装置，其特征在于，在所述直线路径不满足所述工程机械实际行走要求后，删除后一个轨迹点，以形成新的入场路径。

9.根据权利要求6所述的工程机械入场路径规划装置，其特征在于，在转弯路径解析过程中，所述控制器具体用于：

计算所述入场路径中的转弯角；以及

将该转弯角与所述工程机械的最大转弯角相比较，并在所述转弯角大于所述最大转弯角的情况下，则确定所述入场路径不满足所述工程机械实际行走要求。

10.根据权利要求9所述的工程机械入场路径规划装置，其特征在于，所述工程机械信息包括所述工程机械物理模型的行走步长Δstep，在计算所述入场路径中的转弯角的过程中，所述控制器还用于执行以下步骤：

根据所述步长Δstep将所述入场路径的一转弯路径划分为多条线段；

根据第一条线段的两个端点的坐标来计算其方位角θ₁并根据最后一条线段的两个端点的坐标来计算其方位角θ₂；以及

计算转弯角

11.根据权利要求6所述的工程机械入场路径规划装置，其特征在于，在所述碰撞解析过程中，所述控制器具体用于：

使用AABB碰撞检测方法来判断物体的AABB包围盒是否与所述工程机械的AABB包围盒相交；

在所述物体的AABB包围盒与所述工程机械的AABB包围盒相交的情况下，判断所述工程机械的边缘是否与所述物体发生碰撞；以及

在所述工程机械的边缘与所述物体发生碰撞的情况下，则确定所述入场路径不满足所述工程机械实际行走要求。

12.根据权利要求11所述的工程机械入场路径规划装置，其特征在于，所述工程机械信息包括所述工程机械物理模型的宽度，判断所述工程机械的边缘是否与所述物体发生碰撞包括：

将所述物体的中心点的X坐标x_a代入所述工程机械物理模型中心线直线方程y＝tanθ_b*x+(y_b-tanθ_b*x_b)，得到y′_a，并将y′_a与所述物体的中心点的Y坐标y_a进行比较，如果y_a＞y′_a，则确定所述物体在所述工程机械物理模型的上方，如果y_a＜y′_a，则确定所述物体在所述工程机械物理模型的下方；

将所述物体的四个顶点的X坐标x_a1、x_a2、x_a3、x_a4分别代入所述工程机械边缘线的直线方程y＝tanθ_b*x+d'中，分别得出y′_a1、y′_a2、y′_a3、y′_a4，然后将y′_a1、y′_a2、y′_a3、y′_a4分别与所述物体四个顶点的Y坐标y_a1、y_a2、y_a3、y_a4进行比较；

在所述物体在所述工程机械物理模型上方的情况下，如果满足((y_a1＜y'_a1)||(y_a2＜y'_a2)||(y_a3＜y'_a3)||(y_a4＜y'_a4))，则确定所述工程机械的边缘与所述物体会发生碰撞；以及

在所述物体在所述工程机械物理模型下方的情况下，如果满足((y_a1＞y'_a1)||(y_a2＞y'_a2)||(y_a3＞y'_a3)||(y_a4＞y'_a4))，则确定所述工程机械的边缘与所述物体会发生碰撞，

其中x_b、y_b、θ_b为此时所述工程机械物理模型的回转中心点的轨迹点坐标及方位角，如果y_a＞y′_a，

如果y_a＜y′_a，

d=w_b+Δ，w_b为所述工程机械物理模型的宽度，Δ为常数。

13.一种工程机械入场路径规划系统，其特征在于，该系统包括：

权利要求1-12中任一项权利要求所述的工程机械入场路径规划装置；

输入装置，与所述接收器连接，用于输入作业场景信息、所述工程机械信息和行走指令。

14.一种工程机械，其特征在于，该工程机械包括权利要求13所述的工程机械入场路径规划系统。

15.一种工程机械入场路径规划方法，其特征在于，该方法包括：

接收作业场景信息、所述工程机械信息和行走指令；

根据所述作业场景信息创建作业场景；

根据所述工程机械信息创建工程机械物理模型；

根据所述行走指令控制所述工程机械物理模型在所述作业场景中行走，以生成入场路径；

解析所述入场路径，并在该入场路径不满足所述工程机械实际行走要求的情况下，重新生成入场路径，直至所生成的入场路径满足所述工程机械实际行走要求后，保存该入场路径。

16.根据权利要求15所述的工程机械入场路径规划方法，其特征在于，根据所述作业场景信息采用矩形包围盒的方式来确定物体在所述作业场景中所占据的空间，以用于创建所述作业场景。

17.根据权利要求16所述的工程机械入场路径规划方法，其特征在于，所述作业场景信息包括：所述作业场景中物体的中心点坐标、方位角、长度、宽度，所述矩形包围盒的方式通过以下公式计算所述物体的四个顶点投影在所述作业场景中的坐标A(x_a,y_a)、B(x_a,y_a)、C(x_a,y_a)、D(x_a,y_a)：

$A(x_a,y_a)\left\{\begin{array}{c}{x}_a=x_0-(1/2\cos \&PartialD;*\mathrm{Len})-(1/2\sin \&PartialD;*\mathrm{Wid})\\ y_a=y_0-(1/2\sin \&PartialD;*\mathrm{Len})+(1/2\cos \&PartialD;*\mathrm{Wid})\end{array}\right.$ $B(x_b,y_b)\left\{\begin{array}{c}{x}_b=x_0+(1/2\cos \&PartialD;*\mathrm{Len})-(1/2\sin \&PartialD;*\mathrm{Wid})\\ y_b=y_0+(1/2\sin \&PartialD;*\mathrm{Len})+(1/2\cos \&PartialD;*\mathrm{Wid})\end{array}\right.$

$C(x_c,y_c)\left\{\begin{array}{c}{x}_c=x_0+(1/2\cos \&PartialD;*\mathrm{Len})+(1/2\sin \&PartialD;*\mathrm{Wid})\\ y_c=y_0+(1/2\sin \&PartialD;*\mathrm{Len})-(1/2\cos \&PartialD;*\mathrm{Wid})\end{array}\right.$

$D(x_d,y_d)\left\{\begin{array}{c}{x}_d=x_0-(1/2\cos \&PartialD;*\mathrm{Len})+(1/2\sin \&PartialD;*\mathrm{Wid})\\ y_d=y_0-(1/2\sin \&PartialD;*\mathrm{Len})-(1/2\cos \&PartialD;*\mathrm{Wid})\end{array}\right.$

其中，(x₀,y₀)为所述物体的中心点坐标，Len和Wid分别为所述物体的长度和宽度，为所述物体的方位角。

18.根据权利要求15所述的工程机械入场路径规划方法，其特征在于，所述方法还包括：在生成所述入场路径过程中，当所述工程机械物理模型的坐标位置与作业目标之间的距离满足设定阈值时，确定所述工程机械物理模型已经到达站位点，已完成所述入场路径的生成。

19.根据权利要求17所述的工程机械入场路径规划方法，其特征在于，在生成所述入场路径过程中，所述方法还包括：

获取所述工程机械物理模型的回转中心点的轨迹点坐标；

按照轨迹点的先后顺序将各个轨迹点坐标和方位角存储在向量Vector[(x₀,y₀,θ₀),(x₁,y₁,θ₀),(x₂,y₂,θ₀).....(x_n,y_n,θ₀)]中，以形成所述入场路径。

20.根据权利要求19所述的工程机械入场路径规划方法，其特征在于，所述入场路径解析包括直线路径解析、转弯路径解析、和/或碰撞解析。

21.根据权利要求20所述的工程机械入场路径规划方法，其特征在于，所述工程机械信息包括所述工程机械物理模型的行走步长Δstep，所述直线路径解析包括：

计算每两个相邻轨迹点之间的距离ΔL；及

如果ΔL≤Δstep，确定该直线路径不满足所述工程机械实际行走要求。

22.根据权利要求21所述的工程机械入场路径规划方法，其特征在于，在所述直线路径不满足所述工程机械实际行走要求后，删除后一个轨迹点，以形成新的入场路径。

23.根据权利要求20所述的工程机械入场路径规划方法，其特征在于，所述转弯路径解析包括：

计算所述入场路径中的转弯角；以及

将该转弯角与所述工程机械的最大转弯角相比较，并在所述转弯角大于所述最大转弯角的情况下，则确定所述入场路径不满足所述工程机械实际行走要求并重新规划入场路径。

24.根据权利要求23所述的工程机械入场路径规划方法，其特征在于，所述工程机械信息包括所述工程机械物理模型的行走步长Δstep，计算所述入场路径中的转弯角包括：

根据所述步长Δstep将所述入场路径的一转弯路径划分为多条线段；

根据第一条线段的两个端点的坐标来计算其方位角θ₁并根据最后一条线段的两个端点的坐标来计算其方位角θ₂；以及

计算转弯角

25.根据权利要求20所述的工程机械入场路径规划方法，其特征在于，所述碰撞解析包括：

使用AABB碰撞检测方法来判断物体的AABB包围盒是否与所述工程机械的AABB包围盒相交；

在所述物体的AABB包围盒与所述工程机械的AABB包围盒相交的情况下，判断所述工程机械的边缘是否与所述物体发生碰撞；以及

在所述工程机械的边缘与所述物体发生碰撞的情况下，则确定所述入场路径不满足所述工程机械实际行走要求并重新规划入场路径。

26.根据权利要求25所述的工程机械入场路径规划方法，其特征在于，所述工程机械信息包括所述工程机械物理模型的宽度，判断所述工程机械的边缘是否与所述物体发生碰撞包括：

将所述物体的中心点的X坐标x_a代入所述工程机械物理模型中心线的直线方程y＝tanθ_b*x+(y_b-tanθ_b*x_b)，得到y′_a，并将y′_a与所述物体的中心点的Y坐标y_a进行比较，如果y_a＞y′_a，则确定所述物体在所述工程机械物理模型的上方，如果y_a＜y′_a，则确定所述物体在所述工程机械物理模型的下方；

将所述物体的四个顶点的X坐标x_a1、x_a2、x_a3、x_a4分别代入所述工程机械边缘线的直线方程y＝tanθ_b*x+d'中，分别得出y′_a1、y′_a2、y′_a3、y′_a4，然后将y′_a1、y′_a2、y′_a3、y′_a4分别与所述物体四个顶点的Y坐标y_a1、y_a2、y_a3、y_a4进行比较；

在所述物体在所述工程机械物理模型上方的情况下，如果满足((y_a1＜y'_a1)||(y_a2＜y'_a2)||(y_a3＜y'_a3)||(y_a4＜y'_a4))，则确定所述工程机械的边缘与所述物体会发生碰撞；以及

在所述物体在所述工程机械物理模型下方的情况下，如果满足((y_a1＞y'_a1)||(y_a2＞y'_a2)||(y_a3＞y'_a3)||(y_a4＞y'_a4))，则确定所述工程机械的边缘与所述物体会发生碰撞，

其中x_b、y_b、θ_b为此时所述工程机械物理模型回转中心点的轨迹点坐标及方位角，如果y_a＞y′_a，如果y_a＜y′_a，

d=w_b+Δ，w_b为所述工程机械的宽，Δ为常数。

Images