[发明专利]一种基于旋量理论的6R机器人逆运动学几何求解方法

权利要求书

1.一种基于旋量理论的6R机器人逆运动学几何求解方法，其特征在于：该方法的实现过程如下：

S1、建立基坐标系与工具坐标系，通过基坐标系与工具坐标系确定6R机器人运动学参数，并建立正运动学模型；

S2、将6R机器人的前三个关节的逆解运动分解描述：由点q_e到点c₂＝[x₂ y₂ z₂]绕关节1的旋转运动；点c₂到点c₁＝[x₁ y₁ z₁]绕关节2的旋转运动；点c₁到点q_s绕关节3的旋转运动；其中q_s，q_e分别为机器人前三个关节末端的初始和目标位置，根据几何关系描述建立以x₁,y₁,z₁,x₂,y₂和z₂为变量的六元二次方程组；

S3、采用MATLAB中“SOLVE”函数求解以上方程组，得到c₁和c₂的坐标矢量；

S4、自此，前三个关节的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立6R机器人的前三个关节角变量θ₁，θ₂和θ₃的显式求解模型，并进行求解；

S5、在关节6的轴线上取一点作为后三个关节逆解运动中的初始位置q_s1，并基于旋量正运动学模型求解初始位置q_s1对应的目标位置q_e1；

S6、将关节4和5的逆解运动分解描述：由点q_e1到点c₃＝[x₃ y₃ z₃]的旋转运动；由点c₃到点q_s1的旋转运动；根据几何关系建立以x₃,y₃和z₃为变量的三元二次方程组，并采用“SOLVE”函数求解，得到c₃的坐标矢量；

S7、自此，关节4和关节5的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立关节4和关节5的关节角变量θ₄和θ₅显式求解模型，并进行求解；

S8、取不在关节6轴线上的任意一点q_s2，基于旋量正运动学模型求解q_s2对应的目标位置q_e2，则关节6的逆解运动始末位置已知；同理基于子问题1求解θ₆。

2.根据权利要求1所述的一种基于旋量理论的6R机器人逆运动学几何求解方法，其特征在于：S1确定6R机器人运动学参数并建立正运动学模型

已知该6R机器人的初始状态各关节所在位置矢量及旋转矢量如下：

其中r_i，1≤i≤6表示i关节在基坐标系的位置矢量，ω_i表示i关节的旋转矢量；

基于旋量理论，该6R机器人正运动学模型表示为，

其中g_st(0),g_st(θ)分别表示机器人末端的初始位姿与目标位姿，表示i关节旋转运动的指数积形式，

式中θ_i为第i个关节角位移；是i关节旋转矢量ω_i的另一种表示形式，由ω_i＝[ω₁ω₂ ω₃]定义为则ν_i是i关节运动的旋转线速度，ν_i＝-ω_i×r_i；

给定目标位姿，

S2针对前三个关节建立六元二次方程组

基于旋量理论前三个关节的旋转运动描述为，

式中q′_s和q′_e分别表示6R机器人末端在该旋转运动的初始位置与目标位置；由S1可知，

q′_s＝[x_s y_s z_s 1]＝[0 744 940 1]

q′_e＝[x_e y_e z_e 1]＝[-936.6611 631.7859 570.0752 1]

设运动经过点c₁和c₂,根据几何描述建立以下关系式组，

其中q₁，q₂和q₃分别为关节1、关节2和关节3旋转轴线上的任意一点，为简化模型取q₁＝[0 0 0]，q₂＝[0 150 250]和q₃＝[0 150 800]，那么式(7)表示为如下方程组，

采用MATLAB中“SOLVE”函数求解方程组(8)，得到x₁,y₁,z₁,x₂,y₂和z₂的解；

S3计算关节角位移θ₁，θ₂和θ₃

得到过程点坐标c₁＝[x₁ y₁ z₁]和c₂＝[x₂ y₂ z₂]后，将绕关节1、关节2和关节3的旋转运动分别描述如下，

基于Paden-Kahan子问题1，得到关节角位移的显示表达式如下，

S4针对关节4和关节5建立三元二次方程组

由式(3)知，

其中在关节6的轴线上取一点q_s1＝[x_s1 y_s1 z_s1]＝[0 744 0]，其绕关节4与关节5的旋转运动描述如下，

其中q′_e1＝g₁q′_s1＝[x_e1 y_e1 z_e1 1]；

设运动经过点坐标为c₃＝[x₃ y₃ z₃]，根据几何描述建立以下关系式组，

其中q₄＝[0 744 940]；则式(14)表示为如下方程，

采用MATLAB中“SOLVE”函数求解方程组(15)，得到x₃,y₃和z₃的解；

S5计算关节角位移θ₄和θ₅

得到过程点坐标c₃＝[x₃ y₃ z₃]后，将绕关节4和关节5的旋转运动分别描述如下，

基于Paden-Kahan子问题1，得到关节角位移θ₄和θ₅的显示表达式如下，

S6计算关节角位移θ₆

取不在关节6轴线上一点q_s2＝[0 750 940]，其绕关节6的旋转运动描述为，

其中同理基于Paden-Kahan子问题1，得到关节角位移θ₆的显示表达式如下，

通过以上求解得到八组运动学逆解。