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

はじめに

令和3年度発売の旧型機種のhayate_imu v2は多くの企業、学校法人のユーザー様にご利用いただいたことに、厚く御礼申し上げます。ありがとうございます。旧機種はv2.4までとリリースさせていただいておりますが、いまユーザー様のお手元にある旧バージョン製品のファームバージョンアップは、ユーザー様のもとで実施可能なので、詳細については、別途順次ご案内申し上げます。昨今の半導体ショックにより供給不足、価格高騰などの影響を受ける中、後継機種の開発を続けてきた結果、令和5年4月下旬より、9軸IMU/AHRS haya_imu v3.2の発売をお知らせさせていただきます。

製品紹介

Cortex-M4 (クロック周波数120MHz)、新型6軸IMUのICM-42688、高精度3軸AMR方式地磁気センサMMC5983MAの実装により、通常出力モード、デモストレーションモード、キャリブレーションモード(初期バイアス測定)、6軸フュージョン回転ベクトルクォータニオン、9軸フュージョン回転ベクトルクォータニオン、3軸オイラー角の同時出力は最大1000Hzまで可能となります。ROS/ROS2とも対応しており、ドライバーはGithubよりダウンロードして製品とセットでご利用いただけます。

主な仕様

・型番 haya_imu v3.x
・内蔵チップ Microchip Cortex-M4(120MHz)、ICM-42688-VまたはICM-42688-P、MMC5983MA実装
・外部接続 USB2.0+ Type-C、USB+5V給電
・最大出力レート
  - 6軸/9軸フュージョン回転ベクトル四元数 1000Hz
  - 3軸オイラー角  1000Hz
  - 3軸加速度(アクセル)データ  1000Hz
  - 3軸角速度(ジャイロ)データ  1000Hz
  - IMU内部温度データ      1000Hz
  - 3軸地磁気(コンパス)データ  500Hz

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

・バイアス測定補正 初期バイアス測定、動作時即時測定、内蔵補正機能あり
・消費電力 150mW以下(環境温度21℃ 実測値)
・寸法 38.0mm × 39.0mm × 4.8mm(突起物含む)
・取付穴 M3x4、隣り合う穴の中心間距離32.0mm

主な特長

・サービスモード 通常出力モード、デモンストレーションモード、キャリブレーションモード
・結果出力 6軸フュージョン回転クォータニオン、9軸フュージョン回転クォータニオン、3軸オイラー角1KHzまで同時出力、結果出力レートに関わらずIMU/地磁気センサのデータサンプリング周波数、フュージョン周波数は常に1000Hz/500Hzに設定済み
・初期バイアス測定 使用環境変化あった際に利用可能なキャリブレーションモードで最短数分程度で初期バイアス測定完了、MCUフラッシュに自動的に保存して、動作時に読み込んで即時バイアス測定&補正あり
・地磁気センサ温度補償 地磁気センサは、計測時間1msにわたるセットリセット計測(温度補償機能)使用済み
・磁気外乱による干渉 受けにくいことが当社実験(磁束密度約2G)にて確認済み
・ROS/ROS2対応 本体にはROS/ROS2ライブラリを実装せず、対向装置にドライバーインストールにより実現

詳細情報

【製品名称】haya_imu v3.x
【開発会社】ROBOT翔(株式会社翔雲)
【発売時期】令和5年4月下旬頃
【商品情報】9軸センサー6軸&9軸回転ベクトル 3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続 | ROBOT翔

参考情報

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

3+

ロボット・ドローン部品お探しなら
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翔・電子部品ストア

ST-MCSDKでPMSMモータを動かす

はじめに

NUCLEO STM32F446REとIHM07M1の組み合わせに、ST MCSDK(ソフトウェア)を組み込んで、ドローンに使用可能なA2208-1100KVモータを動かした。FOCに手入れなくても動作可能になった。確認環境、確認手順を以下にメモしておく。中間生成物のモータプロファイルとワークベンチに、結果物のSTM32CubeIDEプロジェクトを含めるリポジトリをGithubに公開済み。

確認環境

NUCLEO STM32F446RE
X-NUCLEO-IHM07M1
A2208-1100KV SM-PMSM Motor
DC Power Output 14.8V
X-CUBE-MCSDK_5.4.7(Motor profiler & Motor workbench含む)
stm32cubemx-win_v6-3-0
stm32cubeide_1.14.0_19471_20231121_1200_x86_64.
Windows 11

※何回かの試行錯誤から、MCSDK_5.4.7と、stm32cubemx-win_v6-3-0の組み合わせではエラーなく行けると確認された。

確認手順

以下、Motor Profiler→Workbench→STM32CubeIDE→WorkbenchMonitorの順でメモしていく。

Motor Profiler

12極=6ペア、3セル=14.8V、1100KV * 14.8V=16280RPM、永久磁石は回転子表面実装=SM-PMSMとして入力して、一度モータを馳せて、測定したモータプロファイルを書き出す。入力したMAX SPEEDまでモータを回転させるので、机に固定することに注意すること。

motor-profiler
motor-profiler

Workbench

Workbenchを立ち上げて、制御ボード、モータドライバ、モータプロファイルとともに、Workbenchプロジェクトを書き出す。

create-workbench
create-workbench

save-workbench
save-workbench

project-generation
project-generation

STM32CubeIDE

Workbenchプロジェクトを読み込んで、コンパイルして制御ボードに書き込む。

stm32cubeide-import-workbench
stm32cubeide-import-workbench

Workbench Monitor

Workbench Monitorを立ち上げて、Start/Stop Motorでモータを制御する。

workbench-monitor
workbench-monitor

リポジトリ

https://github.com/soarbear/stm32f446-ihm07m1に公開済み。

参考資料

STM32向けモータ制御ソフトウェア開発キット(MCSDK)
X-NUCLEO-IHM07M1
Three-phase brushless DC motor driver expansion board based on L6230 for STM32 Nucleo

NUCLEO-F446RE
STM32 Nucleo-64 development board with STM32F446RE MCU, supports Arduino and ST morpho connectivity

1+

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

stm32h750のdacとtimerでsine波を作る

はじめに

いよいよSTM32ファミリーのハイクラスに属する、Cortex-M7コアをもつ、stm32h750(クロック周波数480MHz)を新製品に組み込もうと、早速stm32h750の開発ボードでSine波を作ってみよう。今回はSine波のサンプリングデータを1周期当たり100データに、DACでアナログ波に、トリガをTimer6に、サンプリングデータをDMA転送に指定して、Sine波の周波数は、タイマクロック周波数240MHz / タイマのリロード最大カウンタ数(239+1) / Sine波1周期当たりタイマリロード数100 = 10KHzにしよう。

確認環境

・Windows 10 / STM32CubeIDE
・stm32h750 開発ボード
・ST-LINK V2 プログラマ/デバッガ
・オシロスコープ

STM32H750プログラマSTlLINK_V2を使用
STM32H750プログラマSTlLINK_V2を使用

パタメータ配置

RCC、Clock、DAC、Timer、DMA、Serial wireの順でパラメータ配置を行って、STM32CubeIDEにソースコード生成しよう。

Target MCU

TargetをSTM32H750VBに指定する。

STM32H750VBをターゲットとして指定
STM32H750VBをターゲットとして指定

RCC

RCCを外部HSEクリスタルに指定する。

stm32h750のRCC配置
stm32h750のRCC配置

Clock

HSEを25MHzオンボードに指定して、Timerクロックを240MHzにする。Timerクロックは他の値にしてもありうる。

stm32h750のクロック配置
stm32h750のクロック配置

DAC

DACのOutput2に指定する。

STM32H750のDACをOutput2に指定する
STM32H750のDACをOutput2に指定する

Timer

Timer6に指定する。

STM32H750のTimer6に指定
STM32H750のTimer6に指定

DMA

MCUの負担を軽減して、DMA転送方式に指定する。

STM32H750のDMAに指定
STM32H750のDMAに指定

Serial wire

プログラマ&デバッガをSWD(ST-LINK)に指定する。

STM32H750のデバッガをSerial wireに
STM32H750のデバッガをSerial wireに

ソースコード

Generate codeして、main.cへ以下ソースを追加する。

/* USER CODE BEGIN 0 */
uint16_t data[100];
void sine_wave(void) {
	for (uint16_t i = 0; i < 100; i++) {
		data[i] = (uint16_t)(2000 * sin(i * 2 * 3.14159 / 100) + 2000);
	}
}
/* USER CODE END 0 */

int main() {
...
/* USER CODE BEGIN 2 */
sine_wave();
HAL_TIM_Base_Start(&htim6);
HAL_DAC_Start_DMA(&hdac1, DAC_CHANNEL_2, (uint32_t *)data, 100, DAC_ALIGN_12B_R);
/* USER CODE END 2 */
...
}

出力確認

BUILD & RUN、Sine波は予想通り、周期10KHzでPA5から出力されていることを確認済み。

STM32h750の出力Sine波を確認する
STM32h750の出力Sine波を確認する

プロジェクト

https://github.com/soarbear/stm32h750_dac_timer_sine_wave に公開済み。

2+

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

ACMEによる、新規サブドメインのSSL対応

はじめに

数年前から、ドメインのSSL対応にGithub公開リソースのACMEを利用してきた。新規サブドメインの開設に当たって、以前と同様にACMEを利用するつもりで、ACMEの証明書サーバはLets encryptからZeroSSLへ移行済みなど変更があったので、作業手順をメモしておく。

準備

$su root

#cat /etc/redhat-release
CentOS release 6.8 (Final)

#acme.sh --upgrade

#acme.sh --version
https://github.com/acmesh-official/acme.sh
v3.0.7

DNSゾン追加

サーバプロバイダの管理画面またはパネルから、サブドメインに必要なDNSゾンの追加を行うことができる。これでサブドメインとIPと紐づける。追加手順はサーバプロバイダのほうへ確認するので、ここでは詳細を省く。

Apache対応

サブドメイン追加

vhost.confを開く

#nano /etc/httpd/conf.d/vhost.conf

vhost.confへ内容追加。

<VirtualHost *:80>
ServerName new-sub.example.com
ServerAlias www.new-sub.example.com
Redirect / https://new-sub.example.com/
</VirtualHost>
<VirtualHost *:443>
ServerAdmin mai@example.com
DocumentRoot /var/www/html/new-sub.exmaple.com
ServerName new-sub.exmaple.com
ServerAlias www.new-sub.example.com
# ここにあとで追加内容ある
ErrorLog "/var/log/httpd/new_error_log"
CustomLog "/var/log/httpd/new_access_log" combined
</VirtualHost>

Apache再起動

#servcie http restart

ACME対応

証明書作成

#acme.sh --issue -d new-sub.example.com --apache

証明書コピー

コピー先を指定しておく。

#acme.sh --install-cert -d example.com \
--cert-file      /root/apache/new-sub.example.com/cert.pem  \
--key-file       /root/apache/new-sub.example.com/key.pem  \
--fullchain-file /root/apache/new-sub.example.com/fullchain.pem \
--reloadcmd     "service httpd restart"

証明書確認

#acme.sh --info -d new-sub.example.com

vhost.confに内容追加

<VirtualHost *:80>
ServerName new-sub.example.com
ServerAlias www.new-sub.example.com
Redirect / https://new-sub.example.com/
</VirtualHost>
<VirtualHost *:443>
ServerAdmin mail@example.com
DocumentRoot /var/www/html/new-sub.example.com
ServerName new-sub.example.com
ServerAlias www.new-sub.example.com
SSLEngine on
SSLCertificateFile /root/apache/new-sub.example.com/cert.pem
SSLCertificateKeyFile /root/apache/new-sub.example.com/key.pem
SSLCertificateChainFile /root/apache/new-sub.example.com/fullchain.pem
<Directory "/var/www/html/new-sub.example.com">
AllowOverride All
</Directory>
ErrorLog "/var/log/httpd/new_error_log"
CustomLog "/var/log/httpd/new_access_log" combined
</VirtualHost>

Apache再起動

#servcie http restart

証明書自動更新

#EDITOR=nano crontab -e
56 * * * * "/root/.acme.sh"/acme.sh --cron --home "/root/.acme.sh" > /dev/null

参考情報

https://github.com/acmesh-official/acme.sh

0

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

エンコーダ付きDCモータPID制御の実験-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へ接続)

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

開発要素

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ノードの相互作用状況

エンコーダ付き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

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
PID制御の目標値、エンコーダによる回転速度、haya_imu測った回転速度曲線
PID制御の目標値、エンコーダによる回転速度、haya_imu測った回転速度曲線

haya_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は望ましい。

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

リポジトリ

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接続

0

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

エンコーダ付き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翔・電子部品ストア