本发明属于磨抛装置相关技术领域，并公开了一种三自由度磨抛装置及其力位耦合控制方法。该方法包括下列步骤：(a)获取达到预设期望接触正压力的速度控制量；(b)获取达到切平面内预设期望位移偏差的速度控制量；(c)将步骤(a)和(b)中获得的接触正压力和切平面的速度控制量分别解耦至世界坐标系的各个坐标轴方向，以此获得沿各个坐标轴方向的速度总控制量，利用该各个坐标轴方向的总速度控制量对三自由度磨抛装置的控制，使其达到设定的期望正压力和位移偏差，即实现对三自由度磨抛装置的力位耦合控制。通过本发明，提高了磨抛装置的可控性和控制精度，减小加工误差，提高加工精度。
1.一种三自由度磨抛装置的力位耦合控制方法，其特征在于，该方法包括下列步骤：(a)获取达到预设期望接触正压力的速度控制量对于待控制的三自由度磨抛装置的磨头部分，由其三个自由度方向建立坐标系xyz，采集该磨头部分沿x、y和z方向分别受到的力，利用该磨头部分的重力对分别沿x、y和z方向受到的力进行补偿，以此获得磨头部分与被加工工件之间的实际接触正压力；计算该实际接触正压力与期望接触正压力之间的差值，采用PID控制算法结合所述差值计算获得期望接触正压力的速度控制量；
(b)获取达到切平面内预设期望位移偏差的速度控制量将所述磨头部分的位移矢量投影至磨抛位置点处的切平面，以此获得在切平面内的投影位移矢量，利用该投影位移矢量计算其在切平面内的位移偏差，将该位移偏差与预设期望位移偏差相比较获得位移偏差差值，采用PID控制算法结合所述位移偏差差值计算获得在切平面的速度控制量，其中，所述位移矢量包括位移量和该位移量的方向向量；
利用该投影位移矢量计算其在切平面内的位移偏差，按照下列步骤进行：首先，在切平面内建立两个正交的方向，分别为A方向和B方向；
然后，将所述投影位移矢量分解至A方向和B方向，以此获得沿A方向和B方向上的多个分量；
最后，分别计算沿A方向和B方向所有分量的和，以此获得A方向和B方向在切平面内的位移偏差；
(c)将步骤(a)和(b)中获得的接触正压力和切平面的速度控制量分别解耦至各个坐标轴方向，以此获得沿各个坐标轴方向的速度总控制量，利用该各个坐标轴方向的总速度控制量对三自由度磨抛装置的控制，使其达到设定的期望正压力和位移偏差，即实现对三自由度磨抛装置的力位耦合控制。
2.如权利要求1所述的一种三自由度磨抛装置的力位耦合控制方法，其特征在于，在步骤(a)中，所述利用该磨头部分的重力对分别沿x、y和z方向受到的力进行补偿的方法按照下列步骤进行：
采集所述磨头部分在世界坐标系中的姿态；计算磨头部分重力在所述坐标系xyz中沿各个坐标轴方向的补偿量；利用该补偿量计算补偿后的磨头部分受到的实际接触正压力；
所述实际接触正压力按照下列表达式进行：Fc＝Fx3+Fy3+Fz3
[Fx2 Fy2 Fz2]＝[0 0 G0]T其中，Fx3是Fx在接触正压力Fc方向上的分量，Fy3是Fy在接触正压力Fc方向上的分量，Fz3是Fz在接触正压力Fc方向上的分量，Fx，Fy和Fz分别是磨头部分的重力进行补偿后磨头部分x、y和z方向受到的力，Fx1，Fy1和Fz1分别是补偿前磨头部分x、y和z方向受到的力，Fx2、Fy2和Fz2分别是磨头部分重力沿x、y及z方向的分量，αx是Fx与Fc之间的夹角，αy是Fy与Fc之间的夹角,αz是Fz与Fc之间的夹角，T为从世界坐标系运动到坐标系xyz的旋转矩阵。
3.如权利要求1所述的一种三自由度磨抛装置的力位耦合控制方法，其特征在于，在步骤(a)中，所述期望接触正压力的速度控制量按照下列表达式进行：e(t)＝Fe‑Fc
其中，u1(t)是磨头部分在接触力方向的速度控制量，Kp1是比例系数，Ki1是积分系数，Kd1是微分项系数，e(t)是力误差，Fe是期望接触正压力，Fc是实际接触正压力，t是时间。
4.如权利要求2所述的一种三自由度磨抛装置的力位耦合控制方法，其特征在于，在步骤(b)中，所述投影位移矢量包括投影位移量和投影位移方向向量，投影位移量按照下列表达式计算获得：
其中，px、py和pz分别是磨头部分沿x、y及z向的位移量，px1、py1和pz1分别是px、py和pz在切平面内的投影值；
投影位移方向向量按照下列表达是计算获得：Px1＝nx‑ux
其中，ux、uy和uz分别是nx、ny和nz在mc上的投影向量，nx、ny和nz分别是x、y及z向的方向向量，mc是接触正压力Fc的方向向量，Px1、Py1和Pz1分别是nx、ny和nz在切平面内的投影向量。
5.如权利要求1所述的一种三自由度磨抛装置的力位耦合控制方法，其特征在于，在步骤(b)中，所述切平面的速度控制量按照下列方式进行：A方向的速度控制量按照下列表达式进行：u2(t)是切平面内A方向的速度控制量，Kp2是比例系数，Ki2是积分系数，Kd2微分项系数，p1(t)是切平面内A方向的位置误差，εzx是投影位移方向的向量Pz1与Px1之间的夹角，εxy是投影方向向量Px1与Py1之间的夹角；
B方向的速度控制量按照下列表达式进行：其中，u3(t)为切平面内B方向的速度控制量，Kp3是比例系数，Ki3为积分系数，Kd3为微分项系数，p2(t)为切平面内B方向的位置误差，t是时间。
6.如权利要求1所述的一种三自由度磨抛装置的力位耦合控制方法，其特征在于，在步骤(c)中，所述接触正压力和切平面的速度控制量分别解耦至世界坐标系的各个坐标轴方向，按照下列方式进行：
接触正压力速度控制量解耦至世界坐标系的各个坐标轴方向按照下列表达式进行：u1x(t)＝u1(t)cosαxu1y(t)＝u1(t)cosαyu1z(t)＝u1(t)cosαz其中，u1(t)为磨头部分在接触力方向的速度控制量，u1x(t)、u1y(t)和u1z(t)分别是u1(t)在x、y及z方向的分量；
切平面的速度控制量解耦x、y及z方向按照下列表达式进行：u2x(t)＝0
u3x(t)＝0.5u3(t)/sinαxu2(t)是切平面内A向的速度控制量，u2x、u2y和u2z分别是u2(t)在x、y及z方向的分量，ε1、ε2和ε3是中间变量，ε1＝εyz， u3(t)为切平面内B向的位置控制量，u3x、u3y和u3z是u3(t)在x、y及z方向的分量，ε4、ε5和ε6是中间变量，ε4＝π‑ε5‑ε6，ε5＝π‑εxy，ε6＝π‑εzx。
7.如权利要求6所述的一种三自由度磨抛装置的力位耦合控制方法，其特征在于，在步骤(c)中，所述各个坐标轴方向的总速度控制量按照下列表达式进行：ux＝u1x(t)+u2x(t)+u3x(t)uy＝u1y(t)+u2y(t)+u3y(t)uz＝u1z(t)+u2z(t)+u3z(t)其中，ux、uy和uz分别是在x、y及z方向的速度总控制量。
8.一种应用权利要求1‑7任一项所述的力位耦合控制方法的三自由度磨抛装置，其特征在于，该装置包括安装法兰、磨头部分、三自由度伺服平台和传感器，其中：所述安装法兰用于与机械手连接，所述磨头部分与待打磨工件接触，用于打磨待打磨工件，所述三自由度伺服平台与所述磨头部分连接，用于控制所述磨头部分沿x、y及z轴方向运动，所述传感器设置在所述磨头部分的下方，用于测量该磨头部分与待打磨接触受到的力。
一种三自由度磨抛装置及其力位耦合控制方法
技术领域
[0001] 本发明属于磨抛装置相关技术领域，更具体地，涉及一种三自由度磨抛装置及其力位耦合控制方法。
背景技术
[0002] 随着科学技术的发展，复杂曲面在航空航天、汽车、船舶等领域的应用日益广泛。复杂曲面零件的表面粗糙度直接影响其服役的气动性能、效率和疲劳寿命。因此，对复杂曲
面零件进行表面精加工提高其表面质量，对于提高复杂曲面零件的服役性能具有重要影
响。
[0003] 目前自由曲面的表面精加工主要有数控机床抛光技术和人工手工研磨的方式。其中，数控抛光机床价格昂贵、灵活性差，人工抛光存在劳动强度大，加工效率低，工作环境粉
尘较大，严重影响工人身体健康，同时磨抛精度受工人技术熟练程度影响。与传统加工方式
相比，机器人系统有着灵活性好、通用性强、易于拓展等优点，因此研究机器人夹持末端执
行器的磨抛系统及方法很有必要。
[0004] 在力控磨抛方法和系统中需实现接触力的控制，主要包括单自由度接触力控制、二自由度接触力控制和三自由度接触力控制，单自由度的接触力控制方法存在耗时长和容
易过抛的问题，二自由度接触力控制方法只能在一个面内控制接触力，无法在控制三维空
间中的接触力。另外目前的接触力控制方法中通常将磨抛过程中的摩擦系数考虑为恒定系
数，这样会对接触力的控制带来一定的偏差。三自由度磨抛装置及其接触力控制方法可以
解决上述问题，但目前对于如何实现三自由度磨抛装置的设计及其接触力的有效控制，目
前的研究较少。为了设计出三自由度磨抛装置，实现三自由度接触力的有效的控制，有必要
进行进一步的研究。
发明内容
[0005] 针对现有技术的以上缺陷或改进需求，本发明提供了一种三自由度磨抛装置及其力位耦合控制方法，其通过采用位移和接触力两个方面的速度控制，使得磨抛装置对达到
预设期望接触力和预设位移偏差，提高了磨抛装置的可控性和控制精度，减小加工误差，提
高加工精度。
[0006] 为实现上述目的，按照本发明的一个方面，提供了一种三自由度磨抛装置的力位耦合控制方法，该方法包括下列步骤：
[0007] (a)获取达到预设期望接触正压力的速度控制量
[0008] 对于待控制的三自由度磨抛装置的磨头部分，由其三个自由度方向建立坐标系xyz，采集该磨头部分沿x、y和z方向分别受到的力，利用该磨头部分的重力对分别沿x、y和z
方向受到的力进行补偿，以此获得磨头部分与被加工工件之间的实际接触正压力；计算该
实际接触正压力与期望接触正压力之间的差值，采用PID控制算法结合所述差值计算获得
期望接触正压力的速度控制量；
[0009] (b)获取达到切平面内预设期望位移偏差的速度控制量
[0010] 将所述磨头部分的位移矢量投影至磨抛位置点处的切平面，以此获得在切平面内的投影位移矢量，利用该投影位移矢量计算其在切平面内的位移偏差，将该位移偏差与预
设期望位移偏差相比较获得位移偏差差值，采用PID控制算法结合所述位移偏差差值计算
获得在切平面的速度控制量，其中，所述位移矢量包括位移量和该位移量的方向向量；
[0011] (c)将步骤(a)和(b)中获得的接触正压力和切平面的速度控制量分别解耦至世界坐标系的各个坐标轴方向，以此获得沿各个坐标轴方向的速度总控制量，利用该各个坐标
轴方向的总速度控制量对三自由度磨抛装置的控制，使其达到设定的期望正压力和位移偏
差，即实现对三自由度磨抛装置的力位耦合控制。
[0012] 进一步优选地，在步骤(a)中，所述利用该磨头部分的重力对分别沿x、y和z方向受到的力进行补偿的方法按照下列步骤进行：
[0013] 采集所述磨头部分在世界坐标系中的姿态；计算磨头部分重力在所述坐标系xyz中沿各个坐标轴方向的补偿量；利用该补偿量计算补偿后的磨头部分受到的实际接触正压
力；
[0014] 所述实际接触正压力按照下列表达式进行：
[0015] Fc＝Fx3+Fy3+Fz3
[0016]
[0017] [Fx2 Fy2 Fz2]＝[0 0 G0]T
[0018] 其中，Fx3是Fx在接触正压力Fc方向上的分量，Fy3是Fy在接触正压力Fc方向上的分量，Fz3是Fz在接触正压力Fc方向上的分量，Fx，Fy和Fz分别是磨头部分的重力进行补偿后磨
头部分x、y及z方向受到的力，Fx1，Fy1和Fz1分别是补偿前磨头部分x、y及z方向受到的力，Fx2、
Fy2和Fz2分别是磨头部分重力沿x、y及z方向的分量，αx是Fx与Fc之间的夹角，αy是Fy与Fc之间
的夹角,αz是Fz与Fc之间的夹角，T为从世界坐标系运动到力传感器坐标系的旋转矩阵。
[0019] 进一步优选地，在步骤(a)中，所述期望接触正压力的速度控制量按照下列表达式进行：
[0020]
[0021] e(t)＝Fe‑Fc
[0022] 其中，u1(t)是磨头部分在接触力方向的速度控制量，Kp1是比例系数，Ki1是积分系数，Kd1是微分项系数，e(t)是力误差，Fe是期望接触正压力，Fc是实际接触正压力，t是时间。
[0023] 进一步优选地，在步骤(b)中，所述投影位移矢量包括投影位移量和投影位移方向向量，投影位移量按照下列表达式计算获得：
[0024] px1＝pxsinαx
[0025] py1＝pysinαy
[0026] pz1＝pzsinαz
[0027] 其中，px、py和pz分别是磨头部分沿x、y及z向的位移量，px1、py1和pz1分别是px、py和pz在切平面内的投影值；
[0028] 投影位移方向向量按照下列表达是计算获得：
[0029] Px1＝nx‑ux
[0030] Py1＝ny‑uy
[0031] Pz1＝nz‑uz
[0032]
[0033]
[0034]
[0035] 其中，ux、uy和uz分别是nx、ny和nz在mc上的投影向量，nx、ny和nz分别是x、y及z向的方向向量，mc是接触正压力Fc的方向向量，Px1、Py1和Pz1分别是nx、ny和nz在切平面内的投影向
量。
[0036] 进一步优选地，在步骤(b)中，利用该投影位移矢量计算其在切平面内的位移偏差，按照下列步骤进行：
[0037] 首先，在切平面内建立两个不同的方向，A方向和B方向；
[0038] 然后，将所述投影位移矢量分解至A方向和B方向，以此获得沿A方向和B方向上的多个分量；
[0039] 最后，分别计算沿A方向和B方向所有分量的和，以此获得A方向和B方向在切平面内的位移偏差。
[0040] 进一步优选地，所述A方向和B方向为两个正交的方向。
[0041] 进一步优选地，在步骤(b)中，所述切平面的速度控制量按照下列方式进行：
[0042] A方向的速度控制量按照下列表达式进行：
[0043]
[0044]
[0045] u2(t)是切平面内A方向的速度控制量，Kp2是比例系数，Ki2是积分系数，Kd2微分项系数，p1(t)是切平面内A方向的位置误差，εzx是投影位移方向的向量Pz1与Px1之间的夹角，
εxy是投影方向向量Px1与Py1之间的夹角；
[0046] B方向的速度控制量按照下列表达式进行：
[0047]
[0048]
[0049]
[0050]
[0051]
[0052] 其中，u3(t)为切平面内B方向的速度控制量，Kp3是比例系数，Ki3为积分系数，Kd3为微分项系数，p2(t)为切平面内B方向的位置误差，t是时间，。
[0053] 进一步优选地，在步骤(c)中，所述接触正压力和切平面的速度控制量分别解耦至世界坐标系的各个坐标轴方向，按照下列方式进行：
[0054] 接触正压力速度控制量解耦至世界坐标系的各个坐标轴方向按照下列表达式进行：
[0055] u1x(t)＝u1(t)cosαx
[0056] u1y(t)＝u1(t)cosαy
[0057] u1z(t)＝u1(t)cosαz
[0058] 其中，u1(t)为磨头部分在接触力方向的速度控制量，u1x(t)、u1y(t)和u1z(t)分别是u1(t)x、y及z方向的分量；
[0059] 切平面的速度控制量解耦至x、y及z方向：
[0060] u2x(t)＝0
[0061]
[0062]
[0063] u3x(t)＝0.5u3(t)/sinαx
[0064]
[0065]
[0066] u2(t)是切平面内A向的速度控制量，u2x、u2y和u2z分别是u2(t)在x、y及z方向的分量，ε1、ε2和ε3是中间变量，ε1＝εyz， u3(t)为切平面内B向的位置控
制量，u3x、u3y和u3z是u3(t)在x、y及z方向的分量，ε4、ε5和ε6是中间变量，ε4＝π‑ε5‑ε6，ε5＝π‑
εxy，ε6＝π‑εzx。
[0067] 进一步优选地，在步骤(c)中，所述在x、y及z方向的总速度控制量按照下列表达式进行：
[0068] ux＝u1x(t)+u2x(t)+u3x(t)
[0069] uy＝u1y(t)+u2y(t)+u3y(t)
[0070] uz＝u1z(t)+u2z(t)+u3z(t)
[0071] 其中，ux、uy和uz分别是在x、y及z方向的速度总控制量。
[0072] 按照本发明的另一个方面，提供了一种上述所述的一种三自由度磨抛装置，该装置包括安装法兰、磨头部分、三自由度伺服平台和传感器，其中：
[0073] 所述安装法兰用于与机械手连接，所述磨头部分与待打磨工件接触，用于打磨待打磨工件，所述三自由度伺服平台与所述磨头部分连接，用于控制所述磨头部分沿x、y及z
轴方向运动，所述传感器设置在所述磨头部分的下方，用于测量该磨头部分与待打磨接触
受到的力。
[0074] 总体而言，通过本发明所构思的以上技术方案与现有技术相比，具备下列有益效果：
[0075] 1.本发明可实现三自由度磨抛接触力的实时准确控制，使得磨抛接触力始终等于期望接触力，并实现了三维空间中接触力的控制，保证了曲面类零件的精准材料去除，降低
零件因过磨导致报废的概率，提高了曲面类零件的加工效率；
[0076] 2.本发明通过直接利用各轴补偿后的力信息在接触正压力方向的分量来计算接触正压力，并通过将接触正压力与期望接触力进行比较得到力误差，进而根据力误差计算
出接触力方向的速度控制量；
[0077] 3.本发明通过与机器人通信来获得位姿信息，进而通过矩阵变化来计算出重力在各方向的分量，实现对重力的补偿，无需额外的倾角传感器等传感器。
附图说明
[0078] 图1是按照本发明的优选实施例所构建的三自由度磨抛装置及其力位耦合控制方法的流程图；
[0079] 图2是按照本发明的优选实施例所构建的三自由度磨抛装置的结构示意图；
[0080] 图3是按照本发明的优选实施例所构建的三自由度力控装置接触零件过程中的受力示意图；
[0081] 图4是按照本发明的优选实施例所构建的投影向量Px1、Py1和Pz1示意图；
[0082] 图5是按照本发明的优选实施例所构建的切平面内位置的控制量在A、B两个方向上的分解示意图，其中，(a)是A方向控制量分解示意图，(b)是B方向控制量分解示意图。
[0083] 在所有附图中，相同的附图标记用来表示相同的元件或结构，其中：
[0084] 1‑安装法兰，2‑x向移动平台，3‑y向移动平台，4‑z向移动平台，5‑L形连接件，6‑三轴力传感器固定连接板，7‑三轴力传感器，8‑电主轴抱夹连接件，9‑电主轴抱夹，10‑电主
轴，11‑磨头。
具体实施方式
[0085] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并
不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要
彼此之间未构成冲突就可以相互组合。
[0086] 如图2所示，一种三自由度力控磨抛装置，其包括三自由度伺服运动平台模块，磨头部分，三轴力传感器及其固定连接板，安装法兰1，控制器。图2中的世界坐标系是大写的
XYZ，磨头部分三自由度方向的坐标系为小写的xyz。
[0087] 三自由度伺服运动平台模块用于带动磨头部分在空间中运动，以实现磨头部分与工件接触并达到期望的接触力；磨头部分用于直接与工件接触，同时提供磨抛转速对工件
进行打磨；三自由度力传感器用于实时测量接触力信号，该三轴力传感器固定连接板6用于
将三轴力传感器7固定在三自由度伺服运动平台模块上；安装法兰1用于将三自由度伺服运
动平台模块安装在机器人末端，以实现大范围的运动；控制器用于采集接触力信号，通过
TCP/IP通信协议与机器人控制系统进行通信获得机器人的位置和姿态信息，同时通过三自
由度接触力控制方法计算出控制量，该控制器还用于将控制信号发送给三自由度伺服运动
平台模块，进而控制其运动。
[0088] 进一步地，三自由度伺服运动平台模块由x向移动平台2，y向移动平台3，z向移动平台组成4，x向移动平台2与y向移动平台3通过螺栓连接，z向移动平台通过L形连接件5与y
向移动平台连接，且三自由度伺服运动平台模块可分别沿着x向、y向和z向直线运动，进而
可在三维空间中进行运动。
[0089] 进一步地，x向移动平台，y向移动平台，z向移动平台之间两两正交。
[0090] 进一步地，x向移动平台，y向移动平台，z向移动平台结构相同，均由移动平台底座、移动平台滑块、滚珠丝杆、联轴器、伺服电机连接件和伺服电机组成；
[0091] 进一步地，磨头部分由电主轴及其控制组件、电主轴抱夹9、电主轴抱夹连接件8、电主轴10和磨头11组成。
[0092] 如图1所示，一种三自由度磨抛装置的力位耦合控制方法，该方法包括的步骤如下：
[0093] S1采集磨头部分在世界坐标系的姿态信息以及磨头部分沿三个坐标轴方向受到的力，分别为x向力Fx1、y向力Fy1及z向力Fz1；
[0094] S2根据姿态信息对x向力、y向力及z向力进行重力补偿，即减去磨头部分的重力在x、y及z向的分力，获得补偿后的x向力Fx.，y向力Fy，及z向力Fz，根据Fx、Fy和Fz计算得到磨头
部分与工件接触的正压力Fc；
[0095] 重力补偿具体采用如下方式进行：
[0096] 如图3所示，计算三自由度力控装置x向y向及z向的补偿量：
[0097] [Fx2 Fy2 Fz2]＝[0 0 G0]T
[0098] 其中，Fx2为磨头部分x向的补偿量，Fy2为磨头部分y向的补偿量，Fz2为磨头部分z向的补偿量，G0为磨头部分的重力，[0 0 G0]为重力在世界坐标系下的向量表示；T为从世界坐
标系运动到力传感器坐标的旋转矩阵。
[0099] (2)计算补偿后的x向力信息Fx，y向力信息Fy，z向力信息Fz：
[0100] Fx＝Fx1‑Fx2
[0101] Fy＝Fy1‑Fy2
[0102] Fz＝Fz1‑Fz2
[0103] 作为进一步优选的，接触正压力Fc采用如下公式计算：
[0104] Fc＝Fx3+Fy3+Fz3
[0105] 其中Fx3为Fx在接触正压力Fc方向上的分量，且Fx3＝Fxcosαx，Fy3为Fy在接触正压力Fc方向上的分量，且Fy3＝Fycosαy，Fz3为Fz在接触正压力Fc方向上的分量，且Fz3＝Fzcosαz。αx
为Fx与Fc之间的夹角，αy为Fy与Fc之间的夹角,αz为Fz与Fc之间的夹角。
[0106] αx、αy和αz具体采用下列公式计算：
[0107]
[0108]
[0109]
[0110] 其中nx为磨头部分x向的方向向量，ny为磨头部分y向的方向向量，nz为磨头部分z向的方向向量,mc为接触正压力Fc方向的方向向量。
[0111] nx、ny和nz通过矩阵变化来计算
[0112] nx＝nx1T
[0113] ny＝ny1T
[0114] nz＝nz1T
[0115] 其中nx1为机器人处于初始位置时磨头部分x向的方向向量，ny1为机器人处于初始位置时磨头部分y向的方向向量，nz1为机器人处于初始位置时磨头部分z向的方向向量，
nx1、ny1和nz1为已知量；T为从世界坐标系运动到力传感器坐标的旋转矩阵。
[0116] 另外，Fc方向的方向向量mc与工件曲面上各点的法向量相同，通过工件曲面信息获得。
[0117] S3将接触正压力Fc与预设的期望接触力进行比较得到力误差，根据力误差计算出磨抛装置在接触正压力方向上的速度控制量u1(t)；
[0118] 具体采用如下公式计算出磨头部分在接触力方向的速度控制量：
[0119]
[0120] 其中，u1(t)为磨头部分在接触力方向的速度控制量，Kp1为比例系数，Ki1为积分系数，Kd1微分项系数，e(t)为力误差，e(t)＝Fe‑Fc，其中Fe为期望接触正压力，Fc为实际接触正
压力，t为时间。
[0121] S4将磨头部分x方向的位移量px投影到磨抛位置点处切平面得到投影位移量px1，该切平面即为与接触正压力垂直的平面，将磨头部分y方向的位移量py投影到切平面得到
投影位移量py1，将磨头部分z方向的位移量pz投影到切平面得到投影位移量pz1；同时将x向
方向向量(磨头部分x方向的位移的方向)投影到切平面得到投影向量Px1，将y向方向向量投
影到切平面得到投影向量Py1,将z向方向向量投影到切平面得到投影向量Pz1；进而计算出
Px1与Py1之间的夹角εxy，Py1与Pz1之间的夹角εyz，Pz1与Px1之间的夹角εzx。
[0122] 根据下式计算出磨头部分x向位移、y向位移、z向位移在切平面内的分量：
[0123] px1＝pxsinαx
[0124] py1＝pysinαy
[0125] pz1＝pzsinαz
[0126] 其中px为磨头部分x向的位移量，py为磨头部分y向的位移量,pz为磨头部分z向的位移量。px、py、pz可通过伺服电机编码器采集。px1为px在切平面内的投影值，py1为py在切平
面内的投影值，pz1为pz在切平面内的投影值；αx为Fx与Fc之间的夹角，αy为Fy与Fc之间的夹
角,αz为Fz与Fc之间的夹角。
[0127] S5将各个方向的投影位置量分解到两个相交的方向A方向和B方向上，计算出A方向做差后的的位移误差，并根据A方向位置误差计算出磨头部分在A方向的速度控制量分量
u2(t)；计算出B方向做差后的位移误差，并根据B方向位置误差计算出磨头部分在B方向的
速度控制量分量u3(t)；
[0128] 如图4所示，将切平面内的位置控制问题考虑为切平面内A方向上的位置控制和B方向上的位置控制。为简化计算将切平面内A方向和B方向考虑为两个相互垂直的方向，比
较εxy、εyz和εzx，将三者中最小的角所对应的边作为B方向；在切平面内，将与B垂直的方向作
为A方向。
[0129] 根据下式计算Px1与Py1之间的夹角εxy，Py1与Pz1之间的夹角εyz，Pz1与Px1之间的夹角εzx。
[0130]
[0131]
[0132]
[0133] 其中Px1为x向方向向量ux投影到切平面得到的投影向量，Py1为y向方向向量uy投影到切平面得到的投影向量，Pz1为z向方向向量uz投影到切平面得到的投影向量。
[0134] 根据下式计算Px1、Py1和Pz1：
[0135] Px1＝nx‑ux
[0136] Py1＝ny‑uy
[0137] Pz1＝nz‑uz
[0138] 其中ux为nx在mc上的投影向量，uy为ny在mc上的投影向量，uz为nz在mc上的投影向量，作为进一步优选的ux,uy,uz通过下式计算：
[0139]
[0140]
[0141]
[0142] 具体采用如下公式计算出磨头部分在切平面上的速度控制量：
[0143]
[0144] 其中，u2(t)为切平面内A方向的速度控制量，Kp2为比例系数，Ki2为积分系数，Kd2微分项系数，p1(t)为切平面内A方向的位置误差， t
为时间εxy为Px1与Py1之间的夹角，εzx为Pz1与Px1之间的夹角。
[0145]
[0146] 其中，u3(t)为切平面内B方向的速度控制量，Kp3为比例系数，Ki3为积分系数，Kd3为微分项系数，p2(t)为切平面内B方向的位置误差，
t为时间，εxy为Px1与Py1之间的夹角，εzx为Pz1与Px1之间的夹角。
[0147] S6如图5中(a)所示，将接触力方向的速度控制量u1(t)解耦至x向、y向和z向上；将切平面内A方向位置的速度控制量u2(t)解耦至x向、y向和z向上；如图5中(b)所示，将切平
面内B方向位置的速度控制量u3(t)解耦至x向、y向和z向上；并计算出x向总的速度控制量
ux，y向总的速度控制量uy和z向总的速度控制量uz；
[0148] 将接触力速度控制量u1(t)解耦到磨头部分的x向、y向和z向上；
[0149] u1x(t)＝u1(t)cosαx
[0150] u1y(t)＝u1(t)cosαy
[0151] u1z(t)＝u1(t)cosαz
[0152] 其中，u1(t)为磨头部分在接触力方向的速度控制量，αx为Fx与Fc之间的夹角，αy为Fy与Fc之间的夹角,αz为Fz与Fc之间的夹角，u1x(t)为u1(t)在磨头部分x向的分量，u1y(t)为u1
(t)在磨头部分y向的分量，u1z(t)为u1(t)在磨头部分z向的分量。
[0153] 将切平面内两个方向上的速度控制量u2x(t)和u3(t)分别解耦到磨头部分的x向、y向和z向上，根据定正弦理可得。
[0154] u2x(t)＝0
[0155]
[0156]
[0157] u3x(t)＝0.5u3(t)/sinαx
[0158]
[0159]
[0160] u2(t)是切平面内A向的速度控制量，u2x、u2y和u2z分别是u2(t)在x、y及z方向的分量，ε1、ε2和ε3是中间变量，ε1＝εyz， u3(t)为切平面内B向的位置控
制量，u3x、u3y和u3z是u3(t)在x、y及z方向的分量，ε4、ε5和ε6是中间变量，ε4＝π‑ε5‑ε6，ε5＝π‑
εxy，ε6＝π‑εzx。
[0161] (14)作为进一步优选的，通过下式计算磨头部分各个方向的控制量。
[0162] ux＝u1x(t)+u2x(t)+u3x(t)
[0163] uy＝u1y(t)+u2y(t)+u3y(t)
[0164] uz＝u1z(t)+u2z(t)+u3z(t)
[0165] 其中，ux、uy和uz分别是在x、y及z方向的速度总控制量。
[0166] S7利用三自由度力控装置x、y及z向上总的速度控制量实现三自由度力控装置x、y及z向的运动控制，使得磨头部分与工件的实际接触力达到期望接触力，使得磨头部分与工
件在切平面内的接触位置达到期望的接触位置。
[0167] 本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含
在本发明的保护范围之内。
