# US 11,898,321 B2 · Input shaping for error detection and recovery in dynamically agile grading machines > Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt - Number: US 11,898,321 B2 - Document ID: US11898321B2 - Status: Granted - Kind: B2 - Application number: 17/125,967 - Filed: 2020-12-17 - Published: 2022-06-23 - Granted: 2024-02-13 - Earliest claimed: 2020-12-17 - Assignees: Topcon Positioning Systems, Inc. - Applicants: Topcon Positioning Systems, Inc. - Inventors: Shahab Kalantar; Dmytro Poplavskiy - Web page: https://patentgrep.com/patent/US11898321B2 ## CPC - E02F 3/841: {Devices for controlling and guiding the whole machine, e.g. by feeler elements and reference lines placed exteriorly of the machine (construction of roads E01C19/008)} - E02F 3/845: {using mechanical sensors to determine the blade position, e.g. inclinometers, gyroscopes, pendulums} - E02F 3/847: {using electromagnetic, optical or acoustic beams to determine the blade position, e.g. laser beams} - E02F 9/265: {with follow-up actions (e.g. control signals sent to actuate the work tool)} - E02F: DREDGING; SOIL-SHIFTING (winning peat E21C49/00) ## Abstract 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. ## Claims ### Claim 1 A method for adjusting a height of an implement mounted on a body of a vehicle as the vehicle travels over a terrain, the method comprising: receiving sensor data from a set of sensors disposed on the vehicle; determining a trajectory associated with the vehicle based on the received sensor data; estimating a profile of the terrain based on the determined trajectory associated with the vehicle; detecting a ditch in the terrain and determining compensation values for adjusting the height of the implement based on the estimated profile of the terrain to compensate for the detected ditch; and transmitting one or more control signals to one or more actuators for adjusting the height of the implement based on the determined compensation values, wherein transmitting one or more control signals to one or more actuators for adjusting the height of the implement based on the determined compensation value comprises: 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. ### Claim 2 (depends on 1) The method of claim 1, wherein determining a trajectory associated with the vehicle based on the received sensor data comprises: 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. ### Claim 3 (depends on 2) The method of claim 2, wherein determining a state of the body and a state of the implement based on the received sensor data comprises: determining 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. ### Claim 4 (depends on 1) The method of claim 1, wherein estimating a profile of the terrain based on the determined trajectory associated with the vehicle comprises: determining a pitch associated with the body based on the determined trajectory associated with the vehicle. ### Claim 5 (depends on 1) The method of claim 1, wherein detecting a ditch in the terrain and determining a compensation value for adjusting the height of the implement based on the estimated profile of the terrain to compensate for the detected ditch comprises: 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. ### Claim 6 (depends on 5) The method of claim 5, wherein detecting a ditch in the terrain and determining a compensation value for adjusting the height of the implement based on the estimated profile of the terrain to compensate for the detected ditch further comprises: 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. ### Claim 7 (depends on 6) The method of claim 6, wherein the shape characteristics comprises a step shape characteristic, a logarithmic shape characteristic, a quadratic shape characteristic, a ramp shape characteristic, an exponential shape characteristic, or a combination thereof. ### Claim 8 (depends on 1) The method of claim 1, wherein the vehicle comprises a dozer machine and the implement of the vehicle comprises a blade. ### Claim 9 A non-transitory computer readable medium storing computer program instructions for adjusting a height of an implement mounted on a body of a vehicle as the vehicle travels over a terrain, the computer program instructions when executed by a processor cause the processor to perform operations comprising: receiving sensor data from a set of sensors disposed on the vehicle; determining a trajectory associated with the vehicle based on the received sensor data; estimating a profile of the terrain based on the determined trajectory associated with the vehicle; detecting a ditch in the terrain and determining compensation values for adjusting the height of the implement based on the estimated profile of the terrain to compensate for the detected ditch; and transmitting one or more control signals to one or more actuators for adjusting the height of the implement based on the determined compensation values, wherein transmitting one or more control signals to one or more actuators for adjusting the height of the implement based on the determined compensation value comprises: 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. ### Claim 10 (depends on 9) The non-transitory computer readable medium of claim 9, wherein determining a trajectory associated with the vehicle based on the received sensor data comprises: 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. ### Claim 11 (depends on 10) The non-transitory computer readable medium of claim 10, wherein determining a state of the body and a state of the implement based on the received sensor data comprises: determining 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. ### Claim 12 (depends on 9) The non-transitory computer readable medium of claim 9, wherein estimating a profile of the terrain based on the determined trajectory associated with the vehicle comprises: determining a pitch associated with the body based on the determined trajectory associated with the vehicle. ### Claim 13 (depends on 9) The non-transitory computer readable medium of claim 9, wherein detecting a ditch in the terrain and determining a compensation value for adjusting the height of the implement based on the estimated profile of the terrain to compensate for the detected ditch comprises: 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. ### Claim 14 (depends on 13) The non-transitory computer readable medium of claim 13, wherein detecting a ditch in the terrain and determining a compensation value for adjusting the height of the implement based on the estimated profile of the terrain to compensate for the detected ditch further comprises: 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. ### Claim 15 (depends on 14) The non-transitory computer readable medium of claim 14, wherein the shape characteristics comprises a step shape characteristic, a logarithmic shape characteristic, a quadratic shape characteristic, a ramp shape characteristic, an exponential shape characteristic, or a combination thereof. ### Claim 16 A controller comprising: a processor; and a memory to store computer program instructions for adjusting a height of an implement mounted on a body of a vehicle as the vehicle travels over a terrain, the computer program instructions when executed on the processor cause the processor to perform operations comprising: receiving sensor data from a set of sensors disposed on the vehicle; determining a trajectory associated with the vehicle based on the received sensor data; estimating a profile of the terrain based on the determined trajectory associated with the vehicle; detecting a ditch in the terrain and determining compensation values for adjusting the height of the implement based on the estimated profile of the terrain to compensate for the detected ditch; and transmitting one or more control signals to one or more actuators for adjusting the height of the implement based on the determined compensation values, wherein transmitting one or more control signals to one or more actuators for adjusting the height of the implement based on the determined compensation value comprises: 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. ### Claim 17 (depends on 16) The controller of claim 16, wherein detecting a ditch in the terrain and determining a compensation value for adjusting the height of the implement based on the estimated profile of the terrain to compensate for the detected ditch comprises: 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. ### Claim 18 (depends on 17) The controller of claim 17, wherein detecting a ditch in the terrain and determining a compensation value for adjusting the height of the implement based on the estimated profile of the terrain to compensate for the detected ditch further comprises: 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. ### Claim 19 (depends on 18) The controller of claim 18, wherein the shape characteristics comprises a step shape characteristic, a logarithmic shape characteristic, a quadratic shape characteristic, a ramp shape characteristic, an exponential shape characteristic, or a combination thereof. ### Claim 20 (depends on 16) The controller of claim 16, wherein the vehicle comprises a dozer machine and the implement of the vehicle comprises a blade. ### Claim 21 A vehicle comprising: a body; an implement coupled to the body; one or more actuators coupled to the body and the implement; a set of sensors disposed on the vehicle for generating sensor data as the vehicle travels over a terrain; and a controller for: receiving the sensor data; determining a trajectory associated with the vehicle based on the received sensor data; estimating a profile of the terrain based on the determined trajectory associated with the vehicle; detecting a ditch in the terrain and determining compensation values for adjusting a height of the implement based on the estimated profile of the terrain to compensate for the detected ditch; and transmitting one or more control signals to the one or more actuators for adjusting the height of the implement based on the determined compensation values, wherein transmitting one or more control signals to one or more actuators for adjusting the height of the implement based on the determined compensation value comprises: 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. ### Claim 22 (depends on 21) The vehicle of claim 21, wherein determining a trajectory associated with the vehicle based on the received sensor data comprises: 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. ### Claim 23 (depends on 22) The vehicle of claim 22, wherein determining a state of the body and a state of the implement based on the received sensor data comprises: determining 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. ### Claim 24 (depends on 21) The vehicle of claim 21, wherein estimating a profile of the terrain based on the determined trajectory associated with the vehicle comprises: determining a pitch associated with the body based on the determined trajectory associated with the vehicle. ### Claim 25 (depends on 21) The vehicle of claim 21, wherein detecting a ditch in the terrain and determining compensation values for adjusting a height of the implement based on the estimated profile of the terrain to compensate for the detected ditch comprises: 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. ### Claim 26 (depends on 25) The vehicle of claim 25, wherein detecting a ditch in the terrain and determining compensation values for adjusting a height of the implement based on the estimated profile of the terrain to compensate for the detected ditch further comprises: 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. ### Claim 27 (depends on 26) The vehicle of claim 26, wherein the shape characteristics comprises a step shape characteristic, a logarithmic shape characteristic, a quadratic shape characteristic, a ramp shape characteristic, an exponential shape characteristic, or a combination thereof. ### Claim 28 (depends on 21) The vehicle of claim 21, wherein the vehicle comprises a dozer machine and the implement comprises a blade.