文章目录
参考资料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优化 强化学习与最优控制:用于轨迹跟踪的 LQRLinear–quadratic regulator (LQR) steering control路径规划与轨迹跟踪系列算法求解离散黎卡提矩阵代数方程前文:LQR控制算法原理
1. 基本概念
LQR的控制理论原理推导可以翻看前文。
1.1 运动学模型的离散状态方程
对于一个离散时间系统:
X(k+1)=AX(k)+Bu(k)\mathbf{X}(k+1)=A \mathbf{X}(k)+B \mathbf{u}(k) X(k+1)=AX(k)+Bu(k)
其中, A∈Rn×nA \in R^{n \times n}A∈Rn×n , B∈Rn×mB \in R^{n \times m}B∈Rn×m。
关于最优问题,就在于如何选择合适的 u0,u1,…u_{0}, u_{1}, \ldotsu0,u1,… ,使得状态量 x0,x1,…x_{0}, x_{1}, \ldotsx0,x1,… 足够小,因此得到好的调节和控制;或者使得 u0,u1,…u_{0}, u_{1}, \ldotsu0,u1,… 足够小,以使用更少的能量。这两个量通常相互制约,如果采用更大的输入u\mathbf{u}u,就会驱使状态量更快达到0。
这是一个典型的多目标优化最优控制问题,为了表示控制系统达到稳定控制所付出的代价,定义如下二次型代价函数:
J=∑k=1N(XTQX+uTRu)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)
其中,QQQ为半正定的状态加权矩阵, RRR为正定的控制加权矩阵,且两者通常取为对角阵;QQQ矩阵元素变大意味着希望状态量X\mathbf{X}X能够快速趋近于零;RRR矩阵元素变大意味着希望控制输入能够尽可能小。
在轨迹跟踪中,前一项优化目标表示跟踪过程路径偏差的累积大小,第二项优化目标表示跟踪过程控制能量的损耗,这样就将轨迹跟踪控制问题转化为一个最优控制问题。
给定一个大小为n×nn\times nn×n的实对称矩阵A ,若对于任意长度为nnn的非零向量xxx,有xTAx>0x^TAx>0xTAx>0恒成立,则矩阵A是一个正定矩阵。
对于上述目标函数的优化求解,使用线性二次型调节器 ( Linear-Quadratic Regulator),解出的最优控制规律u是关于状态变量XXX的线性函数
u=[−(R+BTPB)−1BTPA]X=KX\mathbf{u}=\left[-\left(R+B^{T} P B\right)^{-1} B^{T} P A\right] \mathbf{X}=K \mathbf{X} u=[−(R+BTPB)−1BTPA]X=KX
其中,P是下述黎卡提方程的解 :
P=ATPA−ATPB(R+BTPB)−1BTPA+QP=A^{T} P A-A^{T} P B\left(R+B^{T} P B\right)^{-1} B^{T} P A+Q P=ATPA−ATPB(R+BTPB)−1BTPA+Q
形如y′=P(x)y2+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算法进行控制率求解的步骤概括为:
确定迭代范围 NNN,预设精度EPSEPSEPS
设置迭代初始值 P=QfP=Q_{f}P=Qf,其中Qf=QQ_f=QQf=Q
循环迭代, t=1,…,Nt=1, \ldots, Nt=1,…,N
Pnew=Q+ATPA−ATPB(R+BTPB)−1BTPAP_{new}=Q+A^{T} P A-A^{T} P B\left(R+B^{T} P B\right)^{-1} B^{T} P A\\ Pnew=Q+ATPA−ATPB(R+BTPB)−1BTPA
若∣∣Pnew−P∣∣<EPS||P_{new}-P||<EPS∣∣Pnew−P∣∣<EPS:跳出循环;否则:P=PnewP=P_{new}P=Pnew
计算反馈系数 K=−(R+BTPnewB)−1BTPnewAK=-\left(R+B^{T} P_{new}B\right)^{-1} B^{T} P_{new} AK=−(R+BTPnewB)−1BTPnewA
最终得优化的控制量 u∗=KXu^{*}=K Xu∗=KX
2. python实现
2.1 车辆模型
我们以后轴中心为车辆中心的单车运动学模型为例,它的离散状态方程如下:
X(k+1)=[10−Tvrsinψr01Tvrcosψr001]X(k)+[Tcosψr0Tsinψr0TtanδrLvrTLcos2δr]u(k)=AX(k)+Bu(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{T\tan \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)
式中,vrv_rvr代表参考轨迹上每一个轨迹点要求的速度值; δr\delta_rδr是每一个轨迹点的参考前轮转角控制量。X=x−xref\mathbf{X}=x-x_{ref}X=x−xref为状态量误差,u=u−uref\mathbf{u}=u-u_{ref}u=u−uref为控制量误差。
期望的系统响应特性有以下两点:
跟踪偏差能够快速、稳定地趋近于零,并保持平衡;前轮转角控制输入尽可能小。
采用线性二次调节原理解决这个问题。
python实现代码如下。
import mathclass KinematicModel_3:"""假设控制量为转向角delta_f和加速度a"""def __init__(self, x, y, psi, v, L, dt):self.x = xself.y = yself.psi = psiself.v = vself.L = L# 实现是离散的模型self.dt = dtdef update_state(self, a, delta_f):self.x = self.x+self.v*math.cos(self.psi)*self.dtself.y = self.y+self.v*math.sin(self.psi)*self.dtself.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dtself.v = self.v+a*self.dtdef get_state(self):return self.x, self.y, self.psi, self.vdef 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 相关参数设置
N=100 # 迭代范围EPS = 1e-4 # 迭代精度Q = np.eye(3)*3R = np.eye(2)*2.dt=0.1 # 时间间隔,单位:sL=2 # 车辆轴距,单位:mv = 2 # 初始速度x_0=0 # 初始xy_0=-3 #初始ypsi_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) # xself.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).# 参考:/weixin_46627433/article/details/123403726self.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_): 当前车辆的位置xy (_type_): 当前车辆的位置yReturns:_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 *= -1return 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.piwhile angle < -np.pi:angle += 2.0 * np.pireturn angle
2.5 解代数里卡提方程
def cal_Ricatti(A,B,Q,R):"""解代数里卡提方程Args:A (_type_): 状态矩阵AB (_type_): 状态矩阵BQ (_type_): Q为半正定的状态加权矩阵, 通常取为对角阵;Q矩阵元素变大意味着希望跟踪偏差能够快速趋近于零;R (_type_): R为正定的控制加权矩阵,R矩阵元素变大意味着希望控制输入能够尽可能小。Returns:_type_: _description_"""# 设置迭代初始值Qf=QP=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@Aif(abs(P_-P).max()<EPS):breakP=P_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 = -np.linalg.pinv(R + B.T @ P @ B) @ B.T @ P @ Au = K @ xu_star = u #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.xrobot_state[1] = ugv.yrobot_state[2]=ugv.psirobot_state[3]=ugv.ve, 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_deltaugv.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仓库。