where k is the increment index and εk is a positive scaling .... System Architecture, where U = [urw]T . More information regarding feature extraction and pose ...
2008 IEEE/RSJ International Conference on Intelligent Robots and Systems Acropolis Convention Center Nice, France, Sept, 22-26, 2008
Visual Servo Control of an Underwater Vehicle Using a Laser Vision System George C. Karras and Kostas J. Kyriakopoulos Abstract— This paper describes a position-based visual servo control scheme designed for an underwater vehicle. The methodology proposes a path planning technique, which guarantees that a flat target is kept in the camera optical field, while the vehicle avoids collision with the surface the target lays on. The vehicle pose (position and orientation) with respect to the target is obtained using a Laser Vision System (LVS). The LVS projects two laser dots in the image plane while it tracks the target using computer vision algorithms. The position of each laser dot in the image plane is directly related to the distance between the vehicle and the surface the target is located. The path planning strategy is based on the Artificial Potential Field method (APF). The attractive part of the APF is responsible for minimizing the error between the current vehicle position and the desired. The repulsive part of the APF restricts the target inside the camera optical field while keeps away the laser dots from image regions related to small distances between the vehicle and the surface the target is located. The steering control of the vehicle is achieved by feeding the computed points of the path planning into a Cartesian kinematic controller, which was slightly modified for the needs of the methodology. The overall efficiency of the system, was proved through an extensive experimental procedure, using a small Remotely Operated Vehicle (ROV) in a test tank.
I. INTRODUCTION The use of underwater robotic vehicles in oceanic surveys, inspections, pipe and cable tracking, has been established in the field of marine engineering since many years ago. An underwater vehicle may carry a variety of inertial or acoustic sensors such as Inertial Navigation Systems (INS), inclinometers, Doppler Velocity Log sensors (DVL), or even Ultra Short Baseline sensors (USBL). The above sensors, especially when combined together, can provide a satisfactory state estimation in long travel and high standard trajectory tracking missions. However, underwater vehicles usually operate in circumstances demanding dexterous operations and delicate motions, such as the inspection of a ship hull or a propeller shaft. In such cases optical devices like cameras can provide a sufficient position estimation, due to the directness of information that are able to offer. This fact necessitates the use of visual servo control in station keeping and delicate point to point underwater missions. In case of an underwater vehicle, vision data is acquired from a camera mounted directly on the robot, having a so-called eye-in-hand camera system. Regarding visual servo control, three main categories can be defined. The first George C. Karras and K. J. Kyriakopoulos are with the Control Systems Lab, School of Mechanical Eng., National Technical University of Athens, 9 Heroon Polytechniou Str, Athens, 157 80, Greece {karrasg,
kkyria}@mail.ntua.gr
978-1-4244-2058-2/08/$25.00 ©2008 IEEE.
4116
is the Position Based Visual Servo Control (PBVSC), or 3D visual servoing. In PBVSC the control error function is defined in the Cartesian space. Image features are extracted from the image and a perfect model of the camera and the target is used to determine its position with respect to the camera frame. The main advantage of PBVSC is that finally the camera is controlled in the Cartesian space, giving us the ability to choose among a variety of Cartesian controllers available in literature. The second category is the Image Based Visual Servo Control (IBVSC) or 2D visual servoing. In IBVSC the control error function is defined in the image space. The IBVSC approach does not need a precise calibration and modeling since a closed loop scheme is performed directly in image coordinates. But in this case there is no controllability of the camera pose in the cartesian space, not to mention that the initial and desired position must be close enough, otherwise stability is not guaranteed and the target might get out of the camera field of view. In the third approach which is called 2-1/2 visual servoing the control error function is defined in part in the Cartesian and in part in the image space. An homography computed at each iteration, is used to extract the Cartesian part of the error. The main drawback of 2-1/2 visual servo control is that it is sensitive to measurement pertubations. As a result keeping the target in the camera field of view is not guaranteed. More information about the three categories of visual servoing can be found at [1],[2],[3] and [4]. Significant work has been done towards the application of visual servo control in underwater vehicles. An application of IBVSC was realized in [5], while a 2-1/2 visual servo control was implemented in [6]. In both cases the vehicles were fully actuated, implying that the camera had 6 degrees of freedom (DOF). This fact facilitates the use of IBVSC or 2-1/2 visual servoing. Regarding stereo vision approaches an extensive description can be found at [7] and [8]. The great majority of underwater vehicles (especially some small-class ROVs) suffer from kinematic constraints due to under-actuation usually found along sway axis. In many cases this fact imposes the use of non-holonomic Cartesian controllers, especially for specific point to point movements, which makes PBVSC an inevitable choice. The use of PBVSC in a task where an underwater vehicle involves, comes up against two major difficulties. First there is the need of reliable localization of the vehicle (camera) with respect to the target. Second, PBVSC does not offer any control in the image space. So a possible target may get out of the camera field of view during servoing. Additionally the vehicle may collide with the surface the target lays on,
Fig. 2. Fig. 1.
Calculation of x position and ψ orientation
ROV and Laser Pointers mount
due to strong external disturbances such as sea currents and tether appearance. This paper proposes a Position Based Visual Servo Control scheme for an underwater vehicle which can be successfully used in station keeping and point to point tasks with respect to a specific target. This scheme guarantees a trajectory that will keep the target inside the camera optical field during the task and also assures the vehicle will not in any case collide with the surface the target lays on. The latter is of outmost significance, not only in closed loop visual servo control tasks, but also in open loop control modes, such as teleoperation, where the user is only able to watch the camera signal from a monitor, having no information about the distance between the vehicle and the target. The pose of the vehicle with respect to the target is available through the LVS. The position of each laser dot in the image plane is directly related to the distance between the vehicle and the surface the target is located. A path planning strategy based on the APF method produces at each iteration new points for the vehicle to follow. The attractive part of the APF is responsible for minimizing the error between the current vehicle position and the desired. The repulsive part of the APF restricts the target inside the camera optical field while keeps away the laser dots from image regions related to small distances between the vehicle and the surface the target is located. Path planning techniques in visual servo control have been used in the past, especially in robotic manipulators [9] and cartesian robots [10]. More specific in [9] Mezouar et al, proposed an APF based path planning in image space and proved the robustness of the system by image based control. The APF method was originally developed for an on-line collision avoidance for mobile robots and robotic manipulators [11]. The main contribution of this paper coheres in an alternative methodology for collision avoidance evaluating the well known APF method, using monocular vision and a simple Laser Vision System. Additionally, a successful integration of the methodology proposed is described, using the existing methodologies of keeping the target inside the camera optical field. All the above, result in a new way of performing PBVSC with underwater vehicles. The methodology is assessed by a number of experiments which were carried out using an under-actuated 3 DOF ROV. The rest of this paper is organized as follows: Section II gives a description of the methodology proposed, with
4117
Fig. 3.
Laser Dots Geometrical Locus in Image Space
the appropriate analysis of the distributed sub-problems. Section III describes the closed-loop motion controller used for vehicle navigation. Section IV illustrates the efficiency of the approach through a number of experimental results, while Section V concludes the paper. II. M ETHODOLOGY Generally the pose and velocity vectors of a marine vehicle, with respect to an earth-fixed frame, are defined as follows: n = [x y z φ θ ψ]T , v = [u υ w p q r]T
(1)
where x, y, z stand for the position, φ, θ, ψ for the orientation, while u, υ, w stand for the linear and p, q, r for the angular velocities of the vehicle [12]. The ROV used, is a 3 DOF non holonomic vehicle and the angles and angular velocities about x and y axes are statically stable and equal to zero due to the vehicle configuration. So the position and velocity vectors of this vehicle can be rewritten as follows: n = [x y z ψ]T , v = [u υ w r]T
(2)
A. Localization of the vehicle - Laser Vision System (LVS) The LVS consists of a CCD camera and two laser pointers which are parallel to the camera axis (Fig.1). The LVS calculates the pose vector of the vehicle with respect to the center of a target which lays on the image plane. Each laser pointer projects a high intensity dot on the surface the target lays on. The two laser dots are visible in the optical field of the camera. Through an experimental procedure a mapping between the position of each laser dot in the image plane, and the range of the laser pointer from the target plane is obtained. The mapping is achieved by polynomials instead of the triangulation technique, which is subject to inaccurate results due to hardware constraints. Using the ranges of each laser pointer and the distance between them (i.e L1 , L2 , dH respectively as shown in Fig.2), the position along x-axis
Fig. 4.
Active Contours application
Fig. 5.
and the orientation about z-axis of the vehicle with respect to the plane the target lays on can be computed. Considering that the target is flat, this angle is in fact the yaw angle ψ of the vehicle with respect to a reference frame placed at the center of the target[13]. As the vehicle moves along camera axis (vehicle surge axis), and the laser dots prostrate at a surface, the geometrical locus of each laser dot in the image plane is approximately a line (Fig.3) As it will be shown in the following section of the paper, this is very important for the design of the APF concerning the collision avoidance task. The target is tracked by using the Active Contours computer vision algorithm [14]. The Active Contours are usually known as Snakes. A Snake is an energy minimizing spline guided by external constraint forces, and influenced by image forces that pull it toward features such as lines and edges. The Active Contours algorithm is implemented in the system software. The center of the Snake (ut , υt ) coincides with the center of the target as it can be seen from Fig.4. The next step is to combine the information provided by the two laser dots and the Active Contours computer vision algorithm with the camera calibration model equations. The result is the calculation of the position along y and z axis. The complete pose vector with respect to the center of the target, is given by: n = f (L1 , L2 , ut , vt , cu , cv , f, a)
(3)
where (cu , cυ ) the center of the image, f the focal length and a the ratio of the pixel dimensions. The analytical expression of n and a more detailed description of the LVS can be found in [13]. It is really important to point out that the LVS is required even when there exist several targets on the surface, because the LVS not only provides the extra features needed in order to compute a good approximation of the interaction matrix, but also because it provides simply and fast a full estimation of the position and orientation of the vehicle with respect to a target, which is quite necessary for any PBVSC task. As it can be seen from [13], the system software gives the ability to the user to select among multiple targets. The system calculates the pose vector of the vehicle with respect to the chosen target. Additionally, the proposed collision avoidance methodology is firmly related to the LVS range estimation. B. APF based Path Planning As described above, the pose of the vehicle with respect to the center of a target can be calculated using the LVS. The path planning is based on the APF method [11]. So the vehicle motion is influenced by a potential field V which
4118
Fig. 6.
Repulsive Potential for target
Repulsive Potential for laser dots (collision avoidance)
is defined as the sum of an attractive potential Va and a repulsive potential Vr . Both the attractive and repulsive potentials are defined in Cartesian space. The attractive potential Va is pulling the underwater vehicle towards a desired pose ng in Cartesian space. On the other hand the repulsive potential Vr is responsible for pushing away the vehicle from specific areas which are considered as obstacles. In our case the obstacles are defined as areas in the workspace the vehicle must not reach. Keeping away the vehicle from these areas, ensures the target will remain in the camera optical field and will not in any case collide with the surface the target lays on. It is more convenient to define the repulsive potential in image space and then convert it to Cartesian space using the information from the LVS and the intrinsic camera parameters, which are estimated during a calibration procedure [15]. At each iteration an artificial force F (n) is induced by the potential field V . This force is the sum of an attractive force, Fa (n), and a repulsive force, Fr (n). So path generation along the direction which is imposed by F (n) is given by the following equation: nk+1 = nk +εk
F (nk ) F (nk )
(4)
where k is the increment index and εk is a positive scaling factor responsible for the length of k th increment. 1) Attractive Potential: The attractive potential field Va is defined directly in the Cartesian space and its purpose is to minimize the error between the current vehicle position and the desired, as described below: 1 2 (5) Va (n) = ka n− ng 2 where ka is a positive scaling factor. As a result the attractive force is given by: Fa (n) = −∇Va (n)
(6)
2) Repulsive Potential: As described before the repulsive potential will first be defined in image space and then it will be transformed to Cartesian space using the intrinsic
parameters of the camera, the information provided from the LVS and the camera model equations. The repulsive potential is divided in two parts. The first repulsive potential Vrtf ov is responsible for keeping the target inside the field of vision, while the second potential Vrca is related to the collision avoidance task. The first imposes forces to the target centroid, while the second imposes forces to the centroid of each laser dot in the image plane. The resulting repulsive potential field Vr is the sum of the previous defined potentials: Vr (s) = Vrtf ov (s) + Vrca (s)
(7)
where s is the vector containing the image features. More specific: s = [sTt sTll sTrl ]T , where st = [ut vt ]T , sll = [ull vll ]T and srl = [url vrl ]T are the vectors containing the position of the target, left and right laser dot centroids in x and y image coordinates respectively. The repulsive potential Vrtf ov is defined as in [9], considering only one point, which is the target centroid. A point P in Cartesian space which is projected onto the camera image plane at a point st = [ut vt ]T is observable by the camera only if ut ∈ [um uM ] ∩ vt ∈ [vm vM ], where um , uM , vm , vM are the limits of the image in x and y image coordinates respectively. In order to assure that the target centroid will always be visible, the repulsive potential Vrtf ov is defined as follows: Vrtf ov (s) = Q (st ) =
1 2
log (Q (st )) , where : u
1− u t
M
1
(1− uumt )
v
1− v t
M
1
(1− vvmt )
(8)
if ut ∈ [um hu ] ∩ [uM − hu uM ], or vt ∈ [vm hv ] ∩ [vM − hv vM ] and Vrtf ov (s) = 0 elsewhere. The variables hu , hv are positive constants denoting the distance of influence of the image edges. The repulsive potential related to the collision avoidance task is also defined in image space. Suppose that the position of the centroid of the left and right laser dot is given by the vectors sll and srl respectively. Let us now define two circular regions in the image space. These regions are related to small distances between the vehicle and the surface the target is located. A virtual obstacle is placed at the center of each region. The vector sllo = [ullo vllo ]T denotes the position of the obstacle for the left laser dot and the vector srlo = [urlo vrlo ]T for the right, in x and y image coordinates respectively. As described in Fig.3, each laser dot moves inside a specific geometrical locus in image space. For that reason we define two repulsive potentials, Vrll for the left laser dot and Vrrl for the right. The repulsive potential related to the collision avoidance task is defined as the sum of these two potentials. More specific: (9) Vrca (s) = Vrll (sll ) + Vrrl (srl ) 2 1 1 1 , if ρ (sll , sllo ) ≤ ρo 2 kll ρ(sll ,sllo ) − ρo Vrll (sll ) = 0 , elsewhere (10) 2 1 1 1 k − , if ρ (s , s ) ≤ ρo rl rlo 2 rl ρ(srl ,srlo ) ρo Vrrl (srl ) = 0 , elsewhere (11)
4119
Fig. 7.
Polar-like frame
Where ρ (srl , srlo ) = srl − srlo and ρ (sll , sllo ) = sll − sllo . The variable ρo denotes the radius of the circular area inside which the potential field for each laser dot is active, while krl , kll are positive constants. The repulsive potential Vrtf ov (s) is depicted in Fig.5, while Vrca (s) is depicted in Fig.6. In order to transform Vr (s) from image space to Cartesian space Vr (n), vector s = [ut vt ull vll url vrl ]T is expressed in Cartesian coordinates using the formulation below: s = A nim + b
(12)
where: A = f a I6x6 T b = cu cv cu cv cu cv nim = yim zim yimll zimll yimrl yim = xy zim = xz yll yimll = x−(yll +y) tan(ψ) zll zimll = x−(yll +y) tan(ψ) yrl yimrl = tan(ψ)dH +x−(y ll +y) tan(ψ) zrl zimrl = tan(ψ)dH +x−(yll +y) tan(ψ)
zimrl
T
(13) where x,y,z,ψ is the pose of the vehicle with respect to the center of the target, computed at each iteration from the LVS. The variables yll , zll ,yrl ,zrl denote the position of the left and right laser pointer in Cartesian space along y-axis, and z-axis with respect to the center of the camera. These values are known and constant due to the fact that the laser pointers are placed parallel and at a fixed distance from the camera axis using a special mount (Fig.1). Using the above information the repulsive force is given by: Fr (n) = −∇Vr (n)
(14)
III. C LOSED L OOP S TEERING C ONTROL This section describes the closed loop steering control of the vehicle during the PBVSC task. The pose vector n provided by the LVS will be used as the state feedback for the real time control system. The points of the path generated along the direction of the potential V will be used as the desired input for each iteration. Due to the fact the vehicle is an under-actuated 3DOF ROV, a non holonomic kinematic controller [16] will navigate the vehicle along x and y axes and a PD controller along z-axis, as the vehicle
Fig. 8.
System Architecture, where U = [u r w]T . More information regarding feature extraction and pose estimation can be found in [13]
has a dedicated thruster for vertical motion. The system architecture is depicted in Fig.8. The ROV kinematic model is described as follows: ·
·
x = u cos(ψ)
z=w ·
·
ψ=r
y = u sin(ψ)
(15)
where x, y, z stand for the vehicle Cartesian position and ψ the orientation angle about z-axis. The u component describes the linear velocity of the vehicle, while x, ˙ y, ˙ z˙ denote the time derivatives of the Cartesian position. The component w stands for the linear velocity along z axis and r the angular velocity about z axis. We represent the position of the vehicle in terms of its polar-like coordinates, involving the error distance e > 0 and its orientation θ with respect to reference frame < G >, which in our case is the center of the target, as shown in Fig.7. Letting a = θ − ψ be the angle measured between the vehicle principal axis and the distance vector e, the kinematic model is described through the equations: e˙ = −u cos a a˙ = −r + u sine a θ˙ = u sine a
(16)
Aicardi et al proved in [16] that using the motion controller: u = (γ cos a)e ; γ > 0 r = ka + γ cos aasin a (a + hθ) ; k, h > 0
(17)
the system state vector [e, a, θ] asymptotically converges to [0, 0, 0]T . Due to the fact that at each iteration the path planning calculates a desired vector nd = [xd yd zd ψd ]T a transformation of the feedback state was made, in order the controller to asymptotically converges to [xd , yd , ψd ]T . Suppose that the pose vector of the vehicle with respect to the target is n = [x y z ψ]T . A different pose vector nf = [xf yf zf ψf ]T is fed to the controller which is responsible for the navigation of the vehicle along x and y axes, where: T
xf = (x − xd ) cos ψd − (y − yd ) sin ψd yf = (x − xd ) sin ψd + (y − yd ) cos ψd zf = z ψf = ψ − ψd
(18)
In this case the system state vector in polar-like coordinates is modified as follows: ef = x2f + yf2 (19) θf = arctan 2 (−yf , −xf ) af = θf − ψf
4120
The navigation and stabilization along z axis is accomplished by the PD controller: w = Kp ez + Kd e˙ z ; Kp , Kd > 0
(20)
where w is the control input for the z-axis (vertical thruster), ez the position error along z-axis and e˙ z its time derivative. At each iteration the computed points of the proposed path planning are being fed to the above control scheme. This guarantees that the target is always kept in the camera field of view. IV. E XPERIMENTS In order to prove the overall efficiency of the system, two different PBVSC experimental procedures were carried out. The first demonstrates the capability of the proposed system to navigate the vehicle to a desired configuration, while keeping the target inside the camera optical field. The second experiment demonstrates the ability of the system to avoid collision with the surface the target lays on. In both cases the experiments took place inside a tank. The LVS calculates the pose vector of the vehicle with respect to the center of a random-shaped target which is located on an aluminium plate inside the tank. It is profound that the LVS delivers data only when the target is in the camera optical field. A. System Components The ROV used is 3-DOF (VideoRay PRO, VideoRay LLC, Fig.1), equipped with three thrusters, a control unit, and a CCD camera. The control unit is connected with a Personal Computer (PC) through a serial communication interface (RS-232). The camera signal is acquired by a framegrabber (Imagenation PXC200). The image dimensions are 768x576 pixels. The LVS provides data at 17 Hz frequency. The laser pointers are equipped with Sony diodes, with wavelength of 635 nm (red color) and power of 5mW. The size of the laser pointers is 185(L) 25(D) mm (cylindrical shape). A randomshaped target is located on an aluminium plate which is fixed inside the tank. The system software is implemented in C. The operating system is Linux. B. Experimental Results The first experiment demonstrates the capability of the proposed system to navigate the vehicle to a desired configuration, while keeping the target inside the optical field. As we mentioned before this is very important for under-actuated vehicles performing position based visual servo control.
V. C ONCLUSION This paper proposes an APF-based path planning methodology for PBVSC of an underwater vehicle. The pose of the vehicle with respect to the center of a target is calculated using a Laser Vision System (LVS). The attractive potential, which is responsible for the minimization of the error between the current vehicle position and the desired, is defined directly in the Cartesian space. The repulsive potential, which is responsible for keeping the target inside the camera optical field as well as for the collision avoidance with the surface the target lays on, is first defined in image space and then transformed to Cartesian space. More specific, the repulsive Artificial Forces are keeping away the target from the image boundaries, and the laser dots from specific regions in the image space, which are related to short distances between the
4121
vehicle and the surface the target is located. The closed loop steering control of the vehicle was realized, using a Cartesian nonholonomic kinematic controller, which was modified for the needs of the methodology. A number of experiments using an under-actuated ROV, prove the successful integration of the proposed methodology for collision avoidance, with the existing methodologies for keeping the target inside the field of vision. R EFERENCES [1] F. Chaumette, S. Hutchinson, Visual Servo Control Part I: Basic Approaches, Robotics and Automation Magazine, IEEE, vol. 13, 2006, pp 82-90. [2] F. Chaumette, S. Hutchinson, Visual Servo Control Part II: Advanced Approaches, Robotics and Automation Magazine, IEEE, vol. 14, 2007, pp 109-118. [3] S. Hutchinson, G. D. Hager and P.I. Corke, A tutorial on visual servo control, IEEE Trans. on Robotics and Automation, 1996, vol. 12(5), pp. 651-670. [4] E. Malis, F. Chaumette and S. Boudet, 2 1/2 d Visual Servoing, IEEE Trans. on Robotics and Automation,, 1999, vol. 15(2), pp. 238-250. [5] J.-F. Lots, D.M. Lane, E. Trucco, F. Chaumette, A 2D visual servoing for underwater vehicle station keeping, IEEE International Conference on Robotics and Automation, 2001, vol. 3, pp. 2767 - 2772. [6] J.-F. Lots, Application of visual servoing to the dynamic position of underwater vehicles, Phd Thesis. [7] Chanop Silpa-Anan, Thomas Brinsmead, Samer Abdallah and Alexander Zelinsky, Preliminary Experiments in Visual Servo Control for Autonomous Underwater Vehicle, IEEE/RSJ Intelligent Robots and Systems, 2001, p.1824-1829 vol.4. [8] Shahriar Negahdaripour, Pezhman Firoozfam, An ROV Stereovision System for Ship-Hull Inspection , IEEE Journal of Oceanic Engineering, 2006, vol. 31(3), pp. 551-564. [9] Y. Mezouar, F. Chaumette, Path planning in image space for robust visual servoing, IEEE International Conference on Robotics and Automation , 2000, vol. 3, pp. 2759 - 2764 . [10] B. Thuilot, P. Martinet, L. Cordesses, J. Gallice, Position based visual servoing: keeping the object in the field of vision, IEEE International Conference on Robotics and Automation , 2002, vol. 2, pp. 1624 1629. [11] O. Khatib, Real time obstacle avoidance for manipulators and mobile robots, International Journal of Robotics research, , 1986, vol. 5(1), pp. 90-98. [12] T. Fossen , Guidance and Control of Ocean Vehicles, Wiley, New York, 1994. [13] George C. Karras, Dimitra J. Panagou and Kostas J. Kyriakopoulos, Target-referenced Localization of an Underwater Vehicle Using a Laser-based Vision System, MTS/IEEE OCEANS, 2006. [14] M. Kass, A. Witkin, and D. Terzopoulos, Snakes: Active contour models, International Journal of Computer Vision, Vol. 1, no. 4, pp. 321-331, 1987. [15] Wang C. C., Shyue S., Hsu H., Sue J., and Huang T., Ccd camera calibration for underwater laser scanning system, MTS/IEEE OCEANS, Vol. 4, pp. 2511-2517, 2001. [16] M. Aicardi, G. Casalino, A. Bicchi, A. Balestrino, Closed Loop Steering of Unicycle-like Vehicles via Lyapunov Techniques, IEEE Robotics and Automation Magazine, Vol.2, no.1, pp. 27-35, March 1995. −50 APF Path Planning No APF Path Planning −60 System response along x−axis (cm)
More specific, the vehicle starts from a difficult configuration with respect to the target, especially considering the high value of the yaw angle (ψ0 = 33 deg). During the task the vehicle must align its axes with respect to the target (yd = 0 cm ,zd = 0 cm,ψd = 0 deg), and stabilize along x axis at xd = −72 cm. The navigation is based on the previous analyzed path planning procedure, while the steering control loop is implemented using the controller described in section III. The system response is shown in Fig. 9-12, while the target position along x and y image axis is shown in Fig. 13-14. It is also shown that when the same experiment was carried out without using the proposed path planning (red dashed line), the vehicle lost the target after a few seconds. As it can be seen from Fig.13, when the system performed PBVSC without the proposed path planning, the vehicle lost the target along x image axis. The explanation is given in Fig.12 which shows that in that the case the controller led the vehicle to a large angle about z axis. On the contrary APF path planning (blue solid line), protected the vehicle by feeding the controller with a safe path, and pulling away the target from image boundaries as it is especially shown in Fig.13 and at time step t = 16 s. The target was finally stabilized at the center of the image (cu = 384,cv = 288), as depicted in Fig.13-14. In the second experiment (Fig.15-21), the desired position along x axis is set behind the target at xd = 40 cm. The desired values for the rest of the states are set to yd = 0 cm,zd = 0 cm,ψd = 0 deg. This should normally lead the vehicle to collide with the surface the target lays on. In this case it is shown that the vehicle not only does not collide with the surface but also keeps the target inside the field of vision. It is also shown, that the other states y,z,ψ converge to the desired values. As it can be seen from Fig. 19-20 the target is stabilized at the center of the image, and does not escape in any case from the field of vision, proving in this way the successful integration of the methodology proposed for collision avoidance with the already existing methodologies of keeping the target inside the field of vision. It is also shown in Fig.15 and Fig. 21 (x, y plane), that the vehicle stabilized itself at x = −50 cm, despite the fact the desired value was set at xd = 40 cm (behind the target).
−70
−80
−90
−100
−110
−120
Fig. 9.
0
10
20
30
40 t (sec)
50
60
70
80
Closed Loop X0 = −112, Xd = −72 cm
50 APF Path Planning No APF Path Planning
40 System response along y−axis (cm)
System response along y−axis (cm)
60
40
20
0
−20
−40
0
Fig. 10.
10
20
30
40 t (sec)
50
60
Closed Loop Y0 = −20, Yd = 0 cm
Fig. 16.
−20 −30
0
10
20
30
40 t (s)
50
60
70
50 40 System response along z−axis (cm)
System response along z−axis (cm)
0
Closed Loop - Collision Avoidance Y0 = 0, Yd = 0 cm
APF Path Planning No APF Path Planning
35 30 25 20 15 10 5 0
30 20 10 0 −10 −20 −30 −40
0
Fig. 11.
10
20
30
40 t (sec)
50
60
70
−50
80
Closed Loop Z0 = 38, Zd = 0 cm
Fig. 17.
0
10
20
30
40 t (s)
50
60
70
Closed Loop - Collision Avoidance Z0 = 0, Zd = 0 cm
50
50 APF Path Planning No APF Path Planning
40 System response about z−axis (deg)
40 System response about z−axis (deg)
10
−10
−50
70
40
30 20 10 0 −10 −20 −30 −40 −50
20
−40
−60
−5
30
30 20 10 0 −10 −20 −30 −40
0
Fig. 12.
10
20
30
40 t (sec)
50
60
−50
70
Closed Loop ψ0 = 33, ψd = 0 deg
Fig. 18.
0
10
20
30
40 t (s)
50
60
70
Closed Loop - Collision Avoidance ψ0 = 0, ψd = 0 deg
450 700
400
600 Target X position (pixels)
Target X position (pixels)
350 300 250 200 150 APF Path Planning No APF Path Planning
100
400 300 200 100
50 0
500
0
Fig. 13.
10
20
30
40 t (sec)
50
60
70
0
80
Target Position along X-image axis
Fig. 19.
0
10
20
30
40 t (s)
50
60
70
Collision Avoidance - Target Position along X-image axis
500 APF Path Planning No APF Path Planning
500
Target Y position (pixels)
Target Y position (pixels)
450
400
350
300
250
0
10
20
30
40 t (sec)
50
60
70
200
0
80
Target Position along Y-image axis
Fig. 20.
0
10
20
30
40 t (s)
50
60
70
Collision Avoidance - Target Position along Y-image axis
60
60
40
40
20
20
0
Cartesian X axis (Surge)
System response along x−axis (cm)
300
100
Fig. 14.
−20 −40 −60 −80 −100
Vehicle Position Surface Desired Position Target
0 −20 −40 −60 −80 −100
Response X−axis Desired X−axis
−120
−120
−140
−140 0
Fig. 15.
400
10
20
30
40 t (s)
50
60
70
−50
Closed Loop - Collision AvoidanceX0 = −115, Xd = 40 cm
4122
Fig. 21.
0 Cartesian Y axis (Sway)
50
Collision Avoidance - Vehicle position X,Y plane
