Embed Size (px)
Citation preview
Real-time motion planning for
Manipulator based on Configuration Space
Chen KemingCis Peking University
Main Contents
Introduction My current work Future work and related work C-Space visualization for Teleoperation
Introduction Manipulator Motion Planning Problems
– Statement:• Compute a collision-free path for a manipulator among
obstacles
– Inputs:• Geometry of manipulator and obstacles• Kinematics of manipulator (degrees of freedom)• Initial and goal manipulator configurations (placements)
– Outputs:• Continuous sequence of collision-free manipulator
configurations connecting the initial and goal configurations
Introduction
Tool: Configuration Space
Introduction
FrameworkManipulator
representation
Obstacles representation
Configuration space formulation
DiscretizationGraph
searching
My Current Work Motivation: Towards real-time
Human-Robot Interaction in dynamic environment
Application– (Mobile based) Manipulator interacts
with human without collision– Dual-arm robot (Chen Fen,Ding Fu-qiang
and Zhao Xi-fang “Collision-free Path Planning of dual-arm Robot.” ROBOT,vol.24,Mar.2002)
My Current Work Assumption
– The input data are readily available at any time
Manipulator representation– Cylinders– Reduction to 3 joints
Obstacles representation– Cylinders– Combination of main body and arms
My Current Work
C-Space formulation– Reduction to determine whether 2 cylinders
collide in 3D W-Space
Case 1: Case 2:
My Current Work
– Schematic
My Current Work
– Goal configurations formulation using inverse kinematics
Discretization– Joint 1: 161, Joint 2: 71, Joint 3: 121
My Current Work
Lazy C-Space computation due to– Large numbers of points in C-Space(total
1,383,151 points)– Real-time process requirement
Graph searching (A*)– Why use A*
• Optimal and complete
• Objective values (expanding nodes, time)
My Current Work
– Speed up A*• OPEN is implemented as
– hash table
– priority list(implemented as Binary Heap)
• CLOSED is implemented as hash table
List implementation Hash table and Binary Heap implementation
An example (collision checking points: more than 30000)
My Current Work
Result:
My Current Work Dealing with dynamic environment
– A* Replanner: Plan by A* using all the available information at the start.
– Start tracing the optimal path– If there is a discrepancy between the initial ma
p and the actual environment, update the new cost values for the corresponding arcs, run A* again for planning between the current position and the goal.
My Current Work
A* Replanner: shortcoming– If the goal configuration is far away, little cha
nges may force the planner to use A* over the whole C-Space, although the changes in the optimal path may be small
– Hence, A* replanner can be grossly inefficient computationally for real-time process
My Current Work Optimization --- Dynamic A*(D*) [Stentz, 1994]
– Functionally equivalent to A* replanner– Make “local” changes to the map and the resultant opt
imal path when a discrepancy between map and the environment is found
• Essentially prunes the graph search
– So, D* could be a proper choice for optimization. But so far, it has only been used in mobile robotics to move a robot to given goal coordinates in unknown terrain [Koenig, 2002].
D* Algorithm
c(x1,x2)=1c(x1,x3)=1.4
c(x1,x8)=10000,if x8 is in obstacle,x1 is a freecell
c(x1,x9)=10000.4, if x9 is in obstacle, x1 is a freecell
x9 x2 x3
x8 x1 x5
x7 x6 x7
6
h=6
k=6
b=
h=5
k=5
b=
h=4
k=4
b=
h=3
k=3
b=
h=2
k=2
b=
h=1
k=1
b=
h=0
k=0
b=
5
h=6.4
k=6.4
b=
h=5.4
k=5.4
b=
h=4.4
k=4.4
b=
h=3.4
k=3.4
b=
h=2.4
k=2.4
b=
h=1.4
k=1.4
b=
h=1
k=1
b=
4
h=6.8
k=6.8
b=
h=5.8
k=5.8
b=
h=4.8
k=4.8
b=
h=3.8
k=3.8
b=
h=2.8
k=2.8
b=
h=2.4
k=2.4
b=
h=2
k=2
b=
3
h=7.2
k=7.2
b=
h=6.2
k=6.2
b=
h=5.2
k=5.2
b=
h=4.2
k=4.2
b=
h=3.8
k=3.8
b=
h=3.4
k=3.4
b=
h=3
k=3
b=
2
h=7.6
k=7.6
b=
h=6.6
k=6.6
b=
h=5.6
k=5.6
b=
h=5.2
k=5.2
b=
h=4.8
k=4.8
b=
h=4.4
k=4.4
b=
h=4
k=4
b=
1
h=8.0
k=8.0
b=
h=7.0
k=7.0
b=
h=6.6
k=6.6
b=
h=6.2
k=6.2
b=
h=5.8
k=5.8
b=
h=5.4
k=5.4
b=
h=5
k=5
B=
r/c 1 2 3 4 5 6 7
Goal
Start
Gate
6
h=6
k=6
b=
h=5
k=5
b=
h=4
k=4
b=
h=3
k=3
b=
h=2
k=2
b=
h=1
k=1
b=
h=0
k=0
b=
5
h=6.4
k=6.4
b=
h=5.4
k=5.4
b=
h=4.4
k=4.4
b=
h=3.4
k=3.4
b=
h=2.4
k=2.4
b=
h=1.4
k=1.4
b=
h=1
k=1
b=
4
h=6.8
k=6.8
b=
h=5.8
k=5.8
b=
h=4.8
k=4.8
b=
h=3.8
k=3.8
b=
h=2.8
k=2.8
b=
h=2.4
k=2.4
b=
h=2
k=2
b=
3
h=7.2
k=7.2
b=
h=6.2
k=6.2
b=
h=5.2
k=5.2
b=
h=4.2
k=4.2
b=
h=3.8
k=3.8
b=
h=3.4
k=3.4
b=
h=3
k=3
b=
2
h=7.6
k=7.6
b=
h=6.6
k=6.6
b=
h=5.6
k=5.6
b=
h=5.2
k=5.2
b=
h=4.8
k=4.8
b=
h=4.4
k=4.4
b=
h=4
k=4
b=
1
h=8.0
k=8.0
b=
h=7.0
k=7.0
b=
h=6.6
k=6.6
b=
h=6.2
k=6.2
b=
h=5.8
k=5.8
b=
h=5.4
k=5.4
b=
h=5
k=5
b=
r/c 1 2 3 4 5 6 7
(7,6) 0
State k
6
h=6
k=6
b=
h=5
k=5
b=
h=4
k=4
b=
h=3
k=3
b=
h=2
k=2
b=
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=6.4
k=6.4
b=
h=5.4
k=5.4
b=
h=4.4
k=4.4
b=
h=3.4
k=3.4
b=
h=2.4
k=2.4
b=
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=6.8
k=6.8
b=
h=5.8
k=5.8
b=
h=4.8
k=4.8
b=
h=3.8
k=3.8
b=
h=2.8
k=2.8
b=
h=2.4
k=2.4
b=
h=2
k=2
b=
3
h=7.2
k=7.2
b=
h=6.2
k=6.2
b=
h=5.2
k=5.2
b=
h=4.2
k=4.2
b=
h=3.8
k=3.8
b=
h=3.4
k=3.4
b=
h=3
k=3
b=
2
h=7.6
k=7.6
b=
h=6.6
k=6.6
b=
h=5.6
k=5.6
b=
h=5.2
k=5.2
b=
h=4.8
k=4.8
b=
h=4.4
k=4.4
b=
h=4
k=4
b=
1
h=8.0
k=8.0
b=
h=7.0
k=7.0
b=
h=6.6
k=6.6
b=
h=6.2
k=6.2
b=
h=5.8
k=5.8
b=
h=5.4
k=5.4
b=
h=5
k=5
b=
r/c 1 2 3 4 5 6 7
(6,6) 1
(7,5) 1
(6,5) 1.4
State k
6
h=6
k=6
b=
h=5
k=5
b=
h=4
k=4
b=
h=3
k=3
b=
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=6.4
k=6.4
b=
h=5.4
k=5.4
b=
h=4.4
k=4.4
b=
h=3.4
k=3.4
b=
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=6.8
k=6.8
b=
h=5.8
k=5.8
b=
h=4.8
k=4.8
b=
h=3.8
k=3.8
b=
h=2.8
k=2.8
b=
h=2.4
k=2.4
b=
h=2
k=2
b=
3
h=7.2
k=7.2
b=
h=6.2
k=6.2
b=
h=5.2
k=5.2
b=
h=4.2
k=4.2
b=
h=3.8
k=3.8
b=
h=3.4
k=3.4
b=
h=3
k=3
b=
2
h=7.6
k=7.6
b=
h=6.6
k=6.6
b=
h=5.6
k=5.6
b=
h=5.2
k=5.3
b=
h=4.8
k=4.8
b=
h=4.4
k=4.4
b=
h=4
k=4
b=
1
h=8.0
k=8.0
b=
h=7.0
k=7.0
b=
h=6.6
k=6.6
b=
h=6.2
k=6.2
b=
h=5.8
k=5.8
b=
h=5.4
k=5.4
b=
h=5
k=5
b=
r/c 1 2 3 4 5 6 7
(7,5) 1
(6,5) 1.4
(5,6) 2
(5,5) 2.4
State k
6
h=6
k=6
b(1,6)=
h=5
k=5
b=
h=4
k=4
b=
h=3
k=3
b=
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=6.4
k=6.4
b(1,5)=
h=5.4
k=5.4
b=
h=4.4
k=4.4
b=
h=3.4
k=3.4
b=
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=6.8
k=6.8
b(1,4)=
h=5.8
k=5.8
b=
h=4.8
k=4.8
b=
h=3.8
k=3.8
b=
h=2.8
k=2.8
b=
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=7.2
k=7.2
b(1,3)=
h=6.2
k=6.2
b=
h=5.2
k=5.2
b=
h=4.2
k=4.2
b=
h=3.8
k=3.8
b=
h=3.4
k=3.4
b=
h=3
k=3
b=
2
h=7.6
k=7.6
b(1,2)=
h=6.6
k=6.6
b=
h=5.6
k=5.6
b=
h=5.2
k=
b=
h=4.8
k=4.8
b=
h=4.4
k=4.4
b=
h=4
k=4
b=
1
h=8.0
k=8.0
b(1,1)=
h=7.0
k=7.0
b=
h=6.6
k=6.6
b=
h=6.2
k=6.2
b=
h=5.8
k=5.8
b=
h=5.4
k=5.4
b=
h=5
k=5
b=
r/c 1 2 3 4 5 6 7
(6,5) 1.4
(5,6) 2
(7,4) 2
(6,4) 2.4
(5,5) 2.4
State k
6
h=6
k=6
b(1,6)=
h=5
k=
b=
h=10003
k=4
b=(4,6)
h=3
k=3
b=(5,6)
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=6.4
k=6.4
b(1,5)=
h=5.4
k=
b=
h=10003.4
k=4.4
b=(4,6)
h=3.4
k=3.4
b=(5,6)
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=6.8
k=6.8
b(1,4)=
h=5.8
k=
b=
h=4.8
k=
b=
h=3.8
k=3.8
b=(5,5)
h=2.8
k=2.8
b=(6,5)
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=7.2
k=7.2
b(1,3)=
h=6.2
k=
b=
h=5.2
k=
b=
h=4.2
k=4.2
b=(5,4)
h=3.8
k=3.8
b=(6,4)
h=3.4
k=3.4
b=(7,4)
h=3
k=3
b=(7,4)
2
h=7.6
k=7.6
b(1,2)=
h=6.6
k=6.6
b=
h=5.6
k=5.6
b=
h=5.2
k=5.2
b=
h=4.8
k=4.8
b=
h=4.4
k=4.4
b=
h=4
k=4
b=
1
h=8.0
k=8.0
b(1,1)=
h=7.0
k=7.0
b=
h=6.6
k=6.6
b=
h=6.2
k=6.2
b=
h=5.8
k=5.8
b=
h=5.4
k=5.4
b=
h=5
k=5
b=
r/c 1 2 3 4 5 6 7
(7,3) 3
(6,3) 3.4
(4,5) 3.4
(5,3) 3.8
(4,4) 3.8
(3,6) 4
(4,3) 4.2
(3,5) 4.4
State k
6
h=20004
k=6
b=(2,6)
h=10004
k=5
b=(3,6)
h=10003
k=4
b=(4,6)
h=3
k=3
b=(5,6)
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=20004.4
k=6.4
b=(2,6)
h=10004.4
k=5.4
b=(3,6)
h=10003.4
k=4.4
b=(4,6)
h=3.4
k=3.4
b=(5,6)
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=20006.8
k=6.8
b=(2,5)
h=10004.8
k=5.8
b=(3,5)
h=10003.8
k=10003.8
b=(4,5)
h=3.8
k=3.8
b=(5,5)
h=2.8
k=2.8
b=(6,5)
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=8.0
k=8.0
b=(2,2)
h=10005.2
k=6.2
b=(3,4)
h=10004.2
k=5.2
b=(4,4)
h=4.2
k=4.2
b=(5,4)
h=3.8
k=3.8
b=(6,4)
h=3.4
k=3.4
b=(7,4)
h=3
k=3
b=(7,4)
2
h=7.6
k=7.6
b=(2,2)
h=6.6
k=6.6
b=(3,2)
h=5.6
k=5.6
b=(4.3)
h=10004.2
k=5.2
b=(5,3)
h=4.8
k=4.8
b=(6,3)
h=4.4
k=4.4
b=(7,3)
h=4
k=4
b=(7,3)
1
h=8.0
k=8.0
b=(2,2)
h=7.0
k=7.0
b=(3,2)
h=6.6
k=6.6
b=(3,2)
h=6.2
k=6.2
b=(5,2)
h=5.8
k=5.8
b=(6,2)
h=5.4
k=5.4
b=(7,2)
h=5
k=5
b=(7,2)
r/c 1 2 3 4 5 6 7
(1,6) 6
(1,5) 6.4
(1,4) 6.8
(1,2) 7.6
(1,3) 8.0
(1,1) 8.0
State k
6
h=20004
k=6
b=(2,6)
h=10004
k=5
b=(3,6)
h=10003
k=4
b=(4,6)
h=3
k=3
b=(5,6)
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=20004.4
k=6.4
b=(2,6)
h=10004.4
k=5.4
b=(3,6)
h=10003.4
k=4.4
b=(4,6)
h=3.4
k=3.4
b=(5,6)
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=20006.8
k=6.8
b=(2,5)
h=10004.8
k=5.8
b=(3,5)
h=10003.8
k=4.8
b=(4,5)
h=3.8
k=3.8
b=(5,5)
h=2.8
k=2.8
b=(6,5)
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=8.0
k=8.0
b=(2,2)
h=10005.2
k=6.2
b=(3,4)
h=10004.2
k=5.2
b=(4,4)
h=4.2
k=4.2
b=(5,4)
h=3.8
k=3.8
b=(6,4)
h=3.4
k=3.4
b=(7,4)
h=3
k=3
b=(7,4)
2
h=7.6
k=7.6
b=(2,2)
h=6.6
k=6.6
b=(3,2)
h=5.6
k=5.6
b=(4.3)
h=10004.2
k=5.2
b
h=4.8
k=4.8
b=(6,3)
h=4.4
k=4.4
b=(7,3)
h=4
k=4
b=(7,3)
1
h=8.0
k=8.0
b=(2,2)
h=7.0
k=7.0
b=(3,2)
h=6.6
k=6.6
b=(3,2)
h=6.2
k=6.2
b=(5,2)
h=5.8
k=5.8
b=(6,2)
h=5.4
k=5.4
b=(7,2)
h=5
k=5
b=(7,2)
r/c 1 2 3 4 5 6 7
(1,6) 6
(1,5) 6.4
(1,4) 6.8
(1,2) 7.6
(1,3) 8.0
(1,1) 8.0
State k
6
h=20004
k=6
b=(2,6)
h=10004
k=5
b=(3,6)
h=10003
k=4
b=(4,6)
h=3
k=3
b=(5,6)
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=20004.4
k=6.4
b=(2,6)
h=10004.4
k=5.4
b=(3,6)
h=10003.4
k=4.4
b=(4,6)
h=3.4
k=3.4
b=(5,6)
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=20006.8
k=6.8
b=(2,5)
h=10004.8
k=5.8
b=(3,5)
h=10003.8
k=4.8
b=(4,5)
h=3.8
k=3.8
b=(5,5)
h=2.8
k=2.8
b=(6,5)
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=8.0
k=8.0
b=(2,2)
h=10005.2
k=6.2
b=(3,4)
h=10004.2
k=5.2
b=(4,4)
h=4.2
k=4.2
b=(5,4)
h=3.8
k=3.8
b=(6,4)
h=3.4
k=3.4
b=(7,4)
h=3
k=3
b
2
h=7.6
k=7.6
b=(2,2)
h=6.6
k=6.6
b=(3,2)
h=5.6
k=5.6
b=(4.3)
h=10004.2
k=5.2
b=(5,3)
h=4.8
k=4.8
b=(6,3)
h=4.4
k=4.4
b=(7,3)
h=4
k=4
b=(7,3)
1
h=8.0
k=8.0
b=(2,2)
h=7.0
k=7.0
b=(3,2)
h=6.6
k=6.6
b=(3,2)
h=6.2
k=6.2
b=(5,2)
h=5.8
k=5.8
b=(6,2)
h=5.4
k=5.4
b=(7,2)
h=5
k=5
b=(7,2)
r/c 1 2 3 4 5 6 7
(1,6) 6
(1,5) 6.4
(1,4) 6.8
(1,2) 7.6
(1,3) 8.0
(1,1) 8.0
State k
(4,3) 4.2
(1,6) 6
(1,5) 6.4
(1,4) 6.8
(1,2) 7.6
(1,3) 8.0
(1,1) 8.0
State k
6
h=20004
k=6
b=(2,6)
h=10004
k=5
b=(3,6)
h=10003
k=4
b=(4,6)
h=3
k=3
b=(5,6)
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=20004.4
k=6.4
b=(2,6)
h=10004.4
k=5.4
b=(3,6)
h=10003.4
k=4.4
b=(4,6)
h=3.4
k=3.4
b=(5,6)
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=20006.8
k=6.8
b=(2,5)
h=10004.8
k=5.8
b=(3,5)
h=10003.8
k=4.8
b=(4,5)
h=3.8
k=3.8
b=(5,5)
h=2.8
k=2.8
b=(6,5)
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=8.0
k=8.0
b=(2,2)
h=10005.2
k=6.2
b
h=10004.2
k=5.2
b=(4,4)
h=4.2
k=4.2
b=(5,4)
h=3.8
k=3.8
b=(6,4)
h=3.4
k=3.4
b=(7,4)
h=3
k=3
b=(7,4)
2
h=7.6
k=7.6
b=(2,2)
h=6.6
k=6.6
b=(3,2)
h=5.6
k=5.6
b=(4.3)
h=10004.2
k=5.2
b=(5,3)
h=4.8
k=4.8
b=(6,3)
h=4.4
k=4.4
b=(7,3)
h=4
k=4
b=(7,3)
1
h=8.0
k=8.0
b=(2,2)
h=7.0
k=7.0
b=(3,2)
h=6.6
k=6.6
b=(3,2)
h=6.2
k=6.2
b=(5,2)
h=5.8
k=5.8
b=(6,2)
h=5.4
k=5.4
b=(7,2)
h=5
k=5
b=(7,2)
r/c 1 2 3 4 5 6 7
6
h=20004
k=6
b=(2,6)
h=10004
k=5
b
h=10003
k=4
b
h=3
k=3
b=(5,6)
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=20004.4
k=6.4
b=(2,6)
h=10004.4
k=5.4
b=(3,6)
h=10003.4
k=4.4
b=(4,6)
h=3.4
k=3.4
b=(5,6)
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=20006.8
k=6.8
b=(2,5)
h=10004.8
k=5.8
b=(3,5)
h=10003.8
k=4.8
b=(4,5)
h=3.8
k=3.8
b=(5,5)
h=2.8
k=2.8
b=(6,5)
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=8.0
k=8.0
b=(2,2)
h=10005.2
k=6.2
b=(3,4)
h=10004.2
k=5.2
b=(4,4)
h=4.2
k=4.2
b=(5,4)
h=3.8
k=3.8
b=(6,4)
h=3.4
k=3.4
b=(7,4)
h=3
k=3
b=(7,4)
2
h=7.6
k=7.6
b=(2,2)
h=6.6
k=6.6
b=(3,2)
h=10004.6
k=5.6
b=(4.3)
h=10004.2
k=5.2
b=(5,3)
h=4.8
k=4.8
b=(6,3)
h=4.4
k=4.4
b=(7,3)
h=4
k=4
b=(7,3)
1
h=8.0
k=8.0
b=(2,2)
h=7.0
k=7.0
b=(3,2)
h=6.6
k=6.6
b=(3,2)
h=6.2
k=6.2
b=(5,2)
h=5.8
k=5.8
b=(6,2)
h=5.4
k=5.4
b=(7,2)
h=5
k=5
b=(7,2)
r/c 1 2 3 4 5 6 7
(3,2) 5.6
(1,6) 6
(1,5) 6.4
(1,4) 6.8
(1,2) 7.6
(1,3) 8.0
(1,1) 8.0
State k
6
h=20004
k=6
b=(2,6)
h=10004
k=5
b
h=10003
k=4
b=(4,6)
h=3
k=3
b=(5,6)
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=20004.4
k=6.4
b=(2,6)
h=10004.4
k=5.4
b=(3,6)
h=10003.4
k=4.4
b=(4,6)
h=3.4
k=3.4
b=(5,6)
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=20006.8
k=6.8
b=(2,5)
h=10004.8
k=5.8
b=(3,5)
h=10003.8
k=4.8
b=(4,5)
h=3.8
k=3.8
b=(5,5)
h=2.8
k=2.8
b=(6,5)
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=8.0
k=8.0
b=(2,2)
h=10005.2
k=6.2
b=(3,4)
h=10004.2
k=5.2
b=(4,4)
h=4.2
k=4.2
b=(5,4)
h=3.8
k=3.8
b=(6,4)
h=3.4
k=3.4
b=(7,4)
h=3
k=3
b=(7,4)
2
h=7.6
k=7.6
b=(2,2)
h=10005.6
k=6.6
b=(3,2)
h=10004.6
k=5.6
b=(4.3)
h=10004.2
k=5.2
b=(5,3)
h=4.8
k=4.8
b=(6,3)
h=4.4
k=4.4
b=(7,3)
h=4
k=4
b=(7,3)
1
h=8.0
k=8.0
b=(2,2)
h=10006.0
k=7.0
b=(3,2)
h=10005.6
k=6.6
b=(3,2)
h=6.2
k=6.2
b=(5,2)
h=5.8
k=5.8
b=(6,2)
h=5.4
k=5.4
b=(7,2)
h=5
k=5
b=(7,2)
r/c 1 2 3 4 5 6 7
(1,6) 6(4,1) 6.2(1,5) 6.4(3,1) 6.6(2,2) 6.6(1,4) 6.8(2,1) 7.0(1,2) 7.6(1,3) 8.0(1,1) 8.0
State k
6
h=20004
k=6
b=(2,6)
h=10004
k=5
b
h=10003
k=4
b=(4,6)
h=3
k=3
b=(5,6)
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=20004.4
k=6.4
b=(2,6)
h=10004.4
k=5.4
b=(3,6)
h=10003.4
k=4.4
b=(4,6)
h=3.4
k=3.4
b=(5,6)
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=20006.8
k=6.8
b=(2,5)
h=10004.8
k=5.8
b=(3,5)
h=10003.8
k=4.8
b=(4,5)
h=3.8
k=3.8
b=(5,5)
h=2.8
k=2.8
b=(6,5)
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=8.0
k=8.0
b=(2,2)
h=10005.2
k=6.2
b=(3,4)
h=10004.2
k=5.2
b=(4,4)
h=4.2
k=4.2
b=(5,4)
h=3.8
k=3.8
b=(6,4)
h=3.4
k=3.4
b=(7,4)
h=3
k=3
b=(7,4)
2
h=7.6
k=7.6
b=(2,2)
h=10005.6
k=6.6
b=(3,2)
h=10004.6
k=5.6
b=(4.3)
h=10004.2
k=5.2
b=(5,3)
h=4.8
k=4.8
b=(6,3)
h=4.4
k=4.4
b=(7,3)
h=4
k=4
b=(7,3)
1
h=8.0
k=8.0
b=(2,2)
h=10006.0
k=7.0
b=(3,2)
h=10005.6
k=6.6
b=(3,2)
h=6.2
k=6.2
b=(5,2)
h=5.8
k=5.8
b=(6,2)
h=5.4
k=5.4
b=(7,2)
h=5
k=5
b=(7,2)
r/c 1 2 3 4 5 6 7
(1,6) 6(4,1) 6.2(1,5) 6.4(3,1) 6.6(2,2) 6.6(1,4) 6.8(2,1) 7.0(1,2) 7.6(1,3) 8.0(1,1) 8.0
State k
6
h=20004
k=6
b=(2,6)
h=10004
k=5
b=(3,6)
h=10003
k=4
b=(4,6)
h=3
k=3
b=(5,6)
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=20004.4
k=6.4
b=(2,6)
h=10004.4
k=5.4
b=(3,6)
h=10003.4
k=4.4
b=(4,6)
h=3.4
k=3.4
b=(5,6)
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=20006.8
k=6.8
b=(2,5)
h=10004.8
k=5.8
b)=(3,5)
h=10003.8
k=4.8
b=(4,5)
h=3.8
k=3.8
b=(5,5)
h=2.8
k=2.8
b=(6,5)
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=8.0
k=8.0
b=(2,2)
h=10005.2
k=6.2
b=(3,4)
h=10004.2
k=5.2
b=(4,4)
h=4.2
k=4.2
b=(5,4)
h=3.8
k=3.8
b=(6,4)
h=3.4
k=3.4
b=(7,4)
h=3
k=3
b=(7,4)
2
h=7.6
k=7.6
b=(2,2)
h=10005.6
k=6.6
b=(3,2)
h=10004.6
k=5.6
b=(4.3)
h=10004.2
k=5.2
b=(5,3)
h=4.8
k=4.8
b=(6,3)
h=4.4
k=4.4
b=(7,3)
h=4
k=4
b=(7,3)
1
h=8.0
k=8.0
b=(2,2)
h=10006.0
k=7.0
b=(3,2)
h=10005.6
k=6.6
b=(3,2)
h=6.2
k=6.2
b=(5,2)
h=5.8
k=5.8
b=(6,2)
h=5.4
k=5.4
b=(7,2)
h=5
k=5
b=(7,2)
r/c 1 2 3 4 5 6 7
(4,1) 6.2(1,5) 6.4(3,1) 6.6(2,2) 6.6(1,4) 6.8(2,1) 7.0(1,2) 7.6(1,3) 8.0(1,1) 8.0
State k
6
h=20004
k=6
b=(2,6)
h=10004
k=5
b=(3,6)
h=10003
k=4
b=(4,6)
h=3
k=3
b=(5,6)
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=20004.4
k=6.4
b=(2,6)
h=10004.4
k=5.4
b=(3,6)
h=10003.4
k=4.4
b=(4,6)
h=3.4
k=3.4
b=(5,6)
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=20006.8
k=6.8
b=(2,5)
h=10004.8
k=5.8
b=(3,5)
h=10003.8
k=4.8
b=(4,5)
h=3.8
k=3.8
b=(5,5)
h=2.8
k=2.8
b=(6,5)
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=8.0
k=8.0
b=(2,2)
h=10005.2
k=6.2
b=(3,4)
h=10004.2
k=5.2
b=(4,4)
h=4.2
k=4.2
b=(5,4)
h=3.8
k=3.8
b=(6,4)
h=3.4
k=3.4
b=(7,4)
h=3
k=3
b=(7,4)
2
h=7.6
k=7.6
b=(2,2)
h=10005.6
k=6.6
b=(3,2)
h=7.6
k=7.6
b=(4.1)
h=10004.2
k=5.2
b=(5,3)
h=4.8
k=4.8
b=(6,3)
h=4.4
k=4.4
b=(7,3)
h=4
k=4
b=(7,3)
1
h=8.0
k=8.0
b=(2,2)
h=10006.0
k=7.0
b=(3,2)
h=7.2
k=7.2
b=(4,1)
h=6.2
k=6.2
b=(5,2)
h=5.8
k=5.8
b=(6,2)
h=5.4
k=5.4
b=(7,2)
h=5
k=5
b=(7,2)
r/c 1 2 3 4 5 6 7
(5,2) 4.8(5,1) 5.8(3,2) 5.6(1,5) 6.4(3,1) 6.6(2,2) 6.6(1,4) 6.8(2,1) 7.0(1,2) 7.6(1,3) 8.0(1,1) 8.0
State k
6
h=20004
k=6
b=(2,6)
h=10004
k=5
b=(3,6)
h=10003
k=4
b=(4,6)
h=3
k=3
b=(5,6)
h=2
k=2
b=(6,6)
h=1
k=1
b=(7,6)
h=0
k=0
b=
5
h=20004.4
k=6.4
b=(2,6)
h=10004.4
k=5.4
b=(3,6)
h=10003.4
k=4.4
b=(4,6)
h=3.4
k=3.4
b=(5,6)
h=2.4
k=2.4
b=(6,6)
h=1.4
k=1.4
b=(7,6)
h=1
k=1
b=(7,6)
4
h=20006.8
k=6.8
b=(2,5)
h=10004.8
k=5.8
b=(3,5)
h=10003.8
k=4.8
b=(4,5)
h=3.8
k=3.8
b=(5,5)
h=2.8
k=2.8
b=(6,5)
h=2.4
k=2.4
b=(7,5)
h=2
k=2
b=(7,5)
3
h=8.0
k=8.0
b=(2,2)
h=10005.2
k=6.2
b=(3,4)
h=10004.2
k=5.2
b=(4,4)
h=4.2
k=4.2
b=(5,4)
h=3.8
k=3.8
b=(6,4)
h=3.4
k=3.4
b=(7,4)
h=3
k=3
b=(7,4)
2
h=7.6
k=7.6
b=(2,2)
h=10005.6
k=6.6
b=(3,2)
h=7.6
k=7.6
b=(4.1)
h=10004.2
k=5.2
b=(5,3)
h=4.8
k=4.8
b=(6,3)
h=4.4
k=4.4
b=(7,3)
h=4
k=4
b=(7,3)
1
h=8.0
k=8.0
b=(2,2)
h=10006.0
k=7.0
b=(3,2)
h=7.2
k=7.2
b=(4,1)
h=6.2
k=6.2
b=(5,2)
h=5.8
k=5.8
b=(6,2)
h=5.4
k=5.4
b=(7,2)
h=5
k=5
b=(7,2)
r/c 1 2 3 4 5 6 7
Exam 1
Exam 2
My Current Work
Compared with A* replanner in our problem, D* performance superior over A* replanner
0
50
100
150
200
250
300
Exam 1 Exam 2
A* repl annerD*
Checking points per replanning
Future work and related work Modify program, make it more robust with more ex
periments, speed up with more modifications. D* Limitation
– D* search from goal configuration, what if there are several goal configurations (it’s common in manipulator motion planning)?
– When the goal object is moving– Current on-line planning methods using A* based techni
ques focus on multi-directional search and parallel planning ([Dominik HENRICH, Christian WURLL and Heinz WÖRN, 1998], etc )
– D* should be adapted for our problems
Future work and related work
– Consult other D*-like replanning algorithms (e.g D* Lite [Koenig, 2002] )
Survey other real-time motion planning techniques in high dimensional C-Space
– Decomposition-based methods ([Kavraki, 2001], [Me
diavilla, 2002], etc)– Probabilistic roadmap based methods(most de
al with static environment)
Future work and related work
Use a more general 3D model to represent manipulator and obstacles
– Hierarchy structure– Tree structure
Future work and related work
– Taxonomy
Future work and related work
Experiment using real robot arm: a challenging work
Images from cameras Model parameters
Computer vision techniquesMotion planning
C-Space Visualization for Teleoperation
Applications of C-Space Visualization– Provide important qualitative information for
mechanical design (E.Sacks, C.Pisula and L.Joskowicz “Visualizing 3D Configuration Spaces for Mechanical Design.” ).
– Evaluation of path planning methods– Teleoperation (I.Ivanisevic and J.Lumelsky “Configuration Spa
ce as a Means for Augmenting Human Performance in Teleoperation Tasks.” IEEE Trans.Syst.Man,Cyber.,vol.30,pp.471-484,Jun.2000).
C-Space Visualization for Teleoperation
It’s easier for humans to handle motion planning problems in C-Space than in W-Space
C-Space Visualization for Teleoperation
Challenges– When the computer which generates C-Space data is
not the same as the computer which receives humans input, C-Space data must be transfered through network
– C-Space data are too large• 161*71*121 for my current implementation
– C-Space data change caused by dynamic environment, etc
– Poor network bandwidth
C-Space Visualization for Teleoperation
So, C-Space data compression is necessary Additional work
C-Space for a Cylinder Object
C-Space Data
3D Models Data
3D Model Data Compression
Framework ：
