Turtlebot+ROS Stage仿真环境实现MPC轨迹跟踪

在无人车系统(十一):轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】中介绍了MPC方法在无人车轨迹跟踪中的应用。以Udacity中的例子作为引子,详细介绍了MPC的原理,无人车的运动学模型,以及给出基于python的实现。Udacity的仿真器是一些特定的环境,没有ros中的stage或gazebo灵活。本文以turtlebot的轨迹跟踪任务作为引子,介绍在ROS Stage仿真环境中的实现移动机器人轨迹跟踪控制。

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图片为
    Turtlebot+ROS Stage仿真环境实现MPC轨迹跟踪_第1张图片
  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
  1. 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"
)

  1. 在.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
  1. 运行roslaunch turtlebot_stage turtlebot_in_stage.launch,发现环境地图已经成功更改为自己的地图。

Turtlebot+ROS Stage仿真环境实现MPC轨迹跟踪_第2张图片
Turtlebot+ROS Stage仿真环境实现MPC轨迹跟踪_第3张图片

1.2 获取要跟踪的轨迹

1.2.1 获取路点

点击rviz工具栏中的"+"号,在新出现的界面中选择“PublishPoint”,点击OK退出该界面。
Turtlebot+ROS Stage仿真环境实现MPC轨迹跟踪_第4张图片

上面工具栏就会多出一个“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()

Turtlebot+ROS Stage仿真环境实现MPC轨迹跟踪_第5张图片

2. Turtlebot的MPC轨迹跟踪问题

与无人车不同,Turtlebot是基于左右轮差速来达到转弯,前行运动的。ROS中的Turtlebot包中的控制项有线速度 v v v与角速度 w w w。Turtlebot的运动学模型如下:
x ˙ = v c o s ( θ ) y ˙ = v s i n ( θ ) θ ˙ = w \dot{x}=vcos(\theta)\\ \dot{y}=vsin(\theta)\\ \dot{\theta}=w x˙=vcos(θ)y˙=vsin(θ)θ˙=w

首先我们需要将以上连续的微分模型离散化成差分模型(差分间隔为 d t d_t 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行代码】。

对于一个预测步长为 N N N的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 \omega_{cte}=\omega_{epsi}=1000 ωcte=ωepsi=1000, ω v 1 = 100 \omega_{v1}=100 ωv1=100, ω w = ω v 2 = 10 \omega_{w}=\omega_{v2}=10 ωw=ωv2=10, ω rate v = ω rate w = 1 \omega_{\text{rate}_{v}}=\omega_{\text{rate}_{w}}=1 ωratev=ωratew=1, v min = − 0.01 v_{\text{min}}=-0.01 vmin=0.01, v max = 2.0 v_{\text{max}}=2.0 vmax=2.0, w min = − 1.5 w_{\text{min}}=-1.5 wmin=1.5, w max = 1.5 w_{\text{max}}=1.5 wmax=1.5,向前预测步为 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能够在回形走廊环境中跟踪上要跟踪的轨迹。


by Toby 2020-2-2
愿疫情早点结束!

你可能感兴趣的:(机器人技术,ros,人工智能)