预测控制（二）：NMPC轨迹跟踪
预测控制（二）：NMPC轨迹跟踪
本文先讲解NMPC如何应用于差速机器人，然后给出python实现的主要代码。
MPC缺陷
MPC的缺陷主要在于线性化和路径上：线性化会使预测出现误差；同时对路径点要求比较苛刻，需要路径上每一个点都有机器人的全部运动学信息，这样的路径应该是比较光滑的，而一般我们得到的路径都很难达到这样的要求。
例如ROS的movebase功能包的全局路径是在栅格地图上规划得到的路径，路径上的每一个点都是栅格点，很明显，这个栅格点不是通过满足机器人运动信息规划出来的，想要让机器人每一时刻都走在栅格点上是很难的。同时路径很不光滑，从正方向转到斜方向上会在一个时间内转过45°，栅格距离一般是0.05m，如果时间设置是0.1s的话，那旋转的角速度是7.8rad/s，一秒内转过两圈多，不现实。（不过TEB算法有将全局路径转化为一个个满足机器人运动学约束的路径点的方法）
NMPC原理
因此，我们需要一个能适合各种路径的预测控制方法，同时最好不要线性化，就使用非线性模型，这就是NMPC。
NMPC轨迹跟踪原理和MPC类似，区别在于，NMPC使用非线性模型进行预测控制；使用三次函数对路径进行拟合，得到光滑路径；MPC中使用与路径点的位置误差最小作为求解目标，而NMPC将机器人位置与路径的横向误差和航向误差最小作为求解目标。
路径拟合和初始误差计算
要计算误差，使用机器人坐标系最简单，因此首先将路径点从世界坐标系转到机器人坐标系下
在机器人坐标系下，路径点的坐标(dxx,dyy)和在世界坐标系下的坐标(dx,dy)关系为：
[ d x x d y y ] = [ c o s θ 0 s i n θ 0 − s i n θ 0 c o s θ 0 ] [ d x d y ] \begin{bmatrix} dxx\\ dyy \end{bmatrix} = \begin{bmatrix} cos\theta_{0} & sin\theta_{0} \\ -sin\theta_{0} & cos\theta_{0} \end{bmatrix}\begin{bmatrix} dx\\ dy \end{bmatrix} [dxxdyy]=[cosθ0−sinθ0sinθ0cosθ0][dxdy]
得到机器人坐标系下的路径点后，需要进行路径拟合，得到光滑的路径，一般来说，使用路径的前N个点，拟合函数采用三次函数，最后可以得到路径的表达式：
f ( x ) = a x 3 + b x 2 + c x + d f(x)=ax^{3}+bx^{2}+cx+d f(x)=ax3+bx2+cx+d
初始的横向误差和角度误差如下，机器人初始肯定在机器人坐标系原点，同时机器人坐标系的x轴一直与机器人朝向一致，因此横向误差就是将x=0代入路径表达式，角度误差为将x=0代入路径导数的表达式在求反正切值。
c t e 0 = d e t h e t a 0 = tan − 1 c cte0 = d \\ etheta0 = \tan^{-1}c cte0=detheta0=tan−1c
非线性模型
机器人的非线性模型如下：
[ x k y k θ k ] = [ x k − 1 + v k t cos θ k − 1 y k − 1 + v k t sin θ k − 1 θ k − 1 + w k t ] \begin{bmatrix} x_{k}\\ y_{k}\\ \theta_{k} \end{bmatrix}=\begin{bmatrix} x_{k-1}+v_{k}t\cos\theta_{k-1}\\ y_{k-1}+v_{k}t\sin\theta_{k-1}\\ \theta_{k-1}+w_{k}t \end{bmatrix} ⎣ ⎡xkykθk⎦ ⎤=⎣ ⎡xk−1+vktcosθk−1yk−1+vktsinθk−1θk−1+wkt⎦ ⎤
误差为：
[ c t e k e t h e t a k ] = [ f ( x k ) − y k tan − 1 [ f ′ ( x k ) ] − θ k ] \begin{bmatrix} cte_{k}\\ etheta_{k} \end{bmatrix}=\begin{bmatrix} f(x_{k})-y_{k} \\ \tan^{-1} [f'(x_{k})]-\theta _{k} \end{bmatrix} [ctekethetak]=[f(xk)−yktan−1[f′(xk)]−θk]
目标函数：
min J U = ∑ i = 1 n [ q 1 ( c t e i − c t e r e f ) 2 + q 2 ( e t h e t a i − e t h e t a r e f ) 2 + q 3 ( v i − v r e f ) 2 + q 4 ( v i 2 + w i 2 ) + q 5 ( ( v i + 1 − v i ) 2 + ( w i + 1 − w i ) 2 ) \underset{ U}{\min J} =\sum_{i=1}^{n}[q_{1}(cte_{i}- cte_{ref})^2+q_{2}(etheta_{i}- etheta_{ref})^2\\+q_{3}(v_{i}- v_{ref})^2 +q_{4}(v_{i}^{2}+w_{i}^{2})\\+q_{5}((v_{i+1}-v_{i})^2+(w_{i+1}-w_{i})^2) UminJ=i=1∑n[q1(ctei−cteref)2+q2(ethetai−ethetaref)2+q3(vi−vref)2+q4(vi2+wi2)+q5((vi+1−vi)2+(wi+1−wi)2)
其中， [ q 1 , q 2 , q 3 , q 4 , q 5 ] [q_{1},q_{2},q_{3},q_{4},q_{5}] [q1,q2,q3,q4,q5] 是权重，这里就不是矩阵了，是一个一维常数。目标函数中，q1项和q2项为横向误差和航向误差， r e f ref ref 是参考值，一般设0；q3项为速度误差，这里是为了保持目标速度；q4是为了使控制量最小，q5是控制量的变化量最小。
大概原理就是这样，预测n步，计算误差，求解非线性优化函数使误差最小。在MPC中，我们是通过参考速度计算矩阵，然后在反推出最优控制速度的，但在NMPC中，没有参考速度，所以就只能交给计算机来算了。
NMPC python实现
python+pyomo的实现非线性优化
该部分参考博客 无人驾驶MPC轨迹跟踪，Turtlebot+ROS MPC轨迹跟踪
这里首先要用到非线性优化问题求解器ipopt，ipopt下载地址，这里是集成好的，无需安装，直接调用即可，然后要安装pyomo，如果是python2的话，得安装低一点的版本。
pip install pyomo # python3
pip install Pyomo==5.7.3 # python2
pyomo建模代码如下：
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from pyomo.environ import *
from pyomo.dae import *
path = "~/ipopt-linux64/ipopt" # 求解器路径
import pyutilib.subprocess.GlobalData
pyutilib.subprocess.GlobalData.DEFINE_SIGNAL_HANDLERS_DEFAULT = False
N = 19 # forward predict steps
ns = 5 # state numbers / here: 1: x, 2: y, 3: theta, 4: cte, 5: etheta
na = 2 # actuator numbers /here: 1: velocity, 2: omiga
class MPC(object):
def __init__(self):
m = ConcreteModel()
m.sk = RangeSet(0, N-1)
m.uk = RangeSet(0, N-2)
m.uk1 = RangeSet(0, N-3)
'''权重和参考量'''
m.wg = Param(RangeSet(0, 3), initialize={0:1., 1:10., 2:100., 3:1000}, mutable=True)
m.dt = Param(initialize=0.1, mutable=True)
m.ref_v = Param(initialize=0.4, mutable=True)
m.ref_cte = Param(initialize=0.0, mutable=True)
m.ref_epsi = Param(initialize=0.0, mutable=True)
'''初始状态量和路径函数'''
m.s0 = Param(RangeSet(0, ns-1), initialize={0:0., 1:0., 2:0., 3:0., 4:0.}, mutable=True)
m.coeffs = Param(RangeSet(0, 3),
initialize={0:-0.000458316, 1:0.00734257, 2:0.0538795, 3:0.080728}, mutable=True)
'''设置矩阵存放变量'''
m.s = Var(RangeSet(0, ns-1), m.sk)
m.f = Var(m.sk)
m.psides = Var(m.sk)
m.uv = Var(m.uk, bounds=(-0.01, 0.4))
m.uw = Var(m.uk, bounds=(-1.5, 1.5))
'''模型'''
# 0: x, 1: y, 2: psi, 3: cte, 4: epsi
'''初始状态存入矩阵'''
m.s0_update = Constraint(RangeSet(0, ns-1), rule = lambda m, i: m.s[i,0] == m.s0[i])
'''矩阵中的状态变量更新，填满上边初始化的矩阵'''
m.x_update = Constraint(m.sk, rule=lambda m, k:
m.s[0,k+1]==m.s[0,k]+m.uv[k]*cos(m.s[2,k])*m.dt
if k<N-1 else Constraint.Skip)
m.y_update = Constraint(m.sk, rule=lambda m, k:
m.s[1,k+1]==m.s[1,k]+m.uv[k]*sin(m.s[2,k])*m.dt
if k<N-1 else Constraint.Skip)
m.psi_update = Constraint(m.sk, rule=lambda m, k:
m.s[2,k+1]==m.s[2,k]+ m.uw[k]*m.dt
if k<N-1 else Constraint.Skip)
m.f_update = Constraint(m.sk, rule=lambda m, k:
m.f[k]==m.coeffs[0]*m.s[0,k]**3+m.coeffs[1]*m.s[0,k]**2+
m.coeffs[2]*m.s[0,k]+m.coeffs[3])
m.psides_update = Constraint(m.sk, rule=lambda m, k:
m.psides[k]==atan(3*m.coeffs[0]*m.s[0,k]**2
+2*m.coeffs[1]*m.s[0,k]+m.coeffs[2]))
m.cte_update = Constraint(m.sk, rule=lambda m, k:
m.s[3,k+1]==(m.f[k]-m.s[1,k]+m.uv[k]*sin(m.s[2,k])*m.dt)
if k<N-1 else Constraint.Skip)
m.epsi_update = Constraint(m.sk, rule=lambda m, k:
m.s[4, k+1]==m.psides[k]-m.s[2,k]+m.uw[k]*m.dt
if k<N-1 else Constraint.Skip)
'''构建目标函数'''
m.cteobj = m.wg[3]*sum((m.s[3,k]-m.ref_cte)**2 for k in m.sk)
m.epsiobj = m.wg[3]*sum((m.s[4,k]-m.ref_epsi)**2 for k in m.sk)
m.vobj = m.wg[2]*sum((m.uv[k]-m.ref_v)**2 for k in m.uk)
m.uvobj = m.wg[1]*sum(m.uv[k]**2 for k in m.uk)
m.uwobj = m.wg[1]*sum(m.uw[k]**2 for k in m.uk)
m.sudobj = m.wg[0]*sum((m.uv[k+1]-m.uv[k])**2 for k in m.uk1)
m.suaobj = m.wg[0]*sum((m.uw[k+1]-m.uw[k])**2 for k in m.uk1)
'''目标函数'''
m.obj = Objective(expr = m.cteobj+m.epsiobj+m.vobj+m.uvobj+m.uwobj+m.sudobj+m.suaobj, sense=minimize)
self.iN = m#.create_instance()
def Solve(self, state, coeffs):
'''赋初值'''
self.iN.s0.reconstruct({0:state[0], 1: state[1], 2:state[2], 3:state[3], 4:state[4]})
self.iN.coeffs.reconstruct({0:coeffs[0], 1:coeffs[1], 2:coeffs[2], 3:coeffs[3]})
self.iN.f_update.reconstruct()
self.iN.s0_update.reconstruct()
self.iN.psides_update.reconstruct()
'''调用求解器求解'''
SolverFactory('ipopt',executable=path).solve(self.iN)
x_pred_vals = [self.iN.s[0,k]() for k in self.iN.sk]
y_pred_vals = [self.iN.s[1,k]() for k in self.iN.sk]
pre_path = np.zeros((N,2))
pre_path[:,0] = np.array(x_pred_vals)
pre_path[:,1] = np.array(y_pred_vals)
v = self.iN.uv[0]()
w = self.iN.uw[0]()
'''返回参考路径和控制量'''
return pre_path, v, w
主函数NMPC部分如下：
...
# 获取机器人此时的状态
pose = np.array([carPose.position.x,carPose.position.y,self.getYaw(carPose)])
# 计算角度的cos和sin值用于路径点坐标系变换
costheta = np.cos(pose[2])
sintheta = np.sin(pose[2])
d = len(path) # path为世界坐标系下的路径
path_ = np.zeros((2,d)) # path_为转换后的路径初始化
for i in range(d):
# 取一个坐标点
map_path_pose = path[i]
# 转换
dx = odom_path_pose.pose.position.x - pose[0]
dy = odom_path_pose.pose.position.y - pose[1]
path_[0,i] = dx * costheta + dy * sintheta
path_[1,i] = dy * costheta - dx * sintheta
# 路径拟合
p1 = np.polyfit(path_[0,:],path_[1,:],3)
# 初始误差
cte = p1[3]
etheta = np.arctan(p1[2])
s0 = np.array([0.0, 0.0, 0.0, cte, etheta])
# 调用求解器
pre_path, vhat, what = myMPC.Solve(s0, p1)
print([vhat,what])
# 得到控制量
cmd_vel.linear.x = vhat
cmd_vel.angular.z = what
...
结论
就不放动图演示了，直接说结论吧：1、在开阔地带的话，效果很不错，但是在比较狭窄的地方，例如两边墙壁刚刚比机器人长度宽10厘米左右，还是比较容易撞墙。2、关于权重，一般来说，权重的话肯定是cte和etheta最大，这样的话速度参考值其实没啥意义，反而是速度的限制范围有用，比如 v r e f = 0.4 v_{ref}=0.4 vref=0.4，但 v b o u n d = [ 0 , 0.6 ] v_{bound}=[0,0.6] vbound=[0,0.6]，这样的话速度都是0.6，所以尽量让 v r e f = v m a x v_{ref}=v_{max} vref=vmax吧。