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文件。

例如：

test.png图片为
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

stage/test.world文件内容如下：

include "turtlebot.inc"
include "myBlock.inc"define floorplan model
(# sombre, sensible, artisticcolor "gray30"# most maps will need a bounding boxboundary 1gui_nose 0gui_grid 0gui_outline 0gripper_return 0fiducial_return 0laser_return 1
)resolution 0.02
interval_sim 100  # simulation timestep in millisecondswindow
(
#  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"
)

在.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

运行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 npfrom geometry_msgs.msg import PointStampedclass 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.xp1[1] = data.point.yself.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 inlineplt.scatter(x, y)
plt.plot(newx, newy)
plt.show()

2. Turtlebot的MPC轨迹跟踪问题

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

首先我们需要将以上连续的微分模型离散化成差分模型(差分间隔为dtd_tdt)：

xk+1=xk+vkcos⁡(θk)dtyk+1=yk+vksin⁡(θk)dtθk+1=θk+wkdtctek+1=ctek+vksin⁡(θk)dtepsik+1=epsik+wkdt(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=1N(ωcte∣∣ctet∣∣2+ωepsi∣∣epsik∣∣2)+∑k=0N−1(ωw∣∣wk∣∣2+ωv2∣∣vk∣∣2+ωv1∣∣vk−vref∣∣2)+∑k=0N−2(ωratew∣∣wk+1−wk∣∣2+ωratev∣∣vk+1−vk∣∣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) \\ & +\sum_{k=0}^{N-1} (\omega_{w}||w_k||^2+\omega_{v2}||v_k||^2+\omega_{v1} ||v_k-v_{\text{ref}}||^2) \\ & +\sum_{k=0}^{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(ωcte∣∣ctet∣∣2+ωepsi∣∣epsik∣∣2)+∑k=0N−1(ωw∣∣wk∣∣2+ωv2∣∣vk∣∣2+ωv1∣∣vk−vref∣∣2)+∑k=0N−2(ωratew∣∣wk+1−wk∣∣2+ωratev∣∣vk+1−vk∣∣2)(4)

注意：预测步长N步是指在初始状态s0s_0s0开始向前滚动预测N步，有预测状态N个，分别为sn,n=1,2,...,Ns_n, n=1,2,...,Nsn,n=1,2,...,N。因此，公式（4）第一行，关于状态项的累积是从k=1k=1k=1到NNN。但是，在代码实现过程中，为了简洁和一致性，通常把s0s_0s0也一并加入到目标函数中，形成k=0k=0k=0到NNN的累加和（详见python代码m.sk = RangeSet(0, N)）。虽然加入了，我们要明白，在MPC中初始状态已经确定了，并不是它的优化对象，它优化的是未来的状态，一般将s0=0s_0=\mathbf{0}s0=0就可以了。

满足动态模型约束：
s.t.xk+1=xk+vkcos(θk)dt,k=0,1,2,...,N−1yk+1=yk+vksin(θk)dt,k=0,1,2,...,N−1θk+1=θk+wkdt,k=0,1,2,...,N−1ctek+1=f(xk)−yk+vksin⁡(θk)dt,k=0,1,2,...,N−1epsik+1=arctan⁡(f′(xk))−θ+wkdt,k=0,1,2,...,N−1(5)\begin{array}{c} \text{s.t.} & x_{k+1}=x_k+v_{k}cos(\theta_k)d_t &, k=0,1,2,...,N-1\\ & y_{k+1}=y_k+v_{k}sin(\theta_k)d_t &, k=0,1,2,...,N-1\\ & \theta_{k+1}=\theta_{k}+w_{k} d_t &, k=0,1,2,...,N-1\\ & \text{cte}_{k+1} =f(x_k)-y_k+v_{k} \sin (\theta_k)d_t &,k=0,1,2,...,N-1 \\ & \text{epsi}_{k+1}=arc\tan(f'(x_k))-\theta+w_{k} d_t &, k=0,1,2,...,N-1 \end{array}\tag{5} s.t.xk+1=xk+vkcos(θk)dtyk+1=yk+vksin(θk)dtθk+1=θk+wkdtctek+1=f(xk)−yk+vksin(θk)dtepsik+1=arctan(f′(xk))−θ+wkdt,k=0,1,2,...,N−1,k=0,1,2,...,N−1,k=0,1,2,...,N−1,k=0,1,2,...,N−1,k=0,1,2,...,N−1(5)

执行器约束:

vk∈[vmin,vmax],k=0,1,2,...,N−1wk∈[wmin,wmax],k=0,1,2,...,N−1(6)\begin{array}{cc} v_k\in[v_{\text{min}}, v_{\text{max}}] &, k=0,1,2,...,N-1\\ w_k\in [w_{\text{min}}, w_{\text{max}}]&, k=0,1,2,...,N-1 \end{array}\tag{6} vk∈[vmin,vmax]wk∈[wmin,wmax],k=0,1,2,...,N−1,k=0,1,2,...,N−1(6)

3. 程序与运行结果

在本此仿真中，一些参数如下：ωcte=ωepsi=1000\omega_{cte}=\omega_{epsi}=1000ωcte=ωepsi=1000,ωv1=100\omega_{v1}=100ωv1=100,ωw=ωv2=10\omega_{w}=\omega_{v2}=10ωw=ωv2=10,ωratev=ωratew=1\omega_{\text{rate}_{v}}=\omega_{\text{rate}_{w}}=1ωratev=ωratew=1,vmin=−0.01v_{\text{min}}=-0.01vmin=−0.01,vmax=2.0v_{\text{max}}=2.0vmax=2.0,wmin=−1.5w_{\text{min}}=-1.5wmin=−1.5,wmax=1.5w_{\text{max}}=1.5wmax=1.5,向前预测步为N=19N=19N=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 displayplt.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[3]    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: omegaclass MPC(object):def __init__(self):m = ConcreteModel()m.sk = RangeSet(0, N)m.uk = RangeSet(0, N-1)m.uk1 = RangeSet(0, N-2)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: epsim.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.0self.crw = 0.0self.mpc = MPC() rate = rospy.Rate(10) # 10HZwhile not rospy.is_shutdown():self.getrobotpose()cwps = getcwps(self.rp[0:2])px = self.rp[0] + self.crv*np.cos(self.rp[2])*0.1py = self.rp[1] + self.crw*np.sin(self.rp[2])*0.1psi = self.rp[2] + self.crw*0.1self.rp[0] = pxself.rp[1] = pyself.rp[2] = psicwps_robot = np.zeros((len(cwps), 2))for i in range(len(cwps)):dx = cwps[i,0] - pxdy = cwps[i,1] - pycwps_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] = ydef odomCallback(self, data):self.crv = data.twist.twist.linear.xself.crw = data.twist.twist.angular.zdef 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 = vtwist.angular.z = wself.pub_cmd.publish(twist)     if __name__ == "__main__":turtlebot_core = Turtlebot_core()

结果如下图所示（绿线为预测的轨迹，红线为turtlebot要跟踪的轨迹）：

4. 写在后面

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

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