[发明专利]自主移动机构法矢检测与调姿运动方法有效
申请号: | 201310423430.X | 申请日: | 2013-09-16 |
公开(公告)号: | CN103447877A | 公开(公告)日: | 2013-12-18 |
发明(设计)人: | 王珉;张得礼;鲍益东;王谢苗;丁力平;陈文亮;侯玉昭 | 申请(专利权)人: | 南京航空航天大学 |
主分类号: | B23Q9/00 | 分类号: | B23Q9/00 |
代理公司: | 南京天华专利代理有限责任公司 32218 | 代理人: | 瞿网兰 |
地址: | 210016 江*** | 国省代码: | 江苏;32 |
权利要求书: | 查看更多 | 说明书: | 查看更多 |
摘要: | |||
搜索关键词: | 自主 移动 机构 检测 运动 方法 | ||
1.一种自主移动机构法矢检测与调姿运动方法,其特征是它包括以下步骤:
首先,在自主移动机构末端执行器上安装有三个或四个激光测距传感器;
其次,利用三个或四个激光测距传感器所测的值求得所测制孔平面法矢n;
第三,根据上述测量计算所的制孔平面法矢n通过调姿运动反解算法,得到自主移动机构到达制孔法矢的各腿驱动量,用于控制系统实现调姿运动;
最后,根据上述测量计算所的制孔平面法矢n通过调姿运动反解算法,得到自主移动机构到达制孔法矢时,刀具的偏移量,用于控制系统实现刀具偏移纠正。
2.根据权利要求1所述的方法,其特征是所述的平面法矢n的确定方法包括利用三个激光测距传感器和利用四个激光测距传感器所测的值确定制孔位置的平面法矢n;
所述的利用三个激光测距传感器所测的值求得的平面法矢n:
n=(l,m,n)T=C1B1×B1A1 (1)
其中由矢量关系可得矢量C1B1如公式(2)所示,注意矢量的方向,
C1B1=C1C+CB+BB1
=(|C1C|-BB1|)·(0,0,1)T-|CB|·(0,1,0)T (2)
同理由矢量关系可得矢量B1A1如公式(3)所示,注意矢量的方向
B1A1=B1B+BA+AA1
=(|B1B|-|AA1|)·(0,0,1)T+|BA|·(1,0,0)T (3)
式(2)、(3)中参数为各个激光传感器的测量值,将式(2)(3)代入公式(1)即可求得所测平面法矢n;
所述的利用四个激光测距传感器所测的值求得的平面法矢n可再由公式(4)求解刀
具点的法向量,与公式(1)的结果取平均值作为所测平面法矢,提高了测量精度。
(l,m,n)T=D1C1×D1A1
whereD1C1=(|D1D|-|C1C|)·(0,0,1)T-|CD|·(1,0,0)T (4)
D1A1=(|D1D|-|A1A|)·(0,0,1)T-|AD|·(0,1,0)T
如果A、B、C、D中有一个偏差很大,说明该处脱离测量范围,则采用其余三点的数据计算法向:
①当A点超范围,则用B、C、D计算,如公式(5)
(l,m,n)T=C1B1×C1D1 (5)
②当B点超范围,则用A、C、D计算,如公式(4)
③当C点超范围,则用A、B、D计算,如公式(6)
(l,m,n)T=A1D1×A1B1 (6)
④当D点超范围,则用A、B、C计算,如公式(1)所示;
以上公式中,设自主移动机构动平台的法向量与制孔主轴始终平行,记n0=(0,0,1)T,激光测距传感器A、B、C、D,测距测量线的向量方向同制孔主轴平行,也为n0=(0,0,1)T,测距传感器ABCD平面平行于自主移动机构框身,且BA与外框架平行,记初始状态下BA方向为(1,0,0)T,BC与外框架垂直,记初始状态下BC方向为(0,1,0)T,|A1A|、|B1B|、|C1C|、|D1D|即为相应传感器测量距离;检测曲面A1B1C1D1区域近似看成一个平面,法矢检测即测曲面A1B1C1D1法向量,即平面A1B1C1的法向量;所测制孔法矢,也称为调姿目标法矢,记为n=(l,m,n)T。
3.根据权利要求1所述的方法,其特征是所述的调姿运动反解算法是指自主移动机构由n0=(0,0,1)T调姿到n=(l,m,n)T状态,反解八个腿升降的位移量和刀具点的偏移量;
自主移动机构调姿时只有3个自由度,其中旋转自由度仅有A、B角两个方向的转动,即为自主移动机构的俯仰与侧滚动作,自主移动机构腿2仅有一平行于外框方向的补偿量,故自主移动机构由n0=(0,0,1)T调姿到n=(l,m,n)T的过程,也可认为自主移动机构经B向摆角A向摆角到达的位姿;
根据坐标旋转变换运算,动坐标系Om与定坐标系Ob的坐标转换矩阵Rbm如式(7)所示;因为空间旋转矩阵为正交矩阵,故Rbm存在逆矩阵,且Rbm-1=RbmT。
其中
目标法矢n=(l,m,n)T已由法矢检测算法测出,因此由公式(8)可求出自主移动机构的调姿A、B摆角即可解出转换矩阵Rbm;
自主移动机构动坐标系相对于定坐标系的位置,记为
ObOm=(c,d,h1)T (9)
任一点D在定坐标系位置坐标矢量Db和在动坐标系的位置矢量Dm关系为:
Db=RbmDm+ObOm (10)
Dm=RbmTDb-RbmTObOm (11)
设点Si在坐标系Ob-xbybzb和Om-xmymzm的坐标分别为Sbi=(xsib,ysib,zsib)T和Smi=(xsim,ysim,zsim)T;
Sim=RbmTSib-RbmTObOm (12)
Sim-S1m=RbmT(Sib-S1b) (13)
Sim=RbmT(Sib-S1b)+S1m (14)
Sim0=Sib-ObOm=Sib-(c,d,h)T (15)
调姿时,固定足足1不升降,将S1b=(0,0,0)T;S1m=(-c,-d,-h1)T代入式(14),可得调姿前后各足偏移量为
ΔSim=Sim-Sim0=RbmTSib-Sib (16)
由式(16)解得调姿后各足的输入量与补偿量为
其中Δzpim为自主移动机构到达目标矢量n=(l,m,n)T时各足的驱动量,Δxpim、Δypim是各足在动平台x,y两个方向的补偿量;
调姿后,刀具点会产生偏移,为保证保证刀尖点不变,需要给出调姿后刀具点X、Y、Z调整量:
刀尖点T,由公式(11)可得
Tm=RbmTTb-RbmTObOm (18)S1m=RbmTS1b-RbmTObOm
因此可得
Tm-S1m=RbmT(Tb-S1b) (19)
Tm0=Tb-(c,d,h)T (20)
其中S1b={xs1b,ys1b,zs1b}T={0,0,0}T,S1m={xs1m,ys1m,zs1m}T={-c,-d,-h1}T
ΔTm=Tm-Tm0=RbmTTb-Tb (21)
自主移动机构刀具点T绝对坐标值记为Tb={xTb,yTb,zTb}T,由公式(21)求得刀具点在动坐标系的坐标值,由此可计算调姿后刀具点在X、Y、Z方向动坐标系下补偿量,如公式(22)所示.
该专利技术资料仅供研究查看技术是否侵权等信息,商用须获得专利权人授权。该专利全部权利属于南京航空航天大学,未经南京航空航天大学许可,擅自商用是侵权行为。如果您想购买此专利、获得商业授权和技术合作,请联系【客服】
本文链接:http://www.vipzhuanli.com/pat/books/201310423430.X/1.html,转载请声明来源钻瓜专利网。
- 上一篇:汽车轮辋及其旋压工艺
- 下一篇:W形预收尘装置