CN104484510A - 一种新的起重机吊装动作规划方法 - Google Patents

Abstract

本发明属于工程机械、计算及控制自动化领域，涉及到利用快速扩展随机树和启发式策略以及基于全新的现代吊装方案设计理论方法的CAD技术——计算机辅助吊装方案设计技术，对履带式起重机进行吊装动作序列规划的方法；有效地解决了现阶段吊装动作规划方法所得路径迂回严重、路径代价大、规划时间长、路径质量差的问题，在障碍物随机分布的环境中，给出了起重机与被吊物在自动规避碰撞的前提下，从起始位姿自动转换到就位位姿所需的具体的动作序列，起重机依据这些动作序列能安全、高效地完成吊装作业。通过对吊装过程的仿真预演，说明了所提方法的有效性和高效性。

Description

一种新的起重机吊装动作规划方法

技术领域

本发明属于工程机械、计算及控制自动化领域，涉及到利用快速扩展随机树(RRT,Rapidly-exploring Random Tree)和启发式策略(heuristic strategy)以及基于全新的现代吊装方案设计理论方法的CAD技术——计算机辅助吊装方案设计(Computer Aided Lift Plan Design，简称CALPAD)技术，对履带式起重机进行吊装动作序列规划的方法；在障碍物随机分布的环境中，给出了起重机与被吊物在自动规避碰撞的前提下，考虑到吊装现场环境、人员安全及吊装的时间效率和被吊物轨迹代价，从起始位姿自动转换到就位位姿所需的具体的动作序列，起重机依据这些动作序列能顺利、安全、高效地完成吊装作业。

背景技术

吊装过程规划可以直观、便捷地设计起重机吊装过程(即吊装状态序列和动作序列)，包含吊装仿真、吊装动作规划两种手段：吊装仿真是一种直观的交互式吊装过程规划工具，在实际吊装之前人机交互地模拟起重机的各项活动，校核是否存在碰撞或超载情况，迭代地设计安全可靠的吊装过程；吊装动作规划是一种智能的自动吊装过程规划工具，根据给定的起吊状态、就位状态及有障碍物的三维环境自动生成一个无超载、无碰撞的吊装动作序列。

初期的动作规划大多采用的是可视图法(visibility graphs)、栅格分解法(cellular decomposition)、沃罗诺伊图法(voronoi method)、Dijkstra方法、A*方法(Yershov D S and LaValle S M,Simplicial Dijkstra and A*algorithms for optimal feedback plannin.2011IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS),San Francisco,CA,2011.3862-3867.)、人工势场法等基于空间几何构造和图搜索的方法以及遗传方法(Fan S,Sun K and Wang Y,GA optimization model for repetitive projects with soft logic.Automation in Construction. 2012.21:253-261.)、蚁群方法、人工神经网络方法等智能规划方法，这些方法对于很多路径规划问题都具有很强的适应性，但当面对高自由度起重机路径规划问题时，其计算复杂度将随自由度呈指数增长，而且对微分约束和复杂环境也缺乏较好的解决办法；而现阶段适用于高维空间动作规划的方法也存在不少的问题，例如，在方法中考虑起重机动作优先级约束，允许起重机在执行动作时优先选择高优先级的动作(Zhang Yu-yuan.Design and Implementation of Collision-free Path Planning for Mobile Crane.Dalian:Dalian University of Technology,2012.)以及用于解决狭窄通道问题的方法(Yershova A,Jaillet L,Simeon T and LaValle S M,Dynamic-domain RRTs:efficient exploration by controlling the sampling domain.IEEE International Conference on Robotics and Automation,Barcelona,Spain,2005.3856-3861.)，仅负责寻找可行动作序列，对序列的质量关注较少，所得路径往往迂回严重，路径代价较大，所耗规划时间较长；改进的概率路标法(Probabilistic Roadmaps Method)通过将自由度分解降维进行分步规划来提高规划效率，但忽略了吊装过程中常见的起重机行走的情况，与实际作业情况差距较大；基于随机图理论的RRT方法(Karaman S and Frazzoli E,Incremental sampling-based algorithms for optimal motion planning.Proc.Robotics:Science and Systems,2010；Karaman S and Frazzoli E,Sampling-based algorithms for optimal motion planning.International Journal of Robotics Research,2011.30:846-894.)在生成树扩展过程中不断调整树结构，对路径进行渐进性优化，然而由于扩展过程中需要对树不断调整并进行大量计算，方法效率偏低。

发明内容

本发明提供了一种起重机吊装动作规划方法，针对现存的高维动作规划方法在起重机吊装动作规划中因所得路径迂回严重、平滑性较差而导致的路径代价大、规划时间长、路径质量差的问题。首先通过定义吊装系统位形对吊装动 作规划问题进行建模，然后对起重机的运动进行非完整性运动学分析建立起重机的运动学模型，最后采用一种启发式采样策略，提高决定RRT数生长方向的随机采样点的质量，引导随机树向能够生成较短路径的方向扩展。

本发明的重机吊装动作规划方法，具体步骤如下：

(1)吊装系统位形建模

称吊装过程中由起重机和被吊物组成的系统为吊装系统。由于起重机在吊装过程中可执行的动作有下车直行、下车转弯、转台回转、臂架变幅和吊钩起升，本方法考虑吊装系统的7个自由度：起重机下车坐标(x,z)(范围取决于吊装场地的大小，具体分别为),下车方向角α([-π,π))，转台回转角β([-π,π))，臂架仰角γ([0,π/2])，起升绳长h(范围由臂长和臂架仰角决定，表示为[h_lmt,h_max(γ)]，h_lmt为起重机的限位高度，h_max(γ)为起升滑轮组到地面的高度)，吊钩旋转角ω([-π,π])，如图1。在吊装动作规划研究中，认为履带起重机各部件不会发生变形并且起升绳始终竖直。这样，吊装系统每一个稳定的位姿都可以表示为一个向量X，如下式(1)所示。方法将在此向量空间中搜索吊装动作序列.

$X=\left[\begin{array}{cc}{x}_{\min }^{\mathrm{site}}& x_{\max }^{\mathrm{site}}\\ z_{\min }^{\mathrm{site}}& z_{\max }^{\mathrm{site}}\\ -\mathrm{\π}& \mathrm{\π}\\ -\mathrm{\π}& \mathrm{\π}\\ o& \mathrm{\π}/2\\ h_{\mathrm{lmt}}& h_{\max }\left(\mathrm{\γ}\right)\\ -\mathrm{\π}& \mathrm{\π}\end{array}\right]---\left(1\right)$

为了统一角度(方向角α、回转角β、臂架仰角γ、吊钩旋转角ω)和长度(起重机位置坐标(x,z)及起升绳长h)的量纲，本方法将两位姿X_i和X_i+1间的距离定义为被吊物从状态X_i变换到X_i+1所经历的轨迹长度，见下式(2)。

$\begin{array}{c}L(X_i,X_{i+1})=|x_{i+1}-x_i|+|z_{i+1}-z_i|+|\left(r(a_{i+1}-a_i)\right|+|(r+(b_{i+1}-b_i)|\\ +|\left(l_b(g_{i+1}-g_i)\right|+\left|(h_{i+1}-h_i)\right|+\left|l_w(w_{i+1}-w_i)\right|\end{array}---\left(2\right)$

其中，X_i＝(x_i,z_i,a_i,b_i,g_i,h_i,w_i)和X_i+1＝(x_i+1,z_i+1,a_i+1,b_i+1,g_i+1,h_i+1,w_i+1)分别为随机空间中2个独立的位姿，r为作业半径，l_b为臂长，l_w为被吊物长。设搜索到的动作序列Path包含的各节点依次为X₀,X₁,……,X_n-1，X_n,则该条动作序列的代价为L(X_i,X_i+1)为点X_i与点X_i+1间的距离。

(2)分析履带起重机的非完整性运动

由于履带起重机的行走和转弯是通过两条履带的驱动轮带动履带转动实现，属于典型的差分驱动，起重机能够在不同的曲率半径的圆弧或直线上运动，也能够原地转向，但不能沿着履带的垂直方向运动。因此，行走和转弯需要满足非完整运动学约束。行走和转弯的运动学模型如图2所示。其中，XOZ为世界坐标系，O_r为履带起重机的转弯中心，点P(x,z)为起重机的位置坐标，α为履带的方向角(世界坐标系下X轴到正向的角度)，v_L、v_R分别是左右履带的线速度，B为左右履带的轨距。不考虑履带打滑，起重机的行走和转弯所满足的运动学约束可表示为式(3)和式(4)，其中R为转弯半径。当v_L、v_R同向并大小相等时，履带起重机直线行走；当v_L、v_R反向并大小相等时，履带起重机原地转向；当v_L、v_R同向并大小不等时，履带起重机以R为转弯半径为进行转向。

$\left\{\begin{array}{c}\stackrel{\·}{x}=\frac{1}{2}(v_R+v_L)\cos \mathrm{\α}\\ \stackrel{\·}{z}=-\frac{1}{2}(v_R+v_L)\sin \mathrm{\α}\\ \stackrel{\·}{\mathrm{\α}}=\frac{1}{2R}(v_R+v_L)\end{array}\right.---\left(3\right)$

$R=\frac{B(v_R+v_L)}{v_R-v_L}---\left(4\right)$

采用式(5)描述起重机的动作，其中w_β、w_γ、w_ω分别为回转、变幅、吊钩旋转的角速度，v_h为起升的线速度。这样，表示起重机非完整运动学约束的状态转移方程可表达为式(6)，通过积分便可得到一个新的位形q′，如式(7)。

$u={[v_L,v_R,w_{\mathrm{\β}},w_{\mathrm{\γ}},v_h,w_{\mathrm{\ω}}]}^T---\left(5\right)$

$\stackrel{\·}{q}=f(q,u)=\left[\begin{array}{c}0.5(v_R+v_L)\cos \mathrm{\α}\\ -0.5(v_R+v_L)\sin \mathrm{\α}\\ 0.5(v_R+v_L)/R\\ w_{\mathrm{\β}}\\ w_{\mathrm{\γ}}\\ v_h\\ w_{\mathrm{\ω}}\end{array}\right]---\left(6\right)$

$q^{\′}=g(q,u)=q+\stackrel{\·}{q}\mathrm{\Δt}---\left(7\right)$

(3)起重机吊装动作规划

RRT方法原型中，首先以起始状态X_init和目标状态X_goal初始化两棵树T_init、T_goal，然后方法对这两棵树进行交替、迭代地扩展：在状态空间中选择一个随机状态X_rand作为T_init本次迭代的扩展目标，并在T_init上选择一个距离X_rand最近的点X_nearest作为生长点，T_init接着试图应用Connect操作尽可能向目标扩展生成X_new，选择T_goal中靠近X_new的节点，将其与X_new相连，若能到达X_new，则认为T_init和T_goal是能连接的，一条可行路径产生，若不能到达X_new，在下次迭代中交换两树的角色，以保证每棵树都有相等的机会延伸。图3为某次迭代中树T_init的扩展示意图，图中，实心点为快速扩展随机树上的点，空心点为一次Connect操作的中间节点。

本方法在RRT原型的基础上，采用一种启发式采样策略，为每个随机点X_rand设置一个平滑度的约束，定义随机目标点X_rand的平滑度为：

R(X_rand)＝L(X_init,X_rand)+L(X_rand,X_goal)        (8) 

只有平滑度满足约束：R(X_rand)≤ε·L(T_init，T_goal)(ε>1)的随机点X_rand才被采纳作为扩展的阶段目标点。这样做能提高阶段目标点的质量，引导随机树向能够生成较短路径的方向扩展。如图4中，虚线所示为R(X_rand)，实线所示为R(X_rand1)，很明显，选随机点X_rand1作为阶段目标点规划所得路径在路径长度和路 径平滑性上要优于选随机点X_rand作为阶段目标点规划所得路径。同时，为保证规划时间不致过长，设置尝试次数Δ,即在尝试次数内选择平滑度满足约束的第一个点作为树扩展中的阶段目标点，若尝试次数超过Δ，则选择R(X_rand)最小的点作为树扩展中的阶段目标点，以此保证树扩展的随机性。

关于ε的取值问题，我们通过对起重机在不同障碍物密度的环境中寻找从初始点到目标点的路径这一规划问题进行大量统计实验得出，障碍物分布稀疏时，ε越小，规划效果越好，障碍物分布稠密时，ε过小，规划效果反而不好。但尽管障碍物分布密度不同，ε取值在1.0～2.0时，规划效果最好。

本发明规划出一条从起始位置到就位位置的安全的、可行的、没有碰撞的、高质量的动作序列，使随机树生长具有一定的方向性，有效地缓解了路径迂回，使最终路径更平滑，从而路径代价降低；有效地避免不必要的碰撞，收敛速度快，能较快找到一条相对平滑的从初始点到目标点的路径，大大节省了规划时间。

附图说明

图1是吊装系统位形示意图。

图2是起重机行走和转弯的运动学模型图。

图3是RRT原型某次迭代中树T_init的扩展图。

图4是所提方法方法某次迭代中树T_init的扩展图。

图5(a)是采用RRT-Connect方法所得规划结果截图。

图5(b)是采用本发明中的方法所得规划结果截图。

具体实施方式

RRT-Connect规划方法是目前被广泛使用的RRT变种方法之一，本实施例通过在同一吊装作业中，将所提方法规划所得数据与RRT-Connect方法规划所得数据进行对比来说明本方法规划所得动作序列的路径代价(路径长度)、规划 时间、路径迂回程度、碰撞检测次数的改善情况。

本实施例中，起重机需要在钢架外吊起被吊物，从两排钢架的入口进到钢架之间，通过回转、行走等动作将被吊物安放在目标钢架上，由于两排钢架相距较近，障碍物密度相对较大，此过程中尤其要避免碰撞，应用RRT-Connect方法和所提方法所得某次规划结果如图5所示。其中，上方的曲线表示吊点的运动轨迹，下方的曲线表示起重机下车的运动轨迹。表1记录了统计实验所得路径代价情况，其中，所得数据是进行100次实验所得结果的平均值。

从规划结果截图可见，采用本发明的方法能够找到一条无碰撞的动作序列实现被吊物从初始位置到就位位置的转移，且与RRT-Connect相比，所得路径更平滑，几乎没有产生路径迂回现象；从记录表格可见，与RRT-Connect方法相比，改进后的方法所得树节点个数减少了碰撞检测次数减少了49.70％，碰撞检测时间减少了从而使得规划时间和动作序列长度分别缩短了51.26％和38.10％，所得路径质量明显提高，这也证明了方法的有效性和高效性。下表1是仿真规划结果数据记录表：

项目	RRT-CONNECT	本发明中的方法	IMPROVEMENT
				路径长度(m)	1620.75m	1003.27m	38.10％
规划时间(s)	5.90S	2.88S	51.26％
				树节点数(个)	803.79	425.94	47.01％
碰撞检测次数(次)	27487.62	13826.69	49.70％
				碰撞检测时间(s)	4.29S	1.97S	54.08％

Claims (1)

1.一种起重机吊装动作规划方法，其特征在于以下步骤，

(1)吊装系统位形建模

由起重机和被吊物组成的系统为吊装系统，吊装系统的7个自由度为：

①起重机下车坐标(x,z)，取值范围分别为和

②下车方向角α([-π,π))；

③转台回转角β([-π,π))；

④臂架仰角γ([0,π/2])；

⑤起升绳长h，范围由臂长和臂架仰角决定，表示为[h_lmt,h_max(γ)]，其中h_lmt为起重机的限位高度，h_max(γ)为起升滑轮组到地面的高度；

⑥吊钩旋转角ω([-π,π])；

吊装系统每一个稳定的位姿都表示为一个向量X，如下式(1)所示；

$X=\left[\begin{array}{cc}{x}_{\min }^{\mathrm{site}}& x_{\max }^{\mathrm{site}}\\ z_{\min }^{\mathrm{site}}& z_{\max }^{\mathrm{site}}\\ -\mathrm{\π}& \mathrm{\π}\\ -\mathrm{\π}& \mathrm{\π}\\ O& \mathrm{\π}/2\\ h_{\mathrm{lmt}}& h_{\max }\left(\mathrm{\γ}\right)\\ -\mathrm{\π}& \mathrm{\π}\end{array}\right]---\left(1\right)$

两位姿X_i和X_i+1的距离是指被吊物从状态X_i变换到X_i+1所经历的轨迹长度，见下式(2)：

L(X_i,X_i+1)＝|x_i+1-x_i|+|z_i+1-z_i|+|(r(a_i+1-a_i)|+|(r(b_i+1-b_i)|

                                                           (2)

+|(l_b(g_i+1-g_i)|+|(h_i+1-h_i)|+|l_w(w_i+1-w_i)|

其中，X_i＝(x_i,z_i,a_i,b_i,g_i,h_i,w_i)和X_i+1＝(x_i+1,z_i+1,a_i+1,b_i+1,g_i+1,h_i+1,w_i+1)分别为随机空间中2个独立的位姿，r为作业半径，l_b为臂长，l_w为被吊物长；

设搜索到的动作序列Path包含的各节点依次为X₀,X₁,……,X_n-1，X_n,则该条动作序列的代价为其中，L(X_i,X_i+1)为点X_i与点X_i+1间的距离；

(2)分析履带起重机的非完整性运动

以XOZ为世界坐标系，O_r为履带起重机的转弯中心，点P(x,z)为起重机的位置坐标，α为履带的方向角，v_L、v_R分别是左右履带的线速度，B为左右履带的轨距；

起重机的行走和转弯所满足的运动学约束表示为式(3)和式(4)，其中R为转弯半径：

$\left\{\begin{array}{c}\stackrel{.}{x}=\frac{1}{2}(v_R+v_L)\cos \mathrm{\α}\\ \stackrel{.}{z}=-\frac{1}{2}(v_R+v_L)\sin \mathrm{\α}\\ \stackrel{.}{\mathrm{\α}}=\frac{1}{2R}(v_R+v_L)\end{array}\right.---\left(3\right)$

$R=\frac{B(v_R+v_L)}{v_R-v_L}---\left(4\right)$

当v_L、v_R同向并大小相等时，履带起重机直线行走；当v_L、v_R反向并大小相等时，履带起重机原地转向；当v_L、v_R同向并大小不等时，履带起重机以R为转弯半径为进行转向；

起重机的动作由式(5)描述：

u＝[v_L,v_R,w_β,w_γ,v_h,w_ω]^T    (5)

其中w_β、w_γ、w_ω分别为回转、变幅、吊钩旋转的角速度，v_h为起升的线速度；

起重机非完整性运动学约束的状态转移方程表达为式(6)，通过积分得到一个新的位形q′，如式(7)：

$\stackrel{.}{q}=f(q,u)=\left[\begin{array}{c}0.5(v_R+v_L)\cos \mathrm{\α}\\ -0.5(v_R+v_L)\sin \mathrm{\α}\\ 0.5(v_R+v_L)/R\\ w_{\mathrm{\β}}\\ w_{\mathrm{\γ}}\\ v_h\\ w_{\mathrm{\ω}}\end{array}\right]---\left(6\right)$

$q^{\′}=g(q,u)=q+\stackrel{.}{q}\mathrm{\Δt}---\left(7\right)$

(3)起重机吊装动作规划

为每个随机点X_rand设置一个平滑度的约束，定义随机目标点X_rand的平滑度为：

R(X_rand)＝L(X_init,X_rand)+L(X_rand,X_goal)    (8)

只有平滑度满足约束：R(X_rand)≤ε·L(T_init，T_goal)(ε>1)的随机点X_rand才被采纳作为扩展的阶段目标点；同时，设置尝试次数Δ,即在尝试次数内选择平滑度满足约束的第一个点作为树扩展中的阶段目标点，若尝试次数超过Δ，则选择R(X_rand)最小的点作为树扩展中的阶段目标点，保证树扩展的随机性。