古いルールでは、単刀直入に、この記事が何を達成するかについてまず説明します。
1. IMUモジュールのみを使用して人の歩行距離の検出を実現します（精度は約0.5m）
2. IMUモジュールのみで2つのピーク検出・機械学習方式により正確な歩数計を実現
バックグラウンド：
近年、ウェアラブルデバイスの開発においては、人の歩行距離を正確に判断し、歩行距離を取得することが必要となっている。この目的は、このプロジェクトでは、SLAM でマッピング、測位、ナビゲーションを実装できるウェアラブル デバイスを必要とするためです。一般的な地図作成アルゴリズム（Gmapping、cartographer）であっても、後段のセルフサービスナビゲーションであっても、走行距離情報（/odom）を購読する必要があることがわかっています。一般的な移動ロボットでは、走行距離情報は比較的簡単に取得できます。 、ホイールスピードメーターまたはシャーシホイールのエンコーダーに直接取り付けることができます。
しかし、このウェアラブル プロジェクトでは車輪のようなものは存在しないため、走行距離計を実現するにはさまざまなセンサーに頼るしかありません。ウェアラブル デバイスに搭載できる一般的なセンサーには、IMU、ライダー、超音波測距プローブ (HC -SR04) 、 GPS。システムエラーやさまざまな制限により、単一のセンサーで取得される距離情報はある程度不正確です。実際、正確な距離情報を取得するには、複数のセンサーのデータ融合を実現する必要があります。カール・マンの知識フィルタリング、およびセンサー データを融合する方法に関する多くの詳細な検討事項は、プロジェクトの発展に応じて後で記録される可能性があります。
ここでは、より正確な走行距離計を実現するために単純な IMU モジュールのみが使用されていることを最初に記録します(複数回のテストの後、誤差は 0.5m 以内に制御できます)。
関連するテクノロジー:
簡単なC++およびROSプログラミングの知識。もう少し時間をかけて遊びたい場合は、Python+pytorchを追加して機械学習ソリューションを導入できます。
具体的なアイデアと実装:
まず、IMU モジュールが何をもたらすか考えてみてください。IMU モジュールは、IMU センサー モジュールを搭載したデバイスの動作中の3 軸加速度 (ax、ay、az)を提供します。この情報を使用すると、最初のアイデアをすぐに生成できます。中学校で習ったニュートンの第 2 法則を使用し、いくつかの速度公式を組み合わせて移動距離を直接大まかに計算できるか?
V = V0 + a*t
D = V0*t + 0.5*a*t*t
まず、ROS で IMU データをサブスクライブして 3 軸の加速度を取得します。テストでは、人が直線を歩く場合のみを考慮します。つまり、X 軸の加速度のみを使用します。停止状態から動き始めたとします。初速度 V0 を 0 としてプログラムを書き、速度公式を使ってロスランノードから歩き始め、5m 歩いたところで止まり、計算された距離が 5m とどのくらい違うかを見てみましょう。 。
void IMUcallback(const sensor_msgs::Imu::ConstPtr& msg)
{
double a_now, volocity, pre_volocity, scantime;
//加速度
double ax = msg->linear_acceleration.x;
//v = v0 + at
velocity =pre_velocity + a_now*scantime;
//d = v0t + 0.5*a*t*t
distance_a += velocity*scantime+0.5*a_now*scantime*scantime;
ROS_INFO("distance_a is %2f",distance_a);
pre_velocity = velocity;
}実験の結果、計算で得られた距離は5Mをはるかに超え、わずか数秒で100M以上にまで跳ね上がることが判明し、その原因を分析したところ以下の点が判明しました。
1.サンプリング周波数imu のサンプリング周波数は 0.1 秒に 1 回であり、人間の歩行速度をはるかに上回っています (人間の歩行速度は 1 歩あたり約 0.5 秒であると文献で知りました)。この時間を代入して計算するとデータが増えすぎる（距離が100Mに達する原因の一つ）
2.人間の歩行パターンを正しく理解していませんでした。上記のプログラムでは、人間の歩行を単純で粗雑な粒子運動として捉えていましたが、文献を調べた結果、人間の歩行は実際には動的なプロセスであることがわかりました。歩行過程における速度や加速度の変化を数理モデルに抽象化したものは正弦関数（SINX）であり、最初は大きくなり、次に小さくなるという動的な過程であり、その過程で重心も変化します。 、加速度が変化します。
(歩行時のIMU加速度データを描画し、歩行時の加速度変化が正弦関数に似ていることを確認します)
上記の 2 つの点を組み合わせると、コードを変更して上記 2 つの問題を解決できます。
最初の点では、ROS::Timer からタイマー オブジェクトを作成し、Duration() に時間を設定することで計算頻度を制御できます。また、ベクター コンテナーを作成してそこにデータを挿入することもできます (コールバック関数は 0.1 秒ごとに実行されるため)。そのため、5 つのデータがベクターに挿入されると、0.5 秒が経過したことになります。 , ここでは、2 番目のスキームを使用してベクター コンテナーを作成します。
2 番目の点については、ベクトル コンテナーの作成により、0.5 秒以内で 0.1 秒ごとの加速度を取得でき、動的プロセスを考慮してこれらの加速度の平均値を取得し、速度も次を使用して計算されます。データの平均値。
//方案一示例
int main(){
ros::Nodehandle nh;
double scantime = 0.5;
ros::Timer timer = nh.createTimer(ros::Duration(scantime), timerCallback);
ros::spin();
}
void timerCallback(const ros::TimerEvent& event){
//进行计算....
}
//方案二示例
void imuCallback(const sensor_msgs::Imu::Constptr& imu_msg){
vector v1;
double ax = imu_msg->linear_acceleration.x;
v1.push_back(ax);
if(v1.size()==5)
{
//开始计算
v1.clear();//计算完了以后清理容器
}
上記の問題を考慮した後、再度テストしたところ、いくつかの問題が見つかりました。
1 つ目は、人が立ち止まっても距離データが蓄積され続けることです。これは、人のわずかな揺れによる静止時の X 軸加速度のデータも IMU が取得しているためであり、小さいながらも距離を計算できます。私たちのアルゴリズムには、同様のループ検出が含まれていないためです。グローバル累積エラーメソッドを排除するため、実行時間の増加に伴うエラーの蓄積を避けるために、初期段階であまりにも多くの小さなエラーが発生することを避けるように努めます。
2 つ目は依然としてデータ爆発の問題であり、計算されたデータは最適化されていない以前のデータよりも優れていますが、それでも 5M をはるかに超えています。
この 2 つの問題を解決するには、人の歩行状態の検出を実現する必要があり、人が歩行していることを検出した場合にのみ、人の移動距離を計算し始めます。蓄積されていない。実現方法は 2 つありますが、まず簡単な方から説明しますと、人が歩いていると 3 軸の加速度はある程度変化するため、データを収集すると、3 軸の加速度は正弦関数モデルに従うことがわかります。上で述べたように、ピークの数がわかります。それは歩数に相当します。データを調べてみると、これは実際に多くの歩数計の実現原理、つまり、歩数計の 3 軸加速度であることがわかります。デバイス内のIMUを通じて人物を収集し、一定のデータ結合後に結合データのピーク値を検出し、ピークが現れるたびに人が一歩を踏み出したことを表します。
(ピーク検出、赤い点がピーク、真ん中の小さなピークが除去されており、冒頭の静的な状態も回避されているのがわかります)
この問題を理解した上で、3 軸の加速度データを結合する式を設計し、同時にピーク検出アルゴリズムを作成しました。ただし、ピーク検出アルゴリズムでは次の点に注意する必要があります。
- ピーク値を検出する前に、データを平滑化し、ピーク値のみを保持し、ノイズ データを除去する必要があります。
- ピーク検出の頻度は人の歩行頻度と一致する必要があることに注意してください。
- ピークを拡大してみると、大きなピークの間に小さなピークがたくさんあることが分かりますが、これを除去しないと判定に影響するため、まず小さなピークを除去して、大きなピークだけを残す必要があります。ピーク間の小さなピークを削除します。
- ピーク検出の検出範囲を設定するには、一定のしきい値を超えるピーク値のみを検出します。これは、人が静止している場合、3 軸の加速度はわずかに変化し、ピークも発生するため、これらを削除し、のみ保持する必要があります。運動中のピークが大きくなる
(拡大して観察すると、小さなピークがたくさんあることがわかります。回避しないと検出に干渉します)
上記の注意点を組み合わせて、ピーク検出アルゴリズムを作成し、リアルタイム検出のために IMU のコールバック関数に直接書き込みます。テストでは、データが非常に正確であることがわかりました (Apple Watch で測定したとおり)、この歩数計は次のことができます。オドメーターの 2 番目のデータ ソースを提供します。詳細については後ほど説明します。
ここに穴を残して、機械学習手法を使用してピーク検出を実現します。データは非常に定期的に変化するため、ここで機械学習を使用して浅いニューラル ネットワークを構築し、データ変化の特性を学習することは完全に可能です。人間が歩くには 2 つの状態しかないため、0 は静止を意味し、1 は歩行を意味します。データに簡単にラベルを付けることができます。Python + pytorchを使用してこの小さなタスクを実現すると、実現後の効果はピーク検出に似ており、比較的正確で、適用範囲が広くなり、後でそれを補う時間があります。
中央部の要約:
これまでのことを思い出してください、まず人間の歩行状態の判定を実現し、同時に人間の歩行モデル、加速度や速度の動的変化過程、歩行頻度を考慮しました。サンプリングデータの周波数の違い、上記の考慮事項に基づいて、現時点で実装されている IMU オドメーターのコードアイデアはおおよそ次のとおりです。
vodi IMUcallback(const sensor_msgs::Imu imu_msg)
{
double ax;
ax = imu_msg.acceleration.x;
vector v1;
v1.push_back(ax);
double imu_xyz;//自己设计的耦合数据
//峰值检测算法
bool is_walk = false;
.....
//如果持续检测到有效峰值，则把is_walk设为true，认为人在行走
//检测到人行走，才执行下面的距离计算代码，考虑人的启停
if(is_walk)
{
if(v1.size()==5)//这里体现的是考虑走路频率
{
double a_mean;//用均值计算，考虑动态过程
a_means = v1[0]+v1[1]...../5;
double distance, velocity;
distance += .....;
v1.clear();
}
}次に、上記のコードと実験室の IMU を組み合わせて実際のテストを行い、10 メートル歩き、往復 25 歩歩き (これが WeChat の歩数をスワイプする方法です)、25 セットのデータを取得します。誤差は1m以下では悪くありません。後ろにはレーダーとGPSがあるので、この仲良し兄弟のデータを融合することができます、この1mの誤差は簡単に解消できるので、これで終わりではありません！
歩数計を追加してデータを最適化する
気まぐれに、副産物である歩数計が上記で実装されました。歩数計によって取得されるデータは非常に正確で、基本的には歩数と同じなので、距離 = 歩数という非常に単純な考え方を使用します。 *毎歩 1 歩の距離も走行距離データを取得できます 1 歩の距離はばかげているように見えますが、歩数は非常に正確であり、まだ実行可能です。
ここで私が歩いた平均距離は約0.6m/歩でしたが、同時にchatgptからアジア人の平均ペース距離は0.7m/歩だと教えてもらいました。遅れているようなのでこれを持ってきてくださいデータ (0.6m/ステップ) を生成し、2 番目の走行距離データ セット distance_steps を生成します。上記の距離式を使用して取得されたデータは distance_a として記録されます。ピーク検出がスムーズであれば、これら 2 セットのデータを使用して最適化を実行します。 、異常なデータはほとんどありません (ピーク検出アルゴリズムのデータで異常なデータを収集しました)。X 軸の加速度はより規則的に変化し、外れ値が少ないため、歩行は非常にスムーズで、すべてのデータは比較的良好です。正確なので、 distance_a を信頼してそれに 75% の重みを与えてもよいでしょう。結局のところ、人の歩行状態の判断は 3 軸データの結合に基づいており、距離の式は X 軸のみに依存するため、誤差許容率が大きくなります。 x軸のデータが変動すると距離が累積するため、誤差が大きく誤差許容率が小さいため、このときの重みの75%を distance_steps に与えます。(もう少し感覚的な融合です)
最終結果
ここまでの作業が終わった後、再度25回行ったり来たりしましたが、そのときの平均誤差は0.5m以内に抑えられており、かなりの進歩と言えます。この記事では、IMU を使用してシンプルな機能を実現しました。走行距離計はより正確な走行距離データを提供し、その後の地図ナビゲーションと位置特定のための良い基盤を築くことができます。