一种高精度舵机位置控制方法与流程

技术特征：

1.一种高精度舵机位置控制方法，其特征在于，包括以下步骤

(1)对当前的电位器电压进行ADC采样，采样的电压值记为当前的舵机位置值；

(2)对舵机位置值进行线性插值算法处理，对非线性区间进行线性校准；

(3)将校准后的位置线，采用一阶惯性滤波算法，对输出最终ADC采集结果做滤波处理；

(4)对位置曲线进行滤波处理，经过线性插值和滤波算法处理，校准后的位置曲线；

(5)基于舵机的物理数学模型，构建串级PID控制器，实现以位置控制为外环，速度控制为内环的高精度舵机位置控制算法。

2.根据权利要求1所述的高精度舵机位置控制方法，其特征在于，所示步骤(2)具体为，在区间[y₀，y₁]中，进行定点采样取值：第一个点(x₀，y₀)和最后一个点(x₁，y₁)，则在区间内的任意点y(i)，其坐标(x，y)可求出：

3.根据权利要求1所述的高精度舵机位置控制方法，其特征在于，所示步骤(3)中，一阶惯性滤波算法具体为，y(n)＝a*x(n)+(1-a)*y(n-1)，其中a为滤波系数，0≤a≤1。

4.根据权利要求1所述的高精度舵机位置控制方法，其特征在于，所示步骤(4)中，滤波处理具体为，

$y\left(i\right)=u\left(i\right)\frac{2\mathrm{\π}fT}{1+2\mathrm{\π}fT}+(1-\frac{2\mathrm{\π}fT}{1+2\mathrm{\π}fT})\left(y\right(i-1)+Ave\_Value).$

其中，f为滤波截止频率，T为采样周期，Ave_Value为当前速度的理想增值。

5.根据权利要求1所述的高精度舵机位置控制方法，其特征在于，所示步骤(5)中，在位置控制中，根据舵机设定的运动完成时间T和预先设定的目标位置R₁(S)计算出当前时刻T_n的参考位置R_1n(S)，计算公式：其中，0≤n≤N，

将当前经过线性插值和滤波处理后的位置值作为位置PID的当前位置输入作为位置控制器的输入H_1n(S)；

当前采样位置H_1n(S)和参考位置R_1n(S)做偏差E₁(S)，位置控制器进行PID处理，在时域里基本公式：

$S_{out1}\left(t\right)=K_{p}e\left(t\right)+\frac{K_p}{T_i}{\∫}_0^{t}e\left(t\right)dt+K_c\left(S_{out2}\right(t)-S_{out1}(t\left)\right)+K_p{T}_d\frac{de\left(t\right)}{dt}$

设S(k)为第k次采样时刻位置控制器的输出值，进行PID离散化，令

$\left\{\begin{array}{c}{K}_i=\frac{T}{T_i}\\ K_d=\frac{T_d}{T}\end{array}\right.$

计算公式：

S_out1(k)＝S_p(k)+S_i(k-1)+K_iS_p(k)+K_c(S_out2(k)-S_out1(k))+K_d(S_p(k)-S_p(k-1))

输出结果S_out1进行预饱和限幅输出得到S_out2。

6.根据权利要求1所述的高精度舵机位置控制方法，其特征在于，所示步骤(5)中，在速度控制中，预饱和输出S_out2将作为当前速度控制器的参考速度R_2n(S)，

参考速度R_2n(S)的计算公式：

当前速度检测H_2n(S)的计算公式：

$H_{2n}\left(s\right)=\frac{R_{1n}\left(S\right)-R_{1(n-1)}\left(S\right)}{\frac{T}{N}}=\frac{N}{\[R_{1n}\left(S\right)-R_{1(n-1)}\left(S\right)\]T}$

当前采样位置H_2n(S)和参考位置R_2n(S)做偏差E₂(S)，速度控制器进行PID处理，令

$\left\{\begin{array}{c}{V}_{out1}\left(k\right)=K_p\left\{e\right(k)+\frac{T}{T_I}\underset{j=0}{\overset{k}{\Σ}}e(j)+\frac{T_D}{T}\[e(k)-e(k-1)\]\}\\ V_{out1}(k-1)=K_p\left\{e\right(k-1)+\frac{T}{T_I}\underset{j=0}{\overset{k-1}{\Σ}}e(j)+\frac{T_D}{T}\[e(k-1)-e(k-2)\]\}\end{array}\right.$

速度控制器输出量是速度增量ΔV_out1，由上式可得

ΔV_out1(k)＝V_out1(k)-V_out1(k-1)＝K_p[e(k)-e(k-1)]+K_Ie(k)+K_D[e(k)-2e(k-1)+e(k-2)]，

根据速度增量，对当前速度进行修正，计算公式：V_out1(k)＝V_out1(k-1)+ΔV_out1(k)

对修正速度V_out1进行限幅输出，输出为最终速度V_out2；

对最终速度V_out2进行PWM转换，转换计算公式：

$Pwm\_Value=\frac{V_{out2}}{K}+Pwm\_Value$

其中，Pwm_Value为用于速度控制的占空比值，

K的比例值由当前电压下，速度与PWM的对应线性关系线求出。