JP5456557B2 - Robot, control system and control program - Google Patents

Description

  The present invention relates to a control system and a robot that is one of its control objects.

  There has been proposed a method of searching for a trajectory that defines the operation of a control target including a joint mechanism having a rotational degree of freedom such as a robot arm (see Patent Document 1). For example, it is conceivable to search for an Euler angle trajectory representing the angle (joint angle) of the joint mechanism or the position or posture of the specified portion of the control target in the local coordinate system.

JP 2010-005761 A

  However, when the trajectory is represented by the joint angle of the control target, an inefficient trajectory that causes the control target to move unnecessarily large may be generated. Further, when the trajectory is expressed by the Euler angle representing the position or orientation of the designated portion of the control target in the local coordinate system, the search time tends to be long because the joint angle limit of the control target is not strictly considered. This is because there is a singular point, that is, a point where the Jacobian matrix is irregular and a derivative cannot be defined (cannot be interpolated) in the space describing the trajectory.

  Accordingly, an object of the present invention is to provide a system or the like that can reliably search for a trajectory expressing an efficient operation of a control target and reduce the search time for the trajectory.

The control system of the present invention for solving the above-mentioned problems is a system for controlling the operation of a controlled object having a joint mechanism in which a bent state is expressed by a rotation angle about each of two or three orthogonal axes. In the model space expressed by quaternions, the rotation angle around each axis in the joint mechanism is based on the point sequences that are arranged away from each other, and the point sequence having a spatial interpolation basis function as a coupling coefficient An interpolation process for generating a line segment expressed as a linear combination of the above is executed, and a new point sequence is defined by adding a point to the point sequence that is the basis of the one line segment generated by the interpolation process. Then, by executing the interpolation process based on the new point sequence, the extension process for generating a new line segment in which the one line segment is extended by the extension line segment is executed. A first arithmetic processing element that is configured, the new point sequence that is the basis of the one line segment that has recently been extended by executing an extension process by the first arithmetic processing element, and the interpolation process The one line segment and the other line segment are connected by a connecting line segment by executing the interpolation process based on another point sequence that is the basis of another line segment generated by A second arithmetic processing element configured to execute a connection process for generating a line segment as a quaternion target trajectory that represents a rotation angle around each axis in the joint mechanism or a candidate thereof; When each of the processing element and the second arithmetic processing element is expressed by a rotation angle about each of two orthogonal axes when the bending state of the joint mechanism is expressed, the first type of quaternary whose real component is 0 While performing the interpolation processing on the basis of the sequence of points representing the emission, if the flexion of the joint mechanism is represented by each rotation of the rotation angle of the three orthogonal axes, a second kind real component is not constrained to zero characterized in that it is configured to perform the interpolation processing based on the sequence of points representing the quaternion.

  According to the control system of the present invention, an interpolation process for generating a line segment expressed by a linear combination of point sequences arranged in a model space, using a spatial interpolation basis function as a coupling coefficient is executed. The “line segment” may be either a straight line or a curved line. As a result, a quaternion target trajectory representing the rotation angle around each axis in the joint mechanism or a candidate thereof is generated. Since there is no singular point in the model space expressed by quaternions, trajectory search time can be shortened. Further, since the model space interpolation described by the quaternion is a spherical linear interpolation, an efficient trajectory from which redundancy is eliminated can be calculated. Therefore, it is possible to reliably search for a trajectory that represents an efficient operation of the control target and to shorten the trajectory search time.

  A first determination process for determining whether or not the first arithmetic processing element satisfies a specified condition in the model space, the extended line segment being out of an obstacle area representing an object in real space; It is comprised so that it may perform, and it may be comprised so that the said extension process may be performed after rejecting said one line segment on the condition that the result of the said 1st determination process is negative.

  A second determination process in which the second calculation processing element determines whether or not the connection line segment satisfies a specified condition including being out of an obstacle area representing an object in the real space in the model space; The first arithmetic processing element is configured to execute the extension process after redefining the new point sequence, on condition that the result of the second determination process is negative. It may be configured.

  According to the control system configured as described above, while avoiding contact with an obstacle, it is possible to reliably search for a trajectory that represents an efficient operation of the control target and to shorten the trajectory search time.

  A control program according to the present invention for solving the above-described problems causes a computer mounted on a control target to function as the control system.

  A robot according to the present invention for solving the above-described problems includes a base body and a limb extending from the base body, and is configured to perform a task by moving the limb body as the control target. A robot comprising the control system.

  According to the robot of the present invention, the motion can be controlled according to the target trajectory expressing the efficient motion.

BRIEF DESCRIPTION OF THE DRAWINGS The structure explanatory drawing of the robot as one Embodiment of this invention. FIG. 2 is a configuration explanatory diagram of a robot control system. Functional explanatory drawing of a control system. Explanatory drawing regarding the production | generation method of the target trajectory in model space. Explanatory drawing regarding the Example of the target track | orbit in model space. Explanatory drawing regarding the operation | movement aspect of the robot according to a target track | orbit. Explanatory drawing regarding the operation | movement aspect of the robot according to a target track | orbit. Explanatory drawing regarding operation | movement of a robot.

  An embodiment of a control system of the present invention and its control object will be described with reference to the drawings.

(Robot configuration)
First, a configuration of a robot as an embodiment of a control target of the present invention will be described.

  The robot R shown in FIG. 1 is a legged mobile robot. Like a human, the robot R extends from both sides of the base B0, the head B1 disposed above the base B0, and the top of the base B0. The left and right arm bodies B2, the hand H provided at the tip of each of the left and right arm bodies B2, and the left and right leg bodies B4 extending downward from the lower portion of the base body B0 are provided.

  The base B0 is composed of an upper part and a lower part that are connected vertically so as to be relatively rotatable about the yaw axis. The head B1 can move, such as rotating around the yaw axis with respect to the base B0.

  The arm body B2 includes a first arm body link B22 and a second arm body link B24. The base body B0 and the first arm body link B21 are connected via a shoulder joint mechanism (first arm joint mechanism) B21, and the first arm body link B22 and the second arm body link B24 are connected to an elbow joint mechanism (second arm). The second arm body link B24 and the hand H are connected via a wrist joint mechanism (third arm joint mechanism) B25. The shoulder joint mechanism B21 has a degree of freedom of rotation around the roll, pitch and yaw axes, the elbow joint mechanism B23 has a degree of freedom of rotation around the roll and pitch axes, and the wrist joint mechanism B25 has a degree of freedom of rotation around the roll, pitch and yaw axes. It has a degree of freedom of rotation around the axis.

  The leg B4 includes a first leg link B42, a second leg link B44, and a foot B5. The base B0 and the first leg link B42 are connected via a hip joint mechanism (first leg joint mechanism) B41, and the first leg link B42 and the second leg link B44 are connected to a knee joint mechanism (second leg joint). The mechanism) is connected via B43, and the second leg link B44 and the foot B5 are connected via an ankle joint mechanism (third leg joint mechanism) B45.

  The hip joint mechanism B41 has a degree of freedom of rotation about the roll, the pitch, and the roll axis, the knee joint mechanism B43 has a degree of freedom of rotation about the pitch axis, and the ankle joint mechanism B45 rotates about the roll and the pitch axis. Has a degree of freedom. The hip joint mechanism B41, the knee joint mechanism B43, and the ankle joint mechanism B45 constitute a “leg joint mechanism group”. The translational and rotational degrees of freedom of each joint mechanism included in the leg joint mechanism group may be changed as appropriate. In addition, an arbitrary one of the hip joint mechanism B41, the knee joint mechanism B43, and the ankle joint mechanism B45 may be omitted, and the leg joint mechanism group may be configured by a combination of the remaining two joint mechanisms. . Further, when the leg body B4 has a second leg joint mechanism different from the knee joint, the leg joint mechanism group may be configured to include the second leg joint mechanism. An elastic material B52 as disclosed in Japanese Patent Application Laid-Open No. 2001-129774 is provided on the bottom of the foot B5 in order to alleviate the impact when landing.

The robot R is equipped with a plurality of internal state sensors S ₁ for measuring internal states such as position and posture of the robot R in the world coordinate system. An encoder (not shown) that outputs a signal corresponding to the bending angle (joint angle) of each joint mechanism of the robot R, and an inclination sensor that outputs a signal corresponding to the posture (identified by the azimuth angle and elevation angle) of the base body B0. , and, like a pressure sensor for determining the different foot B5 and implantation and lifting corresponds to the internal state sensor S _1. An imaging apparatus for recognizing the position of the robot R in the world coordinate system by capturing an image of the surroundings of the robot R and recognizing the position of the marker fixed to the world coordinate system based on the imaging coordinates. corresponding to the state sensor S _1.

  For example, a pair of left and right head cameras C1 mounted on the head B1 and capable of sensing light in various frequency bands, such as a CCD camera and an infrared camera having an imaging range in front of the robot R, can be employed as the imaging device. Further, a waist mounted on the lower front side of the base B0 and used to measure the position and orientation of the object by detecting the reflected light of the near-infrared laser light emitted toward the front lower side of the robot R. A camera (active sensor) C2 may be employed as the imaging device.

The robot R is equipped with an external state sensor S ₂ for measuring an external state such as the position of an object around the robot R. It said such an imaging device corresponds to an external state sensor S _2.

  The robot R includes a computer constituting a part of the control system 1 and a plurality of actuators 2 for moving each of the plurality of joint mechanisms. Each operation of the actuator 2 is controlled according to a control command output from the control system 1 according to the internal state and the external state of the robot R, so that the robot R can behave adaptively in various modes. .

In addition to the robot R (see FIG. 1), any device, such as a vehicle, whose operation is controlled so as to be partially or wholly displaced according to the target trajectory may be controlled.
(Control system configuration)
The control system 1 shown in FIG. 2 is configured by a computer mounted on the robot R. The control system 1 includes a first arithmetic processing element 11 and a second arithmetic processing element 12 that are configured to execute arithmetic processing described later.

  Each of the first arithmetic processing element 11 and the second arithmetic processing element 12 includes a processor and a memory as hardware resources. The first arithmetic processing element 11 and the second arithmetic processing element 12 may be configured by a common hardware resource, or may be configured at least partially by different hardware resources.

  An arithmetic processing element is configured to execute arithmetic processing means that a processor that constitutes the arithmetic processing element reads necessary software from a memory and executes the software to execute the arithmetic processing. Means that

(Robot function)
The functions of the robot R and the control system 1 configured as described above will be described. Here, for example, as shown in FIG. 8, when the robot R moves the arm body B <b> 2, the hand H and the cup are gripped by the hand H so that they do not come into contact with other cups placed on the table. A case will be described in which the operation at the time of executing the task of moving a cup and placing it on this table is controlled.

  The first arithmetic processing element 11 recognizes the start point position and the end point position in the model space defined by the quaternion (FIG. 3 / STEP02).

  The starting point position represents, for example, the position of the quaternion representing the respective bending states of the shoulder joint mechanism B21 and the elbow joint mechanism B23 when the robot R starts to move the cup held by the one hand H. The end point position represents, for example, the position of the quaternion representing the respective bending states of the shoulder joint mechanism B21 and the elbow joint mechanism B23 when the cup held by the robot R with one hand H is placed on the table. The starting point position and the ending point position differ in principle for each joint mechanism, and separate quaternion trajectories are generated (see FIGS. 5A and 5B).

As described above, the elbow joint mechanism B23 has two degrees of freedom of rotation of pitch and roll, and the bent state is represented by rotation angles about two orthogonal axes. For this reason, the bending state of the elbow joint mechanism B23 is represented by “first type quaternion” having a real component of zero. A three-dimensional vector v = (v _x , v) in which the direction of the hand H viewed from the coordinate system fixed to the elbow joint mechanism B23 is normalized so that the norm becomes 1 by the rotation around the two orthogonal axes. _When expressed as _y , v _z ), the first type quaternion real component v _w and each imaginary component v _μ (μ = x, y, z) are expressed by the equation (50).

v _w = 0, v _μ = v .. (50).

The result q of the spherical linear interpolation of the first type quaternions q ₁₁ and q ₁₂ is expressed by equation (100) using the interpolation ratio t.

q (t) = R ⁻¹ q ₁₁ R (where R = exp (ωt / 2), ω≡ln (q ₁₁ ⁻¹ q ₁₂ )) .. (100).

As described above, the shoulder joint mechanism B21 has three degrees of freedom of rotation of pitch and roll, and the bent state is represented by the rotation angles around three orthogonal axes. For this reason, the bending state of the shoulder joint mechanism B21 is represented by “second type quaternion” in which the real component is not restricted to zero. When the rotations about the three orthogonal axes are expressed as rotations of an angle θ about one axis r = (r _x , r _y , r _z ), the real component U _w of the second quaternion and each imaginary number The component U _μ (μ = x, y, z) is represented by the formula (200).

U _w = cos (θ / 2), U _μ = r _μ sin (θ / 2) .. (200).

Each of the start point position and the end point position may be calculated by the control system 1 based on the output signals of the internal state sensor S ₁ and the external state sensor S ₂ , and is transmitted to the control system 1 by wireless communication from the external terminal device. May be entered.

  Further, one of the start point side tree and the end point side tree is selected (FIG. 3 / STEP 04). At this time, an index i indicating the number of times the selected tree is changed due to a failure in the generation of the extended trajectory of the trajectory described later is initially set to “0”.

  Then, a point (node) q is generated at an arbitrary position according to the RRT algorithm (FIG. 3 / STEP06). The starting point side tree is the starting point itself in the initial stage, and then gradually grows by sequentially connecting points q that are sequentially generated so as to expand based on the starting point according to the RRT algorithm. Similarly, the end point side tree is the end point itself in the initial stage, and then gradually grows by sequentially connecting the points q sequentially generated so as to expand based on the end point according to the RRT algorithm. An “extension process” is executed on the selected tree.

In the “extension process”, first, the point q _near closest to the point q is selected from the selection tree (FIG. 3 / STEP08).

When the point q is far from the nearest point q _near by exceeding the threshold ε, the point q is brought closer to the distance from the nearest point q _near to the threshold ε by linear interpolation, and the approached point q becomes the current control point q _new. (FIG. 3 / STEP 10).

A _new point sequence in which the current control point q _new is added to the end of the point sequence from the root of the selection tree to the nearest point q _near and which is the basis of the B-spline curve. As a basis, interpolation processing (spherical linear interpolation processing) using a B-spline curve is executed. Of the B-spline curve extending from the root of the selection tree, a portion extended by adding the current control point q _new to the old point sequence is calculated as an “extension trajectory” (FIG. 3 / STEP 12).

(Interpolation processing of the first type of quaternion)
A B-spline curve q (t) of the first type quaternion (unit quaternion with a real component of 0) representing the bending state of the elbow joint mechanism B23 having a rotational degree of freedom “2” is represented by a given control point group {q _i } Is calculated according to equation (12).

q (t) ＝ R ^-1 q ₀ R,
R = Π _{i = 1 ~ n} q _i (t),
q _i (t) ≡exq {ω _i B _{~ i, k} (t) / 2} (i = 1,2, .., n), ω _i ≡log ((q _i-1 ) ^-1 q _i ) .. (12).

Here, “ _{B˜i, k} (t)” is a cumulative B-spline basis function, and is expressed according to the expression (01) based on the normalized B-spline basis function B _{j, k} (t). .

B _{~ i, k} (t) ≡Σ _{j = 1 ~ n} B _{j, k} (t) .. (01).

The normalized B-spline function B _{j, k} (t) is defined so as to satisfy the following Cox-deBoor induction (02).

B _{i, 1} (t) = 1 (if t _i <t <t _{i + 1} ), 0 (other cases),
B _{i, k} (t) = {(t−t _i ) / (t _{i + k−1} −t _i )} B _{i, k−1} (t) + {(t _{i + k} −t) / (t _{i + k} −t _{i + 1} )} B _{i + 1, k−1} (t) .. (02).

  The differentiation of the B-spline curve of the first type quaternion is expressed according to equation (14).

dq (t) / dt = (1/2) Σ _{i = 1 ~ n} (P _i q ₀ R + Rq ₀ Q _i )
Q _i ≡q ₁ (t) q ₂ (t)… q _i (t) ω _i {dB _{~ i, k} (t) / dt} q _{i + 1} (t)… q _n-1 (t) q _n (t) .. (14).

  By the first type quaternion, the twist or bending state of the elbow joint mechanism B23 is expressed by Expression (16).

q (t + Δt) = q (t) exq (ω (t) Δt)
ω (t) ≡lim _{Δt → 0} log (q ^-1 (t) q (t + Δt)) / Δt
= Lim _{Δt → 0} log (q ^-1 (t) {q (t) + (dq (t) / dt) Δt}) / Δt
= Lim _{Δt → 0} log (1 + q ^-1 (t) (dq (t) / dt) Δt}) / Δt
= Q ^-1 (t) (dq (t) / dt)
= (1/2) Σ _{i = 1 to n} (R ^-1 q ₀ ^-1 RQ _i ⁺ q ₀ R + R ^-1 Q _i ) (16).

(Type 2 quaternion interpolation)
The B-spline curve q (t) of the second kind of quaternion (unit quaternion whose real number component is not constrained to 0) representing the bending state of the shoulder joint mechanism B21 having a rotational degree of freedom “3” is given control points { q _i } is calculated according to equation (22).

q (t) ＝ Π _{i = 0 ~ n} q _i (t),
q ₀ (t) ≡exq {ω ₀ B _{~ 0, k} (t)}, ω ₀ ≡log (q ₀ ),
q _i (t) ≡exq {ω _i B _{~ i, k} (t)}, ω _i ≡log ((q _i-1 ) ^-1 q _i ) (i = 1,2, .., n) .. (twenty two).

  The differentiation of the B-spline curve of the second type quaternion is expressed according to the equation (24).

dq (t) / dt = Σ _{i = 1 ~ n} q ₀ (t) q ₁ (t)… q _i-1 (t) q _i (t) {dB _{~ i, k} (t) / dt} q _{i +1} (t) q _{i + 2} (t)… q _n (t) .. (24).

  By the first type quaternion, the twist or bending state of the elbow joint mechanism B23 is expressed by the equation (26).

q (t + Δt) = q (t) exq (ω (t) Δt)
ω (t) ≡lim _{Δt → 0} log (q ^-1 (t) q (t + Δt)) / Δt
= Lim _{Δt → 0} log (q ^-1 (t) {q (t) + {dq (t) / dt} Δt}) / Δt
= Lim _{Δt → 0} log (1 + q ^-1 (t) {dq (t) / dt} Δt}) / Δt
= Q ^-1 (t) {dq (t) / dt}
＝ {Π _{i = 1 ~ n} q _i (t)} ^-1 q ₀ ^B _~ ^{0, k (t)} log (q ₀ ) {dB _{~ 0, k} (t) / dt} {Π _{i = 1 ~ n} q _i (t)}
+ Σ _{i = 1 ~ n} q _n ^-1 (t) q _n-1 ^-1 (t) ... q _{i + 1} ^-1 (t) {dB _{~ i, k} (t) / dt} q _{i + 1} (t ) ... q _n-1 (t) q _n (t) .. (26).

  By the interpolation process, for example, as shown in FIG. 4A, an extended trajectory (refer to the thick line) following the previous trajectory (refer to the thin line) determined according to the selection tree (start point side tree) is defined. .

Furthermore, the extended trajectory deviates from the obstacle area in the quaternion space representing the table and other obstacles such as a cup placed on the table, and the robot R performs an action exceeding the allowable range of the joint angle. It is determined whether or not the specified condition of not being a compelling trajectory is satisfied (FIG. 3 / STEP 14). The obstacle region based on the output signal of the external state sensor S _2, the position, orientation and shape can be recognized.

If it is determined that the extended trajectory satisfies the specified condition (FIG. 3 / STEP 14... YES), that is, if the extension process is successful, the current control point q _new or the extended trajectory up to the control point q _new is displayed in the selection tree. It is added (FIG. 3 / STEP 15). Further, the previous trajectory and the extended trajectory are defined as the current trajectory, and the current trajectory is handled as the previous trajectory in the next extension process. Following the extension process, a “connection process” is executed.

In the “connection process”, the selection tree is first changed (FIG. 3 / STEP 16). Thereby, when the starting point side tree is the selection tree so far, the subsequent processing is executed with the end point side tree as the new selection tree. Similarly, if the end-side tree is the selection tree so far, the current control point q _new generated first from the selection tree is selected as the new-point selection tree (see FIG. 3 / STEP 10). The point q _near closest to is selected (FIG. 3 / STEP 18).

A point sequence from the root of each of the start point side tree and the end point side tree to the nearest point q _near, and the point sequence that is the basis of the previous start point side position trajectory and the end point side position trajectory, Interpolation processing using a B-spline curve is executed based on a new point sequence in which the control point q _new is added midway. Of the B-spline curve connecting the start point and the end point, a part generated by adding the current control point q _{new to} the old point sequence is generated as a “connection trajectory” (FIG. 3 / STEP 20).

The connection trajectory is obtained by setting the section of the parameter t on the curve as [t _{i + 1} , t _{i + N + 1} ] when the node number of the control point q _new is set to “i” this time. . As a result, as shown in FIG. 4 (b), there is a connection track (see the bold line) that connects the starting point side track (see the alternate long and short dash line) extending from the start point and the end point side track (see the alternate long and two short dashes line) extending from the end point. Generated.

  Further, it is determined whether or not the connection track satisfies the specified condition (FIG. 3 / STEP 22).

  If it is determined that the connection track satisfies the specified condition (FIG. 3 / STEP22... YES), that is, if the connection process is successful, the connection track and the start-point side track and end-point track connected by the connection track are The quaternion target trajectory from the start point to the end point is generated (FIG. 3 / STEP 24).

  As a result, as shown in FIG. 5A, a first type quaternion target trajectory representing the bending state of the elbow joint mechanism B23 is generated. Similarly, as shown in FIG. 5B, a second type quaternion target trajectory representing the bending state of the shoulder joint mechanism B21 is generated.

  Then, as shown in FIG. 8, the operation of the robot R is controlled so that the hand H is displaced in the real space as represented by the curve with an arrow according to the target trajectory. As a result, the robot R can perform the task of placing the cup gripped by the hand H on the table while avoiding contact with other objects during operation and preventing the joint angles from deviating from the allowable range. It can be executed smoothly.

  On the other hand, when it is determined that the connection path does not satisfy the specified condition (FIG. 3 / STEP22... NO), that is, when the connection process fails, the extension process is executed again (see FIG. 3 / STEP08 to STEP014). ).

If it is determined that the extended trajectory does not satisfy the specified condition (FIG. 3 / STEP 14... NO), that is, if the extension process fails, the current control point q _new or the extended trajectory to the control point q _new is It is rejected without being added to the selection tree (FIG. 3 / STEP 25). Then, it is determined whether or not the index i is equal to or lower than the upper limit value K (FIG. 3 / STEP 26).

  When it is determined that the index i is equal to or lower than the upper limit value K (FIG. 3 / STEP 26... YES), the selection tree is changed (FIG. 3 / STEP 28). At this time, the index i increases by 1. In addition, processing after generation of an arbitrary point q (see FIG. 3 / STEP 06) is executed on the selected tree after the change.

When it is determined that the index i exceeds the upper limit K (FIG. 3 / STEP 26... NO), it is recognized that the trajectory search has failed (FIG. 3 / STEP 30). In response, the control system 1 waits for status changes such as moving the object (recognized through the external state sensor S ₂₎ stops the operation in accordance with the trajectory of the robot R, or the leg B4 The task execution start condition is changed by the operation of the robot R by moving the robot R from the spot and twisting the base body B0 on the spot. Then, the control system 1 re-executes the trajectory search as described above.

(Operational effect of the control system of the present invention)
According to the control system 1 that exhibits the above function, an interpolation process for generating a line segment expressed by a linear combination of point sequences arranged in a model space, using a spatial interpolation basis function as a coupling coefficient is executed. (See formulas (12) and (22) and FIGS. 4 (a) and 4 (b)). As a result, a quaternion target trajectory representing the bending state of each joint mechanism (represented by the rotation angle around each axis) or a candidate thereof is generated (see FIGS. 5A and 5B). Since there is no singular point in the model space expressed by quaternions, trajectory search time can be shortened. Further, since the model space interpolation described by the quaternion is a spherical linear interpolation, an efficient trajectory from which redundancy is eliminated can be calculated. Therefore, it is possible to reliably search for a trajectory that expresses an efficient operation of the robot R that is a control target, and to shorten the trajectory search time.

  In FIG. 6A, the change state of the bending state of each joint mechanism of the robot R is represented by the respective quaternions, Euler angles, and joint angles, and the shoulder joint mechanism B21 and the elbow joint mechanism B23 according to the respective paths. The three-dimensional position trajectory of the hand H when each of these movements is controlled is indicated by a solid line, a one-dot chain line, and a two-dot chain line, respectively. FIG. 6B shows the two-dimensional position trajectory of the hand H as a result of projecting the three-dimensional position trajectory of the hand H shown in FIG. 6A onto three two-dimensional planes. Yes.

  In FIG. 7A, the change state of the bending state of each joint mechanism of the robot R is expressed by a quaternion trajectory, and the respective operations of the shoulder joint mechanism B21 and the elbow joint mechanism B23 are controlled according to the trajectory. The movement mode of the arm body B2 of the hand H and the three-dimensional position trajectory (solid line) are shown. In FIG. 7B, the bending state of each joint mechanism of the robot R is expressed by a joint angle trajectory, and the respective operations of the shoulder joint mechanism B21 and the elbow joint mechanism B23 are controlled according to the trajectory. The movement mode of the arm body B2 of the hand H and the three-dimensional position trajectory (dashed line) are shown.

  As apparent from FIGS. 6 and 7, the position trajectory of the hand H when the change state of the normal bending state of the joint mechanism is expressed by the quaternion trajectory (solid lines in FIGS. 6A and 6B, FIG. 7A). The solid line is a position trajectory of the hand H when the change state of the normal bending state of the joint mechanism is expressed by the Euler angle or the joint angle trajectory (FIGS. 6A and 6B). (B) is significantly shorter than that shown in FIG. That is, according to the method of the present invention, it can be seen that the task can be executed by efficiently moving the joint mechanism to the controlled object.

(Other embodiments of the present invention)
The first determination process and the second determination process are omitted, and it is determined whether or not the trajectory generated at the stage when the connection process is completed satisfies the specified condition, and the determination result is positive. The trajectory may be set as a quaternion target trajectory.

  The joint mechanism such as the elbow joint mechanism B23 in which the bent state is expressed by the first type quaternion is configured to have the rotation axes about the roll and the pitch axis, but the rotation about the two orthogonal axes. Various structures may be adopted as long as the bent state is expressed by the angle. Similarly, the joint mechanism such as the shoulder joint mechanism B21 in which the bending state is expressed by the second type quaternion is configured to have a rotation axis around each of the roll, pitch, and yaw axes. Various structures may be employed as long as the bent state is expressed by the rotation angle around the orthogonal axis.

  In addition to the B-spline curve (see Expression (12)), the Hermite curve defined by Expression (32) or the Bezier curve defined by Expression (42) is used as a spatial interpolation curve, thereby extending and connecting processes. Each of these may be performed.

P (t) = (2t ³ −3t ² +1) P _start + (t ³ −2t ² + t) G1 _start + (− 2t ³ + 3t ² ) P _end + (t ³ −t ² ) G1 _end .. (32 )

Here, P _start and P _end represent the position vectors of the end points of the curve. G1 _start and G1 _end represent the tangent vector (first-order differentiation) of the curve at the end point. In the extension process and the connection process, the Hermite curve is applied to the RRT-Connect by interpolating each section of the control point sequence P _i (i = 1, 2,... M) with the Hermite curve. At this time, G1 _start and G1 _end must be selected so that the continuity of the first-order differentiation is maintained in the adjacent section.

x (t) = Σ _{i = 0 to m} J _{n, i} (t) P _i .. (42).

Here, the Bezier basis function (or Bernstein basis function) J _{n, i} (t) is defined by equation (44).

J _{n, i} (t) = n! / {I! (N−i)!} T ⁱ (1−t) ⁿⁱ .. (44).

  Spatial interpolation used in extension processing and connection processing is basically a space that forms a line segment represented by a linear combination of point sequences arranged in model space, with a basis function for spatial interpolation as a coupling engagement. Two conditions are interpolation (first condition), and that when a new control point is added to an existing control point sequence, the partial curve created by the existing control point does not change (second condition). Must meet.

  However, although the Bezier curve does not satisfy the second condition, it can be adopted as a spatial interpolation curve by connecting a plurality of Bezier curves by dividing the path point sequence into a plurality of sections, like the Hermite curve. .

DESCRIPTION OF SYMBOLS 1 ... Control system, 2 ... Actuator, 11 ... 1st arithmetic processing element, 12 ... 2nd arithmetic processing element, R ... Robot (control object).

Claims (5)

A system for controlling the operation of a controlled object having a joint mechanism in which a bending state is expressed by a rotation angle around each of two or three orthogonal axes,
The rotation angle about each axis in the joint mechanism is based on the point sequences arranged away from each other in the model space expressed by the quaternion, and the basis function for spatial interpolation is used as a coupling coefficient. After executing an interpolation process for generating a line segment expressed as a linear combination, a point is added to the point string that is the basis of the one line segment generated by the interpolation process, and a new point string is defined. The extension process is performed to generate a new line segment in which the one line segment is extended by an extension line segment by executing the interpolation process based on the new point sequence. One arithmetic processing element;
The new point sequence that is the basis of the one line segment that has recently been extended by the extension processing being executed by the first arithmetic processing element, and the basis of the other line segment that is generated by the interpolation processing By performing the interpolation process based on a certain other point sequence, the line segment in the form in which the one line segment and the other line segment are connected by a connecting line segment is moved around each axis in the joint mechanism. A quaternion target trajectory representing a rotation angle of the second or a second arithmetic processing element configured to execute a connection process generated as a candidate thereof,
Each of the first arithmetic processing element and the second arithmetic processing element is a first type whose real component is 0 when the bending state of the joint mechanism is expressed by rotation angles about two orthogonal axes, respectively. while performing the interpolation processing on the basis of the sequence of points representing a quaternion, if flexion of the joint mechanism is represented by each rotation of the rotation angle of the three orthogonal axes, a second kind real component is not constrained to zero control system characterized by being configured to execute the interpolation processing based on the sequence of points representing the quaternion. The control system according to claim 1,
A first determination process for determining whether or not the first arithmetic processing element satisfies a specified condition in the model space, the extended line segment being out of an obstacle area representing an object in real space; It is configured to execute, and on the condition that the result of the first determination process is negative, it is configured to execute the extension process after rejecting the one line segment. Control system. The control system according to claim 1 or 2,
A second determination process in which the second calculation processing element determines whether or not the connection line segment satisfies a specified condition including being out of an obstacle area representing an object in the real space in the model space; The first arithmetic processing element is configured to execute the extension process after redefining the new point sequence, on condition that the result of the second determination process is negative. The control system characterized by being comprised in this.   A control program causing a computer mounted on a control target to function as the control system according to claim 1.   A robot as the control object, comprising: a base body; and a limb extending from the base body, and configured to execute a task by moving the limb body. A robot comprising the control system according to claim 1.

Images