RoboMaster電控入門（7）雙軸雲台控制

位置控制原理

最近一段時間一直在忙着寫官方的C型板教程，這個教程咕了兩個多月沒寫了，現在回來把坑填完。

C型板教程的寫作思路是首先介紹板子上各個外設的基本原理，講一下cube的配置和HAL庫的API，然后講解示例工程，論詳細度和專業度比我這邊這個教程要強多了，語言上也比我這邊寫的要嚴謹多了哈哈哈，鏈接如下，希望各位關注關注。

https://github.com/RoboMaster/Development-Board-C-Examples

回歸正題，RM機器人上最復雜的控制計構就是雙軸雲台了，賽場上的情況對雙軸雲台的控制穩定度與響應靈敏度雙方面都提出了很高的要求，雲台控制的好壞在一定程度上就能夠代表一支隊伍的實力。

雙軸雲台采用的控制算法依然是PID控制算法，關於PID控制算法的原理與實現可以看之前的博客https://www.cnblogs.com/sasasatori/p/11672918.html

但雙軸雲台的控制與底盤控制存在直接的區別在於，底盤是一個單環控制系統，用戶對底盤的輸入值為底盤的速度期望，底盤的輸出也同樣是速度。而雲台則是一個雙環控制系統，用戶對雲台的輸入值為雲台的轉動角度期望，而雲台電機能夠直接輸出的是轉速。

輸入是角度的期望，輸出的是電機的速度。就是由於這個原因，必須多引入一級控制器，實現將角度控制轉換成速度期望，然后再通過第二級控制器完成速度的閉環控制，這就是為什么雲台一定要使用雙環控制系統。

針對雲台的控制系統的結構，我們可以畫出簡化的系統框圖如下：

第一環控制器被稱為角度控制器（Angle_Controller），因為它的輸入為期望角度和實際角度之間的差值。第二環控制器被稱為速度控制器（Speed_Controller)，因為它的輸入為期望速度和實際速度之間的差值。

我們給整個系統的輸入值reference就是整個雲台的角度期望，將其與實際的輸出角度angle_feedback（這個值一般是雲台電機編碼器的反饋角度值或者IMU反饋的角度值）做差，將差值作為角度控制器（Angle_controller）的輸入，計算第一環的PID，角度控制器的輸出值直接作為速度控制器（Speed_Controller）的輸入，輸入與角速度的反饋值（一般是IMU反饋的角速度值或者將雲台編碼器的反饋角度值進行差分，在上面的系統圖中公式\(\frac{△u}{△t}\)表示對角度值求導獲得角速度）做差，得到速度控制器的輸出，該輸出值直接給電機。

一般第一次接觸雙環控制者的疑問是，為什么可以直接將角度控制器的輸出作為速度控制器的輸入？角度控制器的輸出似乎沒有什么物理含義，為什么就可以被看成是速度的期望？

從定性的角度來解釋這個問題，這是因為角度控制器的輸出確實在一定程度上反映了速度的期望，角度控制器的輸出值越大，說明總體上角度的誤差是比較大的，也就是目前的雲台位置和期望的雲台位置差的越遠，我們就會越希望雲台以更快的速度達到期望的位置，速度期望較大；而當雲台的實際位置和期望位置接近時，角度控制器的輸出較小，同樣我們會希望雲台盡可能細微的調節自身的位置，因此雲台的速度期望就會小。

由此可見位置控制器的輸出實際上和速度期望之間是存在正相關關系的，如果將其直接視作簡單的線性關系，我們就可以直接將位置控制器的結果作為速度控制器的輸入值。

當然，正相關的函數表達形式有很多，並不一定要采用線性的形式，采用線性的原因更多是因為線性的系統會更加的容易分析，如果引入非線性則系統在數學上的復雜度會增加很多。在部分工程中，我也見過使用位置控制器輸出的平方或者高次冪作為速度控制器的輸入的情況，這樣做的好處是系統在偏差較大的位置時響應更加靈敏，而偏差較小時響應變得不靈敏，但是參數調節方面也會變得困難一些，具體使用何種方案要根據實際情況來斟酌。

在simulink中搭建仿真系統，查看系統的輸出結果——輸入期望為黃線reference，輸出為橙線angle_feedback，可以看到系統的輸出是趨於期望的，總體來說是一個穩定的系統。

這個系統同樣可以信號與系統和自動控制理論的知識進行分析，實際上單環和雙環的系統在分析上不會有很大的差別，不過雙環控制系統不是簡單將兩個單環的系統函數直接相乘，速度環與位置環相互之間還是有影響的，在這里就不進行具體的數學推導了，感興趣的同學可以仿照我在第四章中所進行的數學推導過程，假設一個電機的傳遞函數，然后自己建立整個系統的閉環傳遞函數，分析兩級PID控制器各自的P，I，D參數的作用。

依然是推薦一下官方的相關教程

https://bbs.robomaster.com/thread-4941-1-1.html

https://bbs.robomaster.com/thread-5059-1-1.html

下面是一個位置隨動系統的課程設計報告，其中就對這個系統做了比較詳細的理論分析，對數學原理感興趣的同學可以重點閱讀。

https://wenku.baidu.com/view/0736ede9856a561252d36f14.html

可以看到報告中給出的系統結構框圖實際上和我們上面給出的框圖是一致的，①是位置環的PID控制器，②是速度環的PID控制器，③是電機的傳遞函數，④是對電機輸出的位置值求微分（拉普拉斯變換后，乘一階s因子相當於做一階微分，除以一階s因子相當於做一重積分）

上面這個系統的閉環傳遞函數如下：

\[\Phi(s) = \frac{K_{\epsilon}K_{a}K_{m}/i}{T_{m}s^2+(k_tk_ak_m+1)s+K_{\epsilon}K_{a}K_{m}/i} \]

有了閉環傳遞函數之后，這個系統的后續分析以及調節的方法就可以看報告了，系統一樣的情況下，報告中所使用的分析方法同樣是適用於雲台的雙環控制系統的。

代碼示例

這里依然是采用官方代碼來說明雙環PID控制的代碼實現，新開源的代碼結構和之前的不太一樣了，可讀性也變強了不少，鏈接如下。

https://github.com/RoboMaster/RoboRTS-Firmware

雲台控制的代碼和底盤控制非常相似，熟悉了底盤控制代碼的話應該會很容易理解。這段代碼里還加了一些和模式處理相關的東西，主要還是看我用注釋強調的部分的代碼

void gimbal_task(void const *argu)
{
  gimbal_time_ms = HAL_GetTick() - gimbal_time_last;
  gimbal_time_last = HAL_GetTick();
  
//---------------------------------------------------
  archive_index++;
  if (archive_index > 99) //system delay ms
    archive_index = 0;
  gimbal_attitude_archive[archive_index][0] = gim.sensor.yaw_gyro_angle;//gim.pid.yaw_angle_fdb;
  gimbal_attitude_archive[archive_index][1] = gim.pid.pit_angle_fdb;
  gimbal_attitude_archive[archive_index][2] = gim.pid.yaw_spd_fdb;
  gimbal_attitude_archive[archive_index][3] = gim.pid.pit_spd_fdb;
  get_index = archive_index + 50;
  if( get_index > 99)
    get_index -= 100;
//----------------------------------------------------
  
  mat_init(&yaw_kalman_filter.Q,2,2, yaw_kalman_filter_para.Q_data);
  mat_init(&yaw_kalman_filter.R,2,2, yaw_kalman_filter_para.R_data);
  
  //satori:下面這段代碼是根據雲台的模式進行初始化，雲台的模式不同會影響角度反饋值的輸入來源，pid角度的反饋值就是在下面的幾個模式處理函數之一中獲取的
  switch (gim.ctrl_mode)
  {
    case GIMBAL_INIT:
      init_mode_handler();
    break;
    
    case GIMBAL_NO_ARTI_INPUT:
      no_action_handler();
    break;

    case GIMBAL_FOLLOW_ZGYRO:
      closed_loop_handler();
    break;

    case GIMBAL_PATROL_MODE:
      shoot.c_shoot_cmd = 0;
      gimbal_patrol_handler();
    break;

    case GIMBAL_POSITION_MODE:
      pc_position_ctrl_handler();
    break;

    case GIMBAL_RELATIVE_MODE:
      pc_relative_ctrl_handler();
    break;

    default:
    break;
  }

  //satori:這里是第一級PID控制器，計算yaw軸和pitch軸的角度控制輸出，pid_calc的具體實現可以看第四章，其中對pid_calculate函數的詳細分析
  pid_calc(&pid_yaw, gim.pid.yaw_angle_fdb, gim.pid.yaw_angle_ref);
  pid_calc(&pid_pit, gim.pid.pit_angle_fdb, gim.pid.pit_angle_ref);
  
  //satori:直接將yaw軸和pitch軸第一級控制器的輸出結果作為第二級速度控制器的輸入
  gim.pid.yaw_spd_ref = pid_yaw.out;
  gim.pid.pit_spd_ref = pid_pit.out;
  
  //satori:從傳感器獲取yaw軸和pitch軸的角速度反饋值
  gim.pid.yaw_spd_fdb = gim.sensor.yaw_palstance;
  gim.pid.pit_spd_fdb = gim.sensor.pit_palstance;
  
  //satori:第二級PID控制器，計算yaw軸和pitch軸的角速度控制輸出
  pid_calc(&pid_yaw_spd, gim.pid.yaw_spd_fdb, gim.pid.yaw_spd_ref);
  pid_calc(&pid_pit_spd, gim.pid.pit_spd_fdb, gim.pid.pit_spd_ref);

  //satori:下面這段代碼將yaw軸和pitch軸的輸出結果裝入控制電流的數組
  /* safe protect */
  if (gimbal_is_controllable())
  {
    glb_cur.gimbal_cur[0] = YAW_MOTO_POSITIVE_DIR*pid_yaw_spd.out;
    glb_cur.gimbal_cur[1] = PIT_MOTO_POSITIVE_DIR*pid_pit_spd.out;
    glb_cur.gimbal_cur[2] = pid_trigger_spd.out;
  }
  else
  {
    memset(glb_cur.gimbal_cur, 0, sizeof(glb_cur.gimbal_cur));
    gim.ctrl_mode = GIMBAL_RELAX;
    //pid_trigger.iout = 0;
  }
  
  yaw_angle_ref_js = gim.pid.yaw_angle_ref*1000;
  yaw_angle_fdb_js = gim.pid.yaw_angle_fdb*1000;
  yaw_speed_ref_js = pid_yaw.out*1000;
  yaw_speed_fdb_js = gim.sensor.yaw_palstance*1000;

  pit_angle_ref_js = gim.pid.pit_angle_ref*1000;
  pit_angle_fdb_js = gim.pid.pit_angle_fdb*1000;
  pit_speed_ref_js = pid_pit.out*1000;
  pit_speed_fdb_js = gim.sensor.pit_palstance*1000;

  //satori:觸發can發送任務，將yaw軸和pitch軸的速度控制輸出通過can總線發送給電調
  osSignalSet(can_msg_send_task_t, GIMBAL_MOTOR_MSG_SEND);
  osSignalSet(shoot_task_t, SHOT_TASK_EXE_SIGNAL);

  gimbal_stack_surplus = uxTaskGetStackHighWaterMark(NULL);

}

調參技巧

本來不太計划寫調參技巧的，在第四章的教程中也說過，PID調參如果不采用系統辨析，或者是對系統建模然后做理論分析的話，更多就是靠經驗來做的事情，各人有各人的一套調參習慣，對於各個參數的認知也不太習慣（哪怕是我和我一個隊的隊友也經常在調參時產生一些分期），所以我這里說的也只是我自己調參時的一些個人經驗，拿出來和大家分享分享。

1. 調參順序，可以先角度后速度，也可以先速度后角度，但不管是哪種順序，都一定要保證速度環的系數不全為0，否則相當於速度環沒有在工作，始終沒有任何輸出提供給電機，電機一點都不會動的。
2. 如果是先速度后角度的話，角度環所有系數設置為0時，相當於速度環的期望一直為0，把這個翻譯成人話就是調試時，雲台的角度應該用手掰到任何一個位置都能自然的停住不動，如果用手緩慢的掰動雲台，應該能感受到明顯有反抗的力，但是這個力的大小不會隨着角度的變化而變化，而且雲台的位置也是被推動着改變的。如果手掰動雲台的力度變化，則用力越大，反抗的力就也越大。
3. 如果是先角度后速度的話，速度環可以只給比例項賦值為1，另外兩項設成0。這樣做的主要目的是為了在調角度環時盡量閉環速度環內參數帶來的影響。這個時候如果用手掰雲台的話，除了感受到反抗的力之外，松手后雲台角度應該是可以回到原來的位置的，而且手把雲台掰的角度越大，反抗的力就也應該越大。
4. 盡量不要來回修改兩個環的參數，基本的控制變量思想還是得有，控制其他參數不變的情況下針對某一個參數進行調節，調到可能的最好情況之后再更換下一個參數調節。每次調節出的最好參數記得做下記錄。
5. 整個系統的最終測試一般用給角度環階躍信號作為期望，或者沖激信號作為期望來進行測試。注意，只能給角度環階躍輸入，如果把階躍輸入給速度環的話，電機就會開始360°旋轉了。

我個人的習慣一般還是先位置后速度。雲台的參數調節除了和PID參數有關之外，還和控制周期，傳感器反饋頻率，器件熱噪聲大小等因素有關，有的時候其實不需要復雜的參數就能有很好的控制效果（我們上個賽季的車子雲台雙環都只用了比例項系數就調出了不錯的控制效果），如果始終調不好的話需要去考慮一下有沒有可能是其他因素導致的原因。

結語

這次主要是聊了聊雙軸雲台控制有關的一些東西，這個做起來其實真的不是很難，很多人容易被雲台兩個字給嚇到，但是實際上上手了就會發現，雖然想要調的很好非常難（不然那些相機雲台也不會隨隨便便就大幾百上千的賣了），但是調到能夠適應比賽的程度不會非常困難的，重點還是多調，多積累經驗。