路径规划学习笔记

第一章 环境配置

1 Ubuntu16.04安装ROS Kinetic（20220315亲测可行） 

1.1 配置Ubuntu软件仓库

打开Ubuntu Software

选择Software & Updates

设置仓库更新

修改镜像源为清华大学

1.2 设置 sources.list

sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'

 1.3 设置秘钥

sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

 1.4 安装

更新软件包

sudo apt-get update 

 安装完整版(若长时间不动，可 Ctrl+c 停止，重新开始)

sudo apt-get install ros-kinetic-desktop-full

 1.5 初始化和更新rosdep

sudo rosdep init

 如果初始化报错

sudo gedit /etc/hosts

在弹出的框中，回车添加以下内容名，并保存

199.232.28.133 raw.githubusercontent.com

 初始化成功后，更新rosdep

rosdep update 

 若出现错误，则使用rosdepc

sudo pip install rosdepc（若不行试试sudo pip3 install rosdepc）

还是不行则

sudo apt-get install python3-pip sudo pip install rosdepc

安装完成后，则可初始化和更新

sudo rosdepc initrosdepc update

1.6 环境配置

 echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc

 1.7 构建工厂依赖

sudo apt-get install python-rosinstall python-rosinstall-generator python-wstool build-essential

1.8 检验是否安装成功

roswtf 

可以看到 0 warning , 0 fault

到此环境配置成功

2 作业总结

2.1 下载并解压课程包hw_1

2.2 创建工作空间

Ctrl+Alt+T，开启终端，在home目录下创建工作路径，并打开创建的工作路径

// mkdir filename 创建一个文件夹

// mkdir -p ~/filename1/filename2 这里需要创建一个工作路径，故用该命令

mkdir -p ~/catkin_ws/src     

打开创建的路径（Tab键命令补齐，Ctrl+c终止程序，Ctrl+d退出终端，Ctrl+l清屏）

// cd ~/filename1/filename2 打开文件夹

cd ~/catkin_ws/src/          

复制压缩包hw_1/src里的三个文件夹（功能包）到创建的路径下

# sudo mv filename target_work_path 移动一个文件夹到另一个文件夹

# sudo cp -r filename target_work_path 移动一个文件夹到另一个文件夹

# 当前工作路径应是要移动的文件夹所在路径

sudo cp -r grid_path_searcher ~/catkin_ws/src 

sudo cp -r rviz_plugins ~/catkin_ws/src 

sudo cp -r waypoint_generator ~/catkin_ws/src 

终端执行以下命令

//生成CMakeList.txt，将上述复制的三个文件夹看做员工，则新生成的txt看做是员工表 

catkin_init_workspace

cd ~/catkin_ws/

//编译上述三个功能包，生成两个文件夹：devel（执行文件）、build（编译文件）

catkin_make

//告诉ROS工作空间路径

source devel/setup.bash

2.3 打开 rviz 

//启动ROS自带的节点

roscore

 Ctrl+Alt+t，开启一个新终端（Ctrl+Shift+t），执行以下命令

//告诉ROS工作空间路径

cd ~/catkin_ws/

source devel/setup.bash

// 如果，没有以上两条，可能会出错

rviz

 出现 rviz 初始界面

 2.4 打开 rviz 配置文件

 open config 配置文件路径为

~/catkin_ws/src/grid_path_searcher/launch/rviz_config/demo.rviz

得到以下画面

2.5 配置 rviz 插件

点击“+”，添加 Goal3DTool插件

点击“-”，删除 2D Nav Goal 和 2D Pose Estimate

 2.6 载入地图

Ctrl+Alt+t，开启一个新终端，执行以下命令

//告诉ROS工作空间路径

cd ~/catkin_ws/

source devel/setup.bash

//运行src中第一个功能包的demo.lanch文件，导入地图

roslaunch grid_path_searcher demo.launch

可以看到 rviz 自动载入了点云地图

点击3D New Goal，在此地图中按住左键鼠标，出现绿色箭头，移动鼠标用于调整箭头方向，再 按住鼠标右键，移动鼠标调整箭头的高度，最后松开鼠标左右键，能够看到 在 3 维地图中规划出的一条从终点到达地图中心的一条路径。绿色的为Astar规划出的路径，红的的为JPS规划的路径。

最后一个终端可以看到打印数据。

出现问题可以从创建文件夹开始再试一次，有意想不到的效果。

2.7 rqt_graph工具使用

显示ROS此时有哪些节点node（椭圆形）在工作，发布了哪些话题topic（方形）。

/goal 生成起点

/random_complex 生成随机地图和终点

/demo_node 获取 /goal 和 /random_complex 生成的信息

/demo_node/grid_map_vis 在rviz中显示地图

/demo_node/grid_path_vis 在rviz中显示路径点

第二章 

1 图搜索基础

工作空间：

配置空间：

膨胀检测：

        图由节点和边构成，可分为有向图、无向图和权重图。

图搜索算法框架

• 维护两个容器，一个容器存放将访问的节点，一个存放已访问的节点
• 访问容器首先存放的是起点节点
• 循环
  • 访问：从访问容器中根据某种规则弹出一个节点（对于Astar是 f(n) 值最小的节点）
  • 扩展：扩展当前节点的所有邻居节点
  • 装入容器：将所有邻居装入到访问容器中
• 结束循坏：访问到目标节点或容器中无节点弹出

广度优先搜索BFS：先进先出

深度优先DFS：后进先出

贪心算法：用一个函数估计当前点到终点的距离

2 Astar算法

        在图搜索算法中，若在搜索的每一步都利用估价函数 f(n) = g(n) + h(n) 对容器中的节点进行排序，则该算法为Astar算法。Astar算法是一种图搜索算法，在图中求最短路径的搜索方法。

        Astar算法的估价函数：f(n) = g(n) + h(n)

f(n)：总搜索代价

g(n)：起点到当前节点 n 的代价和

h(n)：启发函数，当前节点 n 到目标节点的实际距离的猜测，指引搜索的正确方向。

        欧几里得启发函数：

        曼哈顿启发函数：

Astar算法伪代码

• 维护容器，这个容器桨扩展的点按从小到大的顺序进行排列
• 将起始节点 A 放入这个容器
• 定义 g(A)=0, g(n)= ∞
• 循环
  • if 容器是空，return False; break
  • 从容器中弹出 f 值最小的节点 n
  • 把这个节点 n 放到另一个放到被访问节点的容器
  • if 节点 n 是目标节点，return True; break
  • 扩展节点 n 的所有邻居节点 m
    • if g(m)=∞（即节点 m 未被扩展过）
      • g(m)=g(n)+Cnm（Cnm为节点 n 到节点 m 的距离）
      • 将节点 m 放到容器中
    • if g(m)>g(n)+Cnm（节点 m 被扩展过，此时需要更新 g(m) 值）
    • g(m)=g(n)+Cnm
  • 结束
• 结束循环