前言
课程的第一个作业是输入起始点和终止点，通过求解优化问题来生成路径平滑。与常规的搜索算法，采样算法的原理完全不同，却也能得到一条比较好的路径。
主节点
主要用来初始化节点和接收话题消息。当存储 ‘/move_base_simple/goal’ 消息的容器满2个时，则进入处理：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
| if (startGoal.size() == 2)
{
const int N = (startGoal.back() - startGoal.front()).norm() / config.pieceLength;
Eigen::Matrix2Xd innerPoints(2, N - 1);
for (int i = 0; i < N - 1; ++i)
{
innerPoints.col(i) = (startGoal.back() - startGoal.front()) * (i + 1.0) / N + startGoal.front();
}
path_smoother::PathSmoother pathSmoother;
pathSmoother.setup(startGoal.front(), startGoal.back(), N, config.circleObs, config.penaltyWeight);
CubicCurve curve;
if (std::isinf(pathSmoother.optimize(curve, innerPoints, config.relCostTol)))
{
return;
}
if (curve.getPieceNum() > 0)
{
visualizer.visualize(curve);
}
}
|
路径平滑器
结构体
这个结构体是实现优化过程的部分，通过设置代价函数，并实现优化，结构如下：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
| class PathSmoother
{
private:
cubic_spline::CubicSpline cubSpline;
int pieceN;
Eigen::Matrix3Xd diskObstacles;
double penaltyWeight;
Eigen::Vector2d headP;
Eigen::Vector2d tailP;
Eigen::Matrix2Xd points;
Eigen::Matrix2Xd gradByPoints;
lbfgs::lbfgs_parameter_t lbfgs_params;
private:
static inline double costFunction(void *ptr,
const Eigen::VectorXd &x,
Eigen::VectorXd &g)；
public:
inline bool setup(const Eigen::Vector2d &initialP,
const Eigen::Vector2d &terminalP,
const int &pieceNum,
const Eigen::Matrix3Xd &diskObs,
const double penaWeight)；
inline double optimize(CubicCurve &curve,
const Eigen::Matrix2Xd &iniInPs,
const double &relCostTol)；
};
}
|
代价函数
首先，作业完成的代价函数。
能量代价将在三次样条插值部分介绍。在这里先介绍障碍物代价，工程中的障碍物只由圆形组成，在diskObstacles分别存储圆心x，圆心y，半径信息。代价函数表示为：
梯度也比较好求，分别为
具体实现如下：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
| static inline double costFunction(void *ptr,
const Eigen::VectorXd &x,
Eigen::VectorXd &g)
{
auto smooth_ptr = reinterpret_cast< PathSmoother*>( ptr) ;
int nums = smooth_ptr->pieceN-1;
double energy_cost = 0 , obstacle_cost = 0;
Eigen::Matrix2Xd grad = Eigen::Matrix2Xd::Zero(2, nums);
Eigen::Matrix2Xd energy_grad = Eigen::Matrix2Xd::Zero(2, nums);
Eigen::Matrix2Xd obstacle_grad = Eigen::Matrix2Xd::Zero(2, nums);
Eigen::Matrix2Xd inPs = Eigen::Matrix2Xd::Zero(2, nums);
inPs.row(0) = x.head(nums);
inPs.row(1) = x.tail(nums);
smooth_ptr->cubSpline.setInnerPoints(inPs);
smooth_ptr->cubSpline.getStretchEnergy(energy_cost);
smooth_ptr->cubSpline.getGrad(energy_grad);
for(int i = 0 ; i < nums ; ++i)
for(int j = 0 ; j < smooth_ptr->diskObstacles.cols() ;++j )
{
Eigen::Vector2d diff = inPs.col(i) - smooth_ptr->diskObstacles.col(j).head(2);
double dis = diff.norm();
double delta = smooth_ptr->diskObstacles(2,j) -dis ;
if(delta > 0)
{
obstacle_cost+= smooth_ptr->penaltyWeight * delta;
obstacle_grad.col(i) += - smooth_ptr->penaltyWeight * diff /dis ;
}
}
double cost = obstacle_cost+energy_cost;
grad = obstacle_grad + energy_grad;
g.setZero();
g.head(nums) = grad.row(0).transpose();
g.tail(nums) = grad.row(1).transpose();
return cost;
}
|
优化调用
我注意到这个函数是static 函数，这个函数的用法很奇怪，等会和optimize() 函数一起说，
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
| inline double optimize(CubicCurve &curve,
const Eigen::Matrix2Xd &iniInPs,
const double &relCostTol)
{
double minCost = 0;
Eigen::VectorXd x = Eigen::VectorXd::Map(iniInPs.data(),2*(pieceN-1));
int status = lbfgs_optimize(iniInPs, minCost, & costFunction,nullptr,this, lbfgs_params);
if(status >= 0 )
{
cubSpline.getCurve();
}
else
std::cout << "Generation failed!" << std::endl;
return minCost;
}
|
lbfgs_optimize 函数在optimize中，传入的是this，是PathSmoother 的类型的。在传入lbfgs_optimize函数中，变为了void ；在lbfgs_optimize函数中创建回调结构体，结构体中设置代价函数的地址，随后在调用proc_evaluate时，又传入instance的地址，即PathSmoother 的this指针。在costFunction函数中又将的void ptr还原成PathSmoother*。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
| inline int lbfgs_optimize(Eigen::VectorXd &x,
double &f,
lbfgs_evaluate_t proc_evaluate,
lbfgs_progress_t proc_progress,
void *instance,
const lbfgs_parameter_t ¶m)
{
...
callback_data_t cd;
cd.instance = instance;
cd.proc_evaluate = proc_evaluate;
cd.proc_progress = proc_progress;
fx = cd.proc_evaluate(cd.instance, x, g);
...
}
typedef double (*lbfgs_evaluate_t)(void *instance,
const Eigen::VectorXd &x,
Eigen::VectorXd &g);
|
实现void 和 PathSmoother 的来回转换本人还不是特别理解。
三次样条插值
特性
三次样条由于n+1点构成（y0,y1, … , yn）。第i段的曲线表达为：
在段与段的交接处，除了曲线连续外，要保证一阶二阶导数也连续，所以有
整合起来，得到最终的系统为：
其中D0 = Dn =0对于曲线的能量函数，表达为
对函数进行求导后平方，再积分得到最终的能量代价为
一阶梯度则为
一阶梯度需要分别求d(di)/dx 和 d(ci)/dx，为：
梯度的第一项为
后面Di的导数可以从上面最终的系统的方程倒推。令过程变量为：
那么D = A^(-1) B，那么d(Di)/dx = A^(-1) d(B)/dx。
最终回带，就求得了能量函数的一阶梯度。具体可参照：
1
| https://mathworld.wolfram.com/CubicSpline.html
|
结构体
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
| class CubicSpline
{
public:
CubicSpline() = default;
~CubicSpline() { A.destroy(); }
private:
int N;
Eigen::Vector2d headP;
Eigen::Vector2d tailP;
BandedSystem A;
Eigen::MatrixX2d b;
Eigen::Matrix2Xd coeffs;
Eigen::MatrixXd partial_c,partial_d;
public:
inline void setConditions(const Eigen::Vector2d &headPos,
const Eigen::Vector2d &tailPos,
const int &pieceNum);
inline void setInnerPoints(const Eigen::Ref<const Eigen::Matrix2Xd> &inPs);
inline void getCurve(CubicCurve &curve) const;
inline void getStretchEnergy(double &energy) const;
inline const Eigen::MatrixX2d &getCoeffs(void) const;
inline void getGrad(Eigen::Ref<Eigen::Matrix2Xd> gradByPoints) const;
inline void getPartial();
};
|
计算系数矩阵
在初始化系统后，输入内点，就可以计算系数了。计算的思路是利用LU分解计算 A*D=b，计算过程变量D后，回带进ai，bi，ci，di的公式中，就可以得到系数矩阵。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
| inline void setInnerPoints(const Eigen::Ref<const Eigen::Matrix2Xd> &inPs)
{
b.setZero();
b.row(0) = 3 * (inPs.col(1).transpose()- headP.transpose() );
b.row(N-2) = 3 * (tailP.transpose()-inPs.col(N-3).transpose() );
for(int i = 1; i < N-2 ;++i)
{
b.row(i) = 3 * (inPs.col(i+1).transpose()-inPs.col(i-1).transpose() );
}
A.reset();
A(0,0)=4,A(0,1)=1;
A(N-2,N-2)=4,A(N-2,N-3)=1;
for(int i = 1; i < N-2 ;++i)
{
A(i,i) = 4, A(i,i-1) = A(i,i+1) = 1;
}
A.factorizeLU();
A.solve(b);
coeffs.resize(2,4*N);
coeffs.setZero();
coeffs.col(0) = headP;
coeffs.col(1) = Eigen::Vector2d::Zero();
coeffs.col(2) = 3*(inPs.col(0) - headP) -b.row(0).transpose();
coeffs.col(3) = 2*(headP - inPs.col(0)) +b.row(0).transpose();
coeffs.col(4*N-4) = inPs.col(N-2);
coeffs.col(4*N-3) = b.row(N-2).transpose();
coeffs.col(4*N-2) = 3*(tailP - inPs.col(N-2)) -2 * b.row(N-2).transpose();
coeffs.col(4*N-1) = 2*(inPs.col(N-2) - tailP) +b.row(N-2).transpose();
for(int i = 1 ; i<N-1;++i)
{
coeffs.col(4*i) = inPs.col(i-1);
coeffs.col(4*i+1) = b.row(i-1).transpose();
coeffs.col(4*i+2) = 3*(inPs.col(i) - inPs.col(i-1)) -2 * b.row(i-1).transpose()-b.row(i).transpose() ;
coeffs.col(4*i+3) = 2*(inPs.col(i-1) - inPs.col(i)) +b.row(i-1).transpose()+ b.row(i).transpose() ;
}
b.resize(4*N,2);
b = coeffs.transpose();
return;
}
|
计算梯度
最终的梯度比较难计算的是d(ci)/dx和d(di)/dx，其中的过程梯度比较多，所以先用一个求解过程梯度函数进行预处理。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
| inline void getPartial()
{
Eigen::MatrixXd A_tmp=Eigen::MatrixXd::Zero(N-1,N-1);
A_tmp(0,0)=4,A_tmp(0,1)=1;
A_tmp(N-2,N-2)=4,A_tmp(N-2,N-3)=1;
for(int i = 1; i < N-2 ;++i)
{
A_tmp(i,i) = 4, A_tmp(i,i-1) = A_tmp(i,i+1) = 1;
}
Eigen::MatrixXd partial_b = Eigen::MatrixXd::Zero(N-1,N-1);
partial_b(0,0)=0,partial_b(0,1)=3;
partial_b(N-2,N-2)=0,partial_b(N-2,N-3)=-3;
for(int i = 1; i < N-2 ;++i)
{
partial_b(i,i) = 0, partial_b(i,i-1) = -3, partial_b(i,i+1) = 3;
}
Eigen::MatrixXd partial_D = A_tmp.inverse() * partial_b;
Eigen::MatrixXd partial_delta = Eigen::MatrixXd::Zero(N,N-1);
partial_delta(0,0) = -1;
partial_delta(N-1,N-2) = 1;
for(int i = 1; i < N-1 ;++i)
{
partial_delta(i,i) =-1, partial_delta(i,i-1) = 1;
}
partial_c.resize(N,N-1);
partial_c.setZero();
partial_d.resize(N,N-1);
partial_d.setZero();
Eigen::MatrixXd Q_c = Eigen::MatrixXd::Zero(N,N-1);
Eigen::MatrixXd Q_d = Eigen::MatrixXd::Zero(N,N-1);
Q_c(0,0) = -1,Q_c(N-1,N-2)=-2;
Q_d(0,0) = 1,Q_d(N-1,N-2) = 1;
for(int i = 1 ;i< N-1;++i)
{
Q_c(i,i) = -1,Q_c(i,i-1)=-2;
Q_d(i,i) = 1,Q_d(i,i-1)=1;
}
partial_c = -3 * partial_delta + Q_c * partial_D;
partial_d = 2 * partial_delta + Q_d * partial_D;
}
|
最终得到梯度：
1
2
3
4
5
6
7
8
9
10
11
12
| inline void getGrad(Eigen::Ref<Eigen::Matrix2Xd> gradByPoints) const
{
gradByPoints.setZero();
for(int i = 0;i<N;++i)
{
Eigen::Vector2d ci = coeffs.col(4*i+2);
Eigen::Vector2d di = coeffs.col(4*i+3);
gradByPoints += (24*di + 12*ci)*partial_d.row(i)+
(12*di + 8*ci)*partial_c.row(i);
}
}
|
L-BFGS
线搜索
通过weak wolfe原则和二分法结合，寻找步长。唯一还是不太理解的是二分法的选择，不满足充分下降条件时stp 缩小，而不满足曲线条件时stp 增大。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
| inline int line_search_lewisoverton(Eigen::VectorXd &x,
double &f,
Eigen::VectorXd &g,
double &stp,
const Eigen::VectorXd &s,
const Eigen::VectorXd &xp,
const Eigen::VectorXd &gp,
const double stpmin,
const double stpmax,
const callback_data_t &cd,
const lbfgs_parameter_t ¶m)
{
int iter = 0 ;
double upper = stpmax, low = stpmin;
double fx,fxp=f;
while(true)
{
iter++;
if(iter>param.max_linesearch)
break;
x = xp + stp * s;
fx = cd.proc_evaluate(cd.instance,x,g);
if( fxp - fx < - stp * param.f_dec_coeff * gp.dot(s))
{
upper = stp;
}
else if(g.dot(s) < param.s_curv_coeff * gp.dot(s))
{
low = stp;
}
else
{
f = fx;
return iter;
}
if (upper < stpmax)
{
stp = 0.5 * (low + upper);
}
else
{
stp = 2 * low;
}
}
return LBFGSERR_MAXIMUMLINESEARCH;
}
|
优化
这一部分是项目自带的。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
| 输入:
- 初始点 x0
- 目标函数 f(x)
- 梯度计算函数 grad_f(x)
- 参数:
- 内存大小 m (存储的向量对数量)
- 最大迭代次数 max_iter
- 梯度容差 g_epsilon
- 线搜索参数 (Wolfe 条件系数)
输出:
- 最优解 x
- 最优值 f(x)
算法步骤:
1. 初始化:
- x = x0
- g = grad_f(x)
- d = -g
- k = 0
- 初始化历史队列 S = [], Y = []
2. 检查初始点是否为驻点:
if ||g||_inf < g_epsilon:
return x, f(x)
3. 主循环 (while k < max_iter):
a. 线搜索 (满足 Wolfe 条件):
- α = line_search(x, f, g, d)
- x_new = x + α * d
- g_new = grad_f(x_new)
b. 检查收敛:
if ||g_new||_inf < g_epsilon:
return x_new, f(x_new)
c. 更新历史向量对:
- s_k = x_new - x
- y_k = g_new - g
- 将 (s_k, y_k) 加入队列 S, Y
- if len(S) > m:
移除 S, Y 中最旧的向量对
d. 计算新的搜索方向 d (L-BFGS 双循环递归):
- q = -g_new
- for i = len(S)-1 downto 0:
ρ_i = 1 / (y_i^T s_i)
α_i = ρ_i * s_i^T q
q = q - α_i * y_i
- r = q * (s_{last}^T y_{last} / y_{last}^T y_{last})
- for i = 0 to len(S)-1:
β_i = ρ_i * y_i^T r
r = r + (α_i - β_i) * s_i
- d = r
e. 更新变量:
- x = x_new
- g = g_new
- k = k + 1
4. 返回结果:
return x, f(x)
|
效果
尽管确实能生成路径，但是容易生成弯绕，曲折的路径。