实现空间机械臂避障的路径规划方法技术

本方法主要用于空间机械臂避障路径规划。包括以下步骤：建立窗口型(正方形)障碍，利用人工势场方法找出一条机械臂末端器的避障路径；通过求机械臂各连杆与窗口障碍各边的距离的碰撞检测来调整整个机械臂末端器轨迹，使得各个连杆均不与障碍物碰撞，从而得到一条能使整个机械臂(包括末端器和连杆)安全避障的末端器路径；运用基于广义雅克比矩阵的空间机械臂运动学逆解对上述的末端器路径上的位置和姿态进行连续跟踪。应用该障碍回避方法可使其针对整个空间机械臂(包括末端器和连杆)进行避障路径规划。

【技术实现步骤摘要】
实现空间机械臂避障的路径规划方法
本专利技术属于空间机械臂的路径规划领域，尤其涉及一种实现空间机械臂的避障路径规划方法。
技术介绍
由于太空任务环境复杂，使用机械臂代替宇航员进行工作是未来航空航天的主要发展方向，而空间机械臂的避障路径规划对机械臂在太空环境中的实际应用起到至关重要的作用。避障路径规划是指给定环境的障碍条件以及起始位姿和目标位姿，要求选择一条从起始点到目标点的路径，使机械臂能安全、无碰撞地通过所有的障碍。关于机械臂的避障路径规划方法有很多，如滚动路径规划法、StentzA的D*算法、遗传算法等，比较经典的是用蚁群算法，其具有极强的鲁棒性和搜索较好解的能力。但是，这种方法多用于2维平面或者2维等高平面，在3维空间中实现基于蚁群算法的避障需要增加大量的计算。而人工势场法是以其快捷的环境描述形式在实时避障、运动规划中得到了广泛应用。人工势场法由Khatib提出，他在文献中构造了人工感应力函数。此后，人工势场法得到不断的改进与完善。Khosla提出超二次函数(SuperquadricFunction)，将不同形状障碍物斥力场统一逼近为球形障碍物的斥力场，又提出从电场、温度场、流体场的角度，运用调和函数(HarmonicFunction)来回避局部最小问题。目前的研究主要集中在地面机器人的路径规划问题以及对空间机械臂末端器避障路径规划的研究，而对整个空间机械臂(包括连杆和末端器)避障的研究还极为有限。受到人工势场方法的启发，本专利技术对其进行扩展，用以对空间机械臂整体进行避障路径规划。针对空间机械臂的控制，由于太空环境中动量守恒，空间机械臂存在动力学冗余特性，空间机械臂末端器的位姿不仅和关节角有关还和机械臂运动的历史有关。所以我们使用广义雅可比矩阵来描述空间机械臂的系统状态。由于单纯的计算逆广义雅可比不可避免的会带来奇异，导致奇异点附近的关节角速度无限大，所以本专利技术通过计算广义雅可比的阻尼最小方差逆的方法来求解关节角，以对空间机械臂进行运动控制。
技术实现思路
本专利技术的目的在于：将2维人工势场方法扩展成适用于3维空间；改进人工势场方法使其能够针对整个机械臂(包括末端器和连杆)进行避障规划并将上述方法与基于广义雅可比的空间机械臂逆运动学方法相结合，进行空间机械臂连续无碰撞路径规划。首先建立窗口型(正方形)障碍，利用人工势场方法找出一条机械臂末端器的避障路径；然后通过求机械臂各连杆与窗口障碍各边的距离的碰撞检测来调整整个机械臂末端器轨迹，使得各个连杆均不与障碍物碰撞，从而得到一条使整个机械臂能安全避障的末端器路径；最后运用基于广义雅克比矩阵的空间机械臂运动学逆解对所述的末端器路径进行连续跟踪。下面叙述算法具体步骤：本专利技术的一种实现空间机械臂避障的路径规划方法，其特征在于，包括步骤：(1)建立中心点带有引力场的窗口型(正方形)障碍，计算其几何中心点，在该点上设置一个带吸引力的虚拟点，该点吸引力与目标点吸引力公式均为：(2)采用人工势场方法，建立斥力场函数和引力场函数，斥力公式为：(3)判断机械臂末端器是否穿过障碍物边框内侧斥力场，若不穿过，转到下一步，若穿过，转到(5)；(4)调整中心点吸引力，使末端器从窗口内侧穿过；(5)在中心点引力和边框斥力的作用下，末端器趋于向窗口中心运动，当末端器穿过窗口向目标点运动时，中心点的引力消失，末端器受到的引力只来自目标点；(6)得到末端器的避障路径后，用逆广义雅克比矩阵跟踪该路径，结合碰撞检测公式其中：i＝2,3,4,5,6；j＝2,3,4。Di,j为各连杆与障碍物之间的距离，Aj为连杆前端到连杆质心的方向矢量；检测连杆与边框碰撞的深度与方向，根据返回结果，相应调整末端器轨迹，反复对各杆检测，最终得到一条使整个机械臂避障的末端器轨迹。根据以上所述的实现空间机械臂避障的路径规划方法，优选，根据以上所述的方法，在步骤(6)中，若成功，输出该路径，若迭代n次仍不成功，输出不存在这样的轨迹。本专利技术与现有技术相比具有以下优点：1、人工势场方法一般的是应用在2维空间中，本专利技术将2维人工势场方法扩展成适用于3维空间。2、一般的空间机械臂避障路径规划都只是控制机械臂末端器避障，本专利技术可以使整个机械臂(包括末端器和连杆)进行避障规划。附图说明以下通过附图及具体实施例对本专利技术进行详细说明。图1是本专利技术实施例提供的障碍回避的人工势场方法的流程图；图2是本专利技术中用到的6自由度机械臂结构示意图；图3是本专利技术实施例提供的窗口型障碍物；图4是本专利技术中用到的机械臂在人工势场中的受力；图5是本专利技术实施例提供的基础人工势场方法的避障图；图6是本专利技术实施例提供的扩展人工势场前后末端器轨迹比较图；图7是本专利技术实施例提供的机械臂关节角曲线图；图8是本专利技术实施例提供的机械臂关节角速度曲线图；图9是本专利技术实施例提供的机械臂跟踪路径的末端器位姿3D图；具体实施方式本专利技术的技术方案是：首先建立窗口型(正方形)障碍，利用人工势场方法找出一条机械臂末端器的避障路径；然后通过求机械臂各连杆与窗口障碍各边的距离的碰撞检测来调整整个机械臂末端器轨迹，使得各个连杆均不与障碍物碰撞，从而得到一条使整个机械臂能安全避障的末端器路径；最后运用基于广义雅克比矩阵的空间机械臂运动学逆解对上述的末端器路径和姿态进行连续跟踪。其具体包括以下环节：1.建立空间机械臂运动学模型6自由度机械臂结构示意图如附图2。该机械臂的D-H参数如表一所示：表一：6自由度空间机械臂的D-H参数符号表示：该机械臂的广义雅克比矩阵的逆解公式为：由于空间机械臂在运动规划过程中存在因动力学奇异而导致的关节速度无限大或无法计算的问题,因此在运用逆广义雅克比矩阵求机械臂各关节角之前要先避奇异。本文采用的是阻尼最小方差法(DLS):J*＝(JmTJm+λ2IM)-1JmT为最小奇异值的估计值，ε为选定的用于判断机械臂是否奇异的阈值，λm为用户设定的奇异区域的最大阻尼值。而是通过对广义雅克比矩阵进行SVD分解得到的。该等式的意义为：对于任意矩阵J可分解为S、VT三个矩阵的点乘。其中：V均为正交阵(均为n×n矩阵)，S为对角阵，对角线上的元素s1，s2，…..，s6是广义雅克比矩阵的奇异值，且满足s1≥s1≥…≥s6≥02.建立窗口型(正方形)障碍窗口型障碍物如附图3。3.利用人工势场方法找出一条机械臂末端器的避障路径人工势场法可以采用不同的势场函数表达形式，其产生的规划路径有所区别，但他们的基本原理一致。常用的势场法是梯度势场法。势场的负梯度作为作用在某点上的虚拟力，障碍物对末端器产生斥力，目标点产生引力，引力和斥力的合力作这个点上的加速力，该力“推动"该点向着目标做无碰运动,其受力分析如附图4所示。以空间机械臂为例，在任意一个状态下，机械臂末端器的位姿可以用q的变化q′表示，势场可以用U(q)表示，目标状态位姿可用qg来表示，并定义和目标位姿qg相关联的吸引势Uatt(q)，以及和障碍物Uobs(q)相关联的排斥势Urep(q)。那么，位姿空间中某一位姿的势能场可以表示如公式(3-1)：Uq＝Uatt(q)+Urep(q)我们规定对于所处空间中的每一个位姿，U(q)都必须是可微分的。那么，末端器所受到的虚拟力为目标位姿的吸引力和障碍物的斥力的合力，按照势场力的定义，势场力本文档来自技高网...

【技术保护点】
实现空间机械臂避障的路径规划方法，其特征在于，包括步骤：(1)建立中心点带有引力场的窗口型(正方形)障碍，计算其几何中心点，在该点上设置一个带吸引力的虚拟点，该点吸引力与目标点吸引力公式均为：▿U(q)=∂U∂x∂U∂y∂U∂z]]>(2)采用人工势场方法，建立斥力场函数和引力场函数，斥力公式为：Uatt(q)=12ξρ2(q)]]>(3)判断机械臂末端器是否穿过障碍物边框内侧斥力场，若不穿过，转到下一步，若穿过，转到(5)；(4)调整中心点吸引力，使末端器从窗口内侧穿过；(5)在中心点引力和边框斥力的作用下，末端器趋于向窗口中心运动，当末端器穿过窗口向目标点运动时，中心点的引力消失，末端器受到的引力只来自目标点；(6)得到末端器的避障路径后，用逆广义雅克比矩阵跟踪该路径，结合碰撞检测公式Di,j=|(pi-1,i×Aj-1,j)·(pi-1-Aj-1)|||pi-1,i×Aj-1,j||]]>其中：i＝2,3,4,5,6；j＝2,3,4。Di,j为各连杆与障碍物之间的距离，Aj为连杆前端到连杆质心的方向矢量；检测连杆与边框碰撞的深度与方向，根据返回结果，相应调整末端器轨迹，反复对各杆检测，最终得到一条使整个机械臂避障的末端器轨迹。...

【技术特征摘要】
1.实现空间机械臂避障的路径规划方法，其特征在于，包括步骤：(1)建立中心点带有引力场的窗口型障碍，计算其几何中心点，在该点上设置一个带吸引力的虚拟点，该点吸引力与目标点吸引力公式均为：式中表示U在q处的梯度；(2)采用人工势场方法，建立斥力场函数和引力场函数，吸引力公式：排斥力公式：(3)判断机械臂末端器是否穿过障碍物边框内侧斥力场，若不穿过，转到下一步，若穿过，转到(5)；(4)调整中心点吸引力，使末端器从窗口内侧穿过；(5)在中心点引力和边框斥力的作用下，末端器趋于向窗口中心...

【专利技术属性】
技术研发人员：张强，刘双，
申请(专利权)人：大连大学，
类型：发明
国别省市：辽宁;21