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

Task

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

算法

位置和向量

向量的模：

$$||\boldsymbol{v}||=\sqrt{x^{2}+y^{2}}\tag{vector-norm}$$
点乘，加法，减法：
$$\boldsymbol{v}\cdot a = (x_{v}a;y_{v}a)\tag{mult-vector}$$
$$\boldsymbol{v}+\boldsymbol{u} = (x_{v}+x_{u};y_{v}+y_{u})\tag{add-vector}$$
$$\boldsymbol{v}-\boldsymbol{u} = (x_{v}-x_{u};y_{v}-y_{u})\tag{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=||\vec{l}-\vec{a}+\vec{s}\cdot2^{-i}+\vec{v}||\tag{approx-condition}$$

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

如果d小于targetDistance，$\vec{s}$，$\vec{v}$相加

$$\vec{v}_{i+1}=\vec{v}_{i}+\vec{s}_{i}\cdot2^{-i}\tag{approx-passed}$$

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

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

角速度

$\varphi=\varphi_{\boldsymbol{t}-\boldsymbol{a}}-\alpha,\tag{target-angle}$

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

避障

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

$$\varphi=-0.3\cdot sign(\beta)\tag{avoid-front}$$
蓝色区域：设置机器人速度为原先的60%，误差角度设置为：
$$\varphi=-0.2\cdot sign(\beta)\tag{avoid-side}$$
红色区域：设置机器人速度为原先的60%，误差角度设置为:
$$\varphi=-0.3\cdot sign(\beta)\tag{avoid-front}$$

code

https://github.com/will1991/rosintrodution

CMakeLists.txt