US11898321B2 - Input shaping for error detection and recovery in dynamically agile grading machines - Google Patents
Input shaping for error detection and recovery in dynamically agile grading machines Download PDFInfo
- Publication number
- US11898321B2 US11898321B2 US17/125,967 US202017125967A US11898321B2 US 11898321 B2 US11898321 B2 US 11898321B2 US 202017125967 A US202017125967 A US 202017125967A US 11898321 B2 US11898321 B2 US 11898321B2
- Authority
- US
- United States
- Prior art keywords
- terrain
- vehicle
- implement
- determining
- determined
- 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, expires
Links
- 238000001514 detection method Methods 0.000 title description 6
- 238000011084 recovery Methods 0.000 title description 2
- 238000007493 shaping process Methods 0.000 title description 2
- 238000000034 method Methods 0.000 claims abstract description 29
- 238000004590 computer program Methods 0.000 claims description 16
- 230000015654 memory Effects 0.000 claims description 14
- 238000013507 mapping Methods 0.000 claims description 8
- 239000011295 pitch Substances 0.000 description 40
- 238000010586 diagram Methods 0.000 description 17
- 230000033001 locomotion Effects 0.000 description 12
- 230000006870 function Effects 0.000 description 10
- 230000003993 interaction Effects 0.000 description 10
- 239000002689 soil Substances 0.000 description 10
- 230000006399 behavior Effects 0.000 description 9
- 230000008859 change Effects 0.000 description 8
- 230000001133 acceleration Effects 0.000 description 7
- 239000011159 matrix material Substances 0.000 description 7
- 238000012937 correction Methods 0.000 description 6
- 238000013500 data storage Methods 0.000 description 6
- 230000003287 optical effect Effects 0.000 description 6
- 230000005484 gravity Effects 0.000 description 5
- 230000007246 mechanism Effects 0.000 description 4
- 230000000717 retained effect Effects 0.000 description 4
- 238000013459 approach Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 3
- 238000013016 damping Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 239000007787 solid Substances 0.000 description 3
- 238000012546 transfer Methods 0.000 description 3
- 230000001276 controlling effect Effects 0.000 description 2
- 230000010355 oscillation Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000002829 reductive effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 239000000725 suspension Substances 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000000875 corresponding effect Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003534 oscillatory effect Effects 0.000 description 1
- 230000035515 penetration Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000000284 resting effect Effects 0.000 description 1
- 239000004576 sand Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 239000002352 surface water Substances 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
Images
Classifications
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/76—Graders, bulldozers, or the like with scraper plates or ploughshare-like elements; Levelling scarifying devices
- E02F3/80—Component parts
- E02F3/84—Drives or control devices therefor, e.g. hydraulic drive systems
- E02F3/841—Devices for controlling and guiding the whole machine, e.g. by feeler elements and reference lines placed exteriorly of the machine
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/76—Graders, bulldozers, or the like with scraper plates or ploughshare-like elements; Levelling scarifying devices
- E02F3/80—Component parts
- E02F3/84—Drives or control devices therefor, e.g. hydraulic drive systems
- E02F3/844—Drives or control devices therefor, e.g. hydraulic drive systems for positioning the blade, e.g. hydraulically
- E02F3/845—Drives or control devices therefor, e.g. hydraulic drive systems for positioning the blade, e.g. hydraulically using mechanical sensors to determine the blade position, e.g. inclinometers, gyroscopes, pendulums
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F3/00—Dredgers; Soil-shifting machines
- E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
- E02F3/76—Graders, bulldozers, or the like with scraper plates or ploughshare-like elements; Levelling scarifying devices
- E02F3/80—Component parts
- E02F3/84—Drives or control devices therefor, e.g. hydraulic drive systems
- E02F3/844—Drives or control devices therefor, e.g. hydraulic drive systems for positioning the blade, e.g. hydraulically
- E02F3/847—Drives or control devices therefor, e.g. hydraulic drive systems for positioning the blade, e.g. hydraulically using electromagnetic, optical or acoustic beams to determine the blade position, e.g. laser beams
-
- E—FIXED CONSTRUCTIONS
- E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
- E02F—DREDGING; SOIL-SHIFTING
- E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
- E02F9/26—Indicating devices
- E02F9/264—Sensors and their calibration for indicating the position of the work tool
- E02F9/265—Sensors and their calibration for indicating the position of the work tool with follow-up actions (e.g. control signals sent to actuate the work tool)
Definitions
- the present invention relates generally to a control system for adjusting the height of an implement of a vehicle, and more particularly to input shaping for error detection and recovery in dynamically agile grading machines.
- Grading is the process of manipulating the surface of terrain to achieve a target terrain surface. Grading is an important process in many different applications. For example, in construction, grading may be performed to set a foundation of a building. In agriculture, grading may be performed to direct surface water runoff. Dozing machines are typically used for grading terrain. A dozing machine comprises a body and a blade coupled to the body. In operation, as the dozing machine travels over the terrain, the height of the blade is adjusted to manipulate the surface of the terrain to achieve the target terrain surface.
- Conventional control systems may be implemented in dozing machines for automatically adjusting the height of the blade to compensate for disturbances to the dozing machine as the dozing machine experiences the disturbances.
- conventional control systems have an inherent response delay time (e.g., approximately 100-300 milliseconds), which prevents the height of the blade from being timely adjusted to compensate for some disturbance.
- the terrain typically includes many disturbances such as ditches.
- a ditch will cause the dozing machine to pitch forward, thereby lowering the blade.
- conventional control systems are not able to timely adjust the height of the blade, the lowered blade will result in the creation of another ditch.
- the dozing machine will then travel over the created ditch, thereby creating another larger ditch. This problem will perpetuate as the dozing machine continues to travel over the terrain, resulting in instability and oscillations in the graded terrain.
- systems and methods for adjusting a height of an implement mounted on a body of a vehicle as the vehicle travels over a terrain are provided.
- Sensor data is received from a set of sensors disposed on the vehicle.
- a trajectory associated with the vehicle is determined based on the received sensor data.
- a profile of the terrain is estimated based on the determined trajectory associated with the vehicle.
- a ditch is detected in the terrain and compensation values for adjusting the height of the implement are determined based on the estimated profile of the terrain to compensate for the detected ditch.
- One or more control signals are transmitted to one or more actuators for adjusting the height of the implement based on the determined compensation values.
- the vehicle may be a dozer machine and the implement of the vehicle may be a blade.
- the trajectory associated with the vehicle is determined by determining a state of the body and a state of the implement based on the received sensor data, mapping the state of the body and the state of the implement to a one dimensional space to determine a state of the vehicle, and determining the trajectory associated with the vehicle based on the state of the vehicle.
- the state of the body and the state of the implement may be determined as a position and an orientation of the body and the implement and a linear velocity and an angular velocity associated with each axis of the position and the orientation of the body and the implement.
- the profile of the terrain is estimated by determining a pitch associated with the body based on the determined trajectory associated with the vehicle.
- the ditch is detected by computing a first derivative of the estimated profile of the terrain, determining that the first derivative of the estimated profile of the terrain for a current point is a zero crossing, and comparing a magnitude of the estimated profile of the terrain for the current point to a magnitude of the estimated profile of the terrain for a last point determined to be a zero crossing.
- the compensation values may be determined by determining a range of points between the current point and a last point determined to be a hump in the estimated profile of the terrain and determining the compensation value for each point in the range of points based on a shape characteristic and a differential magnitude of the estimated profile of the terrain at the current point and the last point determined to be the hump.
- the shape characteristic may include a step shape characteristic, a logarithmic shape characteristic, a quadratic shape characteristic, a ramp shape characteristic, an exponential shape characteristic, or a combination thereof.
- the one or more control signals are transmitted by combining initial error values for achieving a target terrain surface with the determined compensation values to generate final error values and generating the one or more control signals for adjusting the height of the implement according to the final error values.
- FIG. 1 shows an exemplary dozing machine, in accordance with one or more embodiments
- FIG. 2 shows an exemplary 2D environment in which a dozing machine operates, in accordance with one or more embodiments
- FIG. 3 shows a schematic diagram for a predictive control system for adjusting a height of a blade mounted on a body of a dozing machine for a 2D environment, in accordance with one or more embodiments;
- FIG. 4 shows a block diagram of a controller system for adjusting a height of a blade mounted on a body of a dozing machine for a 2D environment, in accordance with one or more embodiments;
- FIG. 5 shows a predictive control system for adjusting a height of an implement mounted on a body of a vehicle for a 3D environment, in accordance with one or more embodiments
- FIG. 6 shows a method for adjusting a height of an implement mounted on a body of a vehicle for a 3D environment, in accordance with one or more embodiments
- FIG. 7 shows an exemplary dozing machine, in accordance with one or more embodiments.
- FIG. 8 shows a schematic diagram illustrating a rigid body model of a vehicle, in accordance with one or more embodiments
- FIG. 9 shows a graph illustrating the detection of ditches in the estimated profile of the terrain, in accordance with one or more embodiments.
- FIG. 10 shows a diagram of various shape characteristics, in accordance with one or more embodiments.
- FIG. 11 shows a graph depicting compensation of ditches, in accordance with one or more embodiments
- FIG. 12 shows an exemplary schematic diagram of a vehicle, in accordance with one or more embodiments.
- FIG. 13 shows a graph illustrating body-to-body correlations and body-to-implement correlations of the vehicle, in accordance with one or more embodiments
- FIG. 14 shows an exemplary Smith predictor type controller, in accordance with one or more embodiments.
- FIG. 15 shows a high-level block diagram of a computer that may be used to implement one or more embodiments.
- Dozing machine 100 includes a body 102 and a blade 104 (or other suitable implement) pivotably coupled to body 102 via a boom arm (not shown).
- Dozing machine 100 also includes tracks 106 for travelling over a terrain 108 and may be operated autonomously or manually by an operator.
- dozing machine 100 may be utilized in a construction environment, agricultural environment, or any other environment to manipulate soil, sand, rubble, etc. on a surface of a terrain 108 via blade 104 to achieve a target terrain surface.
- dozing machine 100 may encounter various disturbances in the terrain 108 .
- a disturbance is a ditch.
- Dozing machine 100 operates in a 3D (three dimensional) environment and blade 104 has three degrees of freedom of rotational movement with respect to the body. Accordingly, the state of dozing machine 100 may be defined by the position of body 102 (in navigation frame 116 ), the orientation of body 102 (in navigation frame 116 ), and a relative orientation of blade 104 with respect to body 102 (in body frame 112 ).
- the position is defined in (X, Y, Z) Cartesian coordinates and the orientation is defined in (yaw, pitch, roll) Euler angles.
- the position and the orientation may be represented in any suitable format.
- the position may be represented as a 2D (two dimensional) curve lying on the surface of terrain 108 (which would require knowledge of terrain 108 ) and the orientation may be represented as rotation matrices, quaternions, etc.
- a dozing machine may be implemented with a conventional control system for adjusting the height of the blade.
- conventional control systems have an inherent response delay time, which prevents such conventional control systems from timely adjusting the height of the blade to compensate for ditches and other such disturbances.
- Embodiments described herein provide for a predictive control system to predict disturbances (e.g., due to a ditch) to body 102 due to disturbances in terrain 108 based on motion of blade 104 .
- the predictive control system comprises a controller 120 , a set of sensors 122 disposed on blade 104 and/or sensors 124 disposed on body 102 , and one or more actuators 118 coupled to body 102 and blade 104 .
- Controller 120 receives data from the set of sensors 122 and/or sensors 124 to predict disturbances to body 102 and generates control signals for controlling actuators 118 to adjust the height of blade 104 to compensate for the predicted disturbances.
- controller 120 predicts the disturbances on body 102 , thereby providing sufficient time to adjust blade 104 for compensating for ditches and other such disturbances.
- controller 120 predicts the prediction of disturbances to body 102 due to disturbances in terrain 108 , thereby providing sufficient time to adjust blade 104 for compensating for ditches and other such disturbances.
- the prediction of disturbances to body 102 due to disturbances in terrain 108 will first be described for a 2D environment with respect to FIGS. 2 - 4 before being described for a 3D environment.
- FIG. 2 shows an exemplary 2D environment 200 in which a dozing machine 202 operates, in accordance with one or more embodiments.
- Dozing machine 202 comprises body 206 and blade 208 .
- dozing machine 202 may be dozing machine 100 of FIG. 1 .
- dozing machine 202 operates in 2D environment 200 to manipulate the surface of terrain 204 .
- Body 206 of dozing machine 202 is parameterized by a height of body 206 and a pitch angle of body 206 with respect to a horizontal.
- Blade 208 of dozing machine 202 is parameterized by a blade pitch angle ⁇ 210 .
- the performance of dozing machine 202 is assessed by measuring the difference between the actual surface of terrain 204 and a target surface of terrain 204 .
- Dozing machine 202 is implemented with one or more sensors, one or more actuators, and a controller for automatically adjusting a height of a blade 208 , in accordance with one or more embodiments.
- FIG. 3 shows a schematic diagram 300 for a predictive control system for adjusting a height of a blade mounted on a body of a dozing machine for a 2D environment, in accordance with one or more embodiments.
- the dozing machine may be dozing machines 100 of FIG. 1 or dozing machine 202 of FIG. 2 .
- body attitude (position) of the vehicle is determined based on a set of sensors mounted on the body of the vehicle.
- the set of sensors for determining the body attitude may comprise a position sensor for determining the (X, Y) Cartesian coordinates of the body of the vehicle.
- body kinematics of the vehicle is determined based on the set of sensors mounted on the body of the vehicle and the body attitude (determined at step 304 ).
- the set of sensors for determining body kinematics may comprise an angular rotation sensor and an accelerometer.
- blade forward kinematics is determined based on the body attitude, the body kinematics, and a set of sensors mounted on the blade of the vehicle.
- blade attitude is determined based on the blade forward kinematics.
- inverse rotational kinematics of the blade is determined based on the blade attitude and the body attitude.
- path and trajectory planning is performed based on surface query results from surface engine 314 .
- blade inverse kinematics is determined based on the path and trajectory planning.
- joint to ram kinematics is determined from the blade inverse kinematics.
- the joint to ram kinematics is input into a controller (e.g., a proportional-integral-derivative controller), which outputs instructions to valves 322 for adjusting the height of the blade.
- FIG. 4 shows a block diagram of a controller system 400 for adjusting a height of a blade mounted on a body of a dozing machine for a 2D environment, in accordance with one or more embodiments.
- controller system 400 comprises controller 402 and plant 404 .
- Controller 402 receives data from sensors 406 mounted on the body and the blade of the dozing machine and estimates a state of the body and the blade by observers 408 .
- Observers 408 generate compensation values (referred to as blade z) 420 , which are combined with initial error values for generating the desired surface 410 to generate error 418 .
- Error 418 is input into low-level controller 412 , which sends commands to hydraulics and delay 414 for adjusting a height of the blade of machine to manipulate the ground 416 .
- FIG. 5 shows a schematic diagram 500 of a predictive control system for adjusting a height of an implement mounted on a body of a vehicle for a 3D environment, in accordance with one or more embodiments.
- FIG. 6 shows a method 600 for adjusting a height of an implement mounted on a body of a vehicle for a 3D environment, in accordance with one or more embodiments.
- FIG. 5 and FIG. 6 will be described together.
- the steps of method 600 of FIG. 6 may be performed by a controller (e.g., controller 120 of FIG. 1 ) or any other suitable computing device or devices, such as, e.g., computer 1102 of FIG. 11 .
- Exemplary controllers include simple controllers such as PID (proportional-integral-derivative) controllers or PLCs (programmable logic controllers) and more sophisticated controllers such as Smith Predictor or MPC (model predictive control).
- the vehicle may be initially travelling over a terrain to manipulate the terrain via the implement to achieve a target terrain surface.
- the height of the implement of the vehicle is adjusted according to initial error values to achieve the target terrain surface.
- adjusting the height of the implement according to the initial error values may not be able to compensate for some disturbances such as, e.g., ditches in the terrain.
- compensated values are determined for compensating for ditches and other disturbances in the terrain.
- the compensated values may be combined with the initial error values to generate final error values for adjusting the height of the implement to achieve the target terrain surface while also compensating for ditches and other disturbances in the terrain.
- sensor data is received from a set of sensors disposed on a vehicle.
- the vehicle may be any vehicle having an implement coupled to a body, such as, e.g., a construction vehicle (e.g., a dozing machine having a blade couple to a body or a compact track loader having a blade coupled to a body) or an agricultural vehicle (e.g., a combine harvester having a header coupled to a body).
- the vehicle is dozing machine 100 of FIG. 1 having blade 104 coupled to body 102 and sensors 122 and 124 .
- the vehicle may be dozing machine 502 and the set of sensors may be sensors 504 .
- the set of sensors may include any number of suitable sensor or sensors for determining the position and the orientation of the body and the implement of the vehicle, as well as the linear and angular velocities for each axis.
- the position is defined in (X, Y, Z) Cartesian coordinates and the orientation is defined in (yaw, pitch, roll) Euler angles.
- Exemplary sensors may include an IMU (inertial measurement unit), a GPS (global positioning system) sensor, an LPS (local positioning system) sensor, a sonic range finder, a laser range finder, an encoder, an in-ram pressure sensor, an odometer, or any other suitable sensor.
- the set of sensors may be disposed on the vehicle at any suitable location or locations for determining the position and the orientation of the body and the implement, as well as the linear and angular velocities for each axis.
- the set of sensors may comprise one or more sensors disposed on the implement and/or one or more sensors disposed on the body.
- the set of sensors is disposed on the vehicle in a mastless configuration, where two GPS sensors are mounted on the body, an IMU is mounted on the body, and an IMU is mounted on the implement.
- the GPS sensors mounted on the body form a main-auxiliary pair and run the RTK (real-time kinematic) algorithm.
- the vehicle is a dozer machine with a push bar
- an additional IMU may be mounted on the push bar.
- the set of sensors is disposed on the vehicle in a masted configuration, where one or two GPS sensors are mounted on the implement, an IMU is mounted on the implement, and optionally an IMU is mounted on the body.
- a trajectory associated with the vehicle is determined based on the received sensor data.
- the trajectory represents the location of a point associated with the vehicle as the vehicle travels over the terrain. The determination of the trajectory will be described with continued reference to FIG. 5 .
- observation (estimation) block 506 receives data from sensors 504 to estimate an observed state of the body 508 and state of the implement 510 .
- the state of the body and the state of the implement are defined in terms of 24 parameters, corresponding to the position (e.g., in X, Y, Z Cartesian coordinates) and orientation (e.g., in yaw, pitch, roll Euler angles) of the body and the implement, as well as the linear and angular velocities for each axis.
- dozing machine 100 operates in a three dimensional environment with various frames of reference, such as, e.g., body frame 114 of body 102 , blade frame 112 of blade 104 , surface patch frame 110 of terrain surface 108 , and navigation frame 116 of the navigation of dozing machine 100 .
- the state of the body 102 is defined in terms of its absolute position and orientation in body frame 114 (with respect to some reference point).
- the state of the blade 104 is defined in terms of its relative position and orientation in blade frame 104 (relative to body frame 114 ).
- the absolute position and orientation (in body frame 114 , with respect to the reference point) of blade 104 can be determined from its relative position and orientation.
- the state of the body and the state of the implement are determined using the following components:
- a dozing machine 700 comprising a body 702 and a blade 704 is shown travelling along dozer path 708 on a terrain, in accordance with one or more embodiments.
- the state of body 702 and blade 704 (in terms of position, orientation, and linear and angular velocities) are mapped to rectifying plane 706 , which is perpendicular to dozer path 708 .
- a goal of the control system is to move blade 704 such that the points of interest (as a function time) converge to the surface intersection (also a function of time) in finite time. More generally, the edge of blade 704 should intersect and lie on the surface patch (which is a spatial plane) that intersects the vertical ray extended from the POI.
- the navigation coordinate frame is denoted N( ⁇ right arrow over (X) ⁇ , ⁇ right arrow over (Y) ⁇ , ⁇ right arrow over (Z) ⁇ ).
- the center of the navigation coordinate frame is not important.
- the middle point of blade 704 is denoted m(x, y, z) in 3D space, expressed with respect to the navigation frame.
- the coordinate frame of the edge of blade 704 (centered at m) is denoted M( ⁇ right arrow over (x) ⁇ , ⁇ right arrow over (y) ⁇ , ⁇ right arrow over (z) ⁇ ), where unit direction ⁇ right arrow over (y) ⁇ points along the edge of blade 704 (positive towards right), ⁇ right arrow over (z) ⁇ points down along vertical when blade 704 is at the zero position (determined at calibration time), and ⁇ right arrow over (x) ⁇ points forward, orthogonal to the ⁇ right arrow over (y) ⁇ and ⁇ right arrow over (z) ⁇ .
- the point of interest is denoted p(x, y, z).
- the surface intersection point is denoted s(x, y, z).
- the surface frame centered at p is denoted S( ⁇ right arrow over (U) ⁇ , ⁇ right arrow over (V) ⁇ , ⁇ right arrow over (W) ⁇ ).
- the sagittal rectifying plane 706 is denoted P( ⁇ right arrow over ( ⁇ ) ⁇ , ⁇ right arrow over (j) ⁇ , ⁇ right arrow over (k) ⁇ ), where ⁇ right arrow over ( ⁇ ) ⁇ lies on the plane pointing to the front of the machine, ⁇ right arrow over (j) ⁇ is orthogonal to the plane, and ⁇ right arrow over (k) ⁇ is along the local gravity vector.
- the state of body 702 and blade 704 may be mapped to rectifying plane 706 as follows.
- a point of interest (the point-fit or best-fit point of interest) is selected by changing the ratio ⁇ .
- the point of interest is represented in the navigation frame to get p(x, y, z).
- the POI is projected in the vertical direction ⁇ right arrow over (Z) ⁇ to intersect a surface patch of the desired surface.
- the line segment L representing the edge of blade 704 , is projected onto the surface patch.
- point s should already be on this projection.
- the unit vector ⁇ right arrow over (l) ⁇ is assumed to be on line segment L.
- m is projected along ⁇ right arrow over (y) ⁇ onto P and the projected point is denoted m P .
- s is projected along ⁇ right arrow over (l) ⁇ onto P, where the projected point is denoted s P .
- m P and s P are added as a blade point and target surface point respectively to the trajectory.
- the frames for both points will be the same as P since they reside on the same plane. It is to be noted that yaw and roll of blade 704 will change the position of m P , however this change is much smaller than the change in m, helping the stability of the system.
- the trajectory associated with the vehicle is determined based on the state of the vehicle.
- state of the vehicle 516 is used by tracking point determination block 518 to determine a trajectory.
- the front (start) of the trajectory is a point representing the position of the implement and the tail (end) of the trajectory extends back to a predefined distance from the front.
- Each point may be located at any position associated with the vehicle. For example, each point may be located at a position determined based on the position of the body and/or the position of the implement.
- each point of the trajectory is determined based on its suitability for eliciting information about the shape of the ground. For example, if the points of the trajectory are taken as the tip of the implement, then it must be assumed that the implement is always in contact with the surface of the terrain, which might not always be true. If the implement pitches up, it will lose contact with the surface of the terrain and lead to fictitious humps and ditches.
- each point of the trajectory is determined based on a virtual point on the body. Such a virtual point is located to be under the blade tip, which is further away from the physical body of the vehicle. The virtual point can be thought of as the vertical projection of the tip of the implement down onto the body, had the body been long enough.
- each point of the trajectory is a point as representing the surface of the terrain instead of blade tip, which may be a good choice when the body only pitches up and down.
- each point of the trajectory can be determined by comparing the vertical heights of the tip of the implement and the virtual point on the body and selecting the point with the lowest vertical height. Where the implement loses contact with the surface of the terrain, the trajectory would still be a reasonable estimate of where the surface of the terrain is.
- each point of the trajectory may be determined to represent the surface of the terrain based on the amount by which the implement penetrates the target surface. This would be a good choice if the machine is close enough to the target surface, or, in other words, where the actual surface of the terrain is almost equal to the desired surface. However, for computational purposes, a small dither must be added to the point. Otherwise, the gradient of the point will be a constant value and not zero at inflection points.
- the trajectory is created in a volatile mode.
- points are added to the trajectory without any constraints. Accordingly, as time advances (regardless of whether the vehicle is moving or not), points are estimated and added. Depending on the rate of the system, points might, for example, be added approximately every 10 milliseconds. So, the points are always 10 ms apart in time. However, if the vehicle is static, the points will be forming a cloud centered at the mean of the added points. The width of the cloud is determined by the variances of the ensemble of filters in the system. The trajectory, however, has a maximum capacity. Once the maximum capacity is exceeded, the oldest point added at the end of the trajectory will be removed to make room for a new point.
- the point cloud will form a trail (referred to as a trajectory history).
- the trajectory will be of variable length as a function of the speed of the vehicle.
- the trajectory is created in a retained mode. In the retained mode, the trajectory has a predefined maximum length. Once the predefined maximum length is reached, as points are added to the trajectory at the front, points are dropped from the trajectory at the tail to maintain the predefined maximum length.
- Points are added to the trajectory spaced apart according to a predefined spacing distance.
- a time delay limit may be defined between two consecutive points for both the volatile mode and the retained mode. If the delay between two consecutive points exceeds the time delay limit, the trajectory will be reset.
- a profile of the terrain is estimated based on the determined trajectory associated with the vehicle.
- the trajectory associated with the vehicle roughly approximates the profile of the terrain, however is a very coarse indicator of the pitching behavior of the body of the vehicle.
- the trajectory may not be an accurate representation of the profile of the terrain where the vehicle encounters a narrow ditch, where the implement is not in contact with the terrain, or where there is oscillation of the implement.
- the profile of the terrain is estimated by simulating the pitch of the body as the vehicle travels over the terrain (as approximated by the trajectory).
- the pitch of the body provides a more accurate profile of the terrain as compared to the trajectory of the vehicle.
- the trajectory is used by body state prediction (simulation) block 520 to predict the profile of the terrain by simulating the pitch of the body.
- motion of the body is simulated using a rigid-body approach to determine the pitch of the body as the vehicle travels over the terrain.
- the vehicle is modelled as a rigid body characterized with a mass and a mass moment of inertia (which changes with changes in articulation geometry).
- Various sub-systems of the vehicle may be modeled in the rigid body model of the vehicle, such as, e.g., the body, the implement, the tracks, and the wheels.
- the sub-systems modelled in the rigid body model of the vehicle may also include the suspension system, track shoes, or any other sub-system of the vehicle.
- the interaction between the tracks and the terrain are modelled without taking into account the portions at the front and back of the tracks that curve up and do not interact with the terrain.
- FIG. 8 shows a schematic diagram 800 illustrating a rigid body model of a vehicle 802 , in accordance with one or more embodiments.
- Vehicle 802 comprises body 804 , implement 806 , and tracks 812 , and has a center of mass 810 .
- Tracks 812 are made of rubber and may therefore be modelled as an elastic medium that can only strain due to interaction with terrain 808 .
- road wheels of vehicle 804 are not modelled.
- tracks 812 are modelled as a rubber band using imaginary spring-damper pairs 814 between the upper and lower boundaries of the rubber band at locations where tracks 812 contact terrain 808 .
- Spring-damper pairs 814 each comprise spring k and damper b.
- Tracks 812 may be modelled using any number of spring-damper pairs 814 .
- Spring-damper pairs 814 stress under forces from terrain 808 (at the locations where tracks 812 contact terrain 808 ) and body 804 .
- spring-damper pairs 814 exert forces on body 804 , resulting in linear forces and rotational torques around center of mass 810 .
- These resultant forces and torques produce linear and angular accelerations, which can be calculated using Newtonian mechanics formulas to calculate the bounce and pitch dynamics of vehicle 802 . Integration of the linear and angular accelerations results in vertical speed and pitch angular speed of vehicle 802 .
- the springs k are nonlinear springs that model the elastic behavior of the rubber track 812 and pressure-sinkage behavior of terrain 808 . According to Bekker's equation, the pressure-sinkage relation is given by:
- k c b , k ⁇ , and n are empirical constant parameters of terrain 808 .
- the calculated sinkage y represents the force produced by terrain 808 (and applied to springs k).
- the exponent of y may potentially make the spring k nonlinear.
- a ditch is detected in the terrain and compensation values for adjusting the height of the implement are determined based on the estimated profile of the terrain to compensate for the detected ditch.
- ditch detection algorithm block 522 determines shape profiles (defining compensation values for ditches detected in the terrain) associated with the profile of the terrain.
- the profile of the terrain comprises a plurality of points.
- the ditch is first detected in the terrain by analyzing a current point in the estimated profile of the terrain.
- the current point in the estimated profile of the terrain is a point at a predetermined distance from the front of the estimated profile of the terrain.
- additional points are be added to the estimated profile of the terrain, advancing the current point forward.
- Each point in the estimated profile of the terrain is associated with a shape profile representing a shape characteristic of the terrain. Based on the analysis of the current point, the shape profile associated with the current point (and possibly other points) may be updated to define the shape characteristic.
- FIG. 9 shows a graph 900 illustrating the detection of ditches in the estimated profile of the terrain, in accordance with one or more embodiments.
- signal 902 represents the estimated profile b z (x) of the terrain.
- the detection of a ditch is performed by analyzing the current point in the estimated profile b z (x).
- a gradient ⁇ x b z (x) (first derivative) of the estimated profile b z (x) is first computed and smoothed.
- the gradient ⁇ x b z (x) of the estimated profile b z (x) is shown as signal 904 in graph 900 .
- a second gradient ⁇ x 2 b z (x) of the estimated profile b z (x) is then computed and smoothed.
- the second gradient ⁇ x 2 b z (x) of the estimated profile b z (x) is shown as signal 906 in graph 900 . It is then determined whether the gradient ⁇ x b z (x) of the current point of estimated profile b z (x) is a zero crossing. As shown in graph 900 , the gradient ⁇ x b z (x) of point 908 and point 910 are zero crossings.
- the current point is not a ditch or a hump in the terrain and the current point is discarded. If the gradient ⁇ x b z (x) of the current point of the estimated profile b z (x) is a zero crossing, it is determined whether the current point in the estimated profile b z (x) is a ditch or a hump in the terrain by comparing the magnitude of the estimated profile b z (x) at the current point with the magnitude of the estimated profile b z (x) at the last point at which the gradient ⁇ x b z (x) is a zero crossing.
- the magnitude of point 908 is compared with the magnitude of point 910 to determine that point 908 is a ditch.
- a ditch is an area between two consecutive zero crossings of the second gradient ⁇ x 2 b z (x) of the estimated profile b z (x) with a negative Z value.
- a hump is an area between two consecutive zero crossings of the second gradient ⁇ x 2 b z (x) of the estimated profile b z (x) with a negative Z value.
- Compensation values for adjusting the height of the implement are then determined based on the estimated profile of the terrain to compensate for the detected ditch.
- the estimated profile b z (x) is traversed back to the last point determined to be a hump (or to the end of the estimated profile b z (x) if a last point determined to be a hump does not exist).
- the differential magnitude of the estimated profile b z (x) at the current point and at the last point determined to be a hump is calculated and used to update the profile associated with the points between the current point and the last point.
- compensation values for adjusting the height of the implement may be determined based on the estimated profile of the terrain to compensate for the detected humps. If the current point is detected to be a hump, the estimated profile b z (x) is traversed back to the last point determined to be a ditch (or to the end of the estimated profile b z (x) if a last point determined to be a ditch does not exist). The differential magnitude of the estimated profile b z (x) at the current point and at the last point determined to be a ditch is calculated and used to update the profile associated with points between the current point and the last point.
- Each point in the estimated profile b z (x) associated with an updated profile is identified as being adjusted. Unadjusted points occur where the body of the vehicle is travelling too fast or the ditch is too wide. In these cases, the height of the implement is user defined.
- the shape profiles associated with points in the estimated profile b z (x) detected as being a ditch may be updated by selecting a shape characteristic of the terrain.
- the shape characteristics may represent any suitable shape.
- FIG. 10 shows a diagram 1000 of various shape characteristics for compensating profile of terrain 1010 , in accordance with one or more embodiments.
- Profile of terrain 1010 comprise a number of ditches.
- Various shape characteristics 1002 - 1008 may be selected to compensate for such ditches in profile of terrain 1010 .
- shape characteristics may include a step shape characteristic 1002 , a logarithmic shape characteristic 1004 , a quadratic shape characteristic 1006 , and a ramp characteristic 1008 .
- Shape characteristics 1002 - 1008 are shown in diagram 1000 in accordance with various differential magnitudes to compensate for ditches in profile of terrain 1010 .
- Other shape characteristics are also contemplated, such as, e.g., an exponential shape characteristic, complex shape characteristics concatenating two or more shape characteristics, etc. After applying shape characteristics 1002 - 1008 , profile of terrain 1010 would look substantially flat.
- the compensation value for each of point in the range of points is determined by combining the shape characteristic with the differential magnitudes.
- the shape characteristic may have a height ranging from 0 to 1 and may be overlaid over the range of points.
- the compensation value for each point may be determined by multiplying the height of the shape characteristic with the differential magnitude.
- FIG. 11 shows a graph 1100 depicting compensation of ditches, in accordance with one or more embodiments.
- Vehicle 1102 comprises body 1104 and implement 1106 .
- Body 1104 has center of mass 1108 and implement 1106 has a tip 1110 .
- a trajectory 1112 is created representing an estimated profile of the surface of the terrain.
- Signal 1114 shows the true body pitch of vehicle 1102 as it travels over the surface of the terrain.
- a correction profile 1116 comprising various shape characteristics is applied to compensate for ditches detected in trajectory 1112 .
- one or more control signals are transmitted to one or more actuators for adjusting the height of the implement based on the determined compensation values.
- the actuators may be actuators 118 in FIG. 1 .
- the actuators may be hydraulic cylinders or drives or any other suitable actuator.
- the profile of the terrain with associated shape profiles are used by low-level controller 526 to transmit one or more control signals to hydraulic system 530 of dozing machine 502 .
- the control signals comprise instructions for adjusting the height of the implement to achieve the target terrain surface while also compensating for ditches and other disturbances in the terrain.
- the initial error values for achieving the target terrain surface are combined (e.g., added) with the compensation values to generate final error values.
- the control signals are generated to comprise instructions for adjusting the height of the implement according to the final error values.
- a simple controller e.g., a PID controller
- Predictive controllers e.g., Smith Predictor, MPCs
- MPCs Smith Predictor
- the vehicle may be mathematically modelled. In FIG. 5 , the vehicle is mathematically modelled by black state predictor block 528 .
- FIG. 12 shows an exemplary schematic diagram 1200 of a vehicle, in accordance with one or more embodiments.
- vehicle 1202 comprises a body 1204 positioned at (x 1 , y 1 ) with center of mass 1206 .
- Center of mass 1206 of body 1204 travels over a surface 1208 of a terrain (represented as the body trajectory) with velocity v 1 .
- Body 1204 pitches around center of mass 1206 by angular rate ⁇ 1 producing body pitch ⁇ 1 .
- Rod 1210 extends from body 1204 and is linked to boom 1214 via pivot 1212 .
- Boom 1214 pitches around pivot 1212 by angular rate ⁇ 2 producing body pitch ⁇ 2 .
- End point 1216 of boom 1214 is configured to couple to a blade. End point 1216 has a position (x 2 , y 2 ). The blade is not important to the basic kinematics and will therefore be assumed to be composed of a point.
- ⁇ 1 is the only controlled variable. However, the control of ⁇ 1 is not direct, but is the output of the actuator.
- the actuator models can be identified as having a first or second order lag step response with a time delay. Such models can also be expressed in the state space and, together with the differential kinematic model of the body and implement, form the complete model for the system.
- a linear state space model for a typical dozing machine (including a CTL) will now be derived.
- This model is based on the kinematic structure of the vehicle.
- the vehicle will be modeled as being composed of rigid links connected to (and moving with respect to) each other via 1 degree of freedom joints.
- Links can have arbitrary shapes and are characterized by their location and orientation with respect to a reference frame.
- To each link is attached a coordinate frame.
- To each joint is attached a unit vector indicating motion along or around it, depending on the type of the joint. Sensor positions and other important points of the machine are considered to be part of some link. These points are expressed with respect to the link they are attached to.
- Links and joints together form open (or closed) chains or trees.
- Such a rigid-body model can be very detailed but only a sufficiently detailed one will be discussed here.
- the linear state space model will have the following links and joints in sequence:
- the kinematics-dynamics module in the proposed system has the following responsibilities:
- p Blade Nav denotes the position of the reference point (0,0,0) on the blade link expressed in the navigation frame. This same point, expressed in the blades own frame will be denoted by p Blade BLade (x, y, z) or simply p Blade (x, y, z). Other points on the blade are relative to this point such as m(x, y, z), the blade middle point.
- p Blade is m itself.
- R Blade Nav denotes the rotation matrix of the blade frame in navigation frame.
- R Blade Body denotes the rotation matrix from body frame to blade frame.
- R Blade Nav denotes the rotation matrix from blade to navigation frame.
- p Body Nav denotes an arbitrary point on the body but usually taken to be the center of mass.
- R Body Nav denotes the rotation matrix from body to navigation frame.
- the kinematic structure has 5 links connected by 4 joints. All the joints are rotary.
- ⁇ (t) ( ⁇ 1 , ⁇ 2 , ⁇ 3 , ⁇ 4 , ⁇ 5 ).
- ⁇ 2 (t), ⁇ 3 (t) and ⁇ 4 (t) are not independent of each other. ⁇ 3 (t) will change if ⁇ 2 (t) changes, and the same is true for ⁇ 4 (t). Change in ⁇ 3 (t) can be compensated by tilting the blade, however ⁇ 4 (t) is a characteristic of the vehicle and cannot be manipulated with actuators.
- ⁇ (t) Is an element of the joint space of the implement.
- m Body (x, y, z) blade middle point in body frame
- ⁇ (t) not true in general but true in our special kinematic structure.
- p Blade Body ( x ( t ), y ( t ), z ( t )) K p ( ⁇ ( t ))
- K p ( ⁇ (t)) is the kinematic mapping. This mapping can be constructed using robotics algorithms.
- K p ( ⁇ ) K ( ⁇ )*(0001) T
- ⁇ dot over (p) ⁇ Blade Body ( x,y,z ) J p ( ⁇ ( t ))* ⁇ dot over ( ⁇ ) ⁇ ( t )
- ⁇ dot over ( ⁇ ) ⁇ (t) is the rate of change of joint values.
- the forward kinematics model can now be states as
- [ p Blade Nav p ⁇ Blade Nav ] [ p Body Nav + R Body Nav * K ⁇ ( ⁇ ) p ⁇ Body Nav + R Body Nav * ( K ⁇ ( ⁇ ) ⁇ ⁇ Body ) + J ⁇ ( ⁇ ) * ⁇ ⁇ ]
- ⁇ Body is the angular rates of turn of the body around its axes.
- ⁇ 1 does not need to be estimated and can be easily determined using the extra IMU as an inclinometer.
- a dynamic model is used to compute body angular velocity of the body when in contact and interacting with the ground.
- the ground refers to the estimate of the surface of the terrain on which the body will be moving, in accordance with embodiments described herein.
- This ground is represented as a curve in 2D space (a profile). It has to be endowed with soil properties to make the interaction realistic.
- a real-time rigid-body approach may be used. It is assumed that a lumped model can be used, comprising a body with mass and moment of inertia, and a series of discrete spring-damper elements attached to and distributed along the track. Interaction can further be limited to the bottom of the track.
- the tabulated parameters of the soil can be used to compute an estimate of the stiffness coefficient of the springs. Damping is added to complement the model. Its value is estimated through experiments.
- Forward speed can, in principle, be found by modeling tractive force between the track and the ground but, in this case, it is known (observed) at the beginning of prediction and can be considered to remain constant during the prediction horizon.
- ⁇ dot over (x) ⁇ c ( t ) v
- the bounce dynamic equation of the body can be stated as
- the performance of a control system in accordance with method 600 may be assessed.
- the predicted body pitch (determined at step 606 ) is correlated with the actual body pitch after the body passes over the point where the implement was located.
- the correlate with body block 524 correlates the predicted body pitch with the actual body pitch.
- the assessment can be used to adjust the confidence in the validity of the compensation.
- the assessments may be output to a supervisory control box for automatically adjusting behaviors and parameters or a user for manual adjustment of behaviors and parameters. Two correlations are of interest: 1) body-to-body correlation and 2) body-to-implement correlation.
- the body-to-body correlation measures the predictability of the interaction between the tracks and the terrain.
- body pitch will be compared to the Z component of a virtual body point located a certain distance from the center of mass of the body. The certain distance is the same distance as the distance between the point at the center of mass of the body and a point at the edge of the implement. If the height of the virtual point corresponds (correlates) with the height of the point at the center of mass of the body (representing the pitch pattern of the body), there is a higher chance that the body will behave as expected when it reaches the location of the virtual point. A higher correlation indicates a higher confidence in the determined compensation values.
- the body-to-implement correlation measures the amount that the motion of the implement can predict motion of the body. This correlation is calculated by comparing pitch of the body with the Z component of a point on the implement. A higher body-to-implement correlation indicates oscillatory behavior of the implement. Thus, the control system should reduce the body-to-implement correlation.
- FIG. 13 shows a graph 1300 illustrating body-to-body correlations and body-to-implement correlations of the vehicle as the vehicle travels over the sinusoidal terrain, in accordance with one or more embodiments.
- Signal 1302 represents the pitch of the body, which is sinusoidal due to the sinusoidal terrain.
- Signal 1306 represents corrections of the height of the implement, determined in accordance with embodiments described herein. The corrections were applied as a step shape characteristic.
- Signal 1304 represents the position of the implement adjusted according to the corrections. As shown in graph 1300 , the position of the implement (signal 1304 ) tracks the corrections (signal 1306 ) fairly well. Accordingly, the body-body correlation (signal 1310 ) is high and the body-implement correlation (signal 1308 ) is low, which indicates good performance. For a well-performing system, the implement is perfectly out of synch with the pitch of the body.
- the lack of confidence of the correlations may be computed as follows
- ⁇ ⁇ ( t ) ⁇ soil ⁇ ⁇ bb ⁇ bu
- ⁇ ub and ⁇ bb represent body-body correlation and body-implement correlation respectively
- ⁇ soil represents softness of the terrain. The softer the terrain, the less chance that the body exhibits expected behavior. In other words, the softer terrain may result in the terrain changing shape and/or the position of the implement not representing the profile of the terrain. In such a situation, the strength of the corrections should be decreased.
- Method 600 may be performed by any suitable controller.
- the controller is a low-level controller.
- the low-level controller ensures that the edge of the implement tracks the surface of the terrain according to the target surface.
- the problem was reduced to that of convergence and tracking of one point on the plane s(x, z) (target level) by another p(x, z) (projection of blade edge on body sagittal). This is called elevation (mainfall) control for dozers.
- the controller must also make sure that the whole edge is on the desired cross-slope (as dictated by the surface).
- the mainfall and cross-slope errors are denoted e mainfall and e cross-slope , respectively.
- Cross-slope control can be thought of as being orthogonal to mainfall control and can be done independently from the other.
- the actuators effecting these two can be actuated separately but there is coupling between them due to the hydraulic system. It is, then, beneficial to couple them to achieve smooth tracking and avoid setpoint chasing.
- K elevation is the gain of the elevation controller.
- the controller should be able to cope with output delay in plants.
- One suitable controller is a specially tuned PID controller.
- a better solution would be a Smith predictor type controller.
- An exemplary Smith predictor type controller 1400 is shown in FIG. 14 .
- Smith predictor type controller 1400 converts the elevation error to pitch error.
- a MPC (model predictive control) controller may be utilized.
- the dynamic model derived above will form the backbone of the MPC controller.
- the MPC controller uses the model to predict the future behavior of the system as a whole within a bounded horizon.
- the scope of the embodiments described herein is more general and may be easily applied to systems equipped with other means of determining the profile of the surface of the terrain.
- the profile of the surface of the terrain may additionally or alternatively be determined using an optical sensor (e.g., laser sensor) directed towards the surface of the terrain, possible mounted on the boom.
- an optical sensor e.g., laser sensor
- the point may be computed using a kinematic formula based on body position and orientation, boom pitch angle, location of the optical sensor on the boom, and the distance measured by the optical sensor.
- Systems, apparatuses, and methods described herein may be implemented using digital circuitry, or using one or more computers using well-known computer processors, memory units, storage devices, computer software, and other components.
- a computer includes a processor for executing instructions and one or more memories for storing instructions and data.
- a computer may also include, or be coupled to, one or more mass storage devices, such as one or more magnetic disks, internal hard disks and removable disks, magneto-optical disks, optical disks, etc.
- Systems, apparatus, and methods described herein may be implemented using a computer program product tangibly embodied in an information carrier, e.g., in a non-transitory machine-readable storage device, for execution by a programmable processor; and the method and workflow steps described herein, including one or more of the steps or functions of FIGS. 3 and 5 - 6 , may be implemented using one or more computer programs that are executable by such a processor.
- a computer program is a set of computer program instructions that can be used, directly or indirectly, in a computer to perform a certain activity or bring about a certain result.
- a computer program can be written in any form of programming language, including compiled or interpreted languages, and it can be deployed in any form, including as a stand-alone program or as a module, component, subroutine, or other unit suitable for use in a computing environment.
- FIG. 15 A high-level block diagram of an example computer 1502 that may be used to implement systems, apparatus, and methods described herein is depicted in FIG. 15 . Any or all of the systems and apparatus discussed herein, including controller 120 of FIG. 1 , controller 400 of FIG. 4 , and controller 1400 of FIG. 14 , may be implemented using one or more computers such as computer 1502 .
- Computer 1502 includes a processor 1504 operatively coupled to a data storage device 1512 and a memory 1510 .
- Processor 1504 controls the overall operation of computer 1502 by executing computer program instructions that define such operations.
- the computer program instructions may be stored in data storage device 1512 , or other computer readable medium, and loaded into memory 1510 when execution of the computer program instructions is desired.
- FIGS. 3 and 5 - 6 can be defined by the computer program instructions stored in memory 1510 and/or data storage device 1512 and controlled by processor 1504 executing the computer program instructions.
- the computer program instructions can be implemented as computer executable code programmed by one skilled in the art to perform the method and workflow steps or functions of FIGS. 3 and 5 - 6 .
- the processor 1504 executes the method and workflow steps or functions of FIGS. 3 and 5 - 6 .
- Computer 1504 may also include one or more network interfaces 1506 for communicating with other devices via a network.
- Computer 1502 may also include one or more input/output devices 1508 that enable user interaction with computer 1502 (e.g., display, keyboard, mouse, speakers, buttons, etc.).
- Processor 1504 may include both general and special purpose microprocessors, and may be the sole processor or one of multiple processors of computer 1502 .
- Processor 1504 may include one or more central processing units (CPUs), for example.
- CPUs central processing units
- Processor 1504 , data storage device 1512 , and/or memory 1510 may include, be supplemented by, or incorporated in, one or more application-specific integrated circuits (ASICs) and/or one or more field programmable gate arrays (FPGAs).
- ASICs application-specific integrated circuits
- FPGAs field programmable gate arrays
- Data storage device 1512 and memory 1510 each include a tangible non-transitory computer readable storage medium.
- Data storage device 1512 , and memory 1510 may each include high-speed random access memory, such as dynamic random access memory (DRAM), static random access memory (SRAM), double data rate synchronous dynamic random access memory (DDR RAM), or other random access solid state memory devices, and may include non-volatile memory, such as one or more magnetic disk storage devices such as internal hard disks and removable disks, magneto-optical disk storage devices, optical disk storage devices, flash memory devices, semiconductor memory devices, such as erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), compact disc read-only memory (CD-ROM), digital versatile disc read-only memory (DVD-ROM) disks, or other non-volatile solid state storage devices.
- DRAM dynamic random access memory
- SRAM static random access memory
- DDR RAM double data rate synchronous dynamic random access memory
- non-volatile memory such as
- Input/output devices 1508 may include peripherals, such as a printer, scanner, display screen, etc.
- input/output devices 1508 may include a display device such as a cathode ray tube (CRT) or liquid crystal display (LCD) monitor for displaying information to the user, a keyboard, and a pointing device such as a mouse or a trackball by which the user can provide input to computer 1502 .
- display device such as a cathode ray tube (CRT) or liquid crystal display (LCD) monitor for displaying information to the user
- keyboard such as a keyboard
- pointing device such as a mouse or a trackball by which the user can provide input to computer 1502 .
- FIG. 15 is a high level representation of some of the components of such a computer for illustrative purposes.
Abstract
Description
-
- (1) an extended Kalman filter for determining the position and the linear and angular velocities of the body based on: a) the accelerometer output from the IMU mounted on the body, b) the position output from the main GPS mounted on the body, and c) the body orientation (output from the extended Kalman filter in (2));
- (2) an extended Kalman filter for determining the orientation of the body based on: a) the gyroscope output from the IMU mounted on the body, b) the baseline (the vector from the auxiliary GPS to the main GPS) output from the auxiliary GPS mounted on the body, and c) the gravity vector (estimated by the body acceleration computer in (3));
- (3) a body acceleration computer for estimating the gravity vector at the location of the IMU mounted on the body based on: a) the body velocity (output from the extended Kalman filter in (1)) and b) the gyroscope output from the IMU mounted on the body;
- (4) an extended Kalman filter for determining the orientation of the implement based on: a) the gyroscope output from the IMU mounted on the implement and b) the gravity vector (estimated by the implement acceleration computer in (5));
- (5) an implement acceleration computer for estimating the gravity vector at the location of the IMU mounted on the implement based on the blade velocity (computed by the blade kinematics module in (6)); and
- (6) a blade kinematics module for computing the position and the orientation of the implement relative to the body, the linear and angular velocities of the implement, and the angles and rates of the joints of the kinematic structure based on: a) the position and the linear and angular velocities of the body (determined by the extended Kalman filter in (1)) and the orientation of the body (determined by the extended Kalman filter in (2)), b) the orientation and the angular velocities of the implement (determined by the extended Kalman filter in (4)), and c) the kinematic structure of the vehicle (e.g., the skeleton or blueprint of the vehicle). The kinematic structure of the vehicle refers to the number of joints of the vehicle and the way they are positioned with respect to each other. The kinematic structure of the vehicle is further described with respect to
FIG. 12 below.
-
- (1) the middle point of
blade 704, defined as being located approximately half-way from both edges ofblade 704. The blade middle point is typically the point that determines the elevation ofblade 704 with respect to the desired surface when the blade is square (zero yaw); - (2) the edge points of
blade 704. For example, the right edge can be used as a reference for measuring up the machine and surveying the locations of sensors. Such an edge can be designated as point (0,0,0); - (3) the point-fit point of interest (POI), defined as an user selected point in between and including the edges of
blade 704. By selecting a particular point, through defining a ratio (from 0 to 1, where 0 indicating one edge and 1 the other), users indicates their preference for a particular area of the desired surface ofblade 704. In other words, the surface patch directly beneath the POI will be used as the basis of calculating mainfall and cross slope errors. In one embodiment, the default value of the ratio is 0.5 representing the middle point; and - (4) the best-fit point of interest, which is the same as a point-fit POI except that it is determined by the control system. In best-fit operation mode, the surface patch selected is based on various factors including which one of the surface patches has biggest overlap with the edge or which one is closest to the blade edge. Typically, the selected best-fit POI is either of the edge points.
- (1) the middle point of
p(x,y,z)=m+α*{right arrow over (y)}
Second, the POI is projected in the vertical direction {right arrow over (Z)} to intersect a surface patch of the desired surface. Third, the line segment L, representing the edge of
where p is pressure, y is the amount of sinkage, b is the radius of the contact area of
kϕ, and n are empirical constant parameters of
{dot over (x)} 1 =v 1 cos(θ1)
{dot over (y)} 1 =v 1 sin(θ1)
x 2 =x 1 +l 1 cos(θ1)+l 2 cos(θ1+θ2)
y 2 =y 1 +l 1 sin(θ1)+l 2 sin(θ1+θ2)
{dot over (x)} 2 ={dot over (x)} 1 −l 1ω1 sin(θ1)−l 2(ω1+ω2)sin(θ1+θ2)
{dot over (y)} 2 ={dot over (y)} 1 +l 1ω1 cos(θ1)+l 2(ω1+ω2)cos(θ1+θ2)
{dot over (x)} 2 =v 1 cos(θ1)−l 1ω1 sin(θ1)−l 2(ω1+ω2)sin(θ1+θ2)
{dot over (y)} 2 =v 1 sin(θ1)+l 1ω1 cos(θ1)+l 2(ω1+ω2)cos(θ1+θ2)
The state-space model for the schematic diagram 1200 is given as follows:
{dot over (x)} 1 =v 1 cos(x 3)−l 1ω1 sin(x 3)−l 2(ω1+ω2)sin(x 3 +x 4)
{dot over (x)} 2 =v 1 sin(x 3)+l 1ω1 cos(x 3)+l 2(ω1+ω2)cos(x 3 +x 4)
where the state is {right arrow over (x)}=(x1, x2, x3, x4)T=(x2, y2, θ1, θ2)T, the input is {right arrow over (u)}=(v1, ω1, ω2)T, the reference input is {right arrow over (ud)}=(v1, ω1, 0)T, and xd=x2=y2=0 gives the reference trajectory. ω1 is the only controlled variable. However, the control of ω1 is not direct, but is the output of the actuator. In one embodiment, the actuator models can be identified as having a first or second order lag step response with a time delay. Such models can also be expressed in the state space and, together with the differential kinematic model of the body and implement, form the complete model for the system.
-
- (1) The vehicle body is the first link in the chain. An IMU and two GPS antennas are rigidly attached to it. It is in contact with the ground and its position and orientation are determined through complex interactions with the soil. Such a link is called mobile and its motion is not determined by motion of the joints (at least not directly).
- (2) The boom (push arm) link is attached to the body via the pivot joint. This link's motion is constrained by the pivot joint. An IMU can be installed on this link. This is the main mechanism for changing the elevation of the blade.
- (3) The blade is attached to the boom through a spherical joint (called the blade center of rotation). This joint is modelled as being composed of three 1 degree of freedom joints occupying the same position. The first two links connecting the three (virtual) joints are modeled as having zero length (so, they are just imaginary links). The third link is the blade itself. A number of sensors can be attached to the blade, including, e.g., an IMU, one or two GPS antennas, sonic sensors, etc. Moreover, the most important entity on the blade link is the middle point and its coordinate frame. The blade can go through yaw, pitch and roll motions around the center of rotation.
-
- (1) Body dynamics: determine the position, orientation, linear and angular velocities of the body link using a dynamic model of track-soil interaction. Note that this information is already estimated by the observer module using GPS and inertial sensors on the body. This modeling is used for prediction.
- (2) Blade inverse kinematics: given location, orientation, and angular rates of the body as well as orientation and angular rates of the blade, determine the values and rates of all the joints connecting the body to the blade.
- (3) Blade forward kinematics: given position, orientation, linear and angular velocities of the body, as well as joint values and rates, determine blade location, linear velocity and acceleration.
- (4) Blade dynamics: together with body dynamics, simulate (predict) blade motion using actuator models for valves-cylinders that move the blade with respect to the body.
-
- (1) θ1(t) is the angle (i.e., the pitch) of the boom with respect to an initial reference (typically the angle it makes with the flat surface on which the body is located when the blade edge is resting on the surface). This pitching happens around the {right arrow over (y)} direction of the body frame.
- (2) θ2(t) is the rotation (i.e., the yaw) of the blade around a vector perpendicular to the boom.
- (3) θ3(t) is the tilt (i.e., roll) of the blade around the forward direction ({right arrow over (x)}) of the yawed blade.
- (4) θ4(t) is an additional pitch around the {right arrow over (y)} direction of the yawed and rolled blade.
p Blade Body(x(t),y(t),z(t))=K p(Θ(t))
where Kp(Θ(t)) is the kinematic mapping. This mapping can be constructed using robotics algorithms. Briefly, it involves constructing special 4 by 4 matrices (homogeneous transformations) Ai(θi) for each link-joint, multiplying them together and then extracting the position part:
K p(Θ)=K(Θ)*(0001)T
where
K(Θ)=A 1(θ1)*A 2(θ2)*A 3(θ3)*A 4(θ4)*A 5(θ5)
is the homogeneous kinematics matrix, such that
Also,
{dot over (p)} Blade Body(x,y,z)=J p(Θ(t))*{dot over (Θ)}(t)
where Jp(Θ(t))=∂Kp(Θ)/∂Θ is the Jacobian matrix and {dot over (Θ)}(t) is the rate of change of joint values. More generally,
where ωBody is the angular rates of turn of the body around its axes.
R=R Blade Body =R Body Nav
which, when expanded, becomes:
Thus, the angles are easily computed as
{dot over (Θ)}(t)=J p(Θ(t))−1ωBlade Body =J p(Θ(t))−1(ωBlade−ωBody)
The rates for computing joint rates are measured by the IMUs on the body, boom, and blade. However, when the above model is used for predictive control, these rates are the output of the dynamic model of the actuators. A full hydraulic model is too cumbersome and also inaccurate if necessary sensors are not installed, which is normally the case in practice. However, simplified models for the dynamics of hydraulic valves can be easily constructed. Such a model would receive a valve command as input and will output the resultant speed of extension-retraction. In a dozer, the change in length of different cylinders ultimately leads to rotary motion of links around the joints. Thus, the hydraulic models can be thought of as mappings between valve command and joint angular rate. Boom pitch and blade tilt are the only controllable joints resulting in
ω={dot over (θ)}=T(c)
where T( ) denotes the transfer function and cis valve command. The transfer function can be experimentally identified. A second order transfer function with delay
has been shown to be more than sufficient for describing the relationship between a command sent to the valve and the measured angular velocity. The measurement can be done using the onboard IMUs.
{dot over (x)} c(t)=v
and where θ(t) is the pitch of the body.
where
τi(t)=l i f i cos(ϕi)cos(θ)
where σub and σbb represent body-body correlation and body-implement correlation respectively and σsoil represents softness of the terrain. The softer the terrain, the less chance that the body exhibits expected behavior. In other words, the softer terrain may result in the terrain changing shape and/or the position of the implement not representing the profile of the terrain. In such a situation, the strength of the corrections should be decreased.
K elevation =Ke (−λe
can make sure that the desired cross-slope has been adequately achieved before attempting elevation (boom pitch) adjustment with full force. Here, Kelevation is the gain of the elevation controller.
Claims (28)
Priority Applications (5)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
US17/125,967 US11898321B2 (en) | 2020-12-17 | 2020-12-17 | Input shaping for error detection and recovery in dynamically agile grading machines |
PCT/US2021/071792 WO2022133363A1 (en) | 2020-12-17 | 2021-10-08 | Input shaping for error detection and recovery in dynamically agile grading machines |
CN202180076493.5A CN116635597A (en) | 2020-12-17 | 2021-10-08 | Input shaping for error detection and recovery in dynamic agile planishing mill |
JP2023537228A JP2024500130A (en) | 2020-12-17 | 2021-10-08 | Input shaping for error detection and error recovery in dynamically agile earth tillers |
EP21807478.9A EP4263955A1 (en) | 2020-12-17 | 2021-10-08 | Input shaping for error detection and recovery in dynamically agile grading machines |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
US17/125,967 US11898321B2 (en) | 2020-12-17 | 2020-12-17 | Input shaping for error detection and recovery in dynamically agile grading machines |
Publications (2)
Publication Number | Publication Date |
---|---|
US20220195693A1 US20220195693A1 (en) | 2022-06-23 |
US11898321B2 true US11898321B2 (en) | 2024-02-13 |
Family
ID=78622129
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
US17/125,967 Active 2042-07-01 US11898321B2 (en) | 2020-12-17 | 2020-12-17 | Input shaping for error detection and recovery in dynamically agile grading machines |
Country Status (5)
Country | Link |
---|---|
US (1) | US11898321B2 (en) |
EP (1) | EP4263955A1 (en) |
JP (1) | JP2024500130A (en) |
CN (1) | CN116635597A (en) |
WO (1) | WO2022133363A1 (en) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11834813B2 (en) * | 2021-07-09 | 2023-12-05 | Topcon Positioning Systems, Inc. | IMU based system for vertical axis joint angle estimation for swing boom excavators |
Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6904979B1 (en) * | 2003-09-16 | 2005-06-14 | Sekely Industries, Inc. | Towable earth working apparatus having adjustable wheel height |
EP2586918A2 (en) | 2010-06-23 | 2013-05-01 | Doosan Infracore Co., Ltd. | Apparatus and method for controlling work trajectory of construction equipment |
US20130277943A1 (en) * | 2012-04-19 | 2013-10-24 | Keith W. Wendte | Automatic Tractor Hitch System For Trailing Implements |
US20140019015A1 (en) | 2012-07-16 | 2014-01-16 | Flanders Electric Motor Service, Inc. | Payload management system |
US20140343800A1 (en) * | 2013-05-15 | 2014-11-20 | Deere & Company | Method for controlling an implement associated with a vehicle |
US20180038068A1 (en) | 2016-08-02 | 2018-02-08 | Komatsu Ltd. | Control system for work vehicle, control method, and work vehicle |
US20190218747A1 (en) * | 2017-03-31 | 2019-07-18 | Komatsu Ltd. | Control system for work vehicle, and method for setting trajectory of work implement |
US20190234045A1 (en) | 2018-01-30 | 2019-08-01 | Caterpillar Trimble Control Technologies Llc | Grading mode integration |
US20200105072A1 (en) * | 2016-12-23 | 2020-04-02 | Caterpillar Sarl | Monitoring The Operation Of A Work Machine |
EP3660231A1 (en) | 2018-11-08 | 2020-06-03 | Intsite Ltd | System and method for autonomous operation of heavy machinery |
US20210340735A1 (en) * | 2020-04-30 | 2021-11-04 | Deere & Company | Ground engaging tool control system and method |
US20220151133A1 (en) * | 2020-11-19 | 2022-05-19 | Deere & Company | Automated implement height or depth control for terrain |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004286724A (en) * | 2003-01-27 | 2004-10-14 | Denso Corp | Vehicle behavior detector, on-vehicle processing system, detection information calibrator and on-vehicle processor |
US9995019B2 (en) * | 2014-06-23 | 2018-06-12 | Topcon Positioning Systems, Inc. | Estimation with gyros of the relative attitude between a vehicle body and an implement operably coupled to the vehicle body |
US9796571B2 (en) * | 2015-08-06 | 2017-10-24 | Cnh Industrial America Llc | Work vehicle with improved implement position control and self-leveling functionality |
US10494789B2 (en) * | 2016-01-06 | 2019-12-03 | Cnh Industrial America Llc | System and method for autonomous steering control of work vehicles |
US10066367B1 (en) * | 2016-06-20 | 2018-09-04 | Robo Industries, Inc. | System for determining autonomous adjustments to an implement position and angle |
US10521703B2 (en) * | 2017-06-21 | 2019-12-31 | Caterpillar Inc. | System and method for controlling machine pose using sensor fusion |
-
2020
- 2020-12-17 US US17/125,967 patent/US11898321B2/en active Active
-
2021
- 2021-10-08 CN CN202180076493.5A patent/CN116635597A/en active Pending
- 2021-10-08 EP EP21807478.9A patent/EP4263955A1/en active Pending
- 2021-10-08 WO PCT/US2021/071792 patent/WO2022133363A1/en active Application Filing
- 2021-10-08 JP JP2023537228A patent/JP2024500130A/en active Pending
Patent Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6904979B1 (en) * | 2003-09-16 | 2005-06-14 | Sekely Industries, Inc. | Towable earth working apparatus having adjustable wheel height |
EP2586918A2 (en) | 2010-06-23 | 2013-05-01 | Doosan Infracore Co., Ltd. | Apparatus and method for controlling work trajectory of construction equipment |
US20130277943A1 (en) * | 2012-04-19 | 2013-10-24 | Keith W. Wendte | Automatic Tractor Hitch System For Trailing Implements |
US20140019015A1 (en) | 2012-07-16 | 2014-01-16 | Flanders Electric Motor Service, Inc. | Payload management system |
US20140343800A1 (en) * | 2013-05-15 | 2014-11-20 | Deere & Company | Method for controlling an implement associated with a vehicle |
US20180038068A1 (en) | 2016-08-02 | 2018-02-08 | Komatsu Ltd. | Control system for work vehicle, control method, and work vehicle |
US20200105072A1 (en) * | 2016-12-23 | 2020-04-02 | Caterpillar Sarl | Monitoring The Operation Of A Work Machine |
US20190218747A1 (en) * | 2017-03-31 | 2019-07-18 | Komatsu Ltd. | Control system for work vehicle, and method for setting trajectory of work implement |
US20190234045A1 (en) | 2018-01-30 | 2019-08-01 | Caterpillar Trimble Control Technologies Llc | Grading mode integration |
EP3660231A1 (en) | 2018-11-08 | 2020-06-03 | Intsite Ltd | System and method for autonomous operation of heavy machinery |
US20210340735A1 (en) * | 2020-04-30 | 2021-11-04 | Deere & Company | Ground engaging tool control system and method |
US20220151133A1 (en) * | 2020-11-19 | 2022-05-19 | Deere & Company | Automated implement height or depth control for terrain |
Non-Patent Citations (1)
Title |
---|
International Search Report and Written Opinion dated Feb. 10, 2022, in connection with International Patent Application No. PCT/US2021/071792, filed Oct. 8, 2021, 14 pgs. |
Also Published As
Publication number | Publication date |
---|---|
US20220195693A1 (en) | 2022-06-23 |
EP4263955A1 (en) | 2023-10-25 |
JP2024500130A (en) | 2024-01-04 |
CN116635597A (en) | 2023-08-22 |
WO2022133363A1 (en) | 2022-06-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109101032B (en) | System and method for controlling machine pose using sensor fusion | |
CN109115213B (en) | System and method for determining machine state using sensor fusion | |
RU2565597C2 (en) | Orientation assessment method, equipment and computer programme medium | |
CN107532897B (en) | Method for providing compensation for kinematic accelerations and system for compensating measurements | |
JP7042846B2 (en) | Blade control below design | |
US8781624B2 (en) | Systems and methods for tracking and balancing robots for imitating motion capture data | |
CN109099033B (en) | Method and system for controlling fluid pressure in a machine using sensor fusion feedback | |
US10494789B2 (en) | System and method for autonomous steering control of work vehicles | |
US11530527B2 (en) | Excavation by way of an unmanned vehicle | |
CN111936706B (en) | Flat mode integration | |
US11085170B2 (en) | Method of operating a machine comprising an implement | |
US9580104B2 (en) | Terrain-based machine comprising implement state estimator | |
Palomba et al. | Estimation of the digging and payload forces in excavators by means of state observers | |
AU2015305864B2 (en) | Earthmoving machine comprising weighted state estimator | |
US11898321B2 (en) | Input shaping for error detection and recovery in dynamically agile grading machines | |
Lee et al. | Blade control in Cartesian space for leveling work by bulldozer | |
US20220298743A1 (en) | Method for automatically controlling cyclical operations of an earthmoving machine | |
US20220325502A1 (en) | Methods and systems for determining machine state | |
WO2023283561A1 (en) | An imu based system for vertical axis joint angle estimation for swing boom excavators | |
US20230359209A1 (en) | Stability system for an articulated machine | |
US20230359203A1 (en) | Stability system for an articulated machine in a coasting mode | |
CN115390580A (en) | Ground-imitating flight method and device, unmanned equipment and storage medium |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
AS | Assignment |
Owner name: TOPCON POSITIONING SYSTEMS, INC., CALIFORNIA Free format text: ASSIGNMENT OF ASSIGNORS INTEREST;ASSIGNORS:KALANTAR, SHAHAB;POPLAVSKIY, DMYTRO;SIGNING DATES FROM 20201215 TO 20201217;REEL/FRAME:054687/0768 |
|
FEPP | Fee payment procedure |
Free format text: ENTITY STATUS SET TO UNDISCOUNTED (ORIGINAL EVENT CODE: BIG.); ENTITY STATUS OF PATENT OWNER: LARGE ENTITY |
|
STPP | Information on status: patent application and granting procedure in general |
Free format text: DOCKETED NEW CASE - READY FOR EXAMINATION |
|
STPP | Information on status: patent application and granting procedure in general |
Free format text: NON FINAL ACTION MAILED |
|
STPP | Information on status: patent application and granting procedure in general |
Free format text: RESPONSE TO NON-FINAL OFFICE ACTION ENTERED AND FORWARDED TO EXAMINER |
|
STPP | Information on status: patent application and granting procedure in general |
Free format text: NOTICE OF ALLOWANCE MAILED -- APPLICATION RECEIVED IN OFFICE OF PUBLICATIONS |
|
STPP | Information on status: patent application and granting procedure in general |
Free format text: PUBLICATIONS -- ISSUE FEE PAYMENT VERIFIED |
|
STCF | Information on status: patent grant |
Free format text: PATENTED CASE |