Input shaping for error detection and recovery in dynamically agile soil graders.

Abstract

A system and method are provided for adjusting the height of an implement (104) mounted on a body (102) of a vehicle (100) as the vehicle (100) travels over a terrain (108). Sensor data is received from a set of sensors (122, 124) 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. Grooves in the terrain are detected, and compensation values ​​for adjusting the implement height are determined based on the estimated profile of the terrain to correct for the detected grooves. One or more control signals are sent to one or more actuators (118) to adjust the implement height based on the determined compensation values.

Description

[Technical Field]

[0001] The present invention relates generally to control systems for adjusting the implement height of a vehicle, and more particularly to input shaping for error detection and recovery in dynamically agile grading machines. [Background technology]

[0002] Land grading is the process of manipulating the surface of a terrain to achieve a target terrain surface. Land grading is an important process in many different applications. For example, in construction, grading may be performed to set the foundation of a building. In agriculture, grading may be performed to direct surface water runoff. Bulldozers are typically used to level terrain. A bulldozer includes a body and a blade coupled to the body. In operation, as the bulldozer travels over the terrain, the height of the blade is adjusted to manipulate the surface of the terrain to achieve the target terrain surface.

[0003] Conventional control systems can be implemented within bulldozers to automatically adjust the blade height to compensate for disturbances to the bulldozer when the bulldozer experiences such disturbances. However, conventional control systems have an inherent response delay time (e.g., approximately 100 to 300 milliseconds), thus preventing the blade height from being adjusted in a timely manner to compensate for some disturbances. For example, terrain typically contains many disturbances, such as grooves. As the bulldozer travels over the terrain, the grooves cause the bulldozer to pitch forward, thereby lowering the blade. Because conventional control systems are unable to adjust the blade height in a timely manner, the lowered blade results in the creation of another groove. The bulldozer then drives over the created groove, thereby creating another, larger groove. This problem becomes perpetual as the bulldozer continues to travel over the terrain, resulting in instability and vibration in the graded terrain. Summary of the Invention [Means for solving the problem]

[0004] According to one or more embodiments, a system and method are provided for adjusting the height of an implement mounted on a body of a vehicle as the vehicle travels over terrain. Sensor data is received from a set of sensors located 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. Grooves in the terrain are detected, and compensation values ​​for adjusting the implement height are determined based on the estimated profile of the terrain to correct for the detected grooves. One or more control signals are sent to one or more actuators to adjust the implement height based on the determined compensation values. The vehicle may be a bulldozer, and the implement on the vehicle may be a blade.

[0005] In one embodiment, a trajectory relative to the vehicle is determined by determining a body state and an equipment state based on received sensor data, mapping the body state and the equipment state into a one-dimensional space to determine the vehicle state, and determining a trajectory relative to the vehicle based on the vehicle state. The body state and the equipment state may be determined in terms of body and equipment position and orientation, and linear and angular velocities associated with each axis of body and equipment position and orientation.

[0006] In one embodiment, the terrain profile is estimated by determining a pitch associated with the body based on a determined trajectory associated with the vehicle.

[0007] In one embodiment, a groove is detected by calculating the first derivative of the estimated terrain profile, determining a zero crossing of the first derivative of the estimated terrain profile at the current point, and comparing the magnitude of the estimated terrain profile at the current point with the magnitude of the estimated terrain profile at the last point determined to be a zero crossing. A compensation value may be determined by determining a series of points between the current point and the last point determined to be a hump in the estimated terrain profile, and determining a compensation value for each point in the series of points based on a shape feature and a difference between the estimated profile at the current point and the estimated terrain profile at the last point determined to be a hump. The shape feature may include a step shape feature, a logarithmic shape feature, a quadratic shape feature, a slope shape feature, an exponential shape feature, or a combination thereof.

[0008] In one embodiment, the one or more control signals are transmitted by combining an initial error value for achieving the target terrain surface with the determined compensation value to generate a final error value, and generating one or more control signals to adjust the height of the implement according to the final error value.

[0009] These and other advantages of the present invention will become apparent to those skilled in the art upon review of the following detailed description and accompanying drawings. [Brief explanation of the drawings]

[0010] [Figure 1] 1 illustrates an exemplary bulldozer according to one or more embodiments.

[0011] [Figure 2] 1 illustrates an exemplary 2D environment in which a bulldozer operates, according to one or more embodiments.

[0012] [Figure 3] FIG. 1 illustrates a schematic diagram of a predictive control system for adjusting the height of a blade mounted on the body of a bulldozer in a 2D environment in accordance with one or more embodiments.

[0013] [Figure 4]FIG. 1 illustrates a block diagram of a controller system for adjusting the height of a blade mounted on a body of a bulldozer in a 2D environment in accordance with one or more embodiments.

[0014] [Figure 5] 1 illustrates a predictive control system for adjusting the height of an implement mounted on the body of a vehicle in a 3D environment in accordance with one or more embodiments.

[0015] [Figure 6] 1 illustrates a method for adjusting the height of an implement mounted on the body of a vehicle in a 3D environment according to one or more embodiments.

[0016] [Figure 7] 1 illustrates an exemplary bulldozer according to one or more embodiments.

[0017] [Figure 8] FIG. 1 shows a schematic diagram illustrating a rigid body model of a vehicle according to one or more embodiments.

[0018] [Figure 9] 10 shows a graph illustrating groove detection in an estimated profile of a terrain in accordance with one or more embodiments.

[0019] [Figure 10] 1 illustrates diagrams of various features according to one or more embodiments.

[0020] [Figure 11] 10 shows a graph depicting groove compensation according to one or more embodiments.

[0021] [Figure 12] 1 shows an exemplary schematic diagram of a vehicle according to one or more embodiments.

[0022] [Figure 13]1 illustrates graphs showing body-to-body and body-to-equipment correlations for a vehicle according to one or more embodiments.

[0023] [Figure 14] 1 illustrates an exemplary Smith Predictor controller according to one or more embodiments.

[0024] [Figure 15] 1 depicts a high-level block diagram of a computer that can be used to implement one or more embodiments. DETAILED DESCRIPTION OF THE INVENTION

[0025] Referring to FIG. 1 , an exemplary bulldozer 100 is shown in accordance with one or more embodiments. The bulldozer 100 includes a body 102 and a blade 104 (or other suitable implement) pivotally coupled to the body 102 via a boom arm (not shown). The bulldozer 100 also includes tracks 106 for traveling over a terrain 108 and may be operated autonomously or manually by an operator. Generally, during operation, the bulldozer 100 may be utilized in a construction environment, an agricultural environment, or any other environment to manipulate soil, sand, debris, etc. on the surface of the terrain 108 via the blade 104 to achieve a target terrain surface. As the bulldozer 100 travels over the terrain 108, it may encounter various disturbances in the terrain 108. One example of such a disturbance is a trench.

[0026] The bulldozer 100 operates in a 3D (three-dimensional) environment, and the blade 104 has three degrees of freedom of rotational movement relative to the body. Thus, the state of the bulldozer 100 can be defined by the position of the body 102 (within the navigation system 116), the orientation of the body 102 (within the navigation system 116), and the relative configuration of the blade 104 with respect to the body 102 (within the body system 112). In one embodiment, position is defined in Cartesian coordinates (X, Y, Z), and orientation is defined in Euler angles (yaw, pitch, roll). However, it should be understood that position and orientation can be represented in any suitable form. For example, position can be represented as a 2D (two-dimensional) curve lying on the surface of the terrain 108 (which would require knowledge of the terrain 108), and orientation can be represented as a rotation matrix, a quaternion, etc.

[0027] Bulldozers may be implemented with conventional control systems for adjusting blade height, but conventional control systems have inherent response delay times that prevent such conventional control systems from adjusting the blade height in a timely manner to compensate for grooves and other such disturbances.

[0028] The embodiments described herein provide a predictive control system for predicting disturbances to the body 102 due to disturbances in the terrain 108 (e.g., due to a groove) based on the motion of the blade 104. The predictive control system includes a controller 120, a set of sensors 122 disposed on the blade 104 and / or a set of sensors 124 disposed on the body 102, and one or more actuators 118 coupled to the body 102 and the blade 104. The controller 120 receives data from the set of sensors 122 and / or sensors 124 to predict disturbances to the body 102 and generates control signals to control the actuators 118 to adjust the height of the blade 104 to correct the predicted disturbances. Advantageously, the controller 120 predicts disturbances on the body 102, thereby providing sufficient time to adjust the blade 104 to correct for grooves and other such disturbances. For ease of presentation, the prediction of disturbances to the body 102 due to disturbances in the terrain 108 will first be described in terms of a 2D environment with reference to FIGS. 2-4 before being described in terms of a 3D environment.

[0029] FIG. 2 illustrates an exemplary 2D environment 200 in which a bulldozer 202 operates, according to one or more embodiments. The bulldozer 202 includes a body 206 and a blade 208. In one example, the bulldozer 202 may be the bulldozer 100 of FIG. 1. As shown in FIG. 2, the bulldozer 202 operates within the 2D environment 200 to manipulate the surface of a terrain 204. The body 206 of the bulldozer 202 is parameterized by the height of the body 206 and the pitch angle of the body 206 relative to the horizontal. The blade 208 of the bulldozer 202 is parameterized by a blade pitch angle θ 210. Performance of the bulldozer 202 is evaluated by measuring the difference between the actual surface of the terrain 204 and the target surface of the terrain 204. The bulldozer 202, according to one or more embodiments, is implemented with one or more sensors, one or more actuators, and a controller for automatically adjusting the height of the blade 208.

[0030] 3 shows a schematic diagram 300 of a predictive control system for adjusting the height of a blade mounted on the body of a bulldozer in a 2D environment according to one or more embodiments. In one example, the bulldozer can be bulldozer 100 of FIG. 1 or bulldozer 202 of FIG. 2.

[0031] In block 304, the vehicle's body pose (position) is determined based on a set of sensors mounted on the vehicle's body. The set of sensors for determining the body pose may include position sensors for determining the (X,Y) Cartesian coordinates of the vehicle's body. In block 302, the vehicle's body kinematics is determined based on the set of sensors mounted on the vehicle's body and the body pose (determined in step 304). In one example, the set of sensors for determining the body kinematics may include angular rotation sensors and accelerometers. In block 308, blade forward kinematics is determined based on the body pose, body kinematics, and a set of sensors mounted on the vehicle's blade. In block 310, blade pose is determined based on the blade forward kinematics. In block 306, the blade's inverse rotation kinematics is determined based on the blade pose and body pose. In block 312, path and trajectory planning is performed based on the surface search results from surface engine 314. In block 316, blade inverse kinematics is determined based on the path and trajectory planning. In block 318, joint-to-ram kinematics are determined from the blade inverse kinematics. In block 320, the joint-to-ram kinematics are input to a controller (e.g., a proportional-integral-derivative controller) that outputs a command to a valve 322 to adjust the height of the blade.

[0032] FIG. 4 shows a block diagram of a controller system 400 for adjusting the height of a blade mounted on the body of a bulldozer in a 2D environment in accordance with one or more embodiments. As shown in FIG. 4, the controller system 400 includes a controller 402 and a plant 404. The controller 402 receives data from sensors 406 mounted on the body and blade of the bulldozer and estimates the state of the body and blade via an observer 408. The observer 408 generates a compensation value (referred to as blade z) 420. The compensation value 420 is combined with an initial error value to generate an error 418 to generate a desired surface 410. The error 418 is input to a low-level controller 412, which sends a command to a hydraulic delay 414 to adjust the height of the bulldozer's blade to manipulate the ground surface 416.

[0033] Figure 5 shows a schematic diagram 500 of a predictive control system for adjusting the height of an implement mounted on a body of a vehicle in a 3D environment, according to one or more embodiments. Figure 6 shows a method 600 for adjusting the height of an implement mounted on a body of a vehicle in a 3D environment, according to one or more embodiments. Figures 5 and 6 will be described together. The steps of method 600 of Figure 6 may be performed by a controller (e.g., controller 120 of Figure 1) or any other suitable computing device (e.g., computer 1102 of Figure 11). Exemplary controllers include simple controllers such as a PID (proportional-integral-derivative) controller or a PLC (programmable logic controller), and more sophisticated controllers such as a Smith predictor or MPC (model predictive control).

[0034] A vehicle may initially drive over a terrain to manipulate the terrain via an implement to achieve a target terrain surface. The height of the vehicle's implement is adjusted according to an initial error value to achieve the target terrain surface. However, adjusting the implement height according to the initial error value may not compensate for some disturbances (e.g., grooves in the terrain). According to method 500, a compensation value is determined to compensate for grooves and other disturbances in the terrain. The compensation value may be combined with the initial error value to generate a final error value for adjusting the implement height to achieve the target terrain surface while also correcting for grooves and other disturbances in the terrain.

[0035] In step 602, sensor data is received from a set of sensors located on a vehicle. The vehicle can be any vehicle having an implement coupled to a body, such as a construction vehicle (e.g., a bulldozer with a blade coupled to a body or a compact track loader with a blade coupled to a body) or an agricultural vehicle (e.g., a combine with a header coupled to a body). In one example, the vehicle is bulldozer 100 of FIG. 1 having blade 104 and sensors 122, 124 coupled to body 102. As shown in FIG. 5, the vehicle can be bulldozer 502 and the set of sensors can be sensor 504.

[0036] The sensor set may include any number of suitable sensors for determining the position and orientation of the vehicle body and equipment, as well as the linear and angular velocities for each axis. In one embodiment, position is defined in Cartesian coordinates (X, Y, Z) and orientation is defined in Euler angles (yaw, pitch, roll). However, it should be understood that position and orientation may be represented in any suitable form. Example sensors may include an IMU (inertial measurement unit), a GPS (global positioning system) sensor, an LPS (local positioning system) sensor, an acoustic range finder, a laser range finder, an encoder, an in-ram pressure sensor, an odometer, or any other suitable sensor.

[0037] The set of sensors may be disposed on the vehicle in any suitable location for determining the position and orientation of the body and implement as well as the linear and angular velocities for each axis. For example, the set of sensors may include one or more sensors disposed on the implement and / or one or more sensors disposed on the body. In one embodiment, 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 sensor mounted on the body forms a main-auxiliary pair and runs an RTK (real-time kinematics) algorithm. Optionally, if the vehicle is a bulldozer with a push bar, an additional IMU may be mounted on the push bar. In another embodiment, the set of sensors is disposed on the vehicle in a mastted 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.

[0038] In step 604, a trajectory associated with the vehicle is determined based on the received sensor data. The trajectory represents the location of points associated with the vehicle as it travels over the terrain. Determining the trajectory will be described with continued reference to FIG. 5.

[0039] To determine the trajectory, the state of the vehicle body and the state of the vehicle equipment are first determined based on the received sensor data. As shown in Figure 5, an observation (estimation) block 506 receives data from sensors 504 to estimate the observed state of the body 508 and the observed state of the equipment 510. The body state and equipment state 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 equipment, as well as the linear and angular velocities for each axis.

[0040] 1 , the bulldozer 100 operates in a three-dimensional environment having various reference frames (e.g., a body frame 114 of the body 102 of the bulldozer 100, a blade frame 112 of the blade 104, a surface patch frame 110 of the terrain surface 108, and a navigation frame 116 of the navigation). The state of the body 102 is defined in terms of its absolute position and orientation (with respect to some reference point) in the body frame 114. The state of the blade 104 is defined in terms of its relative position and orientation (with respect to the body frame 114) in the blade frame 104. The absolute position and orientation (in the body frame 114 with respect to the reference point) of the blade 104 can be determined from its relative position and orientation.

[0041] The condition of the body and the condition of the equipment are judged using the following components: (1) an extended Kalman filter to determine the position and linear and angular velocity of the body based on a) the accelerometer output from the IMU mounted on the body, b) the position output from the primary GPS mounted on the body, and c) the body orientation (the output from the extended Kalman filter in (2)); (2) an extended Kalman filter to determine the body orientation based on a) gyroscope output from an IMU mounted on the body, b) a reference (vector from the auxiliary GPS to the primary GPS) output from an auxiliary GPS mounted on the body, and c) a 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 to determine the orientation of the implement based on a) gyroscope outputs from an 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 (calculated by the blade kinematics module in (6)); and (6) A blade kinematics module for calculating the position and orientation of the implement relative to the body, the linear and angular velocities of the implement, and the angles and angular velocities of the joints of the kinematic structure based on a) the body position, linear and angular velocities (determined by the extended Kalman filter in (1)) and the body orientation (determined by the extended Kalman filter in (2)), b) the implement orientation and angular velocities (determined by the extended Kalman filter in (4)), and c) the vehicle kinematic structure (e.g., the vehicle skeleton or blueprint). The vehicle kinematic structure refers to the number of joints in the vehicle and the way the joints are positioned relative to each other. The vehicle kinematic structure is further described with respect to FIG. 12 below.

[0042] The state of the body and the state of the implement, along with the target terrain surface, are then mapped to one-dimensional space to determine the one-dimensional state of the entire vehicle. As shown in Figure 5, the state of the body 508 and the state of the implement 510 as well as the target terrain surface 512 are mapped to a development plane by mapping to a development plane block 514 to determine the state of the vehicle 516. Referring briefly to Figure 7, a bulldozer 700 including a body 702 and a blade 704 is shown traveling along a bulldozer path 708 over a terrain in accordance with one or more embodiments. The states (in position, orientation, and linear and angular velocities) of the body 702 and the blade 704 are mapped to a development plane 706 that is perpendicular to the bulldozer path 708.

[0043] To map the state of the body 702 and blade 704 onto the one-dimensional development surface 706, various points of interest on the blade 704 are identified: (1) The midpoint of the blade 704, defined as being located approximately halfway between the ends of the blade 704. The blade midpoint is typically the point that determines the angle of elevation of the blade 704 relative to the desired surface when the blade is perpendicular (zero yaw); (2) The two end points of the blade 704. For example, the right end can be used as a reference for measuring the bulldozer and checking the location of the sensor. Such an end can be designated as point (0,0,0); (3) A point-fit point of interest (POI), defined as a user-selected point within (and including) the ends of blade 704. By selecting a particular point via defining a ratio between 0 and 1 (0 indicating one end and 1 indicating the other end), the user indicates their preference for a particular region of the desired surface of blade 704. In other words, the surface patch directly below the POI will be used as the basis for calculating the mainfall and cross slope errors. In one embodiment, the default value for this ratio is 0.5, representing the midpoint; and (4) Best-fit Point of Interest (same as Point-fit POI except determined by the control system). In the best-fit mode of operation, the surface patch selected is based on a variety of factors, including which of the surface patches has the greatest overlap with the edge or which is closest to the blade edge. Typically, the selected best-fit POI is one of the endpoints.

[0044] One or more such points of interest are projected onto a 1D surface that intersects at a point called the surface intersection. The goal of the control system is to move blade 704 such that the points of interest (as a function of time) converge (as a function of time) to the surface intersection within a fixed time. More generally, the end of blade 704 must intersect and lie on a curved surface patch (which is a spatial surface) that intersects a perpendicular ray extended from the POI.

[0045] While the embodiments described herein can be extended to 3D, it is greatly simplified, and more successful, if the problem is posed as a problem defined on a 2D plane (embedded in 3D space). The midpoint of the blade 704 should not be the criterion for selecting the plane, as it constantly changes as the blade tilts and / or rotates. Furthermore, trajectories are generated as the body moves. Ultimately, this plane is selected as the sagittal plane 706, which divides the bulldozer into two parts (e.g., a left part and a right part). It can be shown that this is without loss of generality. In other words, controlling the cylinder on this plane ensures that the problem is also solved in 3D space.

[0046] Formally, the navigation frame is

number

number

number

number

number

number

number

number

number

number

number

number

[0047] The state of the body 702 and blade 704 can be mapped to the development plane 706 as follows: First, a point of interest (point fit or best fit point of interest) is selected by varying the ratio α. Using kinematics, the point of interest is expressed in the navigation system to obtain p(x,y,z). m and M are calculated by kinematics, resulting in:

number

number

number

number

number

[0048] Referring to step 604 of Figure 6, a trajectory associated with the vehicle is determined based on the state of the vehicle. As shown in Figure 5, the state of the vehicle 516 is used by a tracking point determination block 518 to determine the trajectory. As the implement travels over the surface of the terrain, a trajectory is formed by adding points to the surface of the terrain. The front (start) of the trajectory is a point that represents the position of the implement, and the tail (end) of the trajectory extends rearward from the front a predetermined distance. Each point can be at any location relative to the vehicle. For example, each point can be at a location determined based on the position of the body and / or the position of the implement.

[0049] In one embodiment, each point on the trajectory is determined based on its suitability for deriving information about the shape of the ground. For example, if some point on the trajectory is taken as the implement tip, it must be assumed that the implement is not necessarily in contact with the terrain surface, which may not always be true. If the implement pitches up, the implement will lose contact with the terrain surface, leading to false humps and grooves. In another embodiment, each point on the trajectory is determined based on a virtual point on the body. Such a virtual point is located below the blade tip, which is far from the vehicle's physical body. The virtual point can be considered a downward vertical projection of the implement tip onto the body if the body is long enough. In another embodiment, each point on the trajectory is a point that represents the terrain surface instead of the blade tip, which may be a good choice if the body only pitches up / down. In another embodiment, each point on the trajectory can be determined by comparing the vertical height of the implement tip with the vertical height of a virtual point on the body and selecting the point with the lowest vertical height. Even if the implement loses contact with the terrain surface, the trajectory will still be a reasonable estimate of where the terrain surface is. In another embodiment, each point on the trajectory can be determined to represent the surface of the terrain based on the amount of penetration of the implement into the target surface. This may be a good choice if the dozer is close enough to the target surface (i.e., if the actual surface of the terrain is approximately equal to the desired surface). However, for calculation purposes, a small dither must be added to the points; otherwise, the slope of the points will be a constant value rather than zero at the inflection points.

[0050] In one embodiment, the trajectory is generated in a transient mode. In transient mode, points are added to the trajectory without any constraints. Thus, points are estimated and added as time progresses (whether the vehicle is moving or not). Depending on the speed of the system, points may be added, for example, approximately every 10 milliseconds. Therefore, points are always 10 ms apart in time. However, if the vehicle is stationary, the points will form a point cloud centered around the average of the added points. The width of the point cloud is determined by the variance of a set of filters in the system. However, the trajectory has a maximum capacity. If the maximum capacity is exceeded, the oldest points added at the edge of the trajectory will be removed to make room for new points. Once the vehicle begins to move over time, the point cloud forms a trail (called a trajectory history). Depending on how fast the vehicle is moving, new points will be spaced apart based on the vehicle's speed. For example, if the vehicle is traveling at 1 meter per second, approximately 100 points will be added to the trajectory at a rate of 10 ms = 0.01 seconds, spaced 10 mm apart from each other. Thus, the track is of variable length depending on the speed of the vehicle. In another embodiment, the track is generated in a hold mode. In hold mode, the track has a predetermined maximum length. Points are added to the track at the front and when the track reaches the predetermined maximum length, points are removed from the track at the rear to maintain the predetermined maximum length. Points are added to the track spaced according to a predetermined separation distance. For both the temporary and hold modes, a time delay limit can be defined between two consecutive points for both the temporary and hold modes. If the delay between two consecutive points exceeds the time delay limit, the track is reset.

[0051] In step 606, a terrain profile is estimated based on the determined trajectory associated with the vehicle. The trajectory associated with the vehicle roughly approximates the terrain profile but is a very rough indicator of the vehicle's body pitch behavior. For example, the trajectory may not be an accurate representation of the terrain profile if the vehicle encounters a narrow groove, if the implement is not in contact with the terrain, or if implement vibration is present. Therefore, the terrain profile is estimated (approximated by the trajectory) by simulating the body pitch as the vehicle travels over the terrain. The body pitch provides a more accurate profile of the terrain compared to the vehicle's trajectory. As shown in FIG. 5, the trajectory is used by body state prediction (simulation) block 520 to predict the terrain profile by simulating the body pitch.

[0052] In one embodiment, the motion of the body is simulated by using a rigid body approach to determine the pitch of the body as the vehicle travels over the terrain. In the rigid body approach, the vehicle is modeled as a rigid body characterized by a mass and mass moment of inertia (which change with changes in articulation geometry). Various subsystems of the vehicle (e.g., body, implement, tracks, wheels, etc.) may be modeled in a rigid body model of the vehicle. In some embodiments, the subsystems modeled in the rigid body model of the vehicle may also include the vehicle's suspension system, track shoes, or any other subsystem. To simplify the modeling, the interaction of the track with the terrain is modeled without considering the portions at the front and rear of the track that curve up and do not interact with the terrain.

[0053] FIG. 8 shows a schematic diagram 800 illustrating a rigid body model of a vehicle 802 according to one or more embodiments. The vehicle 802 includes a body 804, equipment 806, and a track 812, and has a center of gravity 810. The track 812 is made of rubber and can therefore be modeled as an elastic medium that can distort solely due to interaction with the terrain 808. For simplicity, the road wheels of the vehicle 804 are not modeled. As shown in FIG. 8 , the track 812 is modeled as a rubber band by using virtual spring / shock pairs 814 between the upper and lower boundaries of the rubber band where the track 812 contacts the terrain 808. The spring / shock pairs 814 each include a spring k and a shock absorber b. The track 812 can be modeled using any number of spring / shock pairs 814. The more spring / shock pairs 814 used to model the track 812, the more accurate the model. The spring / shock pair 814 deflects under forces from the terrain 808 (where the track 812 contacts the terrain 808) and from the body 804. The spring / shock pair 814 then exerts a force on the body 804, resulting in a linear force and a rotational torque about the center of gravity 810. These resulting forces and torques generate linear and angular accelerations that can be calculated using Newton's equations of mechanics to calculate the bounce and pitch dynamics of the vehicle 802. The combination of the linear and angular accelerations produces the vertical and pitch angular velocities of the vehicle 802. Because the velocity of the vehicle 802 is known, there is no need to model the tension forces of the terrain 808 (which would require knowledge of solid shear stresses and deformations). Portions of the track 812 may lose contact with the terrain 808, and therefore the spring / shock pair 814 representing those portions of the track 812 will no longer generate any normal force. In some embodiments, the interaction of the implement 806 with the terrain 808 may also be modeled to provide more accurate and realistic results.

[0054] One challenge is to determine the stiffness and damping parameters of spring k and bumper b, respectively, in spring / bumper pair 814. Spring k is a nonlinear spring that models the elastic behavior of rubber track 812 and the pressure-sinking behavior of terrain 808. According to Bekker's equation, the pressure-sinking relationship is given by: p=(k c / b+k φ )y n where p is the pressure, y is the amount of subsidence, b is the radius of the contact area of ​​the topography 808, and k c / b, k φ and n are empirical constant parameters of the topography 808. The calculated sinkage y represents the force generated by the topography 808 (and applied to the spring k). The exponent of y can make the spring k possibly nonlinear.

[0055] Returning to Figure 6, in step 608, grooves in the terrain are detected. A compensation value is then determined for adjusting the implement height based on the estimated profile of the terrain to correct for the detected grooves. As shown in Figure 5, groove detection algorithm block 522 determines a shape profile related to the profile of the terrain, which defines a compensation value for grooves detected in the terrain.

[0056] The terrain profile includes a plurality of points. Grooves are first detected in the terrain by analyzing a current point in the estimated terrain profile. The current point in the estimated terrain profile is a point at a predetermined distance from the front of the estimated terrain profile. As the vehicle travels over the terrain, additional points are added to the estimated terrain profile, moving the current point forward. Each point in the estimated terrain profile is associated with a shape profile that represents a shape feature of the terrain. Based on the analysis of the current point, the shape profile associated with the current point (and possibly other points) can be updated to define the shape feature.

[0057] 9 shows a graph 900 illustrating groove detection within an estimated profile of terrain according to one or more embodiments. In graph 900, signal 902 is detected within estimated profile b of terrain. z (x) represents the groove detection. The estimated profile b z This is done by analyzing the current point in (x). To detect the groove, the estimated profile b z (x) slope

number

number

number

number

number

number

number

number

number

number

number

[0058] A compensation value for adjusting the implement height is then determined based on the estimated profile of the terrain to correct for the detected groove. In particular, if the current point is detected as a groove, the estimated profile b z (x) is the final point determined to be a hump (or the estimated profile b if there is no final point determined to be a hump). z The estimated profile b at the current point determined to be a hump is then traversed (to the end of x) and returned. z (x) and the estimated profile b at the final point z(x) is calculated and used to update the profile associated with the points between the current point and the final point. In one embodiment, a compensation value for adjusting the implement height may be determined based on an estimated profile of the terrain to correct for the detected hump. If the current point is detected to be a hump, the estimated profile b z (x) is the distance up to the final point determined to be a groove (or the estimated profile b if there is no final point determined to be a groove). z The estimated profile b at the current point determined to be a groove is then traversed back to the edge of the groove (x). z (x) and the estimated profile b at the final point z The difference from (x) is calculated and used to update the profile associated with the points between the current point and the final point.

[0059] Estimated profile b related to the updated profile z Each point in (x) is identified as being adjusted. Unadjusted points occur when the vehicle body is traveling too fast or the groove is too wide. In these cases, the implement height is user defined.

[0060] Estimated profile b detected as a groove zThe shape profile associated with the points in (x) (i.e., the series of points between the current point and the final point determined to be a hump) may be updated by selecting a shape feature of the terrain. The shape feature may represent any suitable shape. FIG. 10 shows a diagram 1000 of various shape features for compensating for the profile of the terrain 1010 in accordance with one or more embodiments. The profile of the terrain 1010 includes many grooves. Various shape features 1002-1008 may be selected to correct for such grooves in the profile of the terrain 1010. As shown in the diagram 1000, the shape features may include a step shape feature 1002, a logarithmic shape feature 1004, a quadratic shape feature 1006, and a slope feature 1008. The shape features 1002-1008 are shown in the diagram 1000 according to various differentials to correct for the grooves in the profile of the terrain 1010. Other shape features, such as exponential shape features, complex shape features that connect two or more shape features, etc., are also contemplated. After applying the shape features 1002-1008, the profile of the terrain 1010 will appear approximately flat.

[0061] The compensation value for each point in the series of points is determined by combining the feature and the magnitude. In one embodiment, the feature may have a height ranging from 0 to 1 and may be overlaid on the series of points. The compensation value for each point may be determined by multiplying the feature height by the difference.

[0062] 11 shows a graph 1100 depicting groove compensation according to one or more embodiments. A vehicle 1102 includes a body 1104 and an implement 1106. The body 1104 has a center of gravity 1108, and the implement 1106 has a tip 1110. As the vehicle 1102 travels over the surface of the terrain, a trajectory 1112 is generated that represents the estimated profile of the surface of the terrain. A signal 1114 indicates the true body pitch of the vehicle 1102 as it travels over the surface of the terrain. A correction profile 1116, which includes various shape features, is applied to correct for grooves detected in the trajectory 1112.

[0063] Returning to FIG. 6 , in step 610, one or more control signals are sent to one or more actuators to adjust the implement height based on the determined compensation value. In one example, the actuator may be actuator 118 of FIG. 1 . The actuator may be a hydraulic cylinder or a hydraulic drive or any other suitable actuator. In FIG. 5 , a profile of the terrain with an associated shape profile is used by low-level controller 526 to send one or more control signals to hydraulic system 530 of bulldozer 502. The control signals include instructions for adjusting the implement height to achieve the target terrain surface while also compensating for grooves and other disturbances in the terrain. In one embodiment, an initial error value for achieving the target terrain surface is combined (e.g., added) with the compensation value to generate a final error value. The control signal is generated to include instructions for adjusting the implement height according to the final error value.

[0064] In some embodiments, simple controllers (e.g., PID controllers) that perform method 600 may perform poorly due to actuator delays. Predictive controllers (e.g., Smith Predictor, MPC) and other more sophisticated controllers may perform method 600 well, but may require mathematical modeling of the vehicle's behavior. According to one embodiment, the vehicle may be mathematically modeled. In FIG. 5, the vehicle is mathematically modeled by blade state predictor block 528.

[0065] FIG. 12 shows an exemplary schematic diagram 1200 of a vehicle according to one or more embodiments. As shown in FIG. 12, the vehicle 1202 includes a body 1204 with a center of gravity 1206 located at (x1, y1). The center of gravity 1206 of the body 1204 travels over a terrain surface 1208 (represented as a body trajectory) with a velocity v1. The body 1204 pitches about the center of gravity 1206 at an angular velocity ω1, generating a body pitch θ1. A rod 1210 extends from the body 1204 and is linked to a boom 1214 via a pivot 1212. The boom 1214 pitches about the pivot 1212 at an angular velocity ω2, generating a body pitch θ2. An end point 1216 of the boom 1214 is configured to be coupled to a blade. The end point 1216 has a position (x2, y2). The blade is not important to the basic kinematics and is therefore assumed to consist of a point.

[0066] The kinematic model of schematic 1200 is given as follows:

number

number

number

number

number

number

number

number

number

number

number

number

number

[0067] A linear state-space model (including the CTL) of a typical bulldozer is then derived. This model is based on the kinematic structure of the vehicle. The vehicle will be modeled as consisting of rigid links connected to each other (and moving) through one-degree-of-freedom joints. The links may have arbitrary shapes and be characterized by their location and orientation relative to a reference coordinate system. Each link is attached to a coordinate system. Each joint is attached to a unit vector that directs motion along or about it, depending on the joint type. Sensor locations and other points of interest on the bulldozer are considered to be part of a link. These points are expressed with respect to the link to which they are attached. The links and joints together form an open (or closed) chain or tree. Such rigid body models can be very detailed, but only sufficiently detailed models will be discussed here.

[0068] The linear state space model will have the following links and joints in order: (1) The vehicle body is the first link in the chain. The IMU and two GPS antennas are rigidly attached to it. The vehicle body is in contact with the ground, and its position and orientation are determined through complex interactions with the soil. Such a link is said to be mobile; its motion is not determined (at least not directly) by the motion of its joints. (2) The boom (push arm) link is attached to the body via a pivot joint. The movement of this link is constrained by the pivot joint. An IMU can be installed on this link. This is the main mechanism for changing the blade elevation angle. (3) The blade is attached to the boom via a spherical joint (called the blade center of rotation). This joint is modeled as consisting of three one-degree-of-freedom joints occupying the same position. The first two links connecting the three (virtual) joints are modeled as having zero length (hence, they are simply virtual links). The third link is the blade itself. Many sensors may be attached to the blade, including, for example, an IMU, one or two GPS antennas, acoustic sensors, etc. Furthermore, the most important entities on the blade link are the midpoint and its coordinate system. The blade may experience yaw, pitch, and roll motions around the center of rotation.

[0069] The kinematics-dynamics module in the proposed system has the following responsibilities: (1) Body Dynamics: Determine the body position, orientation, linear velocity, and angular velocity by using a dynamic model of the track-soil interaction. Note that this information is already estimated by the observer module using on-body GPS and inertial sensors. This modeling is used for prediction. (2) Blade inverse kinematics: Given the location, orientation and angular velocity of the body, as well as the orientation and angular velocity of the blade, determine the values ​​and velocities of all joints connecting the body to the blade. (3) Blade forward kinematics: Given the body's position, orientation, linear and angular velocities as well as joint values ​​and velocities determine the blade location, linear velocity and acceleration. (4) Blade Dynamics: Simulate (predict) blade motion by using body dynamics and valve-cylinder actuator models that move the blade relative to the body.

[0070] Officially,

number

number

number

number

number

number

number

[0071] As pointed out above, the kinematic structure has five links connected by four joints. All joints are rotational. The joint angles are expressed as Θ(t)=(θ1,θ2,θ3,θ4,θ5). The description of each joint is as follows: (1) θ1(t) is the angle (i.e., pitch) of the boom relative to the initial reference (usually the angle the boom makes with the flat surface (where the body is positioned when the blade tip is resting on the surface)). This pitching is the angle of the body system

number

number

number

[0072] It should be noted that due to the actual mechanics of the implement, θ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). While changes in θ3(t) can be compensated for by tilting the blade, θ4(t) is a characteristic of the vehicle and cannot be manipulated by an actuator.

[0073] Θ(t) is the element of the joint space of the tool. m BodyEach value of (x,y,z) (the blade midpoint in the body frame) corresponds to a unique Θ(t) (not true in general, but true in our special kinematic structure). These two are related by

number

number

number

number

number

[0074] The forward kinematic model can be in the following state:

number

[0075] Regarding blade inverse kinematics, Θ(t) is

number

number

number

number

number

number

[0076] This method is complemented by a calibration phase, because the calculated boom pitch will be erroneous if the blade rolls. Therefore, while calibrating the bulldozer, the apparent observed boom pitch due to tilting must be modeled and subtracted from the now calculated value. A quadratic curve is sufficient to model this dependence.

[0077] If an additional IMU is installed on the boom, θ1 does not need to be estimated and can be easily determined by using the additional IMU as an inclinometer. On the other hand, the dependence of θ4 on yaw must always be calibrated. Again, a quadratic curve can model this dependence with sufficient accuracy.

[0078] To calculate the angular velocity, we can use the following formula:

number

number

number

[0079] Finally, a dynamic model is used to calculate the body angular velocity of the body as it contacts and interacts with the ground. Ground refers to an estimation of the terrain surface over which the body will move according to the embodiments described herein. This ground is represented as a curve (profile) in 2D space. The ground must have a soil texture to make the interaction realistic. As previously discussed, a real-time rigid body approach can be used. It is assumed that a lumped model can be used, including a body with mass and moments of inertia and a series of discrete spring / buffer elements attached to and distributed along the track. The interaction is further restricted to the bottom of the track. Here, the interaction between the blade tip and the ground is not modeled, as this would require force sensors. It is also assumed that there is no suspension between the road wheels and the body (although for tank-like machines, such a safe assumption is not made). The parameters selected for the stiffness K and damping D of the individual elements must take into account and represent the properties of not only the rubber but also the soil.

[0080] For a particular soil type, Becker's equation (n=1) gives the relationship between the amount of penetration into the soil and the force generated. However, this is precisely the definition of a linear spring. The soil's listed parameters can be used to calculate an estimate of the spring's stiffness coefficient. Damping can be added to complement the model. Its value is estimated through experimentation.

[0081] Given that the problem has been reduced to a 2D problem, assume that travel occurs along x and that points have coordinates (x,z). Let the height of the body's center of gravity be z. c (t), where M and I represent the mass and moment of inertia of the body, respectively. The moment of inertia can generally be variable if the position of the implement relative to the body changes significantly.

[0082] The forward speed could in principle be found by modelling the tractive force between the track and the ground, but in this case the forward speed is known (observed) at the start of the prediction and can be considered to remain constant throughout the prediction horizon.

number

[0083] The body bounce dynamics equations are expressed as:

number

number

number

number

[0084] Similarly, regarding pitch dynamics:

number

[0085] In one embodiment, the performance of the control system according to method 600 may be evaluated. To evaluate the control system, the predicted body pitch (determined in step 606) is correlated with the actual body pitch after the body passes over the point where the equipment is located. In FIG. 5, body correlation block 524 correlates the predicted body pitch with the actual body pitch. The evaluation may be used to adjust the confidence in the adequacy of the compensation. The evaluation may be output to a supervisory control box for automatic adjustment of behavior and parameters, or to a user for manual adjustment of behavior and parameters. Two correlations are of interest: 1) body-to-body correlation, and 2) body-to-equipment correlation.

[0086] Body-to-body correlation measures the predictability of the track's interaction with the terrain. To calculate body-to-body correlation, the body pitch is compared to the Z component of an imaginary body point that is a fixed distance from the body's center of gravity. The fixed distance is the same distance as the distance between the point at the body's center of gravity and a point at the end of the implement. If the height of the imaginary point corresponds (correlates) with the height of the point at the body's center of gravity (representing the body's pitch pattern), there is a higher probability that the body will behave as expected when it arrives at the imaginary point's location. A higher correlation indicates a higher reliability of the determined compensation value.

[0087] Body-to-equipment correlation measures the amount by which the equipment's motion can predict the body's motion. This correlation is calculated by comparing the body's pitch with the Z component of a point on the equipment. A higher body-to-equipment correlation dictates the equipment's oscillatory behavior. Therefore, the control system should reduce the body-to-equipment correlation.

[0088] In one example, a vehicle travels over perfectly sinusoidal terrain. FIG. 13 shows a graph 1300 illustrating the vehicle's body-to-body correlation and body-to-equipment correlation as the vehicle travels over sinusoidal terrain according to one or more embodiments. Signal 1302 represents the body pitch, which is sinusoidal due to the sinusoidal terrain. Signal 1306 represents the implement height correction determined in accordance with embodiments described herein. The correction was applied as a step feature. Signal 1304 represents the implement position adjusted according to the correction. As shown in graph 1300, the implement position (signal 1304) tracks the correction (signal 1306) fairly well. Thus, the body-to-body correlation (signal 1310) is high and the body-to-equipment correlation (signal 1308) is low, indicating good performance. For a well-performing system, the implement will be completely out of sync with the body pitch.

[0089] The lack of confidence in the correlation can be calculated as follows:

number

[0090] Method 600 can be performed by any suitable controller. In one embodiment, the controller is a low-level controller. The low-level controller ensures that the implement tip tracks the terrain surface according to the target surface. This problem is reduced to the problem of convergence and tracking (projection of the blade tip onto the sagittal body) of one point on the (target level) plane s(x,z) by another p(x,z). This is called elevation (main fall) control of the bulldozer. At the same time, the controller must also ensure that the entire tip is on the desired cross slope (governed by the surface). The main fall and cross slope error are e mainfall and e cross-slope The cross slope control can be considered orthogonal to the main fall control and can be performed independently of the other controls. The actuators performing these two controls can be operated separately, but there is a coupling between them due to the hydraulic system. It is beneficial to couple them to achieve smooth tracking and avoid set point tracking. For example,

number

[0091] Due to delays in the actuators, the controller should be able to account for output delays in the plant. One preferred controller is a specially tuned PID controller. However, since the valve model is known with sufficient accuracy through measurements, a better solution would be a Smith predictor controller. An exemplary Smith predictor controller 1400 is shown in FIG. 14. The Smith predictor controller 1400 converts the elevation error into a pitch error. Alternatively, an MPC (model predictive control) controller can 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 entire system within a bounded horizon.

[0092] It should be understood that the scope of the embodiments described herein is more general and can be readily applied to systems with other means of determining the profile of the terrain surface. For example, the profile of the terrain surface can additionally or alternatively be determined by using an optical sensor (e.g., a laser sensor) (perhaps mounted on a boom) pointed toward the terrain surface. By using an optical sensor mounted on a boom instead of determining trajectory points as described above, the points can be calculated by using kinematic equations based on the body position and orientation, the boom pitch angle, the location of the optical sensor on the boom, and the distance measured by the optical sensor.

[0093] The systems, apparatus, and methods described herein may be implemented using digital circuitry or one or more computers using well-known computer processors, memory units, storage devices, computer software, and other components. Typically, 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 (one or more magnetic disks, internal hard disks and removable disks, magneto-optical disks, optical disks, etc.).

[0094] The 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; the methods and workflow steps described herein (including one or more steps or functions of FIGS. 3 and 5-6) may be implemented using one or more computer programs 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 an activity or bring about some result. Computer programs can be written in any type of programming language (including compiled or interpreted languages) and can be deployed in any form (e.g., as a stand-alone program or as a module, component, subroutine, or other unit suitable for use in a computing environment).

[0095] A high-level block diagram of an exemplary computer 1502 that can be used to implement the systems, devices, and methods described herein is depicted in FIG. 15. Any or all of the systems and devices discussed herein (including controller 120 of FIG. 1, controller 400 of FIG. 4, and controller 1400 of FIG. 14) can be implemented using one or more computers, such as computer 1502. Computer 1502 includes a processor 1504 operably 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 operation. The computer program instructions can be stored in data storage device 1512 or other computer-readable storage medium and loaded into memory 1510 when execution of the computer program instructions is desired. 3 and 5-6 may be defined by computer program instructions stored in memory 1510 and / or data storage device 1512 and controlled by processor 1504 executing the computer program instructions. For example, the computer program instructions may 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. Thus, by executing the computer program instructions, processor 1504 performs 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 over a network. Computer 1502 may also include one or more input / output devices 1508 (e.g., a display, keyboard, mouse, speakers, buttons, etc.) that enable user interaction with computer 1502.

[0096] The processor 1504 may include both general-purpose and special-purpose microprocessors, and may be the sole processor or one of multiple processors in the computer 1502. For example, the processor 1504 may include one or more central processing units (CPUs). The processor 1504, the data storage device 1512, and / or the memory 1510 may include, be supplemented by, or be incorporated within one or more application-specific integrated circuits (ASICs) and / or one or more field-programmable gate arrays (FPGAs).

[0097] The data storage device 1512 and the memory 1510 each comprise a tangible, non-transitory computer-readable storage medium. The data storage device 1512 and the memory 1510 each may comprise 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. The data storage device 1512 and the memory 1510 may comprise non-volatile memory, which may include one or more magnetic disk storage devices, such as internal hard disks and removable disks, magneto-optical disk storage devices, and 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 disk read-only memory (CD-ROM); digital versatile disk read-only memory (DVD-ROM) disks; or other non-volatile solid-state storage devices.

[0098] The input / output devices 1508 may include peripherals such as a printer, scanner, display screen, etc. For example, the 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 a user; a keyboard; and a pointing device such as a mouse or trackball by which a user can provide input to the computer 1502.

[0099] Those skilled in the art will recognize that an actual computer or computer system implementation may have other structures and may include other components as well, and that FIG. 15 is a high-level representation of some of the components of such a computer for illustrative purposes.

[0100] The foregoing detailed description should be understood in all respects to be illustrative and not restrictive, and the scope of the invention disclosed herein should be determined not from the detailed description, but rather from the claims, which are to be interpreted in accordance with the full breadth permitted by patent law. It should be understood that the embodiments shown and described herein are merely illustrative of the principles of the invention, and that various modifications can be made by those skilled in the art without departing from the scope and spirit of the invention. Those skilled in the art could implement various other feature combinations without departing from the scope and spirit of the invention.

Claims

1. 1. A method for adjusting the height of a blade mounted on a body of a vehicle as the vehicle travels over terrain, comprising: receiving sensor data from a set of sensors located on the vehicle; determining a trajectory of the vehicle based on the received sensor data; estimating a profile representative of the shape of the terrain based on the determined trajectory of the vehicle; simulating a pitch angle of the body of the vehicle caused by grooves and bumps in the terrain as the vehicle travels over the terrain based on the determined trajectory of the vehicle; estimating the profile of the terrain based on the trajectory of the pitch angle of the body of the vehicle; detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to accommodate the detected grooves; and sending one or more control signals to one or more actuators to adjust the height of the blade based on the determined compensation value; sending one or more control signals to one or more actuators to adjust the height of the blade based on the determined compensation value; combining the compensation value with the initial error value to generate a final error value to achieve a target terrain surface; and generating the one or more control signals for adjusting the height of the blade according to the final error value.

2. Determining the trajectory of the vehicle based on the received sensor data includes: determining the condition of the body and the condition of the blade based on the received sensor data; mapping the state of the body and the state of the blade into a one-dimensional space to determine the state of the vehicle; and The method of claim 1 , comprising determining the trajectory of the vehicle based on the state of the vehicle.

3. 3. The method of claim 2, wherein determining the state of the body and the state of the blade based on the received sensor data includes determining the position and orientation of the body and the blade and linear and angular velocities associated with each axis of the position and orientation of the body and the blade.

4. detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to compensate for the detected grooves; calculating a first derivative of the estimated profile of the terrain; determining that the first derivative of the estimated profile of the terrain at a current point is a zero crossing; and 2. The method of claim 1, comprising comparing the magnitude of the estimated profile of the terrain at the current point with the magnitude of the estimated profile of the terrain at a last point determined to be a zero crossing.

5. Detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to compensate for the detected grooves further comprises: determining a series of points between the current point and a last point determined to be a hump in the estimated profile of the terrain; and 5. The method of claim 4, comprising determining the compensation value for each point in the series of points based on shape features representative of a preferred shape and a difference between the estimated profile of the current point and the estimated profile of the terrain at the last point determined to be a hump.

6. The method of claim 1 , wherein the feature comprises a step feature, a logarithmic feature, a quadratic feature, a slope feature, an exponential feature, or a combination thereof.

7. The method of claim 1 , wherein the vehicle comprises a bulldozer.

8. 1. A non-transitory computer-readable storage medium storing computer program instructions for adjusting the height of a blade mounted on a body of a vehicle as the vehicle travels over terrain, comprising: The computer program instructions, when executed by a processor, cause the processor to: receiving sensor data from a set of sensors located on the vehicle; determining a trajectory of the vehicle based on the received sensor data; estimating a profile representative of the shape of the terrain based on the determined trajectory of the vehicle; simulating a pitch angle of the body of the vehicle caused by grooves and bumps in the terrain as the vehicle travels over the terrain based on the determined trajectory of the vehicle; estimating the profile of the terrain based on the trajectory of the pitch angle of the body of the vehicle; detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to accommodate the detected grooves; and performing an action including sending one or more control signals to one or more actuators to adjust the height of the blade based on the determined compensation value; sending one or more control signals to one or more actuators to adjust the height of the blade based on the determined compensation value; combining the compensation value with the initial error value to generate a final error value to achieve a target terrain surface; and generating the one or more control signals for adjusting the height of the blade according to the final error value.

9. Determining the trajectory of the vehicle based on the received sensor data includes: determining the condition of the body and the condition of the blade based on the received sensor data; mapping the state of the body and the state of the equipment into a one-dimensional space to determine the state of the vehicle; and The non-transitory computer-readable medium of claim 8 , further comprising determining the trajectory of the vehicle based on the state of the vehicle.

10. 10. The non-transitory computer-readable storage medium of claim 9, wherein determining the state of the body and the state of the blade based on the received sensor data includes determining the position and orientation of the body and the blade and linear and angular velocities associated with each axis of the position and orientation of the body and the blade.

11. detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to compensate for the detected grooves; calculating a first derivative of the estimated profile of the terrain; determining that the first derivative of the estimated profile of the terrain at a current point is a zero crossing; and 8. The non-transitory computer-readable storage medium of claim 7, further comprising comparing a magnitude of the estimated profile of the terrain at the current point with a magnitude of the estimated profile of the terrain at a last point determined to be a zero crossing.

12. Detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to compensate for the detected grooves further comprises: determining a series of points between the current point and a last point determined to be a hump in the estimated profile of the terrain; and 12. The non-transitory computer-readable storage medium of claim 11, further comprising determining the compensation value for each point in the series of points based on a shape feature representative of a preferred shape and a difference between the estimated profile of the current point and the estimated profile of the terrain at the last point determined to be a hump.

13. The non-transitory computer-readable storage medium of claim 11 , wherein the shape feature comprises a step shape feature, a logarithmic shape feature, a quadratic shape feature, a slope shape feature, an exponential shape feature, or a combination thereof.

14. a processor, and a controller including a memory for storing computer program instructions for adjusting the height of a blade mounted on a body of a vehicle as said vehicle travels over terrain; The computer program instructions, when executed on the processor, cause the processor to: receiving sensor data from a set of sensors located on the vehicle; determining a trajectory of the vehicle based on the received sensor data; estimating a profile representative of the shape of the terrain based on the determined trajectory of the vehicle; simulating a pitch angle of the body of the vehicle caused by grooves and bumps in the terrain as the vehicle travels over the terrain based on the determined trajectory of the vehicle; estimating the profile of the terrain based on the trajectory of the pitch angle of the body of the vehicle; detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to accommodate the detected grooves; and performing an action including sending one or more control signals to one or more actuators to adjust the height of the blade based on the determined compensation value; sending one or more control signals to one or more actuators to adjust the height of the blade based on the determined compensation value; combining the compensation value with the initial error value to generate a final error value to achieve a target terrain surface; and generating the one or more control signals to adjust the height of the blade according to the final error value.

15. detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to compensate for the detected grooves; calculating a first derivative of the estimated profile of the terrain; determining that the first derivative of the estimated profile of the terrain at a current point is a zero crossing; and 15. The controller of claim 14, including comparing the magnitude of the estimated profile of the terrain at the current point with the magnitude of the estimated profile of the terrain at a last point determined to be a zero crossing.

16. Detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to compensate for the detected grooves further comprises: determining a series of points between the current point and a last point determined to be a hump in the estimated profile of the terrain; and 16. The controller of claim 15, further comprising determining the compensation value for each point in the series of points based on a shape feature representative of a preferred shape and a difference between the estimated profile of the current point and the estimated profile of the terrain at the last point determined to be a hump.

17. The controller of claim 16 , wherein the shape feature comprises a step shape feature, a logarithmic shape feature, a quadratic shape feature, a ramp shape feature, an exponential shape feature, or a combination thereof.

18. The controller of claim 14 , wherein the vehicle comprises a bulldozer.

19. body, a blade coupled to the body; one or more actuators coupled to the body and the blade; a set of sensors disposed on the vehicle for generating sensor data as the vehicle travels over terrain; and receiving the sensor data; determining a trajectory of the vehicle based on the received sensor data; estimating a profile representative of the shape of the terrain based on the determined trajectory of the vehicle; simulating a pitch angle of the body of the vehicle caused by grooves and bumps in the terrain as the vehicle travels over the terrain based on the determined trajectory of the vehicle; estimating the profile of the terrain based on the trajectory of the pitch angle of the body of the vehicle; detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to accommodate the detected grooves; and sending one or more control signals to one or more actuators to adjust the height of the blade based on the determined compensation value; sending one or more control signals to one or more actuators to adjust the height of the blade based on the determined compensation value; combining the compensation value with the initial error value to generate a final error value to achieve a target terrain surface; and generating the one or more control signals to adjust the height of the blade according to the final error value.

20. Determining a trajectory associated with the vehicle based on the received sensor data includes: determining the condition of the body and the condition of the blade based on the received sensor data; mapping the state of the body and the state of the blade into a one-dimensional space to determine the state of the vehicle; and The vehicle of claim 19 , further comprising determining the trajectory of the vehicle based on the state of the vehicle.

21. 21. The vehicle of claim 20, wherein determining the state of the body and the state of the blade based on the received sensor data includes determining the position and orientation of the body and the blade, and the linear and angular velocities associated with each axis of the position and orientation of the body and the blade.

22. Detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to compensate for the detected grooves includes: calculating a first derivative of the estimated profile of the terrain; determining that the first derivative of the estimated profile of the terrain at the current point is a zero crossing; and 20. The vehicle of claim 19, further comprising comparing a magnitude of the estimated profile of the terrain at the current point with a magnitude of the estimated profile of the terrain at a last point determined to be a zero crossing.

23. Detecting grooves in the terrain and determining a compensation value for adjusting the height of the blade based on the estimated profile of the terrain to compensate for the detected grooves further comprises: determining a series of points between the current point and a last point determined to be a hump in the estimated profile of the terrain; and 23. The vehicle of claim 22, further comprising determining the compensation value for each point in the series of points based on a shape feature representative of a preferred shape and a difference between the estimated profile of the current point and the estimated profile of the terrain at the last point determined to be a hump.

24. 24. The vehicle of claim 23, wherein the shape feature comprises a step shape feature, a logarithmic shape feature, a quadratic shape feature, a ramp shape feature, an exponential shape feature, or a combination thereof.

25. 20. The vehicle of claim 19, wherein the vehicle comprises a bulldozer.

Images