MC SDK 5.x中增加位置环
前言
在MC SDK 5.x中有两个环路控制，分别是电流环和速度环，有些应用需要使用到位置环，比如四轴飞行器的云台电机控制，地铁闸机等等应用，本文将阐述如何在现有ST电机库基础上增加简易的位置环。
01理论基础
以表贴电机为例，一般的FOC电机控制是两环控制，速度环+电流环的控制方式，速度环为外环，电流环为内环，
Idref = 0（d轴电流参考为0）的控制为常见控制；参考速度环输出参考转矩后供后端电流环路；
如果增加位置环最简单和直接的方式即为将速度环换为位置环，即变为位置环+电流环的方式：
如果在有减速齿轮电机上，需要加入速度环以便更快速的到达给定位置，也就是三环控制：位置环+速度环+电流环，框图如下：
为了方便控制，以及考虑精度问题，角度单位为rad*10000，比如电机转动10圈，也就是10圈后的位置，则该位置的设定角度为：
位置环采用的是线性系数乘以系数直接输出到速度的参考:
当前位置的角度数字量计算如下：
02位置环控制改进
在三环控制中，不可避免的会涉及到加速度的计算，正常控制过程为：加速→匀速→减速→定位过程
那么会涉及到加速度以及定位阶段的控制，这边我们可以按照加速度与速度差成反比，而定位阶段可以直接设定速度为0，但
是实际使用过程中我们会发现直接直接的速度模式在定位阶段力量有限，方法一：PID 需要特别设定，更能够定位到给定位
置，方法二：抛开速度环，直接力矩控制，实际控制框图如下：
03具体操作步骤
3.1创建编码器可以正常运行的工程
首先要建立可以正常运行的位置传感器为编码器（ENCODER）的电机工程，本文所使用的是NUCLEO-F303RE（控制板） + IHM07M1（功率板） + Shinano LA052-080E3NL1（电机）
3.2生成工程并调试运行
产生编码器工程并且保证电机正常运行，为后续动作做准备，同时配置DAC的两路User数据输出
3.3增加以下变量或函数用于位置环控制
1) 位置环PID结构体PID_Handle_t PIDAngleHandle_M1
2) 位置控制的结构体Position_Handle_t
3) 位置角度误差Position_GetErrorAngle
4) 位置环速度参考输出计算Position_CalcSpeedReferrence
5) 位置环力矩参考输出计算Position_CalcTorqueReferrence
具体参考mc_position.h以及mc_position.c
3.4 mc_task.c文件修改
3.4.1增加头文件
mc_task.c中包含头文件
3.4.2初始化位置环PID参数
MCboot()函数中增加角度PID初始化函数
3.4.3定位后初始测量角度清零
3.4.4增加DAC输出测试程序
函数TSK_HighFrequencyTask()中增加用户DAC输出信号更新数据，将目标角度（位置）以及测量角度（位置）通过DAC输出到示波器
3.4.5增加位置控制程序
在mc_task.c的中频任务函数TSK_MediumFrequencyTaskM1中增加位置环差值数据计算，根据差值计算，当差值在阈值之上的话进行速度控制，否则则进行力矩控制，在RUN下调用下面代码：
（滑动图片浏览，点击图片放大查看）
3.4.6修改参考电流计算
如果差值缩小到一定范围内，则进行力矩控制，在函数FOC_CalcCurrRef中进行调用
3.5电机累计圈数计算
stm32f30x_mc_it.c文件的 SPD_TIM_M1_IRQHandler函数中增加编码器位置累加程序用于计算电机转动的圈数，用于多圈马达转动控制；
04增加测试代码
在main函数中while(1)中增加测试代码，我们先用0—90度电机一圈内的转动来测定角度控制PID
05测定角度PID参数
可以使用DAC将参考电角度和实际电角度输出，观察角度的变化，调节位置环的PID参数，从而达到快速准确的位置定位；定位实现后可以手动炫动电机，看到电机依然可以正常回归到给定位置。数据输出范围限制，可以使用0到90度来回位置变动来测试；
下面几张图是调整不同位置PI参数得到的不同运行结果，同样是由0度角变动为90度角的过程；
Kp = 10000/4096, Ki = 0/32768, 波形震荡状态
Kp = 100/4096, Ki = 0/32768，实际电机位置明显滞后
Kp = 3000/4096, Ki = 2/32768，波形震荡后收敛
Kp = 3000/4096, Ki = 10/32768，基本上当前负载下基本稳定
06必要时增加微分项调节
如果需要快速调节，需要增加微分项，也就是PID调节，库中的微分项默认是屏蔽的，因此，需要修改pid_regulator.c中的代码，打开PID函数，如下图：
07电机转动多圈位置环测试
将位置设定为0—50*360度（电机转动50圈）的位置变动
实际运行效果
位置偏差较大时，电机加速到最大速度，偏差小时将进入力矩控制模式，直到目标位置，在目标位置直接力矩锁定。
设定位置角度值在0→3141600 (电机转动50圈) 实际速度与参考速度曲线
08扩展说明
1. 本文中使用的是相对位置编码器，如果实际过程中所使用的是绝对位置编码器，则需要增加读取绝对位置编码器的程序；
2. 如果是线性位置运动，则位置参量需要修改为角度所对应的线性位置，从而进行有效运算；
3. 对于PID调节部分本文只是根据实际波形DAC输出进行调整，理论以及有赖于后期研究；
4. 三环控制环路，两两环路调整比较好些，调好后再进行三环测试。
5. 本文的位置输出到速度的Kp值没细致考量，这个部分可根据实际应用自行调整。
|
楼主
标题置顶
标题高亮
