これはなに？

LiDAR(VLP16)を使って煙を検知したときのメモです。

背景

自動運転AIチャレンジ2023インテグレーション大会に参加しました。私は今大会初めて参加させていただき、前回までの経験者から色々教えてもらいながら一緒に取り組みました。

今大会では、コース中にスモークが出るシーンがあります。

石油化学プラントなどでは、地面近くに設置されたパイプから放出される蒸気が、センサーの視野を遮ることがあります。多様な環境下での走行には、蒸気、煙の存在する環境でも走行可能な認知システムが求められます。

私はこれまでセンサ開発の経験やLiDARを扱った経験があったため、特に煙対策の部分を楽しませてもらいました。

スモーク対応

2つの機能を検討しました。

スモークフィルタ(smoke_filter)：
点群のうち、スモークらしき点にフラグを立てる。フィルタの点は無視or除去することを想定。
スモーク内検知(is_in_smoke)：
スモーク内にいるかどうかを判断する。スモークの中に入るとLiDARが機能しなくなるので、諸々無視して直進することを想定。

以下、事前配布されたrosbagのデータで動作している様子です。

チームメンバーにVLP16のデータをrosbagからpcapに変換していただき、私はpcapファイルを使って煙対応だけ単体で検討しました。
※ コードも参考まで記載しますが、どこか間違ってる可能性もあります。

スモークフィルタ(smoke_filter)

煙の可能性がある点群にフラグを立てます。フラグが立った点群を除外するなどの処理を想定しています。

VLP16の点群には3次元の座標に加えて強度の情報があります。点群を距離-強度の情報でプロットすると、煙の点群には特徴があることが分かりました（検出距離の割に、反射強度が弱い）。また、今回はVLP16の取り付け位置と煙の発生源から、煙はある程度高い点で検出されるという特徴がありました。

そこで、今回は単純に距離、強度、高さの閾値でフィルタリングすることで、ある程度正しそうに煙を検出することができました。

期待する動作としては、下記のような煙の点群についてフラグがでるはずです。

def smoke_filter(self, cells_lidar):
        # smoke_filter
        # 煙の可能性がある点群を除去する関数

        filt_dist = cells_lidar['Dist']  < self.params_smoke_filt['D']
        filt_I = cells_lidar['I'] < self.params_smoke_filt['I']
        filt_Z = cells_lidar['Z'] + self.params_smoke_filt['SetPosZ'] > self.params_smoke_filt['Z']
        smoke_idx = filt_dist & filt_I & filt_Z
        return smoke_idx

function smoke_idx = func_smoke_filt(cells_lidar,params_smoke_filt);
% func_smoke_filt
% 煙フィルタ：煙の可能性がある点群を除去
% 煙らしい点群にフラグを立てる
       
filt_dist = cells_lidar.Dist(:) < params_smoke_filt.D;
filt_I    = cells_lidar.I  < params_smoke_filt.I;
filt_Z    = cells_lidar.Z + params_smoke_filt.SetPosZ > params_smoke_filt.Z;

smoke_idx = filt_dist & filt_I &  filt_Z;
end

スモーク内検知(is_in_smoke)

煙の中に入ると、何も見えなくなる瞬間があります。特に、LiDAR自体が煙に覆われると点群数が一気に減ります。目の前に何か物があるのか、煙があるのかわかりませんが、LiDARがうまく動いてないときはフラグを出しておくことにしました。

煙中に入るシーンを見てみると、数m以下の極近距離の点ばかりになる、もしくは、本当に何もターゲットがないかのように点群がなくなります。そこで、煙内検出は、

前方の点のうち、ある距離（ここでは5mとしました）以下の点の割合
前方の点のうち、無効な点（ゼロ埋め）の割合

この2つの割合を計算し閾値で切って、どちらか閾値以上であれば煙中だと検出しました。

数では、緑点=前方の点のうち、どの程度が見えなくなったのか、もしくは、データなしの点がある一定値を超えたらフラグを立てるようにしています。煙内の検出したら、画面左上にIn Smoke と表示するようにしてみました。

これで事前公開されたrosbagデータについてはうまく動作しました。

Velodyneの点群を扱う際、どのようにデータを読み込むのかによって、データ無しの表現が異なりました（ゼロ埋め、NaN埋め、データ自体がなくなる）。また、座標の定義も違いました（センサ座標or車両座標）。自動運転AIチャレンジの本番環境に移植するときに、このデータフォーマットの違いに苦労させられました。

function [is_insmoke, valid_idx, valid_ratio, zero_ratio]= func_is_insmoke(cells_lidar,params_is_insmoke);
% func_is_insmoke
% is_insmoke : (bool) 煙の中にいるか？
% valid_ratio : (double) 有効な点群の割合 0~1 %
% zero_ratio : (double) ゼロ埋めされている点群の割合 0~1 %

% 前方() D以上の点群数が~%以上か？
filt_dist = cells_lidar.Dist(:) >  params_is_insmoke.D;

% 前方の点を有効な点とする
% 前方かつDist非ゼロの点をそのフレームの有効な点数とする
filt_front = abs( cells_lidar.Azim(:)) < 90 ;
filt_d0    = cells_lidar.Dist(:) == 0;
front_points_num = nnz(filt_front & ~filt_d0); % 前方かつDist非ゼロ

valid_idx  = filt_dist & filt_front; % 有効な点
valid_num  = nnz(valid_idx); 

% 割合
valid_ratio = valid_num/front_points_num; % 有効な点群の割合 0~1
zero_ratio  = nnz(filt_front & filt_d0) / nnz(filt_front); % 前方の点のうちゼロ埋めされている点群の割合 0~1

% 判定
is_insmoke_valid = valid_ratio < params_is_insmoke.thresh_valid;
is_insmoke_zero  = zero_ratio  > params_is_insmoke.thresh_zero ;

is_insmoke = is_insmoke_valid | is_insmoke_zero;

end

 def is_in_smoke(self, cells_lidar):
        # is_in_smoke
        # is_insmoke : (bool) 煙の中にいるか？
        # valid_ratio : (double) 有効な点群の割合 0~1 %
        # zero_ratio : (double) ゼロ埋めされている点群の割合 0~1 %

        # 前方() D以上の点群数
        filt_dist = cells_lidar['Dist']  > self.params_is_insmoke['D']
        
        # 前方の点を有効な点とする
        # 前方かつDist非ゼロの点をそのフレームの有効な点数とする
        filt_front = np.abs(cells_lidar['Azim']) < 90
        filt_d0 = cells_lidar['Dist'] == 0
        front_points_num = np.count_nonzero(filt_front & ~filt_d0)

        valid_idx = filt_dist & filt_front      # 有効な点
        valid_num = np.count_nonzero(valid_idx)

        # 割合
        valid_ratio = valid_num / front_points_num # 有効な点群の割合 0~1
        zero_ratio = np.count_nonzero(filt_front & filt_d0) / np.count_nonzero(filt_front) # 前方の点のうちゼロ埋めされている点群の割合 0~1
        # zero_ratio = 1-len(cells_lidar['Dist'])/self.params_is_insmoke['max_points_num']
        
        # 判定
        is_insmoke_valid = valid_ratio < self.params_is_insmoke['thresh_valid']
        is_insmoke_zero = zero_ratio > self.params_is_insmoke['thresh_zero']

        is_insmoke = is_insmoke_valid or is_insmoke_zero

        return is_insmoke, valid_idx, valid_ratio, zero_ratio

どちらのアルゴも私の低スペックなノートPCで1フレーム1ms以下で動作していたので、実環境でも問題なしと判断しました。

しかしながら…

簡易的な検討レベルではうまくいきました。

しかし、自動運転AIチャレンジの本番環境に移植するときに、VLP16の点群のデータフォーマットの違いに苦労させられたので、rosbagデータ読み込み部から一貫でコードを書いて、検証しておくべきでした。決勝大会の前週から2週間の海外出張が入ってしまい、本番環境での検証が十分にできないまま、チームメンバーに丸投げしてしまいました。申し訳ない。。。

また、実機テストのチャンスが決勝前日だけだったので、パラメータチューニングに集中できるよう、パラメータもアルゴリズムももっとシンプルにわかりやすくしておくべきでした。

さいごに

忘れないうちに自動運転AIチャレンジ本番環境での動作検証をしておきたいです。

悪天候でのセンシングは個人的にも興味があります。Teslaがカメラだけで自動運転を実現すると言ってますが、カメラ画像データでの霧や雨の対策をしたり、一方で、ミリ波レーダーで補完したり、今後もトライしてみたいです。

本記事のコードはこちら。