[发明专利]一种基于旋量理论的6R机器人逆运动学几何求解方法有效
申请号: | 201710953599.4 | 申请日: | 2017-10-13 |
公开(公告)号: | CN107756400B | 公开(公告)日: | 2020-12-04 |
发明(设计)人: | 刘志峰;许静静;赵永胜;蔡力钢 | 申请(专利权)人: | 北京工业大学 |
主分类号: | B25J9/16 | 分类号: | B25J9/16;B25J19/00;G06F17/11 |
代理公司: | 北京思海天达知识产权代理有限公司 11203 | 代理人: | 沈波 |
地址: | 100124 *** | 国省代码: | 北京;11 |
权利要求书: | 查看更多 | 说明书: | 查看更多 |
摘要: | |||
搜索关键词: | 一种 基于 理论 机器人 运动学 几何 求解 方法 | ||
1.一种基于旋量理论的6R机器人逆运动学几何求解方法,其特征在于:该方法的实现过程如下:
S1、建立基坐标系与工具坐标系,通过基坐标系与工具坐标系确定6R机器人运动学参数,并建立正运动学模型;
S2、将6R机器人的前三个关节的逆解运动分解描述:由点qe到点c2=[x2 y2 z2]绕关节1的旋转运动;点c2到点c1=[x1 y1 z1]绕关节2的旋转运动;点c1到点qs绕关节3的旋转运动;其中qs,qe分别为机器人前三个关节末端的初始和目标位置,根据几何关系描述建立以x1,y1,z1,x2,y2和z2为变量的六元二次方程组;
S3、采用MATLAB中“SOLVE”函数求解以上方程组,得到c1和c2的坐标矢量;
S4、自此,前三个关节的逆解运动始末位置已知,基于Paden-Kahan子问题1分别建立6R机器人的前三个关节角变量θ1,θ2和θ3的显式求解模型,并进行求解;
S5、在关节6的轴线上取一点作为后三个关节逆解运动中的初始位置qs1,并基于旋量正运动学模型求解初始位置qs1对应的目标位置qe1;
S6、将关节4和5的逆解运动分解描述:由点qe1到点c3=[x3 y3 z3]的旋转运动;由点c3到点qs1的旋转运动;根据几何关系建立以x3,y3和z3为变量的三元二次方程组,并采用“SOLVE”函数求解,得到c3的坐标矢量;
S7、自此,关节4和关节5的逆解运动始末位置已知,基于Paden-Kahan子问题1分别建立关节4和关节5的关节角变量θ4和θ5显式求解模型,并进行求解;
S8、取不在关节6轴线上的任意一点qs2,基于旋量正运动学模型求解qs2对应的目标位置qe2,则关节6的逆解运动始末位置已知;同理基于子问题1求解θ6。
2.根据权利要求1所述的一种基于旋量理论的6R机器人逆运动学几何求解方法,其特征在于:S1确定6R机器人运动学参数并建立正运动学模型
已知该6R机器人的初始状态各关节所在位置矢量及旋转矢量如下:
其中ri,1≤i≤6表示i关节在基坐标系的位置矢量,ωi表示i关节的旋转矢量;
基于旋量理论,该6R机器人正运动学模型表示为,
其中gst(0),gst(θ)分别表示机器人末端的初始位姿与目标位姿,表示i关节旋转运动的指数积形式,
式中θi为第i个关节角位移;是i关节旋转矢量ωi的另一种表示形式,由ωi=[ω1ω2 ω3]定义为则νi是i关节运动的旋转线速度,νi=-ωi×ri;
给定目标位姿,
S2针对前三个关节建立六元二次方程组
基于旋量理论前三个关节的旋转运动描述为,
式中q′s和q′e分别表示6R机器人末端在该旋转运动的初始位置与目标位置;由S1可知,
q′s=[xs ys zs 1]=[0 744 940 1]
q′e=[xe ye ze 1]=[-936.6611 631.7859 570.0752 1]
设运动经过点c1和c2,根据几何描述建立以下关系式组,
其中q1,q2和q3分别为关节1、关节2和关节3旋转轴线上的任意一点,为简化模型取q1=[0 0 0],q2=[0 150 250]和q3=[0 150 800],那么式(7)表示为如下方程组,
采用MATLAB中“SOLVE”函数求解方程组(8),得到x1,y1,z1,x2,y2和z2的解;
S3计算关节角位移θ1,θ2和θ3
得到过程点坐标c1=[x1 y1 z1]和c2=[x2 y2 z2]后,将绕关节1、关节2和关节3的旋转运动分别描述如下,
基于Paden-Kahan子问题1,得到关节角位移的显示表达式如下,
S4针对关节4和关节5建立三元二次方程组
由式(3)知,
其中在关节6的轴线上取一点qs1=[xs1 ys1 zs1]=[0 744 0],其绕关节4与关节5的旋转运动描述如下,
其中q′e1=g1q′s1=[xe1 ye1 ze1 1];
设运动经过点坐标为c3=[x3 y3 z3],根据几何描述建立以下关系式组,
其中q4=[0 744 940];则式(14)表示为如下方程,
采用MATLAB中“SOLVE”函数求解方程组(15),得到x3,y3和z3的解;
S5计算关节角位移θ4和θ5
得到过程点坐标c3=[x3 y3 z3]后,将绕关节4和关节5的旋转运动分别描述如下,
基于Paden-Kahan子问题1,得到关节角位移θ4和θ5的显示表达式如下,
S6计算关节角位移θ6
取不在关节6轴线上一点qs2=[0 750 940],其绕关节6的旋转运动描述为,
其中同理基于Paden-Kahan子问题1,得到关节角位移θ6的显示表达式如下,
通过以上求解得到八组运动学逆解。
该专利技术资料仅供研究查看技术是否侵权等信息,商用须获得专利权人授权。该专利全部权利属于北京工业大学,未经北京工业大学许可,擅自商用是侵权行为。如果您想购买此专利、获得商业授权和技术合作,请联系【客服】
本文链接:http://www.vipzhuanli.com/pat/books/201710953599.4/1.html,转载请声明来源钻瓜专利网。