Julia製の経路最適化ソルバALTROのサンプルスクリプト読解

はじめに

Julia製の経路最適化ソルバALTROを試してみるため、サンプルスクリプトを読み解いたメモを記事として残します。読み解いたスクリプトは公式マニュアルのGetting Startedにあるものです。
Getting Started - Altro

パッケージの読み込み

1
2
3
4
5
6


using Altro
using TrajectoryOptimization
using RobotDynamics
using StaticArrays, LinearAlgebra
const TO = TrajectoryOptimization
const RD = RobotDynamics

usingで使用するパッケージを読み込みます。TrajectoryOptimizationとRobotDynamicsは何度も使用するので、constを使って定数として短縮します。

モデルの定義

動的モデルを定義します。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12


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)
    ẋ = @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

上記のモデルだけでも経路最適化可能ですが、以下の通りヤコビアンなどを定義できます。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24


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) で離散化します。

1
2
3
4
5
6


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秒としています。

次に、初期状態と終端状態を定義します。

1
2


x0 = SA_F64[0,0,0]   # start at the origin
xf = SA_F64[1,2,pi]  # goal state

さらに評価関数を定義します。

1
2
3
4


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は重み行列（の対角要素）です。実際の評価関数は次式となります。

$$\frac{1}{2} (x_N - x_f)^{\top} Q_f (x_N - x_f) + \frac{1}{2} \sum_{k=0}^{N-1} \left( (x_k - x_f)^{\top} Q (x_k - x_f) + u_k^{\top} R u_k \right)$$

制約の付加

add_constraint関数を使って、制約条件を状態変数に対して付加します。終端状態xf, 状態変数の上下限、障害物の制約の順に付加しています。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16


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

最適化問題の定義

動的モデル、評価関数、初期状態、終端時刻、終端状態、および制約条件を経路最適化問題として定義します。

1

prob = Problem(model, obj, x0, tf, xf=xf, constraints=cons)

次に初期値をinitial_controls関数で定義します。ここでは簡単な問題なので乱数としています。

1
2


initial_controls!(prob, [@SVector rand(m) for k = 1:N-1])
rollout!(prob)   # simulate the system forward in time with the new controls

ソルバのオプション

ソルバのオプションを設定可能です。

1
2
3
4
5
6


# 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)

利用可能なオプションは以下を参照ください。

Solver Options · Altro

最適化問題を解く

solve関数で最適化問題を解きます。最適化の結果はsolverオブジェクトに格納されます。

1

solve!(solver)

最適化結果の確認

status(solver)で最適化に成功したか確認できます。

1

status(solver)

実行結果

1

SOLVE_SUCCEEDED::TerminationStatus = 2

その他にもイタレーション回数、評価値、制約違反量を確認できます。

1
2
3


println("Number of iterations: ", iterations(solver))
println("Final cost: ", cost(solver))
println("Final constraint satisfaction: ", max_violation(solver))

実行結果

1
2
3


Number of iterations: 17
Final cost: 12.480778190315869
Final constraint satisfaction: 9.890399610412715e-10

参考

Punctuation - The Julia Language