CN105700527A - 一种平面冗余度机器人避障及避奇异的路径规划方法 - Google Patents

Abstract

一种平面冗余度机器人避障及避奇异的路径规划方法，涉及机器人运动控制领域，尤其涉及一种平面冗余度机器人避障及避奇异的路径规划方法。本发明为解决现有平面冗余度机器人在协作过程中存在对工作人员伤害的风险和机器人末端运动至奇异位置时，关节速度会超出机器人允许范围的问题。本发明按以下步骤进行：本发明所述方法涉及两个同时进行的线程：线程一述及避障的路径规划各步骤；线程二述及避奇异的路径规划各步骤。本发明所述方法可以同时完成避障、避奇异的路径规划方法，使其在协作过程中不仅可以完成对协作者的回避，也可完成对其自身奇异构型的回避从而降低人机协作的风险并提高装配质量。本发明所述方法可应用于机器人运动控制领域。

Description

一种平面冗余度机器人避障及避奇异的路径规划方法

技术领域

本发明涉及机器人运动控制领域，尤其涉及一种平面冗余度机器人避障及避奇异的路径规划方法。

背景技术

随着电子产品的小型化和精密化，传统的机器人自动化装配不能很好的完成一些复杂精细化装配任务，而完全由人工完成此类产品的装配任务会使工作人员的劳动强度大大增加，从而影响产品的装配质量。目前代工厂商开始引入平面型机器人作为高速生产线的辅助性设备，协助工作人员完成电子产品的装配作业，降低工作人员的劳动强度并且提升装配质量。在流水线这种狭小空间内协助工人完成装配任务，人机碰撞的概率大大增加。这不仅对工人的生命安全存在很大的威胁，而且对产品的装配质量也会有一定的影响。

平面冗余度机器人，是指关节数大于2的平面关节型机器人，在不影响末端位姿的情况下可以通过其自运动特性实现多种运动优化目标。在人机协作装配过程中，由于装配流水线空间狭小，同一流水线上的协作者和协作机器人的工作空间大面积重叠，再加之此类机器人的工作速度较大，使得人机协作的风险大大增加。再者奇异构形附近冗余度机器人容易出现关节速度非常大，导致机器人末端执行器偏离预期轨迹，这将会对装配质量有一定的影响。

所以用于小型电子产品的人机协作机器人需要一种可以同时完成避障、避奇异的路径规划方法，使其在协作过程中不仅可以完成对协作者的回避，也可完成对其自身奇异构型的回避从而降低人机协作的风险并提高装配质量。目前，平面冗余度机器人运动控制领域内，尚无有关同时完成避障、避奇异的路径规划方法的报道。

发明内容

本发明基于如下理论：

1、避障的路径规划理论

首先计算障碍物和机器人连杆之间的最短距离，针对人机协作装配这类对路径规划方法实时性要求较高的应用场合，提出了一种改进的实时最小距离计算法。

针对人机协作完成装配任务的过程，协作者可考虑成随机的障碍物。平面冗余度机器人各关节上装有测量距离的传感器，可以实时测量机器人关节和障碍物之间的距离。在机器人运动的平面上，可以将障碍物等效成一个半径为R的圆盘，以平面三自由度冗余机器人为例，机器人和障碍物位置示意图如图1所示。

(1)采用本发明所述方法计算实时最小距离

先筛选出不能和障碍物发生碰撞的杆件

本发明所举的例子中连杆是由杆件1、杆件2和杆件3构成；

已知障碍物中心在坐标系{0}中的位置(x_t0,y_t0)，连杆截面为边长为a的正方形，障碍物半径为R，根据坐标系{1}相对于坐标系{0}的旋转矩阵，得出障碍物中心在坐标系{1}中的位置(x_t1,y_t1)，即

$\left[\begin{array}{c}{x}_{t1}\\ y_{t1}\\ 0\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_1& -{\mathrm{sin\θ}}_1& 0\\ {\mathrm{sin\θ}}_1& {\mathrm{cos\θ}}_1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t0}\\ y_{t0}\\ 0\end{array}\right]---\left(1\right)$

其中，θ₁为关节1的转角；

若(x_t1,y_t1)满足

$\left\{\begin{array}{c}\left|y_{A1}-y_{t1}\right|\≥R+a\\ x_{t1}\≥x_{B1}+R\end{array}\right.---\left(2\right)$

或 $\left\{\begin{array}{c}\left|y_{A1}-y_{t1}\right|\≥R+a\\ x_{A1}-R\≥x_{t1}\end{array}\right.---\left(3\right)$

杆件1为安全杆件，不用计算其和障碍物距离；

若(x_t1,y_t1)满足

$\left\{\begin{array}{c}\left|y_{A1}-y_{t1}\right|\≤R+a\\ x_{A1}-R\≤x_{t1}\≤x_{B1}+R\end{array}\right.---\left(4\right)$

其中，x_A1，y_A1为杆件1上A点在坐标系{1}中的位置坐标，x_B1为杆件1上B点在坐标系{1}中的位置横坐标；

则杆件1为可能发生碰撞的连杆，需计算其与障碍物之间的距离；

定义可能发生碰撞的杆件1上距离障碍物最近的点为标志点S，则该连杆上的标志点S在坐标系{1}中的坐标为(x_t1,0)，标志点S和障碍物之间的距离为：

d₁＝|y_A1-y_t1|

同理计算可能发生碰撞的杆件2和杆件3上的标志点和障碍物之间的距离d₂和d₃，并取d₁、d₂和d₃的最小值，即得到实时最小距离d_min。

(2)采用传统方法计算实时最小距离

一般都在基座坐标系下完成，障碍物在基座坐标系中的坐标为(x_t,y_t)，杆件1的方程可以写成

$\frac{(x-x_A)}{(x_B-x_A)}=\frac{(y-y_A)}{(y_B-y_A)}---\left(5\right)$

其中，x_A，y_A为杆件1上A点在基座坐标系中的坐标；x_B，y_B为杆件1上B点在基座坐标系中的坐标；

过障碍物圆心点(x_t,y_t)并垂直于杆件1的直线方程为

$\frac{(y-y_A)}{(x_B-x_A)}=\frac{(x_A-x)}{(y_B-y_A)}---\left(6\right)$

将公式(5)和(6)联立解出碰撞点(x_s,y_s)，进一步得出碰撞点到障碍物距离d₁，

$d_1=\sqrt{{(x_S-x_t)}^2+{(y_S-y_t)}^2}---\left(7\right)$

同理计算出杆件2和杆件3上碰撞点到障碍物距离d₂和d₃，进而计算出实时最小距离d_min。

本发明采用改进的距离计算方法，与传统的实时最小距离计算方法相比，提高了计算效率，更好地满足了动态避障对实时性的要求，主要体现在两个方面：

①在计算杆件距离之前，先筛选出可能发生碰撞的连杆，只计算此类杆件和障碍物的距离，大大减小了计算量；

②改进的距离计算方法采用侧坐标变换的方法计算距离，舍弃了传统方法中解方程组的繁琐计算，大大减少了计算量，提高了计算效率。

(3)避障的路径规划

在人机协作完成产品装配任务时，人与机器人的工作空间相互重叠，碰撞发生的可能性大大增加，再加之机器人运动至奇异位置附近时，关节速度会突然增大甚至超出关节速度限制，对协同工作的人员造成极大的安全隐患。避障避奇异路径规划方法可以在对障碍物回避的同时完成对奇异构型的回避，从而可以消除这些安全隐患，进一步提高人机协作的安全性。

机器人运动过程中的某一时刻，通过上述判断危险杆件的方法，筛选出危险杆件并求出杆件上的标志点S的位置，且该标志点S处的雅可比矩阵J_s满足

$J_S\stackrel{\·}{q}={\stackrel{\·}{x}}_S---\left(8\right)$

其中，J_S∈R^m×n为最靠近障碍物的标志点处的雅可比矩阵，为关节速度向量，为标志点的运动速度向量；

根据梯度投影法，冗余度机器人逆解为

$\stackrel{\·}{q}=J^{+}\stackrel{\·}{p}+(I-J^{+}J)H---\left(9\right)$

其中，为笛卡尔空间中机器人末端速度，I为单位矩阵，J∈R^m×n为机器人的雅克比矩阵，J⁺∈R^n×m为雅可比矩阵的广义逆，H为冗余度机器人零空间避障函数；

冗余度机器人零空间避障函数可由下式求得

$H={\[J_S\left(\right(I-J^{+}J\left)\right)\]}^{+}({\stackrel{\·}{x}}_S-J_S{J}^{+}\stackrel{\·}{x})---\left(10\right)$

其中，为机器人末端的运动速度向量；

那么基于梯度投影法的冗余度机器人避障运动学逆解为

$\stackrel{\·}{q}=J^{+}\stackrel{\·}{x}+(I-J^{+}J){\[J_S\left(\right(I-J^{+}J\left)\right)\]}^{+}({\stackrel{\·}{x}}_S-J_S{J}^{+}\stackrel{\·}{x})---\left(11\right)$

由于(I-J⁺J)的特殊性，引入避障系数a和避障增益b，公式(11)可写成

$\stackrel{\·}{q}=J^{+}\stackrel{\·}{x}+a{\[J_S\left(\right(I-J^{+}J\left)\right)\]}^{+}(b{\stackrel{\·}{x}}_S-J_S{J}^{+}\stackrel{\·}{x})---\left(12\right)$

上式中系数a，b和实时最小距离d_min有关；

定义两个距离d₁<d₂，当d_min<d₂时为危险区域，启动避障策略，当d_min<d₁时杆件为极度危险区域，开启避障增益策略；

$a=\left\{\begin{array}{cc}A& d_{\min }<d_2\\ 0& d_{\min }>d_2\end{array}\right.---\left(13\right)$

$b=\left\{\begin{array}{cc}B& d_{\min }<d_1\\ 0& d_{\min }>d_1\end{array}\right.---\left(14\right)$

对公式(12)求积分后，换算成关节转角。

2、避奇异的路径规划理论

机器人在奇异位置附近时关节速度容易超限，故引入DLS法对其进行优化；

首先对机器人雅可比矩阵进行奇异值分解：

J＝ABC^T(15)

其中，矩阵A和矩阵C都是正交矩阵,矩阵B由机器人雅可比矩阵的奇异值组成的对角矩阵：

$B=\left[\begin{array}{ccccccc}{\mathrm{\&sigma;}}_1& 0& .& .& .& .& 0\\ 0& {\mathrm{\&sigma;}}_2& 0& .& .& .& 0\\ .& .& .& .& .& .& .\\ 0& .& 0& {\mathrm{\&sigma;}}_m& 0& .& 0\end{array}\right]---\left(16\right)$

其中σ₁,σ₂,…,σ_m为机器人雅克比矩阵的奇异值，且σ₁≥σ₂≥…≥σ_m，σ_m是雅可比矩阵的最小奇异值，可以体现机器人接近奇异构形的程度；

针对冗余度机器人求逆解一般使用伪逆法，伪逆法的实质就是在保证末端轨迹误差最小的前提下求解关节速度的最小范数解，即末端跟踪精度的优先级高于关机速度的优先级；伪逆法为了保证跟踪精度最小，而没有考虑关节速度，为了使机械臂在奇异构型附近的关节速度不至于过大，使用DLS法，引入阻尼系数C，用以调节末端跟踪精度和关节速度的优先级别，即在保证

$\left|\right|\stackrel{\&CenterDot;}{p}-J{\stackrel{\&CenterDot;}{q}}^2\left|\right|+{\left(C\right|\left|\stackrel{\&CenterDot;}{q}\right|\left|\right)}^2---\left(17\right)$

最小的前提下，求解关节速度的最小范数解，

则机器人运动学逆解可以写成

$\stackrel{\&CenterDot;}{q}=J_1\stackrel{\&CenterDot;}{p}=J^T{({\mathrm{JJ}}^T+C^{2}I)}^{-1}\stackrel{\&CenterDot;}{p}---\left(18\right)$

其中，J₁＝J^T(JJ^T+C²I)^-1；

由于引入DLS法计算速度级逆解会降低机器人末端轨迹跟踪精度，则在远离奇异构型时采用传统的伪逆法，在接近奇异构型时，引入DLS法对奇异位置附近的机器人性能进行优化，即引入如下的阻尼系数C

$C^2=\left\{\begin{array}{cc}{C_0}^2(1-\frac{{\mathrm{\&sigma;}}_m}{{\mathrm{\&sigma;}}_0})& {\mathrm{\&sigma;}}_m<{\mathrm{\&sigma;}}_0\\ 0& {\mathrm{\&sigma;}}_m\&GreaterEqual;{\mathrm{\&sigma;}}_0\end{array}\right.---\left(19\right)$

其中，C₀为最大阻尼系数，σ_m为最小奇异值，σ₀为奇异值的下限

当最小奇异值σ_m低于奇异值下限σ₀时，表明机器人进入奇异区域，启动避奇异优化策略。当最小奇异值σ_m大于等于奇异值下限σ₀时，表明机械臂离奇异构型较远，避奇异优化策略不启动，速度级逆解退化为一般的伪逆法。

引入DLS法对上面公式(12)提出的避障算法进行避奇异优化，则避障避奇异协同优化解为

$\stackrel{\&CenterDot;}{q}=J_1\stackrel{\&CenterDot;}{x}+a{\&lsqb;J_S\left(\right(I-J^{+}J\left)\right)\&rsqb;}^{+}(b{\stackrel{\&CenterDot;}{x}}_S-J_S{J}^{+}\stackrel{\&CenterDot;}{x})---\left(20\right)$

其中，J₁＝J^T(JJ^T+C²I)^-1，

对公式(20)求积分后，换算成关节转角。

本发明为解决现有平面冗余度机器人在协作过程中存在对工作人员伤害的风险和机器人末端运动至奇异位置时，关节速度会超出机器人允许范围的问题，而提出一种平面冗余度机器人避障及避奇异的路径规划方法。

一种平面冗余度机器人避障及避奇异的路径规划方法，按以下步骤进行：

本发明所述方法涉及两个同时进行的过程：

过程一：避障的路径规划步骤如下：

步骤一：建立各杆件坐标系，并将各障碍物坐标投影到各杆件坐标系中；

步骤二：对杆件是否为安全杆件进行判断；

步骤二.一：如杆件为安全杆件，则剔除此杆件，不作任何处理；

步骤二.二：如杆件不是安全杆件，则转至步骤三；

步骤三：求出连杆和障碍物间的实时最小距离以及各杆件上标志点的坐标；

步骤四：对实时最小距离是否处于安全距离进行判断；

步骤四.一：如实时最小距离大于或等于安全距离，则转至步骤八；

步骤四.二：如实时最小距离小于安全距离，则转至步骤五；

步骤五：根据实时最小距离处于危险距离或极度危险距离，分别启动避障策略或启动避障增益策略；

步骤六：计算机器人雅可比矩阵和标志点处雅可比矩阵；

步骤七：逆运动学求解，积分得到关节的运动角度；

步骤八：不启动避障策略，继续执行机器人末端运动；

步骤九：判断机器人末端是否到达终点；

步骤九.一：机器人末端到达终点，结束

步骤九.二：机器人末端未到达终点，则转至步骤一；

过程二：避奇异的路径规划步骤如下：

步骤A：判断最小奇异值是否低于奇异值下限；

步骤A.一：最小奇异值大于或等于奇异值下限，转至步骤B；

步骤A.二：最小奇异值小于奇异值下限，转至步骤C；

步骤B：不启动避奇异策略，继续执行机器人末端运动，再转至步骤九；

步骤C：启动避奇异策略，再转至步骤七。

本发明包括以下有益效果：

1、本发明所述方法可以同时完成避障、避奇异的路径规划方法，使其在协作过程中不仅可以完成对协作者的回避，也可完成对其自身奇异构型的回避从而降低人机协作的风险并提高装配质量；

2、将避障、避奇异的两种路径规划过程设计成同时执行的两个线程，此二线程有机地结合在一起，不分先后时序，互相配合，共同组合成一个完整的设计构思；

3、与传统的实时最小距离计算方法相比，提高了计算效率，更好地满足了动态避障对实时性的要求，主要体现在两个方面：①在计算杆件距离之前，先筛选出可能发生碰撞的连杆，只计算此类杆件和障碍物的距离，大大减小了计算量；②改进的距离计算方法采用侧坐标变换的方法计算距离，舍弃了传统方法中解方程组的繁琐计算，大大减少了计算量，提高了计算效率。

附图说明

图1为平面三自由度冗余机器人和障碍物位置示意图；

图2为本发明所述方法的流程图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合图1、2和具体实施方式对本发明作进一步详细的说明。

具体实施方式一、本实施方式所述的一种平面冗余度机器人避障及避奇异的路径规划方法，按以下步骤进行：

本发明所述方法涉及两个同时进行的过程：

过程一：避障的路径规划步骤如下：

步骤一：建立各杆件坐标系，并将各障碍物坐标投影到各杆件坐标系中；

步骤二：对杆件是否为安全杆件进行判断；

步骤二.一：如杆件为安全杆件，则剔除此杆件，不作任何处理；

步骤二.二：如杆件不是安全杆件，则转至步骤三；

步骤三：求出连杆和障碍物间的实时最小距离以及各杆件上标志点的坐标；

步骤四：对实时最小距离是否处于安全距离进行判断；

步骤四.一：如实时最小距离大于或等于安全距离，则转至步骤八；

步骤四.二：如实时最小距离小于安全距离，则转至步骤五；

步骤五：根据实时最小距离处于危险距离或极度危险距离，分别启动避障策略或启动避障增益策略；

步骤六：计算机器人雅可比矩阵和标志点处雅可比矩阵；

步骤七：逆运动学求解，积分得到关节的运动角度；

步骤八：不启动避障策略，继续执行机器人末端运动；

步骤九：判断机器人末端是否到达终点；

步骤九.一：机器人末端到达终点，结束

步骤九.二：机器人末端未到达终点，则转至步骤一；

过程二：避奇异的路径规划步骤如下：

步骤A：判断最小奇异值是否低于奇异值下限；

步骤A.一：最小奇异值大于或等于奇异值下限，转至步骤B；

步骤A.二：最小奇异值小于奇异值下限，转至步骤C；

步骤B：不启动避奇异策略，继续执行机器人末端运动，再转至步骤九；

步骤C：启动避奇异策略，再转至步骤七。

本实施方式包括以下有益效果：

1、本实施方式所述方法可以同时完成避障、避奇异的路径规划方法，使其在协作过程中不仅可以完成对协作者的回避，也可完成对其自身奇异构型的回避从而降低人机协作的风险并提高装配质量；

2、将避障、避奇异的两种路径规划过程设计成同时执行的两个线程，此二线程有机地结合在一起，不分先后时序，互相配合，共同组合成一个完整的设计构思；

3、与传统的实时最小距离计算方法相比，提高了计算效率，更好地满足了动态避障对实时性的要求，主要体现在两个方面：①在计算杆件距离之前，先筛选出可能发生碰撞的连杆，只计算此类杆件和障碍物的距离，大大减小了计算量；②改进的距离计算方法采用侧坐标变换的方法计算距离，舍弃了传统方法中解方程组的繁琐计算，大大减少了计算量，提高了计算效率。

具体实施方式二、本实施方式是对具体实施方式一所述的一种平面冗余度机器人避障及避奇异的路径规划方法的进一步说明，步骤一的具体过程为：

本发明所述的连杆是由杆件1、杆件2和杆件3构成；

已知障碍物中心在坐标系{0}中的位置(x_t0,y_t0)，连杆截面为边长为a的正方形，障碍物半径为R，根据坐标系{1}相对于坐标系{0}的旋转矩阵，得出障碍物中心在坐标系{1}中的位置(x_t1,y_t1)，即

$\left[\begin{array}{c}{x}_{t1}\\ y_{t1}\\ 0\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_1& -{\mathrm{sin\&theta;}}_1& 0\\ {\mathrm{sin\&theta;}}_1& {\mathrm{cos\&theta;}}_1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t0}\\ y_{t0}\\ 0\end{array}\right]$

同理得出

$\left[\begin{array}{c}{x}_{t2}\\ y_{t2}\\ 0\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_2& -{\mathrm{sin\&theta;}}_2& 0\\ {\mathrm{sin\&theta;}}_2& {\mathrm{cos\&theta;}}_2& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t0}\\ y_{t0}\\ 0\end{array}\right],\left[\begin{array}{c}{x}_{t3}\\ y_{t3}\\ 0\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_3& -{\mathrm{sin\&theta;}}_3& 0\\ {\mathrm{sin\&theta;}}_3& {\mathrm{cos\&theta;}}_3& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t0}\\ y_{t0}\\ 0\end{array}\right].$

具体实施方式三、本实施方式是对具体实施方式一或二所述的一种平面冗余度机器人避障及避奇异的路径规划方法的进一步说明，步骤二的具体过程为：

若(x_t1,y_t1)满足

$\left\{\begin{array}{c}\left|y_{A1}-y_{t1}\right|\&GreaterEqual;R+a\\ x_{t1}\&GreaterEqual;x_{B1}+R\end{array}\right.$

或 $\left\{\begin{array}{c}\left|y_{A1}-y_{t1}\right|\&GreaterEqual;R+a\\ x_{A1}-R\&GreaterEqual;x_{t1}\end{array}\right.$

杆件1为安全杆件，不用计算其和障碍物距离；

若(x_t1,y_t1)满足

$\left\{\begin{array}{c}\left|y_{A1}-y_{t1}\right|\&le;R+a\\ x_{A1}-R\&le;x_{t1}\&le;x_{B1}+R\end{array}\right.---\left(4\right)$

其中，x_A1，y_A1为杆件1上A点在坐标系{1}中的位置坐标，x_B1为杆件1上B点在坐标系{1}中的位置横坐标；

则杆件1为可能发生碰撞的连杆，需计算其与障碍物之间的距离。

具体实施方式四、本实施方式是对具体实施方式一至三之一所述的一种平面冗余度机器人避障及避奇异的路径规划方法的进一步说明，步骤三的具体过程为：

定义可能发生碰撞的杆件1上距离障碍物最近的点为标志点S，则该连杆上的标志点S在坐标系{1}中的坐标为(x_t1,0)，标志点S和障碍物之间的距离为：

d₁＝|y_A1-y_t1|

同理计算可能发生碰撞的杆件2和杆件3上的标志点和障碍物之间的距离d₂和d₃，并取d₁、d₂和d₃的最小值，即得到实时最小距离d_min。

具体实施方式五、本实施方式是对具体实施方式一至四之一所述的一种平面冗余度机器人避障及避奇异的路径规划方法的进一步说明，步骤六的具体过程为：

标志点S处的雅可比矩阵J_s满足

$J_S\stackrel{\&CenterDot;}{q}={\stackrel{\&CenterDot;}{x}}_S$

其中，J_S∈R^m×n为最靠近障碍物的标志点处的雅可比矩阵，为关节速度向量，为标志点的运动速度向量；

根据梯度投影法，冗余度机器人逆解为

$\stackrel{\&CenterDot;}{q}=J^{+}\stackrel{\&CenterDot;}{p}+(I-J^{+}J)H$

其中，为笛卡尔空间中机器人末端速度，I为单位矩阵，J∈R^m×n为机器人的雅克比矩阵，J⁺∈R^n×m为雅可比矩阵的广义逆，H为冗余度机器人零空间避障函数；

冗余度机器人零空间避障函数可由下式求得

$H={\&lsqb;J_S\left(\right(I-J^{+}J\left)\right)\&rsqb;}^{+}({\stackrel{\&CenterDot;}{x}}_S-J_S{J}^{+}\stackrel{\&CenterDot;}{x})$

其中，为机器人末端的运动速度向量。

具体实施方式六、本实施方式是对具体实施方式一至五之一所述的一种平面冗余度机器人避障及避奇异的路径规划方法的进一步说明，步骤七的具体过程为：

基于梯度投影法的冗余度机器人避障运动学逆解为

$\stackrel{\&CenterDot;}{q}=J^{+}\stackrel{\&CenterDot;}{x}+(I-J^{+}J){\&lsqb;J_S\left(\right(I-J^{+}J\left)\right)\&rsqb;}^{+}({\stackrel{\&CenterDot;}{x}}_S-J_S{J}^{+}\stackrel{\&CenterDot;}{x})$

由于(I-J⁺J)的特殊性，引入避障系数a和避障增益b，上式可写成

$\stackrel{\&CenterDot;}{q}=J^{+}\stackrel{\&CenterDot;}{x}+a{\&lsqb;J_S\left(\right(I-J^{+}J\left)\right)\&rsqb;}^{+}(b{\stackrel{\&CenterDot;}{x}}_S-J_S{J}^{+}\stackrel{\&CenterDot;}{x})$

对上式求积分后，换算成关节转角，公式中系数a，b和实时最小距离d_min有关；

定义两个距离d₁<d₂，当d_min<d₂时为危险区域，启动避障策略，当d_min<d₁时杆件为极度危险区域，开启避障增益策略；

$a=\left\{\begin{array}{cc}A& d_{min}<d_2\\ 0& d_{min}>d_2\end{array}\right.$

$b=\left\{\begin{array}{cc}B& d_{\min }<d_1\\ 0& d_{\min }>d_1\end{array}\right.;$

引入DLS法对上面公式提出的避障算法进行避奇异优化，则避障避奇异协同优化解为

$\stackrel{\&CenterDot;}{q}=J_1\stackrel{\&CenterDot;}{x}+a{\&lsqb;J_S\left(\right(I-J^{+}J\left)\right)\&rsqb;}^{+}(b{\stackrel{\&CenterDot;}{x}}_S-J_S{J}^{+}\stackrel{\&CenterDot;}{x})$

对上式求积分后，换算成关节转角；

其中，J₁＝J^T(JJ^T+C²I)^-1。

具体实施方式七、本实施方式是对具体实施方式二至六之一所述的一种平面冗余度机器人避障及避奇异的路径规划方法的进一步说明，步骤一中所述的连杆由4、5、6或7个杆件构成。

Claims (7)

1.一种平面冗余度机器人避障及避奇异的路径规划方法，其特征在于它按以下步骤进行：

所述方法涉及两个同时进行的过程：

过程一：避障的路径规划步骤如下：

步骤一：建立各杆件坐标系，并将各障碍物坐标投影到各杆件坐标系中；

步骤二：对杆件是否为安全杆件进行判断；

步骤二.一：如杆件为安全杆件，则剔除此杆件，不作任何处理；

步骤二.二：如杆件不是安全杆件，则转至步骤三；

步骤三：求出连杆和障碍物间的实时最小距离以及各杆件上标志点的坐标；

步骤四：对实时最小距离是否处于安全距离进行判断；

步骤四.一：如实时最小距离大于或等于安全距离，则转至步骤八；

步骤四.二：如实时最小距离小于安全距离，则转至步骤五；

步骤五：根据实时最小距离处于危险距离或极度危险距离，分别启动避障策略或启动避障增益策略；

步骤六：计算机器人雅可比矩阵和标志点处雅可比矩阵；

步骤七：逆运动学求解，积分得到关节的运动角度；

步骤八：不启动避障策略，继续执行机器人末端运动；

步骤九：判断机器人末端是否到达终点；

步骤九.一：机器人末端到达终点，结束

过程二：避奇异的路径规划步骤如下：

步骤A：判断最小奇异值是否低于奇异值下限；

步骤A.一：最小奇异值大于或等于奇异值下限，转至步骤B；

步骤A.二：最小奇异值小于奇异值下限，转至步骤C；

步骤B：不启动避奇异策略，继续执行机器人末端运动，再转至步骤九；

步骤C：启动避奇异策略，再转至步骤七。

2.如权利要求1所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特征在于步骤一的具体过程为：

所述连杆是由杆件1、杆件2和杆件3构成；

已知障碍物中心在坐标系{0}中的位置(x_t0,y_t0)，连杆截面为边长为a的正方形，障碍物半径为R，根据坐标系{1}相对于坐标系{0}的旋转矩阵，得出障碍物中心在坐标系{1}中的位置(x_t1,y_t1)，即

$\left[\begin{array}{c}{x}_{t1}\\ y_{t1}\\ 0\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_1& -{\mathrm{sin\&theta;}}_1& 0\\ {\mathrm{sin\&theta;}}_1& {\mathrm{cos\&theta;}}_1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t0}\\ y_{t0}\\ 0\end{array}\right]$

同理得出

$\left[\begin{array}{c}{x}_{t2}\\ y_{t2}\\ 0\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_2& -{\mathrm{sin\&theta;}}_2& 0\\ {\mathrm{sin\&theta;}}_2& {\mathrm{cos\&theta;}}_2& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t0}\\ y_{t0}\\ 0\end{array}\right],\left[\begin{array}{c}{x}_{t3}\\ y_{t3}\\ 0\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{cos\&theta;}}_3& -{\mathrm{sin\&theta;}}_3& 0\\ {\mathrm{sin\&theta;}}_3& {\mathrm{cos\&theta;}}_3& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t0}\\ y_{t0}\\ 0\end{array}\right].$

3.如权利要求1或2所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特征在于步骤二的具体过程为：

若(x_t1,y_t1)满足

$\left\{\begin{array}{c}|y_{A1}-y_{t1}|\&GreaterEqual;R+a\\ x_{t1}\&GreaterEqual;x_{B1}+R\end{array}\right.$

或 $\left\{\begin{array}{c}|y_{A1}-y_{t1}|\&GreaterEqual;R+a\\ x_{A1}-R\&GreaterEqual;x_{t1}\end{array}\right.$

杆件1为安全杆件；

若(x_t1,y_t1)满足

$\left\{\begin{array}{c}|y_{A1}-y_{t1}|\&le;R+a\\ x_{A1}-R\&le;x_{t1}\&le;x_{B1}+R\end{array}\right.---\left(4\right)$

其中，x_A1，y_A1为杆件1上A点在坐标系{1}中的位置坐标，x_B1为杆件1上B点在坐标系{1}中的位置横坐标；

则杆件1为可能发生碰撞的连杆，需计算其与障碍物之间的距离。

4.如权利要求3所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特征在于步骤三的具体过程为：

定义可能发生碰撞的杆件1上距离障碍物最近的点为标志点S，则该连杆上的标志点S在坐标系{1}中的坐标为(x_t1,0)，标志点S和障碍物之间的距离为：

d₁＝|y_A1-y_t1|

同理计算可能发生碰撞的杆件2和杆件3上的标志点和障碍物之间的距离d₂和d₃，并取d₁、d₂和d₃的最小值，即得到实时最小距离d_min。

5.如权利要求4所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特征在于步骤六的具体过程为：

标志点S处的雅可比矩阵J_s满足

$J_s\stackrel{\&CenterDot;}{q}={\stackrel{\&CenterDot;}{x}}_s$

其中，J_S∈R^m×n为最靠近障碍物的标志点处的雅可比矩阵，为关节速度向量，为标志点的运动速度向量；

根据梯度投影法，冗余度机器人逆解为

$\stackrel{\&CenterDot;}{q}=J^{+}\stackrel{\&CenterDot;}{p}+(I-J^{+}J)H$

其中，为笛卡尔空间中机器人末端速度，I为单位矩阵，J∈R^m×n为机器人的雅克比矩阵，J⁺∈R^n×m为雅可比矩阵的广义逆，H为冗余度机器人零空间避障函数；

冗余度机器人零空间避障函数可由下式求得

$H={\&lsqb;J_S\left(\right(I-J^{+}J\left)\right)\&rsqb;}^{+}({\stackrel{\&CenterDot;}{x}}_S-J_S{J}^{+}\stackrel{\&CenterDot;}{x})$

其中，为机器人末端的运动速度向量。

6.如权利要求5所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特征在于步骤七的具体过程为：

基于梯度投影法的冗余度机器人避障运动学逆解为

$\stackrel{\&CenterDot;}{q}=J^{+}\stackrel{\&CenterDot;}{x}+(I-J^{+}J){\&lsqb;J_S\left(\right(I-J^{+}J))\&rsqb;}^{+}({\stackrel{\&CenterDot;}{x}}_S-J_S{J}^{+}\stackrel{\&CenterDot;}{x})$

由于(I-J⁺J)的特殊性，引入避障系数a和避障增益b，上式可写成

$\stackrel{\&CenterDot;}{q}=J^{+}\stackrel{\&CenterDot;}{x}+a{\&lsqb;J_S\left(\right(I-J^{+}J\left)\right)\&rsqb;}^{+}(b{\stackrel{\&CenterDot;}{x}}_S-J_S{J}^{+}\stackrel{\&CenterDot;}{x})$

对上式求积分后，换算成关节转角，公式中系数a，b和实时最小距离d_min有关；

定义两个距离d₁<d₂，当d_min<d₂时为危险区域，启动避障策略，当d_min<d₁时杆件为极度危险区域，开启避障增益策略；

$a=\left\{\begin{array}{cc}A& d_{min}<d_2\\ 0& d_{min}>d_2\end{array}\right.$

$b=\left\{\begin{array}{cc}B& d_{\min }<d_1\\ 0& d_{\min }>d_1\end{array}\right.;$

引入DLS法对上面公式提出的避障算法进行避奇异优化，则避障避奇异协同优化解为

$\stackrel{\&CenterDot;}{q}=J_1\stackrel{\&CenterDot;}{x}+a{\&lsqb;J_S\left(\right(I-J^{+}J\left)\right)\&rsqb;}^{+}(b{\stackrel{\&CenterDot;}{x}}_S-J_S{J}^{+}\stackrel{\&CenterDot;}{x})$

对上式求积分后，换算成关节转角；

其中，J₁＝J^T(JJ^T+C²I)^-1。

7.如权利要求6所述的一种平面冗余度机器人避障及避奇异的路径规划方法，其特征在于步骤一所述的连杆由4、5、6或7个杆件构成。