Combined Path and Motion Planning for Workspace Restricted Mobile Manipulators in Planetary Exploration

A highly restricted workspace of the robotic arm may hinder to perform safely any autonomous mobile manipulation task with planetary exploration rovers. To ensure mission safety as well as high efficiency, a coupled path and motion planner for mobile manipulation is presented in this work. First, a Fast Marching Method based path planner generates a safe trajectory to reach the goal vicinity, avoiding obstacles and non-traversable areas in the scenario. The path planner is able to control the final rover orientation to ensure that the goal is finally reachable by the arm. Second, a 3D Fast Marching Method based motion planner generates the arm joints motion profile, by creating a 3D tunnel-like cost volume surrounding the already computed rover base trajectory. This tunnel makes use of an offline-computed safe workspace of the manipulator, thus ensuring that no self-collision will occur during the planned motion. The presented algorithm has been tested with multiple simulation experiments, benchmarked with an off-the-shelf motion planning library, and validated in a field test campaign with the rover SherpaTT of DFKI Robotics Innovation Center. The tests consisted in SherpaTT approaching an interesting area on the scenario and performing a mobile manipulation sample scanning operation. These experiments have demonstrated that the proposed motion planner increases efficiency as well as ensures mission safety. This is thanks to, on the one hand, a coordinated base-arm movement that results in maximized efficiency in time terms, and, on the other hand, considering the manipulator workspace offline in the mobile manipulation motion planner to guarantee self-collision avoidance.


I. INTRODUCTION
Planetary rovers are playing a crucial role in space exploration nowadays. Space agencies are increasing their budget to develop this kind of robotic vehicle since they provide a wide scientific return and avoid the risk of sending humans outside Earth [1]. From Spirit and Opportunity [2], passing The associate editor coordinating the review of this manuscript and approving it for publication was Yangmin Li . through Curiosity [3] and finishing with Perseverance [4], planetary rovers are usually equipped with a robotic arm that holds several scientific tools, allowing the system to perform scientific measurements in a broad workspace with high flexibility, what is called mobile manipulation.
Although interesting advances have been lately made in robotic teleoperation, including the use of recurrent neural networks to enable manipulation by means of hand gestures [5], it remains a challenge for planetary exploration. This is due to the small duration of the communication window and the considerable latency between Earth and Mars [6], [7]. Recent works are employing deep learning to cope with teleoperation in high latency use cases [8]. Nevertheless, these methods are employed for environments with a latency around 500 ms, which is still far lower than the Mars case, with 20 min on average.
As a result, this type of planetary exploration mission generally requires a high degree of autonomy. Autonomy comes alongside time and energy savings, besides an increase in the safety and the scientific return of the mission [9]. An efficient autonomous guidance, navigation, and control architecture for planetary exploration rovers is, thus, a general requirement [10]. Summarizing, guidance refers to planning the best way to reach a certain goal, navigation consists of localizing the vehicle continuously inside a dynamically updated map using a perception system, and control ensures that the vehicle is correctly commanded to follow the generated plan. Nevertheless, increasing the autonomy level is notably difficult for mobile manipulation. In particular, the motion planning problem, guidance, is hindered by the existence of infinite solutions due to the high kinematic redundancy of these systems. This is a well-known problem in the literature [11], although it has been generally solved for industrial and household applications. Unfortunately, planning the motion for mobile manipulation in planetary exploration presents further challenges, such as high motion efficiency, low computational effort, predictability, and robustness. The latter is particularly critical to ensure compliance with mission-critical constraints, like obstacle and self-collision avoidance.

A. RELATED WORK
The energy and time constraints of planetary missions demand high efficiency of the planned motions, which cannot be achieved with decoupled base-arm solutions like [12] since the rover is missing the chance of moving the arm while the base is reaching the goal. This can be solved with coupled motion planners like the so-called Optimized Hierarchical Mobile Manipulator Planner (OHMP) [13]. OHMP is an example of sub-optimal motion planning which includes, first, a two-dimensional mobile base path planning phase with Probabilistic Road Maps (PRM) [14] and, second, the manipulator trajectory planning with collision avoidance.
Considering the limited and packed processing resources of space exploration systems [15], it is required a computationally lightweight motion planner with low variability. This is a main drawback of optimization mobile manipulation motion planners [16], being their convergence speed expressly sensitive to constraints [17] such as actuator limits, the base non-holonomic constraints or collision avoidance. Latest advances in optimal control show that these safety-critical constraints can be satisfied in a computationally effective manner using Control Barrier Functions (CBFs) and Control Lyapunov Functions (CLFs) [18], which have been applied to reactive planning and control of complex bipedal robots in unstructured scenarios [19]. Nevertheless, reactive planning and control are not applicable to planetary exploration due to the requirements of high long-term predictability and robustness, needed to be sure that the mission is not going to be jeopardized before starting the motion. Thus, holistic reactive approaches for mobile manipulation are not suitable, such as the one presented in [20] or the reactive planner proposed in [19], since there is no long-term awareness of how the system is going to behave. Besides, these high-frequency reactive planning capabilities are not required in static and immutable planetary environments, where no dynamic events take place.
Avoiding collisions is particularly critical, among the aforementioned problems, to ensure mission success in planetary exploration. Specially self-collisions, due to the rover carrying multiple scientific and engineering tools that restrict the free movement range of the manipulator. This topic has been studied in the motion planning literature for mobile manipulation in many different manners. It is usual to employ simplified collision geometries of the robot, e.g. several spheres were used in [21] to represent the collision shape of the mobile manipulator. Then, a Rapidlyexploring Random Tree (RRT) GoalBias algorithm [22] for the whole body was used to plan collision-free motions considering the collision shape, and including a further post-processing method to shorten and smooth the path. Another interesting simplified collision avoidance method, where 2D projections of the 3D space were generated to consider manipulator collisions with the environment, was presented with OHMP [13]. Finally, a different point of view for collision avoidance was depicted in [23], where a weighted Jacobian was integrated to stop joint movements that get close to position constraints, like self-collisions. This was included in a complete optimization scheme for motion planning of a 10 Degrees of Freedom (DoF) mobile manipulator, including joint limits and manipulability maximization.
Although suitable in non-cluttered scenarios, the use of simplified collision models may not be functional when the manipulator workspace is highly restricted, since these simplifications could allow unfeasible motions and vice versa. In these cases, it is common to use robot descriptors like Unified Robot Description Format (URDF) [24], which gathers in a straightforward manner the kinematics, dynamics, visuals, and collisions models of a robot. URDFs can be directly used within collision detection algorithms, like the one included in the Dynamic Animation and Robotics Toolkits (DART) [25].

B. MOTIVATION AND CONTRIBUTIONS
In view of the current state of the art, a combined path and motion planner like OHMP [13] arises as a good approach to tackle the motion planning problem for mobile manipulation in planetary exploration, considering the aforementioned requirements on efficiency, collision avoidance, and low computational cost. Nevertheless, further research is required, in two main senses.
On the one hand, regarding the rover base path, the stage of approaching the goal has to guarantee that the rover will be located in the correct orientation to place the goal within the manipulator workspace. In consequence, planning and controlling the final orientation of the rover base is essential to avoid unnecessary maneuvers, which induce wasting energy and time. An algorithm that could solve this problem was presented in [26], where a globally optimal path planner that considers curvature and orientation was proposed. However, this algorithm is based on the Fast Sweeping Method (FSM) [27] which is only recommended for simple path planning scenarios with few obstacles [28]. In other cases, other advanced path planning methods are computationally cheaper, such as the Fast Marching Method (FMM) [29]. This algorithm has recently demonstrated promising results in terrestrial [28] and planetary exploration scenarios [15], being able to generate optimal and smooth paths with a similar computational cost to any state-of-the-art path planning algorithm. In fact, for a properly defined cost map representing a particular scenario, FMM always returns a safe (avoiding obstacles or non-traversable areas) and globally optimal (with maximized time-energy efficiency) path to reach the goal, if existent.
On the other hand, regarding the manipulator motion planner, it has to rigorously consider the collisions with the environment and the self-collisions, which is particularly challenging bearing in mind the highly restricted arm workspace. To tackle this, the fact that the manipulator motion is based on a previously computed rover path provides several advantages. First, if the planned rover path uses globally optimal approaches like FMM, then the complete motion plan is partially-optimal, and the efficiency is increased. Second, the arm motion range is enclosed in the surrounding volume of the rover path, thus the complexity of the problem is substantially reduced, and so is the computational cost of the algorithm. Third, if the path planner finds a solution to place the rover close to the goal considering the forthcoming retrieving operation, then it is guaranteed that the operation is feasible, i.e. a safe motion plan for the manipulator will be found, improving the predictability and robustness of the algorithm.
Hence, in this paper, an efficient and robust motion planner is presented, which combines path and motion planning for planetary exploration mobile manipulation with a highly restricted arm workspace. The motion planner proposed in [30] is extended, resulting in the following contributions: • An improvement in the Dynamic Multilayered (DyMu) path planner [31], to ensure the base trajectory places the rover with the goal sample inside the manipulator workspace, by planning and controlling the final rover orientation during the stage of approaching the goal.
• A novel motion planning algorithm for the manipulator that considers the real workspace of the arm to avoid collisions, hence directly obtaining safe plans to reach the goal sample location. Besides, the planner increases the efficiency of the motion plan thanks to a coordinated base-arm motion. To this end, the motion planner plans firstly the end effector path using FMM 3D, considering the offline computed collision-free workspace of the manipulator w.r.t. the mobile base trajectory, to later generate the arm joints motion plan.
• A custom coupled arm-base trajectory controller, which ensures that the motion plan is properly followed by the system.
• A validation of the proposed method by means of several simulations and field tests emulating a sample scanning operation, i.e. a dummy scientific measurement with the arm performing a sweeping movement on top of an interesting area in the scenario. This validation was carried out with the DFKI rover SherpaTT [32], shown in Figure 1.
• A comparison with an off-the-shelf motion planner, which highlights the strengths and weaknesses of the proposed approach.

C. PAPER STRUCTURE
The rest of the paper is organized as follows. The first contribution, i.e. the developed path planning algorithm with control of the final rover orientation, is explained in Section II. Later on, Section III details the second contribution, the motion planning algorithm for mobile manipulation with a highly restricted arm workspace, and also details the coupled trajectory controller. A platform survey is presented in Section IV, which describes in detail 78154 VOLUME 11, 2023 Authorized licensed use limited to the terms of the applicable license agreement with IEEE. Restrictions apply.
the rover SherpaTT used in the later experiments. Finally, the simulation tests and their outcome are explained in Section V, Section VI shows the benchmark of the proposed motion planner to an off-the-shelf one, and Section VII presents the field tests and the lessons learned from them. Lastly, Section VIII concludes the paper and proposes some future work.

II. PATH PLANNING WITH FINAL APPROACH CONTROL
Path planning consists in finding a feasible path for the rover base to get safely to the goal, i.e. avoiding obstacles and nontraversable areas. To do so, path planners like FMM require a description of the scenario as a cost map, which represents with scalar costs the areas that are easier or harder to traverse, or the ones that should be avoided. To generate such a cost map in static and unstructured environments the first step is to obtain a Digital Elevation Map (DEM) of the scenario, which is a 2D map where each node indicates the height of the terrain at that position. The DEM can be extracted from aerial or orbital imagery (global DEM), or from the robot (local DEM), using exteroceptive sensors such as cameras or laser scanners. An example of the procedure to generate the cost map from a local DEM is shown in Figure 2. From the DEM, Figure 2a, the scenario can be characterized by estimating terrain features, detecting obstacles and non-traversable areas. The obstacle detection uses different metrics, such as the roughness, shown in Figure 2b, which is directly related to the spherical deviation of the vectors perpendicular to the surface. High roughness is derived from abrupt changes in the DEM, which indicates the presence of obstacles. Additionally, areas with high slopes, shown in Figure 2c, may also not be traversable for the robot and thus must be characterized as obstacles. The traversability map T , Figure 2d, gathers this classification, assigning a category to each of the grid nodes, e.g. obstacles or safe nodes. It also includes a category for obstacle dilatation, which considers the size of the robot to avoid getting too close to unsafe areas.
Thereafter, the traversability map T is used to produce the cost map B , shown in Figure 2e. The cost map B assigns high costs to the nodes labeled as obstacles or non-traversable in T , and low costs to the safe ones. Additionally, to increase the smoothness of future paths, cost gradients are generated in the non-traversable nodes' proximity, as defined in (1), with n = [x, y] ∈ T a node in the map, k 2D a configurable parameter defining the cost sharpness, and D 2D (n) the Euclidean distance from the node n to the closest non-traversable node in T . Remark that D 2D is calculated using standard computer vision algorithms within OpenCV. 1 Once the cost map B is generated, FMM models the propagation rate of a wave by numerically solving the Eikonal 1 https://opencv.org/ equation, i.e. a non-linear partial derivative equation. The wave expands on B visiting every node n from the goal node n g , thus generating what is called the cost to go ϒ B (n, n g ). This represents the required cost to reach the goal n g starting from each node n. Particularly, the propagation rate of the wave at any node n is the corresponding cost of the node B (n), as stated in (2), with slower propagation for higher costs. Note that, if the propagation starts from the goal, its cost to go is zero (no effort is required to reach the goal if starting from the goal, ϒ B (n = n g , n g ) = 0).
The final objective of FMM is to generate the optimal path B = {γ B,0 , γ B,1 , . . . , γ B,l , . . . , γ B,L } to reach the goal n g starting from n 0 , which is composed of L waypoints γ B,l . To do so, FMM solves the optimization problem described in (3)(4). It is defined as minimizing the cost accumulated along the path B ( B (n 0 , n g , v)), with B (n 0 , n g , v) the continuous function returning a pointp ∈ B at a particular path length v from the starting node n 0 , being v g the total length of the path.

Minimize
Although functional in most use cases, this path planning approach is not sufficient for a rover that has to approach a sample for a mobile manipulation operation, since the path planner has to place the rover close enough to the sample and with proper final orientation, to ensure that the sample is finally inside the manipulator workspace. This is further hindered in the case of a highly restricted manipulator workspace since only some particular rover poses leave the sample inside that workspace. For instance, if there is not enough space in front of the rover for manipulation tasks, the planner has to leave the rover with a certain angle w.r.t. the goal or even turned 90 • to perform the manipulation on one of the rover sides.
To ensure the goal is finally inside the arm workspace, two aspects need to be considered. On the one hand, the last segment of the path has to be a straight line towards the sample, thus restraining the rover orientation in those last waypoints. This is necessary because, in the vicinity of obstacles, approaching the goal in a straight line benefits the existence of a safe maneuver to place the sample inside the manipulator workspace. Since the path planner relies on FMM, which produces a continuous and optimal path given a cost map, the shape of the resulting path is subject to variations in the cost of the nodes. Under this premise, the Frontal Approach Cost Editing (FACE) algorithm is presented, which modifies the cost map in the close neighborhood of the sample to ensure that the rover approaches it following a straight line. On the other hand, the later trajectory controller must steer the rover to VOLUME 11, 2023 leave the sample inside the arm workspace, as aforementioned. Performing such an orientation-controlled but armcompliant final maneuver requires a tailored modification of the trajectory controller, thus, the Last Section Control is proposed. These two improvements together, FACE and the Last Section Control, ensure the goal sample is reachable by the arm, as further explained below.

A. FRONTAL APPROACH COST EDITING (FACE)
The main purpose of FACE is to modify the cost map in the proximity of the sample to ensure that the last waypoints of the path are headed toward the target. This modification is exemplified in Figure 3, with the rover going from an initial position (yellow dot) to a position close to the sample (red dot). First, the traversability map is generated, shown in Figure 3a. The planner expands the area of the obstacles to consider the shape of the rover, as aforementioned, although FACE includes two different dilatation areas. The nodes within the first dilatation (green) are not reachable by either the center of the rover or the manipulator. The second dilatation (orange) indicates nodes where the rover center cannot enter, but the manipulator can reach. Note that, even though the rover does not stop on top of the goal, FMM initially plans the path connecting the initial and the goal positions. This is to, in a later step, remove the last portion of the path so the rover stops earlier at a certain distance from the sample, under the maximum arm reachability, what is called the sampling area (grey).
The traversability map is used to generate the cost map as in Figure 3b. Here, the values of cost, positive and non-zero, are proportional to the proximity to non-traversable nodes as in (1). Those non-traversable nodes are colored in black and their cost is infinite. The 2D path that FMM gets between two points is affected by how these values of cost are assigned. An area with constant cost will make the paths go straight, while a gradient of the cost will curve the paths. Therefore, the sampling area should be set as a uniform cost to generate straight paths to approach the sample. However, there may be obstacles in the vicinity, and they must not be omitted.
FACE is aware of this necessity to make the paths go straight meanwhile avoiding existing obstacles, and edits the cost ensuring the resulting path complies with those requirements. As can be seen in Figure 3b, the non-traversable nodes not only include obstacle and dilatation nodes, but also a series of nodes that are located within a ring centered at the sample location. This ring is the one formed by the blue and yellow circumferences, whose radius values are configurable by the user. FACE determines which nodes from this ring are set to obstacle and which are not, forming an opening so the path goes through it, straight towards the sample.
For that purpose, FACE uses an application of FMM called shadowing, which was originally presented in the work of Sethian [29]. In a few words, given a point and obstacles around it, the shadowing serves to project shadows as if the point in question was a light source. In this case, such a point is the sample location. The projected shadows pass through the ring, converting traversable nodes into non-traversable ones. Only those traversable nodes inside the ring that are not touched by the projected shadows form the opening. This means any path passing through such nodes will later go straight toward the sample location as if they were following the light rays. Finally, the resulting path from using FMM is shortened by removing the last waypoints as aforementioned, as the rover is supposed to stop at a certain distance from the sample. In Figure 3b the last waypoints entering the straight area (which contains the sampling area defined in the traversability map) are the ones that are removed.

B. LAST SECTION CONTROL
The trajectory controller ensures the robot follows the planned path under certain conditions. For instance, the conservative pure-pursuit algorithm ensures the rover does not deviate from the path beyond a certain distance [33]. This can be seen as forming a corridor that delimits the area where the rover can be located. Inside this corridor, the conservative pure-pursuit provides the rover with motion commands depending on the waypoints it tracks. Nevertheless, as aforementioned, the rover must orient itself to make the sample reachable by the arm at the last waypoints of the path. This adds a new problem that the trajectory controller must account for. Not only the rover has to drive performing maneuvers that reduce its deviation with respect to the planned path, but it also has to reach this heading at the end. A feasible solution would be to decouple the problem. First, the rover is focused on reaching the last waypoint. Second, it performs a turn on the spot, or point-turn, to set its heading to the desired one. However, this solution is not efficient in time terms. In order to reduce the number of maneuvers at the end, a custom control approach named Last Section Control is presented, which takes over the previous VOLUME 11, 2023 78157 Authorized licensed use limited to the terms of the applicable license agreement with IEEE. Restrictions apply. Last Section Control scheme. On the left, the scheme depicts how the rover cannot arrive at the final waypoint and orientation using a single turning radius for a constant full-Ackermann maneuver. On the right, the vertical and horizontal distances between the rover and the sample serve as input to the Last Section Control to get the curvature the rover should use to turn.
trajectory controller when the rover is close enough to the sample. Figure 4 shows a scheme of how the Last Section Control works. As can be checked, a single full-Ackermann maneuver cannot ensure the rover is well positioned with respect to the sample according to where its arm is placed (colored in purple). In fact, reaching the last waypoint of the path with a certain heading requires starting from a unique position (colored in orange). It is then imperative to control the rover to not only make it stop at a position close to the final waypoint (admitting a certain level of error) but also reduce the distance between the optimal pick-up position (in red) and the location of the sample. Having all this in mind, the Last Section Control takes as inputs only the vertical and horizontal distances from the rover's central position to the sample location. According to these values, the Last Section Control uses a look-up table, plotted at the right side of Figure 4, to dynamically retrieve the curvature of the turning full-Ackermann maneuver the rover must perform. As the rover advances this look-up table is consulted in an iterative way, providing a different value of curvature depending on how the vertical and horizontal distances evolve over time. Following the example of Figure 4, if the horizontal distance is between −0.3 to −0.5 m, the rover can practically just go straight, as the optimal pick-up position is more or less in line with the sample with none or little horizontal distance error. For values of horizontal distance higher than −0.3 m, the rover has to make a right turn as it advances, which is more pronounced as it comes closer to the sample.

III. MOTION PLANNING WITH RESTRICTED ARM WORKSPACE
Once the rover base path is computed, it is used by the mobile manipulation motion planner to generate the arm joints profile of the arm throughout the trajectory, generating a coordinated base-arm motion. The motion planner aims at ensuring the operation safety, i.e. avoiding collisions, in an efficient manner. To do so, a representation of the safe manipulator workspace in the form of a reachability volume is generated offline, only once, prior to planning the manipulator movements. The reachability volume is obtained by employing the collision detector algorithm of DART, to detect self-collisions and collisions with the environment for any specified configuration of the system joints. DART makes use of the URDF of the rover, mainly requiring the kinematic and collision models of the system.
The reachability volume is used by the motion planner, altogether with the previously computed rover base path, to generate the arm joints profile to get to the goal, in three main steps. Firstly, a 3D cost volume is generated, formed by a tunnel surrounding the rover base trajectory. This tunnel represents the manipulator cartesian workspace at each waypoint of the rover base trajectory, using the reachability volume but also considering the scenario (obstacles, soil shape). This way, the later computed motion plan is necessarily safe if encompassed inside this tunnel. Secondly, the manipulator trajectory is planned inside the tunnel using FMM in a 3D version. Note that we refer to the manipulator path in the following since it could be the path for the end effector or for the wrist. Third, the manipulator joint profile is obtained by establishing a relation between the base and the manipulator trajectories. For each waypoint of the base path, a suitable waypoint of the manipulator trajectory is assigned, and then the corresponding arm joints configuration is computed making use of the Inverse Kinematics (IK) model of the manipulator.
Finally, a coupled trajectory controller is required to ensure that both the base path and the manipulator joint profile are properly tracked by the system. The reachability volume computation, the motion planner itself, and the coupled trajectory controller are explained in further detail below.

A. MANIPULATOR WORKSPACE REPRESENTATION
Let us define W as the manipulator workspace, i.e. a set of discrete positions p = [x, y, z] ∈ R 3 where the manipulator end effector can be ideally placed considering its movement range. Then, W R ⊂ W is defined as the safe manipulator workspace considering self-collisions, i.e. a Boolean 3D map indicating for each p if the arm is safe and reachable (true), or if the manipulator cannot reach the position or is in a collision state (false). Then, the concept of reachability volume R is introduced. In a similar way to the Capability Map defined in [34], R is a 3D map ( R ∈ W R ) that indicates to what extent it is dangerous to place the manipulator in every node w.r.t self-collisions. The main benefit of using R within the motion planner is that, if every manipulator configuration leaves the arm inside R , it is guaranteed that no self-collision will occur during the execution. This is assuming that the arm trajectory if isArmColliding(q) then 9: W R (p) ← false 10: break 11: end if 12: end for 13: end for 14 17: if W R (p) is true then 18: 19: else 20: end if 22: end for controller does not deviate substantially from the plan but, if it does, the chances of self-collisions are also minimized, since R will maintain the arm the farthest possible from the closer self-collision. Besides, since R is calculated only once, offline, it is substantially advantageous to reduce the computational cost of the motion planner, being R unique for a robot as long as its kinematic characteristics do not change.
To compute R it is necessary, first, to estimate the safe manipulator workspace W R , which requires several tools. On the one hand, URDF is used to 3D model the physical properties of the system, including the collision geometry, which is slightly expanded to ensure safety even in case of inaccuracies. On the other hand, the URDF is embedded in a collision detector algorithm, in this case, the one provided by DART. This is an open source library where kinematic and dynamic algorithms are developed, in the fields of robotics and computer animation. The collision detection checks whether there is a collision or not for a certain scenario and configuration of the robot joints. Lastly, the IK of the manipulator is also necessary, to correlate each position p ∈ W R with its corresponding arm joints configuration q. Note that, in this work, each W R is representative of only one solution of the IK, i.e. up/down elbow and right/left shoulder.
To obtain W R , each position p inside the manipulator workspace W has to be checked. For every particular p and every possible orientation φ = [ϕ ϑ ψ] (roll, pitch, yaw) of the end effector, the IK is used to extract the arm joints configuration q to reach that end effector pose (position p and orientation φ). Then, DART checks if q leads to a collision, by means of the URDF model of the robot. Whenever a collision is found, the position p is categorized as unsafe in W R , and a new one is selected. On the contrary, if every end effector orientation φ is covered and no collision is detected, then p is classified as safe in W R .
Finally, R is generated by defining a cost shape inside W R , which indicates to what extent it is hazardous to place the manipulator at each position p. For this purpose, the distance to collision volume D 3D is required. This is a 3D discrete volume (D 3D ∈ W R ) that, for each position p, indicates the distance to the closest unsafe position in W R . Thus, unsafe positions in W R have assigned distance 0 in D 3D , and safe positions in W R have assigned the Euclidean distance from that position to the closest unsafe one in W R . Using D 3D a linear cost has been defined, which is inversely proportional to D 3D as stated in equation (5).
where R (p) is the cost associated with the position p; k 3D is a parameter that defines the sharpness of the cost inside R , and D 3D (p) is the distance of the position p to the closest unsafe position in W R . Remark that the minimum cost in R is 1, as needed for correct performance of the later path planning algorithm with FMM. Additionally, the cost of the non-reachable or non-safe nodes in W R is set to infinity.
In the particular case of a manipulator with a spherical wrist, the IK model is easily obtainable with a kinematic decoupling of the position and orientation problems [35]. Defining q = [q p q o ], the first DoFs set the wrist position (position joints, q p ) and the last 3 DoF set the end effector orientation (orientation joints, q o ). Making the most of this kinematic decoupling, the safe workspace W R of a manipulator with a spherical wrist represents all the positions p for the wrist where the full range of the orientation joints Q o is reachable and safe, i.e. without self-collisions. This methodology simplifies substantially the motion planning procedure, as will be explained in the posterior subsection.
The procedure to generate R for a manipulator with a spherical wrist is, thus, summarized in algorithm 1. First, according to the kinematic configuration of the manipulator, the wrist workspace W (getManipulatorWorkspace) and the maximum range Q o of the orientation joints q o (getOrienta-tionJointsRange) are obtained. Second, the safe workspace of the manipulator wrist W R is computed, checking if the manipulator is colliding (isArmColliding) for every position p = [x, y, z] ∈ W and configuration of the orientation joints q o ∈ Q o . Third, once W R is generated, the Euclidean distance volume to the closest collision D 3D is calculated (computeDistanceToCollisions), using again standard computer vision techniques from OpenCV, but in 3D. This distance volume D 3D defines the cost function inside the reachability volume, altogether with k 3D , parameter that configures the cost function sharpness (getCostSharpness). Finally, the reachability volume R is generated, checking every position p = [x, y, z] ∈ W , and assigning the cost in (5) if the position is safe (W R (p) is true), or infinite cost if the position is not reachable (W R (p) is false).

B. MANIPULATOR MOTION PLANNING
Before generating a coupled motion, first of all, it is required to decide when to start the arm motion w.r.t. the rover base path. That is, to estimate the waypoint γ B,α of the base trajectory where the arm should start moving, with α the index of that waypoint. This is necessary to ensure that the manipulator finishes its movements just when the rover gets to the last waypoint of the trajectory, to avoid idle arm time if finishing the arm motion too soon or idle rover time if finishing too late. The selection of γ B,α depends on two main factors. On the one hand, the time required for the rover to follow the trajectory, which is estimated subject to the rover base nominal speed and the length of the trajectory B . On the other hand, the time required for the arm to perform the complete motion, which can be estimated based on the initial and final arm joints' configurations, and the arm joints' speed. Note that the final arm configuration can be computed dependent on the goal pose and the last rover base waypoint.
Then, a 3D cost volume C is generated on top of the rover base trajectory, following algorithm 2. First, the rover base trajectory B (getRoverBaseTrajectory) and the waypoint where to start the arm movements γ B,α (getWaypointStar-tArmMovement) are fetched. Second, the cost volume C is initialized (initialize3DCostVolume) considering every node as an obstacle, with infinite cost. Third, the deployment distance δ is defined (getDeploymentDistance). This is a configurable parameter that sets the fore distance (X-axis) from the arm base to the manipulator end effector (or wrist) at each waypoint, eventually characterizing when the arm will be mainly mobilized during the rover movement, as is later explained in detail. The deployment distance, clearly, always has to be smaller than the maximum range of the manipulator. Fourth, a slice of the reachability volume is extracted (getReachabilityVolumeSlice), ⊂ R , located in the X-axis at δ distance. This slice is used to build the tunnel cost volume, as follows. At each waypoint γ B,l in the rover trajectory B (from γ B,α onwards), is placed in the corresponding nodes of the 3D grid C by means of simple geometrical transformations (getTransform), being a T b the transformation matrix from a to b. Repeating this process for every waypoint in B ends up creating a tunnel-shaped cost volume, as can be observed in the 3D cost volume example shown in Figure 5a. Note that the tunnel is built differently at the first and last waypoints. The first waypoint places every slice from the beginning of the reachability volume R , in the X-axis, to the deployment distance δ. Conversely, the last waypoint places every slice from δ until the end of R . This way, the tunnel is completely closed, which reduces the computational cost of the later path planning algorithm.
Finally, the generated 3D cost volume C is used to plan the arm path M = {γ M ,0 , γ M ,1 , . . . , γ M ,s , . . . , γ M ,S } inside the tunnel. The planner used for this purpose is FMM Algorithm 2 Computation of the 3D Tunnel Cost Volume C 1: B ← getRoverBaseTrajectory() 2: γ B,α ← getWaypointStartArmMovement() 3: ← getReachabilityVolumeSlice(X-axis, δ) 6: for γ B,l in B from γ B,α do 7: for p = [x, y, z] in do 9: γ B,l T p ← getTransform(p) 10: end for 13: end for in a 3D version, which still provides optimal, smooth, and continuous trajectories with low computational cost [29]. FMM3D is defined similarly to standard FMM, as in (2-4), but considering now the height z as a third dimension in the 3D cost volume C and its nodes. The generated manipulator motion will be partially-optimal thanks to the use of FMM3D, in the sense that it is optimal in compliance with an already optimal rover path, but does not consider the mobile manipulator problem as a whole, which could lead to local minimum solutions.
Before obtaining the definitive arm joints profile = {q 0 , q 1 , . . . , q l , . . . , q L }, which indicates the joints position q l of the manipulator at every waypoint γ B,l of the rover base trajectory, it is necessary to establish a relation between the rover and manipulator trajectories, B and M . Each rover waypoint γ B,l is assigned with one or more waypoints of the wrist trajectory γ M ,s , subject to the previously defined deployment distance δ. This assignment takes place as follows. Starting from the last rover waypoint γ B,L , each γ B,l gets assigned every manipulator waypoint γ M ,s that is at a distance lower than δ from the current base waypoint γ B,l . The current base waypoint γ B,l will, thus, seize some assignments from the base waypoints that have already been checked. This procedure maintains the arm at δ distance from the rover since the arm will always try to reach a manipulator waypoint γ M ,s that is at δ distance from the current base waypoint γ B,l .
As a result, the motion planner can be configured to behave differently based on the deployment distance δ, as can be observed in Figure 6. A long δ (longer than half of the workspace W R size, δ b ) causes the arm (green) to mobilize early during the operation, keeping the end effector further from the rover body. This approach is called beginning deployment. Conversely, a short δ (shorter than half of the workspace W R size, δ e ) leads to the opposite effect, i.e. the arm (purple) mainly moves at the end of the operation, maintaining the end effector closer to the body, what is called end deployment. Additionally, there exists the possibility of increasing progressively the deployment distance (from δ e to δ b ) during the trajectory, which generates smoother arm motions. Called progressive deployment, this approach is 78160 VOLUME 11, 2023 Authorized licensed use limited to the terms of the applicable license agreement with IEEE. Restrictions apply.  very interesting since it optimizes the efficiency of the motion plan by distributing equally the arm motion throughout the whole rover base trajectory, as it is deeply analyzed in Section V.
Finally, the complete manipulator motion profile is generated by means of the IK model of the manipulator.
At each rover waypoint γ B,l , the IK generates the corresponding manipulator configuration q l , sequentially obtaining the motion profile. An example of manipulator trajectory and its corresponding arm profile is shown in Figure 5b. As can be observed, it corresponds to a progressive deployment configuration of the motion planner, i.e. the arm is smoothly deployed during the motion. Remark that manipulators with spherical wrists have crucial advantages when computing . In the first place, the position joints q p can be obtained with the IK of the wrist. This reduces the computational cost of the algorithm since the complexity of the IK of the wrist is lower. In the second place, the orientation joints q o can be linearly deployed considering that the whole manipulator trajectory is safe, as aforementioned. This linear deployment of q o is simple, predictable, optimal, and computationally negligible.

C. COUPLED BASE-ARM TRAJECTORY CONTROLLER
It is necessary to use a coupled trajectory controller to execute the generated plan. This component has to ensure, on the one hand, that the rover base properly tracks the base trajectory B , as explained in Section II. On the other hand, it also has to command the adequate position and/or velocities to the arm joints to track the motion profile , depending on the current rover base waypoint γ B,l . Clearly, executing both motion plans (base, arm) coordinately is not straightforward, since the time required to reach the next waypoint γ B,l and configuration q l will differ during the execution. To avoid this, a coupled base-arm trajectory controller is proposed.
The coupled controller works as follows. First, the modified conservative pure-pursuit trajectory controller, which includes the Last Section Control presented in Section II, generates the commands to follow the rover base trajectory, ensuring that the rover is always close to the trajectory inside the safety corridor. Second, the nominal speeds of the rover base and the manipulator's joints are used to estimate the required time for both systems to reach the next waypoint γ B,l and configuration q l . Third, if one of the systems (base, arm) needs more time than the other to reach the next waypoint, which is likely to happen, then the other system is slowed down proportionally, to ensure both systems reach the next waypoint γ B,l and configuration q l at the same time, as planned. Even though this approach slows down the whole motion if one of the systems is not fast enough, the coordination of both systems is significantly faster than a decoupled solution, as will be demonstrated in Section V.

IV. PLATFORM SURVEY
The platform used within the later simulation and field tests is SherpaTT, a legs-on-wheels rover equipped with a 6 DoF manipulator, developed at DFKI Robotics Innovation Center. Its hybrid locomotion enables walking and driving capabilities, i.e. an active suspension system that increases mobility on irregular terrains. Besides, the 6 DoF robotic arm holds an Electro-Mechanical Interface on its end effector, allowing the manipulator to retrieve different tools, placed in four mounts on the central body, to perform a wide variety of tasks. The rover has an Emlid Reach GPS with Real Time Kinematic corrections and two Global Navigation Satellite System antennas for ground truth positioning, an Inertial Measurement Unit mainly used to estimate the rover 3D orientation, and a Velodyne HDL-32E Lidar for mapping. Finally, the rover is also equipped with four NIR Basler acA2040-25gmNIR cameras, one stereo pair of Localization Cameras on top of an avionics box on the foremost side of the rover, and another stereo pair of Navigation Cameras on top of a PTU-E46 Pan & Tilt Unit from FLIR Systems, which is placed on a mast located also at the front of the rover. The cameras are used for perception purposes, i.e. Navigation Cameras for mapping and Localization Cameras for localization, as the name suggests.
With respect to the robotic arm, its joint specifications are shown in Table 1. It is a 6 DoF manipulator with a Roll-Pitch-Pitch-Poll-Pitch-Roll configuration. The first three joints, also called position joints, are slower (≤ 6 o /s) because of high gear ratios. On the other hand, the last three joints are the orientation joints that define the spherical wrist, weaker but faster and with a wide joint movement range. More details about the manipulator can be found in [36].
Regarding computational resources and software, a standard Intel-Core-i7 PC is installed onboard SherpaTT to run the Motion Control Subsystem, using the Rock 2 (Robot Construction Kit) 2 framework. Additionally, the SherpaTT Application Programming Interface (API) also runs on the onboard computer, which allows interchanging information between the robot and external software components, i.e. for receiving commands or sending sensor data. Besides, it is the same interface used to communicate with SherpaTT in the simulator used later in Section V, following the Software in the Loop concept. The SherpaTT API makes use of the Robot Remote Control library, see [37] for more details. Finally, the depicted characteristics of SherpaTT were analyzed for their inclusion into the motion planner, as follows. Making use of the URDF of the platform, the computed reachability volume of SherpaTT manipulator is shown in Figure 7, with the safe volume represented in light blue. Note that the wrist reachability volume was used in this paper, taking advantage of the already mentioned kinematic decoupling, seeing that SherpaTT robotic arm has a spherical wrist. Also note that the up elbow, right shoulder solution of the IK was used, since it keeps the arm elbow as far as possible from the body. It is remarkable that the presence of the legs, the avionics box, the mast, and the cameras restricted noticeably the available free-of-collision workspace of the robotic arm. In particular, a highly unsafe volume can be observed in the fore part of the rover due to the mast. This was a main restriction in the manipulator workspace that had to be considered when planning its movements. Besides, another non-reachable volume can be observed in the rear part of the rover, due to position limits of the first arm joint.

V. SIMULATION
The validation of the proposed motion planner was carried out, first, by means of simulation tests. They were performed using the DFKI MARS simulator, 3

with 3D models of
SherpaTT and Galopprennbahn Bremen, 4 a racecourse located in Bremen near DFKI RIC. Both the model and the simulator characteristics are detailed in this section below.
The simulation tests, which are shown in the attached video 5 including also some footage of the field test campaign, consisted in reaching a particular goal sample in the scenario, to perform a dummy scientific task such as a scanning movement to analyze the surface. First, the motion planning library received the goal sample pose and the global DEM of the scenario, which was obtained by a commercial drone surveying the area beforehand; second, the planner processed the DEM to generate the traversability map and the cost map, to differentiate safe and unsafe areas; third, the cost map and the reachability volume of SherpaTT were used to generate a motion plan; fourth, the platform followed the motion plan by controlling both the base and the manipulator, i.e. sending commands depending on the current rover status. This motion planning procedure is performed only once, offline, prior to starting the motion, i.e. no replanning nor map update capabilities are included in this work. The motion planning library was launched in the same CPU as the simulator, communicating through the SherpaTT API, as aforementioned. The C++ source code of the motion planner is available in GitHub 6 under MIT open source license.
On the one hand, the goal of the simulations was to confirm that the planned motions are suitable for the platform and able to be followed by the coupled controller. On the other hand, a deep analysis of the motion planner's performance was carried out to demonstrate the advantages of using a coupled approach. A simple decoupled motion planner and the proposed coupled approach were compared, considering different planner configurations by changing the deployment distance δ (end deployment, progressive deployment, beginning deployment) and also changing the velocity of the joints of the manipulator relative to the real ones (x0.6, x0.8, x1 i.e. the real speed of the arm joints, x1.2 and x1.4). Note that the decoupled motion planner is just the proposed one but starting the arm motion only in the last waypoint of the rover trajectory, i.e. base and arm never move at the same time.

A. SIMULATION ENVIRONMENT
The simulation environment can be observed in Figure 8, where one of the mobile manipulation tests is shown. The SherpaTT 3D model was directly extracted by MARS from the robot URDF. Conversely, the terrain model was produced beforehand for the areas where the robotic tests were to be performed. A global, offline DEM of the environment was generated by commercial drone surveying hardware and software. This approach, i.e. surveying the area beforehand, has various advantages when considering the final test 4 https://goo.gl/maps/wJnPKLG2qY4QQnAt7 5 https://youtu.be/I-cEbNgtQ9c 6 https://github.com/spaceuma/ADE-Mobile_Manipulation deployment. First, the different reference frames can be set before arrival at the test site, including the map or the global frame, whose position and orientation can be set using the GPS coordinates. Once the integrated software has been tested in the simulated environment, no change in the reference frame definition is needed when setting the robot on the field. Second, the accuracy of the simulation can be evaluated after the rover tests, i.e. the simulated and real executions can be compared to try to improve the simulator parameters towards achieving better realism, although this is out of the scope of this paper. The generated DEM was also used in simulations by the motion planner for the rover base path planning, as aforementioned.
Regarding the robotic simulator MARS, its software core is the Open Dynamics Engine. 7 The physics engine uses optimization techniques to compute the contact points and forces to be applied to the simulated rigid body objects. Tuning of different parameters (e.g. material softness) and the robot models (e.g. simplifications through groupings) are needed to avoid instabilities and an acceptable simulation speed. MARS wraps the Open Dynamics Engine with graphical tools, robotic sensors, and actuator models, as well as several plugins with a great variety of functions. In the presented research multiple plugins were employed, such as a plugin that manages the noise levels in sensors, or a plugin that emulates the real connection of the robotic framework to the sensors and motors drivers of the robot.

B. RESULTS
Multiple simulation experiments were conducted using the simulation environment presented above. For each motion planner configuration and arm joint speed, a total of 40 simulations were run on a single core of an Intel(R) Core(TM) i7-10750H CPU (2.60GHz). At each simulation, the initial rover pose and the goal sample position were changed, but maintaining an average separation of approximately 15 m.
In the first place, Figure 9 shows the average computation time required to obtain the solution for every configuration of the motion planner. As can be observed, the decoupled solution is the fastest (2.85 s), since the computation is  simpler. Nonetheless, the coupled approaches have comparable computation times (4.27 s progressive deployment, 5.12 s beginning deployment, 10.80 s end deployment), being slightly higher for the end deployment configuration (10.80s) due to the bigger size of the 3D tunnel cost volume, which increases noticeably the number of nodes to be checked during the 3D trajectory planning phase.
The average distance to the closest self-collision during the motion is another remarkable result of the simulation tests, as a measure of the risk of self-collisions during the motion. As shown in Figure 10, the fact that the arm and base move at the same time does not increase the risk of self-collisions, which is similar in the decoupled solution (0.23 m) and in the coupled ones (0.26 m progressive deployment, 0.21 m beginning deployment, 0.34 m end deployment). On top of that, the end deployment configuration is considerably safer than the other ones (0.34 m), thanks to maintaining the arm in a comfortable and safer configuration during the motion, deploying mainly afterward in the last waypoints of the trajectory.
As mentioned in Section I, the main contribution of the proposed coupled motion planner is the increased efficiency in the motion plan execution, mainly in time terms, meanwhile ensuring the safety of the platform. This can be clearly observed in Figure 11, which depicts the average required execution time of the solution for every configuration of the motion planner and speed of the arm joints, differentiating whenever the manipulator (green) or the base (blue) are moving. Clearly, the decoupled solution is the slowest, since the base movements are performed first (179.70 s), then the rover stops, and finally the arm moves. The decoupled results can be used as a reference for the worst possible motion in efficiency terms, with an average total required time 19.54% bigger than the rover base required time. On the other hand, the coupled solutions coordinate both the base and arm movements, thus reducing noticeably the total execution time. In particular, the progressive deployment solution is the most efficient for every arm joints speed, actually with almost maximized efficiency since the total spent time is similar to the time required by the rover base to reach the sample vicinity, only 2.16% bigger on average, i.e. the rover base does not need to wait or slow down for the arm to move. This is thanks to the fact that the arm joints' movement profile is the smoothest, distributing the arm joints' motion throughout the whole rover base trajectory. Only for speed x0.2 the required time is slightly higher (194.58 s), but this is due to the rover trajectory not being always long enough to give the arm time to move, since the arm joints are very slow.
Conversely, the end deployment solution is the slowest among the coupled solutions, since it resembles the decoupled approach, i.e. the arm mainly moves when the rover is already close to the goal. On average, the end deployment total required time is 13.72% bigger than the rover base required time. The beginning deployment solution is also quite efficient with an average total required time 9.06% bigger than the base one, although not as much as the progressive deployment since the main motion of the arm is performed at the beginning, which slows down the rover base when the arm starts to move. It is remarkable in Figure 11 how the beginning and end deployments noticeably increase their average execution times when the arm is slow since the arm joints profile is not properly distributed throughout the trajectory, which provokes the coupled controller to slow down the rover base motion quite often during the trajectory. This is not the case for the progressive deployment, which is completely unaffected by the arm joints' velocity as long as the rover trajectory is long enough for the arm to deploy completely.
The aforementioned simulation results demonstrate that the motion planner is able to find solutions with comparable computational cost to the decoupled solution, for all of its configurations. These solutions are equally safe, if not safer, than the standard decoupled ones, meanwhile noticeably increasing the mission efficiency in execution time terms by coordinating the arm and base motions. The progressive deployment is, conclusively, the best configuration for the motion planner, having maximized efficiency and maintaining sufficient distance to self-collisions during the motion, with the smallest computational cost among the coupled approaches. Remark that the slower the arm w.r.t. the rover base, the greater the importance of using a coupled arm-base motion with a progressive deployment, since the execution of the motion plan will be noticeably shorter. On the other hand, the faster the arm w.r.t. the rover base, the less the room for improvement left for the coupled approach, until it could actually slow down the motion considering the increased computational time w.r.t. the decoupled solution of the coupled motion planner. As a result, coupled arm-base motion planners, like the one proposed in this paper, are recommended for systems with a slow manipulator speed w.r.t. the base movement speed. And, in particular, a progressive deployment is the best approach for a coupled arm-rover motion to maximize its efficiency.

VI. BENCHMARK WITH AN OFF-THE-SHELF MOTION PLANNER
As a way to highlight the strengths and weaknesses of the presented motion planner, a comparison with an off-theshelf mobile manipulation motion planner was performed. The selected motion planner was the one included within MoveIt, 8 an open motion planning and control library mainly for robotic arms, used by a great variety of companies and research institutions worldwide. MoveIt was selected due to its popularity and its easiness to include new robots, thanks to the direct use of URDFs.
Remark that there are significant differences between our mobile manipulation motion planner and the one included within MoveIt. First, MoveIt does not generate a coupled motion plan for the base and arm, but rather a path for the mobile base and an independent motion plan for the manipulator, and afterward it executes both plans at the same time. Our motion planner considers both systems in a coupled manner during the motion planning stage, and also meanwhile executing the plan. Second, to the authors knowledge, MoveIt does not include a cartesian planner for mobile manipulation, but a path planner for the mobile platform based on Nav2 9 and a joint space planner for the manipulator. Therefore, MoveIt considers self-collisions of the manipulator with the robot itself, but not collisions with the environment. Third, the path planner does not guarantee the rover base will reach a position in which the sample is inside the manipulator workspace, thus additional maneuvers may be required. Fourth, MoveIt uses A* [38] for the mobile base path planning, and RRT Connect [39] for the manipulator joint space motion planning. These are sub-optimal algorithms that do not always find the globally optimal solution. Conversely, FMM does find the globally optimal solution, maximizing the motion plan efficiency, which is the reason why it is used within the presented motion planner.
As in Section V, 40 simulation experiments were carried out using the simulation environment included within MoveIt, based on Gazebo. For a balanced comparison, the same cost map of Galopprennbahn Bremen generated in our simulations was directly fed to MoveIt. The 40 tests were run on a single core of an Intel(R) Core(TM) i7-10750H CPU (2.60GHz), using the same rover and goal poses as in Section V. An example of the motion plan generated during one of the tests is shown in Figure 12, where the planned path for the mobile base, highlighted in green, avoided an obstacle on the Galopprennbahn Bremen cost map.
The simulations showed that MoveIt motion planner is computationally fast, with an average planning time of 0.47 s in comparison to the 2.87 s of our progressive deployment planner. This is without considering the average 1.51 s required for the cost map generation (in total, the progressive deployment planner takes 4.38 s to compute the cost map and generate the motion plan), which is directly provided to MoveIt, and without any computational optimization of our planner code. The faster computation of MoveIt is also thanks to, as aforementioned, the generation of the manipulator motion plan in the joint space, without a cartesian trajectory, and the use of sub-optimal planners. Nevertheless, the average distance from self-collisions of MoveIt is low in comparison to ours, 0.12 m vs 0.26 m, i.e. MoveIt motion plans get closer to possible self-collisions. In fact, MoveIt is not considering collisions of the manipulator with the scenario, as aforementioned, and can not guarantee that the planned motion will not lead to a self-collision during its execution. Regarding the required time for execution of the motion plan, the comparison is not evenhanded, since MoveIt does not generate coupled motion plans but two independent ones executed at the same time, as stated above. Keeping that in mind, MoveIt requires slightly more time to execute the motion plan, on average 186.53 s vs 181.05 s of the progressive deployment, due to the lower quality of the base path. Both planners have maximized manipulation efficiency, i.e. the whole manipulator motion is performed meanwhile the base is reaching the sample.
This comparison with an off-the-shelf motion planner shows, in conclusion, that the presented motion planner generates safer motion plans, with guaranteed avoidance of self-collisions, with a globally optimal path for the mobile base and maximized efficiency of the manipulator plan execution. The slightly higher computational cost of the proposed method is a drawback, which can be solved by optimizing the motion planner's current implementation, or by running some of the planning tasks in parallel to accelerate the computations, which would require to switch from FMM to similar path planners such as FSM [27].

VII. FIELD TESTS AND LESSONS LEARNED
An ultimate validation of the presented algorithm was carried out by means of a field test campaign, integrating the motion planner into a real platform. The campaign consisted of several analogue tests to the simulations, with the real SherpaTT in Galopprennbahn Bremen as shown in Figure 13, executing the planned motions to reach different samples and perform a dummy scientific analysis on them.
Several sensors were required within the field test campaign. For localization, a Visual Odometry algorithm was used, merging the information received by the Localization Cameras, the wheel odometry, and the Inertial Measurement Unit. Additionally, a GPS with Real Time Kinematic corrections was used to get the initial pose and log the groundtruth positions. For mapping, the stereo Navigation Cameras were used, providing a local DEM of the scenario, offline, before starting the motion. The four cameras were calibrated before the tests using standard distortion estimation methods. Lastly, the rover joints were manually re-calibrated in case of a significant mismatch between the position given by the proprioceptive sensors and the real ones.
The field tests proceeded in a similar way to the simulations, as follows. The motion planning library was launched in a separate Intel-Core-i7 CPU running Ubuntu 18.04, which communicated to the robot through the SherpaTT API using WiFi. Then, the motion planning library received the local DEM of the scenario from the stereo Navigation Cameras, and the goal pose, always located inside the DEM, to generate the motion plan. Lastly, the library controlled the platform (rover base and manipulator) to properly follow the generated motion plan, reading the rover pose and the joint states and commanding the base motion commands as well as the arm joints' position and velocity. As in the simulations, no updates of the map or replans of the motion were performed, i.e. the motion plan was generated once, offline, and thoroughly followed. Whenever the goal was reached, the library launched the dummy scanning measurement, and the execution was finished.
During the field tests, several issues appeared that led to some lessons learned. On the one hand, the arm presented some repeatability inaccuracies in the position of the end effector. This was caused by a small dead zone in the first joint and some occasional calibration errors on the joints. Although this was partially solved with a re-calibration of the arm before every test, the mismatch between the robot model and the real system added some uncertainty w.r.t. the safety of the motion plans to be executed. This danger was enlarged considering the highly restricted manipulator workspace on SherpaTT, i.e. the tight space between the legs, the mast, and the avionic box, since any small error could lead to a collision. Therefore, it was decided to be more conservative, i.e. maximize the security of the platform during the field tests. The motion planner was configured to use the beginning deployment since its motions were the most repetitive (thus predictable) and it maintained the arm far enough from collisions. Besides, the goal pose of the end effector above the sample was modified to be farther from the rover body and the avionics box.
On the other hand, an occasional lack of accuracy was observed in the position of the end effector w.r.t. the sample. This had two main causes. In the first place, the already mentioned positioning error of the manipulator. In the second place, small errors in the localization of the rover (fluctuations of the initial GPS position and the accumulated drift of the Visual Odometry localization) and errors in the position of the sample w.r.t. the actual commanded goal. This could not be solved during the field test campaign, since it required a separate component to detect the sample online and estimate its position, in order to feed back continuously the sample position to the motion planning library and replan the motion thus removing the positioning errors.
Conclusively, even though the aforementioned issues, the motion planning library was correctly deployed, tested, and verified in the real rover SherpaTT during the field test campaign, as can be observed in the appended video. The platform autonomously generated a motion plan to reach a goal sample, and correctly followed the generated trajectory meanwhile the arm was deploying, finally leaving the sample within the reachability area of the manipulator and performing successfully the scanning measurement on the sample.

VIII. CONCLUSION
This paper presents a coupled base-arm motion planning algorithm for planetary exploration mobile manipulation. Considering the case of a highly restricted arm workspace, the proposed algorithm is able to generate a motion plan that, on the one hand, leaves the goal reachable for the manipulator by precisely planning and controlling the last stretch of the rover base path and, on the other hand, ensures the safety of the manipulator motion by preventing self-collisions offline using the real workspace of the manipulator.
The motion planner was validated by means of simulation and field tests, where the rover was able to reach the goal avoiding obstacles in the scenario and smoothly orient itself to leave the goal inside the manipulator workspace, meanwhile, the arm was performing its particular motion plan to perform a scanning measurement on the sample goal. Besides, a benchmark with an off-the-shelf motion planner was performed, demonstrating the increased efficiency of the coupled approach, and the guarantee of self-collision avoidance without a significant increase in the computational effort. In particular, a progressive deployment maximizes the efficiency since the manipulator movements are completely performed meanwhile the rover is still reaching the goal, and the arm joints' motion is equally distributed throughout the whole rover base trajectory.
Although the proposed motion planner has been demonstrated to be comparable to off-the-shelf planners in computational terms, it could be improved with a parallel implementation of some of the motion planning stages, which is foreseen as future work. Additionally, for its use in future space exploration missions, it would be necessary to test the algorithm with real space conditions, i.e. a spacequalified processing unit running an autonomous navigation architecture and including communication delays. In this regard, space-grade CPUs like the LEON III or the LEON IV could be used. Finally, it is foreseen to include on the arm end effector a sample localization subsystem and a LIBS spectrometer, to perform real scientific measurements on interesting areas of the scenario. This, altogether with the inclusion of real space conditions, would allow us to perform a more representative and complete field test campaign, to confirm that the proposed algorithm could be used in future space exploration missions.
J. RICARDO SÁNCHEZ-IBÁÑEZ received the M.Sc. and Ph.D. degrees in mechatronics from the University of Málaga, in 2017 and 2022, respectively. He wrote his Ph.D. thesis with the Space Robotics Laboratory, University of Málaga. It focuses on novel path planning techniques for mobile exploration robots. During his Ph.D. studies, he performed several research stays. He was with the Automation and Robotics Section, ESA-ESTEC, Noordwijk, The Netherlands, for one year. Besides, he did a one-month research visit with DFKI, Bremen, under the Horizon 2020 Project ADE. In 2021, he was a Research and Development Specialist with the Space Robotics (SpaceR) Group, University of Luxembourg. Currently, he is a GNC and Robotics Engineer with Airbus Defence and Space, U.K. He assists in current and future exploration missions with rovers on the Moon and Mars. His research interests include space robotics, autonomous navigation, mobile manipulation, efficient computing, and artificial intelligence.
RAÚL DOMÍNGUEZ received the bachelor's degree in computer science and the master's degree in artificial intelligence from the Technical University of Madrid (UPM). He is currently pursuing the Ph.D. degree with the DFKI Robotics Innovation Center, Bremen. He applied evolutionary strategies and reinforcement learning to robotic team problems with UPM. Later, he focused on autonomous navigation and perception with the Autonomous Driving Group, Spanish National Council of Scientific Research (CSIC). His research interest includes safe robotic autonomous navigation for space.
CARLOS J. PÉREZ-DEL-PULGAR (Member, IEEE) received the M.S. and Ph.D. degrees in computer science from the University of Málaga (UMA). In 2004, he was a Research Support Staff with UMA, until 2017. Since 2010, he has been a part-time Assistant Lecturer with the Electrical Engineering Faculty, where he was responsible for different subjects related to automation and robotics. Currently, he is an Associate Lecturer and responsible for the Space Robotics Laboratory, UMA. His research interests include machine learning, surgical robotics, space robotics, and autonomous astronomy. He has more than 60 publications on these topics and he has been involved in more than 15 different Spanish and international projects.
FRANK KIRCHNER received the degree in computer science and neurobiology and the Dr. rer. nat. degree in computer science from the University of Bonn, in 1994 and 1999, respectively. Since 1994, he has been a Senior Scientist with the Gesellschaft für Mathematik und Datenverarbeitung (GMD), Sankt Augustin. He has been a Senior Scientist with the Department for Electrical Engineering, Northeastern University, Boston, MA, USA, since 1998, where he was appointed as an Adjunct and then a Tenure Track Assistant Professor, in 1999. Since 2002, he has been a Full Professor with the University of Bremen. Since December 2005, he has been the Director of the Robotics Innovation Center (RIC), Bremen.
ALFONSO GARCÍA-CEREZO (Member, IEEE) was born in Sigüenza, Spain, in 1959. He received the degree in industrial electrical engineering and the Doctoral of Engineering degree from Escuela Tecnica Superior de Ingenieros Industriales de Vigo, in 1983 and 1987, respectively. From 1983 to 1988, he was an Associate Professor with the Department of Electrical Engineering, Computers and Systems, University of Santiago de Compostela, where he was an Assistant Professor, from 1988 to 1991. Since 1992 he has been a Professor in system engineering and automation with the University of Málaga, Spain. From 1993 to 2004, he was the Director of the Escuela Tecnica Superior de Ingenieros Industriales de Malaga. He is currently the Head of the Department and responsible for the Instituto de Automática Avanzada y Robótica de Andalucía, Málaga. He has authored or coauthored about 150 journal articles, conference papers, book chapters, and technical reports. His current research interests include mobile robots and autonomous vehicles, surgical robotics, mechatronics, and intelligent control. Besides, he is responsible for more than 19 research projects during the last ten years. He is also a member of several international and national scientific and technical societies, such as EUROBOTICS, IFAC, AER-ATP, CEA, and SEIDROB.