JP2008002867A - Attitude angle determining apparatus and determining method for the same - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide an attitude angle determining apparatus for easily and accurately obtaining the positional relation, even if there is a complex relation among coordinate systems, and to provide its determining method. <P>SOLUTION: The attitude angle determining apparatus comprises an inclination angle, acquiring section for acquiring an inclination angle as an angle between the axial direction and the gravity vector direction, on each axis of a three-axis orthogonal coordinate system configured to each link; an attitude change indicating section 28 for providing three combinations of global attitudes at maximum, as attitude relation between first and second links is fixed; inclination vector calculating sections 33, 34 for obtaining an inclination vector from the inclination angle in each axial direction of the each link in each global attitude; an inclination matrix calculating section 36 for obtaining an inclination matrix from the inclination vector obtained from each global attitude in each link; and a link matrix calculating section 38 for calculating a link matrix, representing the attitude angle by obtaining a product of one and the other inverse matrices. <P>COPYRIGHT: (C)2008,JPO&INPIT

Description

  The present invention relates to a posture angle determination device and a determination method for determining a posture relationship between two coordinate systems connected by a link mechanism. More specifically, the present invention relates to a posture angle determination device and method for representing a posture relationship between two coordinate systems with three posture angles (yaw angle φ, pitch angle θ, roll angle ψ).

  When performing posture control of a plurality of members connected by a link mechanism, generally, a coordinate system representing the position coordinates is provided for each member, and the posture of the member is first specified by the coordinates. Thereafter, the posture relationship between the coordinates is converted using three posture angles to represent the common coordinates. Conventionally, in order to obtain such a posture relationship between coordinates, the dimensions are measured based on a drawing or the angle is measured by a three-dimensional measuring device.

For example, in Patent Document 1, a rigid body link model in which an element coordinate system is set for each rigid body element and each part such as a human thigh and crus as a rigid body element (link) is created, and the model is used. A method for estimating a joint moment of a moving object is disclosed. In the technique described in this document, for example, a conversion tensor for coordinate conversion is created based on outputs from joint displacement sensors and light emitting / receiving devices provided at the hip joint, and each joint element and each It is said that the position of the rigid element is required.
JP 2005-81537 A

  However, in the above-described conventional drawings and methods based on actual measurement, the work becomes complicated as the arrangement of the two coordinate systems becomes complicated, and it is difficult to determine the posture angle. In particular, when the coordinate systems are in a torsional relationship and the relationship cannot be clearly specified on the drawing, the posture relationship cannot be read from the drawing. In addition, when the distance between coordinate systems is long or when there are obstacles between coordinate systems, measurement with a three-dimensional measuring device is difficult. When these conditions overlap, there is a problem that the posture relationship cannot be obtained by drawing or actual measurement. Further, when there is an angular error in the installation of the coordinate system, there is a problem that the posture relationship between the coordinate systems obtained from the drawing is low in accuracy.

  The technique described in Patent Document 1 is a conversion method when the rotation angle between links is grasped by, for example, a joint displacement sensor of a hip joint. Furthermore, assuming a leg plane that passes through the three joints of the hip joint, knee joint, and ankle joint, the moment acting on the joint of the moving human leg is estimated by using the two-dimensional quantity on the leg plane. As it is, it cannot be used to acquire the posture relationship between links.

  The present invention has been made to solve the above-described problems of the prior art. That is, an object of the present invention is to provide a posture angle determination device and a determination method thereof that can easily and accurately determine the posture relationship even between coordinate systems having a complicated relationship.

  The posture angle determination device of the present invention made for the purpose of solving this problem is expressed as a link mechanism having a first link and a second link connected via one or more joints, and is provided in the posture change operation unit. A posture angle determination device for determining a posture angle of a coordinate system of a second link as viewed from a coordinate system of a first link in a mounted or connected structure, and a three-axis orthogonal coordinate system set for each link An inclination angle acquisition unit that acquires an inclination angle, which is an angle between the axial direction and the direction of the gravity vector, and an attitude change operation unit while fixing the attitude relationship between the first link and the second link. The posture change instructing unit that instructs the posture change of the structure to take a maximum of three overall postures, and each axis acquired by the inclination angle acquiring unit for each link in each overall posture according to the instruction from the posture change instructing unit Inclination of direction An inclination vector calculation unit that obtains an inclination vector from an angle, an inclination matrix calculation unit that obtains an inclination matrix from an inclination vector obtained for each overall posture by the inclination vector calculation unit, and an inclination matrix calculation unit A first link matrix calculation unit that calculates a link matrix representing an attitude angle by obtaining a product of one and the other inverse matrix based on a link inclination matrix.

  According to the attitude angle determination device of the present invention, the inclination angle acquisition unit acquires an inclination angle that is an angle between each axis direction of the three-axis orthogonal coordinate system set for each link and the direction of the gravity vector. . In addition, the link mechanism is instructed by the posture change instruction section to take a total of three overall postures. In each overall posture, the inclination vector calculation unit obtains the inclination vector, and then the inclination matrix calculation unit obtains the inclination matrix from the inclination vector. Further, a link matrix representing the attitude angle is calculated based on the obtained inclination matrix. Therefore, even between coordinate systems having a complicated relationship, the posture relationship can be easily obtained with high accuracy using a small device without using an expensive mechanism.

  The “inclination angle acquisition unit”, “inclination vector calculation unit”, and “inclination matrix calculation unit” may be processed together for both links or provided for each link. There may be. The inclination vector is a vector having an inclination angle as a component. The inclination matrix is an array of inclination vectors, that is, a matrix in which inclination angles are two-dimensionally arranged according to an axis and an overall posture. The tilt matrix represents the tilt angle of the coordinate system, but does not represent the posture relationship when another coordinate system is viewed from the coordinate system.

  Further, the tilt angle acquisition unit acquires six tilt angles for the number of axes in each coordinate system (3) × the number of links (2). The inclination vector calculation unit calculates the total number of inclination vectors (maximum of three types) × the number of links (two). The inclination matrix calculation unit calculates the number (2) of inclination matrices.

  Furthermore, in the present invention, it is desirable that the inclination angle acquisition unit obtains the inclination angle by using a level provided in three directions orthogonal to each of the first link and the second link. In this way, the tilt angle can be obtained directly.

  In the present invention, the inclination angle acquisition unit may obtain the inclination angle from output values of acceleration sensors provided in three directions orthogonal to each other on the first link and the second link. In this way, the tilt angle can be easily obtained.

  In the present invention, the inclination angle acquisition unit determines the inclination angle for the first link and the second link, the output value of the acceleration sensor provided in two directions orthogonal to each other, and the output of each acceleration sensor for each link. The sum of the squares of the values may be obtained from the square root of the value obtained by subtracting the square of the value of the gravitational acceleration at the location. In this case, it is preferable that the sign of the square root is given by an external signal, GPS, marker, or the like as a vertical relationship of the coordinate system. In this way, the number of necessary acceleration sensors and the number of changes in the overall posture can be reduced.

  In addition, the posture angle determination device of the present invention is expressed as a link mechanism having a first link and a second link connected via one or more joints, and is mounted or connected to the posture change operation unit. A posture angle determination device for determining a posture angle of a second link coordinate system as viewed from a first link coordinate system in a body, wherein three directions orthogonal to each link are set so that one direction is parallel to each other Instructs the posture change operation unit to change the posture of the structure while fixing the posture relation between the first link and the second link, and an acceleration value acquisition unit that acquires acceleration values in each direction other than the one direction of the coordinate system Then, the acceleration value acquisition unit acquires the posture change instruction unit that takes two general postures by rotation around an axis parallel to the one direction, and each link in each overall posture by the instruction of the posture change instruction unit. Each Inclination for obtaining the inclination vector from the inclination angle in each axial direction obtained by the inclination angle measurement unit for each link in each overall posture as instructed by the posture change instruction unit and the inclination angle measurement unit for obtaining the inclination angle from the direction acceleration value A vector calculation unit, an inclination matrix calculation unit that obtains an inclination matrix based on an inclination vector obtained for each overall posture by an inclination vector calculation unit, and an inclination matrix of each link obtained by the inclination matrix calculation unit for each link , And a first link matrix calculation unit for calculating a link matrix representing the attitude angle by obtaining a product of one and the other inverse matrix.

  That is, when there is a relationship in which one axis is parallel to each other between the coordinate system of the first link and the coordinate system of the second link, the processing can be somewhat simplified. Generally, the acceleration value acquisition unit needs to acquire the acceleration for each link by the number of axes (here, 3 because there are 3 axes). It is only necessary to acquire the two-axis acceleration value excluding the above. That is, in the present invention, the acceleration value acquisition unit acquires four acceleration values for convenience of 2 axes × number of links. Therefore, for example, when the acceleration value is obtained from the output value of the acceleration sensor, the number of acceleration sensors can be reduced as compared with a general case when such a condition is satisfied. In addition, the number of overall postures can be reduced.

  In addition, the posture angle determination device of the present invention is the first link as viewed from the coordinate system of the first link in the structure expressed as a link mechanism having the first link and the second link connected via one or more joints. An attitude angle determination device for determining an attitude angle of a coordinate system of two links, and any one other than the one direction of the three-axis orthogonal coordinate system set so that one direction is parallel and horizontal to each link Acceleration value acquisition unit that acquires the acceleration value of the direction, inclination angle measurement unit that obtains the inclination angle from the acceleration value acquired by the acceleration value acquisition unit, and the inclination of each link obtained by the inclination angle measurement unit for each link An attitude angle calculation unit that calculates an attitude angle from the angle difference may be used.

  That is, if there is a relationship in which one axis is parallel to each other and horizontal between the coordinate system of the first link and the coordinate system of the second link, the processing can be further simplified. The acceleration value acquisition unit only needs to acquire two acceleration values, one for each link. Therefore, the number of acceleration sensors may be two, and the whole posture may be one, and it is not necessary to change. In this case, it is not necessary to have a posture change instruction unit.

  In addition, the posture angle determination device of the present invention is the first link as viewed from the coordinate system of the first link in the structure expressed as a link mechanism having the first link and the second link connected via one or more joints. A posture angle determination device that determines a posture angle of a coordinate system of two links, and includes an acceleration value acquisition unit that is provided in each link and acquires an acceleration value in each link, and a yaw angle between the coordinate systems set in each link. A second link matrix that calculates a link matrix representing an attitude angle from the first yaw angle acquisition unit to be acquired, the acceleration value acquired by the acceleration value acquisition unit, and the yaw angle acquired by the first yaw angle acquisition unit. It may have a calculation part.

  That is, the acceleration value acquisition unit only needs to acquire two acceleration values, one for each link. Therefore, the number of acceleration sensors may be two, and the whole posture may be one, and it is not necessary to change. In this case, it is not necessary to have a posture change instruction unit. Here, the yaw angle is a change in posture angle about one axis between two coordinate systems.

  In addition, the posture angle determination device of the present invention is the first link as viewed from the coordinate system of the first link in the structure expressed as a link mechanism having the first link and the second link connected via one or more joints. An attitude angle determination device for determining an attitude angle of a coordinate system of two links, an acceleration value acquisition unit provided at each link for acquiring an acceleration value at each link, and a coordinate system and a reference set for each link From the second yaw angle acquisition unit that acquires the yaw angle with the coordinate system, the acceleration value acquired by the acceleration value acquisition unit for each link, and the yaw angle acquired by the second yaw angle acquisition unit, By calculating the product of one and the other inverse matrix based on the attitude matrix of each link obtained by the first attitude matrix calculation unit that obtains the attitude matrix with respect to the reference coordinate system and the first attitude matrix calculation unit, Representing the corner It may have a first link matrix calculation unit that calculates a click matrix.

  That is, the acceleration value acquisition unit only needs to acquire two acceleration values, one for each link. Therefore, the number of acceleration sensors may be two, and the whole posture may be one, and it is not necessary to change. In this case, it is not necessary to have a posture change instruction unit.

  In addition, the posture angle determination device of the present invention is the first link as viewed from the coordinate system of the first link in the structure expressed as a link mechanism having the first link and the second link connected via one or more joints. An attitude angle determination device for determining an attitude angle of a coordinate system of two links, an acceleration value acquisition unit provided at each link for acquiring an acceleration value at each link, and an angular velocity provided at each link for acquiring an angular velocity value at each link A second attitude matrix calculation unit that obtains an attitude matrix with respect to the reference coordinate system from the acceleration value acquired by the acceleration value acquisition unit and the angular velocity value acquired by the angular velocity value acquisition unit for each link; Based on the posture matrix of each link obtained by the second posture matrix calculation unit, the first link for calculating the link matrix representing the posture angle is obtained by obtaining the product of one and the other inverse matrix. It may have a matrix calculator.

  That is, the acceleration value acquisition unit only needs to acquire six acceleration values, three for each link. Moreover, the angular velocity value acquisition unit may acquire six angular velocity values for convenience, three for each link. In addition, since there is no need to change the overall posture, it is not necessary to have a posture change instruction unit in this case.

  In addition, the posture angle determination device of the present invention is the first link as viewed from the coordinate system of the first link in the structure expressed as a link mechanism having the first link and the second link connected via one or more joints. An attitude angle determination device for determining an attitude angle of a coordinate system of two links, an acceleration value acquisition unit provided at each link for acquiring an acceleration value at each link, a magnetic sensor provided at each link, and each link A third posture matrix calculating unit for obtaining a posture matrix with respect to the reference coordinate system from the acceleration value acquired by the acceleration value acquiring unit and the information of the intersection angle with the magnetic north direction acquired by the magnetic sensor, and a third posture matrix A first link matrix calculation unit that calculates a link matrix representing an attitude angle by calculating a product of one and the other inverse matrix based on the attitude matrix of each link obtained by the calculation unit; It may be.

  That is, the acceleration value acquisition unit only needs to acquire four acceleration values, two for each link. Therefore, the number of acceleration sensors may be four, and the total posture may be one, and there is no need to change it. In this case, there is no need to have a posture change instruction section.

  Further, according to the present invention, the second posture matrix calculation unit has a magnetic sensor provided in each link, and the second attitude matrix calculation unit includes an acceleration value acquired by the acceleration value acquisition unit, an angular velocity value acquired by the angular velocity value acquisition unit, and a magnetic It is desirable to obtain the attitude matrix with respect to the reference coordinate system from the information of the intersection angle with the magnetic north direction acquired by the sensor.

  Furthermore, in the present invention, it is desirable that the posture changing operation unit is a robot incorporating a posture changing device and forms the whole posture by itself.

  Further, the present invention is expressed as a link mechanism having a first link and a second link connected via one or more joints, and the first link in a structure mounted or connected to the posture changing operation unit. Is a posture angle determination method for determining the posture angle of the coordinate system of the second link viewed from the coordinate system of each of the three axes of the three-axis orthogonal coordinate system set for each link. An inclination angle that is an angle between the first link and the second link is acquired, and the posture change operation unit is instructed to change the posture of the structure while the posture relationship between the first link and the second link is fixed, and a total of three postures are taken. For each link in each overall posture according to the instruction, the inclination vector is obtained from the obtained inclination angle in each axial direction, and for each link, an inclination matrix is obtained from the inclination vector obtained for each overall posture, and each link obtained is obtained. Based on the gradient matrix, by obtaining the product of the one and the other of the inverse matrix, it extends to the attitude angle determination method of calculating the link matrix representing the attitude angle.

  In the present invention, the inclination angle is the sum of the squares of the output values of the acceleration sensors provided in the two directions orthogonal to the first link and the second link and the output values of the acceleration sensors for each link. Is preferably obtained from the square root of the value obtained by subtracting from the square of the value of gravitational acceleration at that location.

  Further, the present invention is expressed as a link mechanism having a first link and a second link connected via one or more joints, and the first link in a structure mounted or connected to the posture changing operation unit. A posture angle determination method for determining a posture angle of the coordinate system of the second link viewed from the coordinate system of the three-axis orthogonal coordinate system set so that one direction is parallel to each link. The acceleration value in each direction other than the above is obtained, and the posture change operation unit is instructed to change the posture of the structure while the posture relationship between the first link and the second link is fixed, and the axis parallel to the one direction is obtained. Two general postures are taken by rotating around, and for each link in each overall posture by instructions, the inclination angle is obtained from the acquired acceleration value in each axial direction, and for each link in each overall posture by instructions The inclination vector is obtained from the inclination angle of the direction, the inclination matrix is obtained from the inclination vector obtained for each overall posture for each link, and the product of one and the other inverse matrix is calculated based on the obtained inclination matrix of each link. Obtaining this also extends to a posture angle determination method for calculating a link matrix representing a posture angle.

  The present invention also relates to the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints. An attitude angle determination method that determines an attitude angle, and obtains an acceleration value in any one direction other than the one direction of the three-axis orthogonal coordinate system set so that one direction is parallel to and horizontal to each link. However, the present invention extends to a posture angle determination method in which a tilt angle is obtained from the acquired acceleration value for each link, and a posture angle is calculated from a difference between the obtained tilt angles.

  The present invention also relates to the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints. A posture angle determination method for determining a posture angle, obtaining an acceleration value at each link, obtaining a yaw angle between coordinate systems set for each link, and obtaining the obtained acceleration value and the obtained yaw angle. This also extends to a posture angle determination method for calculating a link matrix representing the posture angle.

  The present invention also relates to the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints. A posture angle determination method for determining a posture angle, obtaining an acceleration value at each link, obtaining a yaw angle between the coordinate system set there and a reference coordinate system for each link, Then, the attitude angle with respect to the reference coordinate system is obtained from the obtained acceleration value and the obtained yaw angle, and the product of the inverse matrix of one and the other is obtained based on the obtained attitude matrix of each link. It also extends to a posture angle determination method for calculating a link matrix representing

  The present invention also relates to the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints. A posture angle determination method for determining a posture angle, which obtains an acceleration value at each link, obtains an angular velocity value at each link, and obtains reference coordinates from the obtained acceleration value and the obtained angular velocity value for each link. The present invention also extends to a posture angle determination method for calculating a link matrix representing a posture angle by obtaining a posture matrix for the system and obtaining a product of one and the other inverse matrix based on the obtained posture matrix of each link.

  The present invention also relates to the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints. A posture angle determination method for determining a posture angle, which obtains an acceleration value at each link, obtains information on an intersection angle with the magnetic north direction at each link by a magnetic sensor, and obtains the obtained acceleration value for each link, From the obtained information on the crossing angle with the magnetic north direction, the attitude matrix with respect to the reference coordinate system is obtained, and the attitude angle is obtained by calculating the product of one and the other inverse matrix based on the obtained attitude matrix of each link. It extends to a posture angle determination method for calculating a link matrix to be expressed.

  In the present invention, it is desirable that the posture change operation unit is a robot having a posture change device built therein, and it is desirable to form the whole posture by itself.

  According to the posture angle determination device and the determination method of the present invention, the posture relationship can be easily and accurately obtained even between coordinate systems having a complicated relationship.

  DESCRIPTION OF THE PREFERRED EMBODIMENTS Hereinafter, the best mode for embodying the present invention will be described in detail with reference to the accompanying drawings. This embodiment is a posture angle determination device for obtaining a posture relationship between two coordinate systems. Here, the attitude angle represents, for example, an angle formed by a link or the like (yaw angle φ, pitch angle θ, roll angle ψ) in a certain coordinate system. The posture relationship is a relational expression for converting the posture angle into a posture angle viewed from another coordinate system.

  First, FIG. 1 shows an example of a link mechanism whose posture angle is determined by the posture angle determination device of this embodiment. The link mechanism refers to a mechanism composed of two parts, a link and a joint. The link mechanism 10 shown in this figure has three links 11, 12, and 13. The link 11 and the link 12 are connected by a joint 14, and the link 12 and the link 13 are connected by a joint 15, respectively. The joint 14 has three rotational degrees of freedom. The joint 15 also has three degrees of freedom of rotation.

A first coordinate system (x ₁ , y ₁ , z ₁ ) and a second coordinate system (x ₂ , y ₂ , z ₂ ) are installed at two locations on the link mechanism. In FIG. 1, the first coordinate system is fixed to the link 11, and the second coordinate system is fixed to the link 13. If the joint 14 and the joint 15 are fixed, the first coordinate system and the second coordinate system have a predetermined posture relationship with each other. When the joint 14 or the joint 15 is moved, the posture relationship between the coordinate systems changes. It is also possible to change the posture of the link mechanism 10 as a whole while fixing the posture relationship between the two coordinate systems. The posture angle determination device of this embodiment obtains the posture relationship between two coordinate systems provided in such a link mechanism 10.

  Next, some specific examples of the link mechanism 10 are shown. Links themselves can be broadly divided into rigid and flexible ones. As for the relationship between links, that is, the state of the joint, there is a relationship that is always fixed, a relationship that is fixed during operation or only for a certain period, and a flexible relationship. Fixed means that the posture between the links does not change. Being flexible means that the posture between links changes.

  First, FIG. 2 shows an example in which the link is rigid and the relationship is fixed. This is a relationship between two coordinate systems installed on two different surfaces on one rigid object. That is, the example of FIG. 2 can be expressed by a link mechanism including a rigid link and a fixed joint. In this example, the attitude relationship between the first coordinate system installed on the first surface and the second coordinate system installed on the second surface does not change. Further, in this example, the adjacent surfaces are not orthogonal to each other, and the posture relationship between the two coordinate systems is in the twisted position, so the posture relationship between the coordinate systems cannot be easily obtained. Examples of cases that can be represented in this example are immediately after the parts are assembled or immediately after the parts are installed at a predetermined location, when the surfaces within the parts or between the parts and the installation surface are not orthogonal, There is a case where the shape has a deviation from the design value.

  Next, FIG. 3 shows an example in which the link is rigid and the relationship is fixed only for a certain period. This is a robot with a movable link. In the example of FIG. 3, it is composed of a rigid link and a joint that can be freely changed between fixed and non-fixed. As an example that can be represented in this example, the robot link angle is indefinite in the initial state, but during robot operation, the joint is fixed by software by a servo mechanism or by hardware such as screws. In some cases, the link mechanism can be regarded as a rigid relationship. In the fixed state, the relationship between the two coordinate systems placed on each link does not change. In the example of FIG. 3, the first coordinate system is installed on the body of the robot, and the second coordinate system is installed on the arm. However, there is no device for measuring the joint angle at the joint, and the joint angle cannot be measured directly.

  As another example of the above type, the link angle of the robot is indefinite in the initial state, but after the initialization process that the initial value of the link angle is given, it is installed at the joint after initialization. Some joint angle measuring devices can obtain joint angles. In this case, the joint may not be fixed.

  Next, FIG. 4 shows an example in which the link is flexible and the relationship is flexible. The example of FIG. 4 is a structure that is always softly deformed, and the relationship between two coordinate systems installed at two different points on such a structure is always a soft relationship. Therefore, the example of FIG. 4 can be represented by a flexible link or a link mechanism having a joint that is rigid but not fixed. The posture relationship between the first coordinate system and the second coordinate system installed on the link changes with a change in the direction of gravity acting on the link. Examples of cases that can be represented in this example include cases where the link is made of a material that easily deforms, such as rubber, or where the rigidity of the plates constituting the link is low. Note that the links and joints used in the above description are used to easily understand the relationship between the first coordinate system and the second coordinate system, and the links or joints are actually connected to the first coordinate system and the second coordinate system. It does not have to exist between the systems.

  Next, a schematic configuration of the attitude angle determination device 20 for determining the attitude angle of a link mechanism having a fixed link will be described based on the block diagram of FIG. This is intended for the non-deformable link mechanism shown in FIG. The first coordinate system and the second coordinate system in FIG. 2 may be linked by a link 11, a link 12, a link 13, a joint 14, and a joint 15 represented by the link mechanism 10 shown in FIG. .

In this example, as shown in FIG. 5, the link mechanism 10 is provided with an acceleration sensor along each axial direction of each coordinate system to be measured. That is, the link 11 of the first coordinate system is provided with an x ₁ -axis acceleration sensor 21, a y ₁ -axis acceleration sensor 22, and a z ₁ -axis acceleration sensor 23. The link 13 of the second coordinate system is provided with an x ₂ -axis acceleration sensor 24, a y ₂ -axis acceleration sensor 25, and a z ₂ -axis acceleration sensor 26.

  Further, the link mechanism 10 is provided with a posture change actuator 27 having a motor or the like for changing the overall posture. In order to calculate the posture angle by the posture angle determination device 20, it is necessary to operate the posture change actuator 27 and appropriately change the posture of the link mechanism 10 as the measurement target. For this purpose, the posture angle determination device 20 further includes a posture change command device 28 and a measurement number counter 29. The measurement number counter 29 counts the number of times that the overall posture of the link mechanism 10 is changed. Then, until the required number of posture changes are completed, the posture change commanding device 28 issues a command to the posture change operating device 27 to change the overall posture of the link mechanism 10.

The attitude angle determination device 20 also receives a tilt angle output from the x ₁ -axis acceleration sensor 21, the y ₁ -axis acceleration sensor 22, and the z ₁ -axis acceleration sensor 23 and samples the values thereof, and an x ₂ A sampling device 32 that receives the outputs of the axial acceleration sensor 24, the y ₂ -axis acceleration sensor 25, and the z ₂ -axis acceleration sensor 26 and samples the values. The values sampled here are the cosine values of the inclination angles in the respective axial directions.

  The attitude angle determination device 20 includes a gradient vector calculator 33 that calculates a gradient vector from the result of the sampler 31 and a gradient vector calculator 34 that calculates a gradient vector from the result of the sampler 32. These inclination vectors represent the inclination of each coordinate system with respect to the direction of gravity. Further, the attitude angle determination device 20 receives a calculation result of the inclination vector calculator 33 and the inclination vector calculator 34, and an inclination matrix calculator 36 that receives an output of the register 35 and calculates an inclination matrix from each inclination vector. Have

  Here, when the transformation from the reference coordinate system to the first coordinate system is the posture matrix T1, and the transformation from the reference coordinate system to the second coordinate system is the posture matrix T2, from the posture matrix T1 to the posture matrix T2. Is a link matrix T12. The attitude angle determination device 20 includes a link matrix calculator 37, a link matrix display 38, and an attitude angle calculation / display 39. The link matrix calculator 37 receives the result of the gradient matrix calculator 36 and calculates the link matrix T12. The calculated link matrix is displayed by the link matrix display 38. Further, the attitude angle calculation / display unit 39 calculates and displays the attitude angle using the link matrix T12.

  A procedure for obtaining the posture angle using the posture angle determination device 20 will be briefly described. First, there are links 11 to 13 between the first coordinate system and the second coordinate system, and when the setting of the relationship between the links is completed, the posture relationship between the two coordinate systems is fixed. While maintaining this fixed posture relationship, the overall posture of these link mechanisms 10 is changed in a maximum of three ways according to conditions. Here, as a most general example, a case of changing in three ways will be described.

  After changing the overall posture, the output results of the acceleration sensors 21 to 26 in the overall posture are acquired. As a result, data of inclination angles for a maximum of three possible postures are obtained. That is, an instruction to change the posture is sent from the posture change commanding device 28 to the posture changing motion unit 27 while counting the number of times of measurement by the measurement number counter 29. And in each whole attitude | position, the acceleration of each axial direction is measured by the six acceleration sensors 21-26 in total.

  Next, the inclination angle with respect to the gravity direction of each axis of the first coordinate system is extracted from the outputs of the acceleration sensors 21 to 23 by the sampler 31 for each overall posture. In addition, the inclination angle of each axis of the second coordinate system with respect to the gravitational direction is extracted by the sampler 32 from the outputs of the acceleration sensors 24 to 26 for each overall posture. Further, the inclination vector calculator 33 calculates an inclination vector of the first coordinate system from these inclination angles. Similarly, the inclination vector calculator 34 calculates the inclination vector of the second coordinate system.

  As a result, a total of three types of gradient vectors corresponding to the overall postures are obtained, and the gradient matrix in which the gradient vectors are arranged is obtained by the gradient matrix calculator 36. Subsequently, the link matrix calculator 37 calculates a link matrix T12 based on this gradient matrix. Further, the link matrix display 38 displays the link matrix T12. Furthermore, the attitude angle (yaw angle φ, pitch angle θ, roll angle ψ) of the second coordinate system viewed from the first coordinate system is obtained from the link matrix T12 by the attitude angle calculator / display 39.

  Next, each procedure will be described in detail. First, for the example of the object shown in FIG. 2 and FIG. 3, the orientation matrix T1 to the first coordinate system viewed from the reference coordinate system (X, Y, Z), the first coordinate system to the second coordinate system viewed from the reference coordinate system. FIG. 6 and FIG. 7 show the relationship of the posture matrix T2 and the link matrix T12 to the second coordinate system as viewed from the first coordinate system. Here, the orientation and position of the reference coordinate system do not change with time, and the Z-axis is directed in the opposite direction of the gravity vector. These examples of FIGS. 6 and 7 can be expressed as the link configuration shown in FIG.

First, the relationship between the link matrix T12 and each of the attitude matrices T1 and T2 is expressed by the following formula 1.
T2 = T1 · T12 (Formula 1)
When this posture matrix T1 is expressed using the posture angles (yaw angle φ ₁ , pitch angle θ ₁ , roll angle ψ ₁ ) of the first coordinate system viewed from the reference coordinate system, the following equation 2 is obtained.
T1 = RPY (φ ₁ , θ ₁ , ψ ₁ )
= Rot (Z, φ ₁ ) Rot (Y, θ ₁ ) Rot (X, ψ ₁ ) (Formula 2)
This is done by first rotating around the Z axis in the reference coordinate system by φ ₁ and then rotating around the Z axis and then rotating around the Y axis by θ ₁ and then rotating around the Y axis around the X axis. It shows that the first coordinate system can be obtained by rotating ψ ₁ .

Similarly, when the posture matrix T2 is expressed by using the posture angles (yaw angle φ ₂ , pitch angle θ ₂ , roll angle ψ ₂ ) of the second coordinate system viewed from the reference coordinate system, the following equation 3 is obtained. Become.
T2 = RPY (φ ₂ , θ ₂ , ψ ₂ )
= Rot (Z, φ ₂ ) Rot (Y, θ ₂ ) Rot (X, φ ₂ ) (Formula 3)
Further, when the link matrix T12 is expressed using the attitude angles (yaw angle φ ₁₂ , pitch angle θ ₁₂ , roll angle ψ ₁₂ ) of the second coordinate system viewed from the first coordinate system, the following equation 4 is obtained. Become.
_{T12 = RPY (φ 12, θ} 12, ψ 12)
= Rot (z ₁ , φ ₁₂ ) Rot (y ₁ , θ ₁₂ ) Rot (x ₁ , ψ ₁₂ ) (Formula 4)
This indicates that the second coordinate system can be obtained by rotating around the respective axes in the order of φ ₁₂ , θ ₁₂ , and ψ ₁₂ from the first coordinate system.

  Here, as shown in FIG. 8, the links 11 and 13 are provided with acceleration sensors 21 to 26 that detect acceleration in each axis direction of each coordinate system. Furthermore, since the Z axis of the reference coordinate system is set in the direction opposite to the gravity, the angle formed by the Z axis of the reference coordinate system of each axis of the first and second coordinate systems from the output of each acceleration sensor 21-26. Is required. This is because, in the link mechanism 10 that is not moving, only the influence of gravity is measured by the acceleration sensors 21 to 26. A method for obtaining the posture matrices T1 and T2 of the first and second coordinate systems from the outputs of the acceleration sensors 21 to 26 will be described.

  First, when the posture matrix T1, the posture matrix T2, and the link matrix T12 are represented in general form, they can be expressed as shown in the following Equation 5, Equation 6, and Equation 7.

  Using these, when Expression 1 is displayed as an element, the following Expression 8 is obtained.

Here, for example, the element c ₁ of the posture matrix T1 is a direction cosine of an inclination angle formed by the x ₁ axis of the first coordinate system and the Z axis of the reference coordinate system. That is, as shown in FIG. 9, when the inclination angle formed by the x ₁ axis of the first coordinate system and the Z axis of the reference coordinate system is γ ₁ , it is expressed by the following formula 9.
c ₁ = cos γ ₁ (Formula 9)
Similarly, in the posture matrix T1 and the posture matrix T2, each element in the lowermost stage represents a direction cosine between each axis of each coordinate system and the Z axis of the reference coordinate system. C ₁ , f ₁ , i ₁ , c ₂ , f ₂ , i ₂ are obtained from the output values of.

Accordingly, focusing on c ₁ , f ₁ , i ₁ , c ₂ , f ₂ , i ₂ in the element display of Expression 8, the following Expression 10 to Expression 12 are obtained.
c ₂ = c ₁ p + f ₁ q + i ₁ r (Equation 10)
f ₂ = c ₁ s + f ₁ t + i ₁ u (Equation 11)
i ₂ = c ₁ v + f ₁ w + i ₁ α (Formula 12)

  Therefore, Expression 8 can be expressed by Expression 13 below. The right term on the left side of Equation 13 is called the second gradient vector, and the right side is called the first gradient vector.

  Next, the overall posture of the link mechanism 10 is changed in three ways while maintaining the posture relationship between the first coordinate system and the second coordinate system. And the output value of each acceleration sensor 21-26 is obtained for every whole attitude | position. Further, similarly to the above procedure, the tilt vector of the first coordinate system and the tilt vector of the second coordinate system in each overall posture can be obtained. That is, three sets of gradient vectors are obtained for each coordinate system. Here, a set of gradient vectors in the first coordinate system obtained by changing the overall posture of the link mechanism 10 is a first gradient matrix S1, and a set of gradient vectors in the second coordinate system is a second gradient matrix S2. That is, the link matrix T12 can be expressed by the following equation 14 by collectively representing each set of three gradient vectors as a gradient matrix.

In Equation 14, for example, j of c _jk is a coordinate system number, and k indicates the number of measurements. Further, in order for the inverse matrix of the second term on the right side of Equation 14 to exist, the following Equation 15 must be satisfied.
c ₂₁ · f ₂₂ · i ₂₃ + f ₂₁ · i ₂₂ · c ₂₃ + c ₂₂ · f ₂₃ · i ₂₁ -i ₂₁ · f ₂₂ · c _23- f ₂₁ · c ₂₂ · i ₂₃ -i ₂₂ · f ₂₃ · c ₂₁ ≠ 0 (Formula 15)
Specifically, rotation around the Z axis of the reference coordinate system or rotation around the same axis is avoided because the posture has not changed.

Then, using the values of each element of the link matrix T12 obtained by Expression 14, the posture angles of the second coordinate system viewed from the first coordinate system are determined as the following Expressions 16 to 18. .
φ ₁₂ = atan2 (q, p) (Expression 16)
θ ₁₂ = atan2 (−r, cos φ · p + sin φ · q) (Expression 17)
ψ ₁₂ = atan 2 (sin φ · v−cos φ · w, −sin φ · s + cos φ · t) (Equation 18)

Therefore, using the above equations 16 to 18, the second coordinate system uses the first coordinate system as φ ₁₂ around the z ₁ axis, then θ ₁₂ around the rotated y ₂ axis, and finally after the rotation. it can be the of x ₂ axis indicating by which [psi ₁₂ rotate. From this, the relationship between the first coordinate system and the second coordinate system was obtained. However, if the x ₂ axis is close to the z ₁ axis, φ ₁₂ cannot be obtained from Equation 16. In such a case, the link matrix T12 may be used instead of using the attitude angle to represent the relationship between the first coordinate system and the second coordinate system.

  Next, a specific example of the link matrix T12 obtained in this way is shown in Equation 19.

Next, this attitude angle determination method will be described based on the flowchart of FIG. 10 and the block diagram of FIG. When the setting of each link relation of the link mechanism 10 is completed, first, measurement by each acceleration sensor 21 to 26 is started (S101). Then, the sampling unit 31 extracts the tilt angles related to the x ₁ axis, the y ₁ axis, and the z ₁ axis of the first coordinate system from the tilt angle output results of the acceleration sensors 21 to 23 of the first coordinate system (S102). Further, the inclination vector calculator 33 calculates the inclination vector and stores it in the register 35 (S103).

  Similarly, the sampling unit 32 extracts the inclination angle of the second coordinate system from the results of the acceleration sensors 24 to 26 in the second coordinate system (S104). Further, the inclination vector calculator 34 calculates the inclination vector and stores it in the register 35 (S105). When the calculation for the first and second coordinate systems is completed, the posture change commander 28 instructs the posture change motion unit 27 to change the postures of the first and second coordinate systems (S106).

  At this time, the measurement number counter 29 counts the number of measurements, and the posture change commander 28 issues a command to the posture change actuator 27. The total orientation is changed in three ways, and a set of gradient vectors of both coordinate systems is calculated for the three types of overall orientation. When the measurement is performed three times (S107: Yes), the gradient matrix calculator 36 obtains the first gradient matrix S1 from the gradient vector set of the first coordinate system and the second gradient from the gradient vector set of the second coordinate system. Each matrix S2 is generated (S108).

  Subsequently, a link matrix T12 is calculated by the link matrix calculator 37 using the first gradient matrix S1 and the second gradient matrix S2 (S109). Further, the calculated link matrix is displayed by the link matrix display 38 (S110). Then, the attitude angle calculation / display unit 39 calculates and displays the attitude angle (φ, θ, ψ) from the first coordinate system to the second coordinate system based on the obtained link matrix (S111). This process is completed.

Next, simplification of processing when there is a predetermined relationship between the first coordinate system and the second coordinate system will be described. First, if it is known that one of the two coordinate systems is parallel, such as when the z ₁ and z ₂ axes are parallel, the rotation about that parallel axis Can only be described. However, here, the case where the z ₁ axis and the z ₂ axis are parallel to the direction of gravity is excluded. For example, in the case where the z ₁ axis and the z ₂ axis are parallel, θ ₁₂ and ψ ₁₂ do not need to be used for rotation, and can be expressed by the following equation 20 including only φ ₁₂ .

Further, in this case, if the rotary shaft to change the overall orientation and z ₁ axis acceleration sensor both in the z ₁ axis and z ₂ axis is not required. That is, four acceleration sensors 21, 22, 24, and 25 may be used. Furthermore, it is only necessary to take two general postures. Then, the link matrix T12 can be obtained from the two measurement results using the following equation (21).

Next, an example of a specific configuration of a posture angle determination device that targets a rigid structure during operation as in the example of FIG. 3 will be described with reference to the block diagram of FIG. In addition, about the member common to the example shown in FIG. 5, the same code | symbol is attached | subjected and description is abbreviate | omitted. The same applies to the following similar block diagrams. FIG. 11 shows a structure in which the object to be measured is rigid during operation, and one of the two coordinate systems (here, the z ₁ axis and the z ₂ axis) is parallel and the rotation axis is parallel. This information is obtained as an external signal (or up and down of the overall posture). In the attitude angle determination device applied to such a link mechanism 110, the acceleration sensors need only be provided on the x-axis and the y-axis of each coordinate system.

  In a structure that is rigid during operation, when the joint of the link mechanism 110 moves, the in-operation signal generator 111 sends to the link fixing indicator 112 that the link mechanism 110 is in operation. Further, when the operation is finished, the link fixing instruction unit 112 instructs the link mechanism fixing unit 113 to fix the joint in the link mechanism 110. Thereby, the joint of the link mechanism 110 is fixed.

  Here, if translation acceleration other than gravitational acceleration, for example, is applied to each of the acceleration sensors 21, 22, 24, 25 during measurement of the tilt angle, an error occurs in the measurement result of the tilt angle. For this reason, the values of the elements of the gradient vector output from the gradient vector calculators 33 and 34 need to be stable during the overall posture measurement. Therefore, the stationary state determiner 114 determines whether or not each element of the gradient vector output from the gradient vector calculators 33 and 34 is constant within the time counted by the timer 115, and the change amount of each element is within a specified value. If so, the register 35 is instructed to record the acquired gradient vector.

  Next, the stationary state determiner 114 informs the posture change commander 28 that a posture change may be commanded. The posture change command unit 28 instructs the posture change operating unit 27 to change the overall posture. The posture change actuator 27 changes the overall posture of the link mechanism 110 so that the posture relationship between the first coordinate system and the second coordinate system does not change. Outputs of the respective acceleration sensors 21, 22, 24, 25 are always taken in by the sampling units 31, 32, and tilt vectors are generated in the tilt vector calculators 33, 34.

  At this time, in this example, since the rotation axis parallel state detection signal 116 is obtained from the outside, the rotation axis determination unit 117 determines the rotation axis, and the inclination vector calculators 33 and 34 and the measurement number counter 29 are compared. , The axis name of the rotation axis, the acceleration signal name necessary for calculating the tilt matrix among the obtained acceleration signals of the plurality of axes, and the necessary number of measurements are transmitted. Further, the register 35 stores the measurement number value obtained from the measurement number counter 29 and each element value of the gradient matrix. This measurement is repeated the number of times specified by the measurement number counter 29. When the necessary number of measurements is completed, the link matrix calculator 37 calculates the link matrix of the second coordinate system as viewed from the first coordinate system. This is displayed on the link matrix display 38 and further converted into a posture angle by a posture angle calculation / display device 39.

  Next, the object to be measured is a rigid structure during operation, and the set of one axis of both coordinate systems is parallel, but information on the parallel rotation axes (or the upper and lower positions of the entire posture) is obtained as an external signal. An example of the case where it is not possible is shown in the block diagram of FIG. In this case, the rotation axis must be determined internally. Therefore, a total of six acceleration sensors 21 to 26 are required for each axis.

  In this example, the tilt angle signal obtained in the sampling units 31 and 32 is sent to the rotation axis determination unit 117. The rotation axis determination unit 117 compares the inclination angles of the same axis in the first coordinate system and the second coordinate system, and if the change amount of each inclination angle value is within a predetermined value for a specific axis, the axis rotates. It is determined that the axis is a parallel axis in the first coordinate system and the second coordinate system. The rotation axis determination unit 117 determines the set of parallel axes determined in this way as each rotation axis. Further, for the tilt vector calculators 33 and 34 and the measurement count counter 29, the axis name of the rotation axis, the axis name necessary for calculating the tilt matrix among the obtained tilt angle signals of the plurality of axes, and the necessary Tell the number of measurements. The following processing is the same as that in the example of FIG.

In the next example, in addition to the example of FIG. 12 described above, a parallel set of axes is in the horizontal plane. For example, in the example of FIG. 12 above, the rotation axes that change the overall posture are the z ₁ axis and the z ₂ axis that are a set of parallel axes, and these are in the horizontal plane having the gravity direction as a normal line. Is the case. In this case, the acceleration sensor may be installed only in any combination of the x ₁ axis and the x ₂ axis, or the y ₁ axis and the y ₂ axis. That is, either a combination of acceleration sensors 21 and 24 or a combination of acceleration sensors 22 and 25 may be used. And it is only necessary to set the whole posture in one way. In this case, a yaw angle that is a change in posture angle around the z ₁ axis can be obtained from the tilt angle obtained from the output of the acceleration sensor.

If the axis where the acceleration sensor is installed is not parallel to the Z axis of the reference coordinate system between the first overall posture and the second overall posture, the following equation 22 is substituted into equation 20: A link matrix T12 can be obtained.
Yaw angle φ ₁₂ = Inclination angle of second coordinate system−Inclination angle of first coordinate system (Formula 22)
That is, in this case, since the rotation axis is the z ₁ axis, θ ₁₂ and ψ ₁₂ among the posture angles do not need to be used for rotation, and the link matrix T12 can be expressed only by the yaw angle φ ₁₂ .

When the axis where the acceleration sensor is installed is parallel to the Z axis of the reference coordinate system between the first overall posture and the second overall posture, the link matrix T12 is obtained from the following equations 23 and 20. Can be obtained.
Yaw angle φ ₁₂ = Inclination angle of the second coordinate system + Inclination angle of the first coordinate system (Equation 23)

  Furthermore, when the magnitude of the gravitational acceleration is known and the vertical direction (gravity direction) of the coordinate system is known, the following simplification is possible. An example of this case is shown in the block diagram of FIG. In this case, the attitude angle determination device has acceleration calculators 118 and 119, which receive the vertical identification external signal 120 of the coordinate system.

If the magnitude of gravitational acceleration is | G |, for example, the acceleration az in the z ₁ -axis direction has a relationship represented by the following equation 24 with the accelerations ax and ay in the x ₁ -axis and y ₁ -axis directions.
| G | = √ (ax ² + ay ² + az ² ) (Formula 24)
Therefore, the acceleration az in the z ₁ -axis direction can be expressed by the acceleration calculator 118 using the accelerations ax and ay in the x ₁ -axis and y ₁ -axis directions as in the following Expression 25.
az = ± √ (| G | ² −ax ² −ay ² ) (Equation 25)

Further, since the upper and lower sides of the z ₁ axis are known from the coordinate system upper and lower identification external signal 120, the sign of az is given. Therefore, neither the z ₁ axis nor the z ₂ axis requires an acceleration sensor. From the four measurement results of the acceleration sensors 21, 22, 24, 25, the z ₁ axis direction and the z ₂ axis direction Can also be obtained. The link matrix T12 can be calculated from Equation 14. Note that this second condition is generally satisfied in, for example, a robot that moves approximately upright on the ground surface.

Next, as in the example of FIG. 3, a more general configuration of a posture angle determination device for a rigid structure during operation is shown in FIG. Similarly to the example of FIG. 8, the link mechanism 110 of this example is expressed as a combination of the first coordinate system and the second coordinate system by the link 11, the link 12, the link 13, the joint 14, and the joint 15. The The link mechanism 110 is provided with an acceleration sensor along each axial direction of each coordinate system to be measured. That is, the link 11 of the first coordinate system is provided with an x ₁ -axis acceleration sensor 21, a y ₁ -axis acceleration sensor 22, and a z ₁ -axis acceleration sensor 23. The link 13 of the second coordinate system is provided with an x ₂ -axis acceleration sensor 24, a y ₂ -axis acceleration sensor 25, and a z ₂ -axis acceleration sensor 26. Further, the link mechanism 10 is provided with a posture changing unit including a posture changing actuator 27 having a motor or the like for changing the overall posture.

  In a structure that is rigid during operation, when the joint of the link mechanism 110 moves, the in-operation signal generator 111 sends to the link fixing indicator 112 that the link mechanism 110 is in operation. Further, when the operation is finished, the link fixing instruction unit 112 instructs the link mechanism fixing unit 113 to fix the joint in the link mechanism 110. Thereby, the joint of the link mechanism 110 is fixed.

  Here, if translation acceleration other than gravitational acceleration, for example, is applied to each of the acceleration sensors 21 to 26 during measurement of the tilt angle, an error occurs in the measurement result of the tilt angle. For this reason, during the overall posture measurement, the value of each element of the gradient vector output from the gradient vector calculators 33 and 34 needs to be stable. Therefore, the stationary state determination unit 114 determines whether each element of the inclination vector output from the inclination vector calculators 33 and 34 is constant within the time counted by the timer 115, and the amount of change of each element is a specified value. If within, the register 35 is instructed to record the acquired gradient vector.

  Next, the stationary state determiner 114 informs the posture change commander 28 that a posture change may be commanded. The posture change command unit 28 instructs the posture change operating unit 27 to change the overall posture. The posture change actuator 27 changes the overall posture of the link mechanism 110 so that the posture relationship between the first coordinate system and the second coordinate system does not change. The outputs of the acceleration sensors 21 to 26 are always taken in by the sampling units 31 and 32, and the inclination vector calculators 33 and 34 generate inclination vectors.

  The register 35 stores the measurement number value obtained from the measurement number counter 29 and each element value of the gradient vector. This measurement is repeated the number of times specified by the measurement number counter 29. When the necessary number of measurements is completed, the link matrix calculator 37 calculates the link matrix of the second coordinate system as viewed from the first coordinate system. This is displayed on the link matrix display 38, and further converted into an attitude angle and displayed on the attitude angle calculator / display 39.

  Next, a posture angle determination device for a structure having a soft link and a soft relationship as shown in FIG. 4 will be described. In such a structure, changing the overall posture changes the posture relationship between the first coordinate system and the second coordinate system, and thus changing the overall posture may increase measurement errors. Therefore, it is preferable to measure the posture angle without changing the overall posture. FIG. 15 shows the relationship between the structure and each coordinate system in this example. A schematic configuration of the attitude angle determination device is shown in a block diagram of FIG.

  In this example, as shown in FIG. 15, on the first coordinate system and the second coordinate system, acceleration sensors 21 to 21 for measuring the acceleration in the respective axis directions of the coordinate system and the angular velocities around the respective axes. 26, angular velocity sensors 211 to 216 are installed. As shown in FIG. 16, outputs from the acceleration sensors 21 to 26 are sampled by the sampling units 31 and 32, and outputs from the angular velocity sensors 211 to 216 are sampled by the sampling units 217 and 218. The sampled output is introduced into the combiners 219 and 220. The combiners 219 and 220 calculate the most likely current posture based on the respective sensor outputs.

Further, posture matrix calculators 221 and 222 generate posture matrices from the outputs of the combiners 219 and 220. As a result, posture matrices T1 and T2 viewed from the reference coordinate system of the first coordinate system and the second coordinate system that change with time can be obtained continuously. The link matrix T12 can be obtained from the posture matrix thus obtained and the following Expression 26.
T12 = T ₁ ⁻¹ · T2 (Formula 26)
T1: Attitude matrix to the first coordinate system viewed from the reference coordinate system
T2: Attitude matrix to the second coordinate system viewed from the reference coordinate system
T12: Link matrix from the first coordinate system to the second coordinate system If the method of this example is adopted, the overall posture may be one and does not need to be changed.

  Alternatively, in the example shown in FIG. 15, an acceleration sensor for measuring the acceleration in the respective axis directions of the coordinate system and the magnetic north direction on the first coordinate system and the second coordinate system. The magnetic sensor may be installed. If an acceleration sensor is used, a tilt angle can be obtained as an angle between each coordinate axis on each coordinate system and the reference coordinate system Z-axis, and then a roll angle ψ and a pitch angle θ can be obtained from the tilt angle. When roll angles, pitch angles, and yaw angles are used as attitude angles, the yaw angle φ can be easily calculated by installing a magnetic sensor on the x-axis of each coordinate system due to the nature of attitude angle notation. Therefore, it is preferable. Further, if it is installed not only on the x-axis but also on the y-axis and z-axis of each coordinate system, the measurement accuracy and reliability are improved and a more preferable configuration is obtained.

  Next, another example will be described. The attitude of the first coordinate system viewed from the reference coordinate system can be represented by an attitude matrix T1. Specific elements of the posture matrix T1 are expressed by the following Expression 27 obtained by expanding Expression 2 described above.

In this case, the component in the third row, first column of the posture matrix T1, which is the direction cosine of the forming slope angle between the Z-axis of the x ₁ axis and the reference coordinate system of the first coordinate system. The component in the third row and second column is the direction cosine of the inclination angle formed by the y ₁ axis of the first coordinate system and the Z axis of the reference coordinate system. The component in the third row and third column is the direction cosine of the inclination angle formed by the z ₁ axis of the first coordinate system and the Z axis of the reference coordinate system. That is, as shown in FIG. 9, if the inclination angle formed between each axis of the first coordinate system and the Z axis of the reference coordinate system is γ ₁ to γ ₃ , the direction cosine of the inclination angle is expressed by the following equations 28 to 30 It is represented by

−sin θ ₁ = cos γ ₁ (Equation 28)
cos θ ₁ sin ψ ₁ = cos γ ₂ (formula 29)
cos θ ₁ cos ψ ₁ = cos γ ₃ (Equation 30)
Further, from these equations, the roll angle θ ₁ and the pitch angle ψ ₁ of the first coordinate system viewed from the reference coordinate system are expressed by the following equations 31 and 32.
θ ₁ = sin ⁻¹ (−cos γ ₁ ) (Equation 31)
ψ ₁ = tan ⁻¹ (cos γ ₂ / cos γ ₃ ) (Expression 32)

Since the output of the acceleration sensor does not change even if the orientation of the coordinate system rotates around the Z axis of the reference coordinate system, the yaw angle φ cannot be obtained. However, the yaw angle φ can be obtained by obtaining magnetic north using a magnetic sensor. Specifically, the difference between the magnetic north direction viewed from the reference coordinate system and the magnetic north direction viewed from the first coordinate system is set as the yaw angle φ _1, and the attitude matrix T 1 is obtained using Expressions 31 to 32 and Expression 2. Next, the difference between the magnetic north direction viewed from the reference coordinate system and the magnetic north direction viewed from the second coordinate system is set as the yaw angle φ _2, and the posture row T2 is obtained from the equations 31 to 32 and the equation 2 relating to the second coordinate system. By applying the attitude matrix T1 and the attitude matrix T2 to Expression 26, the link matrix T12 can be obtained.

As another method, a yaw angle to the second coordinate system viewed from the first coordinate system may be directly obtained using a magnetic sensor. In this embodiment, instead of the magnetic sensor, GPS (Global Positioning System) or a marker may be used as information from the outside. In the following Expression 33, the yaw angle of the second coordinate system viewed from the first coordinate system obtained from the magnetic sensor or external information is substituted into (φ ₂ −φ ₁ ).

Also, the attitude angles θ ₁ , θ ₂ , ψ ₁ , ψ ₂ are obtained from the output of the acceleration sensor. The link matrix T12 can be obtained by this method. In this way, the number of acceleration sensors installed in the first coordinate system and the second coordinate system may be two, and the measurement accuracy can be improved. The overall posture may be one.

  Next, a schematic configuration of still another posture angle determination device is shown in a block diagram of FIG. On the first coordinate system and the second coordinate system, acceleration sensors 21 to 26 for measuring acceleration in the respective axis directions of the coordinate system, and angular velocity sensors 211 to 21 for measuring angular velocities around the respective axes. 216 and magnetic sensors 231 to 236 for measuring the magnetic north direction are installed. If the acceleration sensors 21 to 26 and the angular velocity sensors 211 to 216 are used, the roll angle ψ, the pitch angle θ, and the yaw angle φ can be obtained. However, the yaw angle φ is changed due to temporal changes in the outputs of the angular velocity sensors 211 to 216. May change.

  Therefore, the amount of change in the yaw angle φ can be reduced by integrating the outputs of the angular velocity sensors 211 to 216 with the outputs of the magnetic sensors 231 to 236. On the other hand, the yaw angle φ obtained by the magnetic sensors 231 to 236 is affected by an external magnetic field, but this influence can be reduced by integrating with the outputs of the angular velocity sensors 211 to 216.

  Since the angular velocity sensors 211 to 216 and the magnetic sensors 231 to 236 complement each other by this method, the posture matrices T1 and T2 obtained from the posture angles are not easily affected by the output fluctuation of the angular velocity sensors 211 to 216 and the external magnetic field. Thus, the measurement accuracy of the link matrix T12 obtained from the attitude matrices T1 and T2 can be improved. In addition, although the flexible structure was demonstrated here, you may apply to the structure which has a link which becomes rigid only at the time of a rigid structure or operation | movement. In this case, the overall posture may be one.

  Furthermore, an example of another posture angle determination device for a structure having a soft link and a soft relationship as shown in FIG. 4 will be described. FIG. 18 shows a schematic configuration of the posture angle determination device when the posture angle φ is given from the outside. In such a case, using the accelerations from the acceleration sensors 21, 22, 24, and 25 installed on the first coordinate system and the second coordinate system, the roll angle ψ and the pitch angle θ are obtained from the above equations 31 and 32. Is obtained. The yaw angle is given as an external attitude angle signal 241. Based on these three attitude angles, attitude matrix calculators 221 and 222 obtain an attitude matrix. After the result is stored in the register 35, the link matrix calculator 37 obtains the link matrix.

  Note that the acceleration sensor, angular velocity sensor, and magnetic sensor described in each of the above examples do not necessarily need to be permanently installed on the first coordinate system and the second coordinate system. In the initial state, these sensors are installed only in the first coordinate system. After measuring the posture of the first coordinate system in a certain overall posture, these sensors are moved to the second coordinate system, and the posture of the second coordinate system in the same overall posture. It may be used to measure

  A specific example of a link mechanism to which the attitude angle determination device as described above is applied is shown in FIG. In this example, there are links 11, 12, 13 and joints 14, 15 that connect the links. A corresponding coordinate system is provided on each link in order to define the posture of each link. The first coordinate system that moves together with the link 11 is located at the part on the link 11, the second coordinate system that moves together with the link 12 is located at the part on the link 12, and the second coordinate system that moves together with the link 12 is located at the part on the link 13. A third coordinate system that moves integrally with the link 13 is provided.

  Here, the joint 14 and the joint 15 are provided with a motor as an actuator for changing the link angle and an encoder for measuring the link angle of the link to which the joint is connected. Normally, the relative angle type is used instead of the absolute angle type to reduce the size and weight of the encoder. Since the initial value is undefined for the relative angle type, a correct link angle cannot be obtained before the link mechanism is initialized. For this reason, before using the link mechanism, an initialization process is required in which each joint is rotated to its maximum rotatable angle and pressed against a mechanical projection such as a pin to define an initial value of the joint angle. The initialization process takes time, and the weight balance of the link mechanism is greatly broken, resulting in unstable posture.

  In contrast, an embodiment of the initialization process using the present invention will be described. As shown in FIG. 19, this link mechanism has link mechanism fixers 16 and 17 at the joint 14 and the joint 15 so that the posture relationship between the first coordinate system and the second coordinate system is constant during measurement. is set up. The link mechanism fixtures 16 and 17 have a function of fixing the angle between the links. As the link mechanism fixing devices 16 and 17, there are three types of methods shown in FIGS. 20, 21, and 22.

  In the method shown in FIG. 20, the link 12 includes an encoder 311 for measuring an angle change between the link 12 and the link 13, and a motor 312 that can change the angle between the link 2 and the link 3. A controller 313 for controlling the motor 312 is installed. And while measuring the attitude | position relationship between links about each whole attitude | position, the output of the encoder 311 is continuously monitored. Then, in response to a command issued by the link fixing indicator 112 (see FIG. 11), the controller 313 issues a servo system position control command to the motor 312 so that the output of the encoder 311 is always constant. As a result, the angle between the link 12 and the link 13 can be made constant by position control.

  This method of fixing the angle between links by position control by the servo system has an advantage that no additional device is required when the encoder 311 and the motor 312 are installed in the joint from the beginning. In addition, this method does not require mechanical switching time because the posture relationship between the links is electrically fixed, and the link mechanism can be used immediately after the initialization process such as the posture relationship measurement between links. it can. For this reason, the efficiency of the initialization process can be improved.

  Alternatively, as shown in FIG. 21, there are provided a clutch 314 using friction for stopping the movement of the link 13 relative to the link 12 by extending from the link 12 and pressing against the link 13 and an actuator 315 for operating the clutch 314. It may be installed. While the posture relationship between the links is measured for each overall posture, the angular relationship of the link 13 with respect to the link 12 can be fixed by enabling the operation of the clutch 314 by a command issued by the link fixing indicator 112. The fixing method using the clutch 314 has an advantage that the link fixing mechanism is simplified without using the encoder 311 or the motor 312.

  Alternatively, as shown in FIG. 22, a rod + pin 316 may be installed between the link 12 and the link 13. The links are mechanically fixed by this rod + pin 316. This fixed method is the strongest compared to other methods, so even if the overall posture is changed, the posture relationship between links can be made the most accurate and constant. Further, the fixing method using the rod + pin 316 does not require an electric signal and is the easiest and most reliable method. Since the rod + pin 316 cannot be easily removed, the fixing method according to this method is suitable for applications in which the posture relationship between links is not continuously changed even after measurement of the link posture relationship.

The link matrix from the first coordinate system to the second coordinate system obtained using these fixing methods is T1, and the link matrix from the second coordinate system to the third coordinate system is T2. At this time, the link matrix T12 to the third coordinate system viewed from the first coordinate system is expressed by the following Expression 34.
T12 = T1 · T2 (Formula 34)

In this way, since the link matrix T12 is obtained, the joint angles at the joints 14 and 15 can be calculated by obtaining the inverse function of Expression 34. An example of how to find the inverse function is given in “Robot Manipulator Corona Translation, Tsuneo Yoshikawa”. Furthermore, once the initial values of each joint angle are obtained, the joint angle after an arbitrary time after the initialization process is completed can be calculated using the initial value of the joint angle and the change in the joint angle as follows: 35, represented by Equation 36. At this time, the change of each joint angle is obtained by a relative angle type encoder.
Angle of joint 14 = initial value of angle of joint 14 + change in angle of joint 14 (Expression 35)
Angle of joint 15 = initial value of angle of joint 15 + change in angle of joint 15 (Expression 36)

  According to the present embodiment, the posture relationship between the first coordinate system and the third coordinate system can be obtained easily and with high accuracy without greatly changing the posture of the link mechanism in the link mechanism initialization step. . Moreover, the initialization process of the link mechanism installed in each joint can be simply performed using the obtained posture relationship between coordinate systems.

  As described above in detail, according to the attitude angle determination device 20 of the present embodiment, the acceleration sensors 21 to 26 are provided in the respective axis directions of both coordinate systems for which the attitude relationship is to be obtained. Thus, the inclination direction of each axis can be calculated. Further, this measurement is performed 1 to 3 times while changing the posture, and the link matrix calculator 37 can calculate the link matrix T12. Therefore, the attitude angle between the two coordinate systems included in the link structure is obtained from the link matrix T12. In other words, the posture relationship can be easily and accurately obtained even between coordinate systems having a complicated relationship without using drawings or actual measurements.

  In addition, this form is only a mere illustration and does not limit this invention at all. Therefore, the present invention can naturally be improved and modified in various ways without departing from the gist thereof.

It is explanatory drawing which shows the example of the target member of the attitude | position angle determination apparatus of this form. It is explanatory drawing which shows the specific example of a target member. It is explanatory drawing which shows the specific example of a target member. It is explanatory drawing which shows the specific example of a target member. It is a block diagram which shows schematic structure of the attitude | position angle determination apparatus of this form. It is explanatory drawing which shows the specific example of a target member. It is explanatory drawing which shows the specific example of a target member. It is explanatory drawing which shows the link structure of an object member. It is explanatory drawing which shows the relationship between the Z-axis of a reference | standard coordinate system, and the axis | shaft of another coordinate system. It is a flowchart which shows the algorithm of the attitude | position angle determination method of this form. It is a block diagram which shows schematic structure of another example of an attitude angle determination apparatus. It is a block diagram which shows schematic structure of another example of an attitude angle determination apparatus. It is a block diagram which shows schematic structure of another example of an attitude angle determination apparatus. It is a block diagram which shows schematic structure of another example of an attitude angle determination apparatus. It is explanatory drawing which shows the link structure of an object member. It is a block diagram which shows schematic structure of another example of an attitude angle determination apparatus. It is a block diagram which shows schematic structure of another example of an attitude angle determination apparatus. It is a block diagram which shows schematic structure of another example of an attitude angle determination apparatus. It is explanatory drawing which shows the specific example of a link mechanism. It is explanatory drawing which shows the specific example of a link mechanism fixing device. It is explanatory drawing which shows the specific example of a link mechanism fixing device. It is explanatory drawing which shows the specific example of a link mechanism fixing device.

Explanation of symbols

DESCRIPTION OF SYMBOLS 10 Link mechanism 11, 12 Link 20 Attitude angle determination apparatus 21, 22, 23, 24, 25, 26 Acceleration sensor 28 Attitude change command device 33 Inclination vector calculator 34 Inclination vector calculator 36 Inclination matrix calculator 37 Link matrix calculator 39 Attitude angle calculation / display

Claims (21)

It is expressed as a link mechanism having a first link and a second link connected via one or more joints, and is seen from the coordinate system of the first link in the structure mounted on or connected to the posture change operation unit. In the attitude angle determination device that determines the attitude angle of the coordinate system of the second link,
An inclination angle acquisition unit that acquires an inclination angle that is an angle between the axial direction and the direction of the gravity vector for each axis of the three-axis orthogonal coordinate system set for each link;
A posture change instructing unit for instructing the posture changing operation unit to change the posture of the structure and taking a total of three total postures while fixing the posture relationship between the first link and the second link;
An inclination vector calculation unit for obtaining an inclination vector from an inclination angle in each axial direction acquired by the inclination angle acquisition unit for each link in each overall posture according to an instruction from the posture change instruction unit;
For each link, an inclination matrix calculation unit that obtains an inclination matrix from an inclination vector obtained for each overall posture by the inclination vector calculation unit;
A first link matrix calculation unit for calculating a link matrix representing an attitude angle by calculating a product of one and the other inverse matrix based on the inclination matrix of each link obtained by the inclination matrix calculation unit; A posture angle determination device characterized by that. The posture angle determination device according to claim 1, wherein the inclination angle acquisition unit calculates an inclination angle,
A posture angle determination device obtained by using a level provided in three directions orthogonal to each other on each of the first link and the second link. The posture angle determination device according to claim 1, wherein the inclination angle acquisition unit calculates an inclination angle,
An attitude angle determination device characterized in that it is obtained from output values of acceleration sensors provided in three directions orthogonal to each other on the first link and the second link. The posture angle determination device according to claim 1, wherein the inclination angle acquisition unit calculates an inclination angle,
Output values of acceleration sensors provided in two directions orthogonal to each other on the first link and the second link;
A posture angle determination device, characterized in that a sum of squares of output values of acceleration sensors for each link is obtained from a square root of a value obtained by subtracting from a square of a value of gravitational acceleration at the place. It is expressed as a link mechanism having a first link and a second link connected via one or more joints, and is seen from the coordinate system of the first link in the structure mounted on or connected to the posture change operation unit. In the attitude angle determination device that determines the attitude angle of the coordinate system of the second link,
An acceleration value acquisition unit that acquires acceleration values in directions other than the one direction of the three-axis orthogonal coordinate system set so that one direction is parallel to each link;
With the posture relationship between the first link and the second link fixed, the posture change operation unit is instructed to change the posture of the structure, and two general postures are obtained by rotation around an axis parallel to the one direction. A posture change instruction unit
An inclination angle measuring unit for obtaining an inclination angle from an acceleration value in each axial direction acquired by the acceleration value acquisition unit for each link in each overall posture according to an instruction from the posture change instruction unit;
An inclination vector calculation unit for obtaining an inclination vector from an inclination angle in each axial direction obtained by the inclination angle measurement unit for each link in each overall posture according to an instruction from the posture change instruction unit;
For each link, an inclination matrix calculation unit that obtains an inclination matrix from an inclination vector obtained for each overall posture by the inclination vector calculation unit;
A first link matrix calculation unit for calculating a link matrix representing an attitude angle by calculating a product of one and the other inverse matrix based on the inclination matrix of each link obtained by the inclination matrix calculation unit; A posture angle determination device characterized by that. Determines the posture angle of the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints In the attitude angle determination device,
An acceleration value acquisition unit for acquiring an acceleration value in any one direction other than the one direction of the three-axis orthogonal coordinate system set so that one direction is parallel and horizontal to each link;
For each link, an inclination angle measurement unit for obtaining an inclination angle from the acceleration value acquired by the acceleration value acquisition unit;
A posture angle determination device, comprising: a posture angle calculation unit that calculates a posture angle from a difference in inclination angle of each link obtained by the inclination angle measurement unit. Determines the posture angle of the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints In the attitude angle determination device,
An acceleration value acquisition unit provided at each link for acquiring an acceleration value at each link;
A first yaw angle acquisition unit for acquiring a yaw angle between coordinate systems set for each link;
And a second link matrix calculation unit that calculates a link matrix representing an attitude angle from the acceleration value acquired by the acceleration value acquisition unit and the yaw angle acquired by the first yaw angle acquisition unit. A posture angle determination device. Determines the posture angle of the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints In the attitude angle determination device,
An acceleration value acquisition unit provided at each link for acquiring an acceleration value at each link;
A second yaw angle acquisition unit that acquires a yaw angle between the coordinate system set therein and the reference coordinate system for each link;
For each link, a first attitude matrix calculation unit for obtaining an attitude matrix with respect to a reference coordinate system from the acceleration value acquired by the acceleration value acquisition unit and the yaw angle acquired by the second yaw angle acquisition unit;
A first link matrix calculation unit for calculating a link matrix representing an attitude angle by obtaining a product of one and the other inverse matrix based on the posture matrix of each link obtained by the first posture matrix calculation unit; A posture angle determination device characterized by comprising: Determines the posture angle of the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints In the attitude angle determination device,
An acceleration value acquisition unit provided at each link for acquiring an acceleration value at each link;
An angular velocity value acquisition unit provided at each link for acquiring an angular velocity value at each link;
For each link, a second posture matrix calculation unit for obtaining a posture matrix with respect to a reference coordinate system from the acceleration value acquired by the acceleration value acquisition unit and the angular velocity value acquired by the angular velocity value acquisition unit;
A first link matrix calculating unit for calculating a link matrix representing an attitude angle by calculating a product of one and the other inverse matrix based on the attitude matrix of each link obtained by the second attitude matrix calculating unit; A posture angle determination device characterized by comprising: Determines the posture angle of the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints In the attitude angle determination device,
An acceleration value acquisition unit provided at each link for acquiring an acceleration value at each link;
A magnetic sensor provided at each link;
For each link, a third posture matrix calculation unit for obtaining a posture matrix with respect to a reference coordinate system from the acceleration value acquired by the acceleration value acquisition unit and information on the intersection angle between the magnetic north direction acquired by the magnetic sensor; ,
A first link matrix calculation unit for calculating a link matrix representing an attitude angle by calculating a product of one and the other inverse matrix based on the attitude matrix of each link obtained by the third attitude matrix calculation unit; A posture angle determination device characterized by comprising: In the posture angle determination device according to claim 9,
Have a magnetic sensor on each link,
The second posture matrix calculation unit is information of an intersection angle between the acceleration value acquired by the acceleration value acquisition unit, the angular velocity value acquired by the angular velocity value acquisition unit, and the magnetic north direction acquired by the magnetic sensor. An attitude angle determination device characterized by obtaining an attitude matrix with respect to a reference coordinate system. In the posture angle determination device according to any one of claims 1 to 5, the posture change operation unit includes:
A posture angle determination device, which is a robot incorporating a posture change device and forms an overall posture by itself. It is expressed as a link mechanism having a first link and a second link connected via one or more joints, and is seen from the coordinate system of the first link in the structure mounted on or connected to the posture change operation unit. In the attitude angle determination method for determining the attitude angle of the coordinate system of the second link,
For each axis of the 3-axis Cartesian coordinate system set for each link, obtain the tilt angle that is the angle between the axial direction and the direction of the gravity vector,
With the posture relationship between the first link and the second link fixed, the posture changing operation unit is instructed to change the posture of the structure to take up to three overall postures.
For each link in each overall posture according to the instruction, a tilt vector is obtained from the acquired tilt angle in each axial direction,
For each link, the slope matrix is obtained from the slope vector found for each overall posture.
A posture angle determination method, wherein a link matrix representing a posture angle is calculated by calculating a product of one and the other inverse matrix based on the calculated inclination matrix of each link. The attitude angle determination method according to claim 13, wherein the inclination angle is:
Output values of acceleration sensors provided in two directions orthogonal to each other on the first link and the second link;
A posture angle determination method, characterized in that a sum of squares of output values of respective acceleration sensors for each link is obtained from a square root of a value obtained by subtracting from a square of a gravitational acceleration value at the location. It is expressed as a link mechanism having a first link and a second link connected via one or more joints, and is seen from the coordinate system of the first link in the structure mounted on or connected to the posture change operation unit. In the attitude angle determination method for determining the attitude angle of the coordinate system of the second link,
Acquire acceleration values in directions other than the one direction of the three-axis orthogonal coordinate system set so that one direction is parallel to each link;
With the posture relationship between the first link and the second link fixed, the posture change operation unit is instructed to change the posture of the structure, and two general postures are obtained by rotation around an axis parallel to the one direction. Let
For each link in each overall posture according to the instruction, the inclination angle is obtained from the acquired acceleration value in each axial direction,
For each link in each overall posture according to the instruction, the inclination vector is obtained from the obtained inclination angle in each axial direction.
For each link, the slope matrix is obtained from the slope vector obtained for each overall posture.
A posture angle determination method, wherein a link matrix representing a posture angle is calculated by calculating a product of one and the other inverse matrix based on the calculated inclination matrix of each link. Determines the posture angle of the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints In the attitude angle determination method,
Obtain an acceleration value in any one direction other than the one direction of the three-axis orthogonal coordinate system set so that one direction is parallel and horizontal to each link;
For each link, the inclination angle is obtained from the acquired acceleration value.
A posture angle determination method, characterized in that a posture angle is calculated from a difference in inclination angle of each link obtained. Determines the posture angle of the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints In the attitude angle determination method,
Get the acceleration value at each link,
Get the yaw angle between the coordinate systems set for each link,
A posture angle determination method, wherein a link matrix representing a posture angle is calculated from the acquired acceleration value and the acquired yaw angle. Determines the posture angle of the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints In the attitude angle determination method,
Get the acceleration value at each link,
For each link, get the yaw angle between the coordinate system set there and the reference coordinate system,
For each link, the attitude matrix with respect to the reference coordinate system is obtained from the acquired acceleration value and the acquired yaw angle.
A posture angle determination method, wherein a link matrix representing a posture angle is calculated by calculating a product of one and the other inverse matrix based on the calculated posture matrix of each link. Determines the posture angle of the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints In the attitude angle determination method,
Get the acceleration value at each link,
Obtain the angular velocity value at each link,
For each link, an attitude matrix with respect to the reference coordinate system is obtained from the acquired acceleration value and the acquired angular velocity value.
A posture angle determination method, wherein a link matrix representing a posture angle is calculated by calculating a product of one and the other inverse matrix based on the calculated posture matrix of each link. Determines the posture angle of the coordinate system of the second link as viewed from the coordinate system of the first link in a structure expressed as a link mechanism having a first link and a second link connected via one or more joints In the attitude angle determination method,
Get the acceleration value at each link,
Information on the crossing angle with the magnetic north direction at each link is acquired by a magnetic sensor.
For each link, the attitude matrix with respect to the reference coordinate system is obtained from the acquired acceleration value and the information of the intersection angle with the acquired magnetic north direction.
A posture angle determination method, wherein a link matrix representing a posture angle is calculated by calculating a product of one and the other inverse matrix based on the calculated posture matrix of each link. In the posture angle determination method according to any one of claims 13 to 15,
The posture change motion unit is a robot incorporating a posture change device;
An attitude angle determination method characterized by forming an overall attitude by itself.

Images