CN114589701A - 一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法 - Google Patents

Abstract

本发明公开了一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法。包括：根据多关节机械臂的结构建立D‑H关节坐标系，然后求出多关节机械臂的正运动学的坐标转换关系和雅可比矩阵；再根据障碍物与多关节机械臂中各连杆之间的相对位置关系以及多关节机械臂的正运动学的坐标转换关系，计算障碍物对多关节机械臂中各连杆的总虚拟斥力；最后基于阻尼最小二乘法，根据雅可比矩阵和虚拟斥力建立多关节机械臂逆运动学优化目标函数，采用数值迭代方法求解多关节机械臂逆运动学优化函数，获得多关节机械臂的末端位姿对应的各关节角度。本发明将避障规划与机械臂逆运动学求解过程融合，简化了避障方法的流程，并保证了此类机械臂避障的实时性。

Description

一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法

技术领域

本发明属于机械臂运动学领域的一种多关节机械臂的逆运动学方法，具体涉及了一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法。

背景技术

多关节机械臂是冗余机械臂的一种，在三个位置自由度和三个姿态自由度以外还拥有较多冗余自由度，这导致了多关节机械臂自运动的特性，也就是在保证末端位姿固定的情况下，各关节仍然可以运动。因此，这类机械臂的运动灵活性很高，可以在末端位姿不变的同时完成避障，十分适合在复杂环境下作业。

多关节机械臂的避障的首要步骤就是逆运动学的求解，由于自由度的极大冗余，多关节机械臂的逆运动学解析解往往不存在，即使存在解析解也往往伴随了难以筛选的多个解，考虑到多关节机械臂逆运动学的复杂性，目前常见的逆运动学求解方法一般不考虑障碍约束。

发明内容

为了克服多关节机械臂冗余自由度带来的困难，在避障需求下充分利用多关节机械臂的灵活性，为提高具有冗余自由度的多关节机械臂在复杂环境下的作业性能，本发明提供了一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法。本发明针对多关节机械臂在运动过程中实时避障问题，给定机械臂的末端位姿、障碍物空间坐标和障碍物大小，基于阻尼最小二乘方法设计了机械臂的逆运动学优化函数，采用迭代求解得到最终满足避障条件的关节角度，简化了多关节机械臂避障的方法复杂度。

为实现上述目的，本发明的技术方案具体内容如下：

本发明包括以下步骤：

第一步：根据多关节机械臂的结构建立D-H关节坐标系，基于D-H关节坐标系求出多关节机械臂的正运动学的坐标转换关系和雅可比矩阵；

第二步：根据障碍物与多关节机械臂中各连杆之间的相对位置关系以及多关节机械臂的正运动学的坐标转换关系，计算障碍物对多关节机械臂中各连杆的总虚拟斥力；

第三步：基于阻尼最小二乘法，根据雅可比矩阵和虚拟斥力建立多关节机械臂逆运动学优化目标函数，采用数值迭代方法求解多关节机械臂逆运动学优化函数，获得多关节机械臂的末端位姿对应的各关节角度。

所述第二步具体为：

2.1)根据障碍物与多关节机械臂中连杆之间的相对位置关系以及多关节机械臂的正运动学的坐标转换关系，计算各个障碍物对多关节机械臂中每一连杆的虚拟斥力的作用方向以及对应虚拟斥力势能；

2.2)将各个障碍物对多关节机械臂中当前连杆的虚拟斥力的作用方向和对应虚拟斥力势能相乘后，获得各个障碍物对多关节机械臂中当前连杆的虚拟斥力，接着对各个虚拟斥力进行求和，获得障碍物对多关节机械臂中当前连杆的总虚拟斥力；

2.3)重复2.1)-2.2)，计算并获得障碍物对多关节机械臂中剩余连杆的总虚拟斥力。

所述各个障碍物对多关节机械臂中每一连杆的虚拟斥力的作用方向由障碍物与连杆之间的相对位置确定，具体地：

针对障碍物k对连杆i的虚拟斥力的作用方向，障碍物k的几何中心O与连杆i的两个端点P_A、P_B之间的距离OP_A、OP_B以及连杆i的两个端点P_A、P_B相连后构成轴线P_AP_B之间的关系分为以下三种情况：

当障碍物k的几何中心O在连杆i的轴线P_AP_B上的投影位于P_B一侧的延长线上时，障碍物k到连杆i的空间距离d_ik满足d_ik＝|OP_B|-R-r_k，R为连杆i端部所在圆的半径，r_k为障碍物的最大直径的一半，虚拟斥力的作用方向为

当障碍物k的几何中心O在连杆i的轴线P_AP_B上的投影位于P_A一侧的延长线上时，障碍物k到连杆i的空间距离d_ik满足d_ik＝|OP_A|-R-r_k，虚拟斥力的作用方向为

当障碍物k的几何中心O在连杆i的轴线P_AP_B上的投影位于轴线P_AP_B之间时，障碍物k到连杆i的空间距离d_ik满足d_ik＝|OP_v|-R-r_k，虚拟斥力的作用方向为

所述各个障碍物对多关节机械臂中当前连杆的虚拟斥力势能的计算公式如下：

式中，E_ik表示障碍物k作用于连杆i的虚拟斥力势能，k_r为障碍物斥力系数，d₀为障碍物的影响距离，d_ik为障碍物k到连杆i的空间距离；

所述第三步中，多关节机械臂逆运动学优化目标函数的公式为：

其中，J是多关节机械臂的雅可比矩阵，F_force(q)表示虚拟斥力对关节的累计作用力，

f_force(q)是虚拟斥力对连杆的作用分量，q表示多关节机械臂的各关节角度，λ²表示正则项系数，x表示多关节机械臂的末端位置，‖‖表示取二范数操作，min表示取最小值操作，γ是与迭代次数p有关的斥力衰退系数。

所述第三步的采用数值迭代法求解逆运动学优化目标函数的求解过程中，通过调整各关节角度q获得更新的末端位姿根据更新的末端位姿与预设末端位姿⁰T_E计算位姿残差，使位姿残差最小后，获得最终的各关节角度q，求解过程中的公式如下：

H_p＝(J_p ^TJ_p+λ²I)^-1

γ＝σ^p

其中，H_p是第p次迭代时的Hessian矩阵，J_p是第p次迭代时的雅可比矩阵，e_p是第p次迭代时的位姿残差，λ²是正则项系数，tr()表示将齐次矩阵转换为向量形式的操作，γ是与迭代次数p有关的斥力衰退系数；

表示正运动学关系，σ是单步斥力衰退系数，T表示转置操作，q_p、q_p+1分别表示第p、p+1次迭代时的各关节角度。

所述多关节机械臂的相邻连杆之间采用万向节进行连接。

与现有技术相比，本发明的有益效果是：

1.提出了一种多关节机械臂的逆运动学方法，能够在已知机械臂基础信息(关节个数，各关节长度等信息)和障碍物信息(空间位置、大小)的情况下，得到满足避障条件的机械臂关节角度组合，充分利用了多关节机械臂的冗余自由度以满足避障作业的需求。

2.综合考虑了多种障碍物与机械臂连杆的相对位置情况，提出了基于势能的虚拟斥力表达式，定义了具有明确物理意义的虚拟斥力表达式，保障了有障碍物情况下多关节机械臂作业的安全性。

3.使用数值迭代方法求解考虑了虚拟斥力的阻尼最小二乘方程，在方程没有解析解的情况下仍满足了逆运动学求解的精确性和实时性。

附图说明

图1是多关节机械臂的外观示意图；

图2是方法步骤流程图；

图3是坐标系定义示意图；

图4是障碍物距离计算示意图；

图5是本发明在Matlab中演示的多关节机械臂避障过程图；

图6是本发明在Matlab仿真实验中的多关节机械臂末端位姿误差结果图。

具体实施方式

下面结合实例与附图对本发明的求解过程做更详细的描述。

本实施例中，多关节机械臂的连杆序号为i＝1,2,…,I，从基座向外依次排序，总数量为I，每节连杆呈圆柱体形状，长度为L，半径为R，连杆之间采用万向节连接，如图1所示。

对于机械臂第i个连杆处的万向节，采用两个相互垂直的旋转角度θ_i、

对万向节的两个旋转自由度进行描述，对于万向节中心处的坐标系{O-xyz}来说，绕z轴的旋转角度为θ_i，绕y轴的旋转角度为

如图3所示，则可以将关节角度定义为

如图2所示，本发明包括以下步骤：

第一步：根据多关节机械臂的结构建立D-H关节坐标系，基于D-H关节坐标系求出多关节机械臂的正运动学的坐标转换关系和雅可比矩阵；

具体实施中，在万向节中心建立D-H坐标系{O_n-x_ny_nz_n}，n＝1,2,…,N表示转动自由度的序号，N＝2I，即每个万向节存在两个垂直方向的转动自由度，万向节处的坐标定义如图3所示。

机械臂的正运动学通常可以表示为如下函数形式：

它表明末端执行器的位姿ξ_E是关于关节角度q的一个函数。根据D-H法，采用齐次变换，其表达式将是由单个连杆变换矩阵的简单乘积，可以得到正运动学的坐标转换关系为：

ξ_E～⁰T_E＝⁰A₁·¹A₂…^n-1A_n

其中，

是4×4的齐次矩阵，由旋转矩阵R_3×3和平移向量T_3×1组成，^n-1A_n是D-H坐标系间的转换矩阵，可表示为：

其中q_n,α_n,a_n,d_n为D-H法中描述机械臂连杆的参数。

同时，为方便后续逆运动学分析，还需要求出机械臂的雅可比矩阵，它的形式为：

雅可比矩阵可以通过对机械臂的正运动学的公式求导得到。

第二步：根据障碍物与多关节机械臂中各连杆之间的相对位置关系以及多关节机械臂的正运动学的坐标转换关系，计算障碍物对多关节机械臂中各连杆的总虚拟斥力；

第二步具体为：

2.1)根据障碍物与多关节机械臂中连杆之间的相对位置关系以及多关节机械臂的正运动学的坐标转换关系，计算各个障碍物对多关节机械臂中每一连杆的虚拟斥力的作用方向以及对应虚拟斥力势能；

各个障碍物对多关节机械臂中每一连杆的虚拟斥力的作用方向由障碍物与连杆之间的相对位置确定，具体地：

针对障碍物k对连杆i的虚拟斥力的作用方向，如图4所示，障碍物k的几何中心O与连杆i的两个端点P_A、P_B之间的距离OP_A、OP_B以及连杆i的两个端点P_A、P_B相连后构成轴线P_AP_B之间的关系分为以下三种情况，其中，连杆i的前后端点的位置P_A、P_B可以通过计算D-H坐标系间的转换矩阵，由正运动学的坐标转换关系求得：

如图4的(a)所示，当障碍物k的几何中心O在连杆i的轴线P_AP_B上的投影位于P_B一侧的延长线上时，即|OP_B|²+|P_AP_B|²≤|OP_A|²，障碍物k到连杆i的空间距离d_ik满足d_ik＝|OP_B|-R-r_k，R为连杆i端部所在圆的半径，r_k为障碍物的最大直径的一半，虚拟斥力的作用方向为

对应的虚拟斥力E_f满足

如图4的(b)所示，当障碍物k的几何中心O在连杆i的轴线P_AP_B上的投影位于P_A一侧的延长线上时，即|OP_A|²+|P_AP_B|²≤|OP_B|²，障碍物k到连杆i的空间距离d_ik满足d_ik＝|OP_A|-R-r_k，虚拟斥力的作用方向为

对应的虚拟斥力E_f满足

如图4的(c)所示，当障碍物k的几何中心O在连杆i的轴线P_AP_B上的投影位于轴线P_AP_B之间时，障碍物k到连杆i的空间距离d_ik满足d_ik＝|OP_v|-R-r_k，虚拟斥力的作用方向为

对应的虚拟斥力E_f满足

各个障碍物对多关节机械臂中当前连杆的虚拟斥力势能的计算公式如下：

式中，E_ik表示障碍物k作用于连杆i的虚拟斥力势能，k_r为障碍物斥力系数，d₀为障碍物的影响距离，d_ik为障碍物k到连杆i的空间距离；在具体实施中，假设障碍物设置在包围球中，障碍物的几何中心位置为包围球的球心，障碍物的最大直径为包围球的直径，包围球的半径为r_k，连杆为圆柱形，即d_ik表示圆柱体连杆与包围球的空间距离。

2.2)将各个障碍物对多关节机械臂中当前连杆的虚拟斥力的作用方向和对应虚拟斥力势能相乘后，获得各个障碍物对多关节机械臂中当前连杆的虚拟斥力，接着对各个虚拟斥力进行求和，获得障碍物对多关节机械臂中当前连杆的总虚拟斥力；

具体实施中，将所有虚拟斥力进行矢量相加，随后通过坐标转换将虚拟斥力转换到各个关节坐标系下，⁰R_B为点P_B在基座坐标系下的旋转矩阵，可以由正运动学的坐标转换关系求得点P_B的转换矩阵T_B并分解后得到，至此，可以得到空间中障碍物对连杆i的总虚拟斥力E_i为：

E_i＝⁰R_B ^-1·∑E_ik

每一个连杆i的姿态由θ_i、

两个关节角度控制，因此作用于θ_i、

的关节斥力

分别为E_i在y轴和z轴的分量，因此连杆i的关节斥力组成向量为：

2.3)重复2.1)-2.2)，计算并获得障碍物对多关节机械臂中剩余连杆的总虚拟斥力。

机械臂逆运动学求解是给定末端位姿矩阵ξ_E，得到关节角

的过程，表示为：

第三步中，在阻尼最小二乘法的基础上，考虑虚拟斥力对关节的影响，多关节机械臂逆运动学优化目标函数的公式为：

其中，J是多关节机械臂的雅可比矩阵，F_force(q)表示虚拟斥力对关节的累计作用力，

f_force(q)是虚拟斥力对连杆的作用分量，q表示多关节机械臂的各关节角度，λ²表示正则项系数，本实施例中取

这可以使最终位姿误差更小，x表示机械臂的末端位置，‖‖表示取二范数操作，min表示取最小值操作，γ是用于迭代求解的斥力衰退系数，与迭代次数p有关，在迭代过程中逐渐减小，具体形式见下文。优化目标函数的公式表明要在机械臂末端跟踪轨迹误差最小和关节速度范数最小的同时，尽量减少斥力的影响，对于多关节机械臂来说，这可以使逆运动学求解得到的多关节机械臂位姿更加远离障碍物和关节极限位置。

第三步的采用数值迭代法求解逆运动学优化目标函数的求解过程中，通过调整各关节角度q获得更新的末端位姿，其中

T表示转置操作，使得更新的末端位姿不断逼近预设末端位姿，根据更新的末端位姿与预设末端位姿⁰T_E计算位姿残差，迭代中使位姿残差最小后，具体实施中，位姿残差小于预设阈值后，即位姿残差最小，停止迭代，获得最终的各关节角度q，求解过程中的公式如下：

H_p＝(J_p ^TJ_p+λ²I)^-1

γ＝σ^p

其中，H_p是第p次迭代时的Hessian矩阵，J_p是第p次迭代时的雅可比矩阵，e_p是第p次迭代时的位姿残差，满足e_p＝[dv_x,dv_y,dv_z,dω_x,dω_x,dω_z]^T，dv_x、dv_y、dv_z分别表示x、y、z轴上的位置残差，dω_x、dω_x、dω_z分别表示x、y、z轴上的角度残差，λ²是正则项系数，tr()表示将齐次矩阵转换为向量形式的操作，γ是与迭代次数p有关的斥力衰退系数，它随着迭代次数减小，σ是单步斥力衰退系数，一个常数，σ∈(0,1)，本实施例中取σ＝0.5；

表示正运动学关系，T表示转置操作，q_p、q_p+1分别表示第p、p+1次迭代时的各关节角度。

开始的几次迭代，f_force(q)引导关节快速的远离障碍物或关节极限位置，避免局部最小值的影响，在迭代后期，f_force(q)的系数γ趋近于0，使迭代更快速地向目标位姿收敛，保证了改进算法的收敛性。

本发明的Matlab仿真避障过程图如图5的(a)-(d)所示，可以看到，多关节机械臂在障碍物移动的情况下成功避障，并可以跟随期望的末端位姿。如图6的(a)和(b)所示，实际与期望末端位置的误差小于10^-10米，实际与期望末端角度的误差小于10^-10rad，满足了逆运动学求解的精确性。

Claims (7)

1.一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法，其特征在于，包括以下步骤：

第一步：根据多关节机械臂的结构建立D-H关节坐标系，基于D-H关节坐标系求出多关节机械臂的正运动学的坐标转换关系和雅可比矩阵；

第二步：根据障碍物与多关节机械臂中各连杆之间的相对位置关系以及多关节机械臂的正运动学的坐标转换关系，计算障碍物对多关节机械臂中各连杆的总虚拟斥力；

第三步：基于阻尼最小二乘法，根据雅可比矩阵和虚拟斥力建立多关节机械臂逆运动学优化目标函数，采用数值迭代方法求解多关节机械臂逆运动学优化函数，获得多关节机械臂的末端位姿对应的各关节角度。

2.根据权利要求1所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法，其特征在于，所述第二步具体为：

2.1)根据障碍物与多关节机械臂中连杆之间的相对位置关系以及多关节机械臂的正运动学的坐标转换关系，计算各个障碍物对多关节机械臂中每一连杆的虚拟斥力的作用方向以及对应虚拟斥力势能；

2.2)将各个障碍物对多关节机械臂中当前连杆的虚拟斥力的作用方向和对应虚拟斥力势能相乘后，获得各个障碍物对多关节机械臂中当前连杆的虚拟斥力，接着对各个虚拟斥力进行求和，获得障碍物对多关节机械臂中当前连杆的总虚拟斥力；

2.3)重复2.1)-2.2)，计算并获得障碍物对多关节机械臂中剩余连杆的总虚拟斥力。

3.根据权利要求2所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法，其特征在于，所述各个障碍物对多关节机械臂中每一连杆的虚拟斥力的作用方向由障碍物与连杆之间的相对位置确定，具体地：

针对障碍物k对连杆i的虚拟斥力的作用方向，障碍物k的几何中心O与连杆i的两个端点P_A、P_B之间的距离OP_A、OP_B以及连杆i的两个端点P_A、P_B相连后构成轴线P_AP_B之间的关系分为以下三种情况：

当障碍物k的几何中心O在连杆i的轴线P_AP_B上的投影位于P_B一侧的延长线上时，障碍物k到连杆i的空间距离d_ik满足d_ik＝|OP_B|-R-r_k，R为连杆i端部所在圆的半径，r_k为障碍物的最大直径的一半，虚拟斥力的作用方向为

当障碍物k的几何中心O在连杆i的轴线P_AP_B上的投影位于P_A一侧的延长线上时，障碍物k到连杆i的空间距离d_ik满足d_ik＝|OP_A|-R-r_k，虚拟斥力的作用方向为

当障碍物k的几何中心O在连杆i的轴线P_AP_B上的投影位于轴线P_AP_B之间时，障碍物k到连杆i的空间距离d_ik满足d_ik＝|OP_v|-R-r_k，虚拟斥力的作用方向为

4.根据权利要求2所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法，其特征在于，所述各个障碍物对多关节机械臂中当前连杆的虚拟斥力势能的计算公式如下：

式中，E_ik表示障碍物k作用于连杆i的虚拟斥力势能，k_r为障碍物斥力系数，d₀为障碍物的影响距离，d_ik为障碍物k到连杆i的空间距离。

5.根据权利要求1所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法，其特征在于，所述第三步中，多关节机械臂逆运动学优化目标函数的公式为：

其中，J是多关节机械臂的雅可比矩阵，F_force(Q)表示虚拟斥力对关节的累计作用力，

f_force(q)是虚拟斥力对连杆的作用分量，q表示多关节机械臂的各关节角度，λ²表示正则项系数，x表示多关节机械臂的末端位置，‖‖表示取二范数操作，min表示取最小值操作，γ是与迭代次数p有关的斥力衰退系数。

6.根据权利要求1所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法，其特征在于，所述第三步的采用数值迭代法求解逆运动学优化目标函数的求解过程中，通过调整各关节角度q获得更新的末端位姿根据更新的末端位姿与预设末端位姿⁰T_E计算位姿残差，使位姿残差最小后，获得最终的各关节角度q，求解过程中的公式如下：

e_p＝tr(⁰T_E-К(q_p))

H_p＝(J_p ^TJ_p+λ²I)^-1

γ＝σ^p

其中，H_p是第p次迭代时的Hessian矩阵，J_p是第p次迭代时的雅可比矩阵，e_p是第p次迭代时的位姿残差，λ²是正则项系数，tr()表示将齐次矩阵转换为向量形式的操作，γ是与迭代次数p有关的斥力衰退系数；К()表示正运动学关系，σ是单步斥力衰退系数，T表示转置操作，q_p、q_p+1分别表示第p、p+1次迭代时的各关节角度。

7.根据权利要求1-6任一所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法，其特征在于，所述多关节机械臂的相邻连杆之间采用万向节进行连接。

Images