一种面向冗余机械臂的无模型碰撞避免控制方法技术

本发明专利技术涉及一种面向冗余机械臂的无模型碰撞避免控制方法，包括：S1：建立冗余机械臂的逆向运动学方程；并建立碰撞避免的约束不等式；S2：将冗余机械臂的逆向运动学问题表示为微分方程形式；S3：建立估计雅克比矩阵的微分方程；估计冗余机械臂末端的雅克比矩阵的估计值和冗余机械臂临界点处的雅克比矩阵估计值；S4：确定冗余机械臂的每个关节的角度随时间变化的函数；将关节角度‑时间信息输入该函数，即可让机械臂按照躲避障碍物的轨迹运动。本发明专利技术所述方法适应于各种类型冗余机械臂的准确的碰撞避免控制。

【技术实现步骤摘要】
一种面向冗余机械臂的无模型碰撞避免控制方法
本专利技术涉及冗余机械臂碰撞避免控制方法领域，更具体地，涉及一种面向冗余机械臂的无模型碰撞避免控制方法。
技术介绍
在现代制造业中，冗余机械臂已经得到了广泛的运用。而冗余机械臂的碰撞避免控制则是一种非常常见的应用场景。在冗余机械臂的任务轨迹中，经常会需要躲避各种各样的环境障碍物，如进行外科手术、组装零件等。即使无环境障碍物，碰撞避免控制也可用于躲避冗余机械臂自身或基座、躲避奇异构形或改善当前构形等。目前对于冗余机械臂的碰撞避免控制算法已经有许多研究，主要方向有两种，一是控制使得冗余机械臂与障碍物之间的距离最大化，二是将无碰撞要求视为一个等式约束，即为易受碰撞的冗余机械臂连杆产生逃离障碍物的速度，但是这些碰撞避免控制算法都需要基于冗余机械臂模型进行控制。目前，有文献中提出的基于线性变分不等式的原始对偶神经网络二次规划冗余机械臂碰撞避免控制方法。该方法提出了以下机械臂的控制方案：约束条件为：其中，是待实时求解的决策变量，也即冗余机械臂的关节的角速度，n是冗余机械臂的自由度。系数矩阵W是单位矩阵。系数向量q＝4(θ-θ(0))，其中θ(0)是每个关节的初始角度，θ是t时刻的关节角度。Je是冗余机械臂末端的雅可比矩阵。是冗余机械臂的末端执行器在任务空间的期望轨迹的时间导数，也即末端执行器的速度。ξ-和ξ+分别表示冗余机械臂的关节的物理极限。sgn函数为符号函数，其输出值为1或-1。Jc是冗余机械臂临界点处的雅可比矩阵。d2为警戒距离，当机械臂与障碍物的距离小于该值时开始避障计算。d1为极限距离，我们一般认为当机械臂与障碍物的距离小于该值时发生碰撞。可使用许多方法求解该线性变分不等式问题，上述文献使用了原对偶神经网络二次规划来将该线性变分不等式转化为微分方程形式，然后使用通用微分方程求解器来对其进行求解。但是，现有技术要求知晓约束条件中冗余机械臂末端的雅可比矩阵Je和临界点处的雅可比矩阵Jc的解析式，以求得每个时刻的雅可比矩阵Je和Jc的精确值，才能确定每个时刻的约束条件，从而得到二次规划在每个时刻的解。该方法的解的精度和正确性会受到雅可比矩阵表达式的直接影响。但是即使是同一批次、型号的机械臂，雅可比矩阵表达式也可能由于机械臂的组装差异而各自不同，在我们使用冗余机械臂的正向运动学模型推导出的雅可比矩阵表达式可能与实际的雅可比矩阵表达式有偏差，致在应用该算法时产生误差，影响解的精确度。其次，冗余机械臂的型号繁多，对于一种机械臂，就需要重新计算一次其雅可比矩阵表达式，而有些机械臂的正向运动学模型难以测算，导致该方法的可移植性不强。最后，冗余机械臂的临界点在整个碰撞避免运动过程中会不断变化，这使得其雅可比矩阵表达式也需要不断重新计算，这就给控制带来了很多的不便。2018年11月9日公开的中国专利CN108772835A公开了一种障碍物与物理极限躲避方法，包括如下步骤：依据目标系统的物理模型，列写其运动学方程表达式；求解目标系统末端执行器的雅克比矩阵；依据目标系统与障碍物之间的关系，定义其障碍物躲避约束；将障碍物躲避约束进行优化，设计基于矢量的障碍物躲避方法；依据目标系统的物理模型，设定其物理极限约束表达式；将基于矢量的障碍物躲避方法与物理极限约束表达式合并，并结合末端执行器的雅克比矩阵，设计目标系统的基于矢量的障碍物与物理极限躲避方法；进行变换得到与其等价的线性微分不等式；进行变换得到与其等价的线性投影方程；将线性投影方程代入原对偶神经网络求解器进行解算，即可完成目标系统对障碍物与物理极限的躲避。CN108772835A是在已知物理模型和运动学情况下实现物理极限的躲避。只能适应特定运动学方程下的冗余机械臂，不能适应多种类型冗余机械臂。
技术实现思路
本专利技术为克服上述现有技术所述的机械臂碰撞避免控制方法不能适应多种类型冗余机械臂的缺陷，提供一种面向冗余机械臂的无模型碰撞避免控制方法。所述方法包括以下步骤：S1：建立冗余机械臂的逆向运动学方程；并建立碰撞避免的约束不等式；S2：将冗余机械臂的逆向运动学问题表示为微分方程形式；得到冗余机械臂的逆向运动学微分方程；S3：建立估计雅克比矩阵的微分方程；估计冗余机械臂末端的雅克比矩阵的估计值和冗余机械臂临界点处的雅克比矩阵估计值；S4：联立冗余机械臂的逆向运动学微分方程、冗余机械臂末端的雅克比矩阵的估计值、冗余机械臂临界点处的雅克比矩阵估计值，得到在碰撞避免运动中冗余机械臂在任意时刻各关节的期望位置，从而控制冗余机械臂完成无模型碰撞避免任务。优选地，S1中所述冗余机械臂的逆向运动学方程具体为：其中，是待实时求解的决策变量，也即冗余机械臂的关节的角速度。n是冗余机械臂的自由度。系数矩阵W是单位矩阵。系数向量q＝4(θ-θ(0))，其中θ(0)是每个关节的初始角度，θ是t时刻的关节角度。W是n*n维的单位矩阵优选地，S1中所述碰撞避免的约束不等式为：其中，Je是冗余机械臂末端的雅可比矩阵。是冗余机械臂的末端执行器在任务空间的期望轨迹的时间导数，也即末端执行器的速度。矩阵A和向量b分别由下式推导得出：sgn函数为符号函数，其输出值为1或-1。Jc是冗余机械臂临界点处的雅可比矩阵。d2为警戒距离，当机械臂与障碍物的距离小于该值时开始避障计算。d1为极限距离，我们一般认为当机械臂与障碍物的距离小于该值时发生碰撞。xo、yo、zo分别为障碍物点O在x轴、y轴、z轴的坐标；xc、yc、zc分别为机械臂临界点C在x轴、y轴、z轴的坐标；ξ-和ξ+都是n维向量，分别表示冗余机械臂的关节的物理最小极限和最大极限。优选地，假设冗余机械臂的关节具有速度极限和角度极限：θ-≤θ≤θ+则ξ-的具体定义如下：β是调节参数，用于调节关节极限转换之后的可行域的大小。θ-、θ+为关节角度的最小、最大极限，为关节角速度的最小、最大极限。优选地，ξ+的具体定义如下：优选地，S2具体为使用原对偶神经网络方法将冗余机械臂的逆向运动学问题表示为微分方程形式。优选地，S2所述冗余机械臂的逆向运动学微分方程为：其中，y是待求解的变量，是一个有着m+n+3个元素的向量。实际上，y是上述二次规划问题中的对偶变量，其前n个元素即为γ是原对偶神经网络的收敛率参数，I是一个行数和列数都为m+2n的单位矩阵。系数向量q＝4(θ-θ(0))；P(·)为投影函数；m是冗余机械臂的末端的任务空间的维度；矩阵Q和向量p的定义如下：优选地，投影函数P(·)的参数是一个向量，这个投影函数对输入的向量vi×1的每一个元素做上下限的约束，第i个元素的上下限的具体值分别由和指定。优选地，冗余本文档来自技高网...

【技术保护点】
1.一种面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，所述方法包括以下步骤：/nS1：建立冗余机械臂的逆向运动学方程；并建立碰撞避免的约束不等式；/nS2：将冗余机械臂的逆向运动学问题表示为微分方程形式；得到冗余机械臂的逆向运动学微分方程；/nS3：建立估计雅克比矩阵的微分方程；估计冗余机械臂末端的雅克比矩阵的估计值和冗余机械臂临界点处的雅克比矩阵估计值；/nS4：联立冗余机械臂的逆向运动学微分方程、冗余机械臂末端的雅克比矩阵的估计值、冗余机械臂临界点处的雅克比矩阵估计值，得到冗余机械臂的每个关节的角度随时间变化的函数；将关节角度-时间信息输入该函数，即可让机械臂按照躲避障碍物的轨迹运动。/n

【技术特征摘要】
1.一种面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，所述方法包括以下步骤：
S1：建立冗余机械臂的逆向运动学方程；并建立碰撞避免的约束不等式；
S2：将冗余机械臂的逆向运动学问题表示为微分方程形式；得到冗余机械臂的逆向运动学微分方程；
S3：建立估计雅克比矩阵的微分方程；估计冗余机械臂末端的雅克比矩阵的估计值和冗余机械臂临界点处的雅克比矩阵估计值；
S4：联立冗余机械臂的逆向运动学微分方程、冗余机械臂末端的雅克比矩阵的估计值、冗余机械臂临界点处的雅克比矩阵估计值，得到冗余机械臂的每个关节的角度随时间变化的函数；将关节角度-时间信息输入该函数，即可让机械臂按照躲避障碍物的轨迹运动。


2.根据权利要求1所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，S1中所述冗余机械臂的逆向运动学方程具体为：



其中，是待实时求解的决策变量，也即冗余机械臂的关节的角速度；n是冗余机械臂的自由度；系数矩阵W是n*n维的单位矩阵；系数向量q＝4(θ-θ(0))，其中θ(0)是每个关节的初始角度，θ是t时刻的关节角度。


3.根据权利要求2所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，S1中所述碰撞避免的约束不等式为：









其中，Je是冗余机械臂末端的雅可比矩阵；是冗余机械臂的末端执行器在任务空间的期望轨迹的时间导数，也即末端执行器的速度；
矩阵A和向量b分别由下式推导得出：









sgn函数为符号函数，其输出值为1或-1；Jc是冗余机械臂临界点处的雅可比矩阵；d2为警戒距离，当机械臂与障碍物的距离小于该值时开始避障计算；d1为极限距离，我们一般认为当机械臂与障碍物的距离小于该值时发生碰撞；xo、yo、zo分别为障碍物点O在x轴、y轴、z轴的坐标；xc、yc、zc分别为机械臂临界点C在x轴、y轴、z轴的坐标；
ξ-和ξ+都是n维向量，分别表示冗余机械臂的关节的物理最小极限和最大极限。


4.根据权利要求3所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，假设冗余机械臂的关节具有速度极限和角度极限：
θ-≤θ≤θ+



则ξ-的具体定义如下：



β是调节参数，用于调节关节极限转换之后的可行域的大小；θ-、θ+为关节角度的最小、最大极限，为关节角速度的最小、最大极限。


5.根据权利要求4所述面向冗余机械臂的无模型碰撞避免控制方法，其特征在于，
ξ+的具体定义如下：





6.根据权利要求4所述面向冗余机械臂的无模型碰撞避免控制方法，其特征...

【专利技术属性】
技术研发人员：谭宁，钟朝晖，廖申，
申请(专利权)人：中山大学，
类型：发明
国别省市：广东;44