CN114877892A - Fusion positioning method for photovoltaic robot - Google Patents

Fusion positioning method for photovoltaic robot Download PDF

Info

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
Application number
CN202210810871.4A
Other languages
Chinese (zh)
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.)
Quanzhou Tongwei Technology Co ltd
Original Assignee
Quanzhou Tongwei 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 Quanzhou Tongwei Technology Co ltd filed Critical Quanzhou Tongwei Technology Co ltd
Priority to CN202210810871.4A priority Critical patent/CN114877892A/en
Publication of CN114877892A publication Critical patent/CN114877892A/en
Pending legal-status Critical Current

Links

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

Fusion positioning method for photovoltaic robot
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:
Figure 522401DEST_PATH_IMAGE001
wherein,
Figure 326409DEST_PATH_IMAGE002
as a parameter of the time of day,
Figure DEST_PATH_IMAGE003
in order to correspond to the observed value of the state quantity,
Figure 242281DEST_PATH_IMAGE004
、
Figure DEST_PATH_IMAGE005
and
Figure 524358DEST_PATH_IMAGE006
respectively the position of the photovoltaic robot in the east direction, the north direction and the height direction,
Figure DEST_PATH_IMAGE007
、
Figure 209286DEST_PATH_IMAGE008
and
Figure DEST_PATH_IMAGE009
respectively the speed of the photovoltaic robot in the east direction, the speed in the north direction and the speed in the height direction,
Figure 184195DEST_PATH_IMAGE010
is zero-mean white Gaussian noise in the east direction,
Figure 603675DEST_PATH_IMAGE008
Is zero-mean Gaussian white noise in the north direction,
Figure DEST_PATH_IMAGE011
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:
Figure 673131DEST_PATH_IMAGE012
wherein,
Figure 760036DEST_PATH_IMAGE002
as a parameter of the time of day,
Figure DEST_PATH_IMAGE013
in order to correspond to the observed value of the state quantity,
Figure 640267DEST_PATH_IMAGE014
is a course angle under an INS coordinate system on the photovoltaic robot,
Figure DEST_PATH_IMAGE015
Is a pitch angle of the photovoltaic robot under an INS coordinate system,
Figure 530732DEST_PATH_IMAGE016
Is the tilt angle under the INS coordinate system on the photovoltaic robot,
Figure DEST_PATH_IMAGE017
is the course angular velocity under the INS coordinate system on the photovoltaic robot,
Figure 889032DEST_PATH_IMAGE018
The pitch elevation speed under the INS coordinate system on the photovoltaic robot,
Figure DEST_PATH_IMAGE019
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:
Figure 814132DEST_PATH_IMAGE020
wherein,
Figure 396423DEST_PATH_IMAGE021
acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,
Figure DEST_PATH_IMAGE022
Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,
Figure 259337DEST_PATH_IMAGE023
The acceleration of the photovoltaic robot in the height direction under the navigation coordinate system is obtained,
Figure DEST_PATH_IMAGE024
acceleration data of the photovoltaic robot in the X-axis direction under the INS coordinate system,
Figure 936174DEST_PATH_IMAGE025
Acceleration data of the photovoltaic robot in the Y-axis direction under the INS coordinate system,
Figure DEST_PATH_IMAGE026
Acceleration data of the photovoltaic robot in the Z-axis direction under the INS coordinate system,
Figure 200934DEST_PATH_IMAGE027
is zero offset relative to the acceleration in the X-axis direction,
Figure DEST_PATH_IMAGE028
Is zero offset relative to acceleration in the Y-axis direction,
Figure 672235DEST_PATH_IMAGE029
For zero offset in relation to acceleration in the direction of the Z axis, said
Figure DEST_PATH_IMAGE030
Is zero-mean white Gaussian noise in the X-axis direction
Figure 22445DEST_PATH_IMAGE031
Is zero mean Gaussian white noise in the Y-axis direction
Figure DEST_PATH_IMAGE032
Is 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:
Figure 502974DEST_PATH_IMAGE033
，
wherein,
Figure DEST_PATH_IMAGE034
Figure 622240DEST_PATH_IMAGE035
，
Figure DEST_PATH_IMAGE036
zero deviation of course angular speed of the photovoltaic robot;
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:
Figure 15175DEST_PATH_IMAGE037
，
wherein,
Figure DEST_PATH_IMAGE038
，
Figure 367528DEST_PATH_IMAGE039
is the interval between the sampling of the samples,
Figure DEST_PATH_IMAGE040
Figure 871321DEST_PATH_IMAGE041
；
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:
Figure DEST_PATH_IMAGE042
，
wherein
Figure 625519DEST_PATH_IMAGE043
Figure DEST_PATH_IMAGE044
，
Figure 189356DEST_PATH_IMAGE045
Zero deviation of the pitch angle speed of the photovoltaic robot;
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:
Figure DEST_PATH_IMAGE046
，
wherein,
Figure 763426DEST_PATH_IMAGE047
，
Figure DEST_PATH_IMAGE048
is the interval between the sampling of the samples,
Figure 70910DEST_PATH_IMAGE049
Figure DEST_PATH_IMAGE050
；
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:
Figure 164768DEST_PATH_IMAGE051
，
wherein,
Figure DEST_PATH_IMAGE052
Figure 414353DEST_PATH_IMAGE053
，
Figure DEST_PATH_IMAGE054
zero deviation of the inclination angular velocity of the photovoltaic robot;
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:
Figure 960872DEST_PATH_IMAGE055
，
wherein,
Figure DEST_PATH_IMAGE056
，
Figure 586894DEST_PATH_IMAGE057
is the interval between the sampling of the samples,
Figure DEST_PATH_IMAGE058
Figure 535258DEST_PATH_IMAGE059
。
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:
Figure DEST_PATH_IMAGE060
wherein
Figure 955744DEST_PATH_IMAGE061
is the position of the photovoltaic robot in the east-ward direction,
Figure DEST_PATH_IMAGE062
The speed of the photovoltaic robot in the east direction,
Figure 989559DEST_PATH_IMAGE063
Acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,
Figure DEST_PATH_IMAGE064
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:
Figure 904426DEST_PATH_IMAGE065
wherein
Figure DEST_PATH_IMAGE066
the position of the photovoltaic robot in the positive north direction,
Figure 956564DEST_PATH_IMAGE067
For photovoltaic machinesThe speed of the robot in the north,
Figure DEST_PATH_IMAGE068
Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,
Figure 298684DEST_PATH_IMAGE069
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:
Figure DEST_PATH_IMAGE070
wherein
Figure 69063DEST_PATH_IMAGE071
the position of the photovoltaic robot in the height direction,
Figure DEST_PATH_IMAGE072
The speed of the photovoltaic robot in the height direction,
Figure 522041DEST_PATH_IMAGE073
Acceleration of the photovoltaic robot in the height direction under a navigation coordinate system,
Figure DEST_PATH_IMAGE074
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:
Figure 694265DEST_PATH_IMAGE075
，
wherein,
Figure DEST_PATH_IMAGE076
，
Figure 207286DEST_PATH_IMAGE077
in order to be a process noise variance matrix,
Figure DEST_PATH_IMAGE078
,
Figure 464961DEST_PATH_IMAGE079
，
Figure DEST_PATH_IMAGE080
，
Figure 456051DEST_PATH_IMAGE081
is added upwards in the oriental
The process noise of the speed is such that,
Figure DEST_PATH_IMAGE082
is zero deviation of the acceleration in the direction of the X-axis,
Figure 499093DEST_PATH_IMAGE083
，
Figure DEST_PATH_IMAGE084
is in the north direction
The process noise of the acceleration is such that,
Figure 166704DEST_PATH_IMAGE085
is zero deviation of the acceleration in the Y-axis direction,
Figure DEST_PATH_IMAGE086
，
Figure 662407DEST_PATH_IMAGE087
in the height direction
The process noise of the acceleration is such that,
Figure DEST_PATH_IMAGE088
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:
Figure 972034DEST_PATH_IMAGE001
wherein,
Figure 135163DEST_PATH_IMAGE002
as a parameter of the time of day,
Figure 255565DEST_PATH_IMAGE013
in order to correspond to the observed value of the state quantity,
Figure 769723DEST_PATH_IMAGE089
、
Figure 883042DEST_PATH_IMAGE005
and
Figure DEST_PATH_IMAGE090
respectively the position of the photovoltaic robot in the east direction, the north direction and the height direction,
Figure 635097DEST_PATH_IMAGE007
、
Figure 191980DEST_PATH_IMAGE008
and
Figure 177123DEST_PATH_IMAGE009
respectively the speed of the photovoltaic robot in the east direction, the speed in the north direction and the speed in the height direction,
Figure 844864DEST_PATH_IMAGE091
is zero-mean white Gaussian noise in the east direction,
Figure DEST_PATH_IMAGE092
Is positiveZero-mean white Gaussian noise in the north direction,
Figure 451426DEST_PATH_IMAGE093
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:
Figure 179211DEST_PATH_IMAGE012
wherein,
Figure 651649DEST_PATH_IMAGE002
as a parameter of the time of day,
Figure 857503DEST_PATH_IMAGE003
in order to correspond to the observed value of the state quantity,
Figure 115309DEST_PATH_IMAGE094
is a course angle under an INS coordinate system on the photovoltaic robot,
Figure 13995DEST_PATH_IMAGE015
Is a pitch angle of the photovoltaic robot under an INS coordinate system,
Figure 708150DEST_PATH_IMAGE095
Is the tilt angle under the INS coordinate system on the photovoltaic robot,
Figure 717694DEST_PATH_IMAGE017
is the course angular velocity under the INS coordinate system on the photovoltaic robot,
Figure 830007DEST_PATH_IMAGE096
The pitch elevation speed under the INS coordinate system on the photovoltaic robot,
Figure 899594DEST_PATH_IMAGE097
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:
Figure 97357DEST_PATH_IMAGE020
wherein,
Figure 894281DEST_PATH_IMAGE021
acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,
Figure 126679DEST_PATH_IMAGE022
Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,
Figure 101588DEST_PATH_IMAGE023
The acceleration of the photovoltaic robot in the height direction under the navigation coordinate system is obtained,
Figure 786647DEST_PATH_IMAGE024
acceleration data of the photovoltaic robot in the X-axis direction under the INS coordinate system,
Figure 387262DEST_PATH_IMAGE025
Acceleration data of the photovoltaic robot in the Y-axis direction under the INS coordinate system,
Figure 474167DEST_PATH_IMAGE026
Acceleration data of the photovoltaic robot in the Z-axis direction under the INS coordinate system,
Figure 885556DEST_PATH_IMAGE027
is zero offset relative to the acceleration in the X-axis direction,
Figure 792332DEST_PATH_IMAGE028
Is zero offset relative to acceleration in the Y-axis direction,
Figure 681791DEST_PATH_IMAGE029
For zero offset in relation to acceleration in the direction of the Z axis, said
Figure 872470DEST_PATH_IMAGE030
Is zero mean white Gaussian noise in the X-axis direction
Figure 454761DEST_PATH_IMAGE031
Is zero mean Gaussian white noise in the Y-axis direction
Figure 848833DEST_PATH_IMAGE098
Is 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:
Figure 807562DEST_PATH_IMAGE099
，
wherein,
Figure 603480DEST_PATH_IMAGE034
Figure 605940DEST_PATH_IMAGE035
，
Figure 487308DEST_PATH_IMAGE100
zero deviation of course angular speed of the photovoltaic robot;
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:
Figure 984148DEST_PATH_IMAGE037
，
wherein,
Figure 634572DEST_PATH_IMAGE038
，
Figure 807934DEST_PATH_IMAGE039
is the interval between the sampling of the samples,
Figure 176598DEST_PATH_IMAGE040
Figure 477129DEST_PATH_IMAGE041
；
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:
Figure 982060DEST_PATH_IMAGE042
，
wherein,
Figure 77055DEST_PATH_IMAGE043
Figure 447862DEST_PATH_IMAGE044
，
Figure 20926DEST_PATH_IMAGE045
zero deviation of the pitch angle speed of the photovoltaic robot;
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:
Figure 645943DEST_PATH_IMAGE101
，
wherein，
Figure 911839DEST_PATH_IMAGE047
，
Figure 74721DEST_PATH_IMAGE048
Is the interval between the sampling of the samples,
Figure 717055DEST_PATH_IMAGE049
Figure 196578DEST_PATH_IMAGE050
；
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:
Figure 633376DEST_PATH_IMAGE102
，
wherein,
Figure 447617DEST_PATH_IMAGE052
Figure 628062DEST_PATH_IMAGE053
，
Figure 227671DEST_PATH_IMAGE054
zero deviation of the inclination angular velocity of the photovoltaic robot;
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:
Figure 835370DEST_PATH_IMAGE055
，
wherein,
Figure 887640DEST_PATH_IMAGE056
，
Figure 121044DEST_PATH_IMAGE057
is the interval between the sampling of the samples,
Figure 575159DEST_PATH_IMAGE058
Figure 353759DEST_PATH_IMAGE059
。
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:
Figure 893325DEST_PATH_IMAGE060
wherein
Figure 681152DEST_PATH_IMAGE061
is the position of the photovoltaic robot in the east-ward direction,
Figure 239041DEST_PATH_IMAGE062
The speed of the photovoltaic robot in the east direction,
Figure 188543DEST_PATH_IMAGE063
Acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,
Figure 949825DEST_PATH_IMAGE064
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:
Figure 541344DEST_PATH_IMAGE065
wherein
Figure 953739DEST_PATH_IMAGE066
the position of the photovoltaic robot in the positive north direction,
Figure 74142DEST_PATH_IMAGE067
The speed of the photovoltaic robot in the positive north direction,
Figure 588300DEST_PATH_IMAGE068
Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,
Figure 717930DEST_PATH_IMAGE069
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:
Figure 735565DEST_PATH_IMAGE070
wherein
Figure 541716DEST_PATH_IMAGE071
the position of the photovoltaic robot in the height direction,
Figure 277590DEST_PATH_IMAGE072
The speed of the photovoltaic robot in the height direction,
Figure 945332DEST_PATH_IMAGE073
Acceleration of the photovoltaic robot in the height direction under a navigation coordinate system,
Figure 83052DEST_PATH_IMAGE074
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:
Figure 60104DEST_PATH_IMAGE075
，
wherein,
Figure 17696DEST_PATH_IMAGE076
，
Figure 754708DEST_PATH_IMAGE077
in order to be a process noise variance matrix,
Figure 746935DEST_PATH_IMAGE078
,
Figure 380041DEST_PATH_IMAGE079
，
Figure 339776DEST_PATH_IMAGE080
，
Figure 349320DEST_PATH_IMAGE081
is added upwards in the oriental
The process noise of the speed is such that,
Figure 461633DEST_PATH_IMAGE082
is zero deviation of the acceleration in the direction of the X-axis,
Figure 531220DEST_PATH_IMAGE083
，
Figure 712672DEST_PATH_IMAGE084
is in the north direction
The process noise of the acceleration is such that,
Figure 525907DEST_PATH_IMAGE085
is zero deviation of the acceleration in the Y-axis direction,
Figure 492726DEST_PATH_IMAGE086
，
Figure 733214DEST_PATH_IMAGE087
in the height direction
The process noise of the acceleration is such that,
Figure 418274DEST_PATH_IMAGE088
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:
Figure 905335DEST_PATH_IMAGE001
，
wherein,
Figure 692026DEST_PATH_IMAGE002
Figure 676162DEST_PATH_IMAGE003
，
Figure 379545DEST_PATH_IMAGE004
zero deviation of course angular speed of the photovoltaic robot;
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:
Figure 158145DEST_PATH_IMAGE005
，
wherein,
Figure 432132DEST_PATH_IMAGE006
，
Figure 954380DEST_PATH_IMAGE007
is the interval between the sampling of the samples,
Figure 777848DEST_PATH_IMAGE008
Figure 727350DEST_PATH_IMAGE009
；
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:
Figure 754211DEST_PATH_IMAGE010
，
wherein,
Figure 80151DEST_PATH_IMAGE011
Figure 508858DEST_PATH_IMAGE012
，
Figure 878528DEST_PATH_IMAGE013
zero deviation of the pitch angle speed of the photovoltaic robot;
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:
Figure 127107DEST_PATH_IMAGE014
，
wherein,
Figure 256737DEST_PATH_IMAGE015
，
Figure 539951DEST_PATH_IMAGE016
is the interval between the sampling of the samples,
Figure 80522DEST_PATH_IMAGE017
Figure 816397DEST_PATH_IMAGE018
；
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:
Figure 749718DEST_PATH_IMAGE019
，
wherein,
Figure 887438DEST_PATH_IMAGE020
Figure 349644DEST_PATH_IMAGE021
，
Figure 822082DEST_PATH_IMAGE022
zero deviation of the inclination angle speed of the photovoltaic robot;
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:
Figure 559094DEST_PATH_IMAGE023
，
wherein,
Figure 551321DEST_PATH_IMAGE024
，
Figure 184428DEST_PATH_IMAGE025
is the interval between the sampling of the samples,
Figure 160474DEST_PATH_IMAGE026
Figure 419286DEST_PATH_IMAGE027
。
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:
Figure 266019DEST_PATH_IMAGE028
wherein,
Figure 335606DEST_PATH_IMAGE029
as a parameter of the time of day,
Figure 533369DEST_PATH_IMAGE030
in order to correspond to the observed value of the state quantity,
Figure 595872DEST_PATH_IMAGE031
、
Figure 297112DEST_PATH_IMAGE032
and
Figure 537600DEST_PATH_IMAGE033
respectively the position of the photovoltaic robot in the east direction, the north direction and the height direction,
Figure 222660DEST_PATH_IMAGE034
、
Figure 574007DEST_PATH_IMAGE035
and
Figure 644600DEST_PATH_IMAGE036
respectively the speed of the photovoltaic robot in the east direction, the speed in the north direction and the speed in the height direction,
Figure 55989DEST_PATH_IMAGE037
is zero-mean white Gaussian noise in the east direction,
Figure 228345DEST_PATH_IMAGE038
Is zero-mean Gaussian white noise in the north direction,
Figure 383383DEST_PATH_IMAGE039
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:
Figure 331920DEST_PATH_IMAGE040
wherein,
Figure 914211DEST_PATH_IMAGE029
as a parameter of the time of day,
Figure 308283DEST_PATH_IMAGE041
in order to correspond to the observed value of the state quantity,
Figure 267012DEST_PATH_IMAGE042
is a course angle under an INS coordinate system on the photovoltaic robot,
Figure 62929DEST_PATH_IMAGE043
Is a pitch angle of the photovoltaic robot under an INS coordinate system,
Figure 799810DEST_PATH_IMAGE044
Is the tilt angle under the INS coordinate system on the photovoltaic robot,
Figure 681178DEST_PATH_IMAGE045
is the course angular velocity under the INS coordinate system on the photovoltaic robot,
Figure 443598DEST_PATH_IMAGE046
The pitch elevation speed under the INS coordinate system on the photovoltaic robot,
Figure 94022DEST_PATH_IMAGE047
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:
Figure 267383DEST_PATH_IMAGE048
wherein,
Figure 370469DEST_PATH_IMAGE041
in order to correspond to the observed value of the state quantity,
Figure 405421DEST_PATH_IMAGE049
acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,
Figure 441510DEST_PATH_IMAGE050
Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,
Figure 536505DEST_PATH_IMAGE051
The acceleration of the photovoltaic robot in the height direction under the navigation coordinate system is obtained,
Figure 376154DEST_PATH_IMAGE052
acceleration data of the photovoltaic robot in the X-axis direction under the INS coordinate system,
Figure 480376DEST_PATH_IMAGE053
Acceleration data of the photovoltaic robot in the Y-axis direction under the INS coordinate system,
Figure 105392DEST_PATH_IMAGE054
Acceleration data of the photovoltaic robot in the Z-axis direction under the INS coordinate system,
Figure 371289DEST_PATH_IMAGE055
is zero offset relative to the acceleration in the X-axis direction,
Figure 698234DEST_PATH_IMAGE056
Is zero offset relative to acceleration in the Y-axis direction,
Figure 74988DEST_PATH_IMAGE057
For zero offset in relation to acceleration in the direction of the Z axis, said
Figure 554511DEST_PATH_IMAGE058
Is zero mean white Gaussian noise in the X-axis direction
Figure 256888DEST_PATH_IMAGE059
Is zero mean Gaussian white noise in the Y-axis direction
Figure 821862DEST_PATH_IMAGE060
Is 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:
Figure 251575DEST_PATH_IMAGE061
wherein
Figure 585604DEST_PATH_IMAGE062
is the position of the photovoltaic robot in the east-ward direction,
Figure 458882DEST_PATH_IMAGE063
The speed of the photovoltaic robot in the east direction,
Figure 511152DEST_PATH_IMAGE064
Acceleration of the photovoltaic robot in the east-righting direction under a navigation coordinate system,
Figure 744556DEST_PATH_IMAGE065
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:
Figure 933092DEST_PATH_IMAGE066
wherein
Figure 977271DEST_PATH_IMAGE067
the position of the photovoltaic robot in the positive north direction,
Figure 251258DEST_PATH_IMAGE068
The speed of the photovoltaic robot in the positive north direction,
Figure 22774DEST_PATH_IMAGE069
Acceleration of the photovoltaic robot in the positive north direction under a navigation coordinate system,
Figure 596974DEST_PATH_IMAGE070
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:
Figure 812055DEST_PATH_IMAGE071
wherein
Figure 838917DEST_PATH_IMAGE072
the position of the photovoltaic robot in the height direction,
Figure 899277DEST_PATH_IMAGE073
The speed of the photovoltaic robot in the height direction,
Figure 311672DEST_PATH_IMAGE074
Acceleration of the photovoltaic robot in the height direction under a navigation coordinate system,
Figure 697654DEST_PATH_IMAGE075
Is zero offset relative to acceleration in the Z direction.
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:
Figure 211812DEST_PATH_IMAGE076
，
wherein,
Figure 75863DEST_PATH_IMAGE077
，
Figure 359077DEST_PATH_IMAGE078
in order to be a process noise variance matrix,
Figure 165228DEST_PATH_IMAGE079
,
Figure 901103DEST_PATH_IMAGE080
，
Figure 303265DEST_PATH_IMAGE081
，
Figure 706565DEST_PATH_IMAGE082
is the process noise of the acceleration in the east direction,
Figure 418038DEST_PATH_IMAGE083
is zero deviation of the acceleration in the direction of the X-axis,
Figure 906788DEST_PATH_IMAGE084
，
Figure 112641DEST_PATH_IMAGE085
process noise that is the acceleration in the due north direction,
Figure 104868DEST_PATH_IMAGE086
is zero deviation of the acceleration in the Y-axis direction,
Figure 3554DEST_PATH_IMAGE087
，
Figure 963288DEST_PATH_IMAGE088
process noise that is the acceleration in the height direction,
Figure 707253DEST_PATH_IMAGE089
zero deviation of acceleration in the Z-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.
CN202210810871.4A 2022-07-11 2022-07-11 Fusion positioning method for photovoltaic robot Pending CN114877892A (en)

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)

* Cited by examiner, † Cited by third party
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

Patent Citations (9)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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