創るたのしみを応援

Pololu LPS25HB 気圧/標高センサボードを使ってみる

建物内で自分が今何階にいるか調べる

なぜ自己位置を求めるのか

例えば建物内を移動する自律型ロボットを作った時に、現在の自己位置を求める必要があります。自己位置がわからないと建物内をどのように移動したらいいのかわからないからです。カメラや、距離センサーなどで、平面状での自己位置を推定することはできますが、上下の位置(現在何階にいるのか)を求めるには少し工夫がいります。

LPS25HBなら気圧から標高を求めることができるので、建物内の階を簡単に調べることができます。気圧は日によって違うので、標高計算にもかなり誤差がありますが、ある階の気圧を基準値として取れば、かなり正確に何階にいるかを調べられます。おおよその誤差は±60cmだそうですが、階を求めるには十分でしょう。

標高の求め方

標高は、ライブラリのpressureToAltitudeMeters関数を使って求めます。気圧から、標高を計算してくれます。

環境

3階建てのアパートです。ちょっと人目が気になりますが、PCをもって階段を上り下りしてみます。そしてログを取ってみたいと思います。

実験

Nucleoで実験をしました。各階で10秒位停止し、ログはTeratermで確認しています。

実験用プログラム

先にLPS25HBライブラリをインポートしてからコンパイルします。

#include "mbed.h"
#include "LPS.h"

I2C i2c(D14,D15);
LPS ps(i2c);

int main(){
    wait_ms(100);
    if (!ps.init()){
        printf("Failed to autodetect pressure sensor!\r\n");
        while (1);
    }
    ps.enableDefault();
    int time = 0;
    float pressure = ps.readPressureMillibars();
    float target_altitude = ps.pressureToAltitudeMeters(pressure); // 基準階の標高
    
    while(1){
        pressure = ps.readPressureMillibars();
        float temperature = ps.readTemperatureC();
        float proportional_altitude = ps.pressureToAltitudeMeters(pressure) - target_altitude;//相対的な標高
        
        printf("\r\nTime: %4d s\r\n",time);
        printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,proportional_altitude,temperature);
        
        time++;
        wait_ms(999);
    }
}

実験結果

つぎのようなデータが取れました。

1階
Time: 0 s p:1003.98 mbar a:0.00 m t:13.17 deg C
階段を上る

2階
Time: 20 s p:1003.64 mbar a:2.80 m t:12.11 deg C
階段を上る

3階
Time: 40 s p:1003.26 mbar a:5.99 m t:10.81 deg C
階段を半分降りる

2.5階
Time: 54 s p:1003.53 mbar a:3.73 m t:10.27 deg C
階段を半分降りる

2階
Time: 70 s p:1003.69 mbar a:2.40 m t:10.04 deg C
1階
Time: 0 s p:1003.98 mbar a:0.00 m t:13.17 deg C
Time: 1 s p:1003.98 mbar a:-0.04 m t:13.04 deg C
Time: 2 s p:1003.93 mbar a:0.37 m t:13.14 deg C
Time: 3 s p:1003.96 mbar a:0.16 m t:13.17 deg C
Time: 4 s p:1004.01 mbar a:-0.27 m t:13.12 deg C
Time: 5 s p:1003.98 mbar a:0.01 m t:13.14 deg C
Time: 6 s p:1003.99 mbar a:-0.14 m t:13.02 deg C
Time: 7 s p:1003.89 mbar a:0.76 m t:13.03 deg C
Time: 8 s p:1003.97 mbar a:0.07 m t:13.02 deg C
Time: 9 s p:1004.01 mbar a:-0.24 m t:12.97 deg C
階段を上る
Time: 10 s p:1003.91 mbar a:0.56 m t:12.79 deg C
Time: 11 s p:1003.98 mbar a:0.00 m t:12.74 deg C
Time: 12 s p:1003.93 mbar a:0.37 m t:12.72 deg C
Time: 13 s p:1003.92 mbar a:0.46 m t:12.69 deg C
Time: 14 s p:1003.84 mbar a:1.13 m t:12.65 deg C
Time: 15 s p:1003.80 mbar a:1.45 m t:12.58 deg C
Time: 16 s p:1003.84 mbar a:1.15 m t:12.54 deg C
Time: 17 s p:1003.81 mbar a:1.41 m t:12.44 deg C
Time: 18 s p:1003.70 mbar a:2.29 m t:12.42 deg C
Time: 19 s p:1003.67 mbar a:2.59 m t:12.35 deg C
2階
Time: 20 s p:1003.64 mbar a:2.80 m t:12.11 deg C
Time: 21 s p:1003.67 mbar a:2.54 m t:12.02 deg C
Time: 22 s p:1003.69 mbar a:2.43 m t:11.90 deg C
Time: 23 s p:1003.61 mbar a:3.06 m t:11.83 deg C
Time: 24 s p:1003.60 mbar a:3.14 m t:11.77 deg C
Time: 25 s p:1003.72 mbar a:2.19 m t:11.70 deg C
Time: 26 s p:1003.62 mbar a:2.99 m t:11.66 deg C
Time: 27 s p:1003.59 mbar a:3.26 m t:11.63 deg C
Time: 28 s p:1003.64 mbar a:2.87 m t:11.61 deg C
Time: 29 s p:1003.62 mbar a:3.02 m t:11.58 deg C
階段を上る
Time: 30 s p:1003.61 mbar a:3.09 m t:11.53 deg C
Time: 31 s p:1003.62 mbar a:2.97 m t:11.56 deg C
Time: 32 s p:1003.56 mbar a:3.48 m t:11.55 deg C
Time: 33 s p:1003.53 mbar a:3.75 m t:11.53 deg C
Time: 34 s p:1003.51 mbar a:3.95 m t:11.46 deg C
Time: 35 s p:1003.39 mbar a:4.95 m t:11.40 deg C
Time: 36 s p:1003.38 mbar a:5.02 m t:11.26 deg C
Time: 37 s p:1003.35 mbar a:5.28 m t:11.14 deg C
Time: 38 s p:1003.37 mbar a:5.12 m t:11.06 deg C
Time: 39 s p:1003.29 mbar a:5.75 m t:10.89 deg C
3階
Time: 40 s p:1003.26 mbar a:5.99 m t:10.81 deg C
Time: 41 s p:1003.27 mbar a:5.92 m t:10.66 deg C
Time: 42 s p:1003.33 mbar a:5.42 m t:10.60 deg C
Time: 43 s p:1003.29 mbar a:5.78 m t:10.64 deg C
Time: 44 s p:1003.26 mbar a:6.03 m t:10.64 deg C
Time: 45 s p:1003.40 mbar a:4.85 m t:10.64 deg C
Time: 46 s p:1003.36 mbar a:5.18 m t:10.60 deg C
Time: 47 s p:1003.29 mbar a:5.78 m t:10.56 deg C
Time: 48 s p:1003.39 mbar a:4.92 m t:10.49 deg C
Time: 49 s p:1003.35 mbar a:5.26 m t:10.38 deg C
階段を半分降りる
Time: 50 s p:1003.36 mbar a:5.16 m t:10.23 deg C
Time: 51 s p:1003.36 mbar a:5.19 m t:10.23 deg C
Time: 52 s p:1003.44 mbar a:4.54 m t:10.24 deg C
Time: 53 s p:1003.47 mbar a:4.25 m t:10.27 deg C
2.5階
Time: 54 s p:1003.53 mbar a:3.73 m t:10.27 deg C
Time: 55 s p:1003.58 mbar a:3.31 m t:10.19 deg C
Time: 56 s p:1003.53 mbar a:3.79 m t:10.16 deg C
Time: 57 s p:1003.54 mbar a:3.69 m t:10.13 deg C
Time: 58 s p:1003.53 mbar a:3.76 m t:10.11 deg C
Time: 59 s p:1003.51 mbar a:3.89 m t:10.07 deg C
Time: 60 s p:1003.53 mbar a:3.78 m t:10.12 deg C
Time: 61 s p:1003.47 mbar a:4.29 m t:10.11 deg C
Time: 62 s p:1003.53 mbar a:3.72 m t:10.12 deg C
Time: 63 s p:1003.47 mbar a:4.29 m t:10.14 deg C
Time: 64 s p:1003.52 mbar a:3.87 m t:10.12 deg C
階段を半分降りる
Time: 65 s p:1003.58 mbar a:3.35 m t:10.14 deg C
Time: 66 s p:1003.57 mbar a:3.43 m t:10.15 deg C
Time: 67 s p:1003.58 mbar a:3.31 m t:10.15 deg C
Time: 68 s p:1003.64 mbar a:2.81 m t:10.11 deg C
Time: 69 s p:1003.67 mbar a:2.60 m t:10.12 deg C
2階
Time: 70 s p:1003.69 mbar a:2.40 m t:10.04 deg C
Time: 71 s p:1003.72 mbar a:2.13 m t:9.98 deg C
Time: 72 s p:1003.66 mbar a:2.63 m t:9.88 deg C
Time: 73 s p:1003.67 mbar a:2.58 m t:9.77 deg C
Time: 74 s p:1003.62 mbar a:3.03 m t:9.64 deg C
Time: 75 s p:1003.56 mbar a:3.48 m t:9.54 deg C
Time: 76 s p:1003.60 mbar a:3.15 m t:9.43 deg C
Time: 77 s p:1003.67 mbar a:2.61 m t:9.36 deg C
Time: 78 s p:1003.70 mbar a:2.29 m t:9.27 deg C
Time: 79 s p:1003.69 mbar a:2.38 m t:9.14 deg C
Time: 80 s p:1003.67 mbar a:2.57 m t:9.15 deg C

現在自分がいる階を求める

実験結果で求めた数値を使って、現在自分がいる階を推定してみます。

どうやって計算しようか迷ったのですが、単純に平均値をとってその値±0.60mをその階の条件として判定にします。

プログラム

#include "mbed.h"
#include "LPS.h"

I2C i2c(D14,D15);
LPS ps(i2c);

int main(){
    wait_ms(100);
    if (!ps.init()){
        printf("Failed to autodetect pressure sensor!\r\n");
        while (1);
    }
    ps.enableDefault();
    int time = 0;
    float pressure = ps.readPressureMillibars();
    float target_altitude = ps.pressureToAltitudeMeters(pressure); // 基準階の標高
    
    while(1){
        pressure = ps.readPressureMillibars();
        float temperature = ps.readTemperatureC();
        float proportional_altitude = ps.pressureToAltitudeMeters(pressure) - target_altitude; // 相対的な標高
        
        printf("\r\nTime: %4d s\r\n",time);
        printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\t",pressure,proportional_altitude,temperature);
        
        if((proportional_altitude - 5.513) >= -0.6 && (proportional_altitude - 5.513) <= 0.6){
            printf("floor:3\r\n");
        }
        else if((proportional_altitude - 2.83) >= -0.6 && (proportional_altitude - 2.83) <= 0.6){
            printf("floor:2\r\n");
        }
        else if((proportional_altitude - 0.068) >= -0.6 && (proportional_altitude - 0.068) <= 0.6){
            printf("floor:1\r\n");
        }
        else{
            printf("steps\r\n"); // 階段
        }
        
        time++;
        wait_ms(999);
    }
}

実行結果

1階→2階→3階→2階→1階→2階と移動しました。

stepsと表示されているときは階段です。少し誤差がありますが、ある程度正確に階を判定することができました。

Time: 0 s p:1003.68 mbar a:0.00 m t:15.69 deg C floor:1
Time: 5 s p:1003.57 mbar a:0.91 m t:14.92 deg C steps
Time: 10 s p:1003.47 mbar a:1.72 m t:14.35 deg C steps
Time: 15 s p:1003.34 mbar a:2.87 m t:13.92 deg C floor:2
Time: 20 s p:1003.20 mbar a:4.00 m t:13.47 deg C steps
Time: 25 s p:1003.10 mbar a:4.82 m t:13.17 deg C steps
Time: 30 s p:1002.96 mbar a:6.00 m t:12.72 deg C floor:3
Time: 35 s p:1003.06 mbar a:5.15 m t:12.45 deg C floor:3
Time: 40 s p:1003.21 mbar a:3.94 m t:12.15 deg C steps
Time: 45 s p:1003.36 mbar a:2.67 m t:11.87 deg C floor:2
Time: 50 s p:1003.48 mbar a:1.66 m t:11.46 deg C steps
Time: 55 s p:1003.60 mbar a:0.67 m t:11.25 deg C floor:1
Time: 60 s p:1003.70 mbar a:-0.16 m t:11.17 deg C floor:1
Time: 65 s p:1003.62 mbar a:0.51 m t:10.86 deg C floor:1
Time: 70 s p:1003.43 mbar a:2.05 m t:10.51 deg C steps
Time: 75 s p:1003.40 mbar a:2.30 m t:10.37 deg C floor:2
Time: 0 s p:1003.68 mbar a:0.00 m t:15.69 deg C floor:1
Time: 1 s p:1003.61 mbar a:0.54 m t:15.56 deg C floor:1
Time: 2 s p:1003.60 mbar a:0.65 m t:15.38 deg C floor:1
Time: 3 s p:1003.60 mbar a:0.66 m t:15.21 deg C floor:1
Time: 4 s p:1003.64 mbar a:0.29 m t:15.01 deg C floor:1
Time: 5 s p:1003.57 mbar a:0.91 m t:14.92 deg C steps
Time: 6 s p:1003.56 mbar a:0.98 m t:14.84 deg C steps
Time: 7 s p:1003.53 mbar a:1.28 m t:14.75 deg C steps
Time: 8 s p:1003.46 mbar a:1.82 m t:14.59 deg C steps
Time: 9 s p:1003.50 mbar a:1.51 m t:14.37 deg C steps
Time: 10 s p:1003.47 mbar a:1.72 m t:14.35 deg C steps
Time: 11 s p:1003.49 mbar a:1.59 m t:14.32 deg C steps
Time: 12 s p:1003.34 mbar a:2.82 m t:14.13 deg C floor:2
Time: 13 s p:1003.35 mbar a:2.77 m t:14.11 deg C floor:2
Time: 14 s p:1003.32 mbar a:3.00 m t:14.00 deg C floor:2
Time: 15 s p:1003.34 mbar a:2.87 m t:13.92 deg C floor:2
Time: 16 s p:1003.31 mbar a:3.09 m t:13.83 deg C floor:2
Time: 17 s p:1003.26 mbar a:3.48 m t:13.73 deg C steps
Time: 18 s p:1003.30 mbar a:3.16 m t:13.64 deg C floor:2
Time: 19 s p:1003.27 mbar a:3.43 m t:13.52 deg C floor:2
Time: 20 s p:1003.20 mbar a:4.00 m t:13.47 deg C steps
Time: 21 s p:1003.24 mbar a:3.70 m t:13.47 deg C steps
Time: 22 s p:1003.15 mbar a:4.43 m t:13.44 deg C steps
Time: 23 s p:1003.22 mbar a:3.84 m t:13.35 deg C steps
Time: 24 s p:1003.12 mbar a:4.67 m t:13.24 deg C steps
Time: 25 s p:1003.10 mbar a:4.82 m t:13.17 deg C steps
Time: 26 s p:1003.06 mbar a:5.15 m t:13.09 deg C floor:3
Time: 27 s p:1002.98 mbar a:5.89 m t:13.00 deg C floor:3
Time: 28 s p:1003.00 mbar a:5.69 m t:12.87 deg C floor:3
Time: 29 s p:1002.95 mbar a:6.12 m t:12.82 deg C steps
Time: 30 s p:1002.96 mbar a:6.00 m t:12.72 deg C floor:3
Time: 31 s p:1003.02 mbar a:5.52 m t:12.76 deg C floor:3
Time: 32 s p:1002.97 mbar a:5.90 m t:12.79 deg C floor:3
Time: 33 s p:1002.91 mbar a:6.48 m t:12.76 deg C steps
Time: 34 s p:1002.91 mbar a:6.46 m t:12.62 deg C steps
Time: 35 s p:1003.06 mbar a:5.15 m t:12.45 deg C floor:3
Time: 36 s p:1003.02 mbar a:5.53 m t:12.25 deg C floor:3
Time: 37 s p:1003.09 mbar a:4.95 m t:12.22 deg C floor:3
Time: 38 s p:1003.11 mbar a:4.73 m t:12.18 deg C steps
Time: 39 s p:1003.23 mbar a:3.76 m t:12.18 deg C steps
Time: 40 s p:1003.21 mbar a:3.94 m t:12.15 deg C steps
Time: 41 s p:1003.23 mbar a:3.74 m t:12.06 deg C steps
Time: 42 s p:1003.24 mbar a:3.67 m t:12.02 deg C steps
Time: 43 s p:1003.21 mbar a:3.93 m t:11.95 deg C steps
Time: 44 s p:1003.34 mbar a:2.86 m t:11.89 deg C floor:2
Time: 45 s p:1003.36 mbar a:2.67 m t:11.87 deg C floor:2
Time: 46 s p:1003.37 mbar a:2.60 m t:11.81 deg C floor:2
Time: 47 s p:1003.30 mbar a:3.16 m t:11.76 deg C floor:2
Time: 48 s p:1003.37 mbar a:2.62 m t:11.73 deg C floor:2
Time: 49 s p:1003.33 mbar a:2.89 m t:11.60 deg C floor:2
Time: 50 s p:1003.48 mbar a:1.66 m t:11.46 deg C steps
Time: 51 s p:1003.43 mbar a:2.05 m t:11.41 deg C steps
Time: 52 s p:1003.54 mbar a:1.13 m t:11.38 deg C steps
Time: 53 s p:1003.60 mbar a:0.66 m t:11.35 deg C floor:1
Time: 54 s p:1003.59 mbar a:0.75 m t:11.33 deg C steps
Time: 55 s p:1003.60 mbar a:0.67 m t:11.25 deg C floor:1
Time: 56 s p:1003.65 mbar a:0.22 m t:11.24 deg C floor:1
Time: 57 s p:1003.64 mbar a:0.31 m t:11.23 deg C floor:1
Time: 58 s p:1003.66 mbar a:0.19 m t:11.24 deg C floor:1
Time: 59 s p:1003.64 mbar a:0.35 m t:11.19 deg C floor:1
Time: 60 s p:1003.70 mbar a:-0.16 m t:11.17 deg C floor:1
Time: 61 s p:1003.69 mbar a:-0.14 m t:11.19 deg C floor:1
Time: 62 s p:1003.66 mbar a:0.18 m t:11.10 deg C floor:1
Time: 63 s p:1003.66 mbar a:0.18 m t:11.00 deg C floor:1
Time: 64 s p:1003.58 mbar a:0.80 m t:10.93 deg C steps
Time: 65 s p:1003.62 mbar a:0.51 m t:10.86 deg C floor:1
Time: 66 s p:1003.60 mbar a:0.66 m t:10.75 deg C floor:1
Time: 67 s p:1003.49 mbar a:1.59 m t:10.66 deg C steps
Time: 68 s p:1003.53 mbar a:1.26 m t:10.61 deg C steps
Time: 69 s p:1003.44 mbar a:1.97 m t:10.52 deg C steps
Time: 70 s p:1003.43 mbar a:2.05 m t:10.51 deg C steps
Time: 71 s p:1003.42 mbar a:2.13 m t:10.50 deg C steps
Time: 72 s p:1003.36 mbar a:2.65 m t:10.51 deg C floor:2
Time: 73 s p:1003.41 mbar a:2.26 m t:10.45 deg C floor:2
Time: 74 s p:1003.34 mbar a:2.85 m t:10.38 deg C floor:2
Time: 75 s p:1003.40 mbar a:2.30 m t:10.37 deg C floor:2
Time: 76 s p:1003.38 mbar a:2.50 m t:10.32 deg C floor:2
Time: 77 s p:1003.34 mbar a:2.80 m t:10.27 deg C floor:2
Time: 78 s p:1003.28 mbar a:3.37 m t:10.24 deg C floor:2
Time: 79 s p:1003.28 mbar a:3.36 m t:10.22 deg C floor:2

まとめ

このように現在何階にいるかを簡単に調べることができました。

この技術を使えばドローンの飛行高度を制御したり、いろいろな用途に使えます。皆さんも何か面白い使い方を考えてみてください。

  1. Pololu LPS25HB概要
  2. Arduino版
  3. Nucleo mbed版
  4. Raspberry Pi版
  5. 建物内で何階にいるか推定実験

SNSでフォローする