4.2. Implementation of mixed optical flow algorithm
4.2.6. Correlation constraint
4.2.6. Correlation constraints
The correlation constraints [27] [28] of optical flow is estimated in terms of the relation of a pixel (x,y) in one frame to its next frame. It is assumed that the pixel motion in two successive frames of instant t and (t+δt) will retain in range of -N<u<N and –N<v<N, where N is the largest possible displacement of u and v, and (u,v) is the optical flow vector.
The optical flow is determined from the correlation match of the patch represented by (2n+1) x (2n+1) pixels centered at (x,y), out of (2N+1) x (2N+1) possible displacements. The correlation match equation is given by [28]:
Figure 4.9 correlation constraint algorithms
where -N<u<N and –N<v<N and SSD denotes the sum of squared difference between the patch and a (2n+1) x (2n+1) window around each pixel in (2N+1) x (2N+1) possible displacement. Figure 4.9 shown the correlation constraint algorithm based on (4.4).
From the (4.4), we can see if the search area is increasing it will greatly increase the computation load. In order to reduce computational complexity and still hold acceptable estimation accuracy, the mixed optical algorithm proposes to include the brightness constraint [26] that already mention in previous section combine with this correlation constraint. So in the mixed optical flow algorithm, it is using the principle of correlation match of correlation-based (4.4) technique as the kernel, and adds the brightness constraint (4.2) of differential-based technique to condense the search area of correlation match.
So the implementation of the correlation constraint in mixed optical flow algorithm is straight forward. Firstly, we find the motion constraint line which is based on the (4.2). After we find the motion constraint line, we then perform SSD (sum of squared difference) based
v
on the (4.4), to find the displacement of current pixel in the possibly search area that lying the motion constraint line. After performing SSD search in all pixels that lying in the motion
constraint line, then we look for the minimum value of all SSD value. The displacement of
the pixel which is motion field vector (u,v) is the minimum value of the SSD value. After finding for one motion field vector, we repeated this step until the entire pixel in the one image frame already done. In our case, we find the motion field vector for 160 x 120.
4.2.7. Create Depth Histogram
Using the obtained optical flow field, we can calculate the scene depth and time-to-collision (TTC). This depth information of object is then input to an obstacle avoidance algorithm which locates in the navigation layer in our behavior-based architecture.
As shown in figure 4.10, the camera focuses to the positive Z-axis direction. Let the origin of the world coordinate Oc locate at the camera center. For both the cases that the camera moves toward the object with a velocity (0,0, Wc) or the object moves toward the camera with a velocity (0,0, Wc), the TTC or depth can be calculated using the equation below [29]:
Z D
d = f … (4.5)
where d is distance between focus of expansion (FOE) to a point p int the image plane (see Fig. 4.10). f is focal length, D is the distance between an interest point P and the Z-axis, Z is the depth. Differentiating with respect to time and dividing by d, we can get:
still
Figure 4.10 (a) The camera moves toward the object.
(b) The object moves toward the camera.
The implementation of computing TTC is also straight forward using (4.6), which is for the condition as the object is still. By modifying the (4.6) with our real implementation in our program we obtain the computation formula for TTC:
t
where Est_v_pos is the position (y-axis) for point p of the motion flow field in the image plane at T (see figure 4.10 (b)). Est_v_pos is subtracted with 60 (the coordinate center of y-axis with 160x120 res.) and then absolute the result value to get the d value (see figure 4.10 (b)). Meanwhile to compute V (motion field speed) we need ∆y(see figure 4.10 (b)) or in this case Esv_v (the motion field compute by mixed optical flow) and the sampling time of the image which equivalent with frame rate per second.
After we get the TTC (τ ), to compute the Z (depth information) we just multiplying the TTC (τ ) with Wc (robot speed) which is shown by Eq.4.8.
4.2.8. Safety distribution histogram
In order to establish a representation of the environment configuration, we transformed the calculated depth to a safety distribution histogram. This histogram is established from a mapping from a 3D space to 2D ZX-plane using the depth information to represent the obstacle in the environment.
Press
Press Press
Z
Y
Safe area X
Z
Y
X Safe area
Dangerous ara Depth
Figure 4.11 the representation of 3D space to 2D ZX-plane
Figure 4.11 illustrates the idea of safety histogram. From this figure, we see that the transformation of 3D space to 2D ZX plane can be done by pressing the y-axis value, pressing the y-axis means that the depth (Z) of the ZX histogram will depend on the y-axis value. This z-axis value is chosen the minimum value from the y-axis. So the bigger value of the z, the safer the location. On the other hand, a smaller value of z means that there are obstacles in front of the robot.
4.3. Experiment results with mixed optical flow algorithm
In this section, we show experiment results of implementation mixed optical flow algorithm and the computation of safety histogram distribution. In figure 4.12 (a) shows the motion field vector in uv space and figure 4.12 (b) shows the safety histogram distribution that divide in to the 7 region. The width of the safety histogram distribution is 160 pixels;
with each region of the safety histogram distribution is 20 pixels.
Figure 4.12 Optical flow field [a] and histogram of TTC (7 regions) [b]
Optical flow field (a)
Histogram of TTC (7 region) (b)
160
0 pixels
Next, we divide the seven regions into three sides which are left side, front side and right side. Figure 4.13 (b) shows the three side of safety histogram distribution that derived from seven regions and figure 4.13 (a) shows its motion field vector. By using these three regions, the robot can easily and fast to determine the safety location to avoid obstacle, like in the case of figure 4.13 (b), the left side of safety histogram distribution is bigger than the right side so the robot will turn left to avoid the obstacle.
The implementation of mixed optical flow in embedded image processing platform took 0.2 second or 5 Hz to perform one calculation of obstacle avoidance which in the previous research [14] it took 0.9 second or approx. 1 Hz.
Figure 4.13 Optical flow field [a] and histogram of TTC (3 regions) [b]
Optical flow field (a)
Left side Front side Right side
Histogram of TTC (7 region) (b)
150 10 50 110
pixels
5 . Experimental Results
5.1. The experimental mobile robot
The experimental mobile robot is equipped with a manipulator. Below is the description of the mobile robot used in experiments:
z Two independent drive wheels (Foot motors) and two casters for mobility.
z A 3 DOF (Degree of freedom) manipulator for grasping.
z Two 12 V and two 6 V batteries.
z A Pentium III 1 GHZ IPC (Industrial Personal Computer) for main system control, using Windows XP as the operating system.
z 12 to 5 V DC – DC converter for power regulation.
z Two DSP motion boards for controlling Foot motor and Head motor, and two DSP motion boards for controlling 3DOF manipulator
z 4 COM ports RS232 interface that provide communication between the embedded image processing and DSP motion boards or IPC.
z Embedded image processing platform that using DSK6416 and CMOS sensor board for controlling the 3DOF manipulator.
Figure 5.1 Mobile manipulator (H2) platforms
5.2. Experiment of grasping
The goal of this experiment is to verify the mobile manipulator can locate barcode-like object using the proposed visual servoing method.
Figure 5.2 the robot is faced to the object that put in the top of chair
115 cm 50 cm
Object that labeled with barcode-like
Gripper and CMOS sensor
Industrial PC (IPC) Power supply
(DC-DC converter)
Battery
Foot & Head Motion Control
Battery
Arm motion control Embedded Imaging board 4 COM Ports I/F
(COM1,2,3 & 4) CMOS Sensor
In the experiment the object is put in certain place, which in this experiment is put in top of a chair. Figure 5.2 show the robot faced to the object. The object which labeled with the barcode-like feature is put at a distance about 115 cm. The object itself is put in the top of chair which height to the ground is 50 cm.
In the first stage, the mobile manipulator robot scan its environment by turning left about 30° (see figure 5.3 (a)) and right about 30° (see figure 5.3 (b)) to trying to locate the
barcode-like object in front of the mobile manipulator.
Figure 5.3 (a) the mobile robot is turned left about 30°
Figure 5.3 (b) the mobile robot is turned right about 30°
The robot is tried to scan the environment for several times, and after the mobile manipulator robot located the object in front of it, it then changed its orientation and speed according to the barcode-like direction and distance. Figure 5.4 shows the mobile manipulator robot locates the object and change its orientation and speed according to the object orientation.
Next, the mobile manipulator robot is approaching the object which adaptively change the orientation and speed according to the distance and the orientation which compute from the feature of the barcode-like that get from visual sensory feedback. (See figure 5.5 (a) and figure 5.5 (b)).
Figure 5.4 the robot change its orientation and speed after it locates the object in front of it
Figure 5.5 (a) the mobile manipulator is approaching the object
Figure 5.5 (b) the mobile manipulator is approaching the object (cont’d)
Figure 5.6 (a) the object is get into the area of the gripper
Figure 5.6 (b) the mobile manipulator grasp the object
After the barcode-like object gets into the gripper area, (see figure 5.6 (a)), the mobile manipulator slowly approaches the object (see figure 5.6 (a)). Lastly, when the barcode-like distance approximately 5 cm, the mobile manipulator grasps the object and stops (see figure 5.6 (b)).
5.3. Experiment of grasping of an object from a person
The goal of this experiment is to show how the mobile manipulator robot can locate barcode-like object as the object is hold by human. A human is standing while holding the object which is labeled by the barcode-like. The mobile manipulator is faced to the object
with distance about 90 cm. The object is hold by human with the height of the object to the ground about 55 cm. Figure 5.7 show the mobile manipulator faced the object that hold by the human.
Similar with the previous experiment result, the mobile manipulator is tried to scan its environment by turning left about 30° (see figure 5.8 (a)) and by the time the robot is turn
back to the forward direction, its locate the object that hold by the human (see figure 5.8 (b))
Figure 5.7 the mobile manipulator robot is faced to the human that hold the object
55 cm
90 cm
Object that labeled with barcode-like
Gripper and
CMOS sensor
Figure 5.8 (a) the robot is turned left about 30°
Figure 5.8 (b) the robot has located the object that holds by human
Next, the mobile manipulation is approaching to the object, (see figure 5.9 (a) and (b)), and when the object already gets in to the gripper, it grasps the object (see figure 5.10 (a) and (b)).
Figure 5.9 (a) the robot is approaching the object Figure 5.9 (b) the robot is approaching [cont’d]
Figure 5.10 (a) the object is in the gripper area Figure 5.10 (b) the gripper grasped the object The representation of this experimental in the qualitative data is shown as below:
a. The mobile manipulator robot trajectory
Figure 5.11 the trajectory of the mobile manipulator
Figure 5.11 has shown the recorded trajectory of the mobile manipulator. This trajectory a
b
c d
e
f
is drawn using encoder pulses from the motor. The movement of the robot in x-axis is recorded in X (cm), meanwhile the movement of the robot in y-axis is recorded in Y (cm).
In the figure 5.11, we divided the trajectory into five parts. In the part (a) the robot is starting from its starting point and is moving forward. Because the mobile manipulator robot can not find the object, the robot is trying to scan its environment by turning left side about 30 degree (part b). After turning left about 30 degree, the robot is then back to the forward position (part c), by the time the robot is turned to the forward position. The robot locates the object. That is why in part (d), the robot change its orientation, and then facing forward to approach the object.
In the part (e), the robot is approaching the object approx 30 – 40 cm, in the last part (f) the robot is nearly the object that the distance about approx 0 - 20 cm. In this distance the velocity of the robot is reduced, and when the object get into the gripper it will grasped the object.
b. The orientation of the robot
The orientation of the robot while approaching the object can be seen in the figure 5.12. In this figure, it plots between the orientation of the robot in radian and time in second. The total time that is using to grasp the object is approx. 30 second. In the part (a), the robot is trying to scan its environment by scanning to the minus direction according to the graphic of the figure 5.12 which turning about 5 seconds. (From 10 second to 15 second).
Next, because the robot does not find the object it then turn to the respectively direction
back to the forward direction (part b). By the time, the robot want to back to the forward direction, the robot find the object, it can see in the part (c) where the robot only turn for a half of its direction. Next in the part (d), the robot already locates the object, and changing its orientation and direction. As we can see that in the part (d), there is an oscillation to make the robot stable at its orientation according to the object orientation. Later in the part (e), the robot is nearly from the object and trying to get the object into its gripper. As we can see that the trajectory in the part (e) is not changed and stable between -0.15 and -0.1.
Figure 5.12 the robot orientation (rad - sec)
5.4. Experiment of obstacle avoidance
The goal of this experiment is to show how the mobile robot can avoid objects in front of it. If there is an obstacle in front of the robot, the robot will try to avoid the obstacle by determining the safety location between the left side and the right. After selection of the safety
a
b
c
d
e
location, the robot then turns about 30 degree to avoid the obstacle.
Int the navigation mode the mobile manipulator robot works using the navigation layer in the behavior-based architecture. In the experiment, the mobile manipulator robot is free-running, if there is an obstacle in front of the mobile manipulator, the robot will try to find the safe location. Once the robot finds the safe location, the robot will change its direction and orientation to the safe location so it can avoid the obstacle safely.
Figure 5.13 illustrate the trajectory of this experiment. Point (a) of figure 5.13, the robot faces the person approximately about 30 cm (see also figure 5.14 (a)). The robot then tries to find the safe location, once it finds the safe location the robot starting to turn about 30 degree to the safe location (see figure 5.14 (b)). At point (a), the robot chooses to turn left about 30 degrees according to the result of safe location estimation.
Point (b) of figure 5.13, the robot faces the person and the wall beside the human (see figure 5.14 (c)). After calculating the safe location, the robot successfully chooses (see figure 5.14 (d)). Similar with points (a) and (b), at points (c) and (d), the robot finds the safe location to avoid the obstacle safely. Figure 5.15 illustrate the recorded trajectory of the robot in this experiment.
60 cm
300
120 cm
Start point
End point 30 cm
-300
300
-300
270 cm 100 cm
a
b
c
c
d
c
Figure 5.13 the trajectory approach of the mobile navigation experiment
Figure 5.14 (a) Figure 5.14 (b)
Figure 5.14 (c) Figure 5.14 (d)
Figure 5.14 (e) Figure 5.14 (f)
Figure 5.14 (g) Figure 5.14 (h)
Figure 5.15 the trajectory of the mobile navigation mode a
b
c
d e
6 . Conclusions and Future Work
6.1. Conclusions
This thesis presents a design and implementation of hardware and software to solve a mobile manipulator problem which is for navigation and grasping. The hardware implementation develops an embedded image processing platform which is stand-alone and can communicate directly to other module like DSP motion board and IPC. The embedded image processing consists of CMOS image sensor and DSK6416 as the main processing board. The result of this embedded image processing platform can be used for acquiring image for 15 fps and 30 fps.
The experiment results of the visual servoing algorithm have shown that the mobile manipulator can locate a barcode-like object, approaching and grasp the object. The performance of this algorithm implement in embedded image processing platform to search the barcode-like object took 0.0667 second or 15 Hz.
Meanwhile, the experiment results of the mixed optical flow algorithm can avoid a person in real time. The performance of this algorithm implement in embedded image processing platform for one calculation of obstacle avoidance took 0.2 second or 5 Hz.
6.2. Future work
The proposed algorithm only has shown the use of the grasping mode and navigation mode independently without combining these two modes at the same time. In the future, by combining these two modes at the same time, the mobile manipulator can autonomously navigate it while the barcode-like object hasn’t detected. And if the barcode-like object already detect, the mobile manipulator can approach and to grasp it.
In this work we just control the speed and the orientation of the mobile robot. The proposed algorithm visual servoing algorithm needs to improve by controlling the manipulator as well as the robot autonomously.
References
[1] D. Kragic and H. I. Christensen, “Robust visual servoing,” in The International Journal of Robotics Research, vol. 22, pp. 923-939, Oct. – Nov. 2003.
[2] L. Petterson, D. Austin, and D. Kragic, ”High-level control of a mobile manipulator for door opening,” in Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, vol. 3, 31 Oct.-5 Nov. 2000, pp. 2333-2338.
[3] O. Khatib, “Mobile manipulation: The robotic assistant,” in Robotics and Autonomous System, vol.26, pp. 175-183, February 1999.
[4] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” in the International Journal of Robotics Research, vol.5, pp. 90-98, 1986.
[5] K. Nagatani and S. Yuta, “An Experiment on opening-door-behavior by an autonomous mobile robot with a manipulator,” in Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, vol. 2, Aug. 1995, pp. 45-50.
[6] K-T Song and T-Z Wu, ”Visual servo control of a mobile manipulator using one-dimensional windows,” in Proc. IEEE IECON’99 Industrial Electronics Society, vol.2, 29 Nov. – 3 Dec. 1999, pp. 686 – 691.
[7] K. Hirai, M. Hirose, Y. Haikawa, and T. Takenaka, “The development of the Honda Humanoid Robot”, in Proc. of the International Conference on Robotics and Automation,
1998, pp.1321-1326
[8] J. H. Connel, “A behavior-based arm controller,” in IEEE transaction on Robotics and Automation, vol. 5, pp. 784 – 791, Dec. 1999,
[9] R. A. Brooks, “A robust layered control system for a mobile robot,” in IEEE journal of Robotics and Automation, vol. RA-2, pp. 14 – 23., March 1986,
[10] Z. Wasik and A. Saffioti, “A fuzzy behavior-based control system for manipulation,” in IEEE/RSJ International Conference on Intelligent Robots and System, vol.2, 30 Sept. – 5
Oct. 2002, pp. 1596 – 1601.
[11] Z. Wasik and A. Saffioti, “A hierarchical behavior-based approach to manipulation tasks,” in Proc. IEEE International Conference on Robotics & Automation, vol. 2, Sept.
2003, pp. 2780 – 2785.
[12] S. Hutchinson, G. D. Hager and P. I. Coke, “A tutorial on visual servo control,” in IEEE transaction on Robotics and Automation, vol. 12, pp. 651-670, Oct. 1996.
[13] C. E. Smith, “Visually guided manipulation of static and moving objects,” Ph.D. Thesis, Univ. Minnesota, 1996
[14] K.T. Song and J. H. Huang, “Fast optical flow estimation and its application to real-time obstacle avoidance,” in Proc. IEEE International Conference on Robotics and Automation, vol.3, 2001, pp. 2891 – 2896.
[15] Y. Yoon, G. N. D. Souza and C. Kak, “Real-time tracking and pose estimation for
industrial geometric features,” in Proc. IEEE International Conference on Robotics and Automation, vol.2, Sept. 2003, pp.3473 – 3478.
[16] M. Kaneko, M. Kessler and A. Weigl, “Capturing pyramidal-like objects,” in Proc. IEEE International Conference on Robotics and Automation, vol. 2, May 1998, pp. 3619 –
3624.
[17] C. A. Richards and N. P. Papanikolopoulus, “The automatic detection and visual tracking of moving objects by eye-in-hand robotics system,” in Proc. IEEE/RSJ Int. Conf.
Intelligent Robots and Systems, vol.3, Aug. 1995, pp. 228 - 233.
[18] ICM205B, Datasheet, “VGA/QVGA CMOS image sensor with digital YUV output,” Oct 2002, IC-MEDIA Corp.
[19] TMS320C6000, Reference Guide, “TMSC6000 Peripherals Reference Guide,
“ Literature Number: SPRU190D, December 2002, Texas Instrument. Inc.
[20] TMS32C6000, Reference Guide, “TMSC6000 McBSP: UART,” Literature Number:
SPRA633B, May 2004, Texas Instrument Inc.
[21] AL422B Data sheet, Data sheet, 2004, Aver Logic Inc
[22] Altera University design laboratory package, User guide, 2001, Altera Corp.
[23] TMS320C6000, Application Report, “EMIF to External FIFO interface,” Literature Number: SPRA543, May 1999, Texas Instrument Inc.
[24] TMS320C6000, Application Report, “Applications using the TMS320C6000 Enhanced
DMA,” Literature Number: SPRA636A, October 2001, Texas Instrument Inc.
[25] R. C. Gonzales and R. E. Woods, Digital Image Processing, Prentice Hall, 2002.
[26] B. K. P. Horn and B. Schunck, “Determining optical flow,” Artificial Intelligence, vol. 17, pp. 185-203, 1981.
[27] T. Camus, “Real-time Quantized Optical Flow,” in Proceedings of 1995 Computer
[27] T. Camus, “Real-time Quantized Optical Flow,” in Proceedings of 1995 Computer