CN112525143A - Method for determining installation angle of equipment and vehicle-mounted terminal - Google Patents

Method for determining installation angle of equipment and vehicle-mounted terminal Download PDF

Info

Publication number
CN112525143A
CN112525143A CN201910887036.9A CN201910887036A CN112525143A CN 112525143 A CN112525143 A CN 112525143A CN 201910887036 A CN201910887036 A CN 201910887036A CN 112525143 A CN112525143 A CN 112525143A
Authority
CN
China
Prior art keywords
estimated
state quantity
vehicle
moment
angular speed
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201910887036.9A
Other languages
Chinese (zh)
Other versions
CN112525143B (en
Inventor
王培光
管守奎
韩永根
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Momenta Technology Co Ltd
Original Assignee
Beijing Chusudu Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Chusudu Technology Co ltd filed Critical Beijing Chusudu Technology Co ltd
Priority to CN201910887036.9A priority Critical patent/CN112525143B/en
Publication of CN112525143A publication Critical patent/CN112525143A/en
Application granted granted Critical
Publication of CN112525143B publication Critical patent/CN112525143B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • G—PHYSICS
    • G07—CHECKING-DEVICES
    • G07C—TIME OR ATTENDANCE REGISTERS; REGISTERING OR INDICATING THE WORKING OF MACHINES; GENERATING RANDOM NUMBERS; VOTING OR LOTTERY APPARATUS; ARRANGEMENTS, SYSTEMS OR APPARATUS FOR CHECKING NOT PROVIDED FOR ELSEWHERE
    • G07C5/00—Registering or indicating the working of vehicles
    • G07C5/08—Registering or indicating performance data other than driving, working, idle, or waiting time, with or without registering driving, working, idle or waiting time
    • G07C5/0808—Diagnosing performance data

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
  • Navigation (AREA)

Abstract

The embodiment of the invention discloses a method for determining an equipment installation angle and a vehicle-mounted terminal. The method comprises the following steps: determining an actual vehicle angular velocity according to the equipment angular velocity at the first moment when the vehicle turns and the angular velocity characteristic when the vehicle turns; acquiring estimated state quantities including an estimated equipment angular velocity and an estimated equipment installation angle at a second moment; determining a measurement equation between the estimated state quantity and the vehicle angular speed according to the preset relationship among the estimated state quantity, the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed; determining a predicted state quantity according to the state equation and the estimated state quantity; converting the predicted state quantity according to a measurement equation to obtain a predicted vehicle angular speed; and modifying the predicted state quantity according to the deviation between the actual vehicle angular speed and the predicted vehicle angular speed to obtain an estimated state quantity containing the estimated equipment installation angle at the first moment. By applying the scheme provided by the embodiment of the invention, the more accurate installation angle of the equipment can be determined in real time.

Description

Method for determining installation angle of equipment and vehicle-mounted terminal
Technical Field
The invention relates to the technical field of intelligent driving, in particular to a method for determining an equipment installation angle and a vehicle-mounted terminal.
Background
In an on-board positioning system, a smart vehicle is generally equipped with a sensor such as an Inertial Measurement Unit (IMU). The vehicle-mounted positioning system can measure the position and the state of the vehicle according to the data output by the IMU. In this measurement, it is generally necessary to know a transformation matrix between the IMU coordinate system and the body coordinate system, which is related to the mounting angle of the IMU with respect to the body coordinate system. By calibration, the mounting angle can be obtained roughly. However, as the smart vehicle continues to travel, the setting angle may change slowly. Because the precision requirement on data in the technical field of intelligent driving is very high, if a new installation angle is determined in a recalibration mode, the real-time requirement cannot be met. Therefore, a method capable of determining a more accurate installation angle of the device in real time is required.
Disclosure of Invention
The invention provides a method for determining an equipment installation angle and a vehicle-mounted terminal, which are used for determining a more accurate equipment installation angle in real time. The specific technical scheme is as follows.
In a first aspect, an embodiment of the present invention provides a method for determining an installation angle of a device, where the method includes:
acquiring the angular speed of equipment acquired by motion detection equipment at a first moment when a vehicle turns;
determining the actual vehicle angular speed at a first moment according to the equipment angular speed and a preset angular speed characteristic when the vehicle turns;
acquiring an estimated state quantity at a second moment; wherein estimating the state quantity comprises: estimating the angular velocity of the equipment and the installation angle of the equipment; the second moment is the previous moment of the first moment;
determining a measurement equation between the estimated state quantity and the vehicle angular speed according to the estimated state quantity at the second moment and the preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed; determining the predicted state quantity at the first moment according to a preset state equation and the estimated state quantity at the second moment;
converting the predicted state quantity according to the measurement equation to obtain a predicted vehicle angular speed at a first moment;
and modifying the predicted state quantity according to the deviation between the actual vehicle angular speed and the predicted vehicle angular speed to obtain an estimated state quantity comprising the estimated equipment angular speed at the first moment and the estimated equipment installation angle at the first moment.
Optionally, the step of determining the actual vehicle angular velocity at the first time according to the device angular velocity and a preset angular velocity characteristic of the vehicle during turning includes:
and taking the preset angular speed direction when the vehicle turns as the direction of the actual vehicle angular speed at the first moment, and determining the size of the device angular speed as the size of the actual vehicle angular speed.
Optionally, each estimated state quantity comprises an estimated state quantity mean value and an estimated state quantity confidence coefficient;
the step of determining a measurement equation between the estimated state quantity and the vehicle angular velocity according to the estimated state quantity at the second moment and the preset relationship among the vehicle angular velocity, the estimated equipment installation angle and the estimated equipment angular velocity includes:
determining a measurement equation between the estimated state quantity mean value and the vehicle angular speed according to the estimated state quantity mean value at the second moment and the preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed;
the step of determining the predicted state quantity at the first time according to the preset state equation and the estimated state quantity at the second time includes:
determining a predicted state quantity average value at the first moment according to a preset state equation and the estimated state quantity average value at the second moment; determining the confidence coefficient of the predicted state quantity at the first moment according to a preset state equation and the confidence coefficient of the estimated state quantity at the second moment;
the step of converting the predicted state quantity according to the measurement equation to obtain the predicted vehicle angular velocity at the first time includes:
converting the predicted state quantity average value according to the measurement equation to obtain a predicted vehicle angular speed at a first moment;
the step of modifying the predicted state quantity according to the deviation between the actual angular velocity and the predicted angular velocity to obtain an estimated state quantity including an estimated device angular velocity at a first time and an estimated device installation angle at the first time includes:
and modifying the predicted state quantity average value according to the deviation between the actual vehicle angular speed and the predicted state quantity confidence coefficient to obtain an estimated state quantity average value containing the estimated equipment angular speed at the first moment and the estimated equipment installation angle at the first moment.
Optionally, the step of determining a measurement equation between the estimated state quantity mean value and the vehicle angular velocity according to the estimated state quantity mean value at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity includes:
determining a rotation matrix between the vehicle angular speed and the estimated equipment angular speed according to the estimated equipment installation angle in the estimated state quantity mean value at the second moment and a preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed;
and constructing a measurement equation between the average value of the estimated state quantities and the angular speed of the vehicle according to the relationship among the angular speed of the vehicle, the angular speed of the estimation equipment and the rotation matrix.
Optionally, the step of determining a rotation matrix between the vehicle angular velocity and the estimated device angular velocity according to the estimated device installation angle in the estimated state quantity mean value at the second time and a preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity includes:
determining a rotation matrix between the vehicle angular velocity and the estimated device angular velocity according to the following formula based on a preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity
Figure BDA0002207612940000031
Figure BDA0002207612940000032
Wherein θ and r are a modulo sum unit vector of the estimated device installation angle in the estimated state quantity average value at the second time, respectively, and r ═ rx，ry，rz]TThe I is an identity matrix;
the step of constructing a measurement equation between the estimated state quantity mean value and the vehicle angular velocity according to the relationship among the vehicle angular velocity, the estimated device angular velocity, and the rotation matrix includes:
according to the relation formula between the vehicle angular speed, the estimated equipment angular speed and the rotation matrix
Figure BDA0002207612940000033
Figure BDA0002207612940000034
The following measurement equation between the estimated state quantity mean value and the vehicle angular speed is constructed:
ωv＝Ct·μ
Figure BDA0002207612940000035
wherein, the ω isvIs the angular velocity of the vehicle, ωiTo estimate the angular velocity of the device, said
Figure BDA0002207612940000036
For the rotation matrix, the μ is an estimated state quantity mean value, μ ═ ωi，α]TWhere α is an estimated device mounting angle, and B0Is a zero matrix corresponding to the vector dimension of the estimated installation angle of the device.
Optionally, the step of determining the predicted state quantity average value at the first time according to a preset state equation and the estimated state quantity average value at the second time includes:
according toFormula (II)
Figure BDA0002207612940000041
Determining a predicted state quantity average value at a first moment; wherein, the
Figure BDA00022076129400000412
Is the mean value of the predicted state quantities at the first time t, mut-1The estimated state quantity average value at the second moment t-1 is obtained, and A is the preset state equation;
the step of determining the confidence of the predicted state quantity at the first time according to a preset state equation and the confidence of the estimated state quantity at the second time includes:
according to the formula
Figure BDA0002207612940000043
Determining a confidence of the predicted state quantity at a first moment; wherein, the
Figure BDA0002207612940000044
For confidence of predicted state quantity at first moment, said ∑t-1The confidence of the estimated state quantity at the second moment is, and R is a preset state equation confidence;
the step of converting the predicted state quantity average value according to the measurement equation to obtain the predicted vehicle angular velocity at the first moment includes:
according to the measurement equation omegav＝Ctμ, averaging the predicted state quantities
Figure BDA0002207612940000045
Obtaining the predicted vehicle angular velocity at the first time as mu
Figure BDA0002207612940000046
Wherein,
Figure BDA0002207612940000047
the step of modifying the predicted state quantity average value according to the deviation between the actual vehicle angular velocity and the predicted state quantity confidence coefficient to obtain an estimated state quantity average value including an estimated device angular velocity at a first time and an estimated device mounting angle at the first time includes:
modifying the average value of the predicted state quantities according to the following formula to obtain an average value mu of the estimated state quantities, wherein the average value mu comprises the angular speed of the estimated equipment at the first moment and the installation angle of the estimated equipment at the first momentt：
Figure BDA0002207612940000048
Wherein,
Figure BDA0002207612940000049
the above-mentioned
Figure BDA00022076129400000410
As the confidence of the predicted state quantity, the CtIs the coefficient in the measurement equation, Q is the preset measurement equation confidence, ZtFor the actual vehicle angular velocity, the
Figure BDA00022076129400000411
For the predicted vehicle angular velocity, T is transposed.
In a second aspect, an embodiment of the present invention provides a vehicle-mounted terminal, including: a processor and a motion detection device; the processor includes:
the first acquisition module is used for acquiring the device angular speed acquired by the motion detection device at a first moment when the vehicle turns;
the first determining module is used for determining the actual vehicle angular speed at a first moment according to the equipment angular speed and a preset angular speed characteristic when the vehicle turns;
the second acquisition module is used for acquiring the estimated state quantity at the second moment; wherein estimating the state quantity comprises: estimating the angular velocity of the equipment and the installation angle of the equipment; the second moment is the previous moment of the first moment;
the second determination module is used for determining a measurement equation between the estimated state quantity and the vehicle angular speed according to the estimated state quantity at the second moment and the preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed; determining the predicted state quantity at the first moment according to a preset state equation and the estimated state quantity at the second moment;
the conversion module is used for converting the predicted state quantity according to the measurement equation to obtain the predicted vehicle angular speed at the first moment;
and the modification module is used for modifying the predicted state quantity according to the deviation between the actual vehicle angular speed and the predicted vehicle angular speed to obtain the estimated state quantity comprising the estimated equipment angular speed at the first moment and the estimated equipment installation angle at the first moment.
Optionally, the first determining module is specifically configured to:
and taking the preset angular speed direction when the vehicle turns as the direction of the actual vehicle angular speed at the first moment, and determining the size of the device angular speed as the size of the actual vehicle angular speed.
Optionally, each estimated state quantity comprises an estimated state quantity mean value and an estimated state quantity confidence coefficient;
the second determining module determines a measurement equation between the estimated state quantity and the vehicle angular velocity according to the estimated state quantity at the second moment and the preset relationship among the vehicle angular velocity, the estimated equipment installation angle and the estimated equipment angular velocity, and includes:
determining a measurement equation between the estimated state quantity mean value and the vehicle angular speed according to the estimated state quantity mean value at the second moment and the preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed;
the second determining module, according to a preset state equation and the estimated state quantity at the second time, determines the predicted state quantity at the first time, and includes:
determining a predicted state quantity average value at the first moment according to a preset state equation and the estimated state quantity average value at the second moment; determining the confidence coefficient of the predicted state quantity at the first moment according to a preset state equation and the confidence coefficient of the estimated state quantity at the second moment;
the conversion module is specifically configured to:
converting the predicted state quantity average value according to the measurement equation to obtain a predicted vehicle angular speed at a first moment;
the modification module is specifically configured to:
and modifying the predicted state quantity average value according to the deviation between the actual vehicle angular speed and the predicted state quantity confidence coefficient to obtain an estimated state quantity average value containing the estimated equipment angular speed at the first moment and the estimated equipment installation angle at the first moment.
Optionally, when the second determining module determines the measurement equation between the estimated state quantity mean value and the vehicle angular velocity according to the estimated state quantity mean value at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity, the second determining module includes:
determining a rotation matrix between the vehicle angular speed and the estimated equipment angular speed according to the estimated equipment installation angle in the estimated state quantity mean value at the second moment and a preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed;
and constructing a measurement equation between the average value of the estimated state quantities and the angular speed of the vehicle according to the relationship among the angular speed of the vehicle, the angular speed of the estimation equipment and the rotation matrix.
Optionally, when the second determining module determines the rotation matrix between the vehicle angular velocity and the estimated device angular velocity according to the estimated device installation angle in the estimated state quantity mean value at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity, the second determining module includes:
estimating the installation angle of the equipment based on the angular velocity of the vehicleDetermining a rotation matrix between the angular velocity of the vehicle and the estimated device angular velocity, using the following formula, which is derived from a preset relationship between the estimated device angular velocities
Figure BDA0002207612940000061
Figure BDA0002207612940000062
Wherein θ and r are a modulo sum unit vector of the estimated device installation angle in the estimated state quantity average value at the second time, respectively, and r ═ rx，ry，rz]TThe I is an identity matrix;
the second determination module, when constructing a measurement equation between the estimated state quantity average value and the vehicle angular velocity according to the relationship between the vehicle angular velocity, the estimated device angular velocity, and the rotation matrix, includes:
according to the relation formula between the vehicle angular speed, the estimated equipment angular speed and the rotation matrix
Figure BDA0002207612940000063
Figure BDA0002207612940000071
The following measurement equation between the estimated state quantity mean value and the vehicle angular speed is constructed:
ωv＝Ct·μ
Figure BDA0002207612940000072
wherein, the ω isvIs the angular velocity of the vehicle, ωiTo estimate the angular velocity of the device, said
Figure BDA0002207612940000073
For the rotation matrix, the μ is an estimated state quantity mean,
Figure BDA0002207612940000074
the alpha is an estimated equipment mounting angle, and B0Is a zero matrix corresponding to the vector dimension of the estimated installation angle of the device.
Optionally, when the second determining module determines the predicted state quantity average value at the first time according to a preset state equation and the estimated state quantity average value at the second time, the second determining module includes:
according to the formula
Figure BDA0002207612940000075
Determining a predicted state quantity average value at a first moment; wherein, the
Figure BDA0002207612940000076
Is the mean value of the predicted state quantities at the first time t, mut-1The estimated state quantity average value at the second moment t-1 is obtained, and A is the preset state equation;
the second determining module, when determining the confidence of the predicted state quantity at the first time according to a preset state equation and the confidence of the estimated state quantity at the second time, includes:
according to the formula
Figure BDA0002207612940000077
Determining a confidence of the predicted state quantity at a first moment; wherein, the
Figure BDA0002207612940000078
For confidence of predicted state quantity at first moment, said ∑t-1The confidence of the estimated state quantity at the second moment is, and R is a preset state equation confidence;
the conversion module is specifically configured to:
according to the measurement equation omegav＝Ctμ, averaging the predicted state quantities
Figure BDA0002207612940000079
Obtaining the predicted vehicle angular velocity at the first time as mu
Figure BDA00022076129400000710
Wherein,
Figure BDA00022076129400000711
the modification module is specifically configured to:
modifying the average value of the predicted state quantities according to the following formula to obtain an average value mu of the estimated state quantities, wherein the average value mu comprises the angular speed of the estimated equipment at the first moment and the installation angle of the estimated equipment at the first momentt：
Figure BDA00022076129400000712
Wherein,
Figure BDA00022076129400000713
the above-mentioned
Figure BDA00022076129400000714
As the confidence of the predicted state quantity, the CtIs the coefficient in the measurement equation, Q is the preset measurement equation confidence, ZtFor the actual vehicle angular velocity, the
Figure BDA00022076129400000715
For the predicted vehicle angular velocity, T is transposed.
As can be seen from the above, the method for determining the installation angle of the device and the vehicle-mounted terminal provided in the embodiments of the present invention provide a virtual measurement of the angular velocity of the vehicle, and a measurement equation between the estimated state quantity and the vehicle angular velocity is determined using a preset relationship among the vehicle angular velocity, the estimated device installation angle, the estimated device angular velocity, and the like as the estimated state quantity, further, the predicted vehicle angular velocity at the present time is determined using the measurement equation and the predicted state quantity predicted from the state equation, and based on the deviation between the actual vehicle angular velocity and the predicted vehicle angular velocity, and modifying the predicted state quantity to obtain the estimated state quantity at the first moment, so that the obtained estimated state quantity is as close to a true value as possible, and a more accurate device installation angle can be determined in real time. Of course, not all of the advantages described above need to be achieved at the same time in the practice of any one product or method of the invention.
The innovation points of the embodiment of the invention comprise:
1. a virtual quantity measurement is provided, namely the angular speed of the vehicle, the state quantity at the last moment is predicted according to a state equation, the predicted value obtained in the prediction stage is modified according to the deviation between the actual quantity measurement and the predicted quantity measurement, a new estimated value closer to the actual value can be obtained, and the more accurate installation angle of the equipment can be determined in real time.
2. The determination of the virtual quantity measurement with the preset angular velocity direction as the vehicle angular velocity direction and the device angular velocity magnitude as the magnitude of the vehicle angular velocity provides a concrete way of implementation.
3. Confidence degrees are introduced for the estimated state quantities, namely each estimated state quantity mean value corresponds to one confidence degree, and the estimated state quantities are calculated according to the confidence degrees, so that the determined estimated state quantities can be more accurate.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below. It is to be understood that the drawings in the following description are merely exemplary of some embodiments of the invention. For a person skilled in the art, without inventive effort, further figures can be obtained from these figures.
Fig. 1 is a schematic flow chart of a method for determining a mounting angle of a device according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a relationship between a device coordinate system and a vehicle body coordinate system according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of the angular velocity direction of a vehicle during a turn according to an embodiment of the present invention;
fig. 4 is another schematic flow chart of a method for determining a mounting angle of a device according to an embodiment of the present invention;
fig. 5 is a schematic structural diagram of a vehicle-mounted terminal according to an embodiment of the present invention.
Detailed Description
The technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. It is to be understood that the described embodiments are merely a few embodiments of the invention, and not all embodiments. All other embodiments, which can be obtained by a person skilled in the art without inventive effort based on the embodiments of the present invention, are within the scope of the present invention.
It is to be noted that the terms "comprises" and "comprising" and any variations thereof in the embodiments and drawings of the present invention are intended to cover non-exclusive inclusions. For example, a process, method, system, article, or apparatus that comprises a list of steps or elements is not limited to only those steps or elements listed, but may alternatively include other steps or elements not listed, or inherent to such process, method, article, or apparatus.
The embodiment of the invention discloses a method for determining an equipment installation angle and a vehicle-mounted terminal, which can determine the more accurate equipment installation angle in real time. The following provides a detailed description of embodiments of the invention.
Fig. 1 is a schematic flow chart of a method for determining a mounting angle of a device according to an embodiment of the present invention. The method is applied to the electronic equipment. The electronic device may be a general Computer, a server, an intelligent terminal device, or the like, or may be a vehicle-mounted Computer or a vehicle-mounted terminal such as an Industrial Personal Computer (IPC). The method specifically comprises the following steps.
S110: the device angular velocity acquired by the motion detection device at the first time when the vehicle turns is acquired.
The motion detection device may be an Inertial Measurement Unit (IMU) or other device capable of performing similar functions to those performed by the IMU. The motion detection apparatus may be provided in a vehicle for detecting a motion state of the vehicle. For example, the motion detection device may acquire motion state quantities such as angular velocity and/or acceleration. The vehicle may be a smart driving vehicle. In the present embodiment, in order to estimate the device installation angle, the vehicle and the motion detection device may be considered as a system.
The motion detection device may collect data according to a preset collection period and determine a collection time of the collected data.
The angular velocity of the device acquired by the motion detection device may be understood as the angular velocity of the motion detection device. The angular velocity may be represented by three angular velocity components in the device coordinate system. The device coordinate system is the coordinate system in which the motion detection device is located.
In vehicle positioning, a body coordinate system is often used. The specific position of the body coordinate system is related to the selected coordinate axis direction and the origin position, for example, the body coordinate system may use the center points of the two rear wheels as the origin, and the coordinate axis directions are the right, front, and upper sides of the vehicle.
The equipment installation angle refers to a relative rotation angle between the vehicle body coordinate system and the equipment coordinate system. Referring to fig. 2, a relationship between the device coordinate system and the vehicle body coordinate system is schematically shown. Examples of a device coordinate system and a body coordinate system are labeled. The installation angle of the equipment may vary depending on the installation position of the equipment in the vehicle.
Ideally, the motion detection device is rigidly connected to the vehicle and the mounting angle of the device is unchanged. In which case the equipment mounting angle can be determined by calibration. In practical applications, however, the installation angle of the equipment may change slowly with the travel and vibration of the vehicle. In this case, the installation angle of the device can be estimated in real time in the manner of the present embodiment.
In this embodiment, whether the vehicle is in a turning state may be detected in the following manner: whether the device angular velocity output by the motion detection device is greater than a threshold value is detected, and if so, it is determined that the vehicle is in a turning state.
S120: and determining the actual vehicle angular speed at the first moment according to the equipment angular speed and the preset angular speed characteristic when the vehicle turns.
Specifically, the present step may include the following embodiments: and taking the preset angular speed direction when the vehicle turns as the direction of the actual vehicle angular speed at the first moment, and determining the size of the device angular speed as the size of the actual vehicle angular speed. For example, the preset angular velocity direction of the vehicle while turning may be, but is not limited to, vertically upward.
Considering the motion characteristics of the vehicle during turning, when the vehicle turns on a horizontal plane, see the schematic diagram in fig. 3, wherein the vehicle is turning left along the road, it can be considered that the vehicle angular velocity is vertically upward under the vehicle body coordinate system during turning, and the vehicle angular velocity can adopt [0,0, | ω |, ] c | ω |,/c |)]TAnd (4) showing. Wherein, | ω | is the magnitude of the angular velocity of the equipment, and the z-axis is vertically upward in the vehicle body coordinate system.
S130: and acquiring the estimated state quantity at the second moment.
Wherein estimating the state quantity comprises: estimating the device angular velocity and estimating the device mounting angle. Since the device mounting angle slowly changes over time, the true device mounting angle at the first time is unknown, and the device angular velocity output by the motion detection device itself is not the true device angular velocity corresponding to the true device mounting angle at the present time. The estimated plant angular velocity is an estimate of the true plant angular velocity given by the system at the current time that the system deems optimal. The estimated plant setting angle is an estimated value of a real plant setting angle which is considered to be optimal by the system at the current moment given by the system. The above system may be understood as an electronic device as the execution subject.
The estimated state quantity at the second time is an estimated state quantity determined according to the device angular velocity acquired at the second time. The second time is the previous time of the first time. The operation of determining the installation angle of the device may be performed periodically according to a preset frequency, and the last time may be understood as the time corresponding to the last time of determining the installation angle of the device.
The estimated device angular velocity in the estimated state quantity at the initial time may be a device angular velocity directly output by the motion detection device, and the estimated device installation angle in the estimated state quantity at the initial time may be a device installation angle calibrated in advance.
S140: determining a measurement equation between the estimated state quantity and the vehicle angular speed according to the estimated state quantity at the second moment and the preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed; and determining the predicted state quantity at the first moment according to a preset state equation and the estimated state quantity at the second moment.
Since the estimated equipment angular velocity is an estimated value of the true equipment angular velocity at the present time which is considered by the system to be optimal, the estimated equipment mounting angle is an estimated value of the true equipment mounting angle at the present time which is considered by the system to be optimal, the vehicle angular velocity corresponds to the vehicle body coordinate system, and the estimated equipment angular velocity corresponds to the equipment coordinate system, the conversion matrix between the two coordinate systems is related to the equipment mounting angle.
In the present embodiment, the vehicle angular velocity is assumed to be a virtual quantity measurement, and a virtual sensor is assumed to be present, and the virtual sensor can measure the vehicle angular velocity. When a measurement is obtained, the state quantity can be estimated from the measurement. The measurement equation enables interconversion between the state quantity and the vehicle angular velocity (measurement quantity).
The state equation can predict the state quantity at the next time from the state quantity at the previous time. The predicted state quantity at the first time includes the predicted equipment mounting angle at the first time and the predicted equipment angular velocity at the first time.
S150: and converting the predicted state quantity according to the measurement equation to obtain the predicted vehicle angular speed at the first moment.
And substituting the predicted state quantity at the first moment into the measurement equation to obtain the predicted vehicle angular speed at the first moment.
S160: and modifying the predicted state quantity according to the deviation between the actual vehicle angular speed and the predicted vehicle angular speed to obtain the estimated state quantity comprising the estimated equipment angular speed at the first moment and the estimated equipment installation angle at the first moment.
The predicted vehicle angular velocity is a predicted amount obtained by using the estimated device attachment angle at the second time as the estimated device attachment angle at the first time, and a more accurate device attachment angle after correction can be obtained on the basis of the estimated device attachment angle at the second time based on the deviation between the actual vehicle angular velocity and the predicted vehicle angular velocity.
As can be seen from the above, the present embodiment provides a virtual quantity measuring vehicle angular velocity, and uses the estimated device angular velocity and the estimated device installation angle, etc. as the estimated state quantities, and uses the preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity to determine the measurement equation between the estimated state quantity and the vehicle angular velocity, and further uses the measurement equation and the predicted state quantity predicted according to the state equation to determine the predicted vehicle angular velocity at the current time, and modifies the predicted state quantity according to the deviation between the actual vehicle angular velocity and the predicted vehicle angular velocity to obtain the estimated state quantity at the first time, so that the obtained estimated state quantity can be as close to the true value as possible, and thus a more accurate device installation angle can be determined in real time.
In another embodiment of the present invention, based on the embodiment shown in fig. 1, when each estimated state quantity includes an estimated state quantity mean and an estimated state quantity confidence, the flowchart shown in fig. 2 may be obtained, and the method specifically includes the following steps:
s210: the device angular velocity acquired by the motion detection device at the first time when the vehicle turns is acquired.
S220: and determining the actual vehicle angular speed at the first moment according to the equipment angular speed and the preset angular speed characteristic when the vehicle turns.
S230: and acquiring the estimated state quantity at the second moment.
Wherein estimating the state quantity comprises: estimating the angular velocity of the equipment and the installation angle of the equipment; the second time is the previous time of the first time.
The steps S210 to S230 are respectively the same as the steps S110 to S130 in the embodiment shown in fig. 1, and specific description can refer to the description of corresponding parts in fig. 1, which is not repeated herein.
S240: determining a measurement equation between the estimated state quantity mean value and the vehicle angular speed according to the estimated state quantity mean value at the second moment and the preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed; and determining the predicted state quantity mean value at the first moment according to the preset state equation and the estimated state quantity mean value at the second moment, and determining the predicted state quantity confidence coefficient at the first moment according to the preset state equation and the estimated state quantity confidence coefficient at the second moment.
Wherein, the confidence of the state quantity can be represented by covariance. The confidence of the estimated state quantity at the initial time may be a preset value.
In this embodiment, the measurement equation can realize conversion between the estimated state quantity average value and the vehicle angular velocity. The state equation can predict the estimated state quantity mean value at the next moment according to the estimated state quantity mean value at the previous moment, and can also predict the estimated state quantity confidence coefficient at the next moment according to the estimated state quantity confidence coefficient at the previous moment.
S250: and converting the average value of the predicted state quantities according to a measurement equation to obtain the predicted vehicle angular speed at the first moment.
And substituting the predicted state quantity average value at the first moment into the measurement equation to obtain the predicted vehicle angular speed at the first moment.
S260: and modifying the predicted state quantity average value according to the deviation between the actual vehicle angular speed and the predicted state quantity confidence coefficient to obtain an estimated state quantity average value containing the estimated equipment angular speed at the first moment and the estimated equipment installation angle at the first moment.
The estimated state quantity confidence at the first time may be determined based on the metrology equation and the predicted state quantity confidence at the first time.
In summary, in the embodiment, confidence degrees are introduced for the estimated state quantities, that is, each estimated state quantity average value corresponds to one confidence degree, and the estimated state quantity average value is calculated according to the deviation between the actual vehicle angular velocity and the predicted state confidence degree, so that the determined estimated state quantities can be more accurate.
In another embodiment of the present invention, based on the embodiment shown in fig. 2, the step of determining the measurement equation between the estimated state quantity mean value and the vehicle angular velocity according to the estimated state quantity mean value at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle and the estimated device angular velocity in step S240 includes the following steps 1 and 2.
Step 1: and determining a rotation matrix between the vehicle angular speed and the estimated device angular speed according to the estimated device installation angle in the estimated state quantity mean value at the second moment and a preset relation among the vehicle angular speed, the estimated device installation angle and the estimated device angular speed.
The step may specifically include: determining a rotation matrix between the vehicle angular velocity and the estimated device angular velocity according to the following formula based on a preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity
Figure BDA0002207612940000131
Figure BDA0002207612940000141
Where θ and r are a modulo sum unit vector of the estimated device installation angle in the estimated state quantity average value at the second time, respectively, and r ═ rx，ry，rz]TAnd I is an identity matrix with dimensions of 3 x 3. The above formula can be obtained according to the rodriex formula.
Step 2: and constructing a measurement equation between the average value of the estimated state quantities and the angular speed of the vehicle according to the relationship among the angular speed of the vehicle, the angular speed of the estimation equipment and the rotation matrix.
The step may specifically include: according to the relation formula between the vehicle angular speed, the estimated equipment angular speed and the rotation matrix
Figure BDA0002207612940000142
The following measurement equation between the estimated state quantity mean value and the vehicle angular speed is constructed:
ωv＝Ct·μ
Figure BDA0002207612940000143
wherein, ω isvAs angular velocity, ω, of the vehicleiIn order to estimate the angular velocity of the device,
Figure BDA0002207612940000144
is a rotation matrix, mu is an estimated state quantity mean value, mu ═ omegai，α]TAlpha is the estimated installation angle of the device, B0Is a zero matrix corresponding to the vector dimension of the estimated installation angle of the device. E.g. CtIt may be a 3 x 6 dimensional matrix,
Figure BDA0002207612940000145
is a 3X 3 dimensional matrix, B0Is a zero matrix of 3 x 3 dimensions.
In summary, the present embodiment provides a specific formula for determining a measurement equation between the mean estimated state quantity and the vehicle angular velocity according to the mean estimated state quantity at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity, and provides a specific implementation manner for implementing the present embodiment.
In another embodiment of the present invention, based on the embodiment shown in fig. 2, the step of determining the predicted state quantity average value at the first time according to the preset state equation and the estimated state quantity average value at the second time in step S240 may include: according to the formula
Figure BDA0002207612940000146
The mean value of the predicted state quantities at the first time instant is determined.
Wherein,
Figure BDA0002207612940000147
is the mean value of the predicted state quantities, mu, at the first time tt-1Is the mean value of the estimated state quantities at the second moment t-1, and A is a preset state equation.
In step S240, the step of determining the confidence of the predicted state quantity at the first time according to the preset state equation and the confidence of the estimated state quantity at the second time may include: according to the formula
Figure BDA0002207612940000148
A predicted state quantity confidence for the first time instance is determined.
Wherein,
Figure BDA0002207612940000151
as confidence of predicted state quantity at first moment, sigmat-1And R is the confidence coefficient of the estimated state quantity at the second moment, and R is the preset confidence coefficient of the state equation.
The step of converting the predicted mean state quantity according to the measurement equation in step S250 to obtain the predicted vehicle angular velocity at the first time may include:
according to the measurement equation omegav＝Ctμ, will predict the mean value of the state quantities
Figure BDA0002207612940000152
Obtaining the predicted vehicle angular velocity at the first time as mu
Figure BDA0002207612940000153
Wherein,
Figure BDA0002207612940000154
in step S260, the step of modifying the predicted state quantity average value according to the deviation between the actual vehicle angular velocity and the predicted state quantity confidence coefficient to obtain an estimated state quantity average value including the estimated device angular velocity at the first time and the estimated device mounting angle at the first time includes:
modifying the mean value of the predicted state quantities according to the following formula to obtain a mean value containing a first parameterEstimated device angular velocity at time and estimated state quantity mean value mu of estimated device installation angle at first timet：
Figure BDA0002207612940000155
Wherein,
Figure BDA0002207612940000156
Figure BDA0002207612940000157
to predict state quantity confidence, CtFor coefficients in the measurement equation, Q is the preset measurement equation confidence, ZtIn order to be the actual angular velocity of the vehicle,
Figure BDA0002207612940000158
in order to predict the angular velocity of the vehicle,
Figure BDA0002207612940000159
t is a transposed symbol, which is the deviation between the actual vehicle angular velocity and the predicted vehicle angular velocity. Estimating the mean value of the state quantities mutIncluding estimating the device mounting angle and estimating the device angular velocity.
According to the measurement equation CtAnd confidence of predicted state quantity at first time
Figure BDA00022076129400001510
The estimated state quantity confidence for the first time may be determined using the following equation:
Figure BDA00022076129400001511
wherein, sigmatAnd I is an identity matrix, and is the confidence of the estimated state quantity at the first moment.
In summary, the present embodiment provides a specific formula for implementing each step, and provides an implementable manner for implementing the present embodiment.
In another embodiment of the invention, the state quantity can be estimated by modifying the existing Kalman filter without rewriting all programs, so that the processing efficiency can be improved.
In another embodiment of the present invention, sensors such as, but not limited to, a Global Positioning System (GPS) and an IMU may be provided in the vehicle. When the vehicle is positioned according to the GPS data and the IMU data, the vehicle-mounted terminal can periodically estimate the equipment installation angle so as to update the equipment installation angle, so that the positioning of the vehicle is more accurate. In this embodiment, estimating the state quantity may include, but is not limited to: IMU pose, IMU installation angle, IMU speed, IMU acceleration, IMU angular velocity and the like. In the measurement equation
Figure BDA0002207612940000161
B in (1)0The setting may be made according to a specific parameter in the estimated state quantity.
Fig. 5 is a schematic structural diagram of a vehicle-mounted terminal according to an embodiment of the present invention. This embodiment corresponds to the embodiment shown in fig. 1. The vehicle-mounted terminal includes: a processor 510 and a motion detection device 520; the processor 510 includes:
a first obtaining module 511, configured to obtain an apparatus angular velocity acquired by the motion detection apparatus at a first time when the vehicle turns;
the first determining module 512 is configured to determine an actual vehicle angular velocity at a first moment according to the device angular velocity and a preset angular velocity characteristic of the vehicle during turning;
a second obtaining module 513, configured to obtain an estimated state quantity at a second time; wherein estimating the state quantity comprises: estimating the angular velocity of the equipment and the installation angle of the equipment; the second moment is the previous moment of the first moment;
a second determining module 514, configured to determine a measurement equation between the estimated state quantity and the vehicle angular velocity according to the estimated state quantity at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity; determining the predicted state quantity at the first moment according to a preset state equation and the estimated state quantity at the second moment;
the conversion module 515 is configured to convert the predicted state quantity according to the measurement equation to obtain a predicted vehicle angular velocity at a first moment;
and a modifying module 516, configured to modify the predicted state quantity according to a deviation between the actual vehicle angular velocity and the predicted vehicle angular velocity, to obtain an estimated state quantity including the estimated device angular velocity at the first time and the estimated device installation angle at the first time.
In another embodiment of the present invention, based on the embodiment shown in fig. 5, the first determining module 512 is specifically configured to:
and taking the preset angular speed direction when the vehicle turns as the direction of the actual vehicle angular speed at the first moment, and determining the size of the device angular speed as the size of the actual vehicle angular speed.
In another embodiment of the present invention, based on the embodiment shown in fig. 5, each estimated state quantity includes an estimated state quantity mean and an estimated state quantity confidence;
the second determining module 514, determining a measurement equation between the estimated state quantity and the vehicle angular velocity according to the estimated state quantity at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle and the estimated device angular velocity, includes:
determining a measurement equation between the estimated state quantity mean value and the vehicle angular speed according to the estimated state quantity mean value at the second moment and the preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed;
the second determining module 514, determining the predicted state quantity at the first time according to the preset state equation and the estimated state quantity at the second time, includes:
determining a predicted state quantity average value at a first moment according to a preset state equation and an estimated state quantity average value at a second moment; determining a predicted state quantity confidence coefficient at a first moment according to a preset state equation and an estimated state quantity confidence coefficient at a second moment;
the conversion module 515 is specifically configured to:
converting the average value of the predicted state quantities according to a measurement equation to obtain the predicted vehicle angular speed at the first moment;
the modification module 516 is specifically configured to:
and modifying the predicted state quantity average value according to the deviation between the actual vehicle angular speed and the predicted state quantity confidence coefficient to obtain an estimated state quantity average value containing the estimated equipment angular speed at the first moment and the estimated equipment installation angle at the first moment.
In another embodiment of the present invention, based on the above-described illustrated embodiments, the determining by the second determining module 514, the measurement equation between the estimated state quantity mean and the vehicle angular velocity according to the estimated state quantity mean at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity includes:
determining a rotation matrix between the vehicle angular speed and the estimated equipment angular speed according to the estimated equipment installation angle in the estimated state quantity mean value at the second moment and a preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed;
and constructing a measurement equation between the average value of the estimated state quantities and the angular speed of the vehicle according to the relationship among the angular speed of the vehicle, the angular speed of the estimation equipment and the rotation matrix.
In another embodiment of the present invention, based on the above-described illustrated embodiments, the determining by the second determining module 514, the rotation matrix between the vehicle angular velocity and the estimated device angular velocity according to the estimated device installation angle in the average value of the estimated state quantities at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity includes:
determining a rotation matrix between the vehicle angular velocity and the estimated device angular velocity according to the following formula based on a preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity
Figure BDA0002207612940000181
Figure BDA0002207612940000182
Where θ and r are a modulo sum unit vector of the estimated device installation angle in the estimated state quantity average value at the second time, respectively, and r ═ rx，ry，rz]TI is an identity matrix;
the second determination module, when constructing the measurement equation between the estimated state quantity average value and the vehicle angular velocity according to the relationship between the vehicle angular velocity, the estimated device angular velocity, and the rotation matrix, includes:
according to the relation formula among the vehicle angular speed, the estimated equipment angular speed and the rotation matrix
Figure BDA0002207612940000183
The following measurement equation between the estimated state quantity mean value and the vehicle angular speed is constructed:
ωv＝Ct·μ
Figure BDA0002207612940000184
wherein, ω isvAs angular velocity, ω, of the vehicleiIn order to estimate the angular velocity of the device,
Figure BDA00022076129400001810
is a rotation matrix, mu is an estimated state quantity mean value, mu ═ omegai，α]TAlpha is the estimated installation angle of the device, B0Is a zero matrix corresponding to the vector dimension of the estimated installation angle of the device.
In another embodiment of the present invention, based on the embodiment shown in fig. 1, the determining the predicted state quantity average value at the first time according to the preset state equation and the estimated state quantity average value at the second time by the second determining module 514 includes:
according to the formula
Figure BDA0002207612940000186
Determining a predicted state quantity average value at a first moment; wherein,
Figure BDA0002207612940000187
is the mean value of the predicted state quantities, mu, at the first time tt-1The estimated state quantity average value at the second moment t-1 is A, and A is a preset state equation;
the second determining module 514, when determining the confidence of the predicted state quantity at the first time according to the preset state equation and the confidence of the estimated state quantity at the second time, includes:
according to the formula
Figure BDA0002207612940000188
Determining a confidence of the predicted state quantity at a first moment; wherein,
Figure BDA0002207612940000189
as confidence of predicted state quantity at first moment, sigmat-1The confidence coefficient of the estimated state quantity at the second moment is R, and the confidence coefficient of the preset state equation is R;
the conversion module 515 is specifically configured to:
according to the measurement equation omegav＝Ctμ, will predict the mean value of the state quantities
Figure BDA0002207612940000191
Obtaining the predicted vehicle angular velocity at the first time as mu
Figure BDA0002207612940000192
Wherein,
Figure BDA0002207612940000193
the modifying module 516 is specifically configured to:
modifying the average value of the predicted state quantities according to the following formula to obtain an average value mu of the estimated state quantities, wherein the average value mu comprises the angular speed of the estimated equipment at the first moment and the installation angle of the estimated equipment at the first momentt：
Figure BDA0002207612940000194
Wherein,
Figure BDA0002207612940000195
Figure BDA0002207612940000196
to predict state quantity confidence, CtFor coefficients in the measurement equation, Q is the preset measurement equation confidence, ZtIn order to be the actual angular velocity of the vehicle,
Figure BDA0002207612940000197
to predict vehicle angular velocity, T is the transposed symbol.
The terminal embodiment and the method embodiment shown in fig. 1 are embodiments based on the same inventive concept, and the relevant points can be referred to each other. The terminal embodiment corresponds to the method embodiment, and has the same technical effect as the method embodiment, and for the specific description, reference is made to the method embodiment.
Those of ordinary skill in the art will understand that: the figures are merely schematic representations of one embodiment, and the blocks or flow diagrams in the figures are not necessarily required to practice the present invention.
Those of ordinary skill in the art will understand that: modules in the devices in the embodiments may be distributed in the devices in the embodiments according to the description of the embodiments, or may be located in one or more devices different from the embodiments with corresponding changes. The modules of the above embodiments may be combined into one module, or further split into multiple sub-modules.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims (10)

1. A method for determining a mounting angle of a device, comprising:
acquiring the angular speed of equipment acquired by motion detection equipment at a first moment when a vehicle turns;
determining the actual vehicle angular speed at a first moment according to the equipment angular speed and a preset angular speed characteristic when the vehicle turns;
acquiring an estimated state quantity at a second moment; wherein estimating the state quantity comprises: estimating the angular velocity of the equipment and the installation angle of the equipment; the second moment is the previous moment of the first moment;
determining a measurement equation between the estimated state quantity and the vehicle angular speed according to the estimated state quantity at the second moment and the preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed; determining the predicted state quantity at the first moment according to a preset state equation and the estimated state quantity at the second moment;
converting the predicted state quantity according to the measurement equation to obtain a predicted vehicle angular speed at a first moment;
and modifying the predicted state quantity according to the deviation between the actual vehicle angular speed and the predicted vehicle angular speed to obtain an estimated state quantity comprising the estimated equipment angular speed at the first moment and the estimated equipment installation angle at the first moment.
2. The method according to claim 1, wherein the step of determining the actual vehicle angular velocity at the first time based on the device angular velocity and a preset angular velocity characteristic of the vehicle while turning comprises:
and taking the preset angular speed direction when the vehicle turns as the direction of the actual vehicle angular speed at the first moment, and determining the size of the device angular speed as the size of the actual vehicle angular speed.
3. The method of claim 1, wherein each estimated state quantity comprises an estimated state quantity mean and an estimated state quantity confidence;
the step of determining a measurement equation between the estimated state quantity and the vehicle angular velocity according to the estimated state quantity at the second moment and the preset relationship among the vehicle angular velocity, the estimated equipment installation angle and the estimated equipment angular velocity includes:
determining a measurement equation between the estimated state quantity mean value and the vehicle angular speed according to the estimated state quantity mean value at the second moment and the preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed;
the step of determining the predicted state quantity at the first time according to the preset state equation and the estimated state quantity at the second time includes:
determining a predicted state quantity average value at the first moment according to a preset state equation and the estimated state quantity average value at the second moment; determining the confidence coefficient of the predicted state quantity at the first moment according to a preset state equation and the confidence coefficient of the estimated state quantity at the second moment;
the step of converting the predicted state quantity according to the measurement equation to obtain the predicted vehicle angular velocity at the first time includes:
converting the predicted state quantity average value according to the measurement equation to obtain a predicted vehicle angular speed at a first moment;
the step of modifying the predicted state quantity according to the deviation between the actual angular velocity and the predicted angular velocity to obtain an estimated state quantity including an estimated device angular velocity at a first time and an estimated device installation angle at the first time includes:
and modifying the predicted state quantity average value according to the deviation between the actual vehicle angular speed and the predicted state quantity confidence coefficient to obtain an estimated state quantity average value containing the estimated equipment angular speed at the first moment and the estimated equipment installation angle at the first moment.
4. The method according to claim 3, wherein the step of determining a measurement equation between the estimated state quantity mean value and the vehicle angular velocity based on the estimated state quantity mean value at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity includes:
determining a rotation matrix between the vehicle angular speed and the estimated equipment angular speed according to the estimated equipment installation angle in the estimated state quantity mean value at the second moment and a preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed;
and constructing a measurement equation between the average value of the estimated state quantities and the angular speed of the vehicle according to the relationship among the angular speed of the vehicle, the angular speed of the estimation equipment and the rotation matrix.
5. The method according to claim 4, wherein the step of determining the rotation matrix between the vehicle angular velocity and the estimated device angular velocity based on the estimated device installation angle in the average value of the estimated state quantities at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity includes:
determining a rotation matrix between the vehicle angular velocity and the estimated device angular velocity according to the following formula based on a preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity
Figure FDA0002207612930000031
Figure FDA0002207612930000032
Wherein θ and r are a modulo sum unit vector of the estimated device installation angle in the estimated state quantity average value at the second time, respectively, and r ═ rx，ry，rz]TThe I is an identity matrix;
the step of constructing a measurement equation between the estimated state quantity mean value and the vehicle angular velocity according to the relationship among the vehicle angular velocity, the estimated device angular velocity, and the rotation matrix includes:
according to the relation formula between the vehicle angular speed, the estimated equipment angular speed and the rotation matrix
Figure FDA0002207612930000033
Figure FDA0002207612930000034
The following measurement equation between the estimated state quantity mean value and the vehicle angular speed is constructed:
ωv＝Ct·μ
Figure FDA0002207612930000035
wherein, the ω isvIs the angular velocity of the vehicle, ωiTo estimate the angular velocity of the device, said
Figure FDA0002207612930000036
For the rotation matrix, the μ is an estimated state quantity mean value, μ ═ ωi，α]TWhere α is an estimated device mounting angle, and B0Is a zero matrix corresponding to the vector dimension of the estimated installation angle of the device.
6. The method of claim 3, wherein the step of determining the predicted state quantity average value at the first time based on a preset state equation and the estimated state quantity average value at the second time comprises:
according to the formula
Figure FDA0002207612930000037
Determining a predicted state quantity average value at a first moment; wherein, the
Figure FDA0002207612930000038
Is the mean value of the predicted state quantities at the first time t, mut-1The estimated state quantity average value at the second moment t-1 is obtained, and A is the preset state equation;
the step of determining the confidence of the predicted state quantity at the first time according to a preset state equation and the confidence of the estimated state quantity at the second time includes:
according to the formula
Figure FDA0002207612930000039
Determining a confidence of the predicted state quantity at a first moment; wherein, the
Figure FDA00022076129300000310
For confidence of predicted state quantity at first moment, said ∑t-1The confidence of the estimated state quantity at the second moment is, and R is a preset state equation confidence;
the step of converting the predicted state quantity average value according to the measurement equation to obtain the predicted vehicle angular velocity at the first moment includes:
according to the measurement equation omegav＝Ctμ, averaging the predicted state quantities
Figure FDA0002207612930000041
Obtaining the predicted vehicle angular velocity at the first time as mu
Figure FDA0002207612930000042
Wherein,
Figure FDA0002207612930000043
the step of modifying the predicted state quantity average value according to the deviation between the actual vehicle angular velocity and the predicted state quantity confidence coefficient to obtain an estimated state quantity average value including an estimated device angular velocity at a first time and an estimated device mounting angle at the first time includes:
modifying the average value of the predicted state quantities according to the following formula to obtain an average value mu of the estimated state quantities, wherein the average value mu comprises the angular speed of the estimated equipment at the first moment and the installation angle of the estimated equipment at the first momentt：
Figure FDA0002207612930000044
Wherein,
Figure FDA0002207612930000045
the above-mentioned
Figure FDA0002207612930000046
As the confidence of the predicted state quantity, the CtIs the coefficient in the measurement equation, Q is the preset measurement equation confidence, ZtFor the actual vehicle angular velocity, the
Figure FDA0002207612930000047
For the predicted vehicle angular velocity, T is transposed.
7. A vehicle-mounted terminal characterized by comprising: a processor and a motion detection device; the processor includes:
the first acquisition module is used for acquiring the device angular speed acquired by the motion detection device at a first moment when the vehicle turns;
the first determining module is used for determining the actual vehicle angular speed at a first moment according to the equipment angular speed and a preset angular speed characteristic when the vehicle turns;
the second acquisition module is used for acquiring the estimated state quantity at the second moment; wherein estimating the state quantity comprises: estimating the angular velocity of the equipment and the installation angle of the equipment; the second moment is the previous moment of the first moment;
the second determination module is used for determining a measurement equation between the estimated state quantity and the vehicle angular speed according to the estimated state quantity at the second moment and the preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed; determining the predicted state quantity at the first moment according to a preset state equation and the estimated state quantity at the second moment;
the conversion module is used for converting the predicted state quantity according to the measurement equation to obtain the predicted vehicle angular speed at the first moment;
and the modification module is used for modifying the predicted state quantity according to the deviation between the actual vehicle angular speed and the predicted vehicle angular speed to obtain the estimated state quantity comprising the estimated equipment angular speed at the first moment and the estimated equipment installation angle at the first moment.
8. The vehicle-mounted terminal of claim 7, wherein the first determining module is specifically configured to:
and taking the preset angular speed direction when the vehicle turns as the direction of the actual vehicle angular speed at the first moment, and determining the size of the device angular speed as the size of the actual vehicle angular speed.
9. The in-vehicle terminal according to claim 7, wherein each estimated state quantity includes an estimated state quantity mean and an estimated state quantity confidence;
the second determining module determines a measurement equation between the estimated state quantity and the vehicle angular velocity according to the estimated state quantity at the second moment and the preset relationship among the vehicle angular velocity, the estimated equipment installation angle and the estimated equipment angular velocity, and includes:
determining a measurement equation between the estimated state quantity mean value and the vehicle angular speed according to the estimated state quantity mean value at the second moment and the preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed;
the second determining module, according to a preset state equation and the estimated state quantity at the second time, determines the predicted state quantity at the first time, and includes:
determining a predicted state quantity average value at the first moment according to a preset state equation and the estimated state quantity average value at the second moment; determining the confidence coefficient of the predicted state quantity at the first moment according to a preset state equation and the confidence coefficient of the estimated state quantity at the second moment;
the conversion module is specifically configured to:
converting the predicted state quantity average value according to the measurement equation to obtain a predicted vehicle angular speed at a first moment;
the modification module is specifically configured to:
and modifying the predicted state quantity average value according to the deviation between the actual vehicle angular speed and the predicted state quantity confidence coefficient to obtain an estimated state quantity average value containing the estimated equipment angular speed at the first moment and the estimated equipment installation angle at the first moment.
10. The in-vehicle terminal according to claim 9, wherein the second determination module, when determining the measurement equation between the estimated state quantity mean and the vehicle angular velocity based on the estimated state quantity mean at the second time and the preset relationship among the vehicle angular velocity, the estimated device installation angle, and the estimated device angular velocity, includes:
determining a rotation matrix between the vehicle angular speed and the estimated equipment angular speed according to the estimated equipment installation angle in the estimated state quantity mean value at the second moment and a preset relation among the vehicle angular speed, the estimated equipment installation angle and the estimated equipment angular speed;
and constructing a measurement equation between the average value of the estimated state quantities and the angular speed of the vehicle according to the relationship among the angular speed of the vehicle, the angular speed of the estimation equipment and the rotation matrix.
CN201910887036.9A 2019-09-19 2019-09-19 Method for determining installation angle of equipment and vehicle-mounted terminal Active CN112525143B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910887036.9A CN112525143B (en) 2019-09-19 2019-09-19 Method for determining installation angle of equipment and vehicle-mounted terminal

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910887036.9A CN112525143B (en) 2019-09-19 2019-09-19 Method for determining installation angle of equipment and vehicle-mounted terminal

Publications (2)

Publication Number Publication Date
CN112525143A true CN112525143A (en) 2021-03-19
CN112525143B CN112525143B (en) 2022-09-27

Family

ID=74974225

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910887036.9A Active CN112525143B (en) 2019-09-19 2019-09-19 Method for determining installation angle of equipment and vehicle-mounted terminal

Country Status (1)

Country Link
CN (1) CN112525143B (en)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040172173A1 (en) * 2003-02-03 2004-09-02 Pioneer Corporation Mounting angle detection device
CN101393028A (en) * 2008-11-07 2009-03-25 北京航空航天大学 Rapid estimating and compensating system with IMU mounting angle obliquely set
CN108007455A (en) * 2017-11-01 2018-05-08 千寻位置网络有限公司 Method for correcting error and device, navigation and service terminal, the memory of inertial navigation system
CN108592950A (en) * 2018-05-17 2018-09-28 北京航空航天大学 A kind of monocular camera and Inertial Measurement Unit are with respect to established angle scaling method
CN108731667A (en) * 2017-04-14 2018-11-02 百度在线网络技术（北京）有限公司 The method and apparatus of speed and pose for determining automatic driving vehicle
CN109029503A (en) * 2018-08-08 2018-12-18 上海博泰悦臻网络技术服务有限公司 Vehicle-mounted gyroscope established angle adaptive calibration method and system, storage medium and car-mounted terminal

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040172173A1 (en) * 2003-02-03 2004-09-02 Pioneer Corporation Mounting angle detection device
CN101393028A (en) * 2008-11-07 2009-03-25 北京航空航天大学 Rapid estimating and compensating system with IMU mounting angle obliquely set
CN108731667A (en) * 2017-04-14 2018-11-02 百度在线网络技术（北京）有限公司 The method and apparatus of speed and pose for determining automatic driving vehicle
CN108007455A (en) * 2017-11-01 2018-05-08 千寻位置网络有限公司 Method for correcting error and device, navigation and service terminal, the memory of inertial navigation system
CN108592950A (en) * 2018-05-17 2018-09-28 北京航空航天大学 A kind of monocular camera and Inertial Measurement Unit are with respect to established angle scaling method
CN109029503A (en) * 2018-08-08 2018-12-18 上海博泰悦臻网络技术服务有限公司 Vehicle-mounted gyroscope established angle adaptive calibration method and system, storage medium and car-mounted terminal

Also Published As

Publication number Publication date
CN112525143B (en) 2022-09-27

Similar Documents

Publication Publication Date Title
CN108731670A (en) Inertia/visual odometry combined navigation locating method based on measurement model optimization
KR100898169B1 (en) Initial alignment method of inertial navigation system
JP5074950B2 (en) Navigation equipment
CN109141411B (en) Positioning method, positioning device, mobile robot, and storage medium
JP5164645B2 (en) Method and apparatus for repetitive calculation control in Kalman filter processing
CN108931233B (en) Road side slope value detection method and device
CN110440797A (en) Vehicle attitude estimation method and system
CN116777984A (en) System for calibrating external parameters of cameras in autonomous transportation vehicles
CN110637209A (en) Method, apparatus, and computer-readable storage medium having instructions for estimating a pose of a motor vehicle
JP5219547B2 (en) Car navigation system and navigation method
CN102706364B (en) Online calibration method of scaling factors of micromachining gyroscope for vehicle
CN112046491B (en) Method and device for estimating cornering stiffness of wheel, vehicle and readable storage medium
CN109506674B (en) Acceleration correction method and device
CN112525143B (en) Method for determining installation angle of equipment and vehicle-mounted terminal
CN112731320A (en) Method, device and equipment for estimating error data of vehicle-mounted radar and storage medium
CN109459769B (en) Autonomous positioning method and system
JP6555033B2 (en) Angular velocity sensor correction apparatus and angular velocity sensor correction method
CN108773378B (en) Automobile running speed real-time estimation method and device based on mobile terminal
CN116972834A (en) Multi-sensor fusion positioning method, device, system and storage medium
CN106441282B (en) A kind of star sensor star tracking method
CN115236708A (en) Method, device and equipment for estimating position and attitude state of vehicle and storage medium
CN101118160A (en) Low accuracy piezoelectric gyroscope zero-bias real-time estimation compensation process
CN114897942A (en) Point cloud map generation method and device and related storage medium
JP2017215153A (en) Angular velocity sensor correction device and angular velocity sensor correction method
JP7409037B2 (en) Estimation device, estimation method, estimation program

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20220303

Address after: 100083 unit 501, block AB, Dongsheng building, No. 8, Zhongguancun East Road, Haidian District, Beijing

Applicant after: BEIJING MOMENTA TECHNOLOGY Co.,Ltd.

Address before: 100083 room 28, 4 / F, block a, Dongsheng building, 8 Zhongguancun East Road, Haidian District, Beijing

Applicant before: BEIJING CHUSUDU TECHNOLOGY Co.,Ltd.

GR01 Patent grant
GR01 Patent grant