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

Task

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

算法

位置和向量

向量的模：

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

||\boldsymbol{v}||=\sqrt{x^{2}+y^{2}}\tag{vector-norm}
点乘，加法，减法：

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

\boldsymbol{v}\cdot a = (x_{v}a;y_{v}a)\tag{mult-vector}

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

\boldsymbol{v}+\boldsymbol{u} = (x_{v}+x_{u};y_{v}+y_{u})\tag{add-vector}

v−u=(xv−xu;yv−yu)(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=||l⃗ −a⃗ +s⃗ ⋅2−i+v⃗ ||(approx-condition)

d=||\vec{l}-\vec{a}+\vec{s}\cdot2^{-i}+\vec{v}||\tag{approx-condition}

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

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

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

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

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

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

角速度

φ=φt−a−α,(target-angle)\varphi=\varphi_{\boldsymbol{t}-\boldsymbol{a}}-\alpha,\tag{target-angle}

ω=k⋅φ

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

避障

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

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

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

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

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

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

\varphi=-0.3\cdot sign(\beta)\tag{avoid-front}

code

https://github.com/will1991/rosintrodution

CMakeLists.txt