Fault Tolerant Control Architecture Design for Mobile Manipulation in Scientific Facilities

2.1 Task description

A mobile manipulator can have wide range of applications within SFs. In this section, to summarize the system description, the authors define a mission as a sequence of tasks to develop necessary hardware and software components. A sample inspection covers the following description:

Mission Receive commands on control room Human Machine Interface (HMI): In point “A”, Fast forward to section “B” addressed on the CAD-based map. Prepare for tele-operation around the device number “36” and then inform the operator for a semi-autonomous operation. Touch the object defined by the operator. Inspect the area defined by the operator and transmit preliminary results. Finally, come back to your station and give detailed data report.2

To task the robot mobile manipulator inside a hazardous area of a SF, there are several limitations and requirements. Some of the key requirements are listed below:

Requirement 1. The physical or chemical effects of materials and test equipment in the environment can affect the work performance and lifetime of electrical components.

Requirement 2. The facilities are mostly designed for humans, the test equipment or special machines accessible through different corridors. There is no dedicated environment or specific landmarks – or setups for a robot except its station. In other words, the environment is unstructured for a robot.

Requirement 3. A lack of communication can frequently occur. The robot shall be tolerant to failures of Wi-Fi or delays of GSM transmitted signals.

Requirement 4. The purpose of utilizing a robot is to prevent human intervention in hazardous environments. Therefore, during the operation of the robot, the area is not suitable for humans, and all robot operations shall be remotely controllable.

Requirement 5. In case of failure, the robot should be able to leave the SF.

Considering these requirements, the mission can break down into sequential tasks. Since, in most cases, the mobile manipulator applications are similar to the described mission, we describe here those tasks, while we describe the overall solution later on.

Task 1. Initialization

Get the global map ​ ℐ M , initial pose ​ ℐ T ​ ℬ 0 , destination ​ ℐ T ​ ℬ f 3, a final workspace ​ ℐ W , the initial status of the robot, and calibration parameters. Monitor output of the sensors and command their initialization. Verify their functionality and consistency with the initialized variables. Initialize the actuators. Perform a predefined motion and examine the actuators and sensors by verifying the consistency of the commands and feedback.

Task 2. Planning

Convert the global map into a 2D occupancy grid ​ ℐ M ' . Plan an energy-optimal obstacle-free path from ​ ℐ T ​ ℬ 0 to ​ ℐ T ​ ℬ f within ​ ℐ M ' . Plan for wondering around the position ​ ℐ T ​ ℬ f within ​ ℐ W . Estimate the elapsed time and power consumption for all of the future tasks programmed within the mission with a safety margin. Compare the power status of the batteries with the estimated power consumption. Wait for extra charge in case of insufficient power.

Task 3. Autonomous Running Mode

Run these modules in parallel, repetitively:

Perception Perceive any change in the environment and status of the robot compared to the preliminary planned motions.

Planning Look–ahead and plan for local movements regarding the high–level commands received and the local obstacles and costs for doing the job.

Actuation Run the planned actions while receiving the joints' feedback for the closed–loop actuator–level control.

Exception Handling In case of single point failures, bypass the current sequence and save the algorithm. Batch the logs and transmit to the supervisory room.

Fault Detection Once in a while, conduct a survey among the software and hardware modules with their model. In case of the detection of unpredicted motions and feedback signals, decide for one of these solutions. Run the relevant fault management component. Call the operator for decision making or manual driving. Run the Safety Anchor (described in Section 5).

Watchdog Independently maintain surveillance of the robot software and hardware modules during the operation.

Task 4. Update Mode

After reaching the target position, map the workspace on a detailed map. Extract the optimum features for accurate localization. Define the local stationary coordinate attached to the features. Obtain transformations between the new coordinate and the predefined coordinates. Upload the collected information to the supervisory room while considering possibility of data loss and a delay in connection. Finalize the autonomous mode and switch to the semi-autonomous mode. Call the operator through all of the available networks, such as Wi-Fi or GSM.

Task 5. Tele-operation Mode

Let the operator define the movements in the virtual environment and execute the commands in order. Update the current status for the operator by the minimal transmission of data to overcome any lack of connection bandwidth and to enable the possibility of augmented reality. In the case of selecting an object, run the visual servoing to approach the vicinity of the target object and do any inspection or manipulation as commanded by the operator. At the same time, maintain the prediction of power consumption. Set an alarm when the power reaches the limit of the safety margin. Quit from the position if the operator has finished the operation or else if the power limitation implies a forced return.

Task 6. Finalization Mode

Return to the nearest safe station. Set all the actuators to their home positions. Next, transmit all the gathered data by the high bandwidth connection, including the final status of the robot and its components and the evaluation of the modules that are gathered during run mode. Clear any temporary information. Recharge the embedded power source.

Task 7. Standby Mode

Turn off any high-consumption hardware and keep the control blocks in sleep mode. Wait for the next trigger and another mission.

According to the tasks and their descriptions, the rest of this paper gives an overview of the hardware and then describes the necessary software modules and the architecture designed for the concisely described tasks.

2.2 Sensors

The robot is equipped with several sensors to collect data from its components and the environment. We describe them in the following subsections.

2.2.1 Laser range finder

(LRF) The laser scanner or LRF provides a 2D map of the environment based on the reflection of its infrared beams. The output data comprise pairs of distance and angle based on a predefined polar coordinate attached to the {ℒℛℱ} coordinate. The origin of {ℒℛℱ} which is virtually defined as attached to the axis of rotation of its motor in such a way that the +z direction is collinear to the axis of rotation. Therefore, all the transmitted points rely on the x – y plane of {ℒℛℱ} The corresponding Cartesian representation of the detected point number k by the laser scanner can be defined as LRF Λ _k (x_Ky_K 0).

The number of detected points during one scan, max(k), depends on the field of view (FOV) and the angular resolution of the laser scanner. Note that the source of systematic and random errors in the positions of the detected points are still in the polar coordinate as errors in the distance and angle [15,16]. We can write for the vector of points:

ℬ Λ ​ ℒ ℛ ℱ = ℬ T ​ ℒ ℛ ℱ ℒ ℛ ℱ Λ

(1)

The output of the laser scanner determines the position of the robot relative to its environment by a Simultaneous

Localization and Mapping Toolbox (SLAM) as described in Subsection 4.2. Moreover, its output can be a local map for obstacle avoidance and heading estimation.

Despite the fact that the LRF is a robust sensor for the navigation of the mobile platform, it fails in the detection of transparent and reflective materials in the environment [17]. It is also hard to detect darker materials at longer distances. Regarding these issues, the sensor can be considered as a safe sensor for short distances (under 20m). Besides the accurate distance angle measurements, it is possible to define warning zones on the same plane. In this case, the scanner transmits the corresponding situation of each zone. This communication can be safe and totally independent of the points measurements, and can be utilized in the safety anchor mechanism described in Section 5. The warning zones can be defined as relay outputs or logical switches on the hardwired network.

3.1 Inverse kinematics

Figure 2 shows a schematic view of a 4WS mobile robot. The denoted parameters for the configuration of the body and the leg modules are presented in the figure. The coordinate frame U{X̂,Ŷ} is the inertial frame. Frame B{x̂, ŷ} is a fixed-body frame defining the heading of the robot, and B_v{v̂, û} is the velocity frame, that is, the unit vector v̂ determines the direction of the robot's base linear velocity vector. Both B and B _v are attached to the robot's base at point Q, which can be chosen at will. The angles ψ _v and θ _B are the angles v̂ and x̂, respectively, in U. The scalar value v is the magnitude of the linear velocity of point Q. The variables ω _B = θ _B and ω _v = ψ _v are the angular velocities of the base and of v̂, respectively. The constant vectors Q L i → presented in frame B are denoted by B ℓ i , i = 1..4 . The angle φ _i is the heading angle of the i^th leg while v_iv̂_i is the velocity vector of the attachment point L _i . The steering and speed control commands for leg i are calculated using φ _i and v_i, respectively. The following kinematic relations hold:

B v ^ = R ( ψ v - θ B ) [ 100 ] T ( a ) v i B v ^ i = v B v ^ + w B ( z ^ × B ℓ i ) ( b )

(2)

in which, ẑ=[0 0 1] ^T and R (ψ _v – θ _B ) is the rotation matrix with angle ψ _v – θ _B around the z-axis, that is, frame B _v in B. For motion control purposes, once the control signals ψ _v ,ω _B and v are calculated for the body frame, the desired signals ψ _i and v_i can be derived using above kinematics equations. Notice that only angle ψ _i and not its derivatives appears in (2a-2b) as the angle of vector ​ B v ^ ​ i . This fact justifies having steering actuators to be controlled in position mode.

3.2 Forward kinematics

As mentioned earlier, the speed of the drive actuators and the positions of the steering actuators are measured, that is, positions φ _i for the steering actuators and speeds v_i for the driving actuators. In this subsection, the goal is to derive ψ _v ,ω _B and v based on the encoders' measurement data, that is, the sets {v_i,φ _i } for i = 1… 4. Clearly, the relative velocity of the wheel modules with respect to each other must be zero. In other words, the set {v_i,φ _i } applied to a leg should be consistent with other sets. Although the set points for the steering and drive commands are exactly with the kinematics relations due to different loads and different servo dynamics. Since the legs are connected to a rigid base, this inconsistency causes the legs to slip with respect to one another. This slippage is different than the overall slippage of the robot, and cannot be observed through odometry data. In order to measure the consistency of the actuators' actual values, we define the following measure:

e i = 1 4 ∑ j = 1 , j ≠ i 4 e i j 2 , i ∈ { 1 , 2 , 3 , 4 }

(3)

where

e i j = ( v i ​ B v ^ ​ i − v j ​ B v ^ ​ j ) . ℓ i j (a) ℓ i j = B ℓ i − B ℓ j ‖ B ℓ i − B ℓ j ‖ 2 (b)

(4)

In the above equations e_ij is the relative velocity line L_i to L_j Clearly, if both wheels are synchronized perfectly and no slippage occurs, e_ij is zero. However, as explained before, in practice these variables are not zero and the value e_ij indicates the relative slippage of the wheel modules i and j. The variable e_i is the measure of the relative slippage of one wheel with respect to the other three wheels. In practice, for the mobile robot to move properly, all e_i s must be small. The value of e_i s determine the overall consistency of wheel i with respect to the other modules. Several cases can be considered for different values of e_i.

In the case where:

m i n ( e i ) > e m a x

(5)

this means that the legs are somewhat faulty and that they are not consistent with one another. Observing such a condition for a pre-specified interval of time should cause the controller to issue a fault signal to stop the robot.

If e_i is large only for one wheel, it can be interpreted such that the wheel is not consistent with the other wheels and that it is being pushed by them. In this case, different strategies can be considered, such as stopping the robot or singling out the faulty leg from the control loop and setting the actuators of the faulty wheel to rotate freely.

Alternatively, more complex strategies can be designed based on the robot's assigned task and application.

After calculating e_i for all the legs and evaluating their values, the forward kinematics problem can be solved as follows. Let ​ B v ^ and ​ B v ^ ​ i be denoted by their components by [v_x v_y]^T and [v_ix, v_iy]T. Next, (2b) for all the legs can be rearranged into the following equation:

A [ v v x v v y w B ] 3 × 1 = B

(6)

in which

A T = [ 1 0 … 1 0 0 1 … 0 1 − l 1 y − l 1 x … − l 4 y − l 4 x ] 3 × 8 (a) B T = [ v 1 x v 1 y … v 4 x v 4 y ] 1 × 8 (b)

(7)

Obviously, in the general case the matrix Equation (6) consists of eight equations and three unknowns. A simple least squares solution is given by:

[ v v x v v y ω B ] 3 × 1 = AB

(8)

Note that the elements of A are the only function of the constant vectors ​ B ℓ ​ i . Hence, A^† = (AA^T)⁻¹ A^T needs to be calculated once. In practice, we need at least three rows of matrices A and B to solve the forward kinematics. Thus, one way to improve the solution is to eliminate the inconsistent wheel module and eliminate the corresponding rows from (6). Our strategy is based on isolating the leg with a maximum of e_i Next, the rows with respect to that leg are eliminated from matrix A in 7a and the least squares is calculated based on this new matrix. In [18], we show the efficacy of the above strategy and further details of the implementation of this approach.

4.1 Dead reckoning

Generally speaking, the time integration of the velocity vector – known as ‘deductive reckoning’ or Dead Reckoning (DR) – for a mobile platform gives its approximate position with respect to its initial pose [19, chap. 20]:

​ I x ​ B ( t ) = ​ I x ​ B 0 + I R ​ B 0 ∫ t 0 t B 0 R ​ B ​ ​ V x ( t ) d t

(9)

I y B ( t ) = I y B 0 + I R B 0 ∫ t 0 t B 0 R B V y ( t ) d t

(10)

I R B ( t ) = I R B 0 R z ( θ B ( t ) ) , θ B ( t ) = ( ∫ t 0 t ω z ( t ) d t )

(11)

The driving velocities of the wheels as regards their steering angles and robot kinematics can generate fundamental data for the localization of the mobile platform.

4.2 SLAM

The errors for the mobile manipulator are accumulative during its motion. In order to reduce the errors, it is necessary to compare the position of the mobile base with respect to its environment. The SLAM [20] method provides the change in location of the robot according to its initial configuration. Moreover, it provides the local map of the environment ​ ℐ M ' , which is convertible to an occupancy grid for the planning module.

4.3 Path planning

The path-planning module comprises two modes: local and global planning. The planner initially plans the required path through requests from ​ ℐ T ​ ℬ 0 to ​ ℐ T ​ ℬ f as described in Task 2, namely a global plan. It considers a configuration space extracted from ​ ℐ M ' based on the initial information and the foreknown structural conditions [21]. The global planner generates a set of via points, ​ ℐ T ​ ℬ v i a , which channelizes the route to approach ​ ℐ T ​ ℬ f .

The module also implements local planning. It receives the current status and some of the upcoming via points, together with the current local map ​ ℬ M imported from the SLAM module based on the LRF sensor. The outcome of the module is a segment of a smooth and obstacle-free path passing near the via points. It can also be extracted from a motion-planning roadmap framework [22, 23].

4.4 Path following

A path follower controller navigates the robot on the given path and returns the resultant pose. The path-following module considers actuator limitations at maximum velocity and acceleration [24-26]. The path follower also avoids obstacle on the robot's route.

4.5 Mobile manipulator controller

Manipulators are usually provided together with their specialized controllers. However, coordinated tasks have to be performed synchronously between the path follower for the mobile base and the arm controller [27]. We use a mobile manipulator controller to close the outer control loop that follows the task commands distributed among the mobile base and the arm in a similar manner to [28].

OPEN IN VIEWER

Figure 3.

The robot's software and hardware modules

4.6 Fault management

Besides software fault detection and fault-tolerant techniques [29], it is necessary to monitor, verify and recover the robot's sensory data and actuator commands from the instrumentation and control viewpoint. This section contains a brief explanation of the fault management module's functionalities. As a general requirement for the fault tolerance of safety-critical systems [30, chap.1], the robot has redundancy in actuation and sensation. Four steerable wheels [31] reduce the risk of immobility caused by the mechanical hardware. The wheel odometry, IMU, laser scanner and camera provide redundancy for the perception of the robot. The fault management module's job includes:

Monitoring the input and output of the other modules.

Verifying the consistency of the resultant sensory information from the different sources, for each sensor as well as results of sensor fusion:

Δ T = B T ​ S 1 ( B T ​ S 2 ) − 1

(12)

where S₁ and S₂ are the localization or detection data from Sources 1 and 2, respectively. For a perfect system, AT becomes the identity 4×4 matrix. It should be noted that the inverse of a transformation matrix is calculated by the transpose of its rotational part as addressed in [28], Therefore, this calculation cannot be singular. Arguments of ΔT represent the inconsistency of the gathered information.

OPEN IN VIEWER

Figure 4.

The loop for the synchronization of the mobile manipulation tasks

Some errors can be solved by outlier detection and the substitution of the fused values. For instance, in the following we describe how, at each segment of the path, the module compensates for small errors between the position and velocity feedback of the wheels.

The localization is obtained by dead-reckoning of the odometry data as described in the previous section. The iMoro mobile platform shown in Figure 1 is used to run the experiments. The desired path that is followed by the mobile platform is a cubic Bezier curve with its control points located at {(0, 0), (2m, 0), (2m, 2m), (0, 2m)}. The path is used with two desired heading profiles. Figure 5 shows the body and legs trajectories for following the path with 180° rotation of the main body. Moreover, Figure 6 shows the same trajectories, this time for −90° rotation of the body. Figure 5 also depicts the estimated location of the body's Instantenous Center of Rotation (ICR) during its movement as red dots.

OPEN IN VIEWER

Figure 5.

Experimental results: the robot's trajectories, 180° rotation

OPEN IN VIEWER

Figure 6.

Experimental results: the robot's trajectories, −90° rotation

OPEN IN VIEWER

Figure 7.

Experimental results: Least squares error before and after the elimination of the outlier leg

OPEN IN VIEWER

Figure 8.

Experimental results: leg number for the most inconsistent leg

The forward kinematics solution is solved both with and without eliminating the leg with maximum e_i Figure 7 depicts the pseudo-inverse error before and after the elimination. From the figure, it is clear that the error is reduced considerably by eliminating the leg with maximum e_i from the forward kinematics solution.

Figure 8 shows the sensor number with the biggest e_i at each sample time. From the figure, it is clear that, at each period, a different leg has maximum e_i. This is due to the change in the dynamic loads acting on each leg during movement. Having one specific leg with maximum e_i all the time could indicate a defect in that leg.

5.1 Hardware architecture

A schematic hardware architecture of the mobile platform is shown in Figure 9. The sensors and actuators are connected to the embedded PC, which runs all of the necessary cognition subsystems. The embedded PC is connected to the motor drives, arm joints, laser scanners, cameras, IMU and encoders. Most of the robot sensors provide the perception of the robot and its environment for object manipulation. The embedded PC is the target of the remote development method described in [32] during its run-time and debugging and the development of its algorithms. It should be noted that any failure in the embedded PC during its run-time can cause the immobility of the robot or a collision with other facilities in the environment.

In order to avoid failure, the authors propose adding a safety anchor topology as a redundant decision-maker for the mobile platform, as shown in Figure 9. During the robot's run-time, in case of any embedded PC failure, the safety PLC can be triggered by the watchdog or supervisory system to manage the mobile platform in escape mode by a reactive controller. Ignoring complex mathematical procedures and high-level algorithms, the PLC solves the navigation problem at the lowest possible level to guide the robot to safe places [33] before the operation of the accelerators. The redundant system can benefit from off-the-shelf, safe and certified components to guaranty the safety of the system.

The LRF sensors provide for the availability or occupancy of any predefined zone by toggling the corresponding signal. The signal goes through the logical circuits of the PLC. By limited conditional statements programmed on the PLC, the system reacts by commanding the motor drives in real-time. Since the logic, codes and connections are reactive, time-invariant and simple, their verification is easier and more dependable. In the same architecture, we can also program and utilize the embedded PC by means of the remote development method described in [32, 34].

OPEN IN VIEWER

Figure 9.

The hardware architecture related to cognition; the yellow parts are the proposed safe components

5.2 SA logic

The hardware described in 5.1 needs a special setup, adjustment and programming. The LRF zones need the warning and alarm zones to be set up similar to the zones shown in Figures 10, 11 and 12. The reason for using the LRFs is their flexibility in the definition of sensitive zones and their compatibility with the safety standards. Generally speaking, the pseudo-code for the PLC can show the reactions between the sensors and actuators in Table

1. It should be noted that the system design is reactive. However, for the clarity of the commands' order, it is shown as a sequence of commands.

OPEN IN VIEWER

Table 1.

The pseudo-code for the PLC

1 Initialize the sensors.

2 Disconnect the actuators from the embedded PC.

3 Stop the wheels.

4 Align the steering.

5 Read LRF output.

6 Adjust the drive speeds by visibility of obstacles.

7 Steer the robot.

8 When robot arrived, finalize the movement.

9 Return to 5.

OPEN IN VIEWER

Figure 10.

LRF zones, crab steering

OPEN IN VIEWER

Figure 11.

LRF zones, robot spins around

OPEN IN VIEWER

Figure 12.

LRF zones, cross movements

Furthermore, in the solution, we consider the conditions in SFs. For instance, it is common that the experiment infrastructures are located at one side of a tunnel or a corridor – as they are at CERN LHC or GSI/FAIR – and the transportation corridor usually runs along beside them in parallel or circular patterns. Therefore, within the path-selection routines, we give higher priority to following the simpler wall of the corridor. According to Table 1, let us divide the navigation into two parts: the determination of the robot's velocity (driving), and steering. In the upcoming paragraphs, we describe the overall idea and the benefits of a 4WS mobile robot for this purpose.

The robot's steering capabilities, as described in Section 3, include a vast area of selection and calculation so as to determine it for each wheel of the robot [24, 35]. However, a robust solution, a dependable implementation and a reliable verification method for such calculations become complicated, especially if a safe solution is required. Recall that the robot's immobility is a high-severity failure. For the purpose of simplification, we assume that the robot has only two types of steering: crab steering and spin around to keep the detection [36], planning [37], kinematics, and control [26] simpler than contemporary solutions.

5.2.1 Crab steering

Crab steering means that the angular velocity ω _B shown in Figure 2 and mentioned in (2b), becomes approximately zero. Fn other words, the instantaneous center of rotation (ICR) goes to infinity. As such, (2b) becomes:

v i B v ^ ​ i = v B v ^

(13)

This means that the velocity vectors of the body and legs become parallel (or equal) to each other. In such a case, it is not necessary to solve the system of kinematic equations of the robot and the direction of B_v^{^}; body's velocity vector defines the wheels' steering angles B_v^{^}_i. Any change in the steering comes from the change in the body's movement direction (not the heading). In this case, the robot can continue with high-speed linear motion up to a sensible change in the velocity direction regarding the actuator limitations.

Crab steering activates when the robot is approximately parallel to the simpler wall of the corridor while the robot has obstacle in front as shown in Figure 10. The crab steering maintains the distance from the side obstacles or walls without lowering speed. The magnitude of velocity vector v is proportional to the proximity of the obstacles in front of the vehicle. It should be noted that, instead of a distance measurement, a corresponding number for the zone is considered.

5.2.2 Spin around

There exists another method which leads to a significant simplification of the complicated path-follower kinematics. The 4WS robot can turn around on the spot without any linear movement, or negligible translational movements. In this case, the ICR converges into the centroid of the robot. In this case, another term of the velocity analysis becomes zero. The kinematics relations in (2b) become:

v i ​ B v ^ ​ i = ω B ( z ^ × ​ B ℓ ​ i )

(14)

which shows the constant angles of the wheels perpendicular to ​ B ℓ ​ i . Therefore, the change rate of the heading defines the driving velocity of each wheel v_i while v v ^ – the linear velocity of the body – becomes zero.