Mobile robot

Abstract

To provide a mobile robot provided with a control system capable of pursuing an operation purpose other than fall prevention while reducing a calculation load related to a gravity center position.SOLUTION: The whole-body motion generation unit generates a joint command, which is a motion command for a motor driver that drives each joint of the manipulator, by optimal control that minimizes a value of an objective function including a term for realizing a center-of-gravity state of the manipulator corresponding to the adjustment model state, the optimal control including, as a term of the objective function or a constraint condition of optimization, a matter related to a motion purpose other than fall prevention due to the fact that the mobile robot includes a manipulator having a degree of freedom larger than that of the coarse-grained model or the carriage has a specific mechanism form, A wheel command which is an operation command to a motor driver for driving wheels is generated.SELECTED DRAWING: Figure 3

Description

[Technical Field]

[0001] The present disclosure relates to mobile robots. [Background technology]

[0002] Mobile robots are used in factories and other places. Some mobile robots are equipped with a manipulator equipped with an arm (multiple links) with multiple joints, and a cart on which the manipulator is mounted, and the manipulator can be moved by driving the cart. Such mobile robots are required to be able to move quickly to their target location while preventing the manipulator from tipping over.

[0003] Known examples of mobile robots equipped with an arm or manipulator and a cart include those described in Non-Patent Documents 1 and 2. Non-Patent Documents 1 and 2 disclose a mobile robot that can prevent the mobile robot from tipping over while running by controlling the motion of five-degree-of-freedom inverted pendulum hardware to which a two-degree-of-freedom inverted pendulum model is applied and the translational speed of a mobile platform (hereinafter referred to as the cart). [Prior art documents] [Patent documents]

[0004] [Non-Patent Document 1] D. Choi, M. Kim, and J. Oh, “Development of a rapid mobile robot with a multi-degree-of-freedom inverted pendulum using the model-based zero-moment point stabilization method,” Advanced Robotics, Vol. 26, No. 5-6, pp. 515-535, 2012. [Non-patent document 2] D. Choi, and J. Oh, “ZMP stabilization of Rapid Mobile Manipulator”, in 2012 IEEE International Conference on Robotics and Automation, St Paul, MN, USA: IEEE, May 2012, pp. 883-888. doi: 10.1109 / ICRA.2012.6225369. Summary of the Invention [Problem to be solved by the invention]

[0005] In the mobile robots in Non-Patent Documents 1 and 2, the motion of each joint of the 5-DOF inverted pendulum hardware is controlled by the command (X pen , Y pen ) is determined by inverse kinematics, so the calculation load for the center of gravity position is small, but there is a problem that the purpose of the movement of each joint is limited to realizing the target center of gravity position.

[0006] In view of the above-mentioned problems, the present disclosure aims to provide a mobile robot equipped with a control method that can pursue operational objectives other than preventing tipping over while reducing the calculation load related to the center of gravity position. [Means for solving the problem]

[0007] A mobile robot according to the present disclosure includes a wheeled cart, a manipulator with an arm having multiple joints, and a robot control unit. The robot control unit acquires a target cart state, which is a target state of the cart for causing the mobile robot to travel according to a travel plan, and uses a coarse-grained model including one or more mass points whose total mass is the mass of the manipulator and virtual mechanisms for moving the mass points with degrees of freedom that are fewer than the number of joints of the arm, to determine an adjusted model state, which is a state of the coarse-grained model, and an adjusted cart state, which is a state of the cart, so as to bring the state of the cart closer to the target cart state while avoiding tipping over of the mobile robot. The robot control unit also includes a coarse-grained model motion planning unit that calculates an adjusted model state, which is a state of the coarse-grained model, and an adjusted cart state, which is a state of the cart, so as to bring the state of the cart closer to the target cart state while avoiding tipping over of the mobile robot. the mobile robot is equipped with a whole-body motion generation unit that generates joint commands, which are motion commands for motor drivers that drive each joint of the manipulator, and wheel commands, which are motion commands for motor drivers that drive the wheels, by using optimal control that minimizes the value of an objective function that includes a term for realizing a center-of-gravity state of the manipulator that is determined by the coarse-grained model, and that includes, as terms of the objective function or optimization constraints, matters related to motion objectives other than preventing tipping over that are due to the mobile robot having a manipulator with more degrees of freedom than the coarse-grained model or the cart having a specific mechanical type; and an execution control unit that operates the mobile robot by periodically executing the coarse-grained model motion planning unit and the whole-body motion generation unit. [Effects of the Invention]

[0008] The mobile robot according to the present disclosure can be provided with a control method that reduces the computational load related to the center of gravity position while enabling the mobile robot to pursue operational objectives other than preventing tipping over. [Brief explanation of the drawings]

[0009] [Figure 1] 1 is a schematic diagram illustrating the general configuration of a mobile robot 1 according to a first embodiment. [Figure 2]FIG. 2 is a block diagram illustrating the hardware configuration of a control unit 30 that controls the operation of the mobile robot 1. [Figure 3] FIG. 2 is a functional block diagram showing an example of the configuration of a control unit 30. [Figure 4] FIG. 10 is a schematic diagram illustrating a coarse-grained model motion planning unit 34. [Figure 5] Schematic diagram illustrating the movement of the zero moment point (ZMP). [Figure 6] 10 is a flowchart illustrating a procedure for calculating an optimization problem in a coarse-grained model motion planning unit 34. [Figure 7] FIG. 10 is a block diagram illustrating the configuration of an action generation unit 31 according to a second embodiment. [Figure 8] FIG. 10 is a block diagram illustrating the configuration of an action generation unit 31 according to a third embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0010] Hereinafter, the present embodiment will be described with reference to the accompanying drawings. In the accompanying drawings, functionally identical elements may be denoted by the same numerals. Note that the accompanying drawings illustrate embodiments and implementation examples according to the principles of the present disclosure, but these are intended to aid in understanding the present disclosure and are not intended to limit the present disclosure in any way. The descriptions in this specification are merely typical examples and are not intended to limit the scope or application of the present disclosure in any way. In each drawing, the same or substantially equivalent elements, components, and parts are denoted by the same reference numerals. Furthermore, the dimensions and proportions of the drawings have been exaggerated for illustrative purposes and may differ from the actual proportions.

[0011] Although the present embodiment has been described in sufficient detail to enable those skilled in the art to implement the present disclosure, it should be understood that other implementations and forms are possible, and that changes in configuration and structure and substitutions of various elements are possible without departing from the scope and spirit of the technical ideas of the present disclosure. Therefore, the following description should not be interpreted as being limited thereto.

[0012] [Terminology explanation] Before describing the embodiments, the meanings of various terms used in the embodiments will be explained below. (1) A "manipulator" may be a device that has multiple links connected by multiple joints and is equipped with an end effector, camera, etc. at the tip of each link. (2) A "travel plan" is a plan for a route, speed, etc., created for a future time range required for the purpose of travel. A "travel plan" may be created before the start of a trip, or it may be a plan created and modified dynamically while traveling. A "travel plan" may be a plan created outside the mobile robot and given to the mobile robot, or it may be a plan created by the mobile robot itself.

[0013] (3) The "motor driver" may be a control system that drives a motor so that the error relative to a target value given by an input command is small, for example. Specific control methods in this control system may be torque control, position control, speed control, or a combination of these. The "whole-body movement generator" can create commands that include values ​​of a type that is compatible with the control method of the motor driver. (4) The "coarse-grained model motion planning unit" may determine the "adjusted model state" and the "adjusted cart state" using the MPC (Model Predictive Control) method, or may determine them using methods such as the LQR (Linear Quadratic Regulator) method.

[0014] (5) In the case of a single mass point model, the "state of the center of gravity of the manipulator corresponding to the adjusted model state" may be not only the state where the state of the mass point (e.g., the position and velocity of the mass point) coincides with the state of the center of gravity of the manipulator (e.g., the position and velocity of the center of gravity), but also the state of the converted object corresponding to the original state when, for example, the spherical surface on which the mass point of an inverted pendulum model moves is converted into a plane of constant height on which the center of gravity of the manipulator moves (approximated by a plane).

[0015] (6) In the mathematical formulas (images) included in the explanation below, bold notations represent matrices or vectors. In order to distinguish these matrices or vectors from scalar quantities, in the main text these are sometimes written with "→" instead of bold notation. Also, the n dots indicating n-th derivatives are written above the letters in the mathematical formulas (images), but to the left in the main text. In addition, two-level subscripts in the mathematical formulas (images) may be written in parallel in parentheses in the main text (e.g., (cmd, Cog)).

[0016] [First embodiment] First, a mobile robot 1 according to a first embodiment will be described with reference to the schematic diagram in FIG. 1. The mobile robot 1 includes a cart 10, a manipulator 20, a control unit 30, and an imaging unit 40. The cart 10 has wheels W (WA, WB) that allow it to move translationally or rotate, and the manipulator 20 is mounted on top of it. As will be described later, the manipulator 20 includes multiple joints and multiple links. The control unit 30 controls the cart 10 and the manipulator 20 according to a travel plan.

[0017] In the example shown in Fig. 1, the carriage 10 has a substantially rectangular parallelepiped shape, but is not limited to this. In the following, an X-axis, a Y-axis, and a Z-axis are set as a coordinate system for the carriage 10. The X-axis direction is the front-rear direction of the carriage 10, the Y-axis is the left-right direction of the carriage 10, and the Z-axis is the height direction of the carriage 10. This left-right direction is also the width direction of the carriage 10. In the following, an example will be described in which the height direction of the carriage 10 coincides with the vertical direction.

[0018] The dolly 10 is equipped with a movement mechanism 11. The movement mechanism 11 has two drive wheels WA, two driven wheels WB, and two rotational drive sources (motors, not shown) that independently drive the two drive wheels WA. The drive wheels WA and driven wheels WB are sometimes collectively referred to as "wheels W." The drive wheels WA and driven wheels WB are in contact with the floor surface LF, thereby supporting the mobile robot 1 and moving the dolly 10. A rectangle connecting the four contact points of the drive wheels WA and driven wheels WB constitutes a support area SA. Note that the dolly 10 may have three or more wheels W so that the manipulator 20 can be stably placed thereon, and the area connecting the contact points of the three or more wheels W may constitute the support area.

[0019] The drive wheels WA are rotationally driven by a rotary drive source (not shown). For example, by rotating the two drive wheels WA at a constant rotation speed, the bogie 10 can be made to move linearly (translationally). Furthermore, by making the rotation speeds of the two drive wheels WA different between the left and right (differential), the bogie 10 can be made to turn. The driven wheels WB are not rotationally driven by a rotary drive source, but are made rotatable by the frictional force with the floor surface LF due to the drive of the drive wheels WA. Furthermore, the rotation axis of the driven wheels WB can freely change direction within a horizontal plane. In other words, the driven wheels WB are swivel casters (swivel casters).

[0020] The manipulator 20 also includes a plurality of joints J, a plurality of links L, and an end effector H. The link L is the portion between two adjacent joints J. Each of the plurality of joints J is configured to be rotatable about a rotation axis AX (in FIG. 1, the symbol AX is shown for only some of the joints J). This allows the manipulator 20 to move the end effector H to any position by rotating the joints J. The end effector H is a device configured to be able to hold an object, and may be, for example, a robot hand or a suction pad. The imaging unit 40 is mounted on the manipulator 20 and captures images of the surroundings of the manipulator 20.

[0021] The hardware configuration of the control unit 30 that controls the operation of the mobile robot 1 will be described with reference to the block diagram in Figure 2. The control unit 30 has a computer 100. The computer 100 has a processor 102, a memory 104, a storage 106, an input device 108, an output device 110, a storage medium reader 112, and a communication I / F (Interface) 114. These elements are connected to each other via a bus 116 so that they can communicate with each other.

[0022] The storage 106 stores a control program 118 for controlling the mobile robot 1. The processor 102 can execute various programs and control each element. Specifically, the processor 102 reads the program from the storage 106 and executes the program using the memory 104 as a work area. That is, the processor 102 controls each element and performs various arithmetic operations in accordance with the program stored in the storage 106.

[0023] The memory 104 can temporarily store programs and various data as a working area.

[0024] The storage 106 is, for example, a read-only memory (ROM), a hard disk drive (HDD), or a solid state drive (SSD), and stores various programs and data. These programs include not only application programs such as the control programs described above, but also an operating system.

[0025] The input device 108 is a device for performing various inputs to the computer 100. The input device 108 includes operation switches, operation buttons, etc., and may also include pointing devices such as a keyboard and a mouse used in a personal computer, etc.

[0026] The output device 110 is a device for outputting various types of information from the computer 100, and includes, for example, a display, an indicator lamp, a speaker, etc. A touch panel display can also be used as the output device 110, in which case the touch panel display also functions as the input device 108.

[0027] The input device 108 and the output device 110 may be detachable when operating the mobile robot 1. Instead of providing the input device 108 and the output device 110, operation commands and other information for the mobile robot 1 may be input and operation status and other information for the mobile robot 1 may be output via wireless communication via the communication I / F 114.

[0028] The storage medium reader 112 is a device that reads data stored in various storage media and writes data to the storage media. Examples of storage media include CD (Compact Disc)-ROM, DVD (Digital Versatile Disc)-ROM, Blu-ray Disc, and USB (Universal Serial Bus) memory.

[0029] The communication I / F 114 is an interface for communicating with other devices, and is compliant with standards such as Ethernet (registered trademark) and FDDI (Fiber Distributed Data Interface).

[0030] The above-described joints J, links L, end effector H, and drive wheels WA are controlled by a motor driver MD via the control unit 30. Specifically, for example, each joint J rotates about its own rotation axis AX as a result of a drive source (not shown) being controlled by the motor driver MD via the control unit 30. Similarly, a drive source (not shown) of the end effector H is controlled by the motor driver MD via the control unit 30 to perform predetermined gripping, suction, manipulation, movement, processing, etc. on an object. The drive wheels WA rotate as a rotational drive source (not shown) is controlled by the control unit 30.

[0031] An example of the configuration of the control unit 30 will be described with reference to the functional block diagram in Figure 3. The control unit 30 of the mobile robot 1 in this embodiment is generally composed of a motion generation unit 31, an execution control unit 32, and a target vehicle state generation unit 33, all of which are implemented within the computer 100 by the control program 118.

[0032] The target vehicle state generation unit 33 generates a target state (target translational velocity v ref , angular velocity ω ref The target vehicle state information indicates a target state of the vehicle 10 of the mobile robot 1 for driving the vehicle 10 according to a driving plan. The motion generation unit 31 receives this target vehicle state information and outputs command signals (joint command signals, wheel command signals) for controlling the vehicle 10 and the manipulator 20.

[0033] The motor driver MD receives this command signal, generates drive signals, and drives various rotational drive units (not shown). The motor driver MD may also have a function to provide feedback signals indicating the current state of the various rotational drive units to the motion generator 31. The execution control unit 32 periodically operates the motion generator 31 to move the mobile robot 1.

[0034] The motion generation unit 31 may be composed of, for example, a coarse-grained model motion planning unit 34 and a whole-body motion generation unit 35. The coarse-grained model motion planning unit 34 is a part that replaces the motion of the manipulator 20 with a coarse-grained model (e.g., an inverted pendulum model) and generates a motion plan for the coarse-grained model. The coarse-grained model is a virtual mechanism that moves one or more mass points whose total mass is the mass of the manipulator 20 and the mass points with degrees of freedom that are fewer than the number of joints in the arm. Specifically, the coarse-grained model motion planning unit 34 generates, as a motion plan, commands to minimize the value of the objective function of the optimization problem that is determined according to the set objective function and constraint conditions, in accordance with the target vehicle state information received from the target vehicle state generation unit 33, and outputs the commands to the whole-body motion generation unit 35. Here, the commands output by the coarse-grained model motion planning unit 34 are adjusted model state commands (e.g., adjusted pendulum angle q ) that command adjusted state quantities of the coarse-grained model. pitch_c , q roll_c , adjusted pendulum angular velocity q pitch_c ,·q roll_c ), and an adjusted carriage state command (e.g., adjusted translational velocity v c , adjusted angular velocity ω c ) By appropriately changing the adjusted model state command and the adjusted carriage state command, the state of the carriage 10 can be brought closer to the target carriage state while preventing the mobile robot 1 from tipping over. The coarse-grained model is a virtual mechanism in which the total mass of multiple mass points is set equal to the mass of the manipulator 20, and the coarse-grained model has fewer links than the manipulator 20, allowing the manipulator 20 to move with fewer degrees of freedom.

[0035] The adjusted state of the coarse-grained model (adjusted model state) and the adjusted state of the carriage 10 (adjusted carriage state) can be the state of the coarse-grained model and the state of the carriage 10 that allow the position of the zero moment point of the mobile robot 1 to move within a predetermined range set within the support area of ​​the mobile robot 1 and minimize the value of an objective function that includes a term for the error amount between the target state and the predicted state for the mobile robot 1 over a certain time range in the future.

[0036] The whole-body motion generation unit 35 is a part that generates and outputs various commands to be supplied to the motor driver MD in accordance with the commands supplied from the coarse-grained model motion planning unit 34. The whole-body motion generation unit 35 can include, for example, a center-of-gravity command generation unit 36 ​​and a whole-body command generation unit 37.

[0037] The center of gravity command generating unit 36 ​​outputs a manipulator center of gravity command, which is an operation command related to the center of gravity of the manipulator 20, in accordance with the adjusted model state command. The manipulator center of gravity command may include, for example, a manipulator center of gravity position command →p(cmd, CoG) related to the position of the center of gravity of the manipulator 20, and a manipulator center of gravity velocity command →p·(cmd, CoG) related to the velocity of the center of gravity.

[0038] The whole body command generating unit 37 outputs a joint command signal and a wheel command signal for controlling the motor driver MD based on the manipulator center of gravity command and the adjustment carriage state command.

[0039] The coarse-grained model motion planning unit 34 will be described in further detail with reference to Fig. 4. As an example, the coarse-grained model motion planning unit 34 uses an inverted pendulum model as a coarse-grained model to coarse-grain the manipulator 20. In the inverted pendulum model, roll and pitch rotate independently.

[0040] In the inverted pendulum model, when the state variable → z and input → u are defined as shown in the following [Equation 1], the state space expression of this model can be obtained as shown in the following [Equation 2] by linearizing the nonlinear equation of motion around the equilibrium point, where the pendulum is standing straight up and at rest, using a first-order Taylor expansion.

[0041]

number

number

[0042] Here, it is assumed that the torque of the wheels W of the bogie 10 is transmitted without loss to the torque of the rotation and translation of the body of the bogie 10, and that the effect of the change in attitude of the pendulum around the pitch axis on the change in inertia around the yaw of the body is small and can be ignored. In the equations that follow, this system is discretized with an appropriate sampling time, and the system matrices →Ad and →Bd of the discrete state equations are also used. The discrete state equations are also used as equality constraints for dynamics in the optimization problem described below.

[0043] Next, we model the zero moment point (ZMP) in the X-axis direction of the robot coordinate system. The ZMP can be calculated, for example, using the method described below. As shown in Figure 5, gravity F1 and inertial force F2 act on the center of gravity GP1 of the entire mobile robot 1. Inertial force F2 is the resultant force of the inertial force associated with the mobile robot 1's forward / backward acceleration, the centrifugal force associated with its turning, and the inertial force associated with the movement of link L due to the drive of multiple joints J. The point where the direction of the resultant force F3 of gravity F1 and inertial force F2 intersects with the floor LF is called the zero moment point (ZMP). If the inertial force F2 becomes large enough that the ZMP reaches the edge of the support area SA, one of the mobile robot 1's wheels will leave the floor LF and the mobile robot 1 will begin to tip over.

[0044] Specifically, we will explain modeling of the ZMP in the X-axis direction. The method for calculating the ZMP will be described later. Assuming that the amount of change in the center of gravity of the manipulator in the Z-axis direction is small and negligible, and linearizing the amount of movement of the center of gravity with respect to the pendulum angle near the equilibrium point, the amount of change X(d, ZMP) in the X-axis direction of the ZMP from an arbitrary origin in the robot can be expressed as follows using the above-mentioned state quantities:

[0045]

number

[0046] In addition, C Z1 , C Z2 , C Z3 is a scalar quantity determined by geometric characteristics. Z1 , C Z2 , CZ3 may be determined by identification or calculated using design values. Furthermore, by using [Equation 2], it can be transformed into the following equations for the state quantity and input quantity:

[0047]

number

[0048] Next, we will explain modeling of the ZMP in the Y-axis direction of the robot coordinate system. The difference from modeling in the X-axis direction is that a differential two-wheel robot such as that shown in Figure 1 cannot generate acceleration in the Y-axis direction. However, when translating and turning, a force is applied in a direction away from the turning center due to centrifugal force, so it is necessary to incorporate this into the ZMP model. If we approximate the manipulator 20 as moving in a uniform circular motion around a certain turning center point for a short period of time, the centrifugal acceleration a at this time is L can be expressed by the following formula:

[0049]

number

[0050] Furthermore, in this method, the centrifugal acceleration a L The rotation speed ω (actual angular velocity or predicted angular velocity predicted as the current value) and the target translation speed v ref can be approximated as follows:

[0051]

number

[0052] Based on these, in the same way as for the X axis, the amount of change Y(d, ZMP) in the Y axis direction of the ZMP from an arbitrary origin can be obtained as follows:

[0053]

number

[0054] In addition, C Z4 , C Z5 , C Z6 is a scalar quantity determined by geometric characteristics. Z4 , C Z5 , C Z6 can be determined by identification or calculated using design values. As in the case of the X-axis direction, [Equation 7] can be transformed into the following equations for state quantities and input quantities: v ref Since is time-varying, d b also changes with time.

[0055]

number

[0056] Next, we will explain the formulation of model predictive control in the coarse-grained model motion planning unit 34. The command values ​​for the state quantities are defined as follows:

[0057]

number

[0058] where ω ref , v ref is a command value sent from the target vehicle state generation unit 33, which is a higher-level controller, and k is a discrete time.

[0059] Here, →z and →z ref When the difference →e[k] is expressed as in the following [Equation 10], the input quantity →u that minimizes the command tracking error within a finite time while keeping the norm of the input quantity small can be expressed by the objective function in the following [Equation 11].

[0060]

number

number

[0061] Np and Nc are the prediction horizon step and the control horizon step, respectively (in this example, Np=Nc).

[0062] The constraints on the dynamics of the coarse-grained model are expressed as the following equation of state, which is a discretization of [Mathematical formula 2]:

[0063]

number

[0064] The constraints for preventing the mobile robot 1 from tipping over can be expressed as follows by using the ZMP model of [Equation 4] and [Equation 8] and setting appropriate constraint ranges.

[0065]

number

[0066] where K(x, zb) and K(y, zb) are adjustment parameters introduced to mitigate modeling errors. The above explanation can be summarized into one optimization problem as shown in [Equation 14] below.

[0067]

number

[0068] The ZMP position is allowed to move within a predetermined range. This can be achieved by using an inequality constraint such as the one in Equation 13, or by incorporating an error term for the ZMP position into the objective function. When using the error term in the objective function, the ZMP position can be allowed to move within the predetermined range by appropriately setting the parameters of the objective function through experimental techniques, given the specifications of the mobile robot 1 and limitations on running acceleration.

[0069] Using the input amount →u obtained so far and [Equation 2], the translational velocity at the next time (adjusted translational velocity v c ), turning speed (adjusted angular speed ω c ), virtual pendulum angle (adjusted pendulum angle q pitch_c ), virtual pendulum angular velocity (adjusted pendulum angular velocity q pitch ,·q roll_c ) is generated. Furthermore, under the assumption that the position of the center of gravity of the manipulator 20 in the Z direction does not change, the pendulum angle and angular velocity are converted into the position of the center of gravity (center of gravity position command → p(cmd, CoG)) and velocity (center of gravity velocity command → p(cmd, CoG)), respectively. These are output to the whole-body command generation unit 37 at the lower stage.

[0070] Here, we will explain how to calculate the ZMP. The position of the ZMP approximated by a two-mass system can be expressed as follows:

[0071]

number

[0072] where M is the mass of the cart 10, m is the mass of the inverted pendulum, g is the acceleration due to gravity, x is the X-axis component of the center of gravity of the cart 10 in the global coordinate system, l is the length from the base of the inverted pendulum to the center of gravity of the inverted pendulum, and x j1 , z0, z1 are the X-axis component of the base position of the virtual pendulum, the height of the center of gravity of the cart 10, and the height of the center of gravity of the virtual pendulum, respectively. wh is the moment of inertia around the wheel axle. Here, the X-axis component of the target ZMP position is X ZMP0 can be set, for example, as follows:

[0073]

number

[0074] The error from this target ZMP position is defined as follows:

[0075]

number

[0076] Assuming that the change in the center of gravity of the arm in the Z-axis direction is small, sinq_ pitch ≒q_ pitch If , and z1 are constant values, the change in ZMP from an arbitrary origin in the mobile robot 1, X(d, ZMP), can be expressed as follows using the above state quantities. However, C Z1 , C Z2 , C Z3 is a constant value.

[0077]

number

[0078] Next, the operation of the whole-body command generation unit 37 will be described. The whole-body command generation unit 37 generates command values ​​by formulating an objective function for calculating command values ​​to the motor drivers MD that drive the joints J and wheels W of the manipulator 20 as an optimization problem and calculating a solution using a linear solver. Specifically, an objective function for making the center-of-gravity velocity and hand velocity of the manipulator 20 follow desired values ​​and making the velocities of the wheels W and the cart 10 follow desired values ​​is formulated as an optimization problem.

[0079] The design of the objective function and the design of the constraints will now be described. In this example, objective functions are designed to cause the velocity of the cart 10, the velocity of the center of gravity of the manipulator 20, and the velocity of the end effector H of the manipulator 20 to follow desired values. As will be described later, the whole-body motion generation unit 35 of this embodiment performs optimal control to minimize the value of an objective function that includes a term for realizing a center of gravity state of the manipulator 20 corresponding to the adjusted model state. The whole-body motion generation unit 35 generates joint commands, which are motion commands for driving each joint J of the manipulator 20, and wheel commands, which are motion commands for driving the wheels W, through optimal control that includes, as objective function terms or optimization constraints, matters related to motion objectives other than preventing tipping over, which are due to the fact that the mobile robot 1 is equipped with a manipulator 20 with more degrees of freedom than the coarse-grained model or that the cart 10 has a specific mechanical form.

[0080] First, the adjusted translational velocity v output from the coarse-grained model motion planning unit 34 c , adjusted angular velocity ω c We will now explain the design of the objective function that optimizes the angular velocity of the wheel W so that it follows the adjusted translational velocity v c , adjusted angular velocity ω c is defined as follows [Equation 19], and the nonholonomic constraints of the differential two-wheeled mobile robot are used to determine the axial speeds of the left and right wheels, including → θ, and the adjusted translational speed v of the cart 10. c , adjusted angular velocity ω c The relationship can be expressed by the following equation:

[0081]

number

number

[0082] However, →A ξ , →A θ is a constraint condition coefficient matrix that is uniquely determined from the geometric structure of the bogie 10. More specifically, →A ξis a coefficient related to the attitude of the bogie 10 and the distance between the two wheels, and →A_ ·θ is a coefficient related to the wheel radius. →·θ includes the rotational speed of each wheel and the rotational speed of each joint, and here the significant information for control is the rotational speed of each wheel.

[0083] Next, we will explain the design of an objective function for optimizing the speed of the joints J of the manipulator 20 in order to make the center of gravity position of the manipulator 20 follow a desired position. Using the manipulator center of gravity position command →p(cmd, CoG) and center of gravity velocity command →·p(cmd, CoG) for avoiding tipping, which are output from the center of gravity command generation unit 36, a reference value →·p(ref, CoG) for the center of gravity velocity is created as follows:

[0084]

number

[0085] Here, K(CoG, p) is the gain related to the position error. This reference value →·p(ref, CoG) and the Jacobian of the center of gravity of the robot →J CoG Using this, the tracking error of the center of gravity can be expressed as follows: Here, the significant information for controlling each axis velocity →·θ is the angular velocity of each joint J.

[0086]

number

[0087] Next, we will explain the design of the objective function that optimizes the velocity of each axis of the manipulator 20 so that it follows the command of the end-of-hand position (tool center point TCP) of the given manipulator 20. When there is a desired command p(ref, TCP) for the end-of-hand velocity, as in the case of the center of gravity position, the Jacobian →J TCP Using this, the error can be defined as follows:

[0088]

number

[0089] If you want to minimize the movement of the hand, you can set a command that will make the velocity zero in the robot coordinate system.

[0090] In this embodiment, the maximum and minimum values ​​of the speed of each axis of the manipulator 20, the maximum and minimum values ​​of the center of gravity speed, the maximum and minimum values ​​of the hand speed, and self-interference avoidance are introduced as constraints. For the first three of the four constraints, appropriate thresholds are set for the hardware specifications, and inequality constraints are set as follows, sandwiching the corresponding variables:

[0091]

number

[0092] In this case, by introducing a speed damper that reduces the upper speed limit as the deceleration margin area up to the position limit is entered, and dynamically changing the threshold using the current position, it is possible to take into account constraints on the position dimension.For the fourth, self-collision avoidance, each link is approximated by a capsule shape, and the closest distance vself(→·θ) between capsules excluding adjacent capsules is constrained so that it does not exceed a certain threshold dclosest.

[0093] The above can be summarized as one optimization problem as follows: [Equation 25] This optimization problem is solved in the whole body command generator 37 to generate joint velocity commands and wheel velocity commands.

[0094]

number

[0095] Minimizing the value of the second term in [Equation 25] reduces the error in the center of gravity state of the manipulator 20 from the center of gravity command generated in response to the adjusted model state command, thereby preventing the mobile robot 1 from tipping over. By minimizing the other terms, optimal control can be implemented, assuming the goal of preventing the mobile robot 1 from tipping over. In situations where the manipulator 20 has more degrees of freedom than the coarse-grained model or the cart 10 has a specific mechanical structure, other operational objectives besides preventing the mobile robot 1 from tipping over can be included as terms in the objective function or as optimization constraints. The rotational speed of each wheel W of the cart may be calculated directly using the relationship in [Equation 20], and the objective function in [Equation 25] may include only the second and third terms, which relate to the speed of the joint J of the manipulator 20. Furthermore, if stabilization of the position of the tool center point TCP is not required, the third term is unnecessary.

[0096] If the motor driver MD is designed to input a velocity command, the whole-body command generating unit 37 can output the calculated velocity commands for each axis of the manipulator 20 and the velocity commands for the wheels W to the motor driver MD as joint commands and wheel commands.

[0097] When the motor driver MD is configured to input a position command, the whole-body command generating unit 37 can integrate the calculated velocity commands for each axis of the manipulator 20 and the velocity commands for the wheels W, convert them into angle commands, and output them to the motor driver MD as joint commands and wheel commands.

[0098] If the motor driver MD is designed to input a torque command, the whole-body command generation unit 37 converts the calculated velocity command for each axis into a torque command using a calculated torque method (dynamics model) and passes it to the motor driver MD as a joint command and a wheel command. The rotation drive unit (motor) driven by the motor driver MD can be equipped with a current loop control controller, which can be tuned separately.

[0099] The procedure for calculating the optimization problem in the coarse-grained model motion planning unit 34 will be described with reference to the flowchart in Figure 6. First, in step S11, the state quantity z of the mobile robot 1 is calculated based on information from sensors (not shown) and other sources. The current state quantity may be obtained as an estimate by a state observer or other device.

[0100] Next, in step S12, a feasible solution is set. A feasible solution must satisfy the equality and inequality constraints. When k=0, the current state quantity is used as the initial feasible solution, and when k=1 or later, the optimal solution obtained in the previous step (step k-1) is used as the feasible solution.

[0101] Next, in step S13, the value of the objective function ([Equation 11]) is obtained based on the set feasible solution, and a command value sequence is obtained that makes the value of the objective function smaller than the previous value. The feasible solution can be updated by calculation using an optimization algorithm such as Newton's method, quasi-Newton's method, or sequential quadratic programming, and a command value sequence is obtained that makes the value of the objective function smaller than the previous value. It is then determined whether the obtained command value sequence satisfies a termination condition for the optimal solution search (step S14). Examples of termination conditions for the optimal solution search include (1) the value of the objective function being equal to or less than a predetermined value, (2) the magnitude of change in the value of the objective function from the previous time being equal to or less than a predetermined value, and (3) the number of loop processes reaching a predetermined number.

[0102] If the determination in step S14 is Yes, the process proceeds to step S15, and the command value series at that time is sent to the subsequent whole-body movement generation unit 35. If the determination is No, step S13 is repeated until a Yes determination is made. The above process is repeated in steps k, k+1, k+2 (step S16), and so on, until the final control objective is achieved.

[0103] As described above, with the mobile robot 1 of the first embodiment, the coarse-grained model motion planning unit 34 outputs the adjusted model state command and the adjusted cart state command, which are the states of the coarse-grained model. The whole-body motion generation unit 35 then performs optimal control to minimize the value of the objective function so as to realize the center-of-gravity state of the manipulator 20 corresponding to the adjusted model state command, while also including matters related to motion objectives other than preventing tipping as terms of the objective function or optimization constraints. This makes it possible to pursue motion objectives other than preventing tipping while reducing the computational load related to the center-of-gravity position.

[0104] [Second embodiment] Next, a mobile robot 1 according to a second embodiment will be described with reference to FIG. 7. The basic configuration of this mobile robot 1 is the same as that of the first embodiment, so a duplicated description will be omitted. In this second embodiment, a coarse-grained model is used, which is a model of two mass points M A , M L In this embodiment, a two-mass model is adopted to represent the manipulator 20 as follows. This is different from the first embodiment.

[0105] The coarse-grained model motion planning unit 34 plans the motion of the two-mass model so that the ZMP position can move within a predetermined range set within the support area SA of the mobile robot 1, and the whole-body motion generation unit 35 determines the states of all joints from the state of the two-mass model. For example, for the mobile robot 1 with the structure shown in Figure 7, the link group A below joint J3 is assigned to mass point M. A The link group B on the end effector H side of the joint J3 is represented by the mass point M B Consider the two mass model expressed as: A , M B is set so that the total mass is equal to the mass of the manipulator 20. According to this two-mass model, it is possible to express the change in height of the center of gravity position accompanying the change in the posture of the manipulator 20.

[0106] In the coarse-grained model motion planning unit 35, the objective function is BBy adding a term that represents the error between the target value and the height of the mass M B The whole body motion generator 35 can make the height of the mass point M follow the target height. A , mass point M B The center of gravity Jacobian →J(CoG,A) that projects the reference values ​​→·p(ref,A), →·p(ref,B) of the center of gravity velocity corresponding to each of the above, and the velocity θ of all joints J onto the center of gravity velocity →·p(CoG,A) of link group A, and the center of gravity Jacobian →J(CoG,B) that projects the center of gravity velocity p(CoG,B) of link group B, are used to calculate the →J of [Equation 25]. C0G By dividing the θ term into two terms, it is possible to solve the optimization problem of the whole body motion generator 35. Coarse-grained models with three or more mass points can also be used in the same way.

[0107] [Third embodiment] Next, a mobile robot 1 according to a third embodiment will be described with reference to Figure 8. The basic configuration of this mobile robot 1 is the same as that of the first embodiment, so a duplicated description will be omitted. However, the third embodiment differs from the first embodiment in the configuration of the whole-body motion generator 35.

[0108] The configuration of the motion generation unit 31 of this third embodiment will be described with reference to Fig. 8. In this motion generation unit 31, a coarse-grained model motion planning unit 34 generates a single mass point model as a coarse-grained model, and uses this single mass point model to generate a command to be output to the motor driver MD without using a center of gravity command.

[0109] Specifically, commands related to the position and velocity of the center of gravity as in the first embodiment are not generated from the state of the single mass point model generated by the coarse-grained model motion planning unit 34, but rather commands related to the state of the joints of the manipulator 20 are directly calculated. As an example, the coarse-grained model motion planning unit 34 calculates a virtual pendulum torque τ pitch Furthermore, by using the pendulum length l, the force →f to be applied to the center of gravity can be determined as follows. Simply considering only the X direction, it can be expressed as follows:

[0110]

number

[0111] Using this force →f, the whole-body motion generation unit 35 can generate commands to the motor driver MD by solving an optimization problem that minimizes the torque of all axes under equality constraints that show the relationship between the force →f and the torque →τ of all axes of the single mass model, for example, as shown in the following [Equation 27].

[0112]

number

[0113] Here, →Jτ,com is the Jacobian of the force acting on the center of gravity relative to the joint torque, and is calculated using the robot dynamics model and the center of gravity Jacobian. Furthermore, it is possible to add a term related to the speed tracking of the cart to the objective function, or to add constraints determined by the cart mechanism, constraints related to the motor specifications, etc.

[0114] The present disclosure is not limited to the above-described embodiments and includes various modifications. For example, the above-described embodiments have been described in detail to clearly explain the present disclosure, and are not necessarily limited to those including all of the described configurations. Furthermore, it is possible to replace part of the configuration of one embodiment with the configuration of another embodiment, or to add the configuration of another embodiment to the configuration of one embodiment. Furthermore, it is possible to add, delete, or replace part of the configuration of each embodiment with other configurations. [Explanation of symbols]

[0115] 1...mobile robot, 10...cart, 11...movement mechanism, 20...manipulator, 30...control unit, WA...drive wheel, WB...driven wheel, J...joint, L...link, H...end effector, AX...rotation axis, 31...motion generation unit, 32...execution control unit, 33...target cart state generation unit, 34...coarse-grained model motion planning unit, 35...whole body motion generation unit, 36...center of gravity command generation unit, 37...whole body command generation unit, 40...imaging unit, 100...computer, 102...processor, 104...memory, 106...storage, 108...input device, 110...output device, 112...storage medium reading device, 116...bus, 118...control program, MD...motor driver, SA...support area.

Claims

1. A mobile robot comprising a wheeled carriage, a manipulator with an arm having multiple joints, and a robot controller, The robot control unit a coarse-grained model motion planning unit that obtains a target cart state, which is a target state of the cart for causing the mobile robot to travel in accordance with a travel plan, and uses a coarse-grained model that includes one or more mass points whose total mass is the mass of the manipulator and virtual mechanisms that move the mass points with degrees of freedom that are fewer than the number of joints of the arm, to determine an adjusted model state, which is a state of the coarse-grained model, and an adjusted cart state, which is a state of the cart, so as to bring the state of the cart closer to the target cart state while avoiding tipping over of the mobile robot; a whole-body motion generator that generates joint commands that are motion commands for motor drivers that drive each joint of the manipulator, and wheel commands that are motion commands for motor drivers that drive the wheels, through optimal control that minimizes the value of an objective function that includes a term for realizing a center-of-gravity state of the manipulator that corresponds to the adjusted model state, where the objective function or optimization constraints include matters related to motion objectives other than preventing tipping, which are due to the mobile robot having a manipulator with more degrees of freedom than the coarse-grained model or the cart having a specific mechanical type; and an execution control unit that periodically executes the coarse-grained model motion planning unit and the whole-body motion generation unit to operate the mobile robot; A mobile robot comprising:

2. Matters concerning the operation objectives other than preventing falls in the above-mentioned optimal control are as follows: the objective function includes a term for causing the state of the carriage to follow the adjusted carriage state; the objective function includes a term for causing the tip position of the manipulator to follow a target tip position; the rotational velocity of at least one of the joints obeys an inequality constraint; The rotational speed of the wheels complies with the constraints imposed by the mechanism of the bogie; the moving speed of the center of gravity of the manipulator obeys an inequality constraint; the velocity of the tool center point of the manipulator obeys an inequality constraint; and The combination of the joint angles complies with the constraints of the manipulator to avoid self-collision.

10. The mobile robot of claim 1, comprising at least one of:

3. 2. The mobile robot of claim 1, wherein the adjusted model states and the adjusted carriage states are states of the coarse-grained model and states of the carriage that allow the position of the zero moment point of the mobile robot to move within a predetermined range set within a support domain of the mobile robot and minimize an objective function that includes a term for an error amount between a target state and a predicted state of the mobile robot over a certain time range into the future.

4. The whole body movement generating unit a center of gravity command generating unit that generates a center of gravity command regarding the center of gravity of the manipulator according to the command of the adjusted model state; a whole-body command generation unit that generates commands related to the operations of the manipulator and the carriage according to the center of gravity command and the carriage state command; The mobile robot of claim 3 further comprising:

Images