JP6468127B2 - Omnidirectional moving body, control method and program thereof - Google Patents

Description

  The present invention comprises an omnidirectional mobile body including a trolley part provided with a plurality of drive wheels and a main body part rotatably provided on the trolley part via a turning shaft, and its control It relates to a method and a program.

  A moving body that controls a drive steering system by performing a speed plan using a minimum speed value as a speed command from values of each allowable speed specified by route conditions (distance and road surface condition) in local map data is known. (See Patent Document 1).

JP 2010-039839 A

  By the way, in the omnidirectional mobile body, there is a limit to the output performance of the actuator that drives the drive wheel of the carriage and the turning shaft of the main body. This limits the driving torque and rotational speed of the driving wheels and the turning shaft. Further, if the omnidirectional mobile body is accelerated and moved, slip occurs between the drive wheels and the road surface. However, the moving body is not controlled in consideration of the output performance of such an actuator.

  The present invention has been made in view of such problems, and provides an omnidirectional mobile body capable of performing optimal control in consideration of output performance of an actuator, a control method thereof, and a program. Main purpose.

In one aspect of the present invention for achieving the above object, a cart unit provided with a plurality of drive wheels, a main body unit rotatably provided on the cart unit via a turning shaft, and driving the drive wheels An omnidirectional mobile body comprising first drive means and second drive means for driving the swivel axis, the omnidirectional movable body being movable in all directions, the motion relating to the center of gravity and the vertical axis of the bogie part and the main body part Based on the equation, an equation of motion relating to the torque of the driving wheel and the turning shaft including the inertia term, centrifugal force and Coriolis term, viscous friction term, and gravity term is calculated, and the equation of motion is preset. The omnidirectional mobile object is a trajectory to be moved and is a condition for restricting the torque of the drive wheel and the turning shaft based on the command trajectory expressed by the trajectory parameter, and expressed by the trajectory parameter. Torque constraint condition is calculated A condition for restricting the speed of the drive wheel and the turning axis based on the preset restriction on the speed of the driving wheel and the turning axis, and the speed restriction condition expressed by the trajectory parameter is calculated, The trajectory that satisfies the calculated torque constraint condition and speed constraint condition and that follows the command trajectory and that is expressed on the plane of the trajectory parameter and its differential value is calculated and calculated. Control means for calculating a target value of the position and orientation of the main body based on the calculated trajectory, and controlling the first and second drive means based on the calculated target value of the position and orientation of the main body. It is a featured omnidirectional mobile.
In this aspect, the calculated trajectory may be a shortest time trajectory that satisfies the calculated torque constraint condition and speed constraint condition and follows the command trajectory in the shortest time.
In this aspect, the calculated trajectory is based on the calculated torque constraint condition and speed constraint condition, and the forward integration and the backward integration are performed with a predetermined integration width from the start point and the end point in the plane of the trajectory parameter and its differential value. May be the shortest time trajectory calculated by searching for a switching point where the minimum acceleration is switched to the maximum acceleration by repeating the above.
In one aspect of the present invention for achieving the above object, a cart unit provided with a plurality of drive wheels, a main body unit rotatably provided on the cart unit via a turning shaft, and driving the drive wheels A control method for an omnidirectional mobile body comprising first drive means and second drive means for driving the pivot axis, wherein the omnidirectional mobile body is movable in all directions, and the center of gravity and vertical axis of the carriage and main body Based on an equation of motion related to the surroundings, an equation of motion relating to the torque of the driving wheel and the turning shaft including an inertial term, a centrifugal force and a Coriolis term, a viscous friction term, and a gravity term is calculated, The trajectory parameter is a condition for restricting the torque of the drive wheel and the turning axis based on a command trajectory expressed by a trajectory parameter that is a trajectory to which the omnidirectional mobile body is set in advance. Torque constraint expressed in And a condition for restricting the speed of the driving wheel and the turning axis based on the preset restriction on the speed of the driving wheel and the turning axis, and the speed restriction condition expressed by the trajectory parameter A trajectory that satisfies the calculated torque constraint condition and speed constraint condition and that follows the command trajectory, the trajectory expressed on the plane of the trajectory parameter and its differential value A calculating step;
Calculating a target value of the position and orientation of the main body based on the calculated trajectory, and controlling the first and second driving means based on the calculated target value of the position and orientation of the main body. The control method of the omnidirectional mobile body characterized by this may be used.
In one aspect of the present invention for achieving the above object, a cart unit provided with a plurality of drive wheels, a main body unit rotatably provided on the cart unit via a turning shaft, and driving the drive wheels A program for an omnidirectional mobile body comprising first driving means and second driving means for driving the swivel axis, the omnidirectional moving body being movable in all directions, the center of gravity of the bogie part and the body part, and the vertical axis around A motion equation relating to the torque of the drive wheel and the turning shaft, including an inertial term, a centrifugal force and a Coriolis term, a viscous friction term, and a gravity term. Based on the set trajectory that the omnidirectional mobile body should move and the command trajectory expressed by the trajectory parameter, it is a condition that restricts the torque of the drive wheel and the turning axis, and the trajectory parameter Expressed torque constraint A condition for restricting the speed of the drive wheel and the turning axis based on a predetermined restriction on the speed of the driving wheel and the turning axis, and a speed restriction expressed by the trajectory parameter Calculate the conditions, and calculate the trajectory that satisfies the calculated torque constraint condition and speed constraint condition and that follows the command trajectory and that is expressed on the plane of the trajectory parameter and its differential value And a process of calculating a target value of the position and orientation of the main body based on the calculated trajectory, and controlling the first and second driving means based on the calculated target value of the position and orientation of the main body And a program that causes a computer to execute.

  ADVANTAGE OF THE INVENTION According to this invention, the output performance of an actuator can be considered and the omnidirectional mobile body which can perform optimal control, its control method, and a program can be provided.

(A) It is a top view which shows schematic structure of the omnidirectional mobile body which concerns on one Embodiment of this invention. (B) It is a front view which shows schematic structure of the omnidirectional mobile body which concerns on one Embodiment of this invention. (C) It is a side view which shows schematic structure of the omnidirectional mobile body which concerns on one Embodiment of this invention. It is a block diagram which shows the schematic system configuration | structure of the omnidirectional mobile body which concerns on one Embodiment of this invention. It is a block diagram showing a schematic system configuration of a control device concerning one embodiment of the present invention. It is a figure which shows MVC, VLC, and the shortest time orbit on an orbital parameter ss (dot) plane. It is a figure which shows the definition of the coordinate system of the omnidirectional mobile body which concerns on one Embodiment of this invention. It is a flowchart which shows the flow of the control method of the omnidirectional mobile body which concerns on one Embodiment of this invention. It is a figure which shows a command track. It is a figure which shows the drive torque of a left drive wheel. It is a figure which shows the drive torque of a right drive wheel. It is a figure which shows the drive torque of a rotating shaft. It is a figure which shows the track | orbit on a track | orbit parameter ss (dot) plane when the drive torque of a left-right drive wheel and a turning shaft is restrained. It is a figure which shows an example of the smooth | smooth and realizable trajectory, passing under the MVC and VLC curve.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.
1A to 1B are diagrams showing a schematic configuration of an omnidirectional mobile body according to an embodiment of the present invention. The omnidirectional mobile body 1 according to the embodiment of the present invention is configured as, for example, an active caster omnidirectional mobile robot.

  The omnidirectional mobile body 1 includes a cart unit 2 that moves and a main body unit 3 that is connected to the upper side of the cart unit 2 so as to be capable of turning. The carriage unit 2 is provided with one auxiliary wheel 21 and a pair of left and right drive wheels 22. The main body 3 rotates relative to the carriage unit 2 around a turning axis (Yaw rotation axis) 4. The drive shaft of the drive wheel 22 and the turning shaft 4 of the main body 3 are offset (do not intersect).

  The omnidirectional mobile body 1 realizes omnidirectional movement by driving the drive shaft and the turning shaft 4 independently. The omnidirectional mobile body 1 realizes omnidirectional movement by, for example, relative rotation of the main body 3 by the turning shaft 4 in addition to the rotation difference of the drive wheel 22 by the drive shaft. In addition, for example, an articulated arm that can grip an object may be rotatably provided in the main body 3. Accordingly, the object can be easily grasped and moved by operating the articulated arm while moving the omnidirectional mobile body 1 in an arbitrary direction.

FIG. 2 is a block diagram illustrating a schematic system configuration of the omnidirectional mobile body according to the present embodiment. The omnidirectional mobile body 1 according to this embodiment includes a pair of rotation sensors 5 that detect rotation information (rotation angle, rotation speed, rotation angle acceleration) of the left and right drive wheels 22 and rotation that detects rotation information of the turning shaft 4. A sensor 6, a pair of wheel actuators 7 that drive the pair of drive wheels 22, a turning actuator 8 that drives the turning shaft 4, and a control device 9 that controls the wheel actuator 7 and the turning actuator 8 are provided.
The control device 9 controls the wheel actuator 7 and the turning actuator 8 based on the rotation information detected by the rotation sensors 5 and 6.

  The control device 9 includes, for example, a CPU (Central Processing Unit) 9a that performs arithmetic processing and the like, a memory 9b that includes a ROM (Read Only Memory) and a RAM (Random Access Memory) in which an arithmetic program executed by the CPU 9a is stored, The hardware configuration is centered on a microcomputer including an interface unit (I / F) 9c for inputting / outputting signals to / from the outside. The CPU 9a, the memory 9b, and the interface unit 9c are connected to each other via a data bus or the like.

  Each rotation sensor 5 is provided, for example, on the drive shafts of the left and right drive wheels 22. Each rotation sensor 5 includes a potentiometer, an encoder, and the like. Each wheel actuator 7 is a specific example of the first drive means. Each wheel actuator 7 is connected to the drive shafts of the left and right drive wheels 22, for example. Each wheel actuator 7 is constituted by a motor or the like.

  The turning actuator 8 is a specific example of the second driving means. The turning actuator 8 is connected to the turning shaft 4, for example. Each turning actuator 8 is configured by a motor or the like. The wheel actuator 7 and the turning actuator 8 are driven to rotate in response to a control signal output from the control device 9.

By the way, there is a limit to the output performance of the driving wheel and the actuator that drives the turning shaft in the omnidirectional mobile body. This limits the driving torque and rotational speed of the driving wheels and the turning shaft. In addition, when the carriage unit is greatly accelerated and moved, slip occurs between the drive wheel and the road surface.
In particular, in the control of an omnidirectional mobile body, the output performance of the actuator is not limited only by the torque of the shaft, but the dynamic effect of the static friction that the drive wheel does not slip and the dynamic center of gravity of the omnidirectional mobile body are combined. It is unique in that it is rate-determined by an index such as ZMP (Zero Moment Point).

On the other hand, the omnidirectional mobile body 1 according to the present embodiment satisfies the above-described constraints on the drive torque and rotational speed of the drive wheels 22 and the turning shaft 4 and follows the set command trajectory in the shortest time. Execute control (Time Optimal Control).
Thereby, it is possible to perform the control in the shortest time while taking into consideration the output performance of the actuator including the unique point that is controlled by the static friction or the index such as ZMP.

Since the shortest time control is a well-known control method, detailed description is omitted. Details of the minimum time control are disclosed in, for example, non-patent literature: J. Bobrow, “Optimal robot path planning using the minimum-time criterion” Robotics and Automation, IEEE Journal vol.4, pp.443 (1988). In the present embodiment, this can be used as appropriate.
In general, an omnidirectional mobile body can be configured to be omnidirectionally movable with a low degree of freedom, and has advantages such as being not over-constrained and having a high load resistance. It is difficult to find a trajectory that satisfies the constraints of torque and speed. In the present embodiment, as described above, the shortest time control is applied to the control of the omnidirectional mobile body 1 so as to satisfy the constraint conditions of the drive torque and the rotational speed of the drive wheels 22 and the turning shaft 4. The shortest time trajectory that follows the command trajectory in the shortest time can be easily generated.

  FIG. 3 is a block diagram illustrating a schematic system configuration of the control device according to the present embodiment. The control device 9 according to the present embodiment includes a constraint condition calculation unit 91 that calculates torque constraint conditions and speed constraint conditions, a track calculation unit 92 that calculates the shortest time trajectory, and the wheel actuator 7 and the turn based on the shortest time trajectory. When the actuator 8 is controlled, a control unit 93 is provided.

  The constraint condition calculation unit 91 includes an inertia term, a centrifugal force and Coriolis term, a viscous friction term, and a gravity term based on the equations of motion related to the center of gravity and the vertical axis of the cart unit 2 and the main body unit 3. An equation of motion relating to the torque of the drive wheel 22 and the turning shaft 4 is calculated. Further, the constraint condition calculation unit 91 calculates the driving wheel 22 and the turn based on the equation of motion and the command trajectory expressed by the trajectory parameter, which is the trajectory to which the omnidirectional mobile body 1 is set in advance. A condition that constrains the torque of the shaft 4 and that indicates a torque constraint condition expressed by a trajectory parameter is calculated.

The restriction condition calculation unit 91 is a condition for restricting the speeds of the drive wheels 22 and the turning shaft 4 based on the preset restriction on the speeds of the driving wheels 22 and the turning shaft 4, and the speed expressed by the trajectory parameters. An expression indicating the constraint condition is calculated.
The constraint condition calculation unit 91 calculates the torque constraint condition and the speed constraint condition offline, but is not limited thereto, and the torque constraint condition and the speed constraint condition may be calculated online.

  The trajectory calculation unit 92 is a trajectory that satisfies the torque constraint condition and the speed constraint condition calculated by the constraint condition calculation unit 91 and follows the command trajectory in the shortest time. Is the shortest time trajectory expressed on a plane (hereinafter referred to as trajectory parameter s-s (dot) plane). The trajectory calculation unit 92 calculates the shortest time trajectory offline, but is not limited thereto, and the shortest time trajectory may be calculated online.

  The control unit 93 is a specific example of the control means. The control unit 93 calculates a target value of the position and orientation of the omnidirectional mobile body 1 based on the shortest time trajectory calculated by the trajectory calculation unit 92. The control unit 93 controls the wheel actuator 7 and the turning actuator 8 on the basis of the calculated target values of the position and orientation of the omnidirectional mobile body 1, so that the driving wheel 22 of the carriage unit 2 and the turning axis of the main body unit 3 are controlled. 4 is controlled.

  Next, a detailed description will be given of the torque constraint conditions of the drive wheels and the turning shaft of the omnidirectional mobile body. As described above, it is necessary to add a constraint condition to the torque of the drive wheel 22 and the turning shaft 4 in consideration of the output performance of the wheels and the turning actuators 7 and 8 that drive the driving wheel 22 and the turning shaft 4.

  Here, the omnidirectional mobile body 1 can be considered as a robot having a plurality of joints. That is, the degree of freedom of the main body 3 by the drive wheels 22 and the turning shaft 4 of the omnidirectional mobile body 1 can be regarded as a virtual robot joint. There are restrictions on the speed, acceleration, torque, etc. of the joints of the robot. This restriction is given by a curve such as a constant upper / lower limit value or a function. Here, consider a case where a command trajectory for the robot to operate is given as a spatial trajectory. In this case, it is assumed that the spatial trajectory can be expressed by a single trajectory parameter s (time parameterized), and the spatial trajectory is continuous up to the second order differential with respect to the trajectory parameter s.

  The joint angle of the robot is determined if one point on the spatial trajectory is designated. In addition, the speed and acceleration of a certain state of the robot can also be expressed by the speed s (dot) and acceleration s (2 dots) of the trajectory parameter s at corresponding points on the spatial trajectory.

  Based on the above assumptions, the constraints on the speed, acceleration, and torque of the joints of the robot can be expressed as inequalities regarding the trajectory parameters s, s (dots), and s (2 dots). The first-order differential value with respect to time s is expressed as s (dot), the second-order differential value with respect to time s is expressed as s (2 dots), and the other parameters thereafter are also expressed in the same manner.

If the inequality of the trajectory parameter is an expression such as the following formula (1), the speed trajectory for following the given spatial trajectory in the shortest time while satisfying all the constraints set for the joint is: It can be obtained according to an algorithm described later.

  However, in the above equation (1), n is the number of constraint conditions, and the upper limit and lower limit constraints are divided and counted as one each. Hereinafter, in the present embodiment, the above equation (1) is referred to as a standard form of the constraint condition of the shortest time control.

  In the shortest time control, the trajectory is searched on the trajectory parameter s-s (dot) plane. FIG. 4 is a diagram illustrating an example of the trajectory parameter s-s (dot) plane. In FIG. 4, the shortest time trajectory of the robot is a curve from the start point (s = 0) to the end point (s = 1).

  The standard form of the constraint condition of the shortest time control is expressed as an area divided by a curve on the trajectory parameter s-s (dot) plane. These curves include an MVC (Maximum Velocity Curve) indicating an unreachable region and a VLC (Velocity Limit Curve) indicating an upper limit of speed. The constraints on the speed, acceleration, and torque of the joints of the robot can be expressed by these MVC and VLC.

  The shortest time trajectory for following a given space trajectory in the shortest time passes under the MVC and VLC curves, for example, and accelerates or decelerates at the maximum or minimum acceleration at each time on the trajectory. Become. More specifically, the shortest time trajectory passes under the MVC and VLC curves, and one of the accelerations of the drive wheel 22 and the turning shaft 4 that can follow the spatial trajectory is the maximum acceleration or the minimum acceleration. It is an orbit like that. Such a shortest time trajectory is shown as, for example, Time optimal trajectory in FIG.

Next, for the omnidirectional mobile body 1 according to the present embodiment, the standard form of the constraint condition of the shortest time control shown in the above equation (1) is derived from the mechanism kinematics and the dynamic model of the omnidirectional mobile body 1.
FIG. 5 is a diagram showing the definition of the coordinate system of the omnidirectional mobile body according to the present embodiment. Here, the world coordinate system is Σ ₀ . A carriage coordinate system Σ ₁ is set at the axle center of the drive wheel 22, and the front direction of the carriage unit 2 is taken as the x-axis. A main body coordinate system Σ ₂ is set at the rotation axis center (swivel axis 4) of the main body 3, and the front direction of the main body 3 is the x axis. In the world coordinate system Σ ₀ , the carriage coordinate system Σ ₁ , and the main body coordinate system Σ ₂ , the vertical axis (vertical axis) is the z axis. In the following equations, the relative transformation matrix from the coordinate system Σ _n to Σ _m is described as a reference coordinate system subscript at the upper left and the target coordinate system subscript at the lower right, as in ⁿ T _m . The relative position from the coordinate system Σ _n to Σ _m is described as ⁿ p _m , the relative angle is described as ⁿ θ _m, and other parameters (speed, force, torque, etc.) are also described in the same manner. If omitted, 0 (world coordinate system reference) is assumed. The vector is bold as v ₁ and its (x, y, z) elements are described as (v _{1, x} , v _{1, y} , v _{1, z} ), respectively.

上記式において、ｖ_１は台車部２の並進速度、ω_１は台車部２の角速度、ω_ｌ、ω_ｒは左右駆動輪２２の速度、ｒは駆動輪２２の半径、ｄは台車部２のトレッドである。 The relationship between the speed of the drive wheel 22 and the speed and angular speed of the carriage unit 2 is given by the following equations (2) and (3).

In the above equation, v ₁ is the translation speed of the carriage unit 2, ω ₁ is the angular velocity of the carriage unit 2, ω _l and ω _r are the speeds of the left and right drive wheels 22, r is the radius of the drive wheels 22, and d is the carriage unit 2. It is a tread.

但し、^０θ_１は台車部２の姿勢角度であり、^１θ_２は旋回軸４の角度である。本体部３の姿勢角度^０θ_２は、下記式で表現される。
^０θ_２＝^０θ_１＋^１θ_２ （６） Posture matrix from the world coordinate system to the bogie coordinate system is ^{(0 R} _1), the position vector is ^{(0 p} _1), the posture matrix from trolley coordinate system to the body coordinate system is in ^{(1 R} ₂₎ The relative position vector is ( ¹ p ₂ ). The posture matrices ( ⁰ R ₁ ) and ( ¹ R ₂ ) are expressed by the following equations.

However, ⁰ θ ₁ is an attitude angle of the carriage unit 2, and ¹ θ ₂ is an angle of the turning shaft 4. The posture angle ⁰ θ ₂ of the main body 3 is expressed by the following equation.
⁰ θ ₂ = ⁰ θ ₁ + ¹ θ ₂ (6)

Next, the equations of motion of the carriage unit 2 and the body unit 3 are formulated by the Newton-Euler method, and a relational expression of the torques of the drive wheels 22 and the turning shaft 4 is derived.
The equation of motion of the center of gravity of the main body 3 can be expressed by the following equation.
f ₂ = m ₂ ⁰ p (2 dots) _{2, cog} (7)

In the above formula, f ₂ is an external force on the main body 3, p _{2, cog} is the center of gravity of the main body 3, and m ₂ is the mass of the main body 3.
The equation of motion around the z-axis of the main body 3 can be expressed by the following equation.

In the above equation, ⁰ τ ₂ is a torque around the z-axis of the main body 3, and I _{2 and z} are moments of inertia around the center of gravity z-axis of the main body 3. The symbol | _z is the z element of the vector.
The equation of motion of the center of gravity of the carriage unit 2 can be expressed by the following equation.

In the above formula, f ₁ is the force received by the carriage unit 2, and m ₁ is the mass of the carriage unit 2. The equation of motion around the z-axis of the carriage unit 2 can be expressed by the following equation.

In the above equation, ⁰ τ ₁ is a torque around the z-axis of the carriage unit 2, and I _{1 and z} are moments of inertia around the center of gravity z-axis of the carriage unit 2.
The relationship between the external force and torque to the carriage unit 2 and the torque of the left and right drive wheels 22 can be expressed by the following equation.

When the above equations (1) to (12) are solved simultaneously, the driving torque τ of the left and right drive wheels 22 and the turning shaft 4 for the movement of the carriage unit 2 and the main body unit 3 is derived as the following equation. Is done.

In the above formula, τ = (τ ₁ , τ _r , ¹ τ ₂ ), q = (x ₂ , y ₂ , θ ₂ ). M (q), C (q, q (dot)), B (q), and g (q) are an inertia matrix, a centrifugal force and Coriolis term, a viscous friction matrix, and a gravity term, respectively.

  As described above, based on the equations of motion (7) to (10) about the center of gravity and the z-axis of the carriage unit 2 and the body unit 3, the inertial term, centrifugal force and Coriolis term, viscous friction term, gravity The equation of motion (13) relating to the torque of the drive wheel 22 and the turning shaft 4 is calculated.

The spatial command trajectory of the position and orientation q = (x ₂ , y ₂ , z ₂ ) of the main body 3 of the omnidirectional mobile body 1 is set in advance. This command trajectory is given as a trajectory by spline interpolation expressed by a plurality of waypoints, for example. The command trajectory f (s) is assumed to be parameterized by the trajectory parameter s. Here, s may be a line length, but for simplification, a parameter is set such that the start point s = 0 and the end point s = n at the via point n.

The equation of motion (13) can be made one-dimensional by the trajectory parameter s using the following equation.

By substituting the above equations (14) to (16) into the above equation (13), the following equation (17) of the trajectory parameter s is derived.

  The above equation (17) indicating the upper and lower limit constraints (MVC) of the torque is a standard form of the constraint condition of the shortest time control shown in the above equation (1). However, in the above (17), a (s), b (s), c (s), and d (s) are coefficients represented by the trajectory parameter s.

  As described above, the constraint condition calculation unit 91 includes the equation of motion (13) and the command trajectory f (s) including the trajectory parameter s, which is the trajectory to which the omnidirectional mobile body 1 is set in advance. Based on this, an equation (17) indicating a torque constraint condition (MVC) expressed by the trajectory parameter, which is a condition for limiting the torque of the drive wheels 22 and the turning shaft 4 is calculated.

  Next, the constraint condition (VLC) of the rotational speed of the drive wheel 22 and the turning shaft 4 of the omnidirectional mobile body 1 will be described. As described above, in consideration of the output performance of the wheels and the turning actuators 7 and 8 that drive the drive wheels 22 and the turning shaft 4, it is necessary to add a constraint condition to the speeds of the driving wheels 22 and the turning shaft 4.

The speed constraint of the drive wheel 22 is set by the following equation.

The speed constraint of the turning shaft 4 is set by the following formula.

However, ω _joint is the rotation speed of the turning shaft 4, ω _{joint, min} and ω _{joint, max} are the upper and lower limits of the speed of the turning shaft 4, respectively, ω _{l, min} , ω _l, and _max are the left drive wheels, respectively. The lower limit value and the upper limit value of the speed 22, ω _{r, min} and ω _{r, max} are the lower limit value and the upper limit value of the speed of the right drive wheel 22, respectively.
From the results of the above equation (15) and inverse kinematics, the inequality constraints of the above equations (20), (21), and (22) can be transformed into the inequality constraints on the trajectory parameter (s dot) as follows. .

However, s (dot) _{l, max} , s (dot) _{r, max} , s (dot) _{joint and max} are s (dots) after transformation of the left drive wheel 22, right drive wheel 22, and turning shaft 4, respectively. ). Using the above equation (23), a curve VLC representing the constraint condition of the rotational speeds of the drive wheels 22 and the turning shaft 4 is calculated.
As described above, the constraint condition calculation unit 91 determines the speeds of the drive wheels 22 and the swing shaft 4 based on the preset speed constraints (20), (21), and (22) of the drive wheels 22 and the swing shaft 4. The speed constraint condition (VLC) expressed by the trajectory parameter s is calculated from the inequality (23).

  The trajectory calculation unit 92 accelerates or decelerates at the maximum or minimum acceleration at each time on the trajectory while passing under the MVC and VLC curves calculated by the constraint condition calculation unit 91. Dot) Calculate the shortest time trajectory on the plane.

Here, an example of a method for calculating the shortest time trajectory will be described in detail.
The trajectory calculation unit 92 starts from the start point (s = 0) and the end point (s = 1) on the trajectory parameter s-s (dot) plane, repeats integration with a predetermined integration width, and calculates the shortest time trajectory. (FIG. 4). Here, the predetermined integration width can be arbitrarily set, but is preferably smaller than the actual sampling period (interpolation can be performed later).

  When integration is performed forward from the start point to the end point (hereinafter, forward integration), the integration is performed at the maximum acceleration. When integrating from the end point side to the start point side by integration (hereinafter referred to as backward integration), the integration is performed at the minimum acceleration. The trajectory calculation unit 92 repeats forward integration and backward integration based on the above rules to generate a shortest time trajectory.

  In the forward integration, the trajectory calculation unit 92 advances the forward integration along the VLC curve when it crosses the VLC curve (step S1). This corresponds to the operation section at the maximum speed of the drive wheel 22 and the turning shaft 4.

  On the other hand, in the forward integration, when intersecting with the VLC curve, if it does not follow the VLC curve or intersects with the MVC curve, it becomes an infeasible trajectory. In this case, the trajectory calculation unit 92 searches for switching point candidates (step S2).

Here, the switching point is a point where the minimum acceleration is switched to the maximum acceleration, and a trajectory passing under the MVC curve can be generated when passing through this point. For example, the following points are known as switching point candidates.
A point where the direction of acceleration on the MVC curve coincides with the tangent of the MVC curve itself (tangent switching point).
A point where the MVC curve itself changes discontinuously (discontinuous switching point).
The point where the torque constraint term a (s) becomes zero (zero inertia switching point).

  The trajectory calculation unit 92 performs backward integration with minimum acceleration, starting from the searched switching point candidate (step S3).

  The trajectory calculation unit 92 resumes forward integration starting from this switching point candidate when the backward integration trajectory is connected to the trajectory generated by forward integration without contradiction. On the other hand, the trajectory calculation unit 92 searches for the next switching point candidate (on the right side in FIG. 4), for example, when the backward integration trajectory does not conflict with the trajectory generated by the forward integration. . Then, the trajectory calculation unit 92 determines whether or not the backward integration trajectory starting from the next switching point candidate is connected to the trajectory generated by the forward integration without contradiction. The trajectory calculation unit 92 repeats the search for switching point candidates until the backward integration trajectory starting from the searched switching point candidate is consistently connected with the trajectory generated by forward integration so far (step S4).

  The trajectory calculation unit 92 repeats the above (Step S1) to (Step S4) to connect the trajectory for forward integration and the trajectory for backward integration (for example, Time optimal trajectory, s (dot) shown in FIG. 4). = G (s)) is calculated. In this way, the trajectory calculation unit 92, based on the calculated torque constraint condition MVC and speed constraint condition VLC, performs forward integration and backward integration with a predetermined integration width from the start point and end point in the trajectory parameter ss (dot) plane. The shortest time trajectory (s (dot) = g (s)) is calculated by searching for a switching point where the minimum acceleration is switched to the maximum acceleration by repeating the above.

The control unit 93 controls the wheel actuator 7 and the turning actuator 8 based on the shortest time trajectory (s (dot) = g (s)) calculated by the trajectory calculation unit 92.
The controller 93 calculates a target value q (t) of the position and orientation of the main body 3 of the omnidirectional mobile body 1 based on the shortest time trajectory (s (dot) = g (s)). The controller 93 controls the wheel actuator 7 and the turning actuator 8 based on the calculated target value q (t) of the main body 3 of the omnidirectional mobile body 1.

Here, an example of a method for calculating the target value q (t) of the position and orientation of the main body 3 of the omnidirectional mobile body 1 will be described in detail.
First, the control unit 93 sets initial values i = 0 and si = 0 (step S11).
The controller 93 calculates qi = f (si) at time ti (step S12).
The controller 93 calculates si when the sample time Δt has advanced by the following equation (step S13).
si + 1 = si + g (si) * Δt
ti + 1 = ti + Δt

The controller 93 determines whether or not si ≦ 1 (step S14).
When determining that si ≦ 1 (YES in step S14), the controller 93 ends the calculation process of the target value q (t) of the position and orientation of the main body 3 of the omnidirectional mobile body 1. On the other hand, when determining that si ≦ 1 is not satisfied (NO in step S14), the control unit 93 increments i (i = i + 1) (step S15) and returns to the above (step S13).

  The control unit 93 sets the calculated combination of qi and ti as the target value q (t) for the position and orientation of the main body 3 of the omnidirectional mobile body 1. Note that the control unit 93 calculates si using Euler integration in the above (step S13), but is not limited thereto. The controller 93 may calculate si using trapezoidal integration in the above (step S13). Thereby, si can be calculated with higher accuracy, and the target value q (t) of the position and orientation of the main body 3 of the omnidirectional mobile body 1 can be calculated with higher accuracy.

FIG. 6 is a flowchart showing the flow of the omnidirectional mobile body control method according to the present embodiment.
The constraint condition calculation unit 91 is based on the equations of motion (7) to (10) about the center of gravity and the z-axis of the carriage unit 2 and the main body unit 3, the inertial term, the centrifugal force and the Coriolis term, and the viscous friction term. And the equation of motion (13) regarding the torque of the drive wheel 22 and the turning shaft 4 including the gravity term are calculated. Further, the constraint condition calculation unit 91, the equation of motion (13), a command trajectory f (s) expressed by the trajectory parameter s, which is a trajectory to which the omnidirectional mobile body 1 is set in advance, Based on the above, the equation (17) indicating the torque constraint condition (MVC) expressed by the trajectory parameter s, which is a condition for limiting the torque of the drive wheel 22 and the turning shaft 4, is calculated (step S101).

  The restriction condition calculation unit 91 is a condition for restricting the speeds of the drive wheels 22 and the turning shaft 4 based on the preset restrictions (20) to (22) of the speeds of the driving wheels 22 and the turning shaft 4. Then, the equation (19) indicating the speed constraint condition (VLC) expressed by the trajectory parameter s is calculated (step S102).

  The trajectory calculation unit 92 starts from the start point (s = 0) and end point (s = 1) in the trajectory parameter s-s (dot) plane based on the torque constraint condition and the speed constraint condition calculated by the constraint condition calculation unit 91. The shortest time trajectory is calculated by searching for switching points by repeating forward integration and backward integration with a predetermined integration width (step S103).

  Based on the shortest time trajectory (s (dot) = g (s)) calculated by the trajectory calculating unit 92, the control unit 93 sets the target posture q (t) of the main body 3 of the omnidirectional mobile body 1. Is calculated (step S104). The control unit 93 controls the wheel actuator 7 and the turning actuator 8 based on the calculated target value q (t) of the position and orientation of the main body 3 of the omnidirectional mobile body 1, thereby driving the drive wheels 22 of the carriage unit 2. And the turning axis 4 of the main-body part 3 is controlled (step S105).

  Next, a description will be given of a simulation result obtained by generating the shortest time trajectory that follows the set command trajectory in the shortest time by adding constraints on the driving torque and rotational speed of the drive wheels 22 and the turning shaft 4.

  Here, the algorithm for generating the shortest time trajectory is generated by, for example, Python and C language, and its operation is confirmed. In this simulation, the command trajectory shown in FIG. 7 is used. This command trajectory has (0, 0), (1, -1), (1, 1), (0) in the world coordinate system where the forward direction of the initial posture of the omnidirectional mobile body 1 is x and the left direction is y. −1, −1), (−1, 1), and (0, 0) are generated by cubic natural spline interpolation. The direction of the main body 3 of the omnidirectional mobile body 1 is instructed to always maintain the angle 0. The upper limit value of the speed of the left and right drive wheels 22 is 18 rad / s, and the upper limit value of the torque of the left and right drive wheels 22 is 0.3 Nm. The upper limit value of the speed of the turning shaft 4 is 3.0 rad / s, and the upper limit value of the torque of the turning shaft 4 is 0.2 Nm.

  FIG. 8 is a diagram illustrating the driving torque of the left driving wheel. FIG. 9 is a diagram showing the drive torque of the right drive wheel. FIG. 10 is a diagram showing the driving torque of the turning shaft. 8 to 10, broken lines indicate the upper and lower limit values of each driving torque. As shown in FIGS. 8 to 10, it can be seen that the drive torques of the left and right drive wheels 22 and the turning shaft 4 change within the upper and lower limit values. Therefore, in this simulation, it can be seen that the drive torques of the left and right drive wheels 22 and the turning shaft 4 are correctly restricted.

  FIG. 11 is a diagram showing a trajectory on the trajectory parameter ss (dot) plane when the drive torques of the left and right drive wheels and the turning shaft are restricted as shown in FIGS. Since the shortest time control is an algorithm that takes the maximum acceleration or the minimum acceleration at each point on the trajectory, the obtained trajectory becomes the shortest time trajectory.

As described above, in the present embodiment, based on the equations of motion related to the center of gravity of the carriage unit 2 and the main body unit 3 and the vertical axis, the inertial term, the centrifugal force and the Coriolis term, the viscous friction term, and the gravity term are included. A motion equation relating to the torque of the drive wheel 22 and the turning shaft 4 is calculated, and the command trajectory f () expressed by the trajectory parameter s is a trajectory to which the omnidirectional mobile body 1 is to be moved in advance. s), the torque constraint condition MVC expressed by the trajectory parameter, which is a condition for constraining the torque of the drive wheel 22 and the turning shaft 4 is calculated. The speed constraint condition VLC expressed by the trajectory parameter s is a condition for restricting the speed of the drive wheel 22 and the turning shaft 4 based on the preset speed restriction of the driving wheel 22 and the turning shaft 4. Calculated. A trajectory that satisfies the calculated torque constraint condition MVC and speed constraint condition VLC and follows the command trajectory f (s) in the shortest time, and is the shortest time expressed on the trajectory parameter ss (dot) plane. A trajectory is calculated. The control unit 93 of the control device 9 calculates a target value of the position and orientation of the main body 3 based on the calculated shortest time trajectory. And the control part 93 of the control apparatus 9 controls a wheel and the turning actuators 7 and 8 based on the calculated target value of the position and orientation of the main body part 3.
Thereby, it is possible to perform the optimum control in the shortest time while considering the output performance of the actuator including the unique point that is controlled by the static friction or the index such as ZMP.

Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention.
For example, in the above embodiment, the control device 9 of the omnidirectional mobile body 1 is configured to include the constraint condition calculation unit 91 and the trajectory calculation unit 92, but is not limited thereto. At least one of the constraint condition calculation unit 91 and the trajectory calculation unit 92 may be provided in a device different from the omnidirectional mobile body 1. In this case, the torque constraint condition and speed constraint condition calculated by the constraint condition calculation unit 91 of another device or the shortest time trajectory calculated by the trajectory calculation unit 92 is transmitted to the control device 9 of the omnidirectional mobile body 1, Or you may input into the control apparatus 9 of the omnidirectional mobile body 1 by the user.

  In the above-described embodiment, the trajectory calculation unit 92 is either the maximum acceleration or the minimum acceleration, which is one of the accelerations of the drive wheel 22 and the turning shaft 4 that can follow the spatial trajectory while passing under the MVC and VLC curves. Although the shortest time trajectory such as is calculated, it is not limited to this. For example, the trajectory calculation unit 92 may calculate a feasible trajectory that is not the shortest (slow) while passing under the MVC and VLC curves.

  In the shortest time orbit where the maximum acceleration or the minimum acceleration is obtained by the shortest time control, the acceleration is discontinuous. When such a discontinuous state of acceleration is not preferable, the trajectory calculation unit 92 is smooth while passing under the MVC and VLC curves (such that the acceleration at each time on the trajectory is continuous). A feasible trajectory may be calculated (FIG. 12). Thereby, it is possible to perform optimal control while considering the output performance of the actuator including the unique point that is controlled by the static friction or the index such as ZMP.

In addition, the present invention can realize the processing shown in FIG. 6 by causing a CPU to execute a computer program, for example.
The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (for example, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (for example, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / W and semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)) are included.

  The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

  DESCRIPTION OF SYMBOLS 1 Omni-directional mobile body, 2 trolley | bogie part, 3 main-body part, 4 turning axis, 5 rotation sensor, 6 rotation sensor, 7 wheel actuator, 8 turning actuator, 9 control apparatus, 21 auxiliary wheel, 22 drive wheel, 91 constraint condition calculation Section, 92 orbit calculation section, 93 control section

Claims (4)

A carriage section provided with a plurality of drive wheels, a main body section rotatably provided on the carriage section via a turning shaft, a first driving means for driving the driving wheels, and a second for driving the turning shaft. And an omnidirectional mobile body that is movable in all directions,
Based on the equations of motion related to the center of gravity and the vertical axis of the bogie part and the main body part, the torque of the driving wheel and the turning shaft including an inertial term, a centrifugal force and Coriolis term, a viscous friction term, and a gravity term. Equation of motion is calculated, and based on the equation of motion and a preset command trajectory expressed by the trajectory parameter, which is a trajectory that the omnidirectional mobile body should move in advance, A condition for constraining torque, and the torque restriction condition expressed by the trajectory parameter is calculated;
Based on a preset restriction on the speed of the drive wheel and the turning axis, a condition for restricting the speed of the driving wheel and the turning axis, the speed restriction condition expressed by the trajectory parameter is calculated,
A trajectory that satisfies the calculated torque constraint condition and speed constraint condition and follows the command trajectory, the trajectory expressed on the plane of the trajectory parameter and its differential value is calculated,
Control means for calculating a target value of the position and orientation of the main body based on the calculated trajectory and controlling the first and second drive means based on the calculated target value of the position and orientation of the main body. ,
The calculated trajectory is a shortest time trajectory that satisfies the calculated torque constraint condition and speed constraint condition and follows the command trajectory in the shortest time ,
Based on the calculated torque constraint condition and speed constraint condition, the calculated trajectory repeats forward integration and backward integration with a predetermined integration width from the start point and end point in the plane of the trajectory parameter and its differential value, and the forward trajectory When integrating, the integration is performed at the maximum acceleration, and when the backward integration is performed, the shortest time trajectory calculated by performing the integration at the minimum acceleration .
An omnidirectional mobile body characterized by that. The omnidirectional mobile body according to claim 1 ,
Based on the calculated torque constraint condition and speed constraint condition, the calculated trajectory repeats forward integration and backward integration with a predetermined integration width from the start point and end point in the plane of the trajectory parameter and its differential value, and minimizes the minimum acceleration. The shortest time trajectory calculated by searching for a switching point that switches from to maximum acceleration,
An omnidirectional mobile object characterized by that. A carriage section provided with a plurality of drive wheels, a main body section rotatably provided on the carriage section via a turning shaft, a first driving means for driving the driving wheels, and a second for driving the turning shaft. An omnidirectional mobile body that is movable in all directions,
Based on the equations of motion related to the center of gravity and the vertical axis of the bogie part and the main body part, the torque of the driving wheel and the turning shaft including an inertial term, a centrifugal force and Coriolis term, a viscous friction term, and a gravity term. Equation of motion is calculated, and based on the equation of motion and a command trajectory expressed by a trajectory parameter that is a trajectory that the omnidirectional mobile body is set in advance, A condition for constraining torque, and calculating a torque constraint condition expressed by the trajectory parameter;
Calculating a speed constraint condition expressed by the trajectory parameter, which is a condition for limiting the speed of the drive wheel and the swing axis based on a preset speed constraint of the drive wheel and the swing axis;
Calculating the trajectory that satisfies the calculated torque constraint condition and speed constraint condition and that follows the command trajectory and that is expressed on a plane of the trajectory parameter and its differential value;
Calculating a target value of the position and orientation of the main body based on the calculated trajectory, and controlling the first and second driving means based on the calculated target value of the position and orientation of the main body,
Only including,
The calculated trajectory is a shortest time trajectory that satisfies the calculated torque constraint condition and speed constraint condition and follows the command trajectory in the shortest time ,
Based on the calculated torque constraint condition and speed constraint condition, the calculated trajectory repeats forward integration and backward integration with a predetermined integration width from the start point and end point in the plane of the trajectory parameter and its differential value, and the forward trajectory When integrating, the integration is performed at the maximum acceleration, and when the backward integration is performed, the shortest time trajectory calculated by performing the integration at the minimum acceleration .
An omnidirectional mobile body control method characterized by the above. A carriage section provided with a plurality of drive wheels, a main body section rotatably provided on the carriage section via a turning shaft, a first driving means for driving the driving wheels, and a second for driving the turning shaft. A program for an omnidirectional mobile body comprising drive means and movable in all directions,
Based on the equations of motion related to the center of gravity and the vertical axis of the bogie part and the main body part, the torque of the driving wheel and the turning shaft including an inertial term, a centrifugal force and Coriolis term, a viscous friction term, and a gravity term. Equation of motion is calculated, and based on the equation of motion and a command trajectory expressed by a trajectory parameter that is a trajectory that the omnidirectional mobile body is set in advance, A condition for constraining torque, and calculating a torque constraint condition expressed by the trajectory parameter;
Based on the preset constraints on the speeds of the drive wheels and the turning axis, the conditions for restricting the speeds of the driving wheels and the turning axis are calculated, and the speed restriction conditions expressed by the trajectory parameters are calculated,
A process of calculating the trajectory that satisfies the calculated torque constraint condition and speed constraint condition and that follows the command trajectory and that is expressed on the plane of the trajectory parameter and its differential value;
A process for calculating a target value of the position and orientation of the main body based on the calculated trajectory, and controlling the first and second driving means based on the calculated target value of the position and orientation of the main body,
To the computer ,
The calculated trajectory is a shortest time trajectory that satisfies the calculated torque constraint condition and speed constraint condition and follows the command trajectory in the shortest time ,
Based on the calculated torque constraint condition and speed constraint condition, the calculated trajectory repeats forward integration and backward integration with a predetermined integration width from the start point and end point in the plane of the trajectory parameter and its differential value, and the forward trajectory When integrating, the integration is performed at the maximum acceleration, and when the backward integration is performed, the shortest time trajectory calculated by performing the integration at the minimum acceleration.
A program characterized by that.

Images