Veriﬁcation of RM-GA Performance by Comparing with Full Search 146

7.3 Experimental Results

7.3.4 Veriﬁcation of RM-GA Performance by Comparing with Full Search 146

The optimal solution for each situation of docking performance by comparing GA and full search will be described as shown in Fig. 7.16. The main idea of full search is to calculate the ﬁtness of each point which are 1 mm apart in the entire searching area. Figure 7.16 shows a distribution graph of the ﬁtness function value of GA and the ﬁtness function value at the time point from (i) to (iii). Upper graph indicates the ﬁtness value with respect to time during docking scheme. The selected points in the graph (i), (ii), and (iii) indicate the start recognition, the visual servoing and start docking condition.

As shown in the second row of Fig. 7.16, Tuna-Sand 2 exists on the negative side of y-axis from the target position at the time (i) and (ii), and whereas the recognized current position and the target position almost coincide in (iii). GA ﬁnds the peak that is a combination of position / posturex_H,y_H,z_H,²₁,²₂,²₃ which gives the highest ﬁtness function value in the distribution graph. The ﬁtness distribution between the x-y plane is shown in the third row in Fig. 7.16. The intersection shape of “X” type can be seen in x-y plane in third row, and the distribution peak appears at this intersection point.

This is because of the parallax of the left and right cameras mounted side by side in the y-axis direction and the x-axis direction that is the depth direction from the camera as shown in the second row of Fig. 7.16. As shown in the fourth row of Fig. 7.16, the intersecting shape does not appear on the surface of the distribution on the y-z plane, which means that the left and right parallaxes do not aﬀect the z-axis direction. At the time (i) and (ii), since the peak appears on the right-hand side(yH axis positive direction), it means that the position of the 3D marker is in they_H axis positive direction as shown in Fig.7.16. In order to compensate this position error, the AUV needs to move to the right side. Likewise, in (iii), the peak appears in the left side (y_H axis negative direction).

Therefore, it is necessary to move the AUV to the left side in order to compensate this position error. According to the graphical results, the peak obtained by GA search (GA

estimated) and the full search (Global peak) coincided. The Real-time Multi-step GA can perform with good accuracy even though it is calculated within 33 ms.

Also, in this experiment, Tuna-Sand 2 did not tilt in the xH axis direction or the yH

axis direction due to the balance of buoyancy. Figure 7.17 shows the attitude angles in three axial directions of the target object indicated by quaternion. Since the buoyancy balance did not tilt in the x_H axis direction or the y_H axis direction as described above, the rotation around thex_H axis and around they_H axis in Figs. 7.17 (a) and 7.17 (b) are always kept near to zero. In fact, it can be considered that the position and orientation recognition was performed correctly.

START

TCP/IP initialization TCPFUN_S() TCPFUN_R()

TCP/IP Receive (Structure) ShmStatusOfTS2

TS2 Ready for control diveMode=51?

YES Wait for next TCP/IP Receive Sleep(100) NO Timer2 (1[s])

TCP/IP SEND (Structure) COMtoTS2 Maintain connection TS2_MODE_CHANGE(50);

GA recognition MultiStepGA() GA roop flag RecvLoop++

Save recognition results M1.Dx=(z_d-(zsft+ee_z))*0.5;

M1.Dy=(x_d-(xsft+ee_x))*0.4;

M1.Dz=(y_d-(ysft+ee_y))*1.0;

M1.Dd=(epsi2_d-(n2+ee_epsi2))*0.4;

GAPC initialization(Image Board) Open5523()

Ready for V.S.?

KGMODE==0 ? YES

4D.o.F. V.S.

Judge_ALL4(xsft,ysft,zsft,n2);

(Details in sub-flow chart) Changing docking mode

KGMODE=1;

Ready for Docking?

KGMODE==1 ? YES

Changing TS2 control mode TS2_MODE_CHANGE(52);

Sleep(130);

DOCKING TS2_GO_XY(0.20,0);

Changing docking mode KGMODE=2;

Docking success?

KGMODE==2 ? RecvLoop larger than 600?

(20s passed?) YES

Save Data (experiment results) Changing docking mode

KGMODE=3;

Ready for Go Back?

KGMODE==3?

RecvLoop larger than 300?

(10s passed?)

YES

TS2 Go Back TS2_GO_XY(-0.20,0);

Changing docking mode KGMODE=4;

End processing fclose() etc.

END YES

Changing TS2 control mode TS2_MODE_CHANGE(60);

Changing docking mode KGMODE=9;

TCP/IP Receive (Structure) ShmStatusOfTS2

Ready for Close?

KGMODE==4?

RecvLoop larger than 300?

(10s passed?) NO Checking conversation

with Tuna-Sand 2 1

Visual servoing step

Docking step

Returning step

1 2 3 4

5 6 7 8

Ready with connection (TS2 to GA-PC) Keep connection (GA-PC to TS2) Report the current status (TS2 to GA-PC)

Visual servoing commands (see sub flowchart in Fig. ) (GA-PC to TS2)

Change mode (52) (GA-PC to TS2)

Go ahead in x-direction for 0.2 m (GA-PC to TS2) Go back in x-direction for 0.2 m (GA-PC to TS2) Report mission complete (GA-PC to TS2)

Fig. 7.8: Detailed ﬂowchart of GA-PC.

START

4D.o.F. V.S.

Judge_ALL4(xsft,ysft,zsft,n2);

(sub-flow chart)

Judgment of Fitness fitness[0]>=0.5?

YES

4 D.o.F. All Clear？？？？

|xd-xt|,|yd-yt|,|zd-zt|<30[mm]

|n2d-n2t|<3[deg]

Time for TCP/IP SEND?

RecvLoop%4==0?

TS2 control mode ready for change?

RecvLoop>=MODECHANGETIMER+90 FLAG_XY=true YES

Change TS2 control mode TS2_MODE_CHANGE(52);

FLAG Changing FLAG_Z=true;

FLAG_HEADING=true;

FLAG_XY=false;

YES

TS2 STOP TS2_XY_STOP();

Change docking mode KGMODE=1;

Z-position Clear？？？？

|zd-zt|<30[mm]

YES

TS2 control mode ready for change?

RecvLoop>=MODECHANGETIMER+90 FLAG_Z=true

Change TS2 control mode TS2_MODE_CHANGE(51);

FLAG Changing FLAG_Z=false;

FLAG_HEADING=true;

FLAG_XY=true;

YES

TS2 Z-position MOVE TS2_GO_Z(-1*M1.Dz/1000.0);

XY-position Clear？？？？

|xd-xt|,|yd-yt|<30[mm]

YES TS2 control mode ready for change?

RecvLoop>=MODECHANGETIMER+90 FLAG_XY=true

Change TS2 control mode TS2_MODE_CHANGE(52);

FLAG Changing FLAG_Z=true;

FLAG_HEADING=true;

FLAG_XY=false;

YES

TS2 XY-position MOVE TS2_GO_XY(-1*M1.Dx/1000.0,

-1*M1.Dy/1000.0);

n2(heading) clear？？？？

|n2d-n2t|<3[deg]

YES

TS2 control mode ready for change?

RecvLoop>=MODECHANGETIMER+90 FLAG_HEADING=true

Change TS2 control mode TS2_MODE_CHANGE(53);

FLAG Changing FLAG_Z=true;

FLAG_HEADING=false;

FLAG_XY=true;

YES

TS2 n2(heading) TURN TS2_TURN(-1*(quo_to_deg(M1.Dd)));

END

Checking ready for docking

Visual servoing in z-direction

Visual servoing in x and y-direction

Visual servoing in heading rotation

9 10

Send desired movement in z-direction (GA-PC to TS2)

12 13

Go to docking step (GA-PC to TS2) 10

Change mode (52) (GA-PC to TS2)

Send desired movement in x,y -direction (GA-PC to TS2)

15 16

Change mode (51) (GA-PC to TS2)

Change mode (52) (GA-PC to TS2)

Change mode (53) (GA-PC to TS2) Send desired heading angle (GA-PC to TS2)

Fig. 7.9: Subﬂowchart of Visual Servoing in Fig.7.8

Tuna-Sand2 remote client

Tuna-Sand2 Docking Station

Tuna-Sand2 Monitoring PC

GA-PC remote client

GA-PC

Fig. 7.10: Appearance of experimental environment to conduct automatic docking using AUV Tuna-Sand 2 in water pool established in University of Tokyo.

Σ_ு ݔ

1 2

Σ_ு ݔ

ݕ Σ_ு

ݕ Docking hole 3D marker

Fig. 7.11: Layout of 3 times docking pretests using only visual servoing. The number in circle °1 - °3 represents for docking iteration. After each docking completion, the AUV moves to the posse as shown in this ﬁgure for next docking. Experimental results are shown in Fig.7.12 and corresponding left and right camera images are illustrated in Fig.

7.13.

0 0.2 0.4 0.6 0.8 1 1.2 1.4

0 50 100 150 200 250 300

-400 -300 -200 -100 0 100 200 300

0 50 100 150 200 250 300

-200 -150 -100 -50 0 50 100

0 50 100 150 200 250 300

Time [s]

Fitness valuePosition along x-axis direction [mm]Position along y-axis direction [mm]Position along z-axis direction [mm]

-20 -15 -10 -5 0 5 10

0 50 100 150 200 250 300

Rotation around z-axis [deg]

Time [s]

0 200 400 600 800 1000

0 50 100 150 200 250 300

1 2 3

(a)

(b)

(c)

(d)

(e)

V.S M.F M.B V.S M.F M.B V.S M.F

Desired position Estimated position Error allowance V.S Visual servoing M.F Moving forward

(Docking) M.B Moving backward

A B C D E F G H

A1 A2

Giving offset

Giving offset A1

Giving offset

Fig. 7.12: 3 times docking pretests using only visual servoing: (a) Fitness value, (b) position along in x-direction, (c) position along y-axis direction, (d) position along z-axis direction, and (e) rotation around z-axis. The number in each circle in each subﬁgure represents docking number. Note the AUV moves to the poses as shown in Fig. 7.11 after each docking completion. The initial poses for docking number°2 and docking number°3

are set by giving oﬀset especially in y-axis direction after moving back process as shown by times A1 and A2 (see Fig.7.12 (c): The 3D marker is in the negative position of y-axis direction of Σ_H for the docking number °2 as shown in Fig. 7.11, and the 3D marker is in the positive position of y-axis direction of Σ_H for the docking number °³ as shown in Fig. 7.11). Left and right camera images taken at the times “A”-“H” are shown in Fig.

7.13.

H C

Fig. 7.13: Left and right camera images corresponding to the times A-H in Fig. 7.12. It can be conﬁrmed that the AUV could perform docking by visual servoing in pretests by checking the left and right images taken at the times “A”-“H”. Subﬁgures 7.13 (“A,” “D,”

“G”) show the condition in which the AUV is stable and ready of docking. Subﬁgures

(a) (b) (c) (d)

(e) (g) (f)

(i)

AUV

Docking station

(j)

(a) (b) (c)

(d) (e)

(f)

(g) (i)

Σ_W

Left camera Right camera

Appearance of AUV

Front of docking hole

Side view of docking hole

Left camera Right camera

Front of docking hole

Side view of docking hole Appearance of AUV

Appearance of AUV Side view of docking hole Front of docking hole Right camera Left camera

Left camera Right camera

Front of docking hole

Side view of docking hole Appearance of AUV

Approaching Step

Visual Servoing Step

Docking Step

(h-1)~(h-4)

(h-1) (h-2)

(h-4) (h-3)

Fig. 7.14: Desired trajectory of Sand 2 in experiment and appearance of Tuna-Sand2: From (a) to (i), appearance of experiments are shown during movement of tra-jectory which is illustrated in (j). Terminal positions of those tratra-jectory are described in Table 2.

0 100 200 300 400 500 600 700 800

0 20 40 60 80 100 -200

-150 -100 -50 0 50 100 150 200 250

0 20 40 60 80 100

-120 -100 -80 -60 -40 -20 0 20 40 60

0 20 40 60 80 100

-30 -20 -10 0 10 20 30

0 20 40 60 80 100

0 0.2 0.4 0.6 0.8 1 1.2

0 20 40 60 80 100

Fitness valuePosition in x-direction [mm]Position in z-direction [mm] Position in y-direction [mm]Rotation around in z-axis [deg]

Time [s]

Time [s] Time [s]

Time [s]

Visual servoing Docking

Visual servoing Docking Visual servoing

Docking Docking End Visual servoing Start

(a)

(c)

(e)

(b)

(d)

(f)

Visual servoing Docking

Visual servoing Docking acceptable

range

acceptable range

acceptable range 49

50 51 52 53

0 20 40 60 80 100

Control mode

Time [s]

Docking Visual servoing

Fig. 7.15: Measured trajectory of Tuna-Sand2 by GA-PC with RM-GA: Fitness value is plotted in (a). When ﬁtness value is low, reliability for measured position is lower. AUV positions in x,y and z axes are plotted in (c), (d) and (e) respectively. Also, rotation angle around z axis is plotted in (f). When those control variables were converged in acceptable range on visual servo state at about 83 second, docking behavior is executed.

y-axis[mm]

z-axis[mm]

y-axis[mm]

z-axis[mm]

y-axis[mm]

z-axis[mm]

Time[s]

(ii)

0 0.2 0.4 0.6 0.8 1 1.2

0 20 40 60 80 100 120

Fitness value

(i) (iii)

(iii)

y-axis[mm]

x-axis[mm]

y-axis[mm]

x-axis[mm]

y-axis[mm]

x-axis[mm]600 Desired pose 78[mm

]

x=−172[mm]

y=181[mm]

Actual pose

x=428[mm]

y=103[mm]

(i) (ii)

Actual pose

600mm

Desired pose 78[mm]

x=−44[mm]

y=20[mm]

y=98[mm]

x=556[mm] 600mm

Desired pose 78[mm]

x=10[mm]

y=−35[mm]

Actual pose Σ

x=610[mm]

113[mm]

Global peek GA estimated Global peek GA estimated Global peek GA estimated

Fig. 7.16: Fitness distribution graph for each situation of docking performance: Upper graph indicates ﬁtness value with respect to time during docking scheme. (i), (ii) and (iii) in this graph mean instants which are picked up for drawing ﬁgures in lower side. Pose relations between AUV and 3D maker in those instants (i, ii, iii) are illustrated in second row. Distribution map with Y-X axes and a pose estimated by proposed GA algorithms are drawn in third row, and that with Y-Z axes and estimated pose are also drawn in ﬁnal row.

-0.15 -0.1 -0.05 0 0.05 0.1 0.15

0 20 40 60 80 100

-0.15 -0.1 -0.05 0 0.05 0.1 0.15

0 20 40 60 80 100

Time [s]

Rotation around in x-axisRotation around in y-axis

-0.15 -0.1 -0.05 0 0.05 0.1 0.15

0 20 40 60 80 100

Rotation around in z-axis

Time [s]

Visual servoing Docking

Visual servoing Docking Docking End Visual servoing Start

(a)

(b)

(c)

Fig. 7.17: Quotanion (a)²1 aroundx_H , (b)²2 aroundy_H , (c)²3 aroundz_H are illustrated.

It can be conﬁrmed that rotation angle aroundx_H andy_H are stable that are out of control targets.

7.4 Summary

This chapter presents the docking experiment for sea bottom underwater battery recharg-ing application in an indoor pool by usrecharg-ing Tuna-Sand 2 AUV. Tuna-Sand 2 was facilitated with dual-eye vision-based 3D pose estimation system. Tuna-Sand 2 AUV approached to the docking station according to the desired trajectory using other navigation sensors such as DVL and depth sensor. In ﬁnal docking step, AUV automatically performs the docking operation only using vision information from dual-eye cameras. Experimental results conﬁrmed the performance of 3D recognition and docking. In future work, the proposed system will be developed for real sea environment, and sea trail docking of AUV using proposed system will be conducted for underwater battery recharging.

Chapter 8 8. Improvement of 3D Pose

Estimation by Using Active Marker in Turbid and Day/Night

Environment

In our previous researches, the passive marker was used to conduct the experiment. Since an intended application of the proposed docking approach is the battery recharging in the deep-sea bottom, completely dark and turbid environment should be considered as an in-evitable condition. Since lighting passive 3D marker by light from vehicles in turbid water environment results in the images taken by video cameras set on the vehicle being looked wholly white, some new idea seems to be required. To overcome the above situation, we introduce newly designed and constructed lighting marker (active 3D marker) that has LEDs inside in this section. The improvement of the proposed system was conﬁrmed by conducting 3D pose estimation using dynamic images against turbidity and changing illu-mination in a simulated pool as shown in Fig. 8.1. Furthermore, sea docking experiment has also been executed to verify the practicality of the active marker. The experimental results have conﬁrmed the eﬀectiveness of the proposed docking system against turbidity

Fig. 8.1: The experimental set-up in the case of night-time condition: (a) ROV and an active marker in the simulated pool, (b) left and right cameras images from which the dotted circles represent the real-time estimated pose by the proposed system.

and illumination variation.

8.1 Docking Strategy

The continuously repeated docking experiments were conducted in the simulated pool and the actual sea. The design of the docking procedure as shown in Fig. 8.2 is discussed in this section with the detailed ﬂowchart. The pool docking strategy is same as the sea docking strategy except the manual approach and the stay step that were developed after pool test. After performing the completion of docking step in the case of pool test, the ROV stops the visual servoing and stores the data ﬁles from memory into the hard disk.

After storing the data, the ROV performs the launching step by visual servoing.

Generally, the various sensors are used for long navigation until the target is in the ﬁeld of view of the camera in ROV. In the sea docking experiment, the manual operation is necessary to appear the image of the target object in the ﬁeld of view of ROV’s cameras.

The marker detection is deﬁned by the control threshold ﬁtness value experimentally.

Firstly, the ROV approaches the docking station by manually. The ﬁtness value recognized by RM-GA is compared with the control threshold ﬁtness value. If the recognized ﬁtness value by RM-GA is lower than the control ﬁtness value, it means that the ROV could not detect the 3D marker. When the recognized ﬁtness value is higher than the control

ﬁtness value, the proposed system switches from manual to automatic control of the visual servoing state. In the visual servoing state, the ROV is controlled by using the estimated relative pose of the vehicle with respect to the 3D marker. In this state, the distance between the ROV and the 3D marker is about 600 mm. Three main steps are performed in the sea docking experiment after the approaching step and visual servoing step.

Start

Fitness≥

threshold?

Visual servoing

Within allowance error level?

Docking (Decrease distance between

ROV and target) Docking

finish ? Count docking successful times Input voltage to ROV

for forward Data storing

Satisfy desired docking

times ?

Check Fitness

Fitness≥

threshold?

Count continuous time Continuous

5s?

Reset time Launching

(Increase distance between ROV and

target) Launching

finish ?

End Mode = Docking?

Changing mode to Docking No

No No

Yes

Yes Yes

Docking step

Launching step

Stay step

Changing mode to Launching Data

storing

Data storing Approached by manually

until the object is in the field of view of the cameras Stop

Fig. 8.2: Flowchart of the docking strategy. In pool test, the manual approach and the stay step were not performed. After completion of docking step, the ROV stops visual servoing and stores the data ﬁles from memory into the hard disk.

ドキュメント内 Visual Docking of Underwater Vehicle (ページ 173-189)