一种障碍物与物理极限躲避方法技术

技术编号:19380667 阅读:6 留言:0更新日期:2018-11-09 23:14
本发明专利技术公开了一种障碍物与物理极限躲避方法,包括如下步骤:依据目标系统的物理模型,列写其运动学方程表达式;求解目标系统末端执行器的雅克比矩阵;依据目标系统与障碍物之间的关系,定义其障碍物躲避约束;将障碍物躲避约束进行优化,设计基于矢量的障碍物躲避方法;依据目标系统的物理模型,设定其物理极限约束表达式;将基于矢量的障碍物躲避方法与物理极限约束表达式合并,并结合末端执行器的雅克比矩阵,设计目标系统的基于矢量的障碍物与物理极限躲避方法;进行变换得到与其等价的线性微分不等式;进行变换得到与其等价的线性投影方程;将线性投影方程代入原对偶神经网络求解器进行解算,即可完成目标系统对障碍物与物理极限的躲避。

An obstacle and physical limit avoidance method

The invention discloses an obstacle and physical limit avoidance method, which includes the following steps: according to the physical model of the target system, formulating its kinematics equation expression; solving the Jacobian matrix of the end-effector of the target system; defining the obstacle avoidance constraint according to the relationship between the target system and the obstacle; Obstacle avoidance constraints are optimized and vector-based obstacle avoidance method is designed; physical limit constraints are formulated according to the physical model of the target system; the vector-based obstacle avoidance method is combined with physical limit constraints expression and the Jacobian matrix of the end-effector is used to design the target system. Vector-based obstacle and physical limit avoidance method; transformation to obtain the equivalent linear differential inequality; transformation to obtain the equivalent linear projection equation; linear projection equation into the original dual neural network solver to solve, can complete the target system to avoid obstacles and physical limits.

【技术实现步骤摘要】
一种障碍物与物理极限躲避方法
本专利技术涉及系统控制与规划
,具体涉及一种障碍物与物理极限躲避方法。
技术介绍
障碍物躲避和物理极限躲避是搭载有冗余机械臂的现代智能系统解析研究中的基础问题之一,其关系到所搭载的冗余机械臂能否成功完成给定的末端任务。因此,动态规划与控制中的障碍物和物理极限躲避是非常重要且具有实际意义的。如若没有考虑对上述障碍物和物理极限的躲避,将很有可能导致工作失败甚至造成机械臂的损坏。传统的方法为基于人工势场的方法,它的基本思想是将机械臂在周围环境中的运动,设计成为一种抽象的在人造引力场中的运动,目标点对移动机械臂产生“引力”,障碍物对移动机械臂产生“斥力”,最后通过求合力来控制移动机械臂的运动。应用势场法规划出来的路径一般比较平滑并且安全,但是这种方法存在局部最优点问题。除此之外,在应用这样一种方法进行障碍物躲避时,要求障碍物最好是规则的,否则算法的计算量将很大,有时甚至是无法计算的。基于人工势场的方法的内在的局限性主要表现在:当目标附近有障碍物时,机械臂将永远也到达不了目的地。在以前的许多研究中,目标和障碍物都离的很远,当机械臂逼近目标时,障碍物的斥力变的很小,甚至可以忽略,机械臂将只受到吸引力的作用而直达目标。但在许多实际环境中,往往至少有一个障碍物与目标点离的很近,在这种情况下,当移动机械臂逼近目标的同时,它也将向障碍物靠近,如果利用以前对引力场函数和斥力场函数的定义,斥力将比引力大的多,这样目标点将不是整个势场的全局最小点,因此机械臂将不可能到达目标。这样就存在局部最优解的问题,因此如何设计“引力场”问题就成为该方法的关键。同时,目前在应用这一方法的同时并未考虑冗余机械臂本身的物理关节极限问题,在实际执行任务时容易导致机械臂锁死等一系列不可行问题。面对这样一个背景,亟待提出一种基于矢量的障碍物与物理极限躲避方法并用以在实际搭载有冗余机械臂的移动平台上。
技术实现思路
本专利技术的目的是为了解决现有技术中的上述缺陷,提供一种基于矢量的障碍物与物理极限躲避方法。本专利技术的目的可以通过采取如下技术方案达到:一种障碍物与物理极限躲避方法,所述的躲避方法包括下列步骤: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)转化成为具有如下形式的线性投影方程:其中,为从投影到集合Ω的线本文档来自技高网
...

【技术保护点】
1.一种障碍物与物理极限躲避方法,其特征在于,所述的躲避方法包括下列步骤:S1、依据目标系统的物理模型,列写其运动学方程表达式;S2、基于目标系统的运动学方程表达式,求解目标系统末端执行器的雅克比矩阵;S3、依据目标系统与障碍物之间的关系,定义其障碍物躲避约束;S4、将所述的障碍物躲避约束进行优化,设计基于矢量的障碍物躲避方法;S5、依据目标系统的物理模型,设定其物理极限约束表达式;S6、将所述的基于矢量的障碍物躲避方法与所述的物理极限约束表达式合并,并结合所述的末端执行器的雅克比矩阵,设计目标系统的基于矢量的障碍物与物理极限躲避方法;S7、将所述的基于矢量的障碍物与物理极限躲避方法进行变换,得到与其等价的线性微分不等式形式;S8、将所述的线性微分不等式进行变换,得到与其等价的线性投影方程形式;S9、将所述的线性投影方程代入原对偶神经网络求解器进行解算,即可完成目标系统对障碍物与物理极限的躲避。

【技术特征摘要】
1.一种障碍物与物理极限躲避方法,其特征在于,所述的躲避方法包括下列步骤:S1、依据目标系统的物理模型,列写其运动学方程表达式;S2、基于目标系统的运动学方程表达式,求解目标系统末端执行器的雅克比矩阵;S3、依据目标系统与障碍物之间的关系,定义其障碍物躲避约束;S4、将所述的障碍物躲避约束进行优化,设计基于矢量的障碍物躲避方法;S5、依据目标系统的物理模型,设定其物理极限约束表达式;S6、将所述的基于矢量的障碍物躲避方法与所述的物理极限约束表达式合并,并结合所述的末端执行器的雅克比矩阵,设计目标系统的基于矢量的障碍物与物理极限躲避方法;S7、将所述的基于矢量的障碍物与物理极限躲避方法进行变换,得到与其等价的线性微分不等式形式;S8、将所述的线性微分不等式进行变换,得到与其等价的线性投影方程形式;S9、将所述的线性投影方程代入原对偶神经网络求解器进行解算,即可完成目标系统对障碍物与物理极限的躲避。2.根据权利要求1所述的一种障碍物与物理极限躲避方法,其特征在于,所述的运动学方程表达式如下:其中,该目标系统为工作空间在三维坐标空间的搭载6自由度冗余机械臂的移动平台系统;矩阵用于描述该系统的总动力学关系;矩阵为移动平台的动力学模型;矩阵为机械臂的动力学模型;为假设已知的机械臂末端执行器的位置向量;所述的矩阵定义如下:其中,φ为移动平台的航向角;x、y、z分别为三维坐标的位置信息;所述的矩阵定义如下:其中,和为该6自由度冗余机械臂的齐次变换矩阵。3.根据权利要求2所述的一种障碍物与物理极限躲避方法,其特征在于,所述的步骤S2如下:依据运动学方程表达式(1),求解得到如下的目标系统末端执行器的雅克比矩阵:其中,θ为机械臂关节的关节角变量。4.根据权利要求3所述的一种障碍物与物理极限躲避方法,其特征在于,所述的步骤S3如下:依据目标系统与障碍物之间的关系,定义如下的障碍物躲避约束:其中,被定义为:其中,m表示工作空间的维度;参数σ表示有效障碍物点与临界点在机械臂关节角上的对数;表示临界点的雅克比矩阵;被如下定义:其中,(xc,yc,zc)和(xo,yo,zo)分别为临界点和障碍物点的坐标;符号sgn[·]表示符号函数;运算符⊙被定义为:其中,b=[b1,b2,…,bl]为一个列向量;为一个矩阵。5.根据权利要求4所述的一种障碍物与物理极限躲避方法,其特征在于,所述的步骤S4如下:对障碍物躲避约束(5)进行如下优化:其中,变量被定义为:平滑方程S(·)被定义为:其中,d为临界点与障碍物点之间的距离;参数d1和d2分别被设定为退出极限距离和进入极限距离;...

【专利技术属性】
技术研发人员:张智军孔令东朱徐鹏
申请(专利权)人:华南理工大学
类型:发明
国别省市:广东,44

网友询问留言 已有0条评论
  • 还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。

1