在無人車系統(tǒng)（十一）：軌跡跟蹤模型預(yù)測(cè)控制(MPC)原理與python實(shí)現(xiàn)【40行代碼】中介紹了MPC方法在無人車軌跡跟蹤中的應(yīng)用。以Udacity中的例子作為引子，詳細(xì)介紹了MPC的原理，無人車的運(yùn)動(dòng)學(xué)模型，以及給出基于python的實(shí)現(xiàn)。Udacity的仿真器是一些特定的環(huán)境，沒有ros中的stage或gazebo靈活。本文以turtlebot的軌跡跟蹤任務(wù)作為引子，介紹在ROS Stage仿真環(huán)境中的實(shí)現(xiàn)移動(dòng)機(jī)器人軌跡跟蹤控制。

1. 仿真環(huán)境設(shè)置

1.1 更改stage環(huán)境地圖

ubuntu 18.04+ros melodic

• 首先，請(qǐng)參照install turtlebot on ubuntu 18.04 + ros melodic在melodic版本的ROS中安裝好turtlebot。
• 然后，參照turtlebot_stageTutorialsindigoCustomizing the Stage Simulator熟悉如何在stage仿真環(huán)境中使用自己的地圖，以及如何在仿真環(huán)境中增減障礙物。stage仿真環(huán)境設(shè)置過程中重要的三個(gè)文件后綴分別為：“.png,.world,.yaml”。其中".png"為地圖的圖片，“.world"為整個(gè)環(huán)境的配置文件（包括地圖，機(jī)器人，障礙物等），”.yaml"為地圖的配置文件。因此，更改要將turtlebot stage中的原始地圖更改成自己的地圖，只需要將.png與.yaml文件替換原始的.png與.yaml文件。

例如：

test.png圖片為

test.yaml文件內(nèi)容如下

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文件內(nèi)容如下：

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）中未尾添加如下內(nèi)容，替換原始stage環(huán)境

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

運(yùn)行roslaunch turtlebot_stage turtlebot_in_stage.launch，發(fā)現(xiàn)環(huán)境地圖已經(jīng)成功更改為自己的地圖。

1.2 獲取要跟蹤的軌跡

1.2.1 獲取路點(diǎn)

點(diǎn)擊rviz工具欄中的"+"號(hào)，在新出現(xiàn)的界面中選擇“PublishPoint”，點(diǎn)擊OK退出該界面。

上面工具欄就會(huì)多出一個(gè)“Publish Point”，每點(diǎn)擊一次該按鈕，就可以在RVIZ界面中點(diǎn)擊任意位置，并通過名稱為“/clicked_point”的Topic發(fā)出該位置在“/map”坐標(biāo)系下的位置（x,y）。

可采用如下程序來依次記錄選取的路點(diǎn)（保存為“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文件內(nèi)容如下：

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 擬合路點(diǎn)得到機(jī)器人要跟蹤的路線

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是基于左右輪差速來達(dá)到轉(zhuǎn)彎，前行運(yùn)動(dòng)的。ROS中的Turtlebot包中的控制項(xiàng)有線速度$v$與角速度$w$。Turtlebot的運(yùn)動(dòng)學(xué)模型如下：
$$\begin{gathered} \dot{x}=vcos(\theta ) \\ \dot{y}=vsin(\theta ) \\ \dot{\theta }=w \end{gathered}$$

首先我們需要將以上連續(xù)的微分模型離散化成差分模型(差分間隔為$d_t$)：

$$\begin{array}{c}{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_k{d}_t\end{array}\text{\hspace{1em}(2)}$$
其中，$\text{cte}$與$\text{epsi}$的計(jì)算法公式詳見無人車系統(tǒng)（十一）：軌跡跟蹤模型預(yù)測(cè)控制(MPC)原理與python實(shí)現(xiàn)【40行代碼】。

對(duì)于一個(gè)預(yù)測(cè)步長(zhǎng)為$N$的MPC控制器求解問題，可以設(shè)計(jì)以下優(yōu)化目標(biāo)函數(shù)：
$$\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}\text{\hspace{1em}(4)}$$

注意：預(yù)測(cè)步長(zhǎng)N步是指在初始狀態(tài)$s_0$開始向前滾動(dòng)預(yù)測(cè)N步，有預(yù)測(cè)狀態(tài)N個(gè)，分別為$s_n,n=1,2,...,N$。因此，公式（4）第一行，關(guān)于狀態(tài)項(xiàng)的累積是從$k=1$到$N$。但是，在代碼實(shí)現(xiàn)過程中，為了簡(jiǎn)潔和一致性，通常把$s_0$也一并加入到目標(biāo)函數(shù)中，形成$k=0$到$N$的累加和（詳見python代碼m.sk = RangeSet(0, N)）。雖然加入了，我們要明白，在MPC中初始狀態(tài)已經(jīng)確定了，并不是它的優(yōu)化對(duì)象，它優(yōu)化的是未來的狀態(tài)，一般將$s_0=0$就可以了。

滿足動(dòng)態(tài)模型約束：
$$\begin{array}{ccc}\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^{\prime }(x_k))?\theta +w_k{d}_t& ,k=0,1,2,...,N?1\end{array}\text{\hspace{1em}(5)}$$

執(zhí)行器約束:

$$\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}\text{\hspace{1em}(6)}$$

3. 程序與運(yùn)行結(jié)果

在本此仿真中，一些參數(shù)如下：${\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.0$,$w_{\text{min}}=?1.5$,$w_{\text{max}}=1.5$,向前預(yù)測(cè)步為$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 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()

結(jié)果如下圖所示（綠線為預(yù)測(cè)的軌跡，紅線為turtlebot要跟蹤的軌跡）：

4. 寫在后面

基于MPC的軌跡跟蹤方法的效果與所設(shè)定的目標(biāo)函數(shù)、目標(biāo)函數(shù)的參數(shù)有很大的關(guān)系。本文最后的參數(shù)都是通過不斷的試驗(yàn)得到的，最終turtlebot能夠在回形走廊環(huán)境中跟蹤上要跟蹤的軌跡。

---

by Toby 2020-2-2
愿疫情早點(diǎn)結(jié)束！

總結(jié)

以上是生活随笔為你收集整理的Turtlebot+ROS Stage仿真环境实现MPC轨迹跟踪的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。