System and method for controlling a wheeled mobile manipulator
Patent Information
- Application Number
- JP2026025295
- Authority / Receiving Office
- JP · JP
- Patent Type
- Applications
- Current Assignee / Owner
- Priority Date
- 2025-03-31
- Filing Date
- 2026-02-19
- Publication Date
- 2026-10-14
Smart Images
Figure 2026167477000001_ABST
Abstract
Description
Technical Field
[0001] The present disclosure relates generally to robotics and associated motion planning. More specifically, the present disclosure relates to a method for controlling a mobile manipulator having a wheeled base and an articulated manipulator.
Background Art
[0002] Mobile manipulators are widely used in the manufacturing industry, logistics industry, medical robotics industry, and service industry, as they allow position adjustment within the workspace while expanding the reach of the associated manipulator arm. Unlike manipulators with a fixed base, mobile manipulators coordinate the movements of both the wheeled base and the manipulator arm to perform tasks such as object manipulation, assembly, inspection, and material transport. However, the simultaneous control of mobility and dexterity presents significant technical challenges, particularly in ensuring stability, safety, and efficient task execution.
[0003] One of the main challenges in mobile manipulators is high-speed motion planning under stability constraints. When the manipulator moves or extends, the center of mass (CoM) shifts, which may lead to tipping. Many existing approaches rely on sequential planning, where the base moves first and then the manipulator operates, to achieve high-speed planning. However, attempts to prevent or manage tipping in such methods slow down task execution and are inefficient for real-time applications. More advanced approaches attempt to synchronize the movements of the base and the manipulator, but such approaches generally impose overly conservative speed limits to prevent tipping, which reduces overall performance.
[0004] Another significant limitation of conventional systems is their reliance on either kinematic or full dynamics models for motion planning. While kinematic models are computationally efficient, they fail to capture 1) the dynamic interaction between the base and the manipulator, and 2) the dynamic interaction between the base and the ground, making them unreliable for high-speed motion and leading to unsafe maneuvers. Full dynamics models, on the other hand, consider force interactions but are computationally expensive and difficult to implement in real-time robot control. In addition, many existing methods rely on reactive anti-tipping mechanisms that adjust the robot's movement only after instability is detected, resulting in inefficient and sometimes unpredictable corrections.
[0005] Furthermore, current methods often fail to distinguish between forces affecting stability and forces affecting the feasibility of motion. Mobile manipulators must consider external forces such as ground reaction forces, gravity, and inertial forces to ensure stability, but internal forces generated by the actuator determine whether the planned trajectory is physically feasible. Many existing planning algorithms either overcompensate for stability, resulting in unnecessarily restrictive motion constraints, or they fail to consider the feasibility of operation, resulting in motion plans that exceed actuator torque limits.
[0006] Considering these challenges, there is a need for improved motion planning and control methods for mobile manipulators that ensure stability and feasibility while maintaining computational efficiency. [Overview of the project]
[0007] This disclosure provides an improved motion planning and control method for mobile manipulators that ensures stability and feasibility while maintaining computational efficiency. This disclosure provides a method that significantly improves the agility, safety, and operational efficiency of mobile manipulators by effectively incorporating stability constraints into motion planning, distinguishing between external and internal forces, and enabling real-time trajectory adjustments.
[0008] This method plans and controls the motion of a mobile manipulator by determining a safe trajectory for the coordinated motion of the base and arm using a joint motion model that considers external forces during motion while ignoring the dynamics of internal forces acting on the robot, and by applying stability constraints to external forces to ensure that the external forces maintain the dynamic stability of the robot during motion in order to determine a safe trajectory. In the embodiment, the stability constraint is applied / implemented by applying a zero moment point (ZMP) constraint to ensure that the resulting external forces and moments acting on the robot keep the robot's ZMP within the supporting polygon. The joint motion model is a reduced-order model having a center of mass momentum model that represents the dynamics of the net linear and angular momentum of the robot's center of mass, thereby enabling the determination of external forces resulting from motion while ignoring internal forces at the joint level.
[0009] Another embodiment discloses a method for controlling a robot to perform a task. The robot is a mobile manipulator, which includes a non-holonomic wheeled base having wheels for movement on a flat surface, and a manipulator arm connected to the wheeled base having additional degrees of freedom for performing a task. The method uses a processor coupled with stored instructions to implement the method. The instructions, when executed by the processor, perform steps of the method, which include receiving a task from a task planner in the form of a series of how-to points. The robot is collision-free at the series of how-to points. The method also includes determining a safe trajectory for the coordinated motion of the wheeled base and manipulator arm of the robot to perform the task, using a joint motion model of the wheeled base and manipulator arm of the robot, which includes external forces acting on the robot as it moves along the trajectory, while ignoring the dynamics of internal forces generated by the robot's actuators that produce the external forces. The safe trajectory is determined under stability constraints against external forces, ensuring that the external forces maintain the dynamic stability of the robot. The method further includes determining the internal forces required for a robot's actuators to track a safe trajectory, under feasibility constraints on the generation of internal forces by the actuators, and adjusting the safe trajectory to satisfy the feasibility constraints in order to generate a feasible trajectory. In addition, the method includes controlling the robot's actuators to generate internal forces that cause the robot to follow a feasible trajectory. [Brief explanation of the drawing]
[0010] [Figure 1] Figure 1 shows a schematic diagram illustrating the transition from a sequential approach to generating motion trajectories using prior art to a direct integration of motion planning and stability constraints to achieve efficient and feasible trajectories, as described in several embodiments. [Figure 2]Figure 2 shows a schematic diagram of the transition from a unified dynamics model that integrates both internal and external forces to generate a motion trajectory, to a motion planning model, as in some embodiments, that is constructed based solely on external forces to generate a safe trajectory. [Figure 3] Figure 3 shows a robot having a wheeled base and a manipulator arm, according to an example of an embodiment. [Figure 4] Figure 4 shows a schematic block diagram illustrating various elements of a controller for controlling a robot according to several embodiments. [Figure 5A] Figure 5A shows a schematic block diagram illustrating various elements of a safety motion planner for generating a safe trajectory for a robot's movement, according to several embodiments. [Figure 5B] Figure 5B shows the transition from modeling a wrench for each wheel on the robot's base to a single virtual aggregated wrench determined using a wheel-ground contact model generated by treating the wheeled base as a rigid body in contact with the ground, as in some embodiments. [Figure 6] Figure 6 shows a block diagram illustrating various elements involved in generating feasible trajectories from safe trajectories, according to several embodiments. [Figure 7] Figure 7 shows a block diagram illustrating various elements involved in generating feasible trajectories from safe trajectories, according to several embodiments. [Figure 8] Figure 8 shows a method for controlling a robot to perform a task according to several embodiments. [Figure 9] Figure 9 shows a method for implementing stability constraints against external forces to generate a safe trajectory, according to several embodiments. [Figure 10] Figure 10 shows a method, according to several embodiments, for determining internal forces to track a safe trajectory and adjusting the safe trajectory to a feasible trajectory. [Figure 11] Figure 11 shows a method for determining a safe trajectory using a joint motion model according to several embodiments. [Modes for carrying out the invention]
[0011] Summary of contributions to the technology This summary is provided to briefly introduce selected concepts from those further explained in the detailed description below. This summary is not intended to identify the main or essential features of the claimed subject matter, nor is it intended to be used to assist in determining the scope of the claimed subject matter.
[0012] Embodiments of this disclosure are based on the understanding that motion planning and stability in mobile manipulators are inherently interconnected. When motion planning is treated as an independent issue and stability constraints are introduced as secondary considerations, this approach leads to inefficiency or a lack of safety assurance. Specifically, when motion trajectories are first generated and then adjusted or restricted to prevent instability such as overturning, such post-correction leads to unpredictable trajectory changes, slower task execution, and an overly conservative tendency. Embodiments recognize that in order to achieve efficient and dynamically feasible trajectory planning, stability must be directly incorporated into the motion planning process rather than being applied as a post-corrective measure.
[0013] Figure 1 shows a schematic diagram illustrating the transition from a conventional sequential approach that generates a motion trajectory 100 using a motion plan 102 and then adjusts the generated trajectory 100 based on stability constraints 104, resulting in unpredictable trajectory changes, to a method that directly integrates the motion plan 102 and stability constraints 106 to achieve an efficient and feasible trajectory 110.
[0014] Further embodiments are based on the recognition that the feasibility and safety of motion stem from different physical principles. Feasibility is primarily governed by internal forces, i.e., forces generated by the robot's actuators, such as torques applied from motors to the manipulator joints and base wheels. In contrast, safety, particularly the prevention of tipping, is governed by external forces, such as gravity, ground reaction forces, and inertial effects during motion. Conventional approaches typically consider a unified dynamics model that integrates both internal and external forces and treats them as a combined entity. However, embodiments recognize that, with safety in mind, external forces are more dominant than internal forces.
[0015] Figure 2 shows a schematic diagram of the transition from a unified dynamics model 202, which integrates both internal and a small number of external forces to generate a motion trajectory 204, involving inefficient and complex calculations, to a motion planning model 210 built on external forces alone to generate a safe trajectory 212, which ensures stability while significantly simplifying calculations. This difference allows the embodiment to generate a safe trajectory in a more efficient way than the conventional approach.
[0016] Some embodiments address the problem of controlling a robot to perform a task, and this robot is a mobile manipulator comprising a non-holonomic base having wheels for movement on a flat surface and a manipulator connected to the base that provides additional degrees of freedom for performing tasks. Examples of such manipulators include a robotic arm having multiple joints and corresponding degrees of freedom, enabling actions such as lifting, inserting, moving, and other manipulative tasks of objects.
[0017] This type of robot has a large number of degrees of freedom corresponding to internal forces generated by the robot's actuators to control its movement. However, despite this complexity, the totality of all these internal forces gives rise to a far smaller number of external forces that act on the robot's wheels and the interaction between the robot and the environment. Since external forces arise as reactions to internal forces, considering only external forces while ignoring internal forces during motion planning can lead to a significant reduction in the dimensionality of the motion planning problem. This simplification enables the generation of more computationally efficient trajectories while maintaining stability and control accuracy.
[0018] The embodiments further recognize that prioritizing safety ensures trajectory stability, but does not inherently guarantee feasibility. In other words, a trajectory planned based solely on external forces may, in very rare cases, require internal forces (such as motor torque) that exceed the physical actuation capability of the robot. However, experimental verification has shown that such situations are rare. In most cases, trajectories designed using external force constraints remain feasible for execution. In addition, even if the initially generated trajectory is infeasible, this means that the trajectory remains safe, i.e., it does not cause tipping or instability. This enables a real-time feasibility correction mechanism that can slightly modify the trajectory at runtime, ensuring that it stays within the actuation limits of the robot while maintaining its stability. As a result, the embodiments recognize that a two-stage approach of first generating a safe trajectory based on external forces and then applying real-time feasibility adjustments leads to a motion planning framework that is computationally efficient yet dynamically feasible.
[0019] Another embodiment is based on the recognition that internal forces typically dominate external forces in most robots, as internal forces induce external reaction forces. For this reason, conventional motion models are described in terms of internal forces, since internal forces primarily propel movement. However, with regard to fall prevention, external forces have the greatest impact. This is because a robot falls when external force interactions (such as ground reaction forces and inertial loads) shift the zero-moment point (ZMP) outside the support polygon. The embodiments recognize that by rewriting the motion model in terms of external forces rather than internal forces, the overall dimensionality of the motion model can be reduced while capturing the dynamics essential for stability. This change simplifies calculations while maintaining the accuracy of predicting stability constraints.
[0020] Based on this insight, a further embodiment recognizes that centroidal momentum provides an optimal framework for modeling external forces while ignoring internal forces. Conventional force-based models track individual interactions between joints, actuators, and the environment, resulting in high-dimensional computations. In contrast, centroidal momentum dynamics integrate the effects of external forces by modeling the net linear momentum and angular momentum at the center of mass (CoM) of the robot. This eliminates the need to explicitly account for all internal joint forces, while still capturing the dynamic effects that affect stability. As a result, centroidal momentum functions as a computationally efficient yet still accurate representation of the external forces acting on the system, which is fully consistent with the previous recognition that safety can only be determined by accounting for external forces.
[0021] In addition, the embodiment recognizes that stability can be efficiently implemented through ZMP constraints. ZMP is an established metric for evaluating whether a robot will tip over based on the balance between external forces and moments. However, conventional approaches often involve reactively calculating ZMP or using high-fidelity dynamics models that require extensive computational resources. The embodiment directly incorporates ZMP constraints into the motion planning stage to ensure that the generated trajectory is stable from the outset, rather than requiring post-processing corrections. Furthermore, since full ZMP calculations can be costly, the embodiment employs order-reducing modeling techniques (such as center-of-mass momentum-based formulations) to ensure that ZMP constraints are implemented without excessive computational overhead.
[0022] Another embodiment is based on the recognition that there is a trade-off between manipulator extension and base velocity when ensuring stability. As the robot's CoM shifts depending on the manipulator's position, the planned trajectory balances the movement of the manipulator arm with the motion of the base in order to maintain stability constraints. If the manipulator arm extends too far while the base is accelerating / decelerating, the ZMP shifts outside the support polygon, which can lead to instability. The embodiment recognizes that by dynamically adjusting the ZMP constraint based on the planned task, the motion planner can determine the optimal balance between the manipulator's reach and the motion of the base. This implementation of adaptive stability allows the robot to maximize its task performance while maintaining safety.
[0023] In light of these understandings, the embodiments provide a novel motion planning framework that integrates safety constraints and feasibility constraints in a computationally efficient manner. By prioritizing external forces for stability (safety) while enabling internal force adjustments to correct feasibility limitations in real time, the embodiments realize a dynamically stable, computationally efficient, and task-effective solution for mobile manipulator motion planning. Furthermore, by utilizing the center of mass momentum as a reduced-order model, some embodiments enable real-time execution without excessive computational requirements. By integrating ZMP-based stability constraints, adaptive manipulator-based coordination, and real-time feasibility adjustments, a robust motion planning method is obtained that ensures mobile manipulators can perform tasks safely, efficiently, and without unnecessary conservative tendencies.
[0024] The purpose of this disclosure is to enable agile operation of certain wheeled mobile manipulators while ensuring dynamic feasibility and safety during motion planning and achieving a better balance between safety and productivity. In short, this means faithfully capturing and effectively utilizing the constraints / models related to dynamic feasibility and safety during motion planning and control, specifically that these constraints and models must be sufficiently simple.
[0025] Embodiments of this disclosure provide a method and system for controlling a wheeled mobile manipulator in which the mobile base is driven by differential drive.
[0026] This disclosure is based on the recognition that it is important to perform motion planning based on a reduced-order model that characterizes kinematic feasibility, dynamic feasibility, and safety. Here, the kinematic feasibility arising from differential drive is characterized by non-holonomic constraints embedded in the base kinematic model across a 2D plane, which involve linear translation in the x and y directions and rotation in the z direction (also named after yaw).
[0027] Furthermore, in order to capture unsafe operations and thereby avoid them, it is necessary to treat the base as a floating base with six degrees of freedom (DoF), and therefore it is understood that a 6DoF kinematic model of the base should be considered.
[0028] Furthermore, to prevent unsafe operations, such as tipping over, it is important to implement a Zone of Motion (ZMP) inside the convex hull formed by the wheels.
[0029] Another recognition of this disclosure is that the dynamic feasibility of motion for a given mobile manipulator is primarily governed by the magnitude of the force and torque (the combination of which is named after a wrench) that the ground can exert on the base through its interaction with the wheel. Given the joint positions and velocities of a particular mobile manipulator, and given the linear and angular momentum of the mobile manipulator's center of mass, and considering the mobile manipulator as an isolated system, the required forces and torques from the ground can be fully captured by the rate of change of momentum, i.e., the center of mass dynamics of the mobile manipulator. In other words, as long as the motion produces a lumped wrench that can be generated by the interaction between the wheel and the ground, the motion is dynamically feasible (in this case, dynamic constraints caused by the actuator are ignored).
[0030] In some embodiments, dynamic feasibility can be expressed by constraining the rate of change of angular momentum corresponding to a wrench applied (through a wheel) to a base located inside a particular friction cone.
[0031] Detailed description of the drawing Referring to Figure 3, an example mobile manipulator robot 300 (hereinafter simply referred to as robot 300) is shown for performing a task, for example, moving an object from one place to another. Robot 300 includes a wheeled base 302 and a manipulator arm 304 that is positioned to be supported on the wheeled base 302 and configured to move relative to the wheeled base 302. The arm 304 includes an end effector 306, for example, a gripper, to lift an object from one place, hold the object while the manipulator arm 304 is being operated and the wheeled base 302 is moving from one place to another, and position the object at the desired location. In this embodiment, to manipulate an object, robot 300 moves across a surface using the wheeled base 302 and simultaneously operates the arm 304 to reach the object. To this end, the controller 310 of robot 300 controls robot 300 using a method that relies on external forces to generate the trajectory of the robot's motion, as described below. The controller 310 includes a processor 312 for executing instructions to control the robot 300 for performing tasks, and a memory 314 for storing instructions and models related to the control of the robot 300.
[0032] Referring to Figure 4, a block diagram schematically showing the various steps / elements of the controller 310 in controlling the robot 300 will be described next. In order to control the robot 300 and move it from one place to another to reach an object, the task planner 400 generates a series of waypoints for reaching the object and performing the task of, for example, lifting the object. Note that the task planner 400 generates the series of waypoints so that the robot's movement / motion does not collide with the environment and the robot itself. The task planner 400 shares the series of waypoints it has generated with the safety motion planner 405, which, based on the series of waypoints, generates a safe trajectory for the coordinated motion of the wheeled base 302 and manipulator arm 304 of the robot 300 to perform the task. In an embodiment, the motion planner 405 uses a joint motion model of the wheeled base 302 and manipulator arm 304 to generate a safe trajectory.
[0033] In some embodiments, the passing point is represented as the orientation of the end effector or the configuration of all DoF of the mobile manipulator, which includes the angle of the wheels, the linear and angular displacements of the base, as well as the displacement along all DoF of the arms.
[0034] In this embodiment, the joint motion model considers / includes external forces acting on the robot as it moves along a trajectory, while neglecting the dynamics of internal forces generated by the actuators of the robot 300 that produce the external forces. The motion planner 405 also determines a safe trajectory by applying stability constraints to the external forces, ensuring that the external forces maintain the robot's dynamic stability. The stability constraints prevent instability of the robot 300 during its motion, such as tipping over. Note that by using / applying stability constraints to the external forces, the motion planner 405 determines the optimal balance between the movement of the manipulator arm 304 and the movement of the wheeled base 302. This implementation of adaptive stability allows the robot to maximize its task performance while maintaining safety.
[0035] Once a safe trajectory is generated, the feasible trajectory generator / tracking controller 410 checks the safe trajectory received from the motion planner 405 and, if it detects that it is not feasible to track / follow the safe trajectory, adjusts the safe trajectory to generate a feasible trajectory so that the feasible trajectory satisfies feasibility constraints. To adjust the safe trajectory, the feasible trajectory generator / tracking controller 410 determines the internal forces generated by the actuators of the robot 300 so that it follows the safe trajectory and satisfies feasibility constraints. In embodiments, the feasibility constraints include limitations on the internal forces that the actuators can generate. Once a feasible trajectory is generated, the robot components 420, such as actuators, are controlled to generate internal forces that cause the robot to follow the safe trajectory or the adjusted safe trajectory, i.e., the feasible trajectory. As the robot 300 is being moved along a feasible trajectory, the robot 300's sensors 425 detect the trajectory the robot is following, the internal forces of the actuators, and the external forces acting on the robot, and share feedback with the motion planner 405, task planner 400, and trajectory generator / controller 410 to enable safe operation of the robot 300.
[0036] Referring to Figure 5A, a block diagram showing the various elements of the safety motion planner 405 is displayed. The motion planner 405 includes a wheel-ground contact model 500 that generates the order reduction model 505 and the safety constraint model 510. Generally, the robot 300 receives contact wrenches at all wheels in contact with the ground, and each wrench is a 6-dimensional vector. The wheel-ground contact wrenches, which encompass vertical forces, frictional forces and moments, as well as the resulting slip dynamics, depend on factors such as slip angle, tire characteristics, surface material, and contact dynamics. Modeling the individual contact wrenches at each wheel would result in considerable computational complexity, and it is not necessary to derive a wheel-ground contact model for the mobile manipulator that captures the wrenches for all wheels and implement motion planning based on this model.
[0037]
number
[0038] The motion planner 405 determines a safe trajectory 520 for the robot 300's movement according to the received waypoints by modeling a reduced-order model 505 or a joint motion model 505 using this single virtual aggregate wrench 502. The safe trajectory 520 includes the position, velocity, and wrench trajectory of the wheeled base and manipulator arm.
[0039] kinematic models
[0040]
number
[0041] In some embodiments, the mobile base employs a different drive system, such as front-wheel drive, and the kinematic model of the mobile base accepts a well-known bicycle model form.
[0042] In some embodiments, the mobile base employs an omnidirectional drive system, and the kinematic model of the mobile base can also be easily found in the prior art.
[0043] The methods for safe motion planning taught in this disclosure can be readily applied to mobile manipulators having different drive mechanisms.
[0044] Explanation of the wheel-ground contact model assumption
[0045]
number
[0046] Decreased Order Model
[0047]
number
[0048]
number
[0049]
number
[0050]
number
[0051]
number
[0052] This combined expression of dynamics ensures that the forces and moments applied through the control inputs are achievable through the robot's physical motion.
[0053] A single virtual aggregate wrench model 500 is also used to model safety constraints, i.e., stability constraints applied to external forces to determine a safer trajectory for the robot 300's motion. Thus, the safety motion planner 405 determines a safe trajectory for the robot's movement following the given points by inputting the wheel-to-ground contact model 500 into the order reduction model 505 and modeling the stability constraints 510.
[0054] stability constraints To ensure the safe and stable operation of robot 300, motion planner 405 imposes safety constraints, including ZMP constraints to prevent falls, friction constraints to avoid slipping, and joint constraints to ensure physical feasibility.
[0055]
number
[0056]
number
[0057]
number
[0058]
number
[0059] This approximation provides a conservative but computationally efficient alternative for friction cones, allowing friction limits to be expressed using linear constraints.
[0060]
number
[0061] Together, these Model 510 safety constraints ensure that applied forces and moments remain within the limits of frictional stability, thereby preventing both sideslip and rotational slip during steering.
[0062]
number
[0063]
number
[0064] Using the stability constraints provided by the reduced-order models 505 and 510, the motion planner 405 generates a safe trajectory 520. To do so, the safe motion planner 405 solves a target-reaching task, formulated as an offline or online nonlinear program (NLP), to ensure safe and efficient movement to a predetermined target.
[0065]
number
[0066]
number
[0067]
number
[0068]
number
[0069]
number
[0070] Here, (dynamic constraints) are implemented as equality constraints used in the collocation of the forward Euler equation of motion (order-reduced model). Safety constraints, including (C-ZMP), (C friction), (C rotation), and (C limit), are expressed together as inequality constraints.
[0071] In this way, the motion planner 405 generates a safe trajectory 520 for the robot's movement to reach an object positioned at a predetermined location. In some embodiments, the safe trajectory 520 is characterized by a time profile of a reduced-order model state x over a time horizon, in other words, a time profile of a reduced-order system / model control u.
[0072] Referring to Figure 6, a block diagram is disclosed showing the various elements / features / steps involved in generating feasible trajectories from a safe trajectory 520. As shown, the feasibility tracker generator 610 utilizes the safe trajectory 520 generated by the motion planner 405 and, in 610, uses a first full-order dynamics model 620 and control constraints 630 to dynamically generate or check the feasibility of the safe trajectory 520. The feasibility trajectory generator 610 also uses input from the robot 300's sensor 425 to further determine the feasibility of the safe trajectory 520 and generates control commands to control and move the robot 300 based on the generated feasible trajectories.
[0073]
number
[0074] The use of 3D base coordinates in the dynamics model of robot 300 stems from the ability of the dynamics model to capture the principal motion characteristics required for planar movement tasks while maintaining computational simplicity. Because the 3D base model does not consider forces and torques along the vertical axis or rotations around the pitch and roll axes of the base, the 3D base model cannot adequately capture the dynamics required for fall prevention. In some embodiments, omitting vertical dynamics under high-speed or high-load conditions leads to inaccuracies in stability predictions. Using the first full-order dynamics model 620 is not a matter of dynamic feasibility, as a safe trajectory 520 is identified when the safety motion planner is in the lead, and the dynamic feasibility trajectory generator 610 finds a feasible trajectory as close as possible to the safe trajectory 520, which means that the safety of the feasible trajectory is likely to be maintained.
[0075]
number
[0076]
number
[0077] The feasible trajectory generator 610 optimizes using a cost function that minimizes the error between the end-effector attitude corresponding to the safe trajectory 520 and the end-effector attitude corresponding to the feasible trajectory. This cost function is based on the understanding that safety requirements are met by bringing the safe trajectory and the feasible trajectory as close as possible while maintaining end-effector attitude tracking accuracy.
[0078]
number
[0079] Referring to Figure 7, a block diagram is disclosed showing the various elements / features of the controller 310 involved in generating feasible trajectories. As shown, the feasibility tracker generator 710 utilizes the safe trajectory 520 generated by the motion planner 405 and uses a second full-order dynamics model 700 and control constraints 630 to dynamically generate or check the feasibility of the safe trajectory 520. The feasibility trajectory generator 710 also uses input from the robot 300's sensor 425 to further determine the feasibility of the safe trajectory 520 and generates control commands to control and move the robot 300 based on the generated feasible trajectory.
[0080]
number
[0081]
number
[0082] Compared to (TSC-QP), this optimization is similar to a quadratic program that solves for the optimal operating torque for the exponential tracking of the desired output. Using a second full-order dynamics model 700, an additional safety criterion is imposed to ensure that the ZMP is within the base support polygon.
[0083] Referring to Figure 8, a method 800 for controlling the robot 300 to perform a task is shown. Method 800 uses a processor to execute method steps / instructions stored in memory. Method 800 includes step 810 of receiving a task in the form of a series of waypoints from a task planner, for example, the task planner 400 of the controller 310. The series of waypoints is determined by the task planner so that the robot 300 does not collide with any of the waypoints. Method 800 then includes step 820 of determining a safe trajectory for the coordinated motion of the wheeled base 302 of the robot 300 and the manipulator arm 304 of the robot 300 to perform the task, using a joint motion model of the wheeled base 302 of the robot 300 and the manipulator arm 304. In some embodiments, the joint motion model is implemented as a reduced-order model 505 that simplifies stability evaluation by aggregating the effects of external forces into a compact representation, thereby reducing computational complexity while maintaining safety assurance.
[0084] In some embodiments, the joint motion model generates a safe trajectory based on the external forces acting on the robot 300 as it moves along the safe trajectory, while ignoring the dynamics of internal forces generated by the robot's actuators that produce external forces. In some embodiments, the safe trajectory is determined under stability constraints against external forces, ensuring that the external forces maintain the robot's dynamic stability. In some embodiments, the stability constraints include one or more of the following: ZMP constraints, friction constraints, rotational constraints, and joint angle constraints. External forces include ground reaction forces, gravity, inertial forces, etc. Also in some embodiments, the motion planner 405 dynamically adapts the stability constraints based on variations in external forces, including payload weight, ground reaction forces, and changes in inertial loads caused by acceleration, to ensure the robot's continued stability during motion. In some embodiments, the joint motion model, i.e., the reduced-order model, includes a center-of-mass momentum model representing the dynamics of the net linear and angular momentum of the robot's center of mass, thereby enabling the determination of external forces resulting from motion while ignoring internal forces at the joint level.
[0085] Once a safe trajectory is determined, method 800 includes step 830 of determining the internal forces required for the robot 300's actuators to track or follow the safe trajectory, subject to feasibility constraints on the generation of internal forces by the actuators. In some embodiments, feasibility constraints on internal forces include actuator torque limits, speed constraints, and power consumption constraints. In some embodiments, if it is detected that tracking or following the safe trajectory is not feasible, the processor adjusts the safe trajectory to satisfy the feasibility constraints and generate a feasible trajectory. In some embodiments, adjusting the safe trajectory includes modifying at least one of the manipulator arm motion, wheeled base velocity, or acceleration to ensure compliance with the feasibility constraints. In some embodiments, the safe trajectory is adjusted in real time, i.e., during the robot's motion. In some embodiments, real-time adjustment of the safe trajectory is performed by prioritizing the maintenance of stability constraints over the robot's precise tracking of the safe trajectory.
[0086] Method 800 also includes step 840 of controlling the actuators of robot 300 to generate internal forces to cause the robot to follow a feasible or safe trajectory for performing the task. In this way, Method 800 facilitates controlling a robot to perform a task in an efficient and fast manner while ensuring the robot's stability while performing the task.
[0087] Referring to Figure 9, a method 900 for imposing stability constraints against external forces is shown. Method 900 includes step 910 of applying a zero moment point (ZMP) constraint to the external force to ensure that the resulting external forces and moments acting on the robot keep the robot's center of mass within a support polygon defined by the contact points between the robot and the surface. Since the ZMP remains within the support polygon, the robot 300 is prevented from tipping over during its motion, i.e., during the combined movement of the wheeled base 302 and the manipulator arm 304. In some embodiments, the stability constraint further characterizes whether or not it is possible to generate external forces through the dynamic interaction between the wheels and the ground. In some embodiments, Method 900 includes step 910 of applying additional stability constraints, e.g., one or more of friction constraints, rotational constraints, and joint angle constraints, to prevent the robot 300 from tipping over.
[0088] Referring to Figure 10, a method 1000 is shown for determining the internal forces required to track / follow a safe trajectory and for adjusting the safe trajectory to a feasible trajectory. Method 1000 includes step 1010 for estimating the actuation force required by the robot's motors / actuators to track / follow a safe trajectory. Once the actuation force is estimated, method 1000 adjusts the trajectory in step 1020 to reduce excessive torque demands while maintaining compliance with stability constraints. The torque to the actuators is reduced so that this torque remains within the torque limits of the corresponding actuators.
[0089] Referring to Figure 11, a method 1100 for determining a safe trajectory using a joint motion model is shown. Method 1100 includes step 1110, which allows determining the external forces acting on the robot while neglecting internal forces, using a center-of-mass momentum model that represents the net linear and angular momentum of the robot's center of mass. Furthermore, in some embodiments, the center-of-mass momentum model incorporates external force estimates from sensor feedback, including ground reaction forces and payload effects, to improve real-time trajectory adjustment.
[0090] Method 1100 also includes step 1120 using kinematic models of the base, the base wheels, and the manipulator arm, the kinematic models providing geometric and velocity constraints for regulating the robot's coordinated motion. In some embodiments, the kinematic model of the wheeled base 302 includes non-holonomic motion constraints derived from the contact dynamics between the wheels and the ground to ensure that the generated safe trajectory adheres to the feasible motion constraints of the wheeled base. Also in some embodiments, the kinematic model of the manipulator arm 304 is used to regulate the position and velocity of the end effector 306 of the robot 300 to ensure the motion of the manipulator arm without violating the stability constraints imposed by the center of mass momentum model.
[0091] Method 1100 further includes step 1130 of integrating a center of mass momentum model and a kinematics model, the center of mass momentum model being used to enforce stability constraints by ensuring that external forces acting on the robot maintain dynamic stability. To do so, in some embodiments, the center of mass momentum model calculates a zero moment point (ZMP) constraint in real time, and the resulting external forces and moments acting on the robot ensure that the center of mass remains within a predetermined support polygon. In some embodiments, the center of mass momentum model and the kinematics model are used together in an optimization-based motion planning process that balances stability constraints imposed by external forces with the robot's motion feasibility constraints. In some embodiments, the method further includes calculating an adaptive trade-off between wheeled base speed and manipulator extension to dynamically optimize stability and task efficiency to achieve speed while maintaining stability.
Claims
1. A method for controlling a robot to perform a task, wherein the robot is a mobile manipulator, the mobile manipulator includes a non-holonomic wheeled base having wheels for moving on a flat surface, and a manipulator arm connected to the wheeled base having additional degrees of freedom for performing the task, the method using a processor coupled with stored instructions to implement the method, the instructions, when executed by the processor, perform a step of the method, the step of the method is The steps of the method include receiving a task in the form of a series of waypoints from a task planner, wherein the mobile manipulator is collision-free at the series of waypoints, and further, The steps of the method include determining a safe trajectory for the coordinated motion of the wheeled base and manipulator arm of the robot to perform the task using a joint motion model of the wheeled base and manipulator arm of the robot, wherein the joint motion model includes external forces acting on the robot as it moves along the trajectory, while ignoring the dynamics directly affected by internal forces generated by the robot's actuators that produce the external forces, and the safe trajectory is determined under stability constraints against the external forces such that the external forces maintain the dynamic stability of the robot, and the steps of the method further include: The robot's actuator determines the internal force required to track the safe trajectory under feasibility constraints on the generation of the internal force by the actuator, and the safe trajectory is adjusted to satisfy the feasibility constraints in order to generate a feasible trajectory. A method comprising controlling the actuators of the robot to generate the internal forces necessary to cause the robot to follow the feasible trajectory.
2. The method according to claim 1, wherein the passing point represents the orientation of the end effector, or the configuration of the manipulator arm and the wheeled base.
3. The method according to claim 1, wherein the stability constraint with respect to the external force is implemented by applying a zero moment point (ZMP) constraint to ensure that the resulting external forces and moments acting on the robot keep the ZMP of the robot within a support polygon defined by the contact points between the robot and the surface.
4. The method according to claim 1, wherein the stability constraints include ZMP constraints, friction constraints, rotational constraints, and joint angle constraints.
5. The method according to claim 1, wherein the joint motion model includes a center-of-mass momentum model representing the dynamics of the net linear and angular momentum of the center of mass of the robot, thereby enabling the determination of external forces arising from motion while neglecting internal forces at the joint level.
6. The method according to claim 1, wherein the joint motion model is influenced by a virtual aggregate wrench that characterizes the interaction between the wheel and the ground.
7. The method according to claim 1, wherein the feasibility constraints for the internal forces include actuator torque limits, speed limits, and power consumption limits, and the real-time adjustment of the trajectory includes modifying at least one of the motion, base velocity, or acceleration of the manipulator to ensure compliance with the feasibility constraints.
8. The method according to claim 1, wherein determining the internal force for tracking the trajectory includes estimating the operating force required by the robot's actuator and adjusting the safe trajectory to reduce excessive torque demands while maintaining compliance with the stability constraints.
9. The method according to claim 1, wherein the trajectory adjustment is performed using optimization-based real-time trajectory correction that prioritizes maintaining stability constraints over precise trajectory tracking.
10. The method according to claim 1, wherein the determination of the trajectory takes into account the dynamic trade-off between the extension of the manipulator and the base velocity, and the system adjusts the position of the manipulator to maintain stability constraints while allowing for increased movement efficiency.
11. The method according to claim 1, wherein the stability constraints are dynamically adapted based on fluctuations in external forces, including changes in payload weight, ground reaction force, or inertial load resulting from acceleration, to ensure the continuous stability of the robot.
12. The method according to claim 1, wherein the joint motion model is implemented as a reduced-order model that simplifies stability evaluation by reducing the complexity of calculations while maintaining safety assurance by summarizing the effects of external forces into a compact representation.
13. Determining the safe trajectory using the aforementioned joint motion model is: Using a center-of-mass momentum model that represents the net linear and angular momentum of the robot's center of mass, it becomes possible to determine the external forces acting on the robot while neglecting internal forces. This includes using kinematic models of the base, the wheels of the base, and the manipulator arm, and integrating the center of gravity momentum model with the kinematic model, wherein the kinematic model provides geometric and velocity constraints for regulating the coordinated motion of the robot, and further, The method according to claim 1, comprising integrating the center of mass momentum model and the kinematic model, wherein the center of mass momentum model is used to enforce stability constraints by ensuring that external forces acting on the robot maintain dynamic stability, and the kinematic model ensures the feasibility of motion by defining allowable speed and positioning constraints.
14. The method according to claim 13, wherein the center of mass momentum model is used to calculate zero moment point (ZMP) constraints in real time, and the resulting external forces and moments acting on the robot ensure that the center of mass remains within a predetermined support polygon.
15. The method according to claim 13, wherein the kinematic model of the wheeled base includes non-holonomic motion constraints derived from the contact dynamics between the wheels and the ground, ensuring that the generated trajectory enforces the feasible motion constraints of the base.
16. The method according to claim 13, wherein the kinematic model of the manipulator arm is used to adjust the position and velocity of the robot's end effector while ensuring the motion of the manipulator without violating the stability constraints imposed by the center of mass momentum model.
17. The method of claim 13, wherein the center of gravity momentum model and the kinematic model are used together in an optimization-based motion planning process that balances the stability constraints imposed by external forces with the motion feasibility constraints of the robot.
18. The method according to claim 13, wherein determining the trajectory further comprises calculating an adaptive trade-off between base velocity and manipulator extension in order to dynamically optimize stability and task efficiency.
19. The method according to claim 13, wherein the center of gravity momentum model incorporates external force estimation from sensor feedback, including ground reaction forces and payload effects, to improve real-time trajectory adjustment.