【RTK22】RTK-MB-IMU最新システム備忘録<F9Pシリアル受信のコツ>

2022年最新システム動作確認完了しました。アイデア出しながらで2か月半くらいかかりました。
目的は、STA22の新機能の開発用です(高精度、高信頼性、IMU補間、板のたわみセンシング)
●2022年5月末バージョンと今までのシステムからの改良点
メインCPU:teensy4.1でUSBHost接続と高速SDログ、多センサ同時ログ
=>ArduinoそのままでSD書き込み、計算速度、コンパイルが超速で、ESP32,M5Stackへは、戻れなくなりました。
WiFi,BlueToothモジュール:M5StickCとRN42モジュールをuartでつけてます
RTKモニター:腕時計型M5StickCブザー付きとAndroidスマホ
SkiTurnアナライザー:Win10タブレットで、SDカードを外さないでMTPモードで読み込んでGraphicデータ処理
基板セット:4月まではF9P-F9H基板セットでしたが、5月からF9P-F9Pに変更してF9H不要になりました
=>板のたわみをMovingBaseのLengthで測定する方針変更で、F9HだとLengthがで得られないため。
=>i2cにIMU2個ぶら下がてRTKデータとクロックと同期させてあります。
※6月4日追記 「IMUは、RTK補間用には使えそうもないので、足と上体の角度測定用に使います。」
IMUをいじってみて定番のKALMANフィルター、IIRフィルタをいじってみて、さらに、DeadReckoning技術の論文を調査した結果、IMUだけでは、RTKの補間できるほど精度がでないと判断しました。そこで、IMUに代わる技術として、GPSドップラ速度ベクトルを利用することにしました。高速測位できるGPSチップM9Nを試してみます。詳細記事はこちらです。

 

■2021システムからの改良点違い
違い0:BaseとRoverを分離させて、Baseの基線長を常時管理できるようにしました。
=>SimpleRTK2B HeadingKitでは、MovingBaseをしながら基線長管理ができないです。
違い1:位置分解能が0.1mm単位で全て管理してます。(2021年はcm単位)
違い2:緯度経度を一切使わないで、基準局からの基線長座標で現在位置を表現してます。
違い3:測定開始前に、統計モードで3シグマばらつき測定してから、計測開始するようになってます。
違い4:IMUを2個つけて、RTK時刻と同期させて、運動軌跡の補間機能を開発します。
=>RTKが8-10HzでIMUが40-50Hzサンプリングして同期ログします。
違い4:MovingBaseのLengthを管理して、スキー板のたわみ変形との相関を計測する機能をつけました。
違い5:SDログファイルは4個同時ログ(Base用UBX、Rover用UBX,モニターデータTXT、IMUデータTXT)


■ノウハウの備忘録(細かいのでわすれてしまいそう)
昨年までのM5Atomの受信プログラムでは、数%データ落ちがあったので、2022年では3シグマ以下のデータ落ちまで、管理してます。3シグマを達成するのは、すごく手間と時間がかかります。覚えているうちに備忘録しておきます。
F9Pは、シリアルデータ送信タイミングが衛星電波受信後50~70msecの範囲で遅延してばらつきます。ですので受信プログラム次第で、データ落ちしやすくなります。特に、衛星電波状態が悪いとき、メッセージ数が多いときなど計算能力を目いっぱい使っているときは、タイミングの乱れが多く発生して、データ落ちしやすくなります。良好で安定した状態なら、あまり気にしなくてもいいですが、数多くの測定をしていくと、プログラムの出来不出来でデータ落ちエラー率が違ってきます。
2022・5・31のプログラムGISTに置いておきます。
https://gist.github.com/dj1711572002/5bf724f4570cec5a9c84ced0be4991c0
STA22_8Hz_USBHOST_Uart4_MPU6500x2_Kalman_Stable2_rev10391_6.ino

IMUは、MPU65002個使って、RTKデータと同期させてあります。BaseアンテナとIMUを自在に運動させる治具を作って、6月からRTKのIMU補間機能を開発していきます。
※F9Pのような出力タイミングがばらつく変なチップを扱うときは、シリアル能力に余裕があるCPUのほうが、プログラム作りが楽ですので、できるならTeensy4.1をお勧めします。ESP32系、M5系はシリアルが貧弱なので、
一旦F9Pシステムを作ってもそれ以上の発展性は、望めません。私は、2021年に苦労してM5StackとM5Atomでシステム作りましたが、2022年の新システムで性能を上げるには、無理があったので、Teensyに乗り換えた経緯があります。最初からTeensyを使っていれば、1-2年くらい得できたはずです。
下記にTeensyのシリアルの凄さがわかる記事を書いてあります。

【RTK22】超高速マイコンCortex-M7搭載Teensy4.1いじる その5<HardwareSerial 超便利>

①シリアル受信部のF9P対応コツ
最新仕様は、baseは、USB HOSTシリアル、roverは、普通のシリアル接続です。USB HOSTの場合データ落ちは非常に少ないのでSDログする分には問題ないのですが、2ポートだとbase,rover間の同期がずれることが多く、同期がずれてしまうとリアルタイムとの同期がとりにくくなるので、ずれが多いroverをシリアル通信に変更して、シリアル専用バッファを172バイトで作って、一括受信できるようにシリアル受信を実現してあります。
F9Pで、高信頼性のデータログをしながら他の仕事をする場合は、ESP32,M5Stack等速度が遅く、シリアル能力が低いので無理なことがわかってきました。teensy4.1一旦使ってしまうとM5Stack系は、使えません。

 

■シリアル受信の場合(ESP32系でシリアルバッファ固定でマルチタスク受信している場合)
細切れで読むと収拾がつかなくなります。F9Pは、電波環境によって送信時間が刻々変わりますので、一定ペースでデータを送信してくれんません、調子の良い時なら、どんな受信プログラムでもOKですが調子が悪いときは、へたなプログラムだとこけます。ですので、一気に172バイトよむまでずっと待つことが重要です
更に、いけないのは、受信タイミングによってヘッダーから入ってこない場合があります。その場合、172バイト受信してからB5,62,01,07の4バイトのヘッダを探して、ソートしないと、PVTメッセージが解読できません。アルゴリズムが結構大変になります。

1)loopでのシリアル受信部
バッファに1個以上データ入ったら、すかさず、while(j<172)に入って、172バイト受信できるまで待ち続けます。USB HOSTの場合は、バッファが172バイトになるまで待ってから読みにいくのですが、普通のマイコンのシリアルだとバッファが小さくて、172バイトためこめないので、1バイトでもはいってきたら、すかさず受信whileに入ってしまわないと、データ欠落してしまいます。バッファが小さい分プログラムに負担が大きく他の処理が難しくなります。ESP32系の場合は、受信してバイナリデータをSDに書き込んで、無線でデータ送信するのがギリギリの処理です。他の機能として、精度向上、統計計算などいれる余裕がありません。

//UART4で受信

if(Serial4.available()){  //1個以上データが入ったら受信開始
j=0;
while(j<172){//172バイトになるまでずっと回す
while(Serial4.available()){//バッファがあるまで回すが172バイトで打ち切る
dBuf1[j]=Serial4.read();
//Serial.print(dBuf1[j],HEX);
j++;
}
}

●Teensyの場合は、ハードウェアシリアル機能は、凄いです
Teensyの場合は、UARTが8個あって、すべてハードウェアシリアルで、ハードで自動的にバッファにため込んでくれますので、CPUの負担ゼロでシリアル受信できます。さらに、ハードウェアのバッファサイズが自由に指定できるので、シリアル通信の受信ループは、待ち無しで読み込めますので、プログラムの浪費時間は1msec以下(172byteの場合)一気に読み込みます。マルチタスク受信だとCPUが結局遅くなりますが、ハードウェアシリアルだと一切CPUが遅くなりませんので、シリアル8個全部同時に使ってもプログラムの遅延がでません。
Teensyでハードウェアシリアルのバッファサイズを指定するのは、setup()内で行います。32byteの倍数で指定します。
100バイトて指定したら、とんでもないことになりましたので、バッファサイズは32の倍数にすることが安全です。
解説ページは、https://www.pjrc.com/teensy/td_uart.html 
命令文は、これだけです。bufferは、クラス直下でグローバル変数として static uint8_t buffer[size];であらかじめ宣言しておきます。
//===============Teensyの場合のシリアル受信Pgmサンプル==============
void setup(){
—————–
Serial1.begin(115200);

int size=192;
Serial1.addMemoryForRead(buffer, size);//これが凄く便利です。
—————–
}// setup end

void loop(){
—————

if(Serial1.available()>171){     //size-1で172byte以上になったら読み込む
i=0;
    while(i<172){
         buffer[i]=Serial1.read();
i++;
}

}//このif文は、1msec以内で済んでしまいます。

}// loop end
//======================================================

■USB HOSTシリアル2CHの場合
シリアル通信ほど苦しくないですが、速度とバッファに余裕がありすぎて、F9Pの微妙なタイミングばらつきを拾って、CH間の同期がずれます。データ落ちは3シグマ以下ですが、CH間同期しないので別々のファイルで保存してます。
F9P自体が50-70msec遅延したり、周期ばらばらでデータ送信してくるので受信データ落ちが発生することがあります。データ落ちを全エポック数の0.3%以下に抑えるために、受信部のアルゴリズムの最適化をはかるのにずいぶん試行錯誤しました。
コツは、「規定バイト数がバッファにたまるまで読み始めないことです。」
=>USBHOSTの場合は、バッファで大きいのでたまるまで待つことができますが、シリアルの場合は、上記の読みながら待つ方式が良いです。USBHOSTとシリアルの組み合わせの場合、USBHOSTはバッファ余裕があるので、172Byteたまるまで読み始めないif文にして、シリアルは、余裕がないので、1Byteでもバッファにたまったら即読み込むif文にしてwhileループで172Byteたまるまで回します。これで走らせると、常にシリアル受信が最初に終わって、次にUSBHOST受信が終わるという順になりました。USBHOST2ポートの場合は、useriab0,userialb111の順になるので、if文の設定の仕方で読み込み方は大きく変化します。

1MBPSくらいで読むので、数μ秒で読み終わってしまうので、遅くてバラツクF9Pの出力をじっと待つほうがいいだろうということです。待たずにコマギレで読むと、後のプログラム処理が大変になります。
受信部のif文例です。

//==========バッファが規定バイト数(172Byte)になるまで待ってから、一挙によむ========

if(userialb0.available()>171 && userialb1.available()>171 )

{
i=0;
while(i<172)   //USB0:NAV-PVT 100byte NAV-RELPOSNED 72Byte
{
while(userialb0.available())
{
dBuf0[i]=userialb0.read();
i++;
}
}
delay(3);//処理が速すぎるのでポート間で3msec遅延させる
j=0;
while(j<172)  //USB1:NAV-PVT 100byte NAV-RELPOSNED 72Byte
{
while(userialb1.available())
{
dBuf1[j]=userialb1.read();
j++;
}
}

}// USB 2Ports Recieve end

②BaseがUSB0になるとは限らない。
当初は、データ受信前にボードIDをリクエストして、ボードIDからUSB0とUSB1のbaseかroverかの区別をしていたのですが、高速受信でパラメータ分別をしないといけないので、1回受信してから、baseとroverをデータ内容で区別してRTKデータに代入する方法にしました。
区別の方法は、BaseのRELPOSNEDのLengthは、数kmですが、RoverのRELPOSNEDのLengthは、数十cmから数mなので、区別がつきます。
=>USB HOSTをbase、roverをシリアル通信に変更したので、USB0がbaseに常になるので、本機能は不要となりました。

③iTow基準でエポック判別
受信が完了したのを認識して次の処理に回す判断基準は、現在のitowと1個前のitow_1との差が
itow-itow_1>123 としました。123とは、規定周期125msec(8Hz)に対してばらつきを含めてます。
さらに、123より大きければいくつでもいいいので、F9Pがつまずいて、データが数エポック飛んでしまってもデータ処理してログできるようにしてあります。

④データ抜けのチェック
受信部の直後にPVTのitow計算して、一個前のitow_1と引き算 itow-itow_1>127msec以上離れていたら抜け発生としてカウントアップしていきます。このミスカウント数が全ログエポック数の0.3%以内におさまっていれば、計測データは有効と判断します。

⑤BaseとRoverで別ファイルにしてログ
BaseとRoverでそれぞれ独立して抜けが発生すると、itowが狂ってくるので、同じ行にログすると、pcで処理するときに面倒になるので、別々のファイルにすることで、pcで読み込むときにbaeeとroverのitowを同期しやすくしました。最悪1-2秒ずれますが、それぞれの抜けが0.3%程度なので、ほとんどのエポックで同期できます。

⑥IMU取得時刻とのGPS時刻の同期
F9Pのデータ送信タイミングが実時間より50-70msec遅れるので、RTKデータを受信したタイミングは、役に立ちませんので、別途baseのtimePulseピンから信号をTeensy4.1のdigitalピン38にINPUTしました。
測定開始時にRTKデータのitow値tpitow0とTimePulse立ち上がりエッジの時刻tptime0=millis()を決める処理を
します。CPUは、RTKデータでなく、自分のクロックで衛星受信タイミングをカウントしていきます。=>Teensyのクロックが14ppmくらいなので、計測時間30分で25msecずれる程度なのでIMUの1周期程度で結果に影響はでないと判断しました。
そのカウント値でIMUの計測時刻を決めます。IMUデータには、ヘッダーでimuitowといitow時刻をつけてログすることで、RTK位置は125msec毎に正確に得られてますので、IMUデータ群を積分処理しながら位置補間に使えるということになります。125msecごとにIMU積分リセットして、バイアスノイズの影響を小さく抑える効果が期待できます。この実験をするためのシステムです。うまくいけば、遅いF9Pでも40-50Hz相当の位置補間で高速測位で使えることの証明できるかもしれません、そうすれば、歩行、スキー競技、高速な運動分野でもF9Pが使えるようになる可能性があります。

⑦IMU LSM9DS1の使い方=>LSM9DS1からMPU6500に変更しました。2個使いし易さと供給性理由です。
9軸ですが、磁気は使ってないので、6軸です。Arduinoi用にSpaarkfunのライブラリを使てます。
https://learn.sparkfun.com/tutorials/lsm9ds1-breakout-hookup-guide?_ga=1.167528382.1408024151.1440392747
これに、KalmanFilterを加えて処理してます。動いている場合の角度変化は、生AccデータのATAN計算した角度より角速度GyroデータとAccデータを使っているKalmanFilterのほうが、よい値がでます。しかし、回転がない併進運動では、KalmanFilterも役に立ちませんので、RTKと比較してAccの積分補正をする以外にないと考えてます。
KalmaFilterは、Arduinoの標準的なもので、KJ Electronicsさんのカルマンフィルタライブリーを使ってます。サンプルプログラムをコピペでできます。
https://www.arduinolibraries.info/libraries/kalman-filter-library

LSM9DS1はSwitch Scienceさんのボードなのですが、いかんせん、I2C Idが1個しか使えない仕様になってました。
https://www.switch-science.com/catalog/2405/

そこで、wire2を使ってもう1個のLSM9DS1を駆動しようとしてますが、Sparkfunのライブラリの使い方がわかりません、どうしてもだめなら、自分用の専用のLSM9DS1ドライバー作成します。

⑧IMU周期確保とSD保存高速化
今までは、1データごとにOpen-Closeしていたのですが、OpenとCloseの時間間隔を8msec遅延させないと暴走してしまうので、結局1行のデータセットをSD書き込みするのに、10数msecかかってました。なぜ毎回open-closeするかというと、スキー滑走中に万一電源が落ちたら、全データがパーになってしまうリスクがあるからです、しかし、IMUで20msecサンプリングするためには、SDで10数msec無駄してられませんのんで、1分間に一回open-Closeをいれることにしました。こうすると、BaseのUBX,,RovrのUBXの2ファイルで2msec、統計データのTXTファイルで2msec,IMUデータのTXTファイルで3msecで、SDログで7msec程度、USB受信で4ー10msec,無線送信で1msec程度でIMU周期20-25msecは確保できまそしたがばらつくので、ヘッダにimuitowのタイムスタンプをつけてあります。

●プログラムコード https://gist.github.com/dj1711572002/dbd4b903f55ad8fe32f0b2eacf1498ab

●以後
夏場で気温があがるとteensyが熱暴走するので、夏場の実験ではヒートシンクを載せます。
屋外での実測でいろいろ不具合がでると予想されますので、対策をうちながらすすめていきます。

 

 

 

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です