上一篇文章用最爲基本SX來構建Single-Shooting解決MPC問題的方法。這篇最開始會簡單講解一下Mult-Shooting的不同之處，並且用其他的結構實現同樣的問題。
Multi-Shooting
如上節對比圖中所示，multi-shooting對狀態也進行離散。這裏不對方法進行過多展開，只介紹一下代碼上的區別之處。具體完整的代碼詳見MPC/sim_2_mpc_mul_shooting.py代碼。
if __name__ == '__main__':
... 建模部分 完全一致 ...
...
# 優化目標及約束條件
obj = 0 # 初始化優化目標
g = [] # 約束條件（相等約束）
g.append(X[:, 0]-P[:3]) # 【主要不同之處】在每一離散段內開始處相等
for i in range(N):
obj = obj + ca.mtimes([(X[:, i]-P[3:]).T, Q, X[:, i]-P[3:]]) + ca.mtimes([U[:, i].T, R, U[:, i]]) # 和之前一樣的代價函數
x_next_ = f(X[:, i], U[:, i])*T +X[:, i]
g.append(X[:, i+1]-x_next_) # 之後每一段開始也是得相等
# 定義優化問題
## 輸入變量。和之前不同的是，這裏需要同時將系統狀態X也作爲優化變量輸入，
## 根據CasADi要求，必須將它們都變形爲一維向量
opt_variables = ca.vertcat( ca.reshape(U, -1, 1), ca.reshape(X, -1, 1))
## 和前例相同的定義方式和設置
nlp_prob = {'f': obj, 'x': opt_variables, 'p':P, 'g':ca.vertcat(*g)}
opts_setting = {'ipopt.max_iter':100, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6}
solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)
和之前一樣，得到問題的定義和其求解方法後，我們可以開始仿真。
if __name__ == '__main__':
...
...
# 仿真部分
## 約束條件，因爲約束條件不變，在while循環外定義
### g（條件約束，均爲0，即起始點相等）
lbg = 0.0
ubg = 0.0
### 優化狀態約束
lbx = []
ubx = []
#### 因爲之前定義的時候是先U再X所以這裏定義的時候也需要先定義U的約束後再添加X約束
for _ in range(N):
lbx.append(-v_max)
lbx.append(-omega_max)
ubx.append(v_max)
ubx.append(omega_max)
for _ in range(N+1):
lbx.append(-2.0)
lbx.append(-2.0)
lbx.append(-np.inf)
ubx.append(2.0)
ubx.append(2.0)
ubx.append(np.inf)
## 其他初始條件，同前
t0 = 0.0
x0 = np.array([0.0, 0.0, 0.0]).reshape(-1, 1)
x0_ = x0.copy()
x_m = np.zeros((n_states, N+1))
next_states = x_m.copy().T
xs = np.array([1.5, 1.5, 0.0]).reshape(-1, 1) # final state
u0 = np.array([1,2]*N).reshape(-1, 2).T # 要注意輸入的形狀爲（n, 2)
x_c = []
u_c = []
t_c = []
xx = []
sim_time = 20.0
## MPC開始
mpciter = 0
start_time = time.time()
index_t = []
while(np.linalg.norm(x0-xs)>1e-2 and mpciter-sim_time/T<0.0 ):
### 每次循環先初始化U，X和參數。注意初始化順序和形狀
c_p = np.concatenate((x0, xs))
init_control = np.concatenate((u0.reshape(-1, 1), next_states.reshape(-1, 1)))
t_ = time.time()
res = solver(x0=init_control, p=c_p, lbg=lbg, lbx=lbx, ubg=ubg, ubx=ubx)
index_t.append(time.time()- t_)
### 得到結果
estimated_opt = res['x'].full() # 先轉換爲numpy
u0 = estimated_opt[:200].reshape(N, n_controls) # 已知前200爲U的結果，轉換成（n，2）
x_m = estimated_opt[200:].reshape(N+1, n_states) # 剩餘部分爲MPC計算出之後N步可能的狀態
x_c.append(x_m.T)
u_c.append(u0[0, :])
t_c.append(t0)
t0, x0, u0, next_states = shift_movement(T, t0, x0, u0, x_m, f)
x0 = ca.reshape(x0, -1, 1)
x0 = x0.full()
xx.append(x0)
mpciter = mpciter + 1
在和Sim1完全相同條件下，sim2的方法只需要0.026秒完成一個循環。可見不同方法對於求解效果的影響十分顯著。
不同版本對比
接下來對其他不同的實現方法進行說明和對比。
MX
MX是CasADi提供的另一種符合實現建模的形式。具體的定義和說明，官網已經給出很好的解釋。
The MX class is used to build up trees made up from MXNodes. It is a more general graph representation than the scalar expression, SX, and much less efficient for small objects. On the other hand, the class allows much more general operations than does SX, in particular matrix valued operations and calls to arbitrary differentiable functions.
理解是一種介於SX和直接數據的格式。由於在構建問題上只是SX->MX這樣切換。這裏就不過多介紹。
Structure
對於更爲複雜的問題，CasADi還提供了一種較爲綜合的方式來定義狀態，不至因爲衆多定義而導致混亂，可以理解爲CasADi提供的一個便捷工具（有SX和MX版本，我這裏只以SX爲例）。使用這個工具需要添加如下引用
import casadi.tools as ca_tools
接下來我們進行具體代碼講解。同樣地，和之前一致的內容不重複介紹。
if __name__ == '__main__':
T = 0.2
N = 100
rob_diam = 0.3
v_max = 0.6
omega_max = np.pi/4.0
# 新的定義狀態方式，非常直觀。
states = ca_tools.struct_symSX([
(
ca_tools.entry('x'),
ca_tools.entry('y'),
ca_tools.entry('theta')
)
])
# 爲了方便操作，可以將裏面的元素單獨取出。如果只有一個狀態，取出的變量後一定要加
# 逗號，例如： a, = some_structure[...]。否則後續會出現維度錯誤的問題。
x, y, theta = states[...]
# 可以直接獲得狀態尺寸
n_states = states.size
# 對於控制變量同理可得
controls = ca_tools.struct_symSX([
(
ca_tools.entry('v'),
ca_tools.entry('omega')
)
])
v, omega = controls[...]
n_controls = controls.size
# 定義函數的時候就可以借用之前定義的狀態，因爲x_dot = f(x)。
# 注意這裏是struct_SX
rhs = ca_tools.struct_SX(states)
rhs['x'] = v*np.cos(theta)
rhs['y'] = v*np.sin(theta)
rhs['theta'] = omega
# 這一步和之前一樣，定義好函數
f = ca.Function('f', [states, controls], [rhs], ['input_state', 'control_input'], ['rhs'])
if __name__ == '__main__':
...
# 定義MPC問題
## 優化的目標，同樣包含U和X。在這裏需要注意形式和結果的差別，
## 之前SX的方法實際上是定義了矩陣，而struc_symSX加上repeat的方式
## 定義的是list，每個list是一個struct
optimizing_target = ca_tools.struct_symSX([
(
ca_tools.entry('U', repeat=N, struct=controls),
ca_tools.entry('X', repeat=N+1, struct=states)
)
])
## 取出，簡化公式書寫
U, X, = optimizing_target[...]
## 對於參數，只有shape，因爲就一維
current_parameters = ca_tools.struct_symSX([
(
ca_tools.entry('P', shape=n_states+n_states),
)
])
P, = current_parameters[...] # 注意逗號
Q = np.array([[1.0, 0.0, 0.0],[0.0, 1.0, 0.0],[0.0, 0.0, .1]])
R = np.array([[0.5, 0.0], [0.0, 0.05]])
## 代價函數和約束
obj = 0
g = []
### 正如之前所說的，repeat定義出來的結果是list，所以這裏index的表達方式跟之前的有所區別
g.append(X[0]-P[:3])
for i in range(N):
obj = obj + ca.mtimes([(X[i]-P[3:]).T, Q, X[i]-P[3:]]) + ca.mtimes([U[i].T, R, U[i]])
x_next_ = f(X[i], U[i])*T + X[i]
g.append(X[i+1] - x_next_)
# 這裏optimizing_target不需要壓縮成一維矩陣就可以作爲輸入
nlp_prob = {'f': obj, 'x': optimizing_target, 'p':current_parameters, 'g':ca.vertcat(*g)}
opts_setting = {'ipopt.max_iter':200, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6}
solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)
求解器定義完後，就可以開始仿真了。不過這裏需要特別說明的是這個約束的定義。這也是我花了很多時間才搞清楚的關係。
if __name__ == '__main__':
...
...
# 約束條件
## g的約束跟之前一致
lbg = 0.0
ubg = 0.0
lbx = []
ubx = []
## 因爲U和X放在一個結構裏，所以在前N循環是根據以下順序定義的
for _ in range(N):
lbx = lbx + [-v_max, -omega_max, -2.0, -2.0, -np.inf]
ubx = ubx + [v_max, omega_max, 2.0, 2.0, np.inf]
## 因爲X有N+1個元素，單獨需要再定義第N+1個元素
lbx.append(-2.0)
lbx.append(-2.0)
lbx.append(-np.inf)
ubx.append(2.0)
ubx.append(2.0)
ubx.append(np.inf)
後來，才發現其實CasADi還提供了更爲優美的定義方法
if __name__ == '__main__':
...
...
# 約束條件
## g的約束跟之前一致
lbg = 0.0
ubg = 0.0
lbx = optimizing_target(-ca.inf)
ubx = optimizing_target(ca.inf)
lbx['U', :, 'v'] = -v_max
lbx['U', :, 'omega'] = -omega_max
lbx['X', :, 'x'] = -2.0
lbx['X', :, 'y'] = -2.0
ubx['U', :, 'v'] = v_max
ubx['U', :, 'omega'] = omega_max
ubx['X', :, 'x'] = 2.0
ubx['X', :, 'y'] = 2.0
瞬間美觀很多。。。接下來就是循環了。
if __name__ == '__main__':
# Simulation
t0 = 0.0
x0 = np.array([0.0, 0.0, 0.0]).reshape(-1, 1)
x0_ = x0.copy()
xs = np.array([1.5, 1.5, np.pi/2.0]).reshape(-1, 1)
u0 = np.array([1,2]*N).reshape(-1, 2).T
ff_value = np.array([0.0, 0.0, 0.0]*(N+1)).reshape(-1, 3).T
x_c = []
u_c = []
t_c = []
xx = []
sim_time = 20.0
## 開始MPC
mpciter = 0
### 初始化變量
c_p = current_parameters(0)
init_control = optimizing_target(0)
start_time = time.time()
index_t = []
while(np.linalg.norm(x0-xs)>1e-2 and mpciter-sim_time/T<0.0 ):
### 設置每次循環的初始值
c_p['P'] = np.concatenate((x0, xs))
init_control['X', lambda x:ca.horzcat(*x)] = ff_value
init_control['U', lambda x:ca.horzcat(*x)] = u0[:, 0:N]
t_ = time.time()
res = solver(x0=init_control, p=c_p, lbg=lbg, lbx=lbx, ubg=ubg, ubx=ubx)
index_t.append(time.time()- t_)
### 主要的不同點是取出結果的時候。暫時不知道有沒有更好的解決辦法。
### 原則上還是需要知道其內部U和X是按照什麼順序定義的。
estimated_opt = res['x'].full()
### 最後三個結果是N+1的X
ff_last_ = estimated_opt[-3:]
### 之前一共有N個U和X
temp_estimated = estimated_opt[:-3].reshape(-1, 5)
u0 = temp_estimated[:, :2].T
ff_value = temp_estimated[:, 2:].T
### 將最後一個N+1接上前面的N個結果
ff_value = np.concatenate((ff_value, estimated_opt[-3:].reshape(3, 1)), axis=1)
x_c.append(ff_value)
u_c.append(u0[:, 0])
t_c.append(t0)
t0, x0, u0 = shift_movement(T, t0, x0, u0, f)
x0 = ca.reshape(x0, -1, 1)
xx.append(x0.full())
mpciter = mpciter + 1
這樣通過結構的方式就能定義好相同問題。方便的地方是對於多個維度變量的處理，相對較爲直觀，缺點是得了解內部排列的順序，否則得到的結果會亂套。速度上其和SX不分伯仲。
Opti
本節最後介紹的就是Opti方法。這個方法最像在寫數學表達式。
if __name__ == '__main__':
T = 0.2
N = 100
v_max = 0.6
omega_max = np.pi/4.0
# 創建一個opti對象
opti = ca.Opti()
# 創建並添加一個變量，這個變量就是優化的變量。
# 注意這裏我使用Nx2維度的表達，比較直觀
opt_controls = opti.variable(N, 2)
# 獲得單獨速度和角速度
v = opt_controls[:, 0]
omega = opt_controls[:, 1]
# 同理對於小車狀態
opt_states = opti.variable(N+1, 3)
x = opt_states[:, 0]
y = opt_states[:, 1]
theta = opt_states[:, 2]
# 添加兩個額外參數，每個opti參數在求解前必須初始化值
opt_x0 = opti.parameter(3)
opt_xs = opti.parameter(3)
# 用Python方式建立數學模型
f = lambda x_, u_: ca.vertcat(*[u_[0]*np.cos(x_[2]), u_[0]*np.sin(x_[2]), u_[1]])
# 這裏是爲了方便用numpy直接調用的版本
f_np = lambda x_, u_: np.array([u_[0]*np.cos(x_[2]), u_[0]*np.sin(x_[2]), u_[1]])
# 初始條件，..受約束於..跟公式一致
opti.subject_to(opt_states[0, :] == opt_x0.T)
for i in range(N):
x_next = opt_states[i, :] + f(opt_states[i, :], opt_controls[i, :]).T*T
opti.subject_to(opt_states[i+1, :]==x_next)
Q = np.array([[1.0, 0.0, 0.0],[0.0, 5.0, 0.0],[0.0, 0.0, .1]])
R = np.array([[0.5, 0.0], [0.0, 0.05]])
# 代價函數，注意index記法
obj = 0
for i in range(N):
obj = obj + ca.mtimes([(opt_states[i, :]-opt_xs.T), Q, (opt_states[i, :]-opt_xs.T).T]) + ca.mtimes([opt_controls[i, :], R, opt_controls[i, :].T])
# 純數學標識
opti.minimize(obj)
# 添加邊界條件
opti.subject_to(opti.bounded(-2.0, x, 2.0))
opti.subject_to(opti.bounded(-2.0, y, 2.0))
opti.subject_to(opti.bounded(-v_max, v, v_max))
opti.subject_to(opti.bounded(-omega_max, omega, omega_max))
# 同樣的設置
opts_setting = {'ipopt.max_iter':100, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6}
opti.solver('ipopt', opts_setting)
# 以下開始仿真
final_state = np.array([1.5, 1.5, 0.0])
## 設定opti參數的值
opti.set_value(opt_xs, final_state)
t0 = 0
init_state = np.array([0.0, 0.0, 0.0])
u0 = np.zeros((N, 2))
current_state = init_state.copy()
next_states = np.zeros((N+1, 3))
x_c = []
u_c = []
t_c = []
xx = []
sim_time = 20.0
## start MPC
mpciter = 0
start_time = time.time()
index_t = []
while(np.linalg.norm(current_state-final_state)>1e-2 and mpciter-sim_time/T<0.0 ):
### 設置x0也就是每一步小車當前的位置作爲參數
opti.set_value(opt_x0, current_state)
### 初始化優化對象的值，注意維度
opti.set_initial(opt_controls, u0)# (N, 2)
opti.set_initial(opt_states, next_states) # (N+1, 3)
t_ = time.time()
sol = opti.solve()
index_t.append(time.time()- t_)
### 獲得結果
u_res = sol.value(opt_controls)
u_c.append(u_res[0, :])
t_c.append(t0)
next_states_pred = sol.value(opt_states)#
x_c.append(next_states_pred)
# 爲下一個循環做準備
t0, current_state, u0, next_states = shift_movement(T, t0, current_state, u_res, next_states_pred, f_np)
mpciter = mpciter + 1
xx.append(current_state)
是不是感覺非常地直觀。缺點嘛就是速度和SX相比會慢一些，可能是調用較多其他API的緣故，但是不失爲初始模型測試的入手格式，或者對時間不敏感的，也可以使用。