Mobile robot

Abstract

To improve the follow-up ability of a truck to the change of a target speed while preventing overturning by utilizing the movement of a manipulator.SOLUTION: The robot control unit includes a target cart state acquisition unit configured to acquire a target cart state that is a target state of a cart for causing the mobile robot to travel according to a travel plan, an operation generation unit configured to obtain a command that allows a position of a zero moment point of the mobile robot to move within a predetermined range set in a support area of the mobile robot and minimizes a value of an objective function including a term of an error amount between a target state and a predicted state of the mobile robot over a certain future time range, and generate, based on the command, a joint command that is an operation command for a motor driver that drives a joint of a manipulator and a wheel command that is an operation command for a motor driver that drives a wheel, and an execution control unit configured to periodically execute the operation generation unit to operate the mobile robot.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] 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 over due to translational motion of the cart is prevented, but the tracking ability to 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 is given, the ZMP position is prevented from changing by balancing the following three effects (1) to (3) on the ZMP position. (1) The acceleration of the cart causes the ZMP position to move backward. (2) The ZMP position moves forward when the inverted pendulum assumes a forward tilting posture. (3) When the inverted pendulum shifts to a forward tilting posture, the ZMP position shifts backward as the inverted pendulum accelerates forward relative to the cart. By constantly balancing these three forces, the ZMP position is maintained at the center of the cart.

[0007] Immediately after the command to start accelerating is given, the actions of (2) and (3) are nearly balanced, and the exertion of the action of (1) is suppressed. In other words, the trolley cannot start accelerating as instructed. When the acceleration of the inverted pendulum to move towards a forward tilting posture ends and the action of (3) disappears, the action of (1) can finally be exerted, which balances the action of (2), and the trolley can start to accelerate significantly. 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 utilize the movement of the manipulator to prevent tipping over while ensuring good tracking of the cart with respect to changes in the target speed. [Means for solving the problem]

[0009] The 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 zero moment point of the mobile robot 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 and wheel commands, which are motion commands for motor drivers that drive the joints of the manipulator and motor drivers that drive the wheels, based on the commands; and an execution controller for periodically executing the motion generation unit to operate the mobile robot. [Effects of the Invention]

[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. [Brief explanation of the drawings]

[0011] [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. [Figure 9] FIG. 10 is a block diagram illustrating the configuration of an action generation unit 31 according to a fourth embodiment. DETAILED DESCRIPTION OF THE INVENTION

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

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

[0015] (3) The position of the zero moment point (ZMP) is allowed to move within a "predetermined range." The means for allowing the ZMP position to move within the predetermined range can be an inequality constraint or by including an error term for the ZMP position in the objective function. When allowing the ZMP position to move within the predetermined range by including an error term for the ZMP in the objective function, the parameters of the objective function can be appropriately set using experimental methods, given the specifications of the mobile robot and the limits on running acceleration.

[0016] (4) "Target state, predicted state over a certain time range in the future" generally refers to the 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 that minimizes the value of the objective function" includes a command sequence (a group of commands that are sequential in time). (6) 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. The specific control method 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 that is compatible with the control method of the motor driver.

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

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

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

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

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

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

[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 programs 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 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. Examples of storage media include CD (Compact Disc)-ROM, DVD (Digital Versatile Disc)-ROM, Blu-ray Disc, and 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, all of which are implemented within the computer 100 by the control program 118.

[0034] The target vehicle state generation unit 33 generates a target state (target translational velocity v ref , angular velocity ω ref The operation generation 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 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.

[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's support region 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. 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 ω cThe 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, and the number of links is smaller than that of the manipulator 20, allowing the manipulator 20 to move 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-gravity 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 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.

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

[0042]

number

number

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

[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 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, causing the mobile robot 1 to begin to tip over.

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

[0046]

number

[0047] In addition, C Z1 , C Z2 , C Z3 is a scalar quantity determined by geometric characteristics. Z1 , C Z2 , C Z3 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:

[0048]

number

[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, 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:

[0050]

number

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

[0052]

number

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

number

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

[0056]

number

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

[0058]

number

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

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

[0061]

number

number

[0062] N p , N c are the prediction horizon step and the control horizon step, respectively (in this example, N p =N c (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 [Mathematical formula 2]:

[0064]

number

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

number

[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 [Equation 14] below.

[0068]

number

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

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

[0072]

number

[0073] 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 ZMP0can be set, for example, as follows:

[0074]

number

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

[0076]

number

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

[0078]

number

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

[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 speed of the carriage 10, the speed of the center of gravity of the manipulator 20, and the speed of the tip of the manipulator 20 (end effector H) follow desired values.

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

[0082]

number

number

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

[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, 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:

[0085]

number

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

[0087]

number

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

[0089]

number

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

number

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

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

[0095]

number

[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 required, 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] 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.

[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 (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.

[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 device.

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

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

[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, the mobile robot 1 equipped with the cart 10 and manipulator 20 generates commands by calculating input amounts according to an objective function while maintaining inequality constraints so that the ZMP position of the manipulator 20 is allowed to move within a predetermined range of the support area SA. This prevents the mobile robot 1 from tipping over while allowing the ZMP to move within the predetermined range, thereby providing a mobile robot with good tracking ability for the cart 10 to changes in the target speed. The predetermined range within the support area SA need only be large enough not to exceed the support area, and is not limited to a specific size.

[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, 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.

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

[0107] In the coarse-grained model motion planning unit 35, the objective function is B By 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]. C0GBy 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 motion generator 35.

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

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

[0111]

number

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

[0113]

number

[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, 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.

[0115] [Fourth embodiment] Next, a mobile robot 1 according to a fourth embodiment will 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 rather 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]

number

number

number

number

number

number

[0118] where || x represents the L2 norm, and the state vector →x k element inside → q k =[q1, q2, . . . , q n ] T is the joint angle of the manipulator 20 (the angle between the links), and →·q k is its angular velocity, and →φ k , →·φ k is the joint angle and angular velocity of the drive shaft (differential two wheels) of the bogie 10, and v k , w k are the translational and angular velocities of the carriage 10. →u k 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 k →e(task, k) specifies the error between the target hand position and the actual position of the manipulator 20, for example, by the following [Equation 34].

[0119]

number

[0120] Also, →Q, →R, →S are weight matrices that determine the contribution of each term in 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 and inequality constraints of [Equation 31] to [Equation 33]. Note that if the controlled object is a deterministic system, →x k, →x k+1 , →x k+2 ,··· is →u k , →x k+1 , →x k+2 , ..., and so 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. This is the weight given to 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 cart 10, and [Equation 31] is an inequality constraint for restricting the position of the ZMP to a predetermined area within the support area SA. Here, the allowable range of the ZMP position is indicated by →p without dividing it into the X and Y directions. From the perspective 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 restricting the state and input (position, velocity, acceleration, jerk, etc.) 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 cart 10 k and angular velocity ω k The goal is k (cmd), ω k 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 is →u k , →u k+1 ... 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, so the optimal solution is calculated so that the L2 norm of the corresponding term is 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. [Explanation of symbols]

[0125] 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 carriage having wheels; a manipulator having an arm with a plurality of joints; and a robot controller for controlling the carriage and the manipulator, The robot control unit a target vehicle state acquisition unit that acquires a target vehicle state, which is a target state of the vehicle for traveling the mobile robot according to a travel plan; a motion generator that calculates commands that minimize an objective function that allows 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 the error between a target state and a predicted state of the mobile robot over a certain time range in the future, and generates joint commands, which are motion commands for motor drivers that drive the manipulator's joints, and wheel commands, which are motion commands for motor drivers that drive the manipulator's wheels, based on the commands; an execution control unit that periodically executes the motion generation unit to cause the mobile robot to move; A mobile robot comprising:

2. 2. The mobile robot of claim 1, wherein the command for minimizing the objective function in the motion generator includes a command expressed as states of a coarse-grained model including one or more mass points whose total mass is the mass of the manipulator and a virtual mechanism for moving the mass points with degrees of freedom that are fewer than the number of joints in the arm.

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

4. The action generation unit a coarse-grained model motion planning unit that generates and outputs, as commands, adjusted state quantities of the coarse-grained model and adjusted state quantities of the bogie that minimize the value of the objective function; a whole-body motion generation unit that generates and outputs commands for driving the carriage and the manipulator in accordance with the commands output by the coarse-grained model motion planning unit; The mobile robot of claim 2 further comprising:

5. 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 a command of the adjusted state quantity of the coarse-grained model; 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 command of the adjusted state quantity of the carriage; The mobile robot of claim 4 further comprising:

6. 2. The mobile robot of claim 1, wherein the motion generator generates the joint commands and the wheel commands to satisfy an inequality constraint 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