サンプルデモ
参考書籍
ロボットのモデル化
不確かさの分類
1. 継続期間が一瞬であるもの
- 小石への片輪の乗り上げ
- 走り出し
- 停止時のロボットの揺れ
2. 継続期間が数秒から数十秒であるもの
- 縁石への乗り上げ
- 走行環境の傾斜
3. 走行中ずっと継続するもの
- 左右の車輪にかかる荷重のバランスやモータの個体差
- タイヤの状態
4. 雑音のレベルを超えたもの
- 走行不能になるレベルのスタック
- 人間の干渉
不確かさの実装
雑音: 突発的な向きの変化
- 地面の小石などをランダムに踏んで、向きが少しずれるという想定
- 走行する直前あるいは小石を踏んだ後に、次に踏むまでの道のりを計算
- 計算した道のり分だけ進んだら、ロボットの向きをランダムにずらす
指数分布による表現方法
- 指数分布とは？
bellcurve.jp は道のりあたりに踏みつける小石の数の期待値
- 逆数の
は小石一つを踏むまでの道のりの期待値
Juliaサンプルコード
- 乱数を生成するためのパッケージ
docs.julialang.org - 各種分布を生成するためのパッケージ
juliastats.org
# 既に作成したロボットクラスのコードに必要な処理を追加 # 追記した部分だけ記載 using Plots, Random, Distributions # 各種分布や乱数を生成するためのパッケージを追加 pyplot() mutable struct DifferentialWheeledRobot # メンバ変数を追加 noise_expon # 指数分布オブジェクト noise_norm # ガウス分布オブジェクト dist_until_noise # 次の小石を踏むまでの道のり # 初期化関数 # 追加引数 noise_per_meter 1mあたりの小石の数(デフォルトでは0) # 追加引数 noise_std 小石を踏んだ時に向きθに発生する雑音の標準偏差(デフォルトでは0) function DifferentialWheeledRobot(noise_per_meter::Int64=0, # 追加 noise_std::Float64=0.0) # 追加 self.noise_expon = Exponential(1.0/(1e-100 + noise_per_meter)) # 指数分布オブジェクトを生成 self.noise_norm = Normal(0.0, noise_std) # 正規分布オブジェクトを生成 self.dist_until_noise = rand(self.noise_expon) # 次の小石を踏むまでの道のりを指数分布に従った乱数で決定 end end # ロボットの姿勢に雑音を付加する関数 # 引数 self::DifferentialWheeledRobot ロボットクラスのオブジェクト # 引数 pose ロボットの姿勢 # 引数 speed 速度指令 # 引数 yaw_rate 角速度指令 # 引数 delta_time サンプリングタイム function noise(self::DifferentialWheeledRobot, pose::Array, speed::Float64, yaw_rate::Float64, delta_time::Float64) # 移動した分だけ次に小石を踏むまでの道のりを減らす self.dist_until_noise -= abs(speed) * delta_time + self.radius * abs(yaw_rate) * delta_time if self.dist_until_noise <= 0.0 # 次の小石を踏む時 self.dist_until_noise += rand(self.noise_expon) # また次に踏むまでの道のりをセット pose[3] += rand(self.noise_norm) # ガウス分布に従った雑音をロボットの向きに加える end return pose # 雑音が加えられた姿勢を返す end # ロボットの向きに雑音を加える処理を追加 function draw!(self::DifferentialWheeledRobot) # 現在の姿勢 x, y, theta = self.pose[1], self.pose[2], self.pose[3] xn = x + self.radius * cos(theta) yn = y + self.radius * sin(theta) # ロボットを描画 plot!([x, xn], [y, yn], color=self.color, legend=false, aspect_ratio=true) plot!(circle(x, y, self.radius), linecolor=self.color, aspect_ratio=true) # 移動軌跡を描画 plot!(self.traj_x, self.traj_y, color="black", legend=false, aspect_ratio=true) # センサによる観測を描画 observation = [] if self.sensor !== nothing data(self.sensor, self.pose) draw!(self.sensor, self.pose) observation = self.sensor.last_data end # エージェントからの制御指令を計算 spd, yr = draw_decision!(self.agent, observation) # 次の時間における姿勢を計算 self.pose = state_transition(spd, yr, self.delta_time, self.pose) self.pose = noise(self, self.pose, spd, yr, self.delta_time) # 雑音を付加 push!(self.traj_x, self.pose[1]), push!(self.traj_y, self.pose[2]) end
- サンプルデモ(灰色:雑音あり、赤色:雑音なし)
github.com
移動速度へのバイアス
- 制御指令の速度
と角速度
に一定の係数を掛け算してバイアスを発生させる
- 係数は初期化時にガウス分布からドローして決める
Juliaサンプルコード
# 既に作成したロボットクラスのコードに必要な処理を追加 # 追記した部分だけ記載 using Plots, Random, Distributions # 各種分布や乱数を生成するためのパッケージを追加 pyplot() mutable struct DifferentialWheeledRobot # メンバ変数を追加 bias_rate_spd # 速度に掛ける係数 bias_rate_yr # 角速度に掛ける係数 # 初期化関数 # 追加引数 bias_rate_std バイアス係数を決めるためのガウス分布の標準偏差(デフォルトでは0.0) function DifferentialWheeledRobot(bias_rate_stds::Array=[0.0, 0.0]) # 追加 self.bias_rate_spd = rand(Normal(1.0, bias_rate_stds[1])) # 速度のバイアス係数を決定 self.bias_rate_yr = rand(Normal(1.0, bias_rate_stds[2])) # 角速度のバイアス係数を決定 end end # バイアスの生じた速度と角速度を計算する関数 # 引数 self::DifferenntialWheeledRobot ロボットクラスのオブジェクト # 引数 speed 速度[m/s] # 引数 yaw_rate 角速度[rad/s] function bias(self::DifferentialWheeledRobot, speed::Float64, yaw_rate::Float64) # それぞれにバイアス係数を掛けた値を返す return speed * self.bias_rate_spd, yaw_rate * self.bias_rate_yr end # 速度と角速度にバイアスを加える処理を追加 function draw!(self::DifferentialWheeledRobot) # 現在の姿勢 x, y, theta = self.pose[1], self.pose[2], self.pose[3] xn = x + self.radius * cos(theta) yn = y + self.radius * sin(theta) # ロボットを描画 plot!([x, xn], [y, yn], color=self.color, legend=false, aspect_ratio=true) plot!(circle(x, y, self.radius), linecolor=self.color, aspect_ratio=true) # 移動軌跡を描画 plot!(self.traj_x, self.traj_y, color="black", legend=false, aspect_ratio=true) # センサによる観測を描画 observation = [] if self.sensor !== nothing data(self.sensor, self.pose) draw!(self.sensor, self.pose) observation = self.sensor.last_data end # エージェントからの制御指令を計算 spd, yr = draw_decision!(self.agent, observation) # 次の時間における姿勢を計算 spd, yr = bias(self, spd, yr) # バイアスを付加 self.pose = state_transition(spd, yr, self.delta_time, self.pose) self.pose = noise(self, self.pose, spd, yr, self.delta_time) # 雑音を付加 push!(self.traj_x, self.pose[1]), push!(self.traj_y, self.pose[2]) end
- サンプルデモ(灰色:バイアスあり、赤色:バイアスなし)
github.com
スタック
- ロボットが何かに引っかかって動かなくなる現象を表現する
- スタックが起きるまでの時間の期待値、抜け出すまでの時間の期待値
- 雑音と同様に指数分布からドローして決定する
Juliaサンプルコード
# 既に作成したロボットクラスのコードに必要な処理を追加 # 追記した部分だけ記載 using Plots, Random, Distributions # 各種分布や乱数を生成するためのパッケージを追加 pyplot() mutable struct DifferentialWheeledRobot # メンバ変数を追加 stuck_exporn # スタックするまでの時間の期待値を決める指数分布オブジェクト escape_exporn # スタックから抜け出すまでの時間の期待値を決める指数分布オブジェクト time_until_stuck # スタックするまでの時間の期待値 time_until_escape # スタックから抜け出すまでの時間の期待値 is_stuck # スタックしたか # 初期化関数 # 追加引数 exp_stuck_time スタックするまでの時間の期待値 # 追加引数 exp_escape_time スタックから抜け出すまでの時間の期待値 function DifferentialWheeledRobot(exp_stuck_time::Float64=1e100, # 追加 exp_escape_time::Float64=1e-100) # 追加 # スタックまでの時間の決定 self.stuck_exporn = Exponential(exp_stuck_time) # 指数分布オブジェクトの生成 self.time_until_stuck = rand(self.stuck_exporn) # 分布に従った乱数 # 抜け出すまでの時間の決定 self.escape_exporn = Exponential(exp_escape_time) # 指数分布オブジェクトの生成 self.time_until_escape = rand(self.escape_exporn) # 分布に従った乱数 end end # スタックと脱出を切り替える関数 # スタック状態になったら制御指令を0にして動かさない # 引数 self::DifferenctialWheeledRobot ロボットクラスのオブジェクト # 引数 speed 速度指令 # 引数 yaw_rate 角速度指令 # 引数 delta_time サンプリングタイム[s] function stuck(self::DifferentialWheeledRobot, speed::Float64, yaw_rate::Float64, delta_time::Float64) if self.is_stuck # スタックしている時 self.time_until_escape -= delta_time # 脱出するまでの時間を減らしていく if self.time_until_escape <= 0.0 # 脱出できるようになった self.time_until_escape += rand(self.escape_exporn) # 次回脱出するまでの時間を設定 self.is_stuck = false end else # スタックしていない時 self.time_until_stuck -= delta_time # スタックするまでの時間を減らしていく if self.time_until_stuck <= 0.0 # スタックするようになった self.time_until_stuck += rand(self.stuck_exporn) # 次回スタックするまでの時間を設定 self.is_stuck = true end end return speed * (!self.is_stuck), yaw_rate * (!self.is_stuck) # スタック状態の時はそれぞれが0になる end # スタックしているか否かで制御指令を切り替える処理を追加 function draw!(self::DifferentialWheeledRobot) # 現在の姿勢 x, y, theta = self.pose[1], self.pose[2], self.pose[3] xn = x + self.radius * cos(theta) yn = y + self.radius * sin(theta) # ロボットを描画 plot!([x, xn], [y, yn], color=self.color, legend=false, aspect_ratio=true) plot!(circle(x, y, self.radius), linecolor=self.color, aspect_ratio=true) # 移動軌跡を描画 plot!(self.traj_x, self.traj_y, color="black", legend=false, aspect_ratio=true) # センサによる観測を描画 observation = [] if self.sensor !== nothing data(self.sensor, self.pose) draw!(self.sensor, self.pose) observation = self.sensor.last_data end # エージェントからの制御指令を計算 spd, yr = draw_decision!(self.agent, observation) # 次の時間における姿勢を計算 spd, yr = bias(self, spd, yr) # バイアスを付加 spd, yr = stuck(self, spd, yr, self.delta_time) # スタック時は速度と角速度を0にする self.pose = state_transition(spd, yr, self.delta_time, self.pose) self.pose = noise(self, self.pose, spd, yr, self.delta_time) # 雑音を付加 push!(self.traj_x, self.pose[1]), push!(self.traj_y, self.pose[2]) end
- サンプルデモ(灰色:スタックあり、赤色:スタックなし)
github.com
誘拐
- 位置計測不良でロボットの位置が突然ワープする現象を表現する
- 誘拐が起こるタイミングを指数分布を使ってドローする
- 誘拐後の姿勢は一様分布で選ぶ
一様分布の確率密度関数
Juliaサンプルコード
# 既に作成したロボットクラスのコードに必要な処理を追加 # 追記した部分だけ記載 using Plots, Random, Distributions # 各種分布や乱数を生成するためのパッケージを追加 pyplot() mutable struct DifferentialWheeledRobot # メンバ変数を追加 kidnap_exporn # 誘拐までの時間の期待値を決める指数分布オブジェクト kidnap_x_uniform # 誘拐後の姿勢のX座標を決める一様分布オブジェクト kidnap_y_uniform # 誘拐後の姿勢のY座標を決める一様分布オブジェクト kidnap_theta_uniform # 誘拐後の姿勢の向きθを決める一様分布オブジェクト time_until_kidnap # 誘拐までの時間の期待値 # 初期化関数 # 追加引数 exp_kidnap_time 誘拐までの時間の期待値 # 追加引数 kidnap_rx 誘拐後のX座標の範囲 # 追加引数 kidnap_ry 誘拐後のY座標の範囲 function DifferentialWheeledRobot(exp_kidnap_time::Float64=1e100, # 追加 kidnap_rx::Array=[-5.0, 5.0], # 追加 kidnap_ry::Array=[-5.0, 5.0]) # 追加 self.kidnap_exporn = Exponential(exp_kidnap_time) # 指数分布オブジェクト self.kidnap_x_uniform = Uniform(kidnap_rx[1], kidnap_rx[2]) # 一様分布オブジェクト self.kidnap_y_uniform = Uniform(kidnap_ry[1], kidnap_ry[2]) # 一様分布オブジェクト self.kidnap_theta_uniform = Uniform(0.0, 2 * pi) # 一様分布オブジェクト self.time_until_kidnap = rand(self.kidnap_exporn) # 指数分布に従った乱数 end end # 誘拐発生後の姿勢を計算する関数 # 誘拐が起きるまでは現在の姿勢を維持する # 引数 self::DifferentialWheeledRobot ロボットクラスのオブジェクト # 引数 pose 誘拐前のロボットの姿勢 # 引数 delta_time サンプリングタイム[s] function kidnap(self::DifferentialWheeledRobot, pose::Array, delta_time::Float64) self.time_until_kidnap -= delta_time # 誘拐発生までの時間をサンプリングタイムずつ減らしていく if self.time_until_kidnap <= 0.0 # 誘拐発生 self.time_until_kidnap += rand(self.kidnap_exporn) # 乱数で次回の誘拐までの時間を設定 return [rand(self.kidnap_x_uniform), rand(self.kidnap_y_uniform), rand(self.kidnap_theta_uniform)] # 一様分布から誘拐後の姿勢を決める else return pose # 姿勢はそのまま end end # 誘拐を発生させる処理を追加 function draw!(self::DifferentialWheeledRobot) # 現在の姿勢 x, y, theta = self.pose[1], self.pose[2], self.pose[3] xn = x + self.radius * cos(theta) yn = y + self.radius * sin(theta) # ロボットを描画 plot!([x, xn], [y, yn], color=self.color, legend=false, aspect_ratio=true) plot!(circle(x, y, self.radius), linecolor=self.color, aspect_ratio=true) # 移動軌跡を描画 plot!(self.traj_x, self.traj_y, color="black", legend=false, aspect_ratio=true) # センサによる観測を描画 observation = [] if self.sensor !== nothing data(self.sensor, self.pose) draw!(self.sensor, self.pose) observation = self.sensor.last_data end # エージェントからの制御指令を計算 spd, yr = draw_decision!(self.agent, observation) # 次の時間における姿勢を計算 spd, yr = bias(self, spd, yr) # バイアスを付加 spd, yr = stuck(self, spd, yr, self.delta_time) # スタック時は速度と角速度を0にする self.pose = state_transition(spd, yr, self.delta_time, self.pose) self.pose = noise(self, self.pose, spd, yr, self.delta_time) # 雑音を付加 self.pose = kidnap(self, self.pose, self.delta_time) # 誘拐が起きるタイミングでロボットをワープさせる push!(self.traj_x, self.pose[1]), push!(self.traj_y, self.pose[2]) end
- サンプルデモ(灰色:誘拐あり、赤色:誘拐なし)
github.com
GitHub
上記全ての不確かさを実装したロボットクラスのコードはこちら
github.com
