エンコーダ付きDCモータPID制御の実験-hayate_imu応用例

はじめに

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

実験環境

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

hayate_imu用いてDCモータPID制御評価環境
hayate_imu用いてDCモータPID制御評価環境

開発要素

ROSパッケージ

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

rosserial
hayate_motor_ros
hayate_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_node
/hayate_imu_ros
/hayate_motor_node
/rosout
/rqt_plot

ROSトピック

/arduino_msg
/diagnostics
/hayate_angular_velocity_z
/imu_cali
/imu_data
/imu_demo
/pid_params
/rosout
/rosout_agg

ROSカスタムメッセージ

hayate_imu_ros/ImuCali
hayate_imu_ros/ImuData
hayate_motor_ros/ArduinoMsgs
hayate_motor_ros/PidMsgs

ROSノードの相互作用状況

エンコーダ付きDCモータPID制御の実験-ROS node & ROS topic
エンコーダ付きDCモータPID制御の実験-ROS node & ROS topic

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

roslaunch haya_imu_ros haya_imu.launch

Launch hayate_imu_node

Launch hayate_imu_node@ROS Slave

roslaunch hayate_imu_ros hayate_imu.launch

Launch hayate_motor_node

Launch hayate_motor_node@ROS Master

roslaunch hayate_motor_ros hayate_motor.launch
rqt_graph

結果確認

PID制御の目標値、制御量

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

/hayate_angular_velocity_z 
/pid_params/target_velocity
/arduino_msg/current_velocity
PID制御の目標値、エンコーダによる回転速度、hayate_imu測った回転速度曲線
PID制御の目標値、エンコーダによる回転速度、hayate_imu測った回転速度曲線

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

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

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

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

PID制御の操作量PWM波形

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

PID制御の操作量PWM波形
PID制御の操作量PWM波形

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

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

エンコーダパルス波形ノイズスパイクあり
エンコーダパルス波形ノイズスパイクあり

リポジトリ

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

最後に

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

参考記事

ロータリエンコーダによる速度計算
9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応

0

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応

V2.4 NEWフィーチャ

令和5年3月、hayate_imu v2.4リリース!
較正モードの追加により、ジャイロスコープ、加速度センサ、地磁気センサの初期バイアスは出荷時の測定のみならず、ユーザー様のもとでも測定することはできる。 ※ いまユーザー様のお手元にある旧バージョン製品のファームウェアのバージョンアップは、ユーザー様のもとで実施可能なので、詳細については、別途順次ご案内する。

はじめに

9軸IMU(型番hayate_imu)は、コロナ禍の中で開発した新商品、令和3年3月まで開発~製造、令和3年4月上旬の出荷と予定して、皆さんの学術研究にお役に立てるようと願って、どうぞご検討ご利用のほど宜しくお願い申し上げます。

製品紹介

9軸センサhayate imu、低消費電力プロセッサーCortexM0+、TDK MPU-9250後継機種である、1.71V低電圧で動作可能なICM-20948使用、6軸/9軸融合クォータニオン(四元数)はFPGA on chip(DMP3)から低遅延出力、別途ソフトでフュージョン必要なし、最大出力レート225Hz、同時に加速度(アクセル)3軸データ225Hz、角速度(ジャイロ)3軸データ225Hz、地磁気(コンパス)3軸データ70Hzまで出力可能、補正済み、ROS対応。ロボット、ドローンなど低遅延が必要とされる科学研究、電子機械の検証試作ヘの活用が期待される。

主な仕様

・ 型番 hayate_imu rev.C 6軸フュージョン or ver.B 9軸フュージョン切替可能
・ 内蔵チップ Cortex-M0+、TDK Invensense ICM-20948(9軸)実装 ※1
・ 外部接続 USB Type-Cコネクタ、USB +5V給電 ※2 ※3
・ 最大出力レート ※4
  - 6軸フュージョン or 9軸フュージョン回転ベクトル四元数 225Hz
  - 加速度(アクセル)3軸センサ  225Hz
  - 角速度(ジャイロ)3軸センサ  225Hz
  - 地磁気(コンパス)3軸センサ  70Hz

・ 測定レンジ
  - 加速度(アクセル)センサ  ±16g
  - 角速度(ジャイロ)センサ  ±2000dps
  - 地磁気(コンパス)センサ  ±4900µT

・ 消費電力 50mW以下(環境温度21℃の実測値)
・ 寸法 30mm × 31.4mm × 4.8mm(突起物含む)
・ 重量 4g以下
・ 取付穴 M3x4、隣り合う穴の中心間距離24.4mm

※1 内蔵Cortex-M0+とICM-20948間インターフェースはSPI(4Mbps)使用、加速度センサ(消耗)、角速度センサ(温度、ドリフト)、地磁気センサ(磁気変動)にダイナミック補正。
※2 USB対向装置OS環境 Ubuntu 16.04以降推奨。
※3 USB対向装置ROS環境 Kinetic以降推奨。
※4 最大出力レートはhayate imuの実力値、IMU対向装置(USB接続先)での実効値はその装置のリソース(CPUクロック周波数、メモリ容量・スピード)に関わる。

デモ情報

hayate_imu ROSパッケージ | Githubリポジトリ

9軸IMUセンサ ICM-20948内蔵 6軸/9軸シュージョン 出力レート225Hz 低遅延 USB出力 ROS対応 | YouTube

9dof_hayate_imu_youtube
9dof_hayate_imu_youtube

販売情報

【製品名称】hayate_imu rev.C 6軸フュージョン or ver.B 9軸フュージョン
【開発会社】ROBOT翔(株式会社翔雲)
【発売時期】令和3年4月上旬頃
【取扱店舗】9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応 | ROBOT翔

後継機種

9軸センサーICM-42688+MMC5983 6軸&9軸回転ベクトル&3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続

参考資料

Migrating from MPU-9250 to ICM-20948-InvenSense
http://wiki.ros.org/ja/9dof_hayate_imu

関連記事

参考情報

エンコーダ付きDCモータPID制御の実験-hayte_imu応用例
9軸IMUセンサ ICM-20948をロボットに組み込もう
オイラー角~ジンバルロック~クォータニオン
ROS・Unity・ロボット・ドローン姿勢制御に関わるクォータニオン

2+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア