1. FOC简介
最近项目中需要使用三相无刷电机带动激光做测距，之前从未接触过三相无刷电机的我来说是一个不小的挑战，起初觉得不就是一个电机嘛，小意思啦，后面越看资料我越慌，越看越觉得我对他一无所知，顿时不知从何下手。但是也没办法，老板给我发工资我就要给老板搬砖，在找资料的过程中慢慢的FOC这个三个英文字母映入我的眼帘，但它是做什么的、有什么用、为什么要使用、涉及到哪些技术等等我是一无所知。
好啦，来简单介绍一下什么是FOC，以及我们为什么要使用它，使用他的好处有哪些？带着这些问题我们来进一步学习。
FOC的全称Field Oriented Control，翻译为中文是磁场定向控制，通过数学中的坐标变换的方式实现三相电机的转矩和励磁控制，其输入为电机的位置（角度、速度）和U、V、W三相的任意两相的电流值。FOC算法有哪些优点呢？FOC算法可以让电机在负载变化时快速的响应且精确，因为使用了逆变技术，使用正弦波驱动电机，因此电机运行时的噪音也很小，可以做一个安静的美男子，这一点有刷电机是无法做到的，且由于三相无刷电机取消了换向的刷头，使用寿命上也比有刷电机更长，也可以避免有刷电机在换向时可能产生的电火花影响外围电路或干扰系统中的其他易受干扰的模块。
2. FOC可以做什么
如图，可以做机器人的关节电机以及一些需要精准控制的场所。
3. 三相无刷电机的结构
三相无刷电机结构的简易图如下，采用星形连接，因此当我们两两通电的时候根据右手螺旋定则就可以判断出转子的位置。
当给AB相、AC相、BC相、BA相、CA相、CB相通电时，其电流流向及产生的磁场如下图所示：
上图的六个过程即完成了六步换相，通过不断改变磁场的方向，牵引着转子旋转。
4. FOC控制原理
对于我这种从来没有接触过这种电机，在刚接触FOC算法的时候感觉整个人都是晕乎乎的，网上找到的资料都是什么Park变换、Clarke变换以及他们的反变换，还有SVPWM、PID、电流环等等，PID算法都还好，之前接触过算是老朋友了，对于涉及到的其他技术根本是一脸懵逼，即使很难我们也要学，增强一下自身的技术同时拓展一些知识面。
下面我们先从FOC控制的总体框图来大概了解一下，如下图（PPT纯手绘，创作不易啊）
从框架图山可以看出，基本的FOC控制算法中包含三个环，从内往外分别是电流环、速度环、位置环，在刚开始想要驱动电机的时候我们可以先使用开环的方式去驱动，不做位置和速度的反馈，开环做好后我们再加入反馈环节调试。
我们先抛开反馈控制，可以看出实际驱动电机的实际是马鞍波，马鞍波是通过反Park变换以及SVPWM实现的，这里首先对反Park变换详细说明一下：
（1）反Park变换
从框图上可以看出反Park变换的输入为Ud和Uq，输出为Uα和Uβ，d表示磁场，q表示转矩，α和β为我们自己定义的两个轴，便于坐标变换。
上图为反Park变换的坐标系图，由高中数学学过的三角函数能够很容易的根据Ud和Uq以及角度q计算出Vα和Vβ，其公式如下：
好了，至此反Park变换我们知道了，重要的就是公式和推导过程，其实这个变换也没啥推导过程，坐标系一画出来就可以看出来啦。
在经过反Park变换后，我们的输出就变换成两相的交流啦，然后经过SVPWM技术就可以驱动电机了，在这里稍晚一点介绍SVPWM技术。我们继续学习下一个变换，Clarke变换。在正常开环驱动电机转动之后我们就要引入闭环控制了，能够更快、更精确的控制电机，也大大的提高了电机的效率。
作为反馈我们这里主要使用运放加ADC采集三相电机中的任意两相的电流，把采集到的电流输入Clarke变换中。为什么三相电流呢？由基尔霍夫电流定律（KCL）可以知道，在流过同一个节点的电流，其流入和流出的和为0，因此当我们知道了三相中的任意两相电流即可根据KCL计算出第三相的电流值。
（2）Clarke变换
采集电机的三相电流ia,ib,ic，通过坐标变换到α和β轴上，使其变为两相静止绕组。
上图的ia、ib、ic为采集到的三相的电流的坐标系，互成120度，α和β为我们定义的坐标系，通过该变换后可以将三相的交流信号编程两相的交流信号。其公式如下：
至于怎么得到下面绿色框中的公式建议大家自己画一下坐标系推导一下，便于理解，在这里我就不去推导啦。这一步做完是不是就把三相坐标系变幻成了直角坐标系啦，我们分析也会更简单，但是MCU似乎处理起来还是有点不行啊，于是我们接着变换，通过Park变换把两相的交流变换成直流信号给单片机处理，这样就可以把Park的输出反馈给MCU处理啦。
（3）Park变换
我们怎么才能得到直流的信号呢，假设两个坐标轴Iq和Id，这两个轴是跟随转子转动的，将Clarke变换后的α和β轴变换到跟随着转子旋转的两轴坐标上（q轴、d轴）分别表示转矩和磁场。这样由于d、q是跟随转子转动的，相对于转子是静止的，是不是就得到了直流的信号呢。坐标系变化图解如下：
q为转子和α和β轴的实时角度，把α和β轴变换q轴、d轴上也就很简单啦，公式如下：
至此FOC涉及到的坐标系变换就全部说完啦，有些人可能会有疑问，为什么Park有反Park变换，Clarke怎么就没有其反变换呢？其实是有反Clarke变换的，只是我们在控制电机的时候使用SVPWM替代了，因此FOC中也就省去了反Clarke，这里可以简单介绍一下，反Clarke变换就是把反Park变换的结果作为输入，输出三相的交流信号，三相交流信号相位相差120度。下面我们介绍一下SVPWM技术。
（4）SVPWM
好了，FOC的核心算法基本介绍的就差不多啦，
5. 坐标变换的算法实现
（1）反Park变换
1. /**
2. * @brief 反park变换:将同步旋转坐标系下的vqd，变换为静止坐标系的v_alpha,v_beta
3. * @param volt_input: vqd
4. * @param theta: 电角度值
5. */
6. alphabeta_t mc_rev_park(qd_t volt_input, int16_t theta)
7. {
8. int32_t q_v_alpha_tmp1, q_v_alpha_tmp2, q_v_beta_tmp1, q_v_beta_tmp2;
9. trig_components local_vector_components;
10. alphabeta_t volt_output;
11.
12. local_vector_components = mc_trig_functions(theta);
13.
14. q_v_alpha_tmp1 = volt_input.q * ( int32_t )local_vector_components.h_cos;
15. q_v_alpha_tmp2 = volt_input.d * ( int32_t )local_vector_components.h_sin;
16.
17. volt_output.alpha = ( int16_t )( ( ( q_v_alpha_tmp1 ) + ( q_v_alpha_tmp2 ) ) >> 15 );
18.
19.
20. q_v_beta_tmp1 = volt_input.q * ( int32_t )local_vector_components.h_sin;
21. q_v_beta_tmp2 = volt_input.d * ( int32_t )local_vector_components.h_cos;
22.
23. volt_output.beta = ( int16_t )( ( q_v_beta_tmp2 - q_v_beta_tmp1 ) >> 15 );
24.
25. return (volt_output);
26.}
27.
28./**
29. * @brief 根据输入的电角度，计算三角正弦和余弦
30. * @param h_angle: 电角度值
31. */
32.trig_components mc_trig_functions( int16_t h_angle )
33.{
34. int32_t shindex;
35. uint16_t uhindex;
36.
37. trig_components local_components;
38.
39. /* 10 bit index computation */
40. shindex = ( ( int32_t )32768 + ( int32_t )h_angle );
41. uhindex = ( uint16_t )shindex;
42. uhindex /= ( uint16_t )64;
43.
44.
45. switch ( ( uint16_t )( uhindex ) & SIN_MASK )
46. {
47. case U0_90:
48. local_components.h_sin = h_sin_cos_table[( uint8_t )( uhindex )];
49. local_components.h_cos = h_sin_cos_table[( uint8_t )( 0xFFu - ( uint8_t )( uhindex ) )];
50. break;
51.
52. case U90_180:
53. local_components.h_sin = h_sin_cos_table[( uint8_t )( 0xFFu - ( uint8_t )( uhindex ) )];
54. local_components.h_cos = -h_sin_cos_table[( uint8_t )( uhindex )];
55. break;
56.
57. case U180_270:
58. local_components.h_sin = -h_sin_cos_table[( uint8_t )( uhindex )];
59. local_components.h_cos = -h_sin_cos_table[( uint8_t )( 0xFFu - ( uint8_t )( uhindex ) )];
60. break;
61.
62. case U270_360:
63. local_components.h_sin = -h_sin_cos_table[( uint8_t )( 0xFFu - ( uint8_t )( uhindex ) )];
64. local_components.h_cos = h_sin_cos_table[( uint8_t )( uhindex )];
65. break;
66. default:
67. break;
68. }
69. return ( local_components );
70.}
（2）Clarke变换
1. alphabeta_t mc_clark(ab_t input)
2. {
3. alphabeta_t output;
4.
5. int32_t a_divSQRT3_tmp, b_divSQRT3_tmp ;
6. int32_t wbeta_tmp;
7. int16_t hbeta_tmp;
8.
9. /* qIalpha = qIas*/
10. output.alpha = input.a;
11.
12. a_divSQRT3_tmp = divSQRT_3 * ( int32_t )input.a;
13.
14. b_divSQRT3_tmp = divSQRT_3 * ( int32_t )input.b;
15.
16. /*qIbeta = -(2*qIbs+qIas)/sqrt(3)*/
17. wbeta_tmp = ( -( a_divSQRT3_tmp ) - ( b_divSQRT3_tmp ) -
18. ( b_divSQRT3_tmp ) ) >> 15;
19.
20.
21. /* Check saturation of Ibeta */
22. if ( wbeta_tmp > INT16_MAX )
23. {
24. hbeta_tmp = INT16_MAX;
25. }
26. else if ( wbeta_tmp < ( -32768 ) )
27. {
28. hbeta_tmp = ( -32768 );
29. }
30. else
31. {
32. hbeta_tmp = ( int16_t )( wbeta_tmp );
33. }
34.
35. output.beta = hbeta_tmp;
36.
37. if ( output.beta == ( int16_t )( -32768 ) )
38. {
39. output.beta = -32767;
40. }
41.
42. return ( output );
43.}
（3）Park变换
1. qd_t mc_park(alphabeta_t input, int16_t theta)
2. {
3. qd_t output;
4. int32_t d_tmp_1, d_tmp_2, q_tmp_1, q_tmp_2;
5. trig_components local_vector_components;
6. int32_t wqd_tmp;
7. int16_t hqd_tmp;
8.
9. local_vector_components = mc_trig_functions(theta);
10.
11. /*No overflow guaranteed*/
12. q_tmp_1 = input.alpha * ( int32_t )local_vector_components.h_cos;
13.
14. /*No overflow guaranteed*/
15. q_tmp_2 = input.beta * ( int32_t )local_vector_components.h_sin;
16.
17. /*Iq component in Q1.15 Format */
18. wqd_tmp = ( q_tmp_1 - q_tmp_2 ) >> 15;
19.
20.
21. /* Check saturation of Iq */
22. if ( wqd_tmp > INT16_MAX )
23. {
24. hqd_tmp = INT16_MAX;
25. }
26. else if ( wqd_tmp < ( -32768 ) )
27. {
28. hqd_tmp = ( -32768 );
29. }
30. else
31. {
32. hqd_tmp = ( int16_t )( wqd_tmp );
33. }
34.
35. output.q = hqd_tmp;
36.
37. if ( output.q == ( int16_t )( -32768 ) )
38. {
39. output.q = -32767;
40. }
41.
42. /*No overflow guaranteed*/
43. d_tmp_1 = input.alpha * ( int32_t )local_vector_components.h_sin;
44.
45. /*No overflow guaranteed*/
46. d_tmp_2 = input.beta * ( int32_t )local_vector_components.h_cos;
47.
48. wqd_tmp = ( d_tmp_1 + d_tmp_2 ) >> 15;
49.
50. /* Check saturation of Id */
51. if ( wqd_tmp > INT16_MAX )
52. {
53. hqd_tmp = INT16_MAX;
54. }
55. else if ( wqd_tmp < ( -32768 ) )
56. {
57. hqd_tmp = ( -32768 );
58. }
59. else
60. {
61. hqd_tmp = ( int16_t )( wqd_tmp );
62. }
63.
64. output.d = hqd_tmp;
65.
66. if ( output.d == ( int16_t )( -32768 ) )
67. {
68. output.d = -32767;
69. }
70.
71. return (output);
72.}
（4）SVPWM
1. /**
2. * @brief 根据v_alpha_beta计算svpwm
3. */
4. void svpwm_calc(alphabeta_t v_alpha_beta)
5. {
6. int32_t w_x, w_y, w_z, w_u_alpha, w_u_beta, w_time_ph_a, w_time_ph_b, w_time_ph_c;
7.
8. uint8_t sector;
9.
10. w_u_alpha = v_alpha_beta.alpha * (int32_t)hT_Sqrt3;
11. w_u_beta = -(v_alpha_beta.beta * ( int32_t )(PWM_PERIOD_CYCLES) ) * 2;
12.
13. w_x = w_u_beta;
14. w_y = ( w_u_beta + w_u_alpha ) / 2;
15. w_z = ( w_u_beta - w_u_alpha ) / 2;
16.
17. /* Sector calculation from w_x, w_y, w_z */
18. if (w_y < 0) {
19. if (w_z < 0) {
20. sector = SECTOR_5;
21. w_time_ph_a = ( int32_t )( PWM_PERIOD_CYCLES ) / 4 + ( ( w_y - w_z ) / ( int32_t )262144 );
22. w_time_ph_b = w_time_ph_a + w_z / 131072;
23. w_time_ph_c = w_time_ph_a - w_y / 131072;
24. //pSetADCSamplingPoint = pHandle->pFctSetADCSampPointSect5;
25. } else if (w_x <= 0) { /* w_z >= 0 */
26. sector = SECTOR_4;
27. w_time_ph_a = ( int32_t )( PWM_PERIOD_CYCLES ) / 4 + ( ( w_x - w_z ) / ( int32_t )262144 );
28. w_time_ph_b = w_time_ph_a + w_z / 131072;
29. w_time_ph_c = w_time_ph_b - w_x / 131072;
30. //pSetADCSamplingPoint = pHandle->pFctSetADCSampPointSect4;
31. } else { // w_x > 0
32. sector = SECTOR_3;
33. w_time_ph_a = ( int32_t )( PWM_PERIOD_CYCLES ) / 4 + ( ( w_y - w_x ) / ( int32_t )262144 );
34. w_time_ph_c = w_time_ph_a - w_y / 131072;
35. w_time_ph_b = w_time_ph_c + w_x / 131072;
36. //pSetADCSamplingPoint = pHandle->pFctSetADCSampPointSect3;
37. }
38. } else { // w_y > 0
39. if ( w_z >= 0 )
40. {
41. sector = SECTOR_2;
42. w_time_ph_a = ( int32_t )( PWM_PERIOD_CYCLES ) / 4 + ( ( w_y - w_z ) / ( int32_t )262144 );
43. w_time_ph_b = w_time_ph_a + w_z / 131072;
44. w_time_ph_c = w_time_ph_a - w_y / 131072;
45. //pSetADCSamplingPoint = pHandle->pFctSetADCSampPointSect2;
46. }
47. else /* w_z < 0 */
48. if ( w_x <= 0 )
49. {
50. sector = SECTOR_6;
51. w_time_ph_a = ( int32_t )( PWM_PERIOD_CYCLES ) / 4 + ( ( w_y - w_x ) / ( int32_t )262144 );
52. w_time_ph_c = w_time_ph_a - w_y / 131072;
53. w_time_ph_b = w_time_ph_c + w_x / 131072;
54. //pSetADCSamplingPoint = pHandle->pFctSetADCSampPointSect6;
55. }
56. else /* w_x > 0 */
57. {
58. sector = SECTOR_1;
59. w_time_ph_a = ( int32_t )( PWM_PERIOD_CYCLES ) / 4 + ( ( w_x - w_z ) / ( int32_t )262144 );
60. w_time_ph_b = w_time_ph_a + w_z / 131072;
61. w_time_ph_c = w_time_ph_b - w_x / 131072;
62. //pSetADCSamplingPoint = pHandle->pFctSetADCSampPointSect1;
63. }
64. }
65.
66. TIM1->CCR1 = ( uint16_t )w_time_ph_a;
67. TIM1->CCR2 = ( uint16_t )w_time_ph_b;
68. TIM1->CCR3 = ( uint16_t )w_time_ph_c;
69.
70.}
