Mobile manipulator, method, and system for motion planning and navigation

Abstract

Disclosed is a mobile manipulator including a mobile base and a manipulator, the manipulator comprising a processor configured to: generate an initial trajectory of the mobile base and the manipulator of the mobile manipulator, the trajectory being constrained by non-holonomic motion of the mobile base; obtain predicted motion of the one or more dynamic obstacles based on sensor data; determine one or more potential collisions between the initial trajectory and the predicted motion of the dynamic obstacles over a predetermined time window; generate a replacement trajectory segment for a portion of the initial trajectory corresponding to the one or more potential collisions; and replace the portion of the trajectory with the replacement trajectory segment such that the replacement trajectory segment connects to a first portion of the initial trajectory corresponding to a current position of the mobile manipulator and to a second portion of the initial trajectory leading to a destination.

Description

MOBILE MANIPULATOR, METHOD, AND SYSTEM FOR MOTION PLANNING AND NAVIGATION CROSS-REFERENCE TO RELATED APPLICATION

[0001] The application claims the benefit of priority of Singapore patent application No.10202500825T, filed on 27 March 2025, the content of it being hereby incorporated by reference in its entirety for all purposes.TECHNICAL FIELD

[0002] Various aspects of this disclosure relate to a method, server apparatus, and system for motion planning and navigation. The disclosure is particularly suited for, but not limited to, usage in mobile manipulators.BACKGROUND

[0003] The following discussion of the background art is intended to facilitate an understanding of the present disclosure only. It should be appreciated that the discussion is not an acknowledgement or admission that any of the material referred to was published, known or is part of the common general knowledge of the person skilled in the art in any jurisdiction as of the priority date of the disclosure.

[0004] Mobile manipulators combining a mobile base and one or more articulated manipulators are increasingly deployed in industrial automation, logistics, service robotics, and healthcare.

[0005] Planning safe motion for such mobile manipulators requires coordination between the motion of the mobile base and the manipulator arm.

[0006] However, existing robotic planning architectures often treat these two components separately, resulting in sub-optimal or unsafe behaviour when the robot operates in dynamic environments. In particular, existing robotic systems face several technical challenges, such as lack of standardised architectures for whole-body mobile manipulation planning; absence of efficient pipelines capable of safety-aware replanning for mobile manipulators operating in dynamic environments; limited ability to perform predictive collision detection based on estimated obstacle motion; and difficulty balancing safety constraints with operational performance.

[0007] A further complication arises from the type of drivetrain used in the mobile base. Many robotic mobile bases are non-holonomic, meaning that the controllable degrees of freedom are fewer than the total degrees of freedom of motion.

[0008] For example, car-like robots cannot move sideways directly and must follow constrained trajectories. Planning safe motion for such systems is therefore more complex than for holonomic bases, which can move freely in all planar directions.

[0009] Existing robotic planning solutions within the Robot Operating System (ROS) ecosystem exhibit limitations. One commonly used ROS navigation framework is the move_base package, which is primarily designed for navigation of mobile robot bases. However, this framework does not support coordinated motion planning for mobile manipulators.

[0010] Another widely used framework is Movelt (including Movelt2), which provides motion planning pipelines primarily for robot manipulators and, in certain implementations, for mobile manipulators comprising a manipulator mounted on a mobile base. Movelt2 is capable of performing coordinated planning for such mobile manipulators; however, its planning formulation generally assumes kinematic models expressed in joint configuration space relative to a fixed or quasi-static world coordinate frame. As a result, Movelt2 may not be suited for mobile platforms subject to non-holonomic constraints (e.g., car-like bases), where feasible motion must satisfy non -integrable kinematic constraints and a continuously evolving reference frame during operation.

[0011] Furthermore, conventional Movelt pipelines generally assume static environments and do not natively provide predictive collision detection for moving obstacles.

[0012] There exists a need for improved planning architectures to address the aforementioned challenges at least in part.SUMMARY

[0013] The present disclosure provides a robotic motion planning architecture for wholebody mobile manipulation in dynamic environments.

[0014] In some embodiments, the disclosure includes a mobile manipulator that provides continuous robot path re-planning and seamless spliced (segmented) path execution, integrating visual and / or LiDAR sensor inputs into a standard Movelt planning pipeline; implementing whole-body collision avoidance / planning within the robot operating system(ROS) framework; and extending functionality beyond base_local_planner of move_base package.

[0015] The technical solution comprises a mobile-manipulator robot having a mobile base and a manipulator, wherein a base trajectory is non-holonomic; the manipulator and base are planned simultaneously with collision checking, forming a whole-body mobile manipulator planner; herein whole-body mobile manipulator planner based on RRTConnect or any other sampling based planning algorithms; wherein the whole-body mobile manipulator planner based on distance and interpolation functions; wherein an interpolation function operable to interpolate the base trajectory by using Reed Shcpp algorithm and wherein the manipulator is linearly interpolated across the differences in joint angles.

[0016] The present disclosure enables coordinated planning of mobile base and manipulator motions; predictive avoidance of dynamic obstacles; continuous replanning during execution; and seamless trajectory transitions without stopping the mobile manipulator.

[0017] The various modules described herein may be implemented independently or in combination, and may be capable of: planning coordinated motion of mobile bases and manipulators; supporting non-holonomic mobile bases; predicting collisions with dynamic obstacles; and enabling safe and efficient trajectory modification during execution.

[0018] According to an aspect of the present disclosure, there is provided a mobile manipulator including a mobile base and a manipulator, the manipulator comprising a processor configured to: generate an initial trajectory of the mobile base and the manipulator of the mobile manipulator, the trajectory being constrained by non-holonomic motion of the mobile base; obtain predicted motion of the one or more dynamic obstacles based on sensor data; determine one or more potential collisions between the initial trajectory and the predicted motion of the dynamic obstacles over a predetermined time window; generate a replacement trajectory segment for a portion of the initial trajectory corresponding to the one or more potential collisions; and replace the portion of the trajectory with the replacement trajectory segment such that the replacement trajectory segment connects to a first portion of the initial trajectory corresponding to a current position of the mobile manipulator and to a second portion of the initial trajectory leading to a destination.

[0019] In some embodiments, the processor is configured to determine one or more configurations of the mobile manipulator along the initial trajectory and one or more positions of the dynamic obstacles at multiple discrete time instances within the predetermined time window.

[0020] In some embodiments, in the generation of the initial trajectory constrained by the non-holonomic motion of the mobile base, the processor is configured to interpolate motion of the mobile base using a sampling-based motion planning algorithm.

[0021] In some embodiments, the sampling-based motion planning algorithm is further configured to determine a shortest path of the mobile base constrained by the non-holonomic motion of the mobile base using a Reeds–Shepp path.

[0022] In some embodiments, the processor is further configured to generate the replacement trajectory segment based on a configuration of the mobile manipulator corresponding to a time prior to the one or more potential collisions.

[0023] In some embodiments, the processor is further configured to determine a distance used by the sampling-based motion planning algorithm based on both motion of the mobile base and motion of the manipulator.

[0024] In some embodiments, the processor is further configured to determine the distance based on (a) a shortest path of the mobile base constrained by the non-holonomic motion of the mobile base; and (b) a distance travelled by the manipulator.

[0025] In some embodiments, the processor is further configured to determine the shortest path of the mobile base using a Reeds–Shepp path.

[0026] In some embodiments, the processor is further configured to determine the distance between the one or more configurations as a maximum value between the shortest path of the mobile base determined using the Reeds–Shepp path constrained by the non-holonomic motion of the mobile base; and a distance travelled by the manipulator.

[0027] In some embodiments, the processor is further configured to update the predicted motion of the dynamic obstacles using updated sensor data obtained from one or more sensors.

[0028] In some embodiments, the processor is further configured to determine interpolated configurations of the mobile base and the manipulator between one or more configurations when generating the initial trajectory.

[0029] In some embodiments, the processor is further configured to determine an interpolated configuration of the mobile base along a Reeds–Shepp path between the one or more configurations of the mobile base.

[0030] In some embodiments, the processor is further configured to determine an interpolated configuration of the manipulator by linearly interpolating differences in joint angles of the manipulator.

[0031] In some embodiments, the processor is further configured to determine the interpolated configurations of the mobile base and the manipulator simultaneously along the Reeds–Shepp path and the interpolated joint angles.

[0032] According to another aspect of the present disclosure, there is provided a method for generating a final trajectory of a mobile manipulator having a mobile base and a manipulator, comprising generating an initial trajectory of the mobile base and the manipulator of the mobile manipulator, the trajectory being constrained by non-holonomic motion of the mobile base; obtaining predicted motion of one or more dynamic obstacles based on sensor data; determining one or more potential collisions between the initial trajectory and the predicted motion of the dynamic obstacles over a predetermined time window; generating a replacement trajectory segment for a portion of the initial trajectory corresponding to the one or more potential collisions; and replacing the portion of the trajectory with the replacement trajectory segment such that the replacement trajectory segment connects to a first portion of the initial trajectory corresponding to a current position of the mobile manipulator and to a second portion of the initial trajectory leading to a destination.

[0033] In some embodiments, generating the initial trajectory further comprises determining one or more configurations of the mobile manipulator along the initial trajectory and determining one or more positions of the dynamic obstacles at multiple discrete time instances within the predetermined time window.

[0034] In some embodiments, generating the initial trajectory further comprises interpolating motion of the mobile base using a sampling-based motion planning algorithm.

[0035] In some embodiments, the method further comprising configuring the samplingbased motion planning algorithm to determine a shortest path of the mobile base constrained by the non-holonomic motion of the mobile base using a Reeds–Shepp path.

[0036] According to another aspect of the present disclosure there is provided a computer program element comprising program instructions, which, when executed by one or more processors, cause the one or more processors to perform the method as described.

[0037] According to another aspect of the present disclosure there is provided a non-transitory computer-readable medium comprising program instructions, which, when executed by one or more processors, cause the one or more processors to perform the method as described.

[0038] According to another aspect of the present disclosure there is provided a system for motion planning and navigation comprising the mobile manipulator as described, and one ormore sensors arranged in data or signal communication with the mobile manipulator to provide sensor data to the mobile manipulator.BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The disclosure will be better understood with reference to the detailed description when considered in conjunction with the non-limiting examples and the accompanying drawings, in which:- FIG. 1 is a general system diagram for a system for motion planning and navigation, according to various embodiments.- FIG. 2 shows a high-level architecture diagram showing a mobile manipulator planner, dynamic obstacle avoidance and / or checking module, trajectory re-planner, and trajectory splicing module according to some embodiments.- FIG. 3 illustrates a result of trajectory interpolation (from a start pose to an end pose) using a standard function in Movelt.- FIG. 4 illustrates a result of trajectory interpolation (from a start pose to an end pose) using a customized or modified function in Movelt.- FIG. 5 is a general flowchart depicting a method for motion planning and navigation.DETAILED DESCRIPTION

[0040] The following detailed description refers to the accompanying drawings that show, by way of illustration, specific details, and embodiments in which the disclosure may be practiced. These embodiments are described in sufficient detail to enable those skilled in the ait to practice the disclosure. Other embodiments may be utilized, and structural and logical changes may be made without departing from the scope of the disclosure. The various embodiments are not necessarily mutually exclusive, as some embodiments can be combined with one or more other embodiments to form new embodiments.

[0041] Features that are described in the context of an embodiment may correspondingly be applicable to the same or similar features in the other embodiments. Features that are described in the context of an embodiment may correspondingly be applicable to the other embodiments, even if not explicitly described in these other embodiments. Furthermore, additions and / orcombinations and / or alternatives as described for a feature in the context of an embodiment may correspondingly be applicable to the same or similar feature in the other embodiments.

[0042] In the context of various embodiments, the articles “a”, “an” and “the” as used with regard to a feature or element include a reference to one or more of the features or elements. As used herein, the term “and / or” includes any and all combinations of one or more of the associated listed items.

[0043] While such terms as “first”, “second”, etc., may be used to describe various elements, such elements must not be limited to the above terms. The above terms are used only to distinguish one element from another, and do not define corresponding elements, for example, an order and / or significance of the elements. Without departing from the scope of rights of the specification, a first element may be referred to as a second element, and similarly, the second element may be referred to as the first element.

[0044] As used herein, the term “data” may be understood to include information in any suitable analog or digital form, for example, provided as a file, a portion of a file, a set of files, a signal or stream, a portion of a signal or stream, a set of signals or streams, and the like. The term data, however, is not limited to the aforementioned examples and may take various forms and represent any information as understood in the art.

[0045] As used herein, the term “processor” refers to a circuit, including analog circuits, digital circuits, or hybrid circuits, or their constituent components. Any other kind of implementation of the respective functions which will be described in more detail below may also be understood as a “circuit” in accordance with an alternative embodiment. A digital circuit may be understood as any kind of a logic implementing entity, which may be special purpose circuitry or a processor executing software stored in a memory, or a firmware.

[0046] As used herein, the term “module” refers to, forms part of, or includes an application specific integrated circuit (ASIC); an electronic circuit; a combinational logic circuit; a field programmable gate array (FPGA); a processor (shared, dedicated, or group) that executes code; other suitable hardware components that provide the described functionality; or a combination of some or all of the above, such as in a system-on-chip. The term module may include memory (shared, dedicated, or group) that stores code executed by the processor. A single module or a combination of modules may be regarded as a device. A processor may include one or more modules. For example, multiple modules described in this disclosure may form a processor.

[0047] As used herein, the term “associate”, “associated”, and “associating” indicate a defined relationship (or cross-reference) between two items.

[0048] As used herein, “memory” may be understood as a non-transitory computer-readable medium in which data or information can be stored for retrieval. References to “memory” included herein may thus be understood as referring to volatile or non-volatile memory, including random access memory (“RAM”), read-only memory (“ROM”), flash memory, solid-state storage, magnetic tape, hard disk drive, optical drive, etc., or any combination thereof. Furthermore, it is appreciated that registers, shift registers, processor registers, data buffers, etc., are also embraced herein by the term memory. It is appreciated that a single component referred to as “memory” or “a memory” may be composed of more than one different type of memory, and thus may refer to a collective component including one or more types of memory. It is readily understood that any single memory component may be separated into multiple collectively equivalent memory components, and vice versa. Furthermore, while memory may be depicted as separate from one or more other components (such as in the drawings), it is understood that memory may be integrated within another component, such as on a common integrated chip.

[0049] As used herein, the term “configured to” broadly refers to the design, arrangement, or adaptation of a system, device, component, or module to perform a specific function or achieve a particular outcome. The term includes both hardware and software implementations wherein in a hardware implementation, the physical components are arranged, programmed, or structured to carry out the intended function(s), and in the context of programming and software, a device is operable under executable instructions (e.g., software, firmware) to perform the specified function(s) when executed by one or more processors. The resultant configuration allows the system or component to perform the stated function, either inherently or after suitable programming or activation, without requiring substantial modifications to its structure or operational logic.

[0050] According to various embodiments, a circuit may include analog circuits or components, digital circuits or components, or hybrid circuits or components. Any other kind of implementation of the respective functions which will be described in more detail below may also be understood as a "circuit" in accordance with an alternative embodiment. A digital circuit may be understood as any kind of a logic implementing entity, which may be special purpose circuitry or a processor executing software stored in a memory, firmware, or any combination thereof. Thus, in various embodiments, a "circuit" may be a digital circuit, e.g. a hard-wired logic circuit or a programmable logic circuit such as a programmable processor, e.g. a microprocessor (e.g. a Complex Instruction Set Computer (CISC) processor or a ReducedInstruction Set Computer (RISC) processor). A "circuit" may also include a processor executing software, e.g. any kind of computer program, e.g. a computer program using a virtual machine code such as e.g. Java.

[0051] As used herein, the term “mobile manipulator” broadly refers to a robot or robotic apparatus that combines a mobile platform / base, and a robotic manipulator. The mobile platform may enable the robot to move within an environment, while the manipulator enables interaction with objects in the environment. A mobile manipulator may include one or more of: robotic arms, grippers or end effectors, mobile wheeled platforms, tracked platforms, legged locomotion systems. The mobile manipulator may operate autonomously or semi-autonomously. The mobile base may include wheeled mechanisms, tracked mechanisms, legged locomotion mechanisms, omnidirectional drive mechanisms. In some embodiments the mobile base follows non-holonomic motion constraints, meaning that the instantaneous allowable motions are restricted relative to the degrees of freedom of the platform.

[0052] As used herein, the term “sensor” refers to a device capable of detecting or measuring properties of an environment and generating data representing those properties. Sensors may include, for example: LiDAR sensors, cameras, depth sensors, radar sensors, ultrasonic sensors, hi some embodiments, The sensor may generate sensor data representing position, distance, velocity, or motion of objects in an environment.

[0053] As used herein, the term “dynamic obstacle” broadly refers to an object in an environment whose position may change over time. Dynamic obstacles may include one or more of humans, vehicles, other robots, and / or moving equipment. Dynamic obstacles may further move unpredictably or according to estimated motion models.

[0054] As used herein, the term “initial trajectory” refers to a planned motion of a robot, such as a mobile manipulator, determined prior to modification in response to predicted collisions. The initial trajectory may represent a planned sequence of configurations of the mobile manipulator over time for moving from a starting configuration toward a destination. The initial trajectory may include motion of: a mobile base, a manipulator, or both the mobile base and the manipulator. The initial trajectory may be generated using any suitable motion planning technique, including, but not limited to: sampling-based motion planning algorithms, graph-based motion planning algorithms, optimization-based motion planning methods, search-based motion planning techniques, hybrid planning techniques combining multiple approaches. In some embodiments, the planners may be based on: rapidly exploring random trees, probabilistic roadmaps, lattice-based planning, kino dynamic planning. In someembodiments, the initial trajectory may include any representation of planned motion of the mobile manipulator prior to generation of a replacement trajectory segment, including a sequence of configurations, a spatial path, a time -parameterized trajectory, or control inputs defining motion of the mobile manipulator.

[0055] As used herein, the term “replacement trajectory segment” refers to a portion of a trajectory generated to replace part of an existing trajectory, which may be an initial trajectory. The replacement trajectory segment may be generated to avoid predicted collisions, connect to remaining portions of the trajectory, and / or preserve continuity of robot motion.

[0056] As used herein, the term “configuration” refers to a state of a robot, such as a mobile manipulator, defined by parameters describing the spatial pose of the mobile base and the positions of the joints of the manipulator. In some embodiments, a configuration may include: a position of the mobile base in an environment, an orientation of the mobile base, and one or more joint positions of the manipulator. For example, the configuration may be represented by one or more parameters, including: a position of the mobile base along one or more spatial axes, an orientation of the mobile base, and joint angles or joint displacements of the manipulator. Thus, a configuration may represent the complete pose of the mobile manipulator at a particular instant.

[0057] According to an aspect of the present disclosure there is provided a system comprising a mobile manipulator including a mobile base and a manipulator; one or more sensors configured to detect dynamic obstacles; and a processor configured to: generate an initial trajectory of the mobile base and the manipulator of the mobile manipulator, the trajectory being constrained by non-holonomic motion of the mobile base; obtain predicted motion of the one or more dynamic obstacles based on sensor data from the one or more sensors; determine one or more potential collisions between the initial trajectory and the predicted motion of the dynamic obstacles over a predetermined time window; generate a replacement trajectory segment for a portion of the initial trajectory corresponding to the one or more potential collisions; and replace the portion of the trajectory with the replacement trajectory segment such that the replacement trajectory segment connects to a first portion of the initial trajectory corresponding to a current position of the mobile manipulator and to a second portion of the initial trajectory leading to a destination.

[0058] FIG. 1 illustrates a system 100 for motion planning and navigation according to various embodiments of the present disclosure. The system 100 comprises a mobile manipulator 110 having a mobile base 112 and a manipulator 114. A processor 120, configuredremotely, may be configured to generate the initial trajectory 131, and receive sensor data continuously so as to generate one or more replacement trajectory segments 132. The mobile manipulator 100 may be an autonomous robot configured to move from a current position, such as a start point 141, to an end point 142. In some embodiments, the processor 120 may form part of the mobile manipulator 110. In some embodiments, the processor 120 may be arrangement in data communication with a controller within the mobile manipulator 110.

[0059] As shown in FIG. 1, the mobile manipulator 110 navigates through one or more potential collisions (shown as a collision zone 150) by generating the replacement trajectory segment 132.

[0060] The system may be used for real-time motion planning and navigation.

[0061] FIG. 2 illustrates a high level block diagram 200 for predictive trajectory planning and collision avoidance for a mobile manipulator operating in an environment containing dynamic obstacles.

[0062] As shown in FIG. 2, the processor may comprise a mobile manipulator planner module 210, an object prediction module 220, a dynamic obstacle avoidance module 230, and a mobile manipulator controller 240. It is contemplated that the modules 210, 220, 230, 240 may be integrated, or may be distributed.

[0063] The modules 210, 220, 230, 240 may interact with sensor inputs and the robot execution system 250 to allow continuous motion planning and real-time trajectory modification.

[0064] Sensor inputs 221 may be continuously obtained from one or more sensors of the mobile manipulator. The sensors may include, for example vision sensors, LiDAR sensors, depth cameras, radar sensors. The sensor data is processed to identify objects in the environment. Object detection, which produces object information relating to detected obstacles including their position 222, motion characteristics, positions and velocities 223, and may be used by subsequent modules to perform predictive collision analysis.

[0065] The mobile manipulator planner module 210 may include a modified Movelt planning framework, which integrates a mobile manipulator planner within a modified planning framework. Within the modified framework, a mobile manipulator planner generates a planned trajectory for the robot. The planner module 210 produces a planned trajectory representing coordinated motion of the mobile base and the manipulator.

[0066] Unlike conventional planning approaches that treat base motion and manipulator motion separately, the planner module 210 performs whole-body planning. The planner mayuse a sampling-based planning algorithm, such as, but not limited to, RRTConnect or other sampling- based planning algorithm. The planner module 210 may be configured to account for non-holonomic motion constraints of the mobile base.

[0067] In some embodiments, an Open Motion Planning Library (OMPL) implemented in Movelt may be modified. In some embodiments, modifications to the “distance function” and the “interpolation function” without changes to the semantics of most general planning algorithms such as RRTConnect and RRT star may be made to plan for non-holonomic path of the mobile base.

[0068] In some embodiments, the mobile base may be modeled as comprising 2 prismatic joints and 1 revolute joint to form a mobile base chain from a fixed frame to represent a special Euclidean group in 2 dimensions, i.e. SE(2) state-space of the mobile base. Such a model represents all possible rigid body transformations in a 2-D plane. The manipulator chain may be represented using in joint-space in a state-space of Rn, where n represent the number of manipulator degree-of-freedoms (DOFs) to be added to the mobile base chain to form a single mobile manipulator chain (with a compound state-space of SE(2) + Rn). The compound statespace can be treated as a state- space of Rn+3 (by concatenating the base and manipulator statespace) and to be planned like a conventional manipulator in Movelt.

[0069] However, the approach above may not generate a non-holonomic path for the mobile base as the default implementation of Movelt interpolates each degree of freedom (DOF) linearly as the trajectory traverses through the nodes of configuration space from start pose to end pose. The interpolation will generate a trajectory that may cause the mobile base to translate in a straight line and rotate as the same time, making it unsuitable for a non-holonomic base using an Ackermann drive, as illustrated in FIG. 3.

[0070] To solve the problem of a non-holonomic constraint, customised versions of a “distance function” and a “interpolation function” are created or configured. The customized versions may be used by many sampling-based planning algorithms such as RRTConnect and RRTstar.

[0071] The distance function may be defined as a function that takes in or obtain two different robot configurations and outputs a scalar distance value, the scalar distance value reflecting how far or close the two configurations arc. This function is important in RRTConnect in order to connect each configuration node to its nearest neighbour recursively until the start state is connected to the goal state.

[0072] The interpolation function is defined as a function that interpolates and creates multiple temporary robot configurations between any two given robot configurations for the purpose of collision checking.

[0073] Distance function- The default implementation of “distance function” used by OMPL is the root mean square of all changes in DOFs between any two robot configurations. The modified “distance function” comprises of two sub-functions, a “mobile base distance function” and a “manipulator distance function”. The two sub-functions may be computed as follows. The “mobile base distance function” is a function that receives two mobile base configurations as input and apply an Reeds-Shepp Path algorithm which will always be a smooth connected path comprising circular-line and straight-line segments. The “manipulator distance function” takes in two robot configurations and computes the end-effector pose in cartesian-space using forward kinematics. This function will then output the cartesian distance between the two end-effector poses.

[0074] The maximum value of the two mentioned functions will then be output as the final value of the main modified “distance function”.

[0075] Interpolation function- The default implementation of “interpolation function” in OMPL simply interpolates each DOFs linearly between every two robot configurations which will always generate straight line path for the base instead of smooth curves which are suitable for non-holonomic base. The modified “interpol tion function” is implemented such that the DOFs of the mobile base pose in a planar environment, i.e. along a first spatial axis x, along a second spatial axis y, and an orientation angle of the mobile base about an axis perpendicular to the plane defined by the first and second spatial axes, i.e. yaw, will be constrained to be interpolated only paths that trace a valid Rccds-Shcpp Path, whereas the interpolation of manipulator DOFs will be interpolated linearly. Such a configuration guarantees a non-holonomic path generated between each robot configuration in the planning tree for the purpose of collision checking and generating a final trajectory that will be valid with respect to the actual steering constraints of the physical non-holonomic robot base (as illustrated in FIG. 4).

[0076] In some embodiments, the output of the mobile manipulator planner is a planned trajectory, which may be regarded as an initial trajectory which may be subject to dynamic adjustments. The planned trajectory may include: a sequence of robot configurations, waypoint information or data, timing information or data. The trajectory may be forwarded toward the execution controller for execution by the mobile manipulator.

[0077] The dynamic obstacle avoidance module 230 is configured to monitor the planned trajectory relative to detected objects. This module 230 comprises several components including: future collision detection module 231, trajectory replanning module 232, trajectory splicing module 233.

[0078] These components enable predictive avoidance of moving obstacles.

[0079] The future collision detection module 240 determines whether the planned trajectory is likely to intersect with the predicted motion of obstacles. The dynamic obstacle avoidance module 230 works in close concert with the customised mobile manipulator controller module 240 that is able to accept real-time or live replanned trajectory messages from the former, unlike standard Movelt controller implementations.

[0080] The module 230 receives as inputs the planned trajectory, detected obstacle information, and computes predicted positions of obstacles over a future time horizon. The module 230 evaluates the initial or planned trajectory and obstacle motion across multiple time instances within the time horizon, and the output of the module 240 includes collision timing information, indicating when predicted collisions may occur.

[0081] In a positive determination of a potential future collision, the system performs trajectory replanning, using the trajectory replanning module 232, which generates an alternative trajectory segment that avoids the predicted collision. The replanning may be performed using the same planning framework used to generate the original trajectory. The generated trajectory segment avoids the predicted obstacle motion while still progressing toward the task objective.

[0082] In some embodiments, the dynamic obstacle avoidance or checking works based on the following assumptions: A perception module is configured to track and locate multiple positions of objects or people. A prediction module is configured to predict the future path of all detected objects or people. The controller of the mobile manipulator robot is able to execute the robot trajectory as close to the indicated timestamps as possible. The controller of the mobile manipulator robot is able to synchronise the position of the manipulator with respect to the execution of the base. The controller of the mobile manipulator robot is able to load a new trajectory and executes from the closest position in close to real-time.

[0083] In some embodiments, the dynamic obstacle avoidance module 230 is built on the existing Movelt planning scene which is used for collision checking for motion planning. A separate instance of the Movelt planning scene (from the planner module 210) may be created for the module 230, which may be different from the one used in motion planning. The module230 may be configured to check for future collisions (~20 sec ahead in time) by moving all dynamic obstacles in some small increments of time (e.g. 0.1 seconds); and, checking if there is a collision at each future time snapshot.

[0084] In some embodiments, the future collision detection module 231 may be configured to receive a current position and initial / planned trajectory of the mobile manipulator, i.e. the respective current position and initial / planned trajectory may be loaded into the dynamic obstacle avoidance module 230. The predicted paths of dynamic obstacles, such as objects or humans, are loaded / updated. The module 231 may add a current collision model of the mobile manipulator robot and all dynamic obstacles as cylinders into the planning scene. Collision checking may then be performed. The timestamp and pose of the dynamic obstacle will be recorded if there is a collision. The robot collision model and all dynamic obstacles have their poses within the planning scene updated by advancing the sampled time-stamp of their planned / predicted trajectories by some short time interval (e.g. 0.1 seconds or any other predefined or pre-determined time). The collision checking and updating of robot collision model any dynamic obstacles may be repeated until the look ahead window time period is exhausted.

[0085] In a positive determination of at least one future collision event, the trajectory replanning module 232 is triggered. Once the new trajectory with a replanned or replacement trajectory segment is generated, the logic flow loops back to the loading of updated (now current) position and updated trajectory may be loaded into the dynamic obstacle avoidance module 230.

[0086] If no collisions are detected, an updated prediction of dynamic obstacles is captured on the next control loop and the logic flow loops back to the loading / updating of the predicted paths of dynamic obstacles.

[0087] The replanned trajectory segment may be sent to the trajectory splicing module 233.

[0088] The trajectory splicing module 233 integrates the new trajectory segment with the previously planned trajectory. The splicing operation may involve retaining a prefix portion of the existing trajectory corresponding to the portion already executed or currently being executed; replacing the portion of the trajectory associated with the predicted collision; and connecting the replanned segment to the remaining portion of the original trajectory leading to the goal or destination.

[0089] Such a process may allow the mobile manipulator robot to seamlessly transition into the replanned trajectory, so that the mobile manipulator robot can continue motion without stopping.

[0090] This trajectory splicing module 233 may be triggered by the dynamic obstacle avoidance or checking module 230 if there is a collision within a window of time in the future. Using the timestamp of collisions and the current robot trajectory, a new or replacement segment trajectory is planned around the imminent collisions and then spliced onto the input trajectory, effectively diverting the robot path around the collision-risk obstacles before the robot collides.

[0091] The detailed steps may be listed below:

[0092] The original planned or initial trajectory and imminent collision objects (pose and time to collide) are taken as input. The imminent collision objects may be added into the planning scene 3 to 4 seconds before the first imminent collision, and this will be used as the timestamp and the robot state as the start state for replanning. The replanning may then proceed, around all imminent collisions and collisions from the environment, towards the goal state of the original trajectory.

[0093] From the original planned trajectory, points after the replan start time are deleted and spliced with the replanned trajectory. Timestamps are then adjusted from the spliced point to ensure continuity in the timestamps of the final trajectory. The final trajectory will be published to the mobile manipulator robot controller module 240 for implementation or execution.

[0094] The final trajectory will then be sent back to the dynamic obstacle avoidance module 230 as the new initial trajectory.

[0095] The mobile manipulator robot controller module 240 executes the trajectory by controlling the robot actuators. The controller 240 may also provide feedback relating to a current execution state of the robot.

[0096] This execution feedback can be used by the planning system to ensure trajectory updates remain consistent with the robot's current configuration.

[0097] The architecture shown in FIG. 2 therefore forms a continuous feedback loop, i.e. sensors detect objects in the environment, object detection produces obstacle information, the planner generates a planned trajectory, future collision detection evaluates the trajectory against predicted obstacle motion. In a positive determination of a potential collision, a new trajectory segment is generated, and the trajectory is spliced to replace the affected portion.

[0098] The updated trajectory is then executed by the controller 240.

[0099] Through this pipeline, the mobile manipulator robot is capable of predictive collision avoidance and continuous trajectory adaptation.

[0100] The proposed architecture in FIG. 2 provides several technical advantages compared with conventional robotic planning systems, comprising whole-body planning of both mobile base and manipulator; predictive collision detection rather than reactive stopping; continuous replanning during motion execution; seamless trajectory splicing, allowing uninterrupted robot motion; and compatibility with existing planning frameworks such as Movelt while extending their functionality.

[0101] According to another aspect of the present disclosure there is provided a method for generating a final trajectory of a mobile manipulator. The mobile manipulator may comprise a mobile base and a manipulator.

[0102] Referring to FIG. 5, the method 500 may comprise the following steps.

[0103] Step S501: generating an initial trajectory of the mobile base and the manipulator of the mobile manipulator, the trajectory being constrained by non-holonomic motion of the mobile base;

[0104] Step S502: obtaining predicted motion of one or more dynamic obstacles based on sensor data;

[0105] Step S5O3: determining one or more potential collisions between the initial trajectory and the predicted motion of the dynamic obstacles over a predetermined time window;

[0106] Step S504: generating a replacement trajectory segment for a portion of the initial trajectory corresponding to the one or more potential collisions; and

[0107] Step S505: replacing the portion of the trajectory with the replacement trajectory segment such that the replacement trajectory segment connects to a first portion of the initial trajectory corresponding to a current position of the mobile manipulator and to a second portion of the initial trajectory leading to a destination.

[0108] In some embodiments, generating the initial trajectory further comprises determining one or more configurations of the mobile manipulator along the initial trajectory and determining one or more positions of the dynamic obstacles at multiple discrete time instances within the predetermined time window.

[0109] In some embodiments, generating the initial trajectory further comprises interpolating motion of the mobile base using a sampling-based motion planning algorithm.

[0110] In some embodiments, the method 500 further comprises configuring the samplingbased motion planning algorithm to determine a shortest path of the mobile base constrained by the non-holonomic motion of the mobile base using a Reeds–Shepp path.

[0111] In the various described embodiments, the generation of the initial trajectory is performed using a sampling-based motion planning algorithm. Such algorithms typically evaluate distances between robot states in order to guide the search for feasible motion trajectories. As mentioned, the mobile manipulator planner module 210 employs a modified distance function that separately evaluates motion of the mobile base and motion of the manipulator.

[0112] The modified distance function may be expressed as:FUNCTION modified_distance_functionpath1→2 := Reeds_Shepp(q1, q2)base_dist1→2 := contour_integral(path1→2(s) ds)eef_dist1→2 := euclidean_dist(FK(q1), FK(q2))RETURN max(eef_dist1→2, base_dist1→2)In this formulation, Reeds_Shepp(ql, q2) determines a feasible path of the mobile base between the first state ql and the second state q2, taking into account non-holonomic motion constraints of the mobile base, path1→2 represents the resulting Reeds-Shepp path. base_dist1→2 represents a path length of the mobile base along the Reeds-Shepp path.

[0113] The path length may be determined by evaluating a contour integral along the path. The contour integral may be mathematically expressed in Equation (1) as follows.base_dist 1 —2 — / pathi^s) dsJ' (1) wherein s represents a parameter along the path.

[0114] The modified distance function further determines a distance associated with motion of the manipulator. This may be mathematically expressed in Equation (2) as follows. eef_disti ->2 = euclidean_dist(FK (QI), F ’(gs))Wherein FK(q) represents a forward kinematic transformation of the manipulator corresponding to state q, and euclidean_dist represents a Euclidean distance between endeffector positions. Thus, eef_dist1→2 represents a displacement of an end-effector of the manipulator between the two states.

[0115] Finally, the modified distance function returns the maximum value between the base distance and the end-effector distance, mathematically expressed in Equation (3), as follows. distance = (3)

[0116] By evaluating both the motion of the mobile base and the motion of the manipulator, the modified distance function enables the motion planner to more accurately represent the effort required for the mobile manipulator to move between states. Using the maximum of the base distance and the end-effector distance ensures that large motion required by cither the mobile base or the manipulator is properly reflected in the distance metric used by the planner. Such an approach allows the planner to perform coordinated whole-body planning of the mobile base and the manipulator while respecting the non-holonomic motion constraints of the mobile base.

[0117] In some embodiments, the mobile manipulator planner module 210 employs an interpolation function for determining intermediate robot states between two states during generation of the initial trajectory.

[0118] The interpolation function may be used when evaluating motion between sampled states produced by a sampling-based motion planning algorithm. The interpolation function determines intermediate robot states along a path connecting the two states so that collision detection and feasibility checks may be performed.

[0119] In some embodiments, the planner employs a modified interpolation function that separately determines motion of the mobile base and motion of the manipulator.

[0120] The modified interpolation function may be expressed as:FUNCTION modified_interpolation_function(ql, q2, t)base_path1→2 := Reeds_Shepp(q1, q2)base_state(t) := base_pose_along(base_path1→2, t)arm_state(t):= ql arm + t(q2_arm - ql arm)RETURN combine(base_state(t), arm_state(t))In this formulation: Reeds_Shepp (ql, q2) determines a feasible path of the mobile base between states q1 and q2 that satisfies non-holonomic motion constraints of the mobile base; base_path1→2 represents the resulting Reeds–Shepp path between the two states.

[0121] The interpolation function determines a base pose along the Reeds-Shepp path using a path parameter t.[00122J Thus, the motion of the mobile base follows the same kinematically feasible path determined by the Reeds-Shepp algorithm. The motion of the manipulator is determined independently by linearly interpolating the joint parameters of the manipulator between the two states. The resulting interpolated state therefore includes: a base pose determined along the Reeds-Shepp path, and one or more manipulator joint positions determined by linear interpolation. The interpolated state may therefore be expressed as the combination of the base pose and the manipulator joint positions.

[0123] By interpolating the mobile base along a Reeds-Shepp path while interpolating the manipulator joints linearly, the modified interpolation function ensures that intermediate robot states correspond to feasible whole- body motion of the mobile manipulator.

[0124] Such an approach allows the motion planner to evaluate collision-free motion of both the mobile base and the manipulator while respecting non-holonomic motion constraints of the mobile base.

[0125] Consequently, the planner can generate trajectories in which motion of the mobile base and motion of the manipulator are coordinated in a manner that more accurately reflects the physical capabilities of the mobile manipulator.

[0126] It may be appreciable that the present planner, i.c. the mobile manipulator planner module 210, is based on the Movelt framework (as opposed to move_base). Such an arrangement provides more direct timing control and thus may be a better fit for achieving “low cycle time” solutions and achieving responsive whole-body predictive reactions in response to a dynamic environment.

[0127] The mobile manipulator planner module 210 leverages the Movelt framework's support for third-party planning libraries in its backend, with the OMPL library selected for use.

[0128] The proposed disclosure may comprise a unified whole -body planner for mobile manipulator that satisfying one or more (or all) the conditions below:a) Non-Holonomic constraint for mobile base (car-like path) is supported.b) Manipulator and Base are planned together with collision checking.c) No predefined learning required

[0129] In some embodiments, the dynamic obstacle avoidance or dynamic obstacle checking (DOC) module 230 is configured to check for future collisions by interpolating / predicting paths of moving obstacles with the (latest) current planned whole-body trajectory path. In some embodiments, the trajectory re -planner that re-plans a trajectory segment if the robot path is determined to be colliding in future time, by the DOC module 230, may then splice the re-planned segment onto the existing path. This is done seamlessly without stopping the mobile manipulator robot.

[0130] It may be appreciable that the present disclosure is suited for implementing safety for not just only robot bases or only robot manipulators, and may be additionally for combined robot bases & manipulators (i.e. whole-body manipulation).

[0131] In some embodiments, the implementation of the standard Movelt planning pipeline may be done under the standard ROS / ROS2 framework with a customised OMPL plugin interface.

[0132] In some embodiments, the merging of traversed portion of last planned trajectory path, with newly re-planned trajectory path or segment to avoid predicted collision(s), may be performed using the trajectory splicing module 233.

[0133] It may be appreciable that a continuous-sensing pipeline of visual and / or LiDAR sensor inputs may be adapted into the standard Movelt planning pipeline that traditionally only performs scan-once-and-plan, and leaves continuous collision checking as a higher-level functionality to be implemented by developers.

[0134] It may be appreciable that the present disclosure provides for an implementation of a whole-body collision avoidance within standard ROS framework, extended functionality beyond ROS move_base package's base_local_planner functionality for simple, reactive collision avoidance for robot bases only, provides a method for performing collision prediction within a standard Movelt pipeline with a minimally-extended plugin interface definition; provides a trajectory path splicing algorithm that supports a seamless grafting of a newly replanned remainder trajectory path (to avoid a predicted collision) to the latest already-traversed trajectory path of the last planned path, such that whole robot can seamlessly transition into the re-planned trajectory path without stopping; and introduces tight coupling between thefeedback loop of the action server used by Movelt for trajectory execution, and the planning plugin, to support seamless trajectory path splicing.

[0135] In some embodiments, an assessment of the proposed disclosure’s features compared with current existing ROS-based solutions available, and in the process interpret the present disclosure’s real-world impact beyond such existing solutions. Since the present disclosure is implemented as a standard Movelt plugin, the performance may be on par with other Movelt plugins. The Movelt plugin of the present disclosure us based on RRTConnect, and its efficiency should be of the same order as RRTConnect. As with other Movelt plugins, the implementation of event-triggered loop to repeatedly call the plugin is external to the Movelt framework, so in terms of implementing a looping logic to achieve dynamic replanning, there would be no additional computational cost between the solution of the present disclosure and other Movelt plugin implementations (which would also be repeatedly called within an event-triggered control loop).

[0136] However, the advantage of the present disclosure solution over other existing Movelt-based solutions implementations, in particular for dynamic safety, is in its support for whole-body path planning for mobile manipulators, which includes, but is not limited to, manipulator on a floating base, as well as its ability to perform collision prediction, as opposed to just obstacle detection, dynamic trajectory path splicing, as opposed to just a brute-force stop / re-plan / re-start for every obstacle detection instance.

[0137] In comparison with move_base, there is already a low -level collision safety built into move_base (for its global planner) which allows move_base to pseudo-dynamically replan a robot base trajectory in response to new obstacles detected (within the global map). However, move_base may only be used for mobile bases and cannot be applied to manipulators, such as arms. Consequently, achieving mobile manipulation using move_base may not be realistic -as it may comprise safety moving the manipulator arm while the base is itself in motion, given that move_base has no information on the state of the arm and cannot test for possible collisions of the moving arm with environment obstacles. Although there is an option to update the robot footprint under move_base, such that it would cover the projected operating volume of the arm, but this may drastically reduce the navigation robustness in narrow spaces. Additionally, move_base planners do not support time constraints for executing their path trajectory given a start and end pose (for the mobile base). This means that is practically impossible to implement a whole-body mobile-manipulator solution whereby the target pose needs to be achieved within a specified time, if leveraging move_base. A hybrid approach of combining Movelt control forthe manipulator arm, and move_base for the mobile base would still be unwieldy and impractical - the manipulator arm would need to tailor its trajectory path execution to match the progress of the base, and the combined movement would still not be able to support task time constraints (on account of move_base's limitation).

[0138] It is envisioned the real-world impact of the proposed disclosure as being able to support real-world tasks that require synchronised whole -body mobile-manipulation that can still offer a degree of dynamic safety. This would allow mo bile -manipulator robots to better achieve real-world seamless co-working & coordination in a shared human-robot work space.

[0139] Possible commercial uses and applications of the present disclosure are those in which a mobile manipulator robot may be needed to safely & efficiently perform a whole-body manipulation task in an environment with other active animated participants, with minimal interruption on the smooth performance of either the mobile manipulator robot or any other entity's task due to intersecting trajectories, such as, but not limited to:• A table cleaning / clearing robot working in a hawker centre with multiple human customers moving around;• A silicon-wafer transporting mobile manipulator robot smoothly transferring fragile payload from one station to another in a closed working environment with other dynamic actors such as human staff or other autonomous robots also attempting to carry out their own tasks efficiently.• Wheel track and bucket planning for excavators performing actions such as digging and moving of soil.• Path planning and picking objects using drones with manipulator.• Complete Plan Planner non-holonomic Mobile Manipulator integrated with Movelt Planner in the ROS framework with the ability to plan in a high variation and cluttered environment with complete full body collision checking.• Dynamic Obstacle Checking moving objects against robot base and arm paths. Partial replanning of robot paths and integration with the old paths.• Merging of traversed portion of last planned trajectory path, with newly re-planned trajectory path to avoid predicted collision(s).

[0140] While the disclosure has been particularly shown and described with reference to specific embodiments, it should be understood by those skilled in the art that various changes in form and detail may be made therein without departing from the spirit and scope of the disclosure as defined by the appended claims. The scope of the disclosure is thus indicated bythe appended claims and all changes which come within the meaning and range of equivalency of the claims are therefore intended to be embraced.

Claims

CLAIMS1. A mobile manipulator including a mobile base and a manipulator, the manipulator comprising a processor configured to:generate an initial trajectory of the mobile base and the manipulator of the mobile manipulator, the trajectory being constrained by non-holonomic motion of the mobile base;obtain predicted motion of the one or more dynamic obstacles based on sensor data;determine one or more potential collisions between the initial trajectory and the predicted motion of the dynamic obstacles over a predetermined time window;generate a replacement trajectory segment for a portion of the initial trajectory corresponding to the one or more potential collisions; andreplace the portion of the trajectory with the replacement trajectory segment such that the replacement trajectory segment connects to a first portion of the initial trajectory corresponding to a current position of the mobile manipulator and to a second portion of the initial trajectory leading to a destination.

2. The mobile manipulator of claim 1, wherein the processor is configured to determine one or more configurations of the mobile manipulator along the initial trajectory and one or more positions of the dynamic obstacles at multiple discrete time instances within the predetermined time window.

3. The mobile manipulator of claim 1 or 2, wherein in the generation of the initial trajectory constrained by the non-holonomic motion of the mobile base, the processor is configured to interpolate motion of the mobile base using a sampling-based motion planning algorithm.

4. The mobile manipulator of claim 3, wherein the sampling-based motion planning algorithm is further configured to determine a shortest path of the mobile base constrained by the non-holonomic motion of the mobile base using a Reeds–Shepp path.

5. The mobile manipulator of any one of claims 1 to 4, wherein the processor is further configured to generate the replacement trajectory segment based on a configuration of the mobile manipulator corresponding to a time prior to the one or more potential collisions.

6. The mobile manipulator of claim 1, wherein the processor is further configured to determine a distance used by the sampling-based motion planning algorithm based on both motion of the mobile base and motion of the manipulator.

7. The mobile manipulator of claim 6, wherein the processor is further configured to determine the distance based on (a) a shortest path of the mobile base constrained by the non-holonomic motion of the mobile base; and (b) a distance travelled by the manipulator.

8. The mobile manipulator of claim 7, wherein the processor is further configured to determine the shortest path of the mobile base using a Reeds–Shepp path.

9. The mobile manipulator of claim 8, wherein the processor is further configured to determine the distance between the one or more configurations as a maximum value between the shortest path of the mobile base determined using the Reeds–Shepp path constrained by the non-holonomic motion of the mobile base; and a distance travelled by the manipulator.

10. The mobile manipulator of claim 1, wherein the processor is further configured to update the predicted motion of the dynamic obstacles using updated sensor data obtained from one or more sensors.

11. The mobile manipulator of claim 1, wherein the processor is further configured to determine interpolated configurations of the mobile base and the manipulator between one or more configurations when generating the initial trajectory.

12. The mobile manipulator of claim 11, wherein the processor is further configured to determine an interpolated configuration of the mobile base along a Reeds–Shepp path between the one or more configurations of the mobile base.

13. The mobile manipulator of claim 12, wherein the processor is further configured to determine an interpolated configuration of the manipulator by linearly interpolating differences in joint angles of the manipulator.

14. The mobile manipulator of claim 13, wherein the processor is further configured to determine the interpolated configurations of the mobile base and the manipulator simultaneously along the Reeds–Shepp path and the interpolated joint angles.

15. A method for generating a final trajectory of a mobile manipulator having a mobile base and a manipulator, comprisinggenerating an initial trajectory of the mobile base and the manipulator of the mobile manipulator, the trajectory being constrained by non-holonomic motion of the mobile base;obtaining predicted motion of one or more dynamic obstacles based on sensor data; determining one or more potential collisions between the initial trajectory and the predicted motion of the dynamic obstacles over a predetermined time window;generating a replacement trajectory segment for a portion of the initial trajectory corresponding to the one or more potential collisions; andreplacing the portion of the trajectory with the replacement trajectory segment such that the replacement trajectory segment connects to a first portion of the initial trajectory corresponding to a current position of the mobile manipulator and to a second portion of the initial trajectory leading to a destination.

16. The method of claim 15, wherein generating the initial trajectory further comprises determining one or more configurations of the mobile manipulator along the initial trajectory and determining one or more positions of the dynamic obstacles at multiple discrete time instances within the predetermined time window.

17. The method of claim 15 or 16, wherein generating the initial trajectory further comprises interpolating motion of the mobile base using a sampling-based motion planning algorithm.

18. The method of claim 17, further comprising configuring the sampling-based motion planning algorithm to determine a shortest path of the mobile base constrained by the non-holonomic motion of the mobile base using a Reeds–Shepp path.

19. A computer program element comprising program instructions, which, when executed by one or more processors, cause the one or more processors to perform the method of any one of claims 15 to 18.

20. A non-transitory computer-readable medium comprising program instructions, which, when executed by one or more processors, cause the one or more processors to perform the method of any one of claims 15 to 18.

21. A system comprisinga mobile manipulator including a mobile base and a manipulator;one or more sensors arranged in data or signal communication with the mobile manipulator, and configured to detect dynamic obstacles; anda processor configured to:generate an initial trajectory of the mobile base and the manipulator of the mobile manipulator, the trajectory being constrained by non-holonomic motion of the mobile base;obtain predicted motion of the one or more dynamic obstacles based on sensor data from the one or more sensors; determine one or more potential collisions between the initial trajectory and the predicted motion of the dynamic obstacles over a predetermined time window;generate a replacement trajectory segment for a portion of the initial trajectory corresponding to the one or more potential collisions; andreplace the portion of the trajectory' with the replacement trajectory segment such that the replacement trajectory segment connects to a first portion of the initial trajectory corresponding to a current position of the mobile manipulator and to a second portion of the initial trajectory' leading to a destination.

Images