Patent Application
20210332555
The present disclosure relates generally to a swing motion actuation and control system for maintaining a swing mechanism of a machine in a stationary state and, more particularly, to a swing motion actuation and control system of a machine including a hydrostatic pump supplying fluid to a motor in a closed loop hydraulic circuit to selectively maintain the swing mechanism in a stationary state.
Machines such as, for example, dozers, motor graders, wheel loaders, wheel tractor scrapers, and other types of heavy equipment are used to perform a variety of tasks. Effective control of the machines requires accurate and responsive sensor reading to perform calculations providing information in near real time to the machine control or operator. Autonomously and semi-autonomously controlled machines are capable of operating with little or no human input by relying on information received from various machine systems. For example, based on machine movement input, terrain input, and/or machine operational input, a machine can be controlled to remotely and/or automatically complete a programmed task. By receiving appropriate feedback from each of the different machine systems and sensors during performance of the task, continuous adjustments to machine operation can be made that help to ensure precision and safety in completion of the task. In order to do so, however, the information provided by the different machine systems and sensors should be accurate and reliable. The position, velocity, and distance traveled by the machine, and positions, movements, and orientations of different parts or components of the machine are parameters whose accuracy may be important for control of the machine and its operations. In machines such as excavators including swing components such as a boom, stick, and implement or work tool, an open loop swing system may be provided to control the swinging movement of the components from a digging or loading position to a dumping position, or from the dumping position back to the digging or loading position. When the machine is located on a slope, for example, the components such as the boom, stick, and implement may drift from a position commanded by an operator as a result of the effects of gravity on the components. A conventional solution to this drift problem may include the use of low leakage hydraulic control valves to attempt to hydraulically lock the swing components and prevent them from drifting when the machine is on a slope.
Conventional machines typically utilize a navigation or positioning system to determine various operating parameters such as position, velocity, pitch rate, yaw rate, and roll rate for the machine. The position and orientation of the machine is referred to as the “pose” of the machine. The machine “state” includes the pose of the machine as well as various additional operating parameters that can be used to model the kinematics and dynamics of the machine, such as parameters characterizing the various links, joints, tools, hydraulics, and power systems of the machine. Some conventional machines utilize a combination of one or more of Global Navigation Satellite System (GNSS) data, a Distance Measurement Indicator (DMI) or odometer measurement data, Inertial Measurement Unit (IMU) data, etc. to determine these parameters. Some machines utilize RADAR sensors, SONAR sensors, LIDAR sensors, IR and non-IR cameras, and other similar sensors to help guide the machines safely and efficiently along different kinds of terrain. Conventional machines have attempted to fuse these different types of data to determine the position of a land-based vehicle.
An exemplary system that may be utilized to determine the position of a machine is disclosed in U.S. Patent Application Publication No. 2008/0033645 (“the '645 publication”) to Levinson et al. that published on Feb. 7, 2008. The system of the '645 publication utilizes location data from sensors such as Global Positioning System (GPS), as well as scene data from a LIDAR (light detection and ranging) device to determine a location or position of the machine. Specifically, the data is used to create a high-resolution map of the terrain and the position of the machine is localized with respect to the map.
Although the system of the '645 publication may be useful in determining the overall position of the machine, the system may not provide accurate estimates for the position of the machine or components of the machine while dead-reckoning (i.e., during periods of time when GPS signals are unavailable). Moreover, the system of the '645 publication may not provide accurate position information when GPS signals are unreliable or erroneous due to multipath errors, position jumps, etc. because the system of the '645 publication does not check for the accuracy of the GPS signal. Finally, the system of the '645 publication does not provide a means to monitor and controllably maintain the position of swing components of the machine when the machine is operating on a slope.
The system and method of the present disclosure provides for accurate determination of the position of a machine and components of the machine, as well as including the use of a dedicated hydrostatic pump supplying fluid to a hydraulic motor in a closed loop system to maintain the position of the swing components of the machine relative to an undercarriage of the machine when the machine is operating on a slope. A swing motion control system for an earth-moving machine according to various exemplary embodiments of this disclosure may include a closed loop swing motion control system including a hydrostatic swing pump fluidly coupled to at least one hydraulic swing motor configured to control a swing mechanism of the earth-moving machine.
In one aspect, the present disclosure is directed to a closed loop swing motion control system for an earth-moving machine. The swing motion control system may include a closed loop hydraulic circuit including a hydrostatic swing pump fluidly coupled to at least one hydraulic swing motor configured to control a swing mechanism of the earth-moving machine, and a pressure control device configured to control the pressure of fluid supplied to the hydrostatic swing pump for control of the pressure output by the pump. The swing motion control system may include a controller configured to monitor and process signals received from sensors and operator input. The signals received from sensors may be indicative of machine position and pose, and inertia mass of swing components and a payload being moved by the swing mechanism of the machine. The controller may also be configured to control at least one of an offset amount for desired pump displacement by the hydrostatic swing pump or an offset amount for pump output pressure from the hydrostatic swing pump based on at least one of an amount of slope on which the machine is operating or the inertia mass of the swing components and payload.
In another aspect, the present disclosure is directed to an earth moving machine including a closed loop swing motion control system. The swing motion control system may include a closed loop hydraulic circuit including a hydrostatic swing pump fluidly coupled to at least one hydraulic swing motor configured to control a swing mechanism of the earth-moving machine, and a pressure control device configured to control the pressure of fluid supplied to the hydrostatic swing pump for control of the pressure output by the pump. The swing motion control system may include a controller configured to monitor and process signals received from sensors and operator input. The signals received from sensors may be indicative of machine position and pose, and inertia mass of swing components and a payload being moved by the swing mechanism of the machine. The controller may also be configured to control at least one of an offset amount for desired pump displacement by the hydrostatic swing pump or an offset amount for pump output pressure from the hydrostatic swing pump based on at least one of an amount of slope on which the machine is operating or the inertia mass of the swing components and payload.
In yet another aspect, the present disclosure is directed to an earth-moving machine including a plurality of sensors configured to generate signals indicative of machine position and pose, and inertia mass of swing components and payload configured to be moved by a swing mechanism of the machine, and a closed loop swing motion control system. The swing motion control system may include a closed loop hydraulic circuit including a hydrostatic swing pump fluidly coupled to at least one hydraulic swing motor configured to control the swing mechanism of the earth-moving machine, and a pressure control device configured to control the pressure of fluid supplied to the hydrostatic swing pump for control of the pressure output by the pump. The swing motion control system may include a controller configured to monitor and process signals received from the sensors and operator input. The controller may also be configured to control at least one of an offset amount for desired pump displacement by the hydrostatic swing pump or an offset amount for pump output pressure from the hydrostatic swing pump based on at least one of an amount of slope on which the machine is operating or the inertia mass of the swing components and payload.
Various non-IMU sensors 230 (see
In various exemplary embodiments according to this disclosure, a swing motion actuation and control system 700 may monitor and responsively and controllably maintain the position of a swing mechanism including swing components such as boom 17, stick 18, and work tool 19 relative to the undercarriage of a machine 10 such as the excavator illustrated in
As shown in
A separate and independent braking system, such as a hydraulic brake or parking brake, may also be operative coupled to the swing mechanism to assist with maintaining the swing mechanism in a stationary state, or otherwise preventing un-commanded rotation or movement of the swing mechanism. The ECM configured to send control signals to the various systems and subsystems of the machine, or a completely separate and dedicated swing controller may be configured to receive and process, via control logic, signals from the IMU's and various non-IMU sensors positioned on machine 10 to determine the roll, angle, slope, and other information indicative of the position and orientation of machine 10, and the relative position and orientation of the swing components relative to the undercarriage or car body of machine 10. The controller may also be configured to receive and process signals indicative of anticipated inertia of front linkages such as boom 17, stick 18, and tool 19, with and without a payload. These signals indicative of anticipated inertia may include, for example, sensor-generated signals indicative of boom head-end pressures, in concert with, based upon, and/or as a function of the roll angle, slope, and positional orientation of machine 10.
When rotation or movement of the swing mechanism, such as boom 17, stick 18, and tool 19 on excavator 10, is commanded by a machine operator, swing motion actuation and control system 700 may be configured to engage and actuate the swing mechanism while simultaneously, controllably, responsively, and progressively actuating disengagement of a braking system. Control of the swing mechanism in conjunction with control of a braking system may be based upon the foregoing positional and inertial information to prevent unintended and/or un-commanded acceleration, swinging, or movement, particularly when machine 10 is operating on a sloped or uneven surface.
In various alternative configurations, machine 10 may be configured to perform some type of operation associated with an industry such as mining, construction, farming, transportation, power generation, or any other industry known in the art. For example, machine 10 may be an earth moving machine such as a haul truck, a dozer, a loader, a backhoe, an excavator, a motor grader, a wheel tractor scraper or any other earth moving machine. Machine 10 may generally include track assemblies or other traction devices 12 (i.e., ground engagement devices) that are mounted on a car body (in between the track assemblies), which supports a rotating frame on which the machine body 14 forming an upper structure is mounted. The rotating frame and machine body 14 may support an operator station or cab, an integrated display 15 mounted within the cab, operator controls 16 (such as integrated joysticks mounted within the cab), and one or more engines and drive trains that drive the traction devices 12 to propel the machine 10. The boom 17 may be pivotally mounted at a proximal end to the machine body 14, and articulated relative to the machine body ds by one or more fluid actuation cylinders (e.g., hydraulic or pneumatic cylinders), electric motors, or other electro-mechanical components. The stick 18 may be pivotally mounted at a distal end of the boom 17 and articulated relative to the boom by one or more fluid actuation cylinders, electric motors, or other electro-mechanical components. The tool 19, such as a bucket, may be mounted at a distal end of the stick 18, optionally articulated relative to the stick 18 by one or more fluid actuation cylinders, electric motors, or other electro-mechanical components, and provided with ground engagement tools or other attachments for performing various tasks.
The IMU's 210 may be applied to the machine in multiple different positions and orientations, including on different portions of the machine body 14, the boom 17, the stick 18, and the work tool (e.g., the bucket) 19. The IMU's may be retrofitted at multiple positions and orientations along each of the portions of the machine, and may be added and removed depending on a particular machine application and configuration. Raw data received from each IMU may be processed through a Kalman filter, as will be described in more detail below. In some implementations the Kalman filter for each IMU sensor may be included as part of the IMU, and in other implementations the Kalman filter may be part of a separate sensor fusion module provided as part of a separate sensor fusion system.
Gyroscopes of each IMU sense orientation through angular velocity changes, while accelerometers of each IMU sense changes in direction with respect to gravity. The gyroscope measurements have a tendency to drift over time because they only sense changes and have no fixed frame of reference. The addition of accelerometer data allows bias in the gyroscope data to be minimized and better estimated to reduce propagating error and improve orientation readings. The accelerometers may provide data that is more accurate in static calculations, when the system is closer to a fixed reference point, while the gyroscopes are better at detecting orientation when the system is already in motion. Signals indicative of linear acceleration and angular rate of motion received from the accelerometers and gyroscopes of the IMU's associated with each of the different portions and/or components of the machine may be combined by the Kalman filter(s) to more accurately predict the output angle, velocity, and acceleration of each of the separate components of the machine.
The Kalman filter associated with each IMU mounted on a separate machine component takes measured values and finds estimates of future values by varying an averaging factor to optimize the weight assigned to estimated or predicted values as compared to the weight assigned to actual measured values, thereby converging on the best estimates of the true values for output joint angles, velocity, and acceleration for each component of the machine. The averaging factor is weighed by a measure of predicted uncertainty, sometimes called the covariance, to pick a value somewhere between the predicted and measured values. The Kalman filter estimates a machine state by using a form of feedback control in a recursive and iterative process, with each iteration including a time update or “predict” phase, and a measurement or “correct phase”. During each iteration performed by the Kalman filter, a “gain” or weighting is determined by comparing an error in the estimate for a measured value and an error in the actual measurement of the value. The Kalman gain is equal to the ratio between the error in the estimate and the sum of the error in the estimate and the error in the actual measurement. A current estimate for the value is then calculated from the previous estimate and a new measured value. A new error in the estimate of the value is then determined and fed back for use in determining the gain to be applied in the next iteration. The combined or fused information provided by the Kalman filter may provide accurate, real time information on pitch rate, yaw rate, roll rate, boom angle, stick angle, and other angles depending on linkage configuration and the number of IMU's installed on different portions or components of the machine.
As shown in the exemplary embodiment of
As further illustrated in the exemplary embodiment of
In the case of an excavator or other machine where IMU's may be mounted on portions of the machine that are rotated or swung through an arc during operation, the 3 dimensional position information associated with each of the IMU's mounted on those portions of the machine may also be fed back to a swing compensation module 220. The swing compensation module 220 may be configured to correct the acceleration information provided by the IMU's mounted on the rotating or swinging portions of the machine by compensating for centripetal acceleration. This correction of the acceleration information received from the IMU's 210 may be performed before the information is provided to the Kalman filter 240.
The additional non-IMU sensors 230 may include any devices capable of generating signals indicative of parametric values or machine parameters associated with performance of the machine 10. For example, the non-IMU sensors 230 may include sensors configured to produce signals indicative of boom and/or stick swing velocity, boom and/or stick position in global and machine reference frames, and work tool angle. A payload sensor may also be included and configured to provide a signal indicative of a payload of the machine 10. A slip detector may be included and configured to provide a signal indicative of a slip of the machine 100. Additional non-IMU sensors may include devices capable of providing signals indicative of a slope of the ground on which the machine 10 is operating, an outside temperature, tire pressure if the traction device 12 is a wheel, hydraulic or pneumatic pressures in various fluid actuation control devices, electrical voltages, currents, and/or power being supplied to electrical control devices, etc.
The non-IMU sensors 230 may include one or more locating devices capable of providing signals indicative of the machine's location and/or the position of various components of the machine relative to a global or local frame of reference. For example, a locating device could embody a global satellite system device (e.g., a GPS or GNSS device) that receives or determines positional information associated with machine 10, and may provide an independent measurement of the machine's position. The locating device and any other non-IMU sensor 230 may be configured to convey signals indicative of the received or determined positional information, or other information relating to various machine operational parameters to one or more interface devices such as the integrated display 15 in the operator cab for display of real time machine operating characteristics. The signals from the IMU's 210 and non-IMU sensors 230 may be directed to a controller configured to include a Kalman filter 240, and the Kalman filter 240 may be configured for implementation by one or more processors 241 associated with storage 243 and memory 245. The one or more processors 241 of the controller may be configured to implement a Kalman filtering process including sensor fusion performed in a sensor fusion module 242. The Kalman filter 240 may also be configured to perform gyroscope bias estimation in a gyroscope bias estimation module 244 in order to compensate for any drift over time in the readings provided by one or more gyroscopes associated with the IMU's. In some exemplary embodiments, a locating device may receive a GPS signal as the location signal indicative of the location of the machine 10 and provide the received location signal to the processor 241 for further processing. Additionally, the locating device may also provide an uncertainty measure associated with the location signal. However, it will be understood by one of ordinary skill in the art that the disclosed exemplary embodiments could be modified to utilize other indicators of the location of the machine 10, if desired.
The non-IMU sensors 230 may also include one or more perception sensors, which may include any device that is capable of providing scene data describing an environment in the vicinity of the machine 10. A perception sensor may embody a device that detects and ranges objects located 360 degrees around the machine 10. For example, a perception sensor may be embodied by a LIDAR device, a RADAR (radio detection and ranging) device, a SONAR (sound navigation and ranging) device, a camera device, or another device known in the art. In one example, a perception sensor may include an emitter that emits a detection beam, and an associated receiver that receives a reflection of that detection beam. Based on characteristics of the reflected beam, a distance and a direction from an actual sensing location of the perception sensor on the machine 10 to a portion of a sensed physical object may be determined. By utilizing beams in a plurality of directions, the perception sensor may generate a picture of the surroundings of the machine 10. For example, if the perception sensor is embodied by a LIDAR device or another device using multiple laser beams, the perception sensor, such as the laser catcher sensor 28 mounted on the stick 18 of the machine, may generate a cloud of points as the scene data describing an environment in the vicinity of the machine 10. It will be noted that the scene data may be limited to the front side (180 degrees or less) of the machine 10 in some embodiments. In other embodiments, the perception sensor may generate scene data for objects located 360 degrees around the machine 10.
The IMU's 210 may include devices that provide angular rates and acceleration of the machine 10 or, more particularly, of components or portions of the machine on which the IMU's are mounted, such as the machine body 14, the boom 17, the stick 18, and the bucket or other tool 19. For example, the IMU's 210 may include a 6-degree of freedom (6 DOF) IMU. A 6 DOF IMU sensor consists of a 3-axis accelerometer, 3-axis angular rate gyroscopes, and sometimes a 2-axis inclinometer. Each of the IMU's 210 may be retrofitted to an existing machine by welding the IMU to a portion or component of the machine where precise information on the real time position, orientation, and motion of that particular portion or component of the machine is desired. The machine's electronic control module (ECM) or other machine controller(s) may be programmed to receive signals from the IMU's and implement various machine controls based at least in part on the inputs received from the IMU's. In some exemplary implementations of this disclosure, the controls implemented by an ECM in response to the signals received from the IMU's may include actuation of one or more electrical or electro-hydraulic solenoids that are configured to control the opening and closing of one or more valves regulating the supply of pressurized hydraulic or pneumatic fluid to one or more fluid actuation cylinders. The 3-axis angular rate gyroscopes associated with the IMU's may be configured to provide signals indicative of the pitch rate, yaw rate, and roll rate of the machine 100 or of the specific portion of the machine on which the IMU sensor is mounted. The 3-axis accelerometer may be configured to provide signals indicative of the linear acceleration of the machine 10 or portion of the machine on which the IMU sensor is mounted, in the x, y, and z directions.
The Kalman filter module 240 may be associated with one or more of the processor 241, storage 243, and memory 245, included together in a single device and/or provided separately. The processor 241 may include one or more known processing devices, such as a microprocessor from the Pentium™ or Xeon™ family manufactured by Intel™, the Turion™ family manufactured by AMD™, any of various processors manufactured by Sun Microsystems, or any other type of processor. The memory 245 may include one or more storage devices configured to store information used by the Kalman filter 240 to perform certain functions related to disclosed embodiments. The storage 243 may include a volatile or non-volatile, magnetic, semiconductor, tape, optical, removable, nonremovable, or other type of storage device or computer-readable medium or device. The storage 243 may store programs and/or other information, such as information related to processing data received from one or more sensors, as discussed in greater detail below.
In one embodiment, the memory 245 may include one or more position estimation programs or subprograms loaded from the storage 243 or elsewhere that, when executed by the processor 241, perform various procedures, operations, or processes consistent with the disclosed embodiments. For example, the memory 245 may include one or more programs that enable the Kalman filter 240 to, among other things, collect data from an odometer, a locating device, a perception sensor, any one or more of the IMU's 210, and any one or more of the non-IMU sensors 230, and process the data according to disclosed embodiments such as those embodiments discussed with regard to
In certain exemplary embodiments, position estimation programs may enable the Kalman filter 240 of the processor 241 to process the received signals to estimate the real time positions and orientations of different portions or components of the machine 10. A Kalman filter implements a method that may be used to determine accurate values of measurements observed over time, such as measurements taken in a time series. The Kalman filter's general operation involves two phases, a propagation or “predict” phase and a measurement or “update” phase. In the predict phase, the value estimate from the previous timestep in the time series is used to generate an a priori value estimate. In the update phase, the a priori estimate calculated in the predict phase is combined with an estimate of the accuracy of the a priori estimate (e.g., the variance or the uncertainty), and a current measurement value to produce a refined a posteriori estimate. The Kalman filter is a multiple-input, multiple output digital filter that can optimally estimate, in real time, the states of a system based on its noisy outputs. These states are all the variables needed to completely describe the system behavior as a function of time (such as position, velocity, voltage levels, and so forth). The multiple noisy outputs can be thought of as a multidimensional signal plus noise, with the system states being the desired unknown signals indicative of the true values for each of the variables. The Kalman filter 240 can be configured to filter the noisy measurements, such as the measurements received as signals from the plurality of IMU's 210 mounted on different portions and components of the machine 10, to estimate desired signals. The estimates derived by the Kalman filter from the signals provided by the IMU's and non-IMU sensors are statistically optimal in the sense that they minimize the mean-square estimation error of the signals. The state uncertainty estimate for the noisy measurements may be determined as a covariance matrix, where each diagonal term of the covariance matrix is the variance or uncertainty of a scalar random variable. A gain schedule module 222 may be configured to calculate weights (or gains) to be used when combining each successive predicted state estimate with a successive actual measurement value to obtain an updated “best” estimate. As the Kalman filter 240 receives multiple measurements over time from the IMU's 210 and the non-IMU sensors 230, a recursive algorithm of the Kalman filter processes each of the multiple measurements sequentially in time, iteratively repeating itself for each new measurement, and using only values stored from the previous cycle (thereby saving memory and reducing computational time).
In one exemplary embodiment, the memory 245 may include one or more pose estimation programs or subprograms loaded from storage 243 or elsewhere that, when executed by the processor 241, perform various procedures, operations, or processes consistent with disclosed embodiments. For example, the memory 245 may include one or more programs that enable the Kalman filter 240 to, among other things, collect data from the above-mentioned units and process the data according to disclosed embodiments such as those embodiments discussed with regard to
In certain embodiments, the memory 245 may store program enabling instructions that configure the Kalman filter 240 (more particularly, the processor 241) to implement a method that uses the Kalman filter to estimate a state of the machine 10. In certain exemplary embodiments, the Kalman filter may be configured to utilize the following equations in its calculations. For the propagation or “predict” phase, the Kalman filter may be configured to utilize the following generic equations:
{circumflex over (x)}
k
−
=F
k−1
{circumflex over (x)}
k−1
+G
k−1
u
k−1 (1)
P
k
−
=F
k−1
P
k−1
+
F
k−1
T
+Q
k−1 (2)
For the measurement or “update” phase, the Kalman filter may be configured to utilize the following generic equations:
K
k
=P
k−1
−
H
T(HkPk−1−HkT+Rk)−1 (3)
{circumflex over (x)}
k
+
={circumflex over (x)}
k
−
+K
k(yk−Hk{circumflex over (x)}k−) (4)
P
k
+=(I−KkHk)Pk− (5)
In the above equations, {circumflex over (x)}k− may be the a priori state estimate of a certain state variable (e.g., pitch rate, yaw rate, roll rate, position, velocity, etc.) that is calculated based on a value ({circumflex over (x)}k−1) of the state variable from an immediately preceding time step. F, G, and H may be appropriate state transition matrices. In the measurement or “update” phase, the Kalman filter 240 may calculate the Kalman gain Kk utilizing equation (3), in which P is an error covariance matrix and R is a matrix setting forth the variance associated with the different state variables. For example, the values in the R matrix may specify the uncertainty associated with the measurement of a given state variable. In the measurement phase, the Kalman filter 240 may also obtain an independent measure of the state variable and set the independent measure as yk. Utilizing the a priori estimate {circumflex over (x)}k− from the “predict” phase, measurement yk, and the Kalman gain Kk (applied from the gain schedule module 222), the Kalman filter 240 may calculate the a posteriori state estimate {circumflex over (x)}k+ utilizing equation (4).
Following the predict phase 501, the Kalman filter 500 may implement the update phase 502 to calculate the a posteriori state estimate {circumflex over (x)}k+ utilizing, for example, equation (4). For example, the Kalman filter 500 may calculate the a posteriori state estimate {circumflex over (x)}k+ using the a priori estimate {circumflex over (x)}k− from the predict phase 501, measurement yk, and the Kalman gain Kk As shown in
The Kalman filter may set the input received from a locating device and from the inclinometers of the IMU's as the measurement yk in equation (4). Additionally, in an exemplary embodiment where the machine 10 is an excavator or other machine that includes portions or components that are rotated or swung through arcs during operation, the Kalman filter may receive acceleration values from the IMU's located on the rotating or swinging portion of the machine (e.g., the boom 17 and the stick 18) that have been pre-processed in a swing cancellation module 220 to compensate for centripetal acceleration occurring during the swinging motion. As shown in the exemplary embodiment of
Also, as discussed above, the Kalman filter 500 in the exemplary embodiment of
The Kalman filter is very useful for combining data from several different indirect and noisy measurements to try to estimate variables that are not directly measurable. For example, the gyroscopes of the IMU's measure orientation by integrating angular rates, and therefore the output signals from the gyroscopes may drift over time. The inclinometer and direction heading features (compass) of the IMU's may provide a different noisy, but drift-free measurement of orientation. In the exemplary embodiment of
In determining the state of the machine 10 using the Kalman filter 240, the filter may also be configured to consider other operational parameters of the machine 10. For example, if the machine 10 is an excavator, the Kalman filter may be configured to consider whether the machine 10 is digging, dumping, swinging in between digging and dumping positions, driving to a new location, etc. When the machine 10 is in one or more of the above operational states, certain parameters of the Kalman filter may be changed to reflect the accuracy or confidence in certain input parameters. For example, when the machine 10 is driving from one location to another, the Kalman filter may be configured to apply a lower weighting (gain) from the gain schedule module 222 to the input from the IMU inclinometers. To lower the weighting applied to the inclinometer input from the IMU's, the Kalman filter may be configured to increase the value of variance ‘R’ associated with the inclinometer input in equation (3). Similarly, when the machine 10 is digging, the Kalman filter may be configured to increase the weighting applied to the inclinometer input to reflect a higher confidence in the accuracy of the inclinometer input. For example, to indicate a higher confidence in the accuracy, the Kalman filter may be configured to apply a higher weighting (gain) from the gain schedule module 222 to the input from the IMU inclinometers and decrease the value of ‘R’ associated with the inclinometer input.
Referring again to
With reference to the diagram of
In one exemplary implementation, swing motion actuation and control system 700 may be configured to command an offset in a desired pump displacement for hydrostatic pump 686 intended to increase the pump displacement when the machine (such as excavator 10 of
Similarly, swing motion actuation and control system 700 may be configured to command an offset in a desired pump displacement for hydrostatic pump 686 intended to decrease the pump displacement when the machine (such as excavator 10 of
Swing motion actuation and control system 700 may be configured to determine the amount of offset to a desired hydrostatic pump displacement based on factors that may include, but are not limited to, the magnitude of the inertial mass of the swing components and carried payload, and the roll rate, yaw rate, and pitch rate of machine 10. For example, as the inertial mass being swung by machine 10 is increased, the amount of offset to a desired hydrostatic pump displacement may also be proportionally increased, and as the inertial mass being swung by machine 10 is decreased, the amount of offset to a desired hydrostatic pump displacement may be proportionally decreased. Similarly, as factors such as roll, yaw, and/or pitch of machine 10 are increased, the amount of offset to a desired hydrostatic pump displacement may also be proportionally increased. As the same factors are decreased, the amount of offset to a desired hydrostatic pump displacement may be decreased. In some exemplary implementations, an additional factor may include a pre-position scale, for example, set from 0 to 1, with the scale providing for additional compensation to pump displacement designed to smooth the effects of swing engagement and brake disengagement when machine 10 is operating on a slope.
As also shown in
Swing pump displacement brake slope control included at brake slope control 727 may include braking logic that achieves braking by offsetting a current pump displacement or commanding the pump in an opposite direction from swing rotation by an amount needed to decelerate swing motion. An amount of braking offset may be increased as the slope on which machine 10 is operating increases, or as the amount of inertial mass of the swing components and payload increases. An amount of braking offset may be decreased as the slope on which machine 10 is operating decreases, or as the amount of inertial mass of the swing components and payload decreases. In some implementations, brake slope control 727 may be configured to scale an offset amount of pump displacement lower as the inertial mass decreases even as the slope increases since the gravitational effects of the slope are offset by the lower inertial mass. A swing pump ePRV brake slope control 747 may also be performed by commanding pilot ePRV to a maximum value when braking machine 10.
The disclosed machine state control system 50 and swing motion actuation and control system 700 may be applicable to any machine or machine system benefiting from accurate, real time detection of the variables needed to completely describe the system behavior as a function of time (such as position, velocity, linear acceleration, and angular rate of motion of each machine component) and from accurate compensation and control of a swing mechanism to mitigate the effects of gravity on drifting of the swing components away from commanded positions. The disclosed sensor fusion system in conjunction with multiple IMU's and non-IMU sensors retrofittably attached to different portions or components of the machine may provide for improved estimation of the positions and orientations of all of the different machine components by utilizing a Kalman filter associated with each IMU mounted on each of a plurality of machine components.
In some exemplary implementations of the disclosed sensor fusion system, the Kalman filter 240 may utilize machine parameters, odometer signals, and IMU inputs received from IMU's mounted on various portions and components of the machine to propagate or “predict” the machine states. For example, the Kalman filter 240 may predict the following states: position, forward velocity, angular velocities, joint angles, and angular orientation (attitude) of each of the machine components relative to global and machine reference frames, and of the machine itself relative to a global reference frame. In addition to the signals indicative of acceleration and angular rate of motion received from each IMU, each of the Kalman filters associated with each IMU mounted on a different machine component may receive, from a variety of different non-IMU sensors, such as an odometer, proximity sensors, and other perception sensors, signals indicative of the distance moved by the component of the machine during operation, or the distance between the component of the machine and a potential obstacle. The Kalman filter may also calculate the distance traveled by the machine 10 itself by multiplying a received scale factor by the number of rotations of a traction device. The Kalman filter may also calculate velocity of the machine 10 or velocity of a portion or component of the machine by integrating a signal indicative of linear acceleration from an IMU sensor mounted on a particular portion or component of the machine. The Kalman filter may calculate the velocity of the machine or the portion or component of the machine on which one or more IMU's are mounted using signals from the IMU's and weighting the resulting velocities to generate a predicted velocity. In some implementations, the distance traveled by the machine itself may be adjusted to account for slipping of the machine. Each Kalman filter may also receive signals indicative of the angular rates of motion (roll rate, yaw rate, and pitch rate) of the machine or portion of the machine from one or more IMU's. By integrating the angular rates of motion, the Kalman filter 240 may determine the attitude or angular orientation (roll, heading, and pitch) of each machine component or of the machine itself.
The Kalman filter may utilize one or more of the propagated states to propagate or “predict” the position of the machine 10 or the position of a portion or component of the machine relative to a machine reference frame and relative to a global reference frame. For example, by utilizing the angular rates of motion and predicted velocities, the Kalman filter may predict a position of the machine or component of the machine. As discussed above, the Kalman filter may also calculate an uncertainty for the predicted position that may be set equal to the uncertainty as designated by an error covariance matrix of the Kalman filter. Various positions on the machine may also be determined independently from the IMU's. Having determined the independent position measurements, the Kalman filter may be configured to fuse the predicted position information and the independent position measurement to determine updated position estimates for each location. Kalman filter measurement update equations may be utilized to determine the updated position estimate. Having determined an updated position estimate for the machine 10, the Kalman filter 240 may also determine the biases for each of the IMU's. As discussed above, an example of a bias parameter estimation that may be performed by the Kalman filter is an estimation of the bias of gyroscope determined angular positions after integration of measured angular rates.
In one exemplary application of the machine state control system according to an implementation of this disclosure, accurate, updated, real time information on the positions and orientations (pose) of the machine and portions or components of the machine may provide feedback to an information exchange interface 350 in order to effect machine controls that achieve optimal positioning and operation of the machine and components of the machine for improved productivity and reliability. In some implementations, the feedback may assist an operator by coaching the operator on how to effect controls that result in improved machine footing and stability, and hence improved productivity. In other implementations the information received at the information exchange interface 350 may result in the generation of autonomous or semi-autonomous control command signals that are provided to various machine systems and subsystems for effecting changes in machine pose and changes in the relative positions and orientations of machine components. In one exemplary implementation, as shown in
In digging with an excavator, the machine state control system 50 may determine from historical and/or empirical data regarding the kinematics and dynamics for the excavator that the car body of the machine should be parallel to the digging linkages with the idler wheels for the tracks of the excavator pointed toward the front linkages for the best stability during digging. Feedback can also be provided to the information exchange interface 350 regarding the machine pitch and roll so that if the footing is poor underneath the idler wheels when the tracks of the excavator are pointed forward, the information exchange interface can result in the generation of control command signals that cause a change in the pose of the machine to improve the footing and prevent the machine from pitching and rolling, as well as maneuvering the machine to make full contact with the ground to counteract digging forces. The angle of the bucket or other tool and the leverage being achieved by the particular orientation of the stick and boom at any particular point in time during a digging operation can also impact the digging efficiency of the excavator. The machine state control system according to this disclosure may provide continually updated feedback information to the information exchange interface regarding the real time efficiency of a linkage position during digging. The feedback information may result in a change to the angle of the bucket or the position of the stick during excavation in order to improve the efficiency of the machine by achieving better loading of the bucket, quicker loading of the bucket, and/or improved kinematics of the linkages that will result in a better use of the machine power and improve the longevity of the machine. The information provided to the information exchange interface from the machine state control system may also result in a change in control command signals that cause a change in the pose of the machine to avoid digging with the car body oriented at 90 degrees to the linkage, which is not optimal for stability or digging. When machine controls respond to the information provided at the information exchange interface 350 to implement controls such that the car body of the machine is parallel to the digging linkages with the idler wheels for the tracks of the excavator pointed toward the front linkages, the result is that energy is not wasted lifting the machine off the ground, productivity rates are increased with the resulting reduced cycle times, and loads on the final drive components are reduced for improved longevity of the machine and reduced down time.
In another exemplary application of the machine state control system according to this disclosure, as shown in
In one exemplary implementation, an excavator may be lifting a heavy load or performing a digging operation, and the boom actuation cylinder(s) may be at their maximum pressure, with the pump output pressure equal to the pressure in the boom actuation cylinders and the boom stalled while the bucket and stick are still moving. The machine state control system can determine from the accurate, real time fused sensor data being received from the sensor fusion system, including data indicative of the pitch rate, and roll rate for the machine, whether the machine is in an unstable and/or overstressed state. The machine state control system may determine that the relief pressure for the boom actuation cylinder(s) can be increased in a controlled ramp up to get the boom moving again without exceeding acceptable stress levels, and while maintaining the stability of the machine.
In additional exemplary implementations of the machine state control system according to this disclosure, the machine state control system may output commands to adjust the maximum output pressure of a pump providing pressurized fluid to various fluid actuation cylinders on the machine. As shown in
As discussed above, machine state control system 50 may include swing motion actuation and control system 700, which may be associated with closed, hydrostatic loop 660. Closed, hydrostatic loop 660 may fluidly connect an independent, dedicated hydrostatic swing pump 686 with one or more hydraulic motors 682, 684 operatively connected to a rotating frame and machine body 14 of machine 10 in order to control the swing motion of boom 17, stick 18, and tool 19 relative to the undercarriage or car body of machine 10.
It will be apparent to those skilled in the art that various modifications and variations can be made to the disclosed method and system for determining the real time state of a machine. Other embodiments and implementations will be apparent to those skilled in the art from consideration of the specification and practice of the disclosed machine state control system. It is intended that the specification and examples be considered as exemplary only, with a true scope being indicated by the following claims and their equivalents.
