今回はMATLABで三軸PID制御を行う方法です。行っている処理それぞれコメントに書かれているとおりです。多分これを動かすと↓のようになるはずです。スクリーンショット 2020-10-16 232625


  1. %ワークスペース初期化
  2. close all
  3. clear
  4. clc
  5. clearvars

  8. FinishTime = 20;
  9. dt = 0.01;
  10. LogCount = 0;

  13. %物体の定義
  14. m = 1; %重量[kg]
  15. g = 9.8; %重力加速度[m/s^2]

  18. %位置,速度,加速度の定義
  19. Current_x = [0;0;0]; %初期位置[m]
  20. Current_v = [0;0;0]; %初期速度[m/s]
  21. Current_a = [0;0;0]; %初期加速度[m/s^2]

  24. %偏差の定義
  25. Error_x = [0;0;0]; %位置の偏差
  26. Past_Error = [0;0;0]; %過去の偏差
  27. I = [0;0;0]; %偏差の累積

  30. %PIDゲインの設定
  31. Kp = 1; %Pゲイン
  32. Ki = 0.2; %Iゲイン
  33. Kd = 1; %Dゲイン

  36. %目標位置の設定
  37. Target_x = [30;60;90]; %目標座標[m]

  40. %シミュレーションのループ
  41. for time = 0 : dt : FinishTime %FinishTimeまでdtで増加
  43. %PID制御
  44. Error_x = Target_x - Current_x; %偏差の算出
  45. I = I + Error_x * dt; %偏差の累積
  46. F = Error_x * Kp + (((Error_x - Past_Error) / dt) * Kd + I * Ki); %PID制御
  47. F = F + [0;0;m*g]; %重力加速度の補正
  48. Past_Error = Error_x; %過去の偏差を更新
  50. %位置,速度,加速度の計算
  51. Current_a = (F + [0;0;-m*g]) / m; %現在の加速度を算出[m/s^2] F+(重力)　= m*a
  52. Current_v = Current_v + Current_a * dt; %現在の速度を算出[m/s] v = v0+a*t
  53. Current_x = Current_x + Current_v * dt + 0.5 * Current_a * (dt)^2; %現在の位置を算出[m] x = x0+v0*t+0.5*a*t^2
  55. %位置,時間,の記録
  56. LogCount = LogCount + 1; %カウントを加算
  57. Log.X(LogCount) = Current_x(1); %現在位置の記録
  58. Log.Y(LogCount) = Current_x(2);
  59. Log.Z(LogCount) = Current_x(3);
  60. Log.nX(LogCount) = Target_x(1); %目標位置の記録
  61. Log.nY(LogCount) = Target_x(2);
  62. Log.nZ(LogCount) = Target_x(3);
  63. Log.Time(LogCount) = LogCount *dt - dt; %微小時間から秒に変換
  65. end

  68. %%シミュレーションの結果処理（グラフ出力）
  69. figure(1)
  70. hold on
  71. plot(Log.Time, Log.X)
  72. plot(Log.Time, Log.nX)
  73. plot(Log.Time, Log.Y)
  74. plot(Log.Time, Log.nY)
  75. plot(Log.Time, Log.Z)
  76. plot(Log.Time, Log.nZ)
  77. xlabel('Time t[s]')
  78. ylabel('X Position[m]')
  79. hold off