CN112987069B - Method for measuring tail end pose of operation part based on vehicle body pose - Google Patents
Method for measuring tail end pose of operation part based on vehicle body pose Download PDFInfo
- Publication number
- CN112987069B CN112987069B CN202110188180.0A CN202110188180A CN112987069B CN 112987069 B CN112987069 B CN 112987069B CN 202110188180 A CN202110188180 A CN 202110188180A CN 112987069 B CN112987069 B CN 112987069B
- Authority
- CN
- China
- Prior art keywords
- vehicle body
- attitude
- tail end
- pose
- information
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/53—Determining attitude
Abstract
A method for measuring the end pose of a working component based on the pose of a vehicle body comprises the steps of a, mounting a pose sensor on the vehicle body or a carrier platform to measure the pose of the vehicle body; the GNSS system measures the pose information of the vehicle body;the laser sensing measurement is based on the relative position information of the reference working surface; c. establishing a vehicle body coordinate system Oxyz; d. the actuator is provided with sensors for detecting changes in attitude and position and establishing a coordinate system Ox of the working membernynzn(ii) a e. Acquiring position information sensed by a satellite antenna or laser and attitude and position information measured by each sensor, and establishing a position resolving model of the center of the tail end of the operation part; f. calculating the elevation difference delta h of the tail end of the operation component based on the reference operation surface; attitude information of the work member is measured, and an angular difference Δ α of the work member end based on the reference work plane is calculated. A GNSS system can be used for positioning a vehicle body and measuring the terminal pose of an operation part, and belongs to the field of engineering machinery and intelligent agricultural machinery.
Description
Technical Field
The invention relates to the field of engineering machinery and intelligent agricultural machinery, in particular to a method for measuring the end pose of an operation part based on the pose of a vehicle body.
Background
With the continuous development of science and technology, the requirements of people on the operation precision and the operation efficiency of machinery are continuously improved, and the requirements on accurate perception of the pose information of the tail-end operation part are required. With the rapid development of the carrier phase differential technology of the satellite positioning system, the measurement of the attitude information of the carrier by using satellite positioning becomes a hot spot of the satellite positioning application research rapidly. The position and orientation information of the GNSS has the advantages of high precision, no error accumulation, no need of initialization, low cost and the like. The laser technology is one of the most important technological inventions since the 20 th century, and the application fields of the laser technology are more and more extensive due to the characteristics of strong collimation, good coherence, stable physical properties, high precision and the like. The laser leveling technology takes rotating laser as a calibration plane, and a laser receiver determines the elevation position of a mounted carrier according to laser signals received by photoelectric conversion elements with different elevations.
In recent years, in China, southern China agricultural university designs a paddy field grader based on GNSS technology, and a GNSS receiving antenna is fixed on a land leveling shovel to acquire elevation positioning information of the land leveling shovel (husband, etc. 2015); the team also designed a high precision laser grader laser receiver for use in farmland leveling and concrete leveling (monster et al, 2020). Abroad, a satellite vehicle (Trimble) Field Level ii System grader and a Leica iCON grade iGD series bulldozer, come card install a GNSS dual antenna on a land leveling/dozing blade to obtain position and attitude information of a working part.
The method conforms to the development trend of unmanned operation, utilizes a plurality of sets of GNSS to realize automatic navigation and measurement of attitude and elevation position information of the operation part, not only increases the cost, but also has large difficulty in mounting a GNSS antenna at the tail end of the operation part, is inaccurate in measurement, and even cannot be mounted, if the operation part needs to be turned over forwards and backwards. In addition, masts, cables and antennas on the work part limit the range of operation of the work part to a certain extent, increasing the risk of damage to the equipment, and also increasing the time required for daily disassembly and reassembly. Therefore, it is necessary to provide a method for satisfying the vehicle body positioning and the end pose measurement of the operation part by only one set of GNSS system or laser sensing. More importantly, the position information of the tail end of the operation part with high precision is required to be measured and obtained quickly in complex modeling construction, underwater operation and the like, and a GNSS system and laser sensing are difficult to obtain through direct measurement.
Therefore, it is necessary to provide a method for measuring the end pose of the working element based on the pose of the vehicle body to meet the application requirements.
Disclosure of Invention
Aiming at the technical problems in the prior art, the invention aims to: the method for measuring the end pose of the operation part based on the vehicle body pose can meet the requirements of vehicle body positioning and end pose measurement of the operation part by using a set of GNSS system or laser sensing.
In order to achieve the purpose, the invention adopts the following technical scheme:
a method for measuring the end pose of a working component based on the pose of a vehicle body comprises the following steps:
a. installing an attitude sensor on a vehicle body or a carrier platform rigidly connected with the vehicle body, and measuring the attitude of the vehicle body;
b. a satellite antenna in a GNSS system is arranged on the top of a vehicle body, and the pose information of the vehicle body is measured; or the laser sensor is arranged on the vehicle body or the carrier platform, and the relative position information based on the reference working surface is measured;
c. establishing a vehicle body coordinate system Oxyz, taking the vehicle body advancing direction as a y axis, enabling the x axis and the y axis to point to the right side of the vehicle body direction perpendicularly, and enabling the z axis to be perpendicular to the x and y axial directions;
d. sensors are mounted on the actuators to detect changes in the attitude and position of the actuators and establish a working member coordinate system Oxnynzn，n＝1,2...；
e. Acquiring position information sensed by a satellite antenna or laser, acquiring vehicle body attitude information, acquiring attitude information and position variation of each actuating mechanism, establishing a position resolving model for solving the center of the tail end of the operating part based on the acquired information and the geometric relation of the actuating mechanisms, and calculating to obtain the center coordinate of the tail end of the operating part;
f. measuring attitude information of the working component and calculating attitude information of the tail end of the working component;
g. and designing a reference working surface, and calculating the height difference delta h between the tail end of the working part and the reference working surface and the angle difference delta alpha between the tail end of the working part and the reference working surface by combining the obtained tail end pose information of the working part.
Preferably, in step b, the GNSS system is used to calculate the yaw angle ψ and the spatial position coordinates. The bar is suitable for GNSS systems.
Preferably, in step d, the sensor is an attitude sensor or a displacement sensor according to the motion requirement of the actuator. The strip is suitable for GNSS and laser sensing systems.
Preferably, in step e, a GNSS coordinate calculation model is established based on the position information of the satellite antenna, the attitude information of the vehicle body, and the attitude information and the position variation of each actuator, and the three-dimensional coordinates of the end of the work part are calculated, where the GNSS coordinate calculation model is as follows:
wherein n is 1, 2.; r represents a three-dimensional coordinate of the center of the end of the work member; p represents position information of the satellite antenna; the psi is set to zero,
theta is respectively a yaw angle, a roll angle and a pitch angle of the vehicle body; beta is a1,β2,......,βnMeasured values of attitude sensors on each actuating mechanism are obtained; l1,l2,......,lnMeasured by displacement sensors on each actuator. The bar is suitable for GNSS systems.
Preferably, the step of calculating the three-dimensional coordinates of the end of the working part comprises the following steps:
(1) measuring angle and length information of the vehicle body and the actuating mechanism and coordinate increment from the satellite antenna to the mass center position of the vehicle body, and inputting the coordinate increment into a GNSS coordinate resolving model;
(2) converting the attitude angle of the vehicle body;
(3) calculating a vehicle body posture rotation matrix;
(4) calculating the position information of the center of the tail end of the operation part based on a vehicle body coordinate system according to the geometric relation or the robot kinematics and the measured attitude information and position variation of each actuating mechanism;
(5) calculating the coordinate increment from the satellite antenna to the center of the tail end of the operation part;
(6) and (5) calculating the three-dimensional coordinates of the center of the tail end of the operation part by combining the position information of the satellite antenna and the step (5).
Preferably, in step e, a laser elevation calculation model is established according to the position information and the vehicle body attitude information of the laser sensor and the attitude information and the position variation of each actuator, and the elevation information of the center of the tail end of the working part is solved, wherein the laser elevation calculation model is as follows:
wherein n is 1, 2.; h represents the relative position information of the center of the tail end of the working component relative to the reference working surface; h ispIndicating the relative elevation of the laser receiver relative to the datum level; the psi is set to zero,
theta is respectively a yaw angle, a roll angle and a pitch angle of the vehicle body; beta is a1,β2,......,βnMeasured values of attitude sensors on each actuating mechanism are obtained; l1,l2,......,lnMeasured by displacement sensors on each actuator. The strip is suitable for use in a laser sensing system.
Preferably, the working element end center elevation calculation step is as follows:
(1) measuring angle and length information of the vehicle body and the actuating mechanism and Z-axis coordinate increment from a laser receiver to the installation position of the vehicle body or the carrier platform attitude sensor, and inputting a laser elevation calculation model;
(2) calculating a vehicle body posture rotation matrix;
(3) calculating a coordinate Z-axis component of the center of the tail end of the operation part based on a vehicle body coordinate system according to the geometrical relationship or the robot kinematics and the measured attitude information and position variation of each actuating mechanism;
(4) calculating the Z-axis coordinate increment from the laser receiver to the center of the tail end of the operation part;
(5) and (5) combining the position information sensed by the laser in the step (4) to calculate the relative position information of the tail end center of the working part relative to the reference working surface.
Preferably, in step g, the design reference working surface comprises an elevation h, and an elevation difference of the tail end of the working component at each position of the reference working surface is calculated based on a Z-axis component of the three-dimensional coordinate of the tail end of the working component; in the laser sensing, the reference working surface is the elevation of a plane, a relative elevation difference is formed between the laser receiver and the laser transmitter, and the elevation difference between the laser receiver and the reference working surface can be known while the reference working surface is designed (namely the position of the laser transmitter is designed). The strip is suitable for GNSS and laser sensing systems.
Preferably, in step g, the design reference working plane includes a design reference working plane angle α, the attitude of the end of the work part with respect to the reference working plane is taken, and the angular difference of the end of the work part based on the reference working plane is calculated.
Preferably, the initial yaw angle psi in the vehicle body attitude angle is obtained by filtering the satellite signal receiving module from the position relation data of the satellite antenna or obtained by fusing the initial yaw angle psi with the yaw angle measured by the attitude sensor; the satellite antenna position information used for calculation is obtained by filtering the antenna initial information and then performing coordinate conversion. The bar is suitable for GNSS systems.
The principle of the invention is as follows: detecting the attitude (course angle, pitch angle and roll angle) of the vehicle body by using an attitude sensor or a combined attitude sensor and a GNSS system, measuring the attitude information and position variation of each actuating mechanism by using the sensor, and establishing a GNSS coordinate resolving model for resolving the real-time position of the tail end center of the operation part by combining the position information measured by a satellite antenna; in a laser sensing system, an attitude sensor is used for measuring the attitude of a vehicle body, the sensor is used for measuring the attitude information and the position variation of each actuating mechanism, and a laser elevation calculation model for calculating the real-time position of the tail end center of an operation part is established by combining the position information measured by laser sensing; and calculating the real-time attitude of the operation part and the real-time height difference between the tail end of the operation part and the reference operation surface based on the reference operation surface. Compared with the traditional manual measurement working mode, the calculation of the tail end position and the attitude of the operation part has the advantages of high precision, high safety and high performance, high speed, capability of meeting the requirements of complex construction shape and underwater operation, and reduction of labor cost and labor intensity. The precision of the operation machine is ensured, so that the purpose of precise operation is achieved.
Compared with the prior art, the invention has the following advantages and effects:
(1) the invention can meet the requirements of vehicle body positioning and operation part end pose measurement only by one set of GNSS system.
(2) The method can obtain not only the position information but also the posture information of the end of the working part. Compared with the traditional manual measurement working mode, the resolving of the three-dimensional coordinates and the postures of the tail end of the operation part has the advantages of high precision, high safety performance and high speed, and meanwhile, the requirements of complex construction shape and underwater operation can be met, and the labor cost and the labor intensity are reduced.
(3) The method comprises the steps of detecting attitude angles (course angle, pitch angle and roll angle) of an equipment vehicle body through a GNSS system and an attitude sensor, measuring attitude information and position variation of each actuating mechanism by adopting the sensor, and establishing a GNSS coordinate resolving model for resolving the real-time position of the tail end of an operation part by combining position information measured by a satellite antenna; the method comprises the steps that a positioning occasion is not needed, the attitude of a vehicle body is measured by utilizing an attitude sensor, attitude information and position variation of each actuating mechanism are measured by adopting the sensor, and a laser elevation calculation model for calculating the real-time position of the tail end center of an operation part is established by combining the position information measured by laser sensing; and resolving the real-time attitude of the work tool and the real-time elevation difference between the tail end of the work part and the reference working plane based on the design reference working plane. The precision of the operation machine is ensured, so that the purpose of precise operation is achieved.
(4) The invention can solve the problems that the movement of the operation part is complex, and the tail end of the operation part is inconvenient to install a GNSS antenna or a laser elevation sensor and cannot be measured. The GNSS or laser equipment is moved from the end of the work piece to the vehicle body or carrier platform, reducing the time required for daily disassembly and reassembly, as well as reducing the risk of damage to the equipment.
Drawings
Fig. 1 is a flowchart of a method for measuring an end pose of a work implement based on a vehicle body pose using a GNSS system.
FIG. 2 is a flow chart of a method for measuring the pose of the end of a work implement based on the pose of a vehicle body using laser sensing.
Fig. 3a is a schematic view of the structure of the leveler.
Fig. 3b is a diagram of the movement trajectory of the leveling blade of the screed.
Fig. 4a is a schematic structural view of a grader.
FIG. 4b is a simplified diagram of a grader.
In fig. 3a and 3b, 1-1 is a vehicle body, 1-2 is a laser receiver, 1-3 is a laser transmitter, 1-4 is a posture sensor, 1-5 is an elevation cylinder, 1-6 is a land leveling shovel, 1-7 is a spike roller, and 1-8 is a displacement sensor.
In fig. 4a and 4b, 2-1 is an equipment vehicle body, 2-2 is a satellite antenna, 2-4 is a hydraulic system, 2-5 is an elevation cylinder, 2-6 is a horizontal cylinder, 2-7 is a flat shovel horizontal attitude sensor, 2-8 is a flat shovel, and 2-3, 2-9 and 2-10 are attitude sensors.
Detailed Description
The present invention will be described in further detail with reference to specific embodiments.
Example one
The embodiment mainly outlines the application of laser sensing to calculate the elevation difference delta h of the tail end of the operation part based on a reference operation plane, as shown in fig. 3a and 3 b.
This embodiment employs a horse planer comprising: the device comprises a vehicle body 1-1, a laser receiver 1-2, a laser emitter 1-3, an attitude sensor 1-4, an elevation oil cylinder 1-5, a land leveling shovel 1-6 (the tail end of an operation part in the embodiment), a cattail roller 1-7 (a carrier platform) and a displacement sensor 1-8. The land leveling shovel is connected with the cattail roller, the elevation adjustment of the land leveling shovel can be realized through the elevation oil cylinder, and the displacement sensor is installed on the elevation oil cylinder and used for measuring the stroke of the oil cylinder. The cattail roll is hung on the vehicle body through three-point suspension, and the attitude sensor is arranged on the cattail roll to indirectly measure the attitude of the vehicle body. And installing a laser receiver above the cattail roller.
And designing a reference working surface (working plane elevation), wherein a relative elevation difference is formed between the laser receiver 1-2 and the laser transmitter 1-3 during working. Since the land scraper component and the cattail roller component are rigidly connected, the elevation change of the land scraper can be influenced by the elevation of the cattail roller, so that the laser receiver 1-2 is arranged above the cattail roller to measure the elevation-based working surfaceRelative elevation hp。
As shown in fig. 3b, mounting attitude sensors 1-4 on the rollers 1-7 to sense attitude information psi, phi and theta of the carrier platform, and establishing a vehicle body coordinate system Oxyz; establishing a coordinate system Ox of the work member1y1z1、Ox2y2z2And calculating the elevation changes of the tail ends of the working parts at different degrees when the hydraulic oil cylinder moves, as shown in a motion trail diagram of the land leveling blade in fig. 3 b. The interaction of the movements of the parts of this embodiment is described below: the extension and retraction of the leveling oil cylinders 1-5 of the leveling shovel can cause the leveling shovel to have two different levels of elevation changes; elevation changes of the cattail roller, the laser receiver and subsequent components are influenced by the lifting of the three-point suspension; the change of the pose of the vehicle body causes the change of the pose of the cattail roller, and then the elevation and the pose of all the components are influenced.
Thus according to the attitude information psi of the carrier platform (cattail),
theta and displacement information l of elevation oil cylinder measured by displacement sensor 1-81Establishing a calculation model for calculating the elevation of the tail end of the working part based on the reference working plane in geometric relation with an actuating mechanism
Wherein H represents relative position information of the center of the distal end of the working member with respect to the reference working plane; h ispRepresenting the relative elevation of the laser receiver relative to the reference working plane; the psi is set to zero,
theta is respectively a yaw angle, a roll angle and a pitch angle of the vehicle body; l1Is the measured value of the displacement sensor on the actuating mechanism. Specifically, the solving steps are as follows:
1) measuring angle and length information of vehicle body and actuating mechanism and based on laser receiver under vehicle body coordinate system
Z-axis seat to installation position of attitude sensor of carrier platformStandard increment ZbInputting a laser elevation calculation model;
2) calculating a vehicle body posture rotation matrix;
wherein:nRba vehicle body attitude rotation matrix; ri(angle) represents a basic rotation matrix for rotating the angle around the i-axis of a Cartesian (local) coordinate system, Ri T(angle) is RiThe transposed matrix of (angle).
3) Calculating a Z-axis component r of the center of the working end based on the vehicle body coordinate system according to the geometric relationship and the measured position variation of the actuating mechanismz；
rz＝f(l1)
Wherein l1And f (x) is a relation function of the elongation of the elevation oil cylinder and the elevation change of the leveling shovel based on the vehicle body coordinate system.
4) Calculating Z-axis coordinate increment Z from laser receiver to tail end center of working part based on vehicle body coordinate systeme；
Ze＝Zb+rz
5) Combining the step 4) and the relative position information h sensed by the laserpRelative position information H of the center of the end of the work member with respect to the reference work plane is calculated.
hp’＝hp+E(nRb)
Wherein E (x) is a compensation function of laser sensing when the attitude of the carrier platform changes. h isp' sensing elevation information for the compensated laser. h issWhen the carrier platform is in a horizontal state, hp＝hpAnd when the' is 0, the laser receiver is different from the working plane in elevation.
Through the calculation, the purpose of calculating the elevation difference delta h of the tail end of the working component based on the reference working plane by using the laser elevation is achieved.
Example two
The invention relates to a method for measuring the end pose of an operation part based on the pose of a vehicle body, which is used for a land leveling machine, and the pose of a land leveling shovel (operation part) is calculated based on the measurement of the pose of the vehicle body, as shown in figure 4a, a vehicle body coordinate system is established for an equipment vehicle body 2-1 (a wheel type tractor), the vehicle body coordinate system Oxyz is defined as taking the mass center of the vehicle body as an original point O, taking the advancing direction of the vehicle body as a y axis, the x axis and the y axis are vertically directed to the right side of the vehicle body direction, and the z axis is vertical to the x axis and the y axis.
The land leveler is adopted in the embodiment and comprises an equipment vehicle body 2-1, a satellite antenna 2-2, a hydraulic system 2-4, an elevation oil cylinder 2-5, a horizontal oil cylinder 2-6, a land scraper horizontal attitude sensor 2-7, a land scraper 2-8 (the tail end of an operation component in the embodiment) and attitude sensors 2-3, 2-9 and 2-10. The land leveling shovel is connected with a platform carrying a hydraulic system in a parallelogram structure, and the elevation and the horizontal adjustment of the land leveling shovel are respectively realized through an elevation oil cylinder and a horizontal oil cylinder. The horizontal attitude sensor 2-7 of the land leveling shovel is arranged on the land leveling shovel and used for measuring the horizontal attitude change of the land leveling shovel. The attitude sensors 2-9 are arranged on a connecting rod of a parallelogram structure between the hydraulic system platform and the land leveling shovel, measure the angle change of the connecting rod and indirectly reflect the elevation change of the land leveling shovel. The hydraulic system platform is hung on the vehicle body through three-point suspension, the attitude sensors 2-10 are installed at the lower pull rod of the three-point suspension, and the attitude change of the lower pull rod is measured. The attitude sensor 2-3 is installed at the origin O of the vehicle body coordinate system, and measures the change of the vehicle body attitude. And (3) installing the satellite antenna 2-2 at the top of the tractor cab to acquire the pose information of the tractor body.
And acquiring position information of the satellite antenna, vehicle body attitude information and attitude information of each executing mechanism, resolving a three-dimensional coordinate of the center of the tail end of the operation part, and establishing a GNSS coordinate resolving model. Calculating the elevation difference of the tail end of the working part based on the reference working surface according to the designed reference working surface (plane or curved surface); attitude information of the work member is measured, and an angular difference of the work member end based on the reference work plane is calculated. And the elevation and leveling control is carried out on the land leveling shovel by combining the obtained elevation difference and angle difference, so that the effect of guiding the precise operation of the land leveling machine is achieved.
As shown in fig. 4a and 4b, the specific calculation steps are as follows:
(1) reading satellite signal receiving module information: the initial yaw angle ψ of the vehicle body attitude angle is obtained by filtering the positional relationship data of the antenna by the receiving module. The satellite antenna position information p which is input to calculation is obtained by filtering the satellite antenna information and then performing coordinate conversion.
(2) Acquiring data of the attitude sensor: reading vehicle body roll angle
Pitch angle theta and attitude change beta of actuator1、β2、βe1。
(3) Obtaining position information of a satellite antenna, vehicle body attitude information and attitude information of each actuating mechanism, and establishing a GNSS coordinate resolving model
Three-dimensional coordinates of the center of the end of the work part are calculated.
Wherein r represents a three-dimensional coordinate of the center of the tip of the working part; p represents position information of the satellite antenna; the psi is set to zero,
theta is respectively a yaw angle measured by the satellite antenna, a roll angle and a pitch angle measured by an attitude sensor on the vehicle body; beta is a1、β2Is an on-actuator attitude sensor measurement. Specifically, the solving steps are as follows:
1) measuring angle and length information of vehicle body and actuating mechanism and coordinate increment I of satellite antenna and mass center positionbInputting a GNSS coordinate resolving model;
2) converting the attitude angle of the vehicle body;
the attitude angle rotation sequence from the geographic coordinate system to the vehicle body coordinate system is a heading angle psi-a pitch angle theta-a roll angle
If a dual axis attitude sensor is used, the pitch angle theta and roll angle measured by the sensor on the vehicle body
One of them must be scaled differently than the coordinate system rotation calculation. Determined by the rotation sequence, the Y-axis (roll) is taken as the first rotation axis of the coordinate system transformation, and the roll angle measured by the attitude sensor
The roll angle used for attitude rotation calculation is changed by taking the X axis (pitching) as a coordinate system to transform a second rotating axis, and the pitch angle theta measured by the attitude sensor is actually the included angle between the rotated X axis and the horizontal plane instead of the original X axis, so the pitch angle needs to be converted.
According to the relation of the triangle and the triangle,
wherein, thetachIs the converted pitch angle.
3) Calculating an actual vehicle body posture rotation matrix;
and (3) rotating the vehicle body coordinate system b for three times, wherein the Euler angle rotation sequence is roll (Y axis) -pitch (X axis) -yaw (Z axis), the Euler angle rotation sequence is rotated to be aligned with the geographic coordinate system n, and the formula for calculating the vehicle body attitude rotation matrix is as follows:
wherein:nRba vehicle body attitude rotation matrix; ri(angle) represents a basic rotation matrix for rotating the angle around the i-axis of a Cartesian (local) coordinate system, Ri T(angle) is RiThe transposed matrix of (angle).
4) And calculating the position information of the center of the tail end of the operation part based on the vehicle body coordinate system according to the geometrical relationship or the robot kinematics and the measured attitude information and the position variation of each actuating mechanism.
And selecting a proper calculation mode according to the mechanical structure characteristics of the vehicle body. For a simple vehicle body mechanical structure, the coordinate r of the center of the tail end of the operation part based on the initial vehicle body coordinate system (the horizontal state of the vehicle body) can be estimated according to the geometric relation and the measurement value of each actuator sensoro. For a mechanical arm type engineering machine/agricultural machine, the position and the direction of each connecting rod can be determined according to the kinematics of the robot and the geometric characteristics of the given machine by knowing the node variable, then a proper coordinate system is established on each connecting rod, the configuration of the adjacent coordinate systems is determined by the rigid body motion mode, and the coordinate r of the center of the tail end of the operation part based on the initial vehicle body coordinate system (the horizontal state of the vehicle body) is calculated according to the configuration of the adjacent coordinate systemo. The vehicle body coordinate system rotates with the vehicle body due to terrain and the like in a real-time state, and if the actuating mechanism is provided with the attitude sensor, the measured value of the actuating mechanism comprises the variation of the vehicle body attitude, so that the coordinate r of the center of the tail end of the operation part based on the vehicle body coordinate system in a real-time staterCalculation is carried out on roConversion is performed.
In the present embodiment, the coordinates of the center of the end of the working element in the vehicle body coordinate system are calculated based on the geometric relationship and the measured attitude information of each actuator.
FIG. 4b is a simplified schematic of a grader calculation to solve the coordinate r of the center of the end of the work part based on the initial body coordinate system (body level condition)o：
ro＝{rox,roy,roz}
From the characteristics of the graderox0, based on fig. 3b, according to geometric relationships
roy＝-(d1+L1cosβ1+L2+L3cosβ2+d4)
roz＝-(d0-d2+L1sinβ1+L3sinβ2+d3)
Wherein d is0Represents the height of the centroid from the ground in mm;
d1the horizontal distance of a connecting point A of the three-point suspension lower pull rod and a hydraulic system 2-4 support frame and the center of mass is expressed in unit mm;
d2the height of a connecting point A between the three-point suspension lower pull rod and a support frame of a hydraulic system 2-4 from the ground is expressed in unit mm;
d3the vertical distance of the D point of the connecting rod where the attitude sensors 2-9 are located from the center of the tail end of the operation part is represented by unit mm;
d4the horizontal distance of the D point of the connecting rod where the attitude sensors 2-9 are located and the center of the tail end of the operation part is represented in unit mm;
L1the length of a connecting rod AB where the attitude sensors 2-10 are located is shown in unit mm;
L2represents the length of the bottom BC of the support frame of the hydraulic system 2-4 in mm;
L3the length of a connecting rod CD where the attitude sensors 2-9 are located is represented in unit mm;
β1representing the measured value of the attitude measured by the attitude sensor 2-10 in unit degree;
β2representing attitude measurements measured by attitude sensors 2-9 in units.
The vehicle body coordinate system rotates with the vehicle body due to terrain and the like in a real-time state, and the coordinate r of the center of the tail end of the operation part based on the vehicle body coordinate system in a real-time state is obtained because the measurement value of the attitude sensor comprises the variation of the attitude of the vehicle bodyrAnd (4) resolving, and converting the mechanism attitude measurement value. What influences the attitude sensors 2-9 and 2-10 in the grader is the pitch angle of the vehicle body, which is:
5) calculating coordinate increment I between satellite antenna and center of tail end of operation parte；
Ie＝Ib+rr
6) And 5) calculating the three-dimensional coordinate r of the center of the tail end of the operation part by combining the position information of the satellite antenna and the step 5).
r＝p+nRbIe
(4) Designing a reference working surface (plane or curved surface), namely the angle alpha and the elevation h of the designed working surface, and measuring the horizontal attitude beta of the land leveling shovel in real timee1And calculating the angle difference delta alpha of the end tool of the working component based on the reference working surface: Δ α ═ βe1- α; z-axis component r based on three-dimensional coordinates of the end of the work partzAnd calculating the elevation difference delta h between the tail end of the working component and the reference working surface.
EXAMPLE III
The invention relates to a method for measuring the end pose of a working part based on the pose of a vehicle body, which is used for an excavator of a four-degree-of-freedom mechanical arm and used for calculating the pose of a bucket (the end of the working part) based on the measurement of the pose of the vehicle body. The GNSS coordinate solution model established in this embodiment is as follows:
wherein r represents a three-dimensional coordinate of the center of the tip of the working part; p represents position information of the satellite antenna; the psi is set to zero,
theta is respectively a yaw angle, a roll angle and a pitch angle of the vehicle body; beta is a1,β2,β3,β4Measured values of attitude sensors on each actuating mechanism are obtained;
other steps of the calculation are similar to those of the embodiment, and only the position information part of the center of the tail end of the operation part based on the vehicle body coordinate system can be different according to the calculation, the coordinate value of the center of the tail end of the operation part can be calculated by using the geometric relation as the embodiment, the position and the direction of each connecting rod can be determined by knowing the node variable according to the geometric characteristics of the given machine according to the robot kinematics, a proper coordinate system is established on each connecting rod, and the adjacent connecting rods are determined by the rigid body motion modeThe coordinate system is configured to calculate the coordinate r of the center of the end of the working member based on the initial body coordinate system (body horizontal state)o。
The conversion of joint space to position space into positive kinematics problem, the establishment of D-H coordinate system of digging robot, the transformation from base coordinate system to bucket coordinate system0T4Comprises the following steps:0T4＝0T1 1T2 2T3 3T4
wherein the content of the first and second substances,i-1Tibased on the specification of the D-H method, the coordinate system BiTransformation into coordinate system Bi-1The transformation matrix of (a) may be expressed as a product of 4 basic transformations of the link (i) and the joint i.
And the position of the bucket end in the base coordinate system is:
the vehicle body coordinate system rotates along with the vehicle body due to terrain and the like in a real-time state, and the coordinate r of the tail end center of the operation part based on the vehicle body coordinate system in a real-time state is because the dip angle sensor measurement value comprises the variation of the vehicle body attituderCalculation is carried out on roConversion is carried out:
rr＝RX(-θ)ro
wherein R isi(angle) represents a basic rotation matrix of angle angles rotated around the i-axis of a cartesian (local) coordinate system.
Due to the fact that the motion track of the machine is complex, the tail end of a working part of the machine (such as a bucket of an excavator) is not suitable for directly installing a GNSS antenna or laser sensing on the tail end to directly sense the position information of the tail end due to the problems of working modes and the like. The method for measuring the end pose of the operation part based on the pose measurement of the vehicle body is applied in consideration of the safety of the working environment, the assembly difficulty and the equipment easy to damage in the motion process. Meanwhile, the measurement is required to quickly obtain the position information of the tail end of the operation part with high precision in complex modeling construction, underwater operation and the like, and the measurement method provided by the invention can be applied to the situation that a GNSS system or laser sensing is difficult to directly obtain the position information.
The above embodiments are preferred embodiments of the present invention, but the present invention is not limited to the above embodiments, and any other changes, modifications, substitutions, combinations, and simplifications which do not depart from the spirit and principle of the present invention should be construed as equivalents thereof, and all such changes, modifications, substitutions, combinations, and simplifications are intended to be included in the scope of the present invention.
Claims (6)
1. A method for measuring the end pose of a working component based on the pose of a vehicle body is characterized by comprising the following steps: the method comprises the following steps:
a. installing an attitude sensor on a vehicle body or a carrier platform rigidly connected with the vehicle body, and measuring the attitude of the vehicle body;
b. a satellite antenna in a GNSS system is arranged on the top of a vehicle body, and the pose information of the vehicle body is measured; or the laser sensor is arranged on the vehicle body or the carrier platform, and the relative position information based on the reference working surface is measured;
c. establishing a vehicle body coordinate system Oxyz, taking the vehicle body advancing direction as a y axis, enabling the x axis and the y axis to point to the right side of the vehicle body direction perpendicularly, and enabling the z axis to be perpendicular to the x and y axial directions;
d. sensors are mounted on the actuators to detect changes in the attitude and position of the actuators and establish a working member coordinate system Oxnynzn，n＝1,2...；
e. Acquiring position information sensed by a satellite antenna or laser, acquiring vehicle body attitude information, acquiring attitude information and position variation of an actuating mechanism, establishing a position resolving model for solving the center of the tail end of the operating part based on the acquired information and the geometric relation of the actuating mechanism, and calculating to obtain the center coordinate of the tail end of the operating part;
f. measuring attitude information of the working component and calculating attitude information of the tail end of the working component;
g. designing a reference working surface, and calculating the elevation difference delta h between the tail end of the working component and the reference working surface and the angle difference delta alpha between the tail end of the working component and the reference working surface by combining the obtained tail end pose information of the working component;
in the step e, a GNSS coordinate resolving model is established according to the position information of the satellite antenna, the attitude information of the vehicle body, the attitude information of each executing mechanism and the position variation, and the three-dimensional coordinate of the tail end of the operation part is resolved, wherein the GNSS coordinate resolving model is as follows:
wherein n is 1, 2.; r represents a three-dimensional coordinate of the center of the end of the work member; p represents position information of the satellite antenna; the psi is set to zero,
theta is respectively a yaw angle, a roll angle and a pitch angle of the vehicle body; beta is a1,β2,......,βnMeasured values of attitude sensors on each actuating mechanism are obtained; l1,l2,......,lnMeasured values of displacement sensors on each actuating mechanism are obtained;
the step of calculating the three-dimensional coordinates of the tail end of the operation part comprises the following steps:
(1) measuring angle and length information of the vehicle body and the actuating mechanism and coordinate increment from the satellite antenna to the mass center position of the vehicle body, and inputting the coordinate increment into a GNSS coordinate resolving model;
(2) converting the attitude angle of the vehicle body;
(3) calculating a vehicle body posture rotation matrix;
(4) calculating the position information of the center of the tail end of the operation part based on a vehicle body coordinate system according to the geometric relation or the robot kinematics and the measured attitude information and position variation of each actuating mechanism;
(5) calculating the coordinate increment from the satellite antenna to the center of the tail end of the operation part;
(6) calculating the three-dimensional coordinate of the center of the tail end of the operation part by combining the step (5) and the position information of the satellite antenna;
or in the step e, according to the position information and the vehicle body attitude information sensed by the laser and the attitude information and the position variation of each actuating mechanism, establishing a laser elevation calculation model, and solving the elevation information of the center of the tail end of the operation part, wherein the laser elevation calculation model is as follows:
wherein n is 1, 2.; h represents the relative position information of the center of the tail end of the working component relative to the reference working surface; h ispIndicating the relative elevation of the laser receiver relative to the datum level; the psi is set to zero,
theta is respectively a yaw angle, a roll angle and a pitch angle of the vehicle body; beta is a1,β2,......,βnMeasured values of attitude sensors on each actuating mechanism are obtained; l1,l2,......,lnMeasured values of displacement sensors on each actuating mechanism are obtained;
the working component tail end center elevation calculation steps are as follows:
(1) measuring angle and length information of the vehicle body and the actuating mechanism and Z-axis coordinate increment from a laser receiver to the installation position of the vehicle body or the carrier platform attitude sensor, and inputting a laser elevation calculation model;
(2) calculating a vehicle body posture rotation matrix;
(3) calculating a coordinate Z-axis component of the center of the tail end of the operation part based on a vehicle body coordinate system according to the geometrical relationship or the robot kinematics and the measured attitude information and position variation of each actuating mechanism;
(4) calculating the Z-axis coordinate increment from the laser receiver to the center of the tail end of the operation part;
(5) and (5) combining the position information sensed by the laser in the step (4) to calculate the relative position information of the tail end center of the working part relative to the reference working surface.
2. The vehicle body pose-based working member end pose measurement method according to claim 1, characterized in that: in step b, the GNSS system is used to calculate the yaw angle ψ and the spatial position coordinates.
3. The vehicle body pose-based working member end pose measurement method according to claim 1, characterized in that: in the step d, according to the motion requirement of the actuating mechanism, the sensor selects an attitude sensor or a displacement sensor.
4. The vehicle body pose-based working member end pose measurement method according to claim 1, characterized in that: step g, designing a datum working surface to include an elevation h, and calculating an elevation difference of the tail end of the working component at each position of the datum working surface based on a Z-axis component of a three-dimensional coordinate of the tail end of the working component; in the laser sensing, the reference working surface is a plane, the laser receiver and the laser emitter form a relative elevation difference, and the elevation difference between the laser receiver and the reference working surface can be known while the reference working surface is designed.
5. The vehicle body pose-based working member end pose measurement method according to claim 1, characterized in that: and g, the design reference working surface comprises a design reference working surface angle alpha, the tail end posture of the working part related to the reference working surface is taken, and the angle difference of the tail end of the working part based on the reference working surface is calculated.
6. The vehicle body pose-based working member end pose measurement method according to claim 1, characterized in that: the initial yaw angle psi in the vehicle body attitude angle is obtained by filtering the satellite signal receiving module from the position relation data of the satellite antenna or fusing the initial yaw angle psi with the yaw angle measured by the attitude sensor; the satellite antenna position information used for calculation is obtained by filtering the antenna initial information and then performing coordinate conversion.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110188180.0A CN112987069B (en) | 2021-02-18 | 2021-02-18 | Method for measuring tail end pose of operation part based on vehicle body pose |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110188180.0A CN112987069B (en) | 2021-02-18 | 2021-02-18 | Method for measuring tail end pose of operation part based on vehicle body pose |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112987069A CN112987069A (en) | 2021-06-18 |
| CN112987069B true CN112987069B (en) | 2021-11-26 |
Family
ID=76393731
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110188180.0A Active CN112987069B (en) | 2021-02-18 | 2021-02-18 | Method for measuring tail end pose of operation part based on vehicle body pose |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112987069B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113405554A (en) * | 2021-06-29 | 2021-09-17 | 东风汽车集团股份有限公司 | Vehicle attitude calculation method based on satellite positioning system |
| CN117040608B (en) * | 2023-10-10 | 2023-12-08 | 四川轻化工大学 | Vehicle-mounted satellite relay |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108132476A (en) * | 2017-12-29 | 2018-06-08 | 北京理工大学 | A kind of engineering machinery hopper alignment system and method |
| CN110608714A (en) * | 2019-08-01 | 2019-12-24 | 江苏科博空间信息科技有限公司 | Tower crane attitude automatic monitoring method based on Beidou/GNSS |
| CN111044073A (en) * | 2019-11-26 | 2020-04-21 | 北京卫星制造厂有限公司 | High-precision AGV position sensing method based on binocular laser |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8145391B2 (en) * | 2007-09-12 | 2012-03-27 | Topcon Positioning Systems, Inc. | Automatic blade control system with integrated global navigation satellite system and inertial sensors |
| CN102607499A (en) * | 2012-03-12 | 2012-07-25 | 华南农业大学 | Vehicle-mounted farm three-dimensional topographical surveying device |
| CN105606049B (en) * | 2015-09-11 | 2018-09-21 | 华南农业大学 | A kind of paddy agriculture equipment operation attitude measurement method of the opposite water surface |
-
2021
- 2021-02-18 CN CN202110188180.0A patent/CN112987069B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108132476A (en) * | 2017-12-29 | 2018-06-08 | 北京理工大学 | A kind of engineering machinery hopper alignment system and method |
| CN110608714A (en) * | 2019-08-01 | 2019-12-24 | 江苏科博空间信息科技有限公司 | Tower crane attitude automatic monitoring method based on Beidou/GNSS |
| CN111044073A (en) * | 2019-11-26 | 2020-04-21 | 北京卫星制造厂有限公司 | High-precision AGV position sensing method based on binocular laser |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112987069A (en) | 2021-06-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109614743B (en) | Excavator, bucket positioning method thereof, electronic equipment and storage medium | |
| CN112987069B (en) | Method for measuring tail end pose of operation part based on vehicle body pose | |
| CN108350679B (en) | Automatic blade control system of motor grader | |
| JP5873607B1 (en) | Work machine calibration apparatus and work machine parameter calibration method | |
| CN101266134B (en) | Cantilever driving frame head posture measuring systems and its method | |
| US8886416B2 (en) | Hydraulic shovel operability range display device and method for controlling same | |
| KR101495582B1 (en) | Hydraulic shovel calibration device and hydraulic shovel calibration method | |
| KR101739309B1 (en) | Work machine and correction method of working equipment parameter for work machine | |
| US8352131B2 (en) | Drainage plow control and method of controlling an elevation of a cutting edge of a drainage plow | |
| JP5841300B1 (en) | Work machine calibration apparatus and work machine parameter calibration method | |
| WO2012128192A1 (en) | Hydraulic shovel calibration device and hydraulic shovel calibration method | |
| KR102076631B1 (en) | Positioning Systems, Work Machines, and Positioning Methods | |
| CN113494104B (en) | Positioning correction method for construction work machine and positioning correction controller therefor | |
| CN111485879B (en) | Heading machine vehicle body and positioning method and positioning system of cutting drum of heading machine vehicle body | |
| US20080087447A1 (en) | Control and method of control for an earthmoving system | |
| JPH10183675A (en) | Device and method to obtain point position of working tool movably mounted in mobile machine | |
| WO2023072044A1 (en) | Excavator bucket teeth tip positioning method and apparatus, and excavator | |
| CN201247045Y (en) | Measuring system for cantalever digging handpiece location posture | |
| CN109070353B (en) | Cartesian control of boom end of large manipulator, in particular of concrete pump | |
| JP6680614B2 (en) | Surveying device, moving body, surveying method and program | |
| CN112817019A (en) | Vibroseis vehicle working point positioning error compensation method based on attitude information | |
| CN108643277B (en) | Excavator position information service system and method based on GNSS-SINS and displacement sensor | |
| CN116752591A (en) | Auxiliary land leveling control method and device for excavator and excavator | |
| CN105928513B (en) | A kind of airborne synthetic aperture radar movement parameter measurement method based on position and attitude measuring system | |
| JP7424960B2 (en) | Information acquisition system and information acquisition method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |