PMSM FOC位置环S曲线控制算法(恒定急动度)

之前做FOC位置环控制的时候，简单地加了一个S曲线控制，参考链接如下： FOC 单电阻采样 位置环控制伺服电机

这里面代码实现其实就是在每step个ADC中断中，根据函数$f(x)=\frac{1}{1+e^{-x}}$生成一个uint16的S曲线定点数组，每次计算位置增加的比例，进而设置位置环的目标值。当然还需要考虑一些问题，比如S曲线有一些地方很平缓，增量不高，所以在还没到step个ADC中断的时候，PID就已经收敛了，所以电机就会在目标位置轻微抖动。当然这个BUG稍稍修改一下代码就可以避免了，但是有没有更好的控制方法呢？

一 原理

如图所示，如果采用恒定加速度，先以加速度a加速，再匀速，再以加速度-a减速。​

​
但是这样的话，加速度就有一个阶跃变化，这在物理上是不可实现的，同时也会损坏电机。所以这里考虑将对加速度进行微分，就可以得到急动度(Jerk/Jolt)，它用物理量J表示，即$J=\frac{da}{dt}$。
这样就可以让加速度以梯形波形式变化：

那么到底这样的加速度函数会产生什么样的速度和角度曲线呢？

编写Matlab程序，对加速度求积分得到速度曲线，对速度曲线求积分得到角度曲线。这里初始化J=1、A=1(单位控制时间)，曲线形状不受这两个变量大小的影响，仅仅会影响曲线的宽窄程度。

可以看到角度的变化正好是S曲线，有了这样的结果，我们开始对整个过程进行数学分析。这里以加速阶段为例进行分析，如下图所示，将加速阶段分为三段，持续时间t=A。

（1）Ⓞ->①阶段：恒定+J
​初始时刻$a_0=0$、${\omega }_0=0$、${\theta }_0=0$。
{ a 1 = a 0 + J t = J A ω 1 = ω 0 + ∫ 0 t a 1 d t = ω 0 + a 0 t + 1 2 J t 2 = 1 2 J A 2 θ 1 = θ 0 + ∫ 0 t ω 1 d t = θ 0 + ω 0 t + 1 2 a 0 t 2 + 1 6 J t 3 = 1 6 J A 3 \left\{

\right. ⎩⎨⎧​a1​=a0​+Jt=JAω1​=ω0​+∫0t​a1​dt=ω0​+a0​t+21​Jt2=21​JA2θ1​=θ0​+∫0t​ω1​dt=θ0​+ω0​t+21​a0​t2+61​Jt3=61​JA3​
（2）①->②阶段：J=0，a恒定
{ a 2 = a 1 = J A ω 2 = ω 1 + ∫ 0 t a 2 d t = ω 1 + a 2 t = 3 2 J A 2 θ 2 = θ 1 + ∫ 0 t ω 2 d t = θ 1 + ω 1 t + 1 2 a 2 t 2 = 7 6 J A 2 \left\{

a2=a1=JAω2=ω1+∫t0a2dt=ω1+a2t=32JA2θ2=θ1+∫t0ω2dt=θ1+ω1t+12a2t2=76JA2$$\begin{array}{c}{a}_2=a_1=JA\\ {\omega }_2={\omega }_1+{\int }_0^t{a}_{2}dt={\omega }_1+a_{2}t=\frac{3}{2}J{A}^2\\ {\theta }_2={\theta }_1+{\int }_0^t{\omega }_{2}dt={\theta }_1+{\omega }_{1}t+\frac{1}{2}{a}_2{t}^2=\frac{7}{6}J{A}^2\end{array}$$

\right. ⎩⎨⎧​a2​=a1​=JAω2​=ω1​+∫0t​a2​dt=ω1​+a2​t=23​JA2θ2​=θ1​+∫0t​ω2​dt=θ1​+ω1​t+21​a2​t2=67​JA2​
（3）②->③阶段：恒定-J
{ a 3 = a 2 − J t = 0 ω 3 = ω 2 + ∫ 0 t a 3 d t = ω 2 + a 2 t − 1 2 J t 2 = 2 J A 2 θ 3 = θ 2 + ∫ 0 t ω 3 d t = θ 2 + ω 2 t + 1 2 a 2 t 2 − 1 6 J A 2 = 3 J A 3 \left\{

a3=a2−Jt=0ω3=ω2+∫t0a3dt=ω2+a2t−12Jt2=2JA2θ3=θ2+∫t0ω3dt=θ2+ω2t+12a2t2−16JA2=3JA3$$\begin{array}{c}{a}_3=a_2-Jt=0\\ {\omega }_3={\omega }_2+{\int }_0^t{a}_{3}dt={\omega }_2+a_{2}t-\frac{1}{2}J{t}^2=2J{A}^2\\ {\theta }_3={\theta }_2+{\int }_0^t{\omega }_{3}dt={\theta }_2+{\omega }_{2}t+\frac{1}{2}{a}_2{t}^2-\frac{1}{6}J{A}^2=3J{A}^3\end{array}$$

\right. ⎩⎨⎧​a3​=a2​−Jt=0ω3​=ω2​+∫0t​a3​dt=ω2​+a2​t−21​Jt2=2JA2θ3​=θ2​+∫0t​ω3​dt=θ2​+ω2​t+21​a2​t2−61​JA2=3JA3​
这样就计算出了加速阶段转过的角度，而减速过程与加速过程对称，故${\theta }_{accelerate}={\theta }_{decelerate}={\theta }_3=3J{A}^3$。中间是加速度为0，速度为${\omega }_3$，时间为3A的匀速阶段。
最终得到总旋转角度${\theta }_{total}={\theta }_{accelerate}+{\omega }_{3}t+{\theta }_{decelerate}=12J{A}^3$

二 代码

参考代码：X-CUBE-MCSDK库
1、位置环结构体
了解了前面的原理后，这些变量的含义就很清楚了。原本角度相关的变量是float类型的，这里为了方便和系统中的电角度变量统一，所以这里都改为了int16类型。而急动度、加速度、时间等参数不得不设置为浮点型，因为特定旋转角度和时间对应的急动度不一定是整数。

typedef enum {
    TC_READY_FOR_COMMAND  = 0,
    TC_MOVEMENT_ON_GOING = 1,         /* MOVE模式 */
    TC_TARGET_POSITION_REACHED = 2,   /* 到达位置 */
} PosCtrlStatus_t;

typedef struct
{
  float SamplingTime;
  float MovementDuration;
  float SubStep[6];
  float SubStepDuration; 
  float ElapseTime;
  float Jerk;
  float CruiseSpeed;
  float Acceleration;
  float Omega;
  float OmegaPrev;
  float ThetaPrev;
  int16_t StartingAngle;
  int16_t FinalAngle;
  int16_t AngleStep;
  int16_t Theta;
  int16_t hMaxTorque; 
  int16_t flag;
  PosCtrlStatus_t PositionCtrlStatus;
  PID_Handle_t *PIPos;
} PosControl_Handle_t;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

2、参数计算与状态切换
计算旋转参数，如急动度、巡航速度(CruiseSpeed)，然后初始化时间、加速度、角速度、角度为初始值，最后将状态设置为TC_MOVEMENT_ON_GOING，这表示在中频任务调用位置环函数中将执行我们的S曲线位置环过程。

void TC_MoveCommand(PosControl_Handle_t *pHandle, int16_t startingAngle, int16_t angleStep, float movementDuration)
{
  float fMinimumStepDuration;
  if ( (pHandle->PositionCtrlStatus == TC_READY_FOR_COMMAND) && (movementDuration > 0) )
  {
	/* 总时间 */
    fMinimumStepDuration = (9.0f * pHandle->SamplingTime);

	/* 传入的运动时间参数是浮点数,但它只能是最小时间的整数倍 */
    pHandle->MovementDuration = (float)((int)(movementDuration / fMinimumStepDuration)) * fMinimumStepDuration;
    
    pHandle->StartingAngle = startingAngle;
    pHandle->AngleStep = angleStep;
    pHandle->FinalAngle = startingAngle + angleStep;

	/* 将总持续时间分为9段 */
    pHandle->SubStepDuration = pHandle->MovementDuration / 9.0f;

    // 加速阶段
    pHandle->SubStep[0] = 1 * pHandle->SubStepDuration;   /* Sub-step 1 of acceleration phase */
    pHandle->SubStep[1] = 2 * pHandle->SubStepDuration;   /* Sub-step 2 of acceleration phase */
    pHandle->SubStep[2] = 3 * pHandle->SubStepDuration;   /* Sub-step 3 of acceleration phase */

    // 减速阶段
    pHandle->SubStep[3] = 6 * pHandle->SubStepDuration;   /* Sub-step 1 of deceleration phase */
    pHandle->SubStep[4] = 7 * pHandle->SubStepDuration;   /* Sub-step 2 of deceleration phase */
    pHandle->SubStep[5] = 8 * pHandle->SubStepDuration;   /* Sub-step 3 of deceleration phase */
    
    // J = DeltaTheta/(12 * A * A * A)
	pHandle->Jerk = pHandle->AngleStep / (12 * pHandle->SubStepDuration * pHandle->SubStepDuration * pHandle->SubStepDuration);
	
    // Speed cruiser = 2*J*A*A)
    pHandle->CruiseSpeed = 2 * pHandle->Jerk * pHandle->SubStepDuration * pHandle->SubStepDuration;

    pHandle->ElapseTime = 0.0f;

    pHandle->Omega = 0.0f;
    pHandle->Acceleration = 0.0f;
    pHandle->Theta = startingAngle;
    pHandle->PositionCtrlStatus = TC_MOVEMENT_ON_GOING; 
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

3、位置环函数
这里就是根据之前计算的急动度等参数，执行位置环过程，最终计算出theta用于位置环PID控制的目标值中。但由于浮点运算的误差，最终值会和目标值有一定误差，有可能超过目标值，也可能小于目标值。所以在到达ElapseTime，直接将theta设置为最终的角度值。这里加了个判断，防止位置过调。

int16_t delta_end,delta_tmp;
int16_t get_delta()
{
	int16_t tmp;
	tmp = PosCtrlM.Theta - PosCtrlM.FinalAngle;
	return tmp;
}

uint8_t flag_end=0;
void TC_MoveExecution(PosControl_Handle_t *pHandle)
{

	float jerkApplied = 0;
	
	if(pHandle->PositionCtrlStatus != TC_MOVEMENT_ON_GOING)
	{
		return;
	}

	if (pHandle->ElapseTime < pHandle->SubStep[0])              // 1st Sub-Step interval time of acceleration phase
	{
		jerkApplied = pHandle->Jerk;
	}
	else if (pHandle->ElapseTime < pHandle->SubStep[1])         // 2nd Sub-Step interval time of acceleration phase
	{
	}
	else if (pHandle->ElapseTime < pHandle->SubStep[2])         // 3rd Sub-Step interval time of acceleration phase
	{
		jerkApplied = -(pHandle->Jerk);
	}
	else if (pHandle->ElapseTime < pHandle->SubStep[3])         // Speed Cruise phase (after acceleration and before deceleration phases)
	{
		pHandle->Acceleration = 0.0f;
		pHandle->Omega = pHandle->CruiseSpeed;
	}
	else if (pHandle->ElapseTime < pHandle->SubStep[4])         // 1st Sub-Step interval time of deceleration phase
	{
		jerkApplied = -(pHandle->Jerk);
	}
	else if (pHandle->ElapseTime < pHandle->SubStep[5])         // 2nd Sub-Step interval time of deceleration phase
	{
	}
	else if (pHandle->ElapseTime < pHandle->MovementDuration)   // 3rd Sub-Step interval time of deceleration phase
	{
		jerkApplied = pHandle->Jerk;
	}
	else
	{
		jerkApplied = pHandle->Jerk;
	}
	delta_end = get_delta();
	
	//a = a0 + jt
	pHandle->Acceleration += jerkApplied * pHandle->SamplingTime;
	//v = v0 + at
	pHandle->Omega += pHandle->Acceleration * pHandle->SamplingTime;
	//θ = θ0 + vt
	pHandle->Theta += pHandle->Omega * pHandle->SamplingTime;
	
	/* 变化小时，0~16384测试，最后会卡一下再继续旋转，因为前面减速认为已经到目标位置了omega几乎为0，随着时间推移才变大 */
	delta_tmp = get_delta();
	if((delta_end < 0 && delta_tmp > 0) || (delta_end > 0 && delta_tmp < 0) || (delta_end == 0))
	{
		pHandle->Theta = pHandle->FinalAngle;
		pHandle->PositionCtrlStatus = TC_READY_FOR_COMMAND;//REACH后不变为TC_READY_FOR_COMMAND
		flag_end = 0;
		return;
	}
	pHandle->ElapseTime += pHandle->SamplingTime;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69

4、位置环函数
该函数在中频任务中执行。

void TC_PositionRegulation(PosControl_Handle_t *pHandle)
{
	int16_t wMecAngleRef;
	int16_t wMecAngle;
	int16_t wError;
	int32_t hTorqueReference;
	
	TC_MoveExecution(pHandle);

	wMecAngleRef = pHandle->Theta;
	wMecAngle = ENCODER_M._Super.hMecPosition;
	wError =  wMecAngle - wMecAngleRef;
	hTorqueReference = PID_Controller(pHandle->PIPos, wError);
	if(hTorqueReference < 0)
	{	
		if(hTorqueReference < -pHandle->hMaxTorque)
			hTorqueReference = -pHandle->hMaxTorque;
	}
	else
	{
		if(hTorqueReference > pHandle->hMaxTorque)
			hTorqueReference = pHandle->hMaxTorque;
	};
	FOCVars.hTeref = hTorqueReference;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

5、更改位置API
在我的应用中，位置环仅在一圈内进行旋转，所以前面设置的是int16类型的角度。在有些应用中，需要旋转多圈。如输入70000，就是先旋转2圈(65535)，在旋转4465，这就需要将前面的变量设置为uint32类型。这里的代码也需要做小小的修改。

void MCI_ExecPositionCommand( PosControl_Handle_t * pHandle, int16_t FinalPosition, float Duration )
{
	int16_t currentPosition = ENCODER_M._Super.hMecPosition;
	int16_t angleStep = FinalPosition - currentPosition;
	pHandle->PositionCtrlStatus = TC_READY_FOR_COMMAND;
	TC_MoveCommand(pHandle, currentPosition, angleStep, Duration);
}
1
2
3
4
5
6
7

6、待解决
浮点运算的误差产生了很多问题：

1. 将float的角度转为int16，每次计算的角度本来是浮点数被化成了整数，所以可以多用一个浮点变量来记录小数点后的角度，最后忽略掉小数点赋值给int16的电角度，这样不会有累积的更多误差。
2. 旋转一圈误差可能还可以接收，但如果旋转多圈，浮点运算次数变多，最终结束后与目标值的偏差就很大，然后运动过程结束，直接设置为目标值，电机就会有一个迅速旋转到目标值的过程，误差越大越明显。实际测试，不仅最后的值会小于目标值，还有可能大于目标值，也就是会超调。

我的解决办法是：由于我们对旋转时间的要求可能没有那么高，所以就根据用户设置的值，寻找一个近似的时间，这样计算出来的急动度为整数，或者说小数部分正好是$2^{-i}$，这样后续的计算就没有浮点误差。如果大家还有什么好的解决办法，欢迎讨论。