CN104029203A - 实现空间机械臂避障的路径规划方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于空间机械臂的路径规划领域，尤其涉及一种实现空间机械臂的避障路径规划方法。

背景技术

由于太空任务环境复杂，使用机械臂代替宇航员进行工作是未来航空航天的主要发展方向，而空间机械臂的避障路径规划对机械臂在太空环境中的实际应用起到至关重要的作用。

避障路径规划是指给定环境的障碍条件以及起始位姿和目标位姿，要求选择一条从起始点到目标点的路径，使机械臂能安全、无碰撞地通过所有的障碍。

关于机械臂的避障路径规划方法有很多，如滚动路径规划法、Stentz A的D*算法、遗传算法等，比较经典的是用蚁群算法，其具有极强的鲁棒性和搜索较好解的能力。但是，这种方法多用于2维平面或者2维等高平面，在3维空间中实现基于蚁群算法的避障需要增加大量的计算。而人工势场法是以其快捷的环境描述形式在实时避障、运动规划中得到了广泛应用。人工势场法由Khatib提出，他在文献中构造了人工感应力函数。此后，人工势场法得到不断的改进与完善。Khosla提出超二次函数(Superquadric Function)，将不同形状障碍物斥力场统一逼近为球形障碍物的斥力场，又提出从电场、温度场、流体场的角度，运用调和函数(Harmonic Function) 来回避局部最小问题。目前的研究主要集中在地面机器人的路径规划问题以及对空间机械臂末端器避障路径规划的研究，而对整个空间机械臂(包括连杆和末端器)避障的研究还极为有限。受到人工势场方法的启发，本发明对其进行扩展，用以对空间机械臂整体进行避障路径规划。

针对空间机械臂的控制，由于太空环境中动量守恒，空间机械臂存在动力学冗余特性，空间机械臂末端器的位姿不仅和关节角有关还和机械臂运动的历史有关。所以我们使用广义雅可比矩阵来描述空间机械臂的系统状态。由于单纯的计算逆广义雅可比不可避免的会带来奇异，导致奇异点附近的关节角速度无限大，所以本发明通过计算广义雅可比的阻尼最小方差逆的方法来求解关节角，以对空间机械臂进行运动控制。

发明内容

本发明的目的在于：将2维人工势场方法扩展成适用于3维空间；改进人工势场方法使其能够针对整个机械臂(包括末端器和连杆)进行避障规划并将上述方法与基于广义雅可比的空间机械臂逆运动学方法相结合，进行空间机械臂连续无碰撞路径规划。

首先建立窗口型(正方形)障碍，利用人工势场方法找出一条机械臂末端器的避障路径；然后通过求机械臂各连杆与窗口障碍各边的距离的碰撞检测来调整整个机械臂末端器轨迹，使得各个连杆均不与障碍物碰撞，从而得到一条使整个机械臂能安全避障的末端器路径；最后运用基于广义雅克比矩阵的空间机械臂运动学逆解对所述的末端器路径进行连续跟踪。下面叙述算法具体步骤：

本发明的一种实现空间机械臂避障的路径规划方法，其特征在于，包括步骤：

(1)建立中心点带有引力场的窗口型(正方形)障碍，计算其几何中心点，在该点上设置一个带吸引力的虚拟点，该点吸引力与目标点吸引力公式 均为：

$\▿U\left(q\right)=\left[\begin{array}{c}\frac{\∂U}{\∂x}\\ \frac{\∂U}{\∂y}\\ \frac{\∂U}{\∂z}\end{array}\right]$

(2)采用人工势场方法，建立斥力场函数和引力场函数，斥力公式为：

$U_{\mathrm{att}}\left(q\right)=\frac{1}{2}{\mathrm{\ξ\ρ}}^2\left(q\right)$

(3)判断机械臂末端器是否穿过障碍物边框内侧斥力场，若不穿过，转到下一步，若穿过，转到(5)；

(4)调整中心点吸引力，使末端器从窗口内侧穿过；

(5)在中心点引力和边框斥力的作用下，末端器趋于向窗口中心运动，当末端器穿过窗口向目标点运动时，中心点的引力消失，末端器受到的引力只来自目标点；

(6)得到末端器的避障路径后，用逆广义雅克比矩阵跟踪该路径，结合碰撞检测公式

$D_{i,j}=\frac{|(p_{i-1,i}\×A_{j-1,j})\·(p_{i-1}-A_{j-1})|}{\left|\right|p_{i-1,i}\×A_{j-1,j}\left|\right|}$

其中：i＝2,3,4,5,6；j＝2,3,4。D_i,j为各连杆与障碍物之间的距离，A_j为连杆前端到连杆质心的方向矢量；

检测连杆与边框碰撞的深度与方向，根据返回结果，相应调整末端器轨迹，反复对各杆检测，最终得到一条使整个机械臂避障的末端器轨迹。

根据以上所述的实现空间机械臂避障的路径规划方法，优选，根据权利要求1的方法，在步骤(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参数

符号表示：

r₀∈R³ 惯性坐标系中基座质心的位置矢量；

r_i∈R³(i＝1,...,n) 惯性坐标系中连杆i质心的位置矢量；

r_g∈R³ 惯性坐标系中系统质心的位置矢量；

b₀∈R³ 惯性坐标系中基座质心到连杆1质心的位置矢 量；

p_i∈R³(i＝1,...,n) 惯性坐标系中连杆i质心的位置矢量；

p_e∈R³ 惯性坐标系中末端器的位置矢量；

v₀∈R³ 某一时刻基座线速度；

v_e∈R³ 某一时刻末端器线速度；

ω₀∈R³ 某一时刻基座角速度；

ω_e∈R³ 某一时刻末端器角速度；

k_i∈R³(i＝1,...,n) 连杆i的Z轴单位方向矢量Σ_i(i＝1,...,n)；

θ_i 关节角i；

Θ∈Rⁿ 关节角速度；

m_i 连杆i的质量；

M 系统质量；

r_i 连杆i的半径(注意:不要把r_i和r_i弄混了)；

l_i 连杆i的长度；

η＝cos(ψ/2) 四元数表示的标量部分；

四元数表示的向量部分；

交叉算子(相当于“×”)；

如果 $r=\left[\begin{array}{c}{r}_x\\ r_y\\ r_z\end{array}\right],$ 那么 $\tilde{r}=\left[\begin{array}{ccc}0& -r_z& r_y\\ r_z& 0& -r_x\\ -r_y& r_x& 0\end{array}\right]$

I_i∈R^3×3 连杆i的惯性矩阵(所有的连杆均被看做圆柱)：

$I_i=\left[\begin{array}{ccc}\frac{1}{12}(l_i^2+3r_i^2)m_i& 0& 0\\ 0& \frac{1}{12}(l_i^2+3r_i^2)m_i& 0\\ 0& 0& \frac{1}{12}{r}_i^2{m}_i\end{array}\right]$

Ψ_b∈R³ 用欧拉角表示的基座姿态；

J^*(Ψ_b,Θ,m_i,I_i) 系统的广义雅可比矩阵；

ε 选定的用于判断机械臂是否奇异的阈值；

λ_m 为用户设定的奇异区域的最大阻尼值；

ρ(q)＝||q-q(g)|| 从q到q_g的欧式距离；

相应的正比例位置增益系数；

ρ₀ 正常数，表示障碍物区域可对被吸引点的运动产生影响的最大距离；

ρ(q) 障碍物区域C_obs位姿q的最小距离，也就是说对于所有的q'∈C_obs；

该机械臂的广义雅克比矩阵的逆解公式为：

$\stackrel{\·}{\mathrm{\Θ}}={\left[J^{*}({\mathrm{\Ψ}}_b,\mathrm{\Θ},m_i,I_i)\right]}^{-1}\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]$

由于空间机械臂在运动规划过程中存在因动力学奇异而导致的关节速度无限大或无法计算的问题,因此在运用逆广义雅克比矩阵求机械臂各关节角之前要先避奇异。本文采用的是阻尼最小方差法(DLS):

J^*＝(J_m ^TJ_m+λ²I_M)^-1J_m ^T

${\mathrm{\λ}}^2=\left\{\begin{array}{cc}0,& \mathrm{if}{\hat{s}}_6\≥\mathrm{\ϵ}\\ (1-{\left(\frac{{\hat{s}}_6}{\mathrm{\ϵ}}\right)}^2){{\mathrm{\λ}}_m}^2,& \mathrm{otherwise}\end{array}\right.$

为最小奇异值的估计值，ε为选定的用于判断机械臂是否奇异的阈值，λ_m为用户设定的奇异区域的最大阻尼值。而是通过对广义雅克比矩阵进行SVD分解得到的。

$J=\stackrel{\‾}{U}{\mathrm{SV}}^T$

该等式的意义为：对于任意矩阵J可分解为S、V^T三个矩阵的点乘。其中：V均为正交阵(均为n×n矩阵)，S为对角阵，对角线上的元素s₁，s₂，…..，s₆是广义雅克比矩阵的奇异值，且满足s₁≥s₁≥…≥s₆≥0

$S=\left[\begin{array}{cccccc}{s}_1& 0& 0& 0& 0& 0\\ 0& s_2& 0& 0& 0& 0\\ 0& 0& s_3& 0& 0& 0\\ 0& 0& 0& s_4& 0& 0\\ 0& 0& 0& 0& s_5& 0\\ 0& 0& 0& 0& 0& s_6\end{array}\right]$

2.建立窗口型(正方形)障碍

窗口型障碍物如附图3。

3.利用人工势场方法找出一条机械臂末端器的避障路径

人工势场法可以采用不同的势场函数表达形式，其产生的规划路径有所区别，但他们的基本原理一致。常用的势场法是梯度势场法。势场的负梯度作为作用在某点上的虚拟力，障碍物对末端器产生斥力，目标点产生引力，引力和斥力的合力作这个点上的加速力，该力“推动"该点向着目标做无碰运动,其受力分析如附图4所示。

以空间机械臂为例，在任意一个状态下，机械臂末端器的位姿可以用q的变化q'表示，势场可以用U(q)表示，目标状态位姿可用q_g来表示，并定义和目标位姿q_g相关联的吸引势U_att(q)，以及和障碍物U_obs(q)相关联的排斥势U_rep(q)。那么，位姿空间中某一位姿的势能场可以表示如公式(3-1)：

U_q＝U_att(q)+U_rep(q)

我们规定对于所处空间中的每一个位姿，U(q)都必须是可微分的。那么，末端器所受到的虚拟力为目标位姿的吸引力和障碍物的斥力的合力，按照势场力的定义，势场力是势场函数的负梯度，吸引力、排斥力以及合力如下式所示：

${\stackrel{\‾}{F}}_{\mathrm{att}}\left(q\right)=-\mathrm{grad}\left[U_{\mathrm{att}}\left(q\right)\right]$

${\stackrel{\‾}{F}}_{\mathrm{rep}}\left(q\right)=-\mathrm{grad}\left[U_{\mathrm{rep}}\left(q\right)\right]$

${\stackrel{\‾}{F}}_{\mathrm{sum}}\left(q\right)=-\▿U\left(q\right)=-\mathrm{grad}\left[U_{\mathrm{att}}\left(q\right)\right]-\mathrm{grad}\left[U_{\mathrm{rep}}\left(q\right)\right]$

式中▽U(q)表示U在q处的梯度，它是一个向量，其方向是位姿q所处势场变化率最大的方向。那么，对于三维空间中的位姿q(x,y,z)来说，有：

$\▿U\left(q\right)=\left[\begin{array}{c}\frac{\∂U}{\∂x}\\ \frac{\∂U}{\∂y}\\ \frac{\∂U}{\∂z}\end{array}\right]$

对于势场U(q)的定义方式可以有很多种，对于吸引势U_att(q)和排斥势U_rep(q)最常用的定义为静电场势场模型，如下式：

$U_{\mathrm{att}}\left(q\right)=\frac{1}{2}{\mathrm{\ξ\ρ}}^2\left(q\right)$

$U_{\mathrm{rep}}\left(q\right)=\left\{\begin{array}{cc}\frac{1}{2}\mathrm{\η}(\frac{1}{\mathrm{\ρ}\left(q\right)}-\frac{1}{{\mathrm{\ρ}}_0})& \mathrm{\ρ}\left(q\right)\≤{\mathrm{\ρ}}_0\\ 0& \mathrm{\ρ}\left(q\right)>{\mathrm{\ρ}}_0\end{array}\right.$

其中：ξ为增益系数，ρ(q)＝min||q-q'||。

这样，势场区域被局限在障碍物表面上。而且当q无限接近于C_obs时，U_rep(q)趋近于无穷大。结合前面几个公式，可以分别得到被吸引点所受吸引力和排斥力为：

${\stackrel{\‾}{F}}_{\mathrm{att}}\left(q\right)=-\mathrm{grad}\left[U_{\mathrm{att}}\left(q\right)\right]=\mathrm{\ξ}(q-q_g)$

${\stackrel{\‾}{F}}_{\mathrm{rep}}\left(q\right)=-\mathrm{grad}\left[U_{\mathrm{rep}}\left(q\right)\right]=\left\{\begin{array}{cc}\frac{\mathrm{\η}}{{\mathrm{\ρ}}^2\left(q\right)}(\frac{1}{\mathrm{\ρ}\left(q\right)}-\frac{1}{{\mathrm{\ρ}}_0})\▿\mathrm{\ρ}\left(q\right)& \mathrm{\ρ}\left(q\right)\≤{\mathrm{\ρ}}_0\\ 0& \mathrm{\ρ}\left(q\right)>{\mathrm{\ρ}}_0\end{array}\right.$

用q_c表示障碍物区域C_obs上距离q最近的位姿点，也就是ρ(g)＝||q-q_c||。则▽ρ(q)是由q_c指向q的单位向量，即：

$\▿\mathrm{\ρ}\left(q\right)=\frac{q-q_c}{\left|\right|q-q_c\left|\right|}$

被吸引点所受到的合力为：

${\stackrel{\‾}{F}}_{\mathrm{sum}}\left(q\right)={\stackrel{\‾}{F}}_{\mathrm{att}}\left(q\right)+{\stackrel{\‾}{F}}_{\mathrm{rep}}\left(q\right)$

这个合力决定了被吸引点的运动。在势场中，被吸引点在三维空间运动并始终受到来到目标节点的引力作用，目标节点决定了被吸引点的整体运动方向。当机器人运动到障碍节点的作用区间后会受到斥力作用，机器人在引力和斥力的合力作用下避开障碍寻找一条碰撞避免路径。

为了使机械臂能够经过窗口内测到达目标点并且避免与窗口框体碰撞,本论文在窗口型障碍物的中点设置了一个具有吸引力的虚拟点，该点并不像目标点一样拥有全局的吸引力，仅当末端器通过窗口平面之前存在，当末端器通过窗口平面之后，该引力点消失，以免其引力与目标点引力形成力平衡。 当空间机械臂末端器接近窗口时，由于此虚拟点的吸引力使机械臂趋于向窗口中心运动。

4.利用碰撞检测来调整整个机械臂末端器轨迹使整个机械臂能安全避障的末端器路径

机械臂各杆的避障规划是通过计算机械臂各杆与窗口各边的距离实现的。如果二者的距离小于设定的值，那么之前由人工势场得到的末端器轨迹会在该点处适当上移并增加虚拟障碍物，调整机械臂连杆不与窗口碰撞。碰撞检测公式如下式。从而得到一条满足各机械臂连杆与窗口边缘无碰撞的末端器轨迹。为了最大的减小各连杆与窗口边缘碰撞的可能，调整机械臂末端器，使其正好垂直穿过窗口平面，之后再向目标姿态进行调整，最后用逆运动学方法，对该轨迹和姿态进行跟踪。

$D_{i,j}=\frac{|(p_{i-1,i}\×A_{j-1,j})\·(p_{i-1}-A_{j-1})|}{\left|\right|p_{i-1,i}\×A_{j-1,j}\left|\right|}$

其中：i＝2,3,4,5,6；j＝2,3,4。D_i,j为各连杆与障碍物之间的距离，A_j为连杆前端到连杆质心的方向矢量。

5.基于改进人工势场方法的空间机械臂的避障路径规划

首先建立窗口型(正方形)障碍，利用人工势场方法找出一条机械臂末端器的避障路径；然后通过求机械臂各连杆与窗口障碍各边的距离的碰撞检测来调整整个机械臂末端器轨迹，使得各个连杆均不与障碍物碰撞，从而得到一条使整个机械臂能安全避障的末端器路径；最后运用基于广义雅克比矩阵的空间机械臂运动学逆解对上述的末端器路径和姿态进行连续跟踪。下面叙述算法具体步骤：

1)、实现空间机械臂避障的路径规划方法，其特征在于，包括步骤：

(1)建立中心点带有引力场的窗口型(正方形)障碍，计算其几何中心点，在该点上设置一个带吸引力的虚拟点，该点吸引力与目标点吸引力公式均为：

$\▿U\left(q\right)=\left[\begin{array}{c}\frac{\∂U}{\∂x}\\ \frac{\∂U}{\∂y}\\ \frac{\∂U}{\∂z}\end{array}\right]$

(2)采用人工势场方法，建立斥力场函数和引力场函数，斥力公式为：

$U_{\mathrm{att}}\left(q\right)=\frac{1}{2}{\mathrm{\ξ\ρ}}^2\left(q\right)$

(3)判断机械臂末端器是否穿过障碍物边框内侧斥力场，若不穿过，转到下一步，若穿过，转到(5)；

(4)调整中心点吸引力，使末端器从窗口内侧穿过；

(5)在中心点引力和边框斥力的作用下，末端器趋于向窗口中心运动，当末端器穿过窗口向目标点运动时，中心点的引力消失，末端器受到的引力只来自目标点；

(6)得到末端器的避障路径后，用逆广义雅克比矩阵跟踪该路径，结合碰撞检测公式

$D_{i,j}=\frac{|(p_{i-1,i}\×A_{j-1,j})\·(p_{i-1}-A_{j-1})|}{\left|\right|p_{i-1,i}\×A_{j-1,j}\left|\right|}$

其中：i＝2,3,4,5,6；j＝2,3,4。D_i,j为各连杆与障碍物之间的距离，A_j为连杆前端到连杆质心的方向矢量；

检测连杆与边框碰撞的深度与方向，根据返回结果，相应调整末端器轨迹，反复对各杆检测，最终得到一条使整个机械臂避障的末端器轨迹。

2)、实现空间机械臂避障的路径规划方法，其特征在于，根据权利要求1的方法，在步骤(6)中，若成功，输出该路径，若迭代n次仍不成功，输出不存在这样的轨迹。

本发明的实施例是在以本发明技术方案为前提下进行实施的，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述实施例。各连杆的质量特性及仿真参数表如表二、三。

表二：空间机械臂各连杆的质量特性

表三：仿真参数表

具体实施步骤为：

步骤1：基础人工势场避障

通过仿真得到有窗口障碍物的末端器的路径，由于窗口边框对末端器斥力的作用使末端器趋于窗口下方运动导致了机械臂杆碰到窗口边框。附图5 用于作比较。图中各曲线从下到上、从左到右依次表示基座、机械臂6个关节及末端器的运动轨迹，红色轨迹为末端器运动轨迹。

步骤2：扩展人工势场避障

附图6是扩展人工势场前后末端器轨迹比较图。图中下面蓝色的曲线表示扩展人工势场之前末端器不避障的轨迹，上面绿色的轨迹表示扩展人工势场后避障的末端器轨迹。图中右边黑色的点表示初始点，左边红色的点表示目标点。附图7是机械臂关节角曲线图。附图8机械臂关节角速度曲线图。

机械臂末端器的线速度和角速度的变化是按梯形规划法规划的。机械臂从初始点到目标点的运动时间为t_f，加速与减速的时间均为t_b，那么机械臂末端器匀速运动的时间为t_f-2t_b。

步骤3：空间机械臂用逆广义雅克比矩阵跟踪避障路径

附图9是机械臂跟踪路径的末端器位姿3D图。从上图中可以看出，整个机械臂包括末端器和各连杆均安全避障通过窗口障碍物。

Claims (2)

1.实现空间机械臂避障的路径规划方法，其特征在于，包括步骤：

(1)建立中心点带有引力场的窗口型(正方形)障碍，计算其几何中心点，在该点上设置一个带吸引力的虚拟点，该点吸引力与目标点吸引力公式均为：

$\▿U\left(q\right)=\left[\begin{array}{c}\frac{\∂U}{\∂x}\\ \frac{\∂U}{\∂y}\\ \frac{\∂U}{\∂z}\end{array}\right]$

(2)采用人工势场方法，建立斥力场函数和引力场函数，斥力公式为：

$U_{\mathrm{att}}\left(q\right)=\frac{1}{2}{\mathrm{\ξ\ρ}}^2\left(q\right)$

(3)判断机械臂末端器是否穿过障碍物边框内侧斥力场，若不穿过，转到下一步，若穿过，转到(5)；

(4)调整中心点吸引力，使末端器从窗口内侧穿过；

(5)在中心点引力和边框斥力的作用下，末端器趋于向窗口中心运动，当末端器穿过窗口向目标点运动时，中心点的引力消失，末端器受到的引力只来自目标点；

(6)得到末端器的避障路径后，用逆广义雅克比矩阵跟踪该路径，结合碰撞检测公式 $D_{i,j}=\frac{|(p_{i-1,i}\×A_{j-1,j})\·(p_{i-1}-A_{j-1})|}{\left|\right|p_{i-1,i}\×A_{j-1,j}\left|\right|}$

其中：i＝2,3,4,5,6；j＝2,3,4。D_i,j为各连杆与障碍物之间的距离，A_j为连杆前端到连杆质心的方向矢量；

检测连杆与边框碰撞的深度与方向，根据返回结果，相应调整末端器轨迹，反复对各杆检测，最终得到一条使整个机械臂避障的末端器轨迹。

2.实现空间机械臂避障的路径规划方法，其特征在于，根据权利要求1的方法，在步骤(6)中，若成功，输出该路径，若迭代n次仍不成功，输出不存在这样的轨迹。