高空作业车避障问题时间最优轨迹规划

Time-optimal trajectory planning for aerial work platformwith obstacles consideredHE Hao，TENG Ru-rain，GAO Shun-de，HAN Zhong-tian复杂的作业环境可能在高空作业车作业区间内形成障碍 (如高压电线、建筑物突出部分 )。在平面运动中使用简单几何元素 (如圆和线段 )构造障碍物，通过对关节空间内的臂架组合动作进行合理的轨迹规划，可以实现臂架由给定初始位姿到确定的末端位姿之间越过障碍的运动。

高空作业车的避障问题属于冗余自由度机械臂的避障运动学规划，核心任务是找寻运动过程中能保证所有臂架不会发生碰撞的位姿序列。常规的冗余自由度机械臂系统轨迹规划的研究主要集中于求解逆运动学问题，用避障指标作为雅可比矩阵零空间的性能优化指标。文献 [1，2]较早提出了以机械臂的关节坐标系建立C空间，寻找不与障碍干涉的机器人构形。文献 [3，4]将预定路径作为引力源，障碍作为斥力源，通过约束调整机器人的自运动使机器人向着斥力减小的方向移动≌间路径搜索方法也多被使用，文献 [5]通过在始末位姿之间建立的长方体框架内进行六向搜索得到机器人连续的可行构形，为实现节点之间的时间-能量最优进行6自由度机械手的空间寻优。文献 [7]利用优化的A 算法，在7自由度空间机械臂的工作空间内进行了无碰撞最优路径的搜索，实现了避障路径规划。障碍物的引入加大了冗余 自由度矩阵求逆与优化的难度，臂架伸缩式运动关节和关节运动限制的存在，使其与普通机械手形式上有所不同。本文采用具有启发式的A 算法确定了障碍环境下的臂架动作序列，继而根据关节序列节点问的能满足的最短运动时间对关节进行样条曲线进行插值得到运动曲线1 模型参数化百米级高空作业车臂架主要结构如图1所示。

考虑实际工作状况，-般-号臂只有稳定于仰角≥85。位置才能工作，另外由于工作平台时刻处于水平平行，其仰角无需进行规划。对于某型高空作业车使用D-H坐标系建立方法进行整体建模得到的依附于各连杆的完整空间坐标系如图2所示，图中0 为回转角度，d 为-号臂长度， .为-号臂仰角， 。为-号臂与二号臂夹角， 为二号臂长度，为二号臂与飞臂夹角。其余D-H参数如表1，表[收稿日期 ]2013-03-13[通讯地址 ]贺浩，大连市高新园区汇贤园1--10楼 (腾飞园区 )CONSTRUCTION MACHINERY 2013.6 63专题研究I SPECIAL RESARCH中壤 示变化量。同时不考虑平台回转，这样关节空间变为平面问题 ，共有-号臂伸缩 、二号臂变幅、二号臂伸缩 、飞臂变幅4个 自由度。如图2所示，此时忽略-号臂与二号臂、二号臂与飞臂的结构轴线的偏距a，、d3，即坐标原点O。与O 、O 与O在平面的投影重合。

00(01)1.-号臂 2.二号臂 3.飞臂图1 高空作业车臂架结构图2 完整D-H坐标系模型表1 D-H坐标系参数连杆 0f i 惋 围Z /(。) /(o) 以i /(o)或mml 0l(V) -5 以1 0 0-3602 -90 90 0 (V) 13800-610003 03(V) 90 0 以 10～1604 0 -90 0 (V) 14800～352O05 0 5(V) 0 a5 0 -182～-9064 建筑机械 2013.6(上半，El刊2 臂架与障碍物碰撞条件的建立由于高空作业车在工作位置就位后臂架运动主要在变幅平面内实现 ，因此从变幅平面YOZ人手进行避障规划。平面问题中的所有不规则障碍物可以抽象为两种简单几何要素的组合--包络圆模型和线段模型。

2.1 包络圆障碍模型包络圆模型用于描述在变幅平面内投影为具有-定形状的障碍，如障碍物为空间中与臂架变幅平面相互垂直的电缆。用具有-定半径尺 圆心坐标为 (Y Zob)的圆表示不规则空间物体在YOZ平面内的投影，该圆如图3中所示，是障碍轮廓的最小外接圆。需要计算-号臂、二号臂及飞臂距离障碍中心的距离。

图3 障碍物的包络圆臂架i与障碍物的关系如图3所示 ，臂架i两个端点分别为P 、P 。已知障碍圆相对于基系的笛卡尔坐标，通过正运动学坐标变化，可以得到各连杆节点相对于转台坐标系的具体坐标值。

兀A U i1，2，，5 (1)kl其中U0 0 0 l ，得到-号臂、二号臂以及飞臂末端点坐标为P (X，Y，z)、P。(X，Y，z)、P3(X，Y，z)。

设障碍圆圆心距臂架轴线距离为 ， 为平台长度、臂架宽度或者误差产生的安全裕度，如图4所示。臂架与障碍位置关系及碰撞条件为(1)当 >R。 寸，显然臂架i不会与障碍物相碰，见图4a；(2)当 ≤ 曲 时，若I J>l I ，臂架 不会与障碍物相碰，若√I l。- - ≤I I，臂架无论如何都会与障碍物相碰撞，见图4b。

图4 臂架与障碍包络圆的非碰撞条件2.2 线段障碍模型对于具有-定高度的高塔、烟囱或山体峭壁、玻璃幕墙的边界在高空作业车的YOZ变幅平面内投影为线段。判断臂架是否与墙壁相碰的几何模型为两条有向线段的相交问题 。如图4所示 ，作臂架i两端点与 相交的垂线矢量，记为、 ， 作A、 两点垂直 并与之相交的矢量 、 ，其中～ 瓦 P，D AD - - ： AP， .a 2B AB- L- - B。- - 。

IABl IAB J同理可得 、 。显然当 、 。位于 同侧或A、 位于 同侧，则臂架不与障碍物相碰撞，判断两点在矢量-侧主要依靠判断两相互平行的垂线矢量的符号是否相反。

侧b图5 臂架与直线障碍矢量的非碰撞条件3 路径搜索与时间最优轨迹规划3.1 A 算法进行关节空间的路径搜索A 算法是-种启发式有序搜索方法。设A、两节点之间的最小路径代价为F(A， )，设起始节点为 ，目标点尸， 目标点为当前点为 ，定义P 的实际估价函数f( )g( ) ( ) (2)其中g(只)F(A，Px)；h(只)F( ，B)。

对于高空作业车使用A 算法路径搜索，主要是四维关节构型空间9内的点的搜索问题，最终路径为每次在空间内相邻点中搜索得到的估价函数最小的点相连所形成。关节空间的某点的构型向量留。

CONSTRUCTION MACHINERY 2013 6 65专题研究各元素包括-号臂的伸缩长度，记为q ；二号臂的变幅角度，记为q ；二号臂的伸缩长度，记为q。；飞臂变幅角度，记为q 。即gf[ 。q ：q 。 ]。定义起始节点坐标向量为g 目标点坐标向量为qslO定义估价函数g(q )∑ Iq，(n)-q川( )lg(留H)(留 ∑4 )-qfi.ol( )I (3)f(q )g( )ph(q )式中k 为第n个关节的影响系数；P为估计函数h的放大系数。相邻节点的关节变化步长设计为：对于伸缩移动关节250mm，对于转动关节1。。在避障时伸缩关节具有更高的灵活性，而且臂架每转动-个步长引起的末端点位移变化也需考虑，因此对于k ，应当增大转动关节的数值。设定式 (3)中的系数 (nl，2，3，4)分别为：1、100000、l、10000，P为10。

3.2 最优时间轨迹规划以上得到了臂架动作组合在关节空间内的位姿序列，为了得到最终的运动轨迹还需确定臂架运动与时间的关系。各关节在节点之间的步长是确定的，为了保证关节运动不超过限制，同时使时间最短，应将两节点之间的4个关节分别以最大运动速度得到的时间中较长的-个作为节点间运动时间。

由于对位姿序列规划得到的是-系列离散点，需要根据节点间耗时对其进行插值形成光滑函数曲线。为了使函数经过关节f的序YOq同时保证在整个区间上 [ ， ]有连续的-阶及二阶导数。

使用三次样条函数 (t)进行插值， (t)在子区间上 [ ， ]上为三次多项式根据实际情况 ，初始与末端速度应为零 ，设置边界条件qf10， q 066 建筑机械 2013 6(上半月刊)4 算例仿真使用计算机数学语言MATLAB编写A 搜索程序。初始设定高空作业车关节空间构型向量和目标向量即D-H参数为口。砌 l 15000mm，50。，20000mm，-120。j口加jl 30000mm，80。，22000mm，-100。J设包络圆障碍物在高空作业车基座标中的坐标为 [0，-l 7 000，25 000]，其半径R。 1000mm。设线段障碍物两端点坐标为 [0，- 19500，-1000 J， 0，-19500，25000]。

搜索结束时搜索点总数为3185个。寻找到最佳路径，路径上的有效点共80个。图6为臂架的运动过程，从图中可以看出臂架成功躲避了障碍物。

图6 臂架运动避障规划过程关节驱动主要由油缸实现 ，根据该车油缸具体参数并