6. ROS编程入门--路径跟随（Trajectory following）

Task

本次实验才采用“Follow the carrot 算法去跟随定义好的路径。控制的目标点在机器人行走的路径上，距离机器人是个常数距离。机器人计算自己的方向角和目标点角度之间的相差度数。控制这个差角为0.
为了在探测时候能够用这个算法，路径需要在实时更新并且需要配合避障，因为在探测时候路径计算的并不精确，机器人有碰撞到障碍物的可能性。

算法

位置和向量

向量的模：

||v||=x2+y2−−−−−−√(vector-norm)

点乘，加法，减法：

v⋅a=(xva;yva)(mult-vector)

v+u=(xv+xu;yv+yu)(add-vector)

v−u=(xv−xu;yv−yu)(add-vector)

处理输入输出

控制机器人采用Twis消息类型
从激光输入进来的数据可以用来避障
输入Path的消息格式：

std_msgs/Header header
geometry_msgs/PoseStamped[] poses
std_msgs/Header header
geometry_msgs/Pose pose
geometry_msgs/Point position
geometry_msgs/Quaternion orientation

本次实验中只有x、y坐标下的位置，路径存储在一个队列对象MyPoint中，你可以仅仅为每一个在poses向量中的元素建立一个新的MyPoint类的对象，然后添加到路径队列中。每次添加新的控制点需要删掉之前的点。在这种情况下，每一次循环路径都会改变，每次都是跟随最新的路径。将每一个路径变换带Odometry坐标下。

路径

移除掉前面的点存储在lastRemoved中。之后需要移除掉路径上被认为已经到达的点，这一步要防止机器人沿着路径后退。
把移除掉的点存储在lastRemoved中。如果没有点移除掉机器人首先去lastRemoved的移除点，否则机器人在lastRemoved和 路径前端之间的线段上寻找目标点。

控制点（control point）

控制点是路径上线和弧的插值点。为了避免计算和选择插值点，估计目标点方法如下：

d=||l⃗ −a⃗ +s⃗ ⋅2−i+v⃗ ||(approx-condition)

l⃗  :上一次从路径上移除掉的点
s⃗  = f⃗ −l⃗  f⃗  :是路径队列的首部值
v⃗  : 前一次迭代的贡献值？？
a⃗  ：机器人坐标
d ： 目标和机器人之间的距离
i : 迭代次数

如果d小于targetDistance， s⃗  ， v⃗  相加

v⃗ i+1=v⃗ i+s⃗ i⋅2−i(approx-passed)

大概迭代10次你会有一个精确的（ ∼2−10⋅||s⃗ || ）目标点的估计值:
t⃗ =l⃗ +v⃗ (target-point)
如果机器人距离路径很远，可能要移除的不仅仅是第一个距离大于targetDistance的点， v⃗  保持为0，机器人朝向第一个被移除的点的移动。

如果路径点之间相距比较远，机器人所处的位置如图所示，机器人会找到两个插值点，有可能会导致机器人后退。简单的解决方法是把目标点设置为：移除点和路径队列首项之间的线段 与 过机器人原点的法线 的交点
xv=||a⃗ −l⃗ ||⋅cos(φf⃗ −l⃗ −φa⃗ −l⃗ )||s⃗ ||⋅xs(intersection-x)
yv=||a⃗ −l⃗ ||⋅cos(φf⃗ −l⃗ −φa⃗ −l⃗ )||s⃗ ||⋅ys(intersection-y)
φf⃗ −l⃗  是 f⃗ ,l⃗  之间的角度， φa⃗ −l⃗  是 a⃗  , l⃗  之间的角度

角速度

φ=φt−a−α,(target-angle)

ω=k⋅φ

k是常值1/t（t 是返回0值的时间），k越大对路径变化的反应越快速，但是太大会导致不稳定，建议设置为2。

避障

绿色区域：设置机器人速度为原先的80%，误差角度设置为：

φ=−0.3⋅sign(β)(avoid-front)

蓝色区域：设置机器人速度为原先的60%，误差角度设置为：

φ=−0.2⋅sign(β)(avoid-side)

红色区域：设置机器人速度为原先的60%，误差角度设置为:

φ=−0.3⋅sign(β)(avoid-front)

code

https://github.com/will1991/rosintrodution

CMakeLists.txt