本发明涉及三自由度摇摆试验台的领域,具体而言,涉及一种三自由度并联机构的姿态角控制方法。
背景技术:
多自由度摇摆试验台包括并联机构和串联机构,并联机构具有刚度大、承载能力大、积累误差大、动态特性好、结构紧凑等特点,近年来广泛应用于模拟战机、机床、机器人等方面。并联机构各零件结构复杂,运动关系复杂,导致并联机构的姿态控制困难,难以满足预期的运动需求。
技术实现要素:
有鉴于此,本发明实施例的目的在于提供一种三自由度并联机构的姿态角控制方法,以改善现有技术中并联机构的姿态控制困难且精度低的问题。
本发明较佳实施例提供了:
三自由度并联机构的姿态角控制方法,所述并联机构包括三组作动器、三组支撑杆和动平台,三组所述作动器的一端分别与所述动平台铰接,三组所述作动器的另一端分别与地基或者所述支撑杆铰接;三组所述支撑杆的一端分别与地基铰接,三组所述作动器的另一端分别与所述动平台或者所述作动器铰接;
控制方法包括:
以动平台上铰接点所在圆的中心处建立体坐标系ob-xbybzb,并在大地上建立固定的参考坐标系op-xpypzp,并联机构处于工作初始位置时体坐标系与参考坐标系重合;
应用体坐标系相对与参考坐标系的广义坐标来描述所述动平台的姿态,所述广义坐标包括三个欧拉角,三个所述欧拉角为横摇角、纵摇角和偏航角;获取体坐标系相对与参考坐标系的旋转矩阵;并获取所述动平台在体坐标系中角速度、角加速度与欧拉角导数之间的关系;
根据期望的姿态角,获得所述动平台的平动位移;
依据所述动平台的姿态角和平动位移,运用运动学反解方法,获得并联结构中各个所述作动器的运动规律;
三个所述作动器依据获得的运动规律进行运动。
进一步的,所述并联机构处于工作初始位置时:
所述作动器、所述支撑杆与所述动平台的上铰接点在同一平面内,且位于一个圆上;
三个所述支撑杆的长度相同,且与所述地基的下铰接点位于一个圆上;所述下铰接点所在圆的圆心是动平台中心在地基上的垂直投影;
三个所述作动器之一与所述地基平行,其余所述作动器与所述地基垂直。
进一步的,采用zyx型欧拉角来描述所述并联机构的姿态;
体坐标系相对于参考坐标系的广义坐标表示为:q=[q1,q2,q3,q4,q5,q6]t;q1、q2和q3依次表示为横摇角、纵摇角和偏航角,q4、q5和q6为体坐标系原点o1在参考坐标系中的坐标分量。
进一步的,获取体坐标系相对与参考坐标系的旋转矩阵,具体包括:
首先,绕oz轴旋转偏航角q3,相应地ox转至ox′,oy转至oy′,此旋转对应的变换矩阵为r3,其中r3表示为:
其次,绕oy′轴旋转纵摇角q2,相应地oz转至oz′,ox′转至ox1,对应的变换矩阵为r2,其中r2表示为:
最后,绕ox1轴旋转横摇角q1,oy′转至oy1,oz′转至oz1,获得体坐标系o-x1y1z1,相应的变换矩阵为r1,其中r1表示为:
式中,sqi表示为sin(qi),i=1,2,3;cqi表示为cos(qi),i=1,2,3;
则体坐标系相对于参考坐标系的旋转矩阵为:
根据权利要求3所述的三自由度并联机构的姿态角控制方法,其特征在于,获取所述动平台在体坐标系中角速度、角加速度与欧拉角导数之间的关系,具体包括:
令x1、y′和z分别为ox1、oy′和oz轴的单位矢量,根据角速度合成定理,获得体坐标系中所述动平台的角速度ωb为:
其中,y′=cq1y1-sq1z1;
z=-sq2x1+cq2z'=-sq2x1+cq2sq1y1+cq2cq1z1;
进行坐标变换,获得体坐标系中所述动平台的角速度为:
设
对体坐标系中所述动平台的角速度进行求导,获得体坐标系中所述动平台的角加速度εb,其中,εb表示为:
式中,
进一步的,根据期望的姿态角,获得所述动平台的平动位移,具体包括:
将体坐标系相对参考坐标系的位移矢量表示为[q4q5q6]t,且欧拉角表示为[q1q2q3]t;将所述动平台的上铰接点在体坐标系中的坐标表示为ai=[axiayiazi]t,并将其下铰接点在参考坐标系中的坐标表示为bi=[bxibyibzi]t,其中i=1~6,则上铰接点在参考坐标系中的矢量坐标表示为:
gi=rai+c
式中,gi表示为上铰接点在参考坐标系中的矢量坐标,gi=[gxigyigzi]t,i=1,2,…,6;
c表示为体坐标系原点在参考坐标系中的矢量,c=[q4q5q6]t;
进而,所述支撑杆的长度约束方程为:
式中,l4、l5、l6分别表示三个所述支撑杆的长度;
基于所述支撑杆的长度约束方程,求解q4、q5、q6,进而获得所述动平台的平动位移;
根据所述动平台的姿态角和平动位移,获得所述作动器的矢量为:
li=rai-bi+c(i=1,2,3);
进一步,所述支撑杆的矢量为:
li=rai-bi+c(i=4,5,6)。
进一步的,依据所述动平台的姿态角和平动位移,运用运动学反解方法,获得并联结构中各个所述作动器的运动规律,具体包括:
根据空间矢量关系,依据所述作动器的矢量与所述支撑杆的矢量,获得各个上铰接点的速度;并将上铰接点速度在所述作动器与所述支撑杆的方向上投影,获得所述作动器与所述支撑杆的伸缩速度;
所述作动器包括活塞杆和缸筒,依据所述作动器或所述支撑杆中上铰接点的角速度、矢量以及加速度的关系,获得所述作动器的缸筒质心的加速度与活塞杆质心的加速度。
本发明的有益效果是:通过建立体坐标系与参考坐标,并采用欧拉角来描述姿态,进而获得动平台的平动位移;最后依据动平台与平动位移,运用运动学反解的方法,获得作动器的运动规律。能够根据期望的姿态角,准确的控制作动器的运动,具有精度高、实时性好的优点。
附图说明
为了更清楚地说明本发明实施例的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,应当理解,以下附图仅示出了本发明的某些实施例,因此不应被看作是对范围的限定,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他相关的附图。
图1为本发明建立的体坐标系与参考坐标系处于工作初始位置时的结构示意图。
图2为本发明中并联机构的姿态角定义示意图。
图3是作动器的结构示意图。
具体实施方式
实施例中,并联机构包括三组作动器、三组支撑杆和动平台,三组作动器的一端分别与动平台铰接,三组作动器的另一端分别与地基或者支撑杆铰接;三组支撑杆的一端分别与地基铰接,三组作动器的另一端分别与动平台或者作动器铰接;
并联机构处于工作初始位置时:作动器、支撑杆与动平台的上铰接点在同一平面内,且位于一个圆上;三个支撑杆的长度相同,且与地基的下铰接点位于一个圆上;下铰接点所在圆的圆心是动平台中心在地基上的垂直投影;三个作动器之一与地基平行,其余作动器与地基垂直。
实施例中,三自由度并联机构的结构参数如表1
表1三自由度并联机构的结构参数
实例中,姿态角控制方法包括:
参见图1所示,以动平台上铰接点所在圆的中心处建立体坐标系ob-xbybzb,并在大地上建立固定的参考坐标系op-xpypzp,并联机构处于工作初始位置时体坐标系与参考坐标系重合;
应用体坐标系相对与参考坐标系的广义坐标来描述动平台的姿态,广义坐标包括三个欧拉角,三个欧拉角为横摇角、纵摇角和偏航角;获取体坐标系相对与参考坐标系的旋转矩阵;并获取动平台在体坐标系中角速度、角加速度与欧拉角导数之间的关系;
根据期望的姿态角,获得动平台的平动位移;
依据动平台的姿态角和平动位移,运用运动学反解方法,获得并联结构中各个作动器的运动规律;
三个作动器依据获得的运动规律进行运动。
通过建立体坐标系与参考坐标,并采用欧拉角来描述姿态,进而获得动平台的平动位移;最后依据动平台与平动位移,运用运动学反解的方法,获得作动器的运动规律。能够根据期望的姿态角,准确的控制作动器的运动,具有精度高、实时性好的优点。
实施例中,并联机构是作动器、支撑杆和动平台等一系列构件用运动副连接而成的空间闭环结构。可以利用kutzbach-glübler公式求解自由度:
式中,f表示机构的自由度数目;n表示为机构构件的总数;m表示为机构中运动副的总数;fi第i个运动副的自由度数;λ表示为机构的局部自由度数目。
实施例中,并联机构具有三个独立自由度,是典型的少自由度并联机构。
实施例中,采用zyx型欧拉角来描述并联机构的姿态;
体坐标系相对于参考坐标系的广义坐标表示为:q=[q1,q2,q3,q4,q5,q6]t;q1、q2和q3依次表示为横摇角、纵摇角和偏航角,q4、q5和q6为体坐标系原点o1在参考坐标系中的坐标分量。
实施例中,参见图2所示,获取体坐标系相对与参考坐标系的旋转矩阵,具体包括:
首先,绕oz轴旋转偏航角q3,相应地ox转至ox′,oy转至oy′,此旋转对应的变换矩阵为r3,其中r3表示为:
其次,绕oy′轴旋转纵摇角q2,相应地oz转至oz′,ox′转至ox1,对应的变换矩阵为r2,其中r2表示为:
最后,绕ox1轴旋转横摇角q1,oy′转至oy1,oz′转至oz1,获得体坐标系o-x1y1z1,相应的变换矩阵为r1,其中r1表示为:
式中,sqi表示为sin(qi),i=1,2,3;cqi表示为cos(qi),i=1,2,3;
则体坐标系相对于参考坐标系的旋转矩阵为:
实施例中,获取动平台在体坐标系中角速度、角加速度与欧拉角导数之间的关系,具体包括:
令x1、y′和z分别为ox1、oy′和oz轴的单位矢量,根据角速度合成定理,获得体坐标系中动平台的角速度ωb为:
其中,y′=cq1y1-sq1z1;
z=-sq2x1+cq2z'=-sq2x1+cq2sq1y1+cq2cq1z1;
进行坐标变换,获得体坐标系中动平台的角速度为:
设
对体坐标系中动平台的角速度进行求导,获得体坐标系中动平台的角加速度εb,其中,εb表示为:
式中,
实施例中,根据期望的姿态角,获得动平台的平动位移,具体包括:
将体坐标系相对参考坐标系的位移矢量表示为[q4q5q6]t,且欧拉角表示为[q1q2q3]t;将动平台的上铰接点在体坐标系中的坐标表示为ai=[axiayiazi]t,并将其下铰接点在参考坐标系中的坐标表示为bi=[bxibyibzi]t,其中i=1~6,则上铰接点在参考坐标系中的矢量坐标表示为:
gi=rai+c
式中,gi表示为上铰接点在参考坐标系中的矢量坐标,gi=[gxigyigzi]t,i=1,2,…,6;
c表示为体坐标系原点在参考坐标系中的矢量,c=[q4q5q6]t;
进而,支撑杆的长度约束方程为:
式中,l4、l5、l6分别表示三个支撑杆的长度;
实施例中,基于支撑杆的长度约束方程,求解q4、q5、q6,具体如下:
令si=rai-bi=[sxisyiszi]t,那么支撑杆的长度约束方程进一步表示为:
基于此方程获得如下方程:
式中,
将方程(2)中q6假定为待定未知数,那么:
将q4和q5用q6表示出来:
再将q4和q5代入方程(1)中,则该式可变形为标准的一元二次方程的形式,表示为:
c1q62+c2q6+c3=0(4)。
求解方程(4),得到q6的两个根,两个根中的一个根表示动平台处在地基之上,另一个根表示动平台处在地基之下。根据三自由度并联机构的实际装配情况可以舍掉其中的一个,再将q6代入方程(3)即可得到q4和q5,这样就解出了动平台的平动位移。
实施例中,根据动平台的姿态角和平动位移,获得作动器的矢量为:
li=rai-bi+c(i=1,2,3);
进一步,支撑杆的矢量为:
li=rai-bi+c(i=4,5,6)。
实施例中,依据动平台的姿态角和平动位移,运用运动学反解方法,获得并联结构中各个作动器的运动规律,具体包括:
根据空间矢量关系,依据作动器的矢量与支撑杆的矢量,获得各个上铰接点的速度;并将上铰接点速度在作动器与支撑杆的方向上投影,获得作动器与支撑杆的伸缩速度;
参见图3所示,作动器包括活塞杆和缸筒,依据作动器或支撑杆中上铰接点的角速度、矢量以及加速度的关系,获得作动器的缸筒质心的加速度与活塞杆质心的加速度。
具体的,根据作动器与支撑杆的矢量,则上铰接点的速度表示为:
式中,ω表示为动平台在参考坐标系中的角速度;
式(5)以矩阵型式表示为:
式中,
具体的,将上铰接点速度在作动器与支撑杆的方向上投影,获得作动器与支撑杆的伸缩速度,表示为:
式中,
将式(4)代入式(7),获得:
基于并联机构有三个作动器和三个支撑杆,将式(8)以矩阵的形式表示:
式中,
进一步的,设作动器或支撑杆绕下铰接点转动的角速度为ωai,则有
对式(8)的两边交叉乘lni,并基于lni·ωai=0,则
式中,jωai,ai表示为上铰接点速度到作动器或支撑杆转动角速度的雅克比矩阵。
进一步,令作动器缸筒质心为dc,该点的速度vdci为:
式中,rbi表示为缸筒质心到下铰点的距离(m);
将式(6)代入式(10)得到:
对式(10)微积分,得到作动器缸筒质心的加速度,表示为:
式中,
进一步的,令作动器活塞杆的质心为uc,该点的速度vuci为
式中,rai表示为缸筒质心到下铰点的距离(m);juci,ai表示为上铰点速度到活塞杆质心速度的雅克比矩阵。
将式(6)代入式(11)得到
对式(11)微积分,得到作动器活塞杆质心的加速度,表示为:
以上仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。