Planning基礎庫——螺旋曲線平滑
Apollo ReferenceLine Smooth——Polynomial Spiral平滑原理一文中詳解介紹了apollo參考線螺旋曲線平滑的演算法原理，本文從程式碼角度介紹螺旋曲線平滑如何求解實現的。
求解器構造
螺旋參考線平滑器SpiralReferenceLineSmoother位置在modules/planning/reference_line/
http：//
spiral_reference_line_smoother。cc
中。
reference_line_provider會呼叫此演算法做參考線平滑。分為兩步，首先設定錨點SetAnchorPoints（），然後進行平滑Smooth（）
smoother_
->
SetAnchorPoints
（
anchor_points
）；
if
（
！
smoother_
->
Smooth
（
raw_ref
，
reference_line
））
{
AERROR
<<
“Failed to smooth prefixed reference line with anchor points”
；
return
false
；
}
錨點的選取是從原始參考線產生的等間隔點，間隔定義在max_constraint_interval
void
ReferenceLineProvider
：：
GetAnchorPoints
（
const
ReferenceLine
&
reference_line
，
std
：：
vector
<
AnchorPoint
>
*
anchor_points
）
const
{
CHECK_NOTNULL
（
anchor_points
）；
const
double
interval
=
smoother_config_
。
max_constraint_interval
（）；
int
num_of_anchors
=
std
：：
max
（
2
，
static_cast
<
int
>
（
reference_line
。
Length
（）
/
interval
+
0。5
））；
std
：：
vector
<
double
>
anchor_s
；
common
：：
util
：：
uniform_slice
（
0。0
，
reference_line
。
Length
（），
num_of_anchors
-
1
，
&
anchor_s
）；
for
（
const
double
s
：
anchor_s
）
{
AnchorPoint
anchor
=
GetAnchorPoint
（
reference_line
，
s
）；
anchor_points
->
emplace_back
（
anchor
）；
}
anchor_points
->
front
（）。
longitudinal_bound
=
1e-6
；
anchor_points
->
front
（）。
lateral_bound
=
1e-6
；
anchor_points
->
front
（）。
enforced
=
true
；
anchor_points
->
back
（）。
longitudinal_bound
=
1e-6
；
anchor_points
->
back
（）。
lateral_bound
=
1e-6
；
anchor_points
->
back
（）。
enforced
=
true
；
}
生成的錨點如下圖Pi所示
然後呼叫Smooth（）函式對參考線平滑
//raw_reference_line為原始參考線，即地圖中lane的節點產生的參考線
//smoothed_reference_line為平滑後的參考線作為輸出
bool
Smooth
（
const
ReferenceLine
&
raw_reference_line
，
ReferenceLine
*
const
smoothed_reference_line
）
override
；
Smooth函式中會用到ipopt進行非線性規劃，產生一個ipopt問題介面類，設定求解器引數
//定義一個新的求解器例項
SpiralProblemInterface
*
ptop
=
new
SpiralProblemInterface
（
point2d
）；
//設定距離平滑點距離錨點的最大偏移距離
ptop
->
set_default_max_point_deviation
（
config_
。
spiral
（）。
max_deviation
（））；
//設定起始點引數，apollo中起始點位置不能被平滑，因此均設定為固定點
if
（
fixed_start_point_
）
{
ptop
->
set_start_point
（
fixed_start_x_
，
fixed_start_y_
，
fixed_start_theta_
，
fixed_start_kappa_
，
fixed_start_dkappa_
）；
}
//設定終止點引數
ptop
->
set_end_point_position
（
fixed_end_x_
，
fixed_end_y_
）；
//設定曲線弧長權重
ptop
->
set_element_weight_curve_length
（
config_
。
spiral
（）。
weight_curve_length
（））；
//設定曲率權重
ptop
->
set_element_weight_kappa
（
config_
。
spiral
（）。
weight_kappa
（））；
//設定曲率變化率權重
ptop
->
set_element_weight_dkappa
（
config_
。
spiral
（）。
weight_dkappa
（））；
//建立一個tnlp的求解器指標
Ipopt
：：
SmartPtr
<
Ipopt
：：
TNLP
>
problem
=
ptop
；
// Create an instance of the IpoptApplication
Ipopt
：：
SmartPtr
<
Ipopt
：：
IpoptApplication
>
app
=
IpoptApplicationFactory
（）；
//設定求解器引數
//透過擬牛頓法近似黑塞矩陣
app
->
Options
（）
->
SetStringValue
（
“hessian_approximation”
，
“limited-memory”
）；
app
->
Options
（）
->
SetIntegerValue
（
“print_level”
，
0
）；
//最大迭代次數，達到迭代次數後，演算法停止最佳化
app
->
Options
（）
->
SetIntegerValue
（
“max_iter”
，
config_
。
spiral
（）。
max_iteration
（））；
//可接受的迭代次數，當演算法沒達到收斂誤差tol，但是達到可接受的收斂誤差acceptable_tol acceptable_iter次數以後，
//演算法也會成功退出
app
->
Options
（）
->
SetIntegerValue
（
“acceptable_iter”
，
config_
。
spiral
（）。
opt_acceptable_iteration
（））；
//收斂誤差，滿足該誤差下演算法即停止最佳化，返回成功求解
app
->
Options
（）
->
SetNumericValue
（
“tol”
，
config_
。
spiral
（）。
opt_tol
（））；
//可接受的收斂誤差
app
->
Options
（）
->
SetNumericValue
（
“acceptable_tol”
，
config_
。
spiral
（）。
opt_acceptable_tol
（））；
//初始化求解器引數
Ipopt
：：
ApplicationReturnStatus
status
=
app
->
Initialize
（）；
if
（
status
！=
Ipopt
：：
Solve_Succeeded
）
{
ADEBUG
<<
“*** Error during initialization！”
；
return
false
；
}
//最佳化計算
status
=
app
->
OptimizeTNLP
（
problem
）；
Ipopt是一個基於內點法求解非線性最佳化問題的函式庫，對於Ipopt最佳化，需要在介面類中實現一些待求解的函式。可以參見modules/planning/reference_line/spiral_problem_interface。h，需要提供的資訊包括：
定義問題規模
變數的數量
約束的數量
定義問題邊界
最佳化變數的邊界
約束的邊界
定義初始值
最佳化變數的初始值
定義問題函式
定義目標函式
定義目標函式導數
定義約束函式
定義約束函式雅克比矩陣
定義拉格朗日函式的黑塞矩陣
我們回顧一下最佳化問題
我們最佳化的變數是原始參考點的幾種狀態值：
目標函式包括了對曲線長度、曲率和曲率變化率的懲罰
約束條件包括了最佳化變數的約束：
以及約束方程
定義問題規模
首先定義問題規模，實現get_nlp_info函式。
注意這裡變數的順序為
bool
SpiralProblemInterface
：：
get_nlp_info
（
int
&
n
，
int
&
m
，
int
&
nnz_jac_g
，
int
&
nnz_h_lag
，
IndexStyleEnum
&
index_style
）
{
// n定義了變數個數，由於每個點包含了5個最佳化變數和1個兩點之間弧長的變數，所以：
n
=
num_of_points_
*
5
+
num_of_points_
-
1
；
num_of_variables_
=
n
；
// m定義了約束的數量，除起始點外，每個點包含x，y的兩個約束和每個點的位置平移約束
m
=
（
num_of_points_
-
1
）
*
2
；
//x，y的約束，不包括第一個點
m
+=
num_of_points_
；
//位置平移約束
}
定義問題邊界
定義問題的變數上下邊界和約束的上下邊界get_bounds_info（）。
bool
SpiralProblemInterface
：：
get_bounds_info
（
int
n
，
double
*
x_l
，
double
*
x_u
，
int
m
，
double
*
g_l
，
double
*
g_u
）
{
//x_l，x_u定義了最佳化變數的下邊界和上邊界
//g_l，g_u定義了約束的下邊界和上邊界
//m為約束的數量，n為變數的數量
}
對於第一個點最佳化變數約束為等式約束，所以其上下界均為本身
if
（
i
==
0
&&
has_fixed_start_point_
）
{
theta_lower
=
start_theta_
；
theta_upper
=
start_theta_
；
kappa_lower
=
start_kappa_
；
kappa_upper
=
start_kappa_
；
dkappa_lower
=
start_dkappa_
；
dkappa_upper
=
start_dkappa_
；
x_lower
=
start_x_
；
x_upper
=
start_x_
；
y_lower
=
start_y_
；
y_upper
=
start_y_
；
}
else
if
（
i
+
1
==
num_of_points_
&&
has_fixed_end_point_
）
{
theta_lower
=
end_theta_
；
theta_upper
=
end_theta_
；
kappa_lower
=
end_kappa_
；
kappa_upper
=
end_kappa_
；
dkappa_lower
=
end_dkappa_
；
dkappa_upper
=
end_dkappa_
；
x_lower
=
end_x_
；
x_upper
=
end_x_
；
y_lower
=
end_y_
；
y_upper
=
end_y_
；
}
對於約束方程，可以轉化為g（x）=0的形式，其上下邊界也均為0
for
（
int
i
=
0
；
i
+
1
<
num_of_points_
；
++
i
）
{
// for x
g_l
［
i
*
2
］
=
0。0
；
g_u
［
i
*
2
］
=
0。0
；
// for y
g_l
［
i
*
2
+
1
］
=
0。0
；
g_u
［
i
*
2
+
1
］
=
0。0
；
}
定義初始值
還需要給出最佳化量的初始值，初始值為原始參考線點的狀態
bool
get_starting_point
（
int
n
，
bool
init_x
，
double
*
x
，
bool
init_z
，
double
*
z_L
，
double
*
z_U
，
int
m
，
bool
init_lambda
，
double
*
lambda
）
override
；
非第一個點的曲率變化率定義為0，第一個點的曲率、曲率變化率根據實際給出
for
（
int
i
=
0
；
i
<
num_of_points_
；
++
i
）
{
int
index
=
i
*
5
；
x
［
index
］
=
relative_theta_
［
i
］；
x
［
index
+
1
］
=
0。0
；
x
［
index
+
2
］
=
0。0
；
x
［
index
+
3
］
=
init_points_
［
i
］。
x
（）；
x
［
index
+
4
］
=
init_points_
［
i
］。
y
（）；
}
if
（
has_fixed_start_point_
）
{
x
［
0
］
=
start_theta_
；
x
［
1
］
=
start_kappa_
；
x
［
2
］
=
start_dkappa_
；
}
弧長的計算，沒太看懂為什麼除以cos（0。5△ θ），瞭解的朋友可以指點下，不過不影響求解
int
variable_offset
=
num_of_points_
*
5
；
for
（
int
i
=
0
；
i
+
1
<
num_of_points_
；
++
i
）
{
double
delta_theta
=
relative_theta_
［
i
+
1
］
-
relative_theta_
［
i
］；
x
［
variable_offset
+
i
］
=
point_distances_
［
i
］
/
std
：：
cos
（
0。5
*
delta_theta
）；
}
非第一個點的曲率計算，用弧長/角度變化：
for
（
int
i
=
0
；
i
+
1
<
num_of_points_
；
++
i
）
{
double
delta_theta
=
relative_theta_
［
i
+
1
］
-
relative_theta_
［
i
］；
x
［（
i
+
1
）
*
5
+
1
］
=
delta_theta
/
x
［
variable_offset
+
i
］；
}
定義目標函式
目標函式可以在eval_f中實現：
bool
SpiralProblemInterface
：：
eval_grad_f
（
int
n
，
const
double
*
x
，
bool
new_x
，
double
*
grad_f
）
其中new_x會反饋出求解器是否更新了最佳化變數，如果更新了最佳化變數，我們就要重新更新螺旋曲線函式，利用新的最佳化變數構造螺旋曲線
bool
SpiralProblemInterface
：：
eval_grad_f
（
int
n
，
const
double
*
x
，
bool
new_x
，
double
*
grad_f
）
{
if
（
new_x
）
{
update_piecewise_spiral_paths
（
x
，
n
）；
}
。。。
}
void
SpiralProblemInterface
：：
update_piecewise_spiral_paths
（
const
double
*
x
，
const
int
n
）
{
int
variable_offset
=
num_of_points_
*
5
；
for
（
int
i
=
0
；
i
+
1
<
num_of_points_
；
++
i
）
{
int
index0
=
i
*
5
；
int
index1
=
（
i
+
1
）
*
5
；
std
：：
array
<
double
，
3
>
x0
=
{
x
［
index0
］，
x
［
index0
+
1
］，
x
［
index0
+
2
］}；
std
：：
array
<
double
，
3
>
x1
=
{
x
［
index1
］，
x
［
index1
+
1
］，
x
［
index1
+
2
］}；
double
delta_s
=
x
［
variable_offset
+
i
］；
piecewise_paths_
［
i
］
=
std
：：
move
（
QuinticSpiralPathWithDerivation
<
N
>
（
x0
，
x1
，
delta_s
））；
}
}
分別求出每一部分的權重值
obj_value
=
0。0
；
for
（
int
i
=
0
；
i
+
1
<
num_of_points_
；
++
i
）
{
const
auto
&
spiral_curve
=
piecewise_paths_
［
i
］；
double
delta_s
=
spiral_curve
。
ParamLength
（）；
obj_value
+=
delta_s
*
weight_curve_length_
；
//這裡對每兩個節點之間又均分出了5個內部節點，分別將內部節點的曲率和曲率變化率加權求和
for
（
int
j
=
0
；
j
<
num_of_internal_points_
；
++
j
）
{
double
ratio
=
static_cast
<
double
>
（
j
）
/
static_cast
<
double
>
（
num_of_internal_points_
）；
//內部節點在曲線的弧長
double
s
=
ratio
*
delta_s
；
//曲率加權和
double
kappa
=
spiral_curve
。
Evaluate
（
1
，
s
）；
obj_value
+=
kappa
*
kappa
*
weight_kappa_
；
//曲率變化率加權和
double
dkappa
=
spiral_curve
。
Evaluate
（
2
，
s
）；
obj_value
+=
dkappa
*
dkappa
*
weight_dkappa_
；
}
}
return
true
；
}
定義目標函式梯度
目標函式的梯度就是目標函式對於每一個最佳化變數的偏導數，在eval_grad_f（）函式中實現
和定義目標函式一樣，梯度函式也需要首先更新螺旋曲線：
bool
SpiralProblemInterface
：：
eval_grad_f
（
int
n
，
const
double
*
x
，
bool
new_x
，
double
*
grad_f
）
{
CHECK_EQ
（
n
，
num_of_variables_
）；
std
：：
fill
（
grad_f
，
grad_f
+
n
，
0。0
）；
if
（
new_x
）
{
update_piecewise_spiral_paths
（
x
，
n
）；
}
。。。
}
目標函式的梯度我們可以構造為：
這個求偏導的函式是不是看著很頭大，沒關係我們可以拆解開來進行推導，首先我們看對Δsi的求偏導，因為Δsi只和第i個螺旋曲線有關，和其他曲線無關，所以目標函式的其他曲線相關量求導可以直接忽略掉了
θ‘（s）和θ’‘（s）對Δsi求導是不是很熟悉，我們在Planning 基礎庫——螺旋曲線類講過對螺旋曲線求最佳化變數偏導的方法，這裡我們可以直接呼叫螺旋曲線求偏導函式
grad_f
［
variable_offset
+
i
］
+=
weight_curve_length_
*
1。0
；
。。。
grad_f
［
variable_offset
+
i
］
+=
weight_kappa_
*
2。0
*
kappa
*
spiral_curve
。
DeriveKappaDerivative
（
DELTA_S
，
j
，
num_of_internal_points_
）；
。。。
grad_f
［
variable_offset
+
i
］
+=
weight_dkappa_
*
2。0
*
dkappa
*
spiral_curve
。
DeriveDKappaDerivative
（
DELTA_S
，
j
，
num_of_internal_points_
）；
下面我們分析一下對θi求偏導，θi是曲線端點的方向角，和第i-1個曲線和第i個曲線都有關係，所以它的偏導函式為
同理可以推匯出θi’，θii‘’，xi，yi的偏導函式，直接呼叫螺旋曲線偏導函式
for
（
int
j
=
0
；
j
<
num_of_internal_points_
；
++
j
）
{
double
ratio
=
static_cast
<
double
>
（
j
）
/
static_cast
<
double
>
（
num_of_internal_points_
）；
double
s
=
ratio
*
delta_s
；
double
kappa
=
spiral_curve
。
Evaluate
（
1
，
s
）；
grad_f
［
index0
］
+=
weight_kappa_
*
2。0
*
kappa
*
spiral_curve
。
DeriveKappaDerivative
（
THETA0
，
j
，
num_of_internal_points_
）；
grad_f
［
index0
+
1
］
+=
weight_kappa_
*
2。0
*
kappa
*
spiral_curve
。
DeriveKappaDerivative
（
KAPPA0
，
j
，
num_of_internal_points_
）；
grad_f
［
index0
+
2
］
+=
weight_kappa_
*
2。0
*
kappa
*
spiral_curve
。
DeriveKappaDerivative
（
DKAPPA0
，
j
，
num_of_internal_points_
）；
grad_f
［
index1
］
+=
weight_kappa_
*
2。0
*
kappa
*
spiral_curve
。
DeriveKappaDerivative
（
THETA1
，
j
，
num_of_internal_points_
）；
grad_f
［
index1
+
1
］
+=
weight_kappa_
*
2。0
*
kappa
*
spiral_curve
。
DeriveKappaDerivative
（
KAPPA1
，
j
，
num_of_internal_points_
）；
grad_f
［
index1
+
2
］
+=
weight_kappa_
*
2。0
*
kappa
*
spiral_curve
。
DeriveKappaDerivative
（
DKAPPA1
，
j
，
num_of_internal_points_
）；
grad_f
［
variable_offset
+
i
］
+=
weight_kappa_
*
2。0
*
kappa
*
spiral_curve
。
DeriveKappaDerivative
（
DELTA_S
，
j
，
num_of_internal_points_
）；
double
dkappa
=
spiral_curve
。
Evaluate
（
2
，
s
）；
grad_f
［
index0
］
+=
weight_dkappa_
*
2。0
*
dkappa
*
spiral_curve
。
DeriveDKappaDerivative
（
THETA0
，
j
，
num_of_internal_points_
）；
grad_f
［
index0
+
1
］
+=
weight_dkappa_
*
2。0
*
dkappa
*
spiral_curve
。
DeriveDKappaDerivative
（
KAPPA0
，
j
，
num_of_internal_points_
）；
grad_f
［
index0
+
2
］
+=
weight_dkappa_
*
2。0
*
dkappa
*
spiral_curve
。
DeriveDKappaDerivative
（
DKAPPA0
，
j
，
num_of_internal_points_
）；
grad_f
［
index1
］
+=
weight_dkappa_
*
2。0
*
dkappa
*
spiral_curve
。
DeriveDKappaDerivative
（
THETA1
，
j
，
num_of_internal_points_
）；
grad_f
［
index1
+
1
］
+=
weight_dkappa_
*
2。0
*
dkappa
*
spiral_curve
。
DeriveDKappaDerivative
（
KAPPA1
，
j
，
num_of_internal_points_
）；
grad_f
［
index1
+
2
］
+=
weight_dkappa_
*
2。0
*
dkappa
*
spiral_curve
。
DeriveDKappaDerivative
（
DKAPPA1
，
j
，
num_of_internal_points_
）；
grad_f
［
variable_offset
+
i
］
+=
weight_dkappa_
*
2。0
*
dkappa
*
spiral_curve
。
DeriveDKappaDerivative
（
DELTA_S
，
j
，
num_of_internal_points_
）；
}
定義約束函式
約束函式的定義在eval_g（）中實現，其中x為最佳化變數，g為約束函式返回值。
和目標函式一樣，對於新的最佳化變數，約束函式首先更新螺旋曲線：
bool
SpiralProblemInterface
：：
eval_g
（
int
n
，
const
double
*
x
，
bool
new_x
，
int
m
，
double
*
g
）
{
CHECK_EQ
（
n
，
num_of_variables_
）；
CHECK_EQ
（
m
，
num_of_constraints_
）；
if
（
new_x
）
{
update_piecewise_spiral_paths
（
x
，
n
）；
}
。。。
}
對於我們的最佳化問題，主要包括三個約束條件
針對前兩個約束函式，Planning 基礎庫——螺旋曲線類我們講過從螺旋曲線計算笛卡爾座標系的方法，這裡直接呼叫函式ComputeCartesianDeviationX（）計算：
for
（
int
i
=
0
；
i
+
1
<
num_of_points_
；
++
i
）
{
int
index0
=
i
*
5
；
int
index1
=
（
i
+
1
）
*
5
；
const
auto
&
spiral_curve
=
piecewise_paths_
［
i
］；
double
delta_s
=
spiral_curve
。
ParamLength
（）；
double
x_diff
=
x
［
index1
+
3
］
-
x
［
index0
+
3
］
-
spiral_curve
。
ComputeCartesianDeviationX
（
delta_s
）；
//不知道這裡apollo為什麼加入了平方計算，這裡是一個等於0的約束，有和沒有平方應該一樣的，有了解的可以指出來
g
［
i
*
2
］
=
x_diff
*
x_diff
；
double
y_diff
=
x
［
index1
+
4
］
-
x
［
index0
+
4
］
-
spiral_curve
。
ComputeCartesianDeviationY
（
delta_s
）；
g
［
i
*
2
+
1
］
=
y_diff
*
y_diff
；
}
// second， fill in the positional deviation constraints
int
constraint_offset
=
2
*
（
num_of_points_
-
1
）；
for
（
int
i
=
0
；
i
<
num_of_points_
；
++
i
）
{
int
variable_index
=
i
*
5
；
double
x_cor
=
x
［
variable_index
+
3
］；
double
y_cor
=
x
［
variable_index
+
4
］；
double
x_diff
=
x_cor
-
init_points_
［
i
］。
x
（）；
double
y_diff
=
y_cor
-
init_points_
［
i
］。
y
（）；
g
［
constraint_offset
+
i
］
=
x_diff
*
x_diff
+
y_diff
*
y_diff
；
}
定義約束函式雅克比矩陣
約束函式雅克比矩陣的定義在eval_jac_g函式中實現。
需要注意的是，函式中雅克比矩陣是透過稀疏矩陣形式呼叫的，
其中iRow，jCol分別表示雅克比矩陣非0項的行號和列號的陣列
nele_jac是雅克比矩陣非0項的個數
value為雅克比矩陣非0項的值
當求解器第一次呼叫該函式時，values為空指標，此時需要定義矩陣中非恆為0項的iRow，iCol。
之後求解器呼叫該函式，values將始終有指標，此時求解雅克比矩陣非0項的值
bool
SpiralProblemInterface
：：
eval_jac_g
（
int
n
，
const
double
*
x
，
bool
new_x
，
int
m
，
int
nele_jac
，
int
*
iRow
，
int
*
jCol
，
double
*
values
）
{
。。。
}
gij表示對於第j條曲線的第i個約束方程，其中非0項是比較少的，g1i僅對θ（i），θ‘（i），θ’‘（i），x（i），Δsi，θ（i+1），θ’（i+1），θ‘’（i+1），x（i+1）求導非0，g2i僅對θ（i），θ‘（i），θ’‘（i），y（i），Δsi，θ（i+1），θ’（i+1），θ‘’（i+1），y（i+1）求導非0，g3i僅對xi，yi求導非0，則有
if
（
values
==
nullptr
）
{
int
nz_index
=
0
；
int
variable_offset
=
num_of_points_
*
5
；
for
（
int
i
=
0
；
i
+
1
<
num_of_points_
；
++
i
）
{
int
variable_index
=
i
*
5
；
// theta0
iRow
［
nz_index
］
=
i
*
2
；
jCol
［
nz_index
］
=
variable_index
+
0
；
++
nz_index
；
// kappa0
iRow
［
nz_index
］
=
i
*
2
；
jCol
［
nz_index
］
=
variable_index
+
1
；
++
nz_index
；
// dkappa0
iRow
［
nz_index
］
=
i
*
2
；
jCol
［
nz_index
］
=
variable_index
+
2
；
++
nz_index
；
// x0
iRow
［
nz_index
］
=
i
*
2
；
jCol
［
nz_index
］
=
variable_index
+
3
；
++
nz_index
；
// theta1
iRow
［
nz_index
］
=
i
*
2
；
jCol
［
nz_index
］
=
variable_index
+
5
；
++
nz_index
；
// kappa1
iRow
［
nz_index
］
=
i
*
2
；
jCol
［
nz_index
］
=
variable_index
+
6
；
++
nz_index
；
// dkappa1
iRow
［
nz_index
］
=
i
*
2
；
jCol
［
nz_index
］
=
variable_index
+
7
；
++
nz_index
；
// x1
iRow
［
nz_index
］
=
i
*
2
；
jCol
［
nz_index
］
=
variable_index
+
8
；
++
nz_index
；
// s
iRow
［
nz_index
］
=
i
*
2
；
jCol
［
nz_index
］
=
variable_offset
+
i
；
++
nz_index
；
// theta0
iRow
［
nz_index
］
=
i
*
2
+
1
；
jCol
［
nz_index
］
=
variable_index
+
0
；
++
nz_index
；
// kappa0
iRow
［
nz_index
］
=
i
*
2
+
1
；
jCol
［
nz_index
］
=
variable_index
+
1
；
++
nz_index
；
// dkappa0
iRow
［
nz_index
］
=
i
*
2
+
1
；
jCol
［
nz_index
］
=
variable_index
+
2
；
++
nz_index
；
// y0
iRow
［
nz_index
］
=
i
*
2
+
1
；
jCol
［
nz_index
］
=
variable_index
+
4
；
++
nz_index
；
// theta1
iRow
［
nz_index
］
=
i
*
2
+
1
；
jCol
［
nz_index
］
=
variable_index
+
5
；
++
nz_index
；
// kappa1
iRow
［
nz_index
］
=
i
*
2
+
1
；
jCol
［
nz_index
］
=
variable_index
+
6
；
++
nz_index
；
// dkappa1
iRow
［
nz_index
］
=
i
*
2
+
1
；
jCol
［
nz_index
］
=
variable_index
+
7
；
++
nz_index
；
// y1
iRow
［
nz_index
］
=
i
*
2
+
1
；
jCol
［
nz_index
］
=
variable_index
+
9
；
++
nz_index
；
// s
iRow
［
nz_index
］
=
i
*
2
+
1
；
jCol
［
nz_index
］
=
variable_offset
+
i
；
++
nz_index
；
}
int
constraint_offset
=
2
*
（
num_of_points_
-
1
）；
for
（
int
i
=
0
；
i
<
num_of_points_
；
++
i
）
{
iRow
［
nz_index
］
=
constraint_offset
+
i
；
jCol
［
nz_index
］
=
i
*
5
+
3
；
++
nz_index
；
iRow
［
nz_index
］
=
constraint_offset
+
i
；
jCol
［
nz_index
］
=
i
*
5
+
4
；
++
nz_index
；
}
CHECK_EQ
（
nz_index
，
nele_jac
）；
apollo中也是對約束函式的實現加入了平方處理：
前一項是兩個點x方向距離差，我們上一篇文章講了，透過螺旋曲線類函式ComputeCartesianDeviationX求解，後一項的求導可以透過類函式DeriveCartesianDeviation（THETA0）來求解，g2i同理
g3i僅對xi和yi求導，以xi舉例：
雅克比矩陣的計算：
for
（
int
i
=
0
；
i
+
1
<
num_of_points_
；
++
i
）
{
int
index0
=
i
*
5
；
int
index1
=
（
i
+
1
）
*
5
；
auto
&
spiral_curve
=
piecewise_paths_
［
i
］；
double
delta_s
=
spiral_curve
。
ParamLength
（）；
double
x_diff
=
x
［
index1
+
3
］
-
x
［
index0
+
3
］
-
spiral_curve
。
ComputeCartesianDeviationX
（
delta_s
）；
double
y_diff
=
x
［
index1
+
4
］
-
x
［
index0
+
4
］
-
spiral_curve
。
ComputeCartesianDeviationY
（
delta_s
）；
auto
pos_theta0
=
spiral_curve
。
DeriveCartesianDeviation
（
THETA0
）；
auto
pos_kappa0
=
spiral_curve
。
DeriveCartesianDeviation
（
KAPPA0
）；
auto
pos_dkappa0
=
spiral_curve
。
DeriveCartesianDeviation
（
DKAPPA0
）；
auto
pos_theta1
=
spiral_curve
。
DeriveCartesianDeviation
（
THETA1
）；
auto
pos_kappa1
=
spiral_curve
。
DeriveCartesianDeviation
（
KAPPA1
）；
auto
pos_dkappa1
=
spiral_curve
。
DeriveCartesianDeviation
（
DKAPPA1
）；
auto
pos_delta_s
=
spiral_curve
。
DeriveCartesianDeviation
（
DELTA_S
）；
// for x coordinate
// theta0
values
［
nz_index
］
+=
2。0
*
x_diff
*
（
-
pos_theta0
。
first
）；
++
nz_index
；
// kappa0
values
［
nz_index
］
+=
2。0
*
x_diff
*
（
-
pos_kappa0
。
first
）；
++
nz_index
；
// dkappa0
values
［
nz_index
］
+=
2。0
*
x_diff
*
（
-
pos_dkappa0
。
first
）；
++
nz_index
；
// x0
values
［
nz_index
］
+=
2。0
*
x_diff
*
（
-
1。0
）；
++
nz_index
；
// theta1
values
［
nz_index
］
+=
2。0
*
x_diff
*
（
-
pos_theta1
。
first
）；
++
nz_index
；
// kappa1
values
［
nz_index
］
+=
2。0
*
x_diff
*
（
-
pos_kappa1
。
first
）；
++
nz_index
；
// dkappa1
values
［
nz_index
］
+=
2。0
*
x_diff
*
（
-
pos_dkappa1
。
first
）；
++
nz_index
；
// x1
values
［
nz_index
］
+=
2。0
*
x_diff
；
++
nz_index
；
// delta_s
values
［
nz_index
］
+=
2。0
*
x_diff
*
（
-
pos_delta_s
。
first
）；
++
nz_index
；
// for y coordinate
// theta0
values
［
nz_index
］
+=
2。0
*
y_diff
*
（
-
pos_theta0
。
second
）；
++
nz_index
；
// kappa0
values
［
nz_index
］
+=
2。0
*
y_diff
*
（
-
pos_kappa0
。
second
）；
++
nz_index
；
// dkappa0
values
［
nz_index
］
+=
2。0
*
y_diff
*
（
-
pos_dkappa0
。
second
）；
++
nz_index
；
// y0
values
［
nz_index
］
+=
2。0
*
y_diff
*
（
-
1。0
）；
++
nz_index
；
// theta1
values
［
nz_index
］
+=
2。0
*
y_diff
*
（
-
pos_theta1
。
second
）；
++
nz_index
；
// kappa1
values
［
nz_index
］
+=
2。0
*
y_diff
*
（
-
pos_kappa1
。
second
）；
++
nz_index
；
// dkappa1
values
［
nz_index
］
+=
2。0
*
y_diff
*
（
-
pos_dkappa1
。
second
）；
++
nz_index
；
// y1
values
［
nz_index
］
+=
2。0
*
y_diff
；
++
nz_index
；
// delta_s
values
［
nz_index
］
+=
2。0
*
y_diff
*
（
-
pos_delta_s
。
second
）；
++
nz_index
；
}
for
（
int
i
=
0
；
i
<
num_of_points_
；
++
i
）
{
values
［
nz_index
］
=
2。0
*
（
x
［
i
*
5
+
3
］
-
init_points_
［
i
］。
x
（））；
++
nz_index
；
values
［
nz_index
］
=
2。0
*
（
x
［
i
*
5
+
4
］
-
init_points_
［
i
］。
y
（））；
++
nz_index
；
}
定義黑塞矩陣
這裡我們設定了呼叫ipopt的擬牛頓法近似求解二階偏導數，所以函式eval_h（）這裡無需實現
app
->
Options
（）
->
SetStringValue
（
“hessian_approximation”
，
“limited-memory”
）；
。。。
bool
SpiralProblemInterface
：：
eval_h
（
int
n
，
const
double
*
x
，
bool
new_x
，
double
obj_factor
，
int
m
，
const
double
*
lambda
，
bool
new_lambda
，
int
nele_hess
，
int
*
iRow
，
int
*
jCol
，
double
*
values
）
{
ACHECK
（
false
）；
return
true
；
}
最後獲取最佳化結果轉存下來：
void
SpiralProblemInterface
：：
finalize_solution
（
Ipopt
：：
SolverReturn
status
，
int
n
，
const
double
*
x
，
const
double
*
z_L
，
const
double
*
z_U
，
int
m
，
const
double
*
g
，
const
double
*
lambda
，
double
obj_value
，
const
Ipopt
：：
IpoptData
*
ip_data
，
Ipopt
：：
IpoptCalculatedQuantities
*
ip_cq
）
{
opt_theta_
。
reserve
（
num_of_points_
）；
opt_kappa_
。
reserve
（
num_of_points_
）；
opt_dkappa_
。
reserve
（
num_of_points_
）；
opt_x_
。
reserve
（
num_of_points_
）；
opt_y_
。
reserve
（
num_of_points_
）；
for
（
int
i
=
0
；
i
<
num_of_points_
；
++
i
）
{
int
index
=
i
*
5
；
opt_theta_
。
push_back
（
x
［
index
］）；
opt_kappa_
。
push_back
（
x
［
index
+
1
］）；
opt_dkappa_
。
push_back
（
x
［
index
+
2
］）；
opt_x_
。
push_back
（
x
［
index
+
3
］）；
opt_y_
。
push_back
（
x
［
index
+
4
］）；
}
opt_s_
。
reserve
（
num_of_points_
-
1
）；
int
variable_offset
=
num_of_points_
*
5
；
for
（
int
i
=
0
；
i
+
1
<
num_of_points_
；
++
i
）
{
opt_s_
。
push_back
（
x
［
variable_offset
+
i
］）；
}
}
Ipopt： Ipopt Options
蕭瀟子：Apollo ReferenceLine Smooth——Polynomial Spiral平滑原理
https：//
github。com/ApolloAuto/a
pollo/blob/master/modules/planning/reference_line/spiral_reference_line_smoother。h
上一篇:微胖的男生穿漢服好看嗎?
