基于waypoint的路径规划与控制算法的实现（上）

关于本文

旨在解决差动转向的机器人在结构化道路中从A跑到B的问题。

背景

我看了下，室内移动机器人的slam自动导航算法有很多，但大多基于 A* 或改进 A* 算法实现的，显然不适用在结构化道路中。
那我想了下，自己接触过的Autoware的Waypoint_planner算法应该能实现，但Autoware面向阿克曼底盘，没法直接拿来用。

为了尽量避免Autoware被我的黑暗料理污染（基于keep it pure的原则），我决定自己用python写个类似的算法来解决上面的问题，就不在Autoware的代码上动手脚了。

概述

本算法主要由waypoint_saver跟waypoint_follower这两部分组成，一个录制路径，一个跟随路径。本文主要介绍路径点数据格式跟waypoint_saver算法及设计思路。

准备工作

本算法只提供简单的规划和控制部分，所以跑本算法有以下几点prerequisitions：

1. python2
2. ROS1
3. 已实现定位，能提供baselink到map的tf（可修改）

前两点是没什么异议的，至于第三点定位部分的实现可以有很多，只要能以tf的形式发布小车的定位信息即可，本算法定位是Autoware自带的ndt配准算法实现的。

路径录制

下面正式介绍waypoint_saver算法。

路径点格式

首先是路径点数据格式的设计，从实际控制的角度出发，差动型小车就纵向跟横向控制，也就是控制速度跟转角就够了。

我想了下，速度可以给个定值，这样就只需要转角控制即可。所以路径点只要提供以下信息：

1. 位置信息：录制小车当前在地图上的x跟y坐标
2. 姿态信息：录制小车当前在地图上的yaw角
3. 索引值：用来管理这些路径点

存放路径点的文件类型我采用跟Autoware一样的csv文件格式，一方面它看起来直观另一方面它用起来也方便，所以最终数据存放格式如下所示：

Idx	x	y	yaw
0	0	0	0
…	…	…	…

流程图

下面是路径录制算法的流程图。它的原理很简单，基于时间间隔对定位信息进行采样，只要隔一段时间能获取到定位信息就把信息写入到csv文件中，直到你把程序（Ctrl+c）kill掉就结束录制。

主程序

waypoint_saver我写的还是蛮通俗的，所以简单提下，程序入口在最底下if __name__ == "__main__":那里，主循环跑的是WayPointSaver()对象的_run()函数。结和上面的流程图来看应该很清晰，它一直在获取路径点、处理路径点、写入路径点到文件。

这里路径点处理部分本来留给自己优化的，想着可能对路径点需要做平滑滤波等处理，但后来发现控制程序里做点小处理也能跑，就没优化了。如果需要读者可自行修改。

import rospy   
from tf_conversions import transformations     
import tf    
import csv

class WayPointSaver:
    def __init__(self):
        self._pIdx = 0
        self._wp = []
        self._header = ['Idx','x','y','yaw']
        self._filename = 'waypoints.csv'
        self.tf_listener = tf.TransformListener()
        with open(self._filename,'w') as f:
                wtr = csv.writer(f)
                wtr.writerow(self._header)
 
    def _get_wp(self):
        ''' get current pose '''
        try:
            trans, rot = self.tf_listener.lookupTransform('/map', '/base_link', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            return
        roll,pitch,yaw = transformations.euler_from_quaternion(rot)  
        x = trans[0]
        y = trans[1]
        if x and y:
            self._wp = [self._pIdx, x, y, yaw]
            return True
        else:
            return False

    def _process_wp(self):
        ''' process points '''
        # your can put your own code here
        self._pIdx += 1
        
    def _write_file(self):
        ''' write point to file '''
        with open(self._filename,'a') as f:
            wtr = csv.writer(f)
            wtr.writerow(self._wp)

    def run(self):
        ''' main loop '''
        if self._get_wp():
            self._process_wp()
            self._write_file()

if __name__ == "__main__":
    rospy.init_node('wayPoint_saver')   
    saver = WayPointSaver()
    r = rospy.Rate(2)
    while not rospy.is_shutdown():  
        saver.run()
        r.sleep()