Robot Path and Motion Planning

1 Path Planning简介

Planning Algorithms, by Steven M. LaValle: http://planning.cs.uiuc.edu/
http://ais.informatik.uni-freiburg.de/teaching/ss15/robotics/slides/19-pathplanning.pdf

1.1 Path Planning VS. Motion Planning

Path Planning和Motion Planning没有本质区别，有时也混用。不过，一般来说Path Planning主要用于无人车、无人机等领域；而Motion Planning主要用于机械臂。如，我们想要机器人帮我们抄个菜，这一般归为Motion Planning问题。

1.2 Open Motion Planning Library (OMPL)

Open Motion Planning Library (OMPL)是用C++开发的一个著名的运动规划库。ROS中的Moveit!使用了OMPL。

2 Configuration Space (C-space)

2.1 Configuration, C-space and Workspace

The configuration of a robot system is a complete specification of the position of every point of that system. The configuration space, or C-space, of the robot system is the space of all possible configurations of the system.

The number of degrees of freedom of a robot system is the dimension of the configuration space, or the minimum number of parameters needed to specify the configuration.

We sometimes refer to this ambient space as the workspace.

Usually a configuration is expressed as a vector of positions and orientations.

2.2 Free Space and Obstacle Region

With $$\mathcal{W} = \mathbb{R}^m$$ being the workspace, $$\mathcal{O} \in \mathcal{W}$$ the set of obstacles, $$\mathcal{A}(q)$$ the robot in configuration $$q \in \mathcal{C}$$, then
\begin{aligned} \mathcal{C}_{free} & := \{ q \in \mathcal{C} \mid \mathcal{A}(q) \cap \mathcal{O} = \emptyset \} \\ \mathcal{C}_{obs} & := C \setminus \mathcal{C}_{free} \\ \end{aligned}

2.3 Two General Approaches (Sampling-Based Planning and Combinatorial Planning)

Sampling-Based planning: Avoid the explicit construction of $$\mathcal{C}_{obs}$$, uses collision-detection to probe and incrementally search the C-space for solution.
Combinatorial planning: Characterizes $$\mathcal{C}_{free}$$ explicitely by capturing the connectivity of $$\mathcal{C}_{free}$$ into a graph and finds solutions using search. Combinatorial planning find paths through the continuous configuration space without resorting to approximations. Combinatorial planning algorithms are alternatively referred to as exact algorithms.

Planning Algorithms, by Steven M. LaValle, Chapter 5 Sampling-Based Motion Planning
Planning Algorithms, by Steven M. LaValle, Chapter 6 Combinatorial Motion Planning

4 Collision Avoidance

• Bug algorithm
• Vector field histogram
• The bubble band technique
• Curvature velocity techniques
• Dynamic window approaches
• The Schlegel approach to obstacle avoidance
• The ASL approach
• Nearness diagram

4.1 Bug Algorithm

The Bug algorithm is perhaps the simplest obstacle avoidance algorithm one could imagine. The basic idea is to follow the contour of each obstacle in the robot’s way and thus circumnavigate it.

With Bug1, the robot fully circles the object first, then departs from the point with the shortest distance toward the goal. This approach is, of course, very inefficient but guarantees that the robot will reach any reachable goal.

With Bug2 the robot begins to follow the object’s contour, but departs immediately when it is able to move directly toward the goal. In general this improved Bug algorithm will have significantly shorter total robot travel.

Bug1和Bug2算法分别如图 3 和图 4 所示。

4.2 Dynamic Window Approache

In robotics motion planning, the dynamic window approach is a real-time collision avoidance strategy for mobile robots developed by Dieter Fox, Wolfram Burgard, and Sebastian Thrun in 1997.

4.2.1 动态窗口法的速度采样空间

(1) 为了在最大的减速度条件下，确保机器人能够在碰到障碍物之前停下来，其速度不能太快，必须有个限制。假设 $$V_a$$ 表示不会碰到障碍物的所有速度的集合，则：
$V_a = \left\{ (v, \omega) \mid v \le \sqrt{2 \cdot \text{dist}(v, \omega) \cdot a_{trans}} \wedge \omega \le \sqrt{2 \cdot \text{dist}(v, \omega) \cdot a_{rot}} \right\}$

(2) 由于电机性能限制，存在一个 动态窗口，在该窗口内的速度是机器人能实际达到的速度。 假设 $$V_d$$ 表示这个动态窗口（当前机器人在 $$\Delta t$$ 内可以达到的速度的集合），则：
$V_d = \left\{ (v, \omega) \mid v \in [v_c - a_{trans} \cdot \Delta t, v_c + a_{trans} \cdot \Delta t] \wedge \omega \in [\omega_c - a_{rot} \cdot \Delta t , \omega_c + a_{rot} \cdot \Delta t ] \right\}$

(3) 机器人自身有一个速度限制，设为 $$V_s$$ 。

$V_r = V_s \cap V_a \cap V_d$

$$V_s$$ = all possible speeds of the robot.
$$V_a$$ = obstacle free area.
$$V_d$$ = speeds reachable within a certain time frame based on possible accelerations.

4.2.2 动态窗口法的评价函数

$G(v, \omega) = \sigma \left(\alpha \cdot \text{heading}(v, \omega) + \beta \cdot \text{dist}(v, \omega) + \gamma \cdot \text{velocity}(v, \omega) \right)$

(1) $$\text{heading}(v, \omega)$$ 用来评价在当前采样速度下，机器人达到预计轨迹末端时朝向和目标朝向之间的角度的差距。 $$\text{heading}(v, \omega)$$ 满足角度差距越小分数越高即可。
(2) $$\text{dist}(v, \omega)$$ 用来评价在当前采样速度下，机器人预计轨迹和最近的障碍物之间的距离。
(3) $$\text{velocity}(v, \omega)$$ 用来标记采样速度（我们希望速度越快越好，可尽快到达目标）。

4.3 Vector Field Histogram (VFH, VFH+, VFH*)

Created: <2016-06-29 Wed 00:00>

Last updated: <2016-08-21 Sun 11:33>

Creator: Emacs 25.1.1 (Org mode 9.0.7)