直接姿态反馈控制方法及直接姿态反馈控制机器与流程

1.本发明涉及一种反馈控制方法及反馈控制机器，且尤其是涉及一种直接姿态反馈控制方法及直接姿态反馈控制机器。


背景技术：

2.cnc机床或工业用关节机器的驱动机构中的线性轴或转动轴作定位运动时会有定位误差，一般以六个误差项来表示，以cnc机床的x轴或工作台做线性定位运动为例，定位误差包含工作台在x、y、z三个方向出现的线性误差，及对x、y、z轴转动的翻滚(roll)、俯仰(pitch)及偏摆(yaw)转动误差，驱动机构中轴与轴间也有垂直度或平行度误差。理想工业用关节机器的手臂中各个转动轴间的机构参数可用例如dh机构参数加以描述，但是因为制造及组装因素，各个机构参数也有几何误差。因此，如何有效地减少机器的定位误差，一直是业者努力的目标。
3.机器除了在驱动机构中有几何误差，在操作时由于受到内部热源及外部环境温度变化或热源影响，机器会出现热变形，内部热源包括运动元件间的摩擦，例如主轴轴承、滚珠螺杆螺帽、导轨等，伺服马达能量转换时会产生热量，外部热源例如是环境温度、操作人员的体温或日光照射。要提高机器的精度，必须排除或降低机器的热误差，例如以温度感测器量出机器结构及元件或组件的实际温度，由热误差模型算出机器的热误差并加以补偿。
4.机器结构及驱动机构中的几何误差及热误差最终造成工具相对工件运动或定位时有姿态误差，包括位置误差及指向误差，先进的控制器中可以建立机器的几何误差模型，例如以激光干涉仪量出三线性轴机器的21项几何误差，进行机器的几何误差补偿，但是这21项几何误差会因机器的温度状态而变化，影响补偿效果。
5.在美国专利us5681981a、us6226884b1及us6662461b2中可见具史都华平台机构(stewart platform mechanism)的测量装置及方法，其中第一平台及第二平台以六支可伸缩测量杆连接形成平行机构，通过测量各测量杆的长度，在测量电脑内算出第一平台相对第二平台间的坐标关系。在美国专利us7810248b2中可见一种坐标测量机器，其中连接二平台间的伸缩杆有特殊安排，形成有利于工件进出的工作空间。在美国专利us2019/0072373a1中可见一种六连杆坐标测量机器，其中连接二平台间的伸缩杆有气压轴承设计。
6.这些具有史都华平台机构的测量装置或机器有诸多缺点，由于机构参数有几何误差及热误差，使这些测量装置的测量精度不高，应用有限。在同一申请人的中国台湾专利twi585363b“应用于测量的双球杆系统及其误差补偿方法”中提出针对双球杆的几何误差及热误差补偿分法，然仍有其缺失存在。


技术实现要素：

7.为了提供更精准的机器，本发明提供一种直接姿态反馈控制方法及直接姿态反馈控制机器。
8.依据本发明的一态样的一实施方式，提供一种直接姿态反馈控制方法，应用于一
直接姿态反馈控制机器，直接姿态反馈控制机器包含一控制器、一双球杆姿态测量机构、多个马达及一工具，双球杆姿态测量机构设置于直接姿态反馈控制机器的一基本端及一工具端间且包含一工具端平台设置于工具端、一基本端平台设置于基本端、至少六支工作双球杆连接工具端平台及基本端平台、以及至少一参考双球杆设置于一参考板，工具端平台包含至少三球窝，基本端平台包含至少三球窝，直接姿态反馈控制方法以一姿态补偿控制配合一位置反馈控制，姿态补偿控制包含一初始化步骤、一坐标转换关系建立步骤、一实际姿态计算步骤及一位置补偿值计算步骤。于初始化步骤中，使控制器建立前述至少一参考双球杆及各工作双球杆的一中心距离函数，及建立双球杆姿态测量机构的一工具平台坐标系与一基本平台坐标系间的坐标转换关系。于坐标转换关系建立步骤中，使控制器依据一内建或是载入的设定值或一机上测量，建立基本平台坐标系与一参考坐标系的坐标转换关系。于实际姿态计算步骤中，使控制器读入各工作双球杆的一工作位移测量值，计算工具的一实际姿态。于位置补偿值计算步骤中，使控制器依一补偿目标姿态及实际姿态得出各马达的一增量位置补偿值及一位置补偿值，其中，姿态补偿控制使用各马达的补偿控制器，由各马达的位置补偿值得出补偿驱动值，位置反馈控制使用各马达的位置控制器，由位置设定值与位置实际值之差，得出驱动值，驱动值与补偿驱动值相加后输出给各马达的驱动器，各马达的位置补偿值维持有效，直到姿态补偿控制更新为止。
9.依据前述的直接姿态反馈控制方法的多个实施例，其中，姿态补偿控制的取样时间大于等于位置反馈控制的取样时间。
10.依据前述的直接姿态反馈控制方法的多个实施例，其中，补偿目标姿态为编码器定义姿态或是设定姿态。
11.依据前述的直接姿态反馈控制方法的多个实施例，其中，以雅可比逆矩阵得出各马达的增量位置补偿值。
12.依据前述的直接姿态反馈控制方法的多个实施例，其中，由各马达的编码器定义姿态位置值减去各马达的等效位置实际值，得出各马达的增量位置补偿值。
13.依据前述的直接姿态反馈控制方法的多个实施例，其中，补偿控制器是比例控制器。
14.依据前述的直接姿态反馈控制方法的多个实施例，其中，各马达的补偿控制器与位置控制器相同，此时以各马达的位置补偿值与各马达的位置设定值相加后，输入位置控制器得出补偿后的驱动值。
15.依据前述的直接姿态反馈控制方法的多个实施例，其中，对各马达的位置补偿值进行低通滤波处理，得出无振动因素的位置补偿值。
16.依据前述的直接姿态反馈控制方法的多个实施例，其中，于初始化步骤中，驱动至少一马达使各工作双球杆过原点以初始化控制器内建或是载入的各工作双球杆的中心距离函数。
17.依据前述的直接姿态反馈控制方法的多个实施例，其中，机上测量包括分别驱动至少二马达定位，由双球杆姿态测量机构的测量结果得出参考坐标系，及得出基本平台坐标系与参考坐标系的坐标转换关系。
18.依据前述的直接姿态反馈控制方法的多个实施例，其中，于位置补偿值计算步骤中，使用一次逼近方法或是迭代多次逼近方法计算得出各马达的增量位置补偿值。
19.依据前述的直接姿态反馈控制方法的多个实施例，其中，控制器读入参考双球杆的热误差测量值，由热误差测量值计算并补偿双球杆姿态测量机构各个机构参数的热误差，包括工具端平台及基本端平台上球窝位置的热误差及工作双球杆中心距离的热误差。
20.依据前述的直接姿态反馈控制方法的多个实施例，其中，实际姿态计算步骤中，控制器读出被一触发信号同步锁存的各工作位移测量值，算出工具末端测量球的中心的坐标位置。
21.依据前述的直接姿态反馈控制方法的多个实施例，其中，直接反馈控制机器还包含安装基本端平台的一支架，且于实际姿态计算步骤中，依参考双球杆的热误差测量值计算并补偿支架高度的热误差。
22.依据前述的直接姿态反馈控制方法的多个实施例，其中，于实际姿态计算步骤中，依参考双球杆的热误差测量值计算并补偿工具长度的热误差。
23.依据前述的直接姿态反馈控制方法的多个实施例，其中，控制器对实际姿态进行频谱分析，得出直接反馈控制机器的振动信息。
24.依据本发明的另一态样的一实施方式提供一种直接姿态反馈控制机器，其包含一机器本体、多个马达、一控制器及一双球杆姿态测量机构，机器本体包含一工具端及一基本端，且工具端设置工具，马达设置于机器本体，控制器驱动马达，使工具相对基本端的一工件运动，双球杆姿态测量机构设置于基本端与工具端之间且包含一工具端平台、一基本端平台、至少六支工作双球杆、一参考板及至少一参考双球杆，工具端平台位于工具端且包含至少三球窝；基本端平台位于基本端且包含至少三球窝，至少六支工作双球杆，连接于工具端平台的球窝及基本端平台的球窝间，各工作双球杆包含二测量球，以各工作双球杆的位移感测器量出二测量球的中心间的距离，参考板包含至少二参考球窝，参考双球杆包含二参考测量球，设置于参考板上至少二参考球窝之间，以一位移感测器量出二参考测量球中心间的距离，其中，控制器以双球杆姿态测量机构作为姿态误差的测量装置，以参考双球杆作为双球杆姿态测量机构的机构参数热误差的测量装置，由双球杆姿态测量机构测量得出工具的实际姿态及连结姿态误差，及各马达的位置补偿值或是补偿驱动值，消除工具的连结姿态误差。
25.依据前述的直接姿态反馈控制机器的多个实施例，可还包含一主轴座及一主轴，主轴座设置于机器本体，主轴连接于主轴座且包含一主轴外壳，其中，工具端平台固定于主轴座或主轴外壳上。
26.依据前述的直接姿态反馈控制机器的多个实施例，其中，直接姿态反馈控制机器可以为一多轴机床、一多轴关节机器或一平行连杆驱动机器。
27.依据前述的直接姿态反馈控制机器的多个实施例，其中，工具可以为一刀具、一测量探头、一触发探头、一共轭聚焦传感器、一位移测量激光或是一光学测量镜头。
28.依据前述的直接姿态反馈控制机器的多个实施例，其中，工具可以产生一触发信号，以触发信号同步锁存各工作双球杆的工作位移测量值。
29.依据前述的直接姿态反馈控制机器的多个实施例，可还包含一参考球，其设置于基本端平台或是工作台上。
30.依据前述的直接姿态反馈控制机器的多个实施例，可还包含一刀长测量机构，其设置于基本端平台或是工作台。
31.依据前述的直接姿态反馈控制机器的多个实施例，其中，工具端平台及基本端平台上的多个球窝可以有不对称分布。
32.依据前述的直接姿态反馈控制机器的多个实施例，其中，基本端平台及工具端平台上的球窝可固定于热胀系数介于0ppm/℃(μm/m/℃)与2ppm/℃间的一材料上。
33.依据前述的直接姿态反馈控制机器的多个实施例，可以还包含一支架，基本端平台安装于支架上。
34.依据前述的直接姿态反馈控制机器的多个实施例，其中，控制器可以包含一实时通信网络，各工作双球杆及参考双球杆通过实时通信网络回传各工作位移测量值及热误差测量值。
35.依据前述的直接姿态反馈控制机器的多个实施例，其中，实时通信网络是实时以太通信网络(ethercat)。
36.依据前述的直接姿态反馈控制机器的多个实施例，其中，各工作双球杆及参考双球杆的位移感测器可以为一激光干涉仪、一光学尺或一磁性尺。
附图说明
37.图1示出依照本发明一实施例的一种直接姿态反馈控制机器的示意图；
38.图2示出依照本发明另一实施例的一种直接姿态反馈控制机器的示意图；
39.图3示出依照本发明又一实施例的一种直接姿态反馈控制机器的示意图；
40.图4示出图3实施例的直接姿态反馈控制机器的控制架构方框示意图；
41.图5示出依照本发明的一种直接姿态反馈控制方法的方块流程图；
42.图6示出图5实施例的直接姿态反馈控制方法的工具的姿态示意图；
43.图7示出图5实施例的直接姿态反馈控制方法的一控制流程图；
44.图8示出图5实施例的直接姿态反馈控制方法的另一控制流程图；
45.图9示出图5实施例的直接姿态反馈控制方法的又一控制流程图；
46.图10示出图5实施例的直接姿态反馈控制方法的再一控制流程图；以及
47.图11示出依照本发明更一实施例的一种直接姿态反馈控制机器的双球杆姿态测量机构示意图。
48.其中，附图标记说明如下：
49.10，10a，10b：直接姿态反馈控制机器
50.11：底座
51.11a，11b，11d：六轴机器手臂
52.12，12a，12b：工作台
53.13b：支架
54.110，110a，110b：工件
55.120：垫高块
56.120a：工件安装座
57.130：刀长测量装置
58.131a，131b：参考球
59.200b：控制器
60.210b：视窗操作模块
61.211b：人机界面
62.220b：运动控制核心模块
63.221b：解译器
64.222b，222d：插补器
65.2221d：路径插补器
66.2222d：指向插补器
67.223b，223d：加减速规划器
68.224d：逆转换器
69.225d：正转换器
70.226d：时间迟延元件
71.227d：驱动系统
72.228d：积分器
73.230b：实时通信网络
74.240b：驱动器
75.250b：数字计数器
76.3：z运动平台
77.300，300a，300b，300c，300d：双球杆姿态测量机构
78.310，310a，310b，310c：基本端平台
79.311，311a，311b：定位层
80.312，312a，312b：支承层
81.320，320a，320b，320c：工具端平台
82.321，321a，321b：定位层
83.322，322a，322b：支承层
84.330，330a，330b，330c：工作双球杆
85.4：主轴座
86.400，400a，400b：参考双球杆
87.5：主轴
88.500，500a，500b：参考板
89.510，520：磁性球窝
90.6，6a，6b：工具
91.800b：马达
92.b1～b6：球窝
93.p1～p6：球窝
94.y：轴
95.{b}：基本平台坐标系
96.{p0}：起始工具平台坐标系
97.{p
n
}：实际工具平台坐标系
98.{r}：参考坐标系
99.91：位置反馈控制
100.911：位置控制器
101.92：姿态补偿控制
102.921：补偿控制器
103.s01：初始化步骤
104.s02：坐标转换关系建立步骤
105.s03：实际姿态计算步骤
106.s04：位置补偿值计算步骤
107.ff：前馈控制
108.j-1
：雅可比逆矩阵
109.p
a
：实际姿态
110.p
e
：编码器定义姿态
111.p
r
：参考姿态
112.p
s
：设定姿态
113.e
a
：实际姿态误差
114.e
l
：连结姿态误差
115.e
f
：追踪姿态误差
116.v
cmd，i
：马达驱动值
117.θ
i，a
，θ
1，a
，θ
6，a
：等效位置实际值
118.θ
i，e
，θ
1，e
，θ
6，e
：编码器定义姿态位置值
119.θ
i，s
，θ
1，s
～θ
6，s
：位置设定值
120.θ
i，enc
，θ
1，enc
，θ
6，enc
：位置实际值
121.δθ
i，comp
，δθ
1，comp
，δθ
6，comp
：增量位置补偿值
122.θ
i，comp
，θ
1，comp
，θ
6，comp
：位置补偿值
123.工具平台坐标系与基本平台坐标系的坐标转换关系
124.实际工具平台坐标系与基本平台坐标系的坐标转换关系
125.实际工具端点坐标系与基本平台坐标系的坐标转换关系
126.工具端点坐标系与基本平台坐标系的坐标转换关系
127.基本平台坐标系与参考坐标系的坐标转换关系
128.工具端点坐标系与参考坐标系的坐标转换关系
129.工具端点坐标系与工具平台坐标系的坐标转换关系
130.编码器定义姿态下实际工具端点坐标系与参考坐标系间的坐标转换关系
131.实际工具端点坐标系与实际工具平台坐标系的坐标转换关系
132.实际工具端点坐标系与参考坐标系的坐标转换关系
具体实施方式
133.以下将参照附图说明本发明的实施例。为明确说明起见，许多实务上的细节将在以下叙述中一并说明。然而，阅读者应了解到，这些实务上的细节不应用以限制本发明，也就是说，在本发明部分实施例中，这些实务上的细节是非必要的。此外，为简化附图起见，一些现有惯用的结构与元件在附图中将以简单示意的方式示出；并且重复的元件将可能使用相同的编号或类似的编号表示。
134.此外，本文中当某一元件(或机构或模块等)“连接”、“设置”或“耦合”于另一元件，可指所述元件是直接连接、直接设置或直接耦合于另一元件，亦可指某一元件是间接连接、间接设置或间接耦合于另一元件，意即，有其他元件介于所述元件及另一元件之间。而当有明示某一元件是“直接连接”、“直接设置”或“直接耦合”于另一元件时，才表示没有其他元件介于所述元件及另一元件之间。而第一、第二、第三等用语只是用来描述不同元件或成分，而对元件/成分本身并无限制，因此，第一元件/成分亦可改称为第二元件/成分。且本文中的元件/成分/机构/模块的组合非此领域中的一般周知、常规或现有的组合，不能以元件/成分/机构/模块本身是否为现有，来判定其组合关系是否容易被技术领域中的通常知识者轻易完成。
135.本发明的一目的在于提供一种直接姿态反馈控制方法，通过直接测量工具相对工件的实际姿态，亦即工具端点的实际位置及工具的实际指向，得出姿态误差，驱动马达进行姿态补偿控制，以消除机器结构及驱动机构中的几何误差及热误差。
136.应用本发明的直接姿态反馈控制方法的机器可称为直接姿态反馈控制机器，直接姿态反馈控制机器的驱动机构可以是串联式或是并联式，以多个马达来驱动工具相对工件运动，直接姿态反馈控制机器可以是多轴机床或多轴关节机器手臂，也可以是平行连杆驱动机器(或称平行机构机器)。在直接姿态反馈控制机器的工具端及基本端间设置有一双球杆姿态测量机构，双球杆姿态测量机构包括安装在工具端的一工具端平台、安装在基本端的一基本端平台及至少六个工作双球杆。工具端平台及基本端平台各包含至少三个球窝；六个工作双球杆连接工具端平台上的球窝及基本端平台上的球窝。双球杆姿态测量机构可以另外包括一参考板及至少一支参考双球杆，参考板包含多个参考球窝，至少一支参考双球杆被置于参考板的二参考球窝上，此参考双球杆用于双球杆姿态测量机构的机构参数热误差补偿，直接姿态反馈控制机器的控制器以此双球杆姿态测量机构，量出工具运动或定位时的实际姿态，进行直接姿态反馈控制。
137.在本发明的直接姿态反馈控制机器，不针对机器结构及驱动机构中的几何误差及热误差配置个别的感测器，而是在工具端及基本端设置双球杆姿态测量机构，机器结构及驱动机构中各个零组件的几何误差、热误差所造成工具相对工件的总成六维姿态误差，由此双球杆姿态测量机构测量得出，然后得出各马达的位置补偿值，补偿控制器输入位置补偿值产生马达的补偿驱动值，与位置控制器产生的马达的驱动值相加，输出至驱动器驱动
马达，使本发明的直接姿态反馈控制机器成为无机构参数误差、无几何误差及无热误差的理想机器。
138.作为直接姿态反馈控制的测量装置，双球杆姿态测量机构的测量精度必须非常高，位移及转动角度的测量精度最好是微米及角秒(arc-second)等级或是更好。双球杆姿态测量机构本身也有几何误差及热误差，包括工作双球杆的几何误差及热误差，及工具端平台及基本端平台上球窝中心的位置误差及热误差，现有的并联测量机构不能准确消除这些误差，精度有限。
139.工作双球杆要由其位移测量值得出其二末端测量球中心间的绝对距离，简称中心距离，为了提升双球杆姿态测量机构的精确度，本发明的直接反馈控制机器中，控制器可以内建或是载入各个工作双球杆的中心距离函数，此中心距离函数以工作双球杆的位移测量值为输入，以其准确的中心距离为输出，于初始化步骤中启动各工作双球杆的中心距离函数。此中心距离函数可以借助一校正板而建立，此校正板上设置有多个磁座，特定二磁座间的中心距离为已知，校正板提供至少两个已知的中心距离，校正时将工作双球杆置于二已知中心距离的磁座上，此时，工作双球杆的位移测量值与此已知的中心距离构成一校正点，以至少二校正点可以建立工作双球杆的中心距离函数。校正点数量越多，精度也越好。
140.本发明可以另外提供设置于参考板上的至少一参考双球杆，依据此参考双球杆的热误差测量值得出因环境温度变化所造成的工作双球杆的热误差，由控制器加以补偿。另一方面，更依据参考双球杆的热误差测量值得出工具端平台及基本端平台上各个球窝中心位置的热误差，并加以补偿，如此，在本发明的直接姿态反馈控制机器中，双球杆姿态测量机构中所有机构参数的几何误差及热误差均被补偿，因而环境温度变化在5℃内时，双球杆姿态测量机构的测量精度仍然可以达到微米及角秒等级。在本发明的直接姿态反馈控制机器中，也针对工具的热误差，例如刀具或是测量探头长度的热误差，依据参考双球杆的热误差测量值加以补偿，通过本发明中的设计及方法，使本发明的直接姿态反馈控制机器有微米及角秒等级的定位精度。
141.参考双球杆的中心距离函数可以是内建于控制器中或是初始化时载入，当工作双球杆或是参考双球杆是使用绝对式位移感测器时，在机器开机后控制器可以直接启动其中心距离函数；当工作双球杆是使用增量式位移感测器时，开机后控制器可以通过驱动机器例如走z轴，使各个工作双球杆过原点而启动其中心距离函数。参考双球杆是使用增量式位移感测器时，由于其不具驱动机构，要有额外手段，例如以手动或是辅助装置使其过原点，以启动其中心距离函数。
142.直接姿态反馈控制机器可以是多轴机床，也可以是多轴关节机器，机器上安装使用的工具可以是主轴带动的刀具，例如铣刀，也可以是固定不动的量具，包括测量探头、共轭聚焦传感器、激光位移测量器、光学测量镜头等。为精准计算工具端点的位置与工具指向，控制器要同步读出所有工作双球杆的工作位移测量值，其可通过实时计时器(real-time timer)的定时中断进行，当工具是测量探头时，可另外通过一触发信号同步锁存所有工作双球杆的工作位移测量值，再由控制器读出。工具是测量探头时，在基本端平台或是置放工件的工作台上可另外设置一参考球，以校正探头末端测量球的中心坐标位置；而若工具是刀具时，基本端平台或是置放工件的工作台上可以另外设置一刀长测量装置，用以校正刀长。
143.在本发明的直接姿态反馈控制机器中，工具端平台及基本端平台可以是独立设计，通过适当的转接件分别安装于直接姿态反馈控制机器的工具端及基本端，另外也可以与直接姿态反馈控制机器进行一体设计，要点是，工具端平台必须可以灵敏反应出机器的几何误差及热误差所造成的工具端点位移及工具指向变化，基本端平台则必须考虑工件的进出及固定方式。在一实施例中，直接姿态反馈控制机器是有主轴的多轴机床，工具端平台是经由转接件固定在主轴座的端面或是主轴外壳上，可以随同主轴的热变形而产生位移及偏转。在另一实施例中，直接姿态反馈控制机器是多轴关节机器手臂，工具端平台是固定在工具安装座上，工具则是固定在工具端平台上。
144.工具端平台及基本端平台各包含三个或六个球窝，当工具端平台及基本端平台各包含三个球窝时，一球窝上可连接两个工作双球杆；当工具端平台及基本端平台各包含六个球窝时，一个球窝上可连接一支工作双球杆，且工具端平台及基本端平台共设置至少六个工作双球杆，而构成史都华平台机构。对标准史都华平台机构而言，工具端平台及基本端平台上六个球窝的位置是对称周期分布，例如以20度及100度的角度间隔，周期分布于一圆周上。为避免各工作双球杆间或是与机器零组件的干涉，或是为了增加特定方向的工作行程，也可以刻意偏移一些球窝的位置，而各个工作双球杆的最大中心距离及最小中心距离则与球窝的分布相配合，可以互不相同。在一实施例中，基本端平台上的六个球窝是不对称分布于一半圆圆周范围，以避开z运动平台及主轴座，工具端平台上的六个球窝也有对应的位置偏移。
145.工具端平台及基本端平台上的球窝中可设有磁铁，而允许工作双球杆末端的球吸附于球窝上，以构成球轴承结构。球窝可以例如是以三点或是以圆弧支撑吸附的球窝，此时球窝与球之间有滑动接触，也可以在球与球窝间设置滚珠，形成滚动接触，如此有较佳耐磨特性。在另一实施例中，工具端平台或基本端平台上的球窝可以改成球，此时工作双球杆的末端即是相配合的球窝。
146.在本发明的直接姿态反馈控制机器中，工具端平台及基本端平台上的球窝与工作双球杆末端的球构成球轴承结构(简称球轴承)，此球轴承的中心位置必须不受外力及热的影响。为降低或排除工具端平台及基本端平台上各个球轴承中心位置因环境温度变化而产生热误差，球窝最好是固定在热胀系数小于2ppm/℃的材料上，甚至是接近零热胀系数(小于0.1ppm/℃)的材料上，例如invar因瓦合金、super invar超因瓦合金、碳纤维复合材料、合金或石英等，或是微晶玻璃，例如微晶玻璃或微晶玻璃。
147.在直接姿态反馈控制机器中，工具及工件可以在工具端平台、基本端平台及六个工作双球杆所包覆的空间之内或外。在一实施例中，直接姿态反馈控制机器是三线性轴机床，且主轴(高速主轴)由x轴及z轴驱动，工件是固定在y轴工作台上，工具端平台是通过一转接件固定在主轴外壳上，基本端平台是中空环形，被固定在y轴工作台上，工件是固定在基本端平台的中空部分，六个工作双球杆连接工具端平台及基本端平台之间，而将刀具及工件均包覆在内，另外，在基本端平台上可安装刀长测量装置，以之校正刀长。
148.在另一实施中，直接姿态反馈控制机器包含一六轴机器手臂及一工作台，工作台可为花岗岩制成，六轴机器手臂被固定于此工作台上，工具端是六轴机器手臂未端的工具安装座，基本端是工作台上的工件安装面，工具可为一测量探头，工具端平台可被固定在工
具安装座上，测量探头可固定在工具端平台上，基本端平台可被固定于工作台上且具有中空部位，工件被置于此中空部位，六支工作双球杆连接工具端平台及基本端平台，测量探头及工件是位在工具端平台、基本端平台及六支工作双球杆包覆的空间内，测量探头有触发信号，可使控制器同步读出六支工作双球杆的位移测量值，以计算得出测量探头末端测量球的中心坐标位置，基本端平台上或是工作台上可另外固定一参考球，作为坐标校正用。此直接姿态反馈控制机器可以用于小工件的坐标测量。
149.在另一实施例中，直接姿态反馈控制机器包含一六轴机器手臂、一工作台及一支架，工具是测量探头，工件固定在工作台上，支架也是固定于工作台上，基本端平台被固定于支架上，工具端平台固定在六轴机器手臂的工具安装面上，六支工作双球杆连接工具端平台及基本端平台，形成倒立式的双球杆姿态测量机构，因而测量探头及工件是在工具端平台、基本端平台及六个工作双球杆包覆的空间外。工作台上可固定有一参考球，其做为坐标校正用。此实施例的直接姿态反馈控制机器手臂的好处是，可以用于中小工件的坐标测量，工件的操作及进出空间较方便。由于基本端平台固定于支架上，故支架最好用低热胀系数高强度的材料制成，例如invar因瓦合金或是合金，以避免支架的热误差影响测量结果，此外，亦可对支架高度的热误差进行补偿，以确保直接姿态反馈控制机器的精度。
150.在一实施例中，控制器可通过一实时通信网络，其例如为以太实时通信网络(ethercat)，读回各工作双球杆的工作位移测量值、参考双球杆的热误差测量值及马达的位置实际值，也借此送出马达驱动值，如此可以大大减少配线数量。于双球杆姿态测量机构中，工作双球杆的位移感测器可以是绝对式或是增量式的激光干涉仪、光学尺或是磁性尺，在初始化后工作双球杆有绝对中心距离。
151.本发明提供一种直接姿态反馈控制方法，其中，在工具端平台上定义有一工具平台坐标系，在基本端平台上定义有一基本平台坐标系，在机器上定义有一参考坐标系，工具平台坐标系中球轴承中心的坐标位置、基本平台坐标系中球轴承中心的坐标位置、及六支工作双球杆的中心距离构成双球杆姿态测量机构的机构参数，进行直接姿态的反馈控制时，以此机构参数做机构正逆转换计算，算出工具平台坐标系相对基本平台坐标系的六维坐标转换关系，再算出工具在参考坐标系的实际姿态，包括三位置及三转动角。
152.本发明的直接姿态反馈控制方法，可以由参考双球杆的热误差测量值得出双球杆姿态测量机构的机构参数的热误差，包括所述工作双球杆中心距离的热误差，及工具端平台及基本端平台上各个球轴承中心位置的热误差，并加以补偿，以补偿后的机构参数做机构转换计算。
153.直接姿态反馈控制机器在机器结构及驱动机构中有静态机构参数误差及几何误差，也有准静态热误差，控制器驱动工具相对工件运动时有动运动态误差，最终造成工具端点的位置误差及工具的指向误差，亦即工具姿态误差。机器手臂控制器中的插补器进行正向及逆向机构转换时所使用的机构参数是理想机器手臂的机构参数，因此，在逆向机构转换中由工具的设定姿态得出各个马达的位置设定值，是理想机器手臂中各个马达的位置设定值。
154.一般机床或机器手臂的控制器进行位置反馈控制时，由各个马达的位置设定值与量出的马达位置实际值算出追踪误差，位置控制器依此追踪误差产生马达驱动值，例如速
度指令。控制器在驱动多个马达运动时，实际运动轨迹会偏离设定路径，产生轨迹误差(contour error)。控制器中有提供控制及补偿方法，用以降低甚至消除追踪误差或是轨迹误差，例如前馈控制(feed forward，ff)、轨迹控制、摩擦力补偿控制及切削力补偿控制等，实现直接姿态的反馈控制时最好能保留控制器中这些现有的控制或补偿方法。
155.本发明的直接姿态反馈控制方法中，除了双球杆姿态测量机构的工具平台坐标系及基本平台坐标系外，还有机器的参考坐标或是理想轴坐标系、工具端点坐标系，各个坐标系间有坐标转换关系，控制器可对这些坐标系进行管理。进行直接姿态的反馈控制时，要由双球杆姿态测量机构中各个工作双球杆的工作位移测量值，经一系列复杂的计算才能得出工具的实际姿态及姿态误差，以姿态误差进行姿态反馈控制，这姿态误差计算极为耗时，而且计算时间变动大，将之安排在控制回路中执行会有死区时间(dead time)问题，对马达伺服控制造成不利影响。
156.因而在本发明的直接姿态反馈控制方法中，拆解姿态误差为连结姿态误差及追踪姿态误差，以双球杆姿态测量机构测量得出工具的一实际姿态，以各个马达的位置实际值减去位置补偿值后做机构正转换计算，得出一工具姿态，称的为编码器定义姿态，由实际姿态与编码器定义姿态得出连结姿态误差，姿态补偿控制处理连结姿态误差，得出各个马达的位置补偿值及补偿驱动值，另一方面，由设定姿态与编码器定义姿态得出追踪姿态误差，位置反馈控制处理追踪姿态误差，得出各个马达的驱动值。由于连结姿态误差变化缓慢，姿态补偿控制不必与位置反馈控制有相同的取样时间。
157.本发明的直接姿态反馈控制方法的优点因而是，维持控制器原有的高取样频率实时位置反馈控制，另外可用一低取样频率的姿态补偿控制与实时位置反馈控制配合，解决了直接姿态反馈控制中因姿态误差计算造成的死区时间问题，使直接姿态反馈控制可以由独立运行而又相互配合的实时位置反馈控制及姿态补偿控制完美实现。实时位置反馈控制与姿态补偿控制可以安排在相同或是不相同的计时器中断服务程序执行，可以有自己的控制器，姿态补偿控制的取样时间最好大于等于位置反馈控制的取样时间。在一实施例中，位置反馈控制与姿态补偿控制使用同一位置控制器，此时，可以姿态补偿控制得出的各个马达的位置补偿值修正各个马达的位置设定值。
158.直接姿态反馈控制方法在不同的机器应用中衍生不同的应用方法，在一实施例中，直接姿态反馈控制机器是坐标测量机，双球杆姿态测量机构的工具端平台上安装的工具为测量探头，且以测量探头的触发信号同步锁存双球杆姿态测量机构中各工作双球杆的工作位移测量值，控制器读出后进行测量探头末端测量球中心的坐标位置计算。
159.在一实施例中，基本端平台是固定于一支架上，支架则是与工件一起固定于工作台上，基本平台坐标系与工件坐标系间有一偏置量，支架及工具于垂直方向的热误差会影响工具的端点在工件坐标系内的坐标值。在一实施例中，建立支架的热误差与参考双球杆的热误差测量值间的关系，由参考双球杆实时的热误差测量值得出支架高度的热误差，并加以补偿。在一实施例中，工具的端点在工具端工具平台坐标系内也有一偏置量，建立工具长度热误差与参考双球杆的热误差测量值间的关系，由参考双球杆实时的热误差测量值得出工具长度的热误差，并加以补偿。
160.请参考图1，其中图1示出依照本发明一实施例的一种直接姿态反馈控制机器10的示意图。直接姿态反馈控制机器10包含一机器本体(未标示)、多个马达(未标示)、一控制器
(未示出)、一双球杆姿态测量机构300、一参考板500及至少一参考双球杆400。机器本体包含一工具端(未标示)及一基本端(未标示)，且工具端设置工具6，马达设置于机器本体上，控制器设置于机器本体外，且控制器驱动马达使工具6相对一工件110位移。双球杆姿态测量机构300设置于基本端与工具端之间且包含一工具端平台320、一基本端平台310及至少六工作双球杆330，工具端平台320位于工具端；基本端平台310位于基本端，至少六工作双球杆330连接工具端平台320及基本端平台310。参考板500不受马达驱动，且参考板500包含至少二磁性球窝510、520，前述至少一参考双球杆400设置于参考板500的至少二磁性球窝510、520上。直接姿态反馈控制机器10可还包含一主轴座4及一主轴5，主轴座4设置于z运动平台3上，主轴5连接于主轴座4，工具端平台320是固定于主轴5外壳上。
161.在图1中，直接姿态反馈控制机器10可还包含一工作台12及一底座11，工作台12位于底座11上且可沿y轴位移，而工件110位于工作台12上，为了进行工具6相对工件110的直接反馈，主轴5的末端与工作台12间装有双球杆姿态测量机构300。工具端平台320可包含多个球窝(未标示)，基本端平台310亦包含多个球窝(未标示)，工具端平台320可还包含一定位层321及至少一支承层322，工具端平台320的球窝位于工具端平台320的定位层321；基本端平台310包含另一定位层311及至少另一支承层312，基本端平台310的球窝位于基本端平台310的定位层311。为了减少热误差，基本端平台310的球窝及工具端平台320的球窝可固定于一材料上，且材料的热胀系数介于0-1ppm/℃之间。因此，固定球窝的定位层311、321的热胀系数可介于0-1ppm/℃之间，而支承层312、322的热胀系数可介于0-2ppm/℃之间。
162.具体而言，工具端平台320具有两层结构，定位层321可以是石英制成的中空环形板，其上有六个球窝，支承层322是invar因瓦合金制成的中空圆柱，支承层322的末端有法兰，用以承托定位层321，工具端平台320通过支承层322被固定于主轴5外壳上。基本端平台310被固定在工作台12上，也由两层结构组成，定位层311是石英制成的中空方形板，支承层312是invar因瓦合金制成的中空方形板，定位层311上有六个球窝，工件110是固定于基本端平台310中空部位的一垫高块120上，六支工作双球杆330连接工具端平台320及基本端平台310，构成直接姿态反馈控制机器10，刀具6及工件110是在工具端平台320、基本端平台310及六支工作双球杆330包覆的空间内。
163.工具6于图1中示例为一刀具，故直接姿态反馈控制机器10可还包含一刀长测量装置130，其设置于基本端平台320，且刀长测量装置130可用以测量刀具的刀长，然在其他实施例中，刀长测量装置亦可设置于工作台上，不以此为限。此外，用于热误差补偿的参考双球杆400被置于参考板500上，参考板500被置于底座11上。
164.请参阅图2，其示出依照本发明另一实施例的一种直接姿态反馈控制机器10a的示意图，可以在一般工厂环境下进行小尺寸工件110a的坐标测量。在图2的实施例中，直接姿态反馈控制机器10a包含一六轴机器手臂11a及一工作台12a，工作台12a可为花岗岩制成，机器手臂11a可设置于工作台12a上。工具端平台320a固定在机器手臂11a末端的工具安装座(未标示)上，工具6a为一测量探头且固定在工具端平台320a上，基本端平台310a则固定于工作台12a上，参考板500a设置于工作台12a上。
165.基本端平台310a有两层结构，基本端平台310a的定位层311a是石英制成的中空环形板，支承层312a是invar因瓦合金制成的圆板，工件110a固定于工件安装座120a上，工件安装座120a是固定在支承层312a上。工具端平台320a包含一定位层321a及一支承层322a，
六支工作双球杆330a连接工具端平台320a及基本端平台310a，构成双球杆姿态测量机构300a，工具6a及工件110a是在工具端平台320a、基本端平台310a及六支工作双球杆330a包覆的空间内，参考双球杆400a被置于参考板500a上，工具6a可以产生一触发信号，以之同步锁存工作位移测量值，再由控制器读出后算出工具6a末端测量球的中心坐标位置，在图2的实施例中，工具6a的坐标位置即是指测量探头末端测量球中心的坐标位置。此外，直接姿态反馈控制机器10a可还包含一参考球131a，其设置于基本端平台310a，更进一步地说，其是设置于支承层312a上，且做为坐标的校正用。然在其他实施例中，参考球可设置于工件安装平台上，不以此为限。
166.请参阅图3，其示出依照本发明的一种直接姿态反馈控制机器10b的又一实施例示意图。在图3的实施例中，直接姿态反馈控制机器10b包含一六轴机器手臂11b、一工作台12b及一支架13b，工作台12b可为花岗岩制成，六轴机器手臂11b及支架13b可设置于工作台12b上，其中，工具端可定义为六轴机器手臂11b未端的工具安装座，基本端可定义为支架13b上的一基本端平台310b安装平面，在图3中，只示出支架13b的右半部，以便显示其他零件，不以此限制本发明。
167.在图3的实施例中，双球杆姿态测量机构300b的基本端平台310b安装于支架13b上，基本端平台310b有两层结构，定位层311b是石英制成的环形板，由invar因瓦合金制成的支承层312b承托，工具端平台320b是通过一转接件固定在六轴机器手臂11b末端的工具安装座上，工具端平台320b也有两层结构，定位层321b是石英制成的环形板，由invar因瓦合金制成的支承层322b承托，工具6b为一测量探头且固定在工具端平台320b上，六支工作双球杆330b连接工具端平台320b及基本端平台310b，如此，工具6b及工件110b是在工具端平台320b、基本端平台310b及六支工作双球杆330b包覆的空间外，参考双球杆400b被置于参考板500b上。此直接姿态反馈控制机器10b的好处是，工件110b的操作及进出空间较大。
168.此外，在直接姿态反馈控制机器10b中，支架13b使用低热胀系数高强度的材料例如invar因瓦合金，可在控制器200b(示出于图4)中建立支架13b高度的热误差模型，依据参考双球杆400b的热误差测量值得出此支架13b上的基本端平台310b于垂直方向的热误差，并加以补偿，亦可依据此热误差测量值计算并补偿工具6b长度的热误差。直接姿态反馈控制机器10b可还包含一参考球131b，其设置于工作台12b上，且参考球131b可做为坐标的校正用。在此要特别说明的是，支架13b的热误差模型可用参考双球杆400b量出的热误差为输入，如此，可由参考双球杆400b量出的热误差计算出支架13b的热误差。类似地，亦可以得到对工具6b的端点的热误差。在其他实施例中，支架的热误差模型也可以将环境温度测量值及/或支架温度测量值为输入。
169.请参阅图4，并一并参阅图3，其中图4示出图3实施例的直接姿态反馈控制机器10b的控制架构方框示意图。直接姿态反馈控制机器10b的控制器200b(可为cnc控制器)包括在一视窗操作模块210b下工作的一人机界面211b，及实时环境下工作的一运动控制核心模块220b，其包括一解译器221b、一插补器222b及一加减速规划器223b，并与一位置反馈控制91及一姿态补偿控制92配合，位置反馈控制91及姿态补偿控制92将于后方再做说明。控制器200b使用一实时通信网络230b，此时，各工作双球杆330b与参考双球杆400b以实时通信网络230b回传各工作位移测量值与热误差测量值。
170.仔细而言，实时通信网络230b可为以太实时通信网络(ethercat)，控制器200b可
以通过以太实时通信网络与六个驱动器240b、七个数字计数器250b通信，而可同步读出各马达800b的位置实际值、各工作双球杆330b的工作位移测量值及参考双球杆400b的热误差测量值，进行姿态反馈控制数据处理，并通过实时通信网络230b将各马达800b的驱动值送至各相应的驱动器240b。在此要特别说明地是，工作双球杆330b及参考双球杆400b的位移感测器可以是一激光干涉仪、一光学尺或是一磁性尺，此外，各位移感测器可为绝对式或增量式，本发明不以此为限。
171.在直接姿态反馈控制机器10b中，工具6b(示例为测量探头)的触发信号可同步锁存六工作双球杆330b的工作位移测量值及参考双球杆400b的热误差测量值，控制器200b读出后计算得出工具6b末端的测量球的中心坐标位置，而当工具6b为扫描探头时，其位移测量信号也可经由实时通信网络230b送出。
172.在机器手臂的机构转换中，机构正转换由轴位置值θ＝(θ1，θ2，θ3，θ4，θ5，θ6)
t
，计算得出参考坐标系中的姿态p＝(x，y，z，α，β，γ)
t
，其中方向可用三转动角(α，β，γ)表示。假设各个轴的位置值有微量变化，以向量δθ表示，δθ＝(δθ1，δθ2，δθ3，δθ4，δθ5，δθ6)
t
，造成工具姿态有微量变化，以向量δp表示，δp＝(δx，δy，δz，δα，δβ，δγ)
t
，此微量轴位置值变化δθ与微量姿态变化δp间的关系可用雅可比矩阵j表示，如式(1)：
173.δp＝j
·
δθ
ꢀꢀꢀꢀꢀꢀ
(1)；
174.反之，微量姿态变化δp与微量轴位置值变化δθ间的关系可用雅可比逆矩阵j-1
表示，如式(2)：
175.δθ＝j-1
·
δp
ꢀꢀꢀꢀꢀꢀꢀꢀ
(2)。
176.请参阅图5、图6、图7、图8、图9及图10，其中图5示出依照本发明的一种直接姿态反馈控制方法中姿态补偿控制92的方块流程图，图6示出图5实施例的直接姿态反馈控制方法的工具的姿态示意图，图7示出图5实施例的直接姿态反馈控制方法的一控制流程图，图8示出图5实施例的直接姿态反馈控制方法的另一控制流程图，图9示出图5实施例的直接姿态反馈控制方法的又一控制流程图，图10示出图5实施例的直接姿态反馈控制方法的再一控制流程图。在图7及图8中，马达驱动系统227d是以二次转移函数表示，其包括自然频率ω及阻尼d，另外包括一积分器228d，转移函数为1/s，将速度转为位移。本发明中的直接姿态反馈控制方法包括位置反馈控制91及姿态补偿控制92，本说明书中马达相关的变数，以数字下标i代表第i马达，例如θ
1，comp
或θ
i，comp
代表第一或第i马达的位置补偿值，以无数字下标的变数例如θ
comp
代表所有马达的位置补偿值向量。
177.直接姿态反馈控制方法中，姿态补偿控制92可包含一初始化步骤s01、一坐标转换关系建立步骤s02、一实际姿态计算步骤s03及一位置补偿值计算步骤s04。于初始化步骤s01中，控制器建立各工作双球杆的中心距离函数，及双球杆姿态测量机构300d工具端的工具平台坐标系{p0}与基本端基本平台坐标系{b}的坐标转换关系
178.于坐标转换关系建立步骤s02中，控制器依据一内建或是载入的设定值，或是进行一机上测量，建立基本平台坐标系{b}与参考坐标系{r}的坐标转换关系
179.于实际姿态计算步骤s03中，控制器读入各工作双球杆的工作位移测量值，计算出工具的实际姿态p
a
。
180.于位置补偿值计算步骤s04中，姿态补偿控制92依据一补偿目标姿态及实际姿态
p
a
，得出各马达800b的位置补偿值θ
comp
。
181.图7及图8的实施例中，位置反馈控制91及姿态补偿控制92共用位置控制器911，其是以比例增益值k
v
表示，此时，以第一马达驱动为例，姿态补偿控制92得出位置补偿值θ
1，comp
，以之修正位置反馈控制91的位置设定值θ
1，s
，图10的实施例中显示位置反馈控制91及姿态补偿控制92共用位置控制器911，转移函数是c(s)时的控制流程图。图9的实施例中，姿态补偿控制92有自己的补偿控制器921，转移函数是cc(s)，由位置补偿值θ
i，comp
得出补偿驱动值，与位置控制器911，转移函数是c(s)，得出的驱动值相加后输出至驱动器。另外，姿态补偿控制92得出的位置补偿值在随后的位置反馈控制91控制周期中均为有效，直到姿态补偿控制92产生新的位置补偿值进行更新。
182.具体而言，可用齐次转换矩阵t来表示坐标系间的转换关系。在初始化步骤s01中，初始化双球杆姿态测量机构300d的机构参数，包括各个工作双球杆的中心距离函数，及工具端平台与基本端平台上各个球窝(即球轴承)中心的坐标位置。在坐标转换关系建立步骤s02中，控制器可以通过一内建或是载入的设定值建立参考坐标系{r}，或是以双球杆姿态测量机构300d进行机上测量，然后建立参考坐标系{r}，在得出参考坐标系{r}后，控制器可建立基本平台坐标系{b}与参考坐标系{r}的坐标转换关系并在完成姿态补偿控制的初始化后得出一起始参考姿态p
r
，及各马达初始的位置补偿值θ
i，comp
及补偿驱动值，此时工具平台坐标系为{p0}，双球杆姿态测量机构300d量出工具平台坐标系{p0}与基本平台坐标系{b}的坐标转换关系工具端点坐标系{t0}与工具平台坐标系{p0}的坐标转换关系为固定关系，在安装工具后经校正得知，工具端点坐标系{t0}与基本平台坐标系{b}的坐标转换关系因而是而工具端点坐标系{t0}与参考坐标系{r}的坐标转换关系为的坐标转换关系为
183.直接姿态反馈控制机器是三线性轴机器时，于坐标转换关系建立步骤s02中，可分别驱动机器的x轴、y轴及z轴的马达分别做定位测量，双球杆姿态测量机构300d量出多个测量点，拟合测量点分别得出直接姿态反馈控制机器的三实际轴线向量l1、l2及l3，以例如l1为理想向量x，以二向量x及l2做外积，可以建立理想向量z，再以z及x做外积可以建立理想向量y，以此三相互垂直的向量x、y及z可以建立机器的理想轴坐标系，可以此理想轴坐标系为参考坐标系{r}，得出机器实际轴坐标系与参考坐标系{r}间的坐标转换关系，及基本平台坐标系{b}与参考坐标系{r}的坐标转换关系且
184.直接姿态反馈控制机器为六轴机器手臂11d时，于坐标转换关系建立步骤s02中可例如驱动第一转动关节马达做定位测量，以双球杆姿态测量机构300d量出多个测量点，处理测量点得出一圆心及一圆平面的法线向量，理想z轴即是通过该圆心，与该法线向量同方向的轴线，同样再驱动第二转动关节马达做定位测量，可以得出一实际y轴轴线向量，以实际y轴轴线向量与理想z轴轴线向量做外积，得出理想x轴轴线向量，以理想z轴轴线向量及与理想x轴轴线向量做外积，得出理想y轴轴线向量，再由理想z轴轴线及与理想y轴轴线间的公垂线在理想z轴轴线上的交点，及理想y轴轴线与六轴机器手臂11d安装面间的距离(公
称设计值)，可以建立六轴机器手臂11d的参考坐标系{r}，及基本平台坐标系{b}与参考坐标系{r}的坐标转换关系且
185.如图7所示，控制器的插补器222d可包含一路径插补器2221d及一指向插补器2222d，加减速规划器223d对加减速进行规划，依据输入的工具路径及指向，实时计算得出工具的设定姿态p
s
，亦即工具端点的设定位置(x，y，z)及工具指向，例如是三转动角(α，β，γ)，p
s
＝(x，y，z，α，β，γ)
st
，并做机构逆转换计算，得出位置设定值向量θ
s
＝(θ
1，s
，θ
2，s
，θ
3，s
，θ
4，s
，θ
5，s
，θ
6，s
)。当工具是固定在工具端平台上，工具端点坐标系{t0}与工具平台坐标系{p0}的坐标转换关系恒定不变，故实际工具端点坐标系{t
n
}与实际工具平台坐标系{p
n
}的坐标转换关系如式(1)所示，由式(2)可求得实际工具端点坐标系{t
n
}与参考坐标系{r}的坐标转换关系而式(3)及式(4)如下所示：
186.及
[0187][0188]
在式(3)中，实际工具平台坐标系{p
n
}与基本平台坐标系{b}的坐标转换关系是由双球杆姿态测量机构300d测量得出，通过式(3)，可将实际工具端点坐标系{t
n
}中的工具位置及指向转换到参考坐标系{r}，得出实际姿态p
a
，p
a
＝(x，y，z，α，β，γ)
at
。实际工具端点坐标系{t
n
}与基本平台坐标系{b}的坐标转换关系为编码器定义姿态下实际工具端点坐标系{t
n
}与参考坐标系{b}间的坐标转换关系为
[0189]
由设定姿态p
s
与测量得出的实际姿态p
a
可以得出实际姿态误差e
a
，e
a
＝p
s-p
a
＝(δx，δy，δz，δα，δβ，δγ)
at
。由于双球杆姿态测量机构300d的正逆转换计算复杂，加上机器手臂的坐标系间正逆转换计算也需要时间，以实际姿态误差e
a
进行直接姿态反馈控制会有死区时间过大及计算时间不稳定问题。在本发明的直接姿态反馈控制方法的姿态补偿控制92中，将实际姿态误差e
a
拆解，由第i个马达的位置实际值θ
i，enc
减去第i个马达的位置补偿值θ
i，comp
，得出第i个马达的编码器定义姿态位置值θ
i，e
，θ
i，e
＝θ
i，enc-θ
i，comp
，以各个马达的编码器定义姿态位置值θ
i，e
构成的编码器定义姿态位置向量θ
e
＝(θ
1，e
θ
2，e
，θ
3，e
，θ
4，e
，θ
5，e
，θ
6，e
)
t
，以元件号225d的机构正转换器fkt做机构正转换，得出一编码器定义姿态p
e
，p
e
＝(x，y，z，α，β，γ)
et
，这是理想机器手臂所定义的姿态。编码器定义姿态p
e
与实际姿态p
a
间的偏差定义为一连结姿态误差e
l
，e
l
＝p
e-p
a
＝(δx，δy，δz，δα，δβ，δγ)
lt
，在本发明的姿态补偿控制中，由此连结姿态误差el得出各个马达的位置补偿值θ
i，comp
，及位置补偿值向量θ
comp
＝(θ
1，comp
，θ
2，comp
，θ
3|，comp
，θ
4，comp
，θ
5，comp
，θ
6，comp
)
t
。
[0190]
设定姿态p
s
与编码器定义姿态p
e
间之差是定义为追踪姿态误差e
f
，e
f
＝p
s-p
e
＝(δx，δy，δz，δα，δβ，δγ)
ft
。上述的设定姿态p
s
、编码器定义姿态p
e
与实际姿态p
a
可以在参
考坐标系{r}得出，也可以在基本平台坐标系{b}得出。追踪姿态误差e
f
对应到轴坐标系中的轴追踪误差向量δθ
f
，
[0191]
δθ
f
＝θ
s-θ
enc
＝(θ
1，s-θ
1，enc
，θ
2，s-θ
2，enc
，θ
3，s-θ
3，enc
，θ
4，s-θ
4，enc
，θ
5，s-θ
5，enc
，θ
6，s-θ
6，enc
）
t
。
[0192]
本发明的直接姿态反馈控制方法的一主要特征是，对追踪姿态误差e
f
对应的追踪误差向量δθ
f
进行位置反馈控制91，另外对连结姿态误差e
l
进行姿态补偿控制92，如此可以快速实现完整的直接姿态的反馈控制而无死区时间问题。
[0193]
在姿态补偿控制92中，可以依运动情况选择补偿目标姿态，得出各个马达的位置补偿值θ
i，comp
。在各个马达的位置设定值θ
i，s
持续改变的运动过程中，可视马达的编码器定义姿态p
e
为补偿目标姿态，如图7所示，增量位置补偿值向量δθ
comp
，δθ
comp
＝(δθ
1，comp
，δθ
2，comp
，δθ
3，comp
，δ
4，comp
，δθ
5comp
，δθ
6，comp
)
t
，可以例如由连结姿态误差e
l
乘上机器手臂机构转换的雅可比逆矩阵j-1
得出，如式(5)：
[0194]
δθ
comp
＝j-1
×
e
l
ꢀꢀꢀꢀꢀꢀ
(5)。
[0195]
当工具的设定姿态p
s
不再改变，各个马达的位置设定值也不再改变，此时可以设定姿态p
s
为补偿目标姿态，由实际姿态误差e
a
计算得出增量位置补偿值向量δθ
comp
，如式(6)：
[0196]
δθ
comp
＝j-1
×
e
a
ꢀꢀꢀꢀꢀ
(6)。
[0197]
请参阅图8，其示出直接姿态反馈控制方法的另一控制流程图。此时，由元件号224d的机构逆转换器(inverse kinematic transformation，ikt)做机构逆转换计算，由量出的工具实际姿态p
a
计算得出各个马达的等效位置实际值θ
i，a
，等效位置实际值向量为θ
a
，θ
a
＝(θ
1，a
，θ
2，a
，θ
3，a
，θ
4，a
，θ
5，a
，θ
6，a
)
t
，再如上所述选择补偿目标姿态，例如图8中以编码器定义姿态p
e
为补偿目标姿态，先从编码器量出的各个马达的位置实际值θ
i，enc
减去各个马达的有效的位置补偿值θ
i，comp
，得出各马达的编码器定义姿态位置值θ
i，e
，θ
i，e
＝θ
i，enc-θ
i，comp
，再减去各个马达的等效位置实际值θ
i，a
，即得出各个马达新的增量位置补偿值δθ
i，comp
，如式(7)：
[0198]
δθ
i，comp
＝θ
i，enc-θ
i，comp-θ
i，a
，i＝1..6
ꢀꢀꢀꢀꢀꢀ
(7)。
[0199]
在此需特别说明的是，使用式(5)及式(6)时是以一次近似方法得出位置补偿值，也可以迭代多次逼近方法得出较准确的位置补偿值，例如牛顿迭代逼近法，或是例如以实际机构参数建立机器实际机构模型做较准确的位置补偿值计算，在此不再重复叙述。另外，得出各马达的增量位置补偿值δθ
i，comp
是新的增量位置补偿值，因而需要设置存储器存储目前有效旧的位置补偿值，再由新的增量位置补偿值得出更新后的位置补偿值，在图7及图8中，是通过一元件号226d的时间延迟元件td示意出，假设[kt]表示第k个姿态补偿控制周期，[kt-t]表示第k-1个姿态补偿控制周期，各马达的位置补偿值的更新如式(8)所示：
[0200]
θ
i，comp
[kt]＝θ
i，comp
[kt-t]+δθ
i，comp
[kt]
ꢀꢀꢀꢀꢀꢀꢀ
(8)；
[0201]
此更新的位置补偿值θi
，comp
[kt]维持有效直到下次更新。
[0202]
如果直接姿态反馈控制机器是三线性轴机床，此时直接姿态反馈控制的要点是驱动三线性轴补偿工具端点位置的误差，工具的指向误差不能通过驱动三线性轴消除，不过，工具端点位置的误差中已包含有工具指向误差的影响，即所谓的阿贝误差，此阿贝误差可以被直接姿态反馈控制消除。
[0203]
请参阅图7、图8及图10，姿态补偿控制92的补偿控制器921与位置反馈控制91的位置控制器911使用同一控制器，转移函数为c(s)时，可将补偿控制器921与位置控制器911合并，如图10所示。在图7、图8的实施例中，c(s)是比例控制器，增益值是k
v
，此时以马达的位置补偿值θ
i，comp
修正位置设定值θ
i，s
，再减去位置实际值θ
i，enc
后，乘以增益值k
v
，即得出各个马达补偿后的驱动值v
cmd，i
，如式(9)，此驱动值v
cmd，i
是控制器输出给各个马达驱动器的速度指令：
[0204]
v
cmd，i
＝k
v
·
(θ
i，s
+θ
i，comp-θ
i，enc
)
ꢀꢀꢀꢀꢀꢀ
(9)。
[0205]
为避免因为连结姿态误差计算时间过长造成直接姿态的反馈控制回路中死区时间的影响，在一直接姿态反馈控制方法实施例中，由一实时计时器(real-time timer)中断服务程序先呼叫执行位置反馈控制，再呼叫执行姿态补偿控制，姿态补偿控制计算出的各个马达的位置补偿值及补偿驱动值后，保留给下一位置反馈控制周期使用。在另一直接姿态反馈控制方法实施例中，补偿控制器与位置控制器共用一比例控制器，例如图10所示，此时，在第k个位置反馈的控制周期马达的位置设定值θ
i，s
，被前一位置反馈控制周期(亦即k-1个控制周期)中姿态补偿控制计算得出的位置补偿值θ
i，comp
修正，如此，在第k个控制周期中，位置反馈控制送出补偿后的马达驱动值v
cmd，i
[kt]，由于θ
i，comp
[kt-t]是在前一控制周期得出，不造成位置反馈控制的死区时间问题，而驱动值v
cmd，i
[kt]如式(10)所示：
[0206]
v
cmd，i
[kt]＝k
v
·
(θ
i，s
[kt]+θ
i，comp
[kt-t]-θ
i，enc
[kt])
ꢀꢀꢀꢀꢀꢀ
(10)；
[0207]
也可以在一控制周期中，位置反馈控制先得出各马达的驱动值后即做输出，然后姿态补偿控制计算得出各马达的补偿驱动值后，与驱动值相加后做二次输出。
[0208]
请参阅图9，位置反馈控制91有一元件号为911的位置控制器，转移函数为c(s)，姿态补偿控制92有一元件号为921的补偿控制器，转移函数为cc(s)，可以例如是比例-积分-微分pid控制器。此图9中显示姿态补偿控制92使用一独立的补偿控制器921，好处是，可以保留位置反馈控制中原有的其他控制器，例如图9中示意的前馈控制器(feed forward，ff)。在一实施例中，补偿控制器921是比例控制器，增益值为k
vc
，位置控制器911也是比例控制器，增益值为k
v
，此时，补偿控制器921由各个马达的位置补偿值θ
i，comp
乘以增益值k
vc
得出补偿驱动值，与位置控制器911得出的驱动值相加，得出补偿后的驱动值v
cmd，i
，如式(11)所示：
[0209]
v
cmd，i
＝k
v
·
(θ
i，s-θ
i，enc
)+k
vc
·
θ
i，comp
ꢀꢀꢀꢀꢀ
(11)。
[0210]
另外，由于机器的静态及准静态误差例如几何误差及热误差等变化缓慢，在一实施例中，姿态补偿控制的取样时间可以远大于位置反馈控制的取样时间，以节省控制器内中央处理器(cpu)或是数字信号处理器(dsp)的计算负荷，亦即，姿态补偿控制可以不安排在实时位置反馈控制的实时计时器中断服务程序中执行，而安排于另一时间间隔较长的控制周期中执行。一般位置反馈控制的取样时间或是控制周期例如是1ms。
[0211]
工具的实际姿态中包含有直接姿态反馈控制机器所有的静态与动态行为信息，例如工具为刀具时的切削力作用信息、机器轴定位动态信息或是结构振动信息，控制器可以对实际姿态进行例如频谱分析等信号处理分析，找出直接姿态反馈控制机器的结构振动或运动动态问题。在一实施例中，也对位置补偿值进行低通滤波数据处理，得出各个马达无振动变化的位置补偿值。
[0212]
请参阅图11，其中图11示出依照本发明更一实施例的一种直接姿态反馈控制机器
的双球杆姿态测量机构300c的示意图。基本端平台310c在上，工具端平台320c在下，而形成倒立式的双球杆姿态测量机构300c，其中工具端平台320c的球窝p1～p6及基本端平台310c的球窝b1～b6具有不对称分布，基本端平台310c上的球窝b1～b6分布偏向前方半圆范围，工具端平台320c上的球窝p1～p6分布移向两侧，以避免工具端平台320c及工作双球杆330c与机器本体发生干涉，有利工件从正前方或横向进出，配合工具端平台320c的球窝p1～p6及基本端平台310c的球窝b1～b6分布，各工作双球杆330c的最大及最小中心距离与球窝p1～p6、b1～b6的分布相配合。换句话说，各工作双球杆330c具有的最大中心距离及最小中心距离是依照球窝p1～p6、b1～b6的配置而定。
[0213]
专业人士可以做出其他变化而不偏离本发明的精神与要点，例如各个马达位置反馈控制的驱动值中包括前馈或是轨迹误差控制值，双球杆姿态测量机构中工具端平台或基本端平台与机器的整合方式可以有其他变化，工具端平台或基本端平台上的球窝也可以改为球，此时连接的工作双球杆的末端即以球窝相配合，工具端平台或是基本端平台上球窝的分布方式可以依据工作空间需要而变化，工作双球杆长度也可以依之变化，工作双球杆与参考双球杆可以加装防水防尘护套，以避免污染，也可对实际姿态进行频谱分析之外的各类滤波处理，诊断机器的动态行为，算出姿态误差后，也可以针对姿态补偿控制设计特别的补偿控制器。
[0214]
虽然本发明已以实施例公开如上，然其并非用以限定本发明，任何本领域技术人员，在不脱离本发明的精神和范围内，当可作各种的变动与润饰，因此本发明的保护范围当视后附的权利要求所界定者为准。