本文探讨了针对阿克曼转向车型的导航末段位姿调整控制，采用TEA算法（Target-End-Adjust Algorithm）进行精细调整。在Nav2hunter项目背景下，车辆使用RPP控制器到达目标点附近后，通过坐标转换和S型路径策略，调整前进和倒车速度及角速度，以实现Y=0和Yaw=0的目标。控制策略包括根据当前位置和速度，通过预设加速度调整Vx和估计内转角以达成目标位姿。在实际应用中，算法仍存在速度调整慢和状态切换问题，需要进一步优化。
1目标
分析RPP算法时控制器算法学习1-RPP受控纯追踪算法发现，在终点时如果角度还有较大偏差，该算法无法进行很好的调整，故开始尝试在末端接近目标点时，用自己的控制算法去调整位姿，姑且命名为TEA算法（Target-End-Adjust Algorithm for Ackermann）
前提 本算法基于自己项目的整体控制策略参见Nav2hunter的控制软件结构设计。 简要说就是采用RPP控制器让小车到达目标正后方一定距离的点（目前设置为2米），把许可误差放得比较大，这样容易到达目标；到达后再用本算法精调位姿进入最终目标。故小车的位置和角度与目标的差值是在一定的范围内的，以此为前提考量设计下面的所有控制策略和控制方法。
2控制思路
2.1 坐标转换
step1.
将小车当前坐标转换到目标点为原点，目标方向为x轴的坐标系（CSgoal）下，如上图所示。
step2.
末端调整算法的目标就是：在终点前面一小段空间内（红线标记范围），把小车位置点调整到Y=0（或直线误差许可范围内：<=XY tolerance），yaw为0（或yaw误差许可范围内,<=yaw tolerance）
step3.
由调整区间（红线标记）可以大致得到倒车位置和前进位置（两个X值：Xrev和Xfwd）。如果step2的目标没有满足，而小车当前坐标的Xcur>Xrev就开始倒车去调整；而如果Xcur<Xfwd就开始前进去调整；在Xrev和Xfwd之间就按原来行进方向加速到正常行进Vxmax。
step4.
如果满足了step2的要求，则直行进入终点，速度随着距离接近而减慢，直到Xcur=0（或直线误差许可范围内：<=XY tolerance）。
3 算法重点（Step3的调整控制）
3.1问题paraphrase：
- 已知当前位置 X c u r X_{cur} Xcur、 Y c u r Y_{cur} Ycur、 Y a w c u r Yaw_{cur} Yawcur（坐标系CS g o a l _{goal} goal下）；当前前进速度 V x c u r Vx_{cur} Vxcur ，前进速度的加速度α固定，忽略角速度的加速度，即不考虑当前的车轮内转角inner-steering-angle，认为需要内转角是多少马上可以切换到（这个简化需要再议）。
- 控制目标两个：1是位置目标Y=0，2是方向目标Yaw=0。
- 控制量有两个：前进速度Vx和角速度ω。
3.2 调整策略
为避免来回走重复的路线，故借鉴开车时侧方位停车（或者往侧面挪动）的方法，走S型路线。
- 在前进的时候，如果还没到Reverse line，期望速度是Vxmax，期望Yaw指向中心线
- 前进到Reverse line时，开始减速，到后退，期望速度是-Vxmax。但在速度为负之前，期望的Yaw是0（拉直）。
- 开始后退（Vx为负数）后，期望速度还是-Vxmax，但在到达Foward line之前的期望Yaw是指向中心线。
- 到达Foward line后，开始减少后退速度（或者叫正向加速吧），逐步到速度为0，再到正向前进，期望的速度都是+Vxmax，但在速度为正之前，期望的Yaw为0。
整个过程就像往侧面挪车，前进先往侧面打方向，再拉直，后退也是一样需要往里面打方向一段距离后拉直车身，否则就会相同路线来回倒腾，不往侧面靠。网上只找到这个图示，也懒得做动图，就这么看吧
- 从上面的描述，可以估算出橙色线（开始减速）与红色线（极限位置）的距离
即从Vxmax按固定加速度减速到0的距离:
Δ x = s = 1 2 v x m a x t 2 = 1 2 v x m a x 2 α \Delta x=s=\frac{1}{2}v_{xmax}t^{2}=\frac{1}{2}\frac{v_{xmax}^{2}}{\alpha} Δx
最低0.47元/天 解锁文章
扫一扫
2883
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
