CN112091979A - 一种基于位置级逆运动学的七自由度机械臂限位优化方法 - Google Patents

Abstract

一种基于位置级逆运动学的七自由度机械臂限位优化方法，它涉及一种七自由度机械臂限位优化方法。本发明为了解决现有的数值解无法得到封闭解，存在终态自运动；解析解无法针对偏置构型，存在无法实现对运动优化的问题。本发明首先基于固定某一关节角的参数化求解方法，得到7自由度机械臂逆运动学的解析解；然后将该固定的关节角度参数作为输入，关节限位作为优化指标，建立最优控制问题；再基于拉格朗日乘子法，将有约束问题转化为无约束问题；最后基于牛顿迭代法实现对最优的关节角参数的求解，通过给定初始构型、期望末端位姿和笛卡尔的路径规划，得到考虑关节限位优化的7个关节空间轨迹。本发明用于七自由度机械臂限位优化。

Description

一种基于位置级逆运动学的七自由度机械臂限位优化方法

技术领域

本发明涉及一种七自由度机械臂限位优化方法，具体涉及一种基于位置级逆运动学的七自由度机械臂限位优化方法，属于机器人逆运动学领域。

背景技术

为了满足机械臂运动的灵活性，机械臂往往具有冗余自由度(如7自由度)，且具有不同的构型，这对逆运动学求解带来挑战。基于雅可比零空间的速度级逆运动学求解方法是一种数值解法，由于冗余机械臂，其逆运动学都是数值解，所以无法得到解析解，会陷入终态自运动，基于雅可比的机械臂逆运动学通解为即机械臂到达期望位置，末端运动停止但是关节运动不停止，使得终态构型不可控(终态构型是指机械臂运动到目标位置的构型，由于自运动存在，目标构型会一直变化，无法确定，所以不可控)，而且不同的初始构型或运动路径到达同一位置的终态构型会不一致，无法预知机械臂的运动结果，这增加了机械臂实时运动规划的风险。对于肩关节和腕关节都不交于一点的7自由度偏置构型机械臂，无法采用臂角法求解，但满足三个相邻关节轴相互平行的机械臂，典型的如国际空间站加拿大臂，在求解逆运动学时，通常采用固定某一关节角作为参数(如关节1、2、6或7)，然后当作6自由度机械来处理。这种关节角参数化方法虽然能得到解析解，但是忽略了7自由度机械臂设计的初衷，没有利用机械臂的冗余性实现对逆解的优化，没有充分体现机械臂运动的灵活性。

综上所述，由于现有的数值解无法得到封闭解，且会存在终态自运动；解析解无法针对偏置构型，降自由度解法限定了解的可能性，把一个关节固定相当于限制了机械臂的灵活性，存在无法实现对运动优化的问题。

发明内容

本发明的目的是为了解决现有的数值解无法得到封闭解，且会存在终态自运动；解析解无法针对偏置构型，降自由度解法限定了解的可能性，把一个关节固定相当于限制了机械臂的灵活性，存在无法实现对运动优化的问题。进而提供一种基于位置级逆运动学的七自由度机械臂限位优化方法。

本发明的技术方案是：一种基于位置级逆运动学的七自由度机械臂限位优化方法，它包括以下步骤：

步骤一：基于固定某一关节角的参数化求解，得到七自由度机械臂逆运动学的解析解；

步骤二：将该固定的关节角度参数作为输入，关节限位作为优化指标，建立最优控制问题；

步骤三：基于拉格朗日乘子法，将有约束问题转化为无约束问题；

步骤四：基于牛顿迭代法实现对最优的关节角参数的求解；

步骤五：通过给定初始构型、期望末端位姿和笛卡尔的路径规划，得到了考虑关节限位优化的7个关节空间轨迹。

进一步地，步骤一包括以下步骤：

步骤一一：对于七自由度机械臂关节θ_i(i＝1，2...7)，固定某一关节u，将其看作六自由度机械臂，机械臂的正运动学关系可以表示为：

f(x，u)＝T (1)

其中，x为除去关节u的其余关节，T为期望位姿，在求解时为常值；

步骤一二：基于关节角参数化方法，求解出逆运动学的解析解，对应的位置级逆运动学表示为：

x＝ikine(u，T，x₀) (2)

其中，x₀为机械臂初始构型。在机械臂的运动空间内，不考虑机械臂奇异位置，则每对应一个固定关节角度u和对应的末端位姿T，都有一组对应的机械臂逆解x，x和u即组成了一组机械臂各关节角度θ。

进一步地，步骤二在建立最优控制问题时，考虑关节限位优化准则，对于各关节角度θ，都需满足的目标函数为：

其中，θ_imax和θ_imin分别为第i个关节的运动范围上下限，公式(1)和公式(3)即形成了七自由度机械臂考虑对关节限位优化的逆运动学求解问题。

进一步地，步骤三中将有约束问题转化为无约束问题：

Min G＝H+λ^T(f(x，u)-T) (4)

其中，λ为拉格朗日乘子。对于目标函数，极值存在的必要条件是

其中，

为目标函数的梯度向量，

为6×6矩阵记为J_6×6，每列由雅可比矩阵的x关节所在列组成，

为6×1的列向量记为J_u，是雅可比矩阵中关节u所在列；求解式(5)有：

带入式(6)中消去拉格朗日乘子，有：

即

进一步地，步骤四中基于牛顿迭代法实现对最优的关节角参数的求解过程为：

令

采用牛顿求根法求解g_x(u)＝0的根u^*，

其中，

假设机械臂构型非奇异，有

且

则牛顿求根迭代公式表示为：

进一步地，步骤五的计算过程为：

将u_k+1带入逆运动学公式(2)中，可得到对应的其余关节角度：

x_k+1＝ikine(u_k+1，T_k+1，x_k) (14)

其中，T_k+1为k+1时刻机械臂末端位姿的齐次变换矩阵，x_k为k时刻的机械臂关节角度，则x和u_k组成的各关节角度即为机械臂末端连续轨迹规划过程中，基于考虑关节限位优化的七自由度冗余机械臂位置级逆运动学求解方法得到的关节空间轨迹。

本发明与现有技术相比具有以下效果：

1、本发明解决了7自由度冗余机械臂逆解求解问题，并实现了对关节运动范围的优化。相比与传统的固定某一关节角度的关节角参数化求解方法，本发明方法实现了对关节角参数的实时调整，即实现了7个关节的同步运动，充分利用了机械臂的冗余特性。

2、本发明相比于基于雅可比速度级逆运动学求解方法，该方法基于位置级逆运动学，能给出目标位姿下机械臂构型的平衡状态(指步骤四：牛顿迭代法求得的根就是关于优化指标的最优解，当u通过迭代求解得到后，相应的机械臂其余关节角度也能确定了)，避免了机械臂的终态自运动。该方法不受规划路径和方向的影响，往复运动求出的关节角度一致，确保了重复运动时求解的一致性。

附图说明

图1是关节型七自由度偏置型机械臂的各关节坐标系。其中，各坐标系的确定方法采取D-H(Craig)方法，实施例将第一个关节的初始位置设置在0系，基座坐标系为B，将第七个关节设置在7系，末端执行器坐标系为E；关节轴线为Z轴，均采用右手定则建立，实施例关节1轴线Z₁和关节2轴线Z₂正交，关节2轴线Z₂和3轴线Z₃正交，关节5轴线Z₅和6轴线Z₆正交，关节6轴线Z₆和7轴线Z₇正交，关节3、4和5三轴轴线Z₃，Z₄，Z₅平行；a₀-关节1到末端的距离，a₁-关节2到关节1的距离，a₂-关节2到关节1的距离，a₃-连杆3长度，a₄-关节3到关节4的距离，a₅-连杆5长度，a₆-关节5到关节6的距离，a₇-关节6到关节7的距离，a₈-关节7到末端的距离。

图2是关节空间轨迹求解流程图。

图3是笛卡尔空间机械臂末端运动规划轨迹，其中①为从A到B的直线，②为从B到A的直线，③为从A到B的圆弧。

图4是末端运动轨迹①对应的关节空间轨迹。

图5是末端运动轨迹②对应的关节空间轨迹。

图6是末端运动轨迹③对应的关节空间轨迹。

具体实施方式

具体实施方式一：本实施方式的一种基于位置级逆运动学的七自由度机械臂限位优化方法，它包括以下步骤：

步骤一：基于固定某一关节角的参数化求解，得到七自由度机械臂逆运动学的解析解；

步骤二：将该固定的关节角度参数作为输入，关节限位作为优化指标，建立最优控制问题；

步骤三：基于拉格朗日乘子法，将有约束问题转化为无约束问题；

步骤四：基于牛顿迭代法实现对最优的关节角参数的求解；

步骤五：通过给定初始构型、期望末端位姿和笛卡尔的路径规划，得到了考虑关节限位优化的7个关节空间轨迹。

本发明针对降自由度求解方法中固定的关节角这一参数的优化，实现固定的关节角也是实时调整的，从而达到对指标的优化。

具体实施方式二：本实施方式的步骤一包括以下步骤：

步骤一一：对于七自由度机械臂关节θ_i(i＝1，2...7)，固定某一关节u，将其看作六自由度机械臂，机械臂的正运动学关系可以表示为：

f(x，u)＝T (1)

其中，x为除去关节u的其余关节，T为期望位姿，在求解时为常值；

步骤一二：基于关节角参数化方法，求解出逆运动学的解析解，对应的位置级逆运动学表示为：

x＝ikine(u，T，x₀) (2)

其中，x₀为机械臂初始构型。在机械臂的运动空间内，不考虑机械臂奇异位置，则每对应一个固定关节角度u和对应的末端位姿T，都有一组对应的机械臂逆解x，x和u即组成了一组机械臂各关节角度θ。该步骤为固定关节角的参数化解法，属于位置级的逆运动学解法。其它组成和连接关系与具体实施方式一相同。

具体实施方式三：本实施方式的步骤二在建立最优控制问题时，考虑关节限位优化准则，对于各关节角度θ，都需满足的目标函数为：

其中，θ_imax和θ_imin分别为第i个关节的运动范围上下限，公式(1)和公式(3)即形成了七自由度机械臂考虑对关节限位优化的逆运动学求解问题。优化的目标是使H最小，该优化指标使机械臂的关节运动远离极限位置。其它组成和连接关系与具体实施方式一或二相同。

具体实施方式四：本实施方式的步骤三中将有约束问题转化为无约束问题：

Min G＝H+λ^T(f(x，u)-T) (4)

其中，λ为拉格朗日乘子。对于目标函数，极值存在的必要条件是

其中，

为目标函数的梯度向量，

为6×6矩阵记为J_6×6，每列由雅可比矩阵的x关节所在列组成，

为6×1的列向量记为J_u，是雅可比矩阵中关节u所在列；求解式(5)有：

带入式(6)中消去拉格朗日乘子，有：

即

满足运动学约束以及使限位优化指标最小的最优解需要满足上述方程(10)。其它组成和连接关系与具体实施方式一、二或三相同。

具体实施方式五：本实施方式的步骤四中基于牛顿迭代法实现对最优的关节角参数的求解过程为：

令

采用牛顿求根法求解_gx(u)＝0的根u^*，即

其中，

假设机械臂构型非奇异，有

且

则牛顿求根迭代公式表示为：

则对于一组关节角度x，通过迭代求解出当前时刻满足限位指标最小的最优关节角参数解u。其它组成和连接关系与具体实施方式一、二、三或四相同。

具体实施方式六：本实施方式的步骤五的计算过程为：

将u_k+1带入逆运动学公式(2)中，可得到对应的其余关节角度：

x_k+1＝ikine(u_k+1，T_k+1，x_k) (14)

其中，T_k+1为k+1时刻机械臂末端位姿的齐次变换矩阵，x_k为k时刻的机械臂关节角度，则x和u_k组成的各关节角度即为机械臂末端连续轨迹规划过程中，基于考虑关节限位优化的七自由度冗余机械臂位置级逆运动学求解方法得到的关节空间轨迹。则在机械臂工作空间内，通过给定期望的笛卡尔空间目标位置，可以求得满足限位优化的七自由度机械臂逆解。其它组成和连接关系与具体实施方式一、二、三、四或五相同。

下面结合附图和具体实施例对本发明进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所实施例不作为对本发明的限定。

步骤一，根据表1和表2所示机械臂杆件参数和D-H参数，以固定关节2为例，求解基于关节2固定的参数化逆运动学解析解。

表1机械臂各杆件参数(单位：mm)

表2机械臂D-H参数

机械臂末端相对于基座的位姿为^BT_E，

式中，[n_x，n_y，n_z]^T、[s_x，s_y，s_z]^T、[a_x，a_y，a_z]^T分别为末端坐标系在基座坐标系中表示的单位矢量，[p_x，p_y，p_z]^T为末端坐标系相对基座的位置。不考虑奇异构型，当给定θ₂时，可依次求得其余各个关节角：

关节1角度

其中，a＝p_z-a₈n_z，b＝a₈n_y-p_y，c＝((a₈n_x-p_x-a₀)c₂-(a₂+a₄+a₆))/s₂。

关节6角度

关节7角度

θ₇＝Atan2(((a_yc₁-a_zs₁)s₂-a_xc₂)/s₆，(s_xc₂-(s_yc₁-s_zs₁)s₂)/s₆) (17)

关节4角度

关节3角度

θ₃＝Atan 2(A(a₃+a₅c₄)-Ba₅s₄，B(a₃+a₅c₄)+Aa₅s₄) (19)

其中，

A＝s₂(-a₀+a₈n_x-p_x+a₇a_xc₇+a₇s_xs₇)

+c₁c₂(a₈n_y-p_y+a₇a_yc₇+a₇s_ys₇)

-c₂s₁(a₈n_z-p_z+a₇a_zc₇+a₇s_zs₇)

B＝-a₁+s₁(a₈n_y-p_y+a₇a_yc₇+a₇s_ys₇)

+c₁(a₈n_z-p_z+a₇a_zc₇+a₇s_zs₇)

关节5角度

其中，

C＝-s₂(a_xc₇+s_xs₇)-c₁c₂(a_yc₇+s_ys₇)+s₁c₂(a_zc₇+s_zs₇)

D＝-s₁(a_yc₇+s_ys₇)-c₁(a_zc₇+s_zs₇)

以上公式(15)，(16)，(17)，(18)，(19)和(20)为给定关节2的图1偏置型七自由度机械臂逆运动学解析解。考虑关节运动范围大于360°，需要通过±360来增加解，然后基于每个关节移动量最小准则，从多组解中选择一组作为逆运动学反解。

对于七自由度机械臂关节θ_i(i＝1，2...7)，以固定关节2为例，定义输入为u＝θ₂，除关节2的其余关节组成的状态变量为：x＝[θ₁ θ₃…θ₇]^T，满足的约束条件可以表示为

f(x，u)＝T (1)

其中，T为关节x和u对应构型的末端位置和姿态的其次矩阵，该方程表示了机械臂的正运动学关系，在求解时为常值。

对应的位置级逆运动学即公式(15)，(16)，(17)，(18)，(19)和(20)组成的与关节2有关的表示写成如下统一形式

x＝ikine(u，T，x₀) (2)

其中，x₀为机械臂初始构型。在机械臂的运动空间内，不考虑机械臂奇异位置，则每对应一个固定关节角度u和对应的末端位姿T，都有一组对应的机械臂逆解x，x和u即组成了一组机械臂7个关节角度。

步骤二，考虑关节限位的优化指标为：

其中，θ_imax和θ_imin分别为关节运动范围上下限。对于关节1、2、3、5、6和7，其运动范围为[-300°，+300°]，由于关节4在0°和180°时为一种奇异构型，因此关节4的运动范围为(-300°，-180°)∪(-180°，0°)∪(0°，+180°)∪(+180°，+300°)，需要对初始时刻关节4所在区间进行判断，如果初始构型中θ₄∈(0°，+180°)，则θ_4max＝+180°，θ_4min＝0°，如果初始构型中θ₄∈(-180°，0°)，则θ_4max＝0°，θ_4min＝-180°，如果初始构型中θ₄∈(+180°，+300°)，则θ_4max＝+300°，θ_4min＝+180°，如果初始构型中θ₄∈(-300°，-180°)，则θ_4max＝+180°，θ_4min＝0°。

步骤三，将有约束问题转化为无约束问题

Min G＝H+λ^T(f(x，u)-T) (4)

对于目标函数，极值存在的必要条件是

其中

为目标函数的梯度向量。

为雅克比矩阵除去第2列的其余向量组成的6×6矩阵，

为雅可比矩阵第2列，为6×1的列向量。

求解公式(5)有：

带入式(6)中，消去拉格朗日乘子，有

即

步骤四，令

采用牛顿求根法求解g_x(u)＝0的根u^*，即

其中，

假设机械臂构型非奇异，有

且

则牛顿求根迭代公式表示为

步骤五，将u_k+1带入逆运动学公式(2)中，可得到对应的其余关节角度：

x_k+1＝ikine(u_k+1，T_k+1，x_k) (14)

其中，T_k+1为k+1时刻机械臂末端位姿的齐次变换矩阵，x_k为k时刻的机械臂关节角度。则x和u_k组成的各关节角度即为机械臂末端连续轨迹规划过程中，基于考虑关节限位优化的七自由度冗余机械臂位置级逆运动学求解方法得到的关节空间轨迹。

为验证该技术方法的有效性，以下结合图2至图6对本发明的技术方案做进一步描述，使用计算机仿真，平台选择Matlab，实施步骤如图2所示。

实施例1

[步骤S1]规划末端从A到B的直线运动，图3中轨迹①，仿真步长0.1s，仿真时间60s，其中初始构型为

q_start＝[0.0058 1.1720 1.4937 -2.0428 2.4953 1.7622 -0.5163]

初始位置A的位姿变换矩阵为

目标位置B的位姿变换矩阵为

[步骤S2]末端位姿插补采用Matlab机器人工具箱trinterp指令，数据点500个，到达目标位置后保持最终位置，关节空间轨迹点600组。

[步骤S3]根据关节4初始构型为-2.0428弧度，确定其应该约束的运动范围为q₄∈(-180°，0°)，而其余关节运动范围为(-300°，300°)。

[步骤S4]根据公式(7)计算各关节的梯度向量。

[步骤S5]将k时刻的关节2角度和k时刻其余关节角度带入公式(23)，得到k+1时刻的关节2角度。

[步骤S6]将k+1时刻的关节2角度和k时刻的其余关节角度以及k+1时刻期望末端位姿矩阵带入到逆运动学求解公式(14)中，求得k+1时刻的关节逆解。

[步骤S7]到求解出最终目标位置的解时运算结束。轨迹如图4所示。关节最终构型为

q_end＝[-0.7786 1.5123 0.3791 -1.5232 0.3041 1.5989 -0.5946]

实施例2

规划末端从B到A的直线运动，图3中轨迹②，仿真步长0.1s，仿真时间60s，实施步骤同实施例1，其中初始构型为

q_start＝[-0.7786 1.5123 0.3791 -1.5232 0.3041 1.5989 -0.5946]

初始位置B的位姿变换矩阵为

目标位置A的位姿变换矩阵为

轨迹如图5所示。关节最终构型为

q_ena＝[0.0058 1.1720 1.4937 -2.0428 2.4953 1.7622 -0.5163]

对比实施例1和实施例2可以看出反向运动的最终构型与正向运动的初始构型一致，由此可知，械臂在不同位置间进行往复运动的得到的关节构型是一致的。

实施例3

规划末端从A到B的圆弧运动，图3中轨迹③，表达式如下

仿真步长0.1s，仿真时间86.4s，插补数据点785个，到达目标位置后保持最终位置，关节空间轨迹点864组，实施步骤同实施例1，其中初始构型为

q_start＝[0.0058 1.1720 1.4937 -2.0428 2.4953 1.7622 -0.5163]

初始位置A的位姿变换矩阵为

目标位置B的位姿变换矩阵为

轨迹如图6所示。关节最终构型为

q_end＝[-0.7786 1.5123 0.3791 -1.5232 0.3041 1.5989 -0.5946]

对比实施例1和实施例3可以看出以不同路径运动到同一目标位置的机械臂最终构型一致。由此可知，对于给定的末端位置，求解出的关节构型可以不受末端路径规划的影响，这在实际应用中机械臂执行循环路径任务时，机械臂的运动构型具有可预见性。

由仿真可知，本发明求解的七自由度冗余机械臂逆解的关节运动范围都在关节极限要求范围之内，方法是有效的。且相比与传统的固定某一关节角度的关节角参数化求解方法，本发明方法实现了对关节角参数的实时调整，即实现了7个关节的同步运动，充分利用了机械臂的冗余特性。且相比于基于雅可比的速度级逆运动学结果受运动方向影响、终态自运动难以停止，该方法给出了目标位姿下机械臂构型的平衡状态，往复运动求出的关节角度一致，这种重复性实现了工作空间到关节空间的固定映射关系，对于构型依赖的算法和控制，可以提前计算位置依赖项。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (6)

1.一种基于位置级逆运动学的七自由度机械臂限位优化方法，其特征在于：它包括以下步骤：

步骤一：基于固定某一关节角的参数化求解，得到七自由度机械臂逆运动学的解析解；

步骤二：将该固定的关节角度参数作为输入，关节限位作为优化指标，建立最优控制问题；

步骤三：基于拉格朗日乘子法，将有约束问题转化为无约束问题；

步骤四：基于牛顿迭代法实现对最优的关节角参数的求解；

步骤五：通过给定初始构型、期望末端位姿和笛卡尔的路径规划，得到了考虑关节限位优化的7个关节空间轨迹。

2.根据权利要求1所述的一种基于位置级逆运动学的七自由度机械臂限位优化方法，其特征在于：步骤一包括以下步骤：

步骤一一：对于七自由度机械臂关节θ_i(i＝1，2...7)，固定某一关节u，将其看作六自由度机械臂，机械臂的正运动学关系可以表示为：

f(x，u)＝T (1)

其中，x为除去关节u的其余关节，T为期望位姿，在求解时为常值；

步骤一二：基于关节角参数化方法，求解出逆运动学的解析解，对应的位置级逆运动学表示为：

x＝ikine(u，T，x₀) (2)

其中，x₀为机械臂初始构型。在机械臂的运动空间内，不考虑机械臂奇异位置，则每对应一个固定关节角度u和对应的末端位姿T，都有一组对应的机械臂逆解x，x和u即组成了一组机械臂各关节角度θ。

3.根据权利要求2所述的一种基于位置级逆运动学的七自由度机械臂限位优化方法，其特征在于：步骤二在建立最优控制问题时，考虑关节限位优化准则，对于各关节角度θ，都需满足的目标函数为：

其中，θ_imax和θ_imin分别为第i个关节的运动范围上下限，公式(2)和公式(3)即形成了七自由度机械臂考虑对关节限位优化的逆运动学求解问题。

4.根据权利要求3所述的一种基于位置级逆运动学的七自由度机械臂限位优化方法，其特征在于：步骤三中将有约束问题转化为无约束问题：

Min G＝H+λ^T(f(x，u)-T) (4)

其中，λ为拉格朗日乘子。对于目标函数，极值存在的必要条件是

其中，

为目标函数的梯度向量，

为6×6矩阵记为J_6×6，每列由雅可比矩阵的x关节所在列组成，

为6×1的列向量记为J_u，是雅可比矩阵中关节u所在列；求解式(5)有：

带入式(6)中消去拉格朗日乘子，有：

即

5.根据权利要求4所述的一种基于位置级逆运动学的七自由度机械臂限位优化方法，其特征在于：步骤四中基于牛顿迭代法实现对最优的关节角参数的求解过程为：

令

采用牛顿求根法求解g_x(u)＝0的根u^*，

其中，

假设机械臂构型非奇异，有

且

则牛顿求根迭代公式表示为：

6.根据权利要求5所述的一种基于位置级逆运动学的七自由度机械臂限位优化方法，其特征在于：步骤五的计算过程为：

将u_k+1带入逆运动学公式(2)中，可得到对应的其余关节角度：

x_k+1＝ikine(u_k+1，T_k+1，x_k) (14)

其中，T_k+1为k+1时刻机械臂末端位姿的齐次变换矩阵，x_k为k时刻的机械臂关节角度，则x和u_k组成的各关节角度即为机械臂末端连续轨迹规划过程中，基于考虑关节限位优化的七自由度冗余机械臂位置级逆运动学求解方法得到的关节空间轨迹。

Images