Fruit picking robot navigation system based on fuzzy immune-PID control

Abstract

The present invention provides a fruit picking robot navigation system based on fuzzy immune-PID control, which relates to the field of intelligent picking. The system includes: collecting discrete orchard paths through teaching, fitting the discrete orchard paths to obtain a fitted path; constructing a crawler chassis model of the picking robot, and obtaining the heading deviation and lateral deviation of the picking robot chassis at the current moment; constructing a fuzzy immune-PID controller, and using the fuzzy immune-PID controller to adjust the control parameters in the navigation system in real time, so that the picking robot can track the target path with minimal lateral deviation during navigation; finally, adopting a "stop and pick" collaborative control strategy between the picking robot arm and the chassis to achieve automated picking. The system can achieve precise navigation and complete automated picking, avoid interference from external environmental factors during the navigation process, effectively save picking costs, and improve picking efficiency.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent picking, and in particular to a fruit picking robot navigation system based on fuzzy immune-PID control. Background Art

[0002] With the continuous development of intelligent technology and to reduce the cost and consumption of harvesting in agricultural production, more and more countries and regions are using harvesting robots in agricultural production to replace manual labor. The navigation system is a prerequisite for harvesting robots to perform precise harvesting, directly determining their picking accuracy and crop damage rate. Currently, mainstream navigation methods include GPS navigation, machine vision navigation, and multi-sensor fusion navigation. GPS navigation alone is not only costly and has low positioning accuracy, but also has low reliability for harvesting operations in agricultural production. Machine vision navigation requires the collection of a large number of images for training, and is easily affected by external factors such as lighting conditions, crop morphology changes, and road obstacles. This leads to poor stability and real-time processing performance. Furthermore, machine vision navigation requires high algorithm efficiency and robustness, and requires significant computing power. Pure multi-sensor fusion navigation not only requires the coordination and calibration of multiple sensors, but also requires the installation of a large number of sensors on the harvesting robot. This leads to high equipment costs, a long calibration process, and is prone to sensor interference and mutual influence, making it very easy for the harvesting robot to issue conflicting navigation instructions. In addition, during the navigation process, existing harvesting robots rely on simple path planning and sensors, which cannot flexibly respond to dynamic changes in the environment (for example, changes in the position of obstacles). Manual adjustments of position and angle are still required, resulting in poor navigation fluency and low efficiency, which in turn affects the overall efficiency and fluency of crop harvesting. Summary of the Invention

[0003] In response to the problems existing in the above-mentioned prior art, the purpose of the present invention is to provide a fruit picking robot navigation system based on fuzzy immune-PID control, which can follow the target path to achieve straight-line tracking and U-turns, can accurately track the target fruit trees and perform precise picking, realize the "one stop and one pick" operation mode, and complete automated picking, thereby effectively saving picking costs and improving picking efficiency.

[0004] The purpose of the present invention is achieved through the following technical solutions:

[0005] A fruit picking robot navigation system based on fuzzy immune-PID control, comprising:

[0006] Firstly, discrete orchard paths are collected through teaching, and the discrete orchard paths are fitted to obtain the fitted paths. Then, a crawler chassis model of the picking robot is constructed, and the heading deviation and lateral deviation of the picking robot chassis at the current moment are obtained. After that, a fuzzy immune-PID controller is constructed, and the control parameters in the navigation system are adjusted in real time by the fuzzy immune-PID controller, so that the picking robot can track the target path with extremely small lateral deviation during the navigation process, thereby realizing precise navigation of the picking robot. Finally, a "one-stop and one-picking" collaborative control strategy between the picking robot arm and the chassis is adopted to realize automated picking.

[0007] Based on the further optimization of the above scheme, the specific method of collecting discrete orchard paths by teaching is as follows:

[0008] First, turn on the teach pendant device and connect it to the mobile device using an RS232 to DB9 data cable. Then, control the picking robot to drive on the orchard road, and mark the positions of the fruit trees during the driving process. After that, when the picking robot returns to the initial position, stop the picking robot and convert the output instructions to obtain the coordinates of the collected path points. Finally, fit the discrete path point coordinates to obtain the discrete orchard path.

[0009] The discrete control points collected by the teaching method will produce noise, resulting in an uneven navigation path, which in turn affects the accuracy of the subsequent navigation path. Based on the further optimization of the above scheme, the specific method of fitting the discrete orchard path and obtaining the fitted path is as follows:

[0010] The B-spline curve fitting method is used to fit the discrete orchard path, where the B-spline curve is composed of P 0 , P 1 ,…,P n common n+ A parameterized curve defined by 1 control point and a set of basis functions, specifically:

[0011]

[0012] Where: C(t) represents a point on the curve; t Represents nodes, the number of which is n+k+ 2, the value range is [0,1]; P i Represents a set of latitude and longitude coordinates of waypoints; n Indicates the number of waypoints; k Indicates the degree of the B-spline curve. k =3;

[0013] B i,k (t) Indicates the i indivual k The B-spline basis function is:

[0014]

[0015] Among them, the node vector T={ t 0 ,t 1 ,t 2 ,…,t m} is the interval of the basis function ( t 0=0, t m =1), node vector length m and the number of control points n+ The relationship of 1 is:

[0016] m=n+k +1;

[0017] The node vector T of the cubic B-spline curve is specifically:

[0018]

[0019] Where: Indicates the distance between two adjacent path points;

[0020]

[0021] The uniform sampling method is used in this system, so all The values ​​of are equal and .

[0022] Since there are turning parts in the discrete orchard paths, in order to ensure the high accuracy of the fitting curve of the turning parts, based on the further optimization of the above scheme, the curvature is used to determine the sampling density under different path conditions during the fitting of the discrete orchard paths using the B-spline curve fitting method, specifically:

[0023] First, the curve C(t) according to The intervals are uniformly sampled to obtain m path points, at this point the node t for ;

[0024] Afterwards, t Substitute into the formula , obtain the coordinates of each path point of the curve;

[0025] Then, calculate the curvature for each path point:

[0026]

[0027] Where: t i represents a node vector, which ranges from [0,1], and i =0,1,2,…, m ;

[0028] Then, the fitting path is divided into a large curvature part and a small curvature part according to the curvature size, and a curvature threshold is set. The area in the large curvature part that is greater than the curvature threshold is the turning part of the path;

[0029] Finally, find the starting and ending points of the turning part by the degree of change in curvature (that is, the change between the curvature of a path point and the curvature of its two adjacent path points before and after, if one is in the maximum value interval and the other is in the minimum value interval, then the path point is the starting or ending point), and use The interval (i.e. the interval formed by the curve connecting the first and end points) is uniformly sampled and then substituted into the formula In the process, the path points of the turning part are densely packed to ensure the smoothness and high precision of the turning part.

[0030] Based on further optimization of the above solution, the specific method for obtaining the heading deviation and lateral deviation of the picking robot chassis at the current moment is:

[0031] First, the current coordinates of the picking robot chassis are obtained by combining the antenna set on the picking robot chassis with satellites (for example, Beidou satellites). x 1 ,y 1) and heading angle ;

[0032] Then, select the initial foresight point ( x 4 ,y 4) Set the foresight distance threshold D If the distance between the current coordinate and the foresight point is less than the foresight distance threshold, the picking robot is controlled to move forward along the fitting path to the next point. Otherwise, the robot continues to search for the foresight point until a qualified foresight point is found.

[0033] Then, connect the current position and the foresight point with x Angle between axes , obtain heading deviation :

[0034]

[0035] Then, obtain the coordinates of the discrete point on the fitting path that is closest to the current coordinates of the picking robot chassis ( x 2, y 2) Consider the distance between two points as the lateral deviation d :

[0036]

[0037] Finally, the relative position between the current coordinates of the picking robot chassis and the fitted path is determined as follows:

[0038] Get the nearest path point on the fitted path ( x 2 ,y 2) The next waypoint ( x 3 ,y 3), and through the vector between the nearest path point and the next path point a , the vector between the current path point and the current coordinates of the picking robot chassis b , complete the judgment of the position between the picking robot and the fitting path:

[0039]

[0040] like , it indicates that the current coordinate of the picking robot chassis is on the left side of the fitting path. At this time, the lateral deviation d The value of is negative; otherwise, the current coordinate of the picking robot chassis is on the right side of the fitting path, and the lateral deviation is d Is a positive value.

[0041] Based on the further optimization of the above scheme, the control parameters in the navigation system are controlled in real time by the fuzzy immune-PID controller, so that the picking robot can track the target path with minimal lateral deviation during the navigation process. Specifically:

[0042] First, establish fuzzy immune feedback control:

[0043]

[0044] Where: k 1. k 2 represents the stimulatory factor and inhibitory factor, respectively, and their signs are both positive; f(·) A nonlinear control function that represents the ability to suppress external stimuli; They represent the output of the fuzzy immune-PID controller and the output change rate of the fuzzy immune-PID controller respectively; Indicates the number of antigens;

[0045] Then, the output of the fuzzy immune-PID controller is and output change rate As the input of the nonlinear P controller using fuzzy immune feedback control, the corresponding output is:

[0046]

[0047]

[0048] Where: k p1 Used to control reaction rate; Indicates the effect on system stability; represents the error change rate; 、 Both are inputs of the fuzzy immune-PID controller;

[0049] Afterwards, the parameters in the PID controller are adjusted by fuzzy control k i 、 k d ;

[0050] Finally, the output of the fuzzy immune-PID controller is:

[0051]

[0052] Where: k p1 represents the proportional parameter calculated by the fuzzy immune feedback mechanism, k i represents the integral parameter calculated by fuzzy control, k d Represents the differential parameter calculated by fuzzy control; e(k ), e(k-1) 、 e(k-2) They represent the current error, the last error, and the previous error respectively.

[0053] Based on the further optimization of the above scheme, the PID controller parameters are adjusted by fuzzy control. k i 、 k d The specific method is:

[0054] First, perform fuzzy processing: using heading deviation As an error , with the heading deviation change rate As the error rate of change , their fuzzy domains are all [-1, 1]; the output is the rate of change of integral and differential parameters , their fuzzy domains are all [-0.3, 0.3]; the membership function uses a triangular distribution curve, the horizontal axis is the range of the fuzzy domain, the vertical axis is the degree of belonging to the fuzzy subset, that is, the membership degree, which ranges from [0, 1]. The fuzzy subsets of the input and output variables are all seven-level fuzzy subsets, that is:

[0055]

[0056] In the formula: P represents positive, N represents negative, Z represents zero, B represents large, M represents medium, and S represents small;

[0057] Among them, fuzzy rules are obtained based on expert experience:

[0058]

[0059] Where: express e The degree of membership to A, express The degree of membership to H; u rule Indicates the most applicable membership degree for determining the fuzzy rule; A and H are both fuzzy sets;

[0060] Finally, the center of gravity method is used to defuzzify the output fuzzy value:

[0061]

[0062]

[0063] Where: K p 、 K i 、 K d Respectively represent the initial parameters of PID control; X i represents fuzzy variable elements; express X i The corresponding membership degree.

[0064] The above only considers the heading deviation Added to PID parameter control, without lateral deviation d Add PID parameter control, at the same time, the picking robot is faster when driving in a straight line, and needs to be slower when turning to ensure the smoothness and safety of the turn. Based on the further optimization of the above scheme, the speed factor is introduced into the input process of the nonlinear P controller of the fuzzy immune-PID controller. v , and by the speed factor vand lateral deviation d Obtaining regulatory factors a(v) :

[0065]

[0066] Where: Indicates the number of antigens, through the speed factor v and lateral deviation d Affects the amount of antigen and thus the concentration of cells;

[0067] After that, set the speed threshold v th ,when v > v th When the adjustment factor is reduced a(v) , to slow down the system response and prevent overshoot, otherwise, increase the adjustment factor;

[0068] Finally, the corresponding output is obtained:

[0069] .

[0070] Based on the further optimization of the above scheme, the collaborative control strategy of "one stop and one pick" between the picking robot arm and the chassis is specifically as follows:

[0071] Step S1, path information acquisition and target fruit tree positioning: The path information of the target orchard and the target fruit tree position information are obtained using the above-mentioned teaching method and B-spline curve fitting method. The target fruit tree is accurately positioned in combination with a machine vision method (i.e., a depth camera is set on the picking robot) to provide support for subsequent automatic navigation and picking.

[0072] Step S2, automatic navigation and docking: After obtaining the target fruit tree path information, the picking robot automatically drives along the orchard road according to the planned path through the fuzzy immune-PID control system, and automatically decelerates and accurately docks when approaching the target fruit tree;

[0073] Step S3, starting the picking robot arm and identifying the fruit: When the robot docks at the target fruit tree, the picking robot arm starts working; a visual system is installed at the end of the picking robot arm to identify the target fruit;

[0074] Step S4, fruit picking: If the fruit is recognized, the picking robot arm performs the fruit picking action; if the fruit is not recognized, an error instruction is sent and the picking robot is started to navigate to the next target fruit tree location and continue the picking task;

[0075] Step S5: Result feedback and path adjustment: After the picking is completed, the picking robot arm transmits the feedback information of the successful picking back to the control system. After receiving the status signal from the picking robot arm, the picking robot chooses whether to adjust the picking strategy. At this time, it can return to the preset starting position or continue to the next target fruit tree according to the completion status of the current task;

[0076] Step S6, cyclic operation: After completing a round of picking operation, the picking robot sends a "field turn" command through the control system, turns around in the field, and prepares to enter the next round of picking operation; at this time, the picking robot starts the automatic navigation system again and starts a new picking task.

[0077] The following are the technical effects of the technical solution of the present invention:

[0078] Based on a harvesting robot, this invention uses a teaching method to obtain a discrete orchard path and then fits the discrete path using a B-spline curve fitting method to obtain a precise navigation path. Furthermore, during the B-spline curve fitting process, a uniform sampling method and curvature comparison are used to obtain path turns, thereby intensively sampling the path turns, ensuring smoothness at turns and further ensuring the accuracy and discontinuity of the fitted path. Subsequently, a fuzzy immune-PID control system is established using a model of the harvesting robot's crawler chassis and the robot's position and heading information as inputs. This enables the harvesting robot to track the fitted target path with minimal lateral deviation, while using the heading deviation as the controller error, thereby achieving precise navigation of the harvesting robot along the orchard path. This navigation system not only obtains a precise navigation path on complex orchard paths but also controls the harvesting robot to accurately follow the navigation path, effectively avoiding interference from external environmental factors. This system achieves high-precision, high-efficiency, and automated fruit picking with high stability and strong real-time performance, thereby saving labor costs during the fruit picking process and improving the economic benefits of fruit cultivation. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 This is a geometric relationship diagram of the crawler chassis model tracking of the picking robot in an embodiment of the present invention.

[0080] Figure 2 This is a flow chart of the fuzzy immune-PID controller in an embodiment of the present invention.

[0081] Figure 3 This is a navigation flowchart of the picking robot in an embodiment of the present invention.

[0082] Figure 4 This is an overall operation flow chart of the picking robot in an embodiment of the present invention.

[0083] Figure 5This is an overall structural diagram of the picking robot in an embodiment of the present invention. DETAILED DESCRIPTION

[0084] The technical solutions in the embodiments of the present invention will be described clearly and completely below. In the following description, for the purpose of illustration rather than limitation, specific details such as specific system structures and technologies are proposed to facilitate a thorough understanding of the embodiments of the present invention.

[0085] Example 1:

[0086] A fruit picking robot navigation system based on fuzzy immune-PID control, wherein the picking robot adopts a navigation system combining Beidou satellite navigation and IMU sensor, its walking mechanism adopts a crawler chassis walking model, and its picking manipulator arm adopts a set of seven-degree-of-freedom manipulator arms (such as Figure 5 shown);

[0087] Specific navigation methods include:

[0088] First, discrete orchard paths are collected through teaching. Specifically, the teaching pendant device is first turned on and connected to the mobile device using an RS232 to DB9 data cable. Then, the picking robot is manually controlled to drive on the (predetermined) orchard road, and the positions of the fruit trees are synchronously marked during driving (using conventional image marking methods). When the picking robot returns to the initial position, the picking robot is stopped and the output instructions are converted to obtain the coordinates of the collected path points. Finally, the discrete path point coordinates are fitted to obtain the discrete orchard path.

[0089] The discrete orchard path is fitted to obtain the fitting path, specifically:

[0090] The B-spline curve fitting method is used to fit the discrete orchard path, where the B-spline curve is composed of P 0 , P 1 ,…,P n common n+ A parameterized curve defined by 1 control point and a set of basis functions, specifically:

[0091]

[0092] Where: C(t) represents a point on the curve; t Represents nodes, the number of which is n+k +2, the value range is [0,1]; P i Represents a set of latitude and longitude coordinates of waypoints; n Indicates the number of waypoints; kIndicates the degree of the B-spline curve. k =3;

[0093] B i,k (t) Indicates the i indivual k The B-spline basis function is:

[0094]

[0095] Among them, the node vector T={ t 0 ,t 1 ,t 2 ,…,t m} is the interval of the basis function ( t 0=0, t m =1), node vector length m and the number of control points n+ The relationship of 1 is:

[0096] m=n+k +1;

[0097] The node vector T of the cubic B-spline curve is specifically:

[0098]

[0099] Where: Indicates the distance between two adjacent path points;

[0100]

[0101] The uniform sampling method is used in this system, so all The values ​​of are equal and .

[0102] During the fitting process, curvature is used to determine the sampling density under different path conditions, specifically:

[0103] The curve C(t) according to The intervals are uniformly sampled (in this embodiment, ),get m path points, at this point the node t for ;

[0104] Will t Substitute into the formula , obtain the coordinates of each path point of the curve;

[0105] Compute the curvature at each path point:

[0106]

[0107] Where: t i represents a node vector, which ranges from [0,1], and i= 0,1,2,…, m ;

[0108] The fitting path is divided into a large curvature part and a small curvature part according to the curvature size, and a curvature threshold is set. The area in the large curvature part that is greater than the curvature threshold is the turning part of the path;

[0109] Find the starting and ending points of the turning part by the degree of change in curvature (that is, the change between the curvature of a path point and the curvature of its two adjacent path points before and after, if one is in the maximum value interval and the other is in the minimum value interval, then the path point is the starting or ending point; for example: path point L 0 and its previous path point L 1, the curvature change is in the minimum range, that is, the curvature change is not large, and the path point L 0 and the next path point L 2, the curvature change is in the maximum range, that is, the curvature change is large, then the path point L 0 is the starting point of the turning part; waypoint L 0 and its previous path point L 1, the curvature change is in the maximum range, that is, the curvature change is large, and the path point L 0 and the next path point L 2, the curvature change is in the minimum range, that is, the curvature change is not large, then the path point L 0 is the end point of the turning part), and The interval (i.e., the interval formed by the curve connecting the first and end points) is uniformly sampled at intervals of 0.02m and then substituted into the formula In the process, the path points of the turning part are densely packed to ensure the smoothness and high precision of the turning part.

[0110] Then, the crawler chassis model of the picking robot is constructed (such as Figure 5 As shown in the figure), the heading deviation and lateral deviation of the picking robot chassis at the current moment are obtained, specifically:

[0111] The current coordinates of the picking robot chassis are obtained by combining the antenna set on the picking robot chassis with satellites (for example, Beidou satellites). x 1 ,y 1) and heading angle (like Figure 1 shown);

[0112] Select the initial foresight point (x 4 ,y 4) Set the foresight distance threshold D（ Based on a large amount of actual empirical data), if the distance between the current coordinate and the foresight point is less than the foresight distance threshold, the picking robot is controlled to move forward along the fitting path to the next point. Otherwise, the robot continues to search for the foresight point until a qualified foresight point is found.

[0113] Connect the current position and the foresight point with x Angle between axes , obtain heading deviation :

[0114]

[0115] Get the coordinates of the discrete point on the fitting path that is closest to the current coordinates of the picking robot chassis ( x 2 ,y 2) Consider the distance between two points as the lateral deviation d :

[0116]

[0117] Determine the relative position between the current coordinates of the picking robot chassis and the fitted path:

[0118] Get the nearest path point on the fitted path ( x 2 ,y 2) The next waypoint ( x 3 ,y 3), and through the vector between the nearest path point and the next path point a , the vector between the current path point and the current coordinates of the picking robot chassis b , complete the judgment of the position between the picking robot and the fitting path:

[0119]

[0120] like , it indicates that the current coordinate of the picking robot chassis is on the left side of the fitting path. At this time, the lateral deviation d The value of is negative; otherwise, the current coordinate of the picking robot chassis is on the right side of the fitting path, and the lateral deviation is d Is a positive value.

[0121] Then, a fuzzy immune-PID controller is constructed. The control parameters in the navigation system are adjusted in real time by the fuzzy immune-PID controller, so that the picking robot can track the target path with minimal lateral deviation during navigation, thus achieving accurate navigation of the picking robot. Specifically:

[0122] Establish fuzzy immune feedback control:

[0123]

[0124] Where: k 1. k 2 represents the stimulatory factor and inhibitory factor, respectively, and their signs are both positive; f(·) A nonlinear control function that represents the ability to suppress external stimuli; They represent the output of the fuzzy immune-PID controller and the output change rate of the fuzzy immune-PID controller respectively; Indicates the number of antigens;

[0125] The output of the fuzzy immune-PID controller and output change rate As the input of the nonlinear P controller using fuzzy immune feedback control, the corresponding output is:

[0126]

[0127]

[0128] Where: k p1 Used to control reaction rate; Indicates the effect on system stability; represents the error change rate; 、 Both are inputs of the fuzzy immune-PID controller;

[0129] Adjusting PID controller parameters by fuzzy control k i 、 k d :

[0130] First perform fuzzy processing: using heading deviation As an error , with the heading deviation change rate As the error rate of change , their fuzzy domains are all [-1, 1]; the output is the rate of change of integral and differential parameters , their fuzzy domains are all [-0.3, 0.3]; the membership function uses a triangular distribution curve, the horizontal axis is the range of the fuzzy domain, the vertical axis is the degree of belonging to the fuzzy subset, that is, the membership degree, which ranges from [0, 1]. The fuzzy subsets of the input and output variables are all seven-level fuzzy subsets, that is:

[0131]

[0132] In the formula: P represents positive, N represents negative, Z represents zero, B represents large, M represents medium, and S represents small;

[0133] Among them, fuzzy rules are obtained based on expert experience:

[0134]

[0135] Where: express e The degree of membership to A, express The degree of membership to H; u rule Indicates the most applicable membership degree for determining the fuzzy rule; A and H are both fuzzy sets;

[0136] Then use the centroid method to defuzzify the output fuzzy value:

[0137]

[0138]

[0139] Where: K p 、 K i 、 K d Respectively represent the initial parameters of PID control; X i represents fuzzy variable elements; express X i The corresponding membership degree.

[0140] The output of the fuzzy immune-PID controller is:

[0141]

[0142] Where: e(k ), e(k-1) 、 e(k-2) They represent the current error, the last error, and the previous error respectively.

[0143] Finally, a collaborative control strategy of "one stop, one pick" between the picking robot arm and the chassis is adopted to achieve automated picking. Specifically:

[0144] Step S1, path information acquisition and target fruit tree positioning: The path information of the target orchard and the target fruit tree position information are obtained using the above-mentioned teaching method and B-spline curve fitting method. The target fruit tree is accurately positioned in combination with a machine vision method (i.e., a depth camera is set on the picking robot) to provide support for subsequent automatic navigation and picking.

[0145] Step S2, automatic navigation and docking: After obtaining the target fruit tree path information, the picking robot automatically drives along the orchard road according to the planned path through the fuzzy immune-PID control system, and automatically decelerates and accurately docks when approaching the target fruit tree;

[0146] Step S3, starting the picking robot arm and identifying the fruit: When the robot docks at the target fruit tree, the picking robot arm starts working; a visual system is installed at the end of the picking robot arm to identify the target fruit (the identification process can use image processing algorithms to extract fruit features such as shape, color, size, etc., and match them with fruit models in the database, using conventional image recognition methods in the field);

[0147] Step S4, fruit picking: If the fruit is recognized, the picking robot arm performs the fruit picking action (i.e., after the visual system confirms the target fruit, the optimal path for the robot arm to reach the fruit is obtained based on the kinematic model of the seven-degree-of-freedom robot arm. This method can adopt existing conventional path calculation methods and complete the picking action of the end effector of the seven-degree-of-freedom robot arm). If the fruit is not recognized, an error command is sent and the picking robot is started to navigate to the next target fruit tree location to continue the picking task;

[0148] Step S5: Result feedback and path adjustment: After the picking is completed, the picking robot arm transmits the feedback information of the successful picking back to the control system. After receiving the status signal from the picking robot arm, the picking robot chooses whether to adjust the picking strategy. At this time, it can return to the preset starting position or continue to the next target fruit tree according to the completion status of the current task;

[0149] Step S6, cyclic operation: After completing a round of picking operation, the picking robot sends a "field turn" command through the control system, turns around in the field, and prepares to enter the next round of picking operation; at this time, the picking robot starts the automatic navigation system again and starts a new picking task.

[0150] Example 2:

[0151] The above only considers the heading deviation Added to PID parameter control, without lateral deviation d Add PID parameter control, at the same time, the picking robot is faster when driving in a straight line, and needs to be slower when turning to ensure the smoothness and safety of the turn. As another preferred embodiment of the present invention, based on the solution in Example 1, the speed factor is introduced into the input process of the nonlinear P controller of the fuzzy immune-PID controller. v (Speed ​​Factor v is acquired synchronously during the movement of the picking robot) and by the speed factorv and lateral deviation d Obtaining regulatory factors a (v) :

[0152]

[0153] Where: Indicates the number of antigens, through the speed factor v and lateral deviation d Affects the amount of antigen and thus the concentration of cells;

[0154] After that, set the speed threshold v th ,when v > v th When the adjustment factor is reduced a(v) , to slow down the system response and prevent overshoot, otherwise, increase the adjustment factor;

[0155] Finally, the corresponding output is obtained:

[0156]

[0157] The final output of the fuzzy immune-PID controller is:

[0158]

Claims

1. A fruit picking robot navigation system based on fuzzy immune-PID control, characterized by: include: Firstly, the discrete orchard path is collected by teaching, and the discrete orchard path is fitted to obtain the fitting path. The discrete orchard path is fitted by the B-spline curve fitting method, where the B-spline curve is composed of P 0 ,P 1 ,…,P n common n +1 control point and a set of parameterized curves defined by basis functions, specifically: Where: C(t) represents a point on the curve; t Represents nodes, the number of which is n+k +2, the value range is [0,1]; P i Represents a set of latitude and longitude coordinates of waypoints; n Indicates the number of waypoints; k Indicates the degree of the B-spline curve. k =3; B i,k (t) Indicates the i indivual k The B-spline basis function is: Among them, the node vector T={ t 0 ,t 1 ,t 2 ,…,t m } is the interval of the basis function, the length of the node vector m and the number of control points n The relationship of +1 is: m=n+k +1； The node vector T of the cubic B-spline curve is specifically: Where: Indicates the distance between two adjacent path points; ； In the process of fitting the discrete orchard paths using the B-spline curve fitting method, the curvature is used to determine the sampling density under different path conditions, specifically: First, the curve C(t) according to The intervals are uniformly sampled to obtain m path points, at this point the node t for ; Afterwards, t Substitute into the formula , obtain the coordinates of each path point of the curve; Then, calculate the curvature for each path point: Where: t i represents a node vector, which ranges from [0,1], and i= 0,1,2,…, m ; Then, the fitting path is divided into a large curvature part and a small curvature part according to the curvature size, and a curvature threshold is set. The area in the large curvature part that is greater than the curvature threshold is the turning part of the path; Finally, find the beginning and end points of the turning part by the degree of change of curvature, and use The area is sampled uniformly at intervals and then substituted into the formula In the process, the path points of the turning part are densely packed to ensure the smoothness and high precision of the turning part; Then, a crawler chassis model of the picking robot is constructed, and the heading deviation and lateral deviation of the picking robot chassis at the current moment are obtained; Then, a fuzzy immune-PID controller was constructed. The control parameters in the navigation system were adjusted in real time by the fuzzy immune-PID controller, enabling the picking robot to track the target path with minimal lateral deviation during navigation, thus achieving precise navigation of the picking robot. The fuzzy immune-PID controller is used to regulate the control parameters in the navigation system in real time, so that the picking robot can track the target path with minimal lateral deviation during the navigation process. Specifically: First, establish fuzzy immune feedback control: Where: k 1. k 2 represents the stimulatory factor and inhibitory factor, respectively, and their signs are both positive; f(·) A nonlinear control function that represents the ability to suppress external stimuli; They represent the output of the fuzzy immune-PID controller and the output change rate of the fuzzy immune-PID controller respectively; Indicates the number of antigens; Then, the output of the fuzzy immune-PID controller is and output change rate As the input of the nonlinear P controller using fuzzy immune feedback control, the corresponding output is: Where: k p1 Used to control reaction rate; Indicates the effect on system stability; represents the error change rate; 、 Both are inputs of the fuzzy immune-PID controller; Afterwards, the parameters in the PID controller are adjusted by fuzzy control k i 、 k d ; Finally, the output of the fuzzy immune-PID controller is: Where: k p1 represents the proportional parameter calculated by the fuzzy immune feedback mechanism, k i represents the integral parameter calculated by fuzzy control, k d Represents the differential parameter calculated by fuzzy control; e(k ), e(k-1) 、 e(k-2) Represent the current error, the last error and the previous error respectively; The PID controller is adjusted by fuzzy control. k i 、 k d The specific method is: First, perform fuzzy processing: using heading deviation As an error , with the heading deviation change rate As the error rate of change , their fuzzy domains are all [-1, 1]; the output is the rate of change of integral and differential parameters , their fuzzy domains are all [-0.3, 0.3]; the membership function uses a triangular distribution curve, whose abscissa is the range of the fuzzy domain, and the ordinate is the degree of belonging to the fuzzy subset, that is, the membership degree, which ranges from [0, 1]. The fuzzy subsets of the input and output variables are all seven-level fuzzy subsets, that is: In the formula: P represents positive, N represents negative, Z represents zero, B represents large, M represents medium, and S represents small; Among them, fuzzy rules are obtained based on expert experience: Where: express e The degree of membership to A, express The degree of membership to H; u rule Indicates the most applicable membership degree for determining the fuzzy rule; A and H are both fuzzy sets; Finally, the center of gravity method is used to defuzzify the output fuzzy value: Where: K p 、 K i 、 K d Respectively represent the initial parameters of PID control; X i represents fuzzy variable elements; express X i The corresponding membership degree; Finally, a "stop-and-pick" collaborative control strategy is adopted between the picking robot arm and the chassis to achieve automated picking.

2. The fruit picking robot navigation system based on fuzzy immune-PID control according to claim 1, characterized in that: The specific method of collecting discrete orchard paths by teaching is as follows: First, turn on the teach pendant device and connect it to the mobile device using an RS232 to DB9 data cable. Then, control the picking robot to drive on the orchard road, and mark the positions of the fruit trees during the driving process. After that, when the picking robot returns to the initial position, stop the picking robot and convert the output instructions to obtain the coordinates of the collected path points. Finally, fit the discrete path point coordinates to obtain the discrete orchard path.

3. The fruit picking robot navigation system based on fuzzy immune-PID control according to claim 1, characterized in that: The specific method for obtaining the heading deviation and lateral deviation of the picking robot chassis at the current moment is: First, the current coordinates of the picking robot chassis are obtained by combining the antenna set on the picking robot chassis with the satellite ( x 1 ,y 1) and heading angle ; Then, select the initial foresight point ( x 4 ,y 4) Set the foresight distance threshold D If the distance between the current coordinate and the foresight point is less than the foresight distance threshold, the picking robot is controlled to move forward along the fitting path to the next point. Otherwise, the robot continues to search for the foresight point until a qualified foresight point is found. Then, connect the current position and the foresight point with x Angle between axes , obtain heading deviation : Then, obtain the coordinates of the discrete point on the fitting path that is closest to the current coordinates of the picking robot chassis ( x 2 ,y 2) The coordinate is the nearest path point on the fitting path, and the distance between the two points is regarded as the lateral deviation d : Finally, the relative position between the current coordinates of the picking robot chassis and the fitted path is determined as follows: Get the nearest path point on the fitted path ( x 2 ,y 2) The next waypoint ( x 3 ,y 3), and through the vector between the nearest path point and the next path point a , the vector between the current path point and the current coordinates of the picking robot chassis b , complete the judgment of the position between the picking robot and the fitting path: like , it indicates that the current coordinate of the picking robot chassis is on the left side of the fitting path. At this time, the lateral deviation d The value of is negative; otherwise, the current coordinate of the picking robot chassis is on the right side of the fitting path, and the lateral deviation is d Is a positive value.

4. The fruit picking robot navigation system based on fuzzy immune-PID control according to claim 1, characterized in that: The collaborative control strategy of "one stop, one pick" between the picking robot arm and the chassis is specifically as follows: Step S1, path information acquisition and target fruit tree positioning: The path information of the target orchard and the target fruit tree position information are obtained by using the above-mentioned teaching method and B-spline curve fitting method, and the target fruit tree is accurately positioned in combination with the machine vision method to provide support for subsequent automatic navigation and picking; Step S2, automatic navigation and docking: After obtaining the target fruit tree path information, the picking robot automatically drives along the orchard road according to the planned path through the fuzzy immune-PID control system, and automatically decelerates and accurately docks when approaching the target fruit tree; Step S3, starting the picking robot arm and identifying the fruit: When the robot docks at the target fruit tree, the picking robot arm starts working; a visual system is installed at the end of the picking robot arm to identify the target fruit; Step S4, fruit picking: If the fruit is recognized, the picking robot arm performs the fruit picking action; if the fruit is not recognized, an error instruction is sent and the picking robot is started to navigate to the next target fruit tree location and continue the picking task; Step S5: Result feedback and path adjustment: After the picking is completed, the picking robot arm transmits the feedback information of the successful picking back to the control system. After receiving the status signal from the picking robot arm, the picking robot chooses whether to adjust the picking strategy. At this time, it can return to the preset starting position or continue to the next target fruit tree according to the completion status of the current task; Step S6, cyclic operation: After completing a round of picking operation, the picking robot sends a "field turn" command through the control system, turns around in the field, and prepares to enter the next round of picking operation; at this time, the picking robot starts the automatic navigation system again and starts a new picking task.

Images