参考资料
1. 基本概念
从控制理论角度出发的讲解可以翻看博客。
1.1 运动学模型的离散状态方程
对于一个离散时间系统:
其中, , 。
关于最优问题,就在于如何选择合适的 ,使得状态量 足够小,因此得到好的调节和控制;或者使得 足够小,以使用更少的能量。这两个量通常相互制约,如果采用更大的输入,就会驱使状态量更快达到0。
这是一个典型的多目标优化最优控制问题,为了表示控制系统达到稳定控制所付出的代价,定义如下二次型代价函数:
其中,为半正定的状态加权矩阵, 为正定的控制加权矩阵,且两者通常取为对角阵;矩阵元素变大意味着希望状态量能够快速趋近于零;矩阵元素变大意味着希望控制输入能够尽可能小。
在轨迹跟踪中,前一项优化目标表示跟踪过程路径偏差的累积大小,第二项优化目标表示跟踪过程控制能量的损耗,这样就将轨迹跟踪控制问题转化为一个最优控制问题。
给定一个大小为的实对称矩阵A ,若对于任意长度为的非零向量,有恒成立,则矩阵A是一个正定矩阵。
对于上述目标函数的优化求解,使用线性二次型调节器 ( Linear-Quadratic Regulator),解出的最优控制规律u是关于状态变量的线性函数
其中,P是下述黎卡提方程的解 :
形如的非线性微分方程称为黎卡提方程。 针对黎卡提方程,可以采用循环迭代的思想求解P:
1)令等式右边的P_old=Q
;
2)计算等式右边的值为P_new
3)比较P_old
和P_new
,若两者的差值小于预设值, 则认为等式两边相等;否则再令P_old=P_new
,继续循环。
备注
AtsushiSakai 的PythonRobotics的代码就是这么求解的
1.2 LQR求解步骤
综上,采用LQR算法进行控制率求解的步骤概括为:
- 确定迭代范围
- 设置迭代初始值 ,其中
- 循环迭代,
- 从 ,循环计算反馈系数
- 最终得优化的控制量
2. python实现
2.1 车辆模型
我们以后轴中心为车辆中心的单车运动学模型为例,它的离散状态方程如下:
式中,代表参考轨迹上每一个轨迹点要求的速度值; 是每一个轨迹点的参考前轮转角控制量。为状态量误差,为控制量误差。
期望的系统响应特性有以下两点:
- 跟踪偏差能够快速、稳定地趋近于零,并保持平衡;
- 前轮转角控制输入尽可能小。
采用线性二次调节原理解决这个问题。
python实现代码如下。
import 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.v*self.dt/(self.L*math.cos(ref_delta)*math.cos(ref_delta))]
])
return A,B
2.2 相关参数设置
N=100 # 迭代范围
Q = np.eye(3)*5
R = np.eye(2)*1.
dt=0.1 # 时间间隔,单位:s
L=2 # 车辆轴距,单位:m
v = 2 # 初始速度
x_0=0 # 初始x
y_0=-3 #初始y
psi_0=0 # 初始航向角
2.3 生成轨迹曲线
class 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 角度归一化
def 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 解代数里卡提方程
def cal_Ricatti(A,B,Q,R):
"""解代数里卡提方程
Args:
A (_type_): 状态矩阵A
B (_type_): 状态矩阵B
Q (_type_): Q为半正定的状态加权矩阵, 通常取为对角阵;Q矩阵元素变大意味着希望跟踪偏差能够快速趋近于零;
R (_type_): R为正定的控制加权矩阵,R矩阵元素变大意味着希望控制输入能够尽可能小。
Returns:
_type_: _description_
"""
P = [None] * (N + 1)
# 设置迭代初始值
Qf=Q
P[N]=Qf
# 循环迭代,for t=N,...,1
for t in range(N,0,-1):
P[t-1]=Q+A.T@P[t]@A-A.T@P[t]@B@np.linalg.pinv(R+B.T@P[t]@B)@B.T@P[t]@A
return P
2.6 LQR控制算法
def 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 = [None] * (N)
u = [None] * (N)
"""计算反馈系数K和优化的控制量U"""
for t in range(N):
K[t] = -np.linalg.pinv(R + B.T @ P[t+1] @ B) @ B.T @ P[t+1] @ A
u[t] = K[t] @ x
u_star = u[N-1] #u_star = [[v-ref_v,delta-ref_delta]]
# print(u_star)
return u_star[0,1]
2.7 主函数
from 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仓库
文章出处登录后可见!