一种障碍物与物理极限躲避方法与流程

本发明涉及系统控制与规划技术领域，具体涉及一种障碍物与物理极限躲避方法。

背景技术

障碍物躲避和物理极限躲避是搭载有冗余机械臂的现代智能系统解析研究中的基础问题之一,其关系到所搭载的冗余机械臂能否成功完成给定的末端任务。因此，动态规划与控制中的障碍物和物理极限躲避是非常重要且具有实际意义的。如若没有考虑对上述障碍物和物理极限的躲避，将很有可能导致工作失败甚至造成机械臂的损坏。

传统的方法为基于人工势场的方法，它的基本思想是将机械臂在周围环境中的运动，设计成为一种抽象的在人造引力场中的运动，目标点对移动机械臂产生“引力”，障碍物对移动机械臂产生“斥力”，最后通过求合力来控制移动机械臂的运动。应用势场法规划出来的路径一般比较平滑并且安全，但是这种方法存在局部最优点问题。除此之外，在应用这样一种方法进行障碍物躲避时，要求障碍物最好是规则的，否则算法的计算量将很大，有时甚至是无法计算的。基于人工势场的方法的内在的局限性主要表现在：当目标附近有障碍物时，机械臂将永远也到达不了目的地。在以前的许多研究中，目标和障碍物都离的很远，当机械臂逼近目标时，障碍物的斥力变的很小，甚至可以忽略，机械臂将只受到吸引力的作用而直达目标。但在许多实际环境中，往往至少有一个障碍物与目标点离的很近，在这种情况下，当移动机械臂逼近目标的同时，它也将向障碍物靠近，如果利用以前对引力场函数和斥力场函数的定义，斥力将比引力大的多，这样目标点将不是整个势场的全局最小点，因此机械臂将不可能到达目标。这样就存在局部最优解的问题，因此如何设计“引力场”问题就成为该方法的关键。同时，目前在应用这一方法的同时并未考虑冗余机械臂本身的物理关节极限问题，在实际执行任务时容易导致机械臂锁死等一系列不可行问题。

面对这样一个背景，亟待提出一种基于矢量的障碍物与物理极限躲避方法并用以在实际搭载有冗余机械臂的移动平台上。

技术实现要素：

本发明的目的是为了解决现有技术中的上述缺陷，提供一种基于矢量的障碍物与物理极限躲避方法。

本发明的目的可以通过采取如下技术方案达到：

一种障碍物与物理极限躲避方法，所述的躲避方法包括下列步骤：

s1、依据目标系统的物理模型，列写其运动学方程表达式；

s2、基于步骤s1中目标系统的运动学方程表达式，求解目标系统末端执行器的雅克比矩阵；

s3、依据目标系统与障碍物之间的关系，定义其障碍物躲避约束；

s4、将步骤s3中所述的障碍物躲避约束进行优化，设计基于矢量的障碍物躲避方法；

s5、依据目标系统的物理模型，设定其物理极限约束表达式；

s6、将步骤s4中所述的基于矢量的障碍物躲避方法与步骤s5中所述的物理极限约束表达式合并，并结合步骤s2中所述的末端执行器的雅克比矩阵，设计目标系统的基于矢量的障碍物与物理极限躲避方法；

s7、将步骤s6中所述的基于矢量的障碍物与物理极限躲避方法进行变换，得到与其等价的线性微分不等式形式；

s8、将步骤s7中所述的线性微分不等式进行变换，得到与其等价的线性投影方程形式；

s9、将步骤s8中所述的线性投影方程代入原对偶神经网络求解器进行解算，即可完成目标系统对障碍物与物理极限的躲避。

进一步地，依据目标系统的物理模型，列写如下的运动学方程表达式：

其中，该目标系统为工作空间在三维坐标空间的搭载6自由度冗余机械臂的移动平台系统；矩阵用于描述该系统的总动力学关系；矩阵为移动平台的动力学模型；矩阵为机械臂的动力学模型；为假设已知的机械臂末端执行器的位置向量。

矩阵定义如下：

其中，φ为移动平台的航向角；x、y、z分别为三维坐标的位置信息。

矩阵定义如下：

其中，和为该6自由度冗余机械臂的齐次变换矩阵。

进一步地，依据运动学方程表达式(1)，求解得到如下的目标系统末端执行器的雅克比矩阵：

其中，θ为机械臂关节的关节角变量。

进一步地，依据目标系统与障碍物之间的关系，定义如下的障碍物躲避约束：

其中，被定义为：

其中，m表示工作空间的维度，当m＝3时代表目标系统工作在三维空间下；参数σ表示有效障碍物点与临界点在机械臂关节角上的对数；表示临界点的雅克比矩阵；被如下定义：

其中，(xc,yc,zc)和(xo,yo,zo)分别为临界点和障碍物点的坐标；符号sgn[·]表示符号函数；运算符⊙被定义为：

其中，b＝[b1,b2,…,bl]为一个列向量；为一个矩阵。

进一步地，所述的步骤s4具体如下：

由于障碍物躲避约束(5)的右端为0，当机械臂靠近障碍物时可能造成紧急停车，对机械臂造成损害，故可对障碍物躲避约束(5)进行如下优化：

其中，变量被定义为：

平滑方程s(·)被定义为：

其中，d为临界点与障碍物点之间的距离；参数d1和d2分别被设定为退出极限距离和进入极限距离。

注意到当临界点与障碍物点之间的距离过小时，运用上述关系不足以激活障碍物躲避约束(9)，即此时满足如下关系：

为了更好地利用可行域空间，将障碍物躲避约束(9)进一步优化，得到如下的基于矢量的障碍物躲避方法：

其中，和被定义如下：

设计作为目标系统临界点的速度矢量，则不等式(13)的左边被拓展为如下形式：

如果式(16)为负，即则矢量和矢量之间的角度将小于或等于90度，也即临界点将朝着远离障碍物点的方向移动。换句话说，这样一种基于矢量的障碍物躲避方法可以利用到整个可行域空间，而不仅仅局限于满足临界点与障碍物点之间距离大于一定范围的空间。

进一步地，所述的步骤s5具体如下：

依据目标系统的物理模型，设定其物理极限约束表达式。为了保证在执行任务时目标系统中冗余机械臂的安全，设定如下的机械臂关节角度极限和关节角速度极限：

θ^-≤θ≤θ⁺(17)

其中，θ^-和θ⁺分别表示关节角度θ的下限约束和上限约束；和分别表示关节角速度的下限约束和上限约束。

由于目标系统的障碍物与物理极限躲避问题将在速度层上进行求解，故角度约束(17)和角速度约束(18)将转化到速度层上进行解析。一种物理极限约束方法被设定如下：

其中，下限约束和上限约束被分别定义为：

其中，参数k>0为调节系数。由物理极限约束(19)知，当一个关节角迫近其极限约束值时，它的角速度会被限制下降至0，从而使关节角到达物理极限之前停止或反向。

进一步地，所述的步骤s6具体如下：

将所述的基于矢量的障碍物躲避方法与所述的物理极限约束方法合并，并结合所述的目标系统末端执行器的雅克比矩阵，设计得到一种基于矢量的障碍物与物理极限躲避方法。一种兼具障碍物躲避方法(13)、物理极限约束方法(19)和末端执行器的雅克比矩阵(4)的基于矢量的障碍物与物理极限躲避方法，具有如下形式：

进一步地，所述的步骤s7具体如下：

将基于矢量的障碍物与物理极限躲避方法(22)-(25)进行变换，得到与其等价的线性微分不等式形式。求解式(22)-(25)即等价于寻找到一个原对偶平衡向量其满足如下关系：

其中，表示原对偶决策变量；表示等式约束(23)的对偶决策变量；表示不等式约束(24)的对偶决策变量；ω作为一个凸集合被定义如下：

其中，下限u^-和上限u⁺分别为：

ε＞＞0被设定得尽可能大以用于替代+∞；向量及其他参数被定义为：

其中，e为n×n的单位矩阵；其他变量与前述保持一致。

进一步地，所述的步骤s8具体如下：

将上述的线性微分不等式(26)转化成为具有如下形式的线性投影方程：

其中，为从投影到集合ω的线性投影算子；矩阵u和与前述定义保持一致。

进一步地，所述的步骤s9具体如下：

将上述的线性投影方程(36)代入如下的原对偶神经网络求解器进行解算：

将解算所得数据输出，即可完成目标系统对障碍物与物理极限的躲避。

本发明相对于现有技术具有如下的优点及效果：

本发明公开的一种障碍物与物理极限躲避方法基于目前新兴的处理二次规划问题的方法，很好地计算出与障碍物之间的距离，并对机械臂施加速度极限，从而保证其对障碍物的躲避。与此同时，该方法充分考虑了实际冗余机械臂的物理关节极限，并将其作为极限约束加入到二次规划表达式中，在完成障碍物躲避的同时，保证机械臂工作在合理的物理极限范围之内。更进一步地，这样一种方法可以推广至类似的搭载有不同型号冗余机械臂的移动平台系统，进行目标系统的障碍物和物理极限躲避任务。

附图说明

图1是本发明公开的障碍物与物理极限躲避方法的流程图；

图2(a)是未使用避障方法的情况下仿真实验一的障碍物点及跟踪轨迹结果图；

图2(b)是使用传统避障方法的情况下仿真实验一的障碍物点及跟踪轨迹结果图；

图2(c)是使用基于矢量的避障方法的情况下仿真实验一的障碍物点及跟踪轨迹结果图；

图3(a)是未使用避障方法的情况下仿真实验一的三维避障结果图；

图3(b)是使用传统避障方法的情况下仿真实验一的三维避障结果图；

图3(c)是使用基于矢量的避障方法的情况下仿真实验一的三维避障结果图；

图4(a)是未使用避障方法的情况下仿真实验一的临界点与障碍物点距离结果图；

图4(b)是使用传统避障方法的情况下仿真实验一的临界点与障碍物点距离结果图；

图4(c)是使用基于矢量的避障方法的情况下仿真实验一的临界点与障碍物点距离结果图；

图5(a)是未使用避障方法的情况下仿真实验一的范数误差结果图；

图5(b)是使用传统避障方法的情况下仿真实验一的范数误差结果图；

图5(c)是使用基于矢量的避障方法的情况下仿真实验一的范数误差结果图；

图6(a)是未使用避障方法的情况下仿真实验二的障碍物点及跟踪轨迹结果图；

图6(b)是使用传统避障方法的情况下仿真实验二的障碍物点及跟踪轨迹结果图；

图6(c)是使用基于矢量的避障方法的情况下仿真实验二的障碍物点及跟踪轨迹结果图；

图7(a)是未使用避障方法的情况下仿真实验二的三维避障结果图；

图7(b)是使用传统避障方法的情况下仿真实验二的三维避障结果图；

图7(c)是使用基于矢量的避障方法的情况下仿真实验二的三维避障结果图；

图8(a)是未使用避障方法的情况下仿真实验二的临界点与障碍物点距离结果图；

图8(b)是使用传统避障方法的情况下仿真实验二的临界点与障碍物点距离结果图；

图8(c)是使用基于矢量的避障方法的情况下仿真实验二的临界点与障碍物点距离结果图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例

图1所示为本发明实例的障碍物与物理极限躲避方法的流程图，该基于矢量的障碍物与物理极限躲避方法包括如下步骤：

s1、依据目标系统的物理模型，列写其运动学方程表达式；

s2、基于步骤s1中目标系统的运动学方程表达式，求解目标系统末端执行器的雅克比矩阵；

s3、依据目标系统与障碍物之间的关系，定义其障碍物躲避约束；

s4、将步骤s3中所述的障碍物躲避约束进行优化，设计基于矢量的障碍物躲避方法；

s5、依据目标系统的物理模型，设定其物理极限约束表达式；

s6、将步骤s4中所述的基于矢量的障碍物躲避方法与步骤s5中所述的物理极限约束表达式合并，并结合步骤s2中所述的末端执行器的雅克比矩阵，设计目标系统的基于矢量的障碍物与物理极限躲避方法；

s7、将步骤s6中所述的基于矢量的障碍物与物理极限躲避方法进行变换，得到与其等价的线性微分不等式形式；

s8、将步骤s7中所述的线性微分不等式进行变换，得到与其等价的线性投影方程形式；

s9、将步骤s8中所述的线性投影方程代入原对偶神经网络求解器进行解算，即可完成目标系统对障碍物与物理极限的躲避。

图2(a)-图2(c)所示分别为未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验一的障碍物点及跟踪轨迹结果图。在仿真实验一中，障碍物点为(0.45,-0.05,0.85)，末端轨迹为圆轨迹。

图3(a)-图3(c)所示分别为未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验一的三维避障结果图。由图3(a)可知，未使用避障方法时，冗余机械臂与障碍物发生碰撞，任务失败。由图3(b)和图3(c)可知，使用传统避障方法和使用基于矢量的避障方法时，冗余机械臂成功避开了障碍物，任务成功。

图4(a)-图4(c)所示分别为未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验一的临界点与障碍物点距离结果图。由图4(a)可知，未使用避障方法时，临界点与障碍物点间距离小于碰撞距离，也即碰撞发生，任务失败。由图4(b)和图4(c)可知，使用传统避障方法和使用基于矢量的避障方法时，临界点与障碍物点间距离大于碰撞距离，也即避开了障碍物，任务成功。

图5(a)-图5(c)所示分别为未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验一的范数误差结果图。由图5(a)可知，未使用避障方法时，任务执行的范数误差为0.0006435；由图5(b)可知，使用传统避障方法时，任务执行的范数误差为0.1079；由图5(c)可知，使用基于矢量的避障方法时，任务执行的范数误差为0.0009241。综合以上三个结果可知，使用本发明所述的基于矢量的避障方法时的范数误差远远小于使用传统避障方法时的范数误差，也即在使用该方法完成避障的同时也取得了很高的精度。

图6(a)-图6(c)所示分别为未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验二的障碍物点及跟踪轨迹结果图。在仿真实验二中，障碍物点为(0.6,0.3,0.8)，末端轨迹为李萨如轨迹。

图7(a)-图7(c)所示分别为未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验二的三维避障结果图。由图7(a)和图7(b)可知，未使用避障方法和使用传统避障方法时，冗余机械臂与障碍物发生碰撞，任务失败。由图7(c)可知，使用基于矢量的避障方法时，冗余机械臂成功避开了障碍物，任务成功。综上所述，在面对复杂的轨迹任务时，传统方法由于其局限性，可能导致任务失败；而本发明所述的基于矢量的避障方法改进了这一缺陷，可以完全利用可行域空间进行避障，体现出其较好的适用性。

图8(a)-图8(c)所示分别为未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验二的临界点与障碍物点距离结果图。由图8(a)和图8(b)可知，未使用避障方法和使用传统避障方法时，临界点与障碍物点间距离小于碰撞距离，也即碰撞发生，任务失败。由图8(c)可知，使用基于矢量的避障方法时，临界点与障碍物点间距离大于碰撞距离，也即避开了障碍物，任务成功。综上所述，在面对复杂的轨迹任务时，传统方法由于其局限性，可能导致任务失败；而本发明所述的基于矢量的避障方法改进了这一缺陷，可以完全利用可行域空间进行避障，体现出其较好的适用性。

根据设计流程图的相关步骤，在此针对本发明进行详细的算法解析。首先，依据目标系统的物理模型，可以列写如下的运动学方程表达式：

其中，该目标系统为工作空间在三维坐标空间的搭载6自由度冗余机械臂的移动平台系统；矩阵用于描述该系统的总动力学关系；矩阵为移动平台的动力学模型；矩阵为机械臂的动力学模型；为假设已知的机械臂末端执行器的位置向量。

矩阵定义如下：

其中，φ为移动平台的航向角；x、y、z分别为三维坐标的位置信息。

矩阵定义如下：

其中，和为该6自由度冗余机械臂的齐次变换矩阵。

依据运动学方程表达式(1)，可以求解得到如下的目标系统末端执行器的雅克比矩阵：

依据目标系统与障碍物之间的关系，可以定义如下的障碍物躲避约束：

其中，被定义为：

m表示工作空间的维度，当m＝3时代表目标系统工作在三维空间下；参数σ表示有效障碍物点与临界点在机械臂关节角上的对数；表示临界点的雅克比矩阵；被如下定义：

其中，(xc,yc,zc)和(xo,yo,zo)分别为临界点和障碍物点的坐标；符号sgn[·]表示符号函数；运算符⊙被定义为：

b＝[b1,b2,…,bl]为一个列向量；为一个矩阵。

由于障碍物躲避约束(5)的右端为0，当机械臂靠近障碍物时可能造成紧急停车，对机械臂造成损害，故可对障碍物躲避约束(5)进行如下优化：

其中，变量被定义为：

平滑方程s(·)被定义为：

d为临界点与障碍物点之间的距离；参数d1和d2分别被设定为退出极限距离和进入极限距离。

注意到当临界点与障碍物点之间的距离过小时，运用上述关系不足以激活障碍物躲避约束(9)，即此时满足如下关系：

为了更好地利用可行域空间，将障碍物躲避约束(9)进一步优化，得到如下的基于矢量的障碍物躲避方法：

其中，和被定义如下：

设计作为目标系统临界点的速度矢量，则不等式(13)的左边可以被拓展为如下形式：

如果式(16)为负，即则矢量和矢量之间的角度将小于或等于90度，也即临界点将朝着远离障碍物点的方向移动。换句话说，这样一种基于矢量的障碍物躲避方法可以利用到整个可行域空间，而不仅仅局限于满足临界点与障碍物点之间距离大于一定范围的空间。

依据目标系统的物理模型，可以设定其物理极限约束表达式。为了保证在执行任务时目标系统中冗余机械臂的安全，设定如下的机械臂关节角度极限和关节角速度极限：

θ^-≤θ≤θ⁺(17)

其中，θ^-和θ⁺分别表示关节角度θ的下限约束和上限约束；和分别表示关节角速度的下限约束和上限约束。

由于目标系统的障碍物与物理极限躲避问题将在速度层上进行求解，故角度约束(17)和角速度约束(18)将转化到速度层上进行解析。一种物理极限约束方法被设定如下：

其中，下限约束和上限约束被分别定义为：

其中，参数k>0为调节系数。由物理极限约束(19)知，当一个关节角迫近其极限约束值时，它的角速度会被限制下降至0，从而使关节角到达物理极限之前停止或反向。

一种兼具障碍物躲避方法(13)、物理极限约束方法(19)和末端执行器的雅克比矩阵(4)的基于矢量的障碍物与物理极限躲避方法在本发明中被设计出来，其具有如下形式：

将基于矢量的障碍物与物理极限躲避方法(22)-(25)进行变换，可以得到与其等价的线性微分不等式形式。求解式(22)-(25)即等价于寻找到一个原对偶平衡向量其满足如下关系：

其中，表示原对偶决策变量；表示等式约束(23)的对偶决策变量；表示不等式约束(24)的对偶决策变量；ω作为一个凸集合被定义如下：

其中，下限u^-和上限u⁺分别为：

ε＞＞0被设定得尽可能大以用于替代+∞；向量及其他参数被定义为：

其中，e为n×n的单位矩阵；其他变量与前述保持一致。

前述的线性微分不等式(26)可以被转化成为具有如下形式的线性投影方程：

其中，为从投影到集合ω的线性投影算子；矩阵u和与前述定义保持一致。

前述的线性投影方程(36)可以被代入如下的原对偶神经网络求解器进行解算：

将解算所得数据输出，即可完成目标系统对障碍物与物理极限的躲避。此处，为了展示本发明所述方法的实际应用过程，利用一个仿真实例对所述问题进行说明。

本仿真实例基于一个搭载有6自由度冗余机械臂的移动平台。

其中，冗余机械臂6个初始关节的关节角度被设定为：

θ(0)＝[262.6,260.0,86.0,228.0,104.0,138.0]^t；

式(11)中的退出极限与进入极限被分别设定为0.10m和0.05m；式(37)中的参数β被设定为1×10⁴m。任务执行时的范数误差定义为x、y、z三个坐标上误差的范数和，即该仿真实例共分为仿真实验一和仿真实验二两个实验。

未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验一的障碍物点及跟踪轨迹结果如图2(a)-图2(c)所示。在仿真实验一中，障碍物点为(0.45,-0.05,0.85)，末端轨迹为圆轨迹。未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验一的三维避障结果如图3(a)-图3(c)所示。由图3(a)可知，未使用避障方法时，冗余机械臂与障碍物发生碰撞，任务失败。由图3(b)和图3(c)可知，使用传统避障方法和使用基于矢量的避障方法时，冗余机械臂成功避开了障碍物，任务成功。未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验一的临界点与障碍物点的距离结果如图4(a)-图4(c)所示。由图4(a)可知，未使用避障方法时，临界点与障碍物点间距离小于碰撞距离，也即碰撞发生，任务失败。由图4(b)和图4(c)可知，使用传统避障方法和使用基于矢量的避障方法时，临界点与障碍物点间距离大于碰撞距离，也即避开了障碍物，任务成功。未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验一的范数误差结果如图5(a)-图5(c)所示。由图5(a)可知，未使用避障方法时，任务执行的范数误差为0.0006435；由图5(b)可知，使用传统避障方法时，任务执行的范数误差为0.1079；由图5(c)可知，使用基于矢量的避障方法时，任务执行的范数误差为0.0009241。综合以上三个结果可知，使用本发明所述的基于矢量的避障方法时的范数误差远远小于使用传统避障方法时的范数误差，也即在使用该方法完成避障的同时也取得了很高的精度。

未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验二的障碍物点及跟踪轨迹结果如图6(a)-图6(c)所示。在仿真实验二中，障碍物点为(0.6,0.3,0.8)，末端轨迹为李萨如轨迹。未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验二的三维避障结果如图7(a)-图7(c)所示。由图7(a)和图7(b)可知，未使用避障方法和使用传统避障方法时，冗余机械臂与障碍物发生碰撞，任务失败。由图7(c)可知，使用基于矢量的避障方法时，冗余机械臂成功避开了障碍物，任务成功。综上所述，在面对复杂的轨迹任务时，传统方法由于其局限性，可能导致任务失败；而本发明所述的基于矢量的避障方法改进了这一缺陷，可以完全利用可行域空间进行避障，体现出其较好的适用性。未使用避障方法、使用传统避障方法和使用基于矢量的避障方法的情况下，仿真实验二的临界点与障碍物点的距离结果如图8(a)-图8(c)所示。由图8(a)和图8(b)可知，未使用避障方法和使用传统避障方法时，临界点与障碍物点间距离小于碰撞距离，也即碰撞发生，任务失败。由图8(c)可知，使用基于矢量的避障方法时，临界点与障碍物点间距离大于碰撞距离，也即避开了障碍物，任务成功。综上所述，在面对复杂的轨迹任务时，传统方法由于其局限性，可能导致任务失败；而本发明所述的基于矢量的避障方法改进了这一缺陷，可以完全利用可行域空间进行避障，体现出其较好的适用性。

本发明的上述各算法解析步骤仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。