[发明专利]一种变桨位置控制三阶运动控制方法及控制装置

权利要求书

1.一种变桨位置控制三阶运动控制方法，其特征在于，包括：当达到规划触发条件时，进入三阶运动规划步骤；其中，所述规划触发条件设置为：设定了新的目标位置，即新输入的目标位移量与之前的不同：

获取变桨装置的目标运动参数；所述目标运动参数包括目标位移量Xref、速度临界值Vref、加速度临界值Aref；

获取所述变桨装置的当前运动参数和当前运动状态；所述当前运动参数包括：当前位移量Xnow、当前速度值Vnow、当前加速度值Anow；还包括加加速度值j，整个变桨控制过程的加加速度只有3个值：±j和0，在各种运动阶段中加速度是均匀变化的，形成三阶运动；还包括当前时刻在原规划中所经过的时间△t、原规划已经运动的位移量△x；所述当前运动状态为以下8种状态中其中一种：静止阶段、加加速运动阶段、匀加速运动阶段、减加速运动阶段、匀速运动阶段、加减速运动阶段、匀减速运动阶段、减减速运动阶段；

根据所述当前运动状态和当前运动参数，规划所述变桨装置的规划运动状态，以达到所述目标运动参数中的目标位移量，完成三阶运动规划；

完成所述三阶运动规划步骤后，还包括：

计算所述规划运动状态中各运动时刻的电机运动参数，所述电机运动参数包括Xnow、Vnow、Anow、Xnext、Vnext、Anext，其中，Xnow、Vnow、Anow分别为当前时刻的位移、速度、加速度，Xnext、Vnext、Anext分别为下一时刻的位移、速度、加速度；

将所述电机运动参数转换为变桨控制值，以控制所述变桨装置根据所述三阶运动规划运动；

变桨控制通过变桨驱动程序实现，在变桨驱动程序中主要包括电机驱动程序，电机驱动程序分为三层：运动规划层、速度控制层和力矩控制层；各层之间由PID控制程序连接，运动规划层为速度控制层提供速度PID控制的设定值，计算实时的速度控制程序的PID设定值g_SpeedCtrl.SpeedRef：

g_SpeedCtrl.SpeedRef＝g_PosCtrl.Kp×(X_now－g_PosCtrl.Profile.PosAct)+

g_PosCtrl.PreKp×(X_next－X_now)；

其中，g_PosCtrl.Profile.PosAct表示电机的实际位置，g_PosCtrl.Kp和g_PosCtrl.PreKp均为调试得到；X_now为当前时刻的位移，X_next下一时刻的位移；

速度控制层为力矩控制层提供力矩PID控制的设定值，最终由力矩控制层输出控制电机的PWM波，配合外围高压电路实现对变桨系统中的电机的控制。

2.根据权利要求1所述的变桨位置控制三阶运动控制方法，其特征在于，在加加速阶段，所述电机运动参数的计算包括：

X_now＝X_begin+D×[|V_begin|×t+(1/6)×j×t³]；

X_next＝X_begin+D×[|V_begin|×(t+0.001)+(1/6)×j×(t+0.001)³]；

V_now＝V_begin+D×0.5×j×t²；

V_next＝V_begin+D×0.5×j×(t+0.001)²；

A_now＝D×j×t；

A_next＝D×j×(t+0.001)；

其中，D为运动方向，t为当前小阶段经历的时间，X_begin、V_begin、A_begin分别为当前小阶段的起始位移、起始速度、起始加速度。