タグ: haya_imu

はじめに

DCモータに対する、PID制御とは、制御量(本文では減速機出力シャフトの回転速度、角速度といい)と、目標値(本文ではターゲット回転速度、目標角速度といい)の差分(本文ではエラーといい)を無くすための操作量(PWMパルス出力)に関わる、P(比例)制御、I(積分)制御、D(微分)制御をかけるとのこと。ロータリエンコーダは回転速度を安易に計算可能な要素(A相／B相パルス)を提供する、常用デバイスとして利用される。本実験の趣旨としては、回転速度の計算ベースとなる、エンコーダのA／B相パルスは正しく計測されたかどうか、またPID制御の精度、確度を評価するに当たって、他のデバイス、本実験ではhaya_imu(ICM-42688、MMC5983MA内蔵)を活用して評価すること。

実験環境

●12V入力DCブラシモータ
●12V出力DC電源(Motor Shieldへ給電)
●磁気式エンコーダ
●Arduino Leonardo正規品基板
●Adafruit Motor Shield v1.2互換基板(ライブラリ修正あり、Timer3A/D5-pin/PWM周波数を4KHzへ変更済み)
●9軸センサhaya_imu v3.2
●Raspberry Pi 3 Model-B(ROS Slave、Unbuntu20.04／Noetic、haya_imuへ接続)
●5V出力DC電源(Raspberry Piへ給電)
●Lenovo L560(ROS Master、Unbuntu18.04／Melodic、Arduino Leonardoへ接続)

開発要素

ROSパッケージ

以下ROSパッケージ、ROSカスタムメッセージまたはライブラリをインストールして、catkin_makeしておく。

rosserial
haya_motor_ros
haya_imu_ros
ros_lib by rosserail_arduino with Arduino IDE
Adafruit-Motor-Shield-library-leonardo
arduino_node(ino) on Leonardo with Arduino IDE 

ROSノード

/arduino_haya_motor
/haya_imu_node
/haya_motor_node
/rosout
/rqt_plot

ROSトピック

/arduino_msg
/diagnostics
/haya_angular_velocity_z
/imu_data
/pid_params
/rosout
/rosout_agg

ROSカスタムメッセージ

haya_imu_ros/ImuData
haya_motor_ros/ArduinoMsgs
haya_motor_ros/PidMsgs

ROSノードの相互作用状況

Arduinoノード

エンコーダA相パルス割込みハンドラ

  if (digitalRead(ENCODER_A) != digitalRead(ENCODER_B)) {
    current_counter++;
  }
  else {
    current_counter--;
  }

エンコーダB相パルス割込みハンドラ

  if (digitalRead(ENCODER_A) == digitalRead(ENCODER_B)) {
    current_counter++;
  }
  else {
    current_counter--;
  }

エンコーダカウンタによる回転速度

// Run the PID loop at 100 times per second
#define PID_RATE_HZ 10 // 10Hz
// Convert the rate into an interval
#define PID_INTERVAL_MS (1000 / PID_RATE_HZ) // 10ms
#define PID_INTERVAL_S (float)(1.0 / PID_RATE_HZ) // 0.1s
#define PPR 12 // Encoder pulse / round
#define COUNTER_FACTOR 4 // 1*encoder-pulse = 4*counter
#define REDUCTION_RATIO 100 // Gear reduction ratio
#define CPR (PPR * COUNTER_FACTOR * REDUCTION_RATIO) // Counters of encoder / round
current_velocity = (current_counter - previous_counter) * 360.0 / (CPR * PID_INTERVAL_S);

PID係数

本実験環境で何回も実験を繰り返して、納得いくPID係数Kp、Ki、Kdを決めておく。

PID操作量

●Position法

  error = target_velocity - current_velocity;
  integral += error * PID_INTERVAL_S;
  differential = (error - previous_error) / PID_INTERVAL_S;
  previous_error = error;
  pwm = (int32_t)(Kp * error + Ki * integral + Kd * differential);

●Increment法

  error = target_velocity - current_velocity;
  proportion = Kp * (error - previous_error);
  integral = Ki * error * PID_INTERVAL_S;
  differential = Kd * (error - 2 * previous_error + previous_error2) / PID_INTERVAL_S;
  pwm += (int32_t)(proportion + integral + differential);
  previous_error2 = previous_error;
  previous_error = error;

実験手順

Run roscore

Run roscore@ROS Master

roscore

Launch haya_imu_node

Launch haya_imu_node@ROS Slave

roslaunch haya_imu_ros haya_imu.launch

Launch haya_motor_node

Launch haya_motor_node@ROS Master

roslaunch haya_motor_ros haya_motor.launch

結果確認

PID制御の目標値、制御量

以下のデータを表示させて、エンコーダ、IMUからのカウンタによる回転速度を確かめて、PID係数Kp、Ki、Kdを決めてまた、エンコーダまたは、PID制御を評価する。PWM周波数をデフォルトの490Hzから4KHzに変更することで回転速度の分散は小さくなっている。これは490Hzより4KHz周波数のほうはモータにブレーキをかけずにPWM波形は滑らかになって果たしてモータ出力シャフトの回転速度も滑らかになったわけである。

/haya_angular_velocity_z 
/pid_params/target_velocity
/arduino_msg/current_velocity

haya_imuで測った回転速度が、エンコーダによる回転速度より分散が大きくなって、これは測定法および測定ポジションは違って、またモータシャフトによる振動や、慣性力に由来するものと考えられる。

PID制御の目標値、制御量、操作量

PID制御の目標値、PWM操作量、エンコーダによる回転速度

PID制御の操作量PWM波形

オシロスコープは測った操作量PWMの波形、モータコイルに起因する逆起電力による、コイルサージは約-14Vあったと、モータシールド実装モータドライバL293Dにコイルサージ(逆起電流)を流すダイオードが効いていると考えられる。ないと一瞬-12Vの数十倍～-1千万V(無限大)に達する可能性ある。これで回路で繋がっているデジタルICを一発でぶち壊すことになる。

エンコーダ出力パルス波形

約1/10の確率でノイズに起因するスパイクが起きるらしいので、MCUにノイズフィルタ実装の入力PINは望ましい。

リポジトリ

haya_motor_ros、arduino_node、修正ありMotor Shieldライブラリ含むリポジトリは、https://github.com/soarbear/haya_motor_ros に公開済み(BSD 3-Clause License)

最後に

IMUはモータシャフトでなくロボット本体に装着した場合、エンコーダのみならずLidar、カメラ、GPS他のセンサーとフュージョンする場合多い。

参考記事

●ロータリエンコーダによる速度計算
●9軸センサーICM-42688+MMC5983 6軸&9軸回転ベクトル&3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続