路径跟踪实验

角速度控制器：
w(k ) = w(k − 1) + kp 2 ( y e (k ) − y e (k − 1)) + ki2 y e (k ) + kp3 (θ e (k ) − θ e (k − 1)) + ki3θ e (k )
其中，kpi是比例增益，kii是积分增益(i=1,2,3)。
仿真结果
机器人在局部坐标系下的位姿误差如图所示，表示为
xe cos θ e p = ye = − sin θ θ e 0 sin θ cos θ 0 0 ex 0 e y = Te (qr − q) 1 eθ
其中，q=[x, y,θ]T为机器人位姿，v和w分别是机器人线速度和角速 度，T为采样时间。
控制规律
假设参考路径可由一系列的参考点组成，即qr=[xr, yr,θr]T, 定义位 姿误差，即 q = qr − q
= [ xr − x, yr − y,θ r − θ ]T = [ex , e y , eθ ]T
Y
w
v
2r
来自百度文库
2l
O
C
ɺ x cos θ y = sin θ ɺ ɺ θ 0
X
0 v 0 w 1
θ
离散化可表示为：
x ( k + 1) = x ( k ) + T ⋅ v ( k ) ⋅ cos( θ ( k )) y ( k + 1) = y ( k ) + T ⋅ v ( k ) ⋅ sin( θ ( k )) x ( k + 1) = x ( k ) + T ⋅ w ( k )
其中，Te为转换矩阵。
Y'
xe
θr
Y
C
(xr， r ) y
ye
(x，y)
O
θ
X'
X
那么机器人路径跟踪的目的就是设计合适的控制规律控制机器人线 速度和角速度，使得机器人能精确跟随参考路径，即 lim e = 0
t →∞ p
PI控制器设计
线速度控制器：
v(k ) = v(k − 1) + kp1 ( xe (k ) − xe (k − 1)) + ki1 x e (k )
移动机器人路径跟踪
Buct
路径跟踪定义
路径跟踪(Path tracking)，是在平面坐标系下设定一条理想的几何路径， 然后要求机器人从某一处出发，按照某种控制规律到达该路径上，并 实现其跟踪运动。
实际 位姿
d
路径
运动学模型
运动学模型直接刻画出了系统位置与速度之间的数学关系，以两 轮驱动移动机器人为例，如图所示，运用物理学质点运动分析法 可得到
直线跟踪
椭圆跟踪
正弦曲线跟踪
实验要求
1、理解移动机器人路径跟踪定义； 2、掌握PI路径跟踪控制器设计方法； 3、完成PI路径跟踪仿真； 4、上交实验报告。