CN106681134A - 一种高精度舵机位置控制方法 - Google Patents

Abstract

本发明公开了一种高精度舵机位置控制方法，对当前的电位器电压进行ADC采样，采样的电压值记为当前的舵机位置值；对舵机位置值进行线性插值算法处理，对非线性区间进行线性校准，将校准后的位置线，采用一阶惯性滤波算法，对输出最终ADC采集结果做滤波处理；对位置曲线进行滤波处理，经过线性插值和滤波算法处理，校准后的位置曲线；基于舵机的物理数学模型，构建串级PID控制器，实现以位置控制为外环，速度控制为内环的高精度舵机位置控制算法。本发明可以解决电位器采样非线性和控制不稳定、不精准等问题，通过线性插值和滤波算法对电位器采样进行线性校准，构建串级PID伺服控制系统，实现舵机速度和位置的精准控制。

Description

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

技术领域

本发明涉及舵机控制技术领域，具体是一种用于舵机伺服控制系统的高精度舵机位置控制方法。

背景技术

在舵机伺服控制系统中，舵机的速度和位置精准控制是研究的主要方向，当前舵机的速度控制算法多为单回路PID控制。

该算法主要利用了实际速度与设定速度的偏差进行单回路PID调节，这种控制方式受电压、负载变化及器件物理误差等影响，导致反馈非线性误差大，从而存在调节时间长、抗干扰能力差、控制精度低等问题。

因此，如何提供一种既能消除反馈非线性，又抗干扰能力强、能实现速度和位置的高精度控制方法是本领域技术人员目前需要解决的问题。

发明内容

本发明的目的在于提供一种高精度舵机位置控制方法，以解决上述背景技术中提出的电位器采样非线性和控制不稳定、不精准等问题，通过线性插值和滤波算法对电位器采样进行线性校准，参考舵机机械结构图，如图1所示，构建串级PID伺服控制系统，实现舵机速度和位置的精准控制。

为解决上述技术问题，本发明提供了一种高精度舵机位置控制方法，包括如下步骤：

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

根据当前电位器的反馈值所反映的位置线总体是线性的，但部分区间存在非线性，如图2；

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

在区间[y₀，y₁]中，进行定点采样取值：第一个点(x₀，y₀)和最后一个点(x₁，y₁)，则在区间内的任意点y(i)，其坐标(x，y)可求出：

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

基本原理公式如下：y(n)＝a*x(n)+(1-a)*y(n-1)......(2)

，(其中a为滤波系数，0≤a≤1)；

(4)对所述位置曲线进行滤波处理，即

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

经过线性插值和滤波算法处理，校准后的位置曲线如图3；

(5)基于舵机的物理数学模型(图4)，构建串级PID控制器，实现以位置控制为外环(位置式PID)，速度控制为内环(增量式PID)的高精度舵机位置控制算法，控制系统框图如图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处理，其位置PID控制原理如图6。

在时域里基本公式：

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

计算公式：

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))......(7)

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

在速度控制器中，预饱和输出S_out2将作为当前速度控制器的参考速度R_2n(S)，

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

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

当前采样位置H_2n(S)和参考位置R_2n(S)做偏差E₂(S)，速度控制器进行PID(增量式)处理，其速度PID控制原理如图7，令

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

Δ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)]......(11)

根据速度增量，对当前速度进行修正，计算公式：

V_out1(k)＝V_out1(k-1)+ΔV_out1(k)......(12)

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

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

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

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

与现有技术相比，本发明的有益效果是：

本发明的方法可以解决电位器采样非线性和控制不稳定、不精准等问题，通过线性插值和滤波算法对电位器采样进行线性校准，构建串级PID伺服控制系统，实现舵机速度和位置的精准控制。

附图说明

图1为本发明提供的舵机伺服系统的硬件结构图。

图2为本发明提供的线性插值和滤波算法处理前的位置曲线图。

图3为本发明提供的非线性区间线性插值示意图。

图4为本发明提供的线性插值和滤波算法处理后的位置曲线图。

图5为本发明提供的一种舵机伺服系统串级PID控制原理框图。

图6为本发明提供的一种位置PID控制框图。

图7为本发明提供的一种速度PID控制框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的

实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种高精度舵机位置控制方法，可以解决电位器采样非线性和控制不稳定、不精准等问题，通过线性插值和滤波算法对电位器采样进行线性校准，构建串级PID伺服控制系统，实现舵机速度和位置的精准控制。

为使本专利实施目的、技术方案和优点更加清楚，下面将结合实施例对本发明进行完整、清楚地描述。

请参考图1，根据硬件结构图进行串级PID伺服控制系统的初步构建，图1为本发明提供的舵机伺服系统的硬件结构图。

请参考图2和图3，其中，图2为本发明提供的线性插值和滤波算法处理前的位置曲线图，图3为本发明提供的非线性区间线性插值示意图，进行区间确定和插值处理，该方法包括：

步骤S101：在初步确定的伺服控制系统中，进行舵机从起始零位置到给定目标位置的位置曲线采集测试；

步骤S102：根据步骤S001得到的位置曲线确定非线性区间；

步骤S103：对非线性区间的起始点(x₀，y₀)和终止点(x1,y2)坐标值的确定；

步骤S104：对非线性区间内的位置点(x，y)通过计算公式(1)进行线性校准；

进一步，对于线性插值校准后的位置值进行数字滤波，滤波原理参考计算公式(2)；

步骤S105：确定滤波截止频率f，采样周期T后，将线性插值校准后的位置值经过计算公式(3)得到滤波后的位置值。

对比图2和图4可知，经过线性插值和滤波算法后，位置曲线的非线性问题基本可以消除，至此，已完成舵机位置曲线的线性校准。

参考图6，位置PID控制框图，进行位置PID控制，该方法包括：

步骤S201：根据舵机设定的运动完成时间T和预先设定的目标位置R₁(S)，根据计算公式(4)计算出当前时刻T_n的参考位置R_1n(S)；

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

步骤S203：当前采样位置H_1n(S)和参考位置R_1n(S)做偏差E₁(S)，位置控制器进行PID处理，控制原理参考公式(5)；

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

步骤S204：根据计算公式(7)，进行位置PID控制，输出新的位置结果为S_out1；

步骤S205：将输出结果S_out1进行预饱和限幅输出得到最后输出位置S_out2。

参考图7，速度PID控制框图，进行速度PID控制，该方法包括：

步骤S301：根据计算公式(8)计算出当前时刻T_n的参考速度R_2n(S)；

步骤S302：根据计算公式(9)计算出当前速度检测H_2n(S)；

当前采样位置H_2n(S)和参考位置R_2n(S)做偏差E₂(S)，速度控制器进行PID控制，控制原理推算本参考公式(10)；

步骤S303:根据计算公式(11)，进行PID离散化计算，输出量是速度增量V_out1；

步骤S:304：根据计算公式(12)出新的速度值，对当前速度进行修正；

步骤S305：根据计算公式(13)，将V_out1进行限幅输出，输出为V_out2；

步骤S306：根据计算公式(14)，将V_out2进行PWM转换。

其中，公式(14)中K的比例值由当前电压下，速度与PWM的对应线性关系线求出。

上述实施例只是本发明一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性改进前提下所获得的所有其他实施例，都属于本发明保护的范围。

Claims (6)

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的对应线性关系线求出。