JP2007257200A - Mobile body and control method for it - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a mobile body capable of moving smoothly and a control method for it. <P>SOLUTION: This control method for the mobile body for performing feedback control on movement of the mobile body following a target route is provided with steps for: estimating a positional attitude of the mobile body (a step S102); deciding a turning speed based on the estimated positional attitude and the target route (a step S103), deciding a straight traveling speed of the mobile body from the decided turning speed for keeping kinetic energy of the mobile body constant (a step S104); and controlling a drive mechanism for the mobile body based on the decided turning speed and straight traveling speed. <P>COPYRIGHT: (C)2008,JPO&INPIT

Description

  The present invention relates to a moving body and a control method thereof, and more particularly to a moving body that follows a target route and a control method thereof.

  2. Description of the Related Art Conventionally, techniques for automatically moving a moving body such as a robot or a vehicle to a target route have been proposed. For example, there is a technique of automatically following a feedback control of a lateral deviation amount or an azimuth angle deviation amount of a moving body with respect to a target route. In such an autonomous moving body, a target route from the movement start position to the target position is planned. The moving body moves following the target route while being feedback controlled.

  An example of such an autonomous moving body is a two-wheeled vehicle. Movement control for a moving body such as a two-wheeled vehicle is usually performed by combining linear movement and curved movement. Therefore, a method of moving by inputting a straight traveling speed and a turning speed (angular speed) is generally used. Such a turning speed is usually obtained from feedback. Further, the straight traveling speed v is obtained as a constant speed. Furthermore, in the case of moving following a target route, a technique for obtaining a straight traveling speed so that the centrifugal force becomes a predetermined threshold value or less is disclosed in order to move the curved portion stably (Patent Document 1). reference). That is, in the above-mentioned document, in the non-holonomic two-wheeled robot, a limitation due to centrifugal force is added as an index for obtaining the straight traveling speed v.

JP-A-11-327641

  A method of calculating the straight traveling speed v in the above technique will be described. The centrifugal force applied to the mobile robot is shown as f = mvω. Here, m is the mass of the mobile robot, and ω is the turning speed of the mobile robot. The curvature κ can be calculated from the turning speed ω.

Then, mvω ≦ f _max is set so that the centrifugal force is within the threshold value. Thereby, the restriction on the straight traveling speed v is given by v ≦ f _max / mω. When the maximum value of the straight traveling speed is v _max , v _max = min (v ₀ , f _max / mω) is given. Here, v ₀ is a preset value. Then, the moving body moves at a straight speed of v _max . Thus, when the straight traveling speed v exceeds f _max / mω, a restriction is provided so that the straight traveling speed v becomes v ₀ .

FIG. 11 shows the relationship between the straight speed v calculated in this way and the turning speed ω. In this prior art, the linear velocity v is limited by centrifugal force. However, in this technique, the acceleration becomes discontinuous as shown by the dotted line in FIG. Therefore, the movement cannot be made smooth. That is, the acceleration changes abruptly at ω = f _max / mv ₀ as a boundary. Thus, the conventional technique has a problem that it cannot move smoothly.

  The present invention has been made to solve such a problem, and an object thereof is to provide a moving body that can move smoothly and a control method thereof.

  The moving body according to the first aspect of the present invention is a moving body having a control unit that performs feedback control so as to follow a target path, and the control unit estimates a position and orientation of the moving body. A posture estimation unit, a turning speed determination unit that determines a turning speed based on the position and posture estimated by the position and posture estimation unit, and the target path, and the turning so that the kinetic energy of the moving body becomes constant A straight-ahead speed determining unit that determines a straight-ahead speed of the moving body from a turning speed determined by the speed-determining unit; a turning speed determined by the turning-speed determining unit; and a straight-ahead speed determined by the straight-ahead speed determining unit; And a drive control unit for controlling the drive mechanism of the moving body. Thereby, smooth movement becomes possible.

（ここで、ｍは移動体の質量、Ｉは移動体の慣性モーメント、ωは移動体の旋回速度、ｖは移動体の直進速度、Ｃは定数である。）
これにより、スムーズな移動を簡便に制御することができる。 In the mobile body according to the second aspect of the present invention, in the mobile body described above, the linear travel speed determination unit determines the straight travel speed so as to satisfy Formula 3, and the motion of the mobile body represented by the left side of Formula 3 The energy is constant at a constant C.

(Where m is the mass of the moving body, I is the moment of inertia of the moving body, ω is the turning speed of the moving body, v is the straight traveling speed of the moving body, and C is a constant.)
Thereby, smooth movement can be controlled easily.

  In the mobile body according to the third aspect of the present invention, in the mobile body described above, the rectilinear speed determining unit is configured so that the kinetic energy of the mobile body is constant regardless of the turning speed determined by the turning speed determining unit. The straight speed is determined. Thereby, smooth movement can be controlled easily.

  In the mobile body according to the fourth aspect of the present invention, in the above mobile body, when the turning speed determined by the turning speed determination unit is within a certain range, the kinetic energy of the mobile body becomes constant. The straight traveling speed determination unit determines the straight traveling speed. Thereby, it can move smoothly compared with the past.

  The moving body according to the fifth aspect of the present invention is the above-described moving body, wherein the straight traveling speed determining unit is configured such that when the turning speed is outside the certain range, the centrifugal force of the moving body is constant. Determines the straight speed. Thereby, even when the movement speed is increased, the movement can be performed smoothly.

  A moving body according to a sixth aspect of the present invention further includes a task processing mechanism for executing a predetermined task in the above moving body, and the mass of the moving body is determined in accordance with a mass change before and after executing the task. The straight traveling speed is determined so that the kinetic energy is constant in the changed state. Thereby, even when mass changes, it can be moved smoothly.

  The moving body according to a seventh aspect of the present invention is the above moving body, wherein the linear velocity is such that the kinetic energy is constant at different values in a state after the task is executed and a state before the task is executed. Is to determine. Thereby, it can be moved more smoothly.

  A control method for a mobile object according to an eighth aspect of the present invention is a control method for feedback control of the movement of a mobile object that follows a target route, the step of estimating the position and orientation of the mobile object, A step of determining a turning speed based on the estimated position and orientation and the target route, and a straight traveling speed of the moving body is determined from the determined turning speed so that the kinetic energy of the moving body becomes constant. And a step of controlling a drive mechanism of the moving body based on the determined turning speed and the determined straight traveling speed. Thereby, smooth movement becomes possible.

（ここで、ｍは移動体の質量、Ｉは移動体の慣性モーメント、ωは移動体の旋回速度、ｖは移動体の直進速度、Ｃは定数である。）
これにより、スムーズな移動を簡便に制御することができる。 According to a ninth aspect of the present invention, in the method for controlling a moving body according to the above control method, in the step of determining the straight traveling speed, the straight traveling speed is determined so as to satisfy the mathematical formula 4, The kinetic energy of the moving body is constant C.

(Where m is the mass of the moving body, I is the moment of inertia of the moving body, ω is the turning speed of the moving body, v is the straight traveling speed of the moving body, and C is a constant.)
Thereby, smooth movement can be controlled easily.

  According to a tenth aspect of the present invention, in the control method for a moving body in the above control method, in the step of determining the straight traveling speed, the kinetic energy of the moving body is constant regardless of the determined turning speed. Thus, the straight speed is determined. Thereby, smooth movement can be controlled easily.

  According to an eleventh aspect of the present invention, there is provided a method for controlling a moving body according to the above control method, wherein in the step of determining the straight traveling speed, the determined turning speed is within a certain range. The straight traveling speed is determined so that the kinetic energy is constant. Thereby, it can move smoothly compared with the past.

  According to a twelfth aspect of the present invention, there is provided a control method for a moving body according to the above control method, wherein in the step of determining the straight traveling speed, if the turning speed is outside the predetermined range, the mobile body is centrifuged. The straight traveling speed is determined so that the force is constant. Thereby, even when the movement speed is increased, the movement can be performed smoothly.

  A control method for a moving body according to a thirteenth aspect of the present invention further includes a step of executing a predetermined task in the control method described above, wherein the mass of the mobile body is changed according to a mass change before and after executing the task. The straight-advancing speed is determined so that the kinetic energy is constant in a state in which is changed. Thereby, even if it is a case where mass is changed, it can be moved smoothly.

  A mobile body control method according to a fourteenth aspect of the present invention is the above control method, wherein the kinetic energy is constant at different values between a state after the task is executed and a state before the task is executed. The straight speed is determined. Thereby, it can be moved more smoothly.

  ADVANTAGE OF THE INVENTION According to this invention, the mobile body which can move smoothly and its control method can be provided.

Embodiment 1 of the Invention
A moving body according to the present embodiment will be described with reference to FIG. FIG. 1A is a top view schematically showing the configuration of the moving body according to the present embodiment, and FIG. 1B is a side view schematically showing the configuration of the moving body according to the present embodiment. It is. Reference numeral 10 denotes a moving body, 11 denotes a vehicle body, 12 denotes driving wheels, 13 denotes passive wheels, 14 denotes a driving mechanism, and 15 denotes a control unit. In the present embodiment, the moving body 10 is described as an opposed two-wheeled wheel moving robot.

  The moving body 10 is a non-holonomic two-wheeled autonomous moving body, and follows a target path using error feedback. That is, feedback control is performed based on the position and orientation error between the current position and orientation of the moving body and the target route. Here, the position / orientation indicates the position and orientation (orientation). That is, the position of the moving body 10 corresponds to the coordinates, and the posture corresponds to the traveling direction. Therefore, the posture changes according to the traveling direction of the moving body 10. In addition, about an attitude | position, only the horizontal direction (left-right direction) may be considered and the inclination degree with respect to a perpendicular direction may be included. Since the moving body 10 is a non-holonomic mobile robot, the constraint condition of the dynamic system includes speed and is not integrable with respect to time. Drive wheels 12 are provided below the side surface of the vehicle body 11. The drive wheels 12 are respectively provided on opposite side surfaces of the vehicle body 11. A passive wheel 13 such as a caster is provided at the front center portion of the vehicle body 11. The two drive wheels 12 rotate independently by a drive mechanism 14 having a motor or the like. That is, the moving body 10 moves when the drive mechanism 14 rotates the left and right drive wheels 12. The two drive wheels 12 are connected to different drive mechanisms 14, respectively. Therefore, the moving direction and moving speed of the moving body 10 can be controlled by rotating the drive wheels 12 at different rotating directions and rotating speeds. Specifically, by rotating the drive wheels 12 at the same direction and at different speeds, the moving body 10 moves while changing its moving direction. That is, the moving body 10 moves while curving in accordance with the distance between the two drive wheels 12 and the difference in rotational speed. Further, by rotating the driving wheel 12 in the opposite direction at the same speed, the moving body 10 turns on the spot. Further, the moving body 10 moves straight by rotating the drive wheels 12 in the same direction and at the same speed.

  The control unit 15 controls the drive mechanism 14 so that the moving body 10 follows the target path. For example, the control unit 15 plans a target route based on the current position and orientation of the moving body 10 and the target position and orientation. Then, the control unit 15 performs feedback control or feedforward control on the drive mechanism 14 so that the moving body 10 moves following the target path. The control unit 15 estimates the current position based on measured values from, for example, a rotary encoder attached to the motor or drive wheel 12 of the drive mechanism 14, a gyro sensor attached to the vehicle body 11, a laser range finder, or the like. Then, the control unit 15 performs feedback control based on the estimated position. As a result, the motor of the drive mechanism 14 rotates at an appropriate rotation angle. And the driving wheel 12 rotates and the mobile body 10 moves following a target path | route.

  The control unit 15 of the moving body according to the embodiment of the present invention is realized by, for example, an arithmetic processing device mounted on the vehicle body 11. The arithmetic processing unit includes, for example, a central processing unit (CPU), a storage device such as a ROM, a RAM, and a hard disk, an input / output interface, and the like. In a storage device such as a ROM, an instruction can be given to a CPU or the like in cooperation with an operating system to record an application program, which is executed by being loaded into the RAM. This application program includes a unique program for realizing the control method according to the present invention. For example, a program for planning a target route, a program for estimating a position and orientation based on a measurement value from a sensor, a program for calculating a turning speed (angular speed during turning of the mobile body 10) and a straight traveling speed from the current position and orientation, turning A program for calculating the rotational speed of the drive wheel 12 from the speed and the straight traveling speed is included. The follow-up control by the control unit 15 is executed when the central processing unit develops the application program on the RAM, reads the data stored in the storage device, and performs processing according to the application program, and stores it. .

  The configuration of the control unit 15 will be described with reference to FIG. FIG. 3 is a block diagram showing a configuration of the control unit 15 used in the moving body 10 according to the present embodiment. The control unit 15 includes a drive control unit 21, a route generation unit 22, a current position / orientation estimation unit 24, a turning speed determination unit 26, a set value storage unit 27, and a straight traveling speed determination unit 28. Yes.

  When the target position and orientation are set, the route generation unit 22 generates a target route based on the initial position and orientation of the moving body 10 and the target position and orientation. That is, a target route from the initial position and orientation to the target position is planned. For example, the target route that is the target trajectory of the mobile object 10 is often given as a set of coordinates. The coordinates are expressed by, for example, grid coordinates discretized in a grid shape. The advantage of using the grid coordinates is that a shortest route search can be easily performed and a route that can reliably reach the destination can be obtained. However, since the target route expressed in the grid coordinates is a discontinuous route in which a straight line is connected, it is difficult for the moving body 10 to follow such a route. Therefore, a smooth target path that can be used for follow-up control of the moving body 10 can be created from given discrete path coordinates by, for example, obtaining an interpolation curve using a weighted moving average. In this way, a target route can be easily planned by forming a grid for a given map.

  Specifically, for example, a route obtained by searching the grid map by A * search (Aester search) can be used. Furthermore, the calculation which smoothes the line segment set which connected the grid is performed. Of course, you may plan a target path | route by methods other than the above. For example, a search technique such as a best priority search or a hill-climbing search can be used.

  A sensor 18 used for estimating the current position and orientation is attached to the moving body 10. Therefore, the current position and orientation are estimated based on the measurement value of the sensor 18. Examples of the sensor 18 include a rotary encoder attached to the motor of the drive mechanism 14 or the drive wheel 12, a gyro sensor attached to the vehicle body 11, and the like. Furthermore, a laser range finder can be used as an environment recognition sensor. The sensor 18 outputs measurement values to the current position / orientation estimation unit 24 of the control unit 15 at predetermined time intervals.

  The current position / orientation estimation unit 24 estimates the current position and orientation of the moving body 10 based on the measurement value of the sensor 18. For example, the current position can be estimated using a rotary encoder that is an internal sensor. The rotational speed of the left and right motors or drive wheels 12 is measured by a rotary encoder. And the control part 15 calculates the moving amount | distance of the drive wheel 12 based on the measured rotation speed. By obtaining the movement locus from the initial position and orientation, the current estimated position and orientation can be calculated. Furthermore, in this embodiment, a laser range finder is used as an environment recognition sensor. The current position / orientation estimation unit 24 performs self-position estimation using a particle filter. Note that a gyro sensor or the like can also be used for posture estimation. The position estimation is not limited to the above method. For example, an acceleration sensor, a camera or other sensor is used. Of course, the position and orientation may be estimated using a plurality of types of sensors. Here, it is assumed that the position and orientation estimated by the current position and orientation estimation unit 24 is (x, y, θ).

  The control position / orientation calculation unit 25 calculates the nearest point from the current position / orientation with respect to the target route. Then, the position of the nearest point on the target route is calculated as the control position, and the direction of the target route at the nearest point is calculated as the control posture. That is, the current position and orientation (x, y, θ) calculated by the current position and orientation estimation unit 24 and the target route generated by the route generation unit 22 are input to the control position and orientation calculation unit 25. Then, the control position / orientation calculation unit 25 searches for the nearest point from the current position to the target route based on the current position / orientation and the target route. The position and orientation of the target path at this nearest point becomes the target position and orientation (X, Y, Θ). Thus, the target position and orientation and the current position and orientation are indicated by a vector having three elements. Various methods can be used for the nearest point search process. In this example, the division method is used. A method for calculating the nearest point will be described later.

  The turning speed determination unit 26 determines a turning speed (turning angular speed) based on the target position / posture and the current position / posture. A drive control unit 21 described later controls the drive mechanism 14 so that the moving body 10 moves at this turning speed. Here, consider a mobile robot coordinate system as shown in FIG. That is, consider a coordinate system in which the position of the moving body 10 is the origin and the traveling direction of the moving body 10 is the x-axis. In the moving plane of the moving body 10, the axis perpendicular to the x axis is the y axis. The target position and orientation in this coordinate system is (Δx, Δy, Δθ) as shown in FIG. Here, (Δx, Δy) is a position error of the current position with respect to the nearest point of the target route. Δθ is a posture error between the posture at the nearest point on the target route and the current posture. Note that the target posture vector in FIG. 3 indicates the control position and posture, that is, the position and posture at the nearest point.

Here, when appropriate gains K _θ , K _y , D _θ , and D _y are set, the turning speed ω can be calculated by Equation 5 below.

The gains K _θ , K _y , D _θ , and D _y are stored in the set value storage unit 27. The set value storage unit 27 is, for example, a storage device such as a RAM, and stores set values. Therefore, the turning speed determination unit 26 refers to the gains K _θ , K _y , D _θ , and D _y stored in the set value storage unit 27 and calculates the turning speed according to the position and orientation error. Furthermore, the gains K _θ , K _y , D _θ , and D _y may be variable according to conditions. In this way, the turning speed determination unit 26 determines the turning speed ω based on the error between the target position and posture and the current position and posture. In this way, the turning speed determination unit 26 determines the turning speed ω based on the current position and orientation estimated by the current position and orientation estimation unit 24 and the shape of the target route. Specifically, the control position / orientation calculation unit 25 calculates the closest point of the target route with respect to the current position / orientation based on the shape of the target route. Then, the control position and orientation calculation unit 25 calculates the coordinates of the nearest point and the direction of the target route at the nearest point as the control position and orientation. Then, the turning speed determination unit 26 determines the turning speed ω by feedback control according to the position and orientation error between the control position and orientation and the current position and orientation.

  The straight speed determination unit 28 determines the straight speed based on the turning speed ω determined by the turning speed determination unit 26. Here, assuming that the mass of the moving body is m, the moment of inertia of the moving body is I, and the straight traveling speed of the moving body is v, the straight traveling speed determination unit 28 calculates the straight traveling speed so as to satisfy the following Expression 6.

Here, C in Expression 6 is an arbitrary constant. Left side of Equation 6, the kinetic energy mv ^2/2 by the linear motion, the sum of the kinetic energy I [omega] ^2/2 by the rotational motion. In other words, the left side of Equation 6 indicates the kinetic energy of the moving body 10. The straight traveling speed determination unit 28 limits the straight traveling speed v so that the kinetic energy of the moving body 10 is constant at a constant C. Thus, the straight traveling speed v is determined so as to satisfy the law of conservation of kinetic energy. From Formula 6, it can be seen that the straight traveling speed v is expressed by Formula 7.

  Thus, the straight traveling speed v can be determined according to the turning speed ω. Here, the mass m and the moment of inertia I of the moving body 10 can be calculated from CAD data at the time of designing the moving body 10. In other words, the mass m and the moment of inertia I of the moving body 10 are calculated based on the mass and arrangement of each component constituting the moving body 10. The mass m and the moment of inertia I of the moving body 10 are stored in the set value storage unit 27. Further, the constant C in Expression 6 is also stored in the set value storage unit 27. The constant C is set to a value that prevents the mobile body 10 from slipping or skidding during movement. Also, the constant C can be made variable according to the conditions.

  In this way, the straight traveling speed determination unit 28 calculates the straight traveling speed v based on the turning speed ω determined by the turning speed determination unit 26 and each set value (mass m, inertia moment I, and constant C). . Here, the relationship between the turning speed ω and the straight traveling speed v will be described with reference to FIG. FIG. 4 is a graph when the horizontal axis is the turning speed ω and the vertical axis is the straight traveling speed v. That is, FIG. 4 is a graph showing Equation 7. As shown in Equation 7, the graph showing the relationship between the straight traveling speed v and the turning speed ω is a half ellipse.

  By controlling to satisfy Expression 6, when the turning speed ω changes, the straight traveling speed v changes gently. A sudden change in acceleration can be prevented. As described above, the straight traveling speed determination unit 28 determines the straight traveling speed v and the turning speed ω so that the kinetic energy is constant at the constant C. That is, the moving body 10 is moved so as to satisfy the law of conservation of kinetic energy. Thereby, it is possible to prevent the acceleration from becoming discontinuous when the turning speed ω changes. Therefore, it is possible to prevent the rotational speed of the motor of the drive mechanism 14 from changing suddenly and to move the moving body 10 smoothly. Therefore, energy efficiency can be improved.

  The drive control unit 21 controls the drive mechanism 14 for driving the drive wheels 12. Specifically, the wheel rotation speed is calculated based on the turning speed ω determined by the turning speed determination unit 26 and the straight traveling speed v determined by the straight traveling speed determination unit 28. For example, the drive control unit 21 calculates the rotational speeds of the left and right wheels by solving inverse kinematics (IK). That is, the drive control unit 21 calculates the rotational speeds of the two drive wheels 12 from the turning speed and the straight traveling speed of the moving body 10. Then, the rotational speed and rotational direction of the motor provided in the drive mechanism 14 are controlled from the rotational speeds of the left and right drive wheels 12. That is, the drive control unit 21 outputs drive signals to the left and right drive mechanisms 14, respectively. As a result, the moving body 10 moves at the turning speed ω determined by the turning speed determination unit 26 and the straight traveling speed v determined by the straight traveling speed determination unit. Furthermore, the moving body 10 moves so as to follow the target route. In this way, the drive control unit 21 performs feedback control based on the current position and orientation.

  The control unit 15 stores, for example, a position / orientation estimation program, a route generation program, a turning speed calculation program, a straight traveling speed calculation program, a wheel angle calculation program, a drive control program, and the like. The above processing is performed by executing these programs.

  Next, a method for controlling the moving body 10 according to the present embodiment will be described with reference to FIG. FIG. 5 is a flowchart showing a method for controlling the moving body 10 according to the present embodiment. First, the route generation unit 22 generates a target route (step S101). That is, a target route to the target position / posture is generated.

  Then, the current position and orientation of the moving body 10 are acquired (step S102). Here, the current position / orientation estimation unit 24 calculates the current position / orientation of the moving body 10 based on the measurement value from the sensor 18. Thereby, the current position and orientation are estimated.

  Further, the control position / orientation calculation unit 25 generates a control position / orientation for performing feedback control of the moving body 10 (step S103). That is, the control position / orientation calculation unit 25 calculates the nearest point from the current position / orientation estimated by the current position / orientation estimation unit 24 in the target route. Then, the position of the nearest point and the direction of the target route at the nearest point are generated as the control position / posture.

  Here, a method of calculating the nearest point on the target route will be described with reference to FIGS. 6, 7, and 8. FIG. 6 is a flowchart showing the nearest point search process, and FIGS. 7 and 8 are explanatory diagrams for explaining the nearest point search process.

  FIG. 6 shows details of the nearest point search process. As shown in FIG. 7, when the nearest point on the target path 100 is calculated when the moving body 10 is at the point A, the distance between the coordinates of the node n and the point A is obtained (S201). It is determined whether the distance obtained in this way is the shortest distance between the points A calculated in the past and any of the nodes (S202). As a result of the determination, if it is determined that the distance is the minimum distance, the node n is stored (S203). As a result of the determination, when it is determined that the distance is not the minimum, or when the node n is stored in step S203, it is determined whether all the nodes have been examined (S204).

As a result of the determination, if it is determined that not all the nodes have been examined, the process of step S201 is further executed for the node obtained by adding 1 to n. As a result of the determination, if it is determined that all nodes have been examined, of the two nodes (adjacent nodes) adjacent to the node having the smallest distance (minimum node) between the minimum nodes An adjacent node having a smaller distance is selected, and a search range is set between the selected adjacent node and the minimum node (S206). Then, as illustrated in FIG. 8, the midpoint between which is a search range node to the start point P _0, the search range w1 be the half nodal distance L (S207). In the example shown in FIG. 8, from the start point P ₀ to (1/2) w1, that is, from the point P _L1 extended to the n point side by (1/4) L, from the start point P ₀ to (1/2) w1, That is, the range up to the point _PR1 extended to the (n + 1) point side by (1/4) L is set as the search range w1. Then, the start point _{P 0} (ie, n is the case _{P 0),} and the node _{P L1} of n- (1/2) w1, n + (1/2) which of w1 node _{P R1} closer to the point A Is determined (S208). As a result of the determination, it is determined whether the closest node is the vicinity point P, and further whether it is close to the point A using w2 that is half of w1 from the vicinity point P as a search range, and the same processing is repeated (S209). As a result of such processing, when convergence has occurred, the neighboring point P determined to be closest to the point A as a result of convergence is identified as the closest point, and its coordinates are obtained. In this way, the position of the nearest point on the target route is searched. Then, the coordinates of the nearest point and the direction of the target route 100 at the nearest point are stored as the control position and posture.

  Next, the turning speed determination unit 26 generates the turning speed ω of the moving body 10 based on the control position and orientation (step S104). According to Equation 5, the turning speed ω is calculated from the position and orientation error. Thereby, the turning speed ω corresponding to the current position and orientation and the target route is calculated. Therefore, the moving body 10 is feedback controlled so as to approach the target route. That is, the posture of the moving body 10 is changed by the turning speed ω. Then, the moving body 10 moves toward the target route. When calculating the turning speed ω, the gain stored in the set value storage unit 27 is referred to.

  The straight speed determination unit 28 generates a straight speed v based on the turning speed ω (step S105). The moving body 10 is feedback controlled so as to move at the straight traveling speed v and the turning speed ω. In the present embodiment, the straight traveling speed v is determined so that the kinetic energy of the moving body 10 is always maintained at a constant C. In other words, the straight traveling speed v corresponding to the turning speed ω is calculated by Expression 7. When calculating the straight traveling speed v, the mass m, the moment of inertia I, and the constant C stored in the set value storage unit 27 are referred to. As described above, the straight traveling speed determination unit 28 determines the straight traveling speed v according to the current position and orientation and the target route. That is, the drive control unit 21 determines the turning speed ω and the straight traveling speed v of the moving body 10 based on the position and orientation error between the current position and orientation and the target route.

  Then, the drive control unit 21 calculates the rotational speed of the left and right drive wheels 12 from the turning speed ω and the straight traveling speed v (step S6). That is, by solving the inverse kinematics, the velocities Vr and Vl by the left and right drive wheels 12 are respectively calculated. Then, the rotational speed is calculated according to the speeds Vr and Vl of the left and right drive wheels 12, the radius of the drive wheels 12, and the like. The drive control unit 21 drives a motor provided in the drive mechanism 14 so that the left and right drive wheels 12 rotate at the calculated rotation speed. Therefore, the linear velocity v of the moving body 10 is limited by kinetic energy. Thereby, the drive mechanism 14 is driven, and the moving body 10 moves at the turning speed ω and the straight traveling speed v (step S107).

  Further, the above process is repeated to control the moving body 10. That is, estimation of current position and orientation (step S102), calculation of control position and orientation (step S103), determination of turning speed (step S104), determination of straight speed (step S105), wheel rotation speed determination (step S106), and The movement of the robot (step S107) is repeated. In this way, the position and orientation estimation based on the measurement value of the sensor 18 is repeatedly performed. Then, the rotational speeds of the left and right drive wheels 12 are determined according to the current position and orientation and the nearest point on the target route. The above feedback is repeated to control the movement of the moving body 10. Thereby, feedback control is performed so that the moving body 10 follows the target route.

  Thus, in this embodiment, after determining the turning speed ω, the straight traveling speed v is calculated based on the turning speed ω. Further, the linear velocity v is limited by kinetic energy. That is, the straight traveling speed v is calculated so that the kinetic energy is constant at a constant C regardless of the calculated turning speed ω. Accordingly, the moving body 10 can always be moved with constant kinetic energy, so that the movement is smooth.

Embodiment 2 of the Invention
The moving body 10 according to the present embodiment differs from the first embodiment in part of the determination of the straight traveling speed. Since the configuration and contents other than the determination of the straight-ahead speed are the same as those in the first embodiment, description thereof is omitted. The moving body 10 according to the present embodiment adds not only a limitation due to kinetic energy but also a limitation due to centrifugal force to the straight traveling speed v. The relationship between the turning speed ω and the straight traveling speed v of the moving body 10 according to the present embodiment will be described with reference to FIG. FIG. 9 is a graph showing the relationship between the turning speed ω and the straight traveling speed v. FIG. 9A is a graph showing the relationship between the straight traveling speed v and the turning speed ω when only the limitation due to the centrifugal force is considered. FIG. 9B is a graph showing the relationship between the straight traveling speed v and the turning speed ω when only the limitation due to kinetic energy is considered. FIG. 9C is a graph showing the relationship between the straight traveling speed v and the turning speed ω of the moving body 10 according to the present embodiment.

  In the present embodiment, when the turning speed ω is within a certain range, a limitation by kinetic energy is added as in the first embodiment. That is, the straight traveling speed v is calculated so that the kinetic energy is constant at a constant C. On the other hand, when the turning speed ω is out of a certain range, a restriction by centrifugal force is added.

  Here, a method of calculating the straight traveling speed v when a limitation due to centrifugal force is applied will be described. The centrifugal force applied to the moving body 10 is shown as f = mvω. The curvature κ can be calculated from the turning speed ω. As m, a value stored in the set value storage unit 27 can be used.

Then, mvω ≦ f _max is set so that the centrifugal force is within the threshold value. The maximum value f _max of the centrifugal force is set so that the moving body 10 does not slip or skid due to the centrifugal force, for example. Here, the maximum value f _max of the centrifugal force is stored in the set value storage unit 27. The restriction on the straight traveling speed v is given by v ≦ f _max / mω. The straight traveling speed v is given by v = min (v ₀ , f _max / mω). Here, v ₀ is a value preset in the set value storage unit 27. When the straight traveling speed calculated based on the turning speed ω exceeds f _max / mω, a restriction is provided so that the straight traveling speed becomes v ₀ .

Considering only the restriction due to the centrifugal force, the relationship between the straight traveling speed v and the turning speed ω is as shown in FIG. That is, when the turning speed ω is in the vicinity of 0, the straight traveling speed v is constant at v ₀ , and in other cases, the straight traveling speed v and the turning speed ω are in an inversely proportional relationship. On the other hand, when the limitation by kinetic energy is added, the relationship between the straight traveling speed v and the turning speed ω is shown in FIG. 9B as in the first embodiment. That is, the straight traveling speed v is calculated from the turning speed ω so as to satisfy Expression 6.

In the present embodiment, the limitation by kinetic energy and the limitation by centrifugal force are combined. Therefore, the relationship between the straight traveling speed v and the turning speed ω is as shown in FIG. The straight traveling speed v is the smaller of the straight traveling speed calculated from the restriction by kinetic energy and the straight traveling speed calculated from the restriction by centrifugal force. Here, the constant C is set so that the straight traveling speed when ω = 0 is smaller than v ₀ . Therefore, in a range where the turning speed ω is close to 0, the straight traveling speed v is calculated so that the kinetic energy is constant. Further, when the absolute value of the turning speed ω increases from near 0, the straight traveling speed v is calculated so that the centrifugal force becomes constant. That is, in this range, the relationship between the turning speed ω and the straight traveling speed v is inversely proportional. When the absolute value of the turning speed ω is further increased, the straight traveling speed v is calculated so that the kinetic energy becomes constant. When the turning speed reaches a certain value, the straight traveling speed v becomes 0, and the vehicle turns on the spot. Therefore, the relationship between the turning speed ω and the straight traveling speed v of the moving body 10 according to the present embodiment is indicated by a thick line in FIG.

  In the present embodiment, when the turning speed ω is within a certain range, the straight traveling speed v is calculated from the turning speed ω so that the kinetic energy becomes constant. Further, when the turning speed ω is out of a certain range, the straight traveling speed v is calculated from the turning speed ω so that the centrifugal force becomes constant. Since the centrifugal force f = mωv, the relationship between the turning speed ω and the straight traveling speed v is inversely proportional. In other words, the straight traveling speed determination unit 28 selects one of the restriction by kinetic energy or the restriction by centrifugal force according to the value of the turning speed ω. Then, the straight traveling speed v is calculated according to the selected restriction. As described above, in the present embodiment, the straight traveling speed v is calculated from the turning speed ω so that the kinetic energy is constant within a certain range. On the other hand, when the turning speed ω is out of a certain range, the straight traveling speed v is calculated from the turning speed ω so that the centrifugal force f becomes constant. When the turning speed ω is out of a certain range, the straight traveling speed v may be determined by a restriction other than the centrifugal force.

  In the first embodiment, the kinetic energy is constant over the entire range of the turning speed ω. That is, the kinetic energy is constant regardless of the determined turning speed ω. On the other hand, in the present embodiment, the kinetic energy is constant only when the turning speed ω is within a certain range. In the present invention, when the turning speed ω determined by the turning speed determination unit 26 falls within at least a part of the range, the straight speed determination unit 28 goes straight from the turning speed ω so that the kinetic energy is constant. What is necessary is just to determine the speed v. Further, in the second embodiment, when the turning speed ω is out of the range, the restriction by the centrifugal force is added. That is, the straight traveling speed v is calculated from the turning speed ω so that the centrifugal force f is constant.

In the present embodiment, a rapid change in the acceleration of the moving body 10 can be mitigated. Therefore, the moving body 10 can be smoothly moved as compared with the prior art. Furthermore, since the restriction | limiting by a centrifugal force is added, a skid and a slip can be prevented. That is, the constant C, and v ₀ to are appropriately set, it is possible to prevent skidding, the generation of the slip. Furthermore, in the present embodiment, the constant C can be made larger than that in the first embodiment, and it is possible to move at a high straight traveling speed v.

Embodiment 3 of the Invention
A moving body 10 according to the present embodiment will be described with reference to FIG. FIG. 10 is a side view schematically showing the configuration of the moving body 10 according to the present embodiment. The movement control of the moving body 10 according to the present embodiment is basically the same as that of the first embodiment. Therefore, the description of the same configuration and contents as those in Embodiment 1 is omitted. The moving body 10 according to the present embodiment is a robot that performs a task of gripping an object.

  A hand 33 for holding the object 40 is attached to the vehicle body 11 of the moving body 10. By closing the hand 33, the target object 40 is gripped. An arm 32 is attached to the vehicle body 11 via a rotation mechanism 31. Further, the arm 32 has a linear actuator, is provided so as to be extendable and contractible, and a hand 33 is provided at the tip of the arm 32. The rotation mechanism 31 can rotate the arm 32 by a predetermined angle.

  When the object 40 is gripped, the rotation mechanism 31 and the arm 32 are driven. For example, the position of the object 40 is recognized by a camera such as a CCD camera, and the hand 33 is moved to a grippable position. As a result, the hand 33 moves to a position where the object 40 can be gripped. Then, the hand 33 is closed and the object 40 is gripped. When the object 40 is gripped, the mass of the moving body 10 changes.

The hand 33 is provided with a force sensor 35. The force sensor 35 detects the force applied to the hand 33. Thereby, the mass of the object 40 can be measured. Then, arithmetic processing is performed by adding the mass of the object 40. That is, the straight traveling speed v is calculated by adding the mass of the grasped object 40 to the mass of the moving body 10 main body. For example, if the mass of the main body of the moving body 10 is m ₁ and the mass of the measured object 40 is m ₂ , the total mass of the moving body 10 is m = m ₁ + m ₂ . In this case, the mass changes by m ₂ before and after gripping the object. Then, the value of the mass m stored in the set value storage unit 27 is rewritten. Based on this m, the straight traveling speed v is calculated from the above equation 7. That is, the straight traveling speed may be determined from the kinetic energy in a state where the mass of the moving body 10 is increased by the mass of the object 40. Furthermore, not only the mass m but also the moment of inertia I may be changed. That is, the inertia moment I may be calculated from the mass of the object 40 and the grip position. As a result, a straight traveling speed more suitable for movement can be calculated, and smooth movement becomes possible.

  Further, when the mass of the entire moving body 10 changes, the value of the constant C may be changed. That is, the value of the constant C stored in the set value storage unit 27 is rewritten. Thus, the straight traveling speed v can be determined so that the kinetic energy becomes constant at different values before and after gripping the object 40. Thereby, it can be made to move at a suitable straight speed with respect to the mass m. Therefore, even when the object 40 is gripped, it can be moved stably. Normally, when the mass increases while holding the object, the constant C is increased.

  Furthermore, when the hand 33 is spread and the gripping of the object 40 is released, it is possible to return to the original mass and determine the straight traveling speed. That is, the straight traveling speed may be determined from the kinetic energy in a state where the mass of the moving body 10 is reduced by the mass of the object 40. Thus, the mass of the moving body is increased or decreased according to the change in mass measured by the mass measuring instrument. Then, the straight traveling speed v may be determined so that the kinetic energy is constant while the mass of the moving body 10 is changed. Thereby, appropriate control can be performed. Of course, when the holding of the object 40 is released, the constant C may be restored. That is, the constant C may be changed according to the mass measured by the mass measuring instrument.

  In the present embodiment, as described above, after holding an object, the mass of the object is measured. Then, the straight traveling speed is determined after adding the mass of the object. Thereby, it can be moved more smoothly. Furthermore, if the constant C is changed according to the mass of the object, more appropriate control can be performed. Moreover, you may add the restriction | limiting by centrifugal force similarly to Embodiment 2. FIG.

  In the above description, the mass of the moving body 10 is changed by gripping the object 40. However, the present embodiment is not limited to this. For example, a mechanism for attracting an object may be provided. That is, a holding mechanism for holding the object may be provided. Further, the mass measuring device for measuring mass is not limited to the force sensor. That is, it is only necessary to provide a mass measuring device that measures the mass of the object held by the holding mechanism. And it can be made to move more smoothly by adding the mass of the object 40 to the mass of the moving body 10, and calculating a straight-ahead speed.

  Further, when a task processing mechanism for executing a predetermined task is provided in the moving body 10, the mass of the moving body 10 may change before and after the task processing. In this case, similarly to the above, the mass of the moving body 10 is changed according to the mass change before and after executing the task. That is, the mass m of the moving body 10 is increased or decreased by the amount that the mass has changed due to execution of the task. Then, the straight traveling speed is determined so that the kinetic energy is constant in a state where the mass is changed. That is, the straight traveling speed is determined from Equation 6 using the increased mass or the decreased mass. Thereby, smoother movement becomes possible. Of course, you may provide the mass measuring device which measures the mass which changes with processing of a task. In addition, when the mass which changes with execution of a task is decided, it is not necessary to provide a mass measuring device. That is, the mass of the moving body 10 before and after the task processing may be stored and appropriately selected. Furthermore, the constant speed may be determined by changing the constant C between the state after the task is executed and the state before the task is executed so that the kinetic energy becomes constant at different values.

Other embodiments.
In the first and second embodiments, the moving body 10 is applied to a two-wheeled moving body. However, the moving body 10 is not limited to this, and can be applied to a four-wheeled robot, for example, more than two wheels. It can also be applied to robots of the above type, bipedal walking and quadruped walking. In the case of a walking robot, it is possible to follow the target route by changing the moving direction by controlling the stride. Furthermore, the present invention is not limited to a robot and can be applied to a two-wheeled or four-wheeled vehicle.

It is a figure which shows the structure of the moving body concerning Embodiment 1 of this invention. It is a block diagram which shows the structure of the control part of the moving body concerning Embodiment 1 of this invention. It is a figure for demonstrating the coordinate system used with the control method of the moving body concerning Embodiment 1 of this invention. It is a figure which shows the relationship between the turning speed in the mobile body concerning Embodiment 1 of this invention, and a rectilinear speed. It is a flowchart which shows the control method of the moving body concerning Embodiment 1 of this invention. It is a flowchart which shows the search process of the nearest point in the mobile body of Embodiment 1 of this invention. It is explanatory drawing for demonstrating the search process of the nearest point in the mobile body of Embodiment 1 of this invention. It is explanatory drawing for demonstrating the search process of the nearest point in the mobile body of Embodiment 1 of this invention. It is a figure which shows the relationship between the turning speed of the moving body concerning Embodiment 2 of this invention, and a rectilinear speed. It is a figure which shows the structure of the moving body concerning Embodiment 3 of this invention. It is a figure which shows the relationship between the turning speed and the straight traveling speed in the conventional mobile robot.

Explanation of symbols

10 moving bodies, 11 vehicles, 12 driving wheels, 13 driven wheels, 14 driving mechanisms,
15 control unit, 18 sensor, 21 drive control unit, 22 path generation unit,
24 current position and orientation estimation unit, 25 control position and orientation calculation unit, 26 turning speed determination unit,
27 Setting Value Storage Unit, 28 Linear Speed Determination Unit, 31 Rotating Mechanism 32 Arm, 33 Hand, 35 Mass Sensor, 40 Object

Claims (14)

A moving body having a control unit that performs feedback control so as to follow a target path,
The control unit is
A position and orientation estimation unit for estimating the position and orientation of the mobile body;
A turning speed determination unit that determines a turning speed based on the position and orientation estimated by the position and orientation estimation unit and the shape of the target path;
A rectilinear speed determining unit that determines the rectilinear speed of the moving body from the turning speed determined by the turning speed determining unit so that the kinetic energy of the moving body is constant;
A moving body comprising: a driving control unit that controls a driving mechanism of the moving body based on the turning speed determined by the turning speed determining unit and the straight traveling speed determined by the straight traveling speed determining unit. The straight speed determining unit determines the straight speed so as to satisfy Formula 1,
The moving body according to claim 1, wherein the kinetic energy of the moving body indicated by the left side of Formula 1 is constant at a constant C.

(Where m is the mass of the moving body, I is the moment of inertia of the moving body, ω is the turning speed of the moving body, v is the straight traveling speed of the moving body, and C is a constant.)   The moving body according to claim 1 or 2, wherein the straight traveling speed determining section determines the straight traveling speed so that the kinetic energy of the moving body is constant regardless of the turning speed determined by the turning speed determining section.   The linear travel speed determination unit determines the linear travel speed so that the kinetic energy of the moving body is constant when the rotational speed determined by the rotational speed determination unit is within a certain range. The moving body described.   The moving body according to claim 4, wherein when the turning speed is out of the certain range, the rectilinear speed determining unit determines the rectilinear speed so that a centrifugal force of the moving body becomes constant. A task processing mechanism for executing a predetermined task;
The movement according to any one of claims 1 to 5, wherein the straight traveling speed is determined so that the kinetic energy is constant in a state in which the mass of the moving body is changed according to a mass change before and after the task is executed. body.   The moving body according to claim 6, wherein the straight traveling speed is determined so that the kinetic energy becomes constant at a different value between a state after the task is executed and a state before the task is executed. A control method for feedback control of movement of a moving body that follows a target route,
Estimating the position and orientation of the moving body;
Determining a turning speed based on the estimated position and orientation and the target route;
Determining a straight traveling speed of the moving body from the determined turning speed so that the kinetic energy of the moving body is constant;
And a step of controlling a driving mechanism of the moving body based on the determined turning speed and the determined straight traveling speed. In the step of determining the straight traveling speed, the straight traveling speed is determined so as to satisfy Formula 2,
The method for controlling a moving body according to claim 8, wherein the kinetic energy of the moving body indicated by the left side of Formula 2 is constant at a constant C.

(Where m is the mass of the moving body, I is the moment of inertia of the moving body, ω is the turning speed of the moving body, v is the straight traveling speed of the moving body, and C is a constant.)   The method for controlling a moving body according to claim 8 or 9, wherein in the step of determining the straight traveling speed, the straight traveling speed is determined so that the kinetic energy of the moving body is constant regardless of the determined turning speed.   The movement according to claim 8 or 9, wherein in the step of determining the straight traveling speed, the straight traveling speed is determined so that the kinetic energy of the moving body is constant when the determined turning speed is within a certain range. How to control the body.   The step of determining the straight traveling speed determines the straight traveling speed so that the centrifugal force of the moving body becomes constant when the turning speed is out of the certain range. Control method. The method further includes a step of performing a predetermined task,
The movement according to any one of claims 8 to 12, wherein the straight traveling speed is determined so that the kinetic energy is constant in a state where the mass of the moving body is changed in accordance with a mass change before and after the task is executed. How to control the body.   The method according to claim 13, wherein the straight traveling speed is determined so that the kinetic energy becomes constant at different values in a state after the task is executed and a state before the task is executed.

Images