Intelligent navigation tracking control method

Abstract

The application discloses an intelligent navigation tracking control method, comprising the following steps: S1, using an adaptive threshold intercepting algorithm to adaptively extract a target route in an original target route image to obtain a target route image; S2, performing optimization processing on the target route image to obtain an optimized route map; S3, performing linear fitting on the optimized route map and an expected route map to obtain a fitting result; S4, establishing a plane coordinate system according to a tracking body; S5, decomposing the position and speed of the tracking body in the forward direction and the lateral offset direction according to the plane coordinate system; S6, determining two groups of control rates according to the angle deviation value and the distance deviation value of the fitting straight line in the fitting result and the straight line in the forward direction respectively; S7, using an improved particle swarm algorithm to iteratively set the two groups of control rates to obtain optimal control rates in the current environment; and S8, performing navigation tracking control according to the optimal control rates.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of tracking control, in particular to an intelligent navigation tracking control method. BACKGROUND

[0002] In the context of the rapid development of production technology in various enterprises and the deepening of intelligent and automated technology, intelligent vehicles and intelligent devices developed based on this platform are playing an increasingly important role in modern social life, logistics transportation and flexible production organization systems.

[0003] However, most of the previous robots use sensors to complete path planning and tracking control. In this regard, the low adaptability of the same sensor on different environmental roads has always been a major defect, often requiring users to modify and debug the sensor module according to the application environment information.

[0004] With the development of computer technology and electronic technology and the emergence of visual navigation using image processing technology, people no longer need to mechanically modify the physical and module to solve this problem, and machine vision is a popular technology. However, when using machine vision to control the route tracking of robots, it has exposed problems such as large and unstable visual image target threshold affected by environmental factors, and the traditional PID parameter setting method in visual tracking is troublesome and difficult. SUMMARY

[0005] The purpose of the present application is to provide an intelligent navigation tracking control method to solve the problems of large and unstable visual image target threshold affected by environmental factors and the traditional PID parameter setting method in visual tracking being troublesome and difficult when controlling the route tracking.

[0006] The technical solution of the present application to solve the above technical problems is as follows:

[0007] The present application provides an intelligent navigation tracking control method, which comprises:

[0008] S1: using an adaptive threshold extraction algorithm to adaptively extract the target route in the original target route image to obtain a target route image;

[0009] S2: optimizing the target route image to obtain an optimized route map;

[0010] S3: linearly fitting the optimized route map and the expected route map to obtain a fitting result;

[0011] S4: establishing a plane coordinate system according to the tracking body;

[0012] S5: decompose the position and velocity of the tracking body in the forward direction and the lateral deviation direction according to the plane coordinate system;

[0013] S6: determine two groups of control rates according to the angle deviation value and the distance deviation value of the fitting straight line in the fitting result and the forward direction straight line, respectively;

[0014] S7: use the improved particle swarm algorithm to iteratively set the two groups of control rates to obtain the optimal control rate in the current environment;

[0015] S8: perform navigation tracking control according to the optimal control rate.

[0016] Optionally, in step S1, the threshold information T of the tracking target in the region in the adaptive threshold extraction algorithm t can be expressed as:

[0017]

[0018] wherein L, A and B represent different population values of the color space of the extraction region, i=1, 2, when i=1, it represents the minimum value, and when i=2, it represents the maximum value, "+" has no numerical summation meaning, and only represents the combination of element arrays, d j represents the threshold disturbance factor and d j the front symbol is "-".

[0019] Optionally, in step S2, the optimization processing of the target route image includes:

[0020] The target route image is subjected to binaryzation processing, filtering and corrosion processing, and random denoising processing.

[0021] Optionally, the step S5 includes:

[0022] x(t) = x(t-Δt) + v x ×Δt

[0023] y(t) = y(t-Δt) + v y ×Δt

[0024] wherein x(t) is the forward direction position information / position coordinate at time t, y(t) is the lateral deviation direction position information / position coordinate at time t, t is the time t, Δt is the sampling time of the visual camera, v x and v y are the vertical decompositions of the velocity vector of the tracking body along the x direction and the y direction, respectively.

[0025] Optionally, in step S6, the two groups of control rates are:

[0026]

[0027]

[0028] wherein, PWM y (t), PWM x (t) correspond to the lateral offset error correction value and the forward direction angle correction value of the controlled object at time t respectively, Kp, Ki, Kd are the parameters of PID, the x, y subscripts of PID correspond to PWM x and PWM y respectively, e y (t) is the lateral offset error at time t, e θ (t) is the angle error between the forward direction and the desired line at time t, and Δt is the sampling time of the vision camera.

[0029] Optionally, the step S7 comprises:

[0030] S71: initializing the search speed and position information of the particle, wherein the information carried by the particle is the PID parameter in the control rate;

[0031] S72: calculating the fitness of the particle according to the accumulated error of the tracking effect;

[0032] S73: judging whether the fitness is better than the individual extreme value, if yes, entering step S74, otherwise, entering step S75;

[0033] S74: updating the individual extreme value and judging whether the fitness is better than the group extreme value, if yes, updating the group extreme value to obtain the optimal control rate under the current environment, otherwise, entering step S75;

[0034] S75: performing local search or global search by using the adaptive inertia weight and compression factor fusion method and automatically updating the search speed of the particle, and returning to step S72.

[0035] Optionally, in the step S72, the function expression of the fitness is:

[0036]

[0037] wherein, is the fitness value of the PID parameter information searched by the i1th particle in the kth iteration, t is time, and the size of the time determines the weight of the error information at this time, represents the error at time t when the i1th particle in the kth iteration is applied to the PID parameter searched by the tracking control, and the absolute value eliminates the influence of the sign.

[0038] Optionally, in the step S75, the expression of the adaptive inertia weight is:

[0039]

[0040] Wk is the inertia weight size of the ith particle in the kth iteration, min Wk is the inertia weight size of the ith particle in the kth iteration, max Wk is the inertia weight size of the ith particle in the kth iteration, Fk is the average fitness of the whole group n particles in the kth iteration and Xk is the position information searched by the ith particle in the kth iteration Fk is the fitness value calculated by substituting into the fitness function, Fk is the minimum fitness value calculated by the whole group in the kth iteration.

[0041] Optionally, in the step S75, the compression factor is:

[0042]

[0043] wherein, C is the compression factor, C represents a learning factor and C=c1+c2, c1 represents an individual learning factor and c2 represents a social learning factor.

[0044] Optionally, in the step S75, the velocity updating formula of the particle is:

[0045]

[0046] wherein, r1 and r2 are random numbers ranging from 0 to 1, Vik is the velocity of the ith particle in the kth iteration, C is the compression factor, Wk is the inertia weight size of the ith particle in the kth iteration, Xik is the position of the ith particle in the kth iteration, gik is the relative optimal position searched by the ith particle in the kth iteration, c1 represents an individual learning factor and c2 represents a social learning factor. k gik is the relative optimal position searched by the ith particle in the kth iteration, c1 represents an individual learning factor and c2 represents a social learning factor.

[0047] The present application has the following beneficial effects:

[0048] 1. The present application can solve the problem of high dependence of the visual system on the environment and sensitivity to light by designing and using the regional adaptive threshold extraction algorithm, thereby providing more accurate target image threshold for visual tracking, and the robustness and adaptability are stronger.

[0049] 2. The improved particle swarm optimization algorithm using adaptive inertia weights and compression factors is used to self-tune PID control according to this invention. The feasibility, high accuracy, and strong robustness of the scheme are verified by a physical simulation platform built through kinematic modeling. Attached Figure Description

[0050] Figure 1 This is a flowchart of the intelligent navigation and tracking control method of the present invention;

[0051] Figure 2 This is a schematic diagram illustrating the truncation effect of the adaptive threshold truncation algorithm of the present invention. Detailed Implementation

[0052] The principles and features of the present invention are described below with reference to the accompanying drawings. The examples given are only for explaining the present invention and are not intended to limit the scope of the present invention.

[0053] This invention provides an intelligent navigation and tracking control method, with reference to... Figure 1 As shown, the intelligent navigation tracking control method includes:

[0054] S1: The target route in the original target route image is adaptively extracted using an adaptive thresholding algorithm to obtain the target route image;

[0055] The main principle of the region adaptive thresholding algorithm is as follows: after the vision module starts running, it extracts a user-specified region from the image captured by the camera, places the region of interest (such as the tracking path) within this region, establishes a threshold tracking object, and then performs information statistics on the pixels in this region in the color space. Let L... i A i B i The numerical value describes the pixel color, where i takes the values: 1—minimum value, 2—maximum value; then the pixel threshold information T can be expressed as:

[0056] T = L1 + L2 + A1 + A2 + B1 + B2

[0057] The "+" sign does not represent numerical summation; it only represents a combination of element arrays.

[0058] After pixel information statistics, the LAB mode information of the color space of the captured area is extracted and denoted as L. m A m B m Let the threshold perturbation factor d be... j Where j takes the following values: 1 – spatial perturbation factor L, 2 – spatial perturbation factor A, 3 – spatial perturbation factor B. Then, the threshold information T for tracking targets in the region...t Can be expressed as:

[0059]

[0060] Wherein, L, A and B represent different modes of the color space of the intercept area, i = 1, 2, when i = 1, it represents the minimum value, when i = 2, it represents the maximum value, "+" has no numerical summation meaning, only represents the combination of element array, d j Threshold disturbance factor and when i is 1 d j The front symbol is "-".

[0061] Through experimental test, in most of the light environment and local light uneven environment, if the value of the disturbance factor d j Is set in the interval [10, 20], there is better effect: at this time L is in [0, 100], A is in [-128, +128], B is in [-128, +128] between random combination, form color map, can get better adaptive threshold extraction effect.

[0062] Again, take the extracted adaptive threshold as the reference, the image is binarized, the intuitive tracking effect can be observed, refer to Figure 2 L m , A m , B m Represent different modes of the color space of the intercept area.

[0063] S2: optimizing the target route image to obtain an optimized route image;

[0064] Here, the optimization of the target route image includes:

[0065] The target route image is binarized, filtered and eroded, and randomly denoised.

[0066] After the image is selected by the target threshold, the image is binarized, however, too many residual noise after binarization will bring greater error influence to the subsequent visual linear regression tracking. For the noise area with large range, the image erosion algorithm is considered to filter the noise. The image erosion algorithm is commonly used in the binarized image, the function is to make the dark area larger and the bright area less, thereby segmenting the image elements and eliminating the noise. For the area with less residual noise, the median filter algorithm is considered, the main principle of the algorithm is to search the field pixel point of the current pixel point and replace the gray value of the current pixel point with the median of the gray value of the field pixel point, and in the binarized image, the main existing noise is the salt and pepper noise, the feature is that the gray value is obviously polarized. At this time, the median is not dependent on the maximum or minimum value of the image gray value, and the filtering error caused by the noise with large difference from the target gray value can be effectively avoided, and the noise in the binarized image can be effectively removed.

[0067] S3: linear fitting is performed on the optimized route map and the expected route map to obtain a fitting result;

[0068] Here, the fitting result includes an angle error value of the tracking body advancing direction and the expected route and a lateral offset distance error value. The present application utilizes the linear regression algorithm for linear fitting, but is not limited to only utilizing the linear regression algorithm.

[0069] S4: a plane coordinate system is established according to the tracking body;

[0070] The present application takes the common two-drive intelligent vehicle as the tracking body to establish the plane coordinate system. It is assumed that the mass of the tracking body is concentrated in the center of mass, which can be equivalent to a particle, and the initial time expected line direction is taken as the x-axis and the expected line vertical direction is taken as the y-axis to establish the plane coordinate system.

[0071] S5: according to the plane coordinate system, the position and speed of the tracking body in the advancing direction and the lateral offset direction are decomposed;

[0072] Since the angle error value of the current controlled object advancing direction and the expected route and the lateral offset distance error value can be output by the linear regression in the visual module, the speed vector of the intelligent vehicle can be vertically decomposed along the x direction and the y direction at the t time to form V x and V y , that is:

[0073] x(t) = x(t-Δt) + v x ×Δt

[0074] y(t) = y(t-Δt) + v y ×Δt

[0075] Wherein, x(t) is the position information / position coordinate of the advancing direction at t time, y(t) is the position information / position coordinate of the lateral deviation direction at t time, t is t time, Δt is the sampling time of the visual camera, v x and v y are the vertical decompositions of the speed vector of the tracking body along the x direction and the y direction respectively.

[0076] S6: Determine two groups of control rates according to the angle deviation value and the distance deviation value of the fitting straight line and the advancing direction straight line in the fitting result respectively;

[0077] PID control is a commonly used and effective control method, and through different combinations of proportion, integral and differential, the dynamic and steady-state performance of the controlled object can be effectively improved. The present application adopts position type PID control, and designs control rates according to the current angle deviation and distance deviation transmitted by the visual module. Since the computer processes digital signals, the control rates are discretized, so the two groups of control rates are:

[0078]

[0079]

[0080] Wherein, PWM y (t), PWM x (t) correspond to the lateral deviation error correction value and the advancing direction angle correction value of the controlled object at t time respectively, Kp, Ki, Kd are the parameters of PID, and the x, y subscripts of PID correspond to the two groups of control parameters of PWM x and PWM y , e y (t) is the lateral deviation error at t time, e θ (t) is the angle error of the advancing direction and the expected line at t time, and Δt is the sampling time of the visual camera. Usually, the sampling time of the camera tracking is 20 frames / s to 50 frames / s, so the sampling time Δt is usually taken as 0.02 seconds to 0.05 seconds.

[0081] S7: Iteratively adjust the two groups of control rates by using the improved particle swarm algorithm to obtain the optimal control rate in the current environment;

[0082] The step S7 comprises:

[0083] S71: Initialize the search speed and position information of the particle, wherein the information carried by the particle is the PID parameter in the control rate;

[0084] S72: Calculate the fitness of the particle according to the cumulative error of the tracking effect;

[0085] In the iterative search optimization, the parameters searched by the particles need to be evaluated, the fitness values of the parameters are compared, and elimination and update are continuously performed, so that optimization is realized. The present application evaluates the fitting deviation of the tracking line and the expected line under the control of different PID parameters, and focuses on the tracking later stage, that is, the steady-state error, so as to design a fitness function based on time weight and absolute error integral, and the fitness function expression is:

[0086]

[0087] wherein, is the fitness value of the PID parameter information searched by the i1th particle in the kth iteration, t is time, and the size of the time determines the weight of the error information at this time, represents the error at t time when the tracking control is performed by applying the PID parameter searched by the i1th particle in the kth iteration, and the absolute value is adopted to eliminate the influence of the sign.

[0088] S73: judging whether the fitness is better than the individual extreme value, if yes, entering step S74, otherwise, entering step S75;

[0089] S74: updating the individual extreme value and judging whether the fitness is better than the group extreme value, if yes, updating the group extreme value to obtain the optimal control rate under the current environment; otherwise, entering step S75;

[0090] S75: performing local search or global search by using a self-adaptive weight and compression factor fusion method and automatically updating the search speed of the particle and then returning to step S72.

[0091] In the traditional particle swarm algorithm, the inertia weight is often a fixed value in 0.9 to 1.2, and the individual and social learning factors are often fixed at 2, however, the particle swarm algorithm based on fixed factors is easy to fall into a local optimal solution, and the particle speed extreme value needs to be limited. In the present application, the self-adaptive inertia weight method is adopted according to the fitness size of the particle position, so that the ability of the particle to inherit the previous speed can be dynamically adjusted, the global or local search strategy can be selectively performed, and falling into a local optimal solution can be effectively avoided.

[0092] The expression of the self-adaptive inertia weight is:

[0093]

[0094] is the inertia weight size of the i1th particle in the kth iteration, W min , W max is a given minimum inertia weight coefficient and a maximum inertia weight coefficient, The average fitness of the entire group n particles in the kth iteration is denoted as The position information searched by the ith1 particle in the kth iteration is denoted as The fitness value calculated by substituting into the fitness function, The minimum fitness value calculated by the entire group in the kth iteration is denoted as Wk. min Wk takes 0.4, W max Wk takes 0.9.

[0095] It is obvious that when the inertia is large, global search is performed, and when the inertia is small, local search is performed, so as to realize self-adaption of the inertia weight.

[0096] By using the method of compression factor, the particle velocity can be autonomously and effectively limited without artificially calculating the velocity extreme value, and the particle is balanced in global and local search, so as to improve the intelligence and universality of the algorithm under the condition of ensuring convergence. The compression factor is:

[0097]

[0098] Wherein, The compression factor is C, C represents a learning factor and C=c1+c2, c1 represents an individual learning factor, and c2 represents a social learning factor. c1 and c2 are usually 2.05.

[0099] The velocity updating formula of the particle is:

[0100]

[0101] Wherein, r1 and r2 are random numbers in the range of [0, 1], Indicates the velocity of the ith1 particle in the kth iteration, The compression factor is C, The inertia weight size of the ith1 particle in the kth iteration is denoted as Indicates the position of the ith1 particle in the kth iteration (i.e. the PID information at this time), Indicates the relative optimal position searched by the ith1 particle in the kth iteration (i.e. the relative optimal PID information at this time), g k Indicates the relative optimal position searched by the entire group in the kth iteration (i.e. the relative optimal PID information of the entire group at this time), c1 represents an individual learning factor, and c2 represents a social learning factor.

[0102] S8: navigation tracking control is performed according to the optimal control rate.

[0103] The above merely describes preferred embodiments of the present application and is not used to limit the present application, and any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. An intelligent navigation and tracking control method, characterized in that, The intelligent navigation tracking control method includes: S1: The target route in the original target route image is adaptively extracted using an adaptive thresholding algorithm to obtain the target route image; S2: Optimize the target route image to obtain an optimized route map; S3: Perform linear fitting on the optimized roadmap and the desired roadmap to obtain the fitting result; S4: Establish a planar coordinate system based on the tracking body; S5: Based on the planar coordinate system, decompose the position and velocity of the tracking body in the forward direction and the lateral offset direction; S6: Determine two sets of control rates based on the angle deviation and distance deviation values ​​between the fitted line and the forward direction in the fitting results; S7: The two sets of control laws are iteratively tuned using an improved particle swarm optimization algorithm to obtain the optimal control law under the current environment; S8: Perform navigation and tracking control based on the optimal control law; In step S1, the threshold information of the tracked target in the region in the adaptive thresholding algorithm T t This can be expressed as: Where L, A, and B represent the different modal values ​​of the color space of the extracted region, i=1 and 2 respectively, where i=1 represents the minimum value and i=2 represents the maximum value. The "+" sign does not represent numerical summation but only represents the combination of the element arrays. d j Represents the threshold perturbation factor and when i is 1 d j The preceding symbol is "-"; In step S6, the two sets of control rates are: in, , Corresponding to t The lateral offset error correction value and the forward direction angle correction value of the controlled object at all times. Kp , Ki , Kd For the parameters of the PID, the PID's x , y The subscripts correspond to and Two sets of control parameters, for t lateral offset error at any time for t The angular error between the direction of travel and the desired line at any given moment. This is the sampling time of the visual camera.

2. The intelligent navigation and tracking control method according to claim 1, characterized in that, In step S2, the optimization processing of the target route image includes: The target route image is subjected to binarization, filtering, erosion, and random noise reduction.

3. The intelligent navigation and tracking control method according to claim 1, characterized in that, Step S5 includes: in, for t Real-time forward direction and position information / position coordinates for t Position information / position coordinates of the horizontal offset direction at any time. t for t time, The sampling time of the visual camera. and The velocity vectors of the tracking body are respectively along... x direction and y Vertical decomposition of directions.

4. The intelligent navigation and tracking control method according to any one of claims 1-3, characterized in that, Step S7 includes: S71: Initialize the search velocity and position information of the particles, wherein the information carried by the particles is the PID parameter in the control law; S72: Calculate the particle's fitness based on the cumulative error of the tracking effect; S73: Determine whether the fitness is better than the individual extreme value. If yes, proceed to step S74; otherwise, proceed to step S75. S74: Update the individual extreme value and determine whether the fitness is better than the population extreme value. If so, update the population extreme value to obtain the optimal control rate under the current environment; otherwise, proceed to step S75. S75: After performing a local or global search using the adaptive inertia weight and compression factor fusion method and automatically updating the particle search velocity, return to step S72.

5. The intelligent navigation and tracking control method according to claim 4, characterized in that, In step S72, the functional expression for fitness is: in, For the first i One particle in the first k The fitness value of the PID parameter information searched in the next iteration. t The magnitude of time determines the weight of the error information at that moment. Indicates when the application is the first i One particle in the first k When using the PID parameters searched in the next iteration for tracking control, t The time error was accounted for, and the influence of the sign was eliminated by using absolute values.

6. The intelligent navigation and tracking control method according to claim 4, characterized in that, In step S75, the expression for the adaptive inertia weight is: For the first The particle in the first k The magnitude of the inertia weight in the next iteration , Given the minimum and maximum inertia weight coefficients, For the entire group n The particle in the first k The average fitness in the next iteration and , To make the first The particle in the first k Location information searched in the next iteration Substitute the calculated fitness value into the fitness function. For the entire group in the first k The minimum fitness value calculated in the next iteration.

7. The intelligent navigation and tracking control method according to claim 4, characterized in that, In step S75, the compression factor is: in, As the compression factor, C Represents the learning factor and , c 1 represents the individual learning factor. c 2 represents the social learning factor.

8. The intelligent navigation and tracking control method according to claim 4, characterized in that, In step S75, the particle velocity update formula is: in, r 1. r 2 is a random number in the range [0,1]. Indicates the first The particle iterates to the... k The speed of the next time, As the compression factor, For the first The particle in the first k The magnitude of the inertia weight in the next iteration Indicates the first The particle iterates to the... k The next position. Indicates the first Individual particles k The relatively optimal position found in the next iteration. Indicates the entire group k The relatively optimal position found in the next iteration. c 1 represents the individual learning factor. c 2 represents the social learning factor.

Images