Estimation with gyros of the relative attitude between a vehicle body and an implement operably coupled to the vehicle body

Patent metadata

Field	Value
Publication number	US-9995019-B2
Application number	US-201415311081-A
Country	US
Kind code	B2
Filing date	Jun 23, 2014
Priority date	Jun 23, 2014
Publication date	Jun 12, 2018
Grant date	Jun 12, 2018

Abstract

An estimate of the relative attitude between an implement and a vehicle body is computed from a body angular velocity measurement received from at least one body gyro mounted on the vehicle body and from an implement angular velocity measurement received from at least one implement gyro mounted on the implement. A first system state vector estimate corresponding to a first time instant includes a representation of a first relative attitude estimate. An updated system state vector is computed based at least in part on the first system state vector estimate, the body angular velocity vector measurement, and the implement angular velocity vector measurement. A second system state vector estimate corresponding to a second time instant is predicted based at least in part on the updated system state vector and a time-dependent system model. The second system state vector estimate includes a representation of a second relative attitude estimate.

First claim

The invention claimed is:

1. A method for estimating a relative attitude between an implement and a vehicle body, wherein the implement is operably coupled to the vehicle body, the vehicle body having a controller attached thereto and the controller is performing the method comprising the steps of:
receiving a first system state vector estimate, wherein the first system state vector estimate:
corresponds to a first time instant in a plurality of time instants; and
comprises a representation of a first relative attitude estimate corresponding to the first time instant;

receiving a body angular velocity measurement from at least one body gyro mounted on the vehicle body;
receiving an implement angular velocity measurement from at least one implement gyro mounted on the implement;
computing an updated system state vector based at least in part on the first system state vector estimate, the body angular velocity vector measurement, and the implement angular velocity vector measurement;
predicting a second system state vector estimate, wherein the second system state vector estimate:
is based at least in part on the updated system state vector and a time-dependent system model;
corresponds to a second time instant in the plurality of time instants; and
comprises a representation of a second relative attitude estimate corresponding to the second time instant;

measuring one or more control signals for use in determining whether a relative angular velocity of the implement with respect to the vehicle body is zero or non-zero; and
controlling the implement relative to the vehicle body using the relative angular velocity, and the relative attitude estimate based at least in part on the first relative attitude estimate or the second relative attitude estimate.

2. The method of claim 1 , wherein the step of computing an updated system state vector based at least in part on the first system state vector estimate, the body angular velocity vector measurement, and the implement angular velocity vector measurement is performed by an extended Kalman filter procedure.

3. The method of claim 1 , wherein the representation of the first relative attitude estimate corresponding to the first time instant is a quaternion, further comprising the step of updating a value of a quaternion norm to 1, wherein:
the step of computing an updated system state vector based at least in part on the first system state vector estimate, the body angular velocity vector measurement, and the implement angular velocity vector measurement is further based at least in part on the updated value of the quaternion norm.

4. The method of claim 1 , further comprising the step of determining that the relative angular velocity of the implement with respect to the vehicle body has the zero value, wherein:
the step of computing an updated system state vector based at least in part on the first system state vector estimate, the body angular velocity vector measurement, and the implement angular velocity vector measurement is further based at least in part on the relative angular velocity with the zero value.

5. The method of claim 4 , wherein:
the vehicle body is a dozer body;
the implement is a dozer blade;
the dozer blade is operably coupled to the dozer body by at least one hydraulic cylinder;
the at least one hydraulic cylinder is controlled by a hydraulic control system;
the hydraulic control system is controlled by a joystick or a controller unit; and
the step of determining that a relative angular velocity of the implement with respect to the vehicle body has a zero value is selected from the group consisting of:
monitoring a translation of the joystick;
monitoring a pressure of a hydraulic fluid in the at least one hydraulic cylinder;
monitoring a flow rate of a hydraulic fluid in the at least one hydraulic cylinder; and
monitoring an electronic control signal in the controller unit or in the hydraulic control system.

6. The method of claim 1 , wherein:
the at least one body gyro mounted on the vehicle body is selected from the group consisting of:
one body gyro mounted on the vehicle body;
two orthogonally-mounted body gyros mounted on the vehicle body; and
three orthogonally-mounted body gyros mounted on the vehicle body; and

the at least one implement gyro mounted on the implement is selected from the group consisting of:
one implement gyro mounted on the implement;
two orthogonally-mounted implement gyros mounted on the implement; and
three orthogonally-mounted implement gyros mounted on the implement.

7. The method of claim 1 , wherein the vehicle body is a dozer body and the implement is a dozer blade.

8. A controller unit for estimating a relative attitude between an implement and a vehicle body, wherein the implement is operably coupled to the vehicle body, the controller unit comprising:
a processor;
memory operably coupled to the processor; and
a data storage device operably coupled to the processor, wherein the data storage device stores computer program instructions for execution by the processor which is configured to:
receive a first system state vector estimate, wherein the first system state vector estimate:
corresponds to a first time instant in a plurality of time instants; and
comprises a representation of a first relative attitude estimate corresponding to the first time instant;

receive a body angular velocity measurement from at least one body gyro mounted on the vehicle body;
receive an implement angular velocity measurement from at least one implement gyro mounted on the implement;
compute an updated system state vector based at least in part on the first system state vector estimate, the body angular velocity vector measurement, and the implement angular velocity vector measurement; and
predict a second system state vector estimate, wherein the second system state vector estimate:
is based at least in part on the updated system state vector and a time-dependent system model;
corresponds to a second time instant in the plurality of time instants; and
comprises a representation of a second relative attitude estimate corresponding to the second time instant;

measure one or more control signals for use in determining whether a relative angular velocity of the implement with respect to the vehicle body is zero or non-zero; and
control the implement relative to the vehicle using the relative angular velocity, and the relative attitude estimate based at least in part on the first relative attitude estimate or the second relative attitude estimate.

9. The controller unit of claim 8 , wherein the processor computes an updated system state vector based at least in part on the first system state vector estimate, the body angular velocity vector measurement, and the implement angular velocity vector measurement using an extended Kalman filter procedure.

10. The controller unit of claim 8 , wherein:
the representation of the first relative attitude estimate corresponding to the first time instant is a quaternion;
the processor is further configured to update a value of a quaternion norm to 1; and
the processor computes an updated system state vector based at least in part on the first system state vector estimate, the body angular velocity vector measurement, and the implement angular velocity vector measurement further based at least in part on the updated value of the quaternion norm.

11. The controller unit of claim 8 , wherein:
the processor is further configured to determine that the relative angular velocity of the implement with respect to the vehicle body has the zero value; and
the processor computes an updated system state vector based at

Assignees

• Llc “Topcon Positioning Systems”
• Topcon Positioning Systems Inc

Inventors

• Kosarev Alexey Andreevich

Classifications

• E02F3/845

  using mechanical sensors to determine the blade position, e.g. inclinometers, gyroscopes, pendulums · CPC title

• E02F3/7618

  with the scraper blade adjustable relative to the pivoting arms about a horizontal axis · CPC title

• E02F9/265 Primary

  with follow-up actions (e.g. control signals sent to actuate the work tool) · CPC title