CN110434851A - 一种5自由度机械臂逆运动学求解方法 - Google Patents

Abstract

一种5自由度机械臂逆运动学求解方法，包括以下步骤：1)根据机械臂D‑H参数建立正运动学模型；2)建立机械臂末端位姿误差函数模型；3)基于进化策略算法最小化末端位姿误差函数；4)迭代机械臂逆运动学方程至误差函数允许误差内；5)若迭代结束，误差函数仍未收敛至零或允许误差内，则返回步骤2)中更换初始关节值，重新计算。本发明提供了一种可以有效解决机械臂逆运动学迭代求解时，雅克比矩阵不满秩导致机械臂逆运动学无解的情况。该机械臂逆运动学求解方法具有较快的速度和极高的精度。

Description

一种5自由度机械臂逆运动学求解方法

技术领域

本发明涉及工业机器人的机械臂控制系统，尤其涉及一种基于蒙特卡洛方法与进化策略(ES)相结合的多自由度机械臂的逆运动学求解方法。

背景技术

随着科学技术和控制技术的发展，机器人被广泛应用于科研、军事、工业以及物流等领域。机械臂为机器人控制中的主要执行机构，该机构的合理性与可靠性将极大地影响整个机器人的应用。

机械臂逆运动学求解通常用于研究具有关节型结构的位姿，是机器人研究领域的基础技术。机械臂逆运动学通过将机械臂末端执行器的位姿从三维笛卡尔空间映射到内部关节空间对应的关节量，从而降低对机械臂控制的难度。机械臂逆运动学要解决的问题是找出一组关节角度使机械臂末端达到理想位姿，即末端坐标系与理想坐标系重合。

机械臂的逆运动学是非线性的问题——求解时必须考虑其解的存在性、多重解性、求解方法。逆运动学求解策略主要分为两类：1、封闭解。2、数值解。

封闭解具有较快的逆向求解速度、封闭的表达式的优点，是多组逆运动学解中的一个特殊解。机械臂逆运动学若要得到封闭解，机械臂机构必须要满足Pieper 准则，即机械臂三个相邻关节轴线交于一点或互相平行。封闭解法往往针对机械臂逆运动学方程不高于四次的多项式、不用迭代便可以完全求解的情况。

对于串联的机械臂而言，逆运动学的封闭解求解比较复杂。而数值法具有计算简单，并且对于任意复杂的机械臂都能进行逆运动学求解。常见的数值法包括: 雅克比矩阵伪逆法，阻尼最小二乘法等。通过雅克比矩阵求解逆运动学方程的解是逆运动学数值迭代求解的基本方法，但存在当雅克比矩阵不满秩时，逆运动学方程无解的情况。

逆运动学数值法求解的另一种思路是通过启发式算法来进行迭代优化求解，如遗传算法、模拟退火算法、人工神经网络等。启发式算法通过给定初始值，不断进行迭代更新，当误差达到所需精度时得到迭代结果。此类算法使用范围广泛，受限制约束少，但存在迭代次数较多、收敛较较慢、易陷入局部最优解、对初始值依赖程度高等缺陷。

发明内容

通过雅克比逆矩阵求解机械臂逆运动学方程时，为了克服由于机构奇异性的局部自由度退化导致逆运动学解不存在的现象，本发明提供了一种基于进化策略的5自由度机械臂逆运动学求解方法，具有较快的速度和极高的精度。

本发明解决其技术问题所采用的技术方案是：

一种5自由度机械臂逆运动学求解方法，包括以下步骤：

1)根据机械臂D-H参数建立正运动学模型

通过D-H参数确定机械臂每个连杆上的坐标系以及其坐标系之间的转换关系，连杆坐标系{i}相对于坐标系{i-1}的变换矩阵A_i，两相邻杆件的坐标系变换关系式根据D-H法则得：

上式各参数含义为杆件i相对于杆件i-1先绕轴z_i-1旋转θ_i角度，再沿轴z_i-1方向移动d_i距离，再沿轴x_i-1移动距离，再绕轴x_i-1旋转α_i角度；

将各个连杆变换矩阵A_i(i＝1,2,...,n)相乘得到机械臂末端坐标系相对于固定坐标系的变换矩阵表达式：

即为坐标系{n}相对于坐标系{0}的变换矩阵。是关于n个关节变量 q₁,q₂,...,q_n的函数，则：

根据式(3)，得机械臂的正运动学方程：

式(4)表明了机械臂末端位置P_3*1、姿态R_3*3与各关节变量之间的函数关系；

2)建立机械臂末端位姿误差函数模型

采用正态分布随机关节变量样本q_sample，任意选取一组q_e＝(θ₁,...,θ_n)作为初始猜测关节角，通过正运动学将该组关节角转换成末端位姿信息矩阵其中机械臂末端位置P_e∈R³，末端姿态R_e∈R^3*3是3*3的旋转矩阵，O为1*3的零矩阵；

对于给定的末端期望位姿信息矩阵其中末端期望位置P_d∈R³,末端期望姿态R_d∈R^3*3，O为1*3的零矩阵，通过初始末端姿态R_e与末端期望姿态R_d的內积求得两姿态矩阵各轴的欧拉角[r p γ]，其中r为绕x轴的旋转量，p为绕y轴的旋转量，γ为绕z轴的旋转量，末端初始猜测位姿和期望位姿的差分别表示为：

Δp_e＝p_d-p_e，Δp_e∈R³ (5)

ΔR_e＝[r p γ]

定义末端位姿误差向量为e＝[Δp_e ΔR_e]∈R^1*6，末端位姿误差函数如下

3)目标是在当前状态小的扰动下计算末端误差函数，即最小化误差函数 Err(θ)，将返回的函数值计入到一个新的状态，迭代直至Err(θ)在允许误差内；

4)进化策略算法首先通过蒙特卡洛方法采样n个随机的方向ε_i，该采样服从均值为零，方差不变σ的正态分布，基本更新状态规则如下：

式(7)中，θ_t+1是t+1时刻的关节量，θ_t是t时刻的关节量，α是学习因子，(7) 式表明状态更新沿着ε_i的方向，并且更新的方向与误差大小成正比，更大的误差函数值Err(θ)，意味在该方向上移动地更远，即状态更新地越快；

式(8)采用有限差分近似的方式来计算误差函数Err(θ)在θ点处的近似梯度， g_σ(θ)采用了对偶采样，有效地降低正态分布随机样本的方差；

5)不断地迭代(7)式，更新状态，直至误差函数收敛至零附近或迭代允许误差内，退出迭代，输出迭代结果q_out(θ)＝[θ₁ ... θ_n]，表明在允许误差内，末端姿态信息矩阵已与末端期望姿态信息矩阵重合，即得到机械臂逆运动学方程的解q_out(θ)＝[θ₁ ... θ_n]；

6)若上式步骤中，迭代结束，误差函数仍未收敛至零或允许误差内，则表明该解是局部最优解，返回步骤2)中更换初始关节值，使其跳出局部最优解。

本发明的技术构思为：首先，根据机器人的D-H参数建立机械臂正运动学模型。然后，基于该模型给出误差函数指标并将其转化为一个最优化问题。最后，采用进化策略求解优化问题，当误差函数收敛至零或允许误差范围内，得到最优结果，即机械臂逆运动学方程的最优解。

本发明的有益效果主要表现在：通过将机械臂末端当前姿态和期望姿态做误差函数转化为一个最优化问题，能够有效解决了机械臂逆运动学迭代求解时，雅克比矩阵不满秩导致机械臂逆运动学无解的情况。该机械臂逆运动学求解方法具有较快的速度和极高的精度。

附图说明

图1是机械臂逆运动学方程问题描述图。

图2是本发明一种5自由度的机械臂逆运动学方程求解算法流程图。

图3基于进化策略逆运动学求解的各关节迭代结果图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1～图3，一种基于进化策略的5自由度机械臂逆运动学解法，包括以下步骤：

1)根据机械臂D-H参数建立正运动学模型

表1为Youbot机械臂D-H参数表：

表1

通过D-H参数表确定机械臂每个连杆上的坐标系以及其坐标系之间的转换关系。连杆坐标系{i}相对于坐标系{i-1}的变换矩阵A_i，两相邻杆件的坐标系变换关系式根据D-H法则得：

上式各参数含义为杆件i相对于杆件i-1先绕轴z_i-1旋转θ_i角度，再沿轴z_i-1方向移动d_i距离，再沿轴x_i-1移动距离，再绕轴x_i-1旋转α_i角度；

将各个连杆变换矩阵A_i(i＝1,2,...,n)相乘可以得到机械臂末端坐标系相对于固定坐标系的变换矩阵表达式：

即为坐标系{n}相对于坐标系{0}的变换矩阵，是关于n个关节变量 q₁,q₂,...,q_n的函数，则：

根据式(3)，得机械臂的正运动学方程：

式(4)表明了机械臂末端位置P_3*1、姿态R_3*3与各关节变量之间的函数关系；

2)建立机械臂末端位姿误差函数模型

采用正态分布随机关节变量样本q_sample，任意选取一组q_e＝(θ₁,...,θ_n)作为初始猜测关节角，通过正运动学将该组关节角转换成末端位姿信息矩阵其中机械臂末端位置P_e∈R³，末端姿态R_e∈R^3*3是3*3的旋转矩阵，O为1*3的零矩阵；

对于给定的末端期望位姿信息矩阵其中末端期望位置P_d∈R³,末端期望姿态R_d∈R^3*3，O为1*3的零矩阵，通过初始末端姿态R_e与末端期望姿态R_d的內积求得两姿态矩阵各轴的欧拉角[r p γ]，其中r为绕x轴的旋转量，p为绕y轴的旋转量，γ为绕z轴的旋转量。末端初始猜测位姿和期望位姿的差分别表示为：

Δp_e＝p_d-p_e，Δp_e∈R³ (5)

ΔR_e＝[r p γ]

定义末端位姿误差向量为e＝[Δp_e ΔR_e]∈R^1*6，末端位姿误差函数如下

3)目标是在当前状态小的扰动下计算末端误差函数，即最小化误差函数 Err(θ)，将返回的函数值计入到一个新的状态，迭代直至Err(θ)在允许误差内；

4)进化策略算法首先通过蒙特卡洛方法采样n个随机的方向ε_i，该采样服从均值为零，方差不变σ的正态分布，基本更新状态规则如下：

式(7)中，θ_t+1是t+1时刻的关节量，θ_t是t时刻的关节量，α是学习因子。(7) 式表明状态更新沿着ε_i的方向，并且更新的方向与误差大小成正比，更大的误差函数值Err(θ)，意味在该方向上移动地更远，即状态更新地越快；

式(8)采用有限差分近似的方式来计算误差函数Err(θ)在θ点处的近似梯度， g_σ(θ)采用了对偶采样，有效地降低正态分布随机样本的方差；

5)不断地迭代(7)式，更新状态，直至误差函数收敛至零附近或迭代允许误差内，退出迭代，输出迭代结果q_out(θ)＝[θ₁ ... θ_n]。表明在允许误差内，末端姿态信息矩阵已与末端期望姿态信息矩阵重合，即得到机械臂逆运动学方程的解q_out(θ)＝[θ₁ ... θ_n]；

6)若上式步骤中，迭代结束，误差函数仍未收敛至零或允许误差内，则表明该解是局部最优解，返回步骤2)中更换初始关节值，使其跳出局部最优解；

7)该逆运动学求解各关节迭代结果，如图3所示，横坐标为迭代时间/秒，纵坐标为各关节变化量/弧度，图3中可看出，该逆运动学求解方法在较短的时间内能够有效地求解，并且具有极高精度，因此本发明的可操作性强。

Claims (1)

1.一种5自由度机械臂逆运动学求解方法，其特征在于，所述方法包括以下步骤：

1)根据机械臂D-H参数建立正运动学模型

通过D-H参数确定机械臂每个连杆上的坐标系以及其坐标系之间的转换关系，连杆坐标系{i}相对于坐标系{i-1}的变换矩阵A_i，两相邻杆件的坐标系变换关系式根据D-H法则得：

上式各参数含义为杆件i相对于杆件i-1先绕轴z_i-1旋转θ_i角度，再沿轴z_i-1方向移动d_i距离，再沿轴x_i-1移动距离，再绕轴x_i-1旋转α_i角度；

将各个连杆变换矩阵A_i(i＝1,2,...,n)相乘得到机械臂末端坐标系相对于固定坐标系的变换矩阵表达式：

即为坐标系{n}相对于坐标系{0}的变换矩阵，是关于n个关节变量q₁,q₂,...,q_n的函数，则：

根据式(3)，得机械臂的正运动学方程：

式(4)表明了机械臂末端位置P_3*1、姿态R_3*3与各关节变量之间的函数关系；

2)建立机械臂末端位姿误差函数模型

采用正态分布随机关节变量样本q_sample，任意选取一组q_e＝(θ₁,...,θ_n)作为初始猜测关节角，通过正运动学将该组关节角转换成末端位姿信息矩阵其中机械臂末端位置P_e∈R³，末端姿态R_e∈R^3*3是3*3的旋转矩阵，O为1*3的零矩阵；

对于给定的末端期望位姿信息矩阵其中末端期望位置P_d∈R³,末端期望姿态R_d∈R^3*3，O为1*3的零矩阵，通过初始末端姿态R_e与末端期望姿态R_d的內积求得两姿态矩阵各轴的欧拉角[r p γ]，其中r为绕x轴的旋转量，p为绕y轴的旋转量，γ为绕z轴的旋转量，末端初始猜测位姿和期望位姿的差分别表示为：

Δp_e＝p_d-p_e，Δp_e∈R³ (5)

ΔR_e＝[r p γ]

定义末端位姿误差向量为e＝[Δp_e ΔR_e]∈R^1*6，末端位姿误差函数如下

3)目标是在当前状态小的扰动下计算末端误差函数，即最小化误差函数Err(θ)，将返回的函数值计入到一个新的状态，迭代直至Err(θ)在允许误差内；

4)进化策略算法首先通过蒙特卡洛方法采样n个随机的方向ε_i，该采样服从均值为零，方差不变σ的正态分布，基本更新状态规则如下：

式(7)中，θ_t+1是t+1时刻的关节量，θ_t是t时刻的关节量，α是学习因子，(7)式表明状态更新沿着ε_i的方向，并且更新的方向与误差大小成正比，更大的误差函数值Err(θ)，意味在该方向上移动地更远，即状态更新地越快；

式(8)采用有限差分近似的方式来计算误差函数Err(θ)在θ点处的近似梯度，g_σ(θ)采用了对偶采样，有效地降低正态分布随机样本的方差；

5)不断地迭代(7)式，更新状态，直至误差函数收敛至零附近或迭代允许误差内，退出迭代，输出迭代结果q_out(θ)＝[θ₁ ... θ_n]，表明在允许误差内，末端姿态信息矩阵已与末端期望姿态信息矩阵重合，即得到机械臂逆运动学方程的解q_out(θ)＝[θ₁... θ_n]；

6)若上式步骤中，迭代结束，误差函数仍未收敛至零或允许误差内，则表明该解是局部最优解，返回步骤2)中更换初始关节值，使其跳出局部最优解。