2020-03-31

3月のまとめ

進捗

SOEMのESP32対応
GR-MANGO(β版)でEtherCAT-IPゲートウェイ作成
ESP32でEtherCAT-IPゲートウェイ作成
EtherCAT-IPゲートウェイと通信するPCアプリ作成

イベント等

3/7～技術書典応援祭
3/14 第1回デジもくオンライン (実験版)
3/21 第2回デジもくオンライン

技術書典応援祭に「GR-ROSEでいこう！」を出品しました。
techbookfest.org

動画公開

所感

コロナの影響でイベントが軒並み中止となる中、デジもく会ではオンラインでのもくもく会開催を試み、良い手ごたえと知見が得られました。当分の間はコロナの脅威が続くと思われますが、健康管理に留意しつつも平常心を保って開発を続けていきたい。

2020-03-27

Madgwickフィルタで姿勢角推定

Madgwickフィルタとは？

マドウィックフィルタと読む
３次元の姿勢角推定に用いる
加速度、ジャイロ、(地磁気) でセンサフュージョンする
相補フィルタより精度が良く、カルマンフィルタより計算が軽い
計算にはクォータニオンを用いる

Arduinoで使えるMadgwickフィルタライブラリ

下記のライブラリが利用できる。
github.com

入力: 6軸センサ(加速度+ジャイロ) または 9軸センサ(加速度+ジャイロ+地磁気)
出力: 姿勢角(ロール、ピッチ、ヨーのオイラー角表現)

f:id:licheng:20200326200938p:plain

地磁気を入力しない場合、鉛直軸の回転はジャイロのドリフトを補正できない。重力加速度による角度推定が鉛直軸回転には利用できないためである。これは相補フィルタやカルマンフィルタも同様である。

使い方

【宣言】

#include <MadgwickAHRS.h>
Madgwick madgwick;

【初期化】

  madgwick.begin(100); //100Hz (10msec周期)

【計算】

    // 6軸センサの場合 (ジャイロは[deg/s]、加速度は生値でも[G]でも[m/s^2]でも可)
    madgwick.updateIMU(gx, gy, gz, ax, ay, az);
    // 9軸センサの場合 (地磁気は生値でも[uT]でも可)
    madgwick.update(gx, gy, gz, ax, ay, az, mx, my, mz);

【姿勢角(オイラー角表現)の取得】

    // [deg]で取得
    float roll  = madgwick.getRoll();
    float pitch = madgwick.getPitch();
    float yaw   = madgwick.getYaw();
    // [rad]で取得
    float roll  = madgwick.getRollRadians();
    float pitch = madgwick.getPitchRadians();
    float yaw   = madgwick.getYawRadians();

注意事項

このライブラリでは、内部計算に用いているクォータニオンがprivate変数になっている。しかし、オイラー角への変換には逆三角関数を用いるので計算量が多いし、その後の計算にもクォータニオンのまま扱ったほうが有利なことも多い。そこで、クォータニオンをpublic変数に変更しておく。具体的には MadgwickAHRS.h の当該箇所を下記のように修正する。

public:
    float q0;
    float q1;
    float q2;
    float q3;

6軸センサBMI160

BMI160はBOSCH社製の6軸センサである。詳しくは下記の記事を参照。
lipoyang.hatenablog.com

Arduino用ライブラリも公開されている。 (非公式)
github.com

実験プログラム

Arduino系ボードのGR-CITRUSに6軸センサBMI160をSPI接続してMadgwickフィルタによる姿勢推定の実験プログラムを作成した。

f:id:licheng:20200326205542j:plain:w480

結線

GR-CITRUS	BMI160
GND	GND
3.3V	3V3
13 SPI CK	SCL/SCx
12 SPI MI	SAO/SDO
11 SPI MO	SDA/SDx
10 SPI SS	CS

スケッチ

こちらからダウンロードできます。

参考ページ

shizenkarasuzon.hatenablog.com

2020-03-26

メモ：クォータニオンと回転行列

回転とクォータニオン

３次元空間での回転をクォータニオンで表すことを考える。

回転軸のベクトルを ${\bf n}$ 、回転角を $\theta$ とすると

回転を表すクォータニオンは ${\bf q} = \cos \frac{\theta}{2} + {\bf n} \sin \frac{\theta}{2}$

${\bf n} = \left(\begin{array}{c} x \\ y \\ z \\ \end{array} \right)$ なら ${\bf q} = \left(\begin{array}{c} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \\ \end{array} \right) = \left(\begin{array}{c} \cos \frac{\theta}{2} \\ x \sin \frac{\theta}{2} \\ y \sin \frac{\theta}{2} \\ z \sin \frac{\theta}{2} \\ \end{array} \right)$

この回転でベクトル ${\bf r} = \left(\begin{array}{c} r_{1} \\ r_{2} \\ r_{3} \\ \end{array} \right)$ が ${\bf r'}$ に写像されるとすると

ベクトル ${\bf r}$ をクォータニオン ${\bf r} = \left(\begin{array}{c} 0 \\ r_{1} \\ r_{2} \\ r_{3} \\ \end{array} \right)$ と読み替えて

${\bf r'} = {\bf q r q^{*}}$

ただし、 ${\bf q^{*}}$ は ${\bf q}$ の共役クォータニオンで、 ${\bf q^{*}} = \left(\begin{array}{r} q_{0} \\ -q_{1} \\ -q_{2} \\ -q_{3} \\ \end{array} \right)$

回転行列

同じ回転を表す行列を $R$ とすると

${\bf r'} = R {\bf r}$

この行列 $R$ とクォータニオン ${\bf q}$ の間には次式が成り立つ。

$R = \left(\begin{array}{ccc} q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} & 2(q_{1}q_{2} - q_{0}q_{3}) & 2(q_{1}q_{3} + q_{0}q_{2})\\ 2(q_{1}q_{2} + q_{0}q_{3}) & q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} & 2(q_{2}q_{3} - q_{0}q_{1})\\ 2(q_{1}q_{3} - q_{0}q_{2}) & 2(q_{2}q_{3} + q_{0}q_{1}) & q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}\\ \end{array} \right)$

これは、基底ベクトル $\left(\begin{array}{c} 1 \\ 0 \\ 0 \\ \end{array} \right) \left(\begin{array}{c} 0 \\ 1 \\ 0 \\ \end{array} \right) \left(\begin{array}{c} 0 \\ 0 \\ 1 \\ \end{array} \right)$ の回転による写像を並べたものである。

回転行列は方向余弦行列(DCM)とも呼ばれる。

回転を表現する方法の比較

方法	パラメータ数	ジンバルロック	計算量
オイラー角	3	×(有)	×(大)
回転ベクトル(軸角度表現)	3(4)	〇(無)	△(中)
回転行列	9	〇(無)	〇(小)
クォータニオン	4	〇(無)	〇(小)

オイラー角は人間にとっては直感的(せやろか？)と言われるが、ジンバルロックがあるし、三角関数を多用し計算量が多いのでコンピュータでの内部表現にはあまり向かない。回転ベクトル(軸角度表現)も三角関数を使うがオイラー角よりは少ない。回転行列はもっとも単純明解な数式で回転を記述できるがパラメータ数が多い。クォータニオンはパラメータ数も少なく、計算量も少ない。(しらんけど)

参考ページ

qiita.com
kamino.hatenablog.com
risalc.info

2020-03-25

メモ：ESP32でmDNS

マイコン

mDNSとは？

multicast DNSの略
ローカルネットワーク内のホストをマルチキャスト転送で名前解決する
DNSサーバーが不要になる
通常のDNSと同様、あらかじめIPアドレスを知らなくてもアクセスできる
Windows, Mac, Linuxで統一された規格 (WindowsはWindows10 1803以降)

ESP32でmDNS

Arduinoスケッチの場合、まず ESPmDNS.h をインクルードする。

#include <WiFi.h>
#include <ESPmDNS.h>

STAモードなりAPモードなりでWiFiを開始してから、ホスト名を指定してmDNSを開始する。

    MDNS.begin("esp32"); // ホスト名 esp32.local

これだけで、とりあえずOK。超簡単。

DNS-SD？

サンプルコードでは下記のようなAPIも使われている。DNS-SD (DNS Service Discovery)へのサービス追加らしいがよく分からない。

    // ポート80のTCPでHTTPやってることを周知させる？
    MDNS.addService("http", "tcp", 80);

2020-03-18

MPLAB X IDEのコンパイラバージョン指定

マイコン

MPLAB X IDEで特定のバージョンのコンパイラを指定する方法まとめ。

コンパイラのインストール

下記ページの「Language Tool Archives」にてXC8、XC16、XC32の各バージョンをダウンロードできるので、所望のバージョンをダウンロードしてインストールする。
www.microchip.com

デフォルトのコンパイラバージョン指定

MPLAB X IDEのメニューの「Tools」→「Options」で、「Embedded」→「Build Tools」タブを開く。「Toolchain」にインストール済みのコンパイラがリスト表示されるので、所望のバージョンを選んで「Default」ボタンを押すとそのバージョンがデフォルトで指定されるようになり、リストの表示が太字になる。

f:id:licheng:20200318162121p:plain:w640

プロジェクトごとのコンパイラバージョン指定

プロジェクト作成時にコンパイラのバージョンを指定できる。

f:id:licheng:20200320094600p:plain:w640

この指定はあとから変更できる。MPLAB X IDEでプロジェクトのプロパティを開き（Dashboardでプロジェクト名をダブルクリック）、「Conf: [default]」を選択すると「Compiler Toolchain」にインストール済みのコンパイラが列挙されるので、所望のバージョンを選択する。

f:id:licheng:20200318163258p:plain:w640