目录
- 一、前言
- 二、轨迹的跟随控制策略
- （1）利用模型预测控制（MPC）的思想控制
- （2） 仿真验证
一、前言
本篇我们依然试着用一些浅显的数学知识，来研究和实现一下常用机器人小车（如AGV）的控制，这里的小车我们先选用二轮驱动的差速小车，即通过两个驱动轮的转速控制实现所有想要的运动。我们会首先对这类小车的运动原理进行一些分析，并通过分析得出的数学步骤，用python去实现机器人小车的正向控制算法、反向控制算法（或者轨迹跟踪），并在此基础上，去尝试实现一下固定场景下，如仓储搬运、工厂转运等的机器人小车的轨迹规划、自动泊车（通常是去充电），并将首先在vrep仿真环境验证，验证完后，可以自己搭建实物去实现。
二、轨迹的跟随控制策略
在上篇我们通过对小车的运动特征分析，通过少量的数学知识构建了小车的正向运动模型，即，给小车输入两个轮子的转速，小车会运动并形成相应的轨迹，并提供了python源码。在实际小车运动控制中，光有正向模型是不够的。我们希望是小车机器人能够按照我们所需要的路线去运动，比如，让它按照指定的路线去仓库某个库位，准确到位，然后通过如机器臂，抓取到相应的货物装上小车，然后再按照我们规定的路线把货物搬运到目的地，如果小车缺电了，又可以根据我们充电桩的位置，自动规划路线然后按照这样的路线运动到充电位置充电，诸如此类的问题。
完整的智能小车应该可以自主完成路径的规划、路径的跟随、避障绕行、自动充电管理等任务。这些任务都非常有挑战性。需要通过一系列的文章和试验去逐步完成。
本节我们首先来解决这样的一个问题：已知一条任意的曲线（即小车要运动的轨迹线），小车接下来如何输出左右两个轮子的转速组合，或者说是控制左右两个轮子的转速值，去很好地逼近给定地轨迹线（在允许的误差范围内）。
（1）利用模型预测控制（MPC）的思想控制
所谓模型预测控制（MPC）是运用现代控制理论，通过构建预测模型，来预测未来多个采样时刻后的一些列状态点，并查看未来预测的系列轨迹与参考轨迹进行比较，通常可以计算均方差衡量，像这样在这个时刻，改变控制量，再预测相同采样时刻后的一系列状态点，并评估，预测多次后，把评估结果最优的一组控制量作为当前时刻的控制量输入，如此重复，这样就形成了模型预测控制的方式。
于是我们根据以上这样的控制思想设计了小车的控制程序，主要的流程如下：
如上图所述，
（1）首先我们定义一根光滑的轨迹曲线，这里我们暂时用样条曲线代替（关于轨迹曲线的生成和规划也是自动驾驶一个核心的复杂算法，我们将在后面章节专门探讨这个问题），python的代码如下：
import math
import matplotlib.pyplot as plt
import numpy as np
import pylab as pl
from scipy import interpolate
from pylab import mpl
xn = [0, 2,4,11,12]
yn = [0,-1,2,0,7]
#生成样条曲线
tck = interpolate.splrep(xn, yn)
#y_bspline = interpolate.splev(x_new, tck)
xnow=xn[0]
ynow=yn[0]
new_data_x1=np.linspace(xn[0], xn[-1:][0], 100)
y_bspline = interpolate.splev(new_data_x1, tck)
plt.plot(new_data_x1,y_bspline,color="blue")
运行结果如下：
（2）其次，我们获取起点的斜率角，作为小车的起点方向角：
#求切角
x0 = xn[0]
i0 = np.argmin(np.abs(new_data_x1-x0))
x1 = new_data_x1[i0:i0+2]
y1 = y_bspline[i0:i0+2]
dydx= np.diff(y1)/np.diff(x1)
tngnt = lambda x: dydx*x + (y1[0]-dydx*x1[0])
thetanow=np.arctan2(np.diff(y1),np.diff(x1))#此为方向角
（3）然后，我们要进行预测，这里我们假设小车的线速度始终是匀速的，有之前的运动分析，即sl+sr为恒定值vc（已知）。这样，其实我们可以将控制变量原来的两个，变成一个sl，则sr=vc-sl。我们以增量0.05，来生成一些列的控制量[sl-0.05n,…,sl-0.052,sl-0.051,sl+0.050,sl+0.051,sl+0.052,…sl+0.05*n]，总共2n+1个，然后将这些控制量全部进行预测，获取预测的3个采样周期后的状态点序列，并与参考曲线进行比较，得出方差最小的那个控制量最为当前时刻的输入量：
def predicate(x,y,theta,sl,sr=0.75,step=3,dt=0.05,vc=1.5):
"""
Parameters
----------
x : TYPE
当前状态x.
y : TYPE
当前状态y.
theta : TYPE
当前状态小车方向角.
sl : TYPE
当前需要预测的控制量，左轮的转速.
sr : TYPE, optional
右轮的转速. The default is 0.75.
step : TYPE, optional
预测的未来步长. The default is 3.
dt : TYPE, optional
采样周期. The default is 0.05.
vc : TYPE, optional
默认的小车恒定线速度. The default is 1.5.
Returns
-------
xx : TYPE
预测的状态序列x.
yy : TYPE
预测的状态序列y.
"""
xx=[]
yy=[]
for i in range(step):#循环计算step个时刻的状态量
sr=vc-sl#根据线速度恒定，得出右轮转速
#计算下一时刻的状态量
res,x_next,y_next,theta_next=NextState(x, y, theta, sl,sr,dt=dt)
if res:
xx.append(x_next)
yy.append(y_next)
tt.append(theta_next)
x=x_next
y=y_next
theta=theta_next
else:
break
return xx,yy
def best_move(sl,tck,x,y,theta):
"""
计算当前时刻，最优的控制量
Parameters
----------
sl : TYPE
当前控制量.
tck : TYPE
样条曲线的函数.
x : TYPE
当前的状态x.
y : TYPE
当前的状态y.
theta : TYPE
当前的状态theta.
Returns
-------
TYPE
获取最佳控制量，和最小均方差.
"""
#控制增量
dvl=0.05
#要预测的控制量
sl_n=sl
#方差
e=10000
#最优的控制量
best_sl=sl
for i in range(10):#逐次减小控制量
sl_n=sl_n-i*dvl
#待入预测函数获取预测序列
xx,yy=predicate(x,y,theta,sl_n,dt=dt)
#获取前面的点，此处待优化
y_ref = interpolate.splev(xx, tck)
ei=np.sum((np.array(yy)-np.array(y_ref))**2)
if ei<e:
e=ei
best_sl=sl_n
sl_n=sl
for i in range(10):#逐次增大控制量
sl_n=sl_n+i*dvl
xx,yy=predicate(x,y,theta,sl_n,dt=dt)
y_ref = interpolate.splev(xx, tck)
ei=np.sum((np.array(yy)-np.array(y_ref))**2)
if ei<e:
e=ei
best_sl=sl_n
#返回最优控制量
return best_sl,np.sqrt(e)
（3）最后我们每次将最优的控制量最为当前时刻的控制输入量，驱动小车运动，并画出轨迹，得到了一个逼近参考曲线的小车运动轨迹：
for i in range(100):
xi=xnow
yi=ynow
thei=thetanow
b_sl,ei=best_move(sl,tck,xi,yi,thei)
res,x_next,y_next,theta_next=NextState(xi, yi, thei, b_sl,1.5-b_sl,dt=dt)
if res:
fix.append(x_next)
fiy.append(y_next)
xnow=x_next
ynow=y_next
thetanow=theta_next
bv.append(b_sl)
e.append(ei)
运行结果如下
图中蓝色是参考曲线，红色是小车的运动轨迹，绿色线是左轮的转速，紫色是误差。可以看出，利用模型预测控制的思想控制差速小车是可行的。
接下来，我们要通过仿真来验证一下这个算法
（2） 仿真验证
（未完待续…）
