python机器人编程——差速机器人小车的控制，控制模型、轨迹跟踪，轨迹规划、自动泊车（中）未完待续..._差速轮小车mpc控制

一、前言

本篇我们依然试着用一些浅显的数学知识，来研究和实现一下常用机器人小车（如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")
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

运行结果如下：

（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))#此为方向角
1
2
3
4
5
6
7
8

（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)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101

（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)
1
2
3
4
5
6
7
8
9
10
11
12
13
14

运行结果如下

图中蓝色是参考曲线，红色是小车的运动轨迹，绿色线是左轮的转速，紫色是误差。可以看出，利用模型预测控制的思想控制差速小车是可行的。
接下来，我们要通过仿真来验证一下这个算法

（2） 仿真验证

仿真验证见另一篇博文：python机器人编程——差速AGV机器、基于视觉和预测控制的循迹、自动行驶（下篇）