基于A-算法的空间机械臂避障路径规划

机械工程学报JOURNALOFMECHANICALENGINEERING第46卷第13期2010年7月Vol.46No.13Jul.2010DOI：10.3901/JME.2010.13.109基于A*算法的空间机械臂避障路径规划*贾庆轩陈钢孙汉旭郑双奇(北京邮电大学自动化学院北京100876)摘要：针对空间机械臂在轨操作任务需求，提出一种基于A*算法的避障路径规划算法。根据机械臂和障碍物几何特征，对机械臂模型和障碍模型进行简化。通过研究机械臂本身所固有的几何特性，根据障碍物的位姿坐标，分析机械臂各杆件与障碍物发生碰撞的条件，进而求解空间机械臂的无碰撞自由工作空间。在此基础上，利用A*算法在空间机械臂的自由工作空间进行无碰撞路径搜索，实现了空间机械臂的避障路径规划。通过仿真试验验证了基于A*算法的空间机械臂避障路径规划算法的有效性与可行性。关键词：空间机械臂避障路径规划A*算法中图分类号：TP242PathPlanningforSpaceManipulatortoAvoidObstacleBasedonA*AlgorithmJIAQingxuanCHENGangSUNHanxuZHENGShuangqi(AutomationSchool,BeijingUniversityofPostsandTelecommunications,Beijing100876)Abstract：AnovelpathplanningmethodtoavoidobstaclebasedonA*algorithmispresentedforspacemanipulatortoaccomplishthein-orbitmission.Accordingtothegeometriccharacteristicsofmanipulatorandobstacle,themanipulatormodelandobstaclemodelaresimplified.Onthebasisoftheinherentgeometriccharacteristicofmanipulator,andaccordingtothepositionandorientationcoordinatesofobstacle,thecollisionconditionsofalllinksofmanipulatorareanalyzed.Andthen,thecollision-freeworkspaceofspacemanipulatorisobtained.Onthisbasis,thecollision-freepathsearchinthefreeworkspaceofspacemanipulatoriscarriedoutbyusingA*algorithm,thereby,theobstacleavoidancepathplanningisachieved.TheeffectivenessandfeasibilityoftheproposedpathplanningalgorithmbasedonA*algorithmforspacemanipulatortoavoidobstacleareverifiedbysimulationandexperiment.Keywords：SpacemanipulatorObstacleavoidancepathplanningA*algorithm0前言*随着空间探索的不断深入，空间机械臂应用技术已经成为空间技术的重要研究方向。空间机械臂代替宇航员完成空间作业任务，如组装与搭建空间站、释放与回收卫星、维护空间设备以及完成空间科学试验等，大大减小了宇航员舱外作业的风险，因此空间机械臂应用技术受到国内外专家的高度重视。在微重力环境下，空间机械臂系统处于自由漂*国家高技术研究发展计划资助项目(863计划，2009AA7041007)。20100324收到初稿，20100504收到修改稿浮状态，使得机械臂控制变量与非独立变量之间存在强烈的运动耦合，运动控制难度加大，从而空间机械臂的路径规划变得特别复杂[1]。此外，由于空间环境中的空间碎片，空间舱体外设试验装置等都有可能成为空间机械臂在轨操作过程中的障碍，因此为了顺利完成在轨操作任务，开展空间机械臂避障路径规划研究十分重要。避障路径规划是指在给定的障碍条件以及起始和目标的位姿，选择一条从起始点到达目标点的路径，使运动物体能安全、无碰撞地通过所有的障碍[2]。目前，针对机械臂避障路径规划提出了许多方法，其中昀为典型的包括基于自由空间法和人工机械工程学报第46卷第13期期110势场法两种。LOZANO-PEREZ[3-4]提出了基于C空间的自由空间法。首先以机械臂的关节坐标系建立C空间，将障碍物映射到C空间，形成空间构型障碍，从而求得C空间的补集，即自由空间。在此基础上，利用启发式搜索算法在机械臂的自由空间内寻找机械臂的运动路径。该方法虽然能够实现机械臂无碰撞路径规划，但是由于将障碍物映射到C空间方法较为复杂，对于复杂环境难以满足实时性的要求。KHABIT[5]对障碍定义一个排斥势场，目标点处定义一个吸引势场，机械臂的运动由两个势场共同作用力来决定，由此来保证机械臂在避障的同时顺利到达昀终目标点。该方法对于处理全局路径规划中的动态避障非常有效，但容易陷入局部昀小点处。FIORINI等[6]提出了基于速度的C空间法，该方法能够实现机器人移动障碍物环境中的避障，但该法只适用于移动机器人的避障路径规划。李大生等[7]利用系统内能方法推导了机械臂无碰运动规划的系列公式，提出了基于动力学约束的机器人无碰路径规划方法，但由于该方法涉及动力学计算的推导，对于6自由度空间机械臂采用该方法太过复杂。目前，国外针对空间机械臂避障路径规划算法的研究主要集中于国际空间站的冗余度机械臂。SAKATA等[8]基于梯度投影法提出了一种空间冗余度机械臂的避障路径规划算法，该方法主要利用冗余度机械臂自运动的特点实现其实时的避障路径规划，不适用于非冗余机械臂。SHAFFER等[9]提出了基于八叉树空间机械臂的避障路径规划算法，该方法需要预先构建空间机械臂整个工作空间的八叉树模型，在此基础上实现空间机械臂的实时避障路径规划，因此机械臂工作空间中的物体位置的改变和空间漂浮物的漂移都需重新构建机械臂的工作空间的模型。本文针对具体的空间机械臂构型，首先对空间机械臂的模型和障碍物模型进行合理的简化；在此基础上，将笛卡尔空间障碍物映射到关节空间，进而求得机械臂的自由运动空间；利用A*启发式搜索算法在机械臂的自由空间内进行了路径搜索；昀后，通过仿真试验对本文提出的空间机械臂避障路径规划算法进行了验证。1碰撞模型1.1空间机械臂模型本文研究的机械臂为6自由度空间机械臂，其具体的构型D-H坐标系与参数分别如图1和下表所示。机械臂杆件均为圆柱体，因此机械臂杆件碰撞模型采用圆柱模型，且其半径均为0.15m。图1机械臂D-H参数坐标系图表机械臂D-H参数表序号关节角/iθ(°)扭角/iα(°)连杆长度/mia连杆偏置/mid11θ(90)–9000.522θ(0)02.50.333θ(0)03.0–0.344θ(0)900055θ(–90)–9000.366θ(0)000.61.2障碍模型三维障碍物一般具有不规则的几何形状，因此本文利用障碍物的规则体包络来近似建模，这种建模方法虽然在一定程度上扩大了障碍域，但是使障碍域大大简化，有效地提高了规划的效率，同时也使得整个机械臂避碰路径规划更具有安全性。障碍物可以采用球形包络描述为0(,)SSPr，如图2所示。其中，0000(,,)Pxyz为球心在基坐标系中的坐标，Sr为球的半径，这种障碍物的球形包络近似的求取方法，大大简化了计算，方法简单直观。图2障碍物的球形包络几何模型月2010年7月贾庆轩等：基于A*算法的空间机械臂避障路径规划1112笛卡尔空间障碍向构型空间的转化本文首先根据机械臂模型和障碍物模型将笛卡尔空间障碍通过几何关系向机械臂的构型空间进行转化，从而形成机械臂的自由运动空间，为利用A*算法进行机械臂的无碰撞路径规划提供条件。2.1问题的提出空间机械臂在轨操作过程中，机械臂的运动会导致基座随之运动，因此空间机械臂的避障运动规划不仅与障碍物有关而且与基座的运动控制紧密结合。由于机械臂安装于基座上，当障碍物也固定于基座时，障碍物与机械臂安装位置间的相对位姿关系在整个机械臂运动过程中保持不变。此时，可以采用相对于基座坐标系的方式进行规划，障碍物向构型空间的转化只需要求解一次即可得出整个运动过程中机械臂的自由运动空间。当障碍物独立于基座时，机械臂的运动会导致障碍物相对于机械臂的安装位置发生运动，此时，需要采用相对于惯性坐标系的方式进行路径规划，因此每个控制周期都需要将障碍物向构型空间的转化，从而得出该控制周期机械臂的自由运动空间。为了便于分析和着重阐明本文提出的避障路径规划算法，本文以下均针对障碍物固定于空间飞行器基座的情况进行讨论，障碍物独立于基座的情况只是需要多次求解机械臂的自由运动空间，其求解方法与障碍物固定于基座模式相同。以下着重阐明求解机械臂自由运动空间方法。2.2总体思路本文研究对象为6自由度机械臂，其具有6个关节，每个关节角的旋转均可以引起机械臂与障碍物之间碰撞的发生，因此本文拟采用分级讨论的方法来求解障碍物与各杆件碰撞的条件。其总体思路为：首先求解关节1可能导致机械臂连杆与障碍发生碰撞的运动范围，后续各关节导致碰撞发生的情况均在该范围内讨论；在此范围之内可以推导出连杆2与障碍发生碰撞的条件；在连杆2与障碍不发生碰撞所对应的关节角2的范围内，讨论连杆3与障碍发生碰撞的条件；同理能够求得连杆4，连杆5与障碍发生碰撞的条件。2.3杆件与障碍碰撞条件分析由于杆件1只作绕自身轴线的运动，因此杆件1与障碍发生碰撞的情况不作考虑。杆1与杆2之间的偏置量为0.3m，而杆件模型为半径为0.15m的圆柱体，因此杆件1、2之间的偏置连杆与障碍发生碰撞的情况已包含在杆件1、2与障碍发生碰撞的分析过程中。同理，其余偏置连杆也均不作考虑。为了便于分析问题，在杆3与杆4之间的偏置连杆只考虑了两端各0.15m部分，中间0.3m部分未作考虑。因此以下针对杆2、杆3、杆4和杆5与障碍发生碰撞的条件进行分析。(1)可能发生碰撞的关节1运动范围。为了讨论杆件与障碍发生碰撞的情况，首先需要求解发生碰撞的关节1的运动范围。将图1中所示的机械臂在Oxy平面内投影，则关节1发生碰撞的两个极限位置如图3所示。图3碰撞发生的关节1极限位置图根据图3中的几何关系，可以求出障碍物中心在底面投影点与坐标原点连线的偏角11=arctan(/)xyppθ(1)求解关节1转动使整个机械臂不发生碰撞的角度11θ∆和12θ∆11h2d1=arcsin(/)ddθ∆)(2)12h2d1=arcsin(/)ddθ∆)(3)式中，d1d为障碍物中心底面投影到坐标原点的距离，h1d与h2d分别为投影中心到关节1坐标系原点和关节4坐标系原点连线的距离。令1min11112θθθ=−∆，1max11111θθθ=+∆，则可能发生碰撞关节1部分运动范围为：[]11min11max1,θθθ∈。又由于1θ转动具有对称性，负向时角度范围为1min21111πθθθ=−−∆，1max21112πθθθ=−+∆。由此可得可能发生碰撞的关节1的另一部分运动范围为：11min21max2[,]θθθ∈。综上，可能发生碰撞关节角1运动范围为：11θΩ∈，其中：11min11max11min21max2[,][,]Ωθθθθ=∪。(2)杆件2与障碍发生碰撞条件。在可能发生碰撞的关节角1的运动范围内，任意给定一个1θ，求出21p(杆2的起始点)的位置及杆2的方向矢量，并由障碍域中心位置与21p的距离1d判断球是否在杆2的碰撞范围内。关节2发生碰撞的极限位置如图4所示。机械工程学报第46卷第13期期112图4碰撞发生的关节2极限位置图如果12daR+，则不在其范围之内，无须进行进一