CN112987061B - Fuzzy fusion positioning method based on GPS and laser radar - Google Patents

Abstract

The invention relates to a fuzzy fusion positioning method based on GPS and lidar. When the GPS signal is poor or no signal, lidar is used to perform feature matching and positioning through a priori maps, and the location of the unmanned ship is predicted based on the acceleration and speed measured by the inertial sensing unit. It is processed with the lidar positioning data, and then the difference between the filtered GPS data and the lidar positioning data and the corresponding measurement data before filtering and the accuracy of the sensor itself are fused through a fuzzy algorithm to obtain The final positioning of man and ship has a simple algorithm, good positioning effect and strong implementability.

Description

A fuzzy fusion positioning method based on GPS and lidar

Technical field

The invention relates to the field of unmanned driving, and in particular to a fuzzy fusion positioning method based on GPS and lidar.

Background technique

With the continuous development of autonomous driving technology, the application of autonomous driving is becoming more and more widespread, and the application of autonomous ships has gradually expanded from the military field to the civilian field. Whether it is a drone, an unmanned vehicle, or an unmanned ship, in order to achieve better use results, accurate positioning is the biggest prerequisite. Traditional GPS positioning can basically meet civilian needs in open environments. However, due to the complexity of the actual environment and the needs of actual tasks, unmanned ships often inevitably need to pass through some places with poor or even non-existent GPS signals, such as bridge openings, Tunnels etc. At this time, relying solely on GPS will cause large positioning errors. Therefore, it is necessary to use tools that can localize. Traditional GPS integrates INS for positioning and navigation. Although it can temporarily replace GPS for positioning when the GPS signal is not good, it is not suitable for long-term independent positioning due to accumulated errors. Currently, lidar is also used as a method for positioning without GPS. , however, due to the instability of the laser radar position on the unmanned ship, which may cause large errors in some data, most methods do not judge and process this or only perform simple filtering, which is difficult to truly eliminate the positioning problems caused by large errors. In addition, traditional GPS-free positioning navigation will directly discard all GPS data within a certain period of time without making full use of it when it is judged that the GPS signal is not good. The GPS and lidar fusion positioning method proposed in this patent makes full use of all data so that the fused positioning can achieve higher accuracy. Therefore, existing positioning methods need to be further improved.

Contents of the invention

The purpose of the present invention is to provide a positioning method based on fuzzy fusion of GPS and lidar to solve the problem of accurate positioning of unmanned ships when the GPS signal is weakened or there is no GPS signal, and can be used when large errors occur in GPS positioning or lidar positioning. It can still ensure the relatively accurate positioning of the unmanned ship.

In order to achieve the above objectives, the technical solution of the present invention is: a fuzzy fusion positioning method based on GPS and lidar, including:

The acceleration of the unmanned ship is obtained in real time through the inertial sensing unit to calculate the current predicted position coordinates (that is, the acceleration of the unmanned ship obtained through the inertial sensing unit is combined with the position of the unmanned ship at the previous moment to estimate the current predicted position coordinates);

Obtain the current global position coordinates of the unmanned ship through GPS and filter the GPS positioning data through the extended Kalman filter method according to the sensor accuracy of the GPS itself to obtain the global positioning of the unmanned ship; if the GPS signal is good, the GPS positioning data will be used as unmanned ship. Human and ship positioning results; the extended Kalman filter method uses the predicted position coordinates as the state prediction value, the displacement difference generated during the period when the system obtains one positioning data as the process noise, and the GPS data as the measured value , the GPS sensor measurement accuracy is used as measurement noise, and the filtering weight between the state prediction value and the measurement value is determined through the relationship between the process noise and the measurement noise;

If the GPS signal is not good, the surrounding environment point cloud information and distance information are obtained through lidar, and the current local position coordinates of the unmanned ship and the current local position coordinates based on the lidar itself are obtained by matching with the built two-dimensional grid map of the surrounding environment. Sensor accuracy uses the extended Kalman filter method to filter the lidar positioning data to obtain the local positioning of the unmanned ship; the extended Kalman filter method is consistent with the method used for GPS data filtering, the difference is that the lidar obtained Positioning data as measurements;

Obtaining local positioning data of lidar: First, establish a two-dimensional raster map based on the point cloud information and distance information of the surrounding environment obtained by scanning the surrounding environment with 360-degree scanning lidar through the cartographer positioning and mapping method; according to the raster map , the lidar scans the point cloud distribution and distance of the surrounding environment to match the raster map and combines the laser odometry information to obtain its local precise positioning in the raster map; corresponding to the starting point of the given raster map The global coordinates and the local positioning coordinates in the raster map are accurately positioned through coordinate conversion;

The fuzzy subset, discourse domain and membership function used for fuzzification of GPS data are determined by understanding the positioning accuracy of the GPS. Since the data error is large when the GPS signal is not good, the fuzzy subset used in fuzzifying the GPS data is a set {N, Z, P} with 3 members, and the value of the universe of discussion is R is the positioning accuracy of the GPS sensor. According to the real-time positioning requirements of unmanned ships, a relatively simple triangular membership function with a small amount of calculation is used, and the specific expression of the triangular membership function is determined based on the fuzzy subset and domain of discussion:

VG is the GPS data innovation value;

If the filtered GPS data information value is not , then it is judged that the GPS data is unavailable, otherwise the innovation value corresponding to the filtered GPS data is brought into the membership function to obtain the membership degree of the GPS data innovation value in each fuzzy value and weighted summation is obtained. The unreliability index after blurring the GPS data is assigned weighted values of 0.25, 0.5, and 0.25 respectively;

By understanding the positioning accuracy of the lidar, the fuzzy subset, discourse domain and membership function used for fuzzification of the lidar positioning data are determined. According to the accuracy of the lidar sensor, a set {NL, NS, Z, PS, PL} with a fuzzy subset member number of 5 is used to fuzzify the lidar positioning data, and the value of the domain of discussion is Rp is the lidar sensor accuracy. A relatively simple triangular membership function with a small amount of calculation is used, and the specific expression of the triangular membership function is determined based on the fuzzy subset and domain of discussion:

VP is the laser radar positioning coordinate information value, and Rp is the laser radar measurement accuracy;

If the filtered lidar data information value is not , it is judged that the lidar positioning data is unavailable, otherwise the filtered lidar positioning data is brought into the membership function to obtain the membership degree of the lidar positioning data in each fuzzy value, and a weighted sum is performed to obtain the fuzzy result The lidar unreliability index is assigned weighted values of 0.1, 0.2, 0.4, 0.2, and 0.1 respectively;

If at least one of the two types of data is available, the weights of the available data are allocated according to the unreliability index, and the sensor data weights are allocated again according to the reliability of the data, that is, the innovation value of the filtered data, to obtain the final weight. Based on this, the available sensor data after filtering are weighted and summed to obtain the final positioning data; if both positioning data are unavailable, the current position coordinates are estimated based on the previous position coordinates and the speed obtained by the inertial sensing unit.

Compared with the existing technology, the present invention has the following beneficial effects:

1. It has a multi-level data processing process. After filtering the GPS data and lidar positioning data respectively, it will further perform fuzzy fusion processing;

2. Make full use of complementary fusion of GPS data and lidar data to avoid positioning failures or large positioning errors caused by individual sensor failures or accidental large deviations in data, and improve the safety of unmanned ship driving;

3. Simple and efficient fusion strategy achieves high positioning accuracy while meeting the real-time requirements of the system.

Description of the drawings

Figure 1 is a flow chart of a fuzzy fusion positioning method based on GPS and lidar provided by the present invention;

Figure 2 is a membership function diagram used to fuzzify the filtered GPS data provided by the present invention;

Figure 3 is a membership function diagram provided by the present invention for fuzzifying the filtered lidar positioning data.

Detailed ways

The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.

It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the present application. Unless otherwise defined, all technical and scientific terms used herein have the same meanings commonly understood by one of ordinary skill in the art to which this application belongs.

It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, the singular forms are also intended to include the plural forms unless the context clearly indicates otherwise. Furthermore, it will be understood that when the terms "comprises" and/or "includes" are used in this specification, they indicate There are features, steps, operations, means, components and/or combinations thereof.

The present invention provides a fuzzy fusion positioning method based on GPS and lidar, which is characterized by including:

The acceleration of the unmanned ship is obtained in real time through the inertial sensing unit to calculate the current predicted position coordinates;

Obtain the current global position coordinates of the unmanned ship through GPS and filter the GPS positioning data through the extended Kalman filter method according to the sensor accuracy of the GPS itself to obtain the global positioning of the unmanned ship; if the GPS signal is good, the GPS positioning data will be used as unmanned ship. Human and ship positioning results; if the GPS signal is not good, the surrounding environment point cloud information and distance information are obtained through lidar, and the current local position coordinates of the unmanned ship are obtained by matching with the built two-dimensional grid map of the surrounding environment. According to the sensor accuracy of the lidar itself, the lidar positioning data is filtered by the extended Kalman filtering method to obtain the local positioning of the unmanned ship;

Determine the membership function based on the sensor accuracy of the GPS and lidar itself and the innovation values of the filtered global positioning data and local positioning data;

According to the membership function, the reliability of the GPS global positioning data and the lidar local positioning data is judged through the fuzzy algorithm, and based on the reliability, it is judged whether the GPS global positioning data and the lidar local positioning data are available; if the GPS global positioning data and the laser If at least one kind of radar local positioning data is available, the available data will be weighted according to the reliability and the respective sensor accuracy of GPS and lidar, and weighted and summed with the corresponding positioning data to obtain the current positioning result; if two types of data If both are unavailable, the predicted position coordinates will be used as the current positioning coordinates.

The following is the specific implementation process of the present invention.

Figure 1 is a flow chart of a fuzzy fusion positioning method based on GPS and lidar provided in this example. As shown in Figure 1, this embodiment provides a fuzzy fusion positioning method based on GPS and lidar, including the following step:

S1. Obtain the predicted positioning of the unmanned ship through the inertial sensing unit. The accelerometer is used to obtain the traveling acceleration of the unmanned ship and perform integral calculation and the position at the previous moment to obtain the predicted positioning of the unmanned ship at the current moment. The inertial sensing unit is integrated into the flight control system, which is installed in the middle of the unmanned ship.

S2. Obtain the global positioning of the unmanned ship through the GPS sensor and perform extended Kalman filtering to obtain the filtered global positioning. The GPS is installed on the front surface of the unmanned ship's hull.

When performing extended Kalman filtering, the predicted positioning is used as the state prediction value, the global positioning is used as the measured value, the position offset estimate within the operating cycle of the positioning system is used as the process noise, and the sensor accuracy of the GPS is used as the measurement noise.

Determine whether the size of the filtered global positioning data innovation value exceeds a given threshold, and if it exceeds the threshold, enable the lidar positioning module.

S3, lidar positioning module, including lidar mapping, lidar positioning, coordinate conversion and extended Kalman filtering.

LiDAR mapping mainly uses the cartographer positioning and mapping method, and the boundaries of the built raster map are relatively clear.

Lidar positioning: Lidar scans the surrounding environment point cloud information and distance information, and performs matching according to the grid map to obtain the local relative positioning of the unmanned ship in the grid map.

According to the local relative positioning and the mapping of the grid map origin in the global positioning, coordinate conversion is performed on the local relative positioning to obtain positioning data based on lidar.

The extended Kalman filter is processed by the extended Kalman filter according to the lidar-based positioning data and the accuracy of the lidar sensor used. The lidar-based positioning data is used as a measurement value, and the accuracy of the lidar sensor is used. As the measurement noise, the filtered lidar positioning data is obtained by calculation.

S4. Fusion of two sets of positioning data, including the determination of fuzzy subsets, discourse domains, membership functions, and fusion rules.

According to the fusion condition that the filtered global positioning data has a large error, it is determined that the fuzzy subset used in the fuzzification of the filtered global positioning data is a set {N, Z, P} with 3 members, and the value of the universe of discussion is R is the positioning accuracy of the GPS sensor. According to the real-time positioning requirements of unmanned ships, a relatively simple triangular membership function with a small amount of calculation is used, and the triangular membership function is determined based on the fuzzy subset and domain of discussion as shown in Figure 2. The specific expression is:

VG is the GPS data innovation value. When VG exceeds the given threshold range , this set of data will not be fused.

According to the accuracy of the lidar sensor, a set {NL, NS, Z, PS, PL} with a fuzzy subset member number of 5 is used to fuzzify the lidar positioning data, and the value of the domain of discussion is Rp is the lidar sensor accuracy. The triangle membership function is determined according to the fuzzy subset and domain of discussion, as shown in Figure 3. The specific expression is:

VP is the laser radar positioning coordinate innovation value, Rp is the laser radar measurement accuracy, when VP exceeds the given threshold range , this set of data will not be fused.

When both sets of data meet the given threshold conditions, the innovation values corresponding to the two sets of filtered data are respectively substituted into the corresponding membership functions to calculate the membership degrees corresponding to the members of each fuzzy subset, and combine them with the corresponding The weighted sum of the weights is used to obtain the credibility of the filtered positioning data, which are gR and pR respectively;

Normalize the two sets of data according to their corresponding credibility values gR and pR, and redistribute the normalized credibility values according to the accuracy of the two sensors, that is Get the final fusion distribution weight.

The final fusion positioning result is calculated according to the fusion allocation weight, that is, x=x1*gR+x2*pR.

If only one set of data meets the given threshold condition, then this set of filtered positioning data will be used as the final result of the current positioning; if both sets of data do not meet the given threshold condition, then the predicted positioning value will be used as the final result of the current positioning.

This embodiment combines GPS and lidar for positioning, so that the system positioning is not affected by the strength of the GPS signal and the sudden failure of the sensor. At the same time, a simple fuzzy decision-making method is used to realize the fusion of GPS data and lidar positioning data, avoiding the complexity due to The time cost brought by decision-making calculations meets the real-time requirements of the system and achieves better integration effects.

Those skilled in the art will understand that embodiments of the present application may be provided as methods, systems, or computer program products. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment that combines software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) having computer-usable program code embodied therein.

The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It will be understood that each process and/or block in the flowchart illustrations and/or block diagrams, and combinations of processes and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing device to produce a machine, such that the instructions executed by the processor of the computer or other programmable data processing device produce a use A device for realizing the functions specified in one process or multiple processes of the flowchart and/or one block or multiple blocks of the block diagram.

These computer program instructions may also be stored in a computer-readable memory that causes a computer or other programmable data processing apparatus to operate in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including the instruction means, the instructions The device implements the functions specified in a process or processes of the flowchart and/or a block or blocks of the block diagram.

These computer program instructions may also be loaded onto a computer or other programmable data processing device, causing a series of operating steps to be performed on the computer or other programmable device to produce computer-implemented processing, thereby executing on the computer or other programmable device. Instructions provide steps for implementing the functions specified in a process or processes of a flowchart diagram and/or a block or blocks of a block diagram.

The above are only preferred embodiments of the present invention, and are not intended to limit the present invention in other forms. Any skilled person familiar with the art may make changes or modifications to equivalent changes using the technical contents disclosed above. Example. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention still fall within the protection scope of the technical solution of the present invention.

Claims (6)

1. A fuzzy fusion positioning method based on GPS and laser radar is characterized by comprising the following steps:

acquiring the acceleration of the unmanned ship in real time through an inertial sensing unit so as to calculate and obtain the current predicted position coordinate;

acquiring the current global position coordinate of the unmanned ship through the GPS, and filtering GPS positioning data according to the sensor precision of the GPS by an extended Kalman filtering method to obtain the global positioning of the unmanned ship; if the GPS signal is good, the GPS positioning data is used as an unmanned ship positioning result; if the GPS signal is not good, acquiring surrounding environment point cloud information and distance information through a laser radar, acquiring the current local position coordinate of the unmanned ship through matching with a built two-dimensional grid map of the surrounding environment, and filtering laser radar positioning data according to the sensor precision of the laser radar by an extended Kalman filtering method to acquire the local positioning of the unmanned ship;

determining membership functions according to the sensor precision of the GPS and the laser radar and the innovation values of the global positioning data and the local positioning data after filtering; the specific determination mode of the membership function is as follows:

when the GPS signal is bad, the fuzzy subset used in the global positioning data fuzzification of the GPS is set { N, Z, P } with the membership number of 3, and the argument is valuedR is the sensor precision of the GPS;

according to the real-time positioning requirement of the unmanned ship, adopting a triangle membership function, and determining the specific expression of the triangle membership function according to the fuzzy subset and the domain as follows:

VG is the GPS data innovation value; if the GPS data innovation value is not in after filteringIf the GPS data is unavailable, otherwise, the innovation value corresponding to the filtered GPS data is brought into the membership function to obtain the membership of the GPS data innovation value in each fuzzy value, and weighted summation is carried out to obtain an unreliable index after the GPS data is fuzzy;

the fuzzy subset member number is set { NL, NS, Z, PS, PL }, and the argument value is equal to that of 5 in the fuzzy of the local positioning data of the laser radar according to the sensor precision of the laser radarRp is the sensor accuracy of the lidar itself; root of Chinese characterAccording to the real-time positioning requirement of the unmanned ship, adopting a triangle membership function, and determining the specific expression of the triangle membership function according to the fuzzy subset and the domain as follows:

VP is a laser radar positioning coordinate innovation value, rp is laser radar measurement precision; if the filtered laser radar data innovation value is not inIf the laser radar positioning data is not available, otherwise, the filtered laser radar positioning data is brought into a membership function to obtain membership of the laser radar positioning data in each fuzzy value, and weighted summation is carried out to obtain a fuzzy laser radar unreliable index;

judging the reliability of GPS global positioning data and laser radar local positioning data through a fuzzy algorithm according to the membership function, and judging whether the GPS global positioning data and the laser radar local positioning data are available or not according to the reliability; if at least one of the GPS global positioning data and the laser radar local positioning data is available, the available data are subjected to weight distribution according to the reliability and the respective sensor precision of the GPS and the laser radar, and the weight distribution and the corresponding positioning data are subjected to weighted summation to obtain a current positioning result; and if both data are not available, taking the predicted position coordinate as the current positioning coordinate.

2. The positioning method based on the fuzzy fusion of the GPS and the laser radar is characterized in that the laser radar is arranged at the middle position of the top of an unmanned ship, a two-dimensional grid map of an environment needing to be subjected to tasks is established by using a laser radar scanning method through a remotely controlled unmanned ship by adopting a cartograph positioning map-establishing method before the unmanned ship is started, and the laser radar obtains the current relative positioning of the unmanned ship on the two-dimensional grid map, namely a local position coordinate by scanning surrounding environment information and matching with the two-dimensional grid map during autonomous positioning navigation; and filtering the local positioning data of the laser radar through an extended Kalman filtering algorithm according to the difference value between the predicted position coordinates and the local position coordinates of the laser radar and the sensor accuracy of the laser radar.

3. The fuzzy fusion positioning method based on the GPS and the laser radar according to claim 1, wherein after the GPS global positioning data or the laser radar local positioning data are judged to be available, corresponding membership values of corresponding positioning data are substituted into the membership functions corresponding to the GPS global positioning data or the laser radar local positioning data to obtain corresponding membership degrees of fuzzy subset members, unreliable indexes of the GPS or the laser radar are respectively obtained through a weighted summation mode, normalization processing is carried out, meanwhile, the unreliable indexes after normalization processing are judged again according to the respective sensor precision of the GPS and the laser radar, and finally, fusion weights of the GPS and the laser radar are obtained, and weighted summation is carried out on the filtered positioning data corresponding to the fusion weights to obtain a final positioning result.

4. A positioning method based on fuzzy fusion of GPS and lidar according to claim 3, wherein the innovation value is the difference between the positioning data obtained by GPS or lidar and the positioning data obtained by filtering, that is, the difference between the positioning data measured by GPS or lidar and the positioning data obtained by filtering, and the larger the difference is, the larger the positioning error of GPS or lidar is, and the lower the reliability is.

5. The method of claim 4, wherein determining whether the data is available for fusion is based on whether the difference between the GPS or lidar filtered data and the predicted data exceeds a set threshold, the thresholds being set to respectivelyAnd->

6. The positioning method based on the fuzzy fusion of the GPS and the laser radar according to claim 1, wherein the GPS positioning data or the laser radar positioning data are processed and fused by using an extended Kalman filtering algorithm and a fuzzy algorithm successively.