为您推荐:
精华内容
最热下载
问答
  • 5星
    230KB weixin_39199083 2021-05-15 10:34:08
  • Turtlebot+ROS Stage仿真环境实现MPC轨迹跟踪1. 仿真环境设置1.1 更改stage环境地图1.2 获取要跟踪的轨迹1.2.1 获取路点1.2.2 拟合路点得到机器人要跟踪的路线2. Turtlebot的MPC轨迹跟踪问题3. 程序与运行结果4. 写...


    本文为CSDN博主「 风起了」的原创文章
    原文链接:https://blog.csdn.net/u013468614/article/details/104139317

    1. 仿真环境设置

    1.1 更改stage环境地图

    ubuntu 18.04+ros melodic

    首先,请参照install turtlebot on ubuntu 18.04 + ros melodic在melodic版本的ROS中安装好turtlebot。
    然后,参照turtlebot_stageTutorialsindigoCustomizing the Stage Simulator熟悉如何在stage仿真环境中使用自己的地图,以及如何在仿真环境中增减障碍物。stage仿真环境设置过程中重要的三个文件后缀分别为:“.png,.world,.yaml”。其中".png"为地图的图片,".world"为整个环境的配置文件(包括地图,机器人,障碍物等),".yaml"为地图的配置文件。因此,更改要将turtlebot stage中的原始地图更改成自己的地图,只需要将.png与.yaml文件替换原始的.png与.yaml文件。
    例如:

    1.test.png图片为
    在这里插入图片描述
    2.test.yaml文件内容如下

    image: test.png
    resolution: 0.05
    origin: [0.0, 0.0, 0.0]
    negate: 0
    occupied_thresh: 0.65
    free_thresh: 0.196
    

    3.stage/test.world文件内容如下:

    include "turtlebot.inc"
    include "myBlock.inc"
    
    
    define floorplan model
    (
      # sombre, sensible, artistic
      color "gray30"
    
      # most maps will need a bounding box
      boundary 1
    
      gui_nose 0
      gui_grid 0
      gui_outline 0
      gripper_return 0
      fiducial_return 0
      laser_return 1
    )
    
    resolution 0.02
    interval_sim 100  # simulation timestep in milliseconds
    
    window
    (
    #  size [ 600.0 700.0 ]
      size [ 600.0 600.0 ]
      center [ 0.0 0.0 ]
      rotate [ 0.0 0.0 ]
      scale 60
    )
    
    floorplan
    (
      name "test"
      bitmap "../test.png"
      size [ 15.0 15.0 2.0 ]
      pose [  7.5  7.5 0.0 0.0 ]
    )
    
    
    # throw in a robot
    turtlebot
    (
      pose [ 2.0 1.5 0.0 0.0 ]
      name "turtlebot"
      color "green"
    )
    
    

    4.在.bashrc文件(打开.bashrc文件在终端使用命令gedit ~/.bashrc)中未尾添加如下内容,替换原始stage环境

    export TURTLEBOT_STAGE_MAP_FILE=/program/turtlebot_world/test.yaml
    export TURTLEBOT_STAGE_WORLD_FILE=/program/turtlebot_world/stage/test.world
    

    5.运行roslaunch turtlebot_stage turtlebot_in_stage.launch,发现环境地图已经成功更改为自己的地图。
    在这里插入图片描述
    在这里插入图片描述

    1.2 获取要跟踪的轨迹

    1.2.1 获取路点

    点击rviz工具栏中的"+"号,在新出现的界面中选择“PublishPoint”,点击OK退出该界面。
    在这里插入图片描述
    上面工具栏就会多出一个“Publish Point”,每点击一次该按钮,就可以在RVIZ界面中点击任意位置,并通过名称为“/clicked_point”的Topic发出该位置在“/map”坐标系下的位置(x,y)。
    在这里插入图片描述
    可采用如下程序来依次记录选取的路点(保存为“wps.txt”):

    import rospy
    import numpy as np
    
    from geometry_msgs.msg import PointStamped
    
    class Turtlebot_core():
        def __init__(self):
            rospy.init_node("Turtlebot_core", anonymous=True)
            self.wps_buf = []
            
            rospy.Subscriber("/clicked_point", PointStamped, self.wpsCallback, queue_size=1)
            rospy.spin()            
                
        def wpsCallback(self, data):
            p1 = np.zeros(2)
            p1[0] = data.point.x
            p1[1] = data.point.y
            self.wps_buf.append(p1)
            np.savetxt("wps.txt", np.array(self.wps_buf))
            print("save way point success!!!")
            print("p:"+str(p1))
            
    
            
    if __name__ == "__main__":
        turtlebot_core = Turtlebot_core()   
    
    

    wps.txt文件内容如下:

    2.775404453277587891e+00 1.849611759185791016e+00
    3.828148126602172852e+00 1.734118461608886719e+00
    5.651257514953613281e+00 1.688370704650878906e+00
    7.475209712982177734e+00 1.680246791839599609e+00
    9.300391197204589844e+00 1.697043418884277344e+00
    1.103435707092285156e+01 1.695110073089599609e+00
    1.266803169250488281e+01 1.93492584228515625e+00
    1.315559005737304688e+01 3.573270320892333984e+00
    1.319308471679687500e+01 5.110162258148193359e+00
    1.324398708343505859e+01 6.808045387268066406e+00
    1.320587062835693359e+01 8.812158584594726562e+00
    1.317775154113769531e+01 1.039849281311035156e+01
    1.308286705017089844e+01 1.215956592559814453e+01
    1.269116973876953125e+01 1.304349098205566406e+01
    1.132855510711669922e+01 1.330544090270996094e+01
    1.001482582092285156e+01 1.338727951049804688e+01
    8.556406974792480469e+00 1.343830871582031250e+01
    6.953704357147216797e+00 1.346025466918945312e+01
    5.448281288146972656e+00 1.340069961547851562e+01
    3.750601768493652344e+00 1.338894081115722656e+01
    2.197587871551513672e+00 1.309178924560546875e+01
    1.648979549407958984e+00 1.113714027404785156e+01
    1.638838768005371094e+00 8.914575576782226562e+00
    1.582886219024658203e+00 6.895450115203857422e+00
    1.609868049621582031e+00 5.023091793060302734e+00
    1.697061538696289062e+00 2.892292022705078125e+00
    2.775404453277587891e+00 1.849611759185791016e+00
    
    

    1.2.2 拟合路点得到机器人要跟踪的路线

    import numpy as np
    from scipy.interpolate import interp1d
    wps = np.loadtxt("wps.txt")
    x = wps[:,0]
    y = wps[:,1]
    t = np.linspace(0, 1, num=len(x))
    f1 = interp1d(t,x,kind='cubic')
    f2 = interp1d(t,y,kind='cubic')
    newt = np.linspace(0,1,100)
    newx = f1(newt)
    newy = f2(newt)
    
    
    import matplotlib.pyplot as plt
    %matplotlib inline
    
    plt.scatter(x, y)
    plt.plot(newx, newy)
    plt.show()
    
    

    在这里插入图片描述

    2. Turtlebot的MPC轨迹跟踪问题

    与无人车不同,Turtlebot是基于左右轮差速来达到转弯,前行运动的。ROS中的Turtlebot包中的控制项有线速度vvv与角速度www。Turtlebot的运动学模型如下:

    x ˙ = v c o s ( θ ) y ˙ = v s i n ( θ ) θ ˙ = w \dot{x}=vcos(\theta)\\\dot{y}=vsin(\theta)\\θ˙=w x˙=vcos(θ)y˙=vsin(θ)θ˙=w

    首先我们需要将以上连续的微分模型离散化成差分模型(差分间隔为 d t dt dt):
    x k + 1 = x k + v k cos ⁡ ( θ k ) d t y k + 1 = y k + v k sin ⁡ ( θ k ) d t θ k + 1 = θ k + w k d t cte k + 1 = cte k + v k sin ⁡ ( θ k ) d t epsi k + 1 = epsi k + w k d t (2) \begin{matrix}x_{k+1}=x_k+v_k\cos(\theta_k)d_t \\ y_{k+1}=y_k+v_k\sin(\theta_k)d_t \\ \theta_{k+1}=\theta_{k}+w_k d_t \\\text{cte}_{k+1} = \text{cte}_k+v_k \sin (\theta_k)d_t \\\text{epsi}_{k+1}=\text{epsi}_k+w_kd_t\end{matrix} \tag{2} xk+1=xk+vkcos(θk)dtyk+1=yk+vksin(θk)dtθk+1=θk+wkdtctek+1=ctek+vksin(θk)dtepsik+1=epsik+wkdt(2)

    其中,cte\text{cte}cte与epsi\text{epsi}epsi的计算法公式详见无人车系统(十一):轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】。

    对于一个预测步长为NNN的MPC控制器求解问题,可以设计以下优化目标函数:
    min  J = ∑ k = 1 N ( ω cte ∣ ∣ cte t ∣ ∣ 2 + ω epsi ∣ ∣ epsi k ∣ ∣ 2 + ω v 1 ∣ ∣ v k − v ref ∣ ∣ 2 ) + ∑ k = 1 N − 1 ( ω w ∣ ∣ w k ∣ ∣ 2 + ω v 2 ∣ ∣ v k ∣ ∣ 2 ) + ∑ k = 1 N − 2 ( ω rate w ∣ ∣ w k + 1 − w k ∣ ∣ 2 + ω rate v ∣ ∣ v k + 1 − v k ∣ ∣ 2 ) (4) \begin{array}{cc} \text{min } &\mathcal{J}=\sum_{k=1}^N(\omega_{\text{cte}}||\text{cte}_t||^2+\omega_{\text{epsi}}||\text{epsi}_k||^2+\omega_{v1} ||v_k-v_{\text{ref}}||^2) \\ & +\sum_{k=1}^{N-1} (\omega_{w}||w_k||^2+\omega_{v2}||v_k||^2) \\ & +\sum_{k=1}^{N-2}(\omega_{\text{rate}_{w}}||w_{k+1}-w_{k}||^2+\omega_{\text{rate}_{v}}||v_{k+1}-v_k||^2) \\ \end{array}\tag{4} min J=k=1N(ωctectet2+ωepsiepsik2+ωv1vkvref2)+k=1N1(ωwwk2+ωv2vk2)+k=1N2(ωratewwk+1wk2+ωratevvk+1vk2)(4)
    满足动态模型约束:
    s.t. x k + 1 = x k + v k c o s ( θ k ) d t , k = 1 , 2 , . . . , N − 1 y k + 1 = y k + v k s i n ( θ k ) d t , k = 1 , 2 , . . . , N − 1 θ k + 1 = θ k + w k d t , k = 1 , 2 , . . . , N − 1 cte k + 1 = f ( x k ) − y k + v k sin ⁡ ( θ k ) d t epsi k + 1 = a r c tan ⁡ ( f ′ ( x k ) ) − θ + w k d t (5) \begin{array}{c}\text{s.t.} & x_{k+1}=x_k+v_kcos(\theta_k)d_t , k=1,2,...,N-1\\ & y_{k+1}=y_k+v_ksin(\theta_k)d_t , k=1,2,...,N-1\\ & \theta_{k+1}=\theta_{k}+w_k d_t , k=1,2,...,N-1\\& \text{cte}_{k+1} =f(x_k)-y_k+v_k \sin (\theta_k)d_t \\& \text{epsi}_{k+1}=arc\tan(f'(x_k))-\theta+w_k d_t\end{array}\tag{5} s.t.xk+1=xk+vkcos(θk)dt,k=1,2,...,N1yk+1=yk+vksin(θk)dt,k=1,2,...,N1θk+1=θk+wkdt,k=1,2,...,N1ctek+1=f(xk)yk+vksin(θk)dtepsik+1=arctan(f(xk))θ+wkdt(5)
    执行器约束:
    v ∈ [ v min , v max ] w ∈ [ w min , w max ] (6) \begin{array}{cc}v\in[v_{\text{min}}, v_{\text{max}}]\\w\in [w_{\text{min}}, w_{\text{max}}]\end{array}\tag{6} v[vmin,vmax]w[wmin,wmax](6)

    3. 程序与运行结果

    在本此仿真中,一些参数如下: ω c t e = ω e p s i = 1000 , ω v 1 = 100 , ω w = ω v 2 = 10 , ω rate v = ω rate w = 1 , v min = − 0.01 , v max = 2.0 v , w min = − 1.5 w , w max = 1.5 w \omega_{cte}=\omega_{epsi}=1000 ,\omega_{v1}=100,\omega_{w}=\omega_{v2}=10,\omega_{\text{rate}_{v}}=\omega_{\text{rate}_{w}}=1,v_{\text{min}}=-0.01,v_{\text{max}}=2.0v,w_{\text{min}}=-1.5w ,w_{\text{max}}=1.5w ωcte=ωepsi=1000,ωv1=100,ωw=ωv2=10,ωratev=ωratew=1,vmin=0.01,vmax=2.0v,wmin=1.5w,wmax=1.5w,向前预测步为 N = 19 N=19 N=19

    import rospy
    import copy
    import tf
    import numpy as np
    from scipy import spatial
    from geometry_msgs.msg import PointStamped
    from geometry_msgs.msg import PoseStamped
    from nav_msgs.msg import Odometry
    from nav_msgs.msg import Path
    from geometry_msgs.msg import Twist
    from pyomo.environ import *
    from pyomo.dae import *
    from scipy.interpolate import interp1d
    import matplotlib
    import matplotlib.pyplot as plt
    %matplotlib inline
    # set up matplotlib
    is_ipython = 'inline' in matplotlib.get_backend()
    if is_ipython:
        from IPython import display
    
    plt.ion()
    
    wps = np.loadtxt("wps.txt")
    x = wps[:,0]
    y = wps[:,1]
    t = np.linspace(0, 1, num=len(x))
    f1 = interp1d(t,x,kind='cubic')
    f2 = interp1d(t,y,kind='cubic')
    newt = np.linspace(0,1,100)
    nwps = np.zeros((100, 2))
    nwps[:,0] = f1(newt)
    nwps[:,1] = f2(newt)
    wpstree = spatial.KDTree(nwps)
    
    def getcwps(rp):
        _, nindex = wpstree.query(rp)
        cwps = np.zeros((5,2))
        for i in range(5):
            cwps[i] = nwps[(nindex+i)%len(nwps)]
            
    #     if (nindex + 5) >= 100:
    #         cwps[0:100-nindex-1] = nwps[nindex:-1]
    #         cwps[100-nindex-1:-1] = nwps[0:nindex+5-100]        
    #     else:
    #         cwps = nwps[nindex:nindex+5]
        return cwps    
    
    def cubic_fun(coeffs, x):
        return coeffs[0]*x**3+coeffs[1]*x**2+coeffs[2]*x+coeffs[0]    
            
    def plot_durations(cwps, prex, prey):
        plt.figure(2)
        plt.clf()
        plt.plot(cwps[:,0],cwps[:,1])
        plt.plot(prex, prey)
        plt.scatter(x, y)
        if is_ipython:
            display.clear_output(wait=True)
            display.display(plt.gcf())
     
            
            
    N = 19 # forward predict steps
    ns = 5  # state numbers / here: 1: x, 2: y, 3: psi, 4: cte, 5: epsi
    na = 2  # actuator numbers /here: 1: steering angle, 2: omega
    
    
    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.5, 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, 2.0))
            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]-0.5)**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').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      
            
            
    class Turtlebot_core():
        def __init__(self):
            rospy.init_node("Turtlebot_core", anonymous=True)
            self.listener = tf.TransformListener()
            rospy.Subscriber("/odom", Odometry, self.odomCallback)
            self.pub_refpath = rospy.Publisher("/ref_path", Path, queue_size=1)
            self.pub_prepath = rospy.Publisher("/pre_path", Path, queue_size=1)
            self.pub_cmd = rospy.Publisher("/cmd_vel_mux/input/teleop", Twist, queue_size=1)
            self.rp = np.zeros(3) 
            self.crv = 0.0
            self.crw = 0.0
            self.mpc = MPC() 
            rate = rospy.Rate(10) # 10HZ
            while not rospy.is_shutdown():
                self.getrobotpose()
                cwps = getcwps(self.rp[0:2])
                px = self.rp[0] + self.crv*np.cos(self.rp[2])*0.1
                py = self.rp[1] + self.crw*np.sin(self.rp[2])*0.1
                psi = self.rp[2] + self.crw*0.1
                
                self.rp[0] = px
                self.rp[1] = py
                self.rp[2] = psi
                
                cwps_robot = np.zeros((len(cwps), 2))
                
                for i in range(len(cwps)):
                    dx = cwps[i,0] - px
                    dy = cwps[i,1] - py
                    
                    cwps_robot[i,0] = dx*np.cos(psi) + dy*np.sin(psi)
                    cwps_robot[i,1] = dy*np.cos(psi) - dx*np.sin(psi)
                    
                coeffs = np.polyfit(cwps_robot[:,0], cwps_robot[:,1], 3)
                cte = cubic_fun(coeffs, 0)
                
                f_prime_x = coeffs[2]
                epsi = np.arctan(f_prime_x)
                s0 = np.array([0.0, 0.0, 0.0, cte, epsi])
                pre_path, v, w = self.mpc.Solve(s0, coeffs)
                self.pub_ref_path(cwps_robot)
                self.pub_pre_path(pre_path)
                self.pub_Command(v, w)
                print(v,w)
    #             plot_durations(cwps, x_pred_vals, y_pred_vals)
                rate.sleep()        
            rospy.spin()            
                
        def getrobotpose(self):
            try:
                (trans,rot) = self.listener.lookupTransform('/map', '/base_link', rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                return   
            self.rp[0] = trans[0]
            self.rp[1] = trans[1]
            r,p,y = tf.transformations.euler_from_quaternion(rot)
            self.rp[2] = y
            
        def odomCallback(self, data):
            self.crv = data.twist.twist.linear.x
            self.crw = data.twist.twist.angular.z
            
        def pub_ref_path(self, ref_path):        
            msg_ref_path = Path()
            msg_ref_path.header.stamp = rospy.Time.now()
            msg_ref_path.header.frame_id = "base_link"
            for i in range(len(ref_path)):
                pose = PoseStamped()
                pose.pose.position.x = ref_path[i,0]
                pose.pose.position.y = ref_path[i,1]
                msg_ref_path.poses.append(copy.deepcopy(pose))
                
            self.pub_refpath.publish(msg_ref_path)
                
        def pub_pre_path(self, pre_path):
            msg_pre_path = Path()
            msg_pre_path.header.stamp = rospy.Time.now()
            msg_pre_path.header.frame_id = "base_link"
            for i in range(len(pre_path)):
                pose = PoseStamped()
                pose.pose.position.x = pre_path[i,0]
                pose.pose.position.y = pre_path[i,1]
                msg_pre_path.poses.append(copy.deepcopy(pose))    
            self.pub_prepath.publish(msg_pre_path)
            
        def pub_Command(self, v, w):
            twist = Twist()
            twist.linear.x = v
            twist.angular.z = w
            self.pub_cmd.publish(twist)     
                    
    if __name__ == "__main__":
        turtlebot_core = Turtlebot_core() 
    
    
    

    结果如下图所示(绿线为预测的轨迹,红线为turtlebot要跟踪的轨迹):
    在这里插入图片描述

    4. 写在后面

    基于MPC的轨迹跟踪方法的效果与所设定的目标函数、目标函数的参数有很大的关系。本文最后的参数都是通过不断的试验得到的,最终turtlebot能够在回形走廊环境中跟踪上要跟踪的轨迹。

    展开全文
    csdn__dongdong 2020-02-08 13:23:29
  • 97KB weixin_38631225 2021-01-06 22:19:37
  • 287KB weixin_42696271 2021-09-11 11:54:29
  • 848KB zxyu1119 2018-01-30 08:53:33
  • 126KB a417478a 2019-01-01 16:41:22
  • weixin_45969183 2021-03-30 14:49:54
  • MPC轨迹跟踪算法学习记录1双移线绘制问题双移线参考轨迹绘制横坐标为X_phi的双移线结果对比 双移线绘制问题 双移线作为一种经典工况,在测试车辆稳定性和无人驾驶轨迹跟踪仿真分析中应用广泛。本人在学习龚老师...

    双移线绘制问题

    双移线作为一种经典工况,在测试车辆稳定性和无人驾驶轨迹跟踪仿真分析中应用广泛。本人在学习龚老师《无人驾驶车辆模型预测控制》第五章时,首先发现配套代码中没有绘制出参考轨迹,这样就不明确追踪效果如何。因此,参考网上案例,调制出了双移线参考轨迹。

    双移线参考轨迹

    这里,本人是将X_predict作为参考双移线的横坐标,纵坐标是Y_ref。

    figure(1);
      plot(X_predict, Y_ref,'r--','LineWidth',1);
      plot(X, Y,'cx','LineWidth',1);
      title('追踪结果对比');
      xlabel('横向位置X(米)');
      axis([0 150 -2 4]);  %坐标轴
      ylabel('纵向位置Y(米)');
      legend('追踪轨迹','参考轨迹');
      hold on;
    

    网上也有博主写到参考双移线的横坐标是X_phi,见下图:
    在这里插入图片描述
    按照网上博主的介绍,如果将X_phi替换X_predict, 总是会出现报错,原因是参考网上的设置很难保证X_phi与Y_ref的向量长度保持一致,原代码中的(X_predict, Y_ref)是在迭代中同时计算出来的,这里本人还没有找出它到底输出的数据长度是多少,如果有同学清楚,烦请指点。

    绘制横坐标为X_phi的双移线

    首先,本人是在MATLAB中将单独绘制出以X_phi为横坐标,Y_ref为纵坐标的双移线,代码如下:

      clc;
      clear all;
      N=300;
      T=0.05;
    
      shape=2.4;%参数名称,用于参考轨迹生成
      dx1=25;dx2=21.95;%没有任何实际意义,只是参数名称
      dy1=4.05;dy2=5.7;%没有任何实际意义,只是参数名称
      Xs1=27.19;Xs2=56.46;%参数名称
      phi_ref=zeros(N,1);%用于保存预测时域内的期望轨迹
      Y_ref=zeros(N,1);%用于保存预测时域内的期望轨迹
      X_phi=0.05:0.5:150;
      X_ref=X_phi';
    
    for p=1:1:N
        %%双移线追踪
          z1=shape/dx1*(X_ref(p,1)-Xs1)-shape/2;
          z2=shape/dx2*(X_ref(p,1)-Xs2)-shape/2;
          Y_ref(p,1)=dy1/2*(1+tanh(z1))-dy2/2*(1+tanh(z2));
          phi_ref(p,1)=atan(dy1*(1/cosh(z1))^2*(1.2/dx1)-dy2*(1/cosh(z2))^2*(1.2/dx2));
          Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)];
    end
    
       figure(1);
      plot(X_ref, Y_ref,'b--','LineWidth',1);
      title('双移线图例');
      xlabel('横向位置X(米)');
      axis([0 150 -2 4]);  %直线工况
      % axis([-50 50 0 100]); 圆形工况
      ylabel('纵向位置Y(米)');
      legend('参考轨迹');
      hold on;
    

    绘制出的参考轨迹如图所示:
    在这里插入图片描述

    结果对比

    这里单独生成的双移线会和之前绘制的以X_predict为横坐标的双移线有差别吗?感兴趣的同学可以把上述代码添加到书上第五章的代码中,这里本人绘制后发现两个参考轨迹并无差别,所以原代码中,直接可以用X_predict绘制横坐标,这样能直接保证它和纵坐标的向量长度保持一致。
    在这里插入图片描述

    展开全文
    xiaocaiji22 2021-06-29 09:23:02
  • 45KB weixin_42696271 2021-09-11 11:54:26
  • 4KB weixin_42696271 2021-09-10 21:12:00
  • MPC控制简介

    MPC控制简介

    众所周知,控制算法中,PID的应用占据了90%,而另外10%就是这次的主角MPC控制算法。
    MPC控制算法全称模型预测控制,它相对比PID有着多输入,多输出以及更加平稳的特点。并且最重要的是,MPC可以针对非线性的系统进行控制
    由于其平稳的和非线性问题有着较强处理能力的特点,其在自动驾驶领域异常流行。

    MPC全称:模型预测控制

    1. 模型:即简单运动学模型,一般需要转化为线性的,离散的状态空间方程作为基本模型
    2. 预测:根据模型预测未来数个时间段内(离散的)的状态
    3. 控制:根据预测内容进行二次优化与处理,得出合适控制量。

    简单流程(个人简单理解,若有误请指出)

    返回前一时刻控制量
    控制量
    返回状态量
    数学模型
    预测未来状态
    二次最优化计算
    作用于系统

    车辆运动学模型

    在这里插入图片描述

    公式为

    x ′ = v ∗ cos ⁡ ( φ ) . y ′ = v ∗ sin ⁡ ( φ ) . x ′ = v ∗ tan ⁡ ( δ ) l . x'=v*\cos(φ).\\ y'=v*\sin(φ).\\ x'=\cfrac{v*\tan(\delta)}{l}. x=vcos(φ).y=vsin(φ).x=lvtan(δ).

    由于MPC的模型需要、状态空间方程线性的离散的表达
    因此第一步就是使其成为状态空间方程

    完整的表示一个车的状态,我们需要用到他的坐标和角度也就是[x,y,φ]
    车辆是一个二自由度模型。控制量分别是:后轮的速度v前轮的转角δ.

    于此,可以确定
    状 态 量 X = [ x y φ ] 状态量\Large X =\large \begin{bmatrix} x \\ y \\ φ \end{bmatrix} X=xyφ
    控 制 量 U = [ v δ ] 控制量\Large U =\large \begin{bmatrix} v \\ δ \end{bmatrix} U=[vδ]
    状 态 空 间 方 程 X ˙ = f ( X , U ) 状态空间方程\\ \Large \text{\.{X}}=\large f(\Large X,U) X˙=f(X,U)
    后对状态空间方程进行一阶泰勒展开(线性化)
    首先设置参考点(通常为规划好的路径中的参考点)
    参 考 点 Xr ˙ = f ( X r , U r ) 参考点\Large \text{\.{Xr}}=\large f(\Large Xr,Ur) Xr˙=f(Xr,Ur)
    泰 勒 展 开 : X ˙ = f ( X r , U r ) + δ f ( X , U ) δ X ∗ ( X − X r ) + δ f ( X , U ) δ U ∗ ( U − U r ) . 泰勒展开:\\ \Large \text{\.{X}}=\large f(\Large Xr,Ur) \large+ \cfrac{\large \delta f(\Large X,U)}{\large \delta \Large X}*\Large(X-Xr)\large + \cfrac{\large \delta f(\Large X,U)}{\large \delta \Large U}*\Large(U-Ur). X˙=f(Xr,Ur)+δXδf(X,U)(XXr)+δUδf(X,U)(UUr).
    由于仍存在无法求得的f(Xr,Ur),因此我们转换策略,改为状态误差
    泰 勒 展 开 : X ˜ ˙ = X ˙ − X ˙ r = δ f ( X , U ) δ X ∗ ( X − X r ) + δ f ( X , U ) δ U ∗ ( U − U r ) . 泰勒展开:\\ \Large \text{\.{\text{\~{X}}}}=\Large \text{\.X}-\text{\.Xr}=\large \cfrac{\large \delta f(\Large X,U)}{\large \delta \Large X}*\Large(X-Xr)\large + \cfrac{\large \delta f(\Large X,U)}{\large \delta \Large U}*\Large(U-Ur). X˜˙=X˙X˙r=δXδf(X,U)(XXr)+δUδf(X,U)(UUr).
    X ˜ = X − X r U ˜ = U − U r \Large \text{\~{X}} = X - Xr\\ \Large \text{\~{U}} = U - Ur X˜=XXrU˜=UUr
    如此便可以得到
    δ f ( X , U ) δ X = A , δ f ( X , U ) δ U = B . \large \cfrac{\large \delta f(\Large X,U)}{\large \delta \Large X}=A , \cfrac{\large \delta f(\Large X,U)}{\large \delta \Large U}=B. δXδf(X,U)=AδUδf(X,U)=B.
    X ˜ ˙ = A ∗ X ˜ + B ∗ U ˜ \Large \text{\.{\text{\~{X}}}} = A*\Large \text{\~{X}} +B*\Large \text{\~{U}} X˜˙=AX˜+BU˜
    即线性化完毕
    线性化完毕后,由于计算机程序是离散的,因此我们需要将其离散化
    X ˜ ( k + 1 ) − X ˜ ( k ) T = A ∗ X ˜ ( k ) + B ∗ U ˜ ( k ) \cfrac{\large \Large \text{\~{X}} (k+1)-\text{\~{X}}(k)}{\large \Large T} = \Large A*\text{\~{X}} (k)+B*\Large \text{\~{U}}(k) TX˜(k+1)X˜(k)=AX˜(k)+BU˜(k)
    为此可以转化为
    X ˜ ( k + 1 ) = a ∗ X ˜ ( k ) + b ∗ U ˜ ( k ) \large \Large \text{\~{X}} (k+1)= \Large a*\text{\~{X}} (k)+b*\Large \text{\~{U}}(k) X˜(k+1)=aX˜(k)+bU˜(k)
    又因为控制量是没有参考值的,所以在此使用误差是不合适的,因此可以将控制量转变为控制增量
    转变过程则是根据已经离散化和线性化好的状态空间方程进行改变。
    也就是说我们可以设计一个新的的空间状态方程,将之前整理好的状态空间方程输出的作为状态量,控制增量作为控制量,并进行推导
    推导流程(图片来源,b站up:小黎的Ally,侵删)
    在这里插入图片描述
    最后一步的输出方程,含义为:
    最终预测结果只输出[x,y,φ]等真正的状态量。

    预测

    根据模型所得得最终得状态空间和输出方程我们就可以对模型进行预测
    也就是
    ξ ( k + 1 ) = A ξ ( k ) + B Δ u ˜ ( k ) ξ ( k + 2 ) = A ξ ( k + 1 ) + B Δ u ˜ ( k + 1 ) ξ ( k + 3 ) = A ξ ( k + 2 ) + B Δ u ˜ ( k + 2 ) . . . ξ ( k + N c + 1 ) = A ξ ( k + N c ) + B Δ u ˜ ( k + N c ) . . . ξ ( k + N p + 1 ) = A ξ ( k + N p ) + B Δ u ˜ ( k + N p ) ξ(k+1) =Aξ(k)+BΔ\text{\~{u}}(k)\\ ξ(k+2) =Aξ(k+1)+BΔ\text{\~{u}}(k+1)\\ ξ(k+3) =Aξ(k+2)+BΔ\text{\~{u}}(k+2)\\ ...\\ ξ(k+Nc+1) =Aξ(k+Nc)+BΔ\text{\~{u}}(k+Nc)\\ ...\\ ξ(k+Np+1) =Aξ(k+Np)+BΔ\text{\~{u}}(k+Np)\\ ξ(k+1)=Aξ(k)+BΔu˜(k)ξ(k+2)=Aξ(k+1)+BΔu˜(k+1)ξ(k+3)=Aξ(k+2)+BΔu˜(k+2)...ξ(k+Nc+1)=Aξ(k+Nc)+BΔu˜(k+Nc)...ξ(k+Np+1)=Aξ(k+Np)+BΔu˜(k+Np)
    这里
    预测时域Np控制时域Nc

    预测时域很好理解,简单来说就是程序中预测多少个时刻的状态

    控制时域是我们可控制未来多少步。

    第一个预测公式逐步代入到下方公式并将公式乘以C(转化为输出方程)

    即为(暂且忽视红圈)

    在这里插入图片描述
    未来状态的输出量 = 当前的状态量 * 系数矩阵 + (从当前时刻到未来时刻的控制量
    在上面的公式中有个问题,就是未来要预测几个矩阵?,未来预测到什么时候结束控制量的累加?

    这两个问题就是对应了预测时域Np控制时域Nc
    这个时域我们一般自定。

    最后观看政个算式,可以将其变化为矩阵运算
    在这里插入图片描述
    红圈部分即为预测出的算式,这个Y就是最终预测的未来时刻的状态矩阵。

    控制

    根据我们预测方程,最终得出状态量与参考量的误差,因此需要输出的状态量(误差)绝对值最小,又因为Y是一个矩阵,所以我们就需要使该矩阵得平方最小。

    J m i n = Y ′ ∗ Y Jmin=Y'*Y Jmin=YY
    同时,我们有需要尽量平滑的抵达目标位置,如何平滑呐?也就是控制量(此处为控制增量)绝对值保持最小!
    J m i n = Δ U ′ ∗ Δ U Jmin = ΔU'*ΔU Jmin=ΔUΔU

    最后对快速抵达目标的目的和平滑的目的取一个权重。
    J m i n = Y ′ Q Y + Δ U ′ R Δ U Jmin=Y'QY+ΔU'RΔU Jmin=YQY+ΔURΔU
    Q大的化就代表更偏向快速到目标位置(但是会增大震荡),R大就代表到达目标位置更平稳(但是响应慢)。
    其中国Q必须是行数和Y'相等列数和Y相等的N倍单位矩阵(N为自定大小)

    MPC的控制总的来说是一次二次规划问题。
    也就是说,他应该长这样。
    在这里插入图片描述

    但是我们这里面却有两个自变量。。。。。把预测中Y等号右边的算式带入整理,去掉常数项可得

    在这里插入图片描述
    这样,结果就成为了一个二次规划问题。

    当然,还有ΔU和U的限制问题,需要自己根据自己车辆参数设计。

    之后利用matlab的二次规划工具箱结算出ΔU矩阵。matlab quadprog函数使用.
    注意:计算出的是一个(Nc*控制量个数)长的向量,且是控制增量

    最后,我们只需将计算出的第一组控制增量加到目前的控制量中即可。

    滚动优化

    控制量作用于系统,并在下一刻得出系统状态在进行一次预测与二次优化即可。

    展开全文
    QWQ_DIODA 2021-08-17 22:17:44
  • m0_50888394 2021-05-24 09:55:07
  • 11KB m0_50888394 2021-04-12 23:54:57
  • 5星
    1.8MB weixin_42680139 2021-09-29 01:54:03
  • a735148617 2021-02-10 18:49:39
  • 3KB weixin_42112894 2021-02-14 08:29:37
  • gophae 2020-02-14 14:44:15
  • 48KB weixin_42696271 2021-09-11 11:54:24
  • qq_40870689 2019-02-27 13:59:02
  • 46KB m0_50888394 2021-08-16 11:17:30
  • qq_35694280 2020-05-15 20:10:28
  • m0_50888394 2021-05-17 16:28:31

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 451
精华内容 180
关键字:

mpc轨迹跟踪