文章目录
- 参考资料
- 1. 基本概念
- 1.1 运动学模型的离散状态方程
- 1.2 LQR求解步骤
- 2. python实现
- 2.1 车辆模型
- 2.2 相关参数设置
- 2.3 生成轨迹曲线
- 2.4 角度归一化
- 2.5 解代数里卡提方程
- 2.6 LQR控制算法
- 2.7 主函数
- 4. c++代码实现
参考资料
- 基于运动模型的LQR优化
- 强化学习与最优控制：用于轨迹跟踪的 LQR
- Linear–quadratic regulator (LQR) steering control
- 路径规划与轨迹跟踪系列算法
- 求解离散黎卡提矩阵代数方程
前文：LQR控制算法原理
1. 基本概念
LQR的控制理论原理推导可以翻看前文。
1.1 运动学模型的离散状态方程
对于一个离散时间系统：
X
(
k
+
1
)
=
A
X
(
k
)
+
B
u
(
k
)
mathbf{X}(k+1)=A mathbf{X}(k)+B mathbf{u}(k)
X(k+1)=AX(k)+Bu(k)
其中，
A
∈
R
n
×
n
A in R^{n times n}
A∈Rn×n ，
B
∈
R
n
×
m
B in R^{n times m}
B∈Rn×m。
关于最优问题，就在于如何选择合适的
u
0
,
u
1
,
…
u_{0}, u_{1}, ldots
u0,u1,… ，使得状态量
x
0
,
x
1
,
…
x_{0}, x_{1}, ldots
x0,x1,… 足够小，因此得到好的调节和控制；或者使得
u
0
,
u
1
,
…
u_{0}, u_{1}, ldots
u0,u1,… 足够小，以使用更少的能量。这两个量通常相互制约，如果采用更大的输入
u
mathbf{u}
u，就会驱使状态量更快达到0。
这是一个典型的多目标优化最优控制问题，为了表示控制系统达到稳定控制所付出的代价，定义如下二次型代价函数：
J
=
∑
k
=
1
N
(
X
T
Q
X
+
u
T
R
u
)
J=sum_{k=1}^{N}left(mathbf{X}^{T} Q mathbf{X}+mathbf{u}^{T} R mathbf{u}right)
J=k=1∑N(XTQX+uTRu)
其中， Q Q Q为半正定的状态加权矩阵, R R R为正定的控制加权矩阵，且两者通常取为对角阵； Q Q Q矩阵元素变大意味着希望状态量 X mathbf{X} X能够快速趋近于零； R R R矩阵元素变大意味着希望控制输入能够尽可能小。
在轨迹跟踪中，前一项优化目标表示跟踪过程路径偏差的累积大小，第二项优化目标表示跟踪过程控制能量的损耗，这样就将轨迹跟踪控制问题转化为一个最优控制问题。
给定一个大小为 n × n ntimes n n×n的实对称矩阵A ，若对于任意长度为 n n n的非零向量 x x x，有 x T A x > 0 x^TAx>0 xTAx>0恒成立，则矩阵A是一个正定矩阵。
对于上述目标函数的优化求解，使用线性二次型调节器 ( Linear-Quadratic Regulator)，解出的最优控制规律u是关于状态变量 X X X的线性函数
u
=
[
−
(
R
+
B
T
P
B
)
−
1
B
T
P
A
]
X
=
K
X
mathbf{u}=left[-left(R+B^{T} P Bright)^{-1} B^{T} P Aright] mathbf{X}=K mathbf{X}
u=[−(R+BTPB)−1BTPA]X=KX
其中，P是下述黎卡提方程的解 :
P
=
A
T
P
A
−
A
T
P
B
(
R
+
B
T
P
B
)
−
1
B
T
P
A
+
Q
P=A^{T} P A-A^{T} P Bleft(R+B^{T} P Bright)^{-1} B^{T} P A+Q
P=ATPA−ATPB(R+BTPB)−1BTPA+Q
形如 y ′ = P ( x ) y 2 + Q ( x ) y + R ( x ) y'=P(x)y^2+Q(x)y+R(x) y′=P(x)y2+Q(x)y+R(x)的非线性微分方程称为黎卡提方程。 针对黎卡提方程，可以采用循环迭代的思想求解P：
1）令等式右边的P_old=Q；
2）计算等式右边的值为P_new
3）比较P_old和P_new，若两者的差值小于预设值， 则认为等式两边相等；否则再令P_old=P_new，继续循环。
备注
AtsushiSakai 的PythonRobotics的代码就是这么求解的
1.2 LQR求解步骤
综上，采用LQR算法进行控制率求解的步骤概括为：
-
确定迭代范围 N N N，预设精度 E P S EPS EPS
-
设置迭代初始值 P = Q f P=Q_{f} P=Qf，其中 Q f = Q Q_f=Q Qf=Q
-
循环迭代， t = 1 , … , N t=1, ldots, N t=1,…,N
P n e w = Q + A T P A − A T P B ( R + B T P B ) − 1 B T P A P_{new}=Q+A^{T} P A-A^{T} P Bleft(R+B^{T} P Bright)^{-1} B^{T} P A\ Pnew=Q+ATPA−ATPB(R+BTPB)−1BTPA
若 ∣ ∣ P n e w − P ∣ ∣ < E P S ||P_{new}-P||<EPS ∣∣Pnew−P∣∣<EPS：跳出循环；否则： P = P n e w P=P_{new} P=Pnew -
计算反馈系数 K = − ( R + B T P n e w B ) − 1 B T P n e w A K=-left(R+B^{T} P_{new}Bright)^{-1} B^{T} P_{new} A K=−(R+BTPnewB)−1BTPnewA
-
最终得优化的控制量 u ∗ = K X u^{*}=K X u∗=KX
2. python实现
2.1 车辆模型
我们以后轴中心为车辆中心的单车运动学模型为例，它的离散状态方程如下：
X
(
k
+
1
)
=
[
1
0
−
T
v
r
sin
ψ
r
0
1
T
v
r
cos
ψ
r
0
0
1
]
X
(
k
)
+
[
T
cos
ψ
r
0
T
sin
ψ
r
0
T
tan
δ
r
L
v
r
T
L
cos
2
δ
r
]
u
(
k
)
=
A
X
(
k
)
+
B
u
(
k
)
begin{aligned} mathbf{X}(k+1)&= left[begin{array}{ccc} 1 & 0 & -T v_{r} sin psi_{r} \ 0 & 1 & Tv_{r} cos psi_{r} \ 0 & 0 & 1 end{array}right] mathbf{X}(k)+left[begin{array}{ccc} T cos psi_{r} & 0 \ T sin psi_{r} & 0 \ frac{Ttan delta_{r}}{L} & frac{v_{r}T}{L cos ^{2} delta_{r}} end{array}right]mathbf{u}(k) \ &=A mathbf{X}(k)+B mathbf{u}(k) end{aligned}
X(k+1)=⎣⎡100010−TvrsinψrTvrcosψr1⎦⎤X(k)+⎣⎡TcosψrTsinψrLTtanδr00Lcos2δrvrT⎦⎤u(k)=AX(k)+Bu(k)
式中，
v
r
v_r
vr代表参考轨迹上每一个轨迹点要求的速度值；
δ
r
delta_r
δr是每一个轨迹点的参考前轮转角控制量。
X
=
x
−
x
r
e
f
mathbf{X}=x-x_{ref}
X=x−xref为状态量误差，
u
=
u
−
u
r
e
f
mathbf{u}=u-u_{ref}
u=u−uref为控制量误差。
期望的系统响应特性有以下两点:
- 跟踪偏差能够快速、稳定地趋近于零，并保持平衡；
- 前轮转角控制输入尽可能小。
采用线性二次调节原理解决这个问题。
python实现代码如下。
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
48import math class KinematicModel_3: """假设控制量为转向角delta_f和加速度a """ def __init__(self, x, y, psi, v, L, dt): self.x = x self.y = y self.psi = psi self.v = v self.L = L # 实现是离散的模型 self.dt = dt def update_state(self, a, delta_f): self.x = self.x+self.v*math.cos(self.psi)*self.dt self.y = self.y+self.v*math.sin(self.psi)*self.dt self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt self.v = self.v+a*self.dt def get_state(self): return self.x, self.y, self.psi, self.v def state_space(self, ref_delta, ref_yaw): """将模型离散化后的状态空间表达 Args: delta (_type_): 参考输入 Returns: _type_: _description_ """ A = np.matrix([ [1.0,0.0,-self.v*self.dt*math.sin(ref_yaw)], [0.0, 1.0, self.v*self.dt*math.cos(ref_yaw)], [0.0,0.0,1.0]]) B = np.matrix([ [self.dt*math.cos(ref_yaw), 0], [self.dt*math.sin(ref_yaw), 0], [self.dt*math.tan(ref_delta)/self.L, self.v*self.dt/(self.L*math.cos(ref_delta)*math.cos(ref_delta))] ]) return A,B
2.2 相关参数设置
1
2
3
4
5
6
7
8
9
10
11N=100 # 迭代范围 EPS = 1e-4 # 迭代精度 Q = np.eye(3)*3 R = np.eye(2)*2. dt=0.1 # 时间间隔，单位：s L=2 # 车辆轴距，单位：m v = 2 # 初始速度 x_0=0 # 初始x y_0=-3 #初始y psi_0=0 # 初始航向角
2.3 生成轨迹曲线
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
56class MyReferencePath: def __init__(self): # set reference trajectory # refer_path包括4维：位置x, 位置y， 轨迹点的切线方向, 曲率k self.refer_path = np.zeros((1000, 4)) self.refer_path[:,0] = np.linspace(0, 100, 1000) # x self.refer_path[:,1] = 2*np.sin(self.refer_path[:,0]/3.0)+2.5*np.cos(self.refer_path[:,0]/2.0) # y # 使用差分的方式计算路径点的一阶导和二阶导，从而得到切线方向和曲率 for i in range(len(self.refer_path)): if i == 0: dx = self.refer_path[i+1,0] - self.refer_path[i,0] dy = self.refer_path[i+1,1] - self.refer_path[i,1] ddx = self.refer_path[2,0] + self.refer_path[0,0] - 2*self.refer_path[1,0] ddy = self.refer_path[2,1] + self.refer_path[0,1] - 2*self.refer_path[1,1] elif i == (len(self.refer_path)-1): dx = self.refer_path[i,0] - self.refer_path[i-1,0] dy = self.refer_path[i,1] - self.refer_path[i-1,1] ddx = self.refer_path[i,0] + self.refer_path[i-2,0] - 2*self.refer_path[i-1,0] ddy = self.refer_path[i,1] + self.refer_path[i-2,1] - 2*self.refer_path[i-1,1] else: dx = self.refer_path[i+1,0] - self.refer_path[i,0] dy = self.refer_path[i+1,1] - self.refer_path[i,1] ddx = self.refer_path[i+1,0] + self.refer_path[i-1,0] - 2*self.refer_path[i,0] ddy = self.refer_path[i+1,1] + self.refer_path[i-1,1] - 2*self.refer_path[i,1] self.refer_path[i,2]=math.atan2(dy,dx) # yaw # 计算曲率:设曲线r(t) =(x(t),y(t)),则曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2). # 参考：https://blog.csdn.net/weixin_46627433/article/details/123403726 self.refer_path[i,3]=(ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) # 曲率k计算 def calc_track_error(self, x, y): """计算跟踪误差 Args: x (_type_): 当前车辆的位置x y (_type_): 当前车辆的位置y Returns: _type_: _description_ """ # 寻找参考轨迹最近目标点 d_x = [self.refer_path[i,0]-x for i in range(len(self.refer_path))] d_y = [self.refer_path[i,1]-y for i in range(len(self.refer_path))] d = [np.sqrt(d_x[i]**2+d_y[i]**2) for i in range(len(d_x))] s = np.argmin(d) # 最近目标点索引 yaw = self.refer_path[s, 2] k = self.refer_path[s, 3] angle = normalize_angle(yaw - math.atan2(d_y[s], d_x[s])) e = d[s] # 误差 if angle < 0: e *= -1 return e, k, yaw, s
2.4 角度归一化
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18def normalize_angle(angle): """ Normalize an angle to [-pi, pi]. :param angle: (float) :return: (float) Angle in radian in [-pi, pi] copied from https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/stanley_control/stanley_control.html """ while angle > np.pi: angle -= 2.0 * np.pi while angle < -np.pi: angle += 2.0 * np.pi return angle
2.5 解代数里卡提方程
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24def cal_Ricatti(A,B,Q,R): """解代数里卡提方程 Args: A (_type_): 状态矩阵A B (_type_): 状态矩阵B Q (_type_): Q为半正定的状态加权矩阵, 通常取为对角阵；Q矩阵元素变大意味着希望跟踪偏差能够快速趋近于零； R (_type_): R为正定的控制加权矩阵，R矩阵元素变大意味着希望控制输入能够尽可能小。 Returns: _type_: _description_ """ # 设置迭代初始值 Qf=Q P=Qf # 循环迭代 for t in range(N): P_=Q+A.T@P@A-A.T@P@B@np.linalg.pinv(R+B.T@P@B)@B.T@P@A if(abs(P_-P).max()<EPS): break P=P_ return P_
2.6 LQR控制算法
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20def lqr(robot_state, refer_path, s0, A, B, Q, R): """ LQR控制器 """ # x为位置和航向误差 x=robot_state[0:3]-refer_path[s0,0:3] P = cal_Ricatti(A,B,Q,R) K = -np.linalg.pinv(R + B.T @ P @ B) @ B.T @ P @ A u = K @ x u_star = u #u_star = [[v-ref_v,delta-ref_delta]] # print(u_star) return u_star[0,1]
2.7 主函数
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
55from celluloid import Camera # 保存动图时用，pip install celluloid # 使用随便生成的轨迹 def main(): reference_path = MyReferencePath() goal = reference_path.refer_path[-1,0:2] # 运动学模型 ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt) x_ = [] y_ = [] fig = plt.figure(1) # 保存动图用 camera = Camera(fig) # plt.ylim([-3,3]) for i in range(500): robot_state = np.zeros(4) robot_state[0] = ugv.x robot_state[1] = ugv.y robot_state[2]=ugv.psi robot_state[3]=ugv.v e, k, ref_yaw, s0 = reference_path.calc_track_error(robot_state[0], robot_state[1]) ref_delta = math.atan2(L*k,1) A, B = ugv.state_space(ref_delta,ref_yaw) delta = lqr(robot_state, reference_path.refer_path,s0, A, B, Q, R) delta = delta+ref_delta ugv.update_state(0, delta) # 加速度设为0，恒速 x_.append(ugv.x) y_.append(ugv.y) # 显示动图 plt.cla() plt.plot(reference_path.refer_path[:,0], reference_path.refer_path[:,1], "-.b", linewidth=1.0, label="course") plt.plot(x_, y_, "-r", label="trajectory") plt.plot(reference_path.refer_path[s0,0], reference_path.refer_path[s0,1], "go", label="target") # plt.axis("equal") plt.grid(True) plt.pause(0.001) # camera.snap() # 判断是否到达最后一个点 if np.linalg.norm(robot_state[0:2]-goal)<=0.1: print("reach goal") break # animation = camera.animate() # animation.save('trajectory.gif') main()
跟踪效果如下：
完整python代码文件见github仓库
4. c++代码实现
由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构与python代码实现类似，这边就不再做相关代码解释了。完整代码详见另一个github仓库。
最后
以上就是机灵猎豹最近收集整理的关于【自动驾驶】LQR控制实现轨迹跟踪 | python实现 | c++实现参考资料1. 基本概念2. python实现4. c++代码实现的全部内容，更多相关【自动驾驶】LQR控制实现轨迹跟踪内容请搜索靠谱客的其他文章。
发表评论 取消回复