[发明专利]一种考虑末端运动误差的六自由度机器人轨迹规划方法

权利要求书

1.一种考虑末端运动误差的六自由度机器人轨迹规划方法，其特征在于：步骤(1)基于旋量理论建立机器人正逆运动学模型

正运动学模型

已知该机器人在初始状态第i个关节所在位置矢量r_i及旋转矢量ω_i如下：

根据旋量理论，关节间的转换矩阵表示为指数积形式，

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

则机器人正运动学模型g_st(θ)表示如下：

逆运动学模型

将各关节角的求解转化为三个Paden-Kahan子问题，由于该机器人末端的位置决定于关节1,2和3，而其位姿决定于关节4,5和6；首先将前三个关节的逆向运动描述为：末端位置向量r_e绕关节1旋转-θ₁至r_e1，再绕关节2旋转-θ₂至r_e2，然后绕关节3旋转-θ₃至r₅，则θ₁，θ₂和θ₃分别通过以下三个表达式求出，其中式(5)属于子问题1，而式(6)和(7)属于子问题3；

其中r_e1由末端位置向量r_e＝[x y z]决定；δ₂,δ₃为定距离，δ₂＝||r_e1-r₂||，δ₃＝||r_e-r₃||；

其次θ₄，θ₅和θ₆分别通过以下两个表达式求出，其中式(8)属于子问题2，式(9)属于子问题1，

其中r₀₄在旋转轴6上而不在旋转轴4和5上，取r₀₄＝[0 744 0]；r₀₆不在旋转轴6上，取r₀₆＝[0 150 860]；

步骤(2)求取各关节轨迹节点并进行插值规划

将末端连续轨迹曲线定义如下，其姿态保持Ω＝[0 0 1]，且操作任务总时间T不大于1min；

其中0≤α≤360°，为等距离取N+1个关键路径点，取α＝(360n/N)°,n＝0,1,2,…N；

将得到的末端位姿代入逆运动学模型可求得各关节轨迹节点N+1个；采用三次样条曲线对关节轨迹节点进行插值运算，对某一关节而言，关节轨迹被分为N个子段，第n段轨迹角位移S_n(t)、角速度S′_n(t)、角加速度S″_n(t)表示如下：

其中，T_n为第n段时间间隔；θ_n,分别为时间t_n所对应的关节角位移，角速度和角加速度，且通过边界条件S_n(t_n-1)＝θ_n-1,S_n(t_n)＝θ_n及计算得到，t∈[t_n-1,t_n]；步骤(3)建立末端追踪误差模型

将规划得到的各关节角位移曲线上每隔20ms取值，得到M＝T/0.02个关节点组，并通过正运动学模型计算各关节点组对应的末端位姿如下，

其中θ_1m…θ_6m为第m个关节点组；提取g_m中对应的位置矢量P_m；

建立末端误差模型E_m如下，

E_m＝||P_m-O||-R (15)

其中O为轨迹圆心的位置矢量，R为连续轨迹上各点曲率半径，R＝150mm；

步骤(4)确定末端关键路径点个数n

取T_n＝2，对末端轨迹按照步骤(2)对各关节进行轨迹规划，再按照步骤(3)求出末端最大误差值max(E_m)，该任务要求末端误差满足以下条件，max(E_m)≤E_max＝1mm，若计算结果不满足任务要求，则增大路径点个数N，继续规划并计算末端误差，依次循环计算直到满足末端精度要求，从而确定N＝24；

步骤(5)以末端误差最小为目标函数优化轨迹

根据步骤(2)求得各关节角速度，角加速度和角加加速度最大值如下，

S′_max＝max(|{S′_n(t),1≤n≤24}|) (19)

S″_max＝max(|{S″_n(t),1≤n≤24}|) (20)

S″′_max＝max(|{S″′_n(t),1≤n≤24}|) (21)

则优化约束条件可表示如下，

其中时间间隔T_n范围根据具体任务确定，和为第i个关节所允许的最大角速度、角加速度和角加加速度值；

优化目标函数表示为，

f＝min(E_max) (23)。