CN112179332A - Hybrid positioning method and system for unmanned forklift - Google Patents
Hybrid positioning method and system for unmanned forklift Download PDFInfo
- Publication number
- CN112179332A CN112179332A CN202011070254.2A CN202011070254A CN112179332A CN 112179332 A CN112179332 A CN 112179332A CN 202011070254 A CN202011070254 A CN 202011070254A CN 112179332 A CN112179332 A CN 112179332A
- Authority
- CN
- China
- Prior art keywords
- positioning
- data
- positioning data
- observation
- sensor
- 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
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/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/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
-
- 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/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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0257—Hybrid positioning
-
- 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
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/10—Position of receiver fixed by co-ordinating a plurality of position lines defined by path-difference measurements, e.g. omega or decca systems
-
- 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
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
- G01S5/26—Position of receiver fixed by co-ordinating a plurality of position lines defined by path-difference measurements
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
The invention relates to the technical field of intelligent warehousing and discloses a hybrid positioning method and a hybrid positioning system for an unmanned forklift, wherein the method comprises the following steps: acquiring first positioning data through a first type of sensor of the unmanned forklift, and fusing the first positioning data to acquire process positioning data; acquiring second positioning data through a second type sensor of the unmanned forklift, and fusing the second positioning data to obtain observation positioning data; and fusing the process positioning data and the observation positioning data through an extended Kalman filter to obtain mixed positioning data. The method comprises the steps of acquiring process positioning data and observation positioning data of the unmanned forklift through a plurality of sensors, enabling the unmanned forklift to still keep normal operation on the premise that a certain sensor fails, improving positioning robustness of the unmanned forklift, and obtaining mixed positioning data through integrating the process positioning data and the observation positioning data through an extended Kalman filter method to realize mixed positioning of the unmanned forklift, so that positioning accuracy of the unmanned forklift is improved.
Description
Technical Field
The invention relates to the technical field of intelligent warehousing, in particular to a hybrid positioning method and system for an unmanned forklift.
Background
Along with the development of intelligent warehousing technology, unmanned fork truck is being applied to links such as the letter sorting in industry more and more, packing, transport, with replace artifical transportation goods, realize unmanned fork truck's automatic operation and corresponding autonomic obstacle avoidance function's in-process, unmanned fork truck's location technique has played the key role, and adopt single sensor to combine the landmark to fix a position among the prior art, there is certain limitation on the location route, positioning accuracy and location robustness can be worried, therefore, how to realize unmanned fork truck's hybrid location in order to improve positioning accuracy and location robustness, become a problem that waits to solve urgently.
The above is only for the purpose of assisting understanding of the technical aspects of the present invention, and does not represent an admission that the above is prior art.
Disclosure of Invention
The invention mainly aims to provide a hybrid positioning method and a hybrid positioning system for an unmanned forklift, and aims to solve the technical problem of how to realize hybrid positioning of the unmanned forklift to improve positioning accuracy and positioning robustness.
In order to achieve the purpose, the invention provides a hybrid positioning method of an unmanned forklift, which comprises the following steps:
acquiring first positioning data through a first type of sensor of the unmanned forklift, and fusing the first positioning data to acquire process positioning data;
acquiring second positioning data through a second type sensor of the unmanned forklift, and fusing the second positioning data to obtain observation positioning data;
and fusing the process positioning data and the observation positioning data through an extended Kalman filter to obtain mixed positioning data.
Preferably, the first type of sensor comprises an inertial navigation unit and a wheel encoder;
correspondingly, the step of obtaining first positioning data through the first type of sensor of unmanned forklift, and fusing the first positioning data to obtain process positioning data specifically includes:
acquiring angle change information and displacement change information through the wheel type encoder, and acquiring angular velocity information through the inertial navigation unit;
and fusing the angle change information, the displacement change information and the angular speed information through a Kalman fusion algorithm to obtain process positioning data.
Preferably, the second type of sensor comprises a laser radar, an ultra-wideband positioning unit and an ultrasonic sensor;
correspondingly, second type sensor through unmanned fork truck acquires the second location data to fuse the second location data obtains the step of observing the location data, specifically includes:
acquiring corresponding laser radar positioning coordinates, ultra-wideband positioning coordinates and ultrasonic positioning coordinates through the laser radar, the ultra-wideband positioning unit and the ultrasonic sensor respectively;
and fusing the laser radar positioning coordinate, the ultra-wideband positioning coordinate and the ultrasonic positioning coordinate through a Kalman fusion algorithm to obtain observation positioning data.
Preferably, the step of obtaining first positioning data through a first type of sensor of the unmanned forklift, fusing the first positioning data, and obtaining process positioning data specifically includes:
acquiring first positioning data through a first type of sensor of an unmanned forklift, and respectively acquiring signal frequencies of the first type of sensor;
respectively carrying out high-pass filtering processing on the first positioning data according to the signal frequency to obtain first filtering data;
fusing the first filtering data to obtain process positioning data;
correspondingly, second type sensor through unmanned fork truck acquires the second location data to fuse the second location data obtains the step of observing the location data, specifically includes:
acquiring second positioning data through a second type sensor of the unmanned forklift, and respectively acquiring signal frequencies of the second type sensor;
respectively carrying out high-pass filtering processing on the second positioning data according to the signal frequency to obtain second filtering data;
and fusing the second filtering data to obtain observation positioning data.
Preferably, the step of fusing the process positioning data and the observation positioning data by using an extended kalman filter to obtain the mixed positioning data specifically includes:
acquiring observation angle data through a laser radar;
and fusing the observation angle data, the process positioning data and the observation positioning data by using an extended Kalman filter to obtain mixed positioning data, and determining the fused position and the fused angle of the unmanned forklift based on the mixed positioning data.
In addition, in order to achieve the above object, the present invention further provides a hybrid positioning system for an unmanned forklift, including:
the process positioning module is used for acquiring first positioning data through a first type of sensor of the unmanned forklift, and fusing the first positioning data to acquire process positioning data;
the observation positioning module is used for acquiring second positioning data through a second type sensor of the unmanned forklift, and fusing the second positioning data to acquire observation positioning data;
and the hybrid positioning module is used for fusing the process positioning data and the observation positioning data through an extended Kalman filter to obtain hybrid positioning data.
The first type of sensor comprises an inertial navigation unit and a wheel type encoder;
the process positioning module is also used for acquiring angle change information and displacement change information through the wheel type encoder and acquiring angular velocity information through the inertial navigation unit;
the process positioning module is further configured to fuse the angle change information, the displacement change information, and the angular velocity information through a kalman fusion algorithm to obtain process positioning data.
The second type of sensor comprises a laser radar, an ultra-wideband positioning unit and an ultrasonic sensor;
the observation positioning module is also used for acquiring corresponding laser radar positioning coordinates, ultra-wideband positioning coordinates and ultrasonic positioning coordinates through the laser radar, the ultra-wideband positioning unit and the ultrasonic sensor respectively;
the observation positioning module is further used for fusing the laser radar positioning coordinate, the ultra-wideband positioning coordinate and the ultrasonic positioning coordinate through a Kalman fusion algorithm to obtain observation positioning data.
The process positioning module is also used for acquiring first positioning data through a first type of sensor of the unmanned forklift and respectively acquiring the signal frequency of the first type of sensor;
the process positioning module is further configured to perform high-pass filtering processing on the first positioning data according to the signal frequency to obtain first filtered data;
the process positioning module is further configured to fuse the first filtering data to obtain process positioning data;
the observation positioning module is also used for acquiring second positioning data through a second type of sensor of the unmanned forklift and respectively acquiring the signal frequency of the second type of sensor;
the observation positioning module is further configured to perform high-pass filtering processing on the second positioning data according to the signal frequencies to obtain second filtering data;
and the observation positioning module is also used for fusing the second filtering data to obtain observation positioning data.
The observation positioning module is also used for acquiring observation angle data through a laser radar;
the hybrid positioning module is further used for fusing the observation angle data, the process positioning data and the observation positioning data by using an extended Kalman filter to obtain hybrid positioning data, and determining the fusion position and the fusion angle of the unmanned forklift based on the hybrid positioning data.
According to the method, first positioning data are obtained through a first type of sensor of the unmanned forklift and are fused to obtain process positioning data, second positioning data are obtained through a second type of sensor of the unmanned forklift and are fused to obtain observation positioning data, and the process positioning data and the observation positioning data are fused through an extended Kalman filter to obtain mixed positioning data. The method comprises the steps that the process positioning data and the observation positioning data of the unmanned forklift are acquired through a plurality of sensors, so that the unmanned forklift can still keep normal operation on the premise that a certain sensor fails, the positioning robustness of the unmanned forklift is improved, the mixed positioning data is acquired by fusing the process positioning data and the observation positioning data acquired by the plurality of sensors, the positioning precision of the unmanned forklift is improved, further, the mixed positioning data is acquired by linearizing a model at each estimation point through the extended Kalman filter method, the mixed positioning of the unmanned forklift is realized, the positioning data noise is effectively inhibited, and the positioning precision of the unmanned forklift is further improved.
Drawings
FIG. 1 is a schematic diagram of a sensor arrangement for an unmanned forklift in a hardware operating environment according to an embodiment of the invention;
fig. 2 is a schematic flow chart of a hybrid positioning method for an unmanned forklift according to a first embodiment of the present invention;
fig. 3 is a block diagram illustrating a hybrid positioning system of an unmanned forklift according to a first embodiment of the present invention.
The implementation, functional features and advantages of the objects of the present invention will be further explained with reference to the accompanying drawings.
Detailed Description
It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Referring to fig. 1, fig. 1 is a schematic diagram of a sensor arrangement of an unmanned forklift in a hardware operating environment according to an embodiment of the present invention.
As shown in fig. 1, the unmanned forklift may include: the system comprises a laser radar 1, an Ultra Wide Band (UWB) positioning unit 2, an ultrasonic sensor 3, a wheel type encoder 4 and an inertial navigation unit 5.
The laser radar 1 is fixed at the top end of the unmanned forklift, continuously emits infrared laser to sense the position of the indoor light-sensing plate, and performs Beam Adjustment (BA) optimization according to an Iterative Closest Point (ICP) method to obtain the real-time position of the unmanned forklift in space.
The ultra-wideband positioning unit 2 is mainly used for receiving photoelectric signals Of ultra-wideband positioning terminals located at different positions, and measuring the positions between the ultra-wideband positioning terminals by using a Two-Way Time Of Flight (TW-TOF) distance measurement method according to a Time Of Flight (TOF). Further, the relative position of the unmanned forklift is obtained by measuring the distance between the ultra-wideband positioning terminals.
The ultrasonic sensor 3 activates the ultrasonic receiving ends by using a radio frequency technology so that the ultrasonic receiving ends send ultrasonic signals, calculates the distance from the ultrasonic transmitting end to the ultrasonic receiving end by calculating the time for the ultrasonic receiving ends to receive the ultrasonic signals, and further obtains the relative position of the unmanned forklift by measuring each distance.
The wheel type encoder 4 is mainly used for recording the rotation quantity of the wheels of the unmanned forklift and obtaining the speed and the angular speed of the wheel type encoder through a chassis kinematic model of the unmanned forklift.
The inertial navigation unit 5 is mainly used for outputting the angular velocity and the acceleration of the unmanned forklift.
Those skilled in the art will appreciate that the configuration shown in fig. 1 is not intended to be limiting of unmanned forklifts and may include more or fewer components than shown, or some components may be combined, or a different arrangement of components.
Based on a sensor arrangement schematic diagram of an unmanned forklift in a hardware operating environment according to an embodiment of the invention, a hybrid positioning method for the unmanned forklift is provided, and fig. 2 is a flowchart of a first embodiment of the hybrid positioning method for the unmanned forklift.
In this embodiment, the hybrid positioning method for the unmanned forklift includes the following steps:
step S10: acquiring first positioning data through a first type of sensor of the unmanned forklift, and fusing the first positioning data to acquire process positioning data;
it should be noted that, the first type of sensor includes, but is not limited to, an inertial navigation unit and a wheel encoder, the inertial navigation unit is mainly used for obtaining angular velocity information, the angular velocity information may be understood as the angular velocity of the wheels of the unmanned forklift, the wheel encoder mainly collects angular change information and displacement change information, the displacement change information may be understood as displacement increment corresponding to three-dimensional coordinates of the unmanned forklift obtained by the wheel encoder, the angular change information may be understood as deflection angle increment of the inertial navigation unit and angle increment of the chassis of the unmanned forklift, the deflection angle increment may be obtained according to the angular velocity information, the angle increment may be obtained according to the deflection angle of the steering wheels of the unmanned forklift, in a specific implementation, in order to improve the positioning accuracy of the unmanned forklift, the first type of sensor of the unmanned forklift may be used for obtaining the first positioning data, respectively acquiring signal frequencies of the first type of sensors, respectively performing high-pass filtering processing on the first positioning data according to the signal frequencies to acquire first filtering data, namely, after acquiring the deflection angle increment and the angle increment, respectively performing high-pass filtering processing on the deflection angle increment and the angle increment to acquire filtered deflection angle increment and filtered angle increment, which can be understood as acquiring variances corresponding to the deflection angle increment and the angle increment, and taking the filtered deflection angle increment, the filtered angle increment and the displacement change information as first filtering data, wherein the high-pass filtering processing can be realized by acquiring sampling data (which can be understood as the deflection angle increment and the angle increment) of a preset number of periods of sensors and high-frequency signal frequencies corresponding to the sensors, the preset number can be determined according to actual requirements, which is not limited in this embodiment, and then, the butterworth high-pass filter is designed according to the high-frequency signal frequency, the preset cutoff frequency and the attenuation condition, which can be implemented by the following formula,
wherein Y is the filtered sample data, X is the sample data, D0And D is the high-frequency signal frequency corresponding to the sensor, and n is the order of the Butterworth high-pass filter.
Further, the first filtered data may be fused to obtain process positioning data. In specific implementation, in order to realize the fusion of data acquired by various sensors, after angle change information and displacement change information are acquired by the wheel-type encoder and angular velocity information is acquired by the inertial navigation unit, the angle change information, the displacement change information and the angular velocity information are fused by a kalman fusion algorithm to acquire process positioning data. That is, the process positioning data may be obtained by fusing the filtered deflection angle increment, the filtered angle increment, and the displacement change information.
Step S20: acquiring second positioning data through a second type sensor of the unmanned forklift, and fusing the second positioning data to obtain observation positioning data;
it should be noted that the second type of sensor includes laser radar, ultra wide band positioning unit and ultrasonic sensor, laser radar for acquire unmanned forklift's laser radar location coordinate, ultra wide band positioning unit is used for acquiring unmanned forklift's ultra wide band location coordinate, ultrasonic sensor is used for acquiring supersound location coordinate, then, fuses through the kalman algorithm laser radar location coordinate ultra wide band location coordinate and supersound location coordinate obtains the observation location data. In the concrete implementation, because the ultrasonic sensor, the ultra-wideband positioning module and the laser radar have time delay, before the sensor is used for collecting positioning data to perform data fusion, the unified work of a time axis is needed firstly, the unmanned forklift can be driven to do uniform acceleration or uniform deceleration motion in a field, the data collected by the sensor is collected by taking encoder data as a reference, so that time delay is obtained, and observation positioning data of the pose of the unmanned forklift at the current moment of the sensor is obtained according to the time delay and the historical data of the encoder.
In a specific implementation, in order to improve the positioning accuracy of the unmanned forklift, a second type of sensor of the unmanned forklift is used to obtain second positioning data, and signal frequencies of the second type of sensor are respectively obtained, then, the second positioning data is respectively subjected to high-pass filtering according to the signal frequencies to obtain second filtering data, which can be understood as obtaining variances corresponding to the lidar positioning coordinate, the ultra-wideband positioning coordinate and the ultrasonic positioning coordinate, a specific formula of the high-pass filtering is the same as the above formula, in a specific implementation, the preset number of the positioning data can be determined according to actual requirements by obtaining sampling data of a sensor (which can be understood as the lidar positioning coordinate, the ultra-wideband positioning coordinate and the ultrasonic positioning coordinate) in a preset number of cycles and high-frequency signal frequencies corresponding to the sensor, the embodiment is not limited to this, then, a butterworth high-pass filter as described in the above formula is designed according to the high-frequency signal frequency, the preset cutoff frequency and the attenuation condition to obtain the filtered lidar positioning coordinate, the filtered ultra-wideband positioning coordinate and the filtered ultrasonic positioning coordinate, and the filtered lidar positioning coordinate, the filtered ultra-wideband positioning coordinate and the filtered ultrasonic positioning coordinate are used as second filtering data, and then the second filtering data is fused through a kalman fusion algorithm card to obtain the observation positioning data.
Step S30: and fusing the process positioning data and the observation positioning data through an extended Kalman filter to obtain mixed positioning data.
It should be noted that, when data fusion is performed through the extended kalman filter, observation angle data can be acquired through a laser radar, then the observation angle data, the process positioning data and the observation positioning data are fused by using the extended kalman filter to acquire mixed positioning data, and the fusion position and the fusion angle of the unmanned forklift are determined based on the mixed positioning data. The extended kalman filter may be implemented by the following equation,
X(k|k-1)＝AX(k-1|k-1)+Bu(k)
P(k|k-1)＝A*P(k-1|k-1)A′+Q
X(k|k)＝X(k|k-1)+Kg(k)*(Z(k)-HX(k|k-1))
Kg(k)＝P(k|k-1)*H′/(H*P(k|k-1)*H′+R)
p(k|k)＝(I-Kg(k)*H)*P(k|k-1)
wherein X (k | k-1) is the prediction of the system process quantity at the previous time (k-1) to the system process quantity at the current time (k-1), X (k-1| k-1) is the optimal result of the system process quantity at the previous time, A and B are preset parameters of the system, in this embodiment, A and B can be preset matrixes, u (k) is the control quantity of the system at the time k, P (k | k-1) is an error estimation covariance matrix corresponding to the system observation X (k | k-1), P (k-1| k-1) is an error estimation covariance matrix corresponding to the system observation X (k-1| k-1), A' is the transpose of the matrix A, Q is the covariance of the noise of the system process, X (k | k) is the system process quantity at the time k, Kg (k) is Kalman gain, or kalman coefficient, which is a middle core coefficient and represents reliability distribution of process quantity and observed quantity, z (k) is system observed quantity at time k, H is a preset parameter for the system, in this embodiment, H may be a matrix, H' is a transpose of the matrix H, R is a covariance of system observation noise, P (k | k) is an error estimation covariance matrix corresponding to system observation measurement X (k | k), and I is a unit matrix.
Through the formula, three times of Kalman fusion of the positioning data collected by the sensor are realized (the first time Kalman fusion is to fuse the displacement increment and the angle increment collected by the wheel type encoder and the angular velocity collected by the inertial navigation unit to obtain a system process quantity, the second time Kalman fusion is to fuse the ultra-wideband positioning coordinate, the laser radar coordinate and the ultrasonic positioning coordinate to obtain a position observed quantity, the third time Kalman fusion is to fuse the system process quantity, the position observed quantity and the angle observed quantity collected by the laser radar to obtain a fusion position and a fusion angle) to obtain final mixed positioning data, then, based on the mixed positioning data, determining a fusion position and a fusion angle of the unmanned forklift, and then based on the fusion position and the fusion angle, realizing the control of the unmanned forklift.
In specific implementation, the data of each sensor can be monitored in real time to judge whether each sensor fails or fails, so that the sensors with faults or failures can be checked in time, the positioning robustness of the system is improved, for example, when the inertial navigation unit, the ultra-wideband positioning unit or the ultrasonic unit is monitored to fail, the alarm level of the system is set to be 3-level abnormal (lower), abnormal information is reported, and the fusion weight of the abnormal sensors is set to be zero; when the wheel type encoder is monitored to be invalid, the alarm level of the system is set to be 2-level abnormity (middle grade), abnormal information is reported, and the fusion weight of the wheel type encoder is set to be zero; when the laser radar is abnormal such as drifting and the like, the system alarm level is set to be 1-level abnormal (advanced), abnormal information is reported, a timer is started, the fusion weight of the laser radar is set to be zero, and when the timer reaches a threshold value and the laser radar still does not recover to be normal through global repositioning, the unmanned forklift stops moving and sends corresponding warning information.
It should be understood that the above is only an example, and the technical solution of the present invention is not limited in any way, and in a specific application, a person skilled in the art may set the technical solution as needed, and the present invention is not limited thereto.
This embodiment obtains first locating data through unmanned fork truck's first class sensor, and fuses first locating data obtains the process positioning data, obtains the second locating data through unmanned fork truck's second class sensor, and fuses the second locating data obtains the observation locating data, will the process positioning data with the observation locating data fuses through extension kalman filter, obtains mixed locating data. Realize right through a plurality of sensors unmanned fork truck's process positioning data and the collection of observation location data to the realization is under the prerequisite of a certain sensor inefficacy, unmanned fork truck still can keep normal operating, improves unmanned fork truck's location robustness, and obtains mixed location data through the process positioning data that fuses a plurality of sensor collection and observation location data and improve unmanned fork truck's positioning accuracy, and further, through this embodiment the extension kalman filter method will model linearization obtain mixed location data at each estimation point, realize unmanned fork truck's mixed location, with restrain the location data noise effectively, further improve unmanned fork truck's positioning accuracy.
Referring to fig. 3, fig. 3 is a block diagram illustrating a hybrid positioning system of an unmanned forklift according to a first embodiment of the present invention.
As shown in fig. 3, the hybrid positioning system for an unmanned forklift according to the embodiment of the present invention includes:
the system comprises a process positioning module 1001, a process positioning module and a control module, wherein the process positioning module is used for acquiring first positioning data through a first type of sensor of the unmanned forklift, fusing the first positioning data and acquiring process positioning data;
the observation positioning module 1002 is configured to acquire second positioning data through a second type of sensor of the unmanned forklift, and fuse the second positioning data to acquire observation positioning data;
and the hybrid positioning module 1003 is configured to fuse the process positioning data and the observation positioning data through an extended kalman filter to obtain hybrid positioning data.
This embodiment obtains first locating data through unmanned fork truck's first class sensor, and fuses first locating data obtains the process positioning data, obtains the second locating data through unmanned fork truck's second class sensor, and fuses the second locating data obtains the observation locating data, will the process positioning data with the observation locating data fuses through extension kalman filter, obtains mixed locating data. Realize right through a plurality of sensors unmanned fork truck's process positioning data and the collection of observation location data to the realization is under the prerequisite of a certain sensor inefficacy, unmanned fork truck still can keep normal operating, improves unmanned fork truck's location robustness, and obtains mixed location data through the process positioning data that fuses a plurality of sensor collection and observe the location house number and improve unmanned fork truck's positioning accuracy, and further, through this embodiment the extension kalman filter method will model linearization obtain mixed location data at each estimation point, realize unmanned fork truck's mixed location, with restrain the location data noise effectively, further improve unmanned fork truck's positioning accuracy.
Based on the first embodiment of the hybrid positioning system for the unmanned forklift, the second embodiment of the hybrid positioning system for the unmanned forklift is provided.
In this embodiment, the first type of sensor includes an inertial navigation unit and a wheel encoder;
the process positioning module 1001 is further configured to acquire angle change information and displacement change information through the wheel encoder, and acquire angular velocity information through the inertial navigation unit;
the process positioning module 1001 is further configured to fuse the angle change information, the displacement change information, and the angular velocity information through a kalman fusion algorithm to obtain process positioning data.
The second type of sensor comprises a laser radar, an ultra-wideband positioning unit and an ultrasonic sensor;
the observation positioning module 1002 is further configured to acquire corresponding laser radar positioning coordinates, ultra-wideband positioning coordinates, and ultrasonic positioning coordinates through the laser radar, the ultra-wideband positioning unit, and the ultrasonic sensor, respectively;
the observation positioning module 1002 is further configured to fuse the laser radar positioning coordinate, the ultra-wideband positioning coordinate, and the ultrasonic positioning coordinate through a kalman fusion algorithm to obtain observation positioning data.
The process positioning module 1001 is further configured to acquire first positioning data through a first type of sensor of the unmanned forklift, and respectively acquire signal frequencies of the first type of sensor;
the process positioning module 1001 is further configured to perform high-pass filtering processing on the first positioning data according to the signal frequency, so as to obtain first filtered data;
the process positioning module 1001 is further configured to fuse the first filtering data to obtain process positioning data;
the observation positioning module 1002 is further configured to acquire second positioning data through a second type of sensor of the unmanned forklift, and respectively acquire signal frequencies of the second type of sensor;
the observation positioning module 1002 is further configured to perform high-pass filtering processing on the second positioning data according to the signal frequencies, so as to obtain second filtering data;
the observation positioning module 1002 is further configured to fuse the second filtering data to obtain observation positioning data.
The observation positioning module 1002 is further configured to obtain observation angle data through a laser radar;
the hybrid positioning module 1003 is further configured to fuse the observation angle data, the process positioning data and the observation positioning data by using an extended kalman filter to obtain hybrid positioning data, and determine a fusion position and a fusion angle of the unmanned forklift based on the hybrid positioning data.
Other embodiments or specific implementation manners of the hybrid positioning system of the unmanned forklift can refer to the above method embodiments, and are not described herein again.
It should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or system. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other like elements in a process, method, article, or system that comprises the element.
The above-mentioned serial numbers of the embodiments of the present invention are merely for description and do not represent the merits of the embodiments.
Through the above description of the embodiments, those skilled in the art will clearly understand that the method of the above embodiments can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware, but in many cases, the former is a better implementation manner. Based on such understanding, the technical solutions of the present invention may be embodied in the form of a software product, which is stored in a storage medium (e.g., a rom/ram, a magnetic disk, an optical disk) and includes instructions for enabling a terminal device (e.g., a mobile phone, a computer, a server, an air conditioner, or a network device) to execute the method according to the embodiments of the present invention.
The above description is only a preferred embodiment of the present invention, and not intended to limit the scope of the present invention, and all modifications of equivalent structures and equivalent processes, which are made by using the contents of the present specification and the accompanying drawings, or directly or indirectly applied to other related technical fields, are included in the scope of the present invention.
Claims (10)
1. A hybrid positioning method for an unmanned forklift is characterized by comprising the following steps:
acquiring first positioning data through a first type of sensor of the unmanned forklift, and fusing the first positioning data to acquire process positioning data;
acquiring second positioning data through a second type sensor of the unmanned forklift, and fusing the second positioning data to obtain observation positioning data;
and fusing the process positioning data and the observation positioning data through an extended Kalman filter to obtain mixed positioning data.
2. The method of claim 1, wherein the first type of sensor comprises an inertial navigation unit and a wheel encoder;
correspondingly, the step of obtaining first positioning data through the first type of sensor of unmanned forklift, and fusing the first positioning data to obtain process positioning data specifically includes:
acquiring angle change information and displacement change information through the wheel type encoder, and acquiring angular velocity information through the inertial navigation unit;
and fusing the angle change information, the displacement change information and the angular speed information through a Kalman fusion algorithm to obtain process positioning data.
3. The method of claim 1, wherein the second type of sensor comprises a lidar, an ultra-wideband positioning unit, and an ultrasonic sensor;
correspondingly, second type sensor through unmanned fork truck acquires the second location data to fuse the second location data obtains the step of observing the location data, specifically includes:
acquiring corresponding laser radar positioning coordinates, ultra-wideband positioning coordinates and ultrasonic positioning coordinates through the laser radar, the ultra-wideband positioning unit and the ultrasonic sensor respectively;
and fusing the laser radar positioning coordinate, the ultra-wideband positioning coordinate and the ultrasonic positioning coordinate through a Kalman fusion algorithm to obtain observation positioning data.
4. The method of claim 1, wherein the step of obtaining first positioning data by a first type of sensor of the unmanned forklift and fusing the first positioning data to obtain process positioning data comprises:
acquiring first positioning data through a first type of sensor of an unmanned forklift, and respectively acquiring signal frequencies of the first type of sensor;
respectively carrying out high-pass filtering processing on the first positioning data according to the signal frequency to obtain first filtering data;
fusing the first filtering data to obtain process positioning data;
correspondingly, second type sensor through unmanned fork truck acquires the second location data to fuse the second location data obtains the step of observing the location data, specifically includes:
acquiring second positioning data through a second type sensor of the unmanned forklift, and respectively acquiring signal frequencies of the second type sensor;
respectively carrying out high-pass filtering processing on the second positioning data according to the signal frequency to obtain second filtering data;
and fusing the second filtering data to obtain observation positioning data.
5. The method according to claim 1, wherein the step of fusing the process positioning data and the observation positioning data by an extended kalman filter to obtain the hybrid positioning data comprises:
acquiring observation angle data through a laser radar;
and fusing the observation angle data, the process positioning data and the observation positioning data by using an extended Kalman filter to obtain mixed positioning data, and determining the fused position and the fused angle of the unmanned forklift based on the mixed positioning data.
6. An unmanned forklift hybrid location system, the system comprising:
the process positioning module is used for acquiring first positioning data through a first type of sensor of the unmanned forklift, and fusing the first positioning data to acquire process positioning data;
the observation positioning module is used for acquiring second positioning data through a second type sensor of the unmanned forklift, and fusing the second positioning data to acquire observation positioning data;
and the hybrid positioning module is used for fusing the process positioning data and the observation positioning data through an extended Kalman filter to obtain hybrid positioning data.
7. The system of claim 6, wherein the first type of sensor comprises an inertial navigation unit and a wheel encoder;
the process positioning module is also used for acquiring angle change information and displacement change information through the wheel type encoder and acquiring angular velocity information through the inertial navigation unit;
the process positioning module is further configured to fuse the angle change information, the displacement change information, and the angular velocity information through a kalman fusion algorithm to obtain process positioning data.
8. The system of claim 6, wherein the second type of sensor comprises a lidar, an ultra-wideband positioning unit, and an ultrasonic sensor;
the observation positioning module is also used for acquiring corresponding laser radar positioning coordinates, ultra-wideband positioning coordinates and ultrasonic positioning coordinates through the laser radar, the ultra-wideband positioning unit and the ultrasonic sensor respectively;
the observation positioning module is further used for fusing the laser radar positioning coordinate, the ultra-wideband positioning coordinate and the ultrasonic positioning coordinate through a Kalman fusion algorithm to obtain observation positioning data.
9. The system of claim 6, wherein the process positioning module is further configured to obtain first positioning data by a first type of sensor of the unmanned forklift and to obtain signal frequencies of the first type of sensor respectively;
the process positioning module is further configured to perform high-pass filtering processing on the first positioning data according to the signal frequency to obtain first filtered data;
the process positioning module is further configured to fuse the first filtering data to obtain process positioning data;
the observation positioning module is also used for acquiring second positioning data through a second type of sensor of the unmanned forklift and respectively acquiring the signal frequency of the second type of sensor;
the observation positioning module is further configured to perform high-pass filtering processing on the second positioning data according to the signal frequencies to obtain second filtering data;
and the observation positioning module is also used for fusing the second filtering data to obtain observation positioning data.
10. The system of claim 6, wherein the observation location module is further configured to obtain observation angle data via a lidar;
the hybrid positioning module is further used for fusing the observation angle data, the process positioning data and the observation positioning data by using an extended Kalman filter to obtain hybrid positioning data, and determining the fusion position and the fusion angle of the unmanned forklift based on the hybrid positioning data.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011070254.2A CN112179332A (en) | 2020-09-30 | 2020-09-30 | Hybrid positioning method and system for unmanned forklift |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011070254.2A CN112179332A (en) | 2020-09-30 | 2020-09-30 | Hybrid positioning method and system for unmanned forklift |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112179332A true CN112179332A (en) | 2021-01-05 |
Family
ID=73949198
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011070254.2A Pending CN112179332A (en) | 2020-09-30 | 2020-09-30 | Hybrid positioning method and system for unmanned forklift |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112179332A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112862161A (en) * | 2021-01-18 | 2021-05-28 | 上海燕汐软件信息科技有限公司 | Goods sorting management method and device, electronic equipment and storage medium |
| CN114720663A (en) * | 2022-04-25 | 2022-07-08 | 国网陕西省电力有限公司电力科学研究院 | Laser, photosensitive and ultrasonic three-phase fusion survey pin device and method |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102121827A (en) * | 2010-11-29 | 2011-07-13 | 浙江亚特电器有限公司 | Positioning system of mobile robot and positioning method thereof |
| KR20140144047A (en) * | 2013-06-10 | 2014-12-18 | 한국건설기술연구원 | System and method for estimating traffic characteristics information in the road network on a real-time basis |
| CN104317289A (en) * | 2014-10-23 | 2015-01-28 | 苏州工业园区艾吉威自动化设备有限公司 | Novel forklift type AGV (automatic guided vehicle) trolley |
| CN107024934A (en) * | 2017-04-21 | 2017-08-08 | 山东大学 | A kind of hospital service robot and method based on cloud platform |
| CN107289933A (en) * | 2017-06-28 | 2017-10-24 | 东南大学 | Double card Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions |
| CN108733055A (en) * | 2018-05-18 | 2018-11-02 | 郑州万达科技发展有限公司 | A kind of method and AGV navigation positional devices of Fusion |
| CN109189074A (en) * | 2018-09-28 | 2019-01-11 | 华南农业大学 | A kind of interior for storage environment is autonomous to build drawing method |
| CN109665464A (en) * | 2018-12-24 | 2019-04-23 | 宁波如意股份有限公司 | A kind of method and system that movable type fork truck automatically tracks |
| WO2019239365A1 (en) * | 2018-06-13 | 2019-12-19 | Purohit Ankit | System and method for position and orientation tracking of multiple mobile devices |
| CN110647089A (en) * | 2019-10-28 | 2020-01-03 | 天津中德应用技术大学 | Intelligent warehouse logistics robot control system and control method |
-
2020
- 2020-09-30 CN CN202011070254.2A patent/CN112179332A/en active Pending
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102121827A (en) * | 2010-11-29 | 2011-07-13 | 浙江亚特电器有限公司 | Positioning system of mobile robot and positioning method thereof |
| KR20140144047A (en) * | 2013-06-10 | 2014-12-18 | 한국건설기술연구원 | System and method for estimating traffic characteristics information in the road network on a real-time basis |
| CN104317289A (en) * | 2014-10-23 | 2015-01-28 | 苏州工业园区艾吉威自动化设备有限公司 | Novel forklift type AGV (automatic guided vehicle) trolley |
| CN107024934A (en) * | 2017-04-21 | 2017-08-08 | 山东大学 | A kind of hospital service robot and method based on cloud platform |
| CN107289933A (en) * | 2017-06-28 | 2017-10-24 | 东南大学 | Double card Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions |
| CN108733055A (en) * | 2018-05-18 | 2018-11-02 | 郑州万达科技发展有限公司 | A kind of method and AGV navigation positional devices of Fusion |
| WO2019239365A1 (en) * | 2018-06-13 | 2019-12-19 | Purohit Ankit | System and method for position and orientation tracking of multiple mobile devices |
| CN109189074A (en) * | 2018-09-28 | 2019-01-11 | 华南农业大学 | A kind of interior for storage environment is autonomous to build drawing method |
| CN109665464A (en) * | 2018-12-24 | 2019-04-23 | 宁波如意股份有限公司 | A kind of method and system that movable type fork truck automatically tracks |
| CN110647089A (en) * | 2019-10-28 | 2020-01-03 | 天津中德应用技术大学 | Intelligent warehouse logistics robot control system and control method |
Non-Patent Citations (2)
| Title |
|---|
| 苏中等: "《仿生蛇形机器人技术》", 31 December 2015, 国防工业出版社 * |
| 达尔达里等: "《卫星及陆基无线电定位技术》", 30 June 2015, 国防工业出版社 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112862161A (en) * | 2021-01-18 | 2021-05-28 | 上海燕汐软件信息科技有限公司 | Goods sorting management method and device, electronic equipment and storage medium |
| CN114720663A (en) * | 2022-04-25 | 2022-07-08 | 国网陕西省电力有限公司电力科学研究院 | Laser, photosensitive and ultrasonic three-phase fusion survey pin device and method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5348514B2 (en) | Method and apparatus for combining 3D position and 2D intensity mapping for localization | |
| CA2854756C (en) | Method and apparatus for providing accurate localization for an industrial vehicle | |
| KR101622028B1 (en) | Apparatus and Method for controlling Vehicle using Vehicle Communication | |
| CN112179332A (en) | Hybrid positioning method and system for unmanned forklift | |
| CN106463059A (en) | Obstacle-information-managing device | |
| CN110470333B (en) | Calibration method and device of sensor parameters, storage medium and electronic device | |
| WO2014190050A2 (en) | Method and system for obstacle detection for vehicles using planar sensor data | |
| CN113168179A (en) | Detecting a location of an autonomous device | |
| WO2018154579A1 (en) | Method of navigating an unmanned vehicle and system thereof | |
| US10777084B1 (en) | Vehicle location identification | |
| US11035679B2 (en) | Localization technique | |
| CN111516777A (en) | Robot trolley and obstacle identification method thereof | |
| CN109313850A (en) | Method, apparatus and system for driver's identification of driving in the wrong direction | |
| CN110703770A (en) | Method and device for controlling automatic running of track inspection vehicle | |
| CN111595328B (en) | Real obstacle map construction and navigation method and system based on depth camera | |
| CN109313851A (en) | Method, apparatus and system for driver's identification of driving in the wrong direction | |
| CN113544465A (en) | Method, device and system for navigating autonomous vehicle | |
| CN110963445A (en) | Movable forklift | |
| US11827244B2 (en) | Enhanced vehicle operation | |
| CN114047514A (en) | Unmanned ship navigation method based on millimeter wave radar | |
| CN114136309A (en) | Positioning method, system and device of unmanned logistics vehicle and storage medium | |
| CN114562994A (en) | Positioning method of mobile robot in dynamic environment | |
| Nielsen et al. | Control of an autonomous vehicle for registration of weed and crop in precision agriculture | |
| CN111731200A (en) | Portable device data calibration | |
| CN113466850A (en) | Environment sensing method and device and mobile tool |
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 |
Application publication date: 20210105 |
|
| RJ01 | Rejection of invention patent application after publication |