CN103278153A - 一种基于空间二维映射的汽车起重机三维路径规划方法 - Google Patents

Abstract

本发明公开了一种基于空间二维映射的汽车起重机三维路径规划方法，针对汽车起重机的三维路径规划问题，将三维空间转化为带有高度信息的二维地图，并在此基础上设计相应搜索算法和回退策略。本发明为汽车起重机的三维路径规划问题提出了一种有效的解决思路，提高了吊装作业的安全性。

Description

一种基于空间二维映射的汽车起重机三维路径规划方法

技术领域

本发明涉及一种汽车起重机三维路径规划方法,特别是一种基于空间二维映射的汽车起重机三维路径规划方法。

背景技术

起重机路径规划作为吊装方案的核心内容，是保证吊装安全的关键，精确有效的路径规划直接影响到操作的安全性和效率。然而，吊装环境的复杂化和设备限制使得人工规划效率和执行精确性都比较低。因此，为了提高吊装效率，节省劳动力，有必要对路径规划进行研究，自动生成一条可执行的优化起重机操作序列。

汽车起重机虽然可以借鉴移动式机器人的路径规划方法，但是也有自己的特点，传统的起重机路径规划方法将规划工作分解为以下两个阶段：

1)在吊装方案的制定阶段，用常规的CAD软件对可能出现危险的离散关键路径点进行校核验证以得到起重机的关键动作；

2)在吊装方案的执行阶段，由资深工程师凭借丰富的经验主观地指挥以补齐离散关键点之间的路径。

但传统方法存在以下问题：

1)由CAD设计软件自身的限制致使关键点的分析和选取不够全面；

2)关键点之间的路径规划依赖于指挥人员的主观判断比较严重，难以保证安全性；

3)整个路径的规划方法不***，难以形成一条优化的完整路径。

通过对各种规划方法的研究分析，对于环境已知情况下的路径规划方法已经非常成熟，且针对起重机的路径规划算法也有较多的研究，但这些路径规划算法的执行时间较长或者所需储存空间大，且没有考虑汽车起重机自身的特点，因此需针对汽车起重机进行相关路径规划算法设计。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种基于空间二维映射的汽车起重机三维路径规划方法，为汽车起重机的三维路径规划问题提出一种有效的解决思路，提高吊装作业的安全性。

为解决上述技术问题，本发明所采用的技术方案是：一种基于空间二维映射的汽车起重机三维路径规划方法，该方法为：

1）对吊装二维地图初始化，确定吊装初始点和目标点位置；确定每个离散点高度的阈值HeightValue为汽车起重机总臂长乘以汽车起重机吊臂的变幅角正弦值；其中离散点高度是指离散点距离地面的高度；

2）将初始点标记为当前点；

3）判断当前点周边4个离散点是否可到达，如何全部不可到达，则判断该点为死点，若是，则进入11）；否则标记出所有可到达点，进入4）；

4）判断当前点所在位置的回转角度θ_起始1和变幅角度θ_起始2以及目标点所在位置的回转角度θ_目标1和变幅角度θ_目标2大小关系，依据下表设置当前点周边4个离散点的选取优先级，其中1代表最高，4代表最低，左回转代表当前点向左转得到的离散点，右回转代表当前当向右转得到的离散点，幅度增加代表当前点往幅值增加方向移动得到的离散点，幅度减小代表当前点往幅值减小方向移动得到的离散点；

5）获取优先级等级为1的离散点，判断该点高度是否超过阈值HeightValue，如果未超过，则将该离散点设置为搜索结果点，并加入已得路径表中，转向8）；否则，进入6）；

6）检查优先级为2的离散点高度，判断该点高度是否超过阈值HeightValue，如果未超过，则将该离散点设置为搜索结果点，并加入已得路径表中，转向8）；否则，检查优先级为3的离散点高度是否超过阈值HeightValue，如果优先级3离散点高度未超过阈值HeightValue，则将该离散点设置为搜索结果点，并加入已得路径表中，转向8）；否则，检查优先级为4的离散点，如果优先级4的离散点高度未超过阈值HeightValue，则将该离散点设置为搜索结果点，并加入已得路径表中，转向8）；否则进入7）；

7）如果优先级为2、3、4的离散点高度均超过所述阈值，则将优先级为1的离散点设置为搜索结果点，并加入已得路径表中；

8）将所述当前点标记为“已在路径中”，将所述搜索结果点标记为新的当前点；

9）判断上述步骤8）中新的当前点是否为目标点，若是，按搜索顺序输出已得路径表中的搜索结果点，转向11）；若否，返回3）；

10）判断新的当前点周围4个离散点的当前状态是否为不可到达或者已经存在于已得路径表中，如果周围4个离散点的状态是以上两种中的任意一种，那么新的当前点周围4个离散点都不能选择，标记该点为死点，回退到已得路径表中新的当前点的上一个离散点上，将新的当前点的上一个离散点标记为当前点，返回3）；

11）结束。

与现有技术相比，本发明所具有的有益效果为：本发明将三维空间转化为带有高度信息的二维地图，并在此基础上设计相应搜索算法；本发明为汽车起重机的三维路径规划问题提出了一种有效的解决思路，提高了吊装作业的安全性。

附图说明

图1为本发明一实施例方法流程图；

图2为本发明一实施例汽车起重机球面坐标示意图；

图3为本发明一实施例汽车起重机极坐标示意图；

图4为本发明一实施例汽车起重机极坐标系离散点示意图；

图5为本发明一实施例路径搜索策略示意图；

图6为本发明一实施例仿真结果示意图。

具体实施方式

如图1所示，为本发明一实施例方法流程图。

汽车起重机三维工作空间描述：

汽车起重机路径规划问题是指在工作环境中，寻找一条从给定位置到目标位置的路径。路径规划算法分为空间划分和搜索算法两部分。针对汽车起重机的动作特点，本文对起重机作业空间进行分析，建立起重机作业空间坐标系，并对空间物体进行描述，将起重机三维作业空间用二维地图进行描述，从而实现三维工作空间划分。

三维空间的坐标系建立：

汽车起重机基本动作主要为起升、回转、变幅以及伸缩，汽车起重机实际吊装过程中，根据实际工艺和操作，设定以下假设：

（1）由于汽车起重机在吊装过程中位置始终保持不变，因此假设臂的长度保持起始状态不变。

（2）由于汽车起重机臂长一旦设定，基本保持不变，因此假设起重机在动作过程中不包含有伸缩动作，只有3个基本动作：起升、回转以及变幅操作。

（3）吊装过程中绳的摆动幅度不大，因此假设吊绳一直保持竖直状态。

根据假设1和2可知，起重机臂端轨迹始终在以臂长为半径的上半球面上，如图2所示，而球面上任意一点坐标如A点坐标为(R*cosθ2*cosθ1,R*cosθ2*sinθ1,R*sinθ2)，其中R为半径，固定不变，那么球面上点的坐标只与变幅角度θ2和回转角度θ1这两个夹角有关，θ1和θ2唯一确定球面点。根据这一特点，将球面上的点垂直映射到xoz平面上，并以球心为坐标中心，建立极坐标系，极坐标系中点的坐标用变幅角度θ2和回转角度θ1表示，如图2.2所示。经过映射和极坐标系建立，那么球面上点就可以用极坐标系中点代替，如图2.2中A，和B，即代表球面上的A和B点。

由于臂端轨迹的可行域是图2中的整个上半球面，在经过垂直映射后，轨迹的可行域为图3中整个圆。因此，在极坐标下，臂端的轨迹可行域可用以下集合表示：

S={X(θ2,θ1)|0°<=θ1<=360°,0°<=θ2<=90°}。

另外，根据假设3可知，被吊物体与臂端处在同一竖直线上，因此，被吊物的x坐标和z坐标与臂端的坐标相同，假设被吊物在图2中A点正下方某一位置M，目标点在B点下方某一位置N，那么，被吊物要从M到N点，臂端位置必然从A点到B点，反映到极坐标系下就是A到B，由此可见，要实现被吊物的三维路径规划，必然要满足臂端位置在极坐标下的路径规划要求。然而，仅仅满足臂端的路径规划是不够的，这是因为被吊物在进行三维路径规划的时候，必然会因为场景中物体高度而产生高度的变化，因此，在解决臂端路径规划的同时，也需要考虑被吊物和场景中物体的高度问题。

根据以上分析，可以将被吊物的路径规划问题分为两个部分，一部分为在极坐标系下二维路径规划问题，另一部分为被吊物的高度变化问题。通过建立极坐标系并对臂端进行垂直映射，解决了第一部分问题，第二部分问题将在下面进行阐述。

坐标系下场景物体的描述：

经过坐标系的建立，虽然将吊装物的三维规划问题转化为极坐标系下的二维路径规划问题，但该规划得到的路径仅包含回转角度和变幅角度信息，体现不了吊装物的高度变化，这是由于极坐标系下可行域中点的坐标只包含回转和变幅角度，缺乏当前位置上空间物体的高度信息，针对这一问题，本文考虑为可行域S中每个点X(θ2,θ1)都加入该点上物体的高度信息，这样，在考虑二维路径规划的同时，可根据当前位置上所加入的高度信息对吊装物进行升降操作，从而解决上述第二部分的问题。

由于可行域S中点全部为连续点，考虑实际吊装过程中，起重机进行回转和变幅操作时，总有一个最小的变化角度，因此，本文考虑将极坐标下的可行区域S根据最小变化角度进行离散化，假设回转最小变化角为α，变幅最小角度为β，那么将区域进行划分如图4所示，其中，如P点到Q点是进行回转操作，P点到M点则是变幅操作。将图中交点作为离散点，那么可行区域S1可用以下集合代替：

S1={X(θ2,θ1)|0°<=θ1<=360°,0o<=θ2<=90°}

θ1=δα，δ为整数

θ2=λβ，λ为整数

根据之前考虑，为可行域S中每个点都加入高度信息，然而在离散化后，整个空间则由离散点集合S1表示，因此，只需在每个离散点中加入当前点所处位置空间物体的高度信息就可达到要求。这样，每个离散点出了包含之前的回转角度和变幅角度信息，还包含了高度信息，该高度信息为当前离散点上物体的最高高度。如场景某物体高度为5，该物体覆盖了3个离散点，则这三个离散点的高度为5，如果当前物体高度超过臂端高度，说明该物体覆盖的离散点为不可到达点。另外，如果离散点上没有空间物体，则该离散点高度为0。通过对极坐标系下的可行域进行离散并加入高度信息，便可得到带有高度信息的二维地图，该地图即可对汽车起重机三维工作空间进行描述。

通过以上分析，利用极坐标系下带有高度参数的二维地图对整个三维空间进行划分和描述，汽车起重机的三维路径规划问题就转化为二维路径规划问题，为之后的搜索算法设计奠定基础。

汽车起重机路径搜索算法：

根据以上分析，汽车起重机的路径规划问题转化为所得二维地图上某一离散点到另一离散点的路径规划问题。本文针对带有高度参数的二维地图的路径规划问题进行搜索算法设计。

路径优化目标：

在将吊装物从起始点吊至终点的过程中，可能存在多条路径满足要求，但不一定最优，因此需设计路径优化目标，根据优化目标进行路径选择。而根据实际吊装特点，路径的好坏主要取决于安全性和经济效益两方面。其中，安全性主要是涉及碰撞问题和超载问题，而经济效益方便主要体现在操作简便性上。本文只考虑到操作简便性，将吊装操作简便性作为优化目标。

路径点选择策略：

由于在二维地图中每个离散点都有4种选择方向：左回转、右回转、幅度增加和幅度减小，因此，需要采取一定的路径点搜索策略来进行路径点的选取。

针对吊装操作最简单的路径优化目标，本文设计两种路径选择策略，一种是全局搜索策略，一种是局部搜索策略。

全局搜索策略是指搜索的总体方向。具体策略为：

（1）判断当前点周边4个离散点是否可达，如何全部不可达，这判断该点为死点，否则标记出所可到达点。

（2）判断当前位置回转角度θ起始1和变幅角度θ起始2和目标位置的回转角度θ目标2和变幅角度θ目标2大小关系，依据下表1所示设置当前点周边4个离散点的选取优先级，其中1代表最高，4代表最低。

（3）根据优先级结合（1）中可达到性判断，对各点的选择优先级进行调整，如果高优先级离散点不可到达，这将该点从优先级中去掉，比该点优先级低的优先级等级全部减1。

（4）根据优先级选择下一个离散点，优先级最高的点作为全局搜索策略输出的结果点。

表1优先级判断条件

局部搜索策略是全局搜索策略的补充及完善。考虑到如图5所示情况，其中M是当前点，B是目标点，假设离散点N点高度为10，N点周围出M点外其余3个离散点高度都为0，如果按照全局搜索策略定的方向，路线为M->N->P->B，显而易见，这种选择不如M->Q->D->L->P->B，因此，有必要在考虑全局搜索策略基础上采用局部策略。针对这一情况，本文采用一种简单的阈值判断方法，如果全局搜索策略所得到的下一离散点的高度与当前点的高度差值超过某一阈值HeightValue，则进行局部搜索策略。具体策略步骤如下：

（1）进行全局搜索策略，获取优先级等级为1的离散点，判断该点高度是否满足阈值要求，如果满足要求，则将该离散点设置为搜索结果点，否则，转向（2）；

（2）根据全局搜索策略中所记录的优先级顺序，检查优先级为2的离散点高度，判断该点高度是否满足阈值要求，如果满足，则将该离散点设置为搜索结果点，否则，检查优先级为3的离散点高度是否满足条件；如果优先级3离散点满足要求，则将该离散点设置为搜索结果点，否则，检查优先级为4的离散点；如果优先级4的离散点高度满足要求，则将该离散点设置为搜索结果点，否则转向（3）。

（3）如果优先级为2、3、4的离散点均不满足阈值要求，则将优先级为1的离散点设置为搜索结果点。

（4）将当前点标记为“已在路径中”，并将所得搜索结果点作为当前点寻找下一个点。

路径回退策略：

由于在进行路径点选择策略后，有可能得到这样一个点：这个点周围出了上一个离散点，其余3个点都是不可达到的，又或者得到的点已经在路径中了，这样算法就会出现陷入死区或者死循环中，因此，需要采取路径回退策略。

回退策略为：通过对路径点选择策略得到的下一个离散点进行搜索，判断该点周围4个点的当前状态是否为不可到达或者已经存在于所得路径中，如果周围4个点的状态都是以上两种中的任意一种，那么该点周围4个点都不能选择，表明该点是死点，需要回退到已得路径中最后一个离散点上。在回退到上一个离散点的同时，标记该点为已搜索的死点，然后继续通过路径搜索策略寻找下一个符合条件点。

本文根据以上分析结果，利用C++进行路径规划算法设计，解决了吊装物三维路径规划问题，并对结果进行分析验证。

根据以上理论分析，针对汽车起重机三维路径规划问题，需要完成带高度参数的二维地图的建立以及路径规划算法两个步骤，针对这两点，需要利用C++对二维地图和搜索算法进行类结构设计，为搜索算法的实现做铺垫。

根据上述分析结果，三维空间简化为一个带有高度参数的二维地图，并根据最小回转角度和最小变幅角度将二维地图离散化，每个点包含有空间中物体高度信息。根据这一特点，本文设计一个Mynode类来表示地图，储存二维地图信息，该类包括有当前点的变幅角度、回转角度以及物体高度信息。又由于要考虑到回退策略，加一个变量来标记当前点的状态，状态有3种，分别为搜索过、在路径序列中和已搜索，因此，该类包括三个变量：变幅角度、回转角度、高度以及当前点状态。

在地图类确定后，考虑到需要对路径规划结果进行保存，因此，本文设计路径结果存储类Node_In_Table来保存路径规划结果，该类包括离散点的变幅角度、回转角度、该点周边上下左右4点的可行状态。

在二维地图已经建立的情况下，设计一个Quadtree_structure类来进行算法的实现。该类实现地图的初始化工作、路径规划算法以及结果输出功能。该类包括Quadtree_structure地图初始化函数，search路径搜索函数以及Node_In_Table储存结果变量。

本文假设当前最小回转角度为6度，最小变幅角度也为6度，那么可行域S1中离散点总数为N*M，其中N=360/6=60,M=90/6=15，那么离散点数为S₁=60*15=900。

假设实例1的初始化条件为：起始点变幅角度为2，回转角度也为2，起始点吊装物的高度为3，目标位置的变幅角度为10，回转角度为10目标高度为6。并考虑场景有物体高度信息，如按如表2方式对离散点进行高度设置，其中1500代表该点不可到达，其余离散点高度全部为0，并设置高度阈值为7。

表2部分离散点高度信息

将初始化信息带入到search函数中得到结果如图6所示，算法执行时间为4.336毫秒，算法迭代次数为17。

针对以上实例，如果采用位置空间描述方法对三维作业空间进行划分，那么根据实例1中的假设，可以将三维空间进行离散化，划分为60*15*20个小立方体，每个小立方体代表一个离散点。根据表4-1的高度初始化条件，将这些小立方体属性进行初始化，如果当前小立方体上有物体，则该立方体属性置为1，否则为0，从而完成工作空间初始化。然后，在本文所述搜索策略基础上，为每个离散点增加吊装物上和下两个搜索方向，利用搜索算法对实例1中相同的初始化条件进行搜索，得到三维搜索路径如下，

(2,2,3)->(3,2,3)->(4,2,3)->(5,2,3)->(6,2,3)->(7,2,3)->(8,2,3)->(9,2,3)->(10,2,3)->(10,3,3)->(10,4,3)->(10,5,3)->(10,6,3)->(10,7,3)->(10,8,3)->(10,9,3)->(10,10,3)->(10,10,4)->(10,10,5)->(10,10,6)。其中每个点三个坐标分别代表回转角度，变幅角度和高度，搜索时间为6.959毫秒，迭代次数20次。

在实例1中，利用本文的空间划分方法，将三维空间进行二维映射，然后按照本发明的搜索策略运行，根据局部搜索策略可知离散点就从(2,2)转到右边离散点(2,3)；然后由于(3,3)点高度没超过阈值，就按全局搜索策略进行，到(4,3)点又超过阈值，则按局部搜索策略选择(3,4)；然后由于(6,4)周围3点全部不可到达，因此(6,4)是死点，采用回退策略，从(5,4)转向(5,5)，之后路径选择同样符合本文的搜索策略。

另外，将实例1与实例2从算法执行时间、迭代次数以及储存空间进行对比如表4-2所示。实例1是本文采用二维映射地图划分方法，实例2是常规的三维空间划分方法。两者对比可知本文提出的算法储存空间大大减少，搜索时间和迭代减少，达到降低汽车起重机的三维路径规划复杂性的目的。

表3  实例1和实例2对比

Claims (1)

1.一种基于空间二维映射的汽车起重机三维路径规划方法，其特征在于，该方法为：

1）对吊装二维地图初始化，确定吊装初始点和目标点位置，确定每个离散点高度的阈值HeightValue为汽车起重机总臂长乘以汽车起重机吊臂的变幅角正弦值；其中离散点高度是指离散点距离地面的高度；

2）将初始点标记为当前点；

3）判断当前点周边4个离散点是否可到达，如果全部不可到达，则判断该点为死点，若是，则进入11）；否则标记出所有可到达点，进入4）；

4）判断当前点所在位置的回转角度θ_起始1和变幅角度θ_起始2以及目标点所在位置的回转角度θ_目标1和变幅角度θ_目标2大小关系，依据下表设置当前点周边4个离散点的选取优先级，其中1代表最高，4代表最低，左回转代表当前点向左转得到的离散点，右回转代表当前当向右转得到的离散点，幅度增加代表当前点往幅值增加方向移动得到的离散点，幅度减小代表当前点往幅值减小方向移动得到的离散点；

5）获取优先级等级为1的离散点，判断该点高度是否超过阈值HeightValue，如果未超过，则将该离散点设置为搜索结果点，并加入已得路径表中，转向8）；否则，进入6）；

6）检查优先级为2的离散点高度，判断该点高度是否超过阈值HeightValue，如果未超过，则将该离散点设置为搜索结果点，并加入已得路径表中，转向8）；否则，检查优先级为3的离散点高度是否超过阈值HeightValue，如果优先级3离散点高度未超过阈值HeightValue，则将该离散点设置为搜索结果点，并加入已得路径表中，转向8）；否则，检查优先级为4的离散点，如果优先级为4的离散点高度未超过阈值HeightValue，则将该离散点设置为搜索结果点，并加入已得路径表中，转向8）；否则进入7）；

7）如果优先级为2、3、4的离散点高度均超过所述阈值，则将优先级为1的离散点设置为搜索结果点，并加入已得路径表中；

8）将所述当前点标记为“已在路径中”，将所述搜索结果点标记为新的当前点；

9）判断上述步骤8）中新的当前点是否为目标点，若是，按搜索顺序输出已得路径表中的搜索结果点，转向11）；若否，返回3）；

10）判断新的当前点周围4个离散点的当前状态是否为不可到达或者已经存在于已得路径表中，如果周围4个离散点的状态是以上两种中的任意一种，那么新的当前点周围4个离散点都不能选择，标记该点为死点，回退到已得路径表中新的当前点的上一个离散点上，将新的当前点的上一个离散点标记为当前点，返回3）；

11）结束。

Images