JP5173958B2 - Moving body posture generation method and moving body posture generation apparatus - Google Patents

Description

The present invention relates to a moving object trajectory generating method and a moving object trajectory generating apparatus for generating an optimal trajectory of an object moving in an environment where an obstacle exists.

  Conventional robot motion design methods include online teaching in which a robot is actually operated at a work site to teach, or offline teaching in which teaching data is created by a computer or the like before the robot is operated. Online teaching is widely used in industrial robots that perform predetermined operations such as assembly work and welding work, and has an advantage that the operation can be designed according to the actual work environment. On the other hand, there are drawbacks in that teaching takes a lot of time and skill is required for operation design. In offline teaching, it is necessary to perform generation of a trajectory for avoiding interference by trial and error while confirming using a simulation or the like, and much effort is required for design and verification.

  On the other hand, as an attempt to automate the motion design of the robot's hand path, there is a path search method using a potential field (see, for example, Patent Document 1). In this method, a virtual potential is set in the work space so that the target position is minimum and the current position is maximum. Furthermore, by providing the obstacle with a large potential, a path to the target position can be generated while avoiding the obstacle by generating a trajectory in a direction in which the potential decreases.

  Further, in order to operate the robot along the generated hand path, it is necessary to determine the entire posture composed of the robot link and joint from the hand position / posture information on the path. In general, the entire posture is determined by a calculation called inverse kinematics in which each joint angle for realizing the position and posture of the hand is calculated as an input condition.

JP 2003-280710 A

  The route search method using the potential field described above has a problem in that a local minimum occurs in the potential field, and a trajectory cannot be generated if the local minimum occurs during the route search. The local minimum refers to a region where the potential is locally minimized. If the local minimum area is entered during the route search, the surrounding potential is higher than that of the local minimum area, and the route search cannot be continued and the trajectory cannot be generated.

  In an environment with obstacles with complex shapes, it is very difficult to set a potential field that does not have a local minimum. In addition, if a detour route is generated to avoid local minimum, the trajectory becomes longer and it becomes a trajectory including unnecessary inflection points, so it is necessary to optimize the trajectory, etc. was there.

  In addition, the method of obtaining the overall posture of the robot that realizes the generated route by inverse kinematics is used when the overall posture that satisfies the position and posture of the hand on the route does not physically exist due to interference with obstacles, etc. Cannot be asked. Furthermore, in a robot having a structure with a very large number of joints, it is difficult to uniquely determine a solution because there are a large number of combinations of joint angles obtained from inverse kinematics.

  In view of the above-mentioned problems, the object of the present invention is to automatically generate a target trajectory of the robot's hand without trial and error, and the same applies to the posture of the entire robot when moving along the target trajectory. It is an object of the present invention to provide a mobile body posture generation method and a mobile body posture generation apparatus that can be automatically generated.

In order to achieve the above object, a mobile body posture generation method according to the present invention is a mobile body posture generation method for generating a posture of an entire mobile body based on a motion trajectory of a part of the mobile body, and a space in which the mobile body moves. A repulsive force vector field setting step for setting a repulsive force vector field composed of a repulsive force vector with an obstacle existing as a generation source, and a moving object shape model for setting a representative point at which repulsive force and attractive force act on the three-dimensional shape model of the moving object A setting step, a force / moment calculation step for calculating the force and moment acting on each joint axis of the moving body from the repulsive force acting on the representative point, a reference point is set on the motion trajectory, and an attractive force toward the reference point is set A torque acting on each joint axis of the moving body is calculated from an attractive force vector setting step to be applied to the moving body, and a force and a moment acting on the three-dimensional shape model of the moving body. Characterized in that it comprises an attitude generating step of changing the angle of each joint axis on the basis of the click, the.

  The moving body posture generation method according to the present invention is a moving body posture generation method for generating the posture of the entire moving body based on the motion trajectory of a part of the moving body, and is a space setting for setting a space in which the moving body moves. An initial position setting step for setting an initial position at which a part of the moving body starts operating in the space; a target position setting step for setting a target position for a part of the moving body in the space; A repulsive force vector field setting step for setting a repulsive force vector field including a repulsive force vector having an obstacle existing in space as a source, and connecting the initial position and the target position, and the gap between the initial position and the target position A motion trajectory model setting step for setting a motion trajectory model having an action point on which a repulsive force vector acts and having both ends fixed at the initial position and the target position; A motion trajectory generation step in which the repulsive force vector of the Kuttle field is applied to the action point and the deformed motion trajectory model is the motion trajectory of the moving body, and a repulsive force and an attraction force act on the three-dimensional shape model of the moving body A moving body shape model setting step for setting points, a force / moment calculation step for calculating the force and moment acting on each joint axis of the moving body from the repulsive force acting on the representative point, and setting a reference point on the generated motion trajectory And calculating the torque acting on each joint axis of the moving body from the force and moment acting on the three-dimensional shape model of the moving body, and calculating the torque acting on each joint axis of the moving body. And a posture generation step of changing the angle of each joint axis based on.

  The mobile body posture generation device according to the present invention is a mobile body posture generation device that generates the posture of the entire mobile body based on the motion trajectory of a part of the mobile body, and is configured to detect obstacles that exist in the space in which the mobile body moves. Repulsive force vector field setting means for setting a repulsive force vector field composed of repulsive force vectors as generation sources, moving body shape model setting means for setting a representative point where repulsive force and attractive force act on the three-dimensional shape model of the moving body, and representative points A force / moment calculation means for calculating the force and moment acting on each joint axis of the moving body from the repulsive force acting on the moving body, a reference point is set on the movement trajectory, and an attractive force directed to the reference point is applied to the moving body The torque acting on each joint axis of the moving body is calculated from the force vector moment and the force and moment acting on the three-dimensional shape model of the moving body, and the angle of each joint axis is changed based on the torque. And orientation generation unit that, characterized in that it comprises a.

  The mobile body posture generation device according to the present invention is a mobile body posture generation device that generates the posture of the entire mobile body based on the motion trajectory of a part of the mobile body, and sets the space in which the mobile body moves Means, initial position setting means for setting an initial position at which a part of the moving body starts operating in the space, target position setting means for setting a target position of a part of the moving body in the space, Repulsive force vector field setting means for setting a repulsive force vector field comprising a repulsive force vector having an obstacle present in space as a source, and connecting the initial position and the target position, and the initial position and the target position between A motion trajectory model setting means for setting a motion trajectory model having a point of action on which a repulsive force vector acts and having both ends fixed at the initial position and the target position; and the repulsive force vector field. A moving trajectory generating means for causing a tortle to act on the action point and using the deformed motion trajectory model as an operating trajectory of the moving body; Shape model setting means, force / moment calculation means for calculating the force and moment acting on each joint axis of the moving body from repulsive force acting on the moving body, and setting a reference point on the generated motion trajectory The attractive force vector setting means for applying the attractive force to the moving body, the torque acting on each joint axis of the moving body from the force and moment acting on the three-dimensional shape model of the moving body, and calculating the torque of each joint axis based on the torque. And posture generating means for changing the angle.

  According to the mobile body posture generation method and the mobile body posture generation apparatus of the present invention, even when the posture generation by the conventional inverse kinematics cannot physically determine the posture, the solution in the vicinity is automatically obtained. It is possible to obtain a posture. In addition, even when the degree of freedom of the moving body is large and there are a large number of postures of the moving body, the attitude can be uniquely generated.

The flowchart of the mobile body attitude | position production | generation by a 1st Example. The conceptual diagram which shows the outline | summary of the repulsive force vector field set based on an obstruction. The conceptual diagram of the coupled spring mass structure which comprises a movement track model. (A) The conceptual diagram which shows the state of the motion track model before a motion track generation process. (B) The conceptual diagram which shows the state of the motion track model after a motion track generation process. The conceptual diagram which shows the state which made the robot manipulator a three-dimensional shape model, and set the representative point on which a repulsive force vector acts. The figure which shows the concept of making the attractive force vector from the reference point set on the motion trajectory act on the hand of the robot manipulator made into the three-dimensional shape model. The figure which shows the concept of the process which sets the plane which includes a reference point perpendicular | vertical with respect to an operation | movement trajectory, and converges the operation | movement trajectory of a part of moving body to the reference point vicinity. The flowchart of a mobile body attitude | position production | generation by 2nd Example. The conceptual diagram which shows the state before the movement of an obstruction in a 2nd Example. The conceptual diagram which shows the state which the obstruction moved in the 2nd Example. The conceptual diagram which shows the state which reproduced | regenerated the operation | movement trajectory with the movement of the obstruction in the 2nd Example.

Embodiments of the present invention will be described below with reference to the drawings.

  A first embodiment of the present invention will be described below with reference to the drawings. FIG. 1 is a flowchart describing the main steps of the moving body posture generation method according to the present embodiment. Processing performed in the moving body posture generation method of the present embodiment will be described with reference to FIG. In each embodiment, the following description will be made assuming that the moving body is a robot manipulator, and a part of the moving body is a hand of the robot manipulator.

  In general, a robot manipulator is composed of a joint and a link mechanism. There are various types of joints such as rotary joints and linear joints as well as spherical joints. There are cases where the joint operates actively with a motor or the like, or passively without a power source. Each joint is linked by a link mechanism, and there are a serial link type in which joints and link mechanisms are alternately arranged, and a parallel link type in which combinations of joints and links are arranged in parallel.

  Hereinafter, a serial link type robot manipulator having a rotating joint will be described as an example.

  First, a process for defining a work space in which the robot manipulator performs work is performed (step S1). The work space is a space constructed on the basis of information on an actual space including an initial position and a work position of the robot manipulator and an intermediate position between the initial position and the work position. This work space is set using design information such as the facility where the work is performed, for example, three-dimensional CAD data, or is set using data collected by camera images or measurement by various sensors, or set manually. It may be possible to do so.

  Specifically, for example, an operation device of a robot manipulator is associated, and this is performed by an arithmetic device and an input device constituting a three-dimensional CAD system that can create, store, and modify various three-dimensional shape models. That is, it is performed by inputting design information and collected data of a facility or the like to the above-described input device, or arbitrarily inputting facility information or the like, and setting a three-dimensional shape model corresponding to the above-described work space.

  Next, a process for generating a repulsive vector field on the work space is performed (step S2). The repulsive force vector field is generated by setting a three-dimensional lattice on the work space and defining a repulsive force vector at each lattice point. This repulsive force vector is set by using an obstacle in the work space defined in step S1 as a generation source. The direction of the repulsive force vector set at each lattice point is the normal direction of the obstacle surface, that is, the direction perpendicular to the obstruction surface, and the magnitude of the repulsive force vector is set so as to be closer to the obstacle. When there are a plurality of obstacles in the work space, for example, a repulsive force vector generated from an obstacle closest to each lattice point is finally set as a repulsive force vector of the lattice point.

  FIG. 2 shows an example of the work space defined in step S1 and the repulsive force vector field generated in step S2. FIG. 2 is a conceptual diagram showing an overview of obstacles and repulsive force vector fields set in the work space. The repulsive force vector field shows only the approximate direction of the repulsive force vector, and the three-dimensional lattice set on the work space is omitted. A cylindrical obstacle 10 is set in the work space, and a repulsive force vector 11 in a direction away from the obstacle 10 is set.

  When the repulsive force vector field is set, when the obstacle 10 has a complicated shape and has a shape or arrangement in which the hand of the robot manipulator is difficult to pass, such as a protrusion or an extremely narrow portion, the repulsive force vector 11 in the vicinity of the corresponding portion is taken. You may perform weighting, such as enlarging.

  Next, a process of generating and arranging an operation trajectory model on the work space so as to connect the initial position of the hand of the robot manipulator and the target position is performed (step S3). An example of the structure of this motion trajectory model will be described with reference to FIG. The motion trajectory model has a coupled spring mass structure in which a plurality of connecting springs 22 that are a combination of a tension spring 20 and a hinge spring 21 are combined, and a mass point 23 is set at a connecting portion between the connecting springs 22. Further, the mass points 23a and 23b set at both ends of the motion trajectory model are fixed to the initial position of the hand and the mass point 23b is fixed to the target position of the hand when the motion trajectory model is arranged. In FIG. 3, two coupling springs 22 are illustrated as being coupled, but the number of coupling springs 22 can be arbitrarily set.

  In FIG. 3, the motion trajectory model is arranged so that the initial position of the hand and the target position are linearly connected, but it is not necessary to connect them linearly, and the initial position and the target position are not connected in space. What is necessary is just to arrange | position so that it may connect arbitrarily.

  Next, the repulsive force vector 11 set in step S2 is applied to the motion trajectory model generated in step S3, and processing for generating a motion trajectory is performed (step S4). A repulsive force vector obtained by spatially interpolating the magnitudes and directions of a plurality of repulsive force vectors 11 set around each mass point 23 is applied to each of the mass points 23 set in the motion trajectory model. Then, the tension spring 20 and the hinge spring 21 constituting the motion trajectory model are deformed. At this time, the hinge spring 21 applies a force in a direction to keep the adjacent mass points 23 in a straight line shape, and prevents the robot manipulator motion trajectory 31 from being bent more than necessary.

  Finally, the motion trajectory model is deformed into a shape in which the repulsive force vector 11 acting on each mass point 23 and the tension of the tension spring 20 and the hinge spring 21 are balanced. The trajectory drawn by the deformed motion trajectory model is the motion trajectory of the robot manipulator.

  An example of generation of the motion trajectory in step S4 will be described with reference to FIG. 4A shows a state where the repulsive force vector 11 is not applied to the motion trajectory model, and FIG. 4B shows a state where the repulsive force vector 11 is applied to the motion trajectory model to generate the motion trajectory. In FIG. 4, a simplified two-dimensional diagram is shown, and the connecting springs 22 connecting the mass points 23 are also shown in a simplified manner.

  In step S3, the motion trajectory model is arranged, but the initial state may penetrate the obstacle 10 as shown in FIG. As described above, the mass point 23a is fixed at the initial position of the hand and the mass point 23b is fixed at the target position of the hand. When the repulsive force vector 11 is applied to the motion trajectory model, the motion trajectory is deformed into the shape shown in FIG. 4B, and a motion trajectory that detours the obstacle 10 from the right side of the page is generated. The mass points 23a and 23b do not move because the positions are fixed.

  Next, with respect to the motion trajectory generated in step S4, a process for smoothing the motion trajectory is performed by interpolating between the mass points 23 of the motion trajectory model (step S5).

  An example of the moving body shape model setting in step S6 will be described with reference to FIG. In FIG. 5, the three-dimensional shape model 25 of the robot manipulator is set in the above-described work space, and a plurality of representative points 26 for applying the repulsive force received from the repulsive force vector field are defined on the set three-dimensional shape model 25. This representative point is set in the outer shape or inside of the robot manipulator in order to avoid interference with the structure. For example, in the case of a robot manipulator having a shape as shown in FIG. 5, the representative point 26 is set evenly on the outer surface of a rectangular parallelepiped or a cylindrical portion that is a component of the model (because it becomes complicated, In the figure, the representative point 26 is partially omitted. In addition, when the hand that is a part of the moving body has a work tool, the representative point 26 is also set on the hand when there is a size that may cause interference with an obstacle.

  Next, the repulsive force in the magnitude and direction of the repulsive force vector 11 set in step S2 is applied to the representative point 26 defined in step S6, and the repulsive force is applied to each link of the manipulator and the joint axis associated with each link. The sum of moments to be calculated is calculated (step S7).

  Next, as shown in FIG. 6, a plurality of reference points 27 are set on the motion trajectory from the initial position 27a to the target position 27b generated in step S4, and the robot manipulator An attractive vector 31 in the direction of the reference point 27 based on the current position of the hand and the positional deviation of the reference point 27 is applied to the hand of the robot manipulator (step S8). At this time, the magnitude of the attractive force vector 31 is calculated and set from, for example, a component proportional to the position deviation amount, a component obtained by integrating the position deviation, a component obtained by differentiating the time variation of the position deviation, or a combination thereof. .

  Next, the repulsive force, moment and attractive force acting on the robot manipulator calculated in step S7 and step S8 are divided by the number of representative points 26 set in each link, and the average of each is obtained with the joint axis as the reference position. Calculate for each link.

Furthermore, by using the Jacobian matrix that converts the joint angular velocity of the current robot manipulator into the velocity of the hand in Cartesian coordinates, the average (Fx, Fy) of the repulsive force (or the sum of repulsive force and attractive force) calculated for each hand and each link , Fz) and the average of moments (Mx, My, Mz), the torque acting on each joint axis is obtained by equation (1).

n is a number counted from the base side of the joint axis. For example, the torque τ obtained from acting on a joint axis is the torque τ of the joint axis on the base side (the side farther from the hand) of the robot manipulator than the joint axis. In the case of F acting on, the torque τ acting on all joint axes is obtained. The finally calculated torque τ is added up for each joint axis.

Finally, by changing the joint axis angle based on the calculated magnitude of the torque, the overall posture of the robot manipulator is changed until the repulsive force acting on the entire robot manipulator and the attractive force of the hand are balanced (step S9).

  The advantage of calculating the torque acting on each joint axis using the Jacobian matrix is that the calculation is easy because it can be calculated from forward kinematics. In general, it is necessary to solve inverse kinematics in order to calculate the robot manipulator posture, that is, each joint angle, from the position and posture of the hand of the robot manipulator, but the inverse kinematic solution requires a computational load compared to forward kinematics. large.

  Through the series of processing from step S6 to step S9, the hand of the robot manipulator is moved from the motion trajectory obtained in step S5 and the current robot manipulator posture to the reference point 27 defined on the motion trajectory. The entire posture of the robot manipulator is automatically generated.

  Next, the reference point on the motion trajectory is updated to another reference point (step S10), and all the references set on the motion trajectory from the initial position 27a to the target position 27b by repeating steps S7 to S9. The overall posture of the robot manipulator at point 27 is generated (step S11). Further, by calculating the posture between the reference points by interpolation or the like, a smooth motion posture from the initial position 27a to the target position 27b can be generated (step S12).

  Through the series of processes described above, the motion trajectory of the hand of the robot manipulator that avoids the interference with the structure and the motion posture of the robot manipulator are generated. According to such motion generation, it is possible to generate the optimal motion trajectory and motion posture of the moving body without causing a local minimum problem in the potential method.

  Further, when an obstacle that moves as described in step S1 is set, it is also possible to perform processing for updating the shape of the repulsive force vector field and the motion trajectory model in real time. Further, when the moving range of the obstacle is determined, it is possible to set the entire moving range of the obstacle as one obstacle.

  In addition, since it is possible to collect and set structure design information and actual work environment information, it can be applied to any facility at the design / planning stage or a facility that has already been constructed.

  Further, the spring constants of the tension spring 20 and the hinge spring 21 constituting the motion trajectory model can be arbitrarily set. When the repulsive force vector field is the same, the larger the spring constant, the harder the deformation of the motion trajectory model. Conversely, the smaller the spring constant, the easier the deformation of the motion trajectory model. Therefore, for example, in the case of a manipulator with a large hand shape, by reducing the spring constant, an operation trajectory that is further away from the obstacle 10 than the operation trajectory when the spring constant is fixed is generated. On the contrary, when the hand is small, by increasing the spring constant, an operation trajectory closer to the obstacle 10 than the operation trajectory when the spring constant is fixed is generated. In other words, it is possible to arbitrarily change the size of the avoidance trajectory of a part of the moving body with respect to the obstacle 10 by arbitrarily setting the spring constant.

  That is, an operation trajectory that secures an interference avoidance distance between the hand of the robot manipulator and the obstacle 10 according to the size of the hand can be generated by the above-described method. Further, the spring constants of the springs constituting the motion trajectory model need not be the same. For example, the spring constant is partially increased in accordance with the shape of the obstacle 10 in the work space 1 such as increasing the spring constant in a place where there are few obstacles 10 and decreasing the spring constant in a place where there are many obstacles 10 and narrow. By changing, it is possible to obtain a more linear trajectory without depending on the shape of the obstacle 10 at a location where the spring constant is large, and an interference avoidance trajectory along the shape of the obstacle 10 at a location where the spring constant is small. Can be obtained.

  Further, when generating the motion trajectory, the movable range of the robot manipulator may be set as the constraint condition for each of the mass points 23. That is, each of the mass points 23 moves in the work space by applying the repulsive force vector 11, but at this time, by setting it so that it cannot move outside the movable range of the robot manipulator, it is surely within the movable range of the robot manipulator. It is possible to generate a motion trajectory.

  In the present embodiment, the motion trajectory model has been described as having the mass points 23 arranged discretely at intervals, but the mass of the motion trajectory model as a whole is assumed to be that the mass points 23 are continuously arranged. It is also possible to have it.

  In addition, as shown in FIG. 7, a plane 29 including the reference point 27c on the motion trajectory and perpendicular to the motion trajectory is defined, and a positional deviation (for example, the current position) between the current position 28 of the hand of the robot manipulator and the vertical plane 29 is defined. By setting the attractive force vector 31 according to the shortest distance between the position 28 and the vertical plane 29) and applying this attractive force to the hand, the final hand position of the robot manipulator is set in the vertical plane including the reference point 27c. It can also be converged to. By generating a robot manipulator posture in the vicinity of the reference point 27c when the entire posture of the robot manipulator that can converge the position of the hand of the robot manipulator to the reference point 27c does not physically exist by using this method. Can do.

  Further, when calculating the amount of change in the joint axis angle based on the torque acting on each joint axis, the ease of movement for each joint can be set by setting the weighting coefficient for each joint.

  Furthermore, when this weight coefficient is variable, when the posture of the robot manipulator approaches a singular posture, the singular posture is avoided by changing the ease of change of the angle of the joint axis related to the singular posture. be able to.

  Further, in the attractive force vector setting step in step S8, the entire robot manipulator can be moved by applying an attractive force directed to the installation point of the robot manipulator in the vertical plane including the reference point 27 and the reference point 27.

  This can be applied not only to a robot manipulator but also to a moving body having a plurality of joints, such as a walking robot, while automatically generating a posture avoiding interference and moving it to a target position.

  A second embodiment of the present invention will be described below with reference to the drawings. In addition, the same code | symbol is attached | subjected to the same structure as a 1st Example, and the overlapping description is abbreviate | omitted.

  FIG. 8 is a flowchart describing the steps of generating the moving body posture from step S1 to step S12 of the first embodiment. The moving body posture generation method according to the present embodiment assumes a work environment in which an obstacle moves, and the processing after the moving body posture is generated is different from that of the first embodiment.

  First, the same processing as in the first embodiment is performed from the definition of the work space (step S1) to the generation and arrangement of the motion trajectory model (step S12).

  Next, a process of moving the obstacle 10 arranged in the work space is performed (step S21).

  Next, the repulsive force vector field accompanying the movement of the obstacle 10 is updated. In addition, the repulsive force vector acting on the motion trajectory model is updated in accordance with the renewal of the repulsive force vector field, and processing for updating the motion trajectory model is performed. Further, using the updated repulsive force vector field and the motion trajectory model, a process for updating the posture of the robot manipulator is performed (step S22).

  Next, processing for determining whether or not the tension of each spring constituting the motion trajectory model exceeds a predetermined threshold is performed for the motion trajectory model updated in step S22 (step S23).

  If it is determined in step S23 that the spring tension is lower than the threshold value, the moving body is moved (step S24). Then, it is determined whether or not a part (hand) of the moving body has reached the target position (step S25). If it has reached, the process ends. If not, the process returns to step S21, and the obstacle 10 Repeat steps after moving.

  If it is determined in step S23 that the spring tension is higher than the threshold value, processing for rearranging the motion trajectory model is performed (step S26). This rearrangement of the motion trajectory model is different from step S4 in that in addition to the updated repulsive force vector field, the current position of a part of the moving body is considered. In step S4, only the initial position and the target position of the hand are arranged as restraint points of the motion trajectory model. However, in step S22, the motion trajectory model is set using the current position of the hand in addition to the initial position and the target position of the hand. Rearrange. After the rearrangement of the motion trajectory model, the steps after step S4 are repeated again.

  With such processing, it is possible to sequentially update the motion trajectory and the posture of the robot manipulator as the obstacle 10 moves. Further, by determining the tension of the spring constituting the motion trajectory model, when the motion trajectory model updated by the movement of the obstacle 10 draws an extreme trajectory, etc., by resetting the motion trajectory model, It is possible to automatically change to the optimal motion trajectory and robot manipulator posture considering the current position of the moving body.

  An example of the operation trajectory changing process according to the present embodiment will be described with reference to the drawings. Note that illustration of the posture of the robot manipulator is omitted. 9, 10, and 11 are conceptual diagrams for explaining the processing after step S <b> 17 according to the present embodiment. FIG. 9 shows a state before the obstacle 10 moves, and FIG. 10 shows the obstacle 10. FIG. 11 shows an example in which the motion trajectory is changed through step S26 after the movement.

  FIG. 9 shows a state in which the motion trajectory is first generated in step S5. The obstacle 10 has a complicated shape, and the motion trajectory model takes a trajectory that detours so as to pass through the recess of the obstacle 10. An arrow 40 indicates the moving direction of the obstacle 10, and a broken line linearly connects an initial position and a target position of a part (hand) of the moving body and corresponds to the position of the motion trajectory model before deformation.

  FIG. 10 shows a state in which the obstacle 10 has moved in step S21 and the repulsive force vector field and the motion trajectory have been updated in step S22. As the obstacle 10 moves in the direction of the arrow 40, it can be seen that the motion trajectory model takes a trajectory that largely bypasses the obstacle 10.

  FIG. 11 shows a state in which it is determined in step S23 that the spring tension of the motion trajectory model is higher than the threshold value, and the motion trajectory is changed in step S5 after the rearrangement of the motion trajectory model in step S26. In FIG. 11, it is illustrated that the hand has moved to the position of the mass point 23c at the time of step S26. Therefore, in addition to the initial position and the target position of the hand, the position of the mass point 23c is also a constraint point of the motion trajectory model. As a result of the motion trajectory being changed, the obstacle 10 is moved from the opposite side to FIG. 19 and FIG. The trajectory takes a detour. In FIG. 11, the broken line indicates the shape of the motion trajectory model when determined in step S23.

  As described above, it is possible to recalculate the motion trajectory and the posture of the robot manipulator in accordance with the movement of the obstacle 10 to update the optimum trajectory / posture.

  In this embodiment, the execution of the process for rearranging the motion trajectory model is determined based on the tension of the spring constituting the motion trajectory model in step S23. For example, the motion trajectory model is determined by the movable range of the robot manipulator. It is also possible to determine that the motion trajectory model is rearranged when deviating from the above.

  Further, in this embodiment, the posture is generated at all reference points (step S11), and update processing (step S21, etc.) by moving the obstacle is performed. The process proceeds to step S12 until the generation of the attitude from the position to the next reference point, performs update processing (step S21, etc.), and generates the attitude at the next reference point when the process proceeds from step S24 to step S25 (step S7). It is also possible to adopt a configuration for performing the subsequent processing. With such a process, when the process proceeds from step S24 to step S26, the amount of operation generated again is reduced, so that the calculation process can be made more rational.

  As described above, the mobile body posture generation according to the present invention has been described using a plurality of embodiments. In each of the above-described embodiments, the generation of the motion trajectory of the hand of the robot manipulator and the generation of the overall posture of the robot manipulator are performed in a series of processes. However, even if the method for generating the motion trajectory of the hand is different from the method described above, the entire posture of the robot manipulator is generated by performing repulsive force vector field generation and processing after the moving body shape model setting. Is possible.

  In each embodiment, the moving body is described as a robot manipulator, but the present invention is not limited to this. For example, the present invention can be applied to the generation of trajectories of various moving bodies in a three-dimensional space where obstacles exist, such as an underwater swimming robot, a walking robot, and a wheel robot.

  In addition, the moving body posture generation method described in each embodiment can be executed by a system including an input device, a calculation device, and an output device in which a function for performing each process is implemented. A general personal computer incorporating the above program can be used.

10 Obstacles 11, 11 a, 11 b Repulsive force vector 20 Tension spring 21 Hinge spring 22 Connecting springs 23, 23 a, 23 b Material point 25 Three-dimensional shape model 26 Representative point 27, 27 c Reference point 27 a Initial position 27 b Target position 28 Current position 29 Vertical plane 31 Attractive vector

Claims (11)

A moving body posture generation method for generating a posture of an entire moving body based on a motion trajectory of a part of the moving body,
A repulsive force vector field setting step for setting a repulsive force vector field composed of a repulsive force vector with an obstacle existing in a space in which the moving object moves;
A moving body shape model setting step for setting a representative point at which repulsive force and attractive force act on the three-dimensional shape model of the moving body;
A force / moment calculation step for calculating the force and moment acting on each joint axis of the moving body from the repulsive force acting on the representative point;
An attractive force vector setting step of setting a reference point on the motion trajectory and causing an attractive force directed to the reference point to act on the moving body;
A posture generation step of calculating a torque acting on each joint axis of the moving body from a force and a moment acting on the three-dimensional shape model of the moving body, and changing an angle of each joint axis based on the torque. A moving body posture generation method. A moving body posture generation method for generating a posture of an entire moving body based on a motion trajectory of a part of the moving body,
A space setting step for setting a space in which the moving body moves;
An initial position setting step for setting an initial position at which a part of the moving body starts operating in the space;
A target position setting step of setting a target position of a part of the moving body in the space;
A repulsive force vector field setting step for setting a repulsive force vector field composed of a repulsive force vector having an obstacle existing in the space as a source;
An operating trajectory model that connects the initial position and the target position, has an action point where the repulsive force vector acts between the initial position and the target position, and has both ends fixed to the initial position and the target position. A motion trajectory model setting step to be set;
An action trajectory generating step in which the repulsive force vector of the repulsive force vector field is applied to the action point and the deformed action trajectory model is used as the movement trajectory of the moving body;
A moving body shape model setting step for setting a representative point at which repulsive force and attractive force act on the three-dimensional shape model of the moving body;
A force / moment calculation step for calculating the force and moment acting on each joint axis of the moving body from the repulsive force acting on the representative point;
An attraction vector setting step for setting a reference point on the generated motion trajectory and causing an attractive force directed to the reference point to act on the moving body;
A posture generation step of calculating a torque acting on each joint axis of the moving body from a force and a moment acting on the three-dimensional shape model of the moving body, and changing an angle of each joint axis based on the torque. A moving body posture generation method.   After the posture generation step, a reference point update step for updating the reference point, until the reference point reaches a target position on the motion trajectory, the force / moment calculation step, the attractive force vector setting step, the posture generation 3. The moving body posture generation method according to claim 1, further comprising an operation generation step of repeating the step and the reference point update step. 2. The attractive force according to a positional deviation between a current position of the position of the moving body on which the attractive force is applied and a reference point on the generated motion trajectory is set in the attractive force vector setting step. The moving body attitude | position production | generation method of 2 description. In the attractive force vector setting step, a plane that includes a reference point on the motion trajectory and that is perpendicular to the motion trajectory is defined, and a position deviation between the current position of the moving body on which the gravitational force is applied and the vertical plane is determined. The mobile body posture generation method according to claim 1, wherein an attractive force is set. 5. The attractive force comprising a combination of a component proportional to a positional deviation amount, a component obtained by integrating the positional deviation, and a component obtained by differentiating the temporal variation of the positional deviation is set in the attractive vector setting step. 5. The moving body posture generation method according to 5. The coefficient representing the ease of movement of the joint is set for each joint axis in the process of calculating the amount of change of each joint axis angle based on torque in the posture generation step. A moving body posture generation method. 8. The moving body posture generation method according to claim 7, wherein, in the posture generation step, a coefficient representing the ease of movement of the joint is changed when the moving body approaches a specific posture. The torque acting on each joint axis is calculated from the force and moment acting on the three-dimensional shape model by using a Jacobian matrix obtained from the current posture of the moving body in the posture generation step. Or the moving body attitude | position production | generation method of Claim 2. A mobile body posture generation device that generates a posture of an entire mobile body based on an operation trajectory of a part of the mobile body,
A repulsive force vector field setting means for setting a repulsive force vector field composed of repulsive force vectors with an obstacle existing in a space in which the moving body moves as a source;
Moving body shape model setting means for setting a representative point at which repulsive force and attractive force act on the three-dimensional shape model of the moving body;
Force / moment calculation means for calculating the force and moment acting on each joint axis of the moving body from the repulsive force acting on the representative point;
An attraction vector setting means for setting a reference point on the motion trajectory and causing an attractive force directed to the reference point to act on the moving body;
Posture generating means for calculating a torque acting on each joint axis of the moving body from a force and a moment acting on the three-dimensional shape model of the moving body, and changing an angle of each joint axis based on the torque. A mobile body posture generation device. A mobile body posture generation device that generates a posture of an entire mobile body based on an operation trajectory of a part of the mobile body,
Space setting means for setting a space in which the moving body moves;
Initial position setting means for setting an initial position at which a part of the moving body starts operation in the space;
Target position setting means for setting a target position of a part of the moving body in the space;
Repulsive force vector field setting means for setting a repulsive force vector field composed of repulsive force vectors having an obstacle existing in the space as a source;
An operating trajectory model that connects the initial position and the target position, has an action point where the repulsive force vector acts between the initial position and the target position, and has both ends fixed to the initial position and the target position. Action trajectory model setting means to set and action trajectory generating means for applying the repulsive force vector of the repulsive force vector field to the action point and using the deformed motion trajectory model as the motion trajectory of the moving body;
A moving body shape model setting means for setting a point at which repulsive force and attractive force act on a three-dimensional shape model of the moving body, and a force / moment for calculating the force and moment acting on each joint axis of the moving body from the repulsive force acting on the moving body A calculation means; an attraction vector setting means for setting a reference point on the generated motion trajectory and causing an attractive force directed to the reference point to act on the moving body;
Posture generating means for calculating a torque acting on each joint axis of the moving body from a force and a moment acting on the three-dimensional shape model of the moving body, and changing an angle of each joint axis based on the torque. A mobile body posture generation device.

Images