Visual Servoing

Figure 4.11 illustrates the visual servoing block diagram where PI

t, PnI and

δPI

n are the target position, the current micropipette tip position estimated 3_{The user can activate an option which forces the micropipettes to remain in the FOV}

by the object tracking algorithm, and the tip position error ofnth_micropipette

produced by the trajectory generation algorithm, all measured in the image coordinate system Cimage_{. I(k) and I(k−1) are current and previous image}

frames, δPmann _{is the displacement command applied to the n}th _micromanip- ulator controller, Jn is the image Jacobian relating displacement (velocity) in

Cimage _{to displacement (velocity) in C}mann_{, f}I

r is the collision avoidance force

in Cimage _{as described in Section. 4.8, α and K are scaling factors adjusting}

the relative weight of trajectory following versus collision avoidance and λ is a positive constant damping factor in the damped least squares method. The whole control loop consists of several parts which are described below:

Trajectory Generation

The robot controller has a closed architecture, i.e., a new move command cannot be issued before the previous displacement has been completed and the controller approves it by sending an acknowledgement signal back to the PC. In other words, the feedback loop for the visual servoing is open when the robot is moving. The amount of delay for completion of each command depends on the amount of displacement as shown in Equation 4.19. To have an effective control on each micropipette tip position,||δP_nman||should be small enough to keep the control loop feedback closed.

When a target point P_tI is specified, the trajectory generation algorithm calculates a displacementδP_nIbased on the current location of the micropipette tip: δP_nI =η P I t −PnI ||PI t −PnI|| (4.14) whereη is a constant (in pixels) which can be adjusted by the user. As shown in Equation 4.14,δP_nI is a displacement vector with a length ofηpixels in the direction from the current location of the tip P_nI to the target position P_tI.

Figure 4.11: Visual serv oing blo ck diagram.

Trajectory Tracking

When a trajectory is generated by the trajectory generation block, a controller is required to make the system follow this trajectory. The displacement δP_nI

is measured in Cimage _{while the robot controller works in its own coordinate}

system Cmann_{. In other words, we need to convertδP}I

n toδPnman. This can be

stated as an inverse kinematics control problem by assumingCmann_{as thejoint}

space for the nth _{robot and takingC}image_{as the task space. There are different}

methods to solve an inverse kinematics problem including a direct analytic approach, a Jacobian inverse/pseudoinverse approach, a Jacobian transpose

approach and aDamped least squares approach [33].

The inverse kinematics problem in our case is a non-singular problem be- cause the task space and joint space are three dimensional Cartesian spaces which are related by an affine transformation which is invertible. Although such a problem can generally be solved using a direct analytic method or an inverse Jacobian method we have chosen a damped least squares method to do this. The reason is that it is not trivial to incorporate task space forces in an inverse kinematics problem unless the solution uses a Jacobian transpose method or one of its variants. The damped least squares technique [33] (also called the Levenberg-Marquardt technique) uses a Jacobian transpose based pseudo-inverse method which allows us to involve task-space forces while hav- ing a numerically stable inverse kinematics solution.

δP_nman =J_nT JnJnT +λ

2

I−1δP_nI (4.15)

When there is a task-space force fI

r, the equation can be modified to:

δP_nman =J_nT h JnJnT +λ

2_I−1

KδP_nI+αf_rIi (4.16) where K and α are scaling factors which are adjusted by the user. The ratio

α

||K|| represents the importance of collision avoidance forces with respect to

position errors.

The actual image Jacobian varies depending on the current location of the micropipette tip, due to image distortions. The Jacobian matrix is basically an

affine matrix in our problem. The uncertainty in the Jacobian matrix consists of scale and rotation uncertainties and can be modeled as:

ˆ

Jn=δJn(p)Jn(p) (4.17)

where ˆJnis the Jacobian matrix estimated for thenthmicromanipulator through

system calibration,Jn(p) is the Jacobian matrix when thenth micropipette tip

is located at the pointpinCimage_{, and δJ}

n(p) is the multiplicative uncertainty

of this Jacobian at that point. However, the feedback loop is robust enough to compensate for small uncertainties as shown in the experimental results in Section. 4.12. To achieve a more efficient controller, the Jacobian matrix can be modified using an adaptive algorithm [34].

4.8

Collision Avoidance

Considering the limited space of operation, there is a high chance of collision among the micropipettes or between micropipettes and fixed obstacles. It is not possible to rely on the human operator to avoid collisions manually because the micropipette tip is very small, tips become very close to each other and they may also collide outside the FOV of the microscope.

Artificial potential fields have been used in several different real-time obsta- cle avoidance applications for robot manipulators and mobile robots [35], [36]. An artificial potential field (APF) algorithm has been implemented to avoid collision among micropipettes and obstacles in the workspace. A virtual force is generated based on this APF. The micropipette under control is assumed as an object with positive charge. Other micropipettes, the microscope lens and the bath are also assumed as objects with positive charges. To keep the algorithm computationally efficient, we have assumed point charges concen- trated at a point. The virtually positive-charged point of an obstacle is the closest point of that obstacle to the micropipette under control. The virtual positive charge on the controlled micropipette, is assumed to be located on the closest point of that micropipette to each obstacle. Although it is also possible

to extend artificial potential fields to perform full navigation including target tracking and collision avoidance at the same time by assigning an opposite charge to the target [37], we have only used APF for collision avoidance to avoid the local minima problem associated with this method.

All calculations are based on the assumption that micropipette tips are located in the focal plane and calculations are done in this plane. A possible scenario is shown in Figure 4.12.

Figure 4.12: Collision avoidance forces are calculated based on the closest distance to other obstacles. Pk shows the kth micropipette. Fi is the repulsion

force acting onPi and generated by other micropipettes.

between Pi and Pj, pointing towards Pi. If the vector with minimum length

is Pi,kmin, then:

Fi =γi X

k∈Si Pi,k

||Pi,k||3

, Si ={k | ||Pi,k|| ≤(1 +i)||Pi,kmin||} (4.18) whereγi is the repulsion factor which determines the amount of force used for

collision avoidance on Pi andSi is the set of indices of the closest obstacles to

Pi, andkmin is the index of the closest obstacle to Pi. i is taken into account

to exclude those micropipettes/obstacles which are not closest to Pi. To be on

the safe side, it is taken as i = 0.1, ∀i to cover up to 10% of the distance to

closest obstacle around Pi. Without a small enoughi, the superposition of the

repulsion forces may vanish and a collision may occur. A collision margin is defined by the user in terms of pixels. The collision avoidance force is ignored if the minimum distance is out of this margin.

To mark fixed obstacles (other than micropipettes), user should move an micropipette close to each obstacle manually and the software would register the coordinates of that obstacle in Cref_{. It is also possible to focus and click}

to register fixed obstacles (such as the mesh).