AirSim学习日志 5-LQR实现无人机轨迹跟踪

1.LQR控制器算法原理推导

1.1 状态反馈控制

连续线性系统的状态空间表示为
{x˙=Ax+Buy=Cx+Dux(t)∈Rn,u(t)∈Rm\left \{ \begin{aligned} \dot{x}&=Ax+Bu \\y&=Cx+Du \end{aligned} \right. \\ x(t)\in R^n,u(t)\in R^m {x˙y=Ax+Bu=Cx+Dux(t)∈Rn,u(t)∈Rm
对其设计状态反馈控制器
u=w−Kxu=w-Kx u=w−Kx
其中www为设定值，KKK为反馈矩阵，得到的结构图如下所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-9hRI3n1D-1657001135363)(https://cdn.jsdelivr.net/gh/kun-k/blogweb/imageimage-20220630163853280.png)]

设计状态反馈的目标为，通过设计反馈矩阵KKK，使得闭环系统的极点为期望的极点位置。

1.2 连续时间系统LQR

LQR（Linear quadratic regulator，线性二次型调节器）设计的目标为，找到一组控制量u0,u1,⋅⋅⋅u_0,u_1,···u0,u1,⋅⋅⋅，使得状态量x0,x1,⋅⋅⋅x_0,x_1,···x0,x1,⋅⋅⋅能够快速、稳定地趋近并保存平衡状态，且控制量尽可能小。

其优化目标为
minJ=12∫0∞(xTQx+uTRu)dtmin\quad J=\frac{1}{2}\int_0^\infty (x^TQx+u^TRu)dt minJ=21∫0∞(xTQx+uTRu)dt
其中，矩阵QQQ为半正定的状态加权矩阵，RRR为正定的控制量加权矩阵，且通常取为对角阵，需根据问题的需求进行设计。接下来对算法进行推导。

使用1.1中的状态反馈，取w=0w=0w=0以简化问题，将u=−Kxu=-Kxu=−Kx带入优化目标后，得到
J=12∫0∞xT(Q+KTRK)xdt(1)J=\frac{1}{2}\int_0^\infty x^T(Q+K^TRK)xdt\qquad (1) J=21∫0∞xT(Q+KTRK)xdt(1)
假设存在一个常数矩阵PPP，
ddt(xTPx)=−xT(Q+KTRK)x(2)\frac{d}{dt}(x^TPx)=-x^T(Q+K^TRK)x \qquad (2) dtd(xTPx)=−xT(Q+KTRK)x(2)
则
KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ J&=\frac{1}{2}…
为使得JJJ达到最小值，t→∞t\rightarrow \inftyt→∞时x(t)x(t)x(t)应趋于0，故
J=12xT(0)Px(0)(3)J=\frac{1}{2}x^T(0)Px(0)\qquad (3) J=21xT(0)Px(0)(3)
将(2)式左侧微分展开后得到
x˙TPx+xTPx˙+xTQx+xTKTRKTx=0(4)\dot{x}^TPx+x^TP\dot{x}+x^TQx+x^TK^TRK^Tx=0\qquad (4) x˙TPx+xTPx˙+xTQx+xTKTRKTx=0(4)
又因为
x˙=Ax+Bu=(A−BK)x=A′x(5)\dot{x}=Ax+Bu=(A-BK)x=A'x\qquad (5) x˙=Ax+Bu=(A−BK)x=A′x(5)
代入(4)式得到s
xTA′TPx+xTPA′x+xTQx+xTKTRKTx=0x^TA'^TPx+x^TPA'x+x^TQx+x^TK^TRK^Tx=0\qquad xTA′TPx+xTPA′x+xTQx+xTKTRKTx=0
整理得到
xT(A′TP+PA′+Q+KTRKT)x=0x^T(A'^TP+PA'+Q+K^TRK^T)x=0\qquad xT(A′TP+PA′+Q+KTRKT)x=0
故
A′TP+PA′+Q+KTRKT=0A'^TP+PA'+Q+K^TRK^T=0 A′TP+PA′+Q+KTRKT=0
即
(A−BK)TP+P(A−BK)+Q+KTRKT=0ATP+PA+Q+KTRK−KTBTP−PBK=0(A-BK)^TP+P(A-BK)+Q+K^TRK^T=0 \\A^TP+PA+Q+K^TRK-K^TB^TP-PBK=0 (A−BK)TP+P(A−BK)+Q+KTRKT=0ATP+PA+Q+KTRK−KTBTP−PBK=0
取K=R−1BTPK=R^{-1}B^TPK=R−1BTP时上式取最小值，此时上式化为
ATP+PA+Q−PBR−1BTP=0A^TP+PA+Q-PBR^{-1}B^TP=0 ATP+PA+Q−PBR−1BTP=0
可求解得到矩阵PPP。

故连续时间系统的LQR算法流程为

根据实际问题选择参数矩阵Q,RQ,RQ,R；
求解方程ATP+PA+Q−PBR−1BTP=0A^TP+PA+Q-PBR^{-1}B^TP=0ATP+PA+Q−PBR−1BTP=0得到矩阵PPP；
计算反馈矩阵K=R−1BTPK=R^{-1}B^TPK=R−1BTP；
计算控制量u=−Kxu=-Kxu=−Kx

1.3 离散时间系统无限时间LQR

离散时间系统的状态空间描述为
X(k+1)=AX(k)+Bu(k)X(k+1)=AX(k)+Bu(k) X(k+1)=AX(k)+Bu(k)
LQR计算的优化目标为
minJ=12∑k=1∞[x(k)TQx(k)+u(k)TRu(k)]min\quad J=\frac{1}{2}\sum_{k=1}^\infty [x(k)^TQx(k)+u(k)^TRu(k)] minJ=21k=1∑∞[x(k)TQx(k)+u(k)TRu(k)]
这个问题可以用线性规划的方法求解，求解过程比较复杂，也没有深究，感兴趣的话可以看这个视频：https://www.bilibili.com/video/BV1P54y1m7CZ?share_source=copy_web

该问题求解的结果为
P=Q+ATPA−ATPB(R+BTPB)−1BTPAK=(R+BTPB)−1BTPAu(k)=−KX(k)P=Q+A^TPA-A^TPB(R+B^TPB)^{-1}B^TPA \\K=(R+B^TPB)^{-1}B^TPA \\u(k)=-KX(k) P=Q+ATPA−ATPB(R+BTPB)−1BTPAK=(R+BTPB)−1BTPAu(k)=−KX(k)
离散时间无限时间系统的LQR算法（DLQR）流程为

根据实际问题选择参数矩阵Q,RQ,RQ,R；
根据方程P=Q+ATPA−ATPB(R+BTPB)−1BTPAP=Q+A^TPA-A^TPB(R+B^TPB)^{-1}B^TPAP=Q+ATPA−ATPB(R+BTPB)−1BTPA，计算矩阵PPP；
计算反馈矩阵K=(R+BTPB)−1BTPAK=(R+B^TPB)^{-1}B^TPAK=(R+BTPB)−1BTPA；
计算控制量u=−KX(k)u=-KX(k)u=−KX(k)

2.无人机轨迹跟踪控制器DLQR算法设计

取无人机的位置和速度为状态量：x=[p,v]Tx=[p, v]^Tx=[p,v]T，加速度为输入量u=au=au=a，则系统状态方程为
KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ &x(k+1)=Ax(k)+…
假设要跟踪的目标轨迹为xdes=[pdes,vdes]Tx_{des}=[p_{des}, v_{des}]^Txdes=[pdes,vdes]T，加速度为adesa_{des}ades，则xdex,adexx_{dex},a_{dex}xdex,adex满足
xdes(k+1)=Axdes(k)+Bades(k)(2)x_{des}(k+1)=Ax_{des}(k)+Ba_{des}(k)\qquad(2) xdes(k+1)=Axdes(k)+Bades(k)(2)
将(1)(2)两个式子相减得到：
[xdes(k+1)−x(k+1)]=A[xdes(k)−x(k)]+B(ades(k)−u(k))[x_{des}(k+1)-x(k+1)]=A[x_{des}(k)-x(k)]+B(a_{des}(k)-u(k)) [xdes(k+1)−x(k+1)]=A[xdes(k)−x(k)]+B(ades(k)−u(k))
上式可以看作是一个以xdes−xx_{des}-xxdes−x为状态变量的状态方程，设计LQR控制器使得xdes−xx_{des}-xxdes−x趋于0，其优化目标为：
minJ=12∑k=1∞{[xdes(k)−x(k)]TQ[xdes(k)−x(k)]+[ades(k)−u(k)]TR[ades(k)−u(k)]}min\quad J=\frac{1}{2}\sum_{k=1}^\infty \{[x_{des}(k)-x(k)]^TQ[x_{des}(k)-x(k)] +[a_{des}(k)-u(k)]^TR[a_{des}(k)-u(k)]\} minJ=21k=1∑∞{[xdes(k)−x(k)]TQ[xdes(k)−x(k)]+[ades(k)−u(k)]TR[ades(k)−u(k)]}
其求解结果为adex(k)−u(k)=−K[xdex(k)−x(k)]a_{dex}(k)-u(k)=-K[x_{dex}(k)-x(k)]adex(k)−u(k)=−K[xdex(k)−x(k)]，则四旋翼的系统输入量为u(k)=−K[x(k)−xdex(k)]+adex(k)u(k)=-K[x(k)-x_{dex}(k)]+a_{dex}(k)u(k)=−K[x(k)−xdex(k)]+adex(k)，可根据1.3中的方法计算反馈矩阵和系统输入量。

加权矩阵可取
Q=[1000010000100001],R=[0.4000.4]Q=\begin{bmatrix}1&0&0&0\\0&1&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix}, R=\begin{bmatrix}0.4&0\\0&0.4\end{bmatrix} Q=⎣⎢⎢⎡1000010000100001⎦⎥⎥⎤,R=[0.4000.4]

3.使用加速度进行无人机控制

3.1 四旋翼姿态解算

根据四旋翼的横滚、俯仰、偏航三个角度的计算，可以得到四旋翼的姿态，横滚角ϕ\phiϕ、俯仰角θ\thetaθ、偏航角ψ\psiψ的示意如下图所示：

假设一向量ttt，其在世界坐标系world和机体坐标系body下均为(t1,t2,t3)T(t_1,t_2,t_3)^T(t1,t2,t3)T。经过偏航、俯仰、横滚，即依次绕x,y,zx,y,zx,y,z轴旋转后，在世界坐标系下得到向量twt^wtw，在机体坐标系下向量ttt依然为(t1,t2,t3)T(t_1,t_2,t_3)^T(t1,t2,t3)T。需计算机体坐标系到世界坐标系之间的旋转矩阵，使tw=R⋅tt^w=R·ttw=R⋅t。

首先计算向量ttt绕xxx轴旋转的旋转矩阵RxR_xRx，将其分解为向量
tx=(t1,0,0)T,ty=(0,t2,0)T,tz=(0,0,t3)Tt_x=(t_1,0,0)^T,t_y=(0,t_2,0)^T,t_z=(0,0,t_3)^T tx=(t1,0,0)T,ty=(0,t2,0)T,tz=(0,0,t3)T
容易计算得三个分量绕xxx轴旋转角度ϕ\phiϕ后，转换为向量
txw=(t1,0,0)T,tyw=(0,t2⋅cosϕ,t2⋅sinϕ)T,tzw=(0,−t3⋅sinϕ,t3⋅cosϕ)Tt′=tx′w+tyw+tzw=(t1,t2⋅cosϕ−t3⋅sinϕ,t2⋅sinϕ+t3⋅cosϕ)Tt_x^w=(t_1,0,0)^T,t_y^w=(0,t_2·cos\phi,t_2·sin\phi)^T,t_z^w=(0,-t_3·sin\phi,t_3·cos\phi)^T \\t'=t_x'^w+t_y^w+t_z^w=(t_1,t_2·cos\phi-t_3·sin\phi,t_2·sin\phi+t_3·cos\phi)^T txw=(t1,0,0)T,tyw=(0,t2⋅cosϕ,t2⋅sinϕ)T,tzw=(0,−t3⋅sinϕ,t3⋅cosϕ)Tt′=tx′w+tyw+tzw=(t1,t2⋅cosϕ−t3⋅sinϕ,t2⋅sinϕ+t3⋅cosϕ)T
故
Rx=[1000cosϕ−sinϕ0sinϕcosϕ]R_x= \begin{bmatrix} 1&0&0\\ 0&cos\phi&-sin\phi\\ 0&sin\phi&cos\phi \end{bmatrix} Rx=⎣⎡1000cosϕsinϕ0−sinϕcosϕ⎦⎤
同理，可计算得
Ry=[cosθ0sinθ010−sinθ0cosθ],Rz=[cosψ−sinψ0sinψcosψ0001]R_y= \begin{bmatrix} cos\theta&0&sin\theta\\ 0&1&0\\ -sin\theta&0&cos\theta \end{bmatrix}, R_z= \begin{bmatrix} cos\psi&-sin\psi&0\\ sin\psi&cos\psi&0\\ 0&0&1 \end{bmatrix} Ry=⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤,Rz=⎣⎡cosψsinψ0−sinψcosψ0001⎦⎤
则向量ttt依次绕x,y,zx,y,zx,y,z轴旋转的旋转矩阵为
R=RzRyRxR=R_zR_yR_x R=RzRyRx

故机体坐标系下的向量ttt，变换到世界坐标系下向量t′t't′，变换矩阵为R=RzRyRxR=R_zR_yR_xR=RzRyRx。

3.2 根据四旋翼加速度解算姿态角

如果坐标系、欧拉角的定义、坐标轴旋转的方向等不同，会导致旋转矩阵的计算结果不同，故3.1中的结果不能直接搬移到其他坐标系中。AirSim中，世界坐标系x,y,zx,y,zx,y,z三个坐标轴的朝向分别为北、东、地，机体坐标系z,y,zz,y,zz,y,z三个坐标轴的朝向分别为前、右、下，滚转角、俯仰角、偏航角的旋转方向分别为顺时针、顺时针、逆时针。按照3.1中的计算方法，计算得三个旋转矩阵为
Rx=[1000cosϕ−sinϕ0sinϕcosϕ]，Ry=[cosθ0−sinθ010sinθ0cosθ],Rz=[cosψsinψ0−sinψcosψ0001]R_x= \begin{bmatrix} 1&0&0\\ 0&cos\phi&-sin\phi\\ 0&sin\phi&cos\phi \end{bmatrix}， R_y= \begin{bmatrix} cos\theta&0&-sin\theta\\ 0&1&0\\ sin\theta&0&cos\theta \end{bmatrix}, R_z= \begin{bmatrix} cos\psi&sin\psi&0\\ -sin\psi&cos\psi&0\\ 0&0&1 \end{bmatrix} Rx=⎣⎡1000cosϕsinϕ0−sinϕcosϕ⎦⎤，Ry=⎣⎡cosθ0sinθ010−sinθ0cosθ⎦⎤,Rz=⎣⎡cosψ−sinψ0sinψcosψ0001⎦⎤
这里只考虑无人机水平运动的情况，即无人机保持一定高度，只存在水平方向的加速度，竖直方向加速度为0。

在机体坐标系下，无人机的四个旋翼产生的升力沿−x-x−x方向，故加速度ab=(0,0,−k)Ta^b=(0,0,-k)^Tab=(0,0,−k)T，kkk为未知参数。

将向量aba^bab变换到世界坐标系下，
aw=Rab=−k⋅(m1,m2,cosϕ⋅cosθ)T(m1,m2)T=[cosψsinψ−sinψcosψ]⋅[−sinθ⋅cosϕ−sinϕ]a^w=Ra^b=-k·(m_1,m_2,cos\phi·cos\theta)^T \\(m_1,m_2)^T= \begin{bmatrix} cos\psi&sin\psi\\ -sin\psi&cos\psi \end{bmatrix}· \begin{bmatrix} -sin\theta·cos\phi\\ -sin\phi \end{bmatrix} aw=Rab=−k⋅(m1,m2,cosϕ⋅cosθ)T(m1,m2)T=[cosψ−sinψsinψcosψ]⋅[−sinθ⋅cosϕ−sinϕ]
在世界坐标系下，无人机同时受到重力和旋翼的升力，其总加速度为
a=g+aw=(−k⋅m1,−k⋅m2,−k⋅cosϕ⋅cosθ+g)Ta=g+a^w=(-k·m_1,-k·m_2,-k·cos\phi·cos\theta+g)^T a=g+aw=(−k⋅m1,−k⋅m2,−k⋅cosϕ⋅cosθ+g)T
由于无人机在zzz轴上加速度为0，故
−k⋅cosϕ⋅cosθ+g=0k=gcosϕ⋅cosθ-k·cos\phi·cos\theta+g=0 \\k=\frac{g}{cos\phi·cos\theta} −k⋅cosϕ⋅cosθ+g=0k=cosϕ⋅cosθg
无人机在其他两个方向的加速度为
KaTeX parse error: No such environment: align at position 8: \begin{̲a̲l̲i̲g̲n̲}̲ (a_x,a_y)^T&= …

然后可以根据加速度解算出无人机的姿态角。

3.3 AirSim中四旋翼的姿态角控制

解算出四旋翼的姿态角后，可使用如下的函数控制；

moveByRollPitchYawZAsync(roll, pitch, yaw, z, duration, vehicle_name)

四个参数分别为滚转角、俯仰角、偏航角、高度、持续时间。

综上，使用LQR实现无人机轨迹跟踪的算法流程为：

参考来源：

四旋翼轨迹跟踪：https://zhuanlan.zhihu.com/p/394491146

连续时间LQR控制算法原理推导：https://blog.csdn.net/weixin_42301220/article/details/124542242

离散时间无限时间系统LQR控制算法推导：https://www.bilibili.com/video/BV1P54y1m7CZ?share_source=copy_web