多自由度振动台控制核心为多作动器位置向振动台面的空间6自由度的转换关系,一旦根据作动器的位移能够相对准确地计算出自由度位移,便可进行自由度下的PID控制,但该过程是否困难涉及到过约束(超静定)系统的求解。
反过来却是唯一且高效的,即6自由度确定的情况下,作动器的前端球铰坐标就完全确定下来了(欧拉旋转变换,需确定先后顺序),根据前后端坐标即可计算出作动器预期的变换量,再对比实际的作动器位移变化量即可确定出作动器偏差,进一步合成为自由度下的误差,多次迭代即可得到准确的自由度位移。
最终振动台的自由度位移整体计算过程如下(未体现迭代部分):
该模块为多自由度振动台系统(主要针对作动器数量水平不超过4套,垂直向不超过4套6自由度系统)的核心控制部分,模块分2部分:
- 根据自由度位移命令与各作动器实际位移,计算出各作动器的当前误差,并最终合成自由度的误差;
- 根据各作动器的压差,合成出系统自由度下的压差。
变换框图如下图所示:
其中,标号1是模态位移命令到作动器长度计算;标号1是作动器位移误差到模态位移误差矩阵;标号3是作动器压差到模态压差矩阵。
当描述作动器前端 P1 移动到 P2时,如下图所示:
⎣⎢⎡x2y2z2⎦⎥⎤=⎣⎢⎡x1y1z1⎦⎥⎤+⎣⎢⎡ΔxΔyΔz⎦⎥⎤
⎣⎢⎡x2y2z2⎦⎥⎤=⎣⎢⎡1000cosαsinα0−sinαcosα⎦⎥⎤⎣⎢⎡x1y1z1⎦⎥⎤
⎣⎢⎡x2y2z2⎦⎥⎤=⎣⎢⎡cos(β)0−sin(β)010sin(β)0cos(β)⎦⎥⎤⎣⎢⎡x1y1z1⎦⎥⎤
⎣⎢⎡x2y2z2⎦⎥⎤=⎣⎢⎡cosγsinγ0−sinγcosγ0001⎦⎥⎤⎣⎢⎡x1y1z1⎦⎥⎤
综合后(先旋转,后平移,具体旋转顺序及平移与旋转的先后顺序统一即可,实际有差异,但统一后逆矩阵计算后会对应即可,不影响控制)
⎣⎢⎡x2y2z2⎦⎥⎤=⎣⎢⎡1000cosαsinα0−sinαcosα⎦⎥⎤⎣⎢⎡cosβ0−sinβ010sinβ0cosβ⎦⎥⎤⎣⎢⎡cosγsinγ0−sinγcosγ0001⎦⎥⎤⎣⎢⎡x1y1z1⎦⎥⎤+⎣⎢⎡ΔxΔyΔz⎦⎥⎤ =⎣⎢⎡cosβcosγcosβsinγ−sinβsinαsinβcosγ−cosαsinγsinαsinβsinγ+cosαcosγsinαcosβcosαsinβcosγ+sinαsinγcosαsinβsinγ−sinαcosγcosαcosβ⎦⎥⎤⎣⎢⎡x1y1z1⎦⎥⎤+⎣⎢⎡ΔxΔyΔz⎦⎥⎤
位移计算:动作后作动器长度-初始长度,
Δxn=Ln−Ln0=∣P2−P0∣−Ln0=∣P2−P1+P1−P0∣−Ln0
理论上通过∣P2−P0∣−Ln0来计算更直接,但Servotest使用的后面的计算方法(∣P2−P1+P1−P0∣−Ln0),原因未知,具体实现P1−P0 提前计算直接写入到下位机。P2−P1按下面的方法计算:
⎣⎢⎡x2−x1y2−y1z2−y1⎦⎥⎤=⎣⎢⎡cosβcosγcosβsinγ−sinβsinαsinβcosγ−cosαsinγsinαsinβsinγ+cosαcosγsinαcosβcosαsinβcosγ+sinαsinγcosαsinβsinγ−sinαcosγcosαcosβ⎦⎥⎤⎣⎢⎡x1y1z1⎦⎥⎤−⎣⎢⎡x1y1z1⎦⎥⎤+⎣⎢⎡ΔxΔyΔz⎦⎥⎤ =⎣⎢⎡cosβcosγ−1cosβsinγ−sinβsinαsinβcosγ−cosαsinγsinαsinβsinγ+cosαcosγ−1sinαcosβcosαsinβcosγ+sinαsinγcosαsinβsinγ−sinαcosγcosαcosβ−1⎦⎥⎤⎣⎢⎡x1y1z1⎦⎥⎤+⎣⎢⎡ΔxΔyΔz⎦⎥⎤
实际过程需要考虑x1、x2、y1、y2、z1、z2、z3、z4 的前端坐标Pt相对Motion Centre of TablePc的偏移,即默认旋转中心在原点(0,0,0)处;
⎣⎢⎡x2−x1y2−y1z2−y1⎦⎥⎤=⎣⎢⎡cos(w)cos(p)−1.0sin(w)cos(p)−sin(p)−sin(w)cos(r)+cos(w)sin(p)sin(r)cos(w)cos(r)+sin(w)sin(p)sin(r)−1.0cos(p)sin(r)sin(w)sin(r)+cos(w)sin(p)cos(r)−cos(w)sin(r)+sin(w)sin(p)cos(r)cos(p)cos(r)−1.0⎦⎥⎤⎣⎢⎡xt−xcyt−yczt−zc⎦⎥⎤+⎣⎢⎡ΔxΔyΔz⎦⎥⎤
同时需要X、Y、Z、roll(r)、pitch(p)、yaw(w)指令从 DSP 单位转换为工程单位涉及到Scale的转换,平动保持为mm单位,转动换算为Rad单位;
其中,xt、yt、zt是TABLE Coordinates中坐标,xc、yc、zc是坐标originX、originY、originZ旋转中心相对于原点的偏移量。
计算出P2−P1后再结合已经预设的P1−P0,矢量相加即得到 P2−P0,取模长即可得到长度值Ln,减去初始长度 Ln0即可得到预期的作动器长度变化量ΔLn,再与作动器传感器实际采集长度Ln作差,即可得到作动器位移的估计误差Ln
计算得到每个作动器的误差后,向自由度误差转换时,需要一个ModalErro计算矩阵。
早期针对正交形式的振动台时可以通过表格手工计算,主要是旋转方向的计算:
后续针对更多的非正交比如六杆并联、八杆并联以及水平向45°风车布置的振动台系统等,更多采用一种小数值驱动求响应的方式获取:
每个自由度方向给定一定的驱动位移量(10mm或10mRad),得到各个作动器响应的赋值,归一化后进行求矩阵的逆即可得到该矩阵。
以某振动为例
| 驱动方向 |
驱动量 |
单位 |
| dX |
10 |
mm |
| dY |
10 |
mm |
| dZ |
10 |
mm |
| dTx |
10 |
mRad |
| dTy |
10 |
mRad |
| dTz |
10 |
mRad |
| dShear |
10 |
mm/m |
| dTwist |
10 |
mm/m2 |
得到的正向响应矩阵为
|
FL1 |
FL2 |
FR1 |
FR2 |
RL1 |
RL2 |
RR1 |
RR2 |
| X |
-0.5 |
0.5 |
-0.5 |
0.5 |
0.5 |
-0.5 |
0.5 |
-0.5 |
| Y |
0.5 |
-0.5 |
-0.5 |
0.5 |
0.5 |
-0.5 |
-0.5 |
0.5 |
| Z |
0.707106781 |
0.707106781 |
0.707106781 |
0.707106781 |
0.707106781 |
0.707106781 |
0.707106781 |
0.707106781 |
| tx |
1.009666964 |
0.758043123 |
-1.009666964 |
-0.758043123 |
1.009666964 |
0.758043123 |
-1.009666964 |
-0.758043123 |
| ty |
-0.758043123 |
-1.009666964 |
-0.758043123 |
-1.009666964 |
0.758043123 |
1.009666964 |
0.758043123 |
1.009666964 |
| tz |
1.249971191 |
-1.249971191 |
-1.249971191 |
1.249971191 |
-1.249971191 |
1.249971191 |
1.249971191 |
-1.249971191 |
| stretch |
-0.994138566 |
0.994138566 |
-0.994138566 |
0.994138566 |
-0.994138566 |
0.994138566 |
-0.994138566 |
0.994138566 |
| twist |
0.702962122 |
0.702962122 |
-0.702962122 |
-0.702962122 |
-0.702962122 |
-0.702962122 |
0.702962122 |
0.702962122 |
再进一步求逆矩阵
| Inverse |
X |
Y |
Z |
tx |
ty |
tz |
stretch |
twist |
| FL1 |
-0.28558613 |
0.21441387 |
0.176776695 |
0.141425906 |
-0.141425906 |
0.100002305 |
-0.125736999 |
0.177818969 |
| FL2 |
0.21441387 |
-0.28558613 |
0.176776695 |
0.141425906 |
-0.141425906 |
-0.100002305 |
0.125736999 |
0.177818969 |
| FR1 |
-0.28558613 |
-0.21441387 |
0.176776695 |
-0.141425906 |
-0.141425906 |
-0.100002305 |
-0.125736999 |
-0.177818969 |
| FR2 |
0.21441387 |
0.28558613 |
0.176776695 |
-0.141425906 |
-0.141425906 |
0.100002305 |
0.125736999 |
-0.177818969 |
| RL1 |
0.28558613 |
0.21441387 |
0.176776695 |
0.141425906 |
0.141425906 |
-0.100002305 |
-0.125736999 |
-0.177818969 |
| RL2 |
-0.21441387 |
-0.28558613 |
0.176776695 |
0.141425906 |
0.141425906 |
0.100002305 |
0.125736999 |
-0.177818969 |
| RR1 |
0.28558613 |
-0.21441387 |
0.176776695 |
-0.141425906 |
0.141425906 |
0.100002305 |
-0.125736999 |
0.177818969 |
| RR2 |
-0.21441387 |
0.28558613 |
0.176776695 |
-0.141425906 |
0.141425906 |
-0.100002305 |
0.125736999 |
0.177818969 |
压差计算通常采用ModalError矩阵的同一矩阵,但实际上计算方法有问题,应该采用响应矩阵,即Drive矩阵,
以下图两自由度系统为例,两套作动器A1 A2 两端布置,分别距离控制中心1m 和2m。
Z向每运动1mm,则每个作动器需要动作[1, 1],Pitch向每运动1mRad,则每个作动器需要动作[-1, 2],即得到下方的响应矩阵(Scale不考虑的情况下):
[A1A2]=[11−12][ZP](响应矩阵)
对上述矩阵求逆得到误差估算矩阵,小幅值范围内也可以作为作动器位移向自由度位移的转换矩阵(自由度位移合成矩阵):
[ZP]=[0.667−0.3330.3330.333][A1A2](误差矩阵)
而对于压差矩阵选用到底是用响应矩阵(驱动矩阵)的转置进行计算,即下方公式,
[ZFPF]=[1−112][A1FA2F](压差合成矩阵 1)
还是误差矩阵,即下方公式:
[ZFPF]=[0.667−0.3330.3330.333][A1FA2F](压差合成矩阵 2)
结论:采用压差合成矩阵1更合理。
从力的合成关系看,直观上看左右各出1kN的力,Z向受压差力应该为2kN,同时受转动 1kN·m的Roll向转矩,该计算对于平动方向不需要调整Scale及Xterm,更直观,对于转动方向需要调整因力臂关系带来的Scale或Xterm设置。
对于早期的正交系统振动台TFM里的压差计算矩阵都存在问题,但由于之前的项目台面多为方台或圆台,布置都较为方正对称,正矩阵和逆矩阵只差系数,计算后通常需要乘上系数再进行压差辅助反馈因此影响不大,而对于六杆并联系统实际上是单独计算,因此未受影响。
注:误差矩阵(自由度位移合成矩阵)的解读,A1 A2 平动时,A1的权重更大,而转动时权重则相同(方向不同),与直观认知略有差异,对于左右同时运动1mm的情况,权重相同与权重不同是体现不出差异的,而对于左右运动不一致时即
[A1A2]=[1 mm2 mm]和[A1A2]=[2 mm1 mm]
的差异则比较明显.
根据几何关系直观计算得到两种情况的自由度位移分别是
[Z1P1]=[1.667 0.333 mmmRad]和[Z2P2]=[1.333 0.333 mmmRad]
该结果与矩阵的合成计算一致:
[Z1P1]=[0.667−0.3330.3330.333][21]=[1.667 0.333 mmmRad](Case1)
[Z2P2]=[0.667−0.3330.3330.333][12]=[1.333 0.333 mmmRad](Case2)
Pulsar关于Transform8s的数据库如下图所示:
大部分的初始参数在tfm文件中体现,动态变换的主要是自由度当前位移即作动器的当前实测位移。
有些数据中含有一部分加速度的信号,删除即可,该矩阵已不处理加速度相关的计算,否则可能引起严重的错误。
- Frank 方式(早期的计算方式)
通过各自由度的当前命令来估算实际的误差,同时也可以计算出自由度下真实的误差,直接进行PID控制,真实的自由度位移也反馈给ServoController,但FeedbackScale=0.
优点是在控制运行过程中快速的计算出误差,缺点是台面处于停机位时,给定不同MeanLevel值,系统的估算的自由度位移会不准确,同时开启低压时需要进行多次迭代求最接近的位移值。
将ServoController所需的误差与自由度迭代计算的误差独立开来,Transform8s只管迭代计算当前的位置,得到准确的位置后反馈给ServoController(FeedbackScale=1),由ServoController自行计算误差。
垂直向主要是Twist计算,垂向位移变换计算方式如下,此时水平坐标不变.
Δx=0,Δy=0,Δz=P.x∗P.y∗Twist∗Factor
水平向的Shear相对较为复杂,早期对于正交且作动器又较少的系统而言,手动可以确定出Shear的方向,但对于更多的作动器系统时,较难找到通用的计算方式来估计矩阵,同时需要保证矩阵满秩,Glen提供的最新的计算方式为将Shear替换为Stretch,
Δx=P.x∗P.y∗Stretch∗Factor,Δy=−P.x∗P.y∗Stretch∗Factor,Δz=0
上述Factor通常为边长的平方分之一,
Factor=Lwindth⋅Lwindth1