[发明专利]一种5自由度机械臂逆运动学求解方法

摘要

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

主权项

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)中更换初始关节值，使其跳出局部最优解。