MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

スタンフォード大学の学生が学ぶ制約付き非線形最小二乗法とその応用

目次

はじめに

スタンフォード大学には

機械学習を学ぶ上での第一歩として、

Introduction to Matrix Methods (EE103)という授業があります。

 

今回の記事では、

この授業の教科書である

Introduction to Applied Linear Algebraを

読んだ際の技術メモです。

 

この教科書は下記のリンクのページから

pdfをダウンロードすることができます。

 

本記事では、

上記の教科書の非線形最小二乗法の部分のみのメモです。

他の部分に関しては、下記の記事を参照下さい。

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

制約付き非線形最小二乗法

制約付き非線形最小二乗法は、

非線形関数f(x)とg(x)において、

下記のような最適化問題を解く方法を指します。

f:id:meison_amsl:20181102201312p:plain:w300

 

制約条件g(x) = 0 が線形の場合は、

下記の記事で述べたレーベンバーグ・マーカート法のStep2で

線形方程式を毎回解くことで、最適化問題を解くことができます。

myenigma.hatenablog.com

 

しかし、g(x)=0が非線形の場合、

最適化問題を解くのは、より一層難しくなります。

 

このような制約付き非線形最小二乗法を解く方法としては、

代表的なものとして、ペナルティ法と拡張ラグランジュ法があります。

 

制約付き非線形最小二乗法の解法1: ペナルティ法

ペナルティ法は、制約条件g(x) = 0を満たすように、

g(x)をコスト関数にいれた下記のようなコスト関数を最小化する方法です。

f:id:meison_amsl:20181104161938p:plain

 

上記のコスト関数を最小化すると、自ずとg(x)も0に近づき、

g(x)=0の制約条件を満たすようになることがわかります。

またペナルティ法では、g(x)のコスト関数の重みであるλを、

反復のたびに、大きくしていくことで収束性を高める方法が取られます。

 

制約付き非線形最小二乗法の解法2: 拡張ラグランジュ法

前述のペナルティ法は、制約付き非線形最小二乗法を解くことができますが、

実際にg(x) = 0とするためには、

μを非常に大きくする必要があり、

それをコンピュータで実現するのは難しく、

またμの増加の方法によっては、うまく収束しない場合があります。

 

このような問題を解決する方法が拡張ラグランジュ法です。

1960年台に開発されました。

 

拡張ラグランジュ法は、

前述のペナルティ法と、

以前紹介したラグランジュの未定乗数法を組み合わせたアルゴリズムです。

myenigma.hatenablog.com

ペナルティ法と比べると、あまりμを大きくしなくても、

g(x) = 0を満たすような形になり、

収束性が高いという特徴があります。

 

拡張ラグランジュ法では、

3つのステップを繰り返し計算することで、

制約付き非線形最小二乗法を解きます。

  • Step1:下記の非線形最小二乗法をレーベンバーグ・マーカートなどで解く

f:id:meison_amsl:20181104163929p:plain

ここで、μはペナルティ項の重みであり、zはラグランジュ定数です。

それぞれ、μ=1, z = 0で初期化しておきます。

  • Step2: zを更新する

f:id:meison_amsl:20181104164102p:plain

これにより、ラグランジュの未定乗数法の収束条件に近づきます。

  • Step3: μを更新する

f:id:meison_amsl:20181104164803p:plain

これによりペナルティ項が収束条件に近づきます。

 

制約付き非線形最小二乗法の応用例1: 自動車の非線形制御

この記事では、この教科書の429Pで説明されている

自動車の非線形制御の簡単なシミュレーションを、

Juliaと最適化モデリングツールであるJuMPを使って解いてみます。

myenigma.hatenablog.com

myenigma.hatenablog.com

 

詳細は教科書を見ていただきたいのですが、

基本的には、x, y, yawの二次元の車両の最終位置を指定すると、

シンプルな車両モデルを元に、その最終位置までの

速度とステアリング角度の予測値を制約付き非線形最小二乗法で計算してくれます。

 

下記がJuliaのコードです。

#
# Nonlinear vehicle control 
#
# author: Atsushi Sakai
#
# see: Boyd, Stephen, and Lieven Vandenberghe.
#      Introduction to Applied Linear Algebra: Vectors, Matrices, and Least Squares.
#      Cambridge University Press, 2018. P429
#

using JuMP
using Ipopt
using PyPlot

function solve_nonlinear_vehicle_control(xfinal, N, gamma, L, dt)

    solver=IpoptSolver(print_level=0)
    model = Model(solver=solver)

    nx = 3
    nu = 2

    @variable(model, x[k=1:nx,t=1:N])
    @variable(model, u[k=1:nu,t=1:N])

    # initial and terminal constraint 
    @constraint(model, x[:,1] .== [0.0; 0.0; 0.0])
    @constraint(model, x[:,N] .== xfinal)

    initx = zeros(nx, N)
    for k in 1:N-1
        # vehicle model
        @NLconstraint(model,  x[1,k+1] == x[1,k] + dt*u[1,k]*cos(x[3,k]))
        @NLconstraint(model,  x[2,k+1] == x[2,k] + dt*u[1,k]*sin(x[3,k]))
        @NLconstraint(model,  x[3,k+1] == x[3,k] + dt*u[1,k]/L*tan(u[2,k]))

        initx[:,k] = xfinal*k/N
    end
    initx[:,N] = xfinal
    setvalue(x,initx)# warm starting 

    # object function 
    obj = sum(sum(u[:, i].^2 for i=1:N))
    obj += gamma*sum(sum((u[:, i+1]-u[:, i]).^2 for i=1:N-1))

    @objective(model, Min, obj)

    status = solve(model)
    println(status)

    x_opt = getvalue(x)
    u_opt = getvalue(u)

    display(x_opt)
    # display(u_opt)

    return x_opt, u_opt
end


function main()
    println(PROGRAM_FILE," start!!")

    xfinal = [-1.0;1.0;deg2rad(90)]
    N = 50 # control steps
    gamma = 10.0 #weight for diff of input
    L = 0.1 # Wheel base
    dt = 0.1 # time tick

    x, u = solve_nonlinear_vehicle_control(xfinal, N, gamma, L, dt)

    plot(x[1,:], x[2,:], "-r")
    plot(xfinal[1], xfinal[2], "ob")
    axis("equal")
    show()

    println(PROGRAM_FILE," Done!!")
end

下記のGitHubリポジトリでも公開しています。

github.com

 

上記のコードを実行すると、

下記のように、指定したx,y,θへの非線形の軌跡を

制約付き最小二乗法によって計算してくれます。

f:id:meison_amsl:20181103213337p:plain

f:id:meison_amsl:20181103213348p:plain

 

制約付き非線形最小二乗法の応用例2: 後処理軌跡推定

続いて、ロボットにおける後処理軌跡推定を

制約付き最小二乗法で解いてみたいと思います。

後処理軌跡推定の詳細に関しては、下記を参照ください。

myenigma.hatenablog.com

前回は、C++の最適化ライブラリであるCeres Solverを使いましたが、

今回はJuliaのJuMPを使いたいと思います。

 

下記がJuliaのコードです。

#
# Trajectory Estimzation with nonlinear optimization
#
# Assume the robot can get dead-reckoning and GPS positioning data.
#
# author: Atsushi Sakai
#

using JuMP
using Ipopt
using PyPlot

function optimize_trajectory(h_x, h_z, h_u, dt, lamda)

    solver=IpoptSolver(print_level=0)
    model = Model(solver=solver)

    N = size(h_x)[2]
    nx = 3
    nu = 2

    @variable(model, x[k=1:nx,t=1:N])
    @variable(model, u[k=1:nu,t=1:N])

    # initial constraint 
    @constraint(model, x[:,1] .== [0.0; 0.0; 0.0])

    for k in 1:N-1
        # vehicle model
        @NLconstraint(model,  x[1,k+1] == x[1,k] + dt*u[1,k]*cos(x[3,k]))
        @NLconstraint(model,  x[2,k+1] == x[2,k] + dt*u[1,k]*sin(x[3,k]))
        @NLconstraint(model,  x[3,k+1] == x[3,k] + dt*u[2,k])
    end
    setvalue(x, h_x)# warm starting 
    setvalue(u, h_u)# warm starting 

    @NLobjective(model, Min, sum(
                   (x[1,k+1] - (x[1,k] + dt*h_u[1,k]*cos(x[3,k])))^2 
                 + (x[2,k+1] - (x[2,k] + dt*h_u[1,k]*sin(x[3,k])))^2 
                 + (x[3,k+1] - (x[3,k] + dt*h_u[2,k]))^2 
                   for k in 1:N-1)
                 +lamda*sum(
                    (x[1,k] - h_z[1,k])^2 
                 +  (x[2,k] - h_z[2,k])^2 
                 for k in 1:N-1 if h_z[1,k] != 0.0
                 ))
    

    status = solve(model)
    println(status)

    x_opt = getvalue(x)
    u_opt = getvalue(u)

    display(x_opt)

    return x_opt, u_opt
end

function predict_motion(x, u, dt)
    x[1]=x[1]+u[1]*dt*cos(x[3])
    x[2]=x[2]+u[1]*dt*sin(x[3])
    x[3]=x[3]+u[2]*dt
    return x
end

function add_input_noize(u,Q)
    un=u[:]
    un[1]=un[1]+randn()*Q[1]
    un[2]=un[2]+randn()*Q[2]
    return un
end


function gps_observation(xTrue, time, zdt, R)
    z=[0.0, 0.0]

    if abs(time%zdt) > 0.01
        return z
    end

    z[1]=xTrue[1]+randn()*R[1]
    z[2]=xTrue[2]+randn()*R[1]

    return z
end


function simulate(u, SIMTIME, DT, ZDT, Q, R)

    h_xTrue = zeros(3,0)
    h_x = zeros(3,0)
    h_z = zeros(2,0)
    h_u = zeros(2,0)

    xTrue = [0.0, 0.0, 0.0]
    x = [0.0, 0.0, 0.0]
    for time=0.0:DT:SIMTIME
        xTrue = predict_motion(xTrue, u, DT)
        ud = add_input_noize(u, Q)
        x = predict_motion(x, ud, DT)
        z = gps_observation(xTrue, time, ZDT, R)

        # save history
        h_xTrue = hcat(h_xTrue, xTrue)
        h_x = hcat(h_x, x)
        h_z = hcat(h_z, z)
        h_u = hcat(h_u, u)
    end

    return h_xTrue, h_x, h_z, h_u
end

function main()
    println(PROGRAM_FILE," start!!")

    SIMTIME = 50.0
    DT = 0.1 # time tick for control
    ZDT = 4.0 # time tick to get observation
    Q = [0.5,0.1] #input noise
    u = [1.0,0.1]# input v[m/s],omega[rad/s]
    R=[0.1] #observation noise

    h_xTrue, h_x, h_z, h_u = simulate(u, SIMTIME, DT, ZDT, Q, R)

    lamda = 0.1 # weight for observation residual
    x_opt, u_opt = optimize_trajectory(h_x, h_z, h_u, DT, lamda)

    close()
    plot(h_xTrue[1,:], h_xTrue[2,:], ".b", label="Ground Truth")
    plot(h_x[1,:], h_x[2,:], ".g", label="Dead Recknoing")
    plot(h_z[1,:], h_z[2,:], "xg", label="GPS Observation")
    plot(x_opt[1,:], x_opt[2,:], "-r", label="Optimized trajectory")
    legend()
    axis("equal")

    println(PROGRAM_FILE," Done!!")
end

@time main()

下記のGitHubリポジトリでも公開しています。

github.com

 

上記のJuliaコードを実行すると、

下記のような結果が得られます。

f:id:meison_amsl:20181104213240p:plain

青線が真のロボットの軌跡、

緑線がオドメトリの軌跡(ノイズにより精度が低下)

緑バツがGPSの位置(精度は良いですが、データの取得間隔が長い)

そして、赤線が最適化後の軌跡です。

 

この赤線の結果を見ると分かる通り、

オドメトリとGPSのデータを使って、

軌跡全体が真の軌跡に近づき、

高精度に推定出来ていることがわかります。

(カルマンフィルタなどを使った場合は、

GPSが取得された点以外は補正されません)

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com