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

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

Info

Publication number
CN103697891B
CN103697891B CN201310684412.7A CN201310684412A CN103697891B CN 103697891 B CN103697891 B CN 103697891B CN 201310684412 A CN201310684412 A CN 201310684412A CN 103697891 B CN103697891 B CN 103697891B
Authority
CN
China
Prior art keywords
engineering machinery
admission
path
physical model
coordinate
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201310684412.7A
Other languages
English (en)
Other versions
CN103697891A (zh
Inventor
郭芙
范卿
黄文华
胡玉茹
段军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan Zoomlion Intelligent Technology Co ltd
Zoomlion Heavy Industry Science and Technology Co Ltd
Original Assignee
Hunan Zoomlion Intelligent Technology Co ltd
Zoomlion Heavy Industry Science and Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hunan Zoomlion Intelligent Technology Co ltd, Zoomlion Heavy Industry Science and Technology Co Ltd filed Critical Hunan Zoomlion Intelligent Technology Co ltd
Priority to CN201310684412.7A priority Critical patent/CN103697891B/zh
Publication of CN103697891A publication Critical patent/CN103697891A/zh
Application granted granted Critical
Publication of CN103697891B publication Critical patent/CN103697891B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Processing Or Creating Images (AREA)

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(xa，ya)、B(xb，yb)、C(xc，yc)、D(xd，yd)分别为物体投影于平面中的四个顶点的坐标，(x0，y0)为物体的中心点坐标，Len、Wid分别为物体的长、宽，为物体的方位角。在确定物体的四个顶点的投影坐标后，既可以确定物体在作业场景中所占据的空间。
如此，控制器122根据作业场景信息按照上述矩形包围盒的方式来进行物体信息的转换，生成物体四个顶点的投影坐标，并从而生成场景三维图和二维平面投影图。应该注意，本领域技术人员可以使用任何合适算法来进行物体信息的转换以实现本发明的技术方案。
因为场景中的物体坐标是基于某个原点的坐标系(XOY)，但是对于不同的触摸屏，由于屏幕大小存在区别，屏幕以左上角为原点(0,0)。因此为了使得显示更加美观并适合观察，需要将作业场景中的物体尺寸和坐标进行坐标变换。假设屏幕坐标系为(XOY)，原场景坐标系为(X′OY′)，而物体在作业场景中的坐标为(x0，y0)，则可以根据方程(2)将作业场景中的坐标转化为屏幕坐标。
其中，X、Y为X0、Y0在坐标系(XOY)中的坐标，X′、Y′为X0、Y0在坐标系(X′OY′)中的坐标，SIZE为屏幕尺寸。
工程机械物理模型创建
在某些情况下，在进行入场路径规划前，需要选择能够满足作业要求的工程机械，如型号、工况等。根据作业场景信息(吊装物信息，如吊装物大小、吊装起点、目标点、吊重等)，由操作人员输入或由系统自动选择满足吊装的工程机械的型号和工况，控制器122根据工程机械型号进行尺寸变换和组装以创建工程机械物理模型。
而且，在进行入场路径规划前，需要对工程机械物理模型的初始位置、方位角θ、行走步长Δstep进行设置，并初始化驱动建，如驱动激活键、停止及结束键等。
入场路径生成
在自动生成的三维作业场景中，通过鼠标拖拽或遥控手柄控制的方式，驱动工程机械物理模型运动。操作者可以选择以“驾驶员视角”或者以“旁观者视角”控制工程机械物理模型行走，以呈现真实操作的感觉。在驱动工程机械物理模型时，光标所在位置代表工程机械物理模型的回转中心点。
工程机械物理模型在行走过程中，存在直线行走和转弯两种模式。根据这两种情况来计算工程机械物理模型的回转中心点的轨迹线。
在入场路径生成时，在工程机械物理模型直线行走的过程中，控制器122获取工程机械物理模型的回转中心点的轨迹点坐标。将这些轨迹点坐标和方位角按照先后顺序存储在向量Vector[(x0,y0,θ0),(x1,y1,θ1),(x2,y2,θ2)…..(xn,yn,θn)]中。当工程机械物理模型的坐标位置与吊装目标之间的距离满足设定阈值时，表明工程机械物理模型已经到达站位点，将轨迹点记载为(xi,yi,θi,k)，xi、yi表示第i点的xy坐标，θi方位角，且当直线行走时k为1，转弯时k为0，之后可以对已规划的入场路径进行解析。
入场路径解析
入场路径解析可以包括直线路径解析、转弯路径解析、和/或碰撞解析，以判断所述入场路径是否满足所述工程机械实际行走要求。以下将通过具体实施方式分别对其进行详细描述。
首先将进行直线路径解析的详细描述。
计算每两个相邻轨迹点之间的距离ΔL，及如果ΔL≤Δstep，则删除后一个轨迹点，并更新所述向量，确保所述向量内的每两个相邻轨迹点之间的距离大于所述行走步长Δstep。
例如，轨迹点(x0,y0,θ0),(x1,y1,θ0)，因为是直线行走，所以轨迹点的方位角相同，根据方程(3)计算二者之间的距离：
如果ΔL≤Δstep，则删除轨迹点(x1,y1,θ0)，并将向量更新为Vector[(x0,y0,θ0),(x2,y2,θ0)…..(xn,yn,θ0)]，否则将轨迹点(x1,y1,θ0)添加到向量Vector中。之后，进行下一个轨迹点的判断。最终，更新后的工程机械物理模型的轨迹点为NewVector[(x0,y0,θ0),(xi,yi,θ0),(xi+1,yi+1,θ0)…(xi+m,yi+m,θ0)]。应注意，该直线轨迹点的确定也可以在入场路径生成后，在解析过程中进行。
下面将进行转弯路径解析的详细描述。
工程机械在转弯时，采用第一个轮胎转弯角为整车转弯角。根据设定的步长将工程机械物理模型的入场路径的一转弯路径划分为多条线段；计算第一条线段和最后一条线段的方位角θ1和θ2(可以沿工程机械物理模型的行走方向确定第一条线段和最后一条线段)；计算转弯角将转弯角与工程机械物理模型的最大转弯角相比较；及如果转弯角大于最大转弯角，则确定入场路径不满足工程机械实际行走要求并重新规划入场路径。
例如，图4中的一转弯路径，根据设定的步长，将该转弯路径划分为三个线段，并获得l1(x1,y1)、l2(x2,y2)、l3(x1′,y1′)和l4(x2′,y2′)四个轨迹点。分别 计算l1l2、l3l4线段的方位角。根据方程(4)计算出方位角的绝对值θ1′和θ2′。
例如，对于方位角θ1，根据方程(5)、(6)、(7)、(8)判断方位角位于哪个象限，并确定θ1的实际值。
同样，可以计算方位角θ2，工程机械物理模型的转弯角如果转弯角大于该工程机械的最大转弯角，则确定所述入场路径不满足所述工程机械实际行走要求并重新规划入场路径。同样，本领域技术人员根据上述描述可以通过依据最小转弯半径来实现转弯计算。
以下将通过具体实施方式对碰撞检测进行详细描述。
根据AABB算法初步检测工程机械物理模型附近是否有障碍物。如图5所示，A代表障碍物，B代表工程机械物理模型。通过AABB包围盒算法已经检测出障碍物A的包围盒与工程机械物理模型B的包围盒相交(AABB包围盒算法为已知算法，因此在此省略详细描述)，但是在工程机械物理模型的行走方向上，障碍物A并没有对工程机械物理模型B造成威胁。因此，本发明提供一种以工程机械车身的边缘是否与障碍物发生碰撞为检测原则的方法，来进一步判断障碍物与工程机械物理模型是否会发生碰撞的碰撞检测，以确定障碍物是否会对工程机械物理模型的前进造成影响。
已知工程机械物理模型B的长、宽为(lb,wb)，回转中心点的轨迹点坐标为(xb,yb)，与X轴坐标的夹角(方位角)为θb；障碍物A的长、宽为(la，wa)，中心点坐标为(xa,ya)，与X轴坐标的夹角为θa。
将障碍物A中心的X坐标xa代入到计算沿工程机械物理模型中心线的直线L的方程(9)中，得到y′a。
y＝tanθb*x+(yb-tanθb*xb) (9)
将y′a与障碍物A的中心点的Y坐标ya进行比较；如果ya＞y′a，则确定障碍物A在工程机械物理模型B的上方，如果ya＜y′a，则确定障碍物A在工程机械物理模型B的下方。
由于工程机械在实际中车身较大，因此不能简单的将其简化为以回转中心为代表的一个点，在计算过程中，应以工程机械物理模型B的边缘是否与障碍物A发生碰撞为检测原则。因此，本发明给出如下进一步判断碰撞的方法。
在上述过程中，如果确定障碍物A在工程机械物理模型B的上方，则通过方程(10)计算d′。
如果确定障碍物A在工程机械物理模型B的下方，则通过方程(11)计算d′。
其中，d＝wb+Δ，Δ为常数。
然后，将障碍物A的四个顶点的X坐标xa1、xa2、xa3、xa4及d′分别代入工程机械边缘线L’的方程(12)中，分别得出y′a1、y′a2、y′a3、y′a4，然后将y′a1、y′a2、y′a3、y′a4分别与所述障碍物四个顶点的Y坐标ya1、ya2、ya3、ya4进行比较。
y＝tanθb*x+d' (12)
在障碍物A在工程机械物理模型B上方的情况下，如果满足((ya1＜y'a1)||(ya2＜y'a2)||(ya3＜y'a3)||(ya4＜y'a4))，则表明障碍物A能与行走的工程机械的边缘发生碰撞，如此则确定入场路径不满足工程机械物理模型B的实际行走要求，障碍物A对工程机械物理模型B的行驶构成威胁，此时工程机械物理模型B应该绕开障碍物A，因此需要重新规划入场路径；在障碍物A在工程机械物理模型B下方的情况下，如果满足((ya1＞y'a1)||(ya2＞y'a2)||(ya3＞y'a3)||(ya4＞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 (22)

1.一种工程机械入场路径规划装置，其特征在于，该装置包括：
接收器，接收作业场景信息、所述工程机械信息和行走指令；以及
控制器，用于：
根据所述作业场景信息创建作业场景；
根据所述工程机械信息创建工程机械物理模型；
根据所述行走指令控制所述工程机械物理模型在所述作业场景中行走，以生成入场路径；
解析所述入场路径，并在该入场路径不满足所述工程机械实际行走要求的情况下，重新生成入场路径，直至所生成的入场路径满足所述工程机械实际行走要求后，保存该入场路径以待所述工程机械执行实际行走；
所述控制器根据所述作业场景信息采用矩形包围盒的方式来确定物体在所述作业场景中所占据的空间，以用于创建所述作业场景；
所述作业场景信息包括：所述作业场景中物体的中心点坐标、方位角、长度、宽度，所述矩形包围盒的方式通过以下公式计算所述物体的四个顶点投影在所述作业场景中的坐标A(xa,ya)、B(xb,yb)、C(xc,yc)、D(xd,yd)：
A ( x a , y a ) x a = x 0 - ( 1 / 2 c o s ∂ * L e n ) - ( 1 / 2 s i n ∂ * W i d ) y a = y 0 - ( 1 / 2 s i n ∂ * L e n ) + ( 1 / 2 cos ∂ * W i d )
B ( x b , y b ) x b = x 0 + ( 1 / 2 c o s ∂ * L e n ) - ( 1 / 2 s i n ∂ * W i d ) y b = y 0 + ( 1 / 2 s i n ∂ * L e n ) + ( 1 / 2 cos ∂ * W i d )
C ( x c , y c ) x c = x 0 + ( 1 / 2 c o s ∂ * L e n ) + ( 1 / 2 s i n ∂ * W i d ) y c = y 0 + ( 1 / 2 s i n ∂ * L e n ) - ( 1 / 2 cos ∂ * W i d )
D ( x d , y d ) x d = x 0 - ( 1 / 2 c o s ∂ * L e n ) + ( 1 / 2 s i n ∂ * W i d ) y d = y 0 - ( 1 / 2 s i n ∂ * L e n ) - ( 1 / 2 cos ∂ * W i d )
其中，(x0,y0)为所述物体的中心点坐标，Len和Wid分别为所述物体的长度和宽度，为所述物体的方位角。
2.根据权利要求1所述的工程机械入场路径规划装置，其特征在于，在生成所述入场路径过程中，当所述工程机械物理模型的坐标位置与作业目标之间的距离满足设定阈值时，所述控制器确定所述工程机械物理模型已经到达站位点，已完成所述入场路径的生成。
3.根据权利要求1所述的工程机械入场路径规划装置，其特征在于，在生成所述入场路径的过程中，所述控制器具体用于：
获取所述工程机械物理模型的回转中心点的轨迹点坐标；
按照轨迹点的先后顺序将各个轨迹点坐标和方位角存储在向量Vector[(x0,y0,θ0),(x1,y1,θ1),(x2,y2,θ2)…..(xn,yn,θn)]中，以形成所述入场路径。
4.根据权利要求3所述的工程机械入场路径规划装置，其特征在于，所述入场路径解析包括直线路径解析、转弯路径解析、和/或碰撞解析。
5.根据权利要求4所述的工程机械入场路径规划装置，其特征在于，所述工程机械信息包括所述工程机械物理模型的行走步长Δstep，在所述直线路径解析的过程中，所述控制器具体用于：
计算每两个相邻轨迹点之间的距离ΔL；及
如果ΔL≤Δstep，确定该直线路径不满足所述工程机械实际行走要求。
6.根据权利要求5所述的工程机械入场路径规划装置，其特征在于，在所述直线路径不满足所述工程机械实际行走要求后，删除后一个轨迹点，以形成新的入场路径。
7.根据权利要求4所述的工程机械入场路径规划装置，其特征在于，在转弯路径解析过程中，所述控制器具体用于：
计算所述入场路径中的转弯角；以及
将该转弯角与所述工程机械的最大转弯角相比较，并在所述转弯角大于所述最大转弯角的情况下，则确定所述入场路径不满足所述工程机械实际行走要求。
8.根据权利要求7所述的工程机械入场路径规划装置，其特征在于，所述工程机械信息包括所述工程机械物理模型的行走步长Δstep，在计算所述入场路径中的转弯角的过程中，所述控制器还用于执行以下步骤：
根据所述步长Δstep将所述入场路径的一转弯路径划分为多条线段；
根据第一条线段的两个端点的坐标来计算其方位角θ1并根据最后一条线段的两个端点的坐标来计算其方位角θ2；以及
计算转弯角
9.根据权利要求4所述的工程机械入场路径规划装置，其特征在于，在所述碰撞解析过程中，所述控制器具体用于：
使用AABB碰撞检测方法来判断物体的AABB包围盒是否与所述工程机械的AABB包围盒相交；
在所述物体的AABB包围盒与所述工程机械的AABB包围盒相交的情况下，判断所述工程机械的边缘是否与所述物体发生碰撞；以及
在所述工程机械的边缘与所述物体发生碰撞的情况下，则确定所述入场路径不满足所述工程机械实际行走要求。
10.根据权利要求9所述的工程机械入场路径规划装置，其特征在于，所述工程机械信息包括所述工程机械物理模型的宽度，判断所述工程机械的边缘是否与所述物体发生碰撞包括：
将所述物体的中心点的X坐标xa代入所述工程机械物理模型中心线直线方程y＝tanθb*x+(yb-tanθb*xb)，得到y′a，并将y′a与所述物体的中心点的Y坐标ya进行比较，如果ya＞y′a，则确定所述物体在所述工程机械物理模型的上方，如果ya＜y′a，则确定所述物体在所述工程机械物理模型的下方；
将所述物体的四个顶点的X坐标xa1、xa2、xa3、xa4分别代入所述工程机械边缘线的直线方程y＝tanθb*x+d'中，分别得出y′a1、y′a2、y′a3、y′a4，然后将y′a1、y′a2、y′a3、y′a4分别与所述物体四个顶点的Y坐标ya1、ya2、ya3、ya4进行比较；
在所述物体在所述工程机械物理模型上方的情况下，如果满足((ya1＜y'a1)||(ya2＜y'a2)||(ya3＜y'a3)||(ya4＜y'a4))，则确定所述工程机械的边缘与所述物体会发生碰撞；以及
在所述物体在所述工程机械物理模型下方的情况下，如果满足((ya1＞y'a1)||(ya2＞y'a2)||(ya3＞y'a3)||(ya4＞y'a4))，则确定所述工程机械的边缘与所述物体会发生碰撞，
其中xb、yb、θb为此时所述工程机械物理模型的回转中心点的轨迹点坐标及方位角，如果ya＞y′a，如果ya＜y′a，d＝wb+Δ，wb为所述工程机械物理模型的宽度，Δ为常数。
11.一种工程机械入场路径规划系统，其特征在于，该系统包括：
权利要求1-10中任一项权利要求所述的工程机械入场路径规划装置；
输入装置，与所述接收器连接，用于输入作业场景信息、所述工程机械信息和行走指令。
12.一种工程机械，其特征在于，该工程机械包括权利要求11所述的工程机械入场路径规划系统。
13.一种工程机械入场路径规划方法，其特征在于，该方法包括：
接收作业场景信息、所述工程机械信息和行走指令；
根据所述作业场景信息创建作业场景；
根据所述工程机械信息创建工程机械物理模型；
根据所述行走指令控制所述工程机械物理模型在所述作业场景中行走，以生成入场路径；
解析所述入场路径，并在该入场路径不满足所述工程机械实际行走要求的情况下，重新生成入场路径，直至所生成的入场路径满足所述工程机械实际行走要求后，保存该入场路径以待所述工程机械执行实际行走；
根据所述作业场景信息采用矩形包围盒的方式来确定物体在所述作业场景中所占据的空间，以用于创建所述作业场景；
所述作业场景信息包括：所述作业场景中物体的中心点坐标、方位角、长度、宽度，所述矩形包围盒的方式通过以下公式计算所述物体的四个顶点投影在所述作业场景中的坐标A(xa,ya)、B(xb,yb)、C(xc,yc)、D(xd,yd)：
A ( x a , y a ) x a = x 0 - ( 1 / 2 c o s ∂ * L e n ) - ( 1 / 2 s i n ∂ * W i d ) y a = y 0 - ( 1 / 2 s i n ∂ * L e n ) + ( 1 / 2 cos ∂ * W i d )
B ( x b , y b ) x b = x 0 + ( 1 / 2 c o s ∂ * L e n ) - ( 1 / 2 s i n ∂ * W i d ) y b = y 0 + ( 1 / 2 s i n ∂ * L e n ) + ( 1 / 2 cos ∂ * W i d )
C ( x c , y c ) x c = x 0 + ( 1 / 2 c o s ∂ * L e n ) + ( 1 / 2 s i n ∂ * W i d ) y c = y 0 + ( 1 / 2 s i n ∂ * L e n ) - ( 1 / 2 cos ∂ * W i d )
D ( x d , y d ) x d = x 0 - ( 1 / 2 c o s ∂ * L e n ) + ( 1 / 2 s i n ∂ * W i d ) y d = y 0 - ( 1 / 2 s i n ∂ * L e n ) - ( 1 / 2 cos ∂ * W i d )
其中，(x0,y0)为所述物体的中心点坐标，Len和Wid分别为所述物体的长度和宽度，为所述物体的方位角。
14.根据权利要求13所述的工程机械入场路径规划方法，其特征在于，所述方法还包括：在生成所述入场路径过程中，当所述工程机械物理模型的坐标位置与作业目标之间的距离满足设定阈值时，确定所述工程机械物理模型已经到达站位点，已完成所述入场路径的生成。
15.根据权利要求13所述的工程机械入场路径规划方法，其特征在于，在生成所述入场路径过程中，所述方法还包括：
获取所述工程机械物理模型的回转中心点的轨迹点坐标；
按照轨迹点的先后顺序将各个轨迹点坐标和方位角存储在向量Vector[(x0,y0,θ0),(x1,y1,θ0),(x2,y2,θ0)…..(xn,yn,θ0)]中，以形成所述入场路径。
16.根据权利要求15所述的工程机械入场路径规划方法，其特征在于，所述入场路径解析包括直线路径解析、转弯路径解析、和/或碰撞解析。
17.根据权利要求16所述的工程机械入场路径规划方法，其特征在于，所述工程机械信息包括所述工程机械物理模型的行走步长Δstep，所述直线路径解析包括：
计算每两个相邻轨迹点之间的距离ΔL；及
如果ΔL≤Δstep，确定该直线路径不满足所述工程机械实际行走要求。
18.根据权利要求17所述的工程机械入场路径规划方法，其特征在于，在所述直线路径不满足所述工程机械实际行走要求后，删除后一个轨迹点，以形成新的入场路径。
19.根据权利要求16所述的工程机械入场路径规划方法，其特征在于，所述转弯路径解析包括：
计算所述入场路径中的转弯角；以及
将该转弯角与所述工程机械的最大转弯角相比较，并在所述转弯角大于所述最大转弯角的情况下，则确定所述入场路径不满足所述工程机械实际行走要求并重新规划入场路径。
20.根据权利要求19所述的工程机械入场路径规划方法，其特征在于，所述工程机械信息包括所述工程机械物理模型的行走步长Δstep，计算所述入场路径中的转弯角包括：
根据所述步长Δstep将所述入场路径的一转弯路径划分为多条线段；
根据第一条线段的两个端点的坐标来计算其方位角θ1并根据最后一条线段的两个端点的坐标来计算其方位角θ2；以及
计算转弯角
21.根据权利要求16所述的工程机械入场路径规划方法，其特征在于，所述碰撞解析包括：
使用AABB碰撞检测方法来判断物体的AABB包围盒是否与所述工程机械的AABB包围盒相交；
在所述物体的AABB包围盒与所述工程机械的AABB包围盒相交的情况下，判断所述工程机械的边缘是否与所述物体发生碰撞；以及
在所述工程机械的边缘与所述物体发生碰撞的情况下，则确定所述入场路径不满足所述工程机械实际行走要求并重新规划入场路径。
22.根据权利要求21所述的工程机械入场路径规划方法，其特征在于，所述工程机械信息包括所述工程机械物理模型的宽度，判断所述工程机械的边缘是否与所述物体发生碰撞包括：
将所述物体的中心点的X坐标xa代入所述工程机械物理模型中心线的直线方程y＝tanθb*x+(yb-tanθb*xb)，得到y′a，并将y′a与所述物体的中心点的Y坐标ya进行比较，如果ya＞y′a，则确定所述物体在所述工程机械物理模型的上方，如果ya＜y′a，则确定所述物体在所述工程机械物理模型的下方；
将所述物体的四个顶点的X坐标xa1、xa2、xa3、xa4分别代入所述工程机械边缘线的直线方程y＝tanθb*x+d'中，分别得出y′a1、y′a2、y′a3、y′a4，然后将y′a1、y′a2、y′a3、y′a4分别与所述物体四个顶点的Y坐标ya1、ya2、ya3、ya4进行比较；
在所述物体在所述工程机械物理模型上方的情况下，如果满足((ya1＜y'a1)||(ya2＜y'a2)||(ya3＜y'a3)||(ya4＜y'a4))，则确定所述工程机械的边缘与所述物体会发生碰撞；以及
在所述物体在所述工程机械物理模型下方的情况下，如果满足((ya1＞y'a1)||(ya2＞y'a2)||(ya3＞y'a3)||(ya4＞y'a4))，则确定所述工程机械的边缘与所述物体会发生碰撞，
其中xb、yb、θb为此时所述工程机械物理模型回转中心点的轨迹点坐标及方位角，如果ya＞y′a，如果ya＜y′a，d＝wb+Δ，wb为所述工程机械的宽，Δ为常数。
CN201310684412.7A 2013-12-13 2013-12-13 一种工程机械及其入场路径规划装置、系统及方法 Active CN103697891B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201310684412.7A CN103697891B (zh) 2013-12-13 2013-12-13 一种工程机械及其入场路径规划装置、系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201310684412.7A CN103697891B (zh) 2013-12-13 2013-12-13 一种工程机械及其入场路径规划装置、系统及方法

Publications (2)

Publication Number Publication Date
CN103697891A CN103697891A (zh) 2014-04-02
CN103697891B true CN103697891B (zh) 2017-05-31

Family

ID=50359503

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201310684412.7A Active CN103697891B (zh) 2013-12-13 2013-12-13 一种工程机械及其入场路径规划装置、系统及方法

Country Status (1)

Country Link
CN (1) CN103697891B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107206592B (zh) * 2015-01-26 2021-03-26 杜克大学 专用机器人运动规划硬件及其制造和使用方法
CN107188042B (zh) * 2017-06-21 2018-12-18 欧浦智网股份有限公司 一种用于货物装卸的地面坐标绘制方法及装置
CN107861909A (zh) * 2017-10-24 2018-03-30 唐山钢铁集团有限责任公司 一种基于矩形相交模型算法的天车作业路径计算方法
CN110530390A (zh) * 2019-09-16 2019-12-03 哈尔滨工程大学 一种在狭窄环境下的非质点车辆路径规划方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101441736A (zh) * 2007-11-21 2009-05-27 新乡市起重机厂有限公司 汽车起重机机器人路径规划方法
CN102663194A (zh) * 2012-04-16 2012-09-12 三一重型装备有限公司 一种掘进机联合仿真方法及其模型
CN102663196A (zh) * 2012-04-17 2012-09-12 中南大学 一种基于虚拟现实的汽车起重机吊装仿真方法
CN103241654A (zh) * 2013-04-03 2013-08-14 大连理工大学 一种桥式起重机智能运行控制系统

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8144245B2 (en) * 2007-02-28 2012-03-27 Caterpillar Inc. Method of determining a machine operation using virtual imaging
TWI444939B (zh) * 2008-01-10 2014-07-11 Univ Nat Taiwan 工程吊車之模擬系統及其方法
KR101155602B1 (ko) * 2010-04-12 2012-06-19 현대중공업 주식회사 선박용 충돌회피 시스템을 위한 항해 장비 시뮬레이터
CN102779215A (zh) * 2012-07-11 2012-11-14 中南大学 基于b/s架构和缓存技术的网络化三维吊装仿真方法
CN103017757B (zh) * 2012-12-06 2015-05-20 中联重科股份有限公司 工程机械入场路径规划方法和路径规划装置

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101441736A (zh) * 2007-11-21 2009-05-27 新乡市起重机厂有限公司 汽车起重机机器人路径规划方法
CN102663194A (zh) * 2012-04-16 2012-09-12 三一重型装备有限公司 一种掘进机联合仿真方法及其模型
CN102663196A (zh) * 2012-04-17 2012-09-12 中南大学 一种基于虚拟现实的汽车起重机吊装仿真方法
CN103241654A (zh) * 2013-04-03 2013-08-14 大连理工大学 一种桥式起重机智能运行控制系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于OGRE的起重机选型与站位仿真研究;李晓燕;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》;20100715(第7期);正文第53-56页 *
移动式起重机无碰撞路径规划的设计与实现;张玉院;《中国优秀硕士学位论文全文数据库·工程科技Ⅱ辑》;20110515;第C029-94页 *

Also Published As

Publication number Publication date
CN103697891A (zh) 2014-04-02

Similar Documents

Publication Publication Date Title
US11009884B2 (en) Method for calculating nominal vehicle paths for lanes within a geographic region
US20210132615A1 (en) Systems and methods for optimizing route planning for tight turns for robotic apparatuses
US10445442B2 (en) System and method for game theory-based design of robotic systems
US8190295B1 (en) Apparatus and method for modifying the operation of a robotic vehicle in a real environment, to emulate the operation of the robotic vehicle operating in a mixed reality environment
Sezer et al. A novel obstacle avoidance algorithm:“Follow the Gap Method”
CN106843236A (zh) 基于权重改进粒子群算法的无人自行车路径规划方法
CN103697891B (zh) 一种工程机械及其入场路径规划装置、系统及方法
CN106873599A (zh) 基于蚁群算法和极坐标变换的无人自行车路径规划方法
JP7130062B2 (ja) 経路決定方法
US10139794B2 (en) Method for determining a movement limit
Gan et al. Real-time decentralized search with inter-agent collision avoidance
Zhu et al. A hierarchical deep reinforcement learning framework with high efficiency and generalization for fast and safe navigation
Valenzuela-Urrutia et al. Virtual reality-based time-delayed haptic teleoperation using point cloud data
Zhou et al. Optimal mission planner with timed temporal logic constraints
Alcalá et al. Comparison of two non-linear model-based control strategies for autonomous vehicles
Chen et al. Dynamic obstacle avoidance for UAVs using a fast trajectory planning approach
Vílez et al. Trajectory generation and tracking using the AR. Drone 2.0 quadcopter UAV
Graf et al. Improving the prediction accuracy of predictive displays for teleoperated autonomous vehicles
Lim et al. Three-dimensional (3D) dynamic obstacle perception in a detect-and-avoid framework for unmanned aerial vehicles
Nguyen et al. A proactive trajectory planning algorithm for autonomous mobile robots in dynamic social environments
Kim et al. Shared teleoperation of a vehicle with a virtual driving interface
Wilburn et al. Implementation of a 3-Dimensional dubins-based UAV path generation algorithm
JPWO2020136977A1 (ja) 経路決定装置、ロボット及び経路決定方法
Alonso-Mora Collaborative motion planning for multi-agent systems
Bipin et al. Autonomous navigation of generic quadrocopter with minimum time trajectory planning and control

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
ASS Succession or assignment of patent right

Owner name: HUNAN ZOOMLION INTELLIGENT TECHNOLOGY CO., LTD.

Free format text: FORMER OWNER: ZOOMLION HEAVY INDUSTRY SCIENCE + TECHNOLOGY DEVELOPMENT CO., LTD.

Effective date: 20150805

Owner name: ZOOMLION HEAVY INDUSTRY SCIENCE + TECHNOLOGY DEVEL

Effective date: 20150805

C41 Transfer of patent application or patent right or utility model
TA01 Transfer of patent application right

Effective date of registration: 20150805

Address after: 410205, Changsha central power software park headquarters building, 39, pointed mountain road, Changsha hi tech Development Zone, Hunan, A152

Applicant after: HUNAN ZOOMLION INTELLIGENT TECHNOLOGY Co.,Ltd.

Applicant after: ZOOMLION HEAVY INDUSTRY SCIENCE&TECHNOLOGY Co.,Ltd.

Address before: 410013 Yuelu District, Hunan, silver basin Road, No. 361, No.

Applicant before: ZOOMLION HEAVY INDUSTRY SCIENCE&TECHNOLOGY Co.,Ltd.

GR01 Patent grant
GR01 Patent grant