一种考虑末端运动误差的六自由度机器人轨迹规划方法技术

本发明专利技术公开了一种考虑末端运动误差的六自由度机器人轨迹规划方法，更具体是提出了一种结合旋量理论、三次样条插值算法及粒子群优化算法对机器人末端连续轨迹进行精确规划的方法。该方法首先基于旋量理论建立机器人正逆运动学模型，简化计算过程；在关节空间内采用三次样条插值，从而保证运动平滑；最后以关键点个数为变量将末端跟踪误差控制在要求范围以内，再以各段时间间隔为设计变量，各关节最大角速度、角加速度及角加加速度为约束条件，以追踪误差最小为优化目标对轨迹进行优化，从而获得规划效率高、跟踪误差小且运动平滑的规划轨迹。

A trajectory planning method for six degree of freedom robot with terminal motion error

The invention discloses a terminal considering the motion error of six DOF robot trajectory planning method is more specific, proposed a combination of spinor theory, three spline interpolation algorithm and particle swarm optimization algorithm for robot trajectory planning method for accurate continuous. Firstly, based on the screw theory to establish the robot kinematics model, simplify the calculation process; the three spline interpolation in the joint space, so as to ensure the smooth movement; finally, with the key point for a number of variables will be at the end of the tracking error control in the required scope, in each time interval as the design variables, each joint of the angular velocity, angular acceleration and angular acceleration as the constraint condition, the tracking error minimum as the optimization goal of trajectory optimization, planning to obtain high efficiency, small tracking error and smooth motion trajectory planning.

【技术实现步骤摘要】

本专利技术涉及到一种考虑运动误差的六自由度机器人轨迹规划方法，属于机器人运动控制研究领域，具体涉及通过结合旋量理论、三次样条插值算法以及粒子群优化算法对机器人末端连续轨迹进行规划，进而达到满足追踪精度要求、提高规划效率同时获得光滑运动轨迹的研究目的。
技术介绍
轨迹规划作为机器人控制研究的基础，对机器人的综合运动性能具有重要影响。按规划空间进行划分，规划方法一般包括关节空间规划和操作空间规划。关节空间规划方法是指直接对关节变量进行插值规划，最终建立关节变量随时间的变化曲线，但由于无法预知运动过程中末端的轨迹变化情况，只适用于末端点到点的简单操作任务。对于末端具有连续轨迹的操作任务，则需采用操作空间规划方法，该方法得到的运动轨迹虽然具有较高的追踪精度，但无法保证运动平稳性。为了能够在追踪末端轨迹的基础上保证运动的柔顺性，一些学者首先将在操作空间内选取能够保证末端轨迹的关键路径点，然后基于运动学逆解模型计算各关节空间内的轨迹节点，再在关节空间内进行轨迹规划，从而同时保证了基本末端轨迹和运动平稳性。在该类方法中规划轨迹仅能够保证关键路径点处的位置精度，由于关节插值带来的连续轨迹的追踪误差依然无法控制。在轨迹规划中，末端关键路径点个数越多越能够保证追踪精度，但路径点越多关节角位置间距越小，过小的间距难以体现插值算法在平滑轨迹方面起到的真实作用，且逆解次数越多，轨迹规划计算越复杂，从而降低规划效率。其次插值算法不仅影响着轨迹曲线，其算法关键参数(如时间间隔)也对追踪精度也有一定影响。为了提升规划效率，本专利基于旋量理论建立了正逆运动学模型，与传统的D-H模型相比，具有几何意义清晰、表达无奇异及运算量很少等优点。为获得平滑的运动，关节空间轨迹规划采用三次样条插值算法，该方法规划得到的关节角速度，角加速度连续。为使追踪误差在满足要求的条件下尽量小，本专利采用粒子群优化算法对轨迹进行优化，能够通过获取合理的关键路径点个数N和合适的时间间隔组合达到减小追踪误差的目的。
技术实现思路
本专利技术旨在提供一种考虑运动误差的六自由度机器人轨迹规划方法。该方法通过在末端连续轨迹上等距离取关键路径点，对采用旋量运动学模型逆解得到的各关节角位置进行插值规划，首先以关键点个数为变量将末端跟踪误差控制在要求范围以内，再以各段时间间隔为设计变量，各关节最大角速度、角加速度及角加加速度为约束条件，以追踪误差最小为优化目标对轨迹进行优化，从而获得规划效率高、跟踪误差小且运动平滑的规划轨迹。本专利技术是采用以下技术手段实现的：S1、基于旋量理论，对机器人模型建立正逆运动学模型。S2、在末端的连续轨迹上等距取N+1个关键路径点，得到N个轨迹段，N表示轨迹段的数量；并通过运动学逆解模型得到各关节的轨迹节点，采用三次修正样条曲线进行插值规划，从而得到各关节角位移、速度、加速度及加加速度随时间的变化曲线。S3、在角位移曲线上每隔20ms取点，并通过正运动学模型计算末端位置，建立跟踪误差模型，计算各位置点的追踪误差，并提取最大轨迹跟踪误差max(Em)。Em为轨迹跟踪误差，m为S4、Tn表示第n轨迹段的时间间隔，取Tn＝t,1≤n≤N，按照以上步骤进行规划轨迹，计算最大跟踪误差max(Em)，如果不满足精度要求max(Em)＜EmaxEmax为根据操作任务限定的最大跟踪误差，取N+1，再计算误差，依次循环计算，直到跟踪误差满足条件。时间间隔t与运动精度限制Emax根据具体任务要求确定。S5、当N确定之后，以各段时间间隔为设计变量，以各关节角速度、加速度及加加速度为约束条件，以跟踪误差最小为优化目标得到优化轨迹。本专利技术的特点是通过在末端连续轨迹上等距离取关键路径点，对采用旋量运动学模型逆解得到的各关节角位置进行插值规划，首先以关键点个数为变量将末端跟踪误差控制在要求范围以内，再以各段时间间隔为设计变量、各关节最大角速度、角加速度及角加加速度为约束条件，以追踪误差最小为优化目标对轨迹进行优化，从而获得规划效率高、跟踪误差小且运动平滑的规划轨迹。附图说明图1机器人末端轨迹规划流程图；图2某六轴工业机器人参数坐标系；图3各关节角速度、角加速度及角加速度曲线图；图4末端追踪误差曲线；具体实施方式步骤(1)基于旋量理论建立机器人正逆运动学模型正运动学模型如图2所示，已知该机器人在初始状态第i个关节所在位置矢量ri及旋转矢量ωi如下：根据旋量理论，关节间的转换矩阵表示为指数积形式，式中表示第i个关节旋量，θi为第i个关节角位移；可由ωi＝[ω1ω2ω3]定义为则νi是第i个关节运动的旋转线速度，νi＝-ωi×ri。则机器人正运动学模型gst(θ)表示如下：逆运动学模型本方法将各关节角的求解转化为三个Paden-Kahan子问题，由于该机器人末端的位置决定于关节1,2和3，而其位姿决定于关节4,5和6。首先将前三个关节的逆向运动描述为：末端位置向量re绕关节1旋转-θ1至re1，再绕关节2旋转-θ2至re2，然后绕关节3旋转-θ3至r5，则θ1，θ2和θ3分别通过以下三个表达式求出，其中式(5)属于子问题1，而式(6)和(7)属于子问题3。其中re1可以由末端位置向量re＝[xyz]决定；δ2,δ3为定距离，δ2＝||re1-r2||，δ3＝||re-r3||。其次θ4，θ5和θ6分别通过以下三个表达式求出，其中式(8)属于子问题2，式(9)属于子问题1，其中r04在旋转轴6上而不在旋转轴4和5上，取r04＝[07440]；r06不在旋转轴6上，取r06＝[0150860]；步骤(2)求取各关节轨迹节点并进行插值规划将末端连续轨迹曲线定义如下，其姿态保持Ω＝[001]，且操作任务总时间T不大于1min。其中0≤α≤360°，为等距离取N+1个关键路径点，取α＝(360n/N)°,n＝0,1,2,…N。将得到的末端位姿代入逆运动学模型可求得各关节轨迹节点N+1个。本专利采用三次样条曲线对关节轨迹节点进行插值运算，对某一关节而言，关节轨迹被分为N个子段，第n段(t∈[tn-1,tn])轨迹角位移Sn(t)、角速度S′n(t)、角加速度S″n(t)可表示如下：其中Tn为第n段时间间隔；θn,分别为时间tn所对应的关节角位移，角速度和角加速度，且可通过边界条件Sn(tn-1)＝θn-1,Sn(tn)＝θn及S′n(tn)＝S′n+1(tn)计算得到。步骤(3)建立末端追踪误差模型将规划得到的各关节角位移曲线上每隔20ms取值，得到M＝T/0.02个关节点组，并通过正运动学模型计算各关节点组对应的末端位姿如下，其中θ1m…θ6m为第m个关节点组；提取gm中对应的位置矢量Pm。建立末端误差模型Em如下，Em＝||Pm-O||-R(15)其中O为轨迹圆心的位置矢量，R为连续轨迹上各点曲率半径，R＝150mm。步骤(4)确定末端关键路径点个数n取Tn＝2，对末端轨迹按照步骤2对各关节进行轨迹规划，再按照步骤3求出末端最大误差值max(Em)，该任务要求末端误差满足以下条件，max(Em)≤Emax＝1mm，若计算结果不满足任务要求，则增大路径点个数N，继续规划并计算末端误差，依次循环计算直到满足末端精度要求，从而确定N＝24。步骤(5)以末端误差最小为目标函数优化轨迹根据步骤(2本文档来自技高网...

【技术保护点】
一种考虑末端运动误差的六自由度机器人轨迹规划方法，其特征在于：该方法通过在末端连续轨迹上等距离取关键路径点，对采用旋量运动学模型逆解得到的各关节角位置进行插值规划，首先以关键点个数为变量将末端跟踪误差控制在要求范围以内，再以各段时间间隔为设计变量，各关节最大角速度、角加速度及角加加速度为约束条件，以追踪误差最小为优化目标对轨迹进行优化，从而获得规划效率高、跟踪误差小且运动平滑的规划轨迹；S1、基于旋量理论，对机器人模型建立正逆运动学模型；S2、在末端的连续轨迹上等距取N+1个关键路径点，得到N个轨迹段，N表示轨迹段的数量；并通过运动学逆解模型得到各关节的轨迹节点，采用三次修正样条曲线进行插值规划，从而得到各关节角位移、速度、加速度及加加速度随时间的变化曲线；S3、在角位移曲线上每隔20ms取点，并通过正运动学模型计算末端位置，建立跟踪误差模型，计算各位置点的追踪误差，并提取最大轨迹跟踪误差max(Em)；Em为轨迹跟踪误差，m为S4、Tn表示第n轨迹段的时间间隔，取Tn＝t,1≤n≤N，按照以上步骤进行规划轨迹，计算最大跟踪误差max(Em)，如果不满足精度要求max(Em)＜Emax Emax为根据操作任务限定的最大跟踪误差，取N+1，再计算误差，依次循环计算，直到跟踪误差满足条件；时间间隔t与运动精度限制Emax根据具体任务要求确定；S5、当N确定之后，以各段时间间隔为设计变量，以各关节角速度、加速度及加加速度为约束条件，以跟踪误差最小为优化目标得到优化轨迹。...

【技术特征摘要】
1.一种考虑末端运动误差的六自由度机器人轨迹规划方法，其特征在于：该方法通过在末端连续轨迹上等距离取关键路径点，对采用旋量运动学模型逆解得到的各关节角位置进行插值规划，首先以关键点个数为变量将末端跟踪误差控制在要求范围以内，再以各段时间间隔为设计变量，各关节最大角速度、角加速度及角加加速度为约束条件，以追踪误差最小为优化目标对轨迹进行优化，从而获得规划效率高、跟踪误差小且运动平滑的规划轨迹；S1、基于旋量理论，对机器人模型建立正逆运动学模型；S2、在末端的连续轨迹上等距取N+1个关键路径点，得到N个轨迹段，N表示轨迹段的数量；并通过运动学逆解模型得到各关节的轨迹节点，采用三次修正样条曲线进行插值规划，从而得到各关节角位移、速度、加速度及加加速度随时间的变化曲线；S3、在角位移曲线上每隔20ms取点，并通过正运动学模型计算末端位置，建立跟踪误差模型，计算各位置点的追踪误差，并提取最大轨迹跟踪误差max(Em)；Em为轨迹跟踪误差，m为S4、Tn表示第n轨迹段的时间间隔，取Tn＝t,1≤n≤N，按照以上步骤进行规划轨迹，计算最大跟踪误差max(Em)，如果不满足精度要求max(Em)＜EmaxEmax为根据操作任务限定的最大跟踪误差，取N+1，再计算误差，依次循环计算，直到跟踪误差满足条件；时间间隔t与运动精度限制Emax根据具体任务要求确定；S5、当N确定之后，以各段时间间隔为设计变量，以各关节角速度、加速度及加加速度为约束条件，以跟踪误差最小为优化目标得到优化轨迹。2.根据权利要求1所述的一种考虑末端运动误差的六自由度机器人轨迹规划方法，其特征在于：步骤(1)基于旋量理论建立机器人正逆运动学模型正运动学模型已知该机器人在初始状态第i个关节所在位置矢量ri及旋转矢量ωi如下：r1=[000]r2=[0150250]r3=[0150800]r4=r5=r6=[0744940]---(1)]]>ω1=[001]ω2=[100]ω3=[100]ω4=[010]ω5=[100]ω6=[001]---(2)]]>根据旋量理论，关节间的转换矩阵表示为指数积形式，exp(ξ^iθi)=exp(ω^iθi)(I-exp(ω^iθi))(ωi×vi)+θωiωiTvi01---(3)]]>式中表示第i个关节旋量，θi为第i个关节角位移；可由ωi＝[ω1ω2ω3]定义为则νi是第i个关节运动的旋转线速度，νi＝-ωi×ri；则机器人正运动学模型gst(θ)表示如下：gst(θ)=exp(ξ^1θ1)exp(ξ^2θ2)...exp(ξ^6θ6)gst(0)---(4)]]>逆运动学模型本方法将各关节角的求解转化为三个Paden-Kahan子问题，由于该机器人末端的位置决定于关节1,2和3，而其位姿决定于关节4,5和6；首先将前三个关节的逆向运动描述为：末端位置向量re绕关节1旋转-θ1至re1，再绕关节2旋转-θ2至re2，然后绕关节3旋转-θ3至r5，则θ1，θ2和θ3分别通过以下三个表达式求出，其中式(5)属于子问题1，而式(6)和(7)属于子问题3；exp(ξ^1θ1)re1=re---(5)]]>||re2-exp(ξ^2θ2)re1||=δ2---(6)]]>||re-exp(ξ^3θ3)re2||=δ3---(7)]]>其中re1由末端位置向量re＝[xyz]决定；δ2,δ3为定距离，δ2＝||re1-r2||，δ3＝||re-r3||；其次θ4，θ5和θ6分别通过以下三个表达式求出...

【专利技术属性】
技术研发人员：刘志峰，许静静，赵永胜，蔡力钢，
申请(专利权)人：北京工业大学，
类型：发明
国别省市：北京;11