FOC 位置环S曲线之定点查表算法

在位置环中，若直接让电机到指定位置刚开始加速度过大，会对电机造成损坏，所以一般不会直接将实际位置误差进行PID计算，而是逐步地增加位置。本篇文章介绍一种简单的定点数查表实现S曲线的方法。

一、原理

实际上就是利用方程\(\large{y=\frac{1}{1+e^{-x}}}\)，其函数图像如下所示：

可以看到该函数的值域是(0,1)，斜率也是先增加再减小，就用这个函数作为我们电机运动的S曲线。代码中就是每隔一定时间设置位置环的目标值，位置误差的增长率就和这个S曲线函数相同。

二、代码实现

1、生成S曲线数组

由于需要用定点S曲线，所以使用Q16格式来保存数组，后续做了乘法后再右移16位即可。S曲线数组由以下C++工程得出：

#define total 720 //s曲线参数个数
float curve[total];
#define MAXSHIFT 16
#define MAX_VALUE 1<<MAXSHIFT
#define ENDLCNT 8

void curve_init(float *buf, uint16_t num, uint8_t range)
{
    float step = ( float )range * 2 / num;
    for (uint16_t i = 0; i < num; i++)
    {
        buf[i] = 1.0 / (1.0 + exp(range-step * i));
        printf("0x%04X,",( int )((MAX_VALUE)*buf[i]));
		if((i+1) % ENDLCNT == 0)
			cout << endl;
    }
}

int main(int argc, char* argv[])
{
    //freopen("out.txt","w",stdout);
    cout << "const uint16_t S_PROFILE[" << total+1 << "] = {"<<endl;
    curve_init(curve, total, 7);
	cout <<"0xFFFF};"<<endl;
    return 0;
}

对于curve_init函数的参数：

• num生成数组的个数。越大越平缓，电机调整速度越慢；越小越陡峭，电机调整速度快
• range:在[-range,range]范围内取值
  • 注意：range取7的时候值域已接近于(0,1),故建议range<=7,若太大则后面太平缓,几乎没有改变

2、位置环结构体

typedef struct
{
	PID_Handle_t *PIPos;
	int16_t  hSetPosition;   //设定机械位置
	int16_t  hStartPosition; //起始机械位置
	int16_t  hCurPosition;   //当前机械位置
	int16_t  hError;         //初始机械位置与目标位置的差
	int16_t  hMaxTorque;     //最大力矩
	uint16_t hCurIndex;      //当前S曲线表的索引位置
	uint16_t hStep;          //S曲线调整的时间
	uint8_t  bIsChanging;    //正在调整位置
} PosControl_Handle_t;

3、位置控制函数

int16_t Pos_CalcTorqueReference( PosControl_Handle_t * pHandle,SpeednPosFdbk_Handle_t * SpeedPos)
{
  int16_t hError;
  int16_t hTorqueReference;
  
  hError = SpeedPos->hMecPosition - pHandle->hCurPosition;

  hTorqueReference = PID_Controller( pHandle->PIPos,(int32_t )hError);
  if(hTorqueReference < 0)
  {	
     if(hTorqueReference < -pHandle->hMaxTorque)
			 hTorqueReference = -pHandle->hMaxTorque;
  }
  else
  {
     if(hTorqueReference > pHandle->hMaxTorque)
			 hTorqueReference = pHandle->hMaxTorque;
  };
  return hTorqueReference;
}

#define NEXT_STEP_ERR 50
void Pos_Control(PosControl_Handle_t * pHandle,SpeednPosFdbk_Handle_t * SpeedPos)
{
	static uint16_t POSCNT = 0;
	int16_t delta;
	if(pHandle->bIsChanging)
	{
		delta = ENCODER_M._Super.hMecPosition - pHandle->hCurPosition;
		/* 每hStep次中断更改一次位置 */
		POSCNT = (POSCNT+1) % pHandle->hStep;
		if(POSCNT == 0 || delta < NEXT_STEP_ERR)
		{
			/* 计算下一次调整的中间位置 */
			pHandle->hCurPosition = pHandle->hStartPosition + ((S_PROFILE[pHandle->hCurIndex]*pHandle->hError)>>16);
			/* 调整位置总共需要S_TOTAL次 */
			pHandle->hCurIndex = (pHandle->hCurIndex+1) % S_TOTAL;
			/* 调整结束,设置位置为目标位置 */
			if(pHandle->hCurIndex == 0)
			{
				pHandle->hError = pHandle->bIsChanging = 0;
				//pHandle->hCurPosition = pHandle->hSetPosition;
			}
		}
	}

	FOCVars.hTeref = Pos_CalcTorqueReference(pHandle,SpeedPos);//计算参考转矩
}

• Pos_Control函数在中频任务中被调用
• NEXT_STEP_ERR为预期位置和实际位置的误差，小于这个值就认为位置已经收敛，进入S曲线的下一个step。否则在S曲线的一些平缓的位置，位置可能提前收敛，电机就会轻微地来回抖动。

4、位置位置API

void POS_SetPos(int16_t pos)
{
	PosCtrlM.hSetPosition = pos;
	PosCtrlM.hStartPosition = ENCODER_M._Super.hMecPosition;
	PosCtrlM.hError = PosCtrlM.hSetPosition - PosCtrlM.hStartPosition;
	PosCtrlM.bIsChanging = 1;
	PosCtrlM.hCurIndex = 0;
}

三、总结

        这个方法的弊端就是无法精确控制旋转的时间，有最低速度的限制。整体来说没什么难度，参考一下就行了，一般不用在实际工业控制中。

        我的另一篇文章有介绍一种恒定Jerk(急动度，即\(\large{J = \frac{da}{dt}}\)的S曲线算法，此时速度-时间曲线为马鞍形，角度-时间曲线为S型，这种方法可以精确控制位置环的时间。

文章地址：PMSM FOC位置环S曲线控制算法(恒定急动度)