智能機器人與運動控制系統(tǒng)19.ROS的KF實現(xiàn)與線性定位KF定位系統(tǒng)與roslaunch簡介KF定位系統(tǒng)節(jié)點與實驗說明實驗步驟與要求KF定位系統(tǒng)與roslaunch簡介系統(tǒng)整體框圖perception部分負責融合預測值與測量值，獲得更接近真實值的濾波值driver部分對應于機器人動力學部分，作用是將控制信號轉(zhuǎn)為機器人的移動controller作為控制信號的發(fā)出者，通過編寫控制程序給出控制序列實現(xiàn)pid，與小海龜?shù)膒id控制對應。也可以使用teleop手動控制Gazebo部分作為仿真環(huán)境，給出機器人移動與環(huán)境的交互并提供傳感器的值

perceptionTwistcallbackKalmanFilterLaserScancallbackplotModelstatecallbackRobotdriverTwistcallbackDynamic&DiscretizationPubmodelstateControlsignalControlsignalrobotSensorcallbackModelStateRobotcontrollerTeleop/controller.pyRobotcontrollerCallbackgazeboperceptionRobotdrivergazeboSensormsgs項目文件結構rospkg的目錄結構(假設包的名稱為cylinder_robot)-cylinder_robot -script

-driver.py #將控制信號轉(zhuǎn)變?yōu)闄C器人在gazebo中的狀態(tài)改變，對應“Robotdriver” -perception.py #對測量值與由控制信號和模型得到的預測值濾波，對應“perception” -controller.py #產(chǎn)生控制信號序列，對應“controller” -world

-cylinder.world #先前保存的帶有機器人和房間的world文件 -config

-rvizConfig.rviz #使用rviz打開可以發(fā)出控制信號的teleop，僅作為測試使用 -launch

-runCylinder.launch #啟動文件，將以上文件結合起來 -XXX.fig

#存儲軌跡、新息等體現(xiàn)濾波效果的結果圖

如果一個一個啟動是否顯得太麻煩？roslaunch簡介為什么需要launch文件？用launch文件來啟動工程可以將需要的節(jié)點同時啟動，不用再一個一個進行，提高了效率。里面還有很多參數(shù)靈活使用會帶來非常高效的調(diào)試。roslaunch的概念：launch文件是一個XML格式的文件，可以同時啟動多個節(jié)點，還可以在參數(shù)服務器中設置參數(shù)。注意：roslaunch命令執(zhí)行l(wèi)aunch文件時，首先會判斷是否啟動了roscore,如果啟動了，則不再啟動，否則，會自動調(diào)用roscore。runCylinder.launch啟動上述節(jié)點<launch>

<!--Weresumethelogicinempty_world.launch,changingonlythenameoftheworldtobelaunched-->

<include

file="$(findgazebo_ros)/launch/empty_world.launch">

<arg

name="world_name"

value="$(findcylinder_robot)/worlds/cylinder.world"/>

<!--Note:theworld_nameiswithrespecttoGAZEBO_RESOURCE_PATHenvironmentalvariable-->

<!–Thefollowingargswillbesetatinitialization-->

<arg

name="paused"

value="false"/>

<arg

name="use_sim_time"

value="true"/>

<arg

name="gui"

value="true"/>

<arg

name="recording"

value="false"/>

<arg

name="debug"

value="false"/>

</include>

<node

pkg="rviz"

name="rviz"

type="rviz"

args="-d$(findcylinder_robot)/config/rvizConfig.rviz"/>

<!–launch

three

python

nodes-->

<node

pkg="cylinder_robot"

name="controller"

type="controller.py"

output="screen"

respawn="false"/>

<node

pkg="cylinder_robot"

name=“driver"

type=“driver.py"

output="screen"

respawn="false"/>

<node

pkg="cylinder_robot"

name=“perception"

type=“perception.py"

output="screen"

respawn="false"/></launch>系統(tǒng)節(jié)點與實驗說明實驗目標機器人移動控制與定位實驗機器人移動控制（driver）機器人動力學模型及離散化Gazebo機器人狀態(tài)發(fā)布perceptionTwistcallbackKalmanFilterLaserScancallbackplotModelstatecallbackRobotdriver(driver.py)TwistcallbackDynamic&DiscretizationPubmodelstateGeometry/twistrobotRobotcontrollerTeleop/controller.pyCallbackgazeboperception設定控制程序（controller）設計軌跡及對應控制序列*加入pid閉環(huán)控制gazebo_msgs/ModelStatedriver.py訂閱對應topic，設定A,B與對應的噪聲classDriver(object):

def__init__(self):

self.time_save=0

='cylinderRobot'

self.pub=rospy.Publisher(...)

rospy.Subscriber(...)

self.state=np.zeros([4,1])

self.A=...

self.B=...

self.Sigma_w=np.eye(4)*0.00001

代碼#類定義：驅(qū)動器。需補全#初始化函數(shù)。需補全以下部分：#gazebo狀態(tài)發(fā)布，…代表需補全的部分#控制信號訂閱#A,B的初始化解釋driver.py接收到控制信號的回調(diào)函數(shù)，負責更新機器人狀態(tài)并發(fā)到gazebodefcallback_control(self,twist):

……defforward_dynamics(self,u,dt): ……def_discretization_Func(self,dt): …

returnAtilde,Btilde,Sigma_w_tildedefsendStateMsg(self): …if__name__=='__main__':

try:

rospy.init_node(‘Driver',anonymous=True)

driver=Driver()

rospy.spin()

exceptrospy.ROSInterruptException:

pass代碼#控制信號回調(diào)函數(shù)，收到一幀控制信號時自動執(zhí)行，已完成，……代表已完成但省略的內(nèi)容#前向動力學函數(shù)，根據(jù)當前狀態(tài)與控制信號計算下一時刻系統(tǒng)狀態(tài)，已完成#離散化函數(shù)，由時間dt求得離散化的A,B,Sigma_w，根據(jù)之前課上所學將其補全#將更新后的狀態(tài)發(fā)送到gazebo中的，已完成#主函數(shù)，已完成解釋controller.py產(chǎn)生一個加速5s減速5s的控制序列，實現(xiàn)移動一段距離的開環(huán)控制#!/usr/bin/envpython

importrospyimportsysfromstd_msgs.msgimportStringfromgeometry_msgs.msgimportTwist

defcontroller():

pub=rospy.Publisher('/robot/control',\

Twist,queue_size=10)

rospy.init_node(‘controller',anonymous=True)

rate=rospy.Rate(10)

foriinrange(0,100):

twist=Twist()

twist.linear.x=1.5*abs(i-49.5)/(i-49.5)#twist.angular.z=0.5*abs(i-49.5)/(i-49.5)

pub.publish(twist)

rate.sleep()

sys.exit(0)

if__name__=='__main__':

try:

controller()

exceptrospy.ROSInterruptException:

pass

代碼#該程序為產(chǎn)生一段加速5s減速5s的控制序列，#實現(xiàn)移動一段距離，實驗中可以自定義一段序#列實現(xiàn)某個控制目標#也可以在獲得Kalmanfilter回傳的反饋信息#基礎上實現(xiàn)pid閉環(huán)控制解釋實驗目標機器人KF定位實現(xiàn)KalmanFilter理論與代碼轉(zhuǎn)化采樣不一致的離散化處理濾波結果輸出*濾波結果的反饋Perception(perception.py)TwistcallbackKalmanFilterLaserScancallbackplotModelstatecallbackRobotdriver(driver.py)TwistcallbackDynamic&DiscretizationPubmodelstateGeometry/twistrobotRobotcontrollerTeleop/controller.pyx_filteredgazeboSensor_msgs/LaserScangazebo_msgs/ModelStategazebo_msgs/ModelStateyx_realx_measuredux_filteredfigs使用launch文件啟動工程perception.pyclassKalmanFilter(object):

#initializationthekalmanfilter.

#

x'(t)=Ax(t)+Bu(t)+w(t)

#

y(t)=Cx(t)+v(t)

#

x(0)~N(x_0,P_0)

def__init__(self,mass,C,Sigma_w,Sigma_v,x_0,P_0):……def_discretization_Func(self,dt):……def_predict_Step(self,ctrl_time): …def_correction_Step(self,y): …defcontrol_moment(self,u_new,time_now): …defobserve_moment(self,y_new,time_now): …代碼#類定義：KalmanFilter，待補全#初始化函數(shù)，已完成#離散化函數(shù)，已完成#KF中的預測，待補全#KF中的更新，待補全#采樣不一致時控制時刻的處理，待補全#采樣不一致時觀測時刻的處理，待補全解釋KalmanFilter類定義perception.pyKalmanFilter的初始化函數(shù)def__init__(self,mass,C,Sigma_w,Sigma_v,x_0,P_0):

self.mass=mass

self.C=C

self.n=4

self.m=2

self.Sigma_w=Sigma_w

self.Sigma_v=Sigma_v

self.t=0

self.x=x_0

self.P=P_0

self.u=np.zeros([self.m,1])代碼#初始化函數(shù)，主要是傳遞參數(shù)，已完成解釋perception.pyKalmanFilter的離散化函數(shù)def_discretization_Func(self,dt):

Atilde=np.array([

[1,0,

0,

0],

[dt,1,

0,

0],

[0,0,

1,

0],

[0,0,

dt,1]

])

Btilde=np.array([

[dt/self.mass,

0],

[dt*dt/2/self.mass,0],

[0,

dt/self.mass],

[0,dt*dt/2/self.mass]

])

q1=self.Sigma_w[0,0]

q2=self.Sigma_w[1,1]

q3=self.Sigma_w[2,2]

q4=self.Sigma_w[3,3]

Sigma_w_tilde=np.array([

[dt*q1,

dt*dt/2*q1,

0,

0],

[dt*dt/2*q1,

(dt*q2)+(dt*dt*dt/3*q1),

0,

0],

[0,

0,

dt*q3,

dt*dt/2*q3],

[0,

0,

dt*dt/2*q3,(dt*q4)+(dt*dt*dt/3*q3)],

])

returnAtilde,Btilde,Sigma_w_tilde代碼#離散化函數(shù)，計算離散化的系統(tǒng)矩陣。與上節(jié)#課相同，這里不再重復。已完成解釋perception.pyLocalization類classLocalization(object):def__init__(self):……defcallback_control(self,twist):

#extractcontrolsignalfrommessage …

#callcontrolmomentfunctioninKalmanfilter …defcallback_observe(self,laserscan):

#extractobservesignalfrommessage

…

#callobservemomentfunctioninKalmanfilter …defcallback_state(self,state):……defsendStateMsg(self):……defvisualization(self):……代碼#類定義：定位器，待補全#初始化函數(shù)，已完成，……代表省略#觀測信號回調(diào)函數(shù)，待補全#twist是ros提供的數(shù)據(jù)幀格式，這里用來傳遞控制信息#twist.linear.x和twist.angular.z存儲了控制信息#具體數(shù)據(jù)格式見下頁#…代表待補全的部分#觀測信號回調(diào)函數(shù)，待補全#laserscan是ros提供的激光雷達數(shù)據(jù)幀格式，#laserscan.ranges中存儲的就是雷達測量到的距離數(shù)據(jù)。#具體數(shù)據(jù)格式見下頁#真值回調(diào)函數(shù)，以作繪圖用，已完成#發(fā)送濾波后定位，做pid用，已完成#可視化函數(shù)，已完成解釋ROS消息類型/en/groovy/api/sensor_msgs/html/msg/LaserScan.html/en/lunar/api/geometry_msgs/html/msg/Twist.htmlperception.pyLocalization類的初始化classLocalization(object):def__init__(self): rospy.Subscriber('/robot/control',Twist,self.callback_control) rospy.Subscriber('/robot/observe',LaserScan,self.callback_observe) rospy.Subscriber('gazebo/set_model_state',ModelState,self.callback_state) self.pub=rospy.Publisher("/robot/esti_model_state",ModelState,queue_size=10) rospy.on_shutdown(self.visualization) self.kf=KalmanFilter(mass=10,C=np.array([[0,1,0,0],[0,0,0,1]]),Sigma_w=np.eye(4)*0.00001,Sigma_v=np.array([[0.008**2,0],[0,0.008**2]]),x_0=np.zeros([4,1]),P_0=np.eye(4)/500) self.x_esti_save=[] self.x_esti_time=[] self.x_true_save=[] self.x_true_time=[]代碼#初始化函數(shù)，已完成#控制程序發(fā)出的控制值#傳感器發(fā)出的激光觀測值#gazebo發(fā)出的位置真值，僅作繪圖用#將濾波后的定位值發(fā)送給controller以做pid#退出運行可視化程序，將移動過程中的濾波結果繪出#調(diào)用KF類創(chuàng)建該參數(shù)下的濾波器#保存濾波結果與真值信息以作繪圖用解釋perception.pyLocalization類的響應函數(shù)defcallback_state(self,state):

current_time=rospy.get_time()

x=np.zeros([4,1])

x[0,0]=state.twist.linear.x

x[1,0]=state.pose.position.x

x[2,0]=state.twist.linear.y

x[3,0]=state.pose.position.y

self.x_true_save.append(x)

self.x_true_time.append(current_time)defsendStateMsg(self):

msg=ModelState()

msg.model_name=

msg.pose.position.x=self.kf.x[1]

msg.pose.position.y=self.kf.x[3]

self.pub.publish(msg)代碼#真值回調(diào)函數(shù)，以作繪圖用，已完成#將當前濾波后的x發(fā)出，做pid用，已完成解釋perception.pyLocalization類的可視化函數(shù)defvisualization(self):

print("Visualizing......")

t_esti=np.array(self.x_esti_time)

x_esti=np.concatenate(self.x_esti_save,axis=1)

p_obsv=np.concatenate(self.p_obsv_save,axis=1)

t_obsv=np.array(self.p_obsv_time)

t_true=np.array(self.x_true_time)

x_true=np.concatenate(self.x_true_save,axis=1)

fig_x=plt.figure(figsize=(16,9))

ax=fig_x.subplots(2,2)

ax[0,0].plot(t_esti,x_esti[1,:].T,label="esti")

ax[0,0].plot(t_true,x_true[1,:].T,label="true")

ax[0,0].legend(bbox_to_anchor=(0.85,1),loc='upperleft')

ax[0,0].set_title('px')

ax[1,0].plot(t_esti,x_esti[3,:].T,label="esti")

ax[1,0].plot(t_true,x_true[3,:].T,label="true")

ax[1,0].legend(bbox_to_anchor=(0.85,1),loc='upperleft')

ax[1,0].set_title('py')代碼#可視化函數(shù)，將存儲的各種值以圖像存儲，觀#測濾波器的效果。已完成#

可以自定義觀察的參數(shù)，得到不同的圖像#接下頁解釋perception.pyLocalization類的可視化函數(shù)

ax[1,1].plot(x_esti[1,:].T,x_e