发表于 | 分类于

参考网址：
https://mp.weixin.qq.com/s?__biz=MzA4NTM0Mzg5Nw==&mid=2247484695&idx=1&sn=e0ea4618a12b0f4e2eb4621a5afdc474&scene=19&token=46625227&lang=zh_CN#wechat_redirect

一、Simulink模型简述

该模型对简化的汽车运动进行仿真。当踩下踏板时，汽车通常处于行进状态。轻踩并松开加速踏板后，汽车先会处于怠速状态，然后停止。基于上述运动模型再对接近传感器建模，该数字传感器用于测量汽车与10米（30英尺）外的障碍物之间的距离。模型根据下列条件来输出传感器的测量值和汽车的位置值：
●汽车在到达障碍物时会紧急刹车。
●在现实世界中，传感器对距离的测量不够精确，从而导致随机数值误差。
●数字传感器以固定时间间隔运行。

二、构建模型

Step 1. 创建新模型

1.要启动Simulink并创建新模型，请在MATLAB命令提示符下输入以下内容： 

simulink

在Simulink Start Page对话框中，点击Blank Model，然后点击Create Model，将打开一个空的Editor 窗口。
2.在Editor工具条的Simulation选项卡上，点击Save > Save As为新模型指定名称。

Step 2. 拖放模块以完成模型

在该模型中需要针对以下每个模块创建一个实例：
●Pulse Generator
该模块的作用是为模型生成脉冲信号，模拟轻踩加速踏板后的输出信号。

●Gain
该模块的作用是将输入信号乘以一个因子，计算踩下加速踏板后如何影响汽车的加速度。

●Integrator, Second Order
该模块的作用是对输入信号进行二次积分，根据加速度计算得到汽车的位置。

●Out1
该模型的作用是将汽车位置作为模型的输出。

Step 3. 配置模块

通过双击模型中的每个模块为模块设置参数。
配置Pulse Generator模块。双击Pulse Generator模块以打开其参数对话框。验证或设置以下参数：

配置Gain模块。双击Gain模块以打开其参数对话框。验证或设置以下参数：

配置Integrator, Second Order模块。双击Integrator, Second Order模块以打开其参数对话框。
在x选项卡上，进行如下设置(其余选项卡按照默认设置）：

Step 4. 连接模块并为信号添加注释

构建完成的模型如下图所示：

Step 5. 添加信号查看器

要查看仿真结果，请将第一个输出连接到一个Signal Viewer。
通过右键点击信号来访问上下文菜单。选择Create & Connect Viewer > Simulink > Scope。信号上会出现查看器图标，并打开一个示波器窗口。

Step 6. 运行仿真

1.在模型Editor工具条的Modeling选项卡中，点击Model Settings。将打开Configuration Parameters对话框。
在Solver Options部分中，进行如下选择：

2.点击Editor工具条的Simulation或Modeling选项卡中的Run按钮，运行仿真。

三、优化模型

Step 7. 拖放新模块以完成模型

●Constant
该模块的作用是为障碍物的位置设置常量值10。

●Subtract
该模块的作用是将两个输入值相减，求出障碍物位置和车辆位置之间的实际距离。

●Band-Limited White Noise
该模块的作用是产生白噪声，模拟真实传感器测量中常见的误差。

●Add
该模块的作用是将两个输入值相加，从而将白噪声添加进数字传感器采样数据中。

●Zero-Order Hold
该模块的作用是让采集的信号样本数据保持一定时间间隔，模拟离散系统对数字传感器采样。

Step 8. 配置新模块

通过双击模型中的每个模块为模块设置参数。
配置Constant模块。双击Constant模块以打开其参数对话框。验证或设置以下参数：

配置Band-Limited White Noise模块。双击Band-Limited White Noise模块以打开其参数对话框。验证或设置以下参数：

配置Zero-Order Hold模块。双击Zero-Order Hold模块以打开其参数对话框。验证或设置以下参数：

Step 9. 连接模块并为信号添加注释

构建完成的模型如下图所示：

Step 10. 比较多个信号

将actual distance信号与measured distance信号进行比较。
1.创建一个示波器Scope1并将其连接到actual distance信号。
2.将measured distance信号添加到同一个查看器中。右键点击信号，然后选择Connect to Viewer > Scope1。确保连接到在上一步中创建的查看器。

Step 11. 运行仿真

查看器显示两个信号：Actual Distance（黄色）和Measured Distance（蓝色）。

发表于 | 分类于

1.一阶RC低通滤波器的连续域数学模型

1.1数学模型的推导

上图是RC低通滤波器的电路模型，根据基尔霍夫定律可知：

即：

根据电容的特性可知：

零状态条件下的时域响应可表示为：

定义时间常数τ=RC，对上述微分方程进行拉氏变换，可得：


由此可得传递函数：

即频域数学模型为：

1.2频率特性

相频特性：

幅频特性：

以R=100Ω，C=100nF为例，该低通滤波器的频率特性如下：

1.3物理作用：

输入一个阶跃信号，经过时间τ之后，输出大约为阶跃量的63%，滞后作用。

2.一阶RC低通滤波器的算法推导

2.1离散化

采用一阶后向差分法将传递函数G(s)从S域转化到Z域，其中一阶后向差分中S域与Z域的变化关系是：

其中T是采样周期，带入传递函数G(s)中得：



Z反变换求差分方程后可得：


令

可得

关于滤波系数A

所以

代入

可以得到滤波系数A与截止频率f的关系：

3.一阶RC低通滤波器的C语言实现

1
2
3
4
5
6
7
8
9
10
11
#define a 0.01               // 滤波系数a（0-1） 

static float oldOutData = 0; //上一次滤波值

char filter(void)
{
    nowData  = get_Data();  //本次滤波值
    nowOutData = a * nowData  + (1.0f - a) * oldOutData;
    oldOutData = nowOutData;
    return nowOutData;  
}

4.缺点及改善方法

4.1缺点

●仍然存在灵敏度与平稳度之间的矛盾；
●小数舍弃带来的误差（单片机很少采用浮点数，小数位要么舍弃，要么四舍五入）。

4.2改善方法——动态调整滤波系数

4.2.1实现功能

●当数据快速变化时，滤波结果能及时跟进，并且数据的变化越快，灵敏度应该越高（灵敏度优先原则）；
●当数据趋于稳定，并在一个范围内振荡时，滤波结果能趋于平稳（平稳度优先原则）；
●当数据稳定后，滤波结果能逼近并最终等于采样数据（消除因计算中小数带来的误差）。

4.2.2调整前判断

●数据变化方向是否为同一个方向（如当连续两次的采样值都比其上次滤波结果大时，视为变化方向一致，否则视为不一致）；
●数据变化是否较快（主要是判断采样值和上一次滤波结果之间的差值）。

4.2.3调整原则

●当两次数据变化不一致时，说明有抖动，将滤波系数清零，忽略本次新采样值；
●当数据持续向一个方向变化时，逐渐提高滤波系数，提供本次采样值得权；
●当数据变化较快（差值＞消抖计数加速反应阈值）时，要加速提高滤波系数。

发表于 | 分类于

参考网址：
https://ww2.mathworks.cn/help/vnt/ug/build-can-communication-simulink-models.html#brzf0ew-4

一、Simulink模型简述

在该模型中使用MathWorks虚拟CAN通道传输和接收报文。
●报文传输部分：使用CAN Configuration模块配置CAN通道，Constant模块向CAN Pack模块提供数据，并使用CAN Transmit模块将数据发送到虚拟CAN通道。
●报文接收部分：使用CAN Configuration模块配置CAN通道，CAN Receive模块接收报文，同时使用包含CAN Unpack模块的Function-Call Subsystem模块从CAN Receive模块获取数据，并使用CAN Unpack模块的参数来解包报文数据，最后通过Scope模块显示接收到的数据。

二、构建模型的报文传输部分

Step 1. 创建新模型

1.要启动Simulink并创建新模型，请在MATLAB命令提示符下输入以下内容：

1
simulink

在Simulink Start Page对话框中，点击Blank Model，然后点击Create Model，将打开一个空的Editor窗口。
2.在Editor工具条的Simulation选项卡上，点击Save > Save As为新模型指定名称。

Step 2. 打开模型库

1.在模型Editor工具条的Simulation选项卡中，点击Library Browser。
2.Simulink Library Browser将打开。其左窗格包含按字母顺序显示的可用模块库的树。展开Vehicle Network Toolbox节点，然后点击CAN Communication。

Step 3. 将Vehicle Network Toolbox模块拖到模型中

要将模块放入模型中，请点击库中的一个模块并将其拖到编辑器中。对于此示例，模型中需要针对以下每个模块创建一个实例：
●CAN Configuration

●CAN Pack

●CAN Transmit

Step 4. 拖放其他模块以完成模型

此示例使用Constant(Simulink)模块作为数据源。从Simulink > Commonly Used Blocks库中，向模型添加一个Constant(Simulink)模块。

Step 5. 连接模块

在Constant模块和CAN Pack模块输入之间建立连接。将指针移至Constant模块的输出端口附近时，指针将变为十字准线。点击Constant模块输出端口并按住鼠标按键，将指针拖动到CAN Pack模块的输入端口。然后松开按键。
以相同的方式，在CAN Pack模块的输出端口与CAN Transmit模块的输入端口之间建立连接。
CAN Configuration模块不连接到任何其他模块，此模块配置其CAN通道以用于通信。

Step 6. 指定模块参数值

通过双击模型中的每个模块为模块设置参数。
配置CAN Configuration模块。双击CAN Configuration模块以打开其参数对话框。验证或设置以下参数：

配置CAN Pack模块。双击CAN Pack模块以打开其参数对话框。验证或设置以下参数：

配置CAN Transmit模块。双击CAN Transmit模块以打开其参数对话框。验证或设置以下参数：

配置Constant模块。双击Constant模块以打开其参数对话框。
在Main选项卡上，进行如下设置：

在Signal Attributes选项卡上，进行如下设置：

经过配置的模型如下图所示：

三、构建模型的报文接收部分

Step 7. 将Vehicle Network Toolbox模块拖到模型中

对于示例的这一部分，首先为Vehicle Network Toolbox CAN Communication模块库中的以下每个模块创建一个实例：
●CAN Configuration

●CAN Receive

Step 8. 拖放其他模块以完成模型

使用Simulink的Ports & Subsystems模块库中的Function-Call Subsystem模块构建您的CAN Message pack子系统。

双击Function-Call Subsystem模块以打开子系统编辑器。双击In1端口标签，将其重命名为CAN Msg。双击Out1端口标签，将其重命名为Data。将CAN Unpack模块从Vehicle Network Toolbox模块库中拖放到此子系统中。如果将该模块放置在输入信号线和输出信号线之间，它们将自动连接。
此时Function-Call Subsystem模块的内部应如下图所示。

将CAN Unpack放在Function-Call Subsystem内的原因是为了捕获所有可能的报文。

Step 9. 连接模块

将Function-Call Subsystem模块重命名为CAN Unpack Subsystem。将CAN Receive模块的CAN Msg输出端口连接到CAN Unpack Subsyste模块的In1输入端口。将CAN Receive模块的f()输出端口连接到CAN Unpack Subsystem模块的function()输入端口。要直观地显示仿真结果，请将Scope (Simulink)模块从Simulink模块库拖放到您的模型中。将CAN Unpack Subsystem模块的CAN Msg输出端口连接到Scope模块的输入端口。CAN Configuration模块不连接到任何其他模块。该模块用于配置CAN Receive模块所使用的CAN通道以接收CAN报文。

Step 10. 指定模块参数值

配置CAN Configuration1模块。双击CAN Configuration模块以打开其参数对话框。进行如下设置：

配置CAN Receive模块。双击CAN Receive模块以打开其Parameters对话框。进行如下设置：

配置CAN Unpack子系统。双击CAN Unpack子系统以打开Function-Call Subsystem编辑器。在模型中，双击CAN Unpack模块以打开其参数对话框。进行如下设置：

四、保存并运行模型

Step 11. 保存模型

在运行仿真之前，点击Save图标或从Editor工具条的Simulation选项卡中选择Save保存模型。

Step 12. 更改配置参数

在模型Editor工具条的Modeling选项卡中，点击Model Settings。将打开Configuration Parameters对话框。
在Solver Options部分中，进行如下选择：

Step 13. 运行仿真

要运行仿真，请点击Editor工具条的Simulation或Modeling选项卡中的Run按钮。
运行仿真时，CAN Transmit模块将从CAN Pack模块中获取报文。然后它会通过Virtual Channel 1传输该报文。Virtual Channel 2上的CAN Receive模块会接收该报文并将其传递到CAN Unpack Subsystem模块以解包报文。
在仿真运行时，模型窗口底部的状态栏会更新仿真的进度。

Step 14. 查看结果

双击Scope模块以在图上查看报文传输。如果您在图上看不到所有数据，请点击Autoscale工具栏按钮，这会自动缩放坐标轴以显示所有存储的仿真数据。

发表于 | 分类于

1.算法简介

相关链接：
https://blog.csdn.net/renyushuai900/article/details/98460758
论文链接：
http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20Stanley.pdf

1.1算法推断

Stanley方法是一种基于横向跟踪误差（cross-track error：e为前轴中心到最近路径点(px​,py​)的距离）的非线性反馈函数，并且能实现横向跟踪误差指数收敛于0。根据车辆位姿与给定路径的相对几何关系可以直观的获得控制车辆方向盘转角的控制变量，其中包含横向误差e和航向误差θe​。

●在不考虑横向跟踪误差e的情况下，前轮偏角和给定路径切线方向一致，如图所示。其中θe​表示车辆航向与最近路径点切线方向之间的夹角，在没有任何横向误差的情况下，前轮方向与所在路径点的方向相同：

●在不考虑航向跟踪误差θe​的情况下，横向跟踪误差越大，前轮转向角越大，假设车辆预期轨迹在距离前轮d(t)处与给定路径上最近点切线相交，根据几何关系得出如下非线性比例函数：

其中d(t)与车速相关，最后用车速v(t)，增益参数k表示。随着横向误差的增加，arctan函数产生一个直接指向期望路径的前轮偏角，并且收敛受车速v(t)限制。
综合两方面控制因素，基本转向角控制率如下：

●使用线性自行车运动模型，可以得到横向误差的变化率：

●其中sinδe​(t)根据几何关系可知：

●当横向跟踪误差e(t)很小时，:

●通过积分上式:

因此横向误差指数收敛于e(t)=0，参数k决定了收敛速度。对于任意横向误差，微分方程都单调的收敛到0。

2.源码解析

Note：由于该部分代码与Pure_Persuit算法的代码有较大相似之处，因此这里只讲解其中的不同以及核心部分。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
def calculateTwistCommand(self):
	lad = 0.0 #look ahead distance accumulator
	targetIndex = len(self.currentWaypoints.waypoints) - 1
	for i in range(len(self.currentWaypoints.waypoints)):
		if((i+1) < len(self.currentWaypoints.waypoints)):
			this_x = self.currentWaypoints.waypoints[i].pose.pose.position.x
			this_y = self.currentWaypoints.waypoints[i].pose.pose.position.y
			next_x = self.currentWaypoints.waypoints[i+1].pose.pose.position.x
			next_y = self.currentWaypoints.waypoints[i+1].pose.pose.position.y
			lad = lad + math.hypot(next_x - this_x, next_y - this_y)
			if(lad > HORIZON):
				targetIndex = i+1
				break

	targetWaypoint = self.currentWaypoints.waypoints[targetIndex]

	targetSpeed = self.currentWaypoints.waypoints[0].twist.twist.linear.x

	targetX = targetWaypoint.pose.pose.position.x
	targetY = targetWaypoint.pose.pose.position.y		
	currentX = self.currentPose.pose.position.x
	currentY = self.currentPose.pose.position.y
	#print '[',targetX, targetY, ']'
	#print targetWaypoint.pose.pose.orientation
	print self.currentVelocity

	#get waypoint yaw angle
	waypoint_quanternion = (targetWaypoint.pose.pose.orientation.x, targetWaypoint.pose.pose.orientation.y, targetWaypoint.pose.pose.orientation.z, targetWaypoint.pose.pose.orientation.w)
	waypoint_euler = tf.transformations.euler_from_quaternion(waypoint_quanternion)
	TargetYaw = waypoint_euler[2]
	print 'TargetYaw = ', TargetYaw
	
	#get vehicle yaw angle
	quanternion = (self.currentPose.pose.orientation.x, self.currentPose.pose.orientation.y, self.currentPose.pose.orientation.z, self.currentPose.pose.orientation.w)
	euler = tf.transformations.euler_from_quaternion(quanternion)
	CurrentYaw = euler[2]
	print 'CurrentYaw = ', CurrentYaw

说明：
●以上部分与Pure_Persuit算法基本相同，大致分为三个小部分，因此只简述其中每一小部分的作用。
●第一部分的主要作用是找到距离当前位置最近的局部路径点的横、纵坐标。
●第二部分的主要作用是获取该局部路径点对应的目标航向角。
●第三部分的主要作用是获取小车当前的航向角。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
#get the difference between yaw angle of vehicle and tangent line of waypoint
if TargetYaw >= 0:
	alpha = TargetYaw - CurrentYaw
	flag_alpha = 1
else:
	if TargetYaw * CurrentYaw < 0:
		if (TargetYaw < 0) and (CurrentYaw > 0):
			alpha = (math.pi - CurrentYaw) + (math.pi + TargetYaw)
			flag_alpha = 2
		else:
			alpha = -((math.pi + CurrentYaw) + (math.pi - TargetYaw))
			flag_alpha = 3
	else:
		alpha = TargetYaw - CurrentYaw
		flag_alpha = 4

print 'flag_alpha = ', flag_alpha
print '(', currentX, currentY, ')'
print '(', targetX, targetY, ')'
print 'alpha = ', alpha
print 'x = ', targetX - currentX
print 'y = ', targetY - current

说明：
●该部分的主要作用是计算得到目标航向角与当前实际航向角之间的航向误差alpha。
1
2
3
4
5
6
#get the error between target position and current position
if alpha >= 0:
	error = abs( math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2)) )
else:
	error = -abs( math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2)) )
print 'error = ', error

说明：
●该部分的主要作用是计算得到小车前轮中心轴位置与目标路径点之间的横向误差error。
1
2
3
#get the velocity of vehicle
vel = math.sqrt(math.pow(self.currentVelocity.twist.linear.x, 2) + math.pow(self.currentVelocity.twist.linear.y, 2))
print 'vel = ', vel

说明：
●该部分的主要作用是获取小车在当前航向角方向上的速度值。
1
2
3
4
5
6
#get the nonlinear proportional function from geometry of Vehicular mobility model
if vel < 0.001:
	delta = 0
else:
	delta = math.atan(k * error / vel)
print 'delta = ', delta

说明：
●该部分的主要作用是根据几何关系得出一个非线性比例函数，从而产生一个直接指向期望路径的前轮偏角delta，并且收敛受车速v(t)限制。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
#get the distance between final position and current position
l = math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2))
l2 = math.sqrt(math.pow(currentX - final_targetX, 2) + math.pow(currentY - final_targetY, 2))

if l > 0.001:
	if l2 > 0.1:
		theta = alpha + delta
		print 'theta = ', theta
		#self.ackermann_steering_control(targetSpeed, theta)
		self.ackermann_steering_control(15, -theta)
		#get twist command
		left_steer = Float64()
		right_steer = Float64()
		left_vel = Float64()
		right_vel = Float64()
		left_steer.data = left_angle
		right_steer.data = right_angle
		left_vel.data = left_speed
		right_vel.data = right_speed
		flag = 1
	else:
		left_steer = Float64()
		right_steer = Float64()
		left_vel = Float64()
		right_vel = Float64()
		left_steer.data = 0
		right_steer.data = 0
		left_vel.data = 0
		right_vel.data = 0
		flag = 2
else:
	left_steer = Float64()
	right_steer = Float64()
	left_vel = Float64()
	right_vel = Float64()
	left_steer.data = 0
	right_steer.data = 0
	left_vel.data = 0
	right_vel.data = 0
	flag = 3

print 'flag = ', flag

return left_vel, right_vel, left_steer, right_steer

说明：
●该部分的主要作用是获取小车当前位置与目标位置之间的距离，若小车当前位置与终点位置（0,4）的距离小于0.1m，则小车到达终点，此时停止运动。

3.效果演示

发表于 | 分类于

参考文献：
(1) https://blog.csdn.net/zhuangxiaobin/article/details/25476833

1.样本的创建

训练样本分为正样本和负样本。正样本是指待检目标样本(例如车辆，行人，人脸等)，负本指其它任意图片。所有的样本图片都被归一化为同样的尺寸大小(例如，20x20，24x24或33x33)。负样本可以来自于任意的图片，但这些图片不能包含目标特征。

1.1准备正样本图片集

进入路径为tools\temp\positive\rawdata的文件夹中，然后在里面放入含有正样本的图片。正样本数目要足够大，一般要是1000以上。
注：图片要用BMP格式，如果图片不是BMP格式的话，可以在文件夹中创建一个空的文本文件，在文本文件中输入ren *.jpg *.bmp，并将该文本文件改成.bat后缀的批处理文件，双击该批处理文件即可将文件夹中.JPG格式的图片批量转换成.BMP格式。

1.2截取目标区域

双击\tools\temp\positive下的objectmarker.exe可执行文件，开始截图目标区域。
使用方法可见readme文件，每次截好一次都要按一次空格键保存区域，多目标的图片可以继续截图并按空格键保存，若这张图片截取完成则按下回车键可以跳到下一张图片。

这个截图的过程不能中断，不然就无法产生相应的文件了，所以在样本太多的情况下，可以先分组，大约100张图片放进rawdata文件夹中，这样子如果有什么意外的话，损失也就100张图的时间而已，最后只需要把几个文件的内容合成一个大文件，并且把所有相关的图片放进rawdata文件夹中，最终的效果是一样的。截图完成后会在路径\tools\temp\positive下生成info.txt文件。

info.txt文件内容说明：
rawdata/1000986.BMP 1 0 1 127 93
①rawdata/1000986.BMP表示文件的相对路径；
②第2位的1表示图片中截取的区域数量，此处仅包含一个目标；
③第3和第4位的0 1表示截取的矩形框的起点坐标；
④第5和第6位的127 93表示截取的矩形框的宽和高；
注：第2位的数字若为2，即截取的目标数量为2个，则后面相应的矩形框起点坐标和矩形框尺寸也应该有2个，否则会在之后生成正样本的描述文件时报如下错误：

1
info.txt(1) : parse errorDone. Created 0 samples

1.3创建正样本描述文件vec文件

打开\tools下的samples_creation.bat文件，可见到如下指令：

1
2
3
4
5
6
cd temp
createsamples.exe -info positive/info.txt -vec data/vector.vec -num 4346 -w 24 -h 24
rem positive/positive_bmp_list.txt - file containing list of positive bmp files along with the rectagle coordinates from objectmarker.exe
rem -vec data/vector.vec - file created by the createsamples tool
rem all the other paramteres are described in the literature (links in the how_to.txt)
pause

其中我们需要根据实际情况修改的参数是-num、-w和-h。
①-num表示正样本的数量；
②-w和-h表示图片压缩后的大小；
运行samples_creation.bat文件后，会在\tools\temp\data下面生成vector.vec文件，这个就是向量描述文件了。

1.4准备负样本图片集

进入路径为\tools\temp\negative的文件夹中，然后在里面放入负样本图片。一般来说，负样本要比正样本数目多很多。为什么呢？因为负样本表示的是随机情况，只要有大量随机样本的情况下，才可以充分表示与正样本相反的情况，大约2-5倍。但也不能太多，不然的话花费的时间太长，分类效果可能反而不好，因为比例偏移太严重。
注：负样本图片也需要采用.BMP格式。

1.5创建负样本描述文件

运行\tools\temp\negative下的create_list.bat文件，可以自动生成描述文件。如果没有找到这个create_list.bat批处理文件，可以自行在该目录下创建一个.bat文件，内容就一句话：dir /b *.BMP >infofile.txt，然后保存。

2.训练分类器

2.1训练器的配置

打开\tools下的haarTraining.bat文件，首先对训练器进行配置。
①－data<dir_name>：存放训练好的分类器的路径；
②－vec<vec_file_name>：正样本描述文件路径；
③－bg<background_file_name>：负样本描述文件路径；
④－npos<number_of_positive_samples>：在每个阶段用于训练的正样本数目；
⑤－nneg<number_of_negative_samples>：在每个阶段用于训练的负样本数目。程序会自动切割负样本图片，使其和正样本大小一致，这个参数一般设置为正样本数目的1~3倍；
⑥－nstages<number_of_stages>：训练的分类器级数，一般选择15~20。每一级的训练目的是为了得到一个强分类器；
⑦－nsplits<number_of_splits>：一个弱分类器中分裂的子节点数目或特征个数。如果1，则只有一个简单的stump classifier被使用。如果是2或者更多，则带有number_of_splits 个内部节点的CART分类器被使用；
⑧－mem<memory_in_MB>：训练时的可用内存，单位为MB。内存越大则训练的速度越快；
⑨－sym（default）和－nonsym：后面不用跟其他参数，指定训练的目标对象是否垂直对称。垂直对称提高目标的训练速度。
⑩－minhitrate<min_hit_rate>：每一级分类器需要的最小的命中率（正检率），默认为0.995。针对每一个正样本累加这级强分类器的所有弱分类器的权重，然后对所有正样本得到的累加权重按从小到大排序，因为minhitrate决定至少要分对minhitrate*npos个正样本，所以取第（1.0-minhitrate）*npos正样本的权重为这级强分类器的阈值，通过这个阈值将大于这个阈值的分为正类，否则分为负类。所以minhitrate这个值决定每个阶段分类器的正样本的正检率，总的正检率为min_hit_rate 的number_of_stages 次方；
⑪－maxfalsealarm<max_false_alarm_rate>：每一级分类器的最大错误报警率（误检率），默认为0.5，总的误检率为max_false_alarm_rate 的number_of_stages 次方。前面通过minhitrate参数与弱分类器的权重累加和，得到了该强分类器的阈值后，通过这个阈值来分类负样本，负样本的分错个数不能超过numneg*maxfalsealarm，如果超过则继续训练本级强分类器，增加新弱分类器直到阈值将负样本分错个数小于numneg*maxfalsealarm为止就跳出循环，则本级强分类器训练成功；
⑫－weighttrimming<weight_trimming>：权重调整系数weightfraction。在每级强分类器的训练中每训练一个弱分类器前将所有样本中，包括正样本与负样本，按权重从小到大排列将权重最低的weightfraction * (npos+nneg)的样本去除掉，因为权重很小说明这个样本总能被正确分类了，删除这些已经能正确分类的样本，让训练更多集中于还不能正确分类的样本上，提高训练效率。默认为0.95；
⑬－eqw：如果有-eqw则equalweights值为1，否则默认为0。1表示所有样本的初始化权重相等，0表示不等；
⑭－mode<BASIC,CORE,ALL>：BASIC仅使用垂直特征（适用于人脸等），CORE仅使用45度旋转特征，ALL使用Haar特征集的种类既有垂直又有45度角旋转的；
⑮－w<sample_width>：样本图片的宽度（以像素为单位）；
⑯－h<sample_height>：样本图片的高度（以像素为单位）；
⑰－bt<DAB,RAB,LB,GAB>：Discrete AdaBoost、Real AdaBoost、LogitBoost和Gentle AdaBoost分别代表4个应用的boost算法的种类，默认为GAB；
⑱－err<misclass,gini,entropy,least sum of squares>：分别代表四种在训练弱分类器时的计算阈值方法，默认为misclass；
⑲－maxtreesplits<maximum_number_of_nodes_in_tree>：树节点数的最大值，默认为0；
⑳－minpos<min_positive>：训练过程中，节点可使用的正样本数目。正样本根据节点被分类。通常来说，minpos不小于npos/nsplits。

2.2运行训练器

①BACKGROUND PROCESSING TIME：负样本切割时间，单位是秒，一般会占用很长时间；
②N：训练的弱分类器序号；
③%SMP：由weightfraction决定，表示通过剔除小权值的样本后与总体样本数的比值；
④ST. THR：弱分类器的阈值；
⑤HR：该阈值下本级强分类器的正检率；
⑥FA：该阈值下本级强分类器的误检率，只有当每一级训练的FA低于定义的最大误检率maxfalsealarm才会进入下一级的训练；
⑦EXP. ERR：经验错误率；

训练过程会在\tools\temp\data\cascade中产生相应级数的文件，训练花费的时间跟电脑配置和训练数据的大小有关。如果训练过程中断也没关系，再次启动训练可以继续训练下去的。
训练完成后，将\temp\data\cascade下的N（这里N=18）个弱分类器文件夹拷贝到cascade2xml\data下，运行\tools\cascade2xml下的convert.bat文件就可以生成.xml分类器文件了，默认在同级目录下生成output.xml。

3.目标检测

如下是launch文件的内容：

1
2
3
4
5
6
7
8
9
10
11
12
<launch>
   	<node pkg="robot_vision" name="vehicle_detector" type="vehicle_detector.py" output="screen">
       	<rosparam>
           	haar_scaleFactor: 1.2
           	haar_minNeighbors: 2
           	haar_minSize: 20
          	haar_maxSize: 55
       	</rosparam>
       	<param name="cascade_vehicle" value="$(find robot_vision)/data/haar_detectors/output.xml" />
       	<param name="video_vehicle" value="$(find robot_vision)/data/video/video2.mp4" />
   	</node>
</launch>

参数说明：
①haar_scaleFactor：检测窗口的缩放比例，一般为1.1或1.2；
②haar_minNeighbors：构成检测目标的相邻矩形的最小个数(默认为－1)。如果组成检测目标的小矩形的个数和小于min_neighbors-1 都会被排除。如果min_neighbors 为 0, 则函数不做任何操作就返回所有的被检候选矩形框，这种设定值一般用在用户自定义对检测结果的组合程序上；
③haar_minSize：检测窗口的最小尺寸，默认情况下被设为分类器训练时采用的样本尺寸（即24x24）；
④haar_maxSize：检测窗口的最大尺寸；
⑤cascade_vehicle：训练好的分类器.xml文件路径；
⑥video_vehicle：需要进行目标识别的视频文件路径。
如下是vehicle_detector.py文件内容：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, RegionOfInterest
from cv_bridge import CvBridge, CvBridgeError

class vehicleDetector:
   	def __init__(self):
       	# 创建cv_bridge
       	self.bridge = CvBridge()
       	self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)
       
       	video = rospy.get_param("~video_vehicle", "")
       	cascade_vehicle = rospy.get_param("~cascade_vehicle", "")
       
       	# 使用级联表初始化haar特征检测器
       	self.cascade_vehicle = cv2.CascadeClassifier(cascade_vehicle)
       
       	# 设置级联表的参数，优化目标识别，可以在launch文件中重新配置
       	self.haar_scaleFactor  = rospy.get_param("~haar_scaleFactor", 1.2)
      	self.haar_minNeighbors = rospy.get_param("~haar_minNeighbors", 2)
       	self.haar_minSize      = rospy.get_param("~haar_minSize", 40)
       	self.haar_maxSize      = rospy.get_param("~haar_maxSize", 60)
       	self.color = (50, 255, 50)
       
       	cap = cv2.VideoCapture(video)
       
	 	# 获取视频图像
       	while(cap.isOpened()):
           	ret, cv_image = cap.read()
           	cv_image_compressed = cv2.resize(cv_image, (1280,720))
           	frame = cv_image_compressed
		
           	if ret==True:
               	# 创建灰度图像
               	grey_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

               	# 创建平衡直方图，减少光线影响
               	grey_image = cv2.equalizeHist(grey_image)

               	vehicle_result = self.detect_vehicle(grey_image)
       		
               	# 将识别到的车辆框出来
               	if len(vehicle_result)>0:
                   	for vehicle in vehicle_result:
                       	x, y, w, h = vehicle
                       	cv2.rectangle(cv_image_compressed, (x, y), (x+w, y+h), self.color, 2)

               	cv2.imshow('frame',cv_image_compressed)
               	if cv2.waitKey(25) & 0xFF == ord('q'):
                   	break
           	else:
               	break

   	def detect_vehicle(self, input_image):
       	# 匹配车辆模型
       	if self.cascade_vehicle:
           	vehicles = self.cascade_vehicle.detectMultiScale(input_image, 
                   self.haar_scaleFactor, 
                   self.haar_minNeighbors, 
                   cv2.CASCADE_SCALE_IMAGE, 
                   (self.haar_minSize, self.haar_maxSize))
       
       	return vehicles

   	def cleanup(self):
       	print "Shutting down vision node."
       	cv2.destroyAllWindows()

if __name__ == '__main__':
   	try:
       	# 初始化ros节点
       	rospy.init_node("vehicle_detector")
       	vehicleDetector()
       	rospy.spin()
   	except KeyboardInterrupt:
       	print "Shutting down face detector node."
       	cv2.destroyAllWindows()

4.演示视频

发表于 | 分类于

参考链接：
(1) https://blog.csdn.net/JasonDing1354/article/details/37558287
(2) https://blog.csdn.net/zouxy09/article/details/7922923
(3) https://blog.csdn.net/zouxy09/article/details/7929570

1.前言

我们要探讨的Haar分类器实际上是Boosting算法的一个应用，Haar分类器用到了Boosting算法中的AdaBoost算法，只是把AdaBoost算法训练出的强分类器进行了级联，并且在底层的特征提取中采用了高效率的矩形特征和积分图方法，这里涉及到的几个名词接下来会具体讨论。
而AdaBoost算法是Freund和Schapire在1995年提出的算法，是对传统Boosting算法的一大提升。Boosting算法的核心思想，是将弱学习方法提升成强学习算法。
Haar分类器算法的要点如下：
①使用Haar-like特征做检测。
②使用积分图（Integral Image）对Haar-like特征求值进行加速。
③使用AdaBoost算法训练区分人脸和非人脸的强分类器。
④使用筛选式级联把强分类器级联到一起，提高准确率。
即Haar分类器 = Haar-like特征 + 积分图方法 + AdaBoost + 级联

2.Haar-like特征

最原始的Haar-like特征在2002年的《A general framework for object detection》提出，它定义了四个基本特征结构，如下A，B，C，D所示，可以将它们理解成为一个窗口，这个窗口将在图像中做步长为1的滑动，最终遍历整个图像。

比较特殊的一点是，当一次遍历结束后，窗口将在宽度或长度上成比例的放大，再重复之前遍历的步骤，直到放大到最后一个比例后结束。
那么可以放大的比例系数如何确定呢？设在宽度上可以放大的最大倍数为Kw，高度上可以放大的最大倍数为Kh，计算公式如下：

其中wI和hI是整个图像的宽高，wwin和hwin是Haar窗口的初始宽、高，可以放大的倍数为Kw⋅Kh。
Haar-like特征提取过程就是利用上面定义的窗口在图像中滑动，滑动到一个位置的时候，将窗口覆盖住的区域中的白色位置对应的像素值的和减去黑色位置对应的像素值的和，得到的一个数值就是Haar特征中一个维度。
其中对于窗口C，黑色区域的像素值加和要乘以2，是为了像素点个数相同而增加的权重。
在基本的四个haar特征基础上，文章《An extended set of Haar-like features for rapid object detection》对其做了扩展，将原来的4个扩展为14个。这些扩展特征主要增加了旋转性，能够提取到更丰富的边缘信息。

如何理解特征呢？以人脸识别为例，我们将上面的任意一个矩形放到人脸区域上，然后将白色区域的像素和减去黑色区域的像素和，得到的值我们暂且称之为人脸特征值，如果你把这个矩形放到一个非人脸区域，那么计算出的特征值应该和人脸特征值是不一样的，而且越不一样越好，所以这些方块的目的就是把人脸特征量化，以区分人脸和非人脸。
为了增加区分度，可以对多个矩形特征计算得到一个区分度更大的特征值，那么什么样的矩形特征怎么样的组合到一块可以更好的区分出人脸和非人脸呢，这就是AdaBoost算法要做的事了。

3.AdaBoost算法

AdaBoost算法的老祖宗可以说是机器学习的一个模型，它的名字叫PAC(Probably Approximately Correct)。
PAC模型是计算学习理论中常用的模型，由Leslie Valiant在1984年提出，他认为“学习”是模式明显清晰或模式不存在时仍能获取知识的一种“过程”，并给出了一个从计算角度来获得这种“过程”的方法，这种方法包括：
①适当信息收集机制的选择；
②学习的协定；
③对能在合理步骤内完成学习的概念的分类。
PAC学习的实质就是在样本训练的基础上，使算法的输出以概率接近未知的目标概念。PAC学习模型是考虑样本复杂度（指学习器收敛到成功假设时至少所需的训练样本数）和计算复杂度（指学习器收敛到成功假设时所需的计算量）的一个基本框架，成功的学习被定义为形式化的概率理论。简单说来，PAC学习模型不要求你每次都正确，只要能在多项式样本和多项式时间内得到满足需求的正确率，就算是一个成功的学习。
基于PAC学习模型的理论分析，Valiant提出了Boosting算法。Boosting算法涉及到两个重要的概念就是弱学习和强学习，所谓的弱学习，就是指一个学习算法对一组概念的识别率只比随机识别好一点，所谓强学习，就是指一个学习算法对一组概率的识别率很高。Kearns和Valiant提出了弱学习和强学习等价的问题，并证明了只要有足够的数据，弱学习算法就能通过集成的方式生成任意高精度的强学习方法。
针对Boosting的若干缺陷，Freund和Schapire于1996年前后提出了一个实际可用的自适应Boosting算法AdaBoost，AdaBoost目前已发展出了大概四种形式的算法，Discrete AdaBoost、Real AdaBoost、LogitBoost、gentle AdaBoost。

3.1弱分类器

最初的弱分类器可能只是一个最基本的Haar-like特征，计算输入图像的Haar-like特征值，和最初的弱分类器的特征值比较，以此来判断输入图像是不是人脸，然而这个弱分类器太简陋了，可能并不比随机判断的效果好，对弱分类器的孵化就是训练弱分类器成为最优弱分类器，注意这里的最优不是指强分类器，只是一个误差相对稍低的弱分类器，训练弱分类器实际上是为分类器进行设置的过程。我们首先分别看下弱分类器的数学结构和代码结构。
数学结构：

其中，hj(x)为弱分类器的判断值，值为1则说明图片为人脸，否则非人脸；x表示输入的图片子窗口，fj(x)为x图像上第j个特征的值；θj为分类器阈值；pj为不等号方向，若分类结果大于阈值，则为−1，否则为+1，以保证不等号<方向不变。
注意：输入x为一幅数字图像，特征fj与弱分类器hj(x)是一一对应的关系；一个弱分类器的训练就是找到最优阈值θj的过程，一轮分类器的训练过程就是找到分类效果最好的弱分类器的过程。
代码结构：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
/*

  * CART classifier

  */

 typedef struct CvCARTHaarClassifier

 {

     CV_INT_HAAR_CLASSIFIER_FIELDS()

     int count;

     int* compidx;

     CvTHaarFeature* feature;

     CvFastHaarFeature* fastfeature;

     float* threshold;

     int* left;

     int* right;

     float* val;

 } CvCARTHaarClassifier;

代码结构中的threshold即代表数学结构中的θ阈值。
这个阈值究竟是干什么的？我们先了解下CvCARTHaarClassifier这个结构，注意CART这个词，它是一种二叉决策树，它的提出者Leo Breiman等称其为“分类和回归树（CART）”。
机器学习中，决策树是一个预测模型；他代表的是对象属性与对象值之间的一种映射关系。树中每个节点表示某个对象，而每个分叉路径则代表的某个可能的属性值，而每个叶结点则对应从根节点到该叶节点所经历的路径所表示的对象的值。决策树仅有单一输出，若欲有复数输出，可以建立独立的决策树以处理不同输出。从数据产生决策树的机器学习技术叫做决策树学习,通俗说就是决策树。
决策树用途很广可以分析因素对事件结果的影响，同时也是很常用的分类方法，我举个最简单的决策树例子，假设我们使用三个Haar-like特征f1，f2，f3来判断输入数据是否为人脸，可以建立如下决策树：

可以看出，在分类的应用中，每个非叶子节点都表示一种判断，每个路径代表一种判断的输出，每个叶子节点代表一种类别，并作为最终判断的结果。
一个弱分类器就是一个基本和上图类似的决策树，最基本的弱分类器只包含一个Haar-like特征，也就是它的决策树只有一层，被称为树桩（stump）。

3.2弱分类器的训练

最重要的是如何决定每个节点判断的输出，要比较输入图片的特征值和弱分类器中特征，一定需要一个阈值，当输入图片的特征值大于该阈值时才判定其为人脸。训练最优弱分类器的过程实际上就是在寻找合适的分类器阈值，使该分类器对所有样本的判读误差最低。
具体操作过程如下：
①对于每个特征f，计算所有训练样本的特征值，并根据特征值大小对特征升序排序；
②扫描一遍排好序的特征值，对排好序的表中的每个元素，计算下面四个值：
●全部人脸样本的权重的和T1；
●全部非人脸样本的权重的和T0；
●在此元素之前的人脸样本的权重的和S1；
●在此元素之前的非人脸样本的权重的和S0；
③选取当前元素特征值fi(x)，和它前面的一个特征值fi-1(x)之间的值作为阈值，该阈值的分类误差为：e = min( (S1+(T0-S0) ,(S0+(T1-S1))
通过把这个排序的表从头到尾扫描一遍可以为弱分类器选择使分类误差最小的阈值（即最优阈值），也就是得到一个最佳弱分类器。

3.3强分类器

AdaBoost训练出来的强分类器一般具有较小的误识率，但检测率并不很高，一般情况下，高检测率会导致高误识率，这是强分类阈值的划分导致的，要提高强分类器的检测率要降低阈值，要降低强分类器的误识率就要提高阈值，这是个矛盾的事情。增加分类器个数可以在提高强分类器检测率的同时降低误识率，所以级联分类器在训练时要考虑如下平衡，一是弱分类器的个数和计算时间的平衡，二是强分类器检测率和误识率之间的平衡。
首先看一下强分类器的代码结构:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
/* internal stage classifier */

 typedef struct CvStageHaarClassifier

 {

     CV_INT_HAAR_CLASSIFIER_FIELDS()

     int count;

     float threshold;

     CvIntHaarClassifier** classifier;

 }CvStageHaarClassifier;

/* internal weak classifier*/

typedef struct CvIntHaarClassifier

{

CV_INT_HAAR_CLASSIFIER_FIELDS()

} CvIntHaarClassifier;

这里要提到的是CvIntHaarClassifier结构：它就相当于一个接口类，当然是用C语言模拟的面向对象思想，利用CV_INT_HAAR_CLASSIFIER_FIELDS()这个宏让弱分类器CvCARTHaarClassifier和强分类器CvStageHaarClassifier继承于CvIntHaarClassifier。
强分类器的诞生需要T轮的迭代，具体操作如下：
①给定训练样本集S，共N（N=X+Y）个样本，其中X和Y分别对应于正样本和负样本的数量，T为训练的最大循环次数；
②初始化样本权重为1/N，即为训练样本的初始概率分布； 　　
③第一次迭代训练N个样本，得到第一个最优弱分类器；
④提高上一轮中被误判的样本的权重；
⑤将新的样本和上次区分错误的样本放在一起进行新一轮的训练；
⑥循环执行4-5步骤，T轮后得到T个最优弱分类器；
⑦组合T个最优弱分类器得到强分类器，组合方式如下：

其中，ht(x)为第t个弱分类器的输出值，αt为弱分类器的权重，它的数学公式为αt=[log((1-et)/et)]，et为弱分类器对训练样本集的错误率。
下图是强分类器的逻辑结构：

在OpenCV中，强分类器是由多个弱分类器“并列”构成，即强分类器中的弱分类器是两两相互独立的。在检测目标时，每个弱分类器独立运行并输出值cascadeLeaves[leafOfs - idx]，然后把当前强分类器中每一个弱分类器的输出值相加求和。因此强分类器的决策相当于让所有弱分类器投票，再对投票结果按照弱分类器的错误率加权求和，将投票加权求和的结果与平均投票结果比较得出最终的结果。

3.4强分类器的级联

我们通过AdaBoost算法训练出了强分类器，然而在现实的人脸检测中，只靠一个强分类器还是难以保证检测的正确率，这个时候，需要一个豪华的阵容，来训练出多个强分类器将它们强强联手，最终形成正确率很高的级联分类器这就是我们最终的目标Haar分类器。
在一个级联分类系统中，对于每一个输入图片，对图片进行多区域，多尺度的检测，所谓多区域，是要对图片划分多块，对每个块进行检测，由于训练的时候用的照片一般都是20*20左右的小图片，所以对于大的人脸，还需要进行多尺度的检测，多尺度检测机制一般有两种策略，一种是不改变搜索窗口的大小，而不断缩放图片，这种方法显然需要对每个缩放后的图片进行区域特征值的运算，效率不高。而另一种方法，是不断初始化搜索窗口size为训练时的图片大小，不断扩大搜索窗口，进行搜索，解决了第一种方法的弱势。在区域放大的过程中会出现同一个人脸被多次检测，这需要进行区域的合并，这里不作探讨。
无论哪一种搜索方法，都会为输入图片输出大量的子窗口图像，这些子窗口图像经过筛选式级联分类器会不断地被每一个节点筛选，抛弃或通过。
另外前面的强分类器相对简单，其包含的弱分类器也相对较少，后面的强分类器逐级复杂，只有通过前面的强分类检测后的图片才能送入后面的强分类器检测，比较靠前的几级分类器可以过滤掉大部分的不合格图片，只有通过了所有强分类器检测的图片区域才是有效的人脸区域。

4.积分图

在上面Haar-like特征这节说到，将模板的特征值定义为白色矩形像素和减去黑色矩形像素和。而积分图就是只遍历一次图像就可以求出图像中所有区域像素和的快速算法，大大的提高了图像特征值计算的效率。
积分图的构造方式是位置（i,j）处的值ii(i,j)是原图像(i,j)左上角方向所有像素的和：

积分图构建算法：
①用s(i,j)表示行方向的累加和，初始化s(i,-1)=0；
②用ii(i,j)表示一个积分图像，初始化ii(-1,i)=0；
③逐行扫描图像，递归计算每个像素f(i,j)行方向的累加和s(i,j)和积分图像ii(i,j)的值；

④扫描图像一遍，当到达图像右下角像素时，积分图像ii就构造好了。

上面这幅图中有四个区域，A，B，C，D。我们将左上角的点记为0，区域A的所有点像素值的和记为sumA，点0与点1之间的积分图记为integral0,1，那么根据积分图的定义：


区域D的像素值和sumD就应该为integral1,4，但是注意，积分图中是没有从点1到点4的概念的，它所有的起点都应该是点0，所以：

转化一下就是

上面的内容就是积分图，比如说我们要求sumDsumD，并不需要从点1到点4做行列的遍历，因为这个遍历过程的时间复杂度是O(mn)的。我们只需要先存在下来从0到点1，2，3，4的积分图，然后做一个简单的加减法就好了，这个时间复杂度仅仅为O(1)。当然了，存储的过程是消耗空间复杂度的，这是很典型的空间换时间的套路。这就是上面提到的积分图加速计算的过程。

发表于 | 分类于

1.算法简介

相关链接：https://zhuanlan.zhihu.com/p/48117381

1.1单车模型(Bicycle Model)

单车模型实际上是对阿克曼转向几何的一个简化，使用单车模型需做如下假设：
（1）不考虑车辆在Z轴方向的运动，只考虑XY水平面的运动；
（2）左右侧车轮转角一致，这样可将左右侧轮胎合并为一个轮胎，以便于搭建单车模型；
（3）车辆行驶速度变化缓慢，忽略前后轴载荷的转移；
（4）车身及悬架系统是刚性的。
采用单车模型的一大好处就在于它简化了前轮转向角与后轴将遵循的曲率之间的几何关系，其关系如下式所示：

其中δ表示前轮的转角，L为轴距，R则为在给定的转向角下后轴遵循着的圆的半径。
从自行车模型出发，纯跟踪算法以车后轴为切点，车辆纵向车身为切线，通过控制前轮转角，使车辆可以沿着一条经过目标路点（goal point）的圆弧行驶，如下图所示：

图中（gx,gy）是我们下一个要追踪的路点，它位于我们已经规划好的全局路径上，现在需要控制车辆的后轴经过该路点，ld表示车辆当前位置（即后轴位置）到目标路点的距离，α表示目前车身姿态和目标路点的夹角，那么根据正弦定理我们可以推导出如下转换式：



因为道路的曲率，上式也可以表示为：

则由式子，可得

2.源码解析

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
def __init__(self):
	rospy.init_node('pure_persuit', log_level=rospy.DEBUG)

	rospy.Subscriber('/rear_pose', PoseStamped, self.pose_cb, queue_size = 1)
	rospy.Subscriber('/velocity', TwistStamped, self.vel_cb, queue_size = 1)
	rospy.Subscriber('/final_waypoints', Lane, self.lane_cb, queue_size = 1)

	self.left_vel_pub =rospy.Publisher('/rear_left_velocity_controller/command', Float64, queue_size = 10)
	self.right_vel_pub = rospy.Publisher('/rear_right_velocity_controller/command', Float64, queue_size = 10)
	self.left_steer_pub = rospy.Publisher('/left_bridge_position_controller/command', Float64, queue_size = 10)
	self.right_steer_pub = rospy.Publisher('/right_bridge_position_controller/command', Float64, queue_size = 10)
	
	self.currentPose = None
	self.currentVelocity = None
	self.currentWaypoints = None

	self.loop()

说明：

1
2
3
4
self.left_vel_pub =rospy.Publisher('/rear_left_velocity_controller/command', Float64, queue_size = 10)
self.right_vel_pub = rospy.Publisher('/rear_right_velocity_controller/command', Float64, queue_size = 10)
self.left_steer_pub = rospy.Publisher('/left_bridge_position_controller/command', Float64, queue_size = 10)
self.right_steer_pub = rospy.Publisher('/right_bridge_position_controller/command', Float64, queue_size = 10)

上面这三段函数创建了三个ROS话题订阅者，分别向ROS系统订阅/rear_pose、/velocity和/final_waypoints三种话题消息，即小车后轮轴中心点坐标、小车速度和局部路径点信息，而这三种话题消息对应的回调函数分别为pose_cb、vel_cb和lane_cb。
1
2
3
4
self.left_vel_pub =rospy.Publisher('/rear_left_velocity_controller/command', Float64, queue_size = 10)
self.right_vel_pub = rospy.Publisher('/rear_right_velocity_controller/command', Float64, queue_size = 10)
self.left_steer_pub = rospy.Publisher('/left_bridge_position_controller/command', Float64, queue_size = 10)
self.right_steer_pub = rospy.Publisher('/right_bridge_position_controller/command', Float64, queue_size = 10)

上面这四段函数创建了四个ROS话题发布者，分别发布/rear_left_velocity_controller/command、/rear_right_velocity_controller/command、/left_bridge_position_controller/command和/right_bridge_position_controller/command四种话题消息，即小车后轮和前轮的速度指令。
1
2
3
4
5
6
7
def loop(self):
	rate = rospy.Rate(20)
	rospy.logwarn("pure persuit starts")
	while not rospy.is_shutdown():
		if self.currentPose and self.currentVelocity and self.currentWaypoints:
			self.calculateTwistCommand()
		rate.sleep()

说明：
●rate = rospy.Rate(20)设置该函数的循环频率为20Hz。
●while not rospy.is_shutdown():表示若ROS未被关闭，则进行以下操作。
●if self.currentPose and self.currentVelocity and self.currentWaypoints:表示若self.currentPose、self.currentVelocity和self.currentWaypoints均为非空时，则进行以下操作。
1
2
3
4
5
6
7
8
def pose_cb(self,data):
	self.currentPose = data

def vel_cb(self,data):
	self.currentVelocity = data

def lane_cb(self,data):
	self.currentWaypoints = data

说明：
●这三个回调函数分别将/rear_pose、/velocity和/final_waypoints的话题消息内容提取出来，赋给self.currentPose、self.currentVelocity和self.currentWaypoints。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
def ackermann_steering_control(self, velocity, radian):
	global left_angle, left_speed, right_angle, right_speed 	
	if radian > 0:
		inside_radius = L / math.tan(radian) - T / 2
		outside_radius = L / math.tan(radian) + T / 2
	else:
		outside_radius = L / math.tan(radian) - T / 2
		inside_radius = L / math.tan(radian) + T / 2

	outside_speed = velocity * ( 1 + T * math.tan(abs(radian)) / ( 2 * L ) )
	inside_speed = velocity * ( 1 - T * math.tan(abs(radian)) / ( 2 * L ) )

	inside_angle = math.atan( L / inside_radius )
	outside_angle = math.atan( L / outside_radius )

	if radian > 0:
		left_angle = outside_angle
		left_speed = outside_speed
		right_angle = inside_angle
		right_speed = inside_speed
	else:
		right_angle = outside_angle
		right_speed = outside_speed
		left_angle = inside_angle
		left_speed = inside_speed

说明：
●该函数以小车的期望转角（弧度）和期望速度为输入，分别根据阿克曼转向模型控制小车两个前轮的转角和两个后轮的转速。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
def calculateTwistCommand(self):
	lad = 0.0 #look ahead distance accumulator
	targetIndex = len(self.currentWaypoints.waypoints) - 1
	for i in range(len(self.currentWaypoints.waypoints)):
		if((i+1) < len(self.currentWaypoints.waypoints)):
			this_x = self.currentWaypoints.waypoints[i].pose.pose.position.x
			this_y = self.currentWaypoints.waypoints[i].pose.pose.position.y
			next_x = self.currentWaypoints.waypoints[i+1].pose.pose.position.x
			next_y = self.currentWaypoints.waypoints[i+1].pose.pose.position.y
			lad = lad + math.hypot(next_x - this_x, next_y - this_y)
			if(lad > HORIZON):
				targetIndex = i+1
				break

	targetWaypoint = self.currentWaypoints.waypoints[targetIndex]

	targetSpeed = self.currentWaypoints.waypoints[0].twist.twist.linear.x

说明：
●lad = 0.0设置的是前瞻距离。
●targetIndex = len(self.currentWaypoints.waypoints) – 1的作用是获取局部路径的长度（局部路径点的数量），并设置列表的大小，其中len(self.currentWaypoints.waypoints)的值就等于waypoints_updater.py中设置的LOOKAHEAD_WPS。
●for i in range(len(self.currentWaypoints.waypoints)):此处是遍历整个局部路径点列表。
●this_x和this_y表示当前最近的局部路径点横、纵坐标，next_x和next_y表示下一个路径点的横、纵坐标。计算当前路径点与下一个路径点的距离，并累加到前瞻距离lad。当前瞻距离lad达到阈值HORIZON时，将该点设置为目标路径点，即targetWaypoint。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
targetX = targetWaypoint.pose.pose.position.x
targetY = targetWaypoint.pose.pose.position.y		
currentX = self.currentPose.pose.position.x
currentY = self.currentPose.pose.position.y
#get vehicle yaw angle
quanternion = (self.currentPose.pose.orientation.x, self.currentPose.pose.orientation.y, self.currentPose.pose.orientation.z, self.currentPose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quanternion)
yaw = euler[2]
#get angle difference
alpha = math.atan2(targetY - currentY, targetX - currentX) - yaw
l = math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2))
if(l > 0.5):
	theta = math.atan(2 * 0.16 * math.sin(alpha) / l)
	self.ackermann_steering_control(targetSpeed, -theta)
	# #get twist command
	left_steer = Float64()
	right_steer = Float64()
	left_vel = Float64()
	right_vel = Float64()
	left_steer.data = left_angle
	right_steer.data = right_angle
	left_vel.data = left_speed
	right_vel.data = right_speed 
else:
	left_steer = Float64()
	right_steer = Float64()
	left_vel = Float64()
	right_vel = Float64()
	left_steer.data = 0
	right_steer.data = 0
	left_vel.data = 0
	right_vel.data = 0
	
self.left_vel_pub.publish(left_vel)
self.right_vel_pub.publish(right_vel)
self.left_steer_pub.publish(left_steer)
self.right_steer_pub.publish(right_steer)

说明：
●targetX和targetY表示目标路径点的横、纵坐标，currentX和currentY表示小车当前的坐标点横、纵坐标。
●euler = tf.transformations.euler_from_quaternion(quanternion)的作用是将小车当前的姿态信息由四元数格式转换到欧拉角的格式，即以roll、pitch和yaw轴偏向角的形式存储在变量euler中。
●alpha = math.atan2(targetY - currentY, targetX - currentX) – yaw的作用是计算小车当前的yaw角与目标点之间的角度偏差。
●l = math.sqrt(math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2))用于计算小车当前位置点与目标路径点的距离。
●当l < 0.5，即小车快要到达全局路径的终点时，直接将小车速度设置为0。反之，则通过Pure Persuit算法得到前轮转向角度，再将该角度作为Ackermann算法的输入，从而实现对小车横向和纵向控制，使其能够沿着全局路径行驶。

3.效果演示

发表于 | 分类于

1.全局路径的读取

waypoint_loader.py的作用是读取.csv文件中的路径点信息，并发布消息waypoints和base_path，其中waypoints包含了路径点的位置信息、小车的期望速度和表明小车是前进还是后退的状态位，base_path包含了实现Rviz可视化的路径点坐标。

1
2
3
4
5
6
7
def new_waypoint_loader(self, path):
       if os.path.isfile(path):
           waypoints, base_path = self.load_waypoints(path)
           self.publish(waypoints, base_path)
           rospy.loginfo('Waypoint Loded')
       else:
           rospy.logerr('%s is not a file', path)

说明：
●os.path.isfile(path)用于判断某一对象是否为文件，其中path必须为绝对路径。
●self.load_waypoints(path)用于加载.csv文件，并以包含路径信息的消息作为返回值。
●self.publish(waypoints, base_path)用于发布话题消息。
●rospy.loginfo(‘Waypoint Loded’)用于输出INFO日志信息。
●rospy.logerr(‘%s is not a file’, path)当判断该对象不为文件时，输出ERROR日志信息。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
def load_waypoints(self, fname):
       waypoints = []
       base_path = Path()
       base_path.header.frame_id = 'world'
       with open(fname) as wfile:
           reader = csv.DictReader(wfile, CSV_HEADER)
           for wp in reader:
               p = Waypoint()
               p.pose.pose.position.x = float(wp['x'])
               p.pose.pose.position.y = float(wp['y'])
               q = self.quaternion_from_yaw(float(wp['yaw']))
               p.pose.pose.orientation = Quaternion(*q)
               p.twist.twist.linear.x = float(self.velocity)
               p.forward = True
               waypoints.append(p)

               path_element = PoseStamped()
               path_element.pose.position.x = p.pose.pose.position.x
               path_element.pose.position.y = p.pose.pose.position.y
               base_path.poses.append(path_element)

               
       waypoints = self.decelerate(waypoints)
       return waypoints,base_path

说明：
●waypoints = []创建了一个空列表，命名为waypoint。
●base_path = Path()和base_path.header.frame_id = ‘world’定义了一个Path类的对象base_path，并以world作为frame id。
●with open(fname) as wfile的作用是打开文件（保证在文件出错后能够正常关闭文件），其中fname在此处是文件的绝对路径。
●reader = csv.DictReader(wfile, CSV_HEADER)以CSV_HEADER的构造格式，即‘x’、‘y’和‘yaw’格式读取文件内容。
●for wp in reader:在此处遍历文件中的每一行。

●p = Waypoint()定义了一个Waypoint类的对象，其中Waypoint类在styx_msgs文件夹的Waypoint.msg中进行了自定义，包含了geometry_msgs/PoseStamped pose、geometry_msgs/TwistStamped twist和bool forward，分别表示小车在环境中的x、y坐标、小车的期望速度以及表明小车是前进还是后退的标志位。

●p.pose.pose.position.x = float(wp[‘x’])和p.pose.pose.position.y = float(wp[‘y’])用于将文件中的x、y所对应的数据填充到相应的消息内容中。
●q = self.quaternion_from_yaw(float(wp[‘yaw’]))即调用tf.transformations.quaternion_from_euler(0., 0., yaw)该函数，用于将 (0., 0., yaw)欧拉角转换成四元数quaternion。
●p.pose.pose.orientation = Quaternion(*q)该操作相当于p.pose.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])，用于将元祖扩展成参数列表，并将四元数数据填充到相应的消息内容中。
●p.twist.twist.linear.x = float(self.velocity)用于将期望的线速度值填充到相应的消息内容中。
●waypoints.append(p)用于将Waypoint类的对象p，即路径信息添加到列表waypoints中。
●path_element = PoseStamped()定义了一个PoseStamped类的对象，用于在Rviz中可视化路径。
●waypoints = self.decelerate(waypoints)根据小车当前位置与终点位置来控制小车速度，并保证在终点位置速度为零。
1
2
3
4
5
6
7
8
9
10
def decelerate(self, waypoints):
       last = waypoints[-1]
       last.twist.twist.linear.x = 0.
       for wp in waypoints[:-1][::-1]:
           dist = self.distance(wp.pose.pose.position, last.pose.pose.position)
           vel = math.sqrt(2 * MAX_DECEL * dist)
           if vel < 1.:
               vel = 0.
           wp.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)
       return waypoints

说明：
●last = waypoints[-1]用于将waypoints列表中的最后一个值，即路径的终点信息赋给变量last。
●for wp in waypoints[:-1][::-1]:用于遍历waypoints列表中的所有值。
●dist = self.distance(wp.pose.pose.position, last.pose.pose.position)用于计算当前坐标点与终点坐标的距离。
●vel = math.sqrt(2 * MAX_DECEL * dist)根据小车当前位置距离终点的路程dist、人为设定的减速度MAX_DECEL和到达终点时的速度（速度为零）来计算得到小车当前的期望速度vel，其中所运用到的是高中物理公式Vt^2 - V0^2 = 2as。
●wp.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)表示在距离终点较远的位置，按照wp.twist.twist.linear.x的默认期望速度行驶，而在距离终点较近的位置，按照自定义的减速度开始减速行驶。
1
2
3
def distance(self, p1, p2):
       x, y, z = p1.x - p2.x, p1.y - p2.y, p1.z - p2.z
       return math.sqrt(x*x + y*y + z*z)

说明：
●该函数用于计算两个坐标点之间的距离。
1
2
def quaternion_from_yaw(self, yaw):
       return tf.transformations.quaternion_from_euler(0., 0., yaw)

说明：
●该函数用于将欧拉角转换成四元数形式。
1
2
def kmph2mps(self, velocity_kmph):
       return (velocity_kmph * 1000.) / (60. * 60.)

说明：
●该函数用于将单位为km/h的速度值转换成以m/s为单位。
1
2
3
4
5
6
7
def publish(self, waypoints, base_path):
       lane = Lane()
       lane.header.frame_id = '/world'
       lane.header.stamp = rospy.Time(0)
       lane.waypoints = waypoints
       self.pub.publish(lane)
       self.pub_path.publish(base_path)

说明：
●该函数用于发布话题消息waypoints和base_path。
1
2
3
4
5
6
7
8
9
def __init__(self):
       rospy.init_node('waypoint_loader', log_level=rospy.DEBUG)

       self.pub = rospy.Publisher('/base_waypoints', Lane, queue_size=1, latch=True)
       self.pub_path = rospy.Publisher('/base_path', Path, queue_size=1, latch=True)

       self.velocity = self.kmph2mps(rospy.get_param('~velocity'))
       self.new_waypoint_loader(rospy.get_param('~path'))
       rospy.spin()

说明：
●rospy.init_node(‘waypoint_loader’, log_level=rospy.DEBUG)用于初始化ROS节点，在节点完全初始化之前，消息不会出现在/rosout话题上，因此可能看不到初始消息。要想在/rosout上看到logdebug消息，可以将log_level参数传递给rospy.init_node()。
●self.pub = rospy.Publisher(‘/base_waypoints’, Lane, queue_size=1, latch=True)和self.pub_path = rospy.Publisher(‘/base_path’, Path, queue_size=1, latch=True)分别用于创建以/base_waypoints和/base_path为话题消息的发布者。其中latch=True将保存最后发布的消息并将其发送给连接的任何将来的订户，这对于诸如地图等缓慢变化的数据或静态数据很有用。
●self.velocity = self.kmph2mps(rospy.get_param(‘~velocity’))用于加载参数velocity的值，并转换成以m/s为单位。
●self.new_waypoint_loader(rospy.get_param(‘~path’))用于加载参数path所对应的文件路径，并发布相关的话题消息。

2.局部路径和历史路径的更新

waypoint_updater.py的作用是通过KD-tree查询距离小车最近的路径点，以该点为起始点设置局部路径，并在Rviz可视化工具中显示局部路径。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
def __init__(self):
       rospy.init_node('waypoint_updater')

       rospy.Subscriber('/smart/rear_pose', PoseStamped, self.pose_cb)
       rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)

       self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1)
       self.final_path_pub = rospy.Publisher('final_path', Path, queue_size=1)
	self.final_waypoints_pub = rospy.Publisher('real_waypoints', Lane, queue_size=1)
       self.final_path_pub = rospy.Publisher('real_path', Path, queue_size=1)

       # TODO: Add other member variables you need below
	self.real_waypoints = []
       self.base_waypoints = None
       self.waypoints_2d = None
       self.waypoint_tree = None
       self.pose = None

       # rospy.spin()
       rate = rospy.Rate(20)
       while not rospy.is_shutdown():
           if self.pose and self.base_waypoints and self.waypoint_tree:
               # Get closest waypoint
               closest_waypoint_idx = self.get_closest_waypoint_idx()
               self.publish_waypoints(closest_waypoint_idx)
           rate.sleep()

说明：
●rospy.init_node(‘waypoint_updater’)用于初始化ROS节点。
●rospy.Subscriber(‘/smart/rear_pose’, PoseStamped, self.pose_cb)和rospy.Subscriber(‘/base_waypoints’, Lane, self.waypoints_cb)分别用于创建以/smart/rear_pose和/base_waypoints为话题消息的订阅者，其中/smart/rear_pose话题消息内容为小车后轮中心点的坐标，/base_waypoints 话题消息内容为全局路径点坐标及小车期望速度等，pose_cb和waypoints_cb分别为/smart/rear_pose和/base_waypoints的消息回调函数。
●self.final_waypoints_pub = rospy.Publisher(‘final_waypoints’, Lane, queue_size=1)和self.final_path_pub = rospy.Publisher(‘final_path’, Path, queue_size=1)分别创建以final_waypoints和final_path为话题消息的发布者，用于发布局部路径信息。
●self.final_waypoints_pub = rospy.Publisher(‘real_waypoints’, Lane, queue_size=1)和self.final_path_pub = rospy.Publisher(‘real_path’, Path, queue_size=1)分别创建以real_waypoints和real_path为话题消息的发布者，用于发布历史路径信息。
●rate = rospy.Rate(20)用于设置ROS节点的更新频率为20Hz。
●while not rospy.is_shutdown():表示在ROS节点未关闭的条件下，执行下面的操作。
●if self.pose and self.base_waypoints and self.waypoint_tree:表示在pose、base_waypoints和waypoint_tree话题消息内容非空的条件下，执行下面的操作。
●closest_waypoint_idx = self.get_closest_waypoint_idx()的作用是返回一个距离当前小车位置最近的路径点的横坐标x。
●self.publish_waypoints(closest_waypoint_idx)用于发布话题消息closest_waypoint_idx。
1
2
def pose_cb(self, msg):
       self.pose = msg

说明：
●该函数为/smart/rear_pose话题消息的回调函数，作用是将该话题消息内容赋给self.pose。
1
2
3
4
5
def waypoints_cb(self, waypoints):
       self.base_waypoints = waypoints
       if not self.waypoints_2d:
           self.waypoints_2d = [[waypoint.pose.pose.position.x, waypoint.pose.pose.position.y] for waypoint in waypoints.waypoints]
           self.waypoint_tree = KDTree(self.waypoints_2d)

说明：
●该函数为/base_waypoints话题消息的回调函数。
●self.waypoints_2d = [[waypoint.pose.pose.position.x, waypoint.pose.pose.position.y] for waypoint in waypoints.waypoints]将waypoints话题消息中的x、y轴坐标提取到一个二维列表self.waypoints_2d中。二维列表self.waypoints_2d的内容如下：

●self.waypoint_tree = KDTree(self.waypoints_2d)的作用是构建一个二维的KD-tree。
1
2
3
4
5
6
7
8
9
10
11
12
def get_closest_waypoint_idx(self):
       x = self.pose.pose.position.x
       y = self.pose.pose.position.y

	q = Waypoint()
	q.pose.pose.position.x = x
	q.pose.pose.position.y = y
	self.real_waypoints.append(q)

       closest_idx = self.waypoint_tree.query([x,y],1)[1]

       return closest_idx

说明：
●q = Waypoint()的作用是创建一个Waypoint类型的对象q，包含坐标点信息。
●self.real_waypoints.append(q)的作用是将最新的实时位置信息添加到列表中。
●closest_idx = self.waypoint_tree.query([x,y],1)[1]的作用是在KD-tree中查询距离当前小车位置最近的一个路径点的横坐标x。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
def publish_waypoints(self, closest_idx):
   	# fill in final waypoints to publish
       lane = Lane()
       lane.header = self.base_waypoints.header
       lane.waypoints = self.base_waypoints.waypoints[closest_idx:closest_idx + LOOKAHEAD_WPS]

	real_lane = Lane()
       real_lane.header = self.base_waypoints.header
       real_lane.waypoints = self.real_waypoints

       # fill in path for visulization in Rviz
       path = Path()
       path.header.frame_id = '/world'
       for p in lane.waypoints:
        path_element = PoseStamped()
        path_element.pose.position.x = p.pose.pose.position.x
        path_element.pose.position.y = p.pose.pose.position.y
        path.poses.append(path_element)

	real_path = Path()
       real_path.header.frame_id = '/world'
       for m in real_lane.waypoints:
        real_path_element = PoseStamped()
        real_path_element.pose.position.x = m.pose.pose.position.x
        real_path_element.pose.position.y = m.pose.pose.position.y
        real_path.poses.append(real_path_element)

       self.final_waypoints_pub.publish(lane)
       self.final_path_pub.publish(path)
	self.final_waypoints_pub.publish(real_lane)
       self.final_path_pub.publish(real_path)

说明：
●lane = Lane()的作用是创建一个Lane类型的对象lane。
●lane.waypoints = self.base_waypoints.waypoints[closest_idx:closest_idx + LOOKAHEAD_WPS]的作用是将距离小车最近的路径点以及之后的LOOKAHEAD_WPS个路径点信息填充到final_waypoints话题消息中，其中LOOKAHEAD_WPS是自定义的局部路径点个数。
●path.header.frame_id = ‘/world’设置/world为局部路径可视化的参考系坐标。
●for p in lane.waypoints:的作用是遍历上述的局部路径点。
●path_element.pose.position.x = p.pose.pose.position.x和path_element.pose.position.y = p.pose.pose.position.y是将局部路径点的坐标信息填充到final_path话题信息中，用于在Rviz可视化工具中显示局部路径。
●self.final_waypoints_pub.publish(lane)和self.final_path_pub.publish(path)的作用是发布话题消息final_waypoints和final_path。
●历史路径消息real_path和real_lane的创建和定义同理。

3.路径可视化

打开一个新的终端，输入如下指令：

1
$ rosrun rviz rviz

在左菜单栏中加入两个Path的可视化插件，分别将Topic设置为/base_path和/final_path，即全局路径和局部路径。显示效果如下图。


在Gazebo中移动小车模型，由于小车当前位置发生变化，局部路径也因此发生变化，如下图中Rviz所显示。

发表于 | 分类于

Github链接：https://github.com/chanchanchan97/ROS

1.仿真描述

本项目的目的是在Gazebo仿真环境下显示一个基于阿克曼转向（Ackermann steering）的自动驾驶小车模型，在之前xacro模型的基础上添加Gazebo标签等，使小车模型具有重力、惯性和摩擦力等物理状态。

2.Gazebo简介

Gazebo是一款3D动态模拟器，能够在复杂的室内和室外环境中准确有效地模拟机器人群。与游戏引擎提供高保真度的视觉模拟类似，Gazebo提供高保真度的物理模拟，其提供一整套传感器模型，以及对用户和程序非常友好的交互方式。

2.1Gazebo的典型用途

（1）测试机器人算法；
（2）设计机器人；
（3）用现实场景进行回归测试。

2.2Gazebo的主要特点

（1）包含多个物理引擎；
（2）包含丰富的机器人模型和环境库；
（3）包含各种各样的传感器；
（4）程序设计方便和具有简单的图形界面。

3.ros_control

如上图所示，ros_control为Gazebo仿真提供了一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等。通过向URDF模型文件中添加Gazebo插件，从而将Gazebo仿真模型与ros_control建立起联系。controller可以实现对URDF模型中每个joint的控制，并且提供了PID控制器，Controller Manager则提供了一种通用接口，负责管理不同的controller。

4.配置物理仿真模型

4.1为link添加惯性参数和碰撞属性

1
2
3
4
5
6
7
8
9
10
11
12
<collision>
	<origin rpy="0 0 0" xyz="0 0 0"/>
	<geometry>
		<cylinder length=".02" radius="0.025"/>
	</geometry>
</collision>

<inertial>
	<origin xyz="0 0 0.055"/>
	<mass value="1.0" />
	<inertia ixx="${1*(0.16*0.16+0.02*0.02)/12}" ixy="0.0" ixz="0.0" iyy="${1*(0.25*0.25+0.02*0.02)/12}" iyz="0.0" izz="${1*(0.16*0.16+0.25*0.25)/12}"/>
</inertial>

如果模型仅需要在Rviz中显示，则上述标签和标签可以不添加。在为模型添加了collision碰撞属性后，该link的collision属性与link的visual属性在形状大小和位置角度上都应当保证重合，可在Gazebo菜单栏中添加collision属性的显示，即如下图所示。

若如下图所示，某个link的collision属性与link的visual属性没有重合，则小车无法正常运动。

同样的在为模型添加了inertial惯性参数后，每个link都会显示一个带有绿色轴的紫色框，每个框的中心应当与其link的指定重心对齐，可在Gazebo菜单栏中添加inertial属性的显示，即如下图所示。

关于惯性矩阵的公式推导和计算可自行上网搜寻，以下为资料链接的分享：
https://wenku.baidu.com/view/8444fe93aa00b52acfc7cae5.html

4.2为link添加Gazebo标签

4.2.1颜色

1
2
3
<gazebo reference="base_link">  
	<material>Gazebo/Blue</material>  
</gazebo>

该标签的作用是定义某个link在Gazebo中显示的颜色。虽然在标签中已经对link的颜色进行了定义，但只是定义了模型在Rviz中显示的颜色，而Gazebo所能够提供的颜色标签可参考：
http://wiki.ros.org/simulator_gazebo/Tutorials/ListOfMaterials

4.2.2重力

1
2
3
<gazebo reference="base_footprint">
       <turnGravityOff>false</turnGravityOff>
   </gazebo>

base_footprint这个link是作为小车本体在地面上的映射，本身并没有任何物理意义，因此通过标签将该link的重力属性关闭。

4.3为joint添加传动装置

Gazebo中的仿真都是基于物理引擎的，因此要想让一个模型运动，从本质上讲，必须要有力施加在模型上，就实际而言，我们必须在模型上加上执行器（通常情况下就是电机了），让模型运动起来。

标签主要的针对对象是joint（因为一般两个link连接处的地方如果是非固定的，那么一定会存在一个执行装置来改变两个link的相对位置），标签的作用就是给某个joint与某类执行器相结合，有了执行器的作用，Gazebo就可以在物理层面上对模型进行驱动了。
这里以前轮转向机构为例进行说明，具体代码如下。

1
2
3
4
5
6
7
8
9
10
11
<!-- Transmission is important to link the joints and the controller -->
<transmission name="right_bridge_joint_trans">
	<type>transmission_interface/SimpleTransmission</type>
	<joint name="right_bridge_to_bridge" >
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
	</joint>
	<actuator name="right_bridge_joint_motor">		<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
	<mechanicalReduction>1</mechanicalReduction>
	<motorTorqueConstant>100</motorTorqueConstant>
	</actuator>
</transmission>

（1）标签唯一指定了一个传动的标签，name可以自己定义，也可以与joint名相同。
（2）标签指定了传动的类型，选择transmission_interface/SimpleTransmission即可。
（3）标签（可定义一个或多个）指定这个传动所依赖的关节，拥有如下标签。
◆（在joint下，可定义一个或多个）指定支持的硬件接口空间。用于结合控制器使用硬件接口来向硬件接口发送和接受指令，请注意：
●当在RobotHW中加载此transmission时，此标签的值应为hardware_interface / XXX。
●在Gazebo中加载此transmission时，此标记的值应为XXX。
（4）标签（定义一个或多个）指定了与joint传动连接的执行器，名字可以随意定义，拥有如下标签、及。
●（可选）定义了电机／关节减速比。
●（可选，只有Indigo及以前版本的在这里指定，目前版本已经移到joint标签下）定义了支持的硬件接口。
●（可选）定义了电机的转矩参数。

4.4为部分link添加阻尼系数和摩擦系数（可选）

这里以车轮为例，具体代码如下。

1
2
3
4
5
6
7
8
<gazebo reference="right_back_wheel">
	<mu1>100000000</mu1>
	<mu2>100000000</mu2>
	<kp>100000000</kp>
	<kd>1</kd>
	<minDepth>0.01</minDepth>
	<maxVel>100</maxVel>  
</gazebo>

（1）和表示沿接触表面的两个不同接触方向的摩擦系数μ，其中表示库仑摩擦系数，它的范围必须在0到无限大之间，0表示无摩擦接触，而无限大表示永不打滑的接触。请注意，无摩擦接触比具有摩擦的接触要花费更少的时间，并且无限摩擦的接触要比具有有限摩擦的接触损耗更大。这个值必须始终被设置。而如果未设置，则在两个摩擦方向上都使用。 如果设置，则将用作摩擦方向1，将用作摩擦方向2。
（2）和表示由Open Dynamics Engine (ODE)定义的刚体接触的接触刚度k_p和阻尼k_d（ODE使用erp和cfm，但是erp/cfm与刚度k_p/阻尼k_d之间存在映射关系）。
（3）表示施加接触校正脉冲之前的最小允许深度。
（4）表示最大接触修正速度截断项。
●附Gazebo官网相关链接：
http://gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros

4.5添加ros_control插件

1
2
3
4
5
6
7
8
<gazebo>
	<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
		<robotNamespace></robotNamespace>
		<robotParam>robot_description</robotParam>
		<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
		<legacyModeNS>true</legacyModeNS>
	</plugin>
</gazebo>

gazebo_ros_control是Gazebo的一个插件用来根据设定装载合适的硬件接口和控制器。这个实现非常简单，由于Gazebo的插件系统具有很强的扩展性， 使得一些高级玩家可以在ros_control和Gazebo之间创建自己的机器人硬件接口。
这里我们没有为标签添加属性reference，这样它就是对整个机器人的描述。 gazebo_ros_control的标签还可以通过如下的子标签指定一些参数：
（1）用来定义插件其对象的ROS命名空间，默认是URDF或者SDF对应的机器人名称。
（2）用来定义控制器的更新周期，单位为秒，默认使用Gazebo的周期。
（3）用来定义ROS参数服务器上的机器人描述(URDF)路径，默认是”/robot_description”。
（4）用来定义机器人仿真接口所使用的插件库名称，默认是”DefaultRobotHWSim”。
（5）用来兼容之前ROS版本的配置。

5.配置ros_control控制器

在config文件夹目录下创建smartcar_joint.yaml，并输入如下代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
joint_state_controller:
  type: "joint_state_controller/JointStateController"
  publish_rate: 50  

rear_right_velocity_controller:
  type: "velocity_controllers/JointVelocityController"
  joint: right_back_wheel_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}
rear_left_velocity_controller:
  type: "velocity_controllers/JointVelocityController"
  joint: left_back_wheel_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}
right_bridge_position_controller:
  type: "effort_controllers/JointPositionController"
  joint: right_bridge_to_bridge
  pid: {p: 40.0, i: 0.0, d: 1.0}
left_bridge_position_controller:
  type: "effort_controllers/JointPositionController"
  joint: left_bridge_to_bridge
  pid: {p: 40.0, i: 0.0, d: 1.0}

用.yaml文件创建一个配置文件用来配置控制器类型和保存各个关节的PID参数，这些参数将通过launch文件加载到参数服务器上。

6.在Gazebo中添加传感器插件

6.1摄像头

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
<!-- camera -->
 	<gazebo reference="camera_link">
	<sensor type="camera" name="camera1">
		<update_rate>30.0</update_rate>
		<camera name="head">
			<horizontal_fov>1.3962634</horizontal_fov>
			<image>
				<width>800</width>
				<height>800</height>
				<format>R8G8B8</format>
			</image>
			<clip>
				<near>0.02</near>
				<far>300</far>
			</clip>
			<noise>
				<type>gaussian</type>
				<!-- Noise is sampled independently per pixel on each frame.
					That pixel's noise value is added to each of its color
					channels, which at that point lie in the range [0,1]. -->
				<mean>0.0</mean>
				<stddev>0.007</stddev>
			</noise>
		</camera>
		<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
			<alwaysOn>true</alwaysOn>
			<updateRate>0.0</updateRate>
			<cameraName>camera</cameraName>
			<imageTopicName>image_raw</imageTopicName>
			<cameraInfoTopicName>camera_info</cameraInfoTopicName>
			<frameName>camera_link</frameName>
			<hackBaseline>0.07</hackBaseline>
			<distortionK1>0.0</distortionK1>
			<distortionK2>0.0</distortionK2>
			<distortionK3>0.0</distortionK3>
			<distortionT1>0.0</distortionT1>
			<distortionT2>0.0</distortionT2>
		</plugin>
	</sensor>
</gazebo>

说明：

1
<gazebo reference="camera_link">

插件是用来描述link、joint的，其本身是一种虚无的属性描述，因此需要关联相应的实体。我们通过来定义关联的link或者joint,这里关联的是camera_link。
1
<sensor type="camera" name="camera1">

声明插件的类型，并为该插件取一个唯一的名称。
1
<update_rate>30.0</update_rate>

设置摄像头数据更新的最大频率。
1
<horizontal_fov>1.3962634</horizontal_fov>

horizontal_fov是指horizontal field of view（水平方向上的视场），此处设置视场大小。
1
2
3
4
5
<image>
	<width>800</width>
	<height>800</height>
	<format>R8G8B8</format>
</image>

设置图像的分辨率和色彩格式。
1
2
3
4
<clip>
	<near>0.02</near>
	<far>300</far>
</clip>

设置摄像头可视的最短距离和最远距离。
1
2
3
4
5
6
7
8
<noise>
	<type>gaussian</type>
	<!-- Noise is sampled independently per pixel on each frame.
	That pixel's noise value is added to each of its color
	channels, which at that point lie in the range [0,1]. -->
	<mean>0.0</mean>
	<stddev>0.007</stddev>
</noise>

此处为图像添加噪声，噪声类型为高斯噪声，设置高斯噪声的均值和标准差。噪声是在每个帧的每个像素上独立采样的，该像素的噪声值会添加到其每个颜色通道中，范围在[0,1]内。
1
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">

关联摄像头插件，该插件已经在Gazebo中实现，所以直接关联即可。
1
<cameraName>camera</cameraName>

设置摄像头消息的命名空间。
1
<imageTopicName>image_raw</imageTopicName>

设置发布的摄像头图像话题名。
1
<cameraInfoTopicName>camera_info</cameraInfoTopicName>

设置发布的摄像头信息话题名。
1
<frameName>camera_link</frameName>

设置摄像头数据所在的参考系。
1
<hackBaseline>0.07</hackBaseline>

此处在ros.wiki上的解释是Hack for right stereo camera，默认为0。
1
2
3
4
5
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>

设置摄像头图像的畸变参数，默认为0。

6.2激光雷达

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
<gazebo reference="laser_link">
	<sensor type="ray" name="head_hokuyo_sensor">
		<pose>0 0 0 0 0 0</pose>
		<visualize>false</visualize>
		<update_rate>40</update_rate>
		<ray>
			<scan>
				<horizontal>
					<samples>720</samples>
					<resolution>1</resolution>
					<min_angle>-1.570796</min_angle>
					<max_angle>1.570796</max_angle>
				</horizontal>
			</scan>
			<range>
				<min>0.10</min>
				<max>30.0</max>
				<resolution>0.01</resolution>
			</range>
			<noise>
				<type>gaussian</type>
				<!-- Noise parameters based on published spec for Hokuyo laser
					achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
					stddev of 0.01m will put 99.7% of samples within 0.03m of the true
					reading. -->
				<mean>0.0</mean>
				<stddev>0.01</stddev>
			</noise>
		</ray>
		<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
			<topicName>laser/scan</topicName>
			<frameName>laser_link</frameName>
		</plugin>
	</sensor>
</gazebo>

激光雷达的插件与摄像头相似，不再做详细说明。

6.3IMU

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
<!-- IMU sensor -->
   <gazebo reference="imu_link">
       <gravity>true</gravity>
       <sensor name="imu_sensor" type="imu">
           <always_on>true</always_on>
           <update_rate>100</update_rate>
           <visualize>true</visualize>
           <topic>__default_topic__</topic>
           <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
               <topicName>imu</topicName>
               <bodyName>imu_link</bodyName>
               <updateRateHZ>100.0</updateRateHZ>
               <gaussianNoise>0.0</gaussianNoise>
               <xyzOffset>0 0 0</xyzOffset>
               <rpyOffset>0 0 0</rpyOffset>
               <frameName>imu_link</frameName>
           </plugin>
           <pose>0 0 0 0 0 0</pose>
       </sensor>
   </gazebo>

IMU的插件与摄像头相似，不再做详细说明。

6.4检查传感器插件

打开一个新的终端，输入如下指令。

1
$ rostopic list

终端界面将会打印出目前所有的topic话题。

其中/camera、/imu和/laser分别对应摄像头、IMU和激光雷达的消息话题。

6.5传感器消息可视化

6.5.1摄像头图像的显示

输入如下指令，打开Rviz。

1
$ rosrun rviz rviz

在Rviz的左菜单栏中添加Camera的可视化插件，并在Topic中选择摄像头图像的消息话题/camera/image_raw。

注意：在虚拟机中运行摄像头仿真图像，会出现如上图所示的图像显示问题，建议使用双系统。

6.5.2激光雷达点云的显示

在Gazebo仿真环境中添加一些已有的模型，将模型放在小车周围合适的范围内。

打开一个新的终端，输入如下指令，打开Rviz。

1
$ rosrun rviz rviz

在Rviz工具的左菜单栏中添加如下所示LaserScan的可视化插件。

同时在LaserScan中的Topic选择激光雷达的消息话题/laser/scan，即如下图所示，Rviz能够显示出小车周围障碍物的点云信息。

6.5.3IMU姿态的显示

在Gazebo菜单栏的Window中选择Topic Visualization，跳出如下界面。

选择gazebo.msgs.IMU，跳出如下界面。如下图所示，IMU消息可分成三个部分，分别为以四元数表示的姿态信息、角速度和线性加速度在xyz三轴上的分量。

6.6搭建Gazebo仿真环境

在Gazebo上方的菜单栏中选择Edit-Building Editor，出现如下界面。

在左边的工具栏中选择Walls，将小车仿真环境的轮廓画出来，然后再选择Features添加门、窗和楼梯等。双击门窗会打开一个Inspector窗口，可以修改位置和尺寸。另外在工具栏中还可以选择修饰墙的颜色和纹理等细节。

完成仿真环境的绘制后，在Gazebo上方的菜单栏中选择File-Save as保存文件。

发表于 | 分类于

1.ros_control

ros_control就是ROS为用户提供的应用与机器人之间的中间件，包含一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等等，可以帮助机器人应用快速落地，提高开发效率。

上图是ros_control的总体框架，可以看到对于不同类型的控制器（底盘、机械臂等），ros_control可以提供多种类型的控制器，但是这些控制器的接口各不相同，为了提高代码的复用率，ros_control还提供一个硬件的抽象层。

硬件抽象层负责机器人硬件资源的管理，而controller从抽象层请求资源即可，并不直接接触硬件。

上图是ros_control的数据流图，可以更加清晰的看到每个层次包含的功能：
（1）Controller Manager：每个机器人可能有多个controller，所以这里有一个控制器管理器的概念，提供一种通用的接口来管理不同的controller。controller manager的输入就是ROS上层应用的输出。
（2）Controller：controller可以完成每个joint的控制，请求下层的硬件资源，并且提供了PID控制器，读取硬件资源接口中的状态，在发布控制命令。
（3）Hardware Rescource：为上下两层提供硬件资源的接口。
（4）RobotHW：硬件抽象层和硬件直接打交道，通过write和read方法来完成硬件的操作，这一层也包含关节限位、力矩转换、状态转换等功能。
（5）Real Robot：实际的机器人上也需要有自己的嵌入式控制器，接收到命令后需要反映到执行器上，比如接收到位置1的命令后，那就需要让执行器快速、稳定的到达位置1。

Controller Manager

controller manager提供了一种控制多个controller的机制，可以加载、开始运行、停止运行、卸载不同的controller，并且提供了多种工具来完成这些操作。

Controller

ros_controllers这个功能包提供了已有的一些controllers，比较常用的是如下几个：
（1）effort_controllers - 直接控制关节的力/扭矩
●joint_effort_controller
●joint_position_controller
●joint_velocity_controller
（2）position_controllers - 一次设置一个／多个关节位置
●joint_position_controller
（3）velocity_controllers - 一次设置一个／多个关节速度
●joint_velocity_controller
（4）joint_state_controller - 读取所有关节位置
●joint_state_controller
更多controller可参考：http://wiki.ros.org/ros_controllers
当然，我们也可以根据自己的需求，创建需要的controller，然后通过controller来管理自己创建的controller，可以参考https://github.com/ros-controls/ros_control/wiki/controller_interface

Hardware Interface

Hardware Interface是controller和RobotHw沟通的接口，基本上和controllers的种类是对应的。ROS提供了如下的hardwareInterface接口供选择：
（1）Joint Command Interfaces
●Effort Joint Interface
●Velocity Joint Interface
●Position Joint Interface
（2）Joint State Interfaces
（3）Actuator State Interfaces
（4）Actuator Command Interfaces
●Effort Actuator Interface
●Velocity Actuator Interface
●Position Actuator Interface
（5）Force-torque sensor Interface
（6）IMU sensor Interface
hardware_interface作为ros_control 组件最重要的一部分，做了以下抽象：
●将能动的部分(类似于机器人关节)抽象为joint，joint类型有旋转，平动，固定等等，详见urdf，joint有三个属性，pose，velocity，effort，位置和速度好理解，effort不同的joint类型有不同的含义，一般是力矩，力（语义比较混乱在各个不同派系的开源代码中）。执行器模型抽象为actuator，与joint不一样的部分是actuator的属性值需要一定变换才能对应到joint，可以理解为电机减速，或者机构传动。
●根据不同的控制方式或者不同的传感器暴露出相应的数据接口，一般移动机器人底盘是速度闭环，而机械臂上又是位置闭环，根据这些控制方式的不同分出了不同的控制接口，暴露给上层的controller。
●作为机器人的硬件资源与上层的直接接口，可以被运行时产生与删除，结合机器人本身的通讯组件这样实际上实现了一种对机器人硬件资源的低层次管理。
而hardware_interface具体实现方式是存储对应状态变量的指针，用字符串表示不同的joint与Actuator资源。robothw基类是硬件通讯库与hardware_interface交互的部分。硬件通讯库具体的读写过程都在read与 write两个虚方法中实现，更新的数据放在robothw类的成员变量中，这些存储着joint与actuator状态与命令的空间被hardware_interface索引，传给controller_manager，通过controller_manager的接口将数据接给controller，所以ros_control内部不存在进程间通讯。
同样的，hardware_interface也可以自己创建需要的接口，可以参考：https://github.com/ros-controls/ros_control/wiki/hardware_interface

Transmissions

Transmissions就是机器人的传动系统，机器人每个需要运动的关节都需要配置相应的Transmission，可以通过代码完成https://github.com/ros-controls/ros_control/wiki/transmission_interface，但大部分情况下，都会在URDF文件中直接添加（http://ros.org/wiki/urdf/XML/Transmission）

1
2
3
4
5
6
7
8
9
10
<transmission name="simple_trans">
	<type>transmission_interface/SimpleTransmission</type>
	<joint name="foo_joint">
		<hardwareInterface>EffortJointInterface</hardwareInterface>
	</joint>
	<actuator name="foo_motor">
		<mechanicalReduction>50</mechanicalReduction>
		<hardwareInterface>EffortJointInterface</hardwareInterface>
	</actuator>
</transmission>

Joint Limits

Joint Limits是硬件抽象层中的一块，维护一个关节限位的数据结构，这些限位数据可以从机器人的URDF文件中加载，也可以ROS的参数服务器上加载（先用YAML配置文件导入ROS parameter server），这些限位数据不仅包含关节速度、位置、加速度、加加速度、力矩等方面的限位，还包含安全作用的位置软限位、速度边界（k_v）和位置边界（k_p）等等。
我们来看一个URDF中设置Joint Limits的例子：

1
2
3
4
5
6
7
<joint name="$foo_joint" type="revolute">
<!-- other joint description elements -->
<!-- Joint limits -->
<limit lower="0.0" upper="1.0" effort="10.0" velocity="5.0" />
<!-- Soft limits -->
<safety_controller k_position="100" k_velocity="10" soft_lower_limit="0.1" soft_upper_limit="0.9" /> 
</joint>

还有一些参数需要通过YAML配置文件先加载到参数服务器中，YAML文件的格式如下：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
	joint_limits:
foo_joint:
	has_position_limits: true
	min_position: 0.0
	max_position: 1.0
	has_velocity_limits: true
	max_velocity: 2.0
	has_acceleration_limits: true
	max_acceleration: 5.0
	has_jerk_limits: true
	max_jerk: 100.0
	has_effort_limits: true
	max_effort: 5.0
	bar_joint:
	has_position_limits: false # Continuous joint
	has_velocity_limits: true
	max_velocity: 4.0

另外，我们还可以在代码中使用joint_limits_interface来加载和设置关节的限位参数，可以参考：https://github.com/ros-controls/ros_control/wiki/joint_limits_interface

2.arbotix_ros

arbotix控制器是一款比较受欢迎的舵机驱动器，用于驱动BIOLOID和DYNAMIXEL舵机。

arbotix_ros是arbotix控制器的ROS软件包。利用arbotix_ros可以很方便通过ROS来控制小车运动。

2.1arbotix在ROS中的使用

在launch文件中添加arbotix插件，加载arbotix节点以及控制器配置文件。

1
2
3
4
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">  
       <rosparam file="$(find smartcar_description)/config/smartcar_arbotix.yaml" command="load" />  
       <param name="sim" value="true"/>  
   </node>

关于arbotix_python可参考：http://wiki.ros.org/arbotix_python
在config文件夹目录下创建一个.yaml文件，用于配置小车的控制器base_controller。
1
2
3
4
5
6
7
8
9
10
11
12
13
controllers: {
  base_controller: {
      type: diff_controller, 
      base_frame_id: base_footprint, 
      base_width: 0.16, 
      ticks_meter: 4100, 
      Kp: 12, 
      Kd: 12, 
      Ki: 0, 
      Ko: 50, 
      accel_limit: 1.0 
}
 }

说明：
●type表示控制器类型，diff_controller是两轮差速的控制器。
●base_frame_id表示参考的坐标系。
●base_width在这里表示左右两个轮子的间距，单位米。
●ticks_meter表示编码器精度，单位是tick/米。
●Kp、Kd和Ki分别表示PID的三个参数，即比例、微分和积分。
●Ko表示PID的缩放比例参数。
●accel_limit表示加速度的限位。
详情可参考：http://wiki.ros.org/arbotix_python/diff_controller