CN114877892A - Fusion positioning method for photovoltaic robot - Google Patents
Fusion positioning method for photovoltaic robot Download PDFInfo
- Publication number
- CN114877892A CN114877892A CN202210810871.4A CN202210810871A CN114877892A CN 114877892 A CN114877892 A CN 114877892A CN 202210810871 A CN202210810871 A CN 202210810871A CN 114877892 A CN114877892 A CN 114877892A
- Authority
- CN
- China
- Prior art keywords
- photovoltaic robot
- data
- coordinate system
- robot
- photovoltaic
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000004927 fusion Effects 0.000 title claims abstract description 24
- 230000008569 process Effects 0.000 claims abstract description 25
- 230000001133 acceleration Effects 0.000 claims description 98
- 239000011159 matrix material Substances 0.000 claims description 12
- 238000005070 sampling Methods 0.000 claims description 9
- 230000008859 change Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000004807 localization Effects 0.000 claims 1
- 229920006395 saturated elastomer Polymers 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 7
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02E—REDUCTION OF GREENHOUSE GAS [GHG] EMISSIONS, RELATED TO ENERGY GENERATION, TRANSMISSION OR DISTRIBUTION
- Y02E10/00—Energy generation through renewable energy sources
- Y02E10/50—Photovoltaic [PV] energy
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a fusion positioning method for a photovoltaic robot, which belongs to the technical field of robot vision and comprises the following steps: acquiring related data sources in the moving process of the photovoltaic robot through a plurality of data source acquisition devices installed on the photovoltaic robot; processing part of the acquired data in the related data source to acquire new data; establishing an observation model based on an extended Kalman filter; inputting part of related data sources in the moving process of the photovoltaic robot and new data obtained by processing part of data in the obtained related data sources into an observation model to obtain a plurality of observed quantities; adjusting relevant parameters of the extended Kalman filter; the method and the device have the advantages that the final fusion positioning data of the photovoltaic robot is obtained, the processed GPS data and the processed INS data are input into the observation model to be fused, and the situations that the INS data are saturated and lost in a short time due to vibration of the robot and the GPS positioning fails due to shielding and other reasons can be avoided.
Description
Technical Field
The invention belongs to the technical field of robot vision, and particularly relates to a fusion positioning method for a photovoltaic robot.
Background
The existing robot positioning method generally adopts a GPS technology as a main technology, however, the GPS is limited by an error range, some errors can not be completely eliminated, meanwhile, the data output frequency of the GPS is usually lower than 10 Hz, namely the GPS data is output for less than 10 times per second, which is not beneficial to the acquisition of the GPS data, and in addition, the GPS has a signal missing phenomenon.
Compared with the GPS technology, the INS technology can accumulate errors by using the principle that displacement is obtained by twice integration of acceleration, the data frequency of the INS is much higher and can even reach more than 100 Hz, the INS is not easily interfered by the outside and can stably output data, and sometimes the INS data is saturated and lost for a short time due to vibration of the robot.
Therefore, it is an urgent need to solve the above problems by providing a new technical solution.
Disclosure of Invention
In view of the above, the present invention provides a fusion positioning method for a photovoltaic robot to solve the above technical problems.
In order to achieve the purpose, the invention provides the following technical scheme:
a fusion positioning method for a photovoltaic robot, comprising:
acquiring related data sources in the moving process of the photovoltaic robot through a plurality of data source acquisition devices installed on the photovoltaic robot;
processing part of the acquired data in the related data source to acquire new data;
establishing an observation model based on an extended Kalman filter;
inputting part of related data sources in the moving process of the photovoltaic robot and new data obtained by processing part of data in the obtained related data sources into an observation model to obtain a plurality of observed quantities;
adjusting relevant parameters of the extended Kalman filter;
and acquiring final fusion positioning data of the photovoltaic robot.
In the foregoing solution, the acquiring, by a plurality of data source collecting devices installed on the photovoltaic robot, a relevant data source in the moving process of the photovoltaic robot includes:
acquiring an initial course angle before the photovoltaic robot starts a new track through a digital compass installed on the photovoltaic robot;
acquiring longitude data, latitude data, altitude data, horizontal speed data, vertical speed data and course angle data of the photovoltaic robot in a relative due north direction under a robot coordinate system by using GPS equipment arranged on the photovoltaic robot;
the method comprises the steps that the course angle, the course angular velocity, the pitch angle, the pitch angular velocity, the inclination angle velocity, the acceleration data on the X axis, the acceleration data on the Y axis and the acceleration data on the Z axis of the photovoltaic robot under an INS coordinate system are obtained through INS equipment installed on the photovoltaic robot.
In the above solution, the processing the acquired partial data in the related data source to acquire new data includes:
transforming the robot coordinate system into a navigation coordinate system;
according to longitude data, latitude data, altitude data, horizontal speed data, vertical speed data and course angle data of the photovoltaic robot relative to the true north direction of the photovoltaic robot under a robot coordinate system, position data of the photovoltaic robot in the true north direction, speed data of the true north direction, position data of the true east direction, speed data of the true east direction, position data of the altitude direction and speed data of the altitude direction are obtained.
In the foregoing solution, the establishing an observation model based on an extended kalman filter includes: establishing a series of discrete time observation equations;
a plurality of state equations are established.
In the above solution, the establishing a series of discrete-time observation equations includes:
establishing an observation equation of the photovoltaic robot under a navigation coordinate system, wherein the observation equation of the photovoltaic robot under the navigation coordinate system comprises the following steps:
wherein,as a parameter of the time of day,in order to correspond to the observed value of the state quantity,、andrespectively the position of the photovoltaic robot in the east direction, the north direction and the height direction,、andrespectively the speed of the photovoltaic robot in the east direction, the speed in the north direction and the speed in the height direction,is zero-mean white Gaussian noise in the east direction,Is zero-mean Gaussian white noise in the north direction,Is zero mean Gaussian white noise in the height direction;
establishing an observation equation of the photovoltaic robot under an INS coordinate system, wherein the observation equation of the photovoltaic robot under the INS coordinate system comprises the following steps:
wherein,as a parameter of the time of day,in order to correspond to the observed value of the state quantity,is a course angle under an INS coordinate system on the photovoltaic robot,Is a pitch angle of the photovoltaic robot under an INS coordinate system,Is the tilt angle under the INS coordinate system on the photovoltaic robot,is the course angular velocity under the INS coordinate system on the photovoltaic robot,The pitch elevation speed under the INS coordinate system on the photovoltaic robot,Is the angular velocity of the INS coordinate system on the photovoltaic robot.
In the above solution, the establishing a series of discrete-time observation equations further includes: establishing a relation equation of the photovoltaic robot under a navigation coordinate system and an INS coordinate system through an extended Kalman filter, wherein the relation equation of the photovoltaic robot under the navigation coordinate system and the INS coordinate system comprises the following steps:
wherein,acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,The acceleration of the photovoltaic robot in the height direction under the navigation coordinate system is obtained,acceleration data of the photovoltaic robot in the X-axis direction under the INS coordinate system,Acceleration data of the photovoltaic robot in the Y-axis direction under the INS coordinate system,Acceleration data of the photovoltaic robot in the Z-axis direction under the INS coordinate system,is zero offset relative to the acceleration in the X-axis direction,Is zero offset relative to acceleration in the Y-axis direction,For zero offset in relation to acceleration in the direction of the Z axis, saidIs zero-mean white Gaussian noise in the X-axis directionIs zero mean Gaussian white noise in the Y-axis directionIs zero mean gaussian white noise in the Z-axis direction.
In the above solution, the establishing a plurality of state equations includes:
establishing a continuous state equation of the course angle and the course angular speed of the photovoltaic robot, wherein the continuous state equation of the course angle and the course angular speed of the photovoltaic robot is as follows:
discretizing a continuous state equation of the heading angle and the heading angular speed of the photovoltaic robot to obtain a discrete state equation of the heading angle and the heading angular speed of the photovoltaic robot, wherein the discrete state equation of the heading angle and the heading angular speed of the photovoltaic robot is as follows:
establishing a continuous state equation of the pitch angle and the pitch angle speed of the photovoltaic robot, wherein the continuous state equation of the pitch angle and the pitch angle speed of the photovoltaic robot is as follows:
discretizing the continuous state equation of the pitch angle and the pitch angle speed of the photovoltaic robot to obtain the discrete state equation of the pitch angle and the pitch angle speed of the photovoltaic robot, wherein the discrete state equation of the pitch angle and the pitch angle speed of the photovoltaic robot is as follows:
establishing a continuous state equation of the inclination angle and the inclination angle speed of the photovoltaic robot, wherein the continuous state equation of the inclination angle and the inclination angle speed of the photovoltaic robot is as follows:
discretizing a continuous state equation of the inclination angle and the inclination angle speed of the photovoltaic robot to obtain a discrete state equation of the inclination angle and the inclination angle speed of the photovoltaic robot, wherein the discrete state equation of the inclination angle and the inclination angle speed of the photovoltaic robot is as follows:
in the above solution, the establishing a plurality of state equations further includes:
establishing a discrete state equation of the relation of acceleration data of the photovoltaic robot in the east-righting direction under the navigation coordinate system and the X-axis direction under the INS coordinate system through an extended Kalman filter, wherein the discrete state equation of the relation of the acceleration data of the photovoltaic robot in the east-righting direction under the navigation coordinate system and the X-axis direction under the INS coordinate system is as follows:
The speed of the photovoltaic robot in the east direction,Acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,Zero offset associated with acceleration in the X-axis direction;
establishing a relation discrete state equation of acceleration data of the photovoltaic robot in the due north direction under the navigation coordinate system and the Y-axis direction under the INS coordinate system through an extended Kalman filter, wherein the relation discrete state equation of the acceleration data of the photovoltaic robot in the due north direction under the navigation coordinate system and the Y-axis direction under the INS coordinate system is as follows:
For photovoltaic machinesThe speed of the robot in the north,Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,Zero offset associated with acceleration in the Y-axis direction;
establishing a relation discrete state equation of acceleration data of the photovoltaic robot in the height direction under the navigation coordinate system and the Z-axis direction under the INS coordinate system through an extended Kalman filter, wherein the relation discrete state equation of the acceleration data of the photovoltaic robot in the height direction under the navigation coordinate system and the Z-axis direction under the INS coordinate system is as follows:
The speed of the photovoltaic robot in the height direction,Acceleration of the photovoltaic robot in the height direction under a navigation coordinate system,Is zero offset relative to acceleration in the Z direction.
In the above solution, the establishing a plurality of state equations further includes: all the established connected discrete state equations are written in one equation and are expressed by a block matrix, and the block matrix is as follows:
The process noise of the speed is such that,is zero deviation of the acceleration in the direction of the X-axis,
The process noise of the acceleration is such that,is zero deviation of the acceleration in the Y-axis direction,
The process noise of the acceleration is such that,zero deviation of acceleration in the Z-direction.
In the foregoing solution, the adjusting the relevant parameter of the extended kalman filter includes:
adjusting elements in an acceleration-related zero offset in the X-axis direction, an acceleration-related zero offset in the Y-axis direction, an acceleration-related zero offset in the Z direction and a process noise variance matrix in the observation model through a gradient factor;
changing the Kalman gain of the extended Kalman filter according to the adjustment result obtained by the gradual change factor;
and adjusting the fusion result of the data in the navigation coordinate system and the data in the INS coordinate system according to the Kalman gain of the extended Kalman filter.
In conclusion, the beneficial effects of the invention are as follows: by establishing an observation model based on an extended Kalman filter and inputting the processed GPS data and the INS data into the observation model for fusion, the situations that the INS data are saturated and lost in short time due to vibration of the robot and the GPS positioning fails due to reasons such as shielding can be avoided.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this application, illustrate embodiments of the invention and, together with the description, serve to explain the invention and not to limit the invention.
Fig. 1 is a step diagram of a fusion positioning method of a photovoltaic robot according to the present invention.
FIG. 2 is a diagram of the steps of collecting relevant data sources in the present invention.
FIG. 3 is a diagram of a part of the data processing steps in the present invention.
FIG. 4 is a diagram illustrating the steps of establishing an observation model according to the present invention.
FIG. 5 is a diagram of the steps of the present invention to establish a series of discrete time observation equations.
Fig. 6 and 7 are diagrams of steps for establishing a plurality of state equations in the present invention.
FIG. 8 is a diagram illustrating the steps of adjusting the parameters associated with the extended Kalman filter in accordance with the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be described in further detail with reference to the following embodiments and accompanying drawings. The exemplary embodiments and descriptions of the present invention are provided to explain the present invention, but not to limit the present invention.
As shown in fig. 1, a fusion positioning method for a photovoltaic robot of the present invention includes:
step S1: acquiring related data sources in the moving process of the photovoltaic robot through a plurality of data source acquisition devices installed on the photovoltaic robot;
step S2: processing part of the acquired data in the related data source to acquire new data;
step S3: establishing an observation model based on an extended Kalman filter;
step S4: inputting part of related data sources in the moving process of the photovoltaic robot and new data obtained by processing part of data in the obtained related data sources into an observation model to obtain a plurality of observed quantities;
step S5: adjusting relevant parameters of the extended Kalman filter;
step S6: and acquiring final fusion positioning data of the photovoltaic robot.
As shown in fig. 2, the acquiring, by a plurality of data source collecting devices installed on the photovoltaic robot, related data sources in the moving process of the photovoltaic robot includes:
step S11: acquiring an initial course angle before the photovoltaic robot starts a new track through a digital compass installed on the photovoltaic robot;
step S12: acquiring longitude data, latitude data, altitude data, horizontal speed data, vertical speed data and course angle data of the photovoltaic robot in a relative due north direction under a robot coordinate system by using GPS equipment arranged on the photovoltaic robot;
step S13: the method comprises the steps that the course angle, the course angular velocity, the pitch angle, the pitch angular velocity, the inclination angle velocity, the acceleration data on the X axis, the acceleration data on the Y axis and the acceleration data on the Z axis of the photovoltaic robot under an INS coordinate system are obtained through INS equipment installed on the photovoltaic robot.
In this embodiment, the robot coordinate system is fixed on the photovoltaic robot body, the center of gravity is used as an origin, the center axis of the body points forward to be an X axis, and the vertical direction is a Z axis.
In this embodiment, the INS coordinate system is defined as the X-Y plane parallel to the earth, the Z-axis perpendicular to the earth down, and the X-axis being the robot heading direction.
In this embodiment, the navigation coordinate system uses the fixed position as an origin, uses the true north and the true east parallel to the earth as horizontal coordinate axes, and the direction of the Z axis is vertically upward.
As shown in fig. 3, the processing the acquired partial data in the related data source to acquire new data includes:
step S21: transforming the robot coordinate system into a navigation coordinate system;
step S22: and acquiring position data of the photovoltaic robot in the due north direction, speed data of the due north direction, position data of the due east direction, speed data of the due east direction, position data of the altitude direction and speed data of the altitude direction in the navigation coordinate system according to longitude data, latitude data, altitude data, horizontal speed data, vertical speed data of the photovoltaic robot in the robot coordinate system and heading angle data of the photovoltaic robot relative to the due north direction.
As shown in fig. 4, the establishing of the observation model based on the extended kalman filter includes:
step S31: establishing a series of discrete time observation equations;
step S32: a plurality of state equations are established.
As shown in fig. 5, the establishing a series of discrete-time observation equations includes:
step S311: establishing an observation equation of the photovoltaic robot under a navigation coordinate system, wherein the observation equation of the photovoltaic robot under the navigation coordinate system comprises the following steps:
wherein,as a parameter of the time of day,in order to correspond to the observed value of the state quantity,、andrespectively the position of the photovoltaic robot in the east direction, the north direction and the height direction,、andrespectively the speed of the photovoltaic robot in the east direction, the speed in the north direction and the speed in the height direction,is zero-mean white Gaussian noise in the east direction,Is positiveZero-mean white Gaussian noise in the north direction,Is zero mean Gaussian white noise in the height direction;
step S312: establishing an observation equation of the photovoltaic robot under an INS coordinate system, wherein the observation equation of the photovoltaic robot under the INS coordinate system comprises the following steps:
wherein,as a parameter of the time of day,in order to correspond to the observed value of the state quantity,is a course angle under an INS coordinate system on the photovoltaic robot,Is a pitch angle of the photovoltaic robot under an INS coordinate system,Is the tilt angle under the INS coordinate system on the photovoltaic robot,is the course angular velocity under the INS coordinate system on the photovoltaic robot,The pitch elevation speed under the INS coordinate system on the photovoltaic robot,For the angular velocity of inclination under the INS coordinate system on the photovoltaic robot。
Step S313: establishing a relation equation of the photovoltaic robot under a navigation coordinate system and an INS coordinate system through an extended Kalman filter, wherein the relation equation of the photovoltaic robot under the navigation coordinate system and the INS coordinate system comprises the following steps:
wherein,acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,The acceleration of the photovoltaic robot in the height direction under the navigation coordinate system is obtained,acceleration data of the photovoltaic robot in the X-axis direction under the INS coordinate system,Acceleration data of the photovoltaic robot in the Y-axis direction under the INS coordinate system,Acceleration data of the photovoltaic robot in the Z-axis direction under the INS coordinate system,is zero offset relative to the acceleration in the X-axis direction,Is zero offset relative to acceleration in the Y-axis direction,For zero offset in relation to acceleration in the direction of the Z axis, saidIs zero mean white Gaussian noise in the X-axis directionIs zero mean Gaussian white noise in the Y-axis directionIs zero mean gaussian white noise in the Z-axis direction.
As shown in fig. 6 and 7, the establishing a plurality of state equations includes:
step S321: establishing a continuous state equation of the heading angle and the heading angular speed of the photovoltaic robot, wherein the continuous state equation of the heading angle and the heading angular speed of the photovoltaic robot is as follows:
step S322: discretizing a continuous state equation of the course angle and the course angular speed of the photovoltaic robot to obtain a discrete state equation of the course angle and the course angular speed of the photovoltaic robot, wherein the discrete state equation of the course angle and the course angular speed of the photovoltaic robot is as follows:
step S323: establishing a continuous state equation of the pitch angle and the pitch angle speed of the photovoltaic robot, wherein the continuous state equation of the pitch angle and the pitch angle speed of the photovoltaic robot is as follows:
step S324: discretizing the continuous state equation of the pitch angle and the pitch angle speed of the photovoltaic robot to obtain the discrete state equation of the pitch angle and the pitch angle speed of the photovoltaic robot, wherein the discrete state equation of the pitch angle and the pitch angle speed of the photovoltaic robot is as follows:
step S325: establishing a continuous state equation of the inclination angle and the inclination angle speed of the photovoltaic robot, wherein the continuous state equation of the inclination angle and the inclination angle speed of the photovoltaic robot is as follows:
step S326: discretizing a continuous state equation of the inclination angle and the inclination angle speed of the photovoltaic robot to obtain a discrete state equation of the inclination angle and the inclination angle speed of the photovoltaic robot, wherein the discrete state equation of the inclination angle and the inclination angle speed of the photovoltaic robot is as follows:
step S327: establishing a discrete state equation of the relation of acceleration data of the photovoltaic robot in the east-righting direction under the navigation coordinate system and the X-axis direction under the INS coordinate system through an extended Kalman filter, wherein the discrete state equation of the relation of the acceleration data of the photovoltaic robot in the east-righting direction under the navigation coordinate system and the X-axis direction under the INS coordinate system is as follows:
The speed of the photovoltaic robot in the east direction,Acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,Zero offset associated with acceleration in the direction of the X axis;
step S328: establishing a relation discrete state equation of acceleration data of the photovoltaic robot in the due north direction under the navigation coordinate system and the Y-axis direction under the INS coordinate system through an extended Kalman filter, wherein the relation discrete state equation of the acceleration data of the photovoltaic robot in the due north direction under the navigation coordinate system and the Y-axis direction under the INS coordinate system is as follows:
The speed of the photovoltaic robot in the positive north direction,Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,Zero offset associated with acceleration in the Y-axis direction;
step S329: establishing a discrete state equation of the relation of acceleration data of the photovoltaic robot in the height direction under the navigation coordinate system and the Z-axis direction under the INS coordinate system through an extended Kalman filter, wherein the discrete state equation of the relation of the acceleration data of the photovoltaic robot in the height direction under the navigation coordinate system and the Z-axis direction under the INS coordinate system is as follows:
The speed of the photovoltaic robot in the height direction,Acceleration of the photovoltaic robot in the height direction under a navigation coordinate system,Is zero offset relative to acceleration in the Z direction.
Step S3210: all the established connected discrete state equations are written in one equation and are expressed by a block matrix, and the block matrix is as follows:
The process noise of the speed is such that,is zero deviation of the acceleration in the direction of the X-axis,
The process noise of the acceleration is such that,is zero deviation of the acceleration in the Y-axis direction,
The process noise of the acceleration is such that,zero deviation of acceleration in the Z-direction.
As shown in fig. 8, the adjusting the relevant parameters of the extended kalman filter includes:
step S51: adjusting elements in an acceleration-related zero offset in the X-axis direction, an acceleration-related zero offset in the Y-axis direction, an acceleration-related zero offset in the Z direction and a process noise variance matrix in the observation model through a gradient factor;
step S52: changing the Kalman gain of the extended Kalman filter according to the adjustment result obtained by the gradual change factor;
step S53: and adjusting the fusion result of the data in the navigation coordinate system and the data in the INS coordinate system according to the Kalman gain of the extended Kalman filter.
The above description is only a preferred embodiment of the present invention, and is not intended to limit the present invention, and various modifications and changes may be made to the embodiment of the present invention by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (8)
1. A fusion positioning method for a photovoltaic robot, comprising:
acquiring related data sources in the moving process of the photovoltaic robot through a plurality of data source acquisition devices installed on the photovoltaic robot;
processing part of the acquired data in the related data source to acquire new data;
establishing an observation model based on an extended Kalman filter;
inputting part of related data sources in the moving process of the photovoltaic robot and new data obtained by processing part of data in the obtained related data sources into an observation model to obtain a plurality of observed quantities;
adjusting relevant parameters of the extended Kalman filter;
acquiring final fusion positioning data of the photovoltaic robot;
wherein the establishing of the observation model based on the extended Kalman filter comprises:
establishing a series of discrete time observation equations;
establishing a plurality of state equations;
the establishing a plurality of state equations comprises:
establishing a continuous state equation of the heading angle and the heading angular speed of the photovoltaic robot, wherein the continuous state equation of the heading angle and the heading angular speed of the photovoltaic robot is as follows:
discretizing a continuous state equation of the heading angle and the heading angular speed of the photovoltaic robot to obtain a discrete state equation of the heading angle and the heading angular speed of the photovoltaic robot, wherein the discrete state equation of the heading angle and the heading angular speed of the photovoltaic robot is as follows:
establishing a continuous state equation of the pitch angle and the pitch angle speed of the photovoltaic robot, wherein the continuous state equation of the pitch angle and the pitch angle speed of the photovoltaic robot is as follows:
discretizing the continuous state equation of the pitch angle and the pitch angle speed of the photovoltaic robot to obtain the discrete state equation of the pitch angle and the pitch angle speed of the photovoltaic robot, wherein the discrete state equation of the pitch angle and the pitch angle speed of the photovoltaic robot is as follows:
establishing a continuous state equation of the inclination angle and the inclination angle speed of the photovoltaic robot, wherein the continuous state equation of the inclination angle and the inclination angle speed of the photovoltaic robot is as follows:
discretizing the continuous state equation of the inclination angle and the inclination angle speed of the photovoltaic robot to obtain a discrete state equation of the inclination angle and the inclination angle speed of the photovoltaic robot, wherein the discrete state equation of the inclination angle and the inclination angle speed of the photovoltaic robot is as follows:
2. the fusion positioning method for the photovoltaic robot as claimed in claim 1, wherein the acquiring the relevant data source during the movement of the photovoltaic robot by the plurality of data source collecting devices installed on the photovoltaic robot comprises:
acquiring an initial course angle before the photovoltaic robot starts a new track through a digital compass installed on the photovoltaic robot;
acquiring longitude data, latitude data, altitude data, horizontal speed data, vertical speed data and course angle data of the photovoltaic robot in a relative due north direction under a robot coordinate system by using GPS equipment arranged on the photovoltaic robot;
the method comprises the steps that the course angle, the course angular velocity, the pitch angle, the pitch angular velocity, the inclination angle velocity, the acceleration data on the X axis, the acceleration data on the Y axis and the acceleration data on the Z axis of the photovoltaic robot under an INS coordinate system are obtained through INS equipment installed on the photovoltaic robot.
3. The fusion positioning method for photovoltaic robots according to claim 1, wherein said processing of the partial data in the acquired relevant data sources to acquire new data comprises:
transforming the robot coordinate system into a navigation coordinate system;
according to longitude data, latitude data, altitude data, horizontal speed data, vertical speed data and course angle data of the photovoltaic robot relative to the true north direction of the photovoltaic robot under a robot coordinate system, position data of the photovoltaic robot in the true north direction, speed data of the true north direction, position data of the true east direction, speed data of the true east direction, position data of the altitude direction and speed data of the altitude direction are obtained.
4. The fusion positioning method for photovoltaic robots of claim 1, wherein said establishing a series of discrete time observation equations comprises:
establishing an observation equation of the photovoltaic robot under a navigation coordinate system, wherein the observation equation of the photovoltaic robot under the navigation coordinate system comprises the following steps:
wherein,as a parameter of the time of day,in order to correspond to the observed value of the state quantity,、andrespectively the position of the photovoltaic robot in the east direction, the north direction and the height direction,、andrespectively the speed of the photovoltaic robot in the east direction, the speed in the north direction and the speed in the height direction,is zero-mean white Gaussian noise in the east direction,Is zero-mean Gaussian white noise in the north direction,Is zero mean Gaussian white noise in the height direction;
establishing an observation equation of the photovoltaic robot under an INS coordinate system, wherein the observation equation of the photovoltaic robot under the INS coordinate system comprises the following steps:
wherein,as a parameter of the time of day,in order to correspond to the observed value of the state quantity,is a course angle under an INS coordinate system on the photovoltaic robot,Is a pitch angle of the photovoltaic robot under an INS coordinate system,Is the tilt angle under the INS coordinate system on the photovoltaic robot,is the course angular velocity under the INS coordinate system on the photovoltaic robot,The pitch elevation speed under the INS coordinate system on the photovoltaic robot,Is the angular velocity of the INS coordinate system on the photovoltaic robot.
5. The fusion localization method for photovoltaic robots of claim 4, wherein the establishing a series of discrete-time observation equations further comprises: establishing a relation equation of the photovoltaic robot under a navigation coordinate system and an INS coordinate system through an extended Kalman filter, wherein the relation equation of the photovoltaic robot under the navigation coordinate system and the INS coordinate system comprises the following steps:
wherein,in order to correspond to the observed value of the state quantity,acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,The acceleration of the photovoltaic robot in the height direction under the navigation coordinate system is obtained,acceleration data of the photovoltaic robot in the X-axis direction under the INS coordinate system,Acceleration data of the photovoltaic robot in the Y-axis direction under the INS coordinate system,Acceleration data of the photovoltaic robot in the Z-axis direction under the INS coordinate system,is zero offset relative to the acceleration in the X-axis direction,Is zero offset relative to acceleration in the Y-axis direction,For zero offset in relation to acceleration in the direction of the Z axis, saidIs zero mean white Gaussian noise in the X-axis directionIs zero mean Gaussian white noise in the Y-axis directionIs zero mean gaussian white noise in the Z-axis direction.
6. The fusion positioning method for photovoltaic robots of claim 1 wherein said establishing a plurality of state equations further comprises:
establishing a discrete state equation of the relation of acceleration data of the photovoltaic robot in the east-righting direction under the navigation coordinate system and the X-axis direction under the INS coordinate system through an extended Kalman filter, wherein the discrete state equation of the relation of the acceleration data of the photovoltaic robot in the east-righting direction under the navigation coordinate system and the X-axis direction under the INS coordinate system is as follows:
whereinis the position of the photovoltaic robot in the east-ward direction,The speed of the photovoltaic robot in the east direction,Acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,Zero offset associated with acceleration in the X-axis direction;
establishing a relation discrete state equation of acceleration data of the photovoltaic robot in the due north direction under the navigation coordinate system and the Y-axis direction under the INS coordinate system through an extended Kalman filter, wherein the relation discrete state equation of the acceleration data of the photovoltaic robot in the due north direction under the navigation coordinate system and the Y-axis direction under the INS coordinate system is as follows:
whereinthe position of the photovoltaic robot in the positive north direction,The speed of the photovoltaic robot in the positive north direction,Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,Zero offset associated with acceleration in the Y-axis direction;
establishing a relation discrete state equation of acceleration data of the photovoltaic robot in the height direction under the navigation coordinate system and the Z-axis direction under the INS coordinate system through an extended Kalman filter, wherein the relation discrete state equation of the acceleration data of the photovoltaic robot in the height direction under the navigation coordinate system and the Z-axis direction under the INS coordinate system is as follows:
7. The fusion positioning method for photovoltaic robots of claim 6 wherein said establishing a plurality of state equations further comprises: all the established connected discrete state equations are written in one equation and are expressed by a block matrix, and the block matrix is as follows:
,is the process noise of the acceleration in the east direction,is zero deviation of the acceleration in the direction of the X-axis,
,process noise that is the acceleration in the due north direction,is zero deviation of the acceleration in the Y-axis direction,
8. The fusion positioning method for the photovoltaic robot as recited in claim 1, wherein the adjusting the relevant parameters of the extended kalman filter comprises:
adjusting elements in an acceleration-related zero offset in the X-axis direction, an acceleration-related zero offset in the Y-axis direction, an acceleration-related zero offset in the Z direction and a process noise variance matrix in the observation model through a gradient factor;
changing the Kalman gain of the extended Kalman filter according to the adjustment result obtained by the gradual change factor;
and adjusting the fusion result of the data in the navigation coordinate system and the data in the INS coordinate system according to the Kalman gain of the extended Kalman filter.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210810871.4A CN114877892A (en) | 2022-07-11 | 2022-07-11 | Fusion positioning method for photovoltaic robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210810871.4A CN114877892A (en) | 2022-07-11 | 2022-07-11 | Fusion positioning method for photovoltaic robot |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114877892A true CN114877892A (en) | 2022-08-09 |
Family
ID=82683077
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210810871.4A Pending CN114877892A (en) | 2022-07-11 | 2022-07-11 | Fusion positioning method for photovoltaic robot |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114877892A (en) |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106969784A (en) * | 2017-03-24 | 2017-07-21 | 中国石油大学(华东) | It is a kind of concurrently to build figure positioning and the combined error emerging system of inertial navigation |
CN107289933A (en) * | 2017-06-28 | 2017-10-24 | 东南大学 | Double card Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions |
CN109324330A (en) * | 2018-09-18 | 2019-02-12 | 东南大学 | Based on USBL/SINS tight integration navigation locating method of the mixing without derivative Extended Kalman filter |
US20200033131A1 (en) * | 2017-03-31 | 2020-01-30 | Commonwealth Scientific And Industrial Research Organisation | Low cost ins |
CN110988951A (en) * | 2019-12-06 | 2020-04-10 | 中国地质大学(北京) | Multi-source data fusion real-time navigation positioning method and system |
CN112476433A (en) * | 2020-11-23 | 2021-03-12 | 深圳怪虫机器人有限公司 | Mobile robot positioning method based on array boundary identification |
CN112729307A (en) * | 2020-12-23 | 2021-04-30 | 南京市晨枭软件技术有限公司 | Multi-sensing information fusion system for path planning robot and control method thereof |
US20210206390A1 (en) * | 2020-07-09 | 2021-07-08 | Beijing Baidu Netcom Science Technology Co., Ltd. | Positioning method and apparatus, vehicle device, and autonomous vehicle |
CN114485641A (en) * | 2022-01-24 | 2022-05-13 | 武汉梦芯科技有限公司 | Attitude calculation method and device based on inertial navigation and satellite navigation azimuth fusion |
-
2022
- 2022-07-11 CN CN202210810871.4A patent/CN114877892A/en active Pending
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106969784A (en) * | 2017-03-24 | 2017-07-21 | 中国石油大学(华东) | It is a kind of concurrently to build figure positioning and the combined error emerging system of inertial navigation |
US20200033131A1 (en) * | 2017-03-31 | 2020-01-30 | Commonwealth Scientific And Industrial Research Organisation | Low cost ins |
CN107289933A (en) * | 2017-06-28 | 2017-10-24 | 东南大学 | Double card Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions |
CN109324330A (en) * | 2018-09-18 | 2019-02-12 | 东南大学 | Based on USBL/SINS tight integration navigation locating method of the mixing without derivative Extended Kalman filter |
CN110988951A (en) * | 2019-12-06 | 2020-04-10 | 中国地质大学(北京) | Multi-source data fusion real-time navigation positioning method and system |
US20210206390A1 (en) * | 2020-07-09 | 2021-07-08 | Beijing Baidu Netcom Science Technology Co., Ltd. | Positioning method and apparatus, vehicle device, and autonomous vehicle |
CN112476433A (en) * | 2020-11-23 | 2021-03-12 | 深圳怪虫机器人有限公司 | Mobile robot positioning method based on array boundary identification |
CN112729307A (en) * | 2020-12-23 | 2021-04-30 | 南京市晨枭软件技术有限公司 | Multi-sensing information fusion system for path planning robot and control method thereof |
CN114485641A (en) * | 2022-01-24 | 2022-05-13 | 武汉梦芯科技有限公司 | Attitude calculation method and device based on inertial navigation and satellite navigation azimuth fusion |
Non-Patent Citations (3)
Title |
---|
LUJUAN DANG 等: ""Cubature Kalman Filter Under Minimum Error Entropy With Fiducial Points for INS/GPS Integration"", 《IEEE/CAA JOURNAL OF AUTOMATICA SINICA》 * |
徐萌 等: ""基于UKF数据融合的勘测机器人定位方法"", 《机械工程师》 * |
项志宇: "面向越野自主导航的鲁棒GPS/INS融合定位***", 《电路与***学报》 * |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107270893B (en) | Lever arm and time asynchronous error estimation and compensation method for real estate measurement | |
CN111024064B (en) | SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering | |
CN110361010B (en) | Mobile robot positioning method based on occupancy grid map and combined with imu | |
CN104635251B (en) | A kind of INS/GPS integrated positionings determine appearance new method | |
CN112505737B (en) | GNSS/INS integrated navigation method | |
CN105674984B (en) | A method of acquisition for mobile terminal pedestrian advancing direction is used without constraint | |
CN114413887B (en) | Sensor external parameter calibration method, device and medium | |
CN110440827B (en) | Parameter error calibration method and device and storage medium | |
CN113220013A (en) | Multi-rotor unmanned aerial vehicle tunnel hovering method and system | |
CN108759815B (en) | Information fusion integrated navigation method used in global visual positioning method | |
CN110017837A (en) | A kind of Combinated navigation method of the diamagnetic interference of posture | |
CN114216456B (en) | Attitude measurement method based on fusion of IMU and robot body parameters | |
CN105910606A (en) | Direction adjustment method based on angular velocity difference | |
WO2023226155A1 (en) | Multi-source data fusion positioning method and apparatus, device, and computer storage medium | |
CN113820735A (en) | Method for determining position information, position measuring device, terminal, and storage medium | |
CN111637888A (en) | Tunneling machine positioning method and system based on inertial navigation and laser radar single-point distance measurement | |
CN108444468B (en) | Directional compass integrating downward vision and inertial navigation information | |
CN111207773B (en) | Attitude unconstrained optimization solving method for bionic polarized light navigation | |
CN204788411U (en) | Wall climbing robot navigational positioning system based on sound localization , INS | |
CN114877892A (en) | Fusion positioning method for photovoltaic robot | |
CN112292578B (en) | Ground level measuring method, measuring device, estimating device and data acquisition device for calculation | |
CN112649001B (en) | Gesture and position resolving method for small unmanned aerial vehicle | |
CN112595330B (en) | Vehicle positioning method and device, electronic equipment and computer readable medium | |
CN104792336B (en) | A kind of state of flight measurement method and device | |
CN113551688A (en) | Quick non-support disassembly-free calibration method and device for vehicle-mounted positioning and directional navigation equipment |
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 | ||
RJ01 | Rejection of invention patent application after publication | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20220809 |