CN112859942A - 一种陀螺框架伺服***高精度位置辨识方法 - Google Patents

Abstract

本发明提出了一种陀螺框架伺服***高精度位置辨识方法，包括四路线性霍尔传感器、信号处理模块、信号采集模块、信号提取算法、框架伺服控制***和框架伺服电机。线性霍尔传感器检测电机转子端部的磁场变化，将其转换为电压信号；电压信号经信号处理模块处理之后，信号采集模块通过AD转换芯片将其采集为数字信号；信号提取算法抑制数字信号中的幅值误差和高次谐波，得到理想的正余弦信号，实现高精度角位置解算；高精度的角位置被送入到框架伺服控制***，以获得精确的驱动电压实现框架伺服电机的精确控制。本发明能够实现基于线性霍尔传感器的高精度角位置解算，有益于整个陀螺框架伺服***的平稳、高精度运转。

Description

一种陀螺框架伺服***高精度位置辨识方法

技术领域

本发明属于基于线性霍尔传感器，针对框架伺服电机高精度低速伺服控制领域，具体涉及一种陀螺框架伺服***高精度位置辨识方法。

背景技术

控制力矩陀螺具有输出力矩大、精度高，无需消耗工质，寿命长的优点，因此被广泛应用于空间站，空间望远镜以及超敏捷机动卫星等大型航天器的姿态控制领域。控制力矩陀螺由高速转子***和框架***两部分组成，其基本工作原理为：当高速转子高速旋转时，框架***改变角动量的方向从而实现力矩的输出，控制航天器姿态。由于陀螺的角动量大小是恒定的，航天器姿态控制精度与陀螺输出力矩精度均由框架***角速度的精度决定。

为了实现陀螺框架***高精度速度伺服控制，需要通过高精度传感器实时检测转子角位置，然而由于大力矩陀螺需要加装谐波减速器等传动组件以提高框架伺服电机的输出力矩，使得负载端的旋转变压器无法为另外一端的电机提供精确角位置信息。因此，需要在电机端重新安装位置传感器，而由于航天器对于体积和重量的严格要求，使得体积小、可靠性强、灵敏度高的线性霍尔传感器成为了控制力矩陀螺电机端传感器的最佳选择。本发明将四路霍尔传感器安装在框架伺服电机转子的末端，通过对检测到的正余弦信号进行解算实现位置的获取。但是，由于转子永磁体性能不理想、传感器安装误差和差分放大器的零位漂移等因素的影响，使得信号采集模块采集到的正余弦信号具有高次谐波和振幅误差，从而使得最后解算得到的转子位置存在偏差，难以满足控制力矩陀螺框架***对于转子位置精度的要求。为了保证通过霍尔传感器获取角位置的精度，中国专利号为201410289383.9的“一种基于线性霍尔的永磁同步电机转子位置误差校正方法”的专利提出了一种通过查表的方法对正余弦信号进行校正，实现角位置精度的提高。但是框架***的转速较低(最大转速仅为15°/s)，需要更为详细的误差校正表，这需要极大的存储区域，会给框架伺服控制***带来很大的负担。中国专利号为202010452437.4的“基于线性霍尔传感器的永磁同步电机角度检测方法和***”的专利并没有考虑电机定子绕组电磁干扰、传感器安装误差和信号处理电路漂移等因素造成的位置解算误差，所以上述方法并不适用于框架***这种超低速和高精度兼具的框架伺服控制***。

结合现有关于线性霍尔传感器在低速伺服控制中的应用现状，发现它们并没有对传感器输出信号畸变导致的角位置解算误差进行具体有效的分析，而这一因素对框架***等高精度伺服控制电机的运行影响较大。由于此类框架伺服控制***中电机转频很低，所以常规使用的正交锁相环和自适应陷波器等信号处理方法难以应用在该领域。为此本发明提出一种可以用于超低速伺服***高精度信号提取算法，该算法利用信号归一化和信号重构来抑制信号中的振幅误差和高次谐波，利用离散ESO通过迭代的方法来抑制重构信号中的残余误差。本发明可以有效地抑制振幅误差和抑制谐波及其他干扰，为超低速框架伺服***提供一个精确的角位置，同时其结构简单有利于工程应用。

发明内容

本发明要解决的技术问题是：克服霍尔传感器输出正余弦信号振幅误差和高次谐波导致的转子位置解算不准确的问题，提出一种基于霍尔传感器的陀螺框架伺服***高精度位置辨识方法，该方法在将两路原始信号进行归一化处理抑制振幅误差的基础上给出一种信号重构的方法抑制信号中所含有的高次谐波，最后通过离散ESO抑制重构后的正余弦信号中仍然含有的扰动信息，从而得到理想的角位置信息，保证陀螺输出力矩的精度要求。

本发明解决上述问题所采取的技术方案为：一种基于霍尔传感器的陀螺框架伺服***高精度位置辨识方法。首先，将四路线性霍尔传感器固定在支架的卡槽中；其次，将支架通过设计好的固定槽安装在电机壳端部；然后，传感器的输出信号通过屏蔽线送至信号处理模块，该信号经过滤波、差分和偏置后接入到信号采集模块；最后，采集后的信号通过信号提取算法(SEM)处理得到振幅误差和谐波被抑制的正余弦信号，对正余弦信号求解得到精确的角位置。得到的角位置信息进入框架伺服控制***之后，结合适当的电机控制算法得到驱动电压，并将该电压送入到框架伺服电机中，实现电机的精确控制。

其中，用于对磁场进行检测的霍尔传感器被分别固定在支架上的四个卡槽内，卡槽之中设计有用于定子绕组布线的凹槽，装配完成之后，安装在支架上的霍尔传感器会处于贴近转子末端压盖的凹槽处，为了能够更好的检测到永磁体磁场应该将其尽可能贴近转子永磁体。本发明所用的四路线性霍尔传感器按照一定角度排布，每对霍尔传感器相差90°或其倍数的电角度，其对应的机械角度计算式如下：

式中θ_m表示霍尔传感器错开的机械角度，p表示极对数，k_h＝1,2,3,4，表示第k_h个霍尔传感器，当极对数为8时，则设置每对霍尔传感器之间的机械角度分别为11.25°或它的整数倍，每对霍尔传感器分别测量永磁体转子的一个N极和一个S极，将四路霍尔信号通过信号处理和采集后使用信号提取算法进行处理；信号提取算法主要包含信号归一化处理，信号重构和离散扩张状态观测器，其具体实现过程为：

步骤1：两路成对的霍尔传感器检测到磁场变化后输出包含k时刻转子角位置信息的正余弦信号：

其中，H_s(k)和H_c(k)表示霍尔传感器输出的正弦和余弦信号，α和β表示信号的振幅，A_i和B_i(i＝3,5,7,9)为谐波的系数，sin(nθ(k))和cos(nθ(k))(n＝3,5,7,9)表示n阶谐波；

步骤2：由于输入到SEM的信号存在振幅误差和大量谐波，为了更好的抑制谐波的影响，SEM算法需要先对原始输入信号进行归一化处理用以抑制振幅误差，具体方法如下式：

式中，H_s(k)和H_c(k)分别为输入到SEM模块的两路正余弦信号，θ_d(k)表示原始的位置信息，H_{s_1}(k)和H_{c_1}(k)分别表示归一化后的正弦信号和余弦信号；经过归一化处理之后的正余弦信号可以改写为：

其中，正余弦信号基频的幅值为1，C_i(i＝3,5,7,9)表示归一化之后谐波信号的幅值，该系数通过参数辨识方法获得；

步骤3：基于上述归一化处理后的信号进行信号重构，可以得到两路重构后的信号。对于正弦信号H_{s_1}(k)，

首先将H_{s_1}(k)与f(·)做差获得误差e(k)，然后使用整定参数γ对误差e(k)进行整定得到γe(k)，最后从H_{s_1}(k)中减去γe(k)和f₁(·)得到重构之后的正弦信号H_{s_2}(k)，其中f(·)和f₁(·)表示自变量为sin(θ)的函数，利用多倍角关系式，高次谐波可以有如下变换：

sin(nθ(k))＝f(sin(θ(k)))

其中，n为谐波的阶数。基于此函数，f(·)和f₁(·)可以表示为：

f(·)＝H_{s_2}(k)+C₃[3H_{s_2}(k)-4H_{s_2}(k)³]+C₅[5H_{s_2}(k)-20H_{s_2}(k)³+16H_{s_2}(k)⁵]+C₇[7H_{s_2}(k)-56H_{s_2}(k)³+112H_{s_2}(k)⁵-64H_{s_2}(k)⁷]+C₉[9H_{s_2}(k)-120H_{s_2}(k)³+432H_{s_2}(k)⁵-576H_{s_2}(k)⁷+256H_{s_2}(k)⁹]

f₁(·)＝f(·)-H_{s_2}(k)

对于信号H_{c_1}(k)其重构过程与H_{s_1}(k)相同，虽然重构后的信号中的谐波含量已经被减少，但是解算出的角位置精度仍无法满足框架伺服控制***的要求，因此需要对重构后的信号进一步处理；

步骤4：以其中一路正弦信号为例，重构后的信号可以表示为：

H_{s_2}(k)＝sin[θ(k)]+G[θ(k)]

其中，G[θ(k)]表示未考虑的谐波和模型残余误差，可以将其视为残余噪声。基于此式，可以得到状态方程：

式中，x₁(k)表示理想的正弦信号sin[θ(k)]的积分；x₂(k)表示残余噪声G[θ(k)]；u(k)作为ESO的输入，表示重构后的信号H_{s_2}(k)；d(k)表示集总扰动的导数；y(k)表示输出变量，可以表示为：

其中，θ_n(k)表示最终输入到框架伺服控制***的角位置，ω_n(k)表示最终的角速度，由于框架伺服控制***为超低速***，所以在相邻采样时刻可以将其视为常值。

上述状态方程满足积分串联形式，所以可以设计如下形式的离散ESO观测器：

式中，z₁(k)和z₂(k)分别用于观测x₁(k)和x₂(k)，h表示积分步长，e(k)表示观测误差，β₁和β₂表示观测器系数；对信号z₂(k)采用如下离散积分的方法进行处理。

z₂(k+1)＝z₂(k)+h[-β₂e(k)-z₂(k-m)]

其中，m表示积分的点数，本发明采用单步积分的方式，因此可以将离散ESO改写为：

经过上述处理后即可得到谐波含量非常少的正余弦信号，利用atan2函数即可得到角位置信息：

θ_n(k)＝atan 2[z_{1 sin}(k),z_{1 cos}(k)]

其中，z_{1 sin}(k)和z_{1 cos}(k)表示离散ESO输出的正余弦信号，θ_n(k)表示最终输入到框架伺服控制***的期望角位置。

本发明与现有技术相比的优点在于：

1、现有的线性霍尔传感器作为电机位置传感器应用领域，在解算角位置的过程中，缺乏考虑定子绕组电磁干扰、传感器安装误差和信号处理电路零位漂移等因素对位置解算精度造成的影响。而这种影响对于具有高精度要求的超低速框架***的控制精度影响很大，同时超低速控制中难以应用正交锁相环等信号处理方法，因此本发明结合具体工程应用通过信号提取算法抑制信号畸变，获取高精度角位置。

2、本发明将霍尔信号通过归一化和重构处理之后，使用离散ESO迭代处理获取理想正余弦信号，实现准确角位置的解算，并且本方法相较于其他方法，运算量小，***负担低。

3、本方法硬件简单可靠，易于工程实现，可以被广泛应用于结构紧凑的机电设备的位置检测***。

4、本发明能够通过使用由信号归一化处理，信号重构及离散ESO构成的信号提取算法(ESM)抑制霍尔传感器输出信号中的高次谐波和噪声，获取理想的正余弦信号，实现高精度角位置解算，并将其用于框架伺服控制***，提高控制力矩陀螺的输出力矩精度。

附图说明

图1为基于霍尔位置传感器的伺服***信号流向图；

图2为信号重构方法结构图；

图3为霍尔信号提取算法过程示意图；

图4为详细的霍尔信号处理示意图；

图5为霍尔安装支架结构图；

图6为基于霍尔位置传感器的框架伺服***控制流程图；

图中：1为线性霍尔传感器，2为霍尔安装支架，3为信号处理模块，4为信号采集模块，5为信号提取算法，6为框架伺服控制***，7为卡槽，8为固定槽，9为框架伺服电机，10为凹槽，11为电机定子绕组，12为转子末端，13为压盖。

第一霍尔传感器1.1，第三霍尔传感器1.3，第二霍尔传感器1.2，第四霍尔传感器1.4。

具体实施方式

下面结合附图以及具体实施方式进一步说明本发明。

本发明的基本原理是：控制力矩陀螺作为惯性执行机构的一种，由于其力矩大，精度高等优点被广泛应用于大型航天器姿态控制。控制力矩陀螺由高速转子***和框架***两部分组成，其基本工作原理为：当高速转子高速旋转时，框架***改变角动量的方向从而实现力矩的输出，控制航天器姿态。由于陀螺的角动量大小是恒定的，航天器姿态控制精度与陀螺输出力矩精度均由框架***角速度的精度决定。在工程应用中，框架***所获取的转子角位置的精度高低是决定框架***角速度控制性能好坏的重要因素。目前常用的高精度位置传感器如光电码盘、编码器等因为体积大，可靠性低，工作环境要求高的原因难以满足航天领域对于体积、质量、可靠性的严格要求。因此，高精度和小体积的线性霍尔传感器成为了框架***检测角位置的最佳选择。但是，由于传感器安装误差、外部电磁干扰及信号处理电路的漂移等因素造成的传感器输出信号畸变，使得角位置解算精度降低。并且由于框架***为超低速伺服***，其转频极低，常规的同频提取器、正交锁相环和自适应陷波器等信号处理方法无法被应用。本发明通过闭环控制***将经过归一化处理和信号重构抑制了正余弦信号中振幅误差和高次谐波，最后使用离散ESO迭代抑制残余误差获取理想正余弦信号，进而解算得到高精度角位置。本发明相较于离线标定而言，其连续性好，工程实现简单，可以广泛应用于多种设备，并且避免了离散标定带来的不连续问题。本发明使得陀螺高精度力矩输出成为可能。

如图1与图4所示，数据采集与处理的原理：安装在支架上的四个霍尔传感器在转子旋转过程中获取永磁体磁场信息，四路霍尔信号(Hall1_1.1～Hall1_1.4)在经过信号处理模块的隔离、滤波和差分之后得到两组同时包含正余弦的信号(Signal_H1,Signal_H2)，通过16位AD转换芯片采集后(Signal_h1,Signal_h2)使用信号提取算法进行处理，信号提取算法将输入信号的振幅误差和高阶谐波进行抑制，得到理想的正余弦信号，通过反正切解算获取精确的转子位置。

如图3所示，信号提取算法的过程为：首先对输入的信号进行归一化处理用以抑制振幅误差，归一化处理之后的信号被送入到信号重构器之中用以抑制信号中所包含的高次谐波，重构之后的信号被送入到离散ESO中，通过迭代抑制其中的残余误差，最后将获取的角位置送入框架控制***，提高陀螺的输出力矩精度。其具体实现过程如下：

步骤1：两路成对的霍尔传感器检测到磁场变化后输出包含k时刻转子角位置信息的正余弦信号：

其中，H_s(k)和H_c(k)表示霍尔传感器输出的正弦和余弦信号，α和β表示信号的振幅，A_i和B_i(i＝3,5,7,9)为谐波的系数，sin(nθ(k))和cos(nθ(k))(n＝3,5,7,9)表示n阶谐波；

步骤2：由于输入到SEM的信号存在振幅误差和大量谐波，为了更好的抑制谐波的影响，SEM算法需要先对原始输入信号进行归一化处理用以抑制振幅误差，具体方法如下式：

式中，H_s(k)和H_c(k)分别为输入到SEM模块的两路正余弦信号，θ_d(k)表示原始的位置信息，H_{s_1}(k)和H_{c_1}(k)分别表示归一化后的正弦信号和余弦信号；经过归一化处理之后的正余弦信号可以改写为：

其中，正余弦信号基频的幅值为1，C_i(i＝3,5,7,9)表示归一化之后谐波信号的幅值，该系数通过参数辨识方法获得；

步骤3：基于上述归一化处理后的信号进行信号重构，可以得到两路重构后的信号。对于正弦信号H_{s_1}(k)，

首先将H_{s_1}(k)与f(·)做差获得误差e(k)，然后使用整定参数γ对误差e(k)进行整定得到γe(k)，最后从H_{s_1}(k)中减去γe(k)和f₁(·)得到重构之后的正弦信号H_{s_2}(k)。其中f(·)和f₁(·)表示自变量为sin(θ)的函数，利用多倍角关系式，高次谐波可以有如下变换：

sin(nθ(k))＝f(sin(θ(k)))

其中，n为谐波的阶数。基于此函数，其中的函数f(·)和f₁(·)可以表示为：

f(·)＝H_{s_2}(k)+C₃[3H_{s_2}(k)-4H_{s_2}(k)³]+C₅[5H_{s_2}(k)-20H_{s_2}(k)³+16H_{s_2}(k)⁵]+C₇[7H_{s_2}(k)-56H_{s_2}(k)³+112H_{s_2}(k)⁵-64H_{s_2}(k)⁷]+C₉[9H_{s_2}(k)-120H_{s_2}(k)³+432H_{s_2}(k)⁵-576H_{s_2}(k)⁷+256H_{s_2}(k)⁹]

f₁(·)＝f(·)-H_{s_2}(k)

对于信号H_{c_1}(k)其重构过程与H_{s_1}(k)相同。虽然重构后的信号中的谐波含量已经被减少，但是解算出的角位置精度仍无法满足框架伺服控制***的要求，因此需要对重构后的信号进一步处理；

步骤4：以其中一路正弦信号为例，重构后的信号可以表示为：

H_{s_2}(k)＝sin[θ(k)]+G[θ(k)]

其中，G[θ(k)]表示未考虑的谐波和模型残余误差，可以将其视为残余噪声。基于此式，可以得到状态方程：

式中，x₁(k)表示理想的正弦信号sin[θ(k)]的积分；x₂(k)表示残余噪声G[θ(k)]；u(k)作为ESO的输入，表示重构后的信号H_{s_2}(k)；d(k)表示集总扰动的导数；y(k)表示输出变量，可以表示为：

其中，θ_n(k)表示最终输入到框架伺服控制***的角位置，ω_n(k)表示最终的角速度，由于框架伺服控制***为超低速***，所以在相邻采样时刻可以将其视为常值。

上述状态方程满足积分串联形式，所以可以设计如下形式的离散ESO观测器：

式中，z₁(k)和z₂(k)分别用于观测x₁(k)和x₂(k)，h表示积分步长，e(k)表示观测误差，β₁和β₂表示观测器系数；对信号z₂(k)采用如下离散积分的方法进行处理。

z₂(k+1)＝z₂(k)+h[-β₂e(k)-z₂(k-m)]

其中，m表示积分的点数，本发明采用单步积分的方式，因此可以将离散ESO改写为：

经过上述处理后即可得到谐波含量非常少的正余弦信号，利用atan2函数即可得到角位置信息：

θ_n(k)＝atan 2[z_{1 sin}(k),z_{1 cos}(k)]

其中，z_{1 sin}(k)和z_{1 cos}(k)表示离散ESO输出的正余弦信号，θ_n(k)表示最终输入到框架伺服控制***的期望角位置。

如图1和图5所示，一种基于线性霍尔传感器的陀螺框架伺服***高精度位置辨识方法，包括线性霍尔传感器1、霍尔安装支架2、信号处理模块3，信号采集模块4，信号提取算法(SEM)5、框架伺服控制***6和框架伺服电机9，其中四路线性霍尔传感器1固定于霍尔安装支架2上的卡槽7内，霍尔安装支架2通过固定槽8固定到电机壳端部，线性霍尔传感器1的信号通过屏蔽线连接到信号处理模块3，输出的霍尔信号经过滤波、差分和偏置后接入到信号采集模块4，采集后的信号经过信号提取算法5处理后得到准确位置信息，得到的准确的位置信息进入框架伺服控制***6中，将位置信息结合电机控制算法得到驱动电压，最后将驱动电压送至框架伺服电机9中，实现电机的精确控制。本方案采用四路线性霍尔传感器对磁场进行检测，四路线性霍尔传感器按照一定的角度排布，第一霍尔传感器1.1和第三霍尔传感器1.3相对应，第二霍尔传感器1.2和第四霍尔传感器1.4相对应，每对霍尔传感器相差90°或其倍数的电角度。

图6为本发明的信号流程图，***上电之后，霍尔传感器安装位置处的磁场强度就可以被实时检测出来，检测到的霍尔信号经过差分、滤波和隔离等预处理之后经AD转换芯片被采集为数字信号。然后，利用本发明提到的信号提取算法对数字信号进行处理，获取理想正余弦信号，解算得到转子准确角位置，基于此位置信息，控制器通过控制算法得到***的控制信号，控制驱动器产生适当的电压驱动框架伺服电机运动，电机通过连接器件带动陀螺房转动，改变角动量方向，实现力矩输出。当电机转子转动时，霍尔传感器安装位置处的磁场跟随永磁体转动角位置的变化而变化，变化的磁场再次经过上述流程继续驱动电机，从而实现了陀螺框架***的精准控制。

本发明未详细公开的部分属于本领域的公知技术。

尽管上面已经对本发明的具体实施细节进行了描述，方便相关领域的技术人员理解本发明，但值得注意的是，本发明不限于具体应用的范围，对于相关领域的普通技术人员而言，只要各种变化在所附的权利要求限定和确定的本发明的范围内，这些改变都是显而易见的，一切利用本发明构思得到的发明创造均在被保护的范围之内。

Claims (2)

1.一种陀螺框架伺服***高精度位置辨识方法，其特征在于，该方法利用的装置包括线性霍尔传感器(1)、霍尔安装支架(2)、信号处理模块(3)、信号采集模块(4)、信号提取算法(SEM)(5)、框架伺服控制***(6)和框架伺服电机(9)，其中四路线性霍尔传感器(1)固定于霍尔安装支架(2)上具有固定角度差的卡槽(7)内，霍尔安装支架(2)通过固定槽(8)固定到电机壳端部，凹槽(10)用于电机定子绕组(11)布线，装配完毕后，霍尔安装支架上的霍尔传感器会处于贴近转子末端(12)的压盖(13)的凹槽位置，尽可能贴近转子上的永磁体，以检测更准确的磁场变化；线性霍尔传感器(1)的信号通过屏蔽线连接到信号处理模块(3)，输出的霍尔信号经过滤波、差分和偏置后接入到信号采集模块(4)，采集后的信号经过信号提取算法(SEM)(5)处理后得到理想的正余弦信号，对正余弦信号解算之后获取准确位置信息，将得到的准确的位置信息送入框架伺服控制***(6)中，使位置信息结合电机控制算法得到驱动电压，最后将驱动电压送至框架伺服电机(9)中，实现电机的精确控制。

2.根据权利要求1所述的一种陀螺框架伺服***高精度位置辨识方法，其特征在于：采用四路线性霍尔传感器对磁场进行检测，四路线性霍尔传感器按照一定的角度排布，每对霍尔传感器相差90°或其倍数的电角度，其对应的机械角度计算式如下：

式中，θ_m表示霍尔传感器错开的机械角度，p表示极对数，k_h＝1,2,3,4，表示第k_h个霍尔传感器，当极对数为8时，则设置每对霍尔传感器之间的机械角度分别为11.25°或它的整数倍，每对霍尔传感器分别测量永磁体转子的一个N极和一个S极，将四路霍尔信号通过信号处理和采集后使用信号提取算法(SEM)进行处理；信号提取算法主要包含信号归一化处理，信号重构和离散扩张状态观测器，其具体实现过程为：

步骤1：两路成对的霍尔传感器检测到磁场变化后输出包含k时刻转子角位置信息的正余弦信号：

其中，H_s(k)和H_c(k)表示霍尔传感器输出的正弦和余弦信号，α和β表示信号的振幅，A_i和B_i(i＝3,5,7,9)为谐波的系数，sin(nθ(k))和cos(nθ(k))(n＝3,5,7,9)表示n阶谐波；

步骤2：由于输入到SEM的信号存在振幅误差和大量谐波，为了更好的抑制谐波的影响，SEM算法需要先对原始输入信号进行归一化处理用以抑制振幅误差，具体方法如下式：

式中，H_s(k)和H_c(k)分别为输入到SEM模块的两路正余弦信号，θ_d(k)表示原始的位置信息，H_{s_1}(k)和H_{c_1}(k)分别表示归一化后的正弦信号和余弦信号；经过归一化处理之后的正余弦信号可以改写为：

其中，正余弦信号基频的幅值为1，C_i(i＝3,5,7,9)表示归一化之后谐波信号的幅值，该系数通过参数辨识方法获得；

步骤3：基于上述归一化处理后的信号进行信号重构，可以得到两路重构后的信号，对于正弦信号H_{s_1}(k)，

首先将H_{s_1}(k)与f(·)做差获得误差e(k)，然后使用整定参数γ对误差e(k)进行整定得到γe(k)，最后从H_{s_1}(k)中减去γe(k)和f₁(·)得到重构之后的正弦信号H_{s_2}(k)，其中f(·)和f₁(·)表示自变量为sin(θ)的函数；利用多倍角关系式，高次谐波可以有如下变换：

sin(nθ(k))＝f(sin(θ(k)))

其中，n为谐波的阶数，基于此函数，其中的函数f(·)和f₁(·)可以表示为：

f(·)＝H_{s_2}(k)+C₃[3H_{s_2}(k)-4H_{s_2}(k)³]+C₅[5H_{s_2}(k)-20H_{s_2}(k)³+16H_{s_2}(k)⁵]+C₇[7H_{s_2}(k)-56H_{s_2}(k)³+112H_{s_2}(k)⁵-64H_{s_2}(k)⁷]+C₉[9H_{s_2}(k)-120H_{s_2}(k)³+432H_{s_2}(k)⁵-576H_{s_2}(k)⁷+256H_{s_2}(k)⁹]

f₁(·)＝f(·)-H_{s_2}(k)

对于信号H_{c_1}(k)其重构过程与H_{s_1}(k)相同，虽然重构后的信号中的谐波含量已经被减少，但是解算出的角位置精度仍无法满足框架伺服控制***的要求，因此需要对重构后的信号进一步处理；

步骤4：以其中一路正弦信号为例，重构后的信号可以表示为：

H_{s_2}(k)＝sin[θ(k)]+G[θ(k)]

其中，G[θ(k)]表示未考虑的谐波和模型残余误差，可以将其视为残余噪声；基于此式，可以得到状态方程：

式中，x₁(k)表示理想的正弦信号sin[θ(k)]的积分；x₂(k)表示残余噪声G[θ(k)]；u(k)作为ESO的输入，表示重构后的信号H_{s_2}(k)；d(k)表示集总扰动的导数；y(k)表示输出变量，可以表示为：

其中，θ_n(k)表示最终输入到框架伺服控制***的角位置，ω_n(k)表示最终的角速度，由于框架伺服控制***为超低速***，所以在相邻采样时刻可以将其视为常值；

上述状态方程满足积分串联形式，所以可以设计如下形式的离散ESO观测器：

式中，z₁(k)和z₂(k)分别用于观测x₁(k)和x₂(k)，h表示积分步长，e(k)表示观测误差，β₁和β₂表示观测器系数；对信号z₂(k)采用如下离散积分的方法进行处理；

z₂(k+1)＝z₂(k)+h[-β₂e(k)-z₂(k-m)]

其中，m表示积分的点数，采用单步积分的方式，因此可以将离散ESO改写为：

经过上述处理后即可得到谐波含量非常少的正余弦信号，利用atan2函数即可得到角位置信息：

θ_n(k)＝atan2[z_1sin(k),z_1cos(k)]

其中，z_1sin(k)和z_1cos(k)表示离散ESO输出的正余弦信号，θ_n(k)表示最终输入到框架伺服控制***的期望角位置。

Images