はじめに #
Julia製の経路最適化ソルバALTROを試してみるため、サンプルスクリプトを読み解いたメモを記事として残します。読み解いたスクリプトは公式マニュアルのGetting Startedにあるものです。 Getting Started - Altro
パッケージの読み込み #
using Altro
using TrajectoryOptimization
using RobotDynamics
using StaticArrays, LinearAlgebra
const TO = TrajectoryOptimization
const RD = RobotDynamics
usingで使用するパッケージを読み込みます。TrajectoryOptimizationとRobotDynamicsは何度も使用するので、constを使って定数として短縮します。
モデルの定義 #
動的モデルを定義します。
using ForwardDiff # needed for @autodiff
using FiniteDiff # needed for @autodiff
RD.@autodiff struct DubinsCar <: RD.ContinuousDynamics end
RD.state_dim(::DubinsCar) = 3
RD.control_dim(::DubinsCar) = 2
function RD.dynamics(::DubinsCar,x,u)
ẋ = @SVector [u[1]*cos(x[3]),
u[1]*sin(x[3]),
u[2]]
end
@autodiffでは、autodiffという名前のマクロを呼び出しています。その後ろで、RD.ContinuousDynamicsを継承したDubinsCarという型 (type) を作成します。さらに、状態と入力の次元を定義します。
最後に動的モデルを関数で定義します。SVectorはStaticArraysパッケージから提供されているベクトル型であり、Julia標準のBase.Array型よりも高速な計算が可能です。
状態変数ベクトル\(x=[x, y, \theta]\), 入力ベクトル\(u=[v, \omega]\)です。 car.jl
上記のモデルだけでも経路最適化可能ですが、以下の通りヤコビアンなどを定義できます。
function RD.dynamics!(::DubinsCar, xdot, x, u)
xdot[1] = u[1] * cos(x[3])
xdot[2] = u[1] * sin(x[3])
xdot[3] = u[2]
return nothing
end
function RD.jacobian!(::DubinsCar, J, xdot, x, u)
J .= 0
J[1,3] = -u[1] * sin(x[3])
J[1,4] = cos(x[3])
J[2,3] = u[1] * cos(x[3])
J[2,4] = sin(x[3])
J[3,5] = 1.0
return nothing
end
# Specify the default method to be used when calling the dynamics
# options are `RD.StaticReturn()` or `RD.InPlace()`
RD.default_signature(::DubinsCar) = RD.StaticReturn()
# Specify the default method for evaluating the dynamics Jacobian
# options are `RD.ForwardAD()`, `RD.FiniteDifference()`, or `RD.UserDefined()`
RD.default_diffmethod(::DubinsCar) = RD.UserDefined()
なお、関数の末尾に!を付けるのは、関数内部で引数を変更することを慣習的に示しています。
評価関数の定義 #
離散化されたモデルを定義します。ルンゲクッタ法 (RK4) で離散化します。
model = DubinsCar()
dmodel = RD.DiscretizedDynamics{RD.RK4}(model)
n,m = size(model) # get state and control dimension
N = 101 # number of time steps (knot points). Should be odd.
tf = 3.0 # total time (sec)
dt = tf / (N-1) # time step (sec)
またタイムステップを101, シミュレーション時間を3.0秒としています。
次に、初期状態と終端状態を定義します。
x0 = SA_F64[0,0,0] # start at the origin
xf = SA_F64[1,2,pi] # goal state
さらに評価関数を定義します。
Q = Diagonal(SA[0.1,0.1,0.01])
R = Diagonal(SA[0.01, 0.1])
Qf = Diagonal(SA[1e2,1e2,1e3])
obj = LQRObjective(Q,R,Qf,xf,N)
ここで、Q, R, Qfは重み行列（の対角要素）です。実際の評価関数は次式となります。
制約の付加 #
add_constraint関数を使って、制約条件を状態変数に対して付加します。終端状態xf, 状態変数の上下限、障害物の制約の順に付加しています。
cons = ConstraintList(n,m,N)
# Goal constraint
goal = GoalConstraint(xf)
add_constraint!(cons, goal, N)
# Workspace constraint
bnd = BoundConstraint(n,m, x_min=[-0.1,-0.1,-Inf], x_max=[5,5,Inf])
add_constraint!(cons, bnd, 1:N-1)
# Obstacle Constraint
# Defines two circular obstacles:
# - Position (1,1) with radius 0.2
# - Position (2,1) with radius 0.3
obs = CircleConstraint(n, SA_F64[1,2], SA_F64[1,1], SA[0.2, 0.3])
add_constraint!(cons, bnd, 1:N-1)
利用できる制約の種類は以下の通りです。
- GoalConstraint
- BoundConstraint
- LinearConstraint
- CircleConstraint
- SphereConstraint
- CollisionConstraint
- NormConstraint
- IndexedConstraint
最適化問題の定義 #
動的モデル、評価関数、初期状態、終端時刻、終端状態、および制約条件を経路最適化問題として定義します。
prob = Problem(model, obj, x0, tf, xf=xf, constraints=cons)
次に初期値をinitial_controls関数で定義します。ここでは簡単な問題なので乱数としています。
initial_controls!(prob, [@SVector rand(m) for k = 1:N-1])
rollout!(prob) # simulate the system forward in time with the new controls
ソルバのオプション #
ソルバのオプションを設定可能です。
# Set up solver options
opts = SolverOptions()
opts.cost_tolerance = 1e-5
# Create a solver, adding in additional options
solver = ALTROSolver(prob, opts, show_summary=false)
利用可能なオプションは以下を参照ください。
最適化問題を解く #
solve関数で最適化問題を解きます。最適化の結果はsolverオブジェクトに格納されます。
solve!(solver)
最適化結果の確認 #
status(solver)で最適化に成功したか確認できます。
status(solver)
実行結果
SOLVE_SUCCEEDED::TerminationStatus = 2
その他にもイタレーション回数、評価値、制約違反量を確認できます。
println("Number of iterations: ", iterations(solver))
println("Final cost: ", cost(solver))
println("Final constraint satisfaction: ", max_violation(solver))
実行結果
Number of iterations: 17
Final cost: 12.480778190315869
Final constraint satisfaction: 9.890399610412715e-10
