JP2012157955A - Device and method for controlling movement, and computer program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To preferably controlling movement of an automated machine such as reaching operation of a manipulator and traveling of a mobile robot.SOLUTION: A current position and a final target position of the automated machine are regarded as each terminal of a parametric curve, and an internally dividing point that internally divides the parametric curve connecting the two points, at an internal division ratio r : 1-r is calculated as an equilibrium point of a virtual spring/dumper system, that is, a temporary target position. The temporary target position is blunted through a secondary filter to generate a target position x, thereby constituting a movement controller that strikes a balance between achieving a follow-up control for a complicated route and also anti-disturbance adaptation ability that is a feature of an online target position shaping to be situated as a base line.

Description

  The present invention relates to a movement control apparatus and a movement control method for controlling movement of an automatic machine such as a reaching operation of a manipulator and a movement of a mobile robot, and a computer program, and in particular, a movement ability of a complicated path and a disturbance adaptation ability. The present invention relates to a movement control device and a movement control method for an automatic machine including a computer program.

  The environment in which humans are active is a dynamic environment that consists of a narrow space and changes over time. In such an environment, if you try to activate various automatic machines such as the reaching operation of a manipulator or the movement of a wheel type or other mobile robot, people and objects (hereinafter referred to as "obstacles" collectively) Contact with can occur. Therefore, in order to operate an automatic machine and realize various tasks, it is necessary to have the ability to move through complicated routes and adapt to disturbances such as contact with obstacles for safety and task execution. Is done.

  In general, in the “movement” of an automatic machine such as a reaching operation of a manipulator and a movement of a mobile robot, a movement control method for generating a target trajectory from a given target position and target arrival time is often used. In other words, the route of the automatic machine is expressed by a function that takes a time as an argument and obtains a target position for each target time. As described above, when the automatic machine is applied to a human living environment, contact with various obstacles is assumed. According to the conventional movement control method, if the target position is not reached at the target arrival time due to contact with an obstacle, the control system breaks down. In other words, in a control system that explicitly includes the time, it is difficult to ensure sufficient safety due to lack of flexibility for disturbance.

  As a technique for achieving the target position without explicitly including the time in the control system, there is a “virtual spring / damper hypothesis” (for example, see Non-Patent Document 1). In the hypothetical spring / damper hypothesis, the current position and the target position of an automatic machine are connected by a virtual spring / damper system, and movement control is performed using a model in which an attractive force generated by the spring is attracted to the target position. For example, the hand of the manipulator is controlled based on an attractive force generated by a virtual spring / damper system. Vibration due to disturbance is suppressed by the damper. The virtual spring / damper hypothesis does not express the movement path of the automatic machine as a time function, and therefore can flexibly cope with disturbances and reach the target position even when contact with an obstacle occurs.

  According to the hypothetical spring / damper hypothesis, the restoring force of the spring at the start of the operation is the maximum, that is, the acceleration of the control target point such as the hand is the maximum at the start of the operation, so it is difficult to realize a smooth and safe operation, The burden on hardware is large. In order to solve this problem, there has been proposed a hand leaching method that introduces a time-varying stiffness that changes a virtual spring coefficient by using a function that depends on time (for example, see Non-Patent Document 2). . However, this method loses the characteristics of the autonomous system that was configured by not including the time explicitly, so it is difficult to apply this method in a dynamic environment where contact with various obstacles is assumed. is there.

  Furthermore, based on the virtual spring / damper hypothesis, a method has been proposed in which a target position to be given to the virtual spring / damper system is shaped online (for example, see Non-Patent Document 3). In this online target position shaping, an equilibrium point is provided between the movement start position and the final target position, and an equilibrium point closer to the current position than the final target position is used as a virtual target position. By applying the hypothesis, it is possible to design a speed profile and smooth out the movement of the automatic machine. The equilibrium point is determined based on the remaining distance from the current position to the final target position. The online target position shaping method generates a smooth leaching operation without explicitly designing the trajectory and without explicitly including the time in the control system. When performing target position shaping online, for example, when external force is applied by feeding back the manipulator state as the hand position, it flexibly follows it, and after the external force is eliminated, the target position does not change rapidly. It has the characteristic that the leaching operation can be resumed.

  However, both the method that introduces time-varying stiffness that changes the virtual spring coefficient and the method that online shapes the target position given to the virtual spring / damper system are both the start point (current position) and end point (target position). It is only a method for controlling movement between two points. That is, it is difficult to apply to tasks that require more complicated movements and movements that require complicated movement paths while avoiding obstacles.

  Even in a method based on a conventional trajectory plan (see, for example, Non-Patent Document 4), it is possible to follow a disturbance by giving a compliance characteristic to the hand of the manipulator. However, since the control system explicitly includes the time, this conventional method cannot obtain sufficient copying performance when a disturbance occurs over a long period of time. Further, if the compliance that is given to the hand of the manipulator is increased with an emphasis on the characteristic of flexibly responding to disturbances, the convergence performance to the target position will be significantly reduced.

S. Arimoto and M.M. Sekimoto “Human-Like Movements of Robotic Arms with Redundant DOFs: Virtual Spring-Dumper Hyperthesto on B. E.ro c. S. Arimoto et al “Natural Resolution of Ill-possessed of Inverse Kinetics for Redundant Robots: A Challenge to Benedtain ’s Degrees. Fumi Seto and Tomomichi Sugihara, "Online Reference Shaping with End-point Position Feedback for Large Acceleration Avoidance on Manipulator Control, 2009 IEEE / RSJ International Conference on Intelligent Robots and Systems, pp.5743-5748, St.Louis, Oct, 14th, 2009)

  An object of the present invention is to provide an excellent movement control device and movement control method, and a computer program capable of suitably controlling movement of an automatic machine such as leaching operation of a manipulator and movement of a mobile robot. .

  A further object of the present invention is to provide an excellent movement control apparatus and movement control method having a complex path movement capability and a disturbance adaptation capability so that an automatic machine can operate in an environment consisting of a narrow space in which humans are active, and To provide computer programs.

The present application has been made in consideration of the above problems, and the invention according to claim 1
In an environment where an obstacle exists, a route avoiding the penetration of the automatic machine and the obstacle is planned for the automatic machine to move from the current position of the automatic machine to the final target position. A route planning unit that outputs a route node sequence composed of nodes of
A control point that searches for a node closest to the current position from the path node sequence and combines a current position of the automatic machine and a node group from the node closest to the current position to the terminal node in the path node sequence. A column is generated, a parametric curve passing through the vicinity of the control point is generated, and the current point on the parametric curve and the end node are calculated as an equilibrium point that internally divides with an internal ratio r: 1-r. A movement control unit for determining a virtual spring / damper generating force generated in a virtual spring / damper system connecting the current position and the equilibrium point;
A drive control unit for controlling the drive of each joint constituting the automatic machine based on the virtual spring / damper generating force;
An automatic machine movement control device comprising:

  According to the invention described in claim 2 of the present application, the path planning unit of the movement control device described in claim 1 uses a trajectory discovered by RRT as an initial condition, and further sets each passing point on the trajectory as a mass point. It is configured to execute a mass system dynamics simulation, plan a route that satisfies at least one of the requirements of smoothing, optimizing the trajectory, and avoiding infiltration with obstacles, and output a route node sequence. Yes.

  According to the invention described in claim 3 of the present application, the path planning unit of the movement control device described in claim 1 uses the trajectory discovered by RRT as an initial condition, and sets an appropriate initial posture at each passing point on the trajectory. A path that satisfies the requirements of at least one of smoothing of track, optimization, and avoidance of obstacles by building a multi-rigid system dynamic model with allocated rigid bodies and executing multi-rigid system dynamics simulation Are configured to output a route node string.

  According to invention of Claim 4 of this application, the movement control part of the movement control apparatus of Claim 1 produces | generates the NURBS curve which passes the vicinity of the said produced | generated control point row | line | column, The said NURBS curve A virtual point generated in a virtual spring / damper system connecting the current position and the end point is calculated as an equilibrium point that internally divides the current node and the terminal node at an internal ratio r: 1-r. The spring / damper generating force is determined.

  According to the invention described in claim 5 of the present application, the drive control unit of the movement control device described in claim 1 is a force, a position, a speed, an acceleration generated in a part of the body of the automatic machine, and a contact with the outside world. The command value to the actuator that drives each joint by determining the joint drive method of the automatic machine that satisfies the constraints, the generated force of each joint constituting the automatic machine, the joint value, the joint speed, the joint acceleration, and the movable range constraint at the same time Is configured to output.

The invention according to claim 6 of the present application is
In an environment where an obstacle exists, a route avoiding the penetration of the automatic machine and the obstacle is planned for the automatic machine to move from the current position of the automatic machine to the final target position. A route planning step for outputting a route node sequence composed of nodes of
A nearest neighbor search step for searching for a node closest to the current position from the path node sequence;
A control point sequence generating step for generating a control point sequence combining a current position of the automatic machine and a node group from a node closest to the current position to a terminal node in the path node sequence;
An equilibrium point generation step of generating a parametric curve that passes in the vicinity of the control point and generating an equilibrium point that internally divides between the current position on the parametric curve and a terminal node at an internal ratio r: 1-r. When,
A virtual spring / damper generating force determining step for determining a virtual spring / damper generating force generated in a virtual spring / damper system connecting the current position and the equilibrium point;
A drive control step for controlling the drive of each joint constituting the automatic machine based on the virtual spring / damper generating force;
It is the movement control method of the automatic machine which has these.

The invention according to claim 7 of the present application is
In an environment where an obstacle exists, a route avoiding the penetration of the automatic machine and the obstacle is planned for the automatic machine to move from the current position of the automatic machine to the final target position. A route planning unit that outputs a route node sequence composed of nodes of
A control point that searches for a node closest to the current position from the path node sequence and combines a current position of the automatic machine and a node group from the node closest to the current position to the terminal node in the path node sequence. A column is generated, a parametric curve passing through the vicinity of the control point is generated, and the current point on the parametric curve and the end node are calculated as an equilibrium point that internally divides with an internal ratio r: 1-r. A movement control unit for determining a virtual spring / damper generating force generated in a virtual spring / damper system connecting between the current position and the equilibrium point;
A drive control unit for controlling the drive of each joint constituting the automatic machine based on the virtual spring / damper generating force;
As a computer program described in a computer-readable format so that a process for controlling the movement of an automatic machine is executed on the computer.

  The computer program according to claim 7 of the present application defines a computer program described in a computer-readable format so as to realize predetermined processing on a computer. In other words, by installing the computer program according to claim 7 of the present application on a computer, a cooperative operation is exhibited on the computer, and the same effect as the movement control device according to claim 1 of the present application is obtained. be able to.

  ADVANTAGE OF THE INVENTION According to this invention, the outstanding movement control apparatus and movement control method which can control suitably the movement of an automatic machine, such as the reaching operation of a manipulator, and the movement of a mobile robot, and a computer program can be provided. .

  According to the present invention, the target position given to the virtual spring / damper system is shaped online, and the movement control is realized without explicitly designing the trajectory and without explicitly including the time in the control system. By introducing the NURBS curve, which is a parametric curve, it is possible to realize movement control that achieves both the movement ability of a complicated route and the ability to adapt to disturbance.

  Other objects, features, and advantages of the present invention will become apparent from more detailed description based on embodiments of the present invention described later and the accompanying drawings.

FIG. 1 is a configuration diagram of a movement control system including a virtual spring / damper system and online target position shaping. FIG. 2 is a diagram illustrating a configuration example of a control system for an automatic machine. FIG. 3 is a flowchart showing a processing procedure executed by the control system 100 shown in FIG. FIG. 4 is a diagram showing how a route is changed in the movement control of the two-dimensional mobile robot to which the control system 100 shown in FIG. 2 is applied. FIG. 5 is a diagram showing a temporary target position that internally divides between the current position of the automatic machine and the final target position on the NURBS curve at an internal ratio r: 1-r.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

  Based on the hypothetical spring / damper hypothesis, the method of online shaping the target position given to the hypothetical spring / damper system is smooth without explicitly designing the trajectory and without explicitly including the time in the control system. It is a technique to control the movement between two points, but it is difficult to apply it to tasks that require more complex movements and movements that require complicated movement paths while avoiding obstacles ( As described above). On the other hand, the present inventors realize smooth movement by online position shaping that does not include the time explicitly, but by feeding back the state of the controlled object to the target position shaping as the position, the convergence performance to the target position A movement control method that realizes the adaptability to disturbances without dropping the frequency will be described below. According to this movement control method, it is possible to prevent sudden fluctuations in the movement speed. Further, by using a NURBS curve that is a parametric curve for target position shaping, it is also possible to follow a complicated route.

  The movement control method proposed by the present inventors is realized by suitably adopting the virtual damper hypothesis and online target position shaping. These methods are described in Non-Patent Documents 1 and 3, respectively, but will also be described in this specification.

The number of joints of the virtual spring / damper hypothesis manipulator is m, the degree of freedom of the workspace is l (usually two or three dimensional degrees of freedom), the stiffness coefficient matrix and the viscosity coefficient matrix of the virtual spring / damper system are K _p , putting the ξ∈R ^{l ×} l, according to the virtual spring-damper hypothesis for the current hand position X∈R ^l manipulator, a force F _d ∈R ^l represented by the following formula (1) (now The first time derivative of the hand position x is the speed of the hand of the manipulator). ∈

In the above equation (1), x _d ∈R ¹ is a (virtual) target position given as an equilibrium point of the virtual spring / damper system. In short, F _d is an attractive force that attracts the hand of the manipulator to the virtual target position x.

When the above expression (1) is developed in the joint space, it is expressed as the following expression (2). Where θ∈R ^m is the current joint angle of the manipulator (the first time derivative of θ is the angular velocity), C∈R ^{m × m} is the viscosity coefficient matrix of the joint, and J (θ) ∈R ^{l × m} is the manipulator It is assumed that the Jacob matrix up to the current hand position x is represented.

Online target position shaping In the conventional virtual spring / damper hypothesis (see, for example, Non-Patent Document 1), the target position x _d given as the equilibrium point of the virtual spring / damper system is set to the final target position x _g ∈R ^l . Fix it. In other conventional methods, the target position _xd given as an equilibrium point of the virtual spring / damper system is often designed as a function of time t. In online target position shaping, an equilibrium point is provided between the movement start position and the final target position, and an equilibrium point closer to the current position than the final target position is set as a virtual target position. Apply the damper hypothesis to smooth out the movement of automatic machines.

In the online target position shaping, the target position _xd given as the equilibrium point of the virtual spring / damper system is shaped in real time by the following equation (3) according to the final target position _xg and the current position x. However, K _I , K _I1 and K _I2 are non-negative constant coefficients.

Here, as shown in the following expression (4) (where 0 ≦ r ≦ 1), the input is the final target position x _g and the current position x, and the output is the target position x that gives the output as the equilibrium point of the virtual spring / damper system. _{Assuming d} , the transfer function is expressed by the following equation (5).

The above equation (5), the final target position x _g, route the internal ratio from the current position x to the final target position x _g r: replaced by internally dividing point which internally divides at 1-r, further its internal division This means that the value obtained by dulling the movement of the point with the secondary filter is given as the equilibrium point x _d of the virtual spring / damper system. In this way, by shaping the target position in real time, the acceleration and speed at the start of the operation of the automatic machine to be controlled become zero, and a smooth movement is realized.

FIG. 1 shows a configuration diagram of a movement control system including a virtual spring / damper system and online target position shaping. According to the hypothetical spring / damper hypothesis, if the movement of an automatic machine to be controlled is hindered by an external force generated by contact with an obstacle, excessive force is generated on the contacted obstacle or the external force is eliminated. The problem is that rapid acceleration occurs when On the other hand, when the current position x is fed back instead of a simple second-order lag system by online target position shaping, the equilibrium point x _d is determined by dividing the final target position x _g and the current position x at that time. To converge to a point. Therefore, even when the movement of the current position x is hindered by an external force generated due to positive action with an obstacle, the equilibrium point x _d is not greatly separated from the current position x, and an excessive contact force is generated. Or sudden acceleration. Further, as can be seen from FIG. 1, since the online target position shaping does not include time explicitly, even if a disturbance occurs over a long period of time, the movement control is not affected. After the disturbance is removed, the target position can be gradually converged.

Addition of follow-up performance for complex paths (extended target position shaping)
The online target position shaping disclosed in Non-Patent Document 3 is a method for controlling a linear movement between two points of a start point (current position) and an end point (target position), that is, a task that requires more complicated movements, It is difficult to apply to a movement that requires a complicated movement route while avoiding an obstacle (described above).

  For example, there are various obstacles such as desks and chairs in a human living environment. For a mobile robot to be active in such an environment, it is necessary to move between two points to touch the obstacle. It is difficult to avoid and causes problems in task performance. In other words, a target position shaper that can follow a complicated path is indispensable to accomplish a complicated task.

  As a route planning method for planning a movement route of a mobile robot or the like on a two-dimensional or three-dimensional work space, a potential method (for example, Jun Ota, “Introduction to Intelligent Robots—Solution of Motion Planning Problems”) (Corona, 2001) )) And RRT (eg, Steven M. LaVall, “Rapidly-Exploring Random Trees: A New Tool for Path Planning” (TR. 98-11, Computer Science Dpt., IowUt. However, most of them express the result of the route plan as a passing point sequence. Further, according to the trajectory planning apparatus disclosed in Japanese Patent Application Laid-Open No. 2009-212571 already assigned to the present applicant, the trajectory discovered by RRT is used as an initial condition, and each passing point on the trajectory is defined as a mass point. By executing the multi-mass point system dynamic simulation, it is possible to generate a trajectory that satisfies the requirements of smoothing, optimizing the trajectory, and avoiding the penetration of obstacles. Further, according to the trajectory planning apparatus disclosed in Japanese Patent Application Laid-Open No. 2010-155328 already assigned to the present applicant, the trajectory discovered by RRT is used as an initial condition, and an appropriate initial value is set for each passing point on the trajectory. Build a multi-rigid system dynamic model with rigid bodies assigned postures and execute multi-rigid system dynamics simulation to generate a trajectory that satisfies the requirements of smoothing and optimizing the trajectory and avoiding infiltration with obstacles can do.

  Here, attention is focused on the following expression (6) which is a part of the right side of the above expression (5).

The above equation (6) is expressed by associating coordinate points with one parameter r, and is equivalent to a general parameter curve. Therefore, the present inventors apply a NURBS (Non-Uniform Rational B-Splines) curve, which is one of typical parametric curves, to the above equation (6). That is, the current position and the final target position are regarded as terminals of a parametric curve, and an internal dividing point that internally divides the parametric curve connecting these two points with an internal ratio r: 1-r is an equilibrium point of the virtual spring / damper system, That is, it is calculated as a temporary target position. Then, the temporary target position is dulled by a secondary filter (described above) to generate the temporary target position _xd . Accordingly, it is possible to configure a movement controller that realizes follow-up control to a complicated route and at the same time has a disturbance adaptability that is a feature of the online target position shaping as a base.

  For example, Yoru Miura and Kazumasa Mochizuki, “Practical NURBS for CAD / CG Engineers” (Industry Research Committee, 2001) describe NURBS curves in detail. Below, it demonstrates centering on the point to keep in mind for applying a NURBS curve to movement control.

  NURBS curves are roughly classified into NURBS approximation and NURBS interpolation (well known). In the NURBS approximation, the curve passes near the point sequence, and in the NURBS interpolation, the curve passes through the point sequence. In view of the characteristics of the curve and the calculation efficiency, the inventors decided to use the NURBS approximation for the movement control. It will be described here that an approximate path having no problem in operation can be obtained by NURBS approximation if the passing point sequence has sufficient granularity. That is, the input to the movement control system is a passing point sequence obtained by the trajectory planning method, and this passing point sequence is regarded as a control point sequence of the NURBS curve. Then, a NURBS curve that passes in the vicinity of the control point is generated, and an internal dividing point that internally divides the current position and the final target position as the starting point on the NURBS curve with an internal ratio r: 1-r is set as the temporary target position. Will be calculated as Of course, it should be noted that even if NURBS interpolation is used instead of NURBS approximation, almost the same function can be realized, but the calculation amount increases.

The NURBS curve is obtained by projecting a B-spline curve defined in the homogeneous coordinate space onto the normal coordinate space by central projection. Since control points are mixed in a homogeneous coordinate space, each control point q _i (corresponding to a passing point sequence obtained by trajectory planning) is given a weight ω _i .

The B-spline curve in the homogeneous coordinate space is obtained by mixing the control point sequence Q _i (ω _i x _i , ω _i y _i , ω _i z _i , ω _i ) of the homogeneous coordinates with the B-spline basis function, As shown in the following formula (7), it is obtained as P ^ω (t).

When the B-spline curve P ^w (t) in the homogeneous coordinate space is centrally projected, the curve P ^* (t) is as shown in the following equation (8).

The curve P ^* (t) represents a curve projected on the surface of ω = 1 in the homogeneous coordinate space. When this is expressed in the normal coordinate system, it becomes as shown in the following formula (9), which becomes the formula of the NURBS curve.

However, in the above formula (9), q _i (x _i , y _i , z _i ) is a control point in the normal coordinate system and has a relationship of Q _i = (ω _i q _i , ω _i ). Further, a knot vector is T = [t ₀ ,..., T _{m + n + 1} ], and m and n represent the rank of the curve and the number of passing point sequences (number of control points), respectively. N _{i, m} (t) represents a B-spline defining function, and a de Boer Cox recursion formula known as a method suitable for implementation on a computer is used as the calculation method.

  For reference, the recurrence formula of de Boor Cox is shown below. However, it should be understood that the gist of the present invention is not limited to the calculation method of the B-spline defining function, but is appropriately selected from a number of calculation methods.

  In general, the knot vector can be freely set as long as it satisfies the monotonically increasing and m + n knots (where m is the rank of the curve and n is the control). ·points). In this specification, an example of a method for creating a knot vector used in movement control in the present embodiment will be mentioned. The knot vector is a parameter representing the range of the curve, and it is possible to characterize the moving speed profile by generating a knot vector. For example, it is possible to impose an effect of suppressing a change in moving speed by increasing a knot vector interval in a portion having a large curvature.

  In general, when setting the curve parameter for the i-th passing point, it is desirable that the parameter interval reflects the ratio of the section length between the passing points. In the present embodiment, following this, a curve parameter that reflects the ratio of the section length is set. However, since the curve length cannot be applied as the section length, it is determined by the distance between the passing points or the ratio of the square roots. . In this specification, a method for setting a curve parameter using a distance between passing points will be described.

  When the displacement of the parameter of the entire curve is 0 ≦ t ≦ 1, the curve parameter for the i-th passing point can be determined as in the following formula (11).

  Next, using the curve parameters shown in (11), a knot vector sequence is generated as shown in the following equation (12). However, terminal knots are defined as many times as the curve rank, and a NURBS curve that always passes through the terminal control point is generated.

  By substituting the internal division ratio r as the argument t of the NURBS curve P (t) represented by the above equation (9), the internal division ratio r: 1 between the current position of the automatic machine on the NURBS curve and the final target position is as follows. A temporary target position that is internally divided by −r can be calculated (see FIG. 5).

As is well known in the art, there are parametric curves other than NURBS curves. However, a polynomial curve including a Bezier curve or a Ferguson curve has a drawback that continuity within a segment cannot be guaranteed because a plurality of segments are connected to generate one curve. In contrast, the NURBS curve guarantees ^Cm-2 class continuity throughout the curve. That is, regardless of the arrangement of the control points, the differential coefficients up to the (m-2) th floor are continuous before and after the position corresponding to each knot. The NURBS curve has a locality that is not found in other curves, such as when a specific control point is moved, the affected range is limited to the vicinity of the control point and does not affect the curve at a distant portion. Also have. These properties are important factors in controlling a real object. Therefore, the present inventors adopted a NURBS curve as a parametric curve.

  FIG. 2 shows a configuration example of a control system for an automatic machine such as a manipulator or a mobile robot. The illustrated control system 100 includes a route planning unit 110, a movement control unit 120, and a drive control unit 130.

  The route planning unit 110 inputs a movement command, plans a route of an automatic machine that satisfies the command (for example, to move to a specified final target position), and a series of nodes on the planned route. That is, a route node sequence is output. The movement command is, for example, a human living environment in which various obstacles exist, and the route planning unit 110 plans a curved route avoiding the obstacles and outputs the route node sequence. For example, the path planning unit 110 is configured by using the trajectory planning apparatus disclosed in Japanese Patent Application Laid-Open No. 2009-211151 or Japanese Patent Application Laid-Open No. 2010-155328 that has already been assigned to the present applicant. The multi-point system dynamics simulation with the trajectory discovered by RRT as the initial condition and the passing points on the trajectory as the mass points is executed to smooth the trajectory, optimize it, and make automatic machinery and obstacles A node sequence on the path that satisfies the requirement of avoiding penetration is generated.

  In general, route planning is computationally expensive. In the present embodiment, the route planning unit 110 is treated as one that operates at a relatively slow cycle, which is different from the movement control unit 120 and the drive control unit 130 that operate at a short cycle. However, the gist of the present invention is not limited to the operation of the path planning unit 110 with a slow cycle, and even if the route planning unit 110 operates with a fast cycle, the entire system 100 is not affected.

  The movement control unit 120 performs control for the automatic machine to move along the route output from the route planning unit 110. The movement control unit 120 controls the movement of the automatic machine by applying an attractive force attracted to the target position using a virtual spring / damper. At this time, an internal dividing point that internally divides the current position and the final target position at an internal ratio r: 1-r is calculated as a temporary target position to be given to the virtual spring / damper system, and the target position is shaped online. Since the route output from the route planning unit 110 is a curved line that avoids infiltration with an obstacle, the route is treated as a NURBS curve, which is one of the parametric curves, and a temporary target given to the virtual spring / damper system. Calculate the position.

  In the example shown in FIG. 2, the movement control unit 120 includes a nearest passing point search unit 121, a control point sequence generation unit 122, an equilibrium point generation unit 123, and a virtual spring / damper generation force determination unit 126. The The equilibrium point generation unit 123 includes a knot vector generation unit 124 and a NURBS approximation unit 125.

  The nearest neighbor search unit 121 provides a function of absorbing an operation cycle with the route planning unit 110, and searches for a node closest to the current position of the automatic machine from the route node sequence planned by the route planning unit 110. However, if the operation cycle of the route planning unit 110 and the movement control unit 120 is the same, the nearest neighbor search unit 121 is not necessary.

The control point sequence generation unit 122 combines the current position of the automatic machine and the node group from the node closest to the current position searched by the nearest point search unit 121 in the route node sequence to the end node. A point sequence {q _i } is generated. In other words, the control point sequence generation unit 122 has an effect of eliminating a node group that has already passed from the route node sequence generated by the route planning unit 110. However, since the nearest point of the current position may be a node that has already passed due to the influence of disturbance or the like, the control point sequence generation unit 122 does not wait until the next route change request is transmitted from the route planning unit 110. It is necessary to hold the planned route node sequence.

The equilibrium point generation unit 123 generates a NURBS curve that passes in the vicinity of the control point, and the internal division ratio r between the current position of the automatic machine that is the starting point on the NURBS curve and the final target position (terminal node): The internal dividing point divided internally by 1-r is calculated as an equilibrium point (temporary target position). Specifically, the knot vector generation unit 124 in the equilibrium point generation unit 123 is based on the distance of the control point sequence {q _i } determined by the control point sequence generation unit 122 and the above-described arithmetic expression (11 ), (12), a knot vector sequence {t _i } is generated. Next, the NURBS approximation unit 125 determines an equilibrium point that is a temporary target position based on the generated knot vector sequence {t _i } and the internal division ratio r described above. By substituting the internal division ratio r as the argument t of the NURBS curve P (t) represented by the above equation (9), the internal division ratio r: 1 between the current position of the automatic machine on the NURBS curve and the final target position is as follows. An equilibrium point that is internally divided by -r is calculated (see FIG. 5).

The virtual spring / damper generating force determining unit 126 uses the equilibrium point determined by the equilibrium point generating unit 123 as the temporary target position _xd , and is generated in the virtual spring / damper system connecting the current position and the temporary target position _xd. The virtual spring / damper generating force F _d to be determined is determined by the above-described arithmetic expression (1) and transmitted to the drive control unit 130. Alternatively, the virtual spring / damper generation force determination unit 126 may convert the virtual spring / damper generation force by an appropriate constant coefficient to convert it into a speed input and transmit it to the drive control unit 130.

The drive control unit 130 controls the drive of each joint constituting the automatic machine based on the input virtual spring / damper generating force F _d . Specifically, the drive control unit 130 is configured to perform other motion constraints (for example, force generated on a part of the body of the automatic machine, position, speed, acceleration, contact constraint with the outside world, each joint constituting the automatic machine). Determine the joint drive method of the automatic machine that simultaneously satisfies the generated force, joint value, joint speed, joint acceleration, movable range constraint, etc.), and command value to the actuator that drives each joint (for example, joint input torque τ) Is output.

  If the actuator that drives the joint of the automatic machine is a force control type, the operation space control can be applied as a whole body cooperation method that emphasizes the whole body of the automatic machine and drives each joint. For example, Japanese Patent Laid-Open No. 2007-108955 already assigned to the present applicant discloses a whole body coordination method to which operation space control is applied. If the actuator that drives the joint is a position control type, generalized inverse kinematics can be used as the whole body coordination method. For example, Japanese Patent No. 3972854 already assigned to the present applicant discloses a whole body coordination method using generalized inverse kinematics. In this specification, description of the details of the whole body cooperation method of the automatic machine is omitted.

  FIG. 3 shows a processing procedure executed by the control system 100 shown in FIG. 2 in the form of a flowchart.

  In the nearest neighbor search process, the nearest neighbor search unit 121 searches the route node sequence planned by the route planning unit 110 for a node closest to the current position (step S31).

  In the subsequent control point sequence generation process, the control point sequence generation unit 122 combines the current position with the node group from the node closest to the current position searched in step S31 to the terminal node, thereby controlling the control point. A column is generated (step S32).

  In the subsequent knot vector generation process, the knot vector generation unit 124 performs knots according to the above-described arithmetic expressions (11) and (12) based on the distance between control points of the control point sequence determined in step S32. A vector is generated (step S33).

  In the subsequent NURBS approximation process, the NURBS approximation unit 125 determines an equilibrium point on the NURBS approximation curve based on the knot vector sequence generated in step S33 and the internal division ratio r described above (step S34). By substituting the internal ratio r into t of the NURBS curve P (t) represented by the above equation (9), an equilibrium point that internally divides the current position and the final target position with an internal ratio r: 1-r is calculated. Is done.

  In the virtual spring / damper generating force determining process, the virtual spring / damper generating force determining unit 126 determines the virtual spring / damper generating force from the equilibrium point of the virtual spring / damper system calculated in step S34 ( Step S35).

  According to the control system 100 shown in FIG. 2, the automatic machine can perform follow-up control (movement control) along a complicated route.

  The control system 100 is based on the online target position shaping disclosed in, for example, Non-Patent Document 3, but has the capability of adapting to disturbances because it inherits the autonomous nature of the target position shaping. That is, the control system 100 has safety by being passive to disturbance.

  Since the control system 100 achieves both the follow-up control to the complicated path and the characteristic of adapting to the disturbance, the mobile robot controlled by the control system 100 becomes a dynamic environment such as a human living environment. Can be adapted.

  Further, since the control system 100 has a target position shaping feature that a speed profile can be designed, it is possible to realize a smooth start of the automatic machine.

Moreover, since the movement control part 120 is comprised by the autonomous system, it can respond to the change of a path | route flexibly and smoothly. FIG. 4 shows how a route is changed in the movement control of the two-dimensional mobile robot to which the control system 100 shown in FIG. 2 is applied. In the figure, the horizontal axis represents the forward distance X [m] of the mobile robot, and the vertical axis represents the lateral distance Y [m] of the mobile robot. In the graph shown in the figure, the route node sequence indicating the initial route plan result is indicated by a white circle (◯), the route node sequence after the route change is indicated by a square (□), and the route replanning execution start point is indicated by a black circle (●). ). Further, the locus of the target position _xd obtained by the target position shaping is indicated by a dotted line, and the robot movement position locus is indicated by a solid line. Due to the difference in operation cycle between the route planning unit 110 and the movement control unit 120, the route is slightly deviated at the time of route re-planning, but the deviation is very small and there is no problem. That is, it can be confirmed that the movement control unit 120 responds flexibly and smoothly to the change of the route.

  The present invention has been described in detail above with reference to specific embodiments. However, it is obvious that those skilled in the art can make modifications and substitutions of the embodiment without departing from the gist of the present invention.

  Since the scope of the present invention is not limited to a specific dimension, it can be applied to the movement control of various automatic machines such as manipulator leaching and mobile robot movement control, and execute various tasks of automatic machines. Can assist.

  In the present specification, the NURBS curve is generated mainly as a parametric curve passing through the vicinity of the control point sequence. However, other parametric curves such as a Bezier curve and a polynomial curve including a Ferguson curve are used. It is also possible to apply. A polynomial curve including a Bezier curve and a Ferguson curve generates a single curve by connecting a plurality of segments. Therefore, there is a drawback that continuity within a segment is not guaranteed, but it is disclosed in Non-Patent Document 3. Based on the online target position shaping, a similar effect can be obtained, such as having the capability of adapting to disturbance by inheriting the autonomous system of the target position shaping.

  In short, the present invention has been disclosed in the form of exemplification, and the description of the present specification should not be interpreted in a limited manner. In order to determine the gist of the present invention, the claims should be taken into consideration.

DESCRIPTION OF SYMBOLS 100 ... Control system 110 ... Path planning part 120 ... Movement control part 121 ... Nearest passing point search part 122 ... Control point sequence generation part 123 ... Equilibrium point generation part 124 ... Knot vector generation part 125 ... NURBS approximation part 126 ... Virtual spring / damper generating force determining unit 130 ... drive control unit

Claims (7)

In an environment where an obstacle exists, a route avoiding the penetration of the automatic machine and the obstacle is planned for the automatic machine to move from the current position of the automatic machine to the final target position. A route planning unit that outputs a route node sequence composed of nodes of
A control point that searches for a node closest to the current position from the path node sequence and combines a current position of the automatic machine and a node group from the node closest to the current position to the terminal node in the path node sequence. A column is generated, a parametric curve passing through the vicinity of the control point is generated, and the current point on the parametric curve and the end node are calculated as an equilibrium point that internally divides with an internal ratio r: 1-r. A movement control unit for determining a virtual spring / damper generating force generated in a virtual spring / damper system connecting the current position and the equilibrium point;
A drive control unit for controlling the drive of each joint constituting the automatic machine based on the virtual spring / damper generating force;
An automatic machine movement control device comprising: The path planning unit executes a multi-mass system dynamics simulation with the trajectory discovered by RRT (Rapidly-Expanding Random Trees) as an initial condition, and further with each passing point on the trajectory as a mass point, to smooth the trajectory, Planning a route that satisfies at least one of the optimization and avoiding infiltration with obstacles, and outputting a route node sequence,
The movement control device according to claim 1. The path planning unit builds a multi-rigid system dynamics model with a rigid body with an appropriate initial posture assigned to each passing point on the trajectory, with the trajectory found by RRT as an initial condition, and performs a multi-rigid system dynamics simulation. Executing a route that satisfies at least one of the requirements of smoothing, optimizing the trajectory and avoiding infiltration with obstacles, and outputting a route node sequence;
The movement control device according to claim 1. The movement control unit generates a NURBS curve passing through the vicinity of the generated control point sequence, and internally divides between the current position on the NURBS curve and a terminal node at an internal ratio r: 1-r. Calculating a virtual spring / damper generating force generated in a virtual spring / damper system connecting between the current position and the equilibrium point;
The movement control device according to claim 1. The drive control unit includes force generated on a part of the body of the automatic machine, position, speed, acceleration, contact restriction with the outside world, generated force of each joint constituting the automatic machine, joint value, joint speed, joint acceleration, Determine the joint drive method of the automatic machine that satisfies the movable range constraint at the same time, and output the command value to the actuator that drives each joint.
The movement control device according to claim 1. In an environment where an obstacle exists, a route avoiding the penetration of the automatic machine and the obstacle is planned for the automatic machine to move from the current position of the automatic machine to the final target position. A route planning step for outputting a route node sequence composed of nodes of
A nearest neighbor search step for searching for a node closest to the current position from the path node sequence;
A control point sequence generating step for generating a control point sequence combining a current position of the automatic machine and a node group from a node closest to the current position to a terminal node in the path node sequence;
An equilibrium point generation step of generating a parametric curve that passes in the vicinity of the control point and generating an equilibrium point that internally divides between the current position on the parametric curve and a terminal node at an internal ratio r: 1-r. When,
A virtual spring / damper generating force determining step for determining a virtual spring / damper generating force generated in a virtual spring / damper system connecting the current position and the equilibrium point;
A drive control step for controlling the drive of each joint constituting the automatic machine based on the virtual spring / damper generating force;
An automatic machine movement control method comprising: In an environment where an obstacle exists, a route avoiding the penetration of the automatic machine and the obstacle is planned for the automatic machine to move from the current position of the automatic machine to the final target position. A route planning unit that outputs a route node sequence composed of nodes of
A control point that searches for a node closest to the current position from the path node sequence and combines a current position of the automatic machine and a node group from the node closest to the current position to the terminal node in the path node sequence. A column is generated, a parametric curve passing through the vicinity of the control point is generated, and the current point on the parametric curve and the end node are calculated as an equilibrium point that internally divides with an internal ratio r: 1-r. A movement control unit for determining a virtual spring / damper generating force generated in a virtual spring / damper system connecting between the current position and the equilibrium point;
A drive control unit for controlling the drive of each joint constituting the automatic machine based on the virtual spring / damper generating force;
A computer program written in a computer-readable format so that the computer functions as a computer and performs a process for controlling the movement of an automatic machine on the computer.

Images