The present invention pertains to control of transporters, and more particularly to methods and apparatus for detecting fault conditions in sensors used to control operation of the transporters.
Dynamically stabilized transporters refer to vehicles having a control system that actively maintains the stability of the transporter while the transporter is operating. The control system maintains the stability of the transporter by continuously sensing the orientation of the transporter, determining the corrective action to maintain stability, and commanding the wheel motors of the transporter to make the corrective action. If the transporter loses the ability to maintain stability, such as through the failure of a component, the rider may experience discomfort at the sudden loss of balance. Vehicles of this sort may be more efficiently and safely operated when they employ system architectural features supplementary to those described in the prior art.
The invention, in one aspect, features a method for initializing a controller of a balancing transporter. The method involves a) acquiring a plurality of orientation signals associated with the balancing transporter. The method also involves b) determining if one or more of a plurality of angular rate sensors has a signal output offset below a threshold based on the plurality of orientation signals. The method also involves c) initializing a pitch state estimator for controlling operation of the balancing transporter based on output of the one or more of the plurality of angular rate sensors. The method also involves d) controlling operation of the balancing transporter based on at least one output of the pitch state estimator.
In some embodiments, the method involves terminating step c) if one or more of the angular rate sensors has a signal output offset at or above the threshold. In some embodiments, step b) involves comparing each of the orientation signals to a corresponding threshold value. In some embodiments, initialization of the pitch state estimator is terminated if one or more of the orientation signals equals or exceeds its corresponding threshold value. In some embodiments, the method involves repeating a), b), c) and d) until the signal output offsets of the angular rate sensors are below the threshold.
In some embodiments, step b) involves determining if pitch or roll of the transporter varies by more than 1.5 degrees over a predetermined period of time, the pitch and roll are estimated based on accelerometer output signals. In some embodiments, step b) involves determining if yaw rate of the transporter varies by more than 4.7 degrees per second over a predetermined period of time, the yaw rate is estimated based on difference in speed of left and right ground-contacting elements of the transporter that apply torque to an underlying surface of the transporter. In some embodiments, step b) involves determining if speed of a left or right ground-contacting element of the transporter exceeds 0.22 meters per second, the left and right ground contacting elements apply torque to an underlying surface of the transporter. In some embodiments, step b) involves determining if the difference between a first yaw rate signal and a second yaw rate signal is greater than 22 degrees per second, the first yaw rate signal is estimated based on difference in speed of left and right ground-contacting elements of the transporter that apply torque to an underlying surface of the transporter and the second yaw rate signal is based on angular rate signals output by two or more of the plurality of angular rate sensors, the angular rate signals corresponding to angular rate around a transporter-fixed substantially vertical axis.
The invention, in another aspect, features a balancing transporter that includes a plurality of devices (e.g., sensors) for providing orientation signals associated with the balancing transporter. The transporter includes a plurality of angular rate sensors. The transporter includes a processor for determining if one or more of the plurality of angular rate sensors has a signal output offset that equals or exceeds a threshold based on the orientation signals and for estimating a pitch state of the balancing transporter based on output of two or more of the plurality of angular rate sensors. The transporter also includes a controller for controlling operation of the balancing transporter based on the pitch state estimate.
In some embodiments, the processor determines if one or more of the angular rate sensors has a signal output offset that equals or exceeds a threshold by comparing each of the orientation sensor signals to a corresponding threshold value. In some embodiments, the transporter includes at least one ground-contacting element for applying torque to an underlying surface of the transporter.
The foregoing and other objects, aspects, features, and advantages of the invention will become more apparent from the following description and from the claims.
The foregoing features of the invention will be more readily understood by reference to the following detailed description, taken with reference to the accompanying drawings, in which:
A transporter may be said to act as ‘balancing’ if it is capable of operation on one or more wheels but would be unable to stand on the wheels but for operation of a control loop governing operation of the wheels. A balancing transporter lacks static stability but is dynamically balanced. A transporter may advantageously be used as a mobile work platform or a recreational vehicle such as a golf cart, or as a delivery vehicle. The wheels, or other ground-contacting elements, that provide contact between such a transporter and the ground or other underlying surface, and minimally support the transporter with respect to tipping during routine operation, are referred to herein as ‘ground-contacting elements.’
A control loop may be provided so that leaning of the subject results in the application of torque to wheel 20 and/or wheel 21 about axle 22 by means of a motor drive. The transporter 10, however, is statically unstable, and, absent operation of the control loop to maintain dynamic stability, transporter 10 will no longer be able to operate in its typical operating orientation. “Stability” as used herein, refers to the mechanical condition of an operating position with respect to which the system naturally returns if the system is perturbed away from the operating position in any respect.
The transporter 10 may be operated in a station-keeping mode, wherein balance is maintained substantially at a specified position. Additionally, the transporter 10, which may be referred to herein, without limitation, as a “vehicle,” may also maintain a fixed position and orientation when the user 8 is not on the platform 12. This mode of operation prevents runaway of the transporter 10 and provides for the safety of the user and other persons. In one embodiment, the transporter 10 is operated in the station-keeping mode while the user is stepping on to the platform 12 of the transporter 10. In some embodiments, a force plate or other sensor (not shown) disposed on the platform 12 is used to detect the presence of a user on the transporter 10.
Different numbers of wheels or other ground-contacting members may advantageously be used in various embodiments of the invention as particularly suited to varying applications.
“Pitch state” as used in this description and the appended claims includes both the pitch in the fore-aft plane and the pitch rate of the vehicle, i.e. θ and θr (or {dot over (θ)}) where θr is the time rate of change of θ.
T=K
1
θ+K
2θr+K3X+K4Xr (EQN. 1)
The values of the gains K1, K2, K3 and K4 are dependent on settings of the control loop, physical parameters of the system, and other effects such as gravity.
The dynamic behavior of a vehicle may be described through reference to a coordinate system. Two such coordinate systems are used in describing the motion of a vehicle about irregular surfaces: the Earth reference frame, “E-frame”, and the vehicle reference frame “V-frame.”
The E-frame defines the vertical axis, Z, to be co-linear to the direction of gravity and passing through the center of gravity 308 of an occupied vehicle 304 as shown in
The axes associated with the V-frame have an origin positioned at the center of gravity 308 of the vehicle 304. In other embodiments, the origin of the axes may be situated at another point on the vehicle 304. The axes are fixed with respect to the vehicle 304. The relative vertical axis, R, is a specified vehicle-fixed substantially vertical axis and may be defined by a line passing through the center of gravity 308 of the vehicle 304 and the support/backrest/head of the operator of the vehicle 304. The relative horizontal axis, P, is perpendicular to the relative vertical axis and has a component parallel to the direction of movement of the vehicle 304. The third axis, Q, is orthogonal to both R and P. The relative orientation of the R, P, Q frame varies with respect to the Z, X, Y frame as the vehicle 304 tilts. As shown in
The equations below present the rate transformations between the E-frame and V-frame under a small angle approximation for the respective rotation rates, denoted by the subscript r. These transformations will be referred to as small angle Euler transforms (SAETs) and inverse SAETs respectively.
Inertial sensors, for example, such as angular rate sensors or rate gyroscopes (hereinafter referred to as gyros) are used to provide pitch state information to the vehicle 304. The inertial sensors measure the rate of change of the orientation of the vehicle 304 about the V-frame and produce a signal which is representative of the rate of change of the pitch, roll, and yaw angles of the vehicle 304. The inertial sensors need to be adjusted regularly due to sensor drift. Thus, tilt sensors are incorporated into the system for providing a stable angular value from which bias errors of the inertial sensors may be compensated, as is discussed with respect to
In addition, U.S. Pat. No. 6,332,103 describes additional methods and apparatus for measuring and/or estimating the pitch state of a vehicle. This patent is incorporated by reference herein in its entirety.
The transformation module 401 provides to the state estimator 402 information corresponding to the pitch angle and roll angle when the vehicle is unaccelerated except for the force of gravity, and the difference in wheel velocities. The difference in wheel velocities provides an absolute value for the yaw rate assuming the wheels do not slip.
Relative reference sensors 403 measure quantities with respect to the V-frame and may include inertial sensors, for example, gyroscopes such as mechanical or solid-state gyroscopes. In order to provide information on the three orientations, at least three single axes gyroscopes are required. Additional gyroscopes may be used to provide fault-tolerance capability.
Additionally, the gyroscopes are set off axes in some embodiments because the vehicles capabilities may be greater than the measurement range of the gyroscopes. For example, the angular rate during a sharp turn could be beyond the measurement range of the gyroscopes. By canting the gyroscopes, the measured rate is scaled by the cosine of the angle between the rotation axis and the canted axis and also resulting in the cross coupling of other orthogonal rate components.
The relative reference sensors 403 transform the angular rate sensed into a digital signal of the represented angular rate and pass the signal to a virtual-gyro construction module 408. Because the orientation of the gyroscopes may not be aligned with the Q, P, and R axes, each signal produced by the gyroscope may be comprised of rate information about multiple axes. The virtual gyro construction module 408 transforms the signals to produce three output signals that correspond to the signals that three virtual gyros would produce if oriented to measure the rotation about the P, Q, and R axes. This operation on the gyro signals produces the virtual gyro Pr, Qr and Rr information.
The virtual gyro construction module performs the matrix equation, V=M G where V is the virtual gyro vector, G is the gyro output vector, and M is a 3×4 construction matrix that converts the four gyro signals in G to the virtual gyro vector, V. The construction matrix is not unique and can be the inverse least squares solution, or the output of any one of the four combinations of three of the four gyros or any combination of the solutions. The coefficients of the construction matrix are determined during the calibration of the vehicle and contain both corrections for gyro alignment and for the individual gyro characteristics. In one embodiment, the construction matrix provides greater fault tolerance for gyroscopes D, E, and F through a combination of the solutions selected from the least squares solution and the four three-gyroscope combinations.
A gyro fault detection signal from the gyro fault detection module 409 is also generated from the raw signals, which is passed to the state estimator 402. One embodiment of a fault detection module for use with the present invention is described with respect to
The state estimator 402 takes the signals from the virtual gyro-construction module 408, the gyro fault detection module 409, and the calibration-transformation module 401 and estimates the pitch state of the vehicle. The state estimator 402 also estimates the roll state and yaw state of the vehicle, but in a preferred embodiment of the invention, only the pitch state of the vehicle is passed on to the control loop for balancing the vehicle about the vehicle's center of gravity.
Although
The pitch and roll error signals 530 are also fed back and passed through a filter 540 which attenuates the signal's strength. The attenuated signal is passed into the summer 520. The feedback loop is provided to correct for inaccuracies due to transients, so as to move the output readings of 506 in the direction of the tilt sensors. The attenuation of the signal attenuator 540 is provided to limit a large erroneous absolute reference sensor value from affecting the E-frame orientation angles. A large erroneous absolute reference sensor value may occur, for example, where the vehicle goes over a sudden bump. As the vehicle goes over a bump, the absolute reference sensors will produce extraneous signals in the form of transients which inaccurately reflect the pitch rate and the pitch angle.
In the inverse SAET 508 an inverse SAET is performed on the pitch, roll and yaw error information. In this inverse transform the error signals 530 of the pitch and roll are inserted in the inverse equation at θr, φr and the yaw error signal 580 is inserted at Ψr where one would expect to find the respective rates. Ψr is provided from summer 572 as the difference between Rr and the difference in the velocity of the wheels 550. Performing the inverse SAET on the error signals rather than the rates has the effect of partitioning the error information. The inverse SAET transforms the E-frame signals into the V-frame coordinate system. This signal is then passed through an attenuation filter 560. After the attenuation filter 560, the signals are placed in an integrator 570 and then passed back to the summer 502. This feedback loop accounts for the long term bias drift of the gyroscopes. The attenuation filter 560 allows the tilt sensors which are more stable over long periods of time to assist in correcting for the drift of the gyroscopes while still allowing the gyroscopes to control short term changes.
Unlike the Pr and Qr corrections which are proportional to the error signal, the yaw rate, Rr, correction is performed only up to a preset error level designated as X. This comparison is done in comparator 568. If the yaw rate error signal becomes larger than the preset error level X, correction to Rr is suspended under the assumption that the wheels no longer represent the yaw rate and a suspension timer is started. The gyro rate value, Rr is still compared to the delta wheel rate 550 during the suspension to calculate a yaw error signal 580. If after a predetermined amount of time the difference between the value of Rr and that of the delta wheel rate is not below the preset error level, an alarm occurs 562 which in one embodiment is a visual signal. If after a second predetermined time on the suspension timer the difference does not fall below the preset error level, the yaw correction is shut off and an alarm sounds indicating that the system cannot distinguish between a fault and the drift error of the gyroscopes. If the second time limit is reached, the system must be reset. The preset error limit is imposed to prevent erroneous R data from being incorporated into the pitch state estimate when, for example, the vehicle is stationary on a moving platform that is turning.
To provide for accurate readings of the pitch state, upon initialization, the pitch state estimator attempts to align the gyroscopes to the tilt sensor as long as the system is non-accelerated. This is accomplished by adjusting the corner frequencies through the two signal attenuators 540 and 560 to orient the system to the tilt sensors. In one embodiment, this is accomplished by adaptively adjusting the gain in response to an error signal. If during initialization, the gyroscopes and the tilt sensors do not align, the system may pause and then re-measure the values for the tilt sensor and gyroscopes to see if they have aligned. If they do not align, the system may reverse the process and begin again until alignment is achieved.
If a gyro failure is detected, the state estimator 402 performs a single axis estimate of the pitch state which is described in U.S. Pat. Nos. 5,701,965 and 5,791,425. The same components and filters of the three axis state estimator are used in the single axis estimate as shown in
Additionally, in box 501 a calibration module is added which corrects for misalignment of the gyroscope. In one embodiment, to provide a smooth transition between the three axis state estimator and the single axis state estimator, the local gyro bias indicator 576 provides an initial condition for the integrator of 570 in single axis mode. Another difference between the three axis state estimator and the single axis estimator is that there is no SAET module 503 and inverse SAET 508 because the single axis state estimator must assume that Q=θr as there is no need to map the pitch in the V frame to the pitch in the E frame as the system is operating in a single plane as opposed to three dimensions. Additionally, the tilt sensor, which is preferably a two axis pendulous sensor, which provides a signal for both the pitch and roll, is only used for the pitch signal and the roll signal is disregarded.
In one embodiment, there are six gyroscopes. Three gyroscopes are configured about the pitch axis of the V frame and the remaining three gyroscopes are set along a combination of the roll and yaw axes of the V frame. In such a fashion, partially triple redundancy is provided. Each gyroscope situated about the pitch axis has an associated processor and tilt sensor the processor capable of providing a single axis state estimate from the associated gyroscope and tilt sensor. The processor receives signals from its associated gyroscope along with the signals from the three gyroscopes which mounted to sense rotation about the yaw and roll axes. The redundancy provides another failsafe system for the vehicle. Each of the three processors independently estimates the pitch state using either the associated gyroscope alone for a single axis state estimate or using both the associated gyroscope and the three remaining gyroscopes for a three axis state estimate. If all the gyroscopes are operating properly, the state estimates produced by the three processors will all agree. However, if one of the associated gyroscopes fails, the state estimates produced by the three processors do not agree. A voting mechanism may be implemented such that the processor producing the erroneous estimate shuts itself off.
Control systems used to operate vehicles typically require one or more powering up/initialization processes that are performed while starting up the vehicle. An initialization process generally insures that the control system and components are ready for operation prior to a user beginning to use (e.g., ride) the vehicle. In some embodiments, during the initialization process, the vehicle control system determines whether the various actuators (e.g., motors for applying torque to the transporter's ground-contacting elements for applying torque to an underlying surface of the transporter) and sensors (e.g., angular rate sensors, tilt sensors, accelerometers) are functioning properly. If the actuators and sensors are functioning properly, the vehicle control system completes the initialization and then notifies a user that the vehicle is functioning properly and ready for use.
Balancing transporters (e.g., the transporter 10 of
In this embodiment, the processor 620 receives the orientation signals 608, angular rate signals 616 and a variety of other signals as may be necessary to control the operation of the transporter. In one embodiment of the invention, the processor 620 implements the methods described with respect to
The method also involves determining 712 if one or more of the plurality of angular rate sensor signals output by the angular rate sensors have an offset below a threshold, based on the plurality of orientation signals. If the angular rate sensor signals output by the angular rate sensors have offset below the threshold (e.g., pre-specified by the manufacturer) the controller initializes a pitch state estimator 716 for controlling operation of the transporter. The controller initializes the pitch state estimator 716 based on output of the one or more angular rate sensors. The method also involves controlling the operation of the transporter 720 based on at least one output of the pitch state estimator.
In some embodiments, the threshold is set at a level that is required to ensure pitch state estimator will function properly. At values at or above the threshold, the pitch state estimator will not provide proper pitch state signals and the controller (e.g., controller 628 of
Referring to
In some embodiments, the controller repeats steps 704, 708 and 712 until the signal output offsets of the angular rate sensors are below the threshold. This may be a useful operating condition where, for example, a user may be inadvertently moving the body of the transporter during initialization and the angular rate sensors are not actually experiencing a fault (e.g., the angular rate sensors do not actually have an offset greater than a predefined threshold).
In some embodiments, the initialization of the pitch estimator 716 is terminated if one or more of the orientation signals equals or exceeds its corresponding threshold value. In various embodiments of the invention, step 712 above involves one or more of the following steps:
In one embodiment of the invention, geometry of the transporter, location of sensors and actuators on the transporter and the ability of the pitch state estimator module 500 of
Further, the following orientation signals associated with the transporter were used to determine if one or more of the plurality of angular rate sensors had a signal output offset below the threshold (20.7 degrees per second):
In various embodiments, the disclosed methods may be implemented as a computer program product for use with a computer system. Such implementations may include a series of computer instructions fixed either on a tangible medium, such as a computer readable medium (e.g., a diskette, CD-ROM, ROM, or fixed disk) or transmittable to a computer system, via a modem or other interface device, such as a communications adapter connected to a network over a medium. The medium may be either a tangible medium (e.g., optical or analog communications lines) or a medium implemented with wireless techniques (e.g., microwave, infrared or other transmission techniques). The series of computer instructions embodies all or part of the functionality previously described herein with respect to the system. Those skilled in the art should appreciate that such computer instructions can be written in a number of programming languages for use with many computer architectures or operating systems.
Furthermore, such instructions may be stored in any memory device, such as semiconductor, magnetic, optical or other memory devices, and may be transmitted using any communications technology, such as optical, infrared, microwave, or other transmission technologies. It is expected that such a computer program product may be distributed as a removable medium with accompanying printed or electronic documentation (e.g., shrink wrapped software), preloaded with a computer system (e.g., on system ROM or fixed disk), or distributed from a server or electronic bulletin board over the network (e.g., the Internet or World Wide Web). Of course, some embodiments of the invention may be implemented as a combination of both software (e.g., a computer program product) and hardware. Still other embodiments of the invention are implemented as entirely hardware, or entirely software (e.g., a computer program product).
Variations, modifications, and other implementations of what is described herein will occur to those of ordinary skill in the art without departing from the spirit and the scope of the invention as claimed. Accordingly, the invention is to be defined not by the preceding illustrative description but instead by the spirit and scope of the following claims.