Mobile robot

Abstract

The present invention imparts favorable followability of a truck with respect to changes in target speed while preventing toppling by utilizing the movement of a manipulator. A robot control unit comprises: a target truck state acquisition unit that acquires a target truck state, which is the target state of a truck for causing a mobile robot to travel in accordance with a travel plan; an operation generation unit that permits the position of the zero moment point of the mobile robot to move within a prescribed range set up in a support region of the mobile robot, that determines a command for minimizing the value of an objective function including a term pertaining to the error amount between the target state and a predicted state of the mobile robot spanning a certain time range in the future, and that generates, on the basis of the command, a joint command, which is an operation command for a motor driver which drives a joint of a manipulator, and a wheel command, which is an operation command for a motor driver which drives a wheel; and an execution control unit that causes the mobile robot to operate by periodically executing the operation generation unit.

Description

Mobile Robot

[0001] The present disclosure relates to mobile robots.

[0002] Mobile robots are used in factories and other places. Some mobile robots are equipped with a manipulator with an arm (multiple links) having multiple joints and a carriage on which the manipulator is mounted, and the manipulator can be moved by driving the carriage. Such mobile robots are required to be able to quickly move 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).

[0004] 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.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

[0005] Non-Patent Documents 1 and 2 control the translational speed of the cart under the condition that the displacement of the zero moment point (hereinafter referred to as ZMP) from a target position (for example, the center position of the cart) is constrained to zero. As a result, tipping due to translational motion of the cart is prevented, but the ability to follow changes in the target speed is insufficient.

[0006] We understand that in the technologies of Non-Patent Documents 1 and 2, for example, after an instruction to start accelerating travel is given, the ZMP position is kept unchanged by balancing the following three effects (1) to (3) on the ZMP position: (1) The effect of the ZMP position moving backward due to the accelerating movement of the cart; (2) The effect of the ZMP position moving forward due to the inverted pendulum taking a forward tilting position; (3) The effect of the ZMP position moving backward as the inverted pendulum accelerates forward relative to the cart when it transitions to a forward tilting position. By constantly balancing these three effects, the ZMP position is maintained at the center of the cart.

[0007] Immediately after the command to start accelerating, the actions of (2) and (3) are nearly balanced, and the exertion of the action of (1) is suppressed. In other words, acceleration cannot begin as instructed to accelerate the trolley. When the acceleration of the inverted pendulum to move toward a forward tilting posture ends and the action of (3) disappears, the action of (1) can finally be exerted, balancing the action of (2), and the trolley can begin large acceleration. Due to the above phenomenon, the trolley's response to changes in the target speed becomes slow.

[0008] In view of the above-mentioned problems, the present disclosure aims to provide a mobile robot equipped with a cart and a manipulator that can be controlled to ensure good tracking of the cart in response to changes in the target speed while utilizing the movement of the manipulator to prevent tipping over.

[0009] A mobile robot according to the present disclosure includes a wheeled carriage, a manipulator with a multi-jointed arm, and a robot controller for controlling the carriage and the manipulator. The robot controller includes a target carriage state acquisition unit for acquiring a target carriage state, which is a target state of the carriage for causing the mobile robot to travel according to a travel plan; a motion generation unit for calculating commands that minimize an objective function that allows the position of the mobile robot's zero moment point to move within a predetermined range within the mobile robot's support volume and includes a term for an error amount between the target state and a predicted state of the mobile robot over a certain time range in the future, and for generating joint commands, which are motion commands for motor drivers that drive the joints of the manipulator, and wheel commands, which are motion commands for motor drivers that drive the wheels; and an execution controller for periodically executing the motion generation unit to operate the mobile robot.

[0010] According to the mobile robot of the present disclosure, it is possible to provide a mobile robot equipped with a cart and a manipulator, which is controlled so that the cart can easily follow changes in the target speed while utilizing the movement of the manipulator to prevent tipping.

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

[0012] 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.

[0013] Although the present embodiment has been described in sufficient detail to enable those skilled in the art to practice 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.

[0014] [Explanation of Terms, etc.] Before describing the embodiments, the meanings of various terms used in the embodiments will be described below. (1) A "manipulator" may be a device in which an end effector, a camera, etc. are attached to the tip of multiple links connected by multiple joints. (2) A "travel plan" is a plan of a route, speed, etc. created for a required future time range depending on the purpose of travel. A "travel plan" may be a plan created before the start of travel, or a plan created and modified dynamically while travelling. A "travel plan" may be a plan created outside the mobile robot and given to the mobile robot, or a plan created by the mobile robot itself.

[0015] (3) The zero moment point (ZMP) position is allowed to change within a "predetermined range." The ZMP position can be allowed to change within the predetermined range by using inequality constraints or by incorporating an error term for the ZMP position into the objective function. When allowing the ZMP position to change within the predetermined range by incorporating an error term for the ZMP into the objective function, the parameters of the objective function can be appropriately set using experimental techniques, given the specifications of the mobile robot and limitations on running acceleration.

[0016] (4) "A target state or predicted state over a certain future time range" generally refers to a state at each of multiple time steps within that time range, but also includes the case where the number of time steps within that time range is one. (5) The "command" in "command to minimize the value of the objective function" includes the case where it is a command series (a group of commands that form a time series). (6) The "motor driver" may be, for example, a control system that drives a motor so that the error from a target value given by an input command is small. Specific control methods in this control system may be torque control, position control, speed control, or a combination of these, and the motion generation unit can create commands including values ​​of a type compatible with the control method of the motor driver.

[0017] (7) In the mathematical formulas (images) included in the following explanation, bold notations represent matrices or vectors. To distinguish these matrices or vectors from scalar quantities, in the 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 text. In addition, two-level subscripts in the mathematical formulas (images) may be written in parallel in parentheses in the text (e.g., (cmd, Cog)).

[0018] First Embodiment 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 translate or rotate, and the manipulator 20 is mounted on top of it. The manipulator 20 includes multiple joints and multiple links, as described below. The control unit 30 controls the cart 10 and the manipulator 20 according to a travel plan.

[0019] In the example shown in Fig. 1, the bogie 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 the coordinate system of the bogie 10. The X-axis direction is the front-rear direction of the bogie 10, the Y-axis is the left-right direction of the bogie 10, and the Z-axis is the height direction of the bogie 10. This left-right direction is also the width direction of the bogie 10. In the following, an example will be described in which the height direction of the bogie 10 coincides with the vertical direction.

[0020] The dolly 10 includes 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 contact the floor surface LF, thereby supporting the mobile robot 1 and moving the dolly 10. A square 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 to stably support the manipulator 20, and the area connecting the contact points of the three or more wheels W may constitute the support area.

[0021] 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 carriage 10 can be made to move linearly (translationally). Furthermore, by making the rotation speeds of the two drive wheels WA different (differential), the carriage 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).

[0022] The manipulator 20 also includes a plurality of joints J, a plurality of links L, and an end effector H. The link L is a 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.

[0023] 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 includes a computer 100. The computer 100 includes a processor 102, a memory 104, a storage device 106, an input device 108, an output device 110, a storage medium reader 112, and a communication I / F (Interface) 114. These elements are interconnected via a bus 116 so as to be able to communicate with each other.

[0024] 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.

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

[0026] 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 program described above, but also an operating system.

[0027] 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.

[0028] 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.

[0029] 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 the operation status and other information for the mobile robot 1 may be output via wireless communication via the communication I / F 114.

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

[0031] 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).

[0032] 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.

[0033] 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, which are implemented within the computer 100 by the control program 118.

[0034] The target carriage state generation unit 33 generates a target state (target translational velocity v ｒｅｆ , angular velocity ω ｒｅｆ The motion generating unit 31 receives the target vehicle state information and outputs command signals (joint command signals, wheel command signals) for controlling the vehicle 10 and the manipulator 20.

[0035] 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 status of the various rotational drive units to the motion generator 31. The execution controller 32 periodically operates the motion generator 31 to move the mobile robot 1.

[0036] The motion generation unit 31 may, for example, be composed of a coarse-grained model motion planning unit 34 and a whole-body motion generation unit 35. The coarse-grained model motion planning unit 34 converts the motion of the manipulator 20 into a coarse-grained model (e.g., an inverted pendulum model) and generates a motion plan for the coarse-grained model. Specifically, the coarse-grained model motion planning unit 34 generates and outputs, as commands, adjusted state quantities of the coarse-grained model and adjusted state quantities 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 mobile robot 1's support domain and minimize the value of an objective function that includes a term for the error amount between the target state and the predicted state of the mobile robot 1 over a certain future time range. 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 the adjusted state quantities of the coarse-grained model. ｐｉｔｃｈ＿ｃ , q ｒｏｌｌ＿ｃ , Adjustment pendulum angular velocity・q ｐｉｔｃｈ＿ｃ , q ｒｏｌｌ＿ｃ ), and an adjusted carriage state command (e.g., adjusted translational velocity v ｃ , adjusted angular velocity ω ｃ The coarse-grained model is a virtual mechanism in which the total mass of multiple mass points is equal to the mass of the manipulator 20, the number of links is smaller than that of the manipulator 20, and the manipulator 20 is moved with fewer degrees of freedom.

[0037] 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-grain command generation unit 36 ​​and a whole-body command generation unit 37.

[0038] The center of gravity command generating unit 36 ​​outputs, in accordance with the adjusted model state command, a manipulator center of gravity command which is an operation command related to the center of gravity of the manipulator 20. 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.

[0039] 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.

[0040] The coarse-grained model motion planning unit 34 will be described in more detail with reference to Fig. 4. The coarse-grained model motion planning unit 34 uses, as an example, 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.

[0041] In the inverted pendulum model, when the state quantity → z and the input quantity → u are defined as 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.

[0042]

[0043] ​Here, it is assumed that the torque of the wheels W of the bogie 10 is transmitted without loss to the torque of the turning 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 negligible. 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.

[0044] 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 following method. As shown in FIG. 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 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 resultant force F3 of gravity F1 and inertial force F2 intersects with the floor LF is called the zero moment point (ZMP). If 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, causing the mobile robot 1 to begin to tip over.

[0045] Specifically, modeling of the ZMP in the X-axis direction will be described. 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 the amount of movement of the center of gravity is linearized 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:

[0046]

[0047] In addition, C Ｚ１ , C Ｚ２ , C Ｚ３ is a scalar quantity determined by geometric characteristics. Ｚ１ , C Ｚ２ , C Ｚ３ 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 quantities and input quantities:

[0048]

[0049] 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, and this must be incorporated 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 Ｌ can be expressed by the following formula:

[0050]

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

[0052]

[0053] 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:

[0054]

[0055] In addition, C Ｚ４ , C Ｚ５ , C Ｚ６ is a scalar quantity determined by geometric characteristics. Ｚ４ , C Ｚ５ , C Ｚ６ may be determined by identification or calculated using a design value. As in the case of the X-axis direction, [Equation 7] can be transformed into the following equations for the state quantity and input quantity. ｒｅｆ Since is time-varying, d ｂ also changes with time.

[0056]

[0057] ​​Next, a description will be given of the formulation of model predictive control in the coarse-grained model motion planning unit 34. A command value for a state quantity is defined as follows.

[0058]

[0059] where ω ｒｅｆ , v ｒｅｆ is a command value sent from the target bogie state generation unit 33, which is a higher-level controller, and k is a discrete time.

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

[0061]

[0062] N ｐ , N ｃ are the prediction horizon step and the control horizon step, respectively (in this example, N ｐ = N ｃ (They state that).

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

[0064]

[0065] 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:

[0066]

[0067] 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 the following [Equation 14].

[0068]

[0069] ​​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.

[0070] Using the input amount →u calculated so far and [Equation 2], the translational velocity at the next time (adjusted translational velocity v ｃ ), turning speed (adjusted angular speed ω ｃ ), virtual pendulum angle (adjusted pendulum angle q pitch＿ｃ ), virtual pendulum angular velocity (adjusted pendulum angular velocity q pitch , q roll＿ｃ ) 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.

[0071] Here, a method for calculating the ZMP will be explained. The position of the ZMP approximated by a two-mass system can be expressed as follows:

[0072]

[0073] where M is the mass of the cart 10, m is the mass of the inverted pendulum, g is the acceleration of 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, x ｊ１ , z ０ , z １ 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. ｗｈ is the moment of inertia around the wheel axle. Here, the X-axis component of the target ZMP position is X ＺＭＰ０ can be set, for example, as follows:

[0074]

[0075] The error from this desired ZMP position is defined as follows:

[0076]

[0077] Assuming that the change in the center of gravity of the arm in the Z-axis direction is small, sinq_ pitch ≒q_ pitch , and z １ If we set C to a constant value, the change in ZMP X(d, ZMP) from an arbitrary origin in the mobile robot 1 can be expressed as follows using the above state quantities. Ｚ１ , C Ｚ２ , C Ｚ３ is a constant value.

[0078]

[0079] Next, the operation of the whole-body command generation unit 37 will be described. The whole-body command generation unit 37 formulates 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 generates the command values ​​by 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.

[0080] The design of the objective function and the design of the constraints will now be described. In this example, an objective function is designed to make the velocity of the carriage 10, the velocity of the center of gravity of the manipulator 20, and the velocity of the tip (end effector H) of the manipulator 20 follow desired values.

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

[0082]

[0083] However, →A ξ , →A θ is a constraint condition coefficient matrix that is uniquely determined from the geometric structure of the carriage 10. More specifically, →A ξ is a coefficient related to the attitude of the carriage 10 and the distance between the two wheels, ・θ 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.

[0084] 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 that 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:

[0085]

[0086] Here, K(CoG, p) is a gain related to the position error. This reference value →·p(ref, CoG) and the robot's center of gravity Jacobian →J ＣｏＧ 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.

[0087]

[0088] Next, we will explain the design of an objective function that optimizes the velocity of each axis of the manipulator 20 so that it follows the command of the tip position (tool center point TCP) of the manipulator 20. When there is a desired command p(ref, TCP) for the tip velocity, as in the case of the center of gravity position, the Jacobian → J ＴＣＰ Using this, we can define the error as follows:

[0089]

[0090] 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.

[0091] 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:

[0092]

[0093] 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-interference avoidance, each link is approximated by a capsule shape, and the closest distance vsself(→·θ) between capsules excluding adjacent capsules is constrained so that it does not exceed a certain threshold dclosest.

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

[0095]

[0096] The rotational speed of each wheel W of the carriage 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 are terms related 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 intended, the third term is unnecessary.

[0097] 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.​

[0098] If the motor driver MD is configured to input a position command, the whole-body command generation 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.

[0099] 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 (dynamic 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 may be equipped with a current loop control controller, which can be tuned separately.

[0100] 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 means.

[0101] Next, in step S12, a feasible solution is set. The 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.

[0102] 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 of the objective function. 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 of the objective function. 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.

[0103] 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.

[0104] As described above, according to the mobile robot 1 of the first embodiment, for the movement of the mobile robot 1 equipped with the cart 10 and manipulator 20, input amounts are calculated in accordance with the objective function while maintaining inequality constraints to generate commands that allow the ZMP position of the manipulator 20 to move within a predetermined range within the support area SA. This allows the ZMP to move within the predetermined range while preventing the mobile robot 1 from tipping over, thereby providing a mobile robot that allows the cart 10 to better track changes in the target velocity. Here, the predetermined range within the support area SA is not limited to a specific size, as long as it does not exceed the support area.

[0105] 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, in which two mass points MＡ , M Ｌ In this embodiment, a two-mass point model is adopted to represent the manipulator 20 as follows. This is different from the first embodiment.

[0106] 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 shown in FIG. 7, the link group A below joint J3 is assigned to mass point M. Ａ and the link group B on the end effector H side from the joint J3 is represented by the mass point M Ｂ Consider the two mass model expressed as follows: Ａ , M Ｂ is set so that the total mass is equal to the mass of the manipulator 20. According to this two-mass point model, it is possible to express the change in the height of the center of gravity position accompanying the change in the posture of the manipulator 20.

[0107] The coarse-grained model motion planning unit 35 uses the mass point M Ｂ By adding a term that represents the error between the target value and the height of the mass point M Ｂ The whole body motion generating unit 35 can make the height of the mass point M follow the target height. Ａ , mass point M Ｂ The center of gravity Jacobian →J(CoG,A) that projects the reference values ​​→·p(ref,A) and →·p(ref,B) of the center of gravity velocity corresponding to 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 velocity θ of all joints J onto the center of gravity velocity φ(CoG,B) of link group B can be used to calculate the →J of [Equation 25]. Ｃ０Ｇ 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.

[0108] [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 movement generator 35.

[0109] The configuration of the motion generation unit 31 of the 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.

[0110] Specifically, commands relating 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 relating 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 τ of the single mass point model. pitch Furthermore, by using the pendulum length l, the force →f to be applied to the center of gravity can be determined as follows. If we simply consider only the X direction, it can be expressed as follows:

[0111]

[0112] Using this force →f, the whole-body movement 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].

[0113]

[0114] 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, a term related to the speed tracking of the cart may be added to the objective function, and constraints determined by the cart mechanism, restrictions related to the motor specifications, etc. may also be added.

[0115] Fourth Embodiment A mobile robot 1 according to a fourth embodiment will now be described with reference to FIG. 9 . The basic configuration of this mobile robot 1 is the same as that of the first embodiment, so a redundant description will be omitted. This fourth embodiment differs from the previous embodiments in that it does not use a coarse-grained model that simplifies the manipulator 20, but instead uses a whole-body model that precisely represents the structure of the manipulator 20 to generate joint commands and wheel commands. Instead of providing a whole-body model motion planning unit 34 and a whole-body motion generation unit 35 separately as shown in the figure, the whole-body model motion planning unit 34 may directly output commands to the motor driver MD.

[0116] In the fourth embodiment, commands for operating the carriage and manipulator 20 are generated in accordance with the objective functions and constraint equations shown in the following [Equation 28] to [Equation 33].

[0117] (4)

[0118] Here, || ｘ represents the L2 norm, and the state vector →x ｋ Element inside → q ｋ = [q １ , q ２ ,...,q ｎ ] Ｔ is the joint angle of the manipulator 20 (the angle between the links), and →·q ｋ is its angular velocity, and →φ ｋ , →・φ ｋ are the joint angles and angular velocities of the drive shafts (differential two wheels) of the carriage 10, and v ｋ , w ｋ are the translational velocity and angular velocity of the carriage 10. ｋ is a vector including torque commands, position commands, or torque commands for all axes of the carriage 10 and manipulator 20 selected by the designer. Also, →e(x, k) is the error between the target state quantity and the actual state →x(cmd, k)-x ｋFor example, e(task, k) specifies the error between the target hand position and the actual position of the manipulator 20 by the following [Equation 34].

[0119]

[0120] Also, →Q, →R, →S are weight matrices that determine the contribution of each term of the objective function to the evaluation value.

[0121] The objective function of [Equation 28] is defined as the sum of the L2 norms of the error between the target state and the actual state, the control input, and any other error amounts, added up from time t = k * Δt to t = (k + N) * Δt. It is possible to obtain a control input that minimizes the value of the objective function while satisfying the equality constraints and inequality constraints of [Equation 31] to [Equation 33]. Note that if the controlled object is a deterministic system, →x ｋ , →x ｋ＋１ , →x ｋ＋２ , ... is →u ｋ , →x ｋ＋１ , →x ｋ＋２ , ..., it is possible to exclude them from the variables. Each term in the objective function is multiplied by a weighting matrix, which is used to specify the relative importance of each term. It is a weight for the error of each state quantity, and the larger its absolute value, the larger the value it is evaluated as, so the optimal solution is calculated so that the L2 norm of the corresponding term is smaller.

[0122] [Equation 30] is a kinematics and dynamics model of the manipulator 20 and the carriage 10, and [Equation 31] is an inequality constraint for limiting the position of the ZMP to a predetermined area within the support area SA. Here, the allowable range of the ZMP position is not divided into the X direction and the Y direction, but is indicated by →p. From the viewpoint of avoiding tipping over, it is preferable that the ZMP be maintained near the center of the support area formed by the contact point. Furthermore, [Equation 32] and [Equation 33] are inequality constraints for limiting the state and input (position, velocity, acceleration, jerk, etc. of the joint J) of the manipulator 20. In addition to these, it is possible to add a self-collision constraint, an obstacle avoidance constraint, a contact force constraint when contacting the environment, etc.

[0123] From the above, for example, the translational velocity v of the carriage 10 ｋ and angular velocity ω ｋThe goal is ｋ (cmd), ω ｋ When tracking control is required for (cmd), if the error is large, the first term of the objective function in equation (1) will take a large value, and the optimal solution that minimizes the effect of the error → u ｋ , →u ｋ＋１ ... is determined. Each term in the objective function is multiplied by a weighting matrix →Q, →R, →S, which is used to specify the relative importance of each term. For example, →Q is the weight for the error of each state quantity, and the larger its absolute value, the larger the value it is evaluated as. Therefore, the optimal solution is calculated so that the L2 norm of the corresponding term is made smaller.

[0124] 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.

[0125] 1...Mobile robot, 10...Cart, 11...Moving mechanism, 20...Manipulator, 30...Control unit, WA...Drive wheel, WB...Driven wheel, J...Joint, L...Link, H...End effector, AX...Rotation axis, 31...Movement generation unit, 32...Execution control unit, 33...Desired 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 reader, 116...Bus, 118...Control program, MD...Motor driver, SA...Support area.

Claims

1. A mobile robot comprising: a wheeled cart, a manipulator with an arm having multiple joints, and a robot controller for controlling the cart and the manipulator. The robot controller comprises: a target cart state acquisition unit that 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; a motion generation unit that calculates commands that minimize an objective function that allows the position of the mobile robot's zero moment point to move within a predetermined range set within the mobile robot's support area and includes a term for the amount of error between the target state and a predicted state of the mobile robot over a certain time range in the future, and generates, based on the commands, joint commands that are motion commands for motor drivers that drive the joints of the manipulator and wheel commands that are motion commands for motor drivers that drive the wheels; and an execution control unit that operates the mobile robot by periodically executing the motion generation unit.

2. The mobile robot of claim 1, wherein the command to minimize the value of the objective function in the motion generation unit includes a command expressed as a state of a coarse-grained model having one or more mass points whose total mass is the mass of the manipulator and a virtual mechanism that moves the mass points with degrees of freedom that are fewer than the number of joints in the arm.

3. The mobile robot according to claim 1, wherein the command for minimizing the value of the objective function in the motion generation unit includes a command expressed as the state of each joint of the arm.

4. The mobile robot of claim 2, wherein the motion generation unit further comprises: a coarse-grained model motion planning unit that generates and outputs commands based on the adjusted state quantities of the coarse-grained model and the adjusted state quantities of the cart that minimize the value of the objective function; and a whole-body motion generation unit that generates and outputs commands to drive the cart and the manipulator in accordance with the commands output by the coarse-grained model motion planning unit.

5. The mobile robot of claim 4, wherein the whole-body motion generation unit further comprises: a center-of-gravity command generation unit that generates a center-of-gravity command for the center of gravity of the manipulator in accordance with a command for the adjusted state quantity of the coarse-grained model; and a whole-body command generation unit that generates commands for motion of the manipulator and the cart in accordance with the center-of-gravity command and a command for the adjusted state quantity of the cart.

6. The mobile robot of claim 1, wherein the motion generator generates the joint commands and the wheel commands to satisfy inequality constraints on the position of the zero moment point, thereby allowing the position of the zero moment point of the mobile robot to move within the predetermined range.

Images