Robot Motion Planning

Given a set of obstacles P1,,PkP_1,\cdots,P_k, and a polygonal robot RR with starting configuration ss and target configuration tt. Find a path for the robot from ss to tt if possible.

机器人路径规划问题,首先应该先解决:存不存在这样一条路径的问题。

Reference point

define a reference point pp of the robot and define position of the vertices of RR w.r.t. pp.

Suppose our robot can move by translating

R(x,y)R(x,y): position of the robot when the reference point is at (x,y)(x,y).

Suppose our robot can move by translating:

R(x,y,α)R(x,y,\alpha): position of the robot when the reference point is at (x,y)(x,y) and the robot has orientation α\alpha.

We represent the robot by a point in the parameter space (configuration space) R2×[0,360)\mathbb R^2\times[0,360)

先只考虑机器人只能平移,不能旋转。

partition the configuration space in free space and forbidden space

pp in forbidden space R(p)\Lrarr R(p) intersects an obstacle.

RR can move from ss to tt\Lrarr there is a path from ss to tt in the free space.

对于机器人上的一个参考点,在图上都有一个 free space 图。如果 free space 联通起始点,那么就存在一个路径使得机器人可以从起点移动到终点。

if RR is a point then configuration space = work space.

Finding a path in the Configuration Space

如果存在路径,那么就找出这样一条路径

given a configuration space P1,,PkP_1,\cdots,P_k of complexity nn find a path from ss to tt if possible.

idea: construct a trapezoidal decomposition of the free space.

思路:将 free space 用竖线进行梯形分解。

construct a road map G=(V,E)G=(V,E)

VV: set of all centers of all trapezoids Δ\Delta and all midpoints of all vertical segments ss.

梯形的中位线中点+所有梯形顶边和底边线段的中点

(Δ,s)Es(\Delta,s)\in E\Lrarr s is a vertical segment bounding Δ\Delta.

add ss and tt to GG and connect them to the trapezoids containing them.

Run a BFS in the road map to find a path from ss to tt.

Theorem

we can compute a path from ss to tt in O(nlogn)\mathcal O(n\log n) expected time.

如何搞定 free space 的计算问题呢?使用闵可夫斯基和解决!

Minkowski Sums

Let PP and RR be two sets in R2\mathbb R^2

P\oplus R=\{p+r|p\in R\and r\in R\} is the Minkowski sum of PP and RR.

where p+r={px+rx,py+ry}p+r=\{p_x+r_x,p_y+r_y\}

observation: let Q=PRQ=P\oplus R, an extreme point on QQ in direction dd is the sum of extreme points in direction dd on PP and RR.

Theorem

  1. Let PP and RR be convex polygons with nn and mm edges, then Q=PRQ=P\oplus R is a convex polygon and has at most m+nm+n edges.
  2. Let PP and RR be convex polygons with nn and mm edges, then Q=PRQ=P\oplus R can e computed in O(m+n)\mathcal O(m+n) time.

Question: What if PP and RR are not convex?

Pseudo-discs

a pair PP and RR are pseudo-discs

the boundaries of PP and RR intersect in at most two points.

i.e.,

Pint(R) is connected andRint(P) is connected.\partial P\cap int(R)\text{ is connected and}\\ \partial R\cap int(P)\text{ is connected.}

observation: A pair of polygonal pseudo-discs PP and RR defines at most two boundary crossing

Theorem

Let S\mathcal S be a collection of convex polygonal pseudo-discs with a total of nn edges. the total complexity of their union is at most 2n2n.

  • pseudo-disk vertices
  • intersection points

Charge every pseudo-disk vertex to itself.

For an intersection point q=efq=e\cap f, by observation: either

  • ee has no other boundary crossing with \part R.
  • ff has no other boundary crossing with \part P, or both

Let ee be the edge without other boundary crossings, and let vv be the endpoint of ee in RR.

Charge qq to vv.

Claim: Every vertex vv (of a pseudo-disk PP) is charged at most twice.

  • Case 1: vv not in any other pseudo-disk ∴ vv is a green vertex and is charged only by itself.

  • Case 2: v,vRv,v\in R lies in the interior of the union.

    Traverse e=vwe=\overline{vw}. There is only one point qq on the boundary of the union on ee that charges to vv.

    Same for the other edge vu\overline{vu}

  • Case 3: v,v\in\part R lies on the boundary of the union. As in case 2 vv is charged only by its incident edges ee and ff and by vv itself

    vv can not be charged by both its incident edges at the same time.

Let S\mathcal S be a collection of pseudo-discs with a total of nn edges. The total complexity of their union is at most O(n)O(n).

Observation: Let PP and RR be convex polygons with disjoint interiors, let d1d_1 and d2d_2 be directions where PP is more extreme than RR.

Then either

  • PP is more extreme in all directions in [d1,d2][d_1,d_2], or
  • PP is more extreme in all directions in [d2,d1][d_2,d_1].

Let PP and QQ be two convex polygons with disjoint interiors, and let RR be another convex polygon. Then PRP\oplus R and QRQ\oplus R are pseudo-discs.

To show: \part P\oplus R\cap int(Q\oplus R) is connected.

Proof by contradiction. Assume \part P\oplus R\cap int(Q\oplus R) is not connected.

By observation, an extreme point on PRP\oplus R corresponds to a extreme point on PP. Same for QQ.

P\Rarr P more extreme on dpd_p and dqd_q than QQ

Q\Rarr Q more extreme on dsd_s and drd_r than PP

What if PP is not convex?

observation: for any set P,Q,RP,Q,R

R(PQ)=(RP)(RQ)R\oplus(P\cup Q)=(R\oplus P)\cup(R\oplus Q)

Triangulate PP

\Rarr this produces a collection of disjoint convex polygons, so the collection of Minkowski sums is a collection of pseudo-disks.

Let PP be a polygon with nn vertices, and let RR be a convex polygon with mm vertices. Then complexity of PRP\oplus R is Θ(mn)\Theta(mn).

What if PP and RR are both not convex?

Triangulate both PP and RR. This gives a collection of O(mn)O(mn) triangles.

Let PP be a polygon with nn vertices, and let RR be a convex polygon with mm vertices, The complexity of PRP\oplus R is Θ(n2m2)\Theta(n^2m^2).

Let PP be a polygon with nn vertices, and let RR be a convex polygon with m=O(1)m=O(1) vertices, we can compute PRP\oplus R in O(n2logn)\mathcal O(n^2\log n) time.

好麻啊这一段

Minkowski Sum & Robot Motion Planning

Let p=(px,py)-p=(-p_x,-p_y) and S={ppS}-\mathcal S=\{-p|p\in\mathcal S\}

Lemma. Let PP be an obstacle and let RR be a planar, translating robot. A point qq in the configuration space is forbidden iff qP(R(0,0))q\in P\oplus(-R(0,0)).

In direction:

suppose qPR(x,y)q\in P\cap R(x,y)

qR(x,y)q(x,y)R(0,0)q+(x,y)R(0,0)qP(x,y)P(R(0,0))q\in R(x,y)\Rarr q-(x,y)\in R(0,0)\\ -q+(x,y)\in-R(0,0)\\ q\in P\Rarr(x,y)\in P\oplus-(R(0,0))

Only if direction: similar.

Question: How do we compute the free/forbidden space in the configuration space?

Forbidden space =iPi(R(0,0))=\cup_iP_i\oplus-(R(0,0))

So if RR is convex and has constant complexity, and the input polygons have a total nn vertices we can compute the forbidden space in O(nlog2n)O(n\log^2n) time.

Shortest Paths

Given a set of obstacles P1,,PhP_1,\cdots,P_h, a starting point ss and target point tt. Find a shortest path from ss to tt,

Lemma A shortest pat between ss and tt among a st of disjoint obstacles P1,,PhP_1,\cdots,P_h is a polygonal path whose inner vertices are vertices of P1,,PhP_1,\cdots,P_h.

Question: how do we compute a shortest path between ss and tt?

idea: Construct a graph G=(V,E,w)G=(V,E,w) with edge length s.t. for (u,v)E(u,v)\in E the edge corresponds to the distance between uu and vv.

V=V= the set of obstacle vertex vv and ss and tt

(u,v)Eu(u,v)\in E\Lrarr u and vv can see each other

w(u,v)=uvw(u,v)=||uv||

Theorem: We can compute GG in O(n2logn)O(n^2\log n) time.

Visibility Graph

Connecting two vertices if they can see each other.

Sub-goal: compute all visible vertices from any point pp in O(nlogn)O(n\log n) time.

i.e. visibility graph in O(n2logn)O(n^2\log n) time.

Idea: Radical sweep ray!

Exactly identical to normal sweep line, if we rotate the ball until pp is a point at infinity.

我的理解是把这个看成以该点为极点的极坐标,可以映射到一个以该点为原点的直角坐标。就可以转化为最普通的扫描线了。

Find vertices visible from \infty X-coordinate.

Compute visible vertices from ss to tt, add to visibility graph, then use Dijkstra for O(n2logn)O(n^2\log n) time in total.

(can be improved to O(nlogn)O(n\log n))