Trajectory generation device, trajectory generation method and program for omnidirectional mobile body

Abstract

To provide a trajectory generation device for an omnidirectional mobile body, a trajectory generation method, and a program capable of calculating a time trajectory more quickly.SOLUTION: A trajectory generating device for an omnidirectional mobile body includes: constraint calculation means for calculating maximum acceleration of joint parameters based on a Jacobian equation derived based on a current joint state of the omnidirectional mobile body; and trajectory calculation means for performing optimization processing by performing forward integration and backward integration on a plane of trajectory parameters s and dotted s on the basis of acceleration limit vectors including the maximum acceleration of joint parameters, and calculating a time trajectory. The trajectory calculation means is configured to: place a virtual joint position in an advancing direction of the joint between a starting point of the joint position trajectory and a point next to the starting point on the plane of the trajectory parameters s; convert the joint position trajectory into straight segments and arc segments that can be expressed by trajectory length; and, while performing forward integration from the starting point (0, vnorm ) on the plane of the trajectory parameters s, perform backward integration from an end point of the joint position trajectory until colliding with the forward integration.SELECTED DRAWING: Figure 4

Description

[Technical field]

[0001] The present invention relates to a trajectory generation device, a trajectory generation method, and a program for an omnidirectional mobile body that generates a time trajectory of an omnidirectional mobile body that can move in all directions. [Background technology]

[0002] There is known a trajectory generation device that calculates constraint conditions based on the kinematics of an omnidirectional mobile body and calculates a time trajectory under the calculated constraint conditions (see, for example, Patent Document 1). [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent No. 6468127 [Non-patent literature]

[0004] [Non-Patent Document 1] Tobias Kunz and Mike Stilman, Time-Optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity. Robotics: Science and Systems. 2012. Summary of the Invention [Problem to be solved by the invention]

[0005] The above-mentioned trajectory planning device cannot utilize the current velocity of the joints when planning a trajectory, and since it uses the kinematics of the omnidirectional mobile body as is, the calculation speed is slow and there is a risk that it will take a long time to calculate the time trajectory.

[0006] The present invention has been made in consideration of the above problems, and has as its main object to provide a trajectory generation device, a trajectory generation method, and a program for an omnidirectional moving body that can calculate a time trajectory at higher speed. [Means for solving the problem]

[0007] In order to achieve the above object, one aspect of the present invention is to A trajectory generation device for an omnidirectional mobile body that is movable in all directions and includes a carriage unit having a plurality of drive wheels and a main body unit that is rotatably mounted on the carriage unit via a rotating shaft, the trajectory generation device comprising: a constraint calculation means for calculating maximum accelerations of joint parameters indicating a position and an attitude of the omnidirectional mobile body in a coordinate system based on a Jacobian equation derived based on a current joint state of the omnidirectional mobile body, the maximum accelerations being calculated based on the degrees of freedom of a main body part defined by drive wheels and a rotating shaft of the omnidirectional mobile body as joints; a trajectory calculation means for performing an optimization process by performing forward integration and backward integration on a plane of a trajectory parameter s and its differential value s (dots) based on an acceleration restriction vector including the maximum acceleration of the joint parameters calculated by the constraint calculation means and having the number of dimensions equal to the number of joints of the omnidirectional mobile body, thereby calculating a time trajectory; Equipped with The trajectory calculation means places a virtual joint position in a moving direction of the joint between a starting point of the joint position trajectory and a point next to the starting point on a plane of the trajectory parameter s, converts the joint position trajectory into a straight line segment and a circular arc segment that can be expressed by a trajectory length, and calculates a current velocity of the joint by v norm As the starting point (0, v norm ) while performing a forward integration from the end point of the joint position trajectory until it collides with the forward integration. Trajectory generation device for omnidirectional moving body It is. In this aspect, the constraint calculation means calculates maximum acceleration and maximum velocity of joint parameters indicating a position and an attitude of the omnidirectional mobile body in a coordinate system based on a Jacobian equation derived based on a current joint state of the omnidirectional mobile body, The trajectory calculation means may perform an optimization process by performing forward integration and backward integration on a plane of the trajectory parameter s based on an acceleration limit vector including a maximum acceleration of the joint parameter and a velocity limit vector including a maximum velocity of the joint parameter, which are calculated by the constraint calculation means, to calculate a time trajectory. In this aspect, the constraint calculation means uses the Jacobian equation to calculate the acceleration α of the right drive wheel such that the acceleration direction of the position (x, y) of the bogie unit is the traveling direction of the bogie unit, and the absolute value of the acceleration of (x, y) becomes large. R , and the acceleration of the left driving wheel α L Each of them explored The preset acceleration α of the rotation axis S and the range of the searched α R and α L Based on the above, the maximum acceleration in the direction φ of the main body is calculated, and the searched α R and α L The maximum acceleration of (x, y) may be calculated based on the above and output to the trajectory calculation means as the maximum acceleration of the joint parameters (x, y, φ). In this aspect, the trajectory calculation means may determine the position of the virtual joint position by searching, on a plane of the trajectory parameter s, in order from a position where the distance between the start point of the joint position trajectory and the next point is smallest, until the forward integration and the backward integration are successful. In order to achieve the above object, one aspect of the present invention is to A method for generating a trajectory of an omnidirectional mobile body that is movable in all directions and includes a carriage unit having a plurality of drive wheels and a main body unit that is rotatably mounted on the carriage unit via a rotating shaft, the method comprising the steps of: calculating maximum accelerations of joint parameters indicating a position and an attitude of the omnidirectional mobile body in a coordinate system based on a Jacobian equation derived based on a current joint state of the omnidirectional mobile body, with the degrees of freedom of a main body part due to drive wheels and a rotating shaft of the omnidirectional mobile body being joints; A step of performing an optimization process by performing forward integration and backward integration on a plane of a trajectory parameter s and its differential value s (dots) based on an acceleration limiting vector including the calculated maximum acceleration of the joint parameter and having the number of dimensions equal to the number of joints of the omnidirectional mobile body, thereby calculating a time trajectory; Including, On the plane of the trajectory parameter s, a virtual joint position is placed in the moving direction of the joint between the starting point of the joint position trajectory and the point next to the starting point, the joint position trajectory is converted into a straight line segment and a circular arc segment that can be expressed by a trajectory length, and the current velocity of the joint is expressed as v norm As the starting point (0, v norm ) while performing a forward integration from the end point of the joint position trajectory until it collides with the forward integration. Trajectory generation method for an omnidirectional moving object may be also possible. In order to achieve the above object, one aspect of the present invention is to A program for an omnidirectional mobile body that is movable in all directions and includes a carriage unit having a plurality of drive wheels and a main body unit that is rotatably mounted on the carriage unit via a pivot shaft, the program comprising: calculating maximum accelerations of joint parameters indicating a position and an attitude of the omnidirectional mobile body in a coordinate system based on a Jacobian equation derived based on a current joint state of the omnidirectional mobile body, with the degrees of freedom of a main body part due to the drive wheels and the rotating shaft of the omnidirectional mobile body being joints; A process of performing an optimization process by performing forward integration and backward integration on a plane of a trajectory parameter s and its differential value s (dots) based on an acceleration limiting vector including the maximum acceleration of the calculated joint parameters and having the number of dimensions equal to the number of joints of the omnidirectional mobile body, thereby calculating a time trajectory; Run the following on your computer: On the plane of the trajectory parameter s, a virtual joint position is placed in the moving direction of the joint between the starting point of the joint position trajectory and the point next to the starting point, the joint position trajectory is converted into a straight line segment and a circular arc segment that can be expressed by a trajectory length, and the current velocity of the joint is expressed as v normAs the starting point (0, v norm ) while performing a forward integration from the end point of the joint position trajectory until it collides with the forward integration. Omnidirectional mobile program may be also possible. Effect of the Invention

[0008] The present invention has been made in consideration of such problems, and is able to provide a trajectory generation device, a trajectory generation method, and a program for an omnidirectional moving body that can calculate a time trajectory at higher speed. [Brief description of the drawings]

[0009] [Figure 1] 1 is a diagram showing a schematic configuration of an omnidirectional mobile body according to an embodiment of the present invention; [Diagram 2] 1 is a block diagram showing a schematic system configuration of an omnidirectional mobile body according to an embodiment of the present invention; [Diagram 3] 1 is a block diagram showing a schematic system configuration of a trajectory generation device according to an embodiment of the present invention. [Figure 4] 2 is a block diagram showing a schematic system configuration of an optimization processing unit according to the present embodiment. FIG. [Diagram 5] FIG. 2 illustrates an example of a time trajectory. [Figure 6] 4 is a flowchart showing a flow of a trajectory generating method according to the present embodiment. [Figure 7] FIG. 13 is a diagram showing whole-body movement. [Figure 8] FIG. 13 is a diagram showing changes in command velocity to each joint. [Figure 9] FIG. 13 is a diagram showing changes in command acceleration to each joint. [Figure 10] FIG. 13 is a diagram showing the actual carriage velocity in the carriage coordinate system when the carriage section is in operation. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0010] EMBODIMENT 1 Hereinafter, an embodiment of the present invention will be described with reference to the drawings. Fig. 1 is a diagram showing a schematic configuration of an omnidirectional mobile body according to an embodiment of the present invention. The omnidirectional mobile body 1 according to this embodiment is configured as, for example, an active caster type omnidirectional mobile robot.

[0011] The omnidirectional mobile body 1 has a carriage 2 which moves, and a main body 3 which is rotatably connected to the upper side of the carriage 2. The carriage 2 is provided with one auxiliary wheel 21 and a pair of left and right drive wheels 22. The main body 3 rotates relatively to the carriage 2 about a rotation axis (Yaw rotation axis) 4. The drive wheel axis of the drive wheel 22 and the rotation axis 4 of the main body 3 are offset.

[0012] The omnidirectional mobile body 1 achieves omnidirectional movement by independently driving the drive wheel axle and the swivel shaft 4. The omnidirectional mobile body 1 achieves omnidirectional movement by, for example, the relative rotation of the main body 3 by the swivel shaft 4 in addition to the rotation difference of the drive wheels 22 by the drive wheel axle. The main body 3 is provided with, for example, a rotatable multi-joint arm capable of grasping an object. The omnidirectional mobile body 1 may be configured as a safe, small-sized life support robot HSR (Human Support Robot) that performs physical tasks.

[0013] 2 is a block diagram showing a schematic system configuration of an omnidirectional mobile body according to this 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 angular acceleration) of the left and right drive wheels 22, a rotation sensor 6 that detects rotation information of the turning shaft 4, a pair of wheel actuators 7 that drive the pair of drive wheels 22, a turning actuator 8 that drives the turning shaft 4, a control device 9 that controls the wheel actuator 7 and the turning actuator 8, and a trajectory generating device 10. 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.

[0014] For example, each rotation sensor 5 is provided on the drive wheel axle of the left and right drive wheels 22. Each rotation sensor 5 is composed of a potentiometer, an encoder, etc. Each wheel actuator 7 is connected to the drive wheel axle of the left and right drive wheels 22, for example. Each wheel actuator 7 is composed of a motor, etc.

[0015] The turning actuator 8 is connected to, for example, the turning shaft 4. Each turning actuator 8 is constituted by a motor or the like. The wheel actuator 7 and the turning actuator 8 are rotationally driven in response to a control signal output from a control device 9.

[0016] The control device 9 executes time control to follow the set command trajectory while satisfying the above-mentioned constraints on the acceleration of the drive wheels 22 and the turning shaft 4. The control device 9 controls the wheel actuator 7 and the turning actuator 8 based on the time trajectory calculated by the trajectory generating device 10.

[0017] The trajectory generating device 10 generates a time trajectory for executing time control. The trajectory generating device 10 and the control device 9 may be configured integrally. In the present embodiment, the trajectory generating device 10 is configured to be mounted on the omnidirectional mobile body 1, but is not limited to this. The trajectory generating device 10 may be provided outside the omnidirectional mobile body 1. In this case, the trajectory generating device 10 may transmit the calculated time trajectory to the control device 9 via wireless communication or the like.

[0018] The trajectory generation device 10 has a typical computer hardware configuration including, for example, a processor such as a CPU (Central Processing Unit) or a GPU (Graphics Processing Unit), internal memory such as a RAM (Random Access Memory) or a ROM (Read Only Memory), storage devices such as an HDD (Hard Disk Drive) or an SSD (Solid State Drive), an input / output I / F for connecting peripheral devices such as a display, and a communication I / F for communicating with devices outside the device.

[0019] The trajectory generation device 10 according to this embodiment performs periodic whole-body trajectory planning in order to realize manipulation that can dynamically change the constraint conditions (target posture, obstacle information, etc.) of the omnidirectional mobile body 1.

[0020] 3 is a block diagram showing a schematic system configuration of a trajectory generating device according to this embodiment. The trajectory generating device 10 includes a candidate generating unit 11 that generates a trajectory candidate group, a candidate evaluating unit 12 that sorts the trajectory candidate group based on an evaluation function, a collision determining unit 13 that determines collision between the trajectory candidate group and an obstacle, and an optimization processing unit 14 that performs optimization processing and adds time information to the trajectory candidates.

[0021] The candidate generation unit 11 generates, for example, N trajectory candidates and outputs them to the candidate evaluation unit 12. The candidate evaluation unit 12 evaluates each trajectory candidate using an evaluation function, sorts each trajectory candidate based on the evaluation result, and outputs them to the collision determination unit 13. The collision determination unit 13 determines whether or not each trajectory candidate will collide with an obstacle when the omnidirectional mobile body 1 moves along the trajectory candidate, and outputs the trajectory candidate determined not to collide to the optimization processing unit 14. The trajectory generation device 10 controls so that all of these processes are completed within a fixed time.

[0022] As described above, the trajectory generation device 10 according to this embodiment can generate trajectories in real time and perform robust control against environmental changes by running a loop of generating, evaluating, verifying feasibility of, and optimizing operation time for a group of trajectory candidates.

[0023] There are three requirements for the optimization processor 14 used in the periodic whole-body trajectory planning described above. First, it is required that the calculation time be short so as to be in time for the periodic whole-body trajectory planning. Second, since time optimization of a new trajectory is performed while the omnidirectional mobile body 1 is in motion, it is required that time optimization taking into account the current joint positions and velocities is possible. Third, since the periodic whole-body trajectory planning uses a multi-point trajectory taking into account obstacle avoidance and the like, it is required that time optimization of the multi-point trajectory is possible.

[0024] The optimization processor 14 according to this embodiment performs time optimization of a multi-point trajectory that takes into account the current joint positions and velocities and that is fast to process, so as to satisfy the above-mentioned first to third requirements. For this reason, the optimization processor 14 performs optimization processing for a trajectory that does not have time information, taking into account the acceleration constraints of the actuator, so as to operate in a shorter time.

[0025] 4 is a block diagram showing a schematic system configuration of the optimization processing unit according to this embodiment. The optimization processing unit 14 according to this embodiment has a constraint calculation unit 141 that calculates an acceleration constraint, and a trajectory calculation unit 142 that calculates a time trajectory.

[0026] Incidentally, the omnidirectional mobile object 1 can be considered as a robot having a plurality of joints. That is, the degrees of freedom of the main body 3 defined by the drive wheels 22 and the rotating shafts 4 of the omnidirectional mobile object 1 can be assumed as the joints of a virtual robot. An acceleration constraint exists in the joints of the virtual robot. This acceleration constraint is given, for example, by an upper limit value (maximum acceleration).

[0027] The constraint calculation unit 141 calculates the above-mentioned acceleration constraint of the omnidirectional mobile body 1. The constraint calculation unit 141 is a specific example of a constraint calculation means.

[0028] Next, a detailed description will be given of a method for calculating the maximum acceleration, which is the acceleration constraint of the joints of the omnidirectional mobile body 1 according to this embodiment. First, each parameter is set as follows.

[0029] (x, y, φ): joint parameters indicating the position and orientation in the coordinate system of the omnidirectional mobile body 1 (hereinafter referred to as the omnidirectional mobile body coordinate system) In addition, x and y are the position of the cart unit 2, and φ is the direction in which the main body unit 3 faces. W: Wheel distance r: Drive wheel radius ω: joint velocity s: Distance between the pivot point and the drive axle α: Joint acceleration θ V : Swivel axis angle R, L, S: Right drive wheel, Left drive wheel, Swivel shaft

[0030] When the Jacobian J is found using the above parameters based on the current joint states of the omnidirectional mobile object 1, the following equations (1) and (2) are derived.

number

[0031] Furthermore, when the acceleration is considered in the omnidirectional mobile body coordinate system and the derivative of the Jacobian J of the omnidirectional mobile body 1 is obtained, the following equations (3) and (4) are derived.

number

[0032] The constraint calculation unit 141 uses the above formula (3) to calculate the acceleration α of the right driving wheel 22 so that the acceleration direction of (x, y) is the input direction and the absolute value of the acceleration of (x, y) becomes large. R , and the acceleration α of the left driving wheel 22 L Explore.

[0033] The above "x, y acceleration directions are input directions" refers to the initial movement direction of the cart unit 2 on the input trajectory. Specifically, it refers to the direction of the position of the cart unit 2 at the zeroth point of the input trajectory as viewed from the current position of the cart unit 2. The input trajectory is, for example, the whole-body trajectory of the omnidirectional mobile body 1 to be reproduced, and is input in advance to the constraint calculation unit 141 from the collision determination unit 13.

[0034] Acceleration α of the left and right drive wheels 22 R , α L When the range is plotted on a two-dimensional coordinate system, the target acceleration α of the left and right drive wheels 22 is plotted on the edge of the range. R , α L exists.

[0035] Specifically, the constraint calculation unit 141 performs the following processes (i) to (iii): R , α L The maximum value of α max Let us assume that. (i) The constraint calculation unit 141 calculates the acceleration α R , α L Calculate the x and y accelerations for each combination (for example, about 200 combinations). Combination: (±α max , (k-25) / 25×α max ), ((k-25) / 25×α max , ±α max ) (k=0, 1, …, 50) (ii) The constraint calculation unit 141 calculates the x and y acceleration directions based on the respective x and y accelerations using the following equations. arctan(y acceleration / x acceleration) (iii) The constraint calculation unit 141 calculates the acceleration α R , α L Select .

[0036] The acceleration in x and y is α R , α L Therefore, the magnitude of the acceleration in x and y ((the acceleration of x) 2 +(y acceleration) 2 ) 1 / 2 is maximized. R , α L is the α we explored this time. R , α L It will be on the edge of the range.

[0037] The constraint calculation unit 141 calculates α based on the traveling direction in the omnidirectional mobile body coordinate system. S As the range of α S Use the lower limit of α S Specifically, when the direction in which the carriage unit 2 should face at the zeroth point of the input trajectory is positive as viewed from the current facing direction of the carriage unit 2, the constraint calculation unit 141 determines whether to use the upper limit of α S The range of α SIf the direction is positive, then α S The range of α S Set the lower limit of.

[0038] In addition, α S is the acceleration of the rotating shaft 4, and is set based on design values ​​such as the output of the rotating actuator 8 of the rotating shaft 4, the gear ratio, the mass of the main body 3, etc.

[0039] The constraint calculation unit 141 sets α S The range (lower or upper limit) of α R , α L and into the above formula (3) to calculate the maximum acceleration of φ. R , and α L Based on this, the maximum acceleration in x and y is calculated.

[0040] The constraint calculation unit 141 calculates the above-mentioned searched α R and α L and α S The maximum acceleration of φ calculated based on the upper limit of R and α L and α S The maximum acceleration of φ calculated based on the lower limit of and the maximum acceleration of φ may be compared, and the smaller one may be used as the final maximum acceleration of φ. This is to improve safety by making the maximum acceleration, which will be described later, smaller.

[0041] The constraint calculation unit 141 calculates the acceleration constraint offline, but is not limited to this, and may calculate the acceleration constraint online. The constraint calculation unit 141 outputs the maximum acceleration of the joint parameters (x, y, φ), which is the acceleration constraint calculated as described above, to the trajectory calculation unit 142.

[0042] Here, we consider the case where the command trajectory on which the robot should move is given as a spatial trajectory, which can be expressed by a single trajectory parameter s (time parameterized).

[0043] The joint angles of a robot are determined by specifying a point on the spatial trajectory. The speed of a robot in a certain state can also be expressed by the speed s of the trajectory parameter s at the corresponding point on the spatial trajectory (hereinafter referred to as s-dot).

[0044] Based on the above, the trajectory calculation unit 142 calculates a time trajectory that satisfies the acceleration constraints calculated by the constraint calculation unit 141 and follows the command trajectory, and is expressed on a plane (hereinafter referred to as the trajectory parameter ss (dot) plane) with the trajectory parameters on the horizontal axis and their differential values ​​on the vertical axis.

[0045] The trajectory calculation unit 142 executes time control by searching for a trajectory on the trajectory parameter ss (dot) plane and calculating a time trajectory. The trajectory calculation unit 142 starts from the start point and the end point on the trajectory parameter ss (dot) plane, respectively, and calculates a time trajectory by repeating integration with a predetermined integration width. The trajectory calculation unit 142 calculates a time trajectory by repeating integration in a forward direction from the start point side to the end point side (hereinafter, forward integration) and integration in a backward direction from the end point side to the start point side (hereinafter, backward integration).

[0046] Next, a method for calculating a time trajectory by the trajectory calculation section 142 according to this embodiment will be described in detail. Fig. 5 is a diagram showing an example of a time trajectory.

[0047] The time trajectory calculation method according to this embodiment extends the well-known method described in Non-Patent Document 1 (Tobias Kunz and Mike Stilman, Time-Optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity. Robotics: Science and Systems. 2012.) in the following points 1) to 3), thereby making it possible to use any current speed and perform high-speed multi-point trajectory optimization.

[0048] 1) Between the start point P0 of the joint position trajectory and the next point P1, a virtual joint position Pv is placed in the v direction (the direction of movement of the joint) from P0. 2) The current velocity of the joint is v norm Then, the starting point (0, v norm ) and perform forward integration. 3) Search for positions from P0 to Pv in ascending order of distance until forward integration and backward integration are successful, and determine the position of Pv.

[0049] Hereinafter, a method for calculating a time trajectory by the trajectory calculation unit 142 according to this embodiment will be described in more detail. Note that the following process is partially disclosed in Non-Patent Document 1, which may be used as appropriate.

[0050] First, the following parameter values ​​are input to the trajectory calculation unit 142: Each joint position trajectory [P0, P1, P2, ... Pn] (Each point is k-dimensional) -Velocity limit vector VMax of each joint (k-dimensional) Acceleration limit vector AMax (k-dimensional) for each joint - Current velocity of each joint v

[0051] The trajectory calculation unit 142 acquires the current velocity v of each joint from a sensor (such as the rotation sensor 5 of the drive wheel 22 and the rotation sensor 6 of the pivot shaft 4) of the omnidirectional mobile body 1. The velocity limit vector VMax of each joint, the acceleration limit vector AMax of each joint, and the position trajectory of each joint are set in advance in the trajectory calculation unit 142.

[0052] Each joint position trajectory is generated based on the desired motion of the omnidirectional mobile body 1. For example, each joint position trajectory may be created manually by the user, or may be generated using a trajectory planning algorithm such as RRT (Rapidly-exploring Random Tree). Each joint position trajectory is, for example, a trajectory candidate created by the candidate generation unit 11 as described above and finally output from the collision determination unit 13.

[0053] The trajectory calculation unit 142 places a virtual joint position Pv between P0 and P1 in the v direction from P0 on the trajectory parameter ss (dot) plane. At this time, the trajectory calculation unit 142 performs a search from a position where the distance between P0 and Pv is small. In this way, by performing a search from a position where the distance is small, a trajectory in a shorter time can be generated.

[0054] Note that if the distance between P0 and Pv is too small for the current velocity v, the forward integration may fail between P0, Pv, and P1. However, even if the forward integration fails, it will fail early in the forward integration, so the impact on the calculation time will be small.

[0055] As described above, if the forward integration fails, the trajectory calculation unit 142 displaces the virtual joint position Pv by a predetermined distance from P0 and performs the forward integration again. The trajectory calculation unit 142 repeats the search for the virtual joint position Pv until the forward integration is successful.

[0056] The trajectory calculation unit 142 converts the joint position trajectory into straight line segments and circular arc segments that can be expressed by the trajectory length s.

[0057] For example, as shown in FIG. 5, the trajectory calculation unit 142 converts P0 to Pv into a straight line segment a, converts Pv to P1 into a straight line segment b, converts P1 to P2 into a straight line segment c, converts the two straight line segments a and b into a continuous arc segment d, and converts the two straight line segments b and c into a continuous arc segment e.

[0058] The current velocity of the joint is v norm Let us assume that. The trajectory calculation unit 142 calculates the starting point (0, v norm In addition, the trajectory calculation unit 142 performs a backward integration from the end point (L, 0) on the trajectory parameter ss (dot) plane until it collides with the forward integration.

[0059] Here, the time optimization by the forward integration and backward integration is performed under the restriction of the acceleration limiting vector AMax of each joint. That is, the trajectory calculation unit 142 performs the above-mentioned forward integration and backward integration on the trajectory parameter ss (dot) plane based on the acceleration limiting vector AMax. Note that this process utilizes, for example, equations (21), (22), and (36) in the above Non-Patent Document 1, and these can be used.

[0060] As described above, AMax is an acceleration restriction vector for each joint, and is a k-dimensional vector with the number of joints k being the number of elements. In this embodiment, the joint parameters (x, y, φ) indicating the position and posture of the omnidirectional mobile body 1 can be regarded as joints. AMax includes information on the maximum acceleration of the arm joints and the bogie joints. The maximum acceleration of the bogie joints is the maximum acceleration of the joint parameters (x, y, φ) calculated by the constraint calculation unit 141. The maximum acceleration of the arm joints is set as a fixed value determined by the actuator, gear ratio, etc. In this embodiment, for example, there are five arm joints and three bogie joints, so AMax is an eight-dimensional vector.

[0061] As described above, the trajectory calculation unit 142 performs optimization processing by performing forward integration and backward integration on the trajectory parameter ss (dot) plane based on the acceleration limited vector AMax, and calculates a time trajectory on the trajectory parameter ss (dot) plane. As described above, when the forward integration and backward integration are successful and the optimization processing is successful, the trajectory calculation unit 142 outputs the calculated time trajectory to the control device 9.

[0062] The control device 9 controls the wheel actuator 7 and the turning actuator 8 based on the time trajectory calculated by the trajectory calculation unit 142. The control device 9 calculates a target value q(t) for the position and attitude of the main body unit 3 of the omnidirectional mobile body 1 based on the time trajectory. The control device 9 controls the wheel actuator 7 and the turning actuator 8 based on the calculated target value q(t) for the position and attitude of the main body unit 3 of the omnidirectional mobile body 1.

[0063] The target value q(t) of the position and orientation of the main body 3 of the omnidirectional mobile body 1 is calculated, for example, as follows. As a result of the above optimization process (forward integration and backward integration), pairs of s and s (dots) are obtained for each width Δs from s=0 to s=L. The obtained pair of s and s (dots) is compared with the previous pair of s and s (dots), thereby calculating the time t of the pair of s and s (dots). The function f of q = f(s) is derived when the joint position trajectory is converted into straight line segments and arc segments that can be expressed by the trajectory length s, as described above. Since the pairs of s, s (dots), and t are known, the relationship between the joint positions q and t can be known.

[0064] Next, a trajectory generation method according to this embodiment will be described with reference to a flowchart shown in FIG.

[0065] The constraint calculation unit 141 calculates the maximum acceleration of each of the joint parameters (x, y, φ) indicating the position and posture in the omnidirectional mobile body coordinate system based on Jacobian equation (3) derived based on the current joint state of the omnidirectional mobile body 1 (step S101).

[0066] The trajectory calculation unit 142 places a virtual joint position Pv on the moving direction v of the joint between the start point P0 of the joint position trajectory and the next point P1 on the trajectory parameter ss (dot) plane (step S102).

[0067] The trajectory calculation unit 142 converts the joint position trajectory into straight line segments and arc segments that can be expressed by the trajectory length s (step S103).

[0068] The trajectory calculation unit 142 calculates the current velocity of the joint as v norm Let the starting point of the orbital parameter ss (dot) on the plane be (0, v norm ) (step S104).

[0069] The trajectory calculation unit 142 calculates the maximum acceleration of the joint parameters (x, y, φ) calculated by the constraint calculation unit 141, and calculates the starting point (0, v norm ), while performing backward integration from the end point, to perform optimization processing and calculate a time trajectory (step S105).

[0070] As described above, according to the trajectory generation device 10 of this embodiment, the acceleration constraint calculation based on the omnidirectional mobile body coordinate system, the insertion of the virtual joint position Pv, ​​and the high-speed time optimization process are repeated, thereby shortening the operation time of the omnidirectional mobile body 1. Note that the calculated acceleration constraint based on the omnidirectional mobile body coordinate system becomes a useless value depending on the operation of the omnidirectional mobile body 1, but this disadvantage can be countered by performing repeated calculations.

[0071] Moreover, in the trajectory generation device 10 according to this embodiment, there is no need to convert the carriage trajectory into the wheel trajectory and vice versa, and the time optimization process is fast, so that the overall processing is speeded up. Furthermore, by inserting the virtual joint position Pv, ​​optimization taking into account the whole-body speed of the omnidirectional moving body 1 at that time becomes possible.

[0072] Furthermore, as described above, the trajectory generation device 10 according to this embodiment executes loop processing and also takes into account the whole-body speed of the omnidirectional mobile body 1, so it can dynamically change the whole-body trajectory for movement. For example, it can fully handle cases where the whole-body trajectory is changed to avoid an obstacle recognized during movement.

[0073] EMBODIMENT 2 In this embodiment, the constraint calculation unit 141 may calculate the maximum acceleration of the joint parameters (x, y, φ) as the acceleration constraint of the omnidirectional mobile body 1, and may also calculate the maximum velocity of the joint parameters (x, y, φ) as the velocity constraint. The trajectory calculation unit 142 performs optimization processing by performing forward integration and backward integration on the trajectory parameter ss (dot) plane based on the acceleration limit vector AMax including the maximum acceleration of the joint parameters (x, y, φ) calculated by the constraint calculation unit 141 and the velocity limit vector VMax including the maximum velocity of the joint parameters (x, y, φ).

[0074] The constraint calculation unit 141 calculates the maximum speed of the joint parameters (x, y, φ), which is a speed constraint similar to the acceleration constraint. Specifically, the constraint calculation unit 141 uses the above formula (1) to calculate the maximum speed ω of the left and right drive wheels 22, which has a speed constraint such that the speed direction of (x, y) is the input direction and the absolute value of the speed of (x, y) becomes large. R , and ω L Explore.

[0075] Specifically, the constraint calculation unit 141 performs the following processes (i) to (iii): R , ω L The maximum value of ω max Let us assume that.

[0076] (i) The constraint calculation unit 141 calculates (ω R , ω L ) to calculate the x and y velocities. (ii) The constraint calculation unit 141 calculates the x and y velocity directions based on the respective x and y velocities using the following equations. arctan(y velocity / x velocity) (iii) The constraint calculation unit 141 calculates the speed ω of the left or right driving wheel 22 whose speed direction is closest to the input direction. R , ω L Select .

[0077] The velocity of x and y is ω R , ω L Therefore, the magnitude of the velocity ((velocity of x) 2+(y velocity) 2 ) 1 / 2 ω where is the maximum R , ω L is the ω that we explored this time. R , ω L It will be on the edge of the range.

[0078] The constraint calculation unit 141 calculates ω based on the traveling direction in the omnidirectional mobile body coordinate system. S As the range of ω S Use the lower limit of ω S Specifically, when the direction in which the carriage unit 2 should face at the zeroth point of the input trajectory is positive as viewed from the current facing direction of the carriage unit 2, the constraint calculation unit 141 determines whether to use the upper limit of ω S The range of ω S If the upper limit of ω is set and the direction is positive, S The range of ω S Set the lower limit of.

[0079] In addition, ω S is the speed of the rotating shaft 4, and is set based on design values ​​such as the output of the actuator of the rotating shaft 4, the gear ratio, the mass of the main body 3, etc.

[0080] The constraint calculation unit 141 calculates the set ω S The range (lower or upper limit) of ω R , ω L and into the above formula (1) to calculate the maximum speed of φ. R , and α L Based on this, calculate the maximum velocity in x and y.

[0081] The constraint calculation unit 141 calculates the ω R and ω L and ω S The maximum speed of φ calculated based on the upper limit of R and ω L and ω SThe maximum speed of φ calculated based on the lower limit of and the maximum speed of φ may be compared, and the smaller one may be used as the final maximum speed of φ. This is to improve safety by making the maximum speed described below smaller.

[0082] The constraint calculation unit 141 outputs the calculated maximum velocity of the joint parameters (x, y, φ) to the trajectory calculation unit 142.

[0083] The trajectory calculation unit 142 performs an optimization process by performing forward integration and backward integration on the trajectory parameter ss (dot) plane based on the acceleration limit vector AMax including the maximum acceleration of the joint parameters (x, y, φ) calculated by the constraint calculation unit 141 and the velocity limit vector VMax including the maximum velocity of the joint parameters (x, y, φ), and calculates a time trajectory.

[0084] Next, the results of an experiment using the trajectory generation device 10 according to this embodiment will be described. In this embodiment, an operation experiment of periodic whole-body trajectory planning of HSR was performed. The whole-body trajectory issuance period by the whole-body trajectory planning was set to 5 [Hz].

[0085] First, as an experiment to optimize the whole-body motion, we set the hand position and posture as constraints and conducted an experiment in which they were changed continuously. Figure 7 shows the whole-body motion. As shown in Figure 7, the HSR continues to move in order to satisfy the given hand position and posture, and the hand tracks the target.

[0086] Next, to evaluate the optimization process according to this embodiment, an experiment was conducted limited to the movement of the cart unit 2. For the movement of the cart unit 2, a periodic whole-body trajectory plan of HSR was used, and the constraint conditions for the position and attitude of the cart unit 2 were updated as follows. 1. Place the HSR at (x, y, φ) = (0.0, 0.0, 0.0). 2. Set the goal to (x, y, φ) = (1.0, 0.0, 0.0). 3. Wait until the distance to the target is less than 0.25m. 4. Given a goal of (x, y, φ) = (1.0, 1.0, 1.57). 5. Wait until the distance to the target is less than 0.25m. 6. Give the goal (x, y, φ) = (1.0, 0.0, 0.0). 7. Wait for the operation to complete.

[0087] The upper limits of joint velocity and acceleration are set as follows: ω R =8.5[rad / sec], ω L =8.5[rad / sec], ω H = 1.8 [rad / sec], α R =5.0[rad / sec 2 ], α L =5.0[rad / sec 2 ], α H =1.8[rad / sec 2 ].

[0088] When the operation of the bogie unit 2 was executed with these targets and parameter settings, the operation time was 16.3 [sec]. On the other hand, when the same command value as above was inputted into the conventional optimization module, the operation time was 15.8 [sec]. It can be said that there is no big difference between the operation time by the optimization process of the trajectory generation device 10 according to this embodiment and the operation time by the conventional optimization module.

[0089] On the other hand, the calculation time by the conventional optimization module is 75 [msec] on average, whereas the calculation time by the optimization process of the trajectory generation device 10 according to this embodiment is 5 [msec] on average per optimization process. In other words, it is found that the calculation time by the optimization process of the trajectory generation device 10 according to this embodiment is overwhelmingly shorter than the calculation time by the conventional optimization module.

[0090] Next, a description will be given of plots of the movement of the cart unit 2. Fig. 8 is a diagram showing changes in command velocity (joint velocity command value) to each joint. Fig. 9 is a diagram showing changes in command acceleration (joint acceleration command value) to each joint.

[0091] In Figures 8 and 9, the maximum values ​​of the joint velocities and accelerations that have been set are indicated as Max. Figure 10 is a diagram showing the actual vehicle velocity in the omnidirectional mobile body coordinate system when the vehicle unit is in operation. The two arrows in Figure 10 indicate the points where the target vehicle position and attitude are switched.

[0092] The command speed adheres to the set upper limit. Meanwhile, the command acceleration sometimes temporarily exceeds the given upper limit. However, looking at the actual operating speed, it operates continuously, and it can be said that the operation is not affected by the temporary exceeding of the command value. In other words, it can be seen that there is no problem with the operation of the cart unit 2 due to the optimization process of the trajectory generation device 10 according to this embodiment.

[0093] Although some embodiments of the present invention have been described, these embodiments are presented as examples and are not intended to limit the scope of the invention. These novel embodiments can be implemented in various other forms, and various omissions, substitutions, and modifications can be made without departing from the spirit of the invention. These embodiments and their modifications are included in the scope and spirit of the invention, and are included in the scope of the invention and its equivalents described in the claims.

[0094] The present invention can also be implemented by causing a processor to execute a computer program to perform the processing shown in FIG.

[0095] The program can be stored and provided to a computer using various types of non-transitory computer readable media. The non-transitory computer readable media includes various types of tangible storage media. Examples of the non-transitory computer readable media include magnetic recording media (e.g., flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (e.g., magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / Ws, and semiconductor memories (e.g., mask ROMs, PROMs (Programmable ROMs), EPROMs (Erasable PROMs), flash ROMs, and RAMs (random access memories)).

[0096] The program may be provided to the computer by various types of transitory computer readable media. Examples of the transitory computer readable medium include an electric signal, an optical signal, and an electromagnetic wave. The transitory computer readable medium can provide the program to the computer via a wired communication path such as an electric wire and an optical fiber, or via a wireless communication path.

[0097] Each unit constituting the trajectory generation device 10 according to each of the above-mentioned embodiments can be realized not only by a program, but also partly or entirely by dedicated hardware such as an ASIC (Application Specific Integrated Circuit) or an FPGA (Field-Programmable Gate Array). [Explanation of symbols]

[0098] REFERENCE SIGNS LIST 1 Omnidirectional mobile body, 2 Cart unit, 3 Main body unit, 4 Swivel axis, 5 Rotation sensor, 6 Rotation sensor, 7 Wheel actuator, 8 Swivel actuator, 9 Control device, 10 Trajectory generation device, 11 Candidate generation unit, 12 Candidate evaluation unit, 13 Collision determination unit, 14 Optimization processing unit, 21 Auxiliary wheels, 22 Driving wheels, 141 Constraint calculation unit, 142 Trajectory calculation unit

Claims

1. A trajectory generation device for an omnidirectional mobile body that is movable in all directions and includes a carriage unit having a plurality of drive wheels and a main body unit that is rotatably mounted on the carriage unit via a rotating shaft, the trajectory generation device comprising: a constraint calculation means for calculating maximum accelerations of joint parameters indicating a position and an attitude of the omnidirectional mobile body in a coordinate system based on a Jacobian equation derived based on a current joint state of the omnidirectional mobile body, the maximum accelerations being calculated based on the degrees of freedom of a main body part defined by drive wheels and a rotating shaft of the omnidirectional mobile body as joints; a trajectory calculation means for performing an optimization process by performing forward integration and backward integration on a plane of a trajectory parameter s parameterized by time and its differential value s (dots) based on an acceleration restriction vector including the maximum acceleration of the joint parameters calculated by the constraint calculation means and having the number of dimensions equal to the number of joints of the omnidirectional mobile body, thereby calculating a time trajectory; Equipped with The trajectory calculation means places a virtual joint position on the trajectory parameter s-s (dot) plane between the start point of the joint position trajectory and the point next to the start point in the moving direction of the joint, converts the joint position trajectory into a straight line segment and a circular arc segment that can be expressed by a trajectory length, and calculates the current velocity of the joint by v ｎｏｒｍ As the starting point (0, v ｎｏｒｍ ) while performing a forward integral from the end point of the joint position trajectory until it collides with the forward integral. A trajectory generation device for an omnidirectional moving object.

2. 2. The trajectory generation device for an omnidirectional moving body according to claim 1, the constraint calculation means calculates maximum acceleration and maximum velocity of joint parameters indicating a position and an attitude of the omnidirectional mobile body in a coordinate system based on a Jacobian equation derived based on a current joint state of the omnidirectional mobile body; The trajectory calculation means performs an optimization process by performing forward integration and backward integration on the trajectory parameter s-s (dot) plane based on an acceleration limit vector including a maximum acceleration of the joint parameter and a velocity limit vector including a maximum velocity of the joint parameter, which are calculated by the constraint calculation means, to calculate a time trajectory. A trajectory generation device for an omnidirectional moving object.

3. 3. The trajectory generation device for an omnidirectional moving body according to claim 1, The constraint calculation means Using the Jacobian equation, the acceleration direction of the position (x, y) of the carriage is the traveling direction of the carriage, and the acceleration α of the right driving wheel is set so that the absolute value of the acceleration of (x, y) becomes large. Ｒ , and the acceleration of the left driving wheel α Ｌ Each of them explored The preset acceleration α of the rotation axis Ｓ and the range of the searched α Ｒ and α Ｌ Based on the above, the maximum acceleration in the direction φ of the main body is calculated, and the searched α Ｒ and α Ｌ and outputting the maximum acceleration of the joint parameters (x, y, φ) to the trajectory calculation means. A trajectory generation device for an omnidirectional moving object.

4. 4. The trajectory generation device for an omnidirectional moving body according to claim 1, the trajectory calculation means searches, on the trajectory parameter s-s (dot) plane, in order from a position where the distance between the start point of the joint position trajectory and the next point is smallest, until the forward integration and the backward integration are successful, and determines the position of the virtual joint position; A trajectory generation device for an omnidirectional moving object.

5. A method for generating a trajectory of an omnidirectional mobile body that is movable in all directions and includes a carriage unit having a plurality of drive wheels and a main body unit that is rotatably mounted on the carriage unit via a rotating shaft, the method comprising the steps of: calculating maximum accelerations of joint parameters indicating a position and an attitude of the omnidirectional mobile body in a coordinate system based on a Jacobian equation derived based on a current joint state of the omnidirectional mobile body, with the degrees of freedom of a main body part due to drive wheels and a rotating shaft of the omnidirectional mobile body being joints; A step of performing an optimization process by performing forward integration and backward integration on a plane of a trajectory parameter s and its differential value s (dots) parameterized by time based on an acceleration limiting vector including the maximum acceleration of the calculated joint parameters and having the number of dimensions equal to the number of joints of the omnidirectional mobile body, thereby calculating a time trajectory; Including, On the trajectory parameter s-s (dot) plane, a virtual joint position is placed in the moving direction of the joint between the starting point of the joint position trajectory and the next point of the starting point, the joint position trajectory is converted into a straight line segment and a circular arc segment that can be expressed by a trajectory length, and the current velocity of the joint is expressed by v ｎｏｒｍ As the starting point (0, v ｎｏｒｍ ) while performing a forward integral from the end point of the joint position trajectory until it collides with the forward integral. A method for generating trajectory for an omnidirectional moving object.

6. A program for an omnidirectional mobile body that is movable in all directions and includes a carriage unit having a plurality of drive wheels and a main body unit that is rotatably mounted on the carriage unit via a pivot shaft, the program comprising: calculating maximum accelerations of joint parameters indicating a position and an attitude of the omnidirectional mobile body in a coordinate system based on a Jacobian equation derived based on a current joint state of the omnidirectional mobile body, with the degrees of freedom of a main body part due to the drive wheels and the rotating shaft of the omnidirectional mobile body being joints; A process of performing an optimization process by performing forward integration and backward integration on a plane of a trajectory parameter s and its differential value s (dots) parameterized by time based on an acceleration limiting vector including the maximum acceleration of the calculated joint parameters and having the number of dimensions equal to the number of joints of the omnidirectional mobile body, thereby calculating a time trajectory; Run the following on your computer: On the trajectory parameter s-s (dot) plane, a virtual joint position is placed in the moving direction of the joint between the starting point of the joint position trajectory and the next point of the starting point, the joint position trajectory is converted into a straight line segment and a circular arc segment that can be expressed by a trajectory length, and the current velocity of the joint is expressed by v ｎｏｒｍ As the starting point (0, v ｎｏｒｍ ) while performing a forward integral from the end point of the joint position trajectory until it collides with the forward integral. An omnidirectional mobile program.

Images