f:id:sy4310:20210918104906g:plain

参考書籍

詳解　確率ロボティクス　Ｐｙｔｈｏｎによる基礎アルゴリズムの実装 (ＫＳ理工学専門書)

作者:上田隆一
講談社

Amazon

想定するロボット

対向2輪型ロボット
f:id:sy4310:20210901214605p:plain

簡易化するための仮定

トルクや加速度は考慮しない
速度と角速度を直接指令として与えられるものとする

世界座標系

Juliaサンプルコード

using Plots # 描画パッケージであるPlotsを取り込む
pyplot() # バックエンドにmatplotlibのPyPlotを指定

# 世界座標系クラスの定義
mutable struct World
  # メンバ変数の宣言
  x_min # X方向描画範囲の最小値
  x_max # X方向描画範囲の最大値
  y_min # Y方向描画範囲の最小値
  y_max # Y方向描画範囲の最大値
  objects # オブジェクトを格納するリスト
  delta_time # サンプリングタイム
  end_time # アニメーション終了時間
  is_test # テストフラグ
  save_path # gifファイルの保存先

  # 初期化する関数
  # 引数 x_min, x_max X方向描画範囲の最小/最大値
  # 引数 y_min, y_max Y方向描画範囲の最小/最大値
  # 引数 delta_time サンプリングタイム(デフォルトは0.1[s])
  # 引数 end_time 実行時間(デフォルトは30[s])
  # 引数 is_test ユニットテストフラグ(テスト時はtrueにしてアニメーションを作らせない)
  # 引数 save_path デモのアニメーションを記録したgifファイルの保存先
  function World(x_min::Float64, x_max::Float64,
                 y_min::Float64, y_max::Float64;
                 delta_time=0.1, end_time=30,
                 is_test=false, save_path=nothing)
    # 引数を各メンバ変数にセット
    self = new()
    self.x_min = x_min
    self.x_max = x_max
    self.y_min = y_min
    self.y_max = y_max
    self.objects = []
    self.delta_time = delta_time
    self.end_time = end_time
    self.is_test = is_test
    self.save_path = save_path
    return self # 初期化された世界座標系クラスのオブジェクトを返す
  end
end

# 描画させるオブジェクトをセットする関数
# 引数 self::World 世界座標系オブジェクト
# 引数 obj アニメーションを描画させるオブジェクト
function append(self::World, obj)
  push!(self.objects, obj) # 配列に追加
end

# サンプリングタイム毎に描画する関数
# 引数 self::World 世界座標系オブジェクト
# 引数 elap_time 経過時間
function one_step(self::World, elap_time)
  # 描画範囲を設定
  plot([], [], aspect_ratio=true, xlabel="X", ylabel="Y",
       xlims=(self.x_min, self.x_max), ylims=(self.y_min, self.y_max),
       legend=false)
  
  # セットされたオブジェクトの描画関数を順次呼び出す
  for obj in self.objects
    draw!(obj)
  end

  # 経過時間を文字列で表示
  annotate!(-3.5, 4.5, "t = $(elap_time)", "black")
end

# アニメーションを作成する関数
# 引数 self::World 世界座標系オブジェクト
function draw(self::World) 
  if self.is_test # ユニットテスト時
    for t in 0:self.delta_time:10 # 30秒だと長いので10秒に設定
      one_step(self, t) # 各時間でのアニメーションを描画
    end
  else # 通常実行時
    anime = @animate for t in 0:self.delta_time:self.end_time # アニメーションを描画
      one_step(self, t)
    end
    # 描画終了後にgifで保存
    gif(anime, fps=15, joinpath(split(@__FILE__, "src")[1], self.save_path))
  end
end

ロボット

位置をx, y, 向きをθで表し、次のようなベクトルとして姿勢を定義する
Juliaサンプルコード

using Plots
pyplot()

# 状態遷移を計算する共通関数をインクルード(別途作成)
include(joinpath(split(@__FILE__, "src")[1], "src/common/state_transition/state_transition.jl"))

# 対向2輪型ロボットクラスの定義
mutable struct DifferentialWheeledRobot
  # メンバ変数の宣言
  pose # 姿勢: [x, y, θ]
  radius # ロボットの体の半径(円型のロボットとする)
  color # 描画時の色
  delta_time # サンプリングタイム
  traj_x # 移動軌跡のX座標
  traj_y # 移動軌跡のY座標

  # 初期化する関数
  # 引数 pose: 姿勢
  # 引数 radius: ロボットの体の半径
  # 引数 color: 描画時の色("red", "blue"などで指定)
  # 引数 delta_time: サンプリングタイム[秒]
  function DifferentialWheeledRobot(pose::Array, radius::Float64, 
                                    color::String,
                                    delta_time::Float64)
      # 各引数をメンバ変数にセット
      self = new()
      self.pose = pose # [x, y, θ]
      self.radius = radius
      self.color = color
      self.delta_time = delta_time
      
      # Juliaでは配列のインデックスは1始まり
      self.traj_x = [pose[1]] # 各時刻のX座標を配列に格納していく
      self.traj_y = [pose[2]] # 各時刻のY座標を配列に格納していく
      return self
  end
end

# 円を描画する関数
# 引数 x: 中心のX座標
# 引数 y: 中心のY座標
# 引数 r: 円の半径
function circle(x, y, r)
  theta = LinRange(0, 2*pi, 500)
  x .+ r * sin.(theta), y .+ r * cos.(theta)
end

# ロボットを描画する関数
# DifferentialWheeledRobotオブジェクトを引数にする
# 先にWorldクラスのdraw()を呼び出してから
function draw!(self::DifferentialWheeledRobot)
  x, y, theta = self.pose[1], self.pose[2], self.pose[3] # 現在位置と向き
  xn = x + self.radius * cos(theta) # ロボットの鼻先のX座標
  yn = y + self.radius * sin(theta) # ロボットの鼻先のY座標
  
  # ロボットの向きを示す線分を描画
  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)
  
  # ロボットの位置座標を記録
  push!(self.traj_x, self.pose[1]), push!(self.traj_y, self.pose[2])
end

GitHub github.com

状態遷移関数の定義

前方方向の速度v[m/s]、ロボット中心の角速度ω[rad/s]を制御指令とする
$\omega_t=0$ の場合
$\omega_t \neq 0$ の場合
Juliaサンプルコード

# 状態遷移を計算する関数
# 引数 speed: 速度指令[m/s]
# 引数 yaw_rate: 角速度指令[rad/s]
# 引数 time: サンプリングタイム[s]
# 引数 pose: ロボットの姿勢[x, y, θ]
function state_transition(speed, yaw_rate, time, pose)
  theta = pose[3] # ロボットの向き

  # 角速度に応じて計算式を切り替える
  if abs(yaw_rate) < 1e-10 # 角速度がほぼ0の場合
      return pose + [speed*cos(theta)*time,
                     speed*sin(theta)*time,
                     yaw_rate*time]
  else # そうでない場合
      return pose + [speed/yaw_rate*(sin(theta+yaw_rate*time)-sin(theta)),
                     speed/yaw_rate*(-cos(theta+yaw_rate*time)+cos(theta)),
                     yaw_rate*time]
  end
end

GitHub github.com

エージェント

考える主体を、ロボティクスや人工知能の研究分野ではエージェントと呼ぶ
指令をエージェントが決めて、ロボットに乗って操縦するという体裁をとる
Juliaサンプルコード

# エージェントクラス
# 使う際はロボットクラスからインクルードさせる
# 制御指令として速度と角速度を与える
mutable struct Agent
    # メンバ変数の定義
    speed # 速度指令[m/s]
    yaw_rate # 角速度指令[rad/s]
    time_interval # サンプリングタイム[s]
    estimator # 状態推定クラスのオブジェクト(今はまだ要らない)
    prev_spd # 1ステップ前の速度指令
    prev_yr # 1ステップ前の角速度指令

    # 初期化する関数
    # 引数 speed: 速度指令
    # 引数 yaw_rate: 角速度指令
    # 引数 time_interval: サンプリングタイム(0.1秒をデフォルト値とする)
    # 引数 estimator: 状態推定クラスのオブジェクト(ここではまだ入れないのでnothingとする)
    function Agent(speed::Float64, yaw_rate::Float64;
                   time_interval::Float64=0.1,
                   estimator=nothing)
        # 各引数をメンバ変数にセット
        self = new()
        self.speed = speed
        self.yaw_rate = yaw_rate
        self.time_interval = time_interval
        self.estimator = estimator
        self.prev_spd = 0.0
        self.prev_yr = 0.0
        return self
    end
end

# 速度指令と角速度指令を与える関数
# 引数 Agent: エージェントクラスのオブジェクト
# 引数 observation: センサからの観測データ(今はまだ使わない)
function draw_decision!(self::Agent, observation)
    # 状態推定用の処理
    # 今はまだ状態推定オブジェクトがないので実行されない
    if self.estimator !== nothing
        motion_update(self.estimator, self.prev_spd, self.prev_yr, self.time_interval)
        self.prev_spd, self.prev_yr = self.speed, self.yaw_rate
        observation_update(self.estimator, observation)
        draw!(self.estimator)
    end

    # 今の時点ではただ速度を角速度を返すだけ
    return self.speed, self.yaw_rate
end

ロボットクラスへの組み込み
先述したコードに必要な処理を追加する

using Plots
pyplot()

include(joinpath(split(@__FILE__, "src")[1], "src/model/agent/agent.jl")) # エージェントクラスをインクルード
include(joinpath(split(@__FILE__, "src")[1], "src/common/state_transition/state_transition.jl"))

mutable struct DifferentialWheeledRobot
  pose
  radius
  color
  agent # メンバー変数にエージェントオブジェクトを追加
  delta_time
  traj_x
  traj_y

  function DifferentialWheeledRobot(pose::Array, radius::Float64, 
                                    color::String, agent, # 初期化時の引数にエージェントを追加 
                                    delta_time::Float64)
      self = new()
      self.pose = pose
      self.radius = radius
      self.color = color
      self.agent = agent # メンバー変数にセット
      self.delta_time = delta_time
      self.traj_x = [pose[1]]
      self.traj_y = [pose[2]]
      return self
  end
end

function circle(x, y, r)
  theta = LinRange(0, 2*pi, 500)
  x .+ r * sin.(theta), y .+ r * cos.(theta)
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)
  
  # エージェントクラスの持つメソッドを呼び出す
  # 速度と角速度の指令を受け取る
  # 第2引数にはobservationが入るが、ここでは空配列としておく
  spd, yr = draw_decision!(self.agent, [])
  
  # 受け取った制御指令から次のステップでの姿勢を計算
  self.pose = state_transition(spd, yr, self.delta_time, self.pose)
  push!(self.traj_x, self.pose[1]), push!(self.traj_y, self.pose[2])
end

ロボットの観測

ロボットにセンサを搭載し、センサで観測する対象を環境に置く
センサで物体を見て、距離と方位を測定する

環境中に置く物体

環境中に物体を $N_m$ 個置けるとする
それぞれが0から $N_m-1$ までのIDを持つ
IDとして番号 $j$ を持つ物体を $m_j$ とする
$m_j$ の世界座標系での座標
Juliaサンプルコード

using Plots # 描画用パッケージ
pyplot() # PyPlotをバックエンドに設定

# 環境中に置く物体クラス
mutable struct Object
    # メンバ変数の宣言
    pose # 世界座標系での座標[x, y]
    id # ID
    shape # 描画時のマーカの形
    color # 描画時のマーカの色
    size # 描画時のマーカの大きさ

    # 初期化する関数
    # 引数 x: 世界座標系のX座標
    # 引数 y: 世界座標系のY座標
    # 引数 shape: マーカの形(デフォルトはスター)
    # 引数 color: マーカの色(デフォルトはオレンジ)
    # 引数 size: マーカの大きさ(デフォルトは15)
    # 引数 id: ID(デフォルトは-1)
    function Object(x::Float64, y::Float64; shape=:star, 
                    color=:orange, size=15, id=-1)
        # メンバ変数にセット
        self = new()
        self.pose = [x, y]
        self.id = id
        self.shape = shape
        self.color = color
        self.size = size
        return self # 初期化されたクラスオブジェクトを返す
    end
end

# 物体を環境中に描画する関数
# 引数 Object 物体クラスのオブジェクト
function draw!(self::Object)
    # 初期化時に設定された位置や形状、色の散布図マーカを描画する
    scatter!([self.pose[1]], [self.pose[2]], markershape=self.shape, 
             markercolor=self.color, markersize=self.size)
    
    # IDが有効なら横に表示する
    if self.id !== -1
        annotate!(self.pose[1], self.pose[2], 
                  text("id:$(self.id)", :black, :left, 10))
    end
end

GitHub github.com

オブジェクトを登録する地図

全オブジェクトの位置を記録した地図の表記
Juliaサンプルコード

using Plots # 描画用パッケージ
pyplot() # バックエンドにPyPlotを設定

# オブジェクトクラスをインクルード
include(joinpath(split(@__FILE__, "src")[1], "src/model/object/object.jl"))

# 物体設置用地図クラス
mutable struct Map
    # メンバ変数を宣言
    objects # オブジェクト配列

    # 初期化する関数
    function Map()
        # メンバ変数を定義
        self = new()
        self.objects = [] # 空の配列として初期化
        return self # 初期化された地図オブジェクトを返す
    end
end

# 地図中にオブジェクトを追加する関数
# 引数 self::Map 地図クラスのオブジェクト
# 引数 object 設置する物体クラスのオブジェクト
function add_object(self::Map, object)
    push!(self.objects, object) # 配列に追加
end

# 地図を描画する関数
# 引数 self::Map 地図クラスのオブジェクト
function draw!(self::Map)
    for obj in self.objects
        draw!(obj) # 配列中の物体を順次描画
    end
end

GitHub github.com

センサによる観測

観測方程式
ロボットは外界センサを持ち、視界に入った物体を観測できる
環境中の物体はランドマーク $m_j$ とする
センサの姿勢はロボットの姿勢と一致していると仮定する
観測できるのはセンサのからの距離 $l_j$ と向き $\phi_j$
Juliaサンプルコード: 観測方程式

# センサ位置と観測対象の位置関係から距離と向きを計算する関数
# 引数 sns_pose::Array センサの姿勢 [x, y, theta]
# 引数 obj_pose::Array 観測対象の位置 [x, y]
function observation_function(sns_pose::Array, obj_pose::Array)
  diff_x = obj_pose[1] - sns_pose[1] # x座標の差分
  diff_y = obj_pose[2] - sns_pose[2] # y座標の差分
  phi = atan(diff_y, diff_x) - sns_pose[3] # 対象のセンサからの向きを計算

  # 向きを-π~πの範囲に入るように正規化
  while phi >= pi
    phi -= 2 * pi
  end
  while phi < -pi
    phi += 2 * pi
  end

  return [hypot(diff_x, diff_y), phi] # [距離, 向き]を返す
end

Juliaサンプルコード: センサモデル

using Plots # 描画用パッケージ
pyplot() # バックエンドをPyPlotに

include(joinpath(split(@__FILE__, "src")[1], "src/model/map/map.jl")) # 地図クラスをインクルード
include(joinpath(split(@__FILE__, "src")[1], "src/common/observation_function/observation_function.jl")) # 観測方程式関数をインクルード

# センサモデルを定義するクラス
mutable struct Sensor
  # メンバ変数を宣言
  map # 地図オブジェクト
  last_data # 最終的な計測結果
  dist_rng # 観測可能な距離の範囲
  dir_rng # 観測可能な向きの範囲

  # センサオブジェクトを初期化する関数
  # 引数 map::Map 地図オブジェクト
  # 引数 dist_rng::Tuple 観測できる最短距離と最長距離のタプル
  # 引数 dir_rng::Tuple 観測できる角度範囲の左境界値と右境界値のタプル
  function Sensor(map::Map;
                  dist_rng::Tuple=(0.5, 6.0),  # デフォルトでは0.5 ~ 6.0[m]
                  dir_rng::Tuple=(-pi/3, pi/3)) # デフォルトでは-pi/3 ~ pi/3[rad]
    # 入力引数をメンバ変数にセット
    self = new()
    self.map = map
    self.last_data = []
    self.dist_rng = dist_rng
    self.dir_rng = dir_rng
    return self # 初期化したオブジェクトを返す
  end
end

# 対象が観測可能範囲内にあるか判定する関数
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 obsrv 観測対象オブジェクトの距離と向き[l, phi]
# 返り値 true or false 
function visible(self::Sensor, obsrv=nothing)
  # 観測対象が無ければ終了
  if obsrv === nothing
    return false
  end

  # 観測可能距離と向きの範囲内にあるかチェック
  return ((self.dist_rng[1] <= obsrv[1] <= self.dist_rng[2])
    && (self.dir_rng[1] <= obsrv[2] <= self.dir_rng[2]))
end

# 地図上の全観測対象に対してセンサの観測可能範囲内か判定する関数
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 sns_pose::Array センサの姿勢[x, y, theta]
# センサとロボットの姿勢は同一とする
function data(self::Sensor, sns_pose::Array)
  observed = [] # 観測範囲内のオブジェクトを入れる配列
  
  # 地図上の全オブジェクトに対して処理
  for obj in self.map.objects
    obsrv = observation_function(sns_pose, obj.pose) # 観測方程式から距離と向きを計算
    
  # 観測可能範囲内かチェック
  if visible(self, obsrv)
      push!(observed, (obsrv, obj.id)) # 観測範囲内のオブジェクトを追加
    end
  end
  
  self.last_data = observed # 最終的に観測されたオブジェクト群としてセット
end

# センサと観測されたオブジェクト間を結んだ直線を描画する関数
# 引数 self::Sensor センサクラスのオブジェクト
# 引数 sns_pose::Array センサの姿勢[x, y, theta]
function draw!(self::Sensor, sns_pose::Array)
  # 最終的に観測されたオブジェクトの分だけ処理
  for obj in self.last_data
    x, y, theta = sns_pose[1], sns_pose[2], sns_pose[3] # センサの姿勢
    distance, direction = obj[1][1], obj[1][2] # 観測されたオブジェクトの距離と向き
    # 観測距離と向きから位置座標を計算
    ox = x + distance * cos(direction + theta) 
    oy = y + distance * sin(direction + theta)
    # センサと観測オブジェクトをピンクの直線で結ぶ
    plot!([x, ox], [y, oy], color="pink",
          legend=false, aspect_ratio=true)
  end
end

GitHub github.com
github.com

サンプルデモの実行

Juliaサンプルコード

# ロボットが移動しながらセンサで周囲のランドマークを観測するデモを実行するモジュール
# このモジュールを読み込んで、main関数を呼び出す
module RobotSensingSample
  # デモの実行に必要なモジュールをインクルード
  include(joinpath(split(@__FILE__, "src")[1], "src/model/world/puddle_world.jl")) # 世界座標系
  include(joinpath(split(@__FILE__, "src")[1], "src/model/agent/agent.jl")) # エージェント
  include(joinpath(split(@__FILE__, "src")[1], "src/model/robot/differential_wheeled_robot/differential_wheeled_robot.jl")) # ロボット
  include(joinpath(split(@__FILE__, "src")[1], "src/model/map/map.jl")) # 地図
  include(joinpath(split(@__FILE__, "src")[1], "src/model/object/object.jl")) # ランドマーク
  include(joinpath(split(@__FILE__, "src")[1], "src/model/sensor/sensor.jl")) # センサ

  # デモを実行するメイン関数
  # 引数 delta_time サンプリングタイム(デフォルトでは0.1[s])
  # 引数 end_time デモを何秒間実行するか(デフォルトでは30[s])
  # 引数 is_test ユニットテスト用フラグ(テスト時はtrueにして、アニメーションを作らせない)
  function main(delta_time=0.1, end_time=30; is_test=false)
    # デモのアニメーションを記録するgifファイルの保存先を定義
    path = "src/model/sensor/robot_sensing_sample.gif"

    # 世界座標系オブジェクト
    world = PuddleWorld(-5.0, 5.0, -5.0, 5.0, # 描画範囲
                        delta_time=delta_time, # サンプリングタイム
                        end_time=end_time, # 実行時間
                        is_test=is_test, # テストフラグ
                        save_path=path) # gif保存先
    
    # ランドマークを設置した地図を作成
    map = Map() # 地図オブジェクト
    add_object(map, Object(2.0, -2.0, id=1)) # 3個のランドマークオブジェクトを設置
    add_object(map, Object(-1.0, -3.0, id=2)) # [X座標、Y座標、ID]
    add_object(map, Object(3.0, 3.0, id=3))
    append(world, map) # 世界座標系にセット
    
    # ロボットオブジェクト
    robot = DifferentialWheeledRobot([-2.0, -1.0, pi/5*6], 0.2, "black", # 初期姿勢、ボディ半径、描画時の色
                                     Agent(0.2, 10.0/180*pi), delta_time, # エージェント、サンプリングタイム
                                     sensor=Sensor(map)) # センサ
    append(world, robot) # 世界座標系にセット
    
    # デモアニメの実行
    # 世界座標系にセットした各オブジェクトを描画する
    draw(world)
  end
end

デモの実行例
GitHub
github.com

EurekaMoments

ロボットや自動車の自律移動に関する知識や技術、プログラミング、ソフトウェア開発について勉強したことをメモするブログ

自律移動ロボットのモデル化とJuliaサンプルコード

参考書籍

想定するロボット

簡易化するための仮定

世界座標系

ロボット

状態遷移関数の定義

エージェント

ロボットの観測

環境中に置く物体

オブジェクトを登録する地図

センサによる観測

サンプルデモの実行