Lidar SLAMアルゴリズム諸元

Lidar実装のROS自律移動ロボットに欠かせないSLAM

※wikipedia: 「SLAM(スラム)とは、自己位置推定と環境地図作成を同時に行うことを言う。正式名称は、Simultaneous Localization and Mapping、位置の推定やマップ作成にはレーザーレンジスキャナー(測域センサ)、カメラ、エンコーダ、マイクロフォンアレイなどが利用されることが多い。」

SLAM実現するための三角法・TOF法等の原理を活用したレーザLidar装置を実装したロボットがある他、RGBDデプスカメラ、RGB 2眼・単眼カメラを実装してSLAMを実現するロボット、またLidarにRGBDデプスカメラの両方混在のロボットもあるようだ。以下、Lidar SLAMと呼ばれる、Gmapping、Hector、Google Cartographer(以下Cartographerに簡略化する)の諸元を比較してみる。

諸元 Gmapping Hector Cartographer
アルゴリズム・ベース RBPF(Rao-Blackwell→Particle Filter) Scan-Matching+拡張Kalman filter Graph-base
Loop-Closing なし なし あり
DOF 3DOF(Odom+Lidar) 3DOF(Lidar)/6DOF(IMU+Lidar) 3DOF(Lidar)/6DOF(IMU+Lidar)
メリット 屋内環境、メジャー 不整地、odom情報いらない 屋内環境向け、odom情報いらない
デメリット odom情報が必須、不整地、広域に不向き Lidarフレーム更新頻度、精度に要求高い CPUにかかる計算負荷が大きい

関連記事

9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応
SLAM~拡張カルマンフィルタ
研究開発用 台車型ロボット キット

0

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

2輪差動駆動型ロボット2WDの運動学モデル

運動学分析

2輪差動駆動型ロボット運動学モデル
2輪差動駆動型ロボット運動学モデル

以上のイラストに示された、車軸中心Cの瞬間速度はv、回転角速度ω、回転半径rは、以下の数式で表される。
$$\begin{bmatrix}v\\ω\\r\end{bmatrix}=\begin{bmatrix}\frac{v_1 + v_2}{2}\\\frac{v_1 – v_2}{d}\\\frac{v}{ω}\end{bmatrix}=\begin{bmatrix}\frac{v_1 + v_2}{2}\\\frac{v_1 – v_2}{d}\\\frac{d}{2} \frac{v_1 + v_2}{v_1 – v_2}\end{bmatrix}$$
但し、d: 車輪間距離、v1: 左輪速度、v2: 右輪速度、ω: 角速度、r: 回転半径

運動状態分析

2輪差動駆動型ロボット運動状態
2輪差動駆動型ロボット運動状態

if v1 > v2 → ロボットが右方向へ回転する
if v1 = v2 → ロボットが直進または後退する
if v1 = -v2 → ロボットが車軸中心Cまわりに回転する、回転半径0m

運動学モデル

ロボット座標系からワールド座標系へ回転変換する際、2次元回転行列を用いる。
$$\begin{bmatrix}x_{t+1}\\x_{t+1}\\θ_{t+1}\end{bmatrix}=\begin{bmatrix}x_{t}\\x_{t}\\θ_{t}\end{bmatrix}+ \begin{bmatrix}cosθ&-sinθ&0\\sinθ&cosθ&0\\0&0&1\end{bmatrix}\begin{bmatrix}Δx_r\\Δy_r\\Δθ_r\end{bmatrix}$$
$$\begin{bmatrix}Δx_r\\Δy_r\\Δθ_r\end{bmatrix}=\begin{bmatrix}cosΔθ_r&0\\sinΔθ_r&0\\0&1\end{bmatrix}\begin{bmatrix}v_rΔt\\ω_rΔt\end{bmatrix}$$
また、他の計算方法があり、いずも近似式で、走行距離とともに誤差が累積していく。
ROSでは、上記情報のもとで、odomトピックを他のノードへブロードキャストし続ける。

関連記事

9軸IMU 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応

0

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

DCブラシモータのPID制御

古典制御

モータへの入力(PWM制御のオンの時間幅より得られた正弦波の交流電圧)とモータからの出力(エンコーダより得られたモータ軸の回転後位置の値)に注目して、しかしピッタリ、しかも早く回転後の目標値に達するには、入力値と出力値は単純に線形関係では済まず、以下のイラストに示される、PID(P:エンコーダ出力値と目標値の差の線形比率成分、I:エンコーダ出力値と目標値の差の積分成分、D:エンコーダ出力値と目標値の差の微分成分)が考案された古典制御である。

Wikipedia PID制御のブロック図
Wikipedia PID制御のブロック図

PID制御の実現について、以下のようにArduinoボードに実装する。

// p->output:モータへの入力、p->Encoder:モータからの出力
input = p->Encoder - p->PrevEnc;
Perror = p->TargetTicksPerFrame - input;
// PIDのP成分、I成分、D成分を線形合成する
output = (Kp * Perror - Kd * (input - p->PrevInput) + p->ITerm) / Ko;
p->PrevEnc = p->Encoder;
// 簡略化を図るゆえにoutputの最大値の判定およびその際の処理を省略する
output += p->output;
p->ITerm += Ki * Perror;
p->output = output;
p->PrevInput = input;

PID制御のP I Dに伴う係数のKp Ki Kdが、以上ソースコードを実装したArduinoボード、モータコントローラ、モータを接続して、ロボットの実重さ、実走行環境にて実験を行い、Kp Ki Kdを決める方法がある。これを別途記述とする。

以上ソースコードの出典は以下のとおり、但し簡略化を図るゆえにソースの一部を削除したことがあり、ご注意願います。

https://github.com/hbrobotics/ros_arduino_bridge/blob/indigo-devel/ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h

現代・ポスト現代制御

現代制御では状態空間モデルが用いられて、ポスト現代制御では代表的なものにロバスト制御や適応制御がある。ニューラルネットワークを用いる深層学習、強化学習の活用が期待される。

関連記事

9軸IMU 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応

0

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

ROS・Unity・ロボット・ドローン姿勢の回転表現

はじめに

姿勢\(v\)から姿勢\(v’\)への変換はどう表現するのかは、ロボットや、ドローンの開発、運用に避けられない要素である。本文に出ているソースコードは、\(Python\)言語を用いる。

オイラー角、回転行列の表現

固定座標系・機体座標系・XYZ軸回転順オイラー角
固定座標系・機体座標系・XYZ軸回転順オイラー角

物体を\(X\)軸、\(Y\)軸(\(Y’\)軸)、\(Z\)軸(\(Z”\)軸)まわりの順にそれぞれオイラー角のロール角\(ϕ\)、ピッチ角\(θ\)、ヨー角\(ψ\)だけ回転させたときに、物体の姿勢の変換は、オイラー角と、もしくはオイラー角の三角関数を用いる以下の回転行列で表す。
$$\small R_{xyz} = \begin{bmatrix} CθCψ & -CθSψ & Sθ \\ SϕSθCψ+CϕSψ & -SϕSθSψ+CϕCψ & -SϕCθ \\-CϕSθCψ+SϕSψ & -CϕSθSψ+SϕCψ & CϕCθ \end{bmatrix} $$ ただし、\(C=cos,S=sin\) とする。
しかし、以下のイラストのとおり、オイラー角による姿勢制御の弱点\((Gimbal \space Lock)\)があり、例えば\(Y’\)軸回りを\(90°\)回転すると、\(X’\)軸と\(Z\)軸が同軸となってしまい、以降は姿勢制御(表現)ができなくなる。
ジンバルロック
ジンバルロック

このジンバルロックを解消するにはクォータニオンの使命となった。

回転ベクトルの表現

\(ROS\)では回転ベクトルのクォータニオン(四元数)\(q=ix+jy+kz+w\)は、

q=(x,y,z,w)

と表す。原点を通す回転軸を表す単位ベクトル\(a=(ax,ay,az)\)で、この回転軸まわり、角度\(θ\)だけを回転する場合は、クォータニオンは

(x,y,z,w)=(ax*sin(θ/2), ay*sin(θ/2), az*sin(θ/2),cos(θ/2))

と表す。

以下の例では、\(XY\)平面\((z=0)\)にて辺長\(1m\)の正方形に沿って例えば仮に自律移動ロボットに走行させて、動作を確認しよう。勿論\(Gazebo\)でも確認できる。

\(ROS\)では、ロボットの位置&姿勢の表現について、ロボットの中心または、ロボットにある他のポイントの位置は\(x,y,z(m)\)、姿勢ポーズはクォータニオンで表す。

from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
...
locations['square_vertex_1'] = pose = Pose(Point(1.0,0.0,0.0), Quaternion(0.0,0.0,0.0,1.0))
locations['square_vertex_2'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
locations['square_vertex_3'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
locations['square_vertex_4'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
...

回転ベクトル→オイラー角

geometry_msgs::Quaternion orientation = msg->pose.pose.orientation;    
tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));    
double yaw, pitch, roll;    
mat.getEulerYPR(yaw, pitch, roll);

オイラー角→回転ベクトル

tf::Quaternion q;
q.setRPY(Out_X, Out_Y, Out_Z);
sensor_msgs::Imu imu_data;
imu_data.orientation.x=q[0];
imu_data.orientation.y=q[1];
imu_data.orientation.z=q[2];
imu_data.orientation.w=q[3];

関連記事

オイラー角~ジンバルロック~クォータニオン
9軸IMU 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応
研究開発用 台車型ロボット キット

0

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

3DR Pixhawk Mini フライトコントローラー キット

3DR Pixhawk Mini フライトコントローラー キット
3DR Pixhawk Mini フライトコントローラー キット

概要

3DR Pixhawk Mini Autopilot & Micro M8N GPS コンパス付き & PM06 PDBボードのフライトコントローラーキットです。3DR Pixhawk MiniオートパイロットはPixhawkの次世代進化バージョンです。 元のPixhawkのサイズの約1/3で、より強力なプロセッサとセンサーを備えています。PPX4オープンハードウェアプロジェクトに基づいており、PX4フライトスタック用に最適化されています。

主な仕様

メイン・プロセッサ

・STM32F427 Rev 3

IOプロセッサ

・STM32F103

センサ

・Accel / Gyro / Mag:MPU9250
・Accel / Gyro:ICM20608
・気圧計:MS5611

電圧定格

・パワーモジュール出力:4.1~5.5V
・最大入力電圧:45V(10S LiPo)
・最大電流検出:90A
・USB電源入力:4.1~5.5V
・サーボレール入力:0~10V

サイズ・重量

・38x43x12mm
・15.8g

GPSモジュール

・GNSS受信機:ublox Neo-M8N
・コンパスHMC5983
・重量:22.4g
・サイズ:37x37x12mm

インタフェース

・1×UARTシリアルポート
・Spektrum DSM / DSM2 / DSM-X?衛星対応
・フタバSBUS?対応
・PPM合計信号入力
・I2C
・CAN
・ADC
・内蔵マイクロUSBポート

開封動画

販売サイト

3DR Pixhawk Mini フライトコントローラー キット

関連記事

研究開発用 台車型ロボット キット

以上

0

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア