Introduction:
本次设计的任务是让机器人沿着墙按照固定距离走
算法
第一步是找到墙,利用激光雷达发现最近障碍物的角度和距离。
根据墙的位置保持固定的距离和固定的角度,可以用两个PID控制器来实现这两个参量的控制。
输入输出处理
程序的输入是激光雷达的数据,输出的是控制Twist消息
每一个激光点对应的实际角度:
φ=(i−l2)⋅θ(angle-ranges)
φ :第 i 个元素对应的角度
i :向量元素的索引
l :向量长度
θ : 角度增量
如果在整个ranges向量空间中寻找最小值,有可能让机器人在狭窄的空间中跟随对面的墙(译者:不是很懂)。为了确保不会发生这种状况,在合理的一般范围内查找最小值。查找的索引起始和终止值可以通过下面的式子调整:
i0=l⋅d+14
in=l⋅d+34
d :方向(1:跟随左边的墙,-1:跟随机器人右边的墙)