CN113211442A - 一种6自由度机器人或7自由度机器人的奇异性处理方法 - Google Patents

Abstract

本发明公开了一种6自由度机器人或7自由度机器人的奇异性处理方法，包括以下步骤：S1、建立运动学模型并完成运动学分析；S2、求解雅可比矩阵，进行奇异性分析，计算出奇异值；S3、获得最大阻尼值λ_max和奇异区域[0,ε]，以该奇异区域作为第一奇异性判据，并引入奇异阻尼函数；S4、以关节速度是否超速度极限作为第二奇异性判据，并引人微缓冲区、以及缓冲阻尼函数；S5、利用虚拟力来引导机器人从非安全区域进入安全区域；S6、进行奇异性校验。该奇异性处理方法在奇异性判断上采用双判据组合的方式，对奇异性进行更细致更准确的划分，引入可产生平滑的虚拟力的奇异势能函数引导机器人从非安全区域进入安全区域，从根本上解决雅克比逆不稳定而产生的弊端。

Description

一种6自由度机器人或7自由度机器人的奇异性处理方法

技术领域

本发明涉及一种奇异性处理方法，尤其涉及一种6自由度机器人或7自由度机器人的奇异性处理方法。

背景技术

机器人关节速度和末端执行器速度之间的关系通过雅可比矩阵描述。工业机器人结构符合Pieper准则之一(准则一是3个相邻关节轴交于一点，准则二是3个相邻关节轴相互平行，此时可理解为相较于无穷远处)，容易获得机器人运动学逆解，对于不符合Pieper准则的机器人，无法获得逆运动学解析解，只能获得数值解，而最常用的数值求解方法是雅可比迭代方法，但是，当机器人处于奇异位形时，雅可比逆矩阵在数值上无法实现，导致关节速度趋于无穷大。实际上，机器人在奇异位形的附近区域也会产生大的关节速度，如果不采取彻底的措施来避免奇异问题，对于操作员，环境和机器人来说不可接受的。因此如何准确判断机器人是否靠近奇异位形及附近区域，并针对不同的情形给出合理的奇异性处理方案，直接关系到机器人整体的工作性能。6自由度机器人和7自由度机器人的奇异性处理问题可分解为两个子问题：(1)判断是否需要进行奇异性处理；(2)如果需要，采用何种处理方案。

现有技术中常见的奇异性处理方法及存在的弊端：(1)通过提前计算避开奇异位形。分析雅克比矩阵提前获得机器人奇异位形对应的关节角度，轨迹规划时避开这些关节角度。但是这种方法适合于工业机器人，对于不满足Pieper准则的机器人，雅克比矩阵复杂，且不能进行有效简化对应的行列式，无法提前获得奇异位形。另外，通过提前计算并没有考虑机器人处于奇异位形邻近区域关节速度超出极限的问题，降低机器人的工作空间和使用性能的同时，也可能对设备造成损坏。

(2)通过梯度投影法解决奇异位形产生关节速度过大的弊端。梯度投影法求解机器人逆运动学时，将避奇异问题作为子任务，这种处理方案虽然可以保证机器人连续运动，但是，需要大量的矩阵计算，实时性较差，且逆运动学求解存在误差，末端执行器运动轨迹会偏离预期的轨迹，不适用于精度要求高和实时性要求高的机器人。

(3)通过最小二乘法解决奇异位形产生关节速度过大的弊端。最小二乘法引入阻尼来解决奇异位形下雅可比逆矩阵出现病态导致关节速度过大的问题，但是该方法最大阻尼值和奇异区域是通过试错法选择的，存在极大的盲目性，也就是说如果最大阻尼值和奇异区域选择过小，则无法提供有效的阻尼，并导致关节速度超过其极限，相反，如果最大阻尼值和奇异区域选择过大，则会影响运动精度，这些都是不合理的；另外奇异区域引入阻尼导致逆运动学解存在误差，末端执行器运动轨迹会偏离预期的轨迹，不适用于精度要求高的机器人。上述三种方案仅能处理某种特定的奇异性问题，并没有从根本上促使机器人远离奇异位形及附近区域，适用性较差。

发明内容

本发明所要解决的技术问题是：提供一种6自由度机器人或7自由度机器人的奇异性处理方法，该奇异性处理方法在奇异性判断上采用双判据组合的方式，以对6自由度机器人或7自由度机器人的奇异性进行更细致更准确的划分，同时引入可产生平滑的虚拟力的奇异势能函数引导6自由度机器人和7自由度机器人从非安全区域进入安全区域，从根本上解决雅克比逆不稳定而产生的弊端，以保证机器人工作性能和运动精度。

为解决上述技术问题，本发明的技术方案是：一种6自由度机器人或7自由度机器人的奇异性处理方法，包括以下步骤：

S1、建立6自由度机器人或7自由度机器人的运动学模型并完成运动学分析；

S2、采用微分变换法求解6自由度机器人或7自由度机器人的雅可比矩阵，对6自由度机器人或7自由度机器人进行奇异性分析，计算出对应的奇异值；

S3、以跟踪精度为目标函数，以关节速度极限为约束函数，通过粒子群优化算法获得变阻尼最小二乘法方法DLS中的最大阻尼值λ_max和奇异区域与非奇异区域的临界值ε，其中[0,ε]定义奇异区域，以该奇异区域作为第一奇异性判据，并引入与奇异区域对应的用于使关节速度降低的奇异阻尼函数；

S4、以关节速度是否超速度极限作为第二奇异性判据，并引人微缓冲区、以及与微缓冲区对应的用于使关节速度降低的缓冲阻尼函数，其中该微缓冲区

奇异区域[0,ε]和微缓冲区域

一并成为非安全区域；

S5、通过奇异势能函数产生光滑且连续的虚拟力来引导6自由度机器人或7自由度机器人从非安全区域进入安全区域；

S6、对所述6自由度机器人或7自由度机器人的运动轨迹进行奇异性校验。

作为一种优选的方案，所述步骤S1中运动学模型并完成运动学分析具体包括以下计算步骤：

S11、建立所述6自由度机器人或7自由度机器人的D-H运动学坐标系；

S12、利用机器人各连杆的D-H参数求取机器人各连杆坐标系之间的变换矩阵

其中^i-1A_i为第i个坐标系相对于第i-1个坐标系的变换关系；θ_i表示绕着z_i轴，从x_i-1旋转到x_i的角度；α_i-1表示绕着x_i-1轴，从z_i-1旋转到z_i的角度；a_i-1表示沿着x_i-1轴，从z_i-1移动到z_i的距离；d_i表示沿着z_i轴，从x_i-1移动到x_i的距离；

S13、利用得到的变换矩阵求取6自由度机器人或7自由度机器人正向运动学方程的数学表达式⁰T_N＝⁰A₁ ¹A₂…^N-1A_N，其中，N为关节数目，^N-1A_N表示从坐标系N-1到坐标N的矩阵变换关系，⁰T_N是末端坐标系相对于基坐标系统的矩阵变换关系；

S14、计算并获得机器人的末端位姿与各关节转角之间的数学表达式

其中，n_x为末端执行器坐标系x轴相对于基坐标系x轴的方向矢量，n_y为末端执行器坐标系x轴相对于基坐标系y轴的方向矢量，n_z为末端执行器坐标系x轴相对于基坐标系z轴的方向矢量，o_x为末端执行器坐标系y轴相对于基坐标系x轴的方向矢量，o_y为末端执行器坐标系y轴相对于基坐标系y轴的方向矢量，o_z为末端执行器坐标系y轴相对于基坐标系z轴的方向矢量，a_x为末端执行器坐标系z轴相对于基坐标系x轴的方向矢量，a_y为末端执行器坐标系z轴相对于基坐标系y轴的方向矢量，a_z为末端执行器坐标系z轴相对于基坐标系z轴的方向矢量，p_x为末端执行器坐标系x轴相对于基坐标系x轴的位置矢量，p_y为末端执行器坐标系y轴相对于基坐标系y轴的位置矢量，p_z为末端执行器坐标系z轴相对于基坐标系z轴的位置矢量。

作为一种优选的方案，步骤S2中奇异值的计算方式为：

S21、利用机器人的末端位姿与各关节转角之间的数学表达式，采用微分变换法求解机器人雅可比矩阵J(q)＝[¹J_N ²J_N … ⁱJ_N]；其中，^i-1T_N＝^i-1A_i ⁱA_i+1…^N-1A_N,i为列,

其中p_xi，p_yi，n_xi，ⁿ _yi，n_zi，o_xi，o_yi，o_zi，a_xi，a_yi，a_zi为^i-1T_N变换中的位置和方向矢量；

S22、运用奇异值分解原理J(q)＝UΣV^T对机器人奇异性进行分析，其中U∈R^m×m和V∈R^n×n都是正交矩阵,

r为J(q)的秩,奇异值σ₁≥σ₂≥…≥σ_r>0,σ_r+1＝…＝σ_m＝0。

作为一种优选的方案，所述步骤S3中、以跟踪精度为目标函数，以关节速度极限为约束函数，通过粒子群优化算法获得变阻尼最小二乘法DLS方法中的最大阻尼值λ_max和临界值ε的计算方法为：

S31、提出变阻尼最小二乘法DLS，即关节速度公式

结合奇异值分解原理；

而6自由度机器人时，其关节速度公式为：

其中σ_i，v_i，u_i分别为6自由度机器人雅可比矩阵奇异值分解J(q)＝UΣV^T中Σ，V，U对应的元素；

而7自由度机器人关节速度公式为：

此时其中σ_i，v_i，u_i分别为7自由度机器人雅可比矩阵奇异值分解J(q)J^T(q)＝UΣV ^T中Σ，V，U对应的元素；

奇异阻尼函数

其中σ(q)表示J(q)奇异值分解的最小奇异值，ε为奇异区域与非奇异区域的临界值，

为第N个关节的速度，v_x,v_y,v_z为末端执行器坐标系在x,y,z方向上的线速度，w_x,w_y,w_z为末端执行器坐标系在x,y,z方向上的角速度；

S32、6自由度和7自由度机器人均以跟踪精度

为目标函数，

为约束函数，其中

为关节速度极限，通过粒子群优化算法获得所述DLS方法中的λ_max和ε，并以奇异区域作为第一奇异性判据0≤σ(q)≤ε。

作为一种优选的方案，所述步骤S4中的缓冲阻尼函数为

作为一种优选的方案，所述S5中引入的奇异势能函数为

其中梯度

使机器人以最有效的方向远离非安全区域，K＝diag(k₁,k₂,…,k_N)为关节速度超出极限的关节对应的最大扭矩或力，函数

保证奇异势能函数产生连续且光滑的力，其中d(σ)是奇异值σ到σ_m＝0的距离,取

采用了上述技术方案后，本发明的效果是：1、该奇异性处理方法在奇异性判断上采用两种判据组合的方式，一种是以奇异区域作为第一奇异性判据，该奇异区域为奇异值小于ε的区域；本发明避开试错法获得λ_max和ε，以跟踪精度为目标函数，以关节速度极限为约束函数，通过粒子群优化算法获得变阻尼最小二乘法方法DLS中的最大阻尼值λ_max和奇异区域，解决因奇异引发的关节速度过大问题；而另一种判断依据是以关节速度是否超速度极限作为第二奇异性判据，任何优化算法都不能完全保证获得的结果为最优值，即可能出现局部最优值，因此考虑这一弊端，引入微缓冲区

这样避免了机器人终端执行器在奇异区域之外关节速度超出极限的现象，此时奇异区域和微缓冲区域一并成为非安全区域；这样通过两种奇异性判据，解决了目前通过试错方式获得的λ_max和ε；2、DLS算法只是解决机器人处于奇异位形区域和邻近区域超关节速度极限及速度颠簸的问题，并没有从根本上使机器人远离奇异，因此本发明的继续对该问题进行深入研究并提出了使机器人执行末端远离奇异的方法，通过奇异势能函数产生光滑且连续的虚拟力来引导6自由度机器人或7自由度机器人从非安全区域进入安全区域；从根本上解决雅克比逆不稳定而产生的弊端。

附图说明

下面结合附图和实施例对本发明进一步说明。

图1是本发明实施例的机器人的D-H运动学坐标系示意图；

图2是虚拟力与奇异值的关系示意图像；

图3是6自由度机器人在无阻尼情况下各关节速度与奇异值的关系图；

图4a是6自由度机器人在有阻尼情况下的q₁关节速度图；

图4b是6自由度机器人在有阻尼情况下的q₂关节速度图；

图4c是6自由度机器人在有阻尼情况下的q₆关节速度图；

图5a是q₂从安全区域进入非安全区域在无DLS无虚拟力F，有DLS无无虚拟力F和有DLS有无虚拟力F的变化曲线；

图5b是σ(q)从安全区域进入非安全区域在无DLS无虚拟力F，有DLS无虚拟力F和有DLS有虚拟力F的变化曲线图；

图6是7自由度机器人在无阻尼情况下各关节速度与奇异值的关系图；

图7a是7自由度机器人在有阻尼情况下的d₁关节速度图；

图7b是7自由度机器人在有阻尼情况下的q₃关节速度图；

图7c是7自由度机器人在有阻尼情况下的q₄关节速度图；

图7d是7自由度机器人在有阻尼情况下的q₅关节速度图；

图7e是7自由度机器人在有阻尼情况下的q₆关节速度图；

图7f是7自由度机器人在有阻尼情况下的q₇关节速度图；

图8a是σ(q)从安全区域进入非安全区域在无DLS无虚拟力F，有DLS无虚拟力F和有DLS有虚拟力F的变化曲线图；

图8b是q₄从安全区域进入非安全区域在无DLS无无虚拟力F，有DLS无无虚拟力F和有DLS有无虚拟力F的变化曲线；

具体实施方式

下面通过具体实施例对本发明作进一步的详细描述。

本实施例中的附图是模拟机器人在安全区域和非安全区域的运动，通过MATLAB编辑程序，计算无阻尼情况和有阻尼情况下的各个关节速度变化；计算引入虚拟力F前后，关节角度和奇异值的变化。

一种6自由度机器人或7自由度机器人的奇异性处理方法，包括以下步骤：

S1、建立6自由度机器人或7自由度机器人的运动学模型并完成运动学分析；所述步骤S1中运动学模型并完成运动学分析具体包括以下计算步骤：

S11、建立所述6自由度机器人或7自由度机器人的D-H运动学坐标系；如图1所示。

S12、利用机器人各连杆的D-H参数求取机器人各连杆坐标系之间的变换矩阵

其中^i-1A_i为第i个坐标系相对于第i-1个坐标系的变换关系；θ_i表示绕着z_i轴，从x_i-1旋转到x_i的角度；α_i-1表示绕着x_i-1轴，从z_i-1旋转到z_i的角度；a_i-1表示沿着x_i-1轴，从z_i-1移动到z_i的距离；d_i表示沿着z_i轴，从x_i-1移动到x_i的距离；

S13、利用得到的变换矩阵求取6自由度机器人或7自由度机器人正向运动学方程的数学表达式⁰T_N＝⁰A₁ ¹A₂…^N-1A_N，其中，N为关节数目，^N-1A_N表示从坐标系N-1到坐标N的矩阵变换关系，⁰T_N是末端坐标系相对于基坐标系统的矩阵变换关系；

S14、计算并获得机器人的末端位姿与各关节转角之间的数学表达式

其中，n_x为末端执行器坐标系x轴相对于基坐标系x轴的方向矢量，n_y为末端执行器坐标系x轴相对于基坐标系y轴的方向矢量，n_z为末端执行器坐标系x轴相对于基坐标系z轴的方向矢量，o_x为末端执行器坐标系y轴相对于基坐标系x轴的方向矢量，o_y为末端执行器坐标系y轴相对于基坐标系y轴的方向矢量，o_z为末端执行器坐标系y轴相对于基坐标系z轴的方向矢量，a_x为末端执行器坐标系z轴相对于基坐标系x轴的方向矢量，a_y为末端执行器坐标系z轴相对于基坐标系y轴的方向矢量，a_z为末端执行器坐标系z轴相对于基坐标系z轴的方向矢量，p_x为末端执行器坐标系x轴相对于基坐标系x轴的位置矢量，p_y为末端执行器坐标系y轴相对于基坐标系y轴的位置矢量，p_z为末端执行器坐标系z轴相对于基坐标系z轴的位置矢量。

S2、采用微分变换法求解6自由度机器人或7自由度机器人的雅可比矩阵，对6自由度机器人或7自由度机器人进行奇异性分析，计算出对应的奇异值；

步骤S2中奇异值的计算方式为：

S21、利用机器人的末端位姿与各关节转角之间的数学表达式，采用微分变换法求解机器人雅可比矩阵J(q)＝[¹J_N ²J_N … ⁱJ_N]；其中，^i-1T_N＝^i-1A_i ⁱA_i+1…^N-1A_N,i为列,

其中p_xi，p_yi，n_xi，n_yi，n_zi，o_xi，o_yi，o_zi，a_xi，a_yi，a_zi为^i-1T_N变换中的位置和方向矢量；

S22、运用奇异值分解原理J(q)＝UΣV^T对机器人奇异性进行分析，其中U∈R^m×m和V∈R^n×n都是正交矩阵,

r为J(q)的秩,奇异值σ₁≥σ₂≥…≥σ_r>0,σ_r+1＝…＝σ_m＝0。

S3、以跟踪精度为目标函数，以关节速度极限为约束函数，通过粒子群优化算法获得变阻尼最小二乘法方法DLS中的最大阻尼值λ_max和奇异区域与非奇异区域的临界值ε，其中[0,ε]定义奇异区域，以该奇异区域作为第一奇异性判据，并引入与奇异区域对应的用于使关节速度降低的奇异阻尼函数；如图2所示。

所述步骤S3中、以跟踪精度为目标函数，以关节速度极限为约束函数，通过粒子群优化算法获得变阻尼最小二乘法DLS方法中的最大阻尼值_λmax和临界值ε的计算方法为：

S31、提出变阻尼最小二乘法DLS，即关节速度公式

结合奇异值分解原理；

而6自由度机器人时，其关节速度公式为：

其中σ_i，v_i，u_i分别为6自由度机器人雅可比矩阵奇异值分解J(q)＝UΣV^T中Σ，_V，U对应的元素；

而7自由度机器人关节速度公式为：

此时其中σ_i，v_i，u_i分别为7自由度机器人雅可比矩阵奇异值分解J(q)J^T(q)＝UΣV^T中Σ，V，U对应的元素；

奇异阻尼函数

其中σ(_q)表示J(q)奇异值分解的最小奇异值，ε为奇异区域与非奇异区域的临界值，

为第N个关节的速度，v_x,v_y,v_z为末端执行器坐标系在x,y,z方向上的线速度，w_x,w_y,w_z为末端执行器坐标系在x,y,z方向上的角速度；

S32、6自由度和7自由度机器人均以跟踪精度

为目标函数，

为约束函数，其中

为关节速度极限，通过粒子群优化算法获得所述DLS方法中的λ_max和ε，并以奇异区域作为第一奇异性判据0≤σ(q)≤ε。

S4、以关节速度是否超速度极限作为第二奇异性判据，并引人微缓冲区、以及与微缓冲区对应的用于使关节速度降低的缓冲阻尼函数，其中该微缓冲区

奇异区域[0,ε]和微缓冲区域

一并成为非安全区域；所述步骤S4中的缓冲阻尼函数为

S5、通过奇异势能函数产生光滑且连续的虚拟力来引导6自由度机器人或7自由度机器人从非安全区域进入安全区域；所述S5中引入的奇异势能函数为

其中梯度

使机器人以最有效的方向远离非安全区域，K＝diag(k₁,k₂,…,k_N)为关节速度超出极限的关节对应的最大扭矩或力，函数

保证奇异势能函数产生连续且光滑的力，其中d(σ)是奇异值σ到σ_m＝0的距离,取

S6、对所述6自由度机器人或7自由度机器人的运动轨迹进行奇异性校验。

本实施例中按照上述奇异性处理方法后，在奇异性校验的过程中选取了目前常规的四种传统DLS算法进行对比。其中四种传统DLS算法中λ_max和σ(q)的取值与本发明所提出的阻尼函数h中的λ_max和σ(q)一样，进一步说明本发明在解决奇异问题上的有效性。

四种传统DLS算法的阻尼函数如下，

(一)

(二)

(三)

(四)

并且，先对6自由度的机器人进行奇异性验证，其中，6自由度的机器人的D-H参数如下表1。

表1

选取q＝[π/30.001π/400π/6]^T应用于粒子群优化算法，设置末端执行器速度

定位精度

λ_max∈[0.01,0.9]，ε∈[0.01,0.2],优化算法结束后获得λ_max＝0.7，ε＝0.15。

如图3所示，图3示意了6自由度机器人在无DLS算法时的关节速度图，图中可以发现，发现q₁、q₂、q₆三个关节超出了关节速度极限，当奇异值在[0.004,0.02]时，q₁和q₂的速度在10到30rad/s之间，超过其极限2rad/s。而q₆在整个不安全区域中具有更高的速度，并且也超过了极限值6rad/s

再如图4a、图4b和图4c所示，图4a中示意了q₁关节在采用了DLS算法下本发明的处理方法与目前常规的四种方法的比对曲线图，附图中纵坐标为q₁关节的角速度，横坐标为奇异值。同理，图4b和图4c分别表示了q₂、q₃关节的比对曲线图。从图4a、图4b和图4c中可以看出经过，经过本发明的处理方法中DLS算法后，q₁、q₂、q₆在非安全区域均在关节速度极限之内，并且采用了本发明的处理方法后，我们发现，本发明的处理方法与常规的四种方法在非安全区域均有效抑制q₁和q₂的速度。但是对于q₆，当奇异值在[0,0.13]，h₂和h₄阻尼效果明显，但当奇异值在微缓冲区域(0.13,0.2]时，q₆的速度超过其极限，这是不允许的。在正常情况下，在安全区域中不需要阻尼系数。但是，h₁和h₃的阻尼函数却在安全区域仍产生阻尼效果，降低机器人运动精度，这是不合理的。然而，在不安全区域内，本发明的处理方法始终确保q₆的速度在其极限之内。进入安全区域后，本发明的阻尼消失。此外，在不安全区域末端，q₁、q₂和q₆的速度更接近安全区域初始阶段的相应关节速度，从而防止了两个区域的交界处由关节速度之差引起的速度波动。

选取关节角度起点q_初＝[1.0852 0.98 0.1876 0.038 0.038 0.5616]^T和终点q_末＝[1.0732 0.62 0.1756 0.026 0.026 0.5496]^T的一段轨迹，角度增量△q＝[-0.001 -0.03 -0.001 -0.001 -0.001 -0.001]^T.这段轨迹从安全区域进入非安全区域的轨迹，经过运动学和奇异分解分析，该段轨迹奇异是由q₂引起的.因此

虚拟力如下：

其中K＝diag([0 10 0 0 0 0]).图5a和图5b分别为这段轨迹的运动过程中q₂和σ(q)在无DLS无虚拟力F，有DLS无虚拟力F和有DLS有虚拟力F的变化曲线图,不难看出因为安全区域不存在奇异问题，这三种方法下q₂和σ(q)的变化在安全区域是一致的，进入非安全区域，虽然无阻尼无势能函数，有阻尼无势能函数q₂下和σ(q)的变化也是一致的，但是无DLS无虚拟力F方法将导致关节角q₁、q₂、q₆在非安全区域超过速度极限。尽管有DLS无虚拟力F方法解决了该问题，但末端执行器轨迹产生了误差。本发明所提出的有DLS有虚拟力F方法下q₂和σ(q)变化轨迹如下。当q₂即将从0.97rad达到0.8rad时，由于虚拟力的存在，促使q₂从0.8rad移至0.85rad。按照设置的关节增量运动至0.82rad，当即将再次达到0.8rad时，虚拟力再次驱动至0.85rad。相对应的最小奇异值σ(q)直接从0.1965到0.198，再到0.1962，然后再到0.198，然后重复此过程以使机械手始终在安全区域内运行。

再对7自由度机器人进行奇异性校验，该校验中还是选用本发明中的奇异性处理方法和上述的四种常规方法进行比对。其中7自由度机器人D-H参数如下表：

选取q＝[44 π/3 π/6 17π/10 π/3 π/4 π/3]^T应用于粒子群优化算法.设置末端执行器速度

定位精度

λ_max∈[0.01,0.9]，ε∈[0.01,0.2]，优化算法结束后获得λ_max＝0.86，ε＝0.037。

如图6所示，图6中记载了7自由度机器人无DLS时关节速度图，其中，可以明确发现，d₁,q₃,q₄,q₅,q₆,q₇超出了关节速度极限。

而使用了本发明的DLS后，如图7a-图7f所示，我们发现，本发明的处理方法与常规的四种方法在非安全区域均有效抑制d₁,q₃,q₄,q₅,q₆,q₇的速度。但是对于q₄，当奇异值在微缓冲区域(0.037,0.0481)时，q₄的速度超过其极限，这是不允许的。在正常情况下，在安全区域中不需要阻尼系数。但是，h₁和h₃的阻尼函数却在安全区域仍产生阻尼效果，降低机器人运动精度，这是不合理的。然而，在不安全区域内，本发明的处理方法始终确保q₄的速度在其极限之内。进入安全区域后，本发明的阻尼消失。此外，在不安全区域末端，d₁,q₃,q₄,q₅,q₆,q₇的速度更接近安全区域初始阶段的相应关节速度，从而防止了两个区域的交界处由关节速度之差引起的速度波动。选取关节角度起点q_initial＝[20 0.9992 0.4756 0.110.9992 0.7374 0.9992]^T和终点q_final＝[28 1.0152 0.4916 0.59 1.0152 0.75341.0152]^T的一段轨迹，角度增量△q＝[0.5 0.001 0.001 0.03 0.001 0.001 0.001]^T.这段轨迹从安全区域进入非安全区域，经过运动学和奇异分解分析，该段轨迹奇异是由q₄引起的.因此

虚拟力如下：

其中K＝diag([0 0 0 10 0 0 0]).图8a和图8b为这段轨迹的运动过程中的q₄和σ(q)在无DLS无虚拟力F，有DLS无虚拟力F和有DLS有虚拟力F的变化曲线图。不难看出因为安全区域不存在奇异问题，这三种方法下q₂和σ(q)的变化在安全区域是一致的，但是，当进入非安全区域时，与无DLS无F方法相比，有DLS无虚拟力F方法下的q₄和σ(q)会从非安全区域向安全区域方向移动一点，但仍处于不安全区域，然而，根据先前的分析，有DLS无虚拟力F方法可以解决d₁,q₃,q₄,q₅,q₆,q₇超出关节速度极限的问题，但末端执行器轨迹产生了误差。本发明所提出的有DLS有虚拟力F方法下q₄和σ(q)变化如下：当q₄即将从0.11rad达到0.41rad时，由于虚拟力的存在，促使q₄从0.41rad移至0.356rad，按照设置的关节增量运动至0.386rad，当即将再次达到0.41rad时，虚拟力再次驱动q₄运动至0.356rad。相对应的最小奇异值σ(q)直接从0.0495到0.0505，再到0.0492，然后再到0.0505，然后重复此过程以使机械手始终在安全区域内运行。

本发明提出了一种变阻尼最小二乘法解决机器人在奇异位形及附近区域关节速度过大的问题。首先将末端执行器跟踪精度作为目标函数，关节速度极限作为约束函数，使用粒子群优化算法找到最佳的最大阻尼值和奇异区域，除了奇异区域外，还引入了微缓冲区区域，以避免传统DLS算法由于对奇异区域的选择较小导致关节速度超过极限的现象，图4a、4b和4c以及图8a和图8b表明本发明提出的变阻尼函数在非安全区域解决关节速度过大问题上优于传统最小二乘法。然后引入奇异排斥势场函数，并应用于6自由度机器人或7自由度机器人从安全区域到不安全区域的运动轨迹，图5a和图5b、图8a和图8b表明，本发明可以产生平滑的虚拟力促使6自由度机器人或7自由度机器人远离非安全区域，即始终在安全区域内运行，从根本上解决了雅可比逆在奇异位形及附近区域不稳定带来的弊端。

Claims (6)

1.一种6自由度机器人或7自由度机器人的奇异性处理方法，其特征在于：包括以下步骤：

S1、建立6自由度机器人或7自由度机器人的运动学模型并完成运动学分析；

S2、采用微分变换法求解6自由度机器人或7自由度机器人的雅可比矩阵，对6自由度机器人或7自由度机器人进行奇异性分析，计算出对应的奇异值；

S3、以跟踪精度为目标函数，以关节速度极限为约束函数，通过粒子群优化算法获得变阻尼最小二乘法方法DLS中的最大阻尼值λ_max和奇异区域与非奇异区域的临界值ε，其中[0,ε]定义奇异区域，以该奇异区域作为第一奇异性判据，并引入与奇异区域对应的用于使关节速度降低的奇异阻尼函数；

S4、以关节速度是否超速度极限作为第二奇异性判据，并引人微缓冲区、以及与微缓冲区对应的用于使关节速度降低的缓冲阻尼函数，其中该微缓冲区

奇异区域[0,ε]和微缓冲区域

一并成为非安全区域；

S5、通过奇异势能函数产生光滑且连续的虚拟力来引导6自由度机器人或7自由度机器人从非安全区域进入安全区域；

S6、对所述6自由度机器人或7自由度机器人的运动轨迹进行奇异性校验。

2.如权利要求1所述的一种6自由度机器人或7自由度机器人的奇异性处理方法，其特征在于：所述步骤S1中运动学模型并完成运动学分析具体包括以下计算步骤：

S11、建立所述6自由度机器人或7自由度机器人的D-H运动学坐标系；

S12、利用机器人各连杆的D-H参数求取机器人各连杆坐标系之间的变换矩阵

其中^i-1A_i为第i个坐标系相对于第i-1个坐标系的变换关系；θ_i表示绕着z_i轴，从x_i-1旋转到x_i的角度；α_i-1表示绕着x_i-1轴，从z_i-1旋转到z_i的角度；a_i-1表示沿着x_i-1轴，从z_i-1移动到z_i的距离；d_i表示沿着z_i轴，从x_i-1移动到x_i的距离；

S13、利用得到的变换矩阵求取6自由度机器人或7自由度机器人正向运动学方程的数学表达式⁰T_N＝⁰A₁ ¹A₂…^N-1A_N，其中，N为关节数目，^N-1A_N表示从坐标系N-1到坐标N的矩阵变换关系，⁰T_N是末端坐标系相对于基坐标系统的矩阵变换关系；

S14、计算并获得机器人的末端位姿与各关节转角之间的数学表达式

其中，n_x为末端执行器坐标系x轴相对于基坐标系x轴的方向矢量，n_y为末端执行器坐标系x轴相对于基坐标系y轴的方向矢量，n_z为末端执行器坐标系x轴相对于基坐标系z轴的方向矢量，o_x为末端执行器坐标系y轴相对于基坐标系x轴的方向矢量，o_y为末端执行器坐标系y轴相对于基坐标系y轴的方向矢量，o_z为末端执行器坐标系y轴相对于基坐标系z轴的方向矢量，a_x为末端执行器坐标系z轴相对于基坐标系x轴的方向矢量，a_y为末端执行器坐标系z轴相对于基坐标系y轴的方向矢量，a_z为末端执行器坐标系z轴相对于基坐标系z轴的方向矢量，p_x为末端执行器坐标系x轴相对于基坐标系x轴的位置矢量，p_y为末端执行器坐标系y轴相对于基坐标系y轴的位置矢量，p_z为末端执行器坐标系z轴相对于基坐标系z轴的位置矢量。

3.如权利要求2所述的一种6自由度机器人或7自由度机器人的奇异性处理方法，其特征在于：步骤S2中奇异值的计算方式为：

S21、利用机器人的末端位姿与各关节转角之间的数学表达式，采用微分变换法求解机器人雅可比矩阵J(q)＝[¹J_N ²J_N…ⁱJ_N]；其中，^i-1T_N＝^i-1A_i ⁱA_i+1…^N-1A_N,i为列,

其中p_xi，p_yi，n_xi，ⁿ _yi，n_zi，o_xi，o_yi，o_zi，a_xi，a_yi，a_zi为^i-1T_N变换中的位置和方向矢量；

S22、运用奇异值分解原理J(q)＝UΣV^T对机器人奇异性进行分析，其中U∈R^m×m和V∈R^{n ×n}都是正交矩阵,

r为J(q)的秩,奇异值σ₁≥σ₂≥…≥σ_r>0,σ_r+1＝…＝σ_m＝0。

4.如权利要求3所述的一种6自由度机器人或7自由度机器人的奇异性处理方法，其特征在于：

所述步骤S3中、以跟踪精度为目标函数，以关节速度极限为约束函数，通过粒子群优化算法获得变阻尼最小二乘法DLS方法中的最大阻尼值λ_max和临界值ε的计算方法为：

S31、提出变阻尼最小二乘法DLS，即关节速度公式

结合奇异值分解原理；

而6自由度机器人时，其关节速度公式为：

其中σ_i，v_i，u_i分别为6自由度机器人雅可比矩阵奇异值分解J(q)＝UΣV^T中Σ，V，U对应的元素；

而7自由度机器人关节速度公式为：

此时其中σ_i，v_i，u_i分别为7自由度机器人雅可比矩阵奇异值分解J(q)J^T(q)＝UΣV^T中Σ，V，U对应的元素；

奇异阻尼函数

0≤σ(q)≤ε,其中σ(q)表示J(q)奇异值分解的最小奇异值，ε为奇异区域与非奇异区域的临界值，

为第N个关节的速度，v_x,v_y,v_z为末端执行器坐标系在x,y,z方向上的线速度，w_x,w_y,w_z为末端执行器坐标系在x,y,z方向上的角速度；

S32、6自由度和7自由度机器人均以跟踪精度

为目标函数，

为约束函数，其中

为关节速度极限，通过粒子群优化算法获得所述DLS方法中的λ_max和ε，并以奇异区域作为第一奇异性判据0≤σ(q)≤ε。

5.如权利要求4所述的一种6自由度机器人或7自由度机器人的奇异性处理方法，其特征在于：所述步骤S4中的缓冲阻尼函数为

6.如权利要求5所述的一种6自由度机器人或7自由度机器人的奇异性处理方法，其特征在于：所述S5中引入的奇异势能函数为

其中梯度

使机器人以最有效的方向远离非安全区域，K＝diag(k₁,k₂,…,k_N)为关节速度超出极限的关节对应的最大扭矩或力，函数

保证奇异势能函数产生连续且光滑的力，其中d(σ)是奇异值σ到σ_m＝0的距离,取

Images