【論文閱讀】Optimization-Based Collision Avoidance

前言與參考

1. 論文地址：https://ieeexplore.ieee.org/document/9062306

   文章是2018年5月提出的，但是到了2020年才發表到ACC 所以時間軸上寫的是2021年5月刊的ACC，算是近年來把凸優化來表示這個的開端（當然有更早的） 然后帶起了一波熱度的感覺。這篇文章自身也有 74cites了

2. 代碼Julia語言：https://github.com/XiaojingGeorgeZhang/OBCA

3. 知乎的介紹：一種避障的軌跡生成方法(1) - 知乎 (zhihu.com)

我的主要是順整個論文而不是只提取一個小部分... 雖然這樣子干起來 套娃現象非常嚴重。基於優化的方法主要是：如何將碰撞問題轉換成一個優化約束問題，然后用數值優化的方法去做求解。

基礎知識

避障問題是一個NP-hard問題：

P問題、NP問題、NP完全問題和NP難問題 - 知乎 (zhihu.com)

也就是NP問題能約化到他，但是他並不是一個NP問題

I 介紹

對於基於優化計算軌跡的最大挑戰在於：如何合適的表示collision avoidance，一般碰撞問題是非凸的並且很難進行求解。當然也有很多論文指出了應該怎樣去求解這個碰撞問題

• 碰撞約束被近似成一個線性約束，但是很難去度量這個近似的誤差 [10]

• 部分文章直接將這個問題下的機器人看做一個點物體，所以並不適用於多維度的機器人 比如機械臂

• 如果將障礙物看做多面體，那么碰撞約束通常會轉成一個整數變量[16]

  the discrete set Y corresponds to a polyhedral set of integer points, \(Y=\left\{y \mid y \in \boldsymbol{Z}^{m}, A y \leq a\right\}\), which in most applications is restricted to 0–1 values, \(y ∈ \{0, 1\}^m\).

  

[16] Grossmann, Ignacio E. "Review of nonlinear mixed-integer and disjunctive programming techniques." Optimization and engineering 3.3 (2002): 227-252.

所以以上這三種reformulation對於凸限制下的線性系統來說非常具有吸引力，因為在這樣的條件下 混合整數優化問題可以被解決。一般來說當設計實時性系統時，整數變量應該避免在非線性系統中優化問題中出現

在本文中，我們主要研究：機器人在n維空間內避障運動，提出模型：建模避障問題約束，並避免之前說到的這三個問題出現

• 機器人和障礙物都描述成凸集，例如polytopes或ellipsoids(橢圓體) 然后碰撞問題就可以exactly and non conservatively reformulated 成一系列平滑的非凸限制
  主要是使用strong duality of convex optimization在兩個凸集間reformulating距離函數

• 還有第二種方法是建立在signed distance，也就是帶符號的距離值，這樣就能表達物體之間的交叉現象問題。這種情況下也能計算即使在要碰撞時 最小損壞程度的碰撞軌跡是怎樣的

• 這里的handle in general是指泛化性問題還是說不能很容易求解

  較難求解

• 非凸限制的平滑是指什么？a set of smooth non convex constraints

• 這里的整數約束主要在了誰的身上？為什么將障礙物看成多面體后，限制會使用整數？

  傑哥：可能是和哪條邊碰

  吳院：也就是說在表達障礙物這件事上是整數約束給出的 因為渲染像素，mesh值等都不能切半（分辨率確定情況下）；還有一些東西，比如說特征點的位置的優化，如果說是一定要在圖像當中的像素的話，那就是一個整數規划問題，但是現在比如說VINS等等，都是用的浮點數去去去計算的，只是這樣算出來的，對於圖像來講它沒有意義。

  參考16：離散集 Y 對應於整數點的多面體集

  • 具體可能需要再找一下，哪幾篇論文是使用這個方法 表現如何的

  好像是M個障礙物，M整數約束在障礙物數量上

A. Related work

作者挺剛的，一上來就是：in this article, we do not review, or compare, optimization-based collision avoidance methods with alternative approaches, such as those based on xxx

實際上在實驗上也是如此，只是一開始說Hybrid A*太bangbang了，不像人開的一把倒車這種（雖然有時候人也不一定能一把倒好）。那就大致總結一下作者的看法：

• dynamic programming [17]
• reachability analysis [18]-[20]
• graph search method 比如A, Hybrid A [21]-[23]
• (random) sampling，比如急速隨機探索的RRT和RRT* [24]-[27]

也就是以上幾種都是作為基於優化的方法但是隨同了一些其他方法，但是一般優化都需要一個warm start 肯定是需要隨同其他給出一個呀 比如作者自己也是用了hybrid A*的warm start

從[29] 我們知道避障問題是一個已知的NP-hard問題，基本上所有的實用性方法都會有一些啟發項的東西，而這些性能上的表現都是基於特定的問題和配置參數下，也就是泛化能力不強。

[30]-[34] 主要就是基於優化方法來考慮這一問題

II 問題描述

目標：找到一系列的控制指令使得機器人能從初始狀態 \(x_s\) 運動到終點 \(x_F \in \mathbb R^{n_x}\)

• 同時優化我們的目標函數 \(J=\sum_{k=0}^{N} \ell\left(x_{k}, u_{k}\right)\) ，其中 \(\ell:\mathbb R^{n_x} \times \mathbb R^{n_u} \rightarrow \mathbb R\) 是一個stage cost
• 避免碰撞 \(M>1\) 個障礙物 \(\mathbb{O}^{(1)}, \mathbb{O}^{(2)}, \ldots, \mathbb{O}^{(M)} \subset \mathbb{R}^{n}\)

自身運動

狀態間的轉移：

\[x_{k+1}=f(x_k,u_k) \]

• \(x_k \in \mathbb R^{n_x}\) 是指機器人初始狀態 \(x_0=x_S\)在時間 \(k\) 時的狀態，大多數時候這里的 \(x_k\) 表示的是位置 \(p_k\in \mathbb R^{n}\) 與角度 \(\theta_k \in \mathbb R^n\)，當然也有速度 \(\dot p_k\) 和 角速度 \(\dot \theta_k\)
• \(u_k \in \mathbb R^{n_u}\) 是指控制輸入
• \(f:\mathbb R^{n_x}\times \mathbb R^{n_u} \rightarrow \mathbb R^{n_x}\) 描述了系統的動力學

此文中假設沒有噪音和擾動，那么系統受限於輸入和狀態：

\[h(x_k,u_k)\le0 \tag{2} \]

自身空間內的表示：

1. point-mass方式 自身就是一個小點 也就是位置點：

\[\mathbb{E}\left(x_{k}\right)=p_k \]

1. full dimensional set方式 自身也是一個多邊形表示 所以是在一個初始集上進行旋轉平移

\[\mathbb{E}\left(x_{k}\right)=R\left(x_{k}\right) \mathbb{B}+t\left(x_{k}\right), \quad \mathbb{B}:=\left\{y: G y \preceq_{\overline{\mathcal{K}}} g\right\} \]

• 然后 \(\mathbb B\) 是一個多邊形表示方式，"initial" convex set \(\mathbb B \subset \mathbb R^n\)
• 這個式子表示的是初始的一個多邊形應該經過正陽的旋轉和平移到自身的這樣一個狀態點 \(x_k\)

障礙物

多邊形的表達方式，具體可以參照《凸優化》，其實多邊形就是多條直線的交集，所以多邊形就可以表達為如下，其中下面的 \(\mathcal K\) 是一種廣義不等式的表達形式，可以簡單理解為不等式就行。然后障礙物也是多邊形，所以 \(\mathbb O\) 就認為是一個障礙物的表達式

\[\mathbb{O}^{(m)}=\left\{y \in \mathbb{R}^{n}: A^{(m)} y \preceq_ \mathcal{K} b^{(m)}\right\} \]

其中 \(\preceq\) 在原文notation進行了一定的解釋，不過就和上面說的將其理解成多條直線的交集即可。

結合上面公式，相關車體自身和障礙物的限制也就能表示出來了：

\[\mathbb{E}\left(x_{k}\right) \cap \mathbb{O}^{(m)}=\emptyset \quad \forall m=1, \ldots, M \tag{3} \]

優化問題

結合公式(1)(2)(3)，那么整體的優化問題就是這樣的形式了：

\[\begin{align*}&\!\!\!\!\!\!\!\!\displaystyle \min _{\mathbf x, \mathbf u} \quad \displaystyle \sum _{k=0}^{N} \ell (x_{k}, u_{k}) \\&\!\!\!\!\!\!\!\!\text {s.t.} \quad ~x_{0} = x_{S},\quad x_{N+1} = x_{F} \\&\left.{ \begin{array}{ll} & x_{k+1} = f(x_{k},u_{k}), \\ & h(x_{k}, u_{k}) \leq 0, \\ & \mathbb E (x_{k}) \cap \mathbb O ^{(m)}= \emptyset, \\ \end{array} }\right \} \begin{array}{lll} k=0,\ldots,N\\[0ex] m=1,\ldots,M \end{array}\tag{6}\end{align*} \]

其中 \(\mathbb{E}\left(x_{k}\right) \cap \mathbb{O}^{(m)}=\emptyset\) 通常情況下是一個非凸和不可微的約束。所以接下來我們會介紹兩種novel的方法去建模這個碰撞問題使得其連續和可微，以便可以使用現有的off-the-shelf gradient或者是基於hassian的優化算法來求解。

• 但是這里的限制 \(h(x_k,u_k)\le0\) 是我一直沒搞清楚，即使到了文章的最后也沒指出這個的形式，后續可能需要看一下代碼中的表示

III Collision Avoidance

首先介紹一下他人的工作比如這篇

[10] Schulman, John, et al. "Motion planning with sequential convex optimization and convex collision checking." The International Journal of Robotics Research 33.9 (2014): 1251-1270.

提出了如何formulating這個碰撞檢測問題，基於他們提出來的signed distance

\[\begin{equation*} \text {sd}(\mathbb {E}(x), \mathbb O):= \text {dist}(\mathbb {E}(x), \mathbb O) - \text {pen}(\mathbb {E}(x), \mathbb O)\tag{7}\end{equation*} \]

其中 \(\text {dist}(\cdot,\cdot)\) 和 \(\text {pen}(\cdot,\cdot)\) 公式如下：

\[\begin{align*} \text {dist}(\mathbb {E}(x), \mathbb O):=&\min _{t}\{\|t\| \colon (\mathbb {E}(x)+t) \cap \mathbb O \neq \emptyset \} \\ \text {pen}(\mathbb {E}(x), \mathbb O):=&\min _{t}\{\|t\| \colon (\mathbb {E}(x)+t) \cap \mathbb O = \emptyset \}.\end{align*} \]

簡單的來說就是sd>0的時候，他兩不相交的最短距離值，sd<0時，則是他兩相交的交集內的最短距離，可以大概結合一下上圖看出來

所以這樣子的情況下，我們的那個公式(3)的非凸和不可微的約束就可以換成 \(\text {sd}(\mathbb {E}(x), \mathbb O)>0\)。但是呢，問題還是存在的，還是非凸和不可微的；其次，為了讓優化算法更有效求解，我們需要一個explicit的表達方式 但是 sd這種，本身就是dist 和pen這兩個小的優化的最優解了。而這種情況下 我們會使用局部線性化近似（有空的話 再套娃看一下吧，這就是一開始介紹中說的 這樣子方法的error是不好估計的。也就是說近似可以 但是你要知道近似的error這樣好把控這個求解

本文提出的reformulation的方法就可以克服這個不可微問題 也不需要一個對於sd距離的explicit表達。雖然前文中提出的兩種方法 其實是.. 一個用在point-mass，一個在full-dimensional 前者的更容易寫，后者是用於實驗，原理是一個原理

• error的用處？

  ybw：如果知道的話，可能可以用在膨脹 \(d_{min}\) ，比如如果這個error超過膨脹的限值 可能這個解我們就不相信

本文方法

根據前面表達的：

\[\mathbb{E}\left(x_{k}\right)=R\left(x_{k}\right) \mathbb{B}+t\left(x_{k}\right), \quad \mathbb{B}:=\left\{y: G y \preceq_{\overline{\mathcal{K}}} g\right\} \]

\[\mathbb{O}^{(m)}=\left\{y \in \mathbb{R}^{n}: A^{(m)} y \preceq_ \mathcal{K} b^{(m)}\right\} \]

則我們的碰撞問題中dist表達，其中\(d_{min}\) 是自身設定的安全距離閾值

\[\begin{align*}&\text {dist}(\mathbb {E}(x), \mathbb {O}) > {\mathsf {d}_{\text {min}}}\\ &\quad \Leftrightarrow \exists \lambda \succeq _{ \mathcal {K}^{*}}0,\quad \mu \succeq _{\bar{\mathcal {K}} ^{*}}0 \colon -g^\top \mu +(At(x) - b)^\top \lambda > \mathsf {d}_{\text {min}},\\ & \qquad \quad G^\top \mu + R(x)^\top A^\top \lambda = 0,\quad \|A^\top \lambda \|_{*} \leq 1.\tag{14}\end{align*} \]

公式(14) 已經直接把解法給出了，實際的證明過程如下：

1. 這是根據前面我們表達的自身和障礙物而的出來的求解式子：

   \[\begin{align*}\operatorname{dist}(\mathbb{E}(x), \mathbb{O})&=\min _{e, o}\left\{\|e-o\|: A o \preceq_{\mathcal{K}} b, e \in \mathbb{E}(x)\right\}\\&=\min _{e^{\prime}, o}\left\{\left\|R(x) e^{\prime}+t(x)-o\right\|: A o \preceq_{\mathcal{K}} b, G e^{\prime} \preceq_{\overline{\mathcal{K}}} g\right\}\end{align*} \]

2. 對這個問題進行dual

   \[\max _{\lambda, \mu}\left\{-g^{\top} \mu+(A t(x)-b)^{\top} \lambda: G^{\top} \mu+R(x)^{\top} A^{\top} \lambda=0,\left\|A^{\top} \lambda\right\|_{*} \leq 1, \lambda \succeq_{\mathcal{K}} 0, \mu \succeq_{\overline{\mathcal{K}}} \cdot 0\right\} \]

   • 詳情推導展開

     首先需要打開凸優化英文原版418頁 頁碼402頁

     

3. dual全部完成后 還是一個nonconvex的優化問題，但是滿足KKT條件就可以引入數值優化求解的方法，類似於一種迭代？但是因為是這樣的 所以需要一個warm start 而這個warm start很大程度上決定了你是否能找到較好的解。

   \[\begin{align*}&\displaystyle \min _{\mathbf x, \mathbf u,\boldsymbol \lambda, \boldsymbol \mu } \displaystyle \quad \sum _{k=0}^{N} \ell (x_{k}, u_{k}) \\&\,\,\, \text {s.t.} \quad \,\,~~x_{0} = x(0),\quad x_{N+1} = x_{F} \\&\qquad \quad \,\,\,\,\,~ x_{k+1} = f(x_{k},u_{k}),\quad h(x_{k}, u_{k}) \leq 0 \\&\qquad \quad \,\,\,\,\,-g^\top \mu _{k}^{(m)}+(A^{(m)}\,t(x_{k}) - b^{(m)})^\top \lambda _{k}^{(m)} > 0 \\&\qquad \quad \,\,~~ G^\top \mu _{k}^{(m)} + R(x_{k})^\top {A^{(m)}}^\top \lambda _{k}^{(m)} =0 \\&\qquad \quad \,\,~~ \big \|{A^{(m)}}^\top \lambda _{k}^{(m)}\big \|_{*} \leq 1, \quad \lambda _{k}^{(m)}\succeq _{ \mathcal {K}^{*}}0,~ \mu _{k}^{(m)}\succeq _{\bar{\mathcal {K}} ^{*}}0 \\&\qquad \quad \,\,\,~~ \text {for } k=0,\ldots,N,\quad m=1,\ldots,M\tag{15}\end{align*} \]

后半段是加入了sd為負的情況，也就是如果一定要撞上的話 怎樣撞 撞的比較輕：

\[\begin{align*}&\displaystyle \min _{\mathbf x, \mathbf u,\mathbf s, \boldsymbol \lambda, \boldsymbol \mu } \qquad \displaystyle \sum _{k=0}^{N} \left [{ \ell (x_{k}, u_{k}) + \kappa \cdot \sum _{m=1}^{M} s_{k}^{(m)} }\right] \\&\,\, \text {s.t.}\, \qquad \,\,\,~~x_{0} = x_{S},\quad x_{N+1} = x_{F} \\&\qquad \quad \,\,\,\,\quad x_{k+1} = f(x_{k},u_{k}),\quad h(x_{k}, u_{k}) \leq 0 \\&\qquad \quad \,\,\,\,~~ -g^\top \mu _{k}^{(m)}+(A^{(m)}\,t(x_{k}) - b^{(m)})^\top \lambda _{k}^{(m)} > -s_{k}^{(m)} \\&\qquad \quad \,\,\,\,\,~~ G^\top \mu _{k}^{(m)} + R(x_{k})^\top {A^{(m)}}^\top \lambda _{k}^{(m)} =0 \\&\qquad \quad \,\,\,\,\,~~ \big \|{A^{(m)}}^\top \lambda _{k}^{(m)}\big \|_{*} = 1 \\&\qquad \quad \,\,\,\,\,\,~~ s_{k}^{(m)}\geq 0,\quad \lambda _{k}^{(m)}\succeq _{ \mathcal {K}^{*}}0,~\mu _{k}^{(m)}\succeq _{\bar{\mathcal {K}} ^{*}}0 \\&\qquad \quad \,\,\,\,\,~~ \text {for } k=0,\ldots,N,\quad m=1,\ldots,M\tag{18}\end{align*} \]

其中 \(s_{k}^{(m)} \in \mathbb R_+\)是一個在時間\(k\)下的slack variable associated with obstacle \(\mathbb O^{(m)}\)，\(\kappa \ge0\) is a weight factor that keeps the slack variable as small as possible.

所以這么看下來主要不是說把非凸問題轉成一個凸問題，而是非凸+不可微 → 非凸，然后使用Lagrange dual formulation去做成dual問題后 用數值優化方法進行求解。最后數值優化求解實驗環節放在代碼部分講解，比較好直接對應，代碼是julia版的

IV 代碼閱讀

車輛動力學

車的動力學 阿克曼模型的：

\[\begin{align*} \dot {X}=&v \cos (\varphi) \\ \dot {Y}=&v \sin (\varphi) \\ \dot {\varphi }=&\frac {v \tan (\delta)}{L} \\ \dot {v}=&a\tag{19}\end{align*} \]

下圖為各個解釋：

加上把這些范圍對應到代碼里

steering angle： \(\dot \delta \in [-0.6,0.6]\)

車輛速度：-1 到 2m/s

# [x_lower, x_upper, -y_lower,   y_upper  ]
XYbounds =  [ -15, 15,  1,  10]	# constraints are on (X,Y)

#input constraints
@constraint(m, [i=1:N], -0.6 <= u[1,i] <= 0.6)
@constraint(m, [i=1:N], -0.4 <= u[2,i] <= 0.4)

#state constraints
@constraint(m, [i=1:N+1], XYbounds[1] <= x[1,i] <= XYbounds[2])
@constraint(m, [i=1:N+1], XYbounds[3] <= x[2,i] <= XYbounds[4])
@constraint(m, [i=1:N+1], -1  <= x[4,i] <= 2)

##############################
# dynamics of the car
##############################
# - unicycle dynamic with euler forward
# - sampling time scaling, is identical over the horizon

for i in 1:N
		if fixTime == 1
			@NLconstraint(m, x[1,i+1] == x[1,i] + Ts*(x[4,i] + Ts/2*u[2,i])*cos((x[3,i] + Ts/2*x[4,i]*tan(u[1,i])/L)))
	    @NLconstraint(m, x[2,i+1] == x[2,i] + Ts*(x[4,i] + Ts/2*u[2,i])*sin((x[3,i] + Ts/2*x[4,i]*tan(u[1,i])/L)))
	    @NLconstraint(m, x[3,i+1] == x[3,i] + Ts*(x[4,i] + Ts/2*u[2,i])*tan(u[1,i])/L)
	    @NLconstraint(m, x[4,i+1] == x[4,i] + Ts*u[2,i])
    else
	    @NLconstraint(m, x[1,i+1] == x[1,i] + timeScale[i]*Ts*(x[4,i] + timeScale[i]*Ts/2*u[2,i])*cos((x[3,i] + timeScale[i]*Ts/2*x[4,i]*tan(u[1,i])/L)))
	    @NLconstraint(m, x[2,i+1] == x[2,i] + timeScale[i]*Ts*(x[4,i] + timeScale[i]*Ts/2*u[2,i])*sin((x[3,i] + timeScale[i]*Ts/2*x[4,i]*tan(u[1,i])/L)))
	    @NLconstraint(m, x[3,i+1] == x[3,i] + timeScale[i]*Ts*(x[4,i] + timeScale[i]*Ts/2*u[2,i])*tan(u[1,i])/L)
	    @NLconstraint(m, x[4,i+1] == x[4,i] + timeScale[i]*Ts*u[2,i])
    end

    if fixTime == 0
    	@constraint(m, timeScale[i] == timeScale[i+1])
  	end
end

u0 = [0,0]
if fixTime == 1
	for i in 1:N
		if i==1
			@constraint(m,-0.6<=(u0[1]-u[1,i])/Ts <= 0.6)
		else
			@constraint(m,-0.6<=(u[1,i-1]-u[1,i])/Ts <= 0.6)
		end
	end
else
	for i in 1:N
		if i==1
			@NLconstraint(m,-0.6<=(u0[1]-u[1,i])/(timeScale[i]*Ts) <= 0.6)
		else
			@NLconstraint(m,-0.6<=(u[1,i-1]-u[1,i])/(timeScale[i]*Ts) <= 0.6)
		end
	end
end

cost function

最后cost function的形式是：

\[\begin{equation*} J(\mathbf u, T_{\text {opt}}) = q N T_{\text {opt}} + \sum _{k=0}^{N-1} u_{k}^\top R u_{k} + \Delta {u}_{k}^\top R_{\Delta } \Delta {u}_{k}.\tag{20}\end{equation*} \]

##############################
# cost function
##############################
# (min control inputs)+      
# (min time)+
# (regularization dual variables)

u0 = [0,0]
#fix time objective
if fixTime == 1
	@NLobjective(m, Min,sum(0.01*u[1,i]^2 + 0.5*u[2,i]^2 for i = 1:N) + 
	 					 sum(0.1*((u[1,i+1]-u[1,i])/Ts)^2 + 0.1*((u[2,i+1]-u[2,i])/Ts)^2 for i = 1:N-1)+
	 					 	(0.1*((u[1,1]-u0[1])/(Ts))^2 + 0.1*((u[2,1]-u0[2])/(Ts))^2) +
	 					 sum(0.0001*x[4,i]^2 for i=1:N+1)+
	 					 sum(0.001*(x[1,i]-rx[i])^2 + 0.001*(x[2,i]-ry[i])^2 + 0.01*(x[3,i]-ryaw[i])^2 for i=1:N+1))
else
#varo time objective
	@NLobjective(m, Min,sum(0.01*u[1,i]^2 + 0.5*u[2,i]^2 for i = 1:N) + 
	 					 sum(0.1*((u[1,i+1]-u[1,i])/(timeScale[i]*Ts))^2 + 0.1*((u[2,i+1]-u[2,i])/(timeScale[i]*Ts))^2 for i = 1:N-1) +
	 					    (0.1*((u[1,1]-u0[1])   /(timeScale[1]*Ts))^2 + 0.1*((u[2,1]-u0[2])   /(timeScale[1]*Ts))^2) +
	 					 sum(0.5*timeScale[i] + 1*timeScale[i]^2 for i = 1:N+1)+
	 					 sum(0.0001*x[4,i]^2 for i=1:N+1)+
	 					 sum(0.001*(x[1,i]-rx[i])^2 + 0.001*(x[2,i]-ry[i])^2 + 0.0001*(x[3,i]-ryaw[i])^2 for i=1:N+1) 	) 
						#    
end

這里是兩個形式為了簡單解釋 我們以fixTime那個 雖然我也不知道fix的是啥，似乎是離散的時間步長還不一樣。

每個sum都對應了公式(20)

1. \(\sum _{k=0}^{N-1} u_{k}^\top R u_{k}\) ，其中 \(R=\text{diag}(0.01,0.5)\)

   sum(0.01*u[1,i]^2 + 0.5*u[2,i]^2 for i = 1:N)
   

2. \(\sum _{k=0}^{N-1}\Delta {u}_{k}^\top R_{\Delta } \Delta {u}_{k}\)，其中 \(R_{\Delta }=\text{diag}(0.1,0.1)\)

   sum(0.1*((u[1,i+1]-u[1,i])/Ts)^2 + 0.1*((u[2,i+1]-u[2,i])/Ts)^2 for i = 1:N-1)
   +(0.1*((u[1,1]-u0[1])/(Ts))^2 + 0.1*((u[2,1]-u0[2])/(Ts))^2)
   

3. \(\sum _{k=0}^{N-1}\ell (x_{k}, u_{k})\) 應該是這個，然后對應的是公式 (19) 所有平方和相加，也就是\(\dot {X}, \dot {Y}, \dot {\varphi }, \dot {v}\)

   \[\sum _{k=0}^{N-1}\ell (x_{k}, u_{k})=\sum _{k=0}^{N-1}(\Delta \dot X^2+\Delta \dot Y^2+\Delta \dot {\varphi }^2+ \dot {v}^2) \]

   sum(0.0001*x[4,i]^2 for i=1:N+1) + 
   sum(0.001*(x[1,i]-rx[i])^2 + 0.001*(x[2,i]-ry[i])^2 + 0.01*(x[3,i]-ryaw[i])^2 for i=1:N+1)
   

• 但是第一項 也就是注釋里寫的 regularization dual variables 不太明白為什么是剩余的sum

碰撞約束

這一段的約束全都限制在公式 (15)/(18)中，注釋中都詳細標明了對應的約束公式簡述：

##############################
# obstacle avoidance constraints
##############################

# width and length of ego set
W_ev = ego[2]+ego[4]
L_ev = ego[1]+ego[3]

g = [L_ev/2,W_ev/2,L_ev/2,W_ev/2]

# ofset from X-Y to the center of the ego set
offset = (ego[1]+ego[3])/2 - ego[3]

for i in 1:N+1 	# iterate over time steps
	for j = 1 : nOb 	# iterate over obstacles
		Aj = A[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:]	# extract obstacle matrix associated with j-th obstacle
		lj = l[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:]	# extract lambda dual variables associate j-th obstacle
		nj = n[(j-1)*4+1:j*4 ,:] 						# extract mu dual variables associated with j-th obstacle
		bj = b[sum(vOb[1:j-1])+1 : sum(vOb[1:j])]		# extract obstacle matrix associated with j-th obstacle

		# norm(A'*lambda) <= 1
		@NLconstraint(m, (sum(Aj[k,1]*lj[k,i] for k = 1 : vOb[j]))^2 + (sum(Aj[k,2]*lj[k,i] for k = 1 : vOb[j]))^2  <= 1   )

		# G'*mu + R'*A*lambda = 0
		@NLconstraint(m, (nj[1,i] - nj[3,i]) +  cos(x[3,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + sin(x[3,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0  )
		@NLconstraint(m, (nj[2,i] - nj[4,i]) -  sin(x[3,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + cos(x[3,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0  )

		# -g'*mu + (A*t - b)*lambda > 0
		@NLconstraint(m, (-sum(g[k]*nj[k,i] for k = 1:4) + (x[1,i]+cos(x[3,i])*offset)*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j])
						+ (x[2,i]+sin(x[3,i])*offset)*sum(Aj[k,2]*lj[k,i] for k=1:vOb[j]) - sum(bj[k]*lj[k,i] for k=1:vOb[j]))  >= dmin  )
	end
end

\[\begin{align*}&\displaystyle \min _{\mathbf x, \mathbf u,\boldsymbol \lambda, \boldsymbol \mu } \displaystyle \quad \sum _{k=0}^{N} \ell (x_{k}, u_{k}) \\&\,\,\, \text {s.t.} \quad \,\,~~x_{0} = x(0),\quad x_{N+1} = x_{F} \\&\qquad \quad \,\,\,\,\,~ x_{k+1} = f(x_{k},u_{k}),\quad h(x_{k}, u_{k}) \leq 0 \\&\qquad \quad \,\,\,\,\,-g^\top \mu _{k}^{(m)}+(A^{(m)}\,t(x_{k}) - b^{(m)})^\top \lambda _{k}^{(m)} > 0 \\&\qquad \quad \,\,~~ G^\top \mu _{k}^{(m)} + R(x_{k})^\top {A^{(m)}}^\top \lambda _{k}^{(m)} =0 \\&\qquad \quad \,\,~~ \big \|{A^{(m)}}^\top \lambda _{k}^{(m)}\big \|_{*} \leq 1, \quad \lambda _{k}^{(m)}\succeq _{ \mathcal {K}^{*}}0,~ \mu _{k}^{(m)}\succeq _{\bar{\mathcal {K}} ^{*}}0 \\&\qquad \quad \,\,\,~~ \text {for } k=0,\ldots,N,\quad m=1,\ldots,M\tag{15}\end{align*} \]

然后就是使用IPOPT求解的過程了，其中有關於兩個Lagrange factor的 warm start 求解過程一個initial