WO2008111100A1 - System for measuring an inertial quantity of a body and method thereon - Google Patents
System for measuring an inertial quantity of a body and method thereon Download PDFInfo
- Publication number
- WO2008111100A1 WO2008111100A1 PCT/IT2007/000189 IT2007000189W WO2008111100A1 WO 2008111100 A1 WO2008111100 A1 WO 2008111100A1 IT 2007000189 W IT2007000189 W IT 2007000189W WO 2008111100 A1 WO2008111100 A1 WO 2008111100A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- vector
- accelerometers
- module
- state
- observation
- Prior art date
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
- G01P7/00—Measuring speed by integrating acceleration
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
- G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
- G01P15/02—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of inertia forces using solid seismic masses
- G01P15/08—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of inertia forces using solid seismic masses with conversion into electric or magnetic values
- G01P15/0888—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of inertia forces using solid seismic masses with conversion into electric or magnetic values for indicating angular acceleration
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
- G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
- G01P15/18—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration in two or more dimensions
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
- G01P3/00—Measuring linear or angular speed; Measuring differences of linear or angular speeds
- G01P3/42—Devices characterised by the use of electric or magnetic means
- G01P3/44—Devices characterised by the use of electric or magnetic means for measuring angular speed
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/37—Measurements
- G05B2219/37388—Acceleration or deceleration, inertial measurement
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/45—Nc applications
- G05B2219/45083—Manipulators, robot
Definitions
- the present invention relates to a system for measuring an inertial quantity, for example the angular velocity, of a body.
- the linear acceleration, angular acceleration, and angular velocity representative of a body movement are defined the "inertial quantities" .
- IMUs Inertial Measurement Units
- IMUs Inertial Measurement Units
- the most widespread types of inertial measurement units are equipped with an orthogonal triad of accelerometers and an orthogonal triad of gyroscopes, so as to provide a user with a measurement of triaxial linear accelerations and a measurement of triaxial angular speeds, respectively. These measurements are carried out with reference to a coordinate system integral with the inertial measurement unit.
- IMUs inertial measurement units
- inertial measurement units which are able to provide for highly accurate measurements of inertial quantities, the cost associated thereto can become particularly high, preventing their applications in some areas of interest, such as industrial automation, robotics and production systems. Furthermore, the inertial measurement units currently on the market often have features (as regards their range and operative frequencies) which poorly match the abovementioned applications .
- a prior art inertial measurement unit is disclosed in the scientific article "MEMS SoC: Observer-based coplanar gyro-free inertial measurement unit," of Tsung- Lin Chen and Sungsu Park, Journal of Micromechanics and Microengineering, 15 (2005) 1664-1673, IOP, 2005.
- This article discloses a co-planar inertial measurement unit in which gyroscopes are not employed, in order to reduce the dimensions and to control the manufacturing costs .
- the inertial measurement unit of the article cited above employs monoaxial accelerometers and an Extended Kalman Filter (EKF) estimation algorithm in order to get inertial quantities (angular velocity and linear acceleration) measurements indicative of a body movement .
- EKF Extended Kalman Filter
- the inertial measurement unit described in the abovementioned article disadvantageously makes use of extra accelerometers, redundant as well as necessary for the improving of the measurements, which anyhow add to the design costs and the computing times of the same measurement unit.
- the monoaxial accelerometers act also as observers (inner, in this case) in order to provide the estimation algorithm with quantities representative of the body movement necessary for its implementation (self- referencing estimation algorithm) .
- the Applicant notes that the employment of the monoaxial accelerometers as inner observers makes the inertial measurement unit referred to in the aforesaid scientific article poorly accurate because of the uncertainties thereof.
- the object of the present invention is to provide a system for measuring an angular velocity of a body, which is adapted to reduce the drawbacks set forth above with reference to the prior art and which, compared to the latter, has higher reliability and precision as regards the measurements which can be implemented, while ensuring reduced costs and dimensions.
- the object of the present invention is also a method for measuring an angular velocity of a body as defined and characterized in claim 49.
- Fig. 1 schematically shows a measurement system according to an example of the invention
- Fig. 2 shows an example of an inertial measurement device which can be employed in the measurement system of Fig. 1 ;
- Fig. 3 shows an example of triaxial accelerometer which can be employed in the inertial measurement device of Fig. 2; - Fig. 4 illustrates a block diagram representative of an exemplary estimation module which can be employed in ' the measurement system according to the example of the invention;
- Fig. 5 shows two coordinate systems of the mathematical computational principle which can be employed in the measurement system according to the example of the invention.
- Such a body can be, for example, a portion of an object, such as the working head of a robotized device (for example a manipulator) or any mobile object (for example a vehicle, a missile, a spacecraft) .
- a measurement system is described, generally designated with the reference number 100, which is adapted to provide at least the angular velocity ⁇ a of a working head 1 of a manipulator 2.
- the measurement system 100 can be, advantageously, of a Gyro-Free type, i.e. it can be implemented without using any measurement sensor based on the gyroscopic effect.
- Such a manipulator 2 is provided with an articulated quadrilateral frame 3 mechanically connected to an actuating or functioning unit (generally designated with 4) .
- the articulated quadrilateral 3 includes a first driven shaft 5 and a second driven shaft 6, both mechanically connected to the actuating unit 4 so as to be hinged to be able to rotate about a common rotational axis.
- the first and second driven shafts 5 and 6 can be rotated by respective engines, independent one from the other, which are part of the actuating unit 4.
- the first driven shaft 5 is connected, through a first passive rotary joint Gl, to a first intermediate shaft 7, thereby to allow a mutual rotation of the two shafts.
- this first passive joint Gl comprises a pin and plain bearings.
- the second driven shaft 6 is connected to a second intermediate shaft 8 through a second passive rotary joint G2 made, for example, in exactly the same way as the first joint Gl.
- the driven movement of the articulated quadrilateral 3 allows for the working head 1 movement of the manipulator 2.
- manipulator 2 is only illustrative, and that the teachings of the present invention are applicable to other manipulator types.
- the measurement system 100 further comprises an inertial measurement device 10 preferably associated to the working head 1 of the manipulator 2.
- Such an inertial measurement device 10 comprises a plurality PA of axial inertial accelerometers (a triad in this example) and a microcontroller MIC mounted, for example, on a printed circuit board which, preferably, is attached to the manipulator working head 1. More details about the inertial measurement device will be provided herein below.
- the use of a printed circuit board reduces the inertial measurement device dimensions to a bidimensional plan, while promoting an easy application of the same board to a planar surface of the body without sensibly altering the shape and dimensions thereof.
- the measurement system 100 further comprises a first Sl, and a second observation sensor S2 (indicated by symbols in figure 1) , for example two resolvers, known per se, respectively mounted at first and second active joints (not shown in figure 1) also being part of the actuating unit 4.
- Each observation sensor (Sl or S2) is adapted to provide an electrical signal indicative of the measure of a preferably noninertial quantity representative of the respective active joint movement
- electrical signals representative of the working head 1 position and orientation (which can be both considered as noninertial quantities) are therefore achievable.
- the determination of electrical signals representative of the working head 1 position and orientation is carried out by the processing unit EE or by other programmable control unit which the actuating unit 4 of the industrial manipulator 2 can be provided with, for example a control unit UC (shown in figure 1) about which the following of the present specification will further discuss.
- the processing unit EE or the control unit UC is configured so as to implement a function adapted for defining the kinematics of the manipulator 2.
- observation sensors are, advantageously, gyro- free.
- these observation sensors Sl and S2 are, preferably, of a noninertial type, as they are adapted to provide electrical signals representative of noninertial quantities, such as the body 1 position and orientation .
- the resolvers are fixed relative to the respective joints of the manipulator and, thereby, during movement of the same joints, they are able to provide electrical signals representative of the quantity under observation.
- Observation sensors in this implementation fall within an observation sensors class designed as ⁇ outside-in" class .
- the measurement system 100 may include other types of observation sensors, provided they are of a gyro-free and noninertial type.
- linear or angular encoders which can be mounted to the joints or in general on other handling means of the manipulator (shafts of the articulated quadrilateral) , but however separated and distinct from the working head 1, can be employed. This kind of observation sensors falls within the "outside-in" class.
- the videocamera separated from the manipulator can be considered as belonging to the observation sensors class of the "outside-in" kind.
- observation sensors can be implemented in a videocamera or a receiver of the GPS type directly mounted on the working head 1.
- GPS videocamera or receiver integral with the working head 1 belong to a further class of observation sensors, alternative to the "outside-in” one, denoted as the "inside-out” class.
- the measurement system 100 may further include other support sensor, of a noninertial type, such as for example temperature sensors which the inertial measurement device 10 can be provided with, and which can be advantageously employed for the measurement system 100 calibration.
- the measurement system 100 further includes a processing unit EE, for example an electronic processor, operatively connected to the manipulator 2, which is provided with a microprocessor MICR and a memory M, which are operatively connected one to the others, adapted for loading and run respective program codes to generate a piece of information representative of the working head 1 motion (for example relative to the angular velocity ⁇ ⁇ )as a function of the electrical signals which can be respectively provided by the inertial measurement device 10 and by the first Sl and the second S2 observation sensors.
- a processing unit EE for example an electronic processor, operatively connected to the manipulator 2, which is provided with a microprocessor MICR and a memory M, which are operatively connected one to the others, adapted for loading and run respective program codes to generate a piece of information representative of the working head 1 motion (for example relative to the angular velocity ⁇ ⁇ )as a function of the electrical signals which can be respectively provided by the inertial measurement device 10 and by the first Sl and the second S2
- the processing unit EE is operatively connected to the inertial measurement device 10, preferably, by means of a conventional serial communication standard of the USB type (by the acronym Universal Serial Bus) or other serial communication standards known per se, such as, for example, standards RS-232, RS-285, Ethernet, or CAN (Controller Area Network) .
- a conventional serial communication standard of the USB type by the acronym Universal Serial Bus
- other serial communication standards known per se such as, for example, standards RS-232, RS-285, Ethernet, or CAN (Controller Area Network) .
- the program codes loadable and runnable, respectively, by memory M and microprocessor MICR of the processing unit EE comprise an estimation module 200 (Fig. 4) of the quantity representative of the manipulator working head 1 angular velocity ⁇ a as a function of, respectively, the same working head linear acceleration and the position and orientation achieved starting from measurements carried out by each of the observation sensors Sl and S2 on the respective active joints.
- Fig. 2 a particular embodiment of the inertial measurement device 10 according to the example described is schematically illustrated.
- the plurality of axial accelerometers PA comprises a first Al, a second A2, and a third A3 axial inertial accelerometers that are arranged relative to each other so as to geometrically define an equilateral triangle (indicated by a sectioning in figure 2) , in which each of the vertexes is represented by one of the abovementioned accelerometers .
- each of the accelerometers in the plurality corresponds to one of the vertexes of a plane figure, regular and symmetrical around its barycentre.
- regular and symmetrical plane figures can be a square, a pentagon, a hexagon.
- an inertial monoaxial accelerometer (suitable to provide linear acceleration values along an individual axis)
- an inertial biaxial accelerometer (suitable to provide acceleration values along a pair of axes lying in the same plane)
- an inertial triaxial accelerometer (suitable to provide acceleration value along an orthogonal triad of axes)
- axial inertial accelerometer is designed by the general term "axial inertial accelerometer” .
- the first axial inertial acceler ⁇ meter Al shown in figure 3 is, preferably, a triaxial accelerometer of the MEMS monolithic type.
- the contraction MEMS corresponds to the acronym Micro Electro-Mechanical Systems conventionally selected in order to indicate the manufacturing and operative technology, well-established in the field.
- this type of accelerometers also is, suitably, of the gyro- free type .
- the choice for the use of accelerometers of the MEMS type mainly falls within the reliability, performance quality, availability on the market and the reduced cost that this type of integrated devices nowadays ensures.
- a MEMS-type accelerometer commercially available is the model LIS3L06AL by STMicroelectronics . Furthermore, the fact that each accelerometer is gyro-free highly reduces its dimensions, since, from a dimensional point of view, they mainly develop on a bidimensional plane and not along a third dimension.
- a triaxial accelerometer of the MEMS type is applicable to a general body and presents a micro-mechanical structure provided with an electrical circuit comprising a tilting member (for example a sphere) adapted to move correspondingly to the body movement, thus modifying the circuit electrical parameters.
- the triaxial accelerometer through the abovementioned electrical circuit, is capable of providing analog electrical signals indicative of the measurement of linear accelerations which the moving body is subjected to.
- a three-coordinate X, Y, Z reference system is definable thereupon, relative to which the first accelerometer Al is able to provide, respectively, an analog electrical signal representative of a linear acceleration a x measurable along the x axis, an analog electrical signal representative of a linear acceleration a y measurable along the y axis and an analog electrical signal representative of a linear acceleration a z measurable along the z axis, during the tilting member movement provided within the accelerometer.
- the second A2, and the third A3 triaxial accelerometers are also capable of providing analog electrical signals representative of respective linear accelerations a x , a y , and a z values triads measurable relative to correspondent reference systems specially defined on each of the accelerometers .
- the first Al, second A2, and third A3 triaxial accelerometers are able to measure a linear acceleration A vector having nine components a ⁇ -a 9 which can be designated as: ai, a 2 , a 3 : components a x , a y , and a z measured by the first triaxial accelerometer Al, respectively; a 4 , a 5 , a 6 : components a x , a y , and a z measured by the second triaxial accelerometer A2, respectively; a 7 , a 8 , a 9 : components a x/ a y , and a 2 measured by the third triaxial accelerometer A3, respectively.
- the accelerometers Al-A3 are therefore such as to provide a measurement of the linear acceleration A vector of the manipulator 2 working head 1 which can be represented by the following relationship (1) :
- the electrical signals provided by the plurality of triaxial accelerometers PA are, typically, of the analog type.
- the microcontroller MIC is preferably equipped with an analog-to-digital converter module (for example a typical sampling network, not shown in the figures) which, starting from said analog electrical signals, provides a plurality of discrete values representative of the components ai-ag of the linear acceleration A vector so that it is receivable and computable by the microcontroller and the processing unit EE.
- an analog-to-digital converter module for example a typical sampling network, not shown in the figures
- a processing module of the electrical signals can be also directly included within each of the accelerometers Al, A2, A3.
- the accelerometers plurality PA and the microcontroller MIC of the inertial measurement device 10 instead of being discrete components susceptible to be mounted on a printed circuit board (as described above) , can be fabricated inside a single substrate of a respective chip which can be secured to the working head 1.
- a chip of the type described above is a so-called integrated circuit specifically created for a specific application (Application Specific Integrated Circuit, ASIC) .
- the angular acceleration ⁇ a of the manipulator working head 1 is expressed in terms of a first linear component, represented by the matrix-vector product between a submatrix J + (i3,i:9) and the linear acceleration A vector measurable by the accelerometers plurality PA, and by a second nonlinear component.
- the abovementioned submatrix comprises the matrix J + first three rows and nine columns (set forth below) obtained starting from the position of the first Al, second A2, and third A3 triaxial accelerometers relative to a mobile reference system in which the origin is, in the described example, the equilateral triangle barycentre B (Fig. 2) .
- the mathematical relationship 2 and other mathematical relationships which follow are to be understood as correct if the so-called sensitive axes of the first Al, second A2, and third A3 triaxial accelerometer are parallel to each other.
- sensitive axis of a triaxial accelerometer is meant the spatial direction along which the acceleration is projected, to which the triaxial accelerometer is subjected
- the matrix J + only depends on the selections made by the manufacturer during the inertial measurement device 10 production and, in particular, on the geometrical distribution of the plurality of triaxial accelerometers
- the second nonlinear component it is expressed in terms of a vector having a first term equal to - ⁇ y ⁇ z , a second term equal to ⁇ x ⁇ z and a third term with null (0) value.
- the processing unit EE is preferably arranged so as to carry out an integration operation of the angular acceleration ⁇ a in order to calculate the angular velocity ⁇ a , but the presence of the second nonlinear component makes said integration operation more complex. Therefore, it follows that, in order to highly simplify the task of the processing unit EE, the second nonlinear component should be made minimum or ideally null . To the purpose, it can be demonstrated that the second nonlinear component depends on the geometrical distribution of the plurality of triaxial accelerometers PA which may advantageously allow reducing or even nullify the value thereof. With reference to the described example, the equilateral triangle distribution advantageously allows having the second nonlinear component in which the first and second terms are different from 0, while the latter is equal to 0.
- the preferred choice of the equilateral triangle distribution is one among those which ensure the right balancing between simplification of the mathematical relationship (2) and the planar arrangement of the plurality of accelerometers PA.
- the measurement system in accordance with the invention is arranged so as to optimize the nonlinear component of the angular acceleration vector.
- This relationship (4) directly depends by the selected geometrical configuration (equilateral triangle distribution) and expresses the linear acceleration a B to which the origin of the Local reference coordinate system is subjected, in the example described herein the barycentre B of the equilateral triangle formed by the first Al, second A2, and third A3 triaxial accelerometer, as a function of the components a ⁇ -a 9 of the linear acceleration vector A measured by the plurality of accelerometers PA.
- the linear acceleration vector a B relative to the reference system has a first component a Bx , valuated along the X axis, as the average of the components SL 1 , a 4 , a 7 provided by the accelerometers along the same X axis; a second component a By , valuated relative to the y axis, as the average of the components a 2 , a 5 , a 8 provided by the accelerometers relative to the y axis; a third component a Bz , valuated relative to the z axis of the reference system, as the average among the components a 3 , a 6 , a 9 provided by the accelerometers relative to the z axis .
- the processing unit EE is arranged so as to calculate the linear acceleration vector a B .
- this task can be carried out directly by the microprocessor MIC which the inertial measurement device 10 is provided with.
- the Applicant in order to prevent the computational drawbacks mentioned above, proposes the use of an estimation module loadable and runnable by the processing unit EE to get an estimated quantity representative of the angular velocity ⁇ a .
- the estimation module allows solving a dynamic system of the type represented by the mathematical relationship (2) , in which a differential equation is expressed in terms of angular velocity ⁇ a , making reference to a statistical characterization of the state of the moving body (the manipulator head, in the example) starting from a respective starting condition.
- the employed estimation algorithm (which can also be designated as Sensor-Fusion) is, preferably, the Square-Root Central-Difference Kalman Filter (SR-CDFK) , known per se in the literature.
- SR-CDFK Square-Root Central-Difference Kalman Filter
- Such an algorithm is of the Bayesian type, since it is based on a probabilistic approach of the Bayesian type and therefore on the Bayes' theorem, known in the literature.
- Such an algorithm belongs to the family of SPKF filters or estimators (by the acronym, Sigma-Point Kalman Filter) , also known to those skilled in the art.
- the preferred selection for this family of estimators among all the families of estimators known in the literature is justifiable by the better computational efficacy and the presence of an individual parameter inherent to the algorithm.
- SR-CDFK estimation algorithm As an alternative to the SR-CDFK estimation algorithm, other estimation algorithms which belong to the SPKF estimators family may be employed such as, for example, the Unscented Kalman Filter (UKF) , Central- Difference Kalman Filter (CDFK) , Square-Root Unscented Kalman Filter (SR-UKF) algorithms.
- UPF Unscented Kalman Filter
- CDFK Central- Difference Kalman Filter
- SR-UKF Square-Root Unscented Kalman Filter
- estimation module suggests the definition of a respective estimation model .
- an exemplary chart of an estimation model (indicated in general by the reference number 200) applicable to the particular embodiment of the measurement system hitherto described
- the state vector of the manipulator head 1 in a current discrete moment k is designated with x k .
- the state vector x k is representable, preferably, as a concatenation, in the same current moment k, of a plurality of physical quantities representative of the working head 1 movement, among which: a position vector Pk, a velocity vector v k , an orientation vector e k , and an angular velocity vector ⁇ ak .
- the state vector x ⁇ may also comprise first bi and second b 2 bias vectors representative of optional biases, for example due to the temperature or other noises, to which the measurements carried out by the observation sensors employed in the embodiment of the invention are subjected. These biases are considered in the state vector since, being entirely unpredictable, it is preferred to get a real-time estimation.
- state vector x k presents more components, each of which is implemented by a three-element vector, excepted for the orientation
- vector e k which, being a quaternion, comprises four components .
- the estimation model 200 comprises an evolution state module 201 adapted to receive inputting state vector X k , a linear acceleration vector u k/ and a noise0 vector v k in the current moment k.
- the linear acceleration vector u k is composed by a plurality of discrete values a lk , a 2k , ..., a 9k , representative of, said components SL 1 -SL 9 of the linear acceleration vector A. 5 It should be noted that each component of the linear acceleration vector u k is achievable, for example, through the microcontroller MIC of the inertial measurement device 10, which comprises, as previously stated, an analog-to-digital converter module which,0 starting from said electrical signals provided by the accelerometers plurality PA and representative of the linear acceleration vector A components SL 1 -SL 9 , is adapted to provide respective digital electrical signals representative of the discrete components a ik , a 2k , ..., a 9k .
- Fig. 4 and accounts for outer noise factors, for example electrical noise, to which the linear acceleration vector (u k ) is susceptible in the current discrete moment k .
- the state evolution module 201 starting from the data belonging to the state vector of the body x k in the current moment k, linear acceleration vector u k and outer noise factors vector v k , respectively, is suitable to output a state evolution vector of the body x k+ i in a subsequent moment k+1.
- the evolution module 201 is arranged to implement mathematical relationships of a function f representative of the state evolution.
- Each of the mathematical relationships expresses each of the state vector components in the subsequent moment k+1 as a function of the respective components of the state vector in the current moment K.
- function f and respective mathematical relationships which can be implemented by the evolution module 201, is the following:
- ⁇ ak relationship of the discrete angular acceleration which can be valuated by the processing unit EE, for example, to be employed in the compensation relationship of the leverage effect set forth above; I identity matrix 4 x 4;
- the estimation module 200 further comprises a unit delay module ⁇ positioned cascading relative to the evolution module 201 and adapted to receive the inputting state evolution vector X k+ i and to output a delayed state evolution vector x ⁇ k .
- the reason for this unit delay module ⁇ will be described in herein below.
- the estimation module 200 includes a third observation module 202 positioned cascading relative to the abovementioned delay module.
- the observation module 202 is adapted to receive the inputting delayed state vector x ⁇ k and to output an estimated observation vector y ⁇ representative of an estimated observation quantity.
- the observation module 202 is implemented, for example, in a selection function g of the following kind:
- the selection function g is a matrix of 0 and 1 adapted for selecting the position component and the orientation component of the delayed state vector x " k in order to yield the estimated observation vector y k .
- the estimated observation vector y k is a function of the state vector X k in the current discrete moment, and not of the state evolution vector x k+1 in the subsequent moment or sample.
- the unit delay module simply brings the state evolution vector xj t+1 back to the preceding discrete sample (k) so that, for the subsequent processing, both the state evolution vector and the observation vector refer to the same current discrete moment k.
- the estimation module 200 further comprises a comparison module CNF between the estimated observation vector y k and a measured observation vector y * k representative of a measured observation quantity associable to the working head 1 movement.
- the measured observation vector y ⁇ comprises the observation quantities measurable by the first Sl and second S2 observation sensors of the manipulator 2.
- the measured observation vector y * k comprises, for example, body position p k and orientation e ⁇ , as indicated by the following expression:
- the comparison module CNF is suitable to compare the estimated observation vector y k to the measured observation vector y ⁇ in order to generate an innovation vector z k representative of an innovation quantity.
- the estimation module 200 further comprises an sub-estimation module 203 suitable to receive said innovation vector z k and said delayed state vector x ⁇ k and to output an estimated body state vector x k including a quantity representative of the angular velocity of the working head 1.
- the sub-estimation module 203 comprises, preferably, a filter having gain factor KG k and an adder adapted to carry out the summation between the filtered innovation vector zk (amplified by factor KG k ) and the delayed state vector x " k . Therefore, the estimated body state vector x k can be expressed by the following mathematical relationship (7) :
- the filter KG k gain factor is a matrix which can be calculated starting from Cholesky decompositions, known in the literature, of covariance matrices S v and S w respectively associated to first noise factors v which insist on the state and second noise factors w which assure on the observation, beside on the state S x covariance matrix.
- the elements correlated to the first noise factors v on the acceleration and angular velocity measurements can be determined starting from the noise statistics for the triaxial accelerometers, by the study of the Allan analysis (known in the literature) , or by datasheets provided by the manufacturers.
- the elements correlated to the second noise factors w can instead be determined by using the noise statistics for the resolvers which the manipulator joints are provided with, suitably propagated through the direct cinematic transformation function.
- the sub-estimation module 203 is adapted to provide the estimated body- state vector x k , previously generated, inputting to the evolution module 201 as a subsequent body state vector
- the processing unit EE is adapted to output the estimated body state vector x k which is implemented in the piece of information I representative of the angular velocity ⁇ a of the working head 1 of the manipulator.
- the piece of information I can be provided to an information reader (not shown in the figure) to allow an operator to learn of the angular velocity which the industrial manipulator 1 working head is subjected to, in order to be able to override, for example, the actuating unit 4 of the manipulator 2 to optionally set a particular angular velocity value different from the one provided by the processing unit EE.
- the piece of information I can be provided, by the processing unit EE, automatically to the control unit UC which the industrial manipulator 2 actuating unit 4 is equipped with.
- the control unit UC is adapted, for example, to compare the angular velocity ⁇ a value represented in the piece of information I to an angular velocity value, preset and adapted to allow a correct functioning of the working head 1.
- the control unit UC is arranged to modify the operation of the actuating unit 4, in
- This second aspect is quite advantageous, since by providing the piece of information I directly to the control unit UC of the actuating unit 2, an automatic-type control can be carried out, without a manually overriding operator, of the actuating unit 4. In case of automatic control, the piece of information I can be provided to the information reader and, after that, to the operator only for his/her information.
- the plurality of accelerometers PA can comprise first and second biaxial accelerometers coplanar one to the other, and geometrically arranged on opposite vertexes of a rectangle or a square, so as to provide linear accelerations values measurable relative to mutually parallel axes .
- this solution allows computing the linear accelerations in the x-y plane and the angular velocity outside the plane along the z axis .
- This geometrical distribution and the (biaxial) type of the accelerometer allows for further simplifications to the previously described mathematical model, with reference to the equilateral triangle configuration.
- the arithmetic average of the accelerations yields the accelerations in the square barycentre, while the subsequent linear composition gives the angular velocity derivative, with the advantageous absence of nonlinear terms.
- the two biaxial-type accelerometers solution can be employed for the angular velocity measurement of a rotary joint or a motor, or it can be employed in the vehicle stability sensors (for example, in Electronic Program Stability, ESP) , in which the major need is felt for angular acceleration measurement in a direction orthogonal to the ground plane, and coplanar linear accelerations.
- ESP Electronic Program Stability
- the actuating unit 4 is operated by an operator, thus having the industrial manipulator 2 working head 1 to move .
- the inertial measurement device 10 Following the working head 1 movement, the inertial measurement device 10 generates, through the triaxial accelerometers plurality PA, the electrical signals representative of the measured linear acceleration vector A. These electrical signals are transmitted to the processing unit EE. Contemporaneously to the reception of the electrical signals generated by the inertial measurement device 10, the processing unit 10 receives further electrical signals generated by the first Sl and second S2 outer observation sensors (resolvers) , representative of the outer quantities correspondent to the movement, respectively, of the first and the second active joints which are present inside the industrial manipulator 2 actuating unit . The processing unit EE further receives electrical signals representative of further noise factors to which the working head is subjected, for example noise .
- the processing unit EE thus implements an estimation algorithm like the one described above with reference to the estimation model example of Fig. 4 and generates, based on the electrical signals received, the piece of information I representative of the estimated angular velocity ⁇ a value. This piece of information is transmitted both to the information reader and the control unit UC with which the manipulator 2 actuating unit 4 is provided.
- the control unit UC compares the estimated angular velocity ⁇ a value to a preset value. In case where the estimated value turns out to be different from the preset value, the control unit automatically acts upon the handling unit 4 to re ⁇ establish an angular velocity ⁇ a value of the working head 1 equal to the preset value. Contemporaneously, the information reader provides the operator with the estimated value.
- the object of the invention is fully achieved, since the proposed solution, in the case of industrial automation applications, robotics and tool machines, ensures reduced costs and dimensions, since it does not make use of gyroscopes, which necessarily have a three-dimensional structure.
- the measurement system in accordance with the present invention comprises gyro-free observation sensors which are different from the triaxial accelerometers, thus making the whole measurement system definable as non-self-referring, and thus suitable to ensure higher reliability and accuracy.
- the measurement system proposed herein comprises a plurality of inertial accelerometers which, advantageously, can be integrated on a electronic printed board (bidimensional plane) , which is therefore less bulky and easily applicable to an object
- the measurement system of the invention advantageously allows solving the possible presence of the so-called leverage effect.
- the problem associated with the presence of the leverage effect is, in general, derived by the arrangement of the inertial measurement device in a site far from the centre of gravity of the object or object portion the movement of which is desired to be measured or monitored. This distance, also called the “leverage”, invalidates the measurement of the acceleration, introducing noise terms linked to the angular velocity, angular acceleration and leverage size.
- the conventional prior art measurement systems measure a compensation correction to be adopted, but provide that the numerical derivation of the angular velocity is required to achieve the angular acceleration, with a subsequent decline in the information quality, as it is well known from the scientific literature about the signal numerical derivation. This procedure is so unsuitable that it is often preferred not to correct the error (lower performance) , and it is attempted to locate the sensor so as to reduce the leverage size.
- the measurement system in accordance with the invention is capable of measuring the angular acceleration implementing the relationship of ⁇ ak set forth above, and without carrying out a derivation operation, thus advantageously reducing the amplification of a possible noise on the signal, thus allowing for a more precise estimation of factors which compensate for the leverage effect and, accordingly, allowing for a higher freedom of choice on the sensor position and a higher flexibility of use.
- a first coordinate system L called Local
- L is considered fixed (relative to the ground plane) and is the reference system for the measurements of an object (machine or robot) or object portion.
- the origin of the Local reference system matches with the reference point of the manipulator or robot base.
- the second coordinate system B is considered mobile and its origin matches with the object working head or end portion, called Tool Center Point, TCP.
- TCP Tool Center Point
- the origin of the Body coordinate system is represented by the equilateral triangle barycentre B represented by the distribution of the plurality of accelerometers PA. It is the reference system for the quantities which a sensor can measure.
- the so-called relative acceleration theorem correlates the acceleration of a material point integral with the first Body coordinate system to the one which would be measured in the second Local coordinate system.
- C b l rotation matrix between Body and Local An accelerometer measures the specific force which acts upon the little mass contained therein, projected on its reference axis.
- an accelerometer which is located at s with the sensitive axis individuated by the versor ⁇ s will measure:
- the sensors are nine, thus the index s can take 1 to 9 values, and therefore the mathematical relationship (9) can be written nine times and, by arranging the nine achievable equations in matrix form, the following mathematical relationship (10) is achieved:
- the system can be solved making the vector composed by angular acceleration and linear acceleration explicit, as indicated by the following mathematical relationship (12) :
- J + pseudoinverse matrix of the matrix J
- An estimation algorithm allows solving a dynamic system, expressed in form of differential equations in terms of angular velocity, making use of information about starting conditions, outer observation of related dynamic quantities and the knowledge of the statistical characterization of the quantities involved.
- x indicates the state of the dynamic system, as a function of time t, of a known (or measurable) input variable u, and influenced by a noise v (having a known or estimable covariance)
- y represents the observation of the state , and is influenced by a noise w.
- Functions f and g are generally nonlinear.
- SR-CDKF Square-root Central-Difference Kalman Filter
- h ⁇ are linear compositions of the sampled accelerations and, within the sampling range, are assumed as constant.
- the analytic solution of the system ( 15) is formulated in terms of implicit integrals , but by developing in Taylor series about the origin to the first order, the following approximated solution is achieved :
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Gyroscopes (AREA)
Abstract
System (100) for measuring an angular velocity (ωa) of a body (1) , comprising: a plurality (PA) of axial inert ial accelerometers associable to said body (1) for the measurement of linear accelerations of the body (1); at least one gyro- free observation sensor (Sl, S2) to measure a respective observation quantity associable to the body (1) movement; an estimation module of at least one quantity representative of the body angular velocity as a function of the linear accelerations and the quantity associable to the body movement.
Description
SYSTEM FOR MEASURING AN INERTIAL QUANTITY OF A BODY AND
METHOD THEREON
The present invention relates to a system for measuring an inertial quantity, for example the angular velocity, of a body.
For the purposes of the present specification, the linear acceleration, angular acceleration, and angular velocity representative of a body movement are defined the "inertial quantities" .
It is known that systems for measuring an inertial quantity are also designated as Inertial Measurement Units and are known in the literature by the acronym IMUs. Typically, the most widespread types of inertial measurement units are equipped with an orthogonal triad of accelerometers and an orthogonal triad of gyroscopes, so as to provide a user with a measurement of triaxial linear accelerations and a measurement of triaxial angular speeds, respectively. These measurements are carried out with reference to a coordinate system integral with the inertial measurement unit.
The most widespread applications of the inertial measurement units, IMUs, are found in the air or land navigation systems field, both for civilian applications
and for military applications.
Although there are inertial measurement units which are able to provide for highly accurate measurements of inertial quantities, the cost associated thereto can become particularly high, preventing their applications in some areas of interest, such as industrial automation, robotics and production systems. Furthermore, the inertial measurement units currently on the market often have features (as regards their range and operative frequencies) which poorly match the abovementioned applications .
Among the reasons why the cost of accurate inertial measurement units is so high, this is mainly due to a restricted, high-level market (air, see or land navigation, for civilian or military uses) , high reliability standard required, moreover in the aforementioned fields; to the integration of advanced computing and data transmission functions on the sensor; high cost (and dimensions) of precision, typically optical fibre, gyroscopes.
Therefore, in order to break through the markets of interest, the need is felt of introducing innovative solutions which avoid using expensive, bulky sensors, and which replace them with other types of sensors, more cost-effective and compact, which are selected as a
function of the particular application.
A prior art inertial measurement unit is disclosed in the scientific article "MEMS SoC: Observer-based coplanar gyro-free inertial measurement unit," of Tsung- Lin Chen and Sungsu Park, Journal of Micromechanics and Microengineering, 15 (2005) 1664-1673, IOP, 2005. This article discloses a co-planar inertial measurement unit in which gyroscopes are not employed, in order to reduce the dimensions and to control the manufacturing costs . The inertial measurement unit of the article cited above employs monoaxial accelerometers and an Extended Kalman Filter (EKF) estimation algorithm in order to get inertial quantities (angular velocity and linear acceleration) measurements indicative of a body movement . The inertial measurement unit described in the abovementioned article disadvantageously makes use of extra accelerometers, redundant as well as necessary for the improving of the measurements, which anyhow add to the design costs and the computing times of the same measurement unit.
Furthermore, in that article it is specified that the monoaxial accelerometers act also as observers (inner, in this case) in order to provide the estimation algorithm with quantities representative of the body movement necessary for its implementation (self-
referencing estimation algorithm) . The Applicant notes that the employment of the monoaxial accelerometers as inner observers makes the inertial measurement unit referred to in the aforesaid scientific article poorly accurate because of the uncertainties thereof.
The object of the present invention is to provide a system for measuring an angular velocity of a body, which is adapted to reduce the drawbacks set forth above with reference to the prior art and which, compared to the latter, has higher reliability and precision as regards the measurements which can be implemented, while ensuring reduced costs and dimensions.
This object is achieved by a system for measuring an angular velocity of a body as defined and characterized in claim 1.
Preferred embodiments of said measurement system are defined by the annexed dependant claims 2-48.
The object of the present invention is also a method for measuring an angular velocity of a body as defined and characterized in claim 49.
Further features and advantages of the system according to the invention will be understood from the description set forth herein below of preferred exemplary embodiments, given as an illustrative and not limitative example, with reference to the annexed
figures, in which:
Fig. 1 schematically shows a measurement system according to an example of the invention;
Fig. 2 shows an example of an inertial measurement device which can be employed in the measurement system of Fig. 1 ;
Fig. 3 shows an example of triaxial accelerometer which can be employed in the inertial measurement device of Fig. 2; - Fig. 4 illustrates a block diagram representative of an exemplary estimation module which can be employed in ' the measurement system according to the example of the invention;
Fig. 5 shows two coordinate systems of the mathematical computational principle which can be employed in the measurement system according to the example of the invention.
An example of measurement system of an angular velocity of a body and, advantageously, also of other parameters representative of the body movement will be now described.
Such a body can be, for example, a portion of an object, such as the working head of a robotized device (for example a manipulator) or any mobile object (for example a vehicle, a missile, a spacecraft) .
With reference to the example in figure 1, a measurement system is described, generally designated with the reference number 100, which is adapted to provide at least the angular velocity ωa of a working head 1 of a manipulator 2. As it will be understood herein below, the measurement system 100 can be, advantageously, of a Gyro-Free type, i.e. it can be implemented without using any measurement sensor based on the gyroscopic effect. Such a manipulator 2 is provided with an articulated quadrilateral frame 3 mechanically connected to an actuating or functioning unit (generally designated with 4) .
The articulated quadrilateral 3 includes a first driven shaft 5 and a second driven shaft 6, both mechanically connected to the actuating unit 4 so as to be hinged to be able to rotate about a common rotational axis. The first and second driven shafts 5 and 6 can be rotated by respective engines, independent one from the other, which are part of the actuating unit 4.
The first driven shaft 5 is connected, through a first passive rotary joint Gl, to a first intermediate shaft 7, thereby to allow a mutual rotation of the two shafts. According to a particular example, this first passive joint Gl comprises a pin and plain bearings. The
second driven shaft 6 is connected to a second intermediate shaft 8 through a second passive rotary joint G2 made, for example, in exactly the same way as the first joint Gl. The driven movement of the articulated quadrilateral 3 allows for the working head 1 movement of the manipulator 2.
It should be noted that the manipulator 2 is only illustrative, and that the teachings of the present invention are applicable to other manipulator types.
The measurement system 100 further comprises an inertial measurement device 10 preferably associated to the working head 1 of the manipulator 2.
Such an inertial measurement device 10, as schematically shown in Fig. 1, comprises a plurality PA of axial inertial accelerometers (a triad in this example) and a microcontroller MIC mounted, for example, on a printed circuit board which, preferably, is attached to the manipulator working head 1. More details about the inertial measurement device will be provided herein below.
It is to be noted that, advantageously, the use of a printed circuit board reduces the inertial measurement device dimensions to a bidimensional plan, while promoting an easy application of the same board to a
planar surface of the body without sensibly altering the shape and dimensions thereof.
The measurement system 100 further comprises a first Sl, and a second observation sensor S2 (indicated by symbols in figure 1) , for example two resolvers, known per se, respectively mounted at first and second active joints (not shown in figure 1) also being part of the actuating unit 4. Each observation sensor (Sl or S2) is adapted to provide an electrical signal indicative of the measure of a preferably noninertial quantity representative of the respective active joint movement
(for example the elevations of each joint or the axis position which can be defined at each of them) . Starting from the electrical signals which can be provided by the first Sl and second S2 observation sensors, electrical signals representative of the working head 1 position and orientation (which can be both considered as noninertial quantities) are therefore achievable. The determination of electrical signals representative of the working head 1 position and orientation is carried out by the processing unit EE or by other programmable control unit which the actuating unit 4 of the industrial manipulator 2 can be provided with, for example a control unit UC (shown in figure 1) about which the following of the present specification will
further discuss. To this aim, the processing unit EE or the control unit UC is configured so as to implement a function adapted for defining the kinematics of the manipulator 2. It should be noted that the observation sensors (resolvers) described herein are, advantageously, gyro- free. Furthermore, these observation sensors Sl and S2 are, preferably, of a noninertial type, as they are adapted to provide electrical signals representative of noninertial quantities, such as the body 1 position and orientation .
Furthermore, it should be considered that the resolvers are fixed relative to the respective joints of the manipulator and, thereby, during movement of the same joints, they are able to provide electrical signals representative of the quantity under observation. Observation sensors in this implementation fall within an observation sensors class designed as ΛΛoutside-in" class . In addition, it should be noted that the measurement system 100, as an alternative to the ones cited in the described example (resolvers) , may include other types of observation sensors, provided they are of a gyro-free and noninertial type. For example, linear or angular encoders which can
be mounted to the joints or in general on other handling means of the manipulator (shafts of the articulated quadrilateral) , but however separated and distinct from the working head 1, can be employed. This kind of observation sensors falls within the "outside-in" class.
In another example, it is possible to locate a videocamera which is fixed and separated relative to the manipulator in order to measure one or more observation quantities. Again, in this case, the videocamera separated from the manipulator can be considered as belonging to the observation sensors class of the "outside-in" kind.
Still alternatively, the observation sensors can be implemented in a videocamera or a receiver of the GPS type directly mounted on the working head 1. GPS videocamera or receiver integral with the working head 1 belong to a further class of observation sensors, alternative to the "outside-in" one, denoted as the "inside-out" class. The measurement system 100 may further include other support sensor, of a noninertial type, such as for example temperature sensors which the inertial measurement device 10 can be provided with, and which can be advantageously employed for the measurement system 100 calibration.
Referring back to figure 1, the measurement system 100 further includes a processing unit EE, for example an electronic processor, operatively connected to the manipulator 2, which is provided with a microprocessor MICR and a memory M, which are operatively connected one to the others, adapted for loading and run respective program codes to generate a piece of information representative of the working head 1 motion (for example relative to the angular velocity ωΩ)as a function of the electrical signals which can be respectively provided by the inertial measurement device 10 and by the first Sl and the second S2 observation sensors.
Furthermore, the processing unit EE is operatively connected to the inertial measurement device 10, preferably, by means of a conventional serial communication standard of the USB type (by the acronym Universal Serial Bus) or other serial communication standards known per se, such as, for example, standards RS-232, RS-285, Ethernet, or CAN (Controller Area Network) .
From an operative point of view, the program codes loadable and runnable, respectively, by memory M and microprocessor MICR of the processing unit EE comprise an estimation module 200 (Fig. 4) of the quantity representative of the manipulator working head 1 angular
velocity ωa as a function of, respectively, the same working head linear acceleration and the position and orientation achieved starting from measurements carried out by each of the observation sensors Sl and S2 on the respective active joints.
In Fig. 2 a particular embodiment of the inertial measurement device 10 according to the example described is schematically illustrated.
As it can be noted, the plurality of axial accelerometers PA comprises a first Al, a second A2, and a third A3 axial inertial accelerometers that are arranged relative to each other so as to geometrically define an equilateral triangle (indicated by a sectioning in figure 2) , in which each of the vertexes is represented by one of the abovementioned accelerometers .
As will be described below, the geometric distribution of the plurality of accelerometers illustrated herein advantageously allows highly simplifying mathematical relationships useful to the object of the invention, the discussion thereof will be given below. Beside the equilateral triangle distribution, other geometric distributions can be employed, with the proviso that each of the accelerometers in the plurality corresponds to one of
the vertexes of a plane figure, regular and symmetrical around its barycentre. Examples of regular and symmetrical plane figures can be a square, a pentagon, a hexagon. Furthermore, it should be considered that among the possible geometric configurations, also those which develop in a third dimension (for example cube, regular pyramid) could be optionally employed, but this solution does not seem feasible, since the measurement device is intended to be built on a plane electronic board and, therefore, on a bidimensional plane. This advantageously allows reducing uncertainties in the orthogonal assembly of electronic boards .
For the purposes of the present specification, an inertial monoaxial accelerometer (suitable to provide linear acceleration values along an individual axis) , and an inertial biaxial accelerometer (suitable to provide acceleration values along a pair of axes lying in the same plane) , and an inertial triaxial accelerometer (suitable to provide acceleration value along an orthogonal triad of axes) , all known by one skilled in the art, are designed by the general term "axial inertial accelerometer" .
With reference to an individual accelerometer in the plurality PA, for example the first axial inertial
accelerόmeter Al shown in figure 3 , it should be noted that it is, preferably, a triaxial accelerometer of the MEMS monolithic type. The contraction MEMS corresponds to the acronym Micro Electro-Mechanical Systems conventionally selected in order to indicate the manufacturing and operative technology, well-established in the field. Furthermore, it should be noted that this type of accelerometers also is, suitably, of the gyro- free type . Advantageously, the choice for the use of accelerometers of the MEMS type mainly falls within the reliability, performance quality, availability on the market and the reduced cost that this type of integrated devices nowadays ensures. A MEMS-type accelerometer commercially available is the model LIS3L06AL by STMicroelectronics . Furthermore, the fact that each accelerometer is gyro-free highly reduces its dimensions, since, from a dimensional point of view, they mainly develop on a bidimensional plane and not along a third dimension.
Broadly stated, a triaxial accelerometer of the MEMS type is applicable to a general body and presents a micro-mechanical structure provided with an electrical circuit comprising a tilting member (for example a sphere) adapted to move correspondingly to the body
movement, thus modifying the circuit electrical parameters. On the basis of the movement of said tilting member, the triaxial accelerometer, through the abovementioned electrical circuit, is capable of providing analog electrical signals indicative of the measurement of linear accelerations which the moving body is subjected to.
In more detail, with reference to Fig. 3 and first triaxial accelerometer Al, a three-coordinate X, Y, Z reference system is definable thereupon, relative to which the first accelerometer Al is able to provide, respectively, an analog electrical signal representative of a linear acceleration ax measurable along the x axis, an analog electrical signal representative of a linear acceleration ay measurable along the y axis and an analog electrical signal representative of a linear acceleration az measurable along the z axis, during the tilting member movement provided within the accelerometer. In an entirely similar manner to the first accelerometer Al, the second A2, and the third A3 triaxial accelerometers are also capable of providing analog electrical signals representative of respective linear accelerations ax, ay, and az values triads measurable relative to correspondent reference systems
specially defined on each of the accelerometers .
In general, the first Al, second A2, and third A3 triaxial accelerometers are able to measure a linear acceleration A vector having nine components aχ-a9 which can be designated as: ai, a2, a3 : components ax, ay, and az measured by the first triaxial accelerometer Al, respectively; a4, a5, a6: components ax, ay, and az measured by the second triaxial accelerometer A2, respectively; a7, a8, a9: components ax/ ay, and a2 measured by the third triaxial accelerometer A3, respectively.
From what stated above, the accelerometers Al-A3 are therefore such as to provide a measurement of the linear acceleration A vector of the manipulator 2 working head 1 which can be represented by the following relationship (1) :
It is stated that the electrical signals provided by the plurality of triaxial accelerometers PA are,
typically, of the analog type. In this connection, the microcontroller MIC is preferably equipped with an analog-to-digital converter module (for example a typical sampling network, not shown in the figures) which, starting from said analog electrical signals, provides a plurality of discrete values representative of the components ai-ag of the linear acceleration A vector so that it is receivable and computable by the microcontroller and the processing unit EE. Furthermore, a processing module of the electrical signals can be also directly included within each of the accelerometers Al, A2, A3.
In an alternative embodiment of the invention, the accelerometers plurality PA and the microcontroller MIC of the inertial measurement device 10, instead of being discrete components susceptible to be mounted on a printed circuit board (as described above) , can be fabricated inside a single substrate of a respective chip which can be secured to the working head 1. A chip of the type described above is a so-called integrated circuit specifically created for a specific application (Application Specific Integrated Circuit, ASIC) .
For the purposes of the present specification, reference will be made also to a Local Coordinate System, fixed and definable at the ground plane, on
which the manipulator 2 lies, and a Body Coordinate System, mobile and definable on the working head 1.
From an analytical point of view, considering the geometrical distribution of the plurality of triaxial accelerometers PA according to the example described herein (equilateral triangle configuration) , the following mathematical relationship (2) can be written:
in which the angular acceleration ώa of the manipulator working head 1 is expressed in terms of a first linear component, represented by the matrix-vector product between a submatrix J+(i3,i:9) and the linear acceleration A vector measurable by the accelerometers plurality PA, and by a second nonlinear component.
In greater detail, as regards the first linear component, the abovementioned submatrix comprises the matrix J+ first three rows and nine columns (set forth below) obtained starting from the position of the first Al, second A2, and third A3 triaxial accelerometers relative to a mobile reference system in which the origin is, in the described example, the equilateral triangle barycentre B (Fig. 2) . It is to be observed, in
particular, that the mathematical relationship 2 and other mathematical relationships which follow are to be understood as correct if the so-called sensitive axes of the first Al, second A2, and third A3 triaxial accelerometer are parallel to each other. By sensitive axis of a triaxial accelerometer is meant the spatial direction along which the acceleration is projected, to which the triaxial accelerometer is subjected
Thus, it should be noted that, advantageously, the matrix J+ only depends on the selections made by the manufacturer during the inertial measurement device 10 production and, in particular, on the geometrical distribution of the plurality of triaxial accelerometers
PA.
As regards the second nonlinear component, it is expressed in terms of a vector having a first term equal to -ωyωz , a second term equal to ωxωz and a third term
with null (0) value.
The processing unit EE is preferably arranged so as to carry out an integration operation of the angular acceleration ώa in order to calculate the angular velocity ωa , but the presence of the second nonlinear component makes said integration operation more complex. Therefore, it follows that, in order to highly simplify the task of the processing unit EE, the second nonlinear component should be made minimum or ideally null . To the purpose, it can be demonstrated that the second nonlinear component depends on the geometrical distribution of the plurality of triaxial accelerometers PA which may advantageously allow reducing or even nullify the value thereof. With reference to the described example, the equilateral triangle distribution advantageously allows having the second nonlinear component in which the first and second terms are different from 0, while the latter is equal to 0. The preferred choice of the equilateral triangle distribution is one among those which ensure the right balancing between simplification of the mathematical relationship (2) and the planar arrangement of the plurality of accelerometers PA.
In general, it is stated that the measurement system in accordance with the invention is arranged so as to optimize the nonlinear component of the angular acceleration vector. Furthermore, again with reference to the described example, it is possible to write the following mathematical relationship (4) :
This relationship (4) directly depends by the selected geometrical configuration (equilateral triangle distribution) and expresses the linear acceleration aB to which the origin of the Local reference coordinate system is subjected, in the example described herein the barycentre B of the equilateral triangle formed by the first Al, second A2, and third A3 triaxial accelerometer, as a function of the components aχ-a9 of the linear acceleration vector A measured by the plurality of accelerometers PA.
In particular, the linear acceleration vector aB , relative to the reference system has a first component aBx , valuated along the X axis, as the average of the components SL1, a4, a7 provided by the accelerometers
along the same X axis; a second component aBy , valuated relative to the y axis, as the average of the components a2, a5, a8 provided by the accelerometers relative to the y axis; a third component aBz , valuated relative to the z axis of the reference system, as the average among the components a3, a6, a9 provided by the accelerometers relative to the z axis .
It should be noted that the mathematical relationships (2) and (4) define a dynamic system of equations associable to the measurement system according to the example of the invention described herein.
The processing unit EE is arranged so as to calculate the linear acceleration vector aB . Alternatively, this task can be carried out directly by the microprocessor MIC which the inertial measurement device 10 is provided with.
Referring back to the mathematical relationship (2) , it should be noted that the presence therein of the second non-null nonlinear component, consequence of the geometrical configuration selected for the distribution of the triaxial accelerometers Al-A3, makes the integration operation of the angular acceleration ώa to calculate the velocity ωa quite complex, even for the
processing unit EE. In particular, the abovementioned integration operation would be possible numerically, starting from a given starting condition, but any integration process implementable by the processing unit EE would tend to add noise and uncertainty until bringing the measured quantity to an adrift value, i.e. to a value very far from the expected one, therefore proving to be unacceptable and not utilizable afterwards . In order to obtain angular velocity ωa , as an alternative to the calculation of the angular acceleration ώa (relationship (4) ) and to the subsequent integration operation, the Applicant, in order to prevent the computational drawbacks mentioned above, proposes the use of an estimation module loadable and runnable by the processing unit EE to get an estimated quantity representative of the angular velocity ωa .
The estimation module allows solving a dynamic system of the type represented by the mathematical relationship (2) , in which a differential equation is expressed in terms of angular velocity ωa , making reference to a statistical characterization of the state of the moving body (the manipulator head, in the example) starting from a respective starting condition.
In the exemplary measurement system described herein, the employed estimation algorithm (which can also be designated as Sensor-Fusion) is, preferably, the Square-Root Central-Difference Kalman Filter (SR-CDFK) , known per se in the literature. Such an algorithm is of the Bayesian type, since it is based on a probabilistic approach of the Bayesian type and therefore on the Bayes' theorem, known in the literature. Such an algorithm belongs to the family of SPKF filters or estimators (by the acronym, Sigma-Point Kalman Filter) , also known to those skilled in the art. The preferred selection for this family of estimators among all the families of estimators known in the literature is justifiable by the better computational efficacy and the presence of an individual parameter inherent to the algorithm.
As an alternative to the SR-CDFK estimation algorithm, other estimation algorithms which belong to the SPKF estimators family may be employed such as, for example, the Unscented Kalman Filter (UKF) , Central- Difference Kalman Filter (CDFK) , Square-Root Unscented Kalman Filter (SR-UKF) algorithms.
Further general and theoretical considerations upon estimation algorithms will be described herein
below in an APPENDIX thereof.
The use of an estimation module suggests the definition of a respective estimation model .
With reference to figure 4, an exemplary chart of an estimation model (indicated in general by the reference number 200) applicable to the particular embodiment of the measurement system hitherto described
(Fig. 1 and 2) will be now described.
In the abovementioned estimation model 200, the state vector of the manipulator head 1 in a current discrete moment k is designated with xk.
The state vector xk is representable, preferably, as a concatenation, in the same current moment k, of a plurality of physical quantities representative of the working head 1 movement, among which: a position vector Pk, a velocity vector vk, an orientation vector ek, and an angular velocity vector ωak . In addition, the state vector xκ may also comprise first bi and second b2 bias vectors representative of optional biases, for example due to the temperature or other noises, to which the measurements carried out by the observation sensors employed in the embodiment of the invention are subjected. These biases are considered in the state vector since, being entirely unpredictable, it is
preferred to get a real-time estimation.
It should be noted that the state vector xk presents more components, each of which is implemented by a three-element vector, excepted for the orientation
5. vector ek which, being a quaternion, comprises four components .
The estimation model 200 comprises an evolution state module 201 adapted to receive inputting state vector Xk, a linear acceleration vector uk/ and a noise0 vector vk in the current moment k.
The linear acceleration vector uk is composed by a plurality of discrete values alk, a2k, ..., a9k, representative of, said components SL1-SL9 of the linear acceleration vector A. 5 It should be noted that each component of the linear acceleration vector uk is achievable, for example, through the microcontroller MIC of the inertial measurement device 10, which comprises, as previously stated, an analog-to-digital converter module which,0 starting from said electrical signals provided by the accelerometers plurality PA and representative of the linear acceleration vector A components SL1-SL9, is adapted to provide respective digital electrical signals representative of the discrete components aik, a2k, ...,
a9k.
The noise vector Vk is designated with symbols in
Fig. 4 and accounts for outer noise factors, for example electrical noise, to which the linear acceleration vector (uk) is susceptible in the current discrete moment k .
The state evolution module 201, starting from the data belonging to the state vector of the body xk in the current moment k, linear acceleration vector uk and outer noise factors vector vk, respectively, is suitable to output a state evolution vector of the body xk+i in a subsequent moment k+1.
In particular, in order to generate the abovementioned state evolution, the evolution module 201 is arranged to implement mathematical relationships of a function f representative of the state evolution. Each of the mathematical relationships expresses each of the state vector components in the subsequent moment k+1 as a function of the respective components of the state vector in the current moment K. An example of function f and respective mathematical relationships, which can be implemented by the evolution module 201, is the following:
ek+1 = [lcos(sk) -Ωsm(sk)/sk}:k
Rb l k Body to Local rotation matrix; αα/i. = ώafc x r0^. + ωak x (ωώ xrojfJ compensation relationship of the so-called leverage effect; roff distance between Tool Center Point (TCP) and body site in which the inertial measurement device is installed for any estimation of the leverage effect;
ώak
: relationship of the discrete angular acceleration which can be valuated by the processing unit EE, for example, to be employed in the compensation relationship of the leverage effect set forth above; I identity matrix 4 x 4;
R z,,mψ rotation matrix about the z axis of the Body
reference system by an angle ψ = J f ωz (t)dt = ωzOt + — 1 hzt 2 . o 2
The estimation module 200 further comprises a unit delay module Δ positioned cascading relative to the evolution module 201 and adapted to receive the inputting state evolution vector Xk+i and to output a delayed state evolution vector x~ k. The reason for this unit delay module Δ will be described in herein below. In addition, the estimation module 200 includes a third observation module 202 positioned cascading relative to the abovementioned delay module.
The observation module 202 is adapted to receive the inputting delayed state vector x~ k and to output an estimated observation vector y^ representative of an estimated observation quantity. The observation module 202 is implemented, for example, in a selection function g of the following kind:
f ^3*3 O3x3 O4x4 O3x3 O3xi O3x3 J
[O330 O3x3 IAxA O3x3 O3x3 O3J
in which :
Inxrn identity matrix n x m;
Oman null matrix n x m.
As it can be noted, the selection function g is a matrix of 0 and 1 adapted for selecting the position component and the orientation component of the delayed state vector x" k in order to yield the estimated observation vector yk.
Now, referring back to the unit delay module Δ, it should be noted that the estimated observation vector yk is a function of the state vector Xk in the current discrete moment, and not of the state evolution vector xk+1 in the subsequent moment or sample. The unit delay module simply brings the state evolution vector xjt+1 back to the preceding discrete sample (k) so that, for the subsequent processing, both the state evolution vector and the observation vector refer to the same current discrete moment k.
The estimation module 200 further comprises a comparison module CNF between the estimated observation vector yk and a measured observation vector y* k representative of a measured observation quantity associable to the working head 1 movement.
As regards the measured observation vector y\, it should be noted that each of its components respectively represents an observation quantity provided by an outer
observation sensor, in the discrete moment k. In the example described herein, the measured observation vector y* k comprises the observation quantities measurable by the first Sl and second S2 observation sensors of the manipulator 2. The measured observation vector y* k comprises, for example, body position pk and orientation e^, as indicated by the following expression:
Yk = (Pk , ek ) The comparison module CNF is suitable to compare the estimated observation vector yk to the measured observation vector y\ in order to generate an innovation vector zk representative of an innovation quantity. The estimation module 200 further comprises an sub-estimation module 203 suitable to receive said innovation vector zk and said delayed state vector x~ k and to output an estimated body state vector xk including a quantity representative of the angular velocity of the working head 1.
In particular, the sub-estimation module 203 comprises, preferably, a filter having gain factor KGk and an adder adapted to carry out the summation between the filtered innovation vector zk (amplified by factor
KGk) and the delayed state vector x" k. Therefore, the estimated body state vector xk can be expressed by the following mathematical relationship (7) :
Xk = x'k + KGkZk (7)
It should be noted that the filter KGk gain factor is a matrix which can be calculated starting from Cholesky decompositions, known in the literature, of covariance matrices Sv and Sw respectively associated to first noise factors v which insist on the state and second noise factors w which insist on the observation, beside on the state Sx covariance matrix.
Furthermore, it is to be noted that, for example, the elements correlated to the first noise factors v on the acceleration and angular velocity measurements can be determined starting from the noise statistics for the triaxial accelerometers, by the study of the Allan analysis (known in the literature) , or by datasheets provided by the manufacturers. The elements correlated to the second noise factors w can instead be determined by using the noise statistics for the resolvers which the manipulator joints are provided with, suitably propagated through the direct cinematic transformation
function. ,
Referring back to Fig. 5, the sub-estimation module 203 is adapted to provide the estimated body- state vector xk , previously generated, inputting to the evolution module 201 as a subsequent body state vector
Referring back to Fig. 1, the processing unit EE is adapted to output the estimated body state vector xk which is implemented in the piece of information I representative of the angular velocity ωa of the working head 1 of the manipulator.
In particular, the piece of information I can be provided to an information reader (not shown in the figure) to allow an operator to learn of the angular velocity which the industrial manipulator 1 working head is subjected to, in order to be able to override, for example, the actuating unit 4 of the manipulator 2 to optionally set a particular angular velocity value different from the one provided by the processing unit EE.
Alternatively, or in addition to the provision of said piece of information I and a respective reader, the piece of information I can be provided, by the processing unit EE, automatically to the control unit UC
which the industrial manipulator 2 actuating unit 4 is equipped with. The control unit UC is adapted, for example, to compare the angular velocity ωa value represented in the piece of information I to an angular velocity value, preset and adapted to allow a correct functioning of the working head 1. In the case where the angular velocity ωa value represented by the piece of information I turns out to be different from the preset angular velocity value, the control unit UC is arranged to modify the operation of the actuating unit 4, in
, order to bring an angular velocity value of the working head back to the preset value . This second aspect is quite advantageous, since by providing the piece of information I directly to the control unit UC of the actuating unit 2, an automatic-type control can be carried out, without a manually overriding operator, of the actuating unit 4. In case of automatic control, the piece of information I can be provided to the information reader and, after that, to the operator only for his/her information.
In an alternative embodiment of the invention (not shown in the figures) , the plurality of accelerometers PA can comprise first and second biaxial accelerometers coplanar one to the other, and geometrically arranged on
opposite vertexes of a rectangle or a square, so as to provide linear accelerations values measurable relative to mutually parallel axes .
With reference to an orthogonal axes triad, this solution allows computing the linear accelerations in the x-y plane and the angular velocity outside the plane along the z axis .
This geometrical distribution and the (biaxial) type of the accelerometer allows for further simplifications to the previously described mathematical model, with reference to the equilateral triangle configuration.
For example, the arithmetic average of the accelerations yields the accelerations in the square barycentre, while the subsequent linear composition gives the angular velocity derivative, with the advantageous absence of nonlinear terms.
In particular, it is stated that the mathematical relationships relative to this solution can be achieved applying the same mathematical model as described above with reference to the equilateral triangle configuration.
By comparing the two biaxial-type accelerometers solution with the three equilateral-triangle-like
accelerometers one, the former is even more cost- effective and less bulky, but yields less information, which anyhow are to be deemed sufficient in some applicative fields. For example, the two biaxial-type accelerometer solution can be employed for the angular velocity measurement of a rotary joint or a motor, or it can be employed in the vehicle stability sensors (for example, in Electronic Program Stability, ESP) , in which the major need is felt for angular acceleration measurement in a direction orthogonal to the ground plane, and coplanar linear accelerations.
An exemplary operation of the measurement system in accordance with the invention will be now described, and with particular reference to Fig. 1 and 4.
The actuating unit 4 is operated by an operator, thus having the industrial manipulator 2 working head 1 to move .
Following the working head 1 movement, the inertial measurement device 10 generates, through the triaxial accelerometers plurality PA, the electrical signals representative of the measured linear acceleration vector A. These electrical signals are transmitted to the processing unit EE.
Contemporaneously to the reception of the electrical signals generated by the inertial measurement device 10, the processing unit 10 receives further electrical signals generated by the first Sl and second S2 outer observation sensors (resolvers) , representative of the outer quantities correspondent to the movement, respectively, of the first and the second active joints which are present inside the industrial manipulator 2 actuating unit . The processing unit EE further receives electrical signals representative of further noise factors to which the working head is subjected, for example noise .
The processing unit EE thus implements an estimation algorithm like the one described above with reference to the estimation model example of Fig. 4 and generates, based on the electrical signals received, the piece of information I representative of the estimated angular velocity ωa value. This piece of information is transmitted both to the information reader and the control unit UC with which the manipulator 2 actuating unit 4 is provided.
The control unit UC, as described above, compares the estimated angular velocity ωa value to a preset value. In case where the estimated value turns out to be
different from the preset value, the control unit automatically acts upon the handling unit 4 to re¬ establish an angular velocity ωa value of the working head 1 equal to the preset value. Contemporaneously, the information reader provides the operator with the estimated value.
As it can be noted, the object of the invention is fully achieved, since the proposed solution, in the case of industrial automation applications, robotics and tool machines, ensures reduced costs and dimensions, since it does not make use of gyroscopes, which necessarily have a three-dimensional structure.
Furthermore, the measurement system in accordance with the present invention comprises gyro-free observation sensors which are different from the triaxial accelerometers, thus making the whole measurement system definable as non-self-referring, and thus suitable to ensure higher reliability and accuracy.
The measurement system proposed herein comprises a plurality of inertial accelerometers which, advantageously, can be integrated on a electronic printed board (bidimensional plane) , which is therefore less bulky and easily applicable to an object
(manipulator) surface, the movement of which is desired
to be checked by measuring, for example, the angular velocity ωa of the same object, or a portion (working head) thereof .
In addition, the measurement system of the invention advantageously allows solving the possible presence of the so-called leverage effect. The problem associated with the presence of the leverage effect is, in general, derived by the arrangement of the inertial measurement device in a site far from the centre of gravity of the object or object portion the movement of which is desired to be measured or monitored. This distance, also called the "leverage", invalidates the measurement of the acceleration, introducing noise terms linked to the angular velocity, angular acceleration and leverage size. The conventional prior art measurement systems measure a compensation correction to be adopted, but provide that the numerical derivation of the angular velocity is required to achieve the angular acceleration, with a subsequent decline in the information quality, as it is well known from the scientific literature about the signal numerical derivation. This procedure is so unsuitable that it is often preferred not to correct the error (lower performance) , and it is attempted to locate the sensor
so as to reduce the leverage size.
The measurement system in accordance with the invention, unlike the prior art, is capable of measuring the angular acceleration implementing the relationship of ώak set forth above, and without carrying out a derivation operation, thus advantageously reducing the amplification of a possible noise on the signal, thus allowing for a more precise estimation of factors which compensate for the leverage effect and, accordingly, allowing for a higher freedom of choice on the sensor position and a higher flexibility of use.
To the embodiments of the above described device, one skilled in the art, in order to meet contingent needs, will be able to make modifications, adaptations and substitutions of elements with other functionally equivalent ones, without departing from the scope of the following claims. Each of the features, described as belonging to a possible embodiment, can be carried out independently from the other embodiments described.
APPENDIX
A) Operation Mathematical Principle: theoretical argumentation
In order to understand the operation principle of the proposed sensor, it is possible to start from equations of the relative accelerations between two coordinate systems, with reference to Fig. 5, a first coordinate system L, called Local, is considered fixed (relative to the ground plane) and is the reference system for the measurements of an object (machine or robot) or object portion. Typically, the origin of the Local reference system matches with the reference point of the manipulator or robot base. The second coordinate system B is considered mobile and its origin matches with the object working head or end portion, called Tool Center Point, TCP. In the described example, the origin of the Body coordinate system is represented by the equilateral triangle barycentre B represented by the distribution of the plurality of accelerometers PA. It is the reference system for the quantities which a sensor can measure. The so-called relative acceleration theorem correlates the acceleration of a material point integral with the first Body coordinate system to the one which would be measured in the second Local
coordinate system.
Again, with reference to figure 5, the following mathematical relationships apply: ϊsι=ϊbi +ώxr sb+ωx(ωxr sb) + 2ωχrsb+rsb (7) in which: rsi = distance between Sensor and Local rbi = distance between Body and Local ω = Body to Local angular velocity In the foregoing mathematical relationship and in all the following ones, vectors in the three-dimensional space (except for the case of the accelerations measured by the individual sensor axes, which are scalars) are generally written in small letters, while suitably sized matrices are written in capital letters.
Since a sensor is fixedly connected to an object
(Body) (even at a distance rsb) , the relative velocity and acceleration are null (ή,6=0; ^6=O), and the formula
(7) can be rewritten as follows: i"sl=rb l ι+Cb ιlώb h lxrs b b+ωb b lx(ωb ι lxrs b b)] (8) in which xa c b = quantity x measured between a and b, expressed in the reference c;
Cb l = rotation matrix between Body and Local
An accelerometer measures the specific force which acts upon the little mass contained therein, projected on its reference axis. Thus an accelerometer which is located at s with the sensitive axis individuated by the versor ηs will measure:
«. x /£• )}• ηs ηs
O) xrb b)\ηs
in which: ab b l = Body to Local linear acceleration expressed at Body; ώbl = Body to Local angular acceleration expressed at
Body.
In the described inventive example, the sensors are nine, thus the index s can take 1 to 9 values, and therefore the mathematical relationship (9) can be written nine times and, by arranging the nine achievable equations in matrix form, the following mathematical relationship (10) is achieved:
in which:
If the matrix J is invertible (that is, if there are at least 6 non-aligned accelerometers) , the system can be solved making the vector composed by angular acceleration and linear acceleration explicit, as indicated by the following mathematical relationship (12) :
in which J+ = pseudoinverse matrix of the matrix J The mathematical relationship (12) indicates that angular acceleration and linear acceleration of the Body
reference system origin are expressed as a linear composition of the accelerations measured by the various accelerometers, corrected by nonlinear terms due to the angular velocity and to the distance from the origin of each of the sensors.
B) Estimation algorithm
An estimation algorithm (or Sensor-Fusion algorithm) allows solving a dynamic system, expressed in form of differential equations in terms of angular velocity, making use of information about starting conditions, outer observation of related dynamic quantities and the knowledge of the statistical characterization of the quantities involved.
In more detail, the applicative context of an estimation algorithm is as follows : x = f(x,u,t;v) y = g(x,u,t;w) where x indicates the state of the dynamic system, as a function of time t, of a known (or measurable) input variable u, and influenced by a noise v (having a known or estimable covariance) . Instead, the term y represents the observation of the state , and is influenced by a noise w. Functions f and g are generally nonlinear. The Bayesian estimators (as they are frequently designed) theory allows, starting from a discrete time version of the preceding system, solving the problem of the state xk recursive estimation:
yk = g(.χk>uk>' wk)
One of the most promising estimators families is the Sigma-Point Kalman Filter (SPKF) family. The concept underlying the technique implemented by these types of filters is that the state xk+1 probabilistic distribution can be approximated by a finished number of deterministically extracted samples (Sigma-Points, SP) , starting from xk distribution.
Then it is possible to apply the state evolution equation, as is (nonlinear) , to all the SPs, and it is possible to demonstrate that this method retains the form and propriety of the state covariance, sensibly improving the estimation quality compared to other approximated techniques (the most famous among which is the EKF, Extended Kalman Filter technique) .
It is worth noting that these algorithms allow dealing with general dynamic systems also in the presence of non-Gaussian noise factors.
To the SPKF family belong the Unscented Kalman Filter (UKF) and the Central-Difference Kalman Filter
(CDKF) , along with the relative square-root forms, the
Square-root Unscented Kalman Filter (SR-UKF) and the
Square-root Central-Difference Kalman Filter (SR-CDKF) , which further add to the computational efficacy of the
algorithms with the same performance.
C) Estimation algorithm to the dynamic equations of the measurement system according to the invention
In order to build an estimation scheme for the quantities that can be measured by the measurement system according to the invention, it is necessary to make the dynamic system of equations defined by the mathematical relationships (2) and (4) discrete.
Particularly, in the example of the invention described herein it is preferred to get the analytical solution of the differential equations of the mathematical relationship (2) for the components of the angular velocity:
The terms hi (t) , where i = x,y,z, represent the linear factors for the abovementioned dynamic system.
In a discrete time dominium, the terms h± are linear compositions of the sampled accelerations and, within the sampling range, are assumed as constant.
Therefore, the solution for the angular velocity along the z axis is: ω∑(t) = ωz0 +hzt (14) in which ωz0=ωz(0), taking into consideration the
relevant range [0;t]
Thus, in any of the integration ranges, the differential equations for the angular velocities along x axis and y axis become:
ώx (t) = hx - ωy (t)ω2 (t) = hx - ωy (t)(ωz0 + hzt) ώy(t) = hy + ωx(t)ωz(t) = hy +ωx(t)(ω∑0 +hzt) (15)
O)x(O) = ωx0; ωy(0) = ωy0
The analytic solution of the system ( 15) is formulated in terms of implicit integrals , but by developing in Taylor series about the origin to the first order, the following approximated solution is achieved :
\ωx (t) ≤ (ωx0 + hj) cos ψ - (ωy0 + hyt) sin ψ
( 16 )
\ ωy (t) ≤ (ωy0 + hyt) cos ψ + (ωx0 + hxt) sin ψ
or
(ωx(t)λ fcosψ -sinψY ωx0 +hxt^ {ωy(t)j {sΪΑψ cos^ J(^0 +hyty t -. in whi ch ψ = \ ωz (t)dt = ωzOt + —h ΛjΛ
2
Solutions for ωx and ωy are, ultimately, given by a rotation about the z axis by an angle ψ of the constant term solutions. If the sampling time is short (high acquisition frequency) , the nonlinear terms can be neglected and the constant term solution becomes a good approximation of the system effective behaviour.
Claims
1. A system (100) for measuring an angular velocity (ωa) of a body (1) , comprising: a plurality (PA) of axial inertial accelerometers (Al, A2, A3) associable to said body (1) for the measurement of linear accelerations (ai-a9, u*) of the body (l) ;
. at least one gyro-free observation sensor (Sl, S2) , different from said plurality of accelerometers (PA) , to measure a respective observation quantity (y\) associable to the body (1) movement; an estimation module (200) of at least one quantity representative of the angular velocity of the body ( xk ) as a function of the linear accelerations (ai- a9, Uk) and of the quantity associable to the body movement (y*k) •
2. The system (100) according to claim 1, wherein the estimation module (200) is of the Bayesian type.
3. The system (100) according to claim 2, wherein the estimation module (200) comprises a filter belonging to the Sigma-Point Kalman Filter (SPKF) family of estimators .
4. The system (100) according to claim 3, wherein said filter is of the Square-Root Central Difference Kalman Filter, SR-CDKF type. W
5. The system (100) according to claim 3, wherein said filter is of the type belonging to the group comprising: Unscented Kalman Filter, UKF; Central- Difference Kalman Filter, CDFK; Square-Root Unscented Kalman Filter, SR-UKF.
6. The system (100) according to claim 1, further comprising an inertial measurement device (10) associable to said body (1) , the inertial measurement device (10) comprising a plurality of accelerometers (PA) and a microcontroller (MIC) operatively associated one to the others .
7. The system (100) according to claim 6, wherein the plurality of accelerometers (PA) and the microcontroller
(MIC) are mounted on a printed circuit board which can be secured upon the body (1) .
8. The system (100) according to claim 1, wherein the plurality of accelerometers (PA) comprises a first (Al) , a second (A2) , and a third (A3) axial inertial accelerometers coplanar one to the others and arranged so as to geometrically form an equilateral triangle, the first (Al), the second (A2) , and the third (A3) accelerometers being, each, correspondent to one of the equilateral triangle vertexes.
9. The system (100) according to claim 1, wherein the plurality of accelerometers (PA) comprises accelerometers coplanar one to the others and arranged so as to geometrically form a regular plane figure which each accelerometer represents one of the vertexes thereof .
10. The system (100) according to claim 9, wherein the regular plane figure is of the type belonging to the group comprising: square, pentagon, hexagon.
11. The system (100) according to claims 8 or 9, wherein each axial inertial accelerometer is a monolithic triaxial accelerometer.
12. The system (100) according to claim 11, wherein said monolithic triaxial accelerometer is manufactured in MEMS-type technology.
13. The system (100) according to claim 1, wherein the plurality of accelerometers (PA) is adapted to provide analog electrical signals indicative of the measurement of said linear accelerations (al-a9) .
14. The system (100) according to claim 13, further comprising an analog-to-digital conversion module which, starting from said analog electrical signals, provides for a plurality of discrete values (aik~a9k) representative of the linear accelerations (al-a9) measured by the accelerometers plurality.
15. The system (100) according to claim 14, wherein said conversion module is a sampling network.
16. The system (100) according to claim 1, wherein said at least one observation sensor (Sl, S2) is of the noninertial type, and said respective observation quantity (y* k) associable to the body (1) movement is of the noninertial type.
17. The system (100) according to claim 16, wherein said observation quantity (y\) of the noninertial type comprises body (1) position and orientation.
18. The system (100) according to claim 17, wherein said at least one observation sensor (Sl, S2) is integral with the body (1) .
19. The system (100) according to claim 18, wherein said at least one observation sensor (Sl, S2) is a videocamera or a GPS receiver.
20. The system (100) according to claim 17, wherein said at least one observation sensor (Sl, S2) is separated from the body (1) .
21. The system (100) according to claim 20, further comprising handling means (4) of the body (1) , said at least one observation sensor (Sl, S2) being mounted within the handling means (4) .
22. The system (100) according to claim 21, wherein said at least one observation sensor (Sl, S2) belongs to the group comprising: resolver, linear encoder, angular encoder.
23. The system (100) according to claim 20, further comprising handling means (4) of the body (1) , said at least one observation sensor (Sl, S2) being separated from the handling means (4) .
24. The system (100) according to claim 23, wherein said at least one observation sensor (Sl, S2) is a videocamera .
25. The system (100) according to claim 1, wherein the estimation module (200) comprises a state evolution module (201) adapted to receive an input state vector (xk) representative of the body (1) state in a current discrete moment.
26. The system (100) according to claim 25, wherein the state vector (xk) is indicative of a plurality of physical quantities representative of the body (1) movement .
27. The system (100) according to claim 26, wherein said state vector (xk) comprises at least one angular velocity vector (ωak) representative of the body (1) angular velocity in the current discrete moment.
28. The system (100) according to claim 27, wherein said state vector (xk) further comprises a position vector (pk) , a linear velocity vector (vk) and an orientation vector (ek) respectively representative of the body (21) position, linear velocity and orientation in the current discrete moment.
29. The system (100) according to claim 25, wherein said state evolution module (201) is further suitable to receive an inputting linear acceleration vector (uk) indicative of a plurality of discrete values (aik-a9k) correspondent to said linear accelerations (ax-a9) measured by said plurality of axial inertial accelerometers (PA) .
30. The system (100) according to claim 29, wherein the state evolution module (201) further accounts for outer noise factors (vk) , for example electrical noise, to which the linear acceleration vector (uk) in the current discrete moment is susceptible.
31. The system (100) according to claim 30, wherein the state evolution module (201) , based on data respectively belonging to the state vector (xk) , linear acceleration vector (uk) , and noise vector (vk) , is suitable to output a state evolution vector of the body (xk+1) in a subsequent discrete moment.
32. The system (100) according to claim 31, wherein said state evolution module (201) is arranged to mathematical relationships representative of the state evolution (f) in order to generate said body (xk+i) state evolution vector.
33. The system (100) according to claim 31, wherein the estimation module (200) further comprises a delay- module (Δ) arranged cascading to the state evolution module (201) and suitable to receive the inputting state evolution vector (Xk+i) to provide an outputting respective delayed state evolution vector (x~ k) •
34. The system (100) according to claim 33, wherein the estimation module (200) further comprises an observation module (202) , arranged cascading to the delay module (Δ) , suitable to receive the inputting the delayed state vector to output an estimated observation vector (yk) representative of an estimated observation quantity.
35. The system (100) according to claim 34, wherein said observation module (202) is arranged to implement a selection function (g) suitable to select a position component and an orientation component of the delayed state vector (x~k) in order to get the estimated state observation vector (yk) .
36. The system (100) according to claim 34, wherein the estimation module (200) further comprises a comparison module (GNF) between said estimated state observation vector (yk) and a measured state vector
(y\) .
37. The system (100) according to claim 17 and 36, wherein said measured state vector (y\) is representative of discrete values, in the current discrete moment, of the body position and orientation corresponding to body position and orientation measured by said at least one observation sensor (Sl, S2) .
38. The system (100) according to claim 36, wherein the comparison module (CNF) , based on said estimated state observation vector (yk) and said measured state observation vector (y* k) , is suitable to output an innovation vector (zk) representative of an innovation quantity.
39. The system (100) according to claim 38, wherein the estimation module (200) further comprises a sub- estimation module (203) suitable to receive said inputting innovation vector (zk) and said delayed state vector (x-k) to output an estimated body state vector (xk) including said angular quantity representative of the body (1) angular velocity.
40. The system (100) according to claim 39, wherein the sub-estimation module (203) comprises an amplification filter of the innovation vector (zk) and an adder module adapted to sum said amplified innovation vector (zk) to the delayed state vector (x~ k) in order to get the estimated body state vector ( xk ) including said angular velocity (ωa) .
41. . The system (100) according to claim 40, wherein the sub-estimation module (203) is suitable to provide the estimated body state vector ( xk ) in input to the state evolution module (201) .
42. The system (100) according to claim 1, further comprising a processing unit (EE) operatively associated to the body (1) , including a memory (M) and a microprocessor (MICR) respectively suitable to load and run program codes for the implementation of said estimation module (200) and the generation of a piece of information (I) representative of said angular velocity
(ωa) •
43. The system (100) according to claim 42, wherein said body (1) comprises a control unit (UC) for the handling of said body, said control unit (UC) being operatively associated with said processing unit (EE) in order to handle the body as a function of the piece of information (I) generated by the processing unit (EE) .
44. The system (100) according at least one of the preceding claims, wherein said system is of the gyro- free type.
45. The system (100) according at least any of the preceding claims, wherein each accelerometer (Al, A2, A3) of said plurality of triaxial accelerometers (PA) is of the gyro-free type.
46. The system (100) according to claim 6, wherein the accelerometers plurality (PA) and the microcontroller (MIC) are built inside a substrate of an integrated circuit which can be secured to the body (1) .
47. The system (100) according to claim 20, further comprising support sensors of the noninertial type, such as for example temperature sensors, with which the inertial measurement device (10) can be equipped.
48. The system (100) according to claim 1, wherein the plurality of accelerometers (PA) comprises first and second inertial biaxial accelerometers that are coplanar to each other and arranged in opposite vertexes of a rectangle or a square.
49. A method for measuring the angular velocity of a body (1) , comprising the steps of: - associating with said body (1) a plurality of axial inertial accelerometers (Al, A2, A3) for the measurement of linear accelerations (al-a9, uk) of the body (1) ; estimating, through an estimation module (200) at least one quantity representative of the angular velocity of the body ( xk ) as a function of linear accelerations (al-a9, uk) and of the quantity associable to the body movement (y* k) .
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
EP07736694A EP2135142A1 (en) | 2007-03-15 | 2007-03-15 | System for measuring an inertial quantity of a body and method thereon |
PCT/IT2007/000189 WO2008111100A1 (en) | 2007-03-15 | 2007-03-15 | System for measuring an inertial quantity of a body and method thereon |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
PCT/IT2007/000189 WO2008111100A1 (en) | 2007-03-15 | 2007-03-15 | System for measuring an inertial quantity of a body and method thereon |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2008111100A1 true WO2008111100A1 (en) | 2008-09-18 |
Family
ID=38738718
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/IT2007/000189 WO2008111100A1 (en) | 2007-03-15 | 2007-03-15 | System for measuring an inertial quantity of a body and method thereon |
Country Status (2)
Country | Link |
---|---|
EP (1) | EP2135142A1 (en) |
WO (1) | WO2008111100A1 (en) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2199880A1 (en) | 2008-12-19 | 2010-06-23 | Sintesi SCpA | Method and system for estimating the position of a series of articulated elements |
EP2221623A1 (en) * | 2009-02-19 | 2010-08-25 | Sintesi SCpA | Method for estimating the angular acceleration and related inertial measurement unit |
EP2221624A1 (en) * | 2009-02-19 | 2010-08-25 | Sintesi SCpA | Method for estimating the angular acceleration and related inertial measurement unit |
CN103552059A (en) * | 2013-10-28 | 2014-02-05 | 哈尔滨工业大学深圳研究生院 | Pick-and-place operation oriented parallel robot mechanism with four degrees of freedom and large working space |
DE102013221899A1 (en) * | 2013-10-29 | 2015-04-30 | Volkswagen Aktiengesellschaft | industrial robots |
US9568320B2 (en) | 2015-05-05 | 2017-02-14 | King Fahd University Of Petroleum And Minerals | Method and apparatus for estimation of center of gravity using accelerometers |
CN109115213A (en) * | 2017-06-21 | 2019-01-01 | 卡特彼勒公司 | For merging the system and method to determine machine state using sensor |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS6123212A (en) * | 1984-07-11 | 1986-01-31 | Hitachi Ltd | Controller of multi-joint structure machine |
-
2007
- 2007-03-15 WO PCT/IT2007/000189 patent/WO2008111100A1/en active Application Filing
- 2007-03-15 EP EP07736694A patent/EP2135142A1/en not_active Withdrawn
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS6123212A (en) * | 1984-07-11 | 1986-01-31 | Hitachi Ltd | Controller of multi-joint structure machine |
Non-Patent Citations (2)
Title |
---|
BUHMANN A ET AL: "A GPS aided Full Linear Accelerometer Based Gyroscope-free Navigation System", POSITION, LOCATION, AND NAVIGATION SYMPOSIUM, 2006 IEEE/ION CORONADO, CA APRIL 25-27, 2006, PISCATAWAY, NJ, USA,IEEE, 25 April 2006 (2006-04-25), pages 622 - 629, XP010924981, ISBN: 0-7803-9454-2 * |
TSUNG-LIN CHEN ET AL: "MEMS SoC: observer-based coplanar gyro-free inertial measurement unit; Observer-based coplanar gyro-free IMU", JOURNAL OF MICROMECHANICS & MICROENGINEERING, INSTITUTE OF PHYSICS PUBLISHING, BRISTOL, GB, vol. 15, no. 9, 1 September 2005 (2005-09-01), pages 1664 - 1673, XP020091664, ISSN: 0960-1317 * |
Cited By (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2199880A1 (en) | 2008-12-19 | 2010-06-23 | Sintesi SCpA | Method and system for estimating the position of a series of articulated elements |
EP2221623A1 (en) * | 2009-02-19 | 2010-08-25 | Sintesi SCpA | Method for estimating the angular acceleration and related inertial measurement unit |
EP2221624A1 (en) * | 2009-02-19 | 2010-08-25 | Sintesi SCpA | Method for estimating the angular acceleration and related inertial measurement unit |
CN103552059A (en) * | 2013-10-28 | 2014-02-05 | 哈尔滨工业大学深圳研究生院 | Pick-and-place operation oriented parallel robot mechanism with four degrees of freedom and large working space |
DE102013221899A1 (en) * | 2013-10-29 | 2015-04-30 | Volkswagen Aktiengesellschaft | industrial robots |
DE102013221899B4 (en) | 2013-10-29 | 2022-05-25 | Volkswagen Aktiengesellschaft | industrial robot |
US9568320B2 (en) | 2015-05-05 | 2017-02-14 | King Fahd University Of Petroleum And Minerals | Method and apparatus for estimation of center of gravity using accelerometers |
CN109115213A (en) * | 2017-06-21 | 2019-01-01 | 卡特彼勒公司 | For merging the system and method to determine machine state using sensor |
CN109115213B (en) * | 2017-06-21 | 2023-09-01 | 卡特彼勒公司 | System and method for determining machine state using sensor fusion |
Also Published As
Publication number | Publication date |
---|---|
EP2135142A1 (en) | 2009-12-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
EP2135142A1 (en) | System for measuring an inertial quantity of a body and method thereon | |
Cavallo et al. | Experimental comparison of sensor fusion algorithms for attitude estimation | |
Beravs et al. | Three-axial accelerometer calibration using Kalman filter covariance matrix for online estimation of optimal sensor orientation | |
CN107433590A (en) | Mechanical arm load quality and the gravitational compensation method of sensor fluctating on-line identification | |
Edwan et al. | Reduced DCM based attitude estimation using low-cost IMU and magnetometer triad | |
EP2951530A1 (en) | Inertial device, method, and program | |
Blocher et al. | Purely inertial navigation with a low-cost MEMS sensor array | |
Hu et al. | A robust orientation estimation algorithm using MARG sensors | |
WO2013125242A1 (en) | Offset estimation device, offset estimation method, offset estimation program and information processing device | |
Ariffin et al. | Low cost MEMS gyroscope and accelerometer implementation without Kalman Filter for angle estimation | |
Abir et al. | Optimized estimator for real-time dynamic displacement measurement using accelerometers | |
Olivares et al. | High-efficiency low-cost accelerometer-aided gyroscope calibration | |
JP6101835B2 (en) | Continuous calibration of inertial systems | |
Dong et al. | Calibration of low cost IMU’s inertial sensors for improved attitude estimation | |
Carratù et al. | Energy characterization of attitude algorithms | |
Farsoni et al. | Real-time identification of robot payload using a multirate quaternion-based kalman filter and recursive total least-squares | |
Han et al. | Kinematics characteristics analysis of a 3-UPS/S parallel airborne stabilized platform | |
CN109916399A (en) | A kind of attitude of carrier estimation method under shade | |
Wang et al. | Extended kalman filtering for robot joint angle estimation using mems inertial sensors | |
Hwangbo et al. | Factorization-based calibration method for MEMS inertial measurement unit | |
Schwaab et al. | Measurement analysis of multiple MEMS sensor array | |
Avrutov et al. | Calibration of an inertial measurement unit | |
Wongwirat et al. | A position tracking experiment of mobile robot with inertial measurement unit (imu) | |
Maged et al. | Stewart platform manipulator: State estimation using inertia sensors and unscented Kalman filter | |
CN110988400A (en) | MEMS accelerometer combination calibration method and calibration device |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 07736694 Country of ref document: EP Kind code of ref document: A1 |
|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
WWE | Wipo information: entry into national phase |
Ref document number: 2007736694 Country of ref document: EP |