stewart运动学分析

Stewart型并联支撑机构运动学公式推导一、构型分析及坐标系建立静基座自动调平系统Stewart平台型并联支撑机构为双三角形机构，由一个活动上平台和一个固定的下平台所组成。

上平台铰链点和基座平台铰链点的分布形式相同，但铰接点相互交错，六根支链分别用移动副和两个球铰链与上下平台连接。

并联机构示意图如图1所示。

图1 Stewart并联机构示意图支链与动平台铰接点为A1,A2,A3,支链与基座铰接点标记为B1，B2,B3。

坐标系选在平台的三角几何中心，由右手螺旋法则确定。

动平台三角边长为a，定平台三角边长为b，动平台起始高度为h。

根据设定的初始值，各支链与定平台、动平台铰接点的坐标如表一所示。

二、并联支撑机构正反解两个坐标系，o 和o ′，其中，o 为固定坐标系。

（1）将坐标系o 绕自身的x 轴旋转γ；（2）将旋转后的坐标系绕固定坐标系的y 轴旋转β； （3）将第二步的坐标系绕固定坐标系的z 轴旋转α； 旋转矩阵分别为R x =[1000cγ−sγ0sγcγ]R y =[cβ0sβ010−sβ0cβ] R z =[cα−sα0sαcα0001]按上述方式得到的总旋转变换矩阵为：R o ′o=R z R y R x =[cαcβcαsβsγ−sαcγcαsβcγsαcβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ]设动平台的平移参数为（d x ,d y ,d z ）,则坐标的齐次变换矩阵为：T o ′o=[cαcβcαsβsγ−sαcγcαsβcγ+sαsγ0sαcβsαsβsγ+cαcγsαsβcγ−cαsγ0−sβcβsγcβcγ001] 对于与动平台铰接的各点A i (i=1,2,3),点的齐次坐标为p A i ,经过变换后的点对应标记为A i ′,变换后的齐次坐标为p A i′,则， p A i′=T o ′op A i 带入初始坐标后，得出变换后与动平台铰接的各点坐标值为： [A 1x′A 1y ′A 1z ′]=[ −√36acαcβ+12a (sαcγ−cαsβsγ)+ℎ(cαsβcγ+sαsγ)+d x −√36asαcβ−12a (sαsβsγ+cαcγ)+ℎ(sαsβcγ−cαsγ)+d y √36asβ−12acβsγ+ℎcβcγ+d z ][A 2x′A 2y ′A 2z ′]=[ √33acαcβ+ℎ(cαsβcγ+sαsγ)+d x √33asαcβ+ℎ(sαsβcγ−cαsγ)+d y −√33asβ+ℎcβcγ+d z ][A 3x′A 3y ′A 3z ′]=[ −√36acαcβ+12a (cαsβsγ−sαcγ)+ℎ(cαsβcγ+sαsγ)+d x −√36asαcβ+12a (sαsβsγ+cαcγ)+ℎ(sαsβcγ−cαsγ)+d y √36asβ+12acβsγ+ℎcβcγ+d z ]设六个驱动器的伸展长度为∆l i (i=1--6),则与之相应的六个方程式表示为：∆l 1=A 1′B 1⃗⃗⃗⃗⃗⃗⃗⃗⃗ −A 1B 1⃗⃗⃗⃗⃗⃗⃗⃗⃗ =√(A 1x ′−B 1x )2+(A 1y ′−B 1y )2+(A 1z ′−B 1z )2−l 1 ∆l 2=A 2′B 1⃗⃗⃗⃗⃗⃗⃗⃗⃗ −A 2B 1⃗⃗⃗⃗⃗⃗⃗⃗⃗ =√(A 2x ′−B 1x )2+(A 2y ′−B 1y )2+(A 2z ′−B 1z )2−l 2 ∆l 3=A 2′B 2⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗ −A 2B 2⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗ =√(A 2x ′−B 2x )2+(A 2y ′−B 2y )2+(A 2z ′−B 2z )2−l 3 ∆l 4=A 3′B 2⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗ −A 3B 2⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗ =√(A 3x ′−B 2x )2+(A 3y ′−B 2y )2+(A 3z ′−B 2z )2−l 4 ∆l 5=A 3′B 3⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗ −A 3B 3⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗ =√(A 3x ′−B 3x )2+(A 3y ′−B 3y )2+(A 3z ′−B 3z )2−l 5 ∆l 6=A 1′B 3⃗⃗⃗⃗⃗⃗⃗⃗⃗ −A 1B 3⃗⃗⃗⃗⃗⃗⃗⃗⃗ =√(A 1x ′−B 3x )2+(A 1y ′−B 3y )2+(A 1z ′−B 3z )2−l 6由α、β、γ、d x 、d y 、d z 经过上式推导得出∆l i 的过程，称为Stewart 平台的反解过程。

三、并联支撑机构速度/加速度分析设S i 为沿驱动器i 的单位矢量，l i 为驱动器i 的长度，r o ′B 运动平台质心o ′到B i 点的位置矢量。

ωo ′和v o ′分别是运动平台在惯性参考系中的角速度和线速度矢量，则运动平台上B i 点处的速度矢量为：v B i =ωo ′×r o ′B +v o ′矩阵形式为：v B i =[−r̂o ′B I][ωo ′v o′]式中^表示矢量的反对称矩阵。

对一个矢量x ，有x ̂=[0−x 3x 2x 30−x 1−x 2x 10] 通过将运动平台上B i 点处的速度矢量v B i 向驱动器方向投影（即用单位矢量S i 点乘B i 点的速度矢量v B i ），可以得到驱动器i 的上下两部分沿驱动器方向的相对移动速度：l i=S i∙v Bi=S i∙(ωo′×r o′B+v o′)=(r o′B×S i)∙ωo′+S i∙v o′将上式写为矩阵形式为：l i=[（ro′B ×S i）T S i T][ωo′v o′]， i=1,2,…,6用一个广义速度矢量V来表示运动平台的角速度和线速度，即末端直角坐标速度：V=[ωo′v o′]T=[ωxo′ωyo′ωzo′v xo′v yo′v zo′]T 用六维矢量q̇来表示六个驱动器的上下两部分沿驱动器方向的相对移动速度，即关节速度。

联立成统一矩阵形式为：q̇=[l1⋮l6]=[（r o′B1×S1）T S1T⋮⋮（r o′B6×S6）T S6T][ωo′v o′]=J∗V式中6×6维矩阵J∗称为末端直角坐标速度对关节速度的影响矩阵。

上式给出了Stewart平台直角坐标速度对关节速度的变换关系。

当给定某一时刻平台的位移参数以及在这个时刻平台的角速度及线速度，则可利用该式求得六个驱动器的运动速度，即Stewart平台机构的速度反解方程。

相反，机构的速度正解就是根据某一时刻的位移参数以及6个驱动器的运动速度求解该时刻平台的角速度及线速度，有：V=(J∗)−1q̇=Jq̇矩阵J就是Stewart平台的雅克比矩阵。

运动平台B i处的加速度可通过平台质心的角加速度εo′和线加速度δo′得到：δBi =εo′×r o′Bi+δo′+ωo′×(ωo′×r o′Bi)对l i求导，得出：l i=S i∙δBi +(v o′Bi∙v o′Bi−l i2)/l i上式中，δBi为运动平台上B i点处的加速度，将此项中的部分项展开，有：v o′Bi ∙v o′Bi=[ωo′T v o′T][−r̂o′BiI][−r̂o′BiI][ωo′v o′] =V T[−r̂o′Bir̂o′Bir̂o′Bi−r̂o′BiI]Vl i2=[ωo′T v o′T][r̂o′BiS iS i][−S i T r̂o′BiI][ωo′v o′]=V T[−r ̂o ′B i S i S i Tr ̂o ′B i r ̂o ′B i S i S i T −S i S i T r̂o ′B i S i S iT]V将上述两式相减，有：v o ′B i ∙v o ′B i −l i 2=V T [r ̂o ′B i S i S i T r ̂o ′B i −r ̂o ′B i S i S i T r̂o ′B i S i S i T −S i S iT]V用单位矢量S i 点乘方程右边最后一项，有：S i ∙[ωo ′×(ωo ′×r o ′B i )]=ωo ′T r ̂o ′B i S ̂i ωo ′=V T [r ̂o ′B i S ̂i 000]V化简得到驱动器l i 的上下两部分沿驱动器方向的相对移动加速度，即关节加速度为：l i=J i V +V T H i V 式中，J i 为雅克比矩阵的第i 行。

H i =1l i [r̂o ′B i S i S i T r ̂o ′B i −r ̂o ′B i S i S i T r̂o ′B iS i S i T −S i S iT]Stewart 平台的加速度反解方程可写为：q̈=[l 1⋮l 6]=JV +V T HV矩阵H 称为加速度影响矩阵。

Stewart 加速度正解方程：V =J −1q̈+q̇T H ∗q̇式中，H ∗=−J T (J⨂H)J ，符号⨂表示矩阵的广义标量积，相当于把前面的矩阵看成一个常数，运算时用后面矩阵的每一个元素乘以前面的矩阵。

例如，对于矩阵A 和B ，有：A⨂B=[Ab11⋯Ab1n ⋮⋮Ab n1⋯Ab nn]。