Multi-source fusion positioning method and system for autonomous operation stacking machine of wharf

Abstract

The invention relates to a multi-source fusion positioning method and system for a wharf autonomous operation stacking machine. The method comprises the following steps: acquiring the current position of the stacking machine, and judging whether the current position is in a transition state outside a box area or an operation state inside the box area: if the current position is in the transition state outside the box area, acquiring the current satellite signal intensity, and judging whether the current satellite signal intensity is higher than a preset signal intensity threshold value, and if so, entering a real-time dynamic global satellite positioning mode, acquiring first positioning information, otherwise, entering an inertial measurement unit and laser radar fusion positioning mode, and acquiring second positioning information; if the operation state is in the box area, entering a visual sensor and magnetic induction sensor fusion positioning mode, and obtaining third positioning information; and after the positioning information is obtained, returning to re-judge the position state of the stacking machine. Compared with the prior art, the method has the advantages that the positioning precision of the stacking machine in safe operation in a complex scene is improved, and the like.

Description

Technical Field

[0001] The present invention relates to the field of engineering machinery automation and positioning technology, and in particular to a multi-source fusion positioning method and system for an autonomous dock forklift. Background Art

[0002] Empty container stackers (referred to as stackers) are key equipment in container transportation and are widely used in the stacking and transshipment of empty containers in ports, docks, and yards. Stackers have the advantages of simple structure, high stacking layers, fast stacking and handling speed, high operating efficiency, flexible control, and good micro-mobility. At present, with the rapid advancement of the construction of smart ports and unmanned terminals around the world, the intelligence of stackers has become an important part of realizing terminal automation for the loading and unloading scenarios of container yards.

[0003] Among them, precise positioning of the forklift's autonomous operation is an indispensable part of realizing intelligence. The forklift operation scene is usually a large and complex environment with a large number of obstacles and dynamically changing conditions (such as moving containers and vehicles). Traditional single positioning technology may encounter problems such as signal blocking and multipath interference in such an environment, resulting in insufficient positioning accuracy. In open areas such as docks, GNSS RTK can provide better positioning services, but in densely stacked areas or next to tall structures, GPS signals are easily blocked and reflected, resulting in increased positioning errors. Camera-based visual positioning works better under good lighting conditions, but the performance will drop significantly at night, in bad weather, or when the lighting conditions change greatly. LiDAR can provide high-precision environmental perception and positioning, but its cost is high and its performance will be affected in rainy and snowy weather. Therefore, how to improve the positioning accuracy of forklifts in complex scenes has become a problem that needs to be solved in this field. Summary of the invention

[0004] The purpose of the present invention is to overcome the defect of poor positioning accuracy of forklifts operating autonomously in complex scenarios in the above-mentioned prior art and to provide a multi-source fusion positioning method and system for forklifts operating autonomously at a terminal.

[0005] The purpose of the present invention can be achieved by the following technical solutions:

[0006] According to a first aspect of the present invention, a multi-source fusion positioning method for an autonomous terminal forklift is provided, comprising the following steps: S1, obtaining the current position of the forklift, and determining whether the current position is in a transfer state outside a container area or an operating state inside the container area, if it is in a transfer state outside the container area, executing S2, and if it is in an operating state inside the container area, executing S5; S2, obtaining the current satellite signal strength, and determining whether it is higher than a preset signal strength threshold, if so, executing S3, otherwise executing S4; S3, entering a real-time dynamic global satellite positioning mode, obtaining first positioning information, and returning to S1; S4, entering an inertial measurement unit and laser radar fusion positioning mode, obtaining second positioning information, and returning to S1; S5, entering a visual sensor and magnetic induction sensor fusion positioning mode, obtaining third positioning information, and returning to S1.

[0007] As a preferred technical solution, judging whether it is higher than a preset signal strength threshold specifically includes: when the number of received satellites is less than the preset satellite number threshold for 5 consecutive frames, judging that the current positioning antenna signal is interfered with and the satellite signal strength is lower than the preset signal strength threshold; when the number of received satellites is greater than or equal to the preset satellite number threshold for 5 consecutive frames, judging that the current positioning antenna signal is stable and the satellite signal strength is higher than the preset signal strength threshold.

[0008] As a preferred technical solution, the real-time dynamic global satellite positioning module includes an antenna and a host, and S3 specifically includes: obtaining the longitude and latitude coordinates of the antenna center in the first coordinate system, obtaining the longitude and latitude coordinates of the vehicle center in the first coordinate system by calculating the relationship between the vehicle center of the forklift and the antenna center position, and converting them to the longitude and latitude coordinates in the second coordinate system; comparing the longitude and latitude coordinates of the vehicle center in the second coordinate system with the pre-acquired port prior map to determine the position of the forklift and obtain the first positioning information.

[0009] As an optimal technical solution, the S4 specifically includes: S41, acquiring 3D point cloud data and establishing a map, and at the same time collecting the acceleration and angular velocity data of the forklift; S42, taking the forklift positioning information at the previous moment when the number of satellites is missing as the initial posture, and obtaining the forklift continuous motion model based on the first frame of the inertial measurement unit as the global coordinate system; S43, de-motion distorting the original point cloud; S44, searching for n nearest neighbor points for each point on the constructed local map, and constructing an observation model according to the distance from the point to the plane; S45, based on the forklift continuous motion model and the observation model, tightly coupling the data collected by the inertial measurement unit and the data obtained by the laser radar, and using the absolute position information provided by the laser radar to correct the positioning drift of the inertial measurement unit to obtain the second positioning information; S46, converting the longitude and latitude coordinates in the first coordinate system to the Cartesian coordinate system, and using the second coordinate system as the Cartesian coordinate system for the global positioning of the forklift.

[0010] As a preferred technical solution, the first coordinate system is the WGS84 coordinate system, and the second coordinate system is the UTM coordinate system.

[0011] As a preferred technical solution, the first frame of the inertial measurement unit is recorded as I, the global coordinate system is recorded as G, and the external parameter matrix of the laser radar and the inertial measurement unit is recorded as I T L =( I R L , I P L ), the forklift continuous motion model is expressed as:

[0012]

[0013] In the formula, R I is the attitude of the inertial measurement unit in the global coordinate system, p I is the position of the inertial measurement unit in the global coordinate system, a m is the measured acceleration value of the inertial measurement unit, ω M is the angular velocity value measured by the inertial measurement unit, b a , b ω are the random walk noise n ba and n bω After modeling, the angular velocity and acceleration bias of the inertial measurement unit, n ω and n a are the measurement noise of angular velocity and acceleration, respectively, v I is the velocity in the global coordinate system, v i is the acceleration, g is the acceleration due to gravity, R L is the rotation extrinsic parameter matrix from radar to inertial measurement unit, p L is the translation extrinsic parameter matrix from radar to inertial measurement unit.

[0014] As a preferred technical solution, S5 specifically includes: using a visual sensor to obtain the bay line information of the container area to guide the forklift into the designated working area; obtaining the position information of the pre-laid magnetic nails in real time, and fusing it with the bay line information to determine the working position of the forklift and obtain the third positioning information.

[0015] As a preferred technical solution, a visual sensor is used to obtain the bay line information of the container area. The specific process includes: continuously obtaining images in front of the forklift; based on the image, using an algorithm based on edge detection and shape recognition to identify the parallel bay lines marked on the ground; obtaining the position and angle data of the forklift, and comparing and correcting it with the identified bay line information.

[0016] As a preferred technical solution, the position information of the pre-laid magnetic nails is obtained in real time and fused with the Bay Line information to determine the working position of the forklift and obtain the third positioning information. The specific process includes: obtaining the heading angle data of the forklift; based on the magnetic nail position information and the Bay Line information, using a data fusion algorithm to perform error compensation on the current position and heading angle of the forklift to obtain the compensated working position of the forklift.

[0017] According to a second aspect of the present invention, a multi-source fusion positioning system for an autonomous terminal forklift is provided, the positioning system being used to implement the method described, the positioning system comprising a communicatively connected processor, a real-time dynamic global satellite positioning module, an inertial measurement unit, a laser radar, a visual sensor and a magnetic induction sensor, the processor being used to process and fuse the acquired multi-source data, and switch the corresponding forklift positioning mode according to different working conditions, the real-time dynamic global satellite positioning module being used to run the real-time dynamic global satellite positioning mode, the inertial measurement unit and the laser radar being used to run the inertial measurement unit and laser radar fusion positioning mode, the visual sensor and the magnetic induction sensor being used to run the visual sensor and magnetic induction sensor fusion positioning mode.

[0018] Compared with the prior art, the present invention has the following beneficial effects:

[0019] 1. The present invention uses a multi-source fusion positioning algorithm combined with multiple sensor data to adaptively select a positioning method according to the current scene where the stacker is located, and improves the robustness and accuracy of positioning through data fusion technology. It selects the most accurate positioning according to the complex dynamic environment where the stacker is currently located, that is, adaptive multi-source fusion positioning mode switching, which greatly reduces the computing burden of the vehicle computer and ensures the positioning accuracy requirements for the stacker to operate safely in complex scenes;

[0020] 2. When the stacker is in the transfer state outside the container area and the satellite signal strength is higher than the preset signal strength threshold, the present invention enters the real-time dynamic global satellite positioning mode, calculates the longitude and latitude coordinates of the center of the stacker by using the positional relationship between the center of the vehicle and the center of the antenna, and compares them with the pre-acquired port prior map to determine the position of the stacker, and can achieve centimeter-level positioning in an open environment, fully guaranteeing the positioning accuracy requirements of the automatic driving of the empty container stacker outside the container area;

[0021] 3. When the stacker is in the state of transferring outside the container area and the satellite signal strength is lower than the preset signal strength threshold, the present invention enters the inertial measurement unit and laser radar fusion positioning mode, builds a continuous motion model and observation model for the stacker, tightly couples the data collected by the inertial measurement unit and the data obtained by the laser radar, and uses the absolute position information provided by the laser radar to correct the positioning drift of the inertial measurement unit, which can improve the real-time positioning by using the high-frequency data provided by the inertial measurement while ensuring the positioning accuracy;

[0022] 4. When the stacker is in the working state in the box area, the present invention enters the fusion positioning mode of the visual sensor and the magnetic induction sensor, uses the visual sensor to obtain the bay line information of the box area, guides the stacker to enter the designated working area, obtains the position information of the pre-laid magnetic nails in real time and fuses it with the bay line information, accurately locates the working position of the stacker, and can ensure that the positioning accuracy error of the stacker is less than one centimeter when operating;

[0023] 5. In the present invention, the first coordinate system is the WGS84 coordinate system, and the second coordinate system is the UTM coordinate system. Through the conversion between different coordinate systems, the coordinates between the real-time dynamic global satellite positioning module and the radar inertial odometer can be smoothly connected and maintained consistent in time. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 A schematic diagram of the structure of a multi-source fusion positioning system in an embodiment of the present invention;

[0025] Figure 2 is a flow chart of a multi-source fusion positioning method in an embodiment of the present invention;

[0026] Figure 3 It is a flow chart of IMU and laser radar fusion positioning in an embodiment of the present invention;

[0027] Among them: 1. Processor; 2. Real-time dynamic global satellite positioning module; 3. Inertial measurement unit; 4. LiDAR; 5. Visual sensor; 6. Magnetic induction sensor. DETAILED DESCRIPTION

[0028] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0029] Example

[0030] This embodiment provides a multi-source fusion positioning method for an autonomous dock forklift, specifically including:

[0031] Step S1, obtaining the current position of the forklift, and determining whether the current position is in a transfer state outside the container area or an operation state inside the container area. If it is in a transfer state outside the container area, executing step S2; if it is in an operation state inside the container area, executing step S5;

[0032] Step S2, obtaining the current satellite signal strength and determining whether it is higher than a preset signal strength threshold, if yes, executing step S3, otherwise executing step S4;

[0033] Step S3, entering the real-time dynamic global satellite positioning mode, obtaining the first positioning information, and returning to step S1;

[0034] Step S4, entering the inertial measurement unit and laser radar fusion positioning mode, obtaining second positioning information, and returning to step S1;

[0035] Step S5, entering the visual sensor and magnetic induction sensor fusion positioning mode, obtaining third positioning information, and returning to step S1.

[0036] This method is applied to Figure 1 In the positioning system shown, the positioning system includes a processor 1, a real-time dynamic global satellite positioning module 2, an inertial measurement unit 3, a laser radar 4, a visual sensor 5 and a magnetic induction sensor 6 that are communicatively connected. Among them, the processor 1 is used to process and fuse the acquired multi-source data, and switch the corresponding forklift positioning mode according to different working conditions, that is, to realize adaptive positioning mode switching; the real-time dynamic global satellite positioning module 2 is a GNSS RTK (i.e., a real-time dynamic global satellite positioning system), which is used to run the real-time dynamic global satellite positioning mode to realize automatic driving positioning outside the container area; the inertial measurement unit 3 is an inertial sensor (Inertial Measurement Unit, IMU), and the laser radar 4 and IMU are used to run the inertial measurement unit and laser radar fusion positioning mode. Under harsh conditions such as weak satellite signals, the positioning of the local area is guaranteed by fusing the laser radar and IMU data; the visual sensor 5 and the magnetic induction sensor 6 are used to run the visual sensor and magnetic induction sensor fusion positioning mode. The visual sensor 5 identifies the bay line information and guides the information fusion with the magnetic induction sensor 6 to complete the operation positioning in the container area. Specifically:

[0037] Processor 1 is an industrial computer on the forklift, which is used to process sensor data, fuse multi-sensor data, and autonomously decide the forklift positioning mode according to different working conditions. The industrial computer is set inside the electric control cabinet, where the electric control cabinet is set on the right side of the forklift body, and processor 1 is powered by the forklift.

[0038] GNSS RTK is a real-time dynamic global satellite positioning system, which consists of two GNSS antennas and a host. The host is set at the tail of the forklift and below the cockpit, and the GNSS antennas are set on the left and right sides of the forklift roof to obtain the longitude and latitude and other location information of the forklift. The top of the cockpit can achieve centimeter-level positioning in an open environment. However, in the narrow environment of multi-layer stacking of containers and the obstruction of large equipment such as quay cranes and tower cranes, the satellite signal will be lost and the positioning accuracy will be affected.

[0039] IMU is composed of accelerometer, gyroscope and magnetometer, and is a six-axis IMU. It is installed at the tail of the forklift and below the cockpit to measure the attitude of the forklift in real time, provide high-frequency, high-precision attitude, acceleration and angular velocity data, and make up for the low frequency of GNSS RTK attitude data. However, due to the problem of error accumulation, long-term use will cause positioning drift. LiDAR 4 is a 32-line solid-state LiDAR fixed on both sides of the rear of the vehicle, which is used to obtain three-dimensional point cloud data of the environment and complete simultaneous positioning and mapping (SLAM). The installation direction of LiDAR 4 and IMU is that the positive direction of the sensor's x-axis points to the front of the vehicle, the positive direction of the y-axis points to the left side of the vehicle body, and the positive direction of the z-axis points upward; the positive direction of the y-axis of the GNSS RTK host points to the front of the vehicle, the positive direction of the x-axis points to the right side of the vehicle body, and the positive direction of the z-axis points upward.

[0040] The visual sensor 5 is a monocular camera, which is set above the left headlight, and is used to collect the image information of the painted bay lines in the container area, and complete the recognition of the bay lines in the container area through edge detection algorithm and color recognition algorithm, so as to select two parallel bay lines perpendicular to the container to be operated as a guide, so that the stacker can accurately locate the bay to be operated after receiving the operation task. The magnetic induction sensor 6 is used to identify the magnetic nails laid in advance in front of the container position, and is set at the bottom of the stacker to achieve the posture adjustment and precise parking of the stacker before the operation; in order to reduce costs, in this embodiment, only the magnetic nails are laid in front of the container position.

[0041] like Figure 2 As shown, based on this system, the specific execution process of the above method is as follows:

[0042] Step S1, obtain the current position of the forklift, and determine whether the current position is in a transfer state outside the container area or an operating state inside the container area. If it is in a transfer state outside the container area, execute step S2; if it is in an operating state inside the container area, execute step S5.

[0043] Step S2, obtaining the current GNSS RTK received satellite signal strength, and determining whether it is higher than a preset signal strength threshold, if yes, executing step S3, otherwise executing step S4.

[0044] The determination of whether the signal strength is higher than a preset signal strength threshold specifically includes:

[0045] The satellite signal is weak and the number of received satellites is insufficient: when the number of received satellites is less than the preset satellite number threshold for 5 consecutive frames, it is determined that the current positioning antenna signal is interfered and the satellite signal strength is lower than the preset signal strength threshold;

[0046] The satellite signal is good and meets the positioning requirements: when the number of received satellites is greater than or equal to the preset satellite number threshold for 5 consecutive frames, it is determined that the current positioning antenna signal is stable and the satellite signal strength is higher than the preset signal strength threshold.

[0047] According to the judgment result of satellite signal strength, switch to GNSS RTK positioning mode or IMU and LiDAR fusion positioning mode. When switching, the longitude and latitude coordinates in the WGS84 coordinate system (i.e. the first coordinate system) need to be converted to the Cartesian coordinate system, and the UTM coordinate system (i.e. the second coordinate system) is used as the global positioning Cartesian coordinate system of the forklift. The full name of the UTM coordinate system is the Universal Transverse Mercator projection (UTM) coordinate system, thereby ensuring the smooth connection of the coordinates between the GNSS RTK and the radar inertial odometer:

[0048] The coordinate value of the last frame of stable GPS data in the world coordinate system is (x first y first z first ), after the positioning signal returns to a stable state, the output result of the last frame of the radar inertial odometer is recorded as (x lio y lio z lio ), as the positioning data before the signal stabilizes, its coordinate value in the world coordinate system can be obtained as:

[0049]

[0050] In the formula, (x first ,y first ,z first ) is the coordinate value of the forklift in the world coordinate system when the positioning system is started, ω, θ and γ are the roll angle, pitch angle and yaw angle in the corresponding GNSS RTK data, respectively. b x lio , b y lio , b z lio ) is the coordinate value in the IMU coordinate system.

[0051] When the positioning antenna signal is restored, the value of the first frame of GPS data in the world coordinate system after the signal is restored is (x latest ,ylatest ,z latest ), at this time, (x last ,y last ,z last )、( W x lio , W y lio , W z lio ) and (x latest ,y latest ,z latest ) are in the same coordinate system, completing a smooth transition between the radar inertial odometer and GPS data, and maintaining consistency in time.

[0052] Step S3, entering the GNSS RTK positioning mode, obtaining first positioning information, and returning to step S1.

[0053] This step is to ensure the positioning accuracy requirements of the empty container handler for automatic driving outside the container area, including:

[0054] Step S31, obtaining the longitude and latitude coordinates of the centers of the two antennas in the WGS84 coordinate system through GNSS,

[0055] Step S32, by calculating the relationship between the vehicle center of the forklift and the center positions of the two antennas, the longitude and latitude coordinates of the vehicle center in the WGS84 coordinate system are calculated, and the longitude and latitude coordinates in the WGS84 coordinate system are converted to the longitude and latitude coordinates in the UTM coordinate system through an industrial computer;

[0056] Step S33, comparing the longitude and latitude coordinates of the vehicle center in the UTM coordinate system with the pre-acquired high-precision priori map of the port to determine the position of the forklift and obtain first positioning information.

[0057] In addition, it should be noted that in step S1, the GNSS RTK is also used as the basis for judging whether the current empty container stacker is in the transfer state outside the container area or the operating state inside the container area. Specifically, the RTK positioning is compared with the high-precision prior map of the port. If the stacker is located in the container area, it switches to the visual sensor and magnetic induction sensor fusion positioning mode; if the stacker is located outside the container area, it switches to the GNSS RTK positioning mode. That is, when it is in the container area, the RTK positioning information will also be recorded in real time, and continuous polling will be performed to detect whether it has left the container area.

[0058] Step S4, enter the IMU and laser radar fusion positioning mode, use the radar inertial odometer to obtain the second positioning information, and return to step S1.

[0059] like Figure 3 As shown, the specific execution process of this step is as follows:

[0060] Step S41, using laser radar to obtain 3D point cloud data and build a map, and at the same time, using IMU to collect acceleration and angular velocity data of the forklift;

[0061] Step S42, the forklift positioning information before the satellite number is missing is used as the initial posture, the first frame based on the IMU (denoted as I) is used as the global coordinate system (denoted as G), and the external parameter matrix of the laser radar and IMU is recorded as I T L =( I R L , I P L ), the continuous motion model of the forklift is established and expressed as:

[0062]

[0063] In the formula, R I is the posture of IMU in the global coordinate system, p I is the position of IMU in the global coordinate system, a m is the measured acceleration value of IMU, ω M is the angular velocity value measured by IMU, b a , b ω is the random walk noise n ba and n bω Modeling is performed, which are the angular velocity and acceleration bias of IMU, n ω and n a are the measurement noise of angular velocity and acceleration, respectively, v I is the velocity in the global coordinate system, v i is the acceleration, g is the acceleration due to gravity, R L is the rotation extrinsic parameter matrix from radar to IMU, p L is the translation extrinsic parameter matrix from radar to IMU.

[0064] Step S43, de-motion distortion is performed on the original point cloud to obtain L p j , j is the index of the point cloud in the radar coordinate system L;

[0065] Step S44, searching for n nearest neighbor points for each point on the constructed local map, and constructing an observation model according to the distance from the point to the plane;

[0066] Step S45, based on the continuous motion model and observation model of the forklift, the data collected by the IMU and the data obtained by the laser radar are tightly coupled through iterative Kalman filtering, the positioning drift of the IMU is corrected by using the absolute position information provided by the laser radar, and the real-time positioning is improved by using the high-frequency data provided by the IMU, so as to obtain the second positioning information;

[0067] Step S46, converting the longitude and latitude coordinates in the WGS84 coordinate system to the Cartesian coordinate system, using the UTM coordinate system as the Cartesian coordinate system for global positioning of the forklift, and ensuring a smooth connection between the coordinates of the GNSS RTK and the radar inertial odometer.

[0068] Step S5, entering the visual sensor and magnetic induction sensor fusion positioning mode, obtaining third positioning information, and returning to step S1.

[0069] This step uses visual sensors to identify the bay line information of the container area, uses the bay line information of the container area to guide the forklift into the designated operation area, and obtains the position information of the magnetic nails pre-laid between the bays in real time, and accurately locates its operation position through the magnetic nail position information to ensure that the positioning accuracy error is less than one centimeter during operation. In actual application, it is specifically:

[0070] The vision sensor continuously captures images in front of the forklift;

[0071] The captured images are processed using algorithms based on edge detection and shape recognition to identify the parallel bay lines marked on the ground and obtain the precise position of the bay lines;

[0072] Obtain the position and angle data of the forklift, and compare and calibrate it with the identified bay line information to ensure that the forklift always stays between the two parallel lines;

[0073] At the same time, the visual sensor is used to calculate the heading angle data of the forklift so that it remains perpendicular to the direction of the container to be operated.

[0074] In order to further improve the positioning accuracy, the forklift is also equipped with a magnetic induction sensor. The magnetic induction sensor is installed at the bottom of the forklift to detect the magnetic nails on the ground. The magnetic nails are usually arranged on a predetermined path to form an invisible navigation line. When the forklift is driving, the magnetic induction sensor detects the position of the magnetic nails in real time and transmits its data to the industrial computer.

[0075] After receiving the magnetic nail position information, the industrial computer fuses it with the Bayer line information provided by the visual sensor, and uses the data fusion algorithm to compensate for the error of the forklift's current position and heading angle to obtain the compensated forklift's operating position.

[0076] The fusion positioning mode adopted in step S5 combines the advantages of both vision and magnetic induction sensors. Based on the bay line information collected by the vision sensor, the stacker is kept between two parallel lines, with the heading angle perpendicular to the container to be operated. The error compensation of the stacker's operating heading angle is combined with magnetic nail navigation, so that the stacker can achieve a positioning accuracy of 1 cm during operation. Through the advanced image processing algorithm of the vision sensor and the precise navigation of the magnetic induction sensor, the stacker can achieve high-precision fusion positioning in complex container areas.

[0077] In summary, the multi-source fusion positioning method and system for the autonomous dock forklift provided by the present invention can significantly reduce the computing burden of the vehicle computer and ensure the positioning accuracy requirements for the safe operation of the forklift by switching the multi-source fusion positioning mode adaptively.

[0078] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A multi-source fusion positioning method for autonomous dock forklift, characterized in that: The following steps are involved: S1, obtain the current position of the forklift, and determine whether the current position is in the state of transferring outside the container area or in the state of operating inside the container area. If it is in the state of transferring outside the container area, execute S2; if it is in the state of operating inside the container area, execute S5; S2, obtaining the current satellite signal strength and determining whether it is higher than a preset signal strength threshold, if yes, executing S3, otherwise executing S4; S3, enter the real-time dynamic global satellite positioning mode, obtain the first positioning information, and return to S1; S4, enter the inertial measurement unit and laser radar fusion positioning mode, obtain the second positioning information, and return to S1; S5, enter the visual sensor and magnetic induction sensor fusion positioning mode, obtain the third positioning information, and return to S1.

2. The multi-source fusion positioning method for autonomous dock stacker according to claim 1 is characterized in that: Determine whether the signal strength is higher than a preset threshold, specifically including: When the number of received satellites is less than the preset satellite number threshold for 5 consecutive frames, it is determined that the current positioning antenna signal is interfered and the satellite signal strength is lower than the preset signal strength threshold; When the number of received satellites is greater than or equal to the preset satellite number threshold for 5 consecutive frames, it is determined that the current positioning antenna signal is stable and the satellite signal strength is higher than the preset signal strength threshold.

3. The multi-source fusion positioning method for autonomous dock stacker according to claim 1 is characterized in that: The real-time dynamic global satellite positioning module includes an antenna and a host, and S3 specifically includes: Obtain the longitude and latitude coordinates of the antenna center in the first coordinate system, By calculating the relationship between the center of the vehicle of the forklift and the center position of the antenna, the longitude and latitude coordinates of the center of the vehicle in the first coordinate system are obtained, and converted into the longitude and latitude coordinates in the second coordinate system; The latitude and longitude coordinates of the vehicle center in the second coordinate system are compared with the pre-acquired port prior map to determine the position of the forklift and obtain the first positioning information.

4. The multi-source fusion positioning method for autonomous dock stacker according to claim 1 is characterized in that: The S4 specifically includes: S41, acquiring 3D point cloud data and establishing a map, and collecting acceleration and angular velocity data of the forklift; S42, taking the positioning information of the forklift at the moment before the number of satellites is missing as the initial position and posture, and obtaining the continuous motion model of the forklift based on the first frame of the inertial measurement unit as the global coordinate system; S43, removing motion distortion from the original point cloud; S44, searching for n nearest neighbor points for each point on the constructed local map, and constructing an observation model according to the distance from the point to the plane; S45, based on the continuous motion model of the forklift and the observation model, tightly couple the data collected by the inertial measurement unit and the data acquired by the laser radar, and use the absolute position information provided by the laser radar to correct the positioning drift of the inertial measurement unit to obtain second positioning information; S46, converting the longitude and latitude coordinates in the first coordinate system to the Cartesian coordinate system, and using the second coordinate system as the Cartesian coordinate system for global positioning of the forklift.

5. The multi-source fusion positioning method for autonomous dock stacker according to claim 3 or 4, characterized in that: The first coordinate system is the WGS84 coordinate system, and the second coordinate system is the UTM coordinate system.

6. The multi-source fusion positioning method for autonomous dock stacker according to claim 4 is characterized in that: The first frame of the inertial measurement unit is denoted as I, the global coordinate system is denoted as G, and the external parameter matrix of the lidar and inertial measurement unit is denoted as I T L =( I R L , I P L ), the forklift continuous motion model is expressed as: Where R is the attitude of the inertial measurement unit in the global coordinate system, pI is the position of the inertial measurement unit in the global coordinate system, and a m is the measured acceleration value of the inertial measurement unit, ω M is the angular velocity value measured by the inertial measurement unit, b a , b ω are the random walk noise n ba and n bω After modeling, the angular velocity and acceleration bias of the inertial measurement unit, n ω and n a are the measurement noise of angular velocity and acceleration, respectively, v I is the velocity in the global coordinate system, v i is the acceleration, g is the acceleration due to gravity, R L is the rotation extrinsic parameter matrix from radar to inertial measurement unit, p L is the translation extrinsic parameter matrix from radar to inertial measurement unit.

7. The multi-source fusion positioning method for autonomous dock stacker according to claim 1 is characterized in that: The S5 specifically includes: Use visual sensors to obtain the container area bay line information and guide the forklift into the designated operating area; The position information of the pre-laid magnetic nails is obtained in real time and integrated with the Bay Line information to determine the operating position of the forklift and obtain the third positioning information.

8. The multi-source fusion positioning method for autonomous dock stacker according to claim 7 is characterized in that: The visual sensor is used to obtain the bay line information of the box area. The specific process includes: Continuously acquire images in front of the forklift; Based on the image, identifying parallel bay lines marked on the ground using an algorithm based on edge detection and shape recognition; The position and angle data of the forklift are obtained, and compared and corrected with the identified bay line information.

9. The multi-source fusion positioning method for autonomous dock stacker according to claim 8, characterized in that: The position information of the pre-laid magnetic nails is obtained in real time, and is integrated with the Bay Line information to determine the operation position of the forklift and obtain the third positioning information. The specific process includes: Get the heading angle data of the forklift; Based on the magnetic nail position information and the Bay Line information, a data fusion algorithm is used to perform error compensation on the current position and heading angle of the forklift to obtain the compensated forklift operating position.

10. A multi-source fusion positioning system for autonomous dock forklift, characterized in that: The positioning system is used to implement the method described in any one of claims 1 to 9, and the positioning system includes a communicatively connected processor, a real-time dynamic global satellite positioning module, an inertial measurement unit, a laser radar, a visual sensor, and a magnetic induction sensor. The processor is used to process and fuse the acquired multi-source data, and switch the corresponding forklift positioning mode according to different working conditions. The real-time dynamic global satellite positioning module is used to run the real-time dynamic global satellite positioning mode, the inertial measurement unit and the laser radar are used to run the inertial measurement unit and laser radar fusion positioning mode, and the visual sensor and the magnetic induction sensor are used to run the visual sensor and magnetic induction sensor fusion positioning mode.

Images