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

Abstract

The invention provides a fruit picking robot navigation system based on fuzzy immune-PID control, and relates to the field of intelligent picking, and the system comprises the steps: collecting discrete orchard paths through a teaching mode, and carrying out the fitting of the discrete orchard paths, and obtaining a fitting path; constructing a crawler chassis model of the picking robot, and obtaining course deviation and transverse deviation of a chassis of the picking robot at the current moment; a fuzzy immune-PID controller is constructed, and control parameters in a navigation system are regulated and controlled in real time through the fuzzy immune-PID controller, so that the picking robot tracks a target path with extremely small transverse deviation in the navigation process; finally, a cooperative control strategy of'stopping and picking 'between the picking mechanical arm and the chassis is adopted, and automatic picking is achieved. The system can realize accurate navigation, complete automatic picking, avoid interference of external environmental factors in the navigation process, effectively save the picking cost and improve the picking efficiency.

Description

Technical Field

[0001] The 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 in order to reduce the cost and consumption of picking in the agricultural production process, more and more countries and regions have used picking robots in agricultural production to replace manual operations. Among them, the navigation system is the premise for the picking robot to carry out precise picking, which directly determines the picking accuracy and crop damage rate of the picking robot; At present, the mainstream navigation methods include GPS navigation, machine vision navigation, multi-sensor fusion navigation, etc. Single GPS navigation is not only costly and has low positioning accuracy, but also has low reliability for picking operations in the agricultural production process; Machine vision navigation requires the collection of a large number of images for training in the early stage, and is easily disturbed by external factors such as lighting conditions, crop morphology changes, and road obstacles. It has poor stability and poor real-time processing effect. At the same time, machine vision navigation has high requirements for the efficiency and robustness of the algorithm and requires huge computing power support; Pure multi-sensor fusion navigation not only requires the coordination between multiple sensors to be adjusted, but also requires a large number of sensors to be set on the picking robot. The equipment cost is high, and the multi-sensor fusion adjustment takes a long time. It is easy to cause problems such as mutual interference and mutual influence of sensors, and it is very easy to cause contradictions in the navigation instructions of the picking robot. In addition, during the navigation process, existing harvesting robots rely on simple path planning and sensors and cannot flexibly respond to dynamic changes in the environment (for example, changes in the position of obstacles). They still need manual adjustments of position and angle, resulting in poor navigation fluency and low efficiency, which in turn affects the overall harvesting efficiency and fluency of crops. Summary of the invention

[0003] In view of 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: A fruit picking robot navigation system based on fuzzy immune-PID control, comprising: Firstly, the discrete orchard paths are collected through teaching, and the discrete orchard paths are fitted to obtain the fitted paths; then, the tracked 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 through the fuzzy immune-PID controller, so that the picking robot can track the target path with a very small lateral deviation during the navigation process, thereby realizing the precise navigation of the picking robot; finally, a "stop and pick" collaborative control strategy between the picking robot arm and the chassis is adopted to realize automated picking.

[0005] Based on the further optimization of the above scheme, the specific method of collecting discrete orchard paths by teaching is: 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 road in the orchard, and mark the position of the fruit trees simultaneously during the driving process; after that, when the picking robot returns to the initial position, stop the operation of 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.

[0006] The discrete control points collected by teaching will have 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 fitting path is as follows: 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:

[0007] 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 collection 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 i indivualk The B-spline basis function is:

[0008] 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), the node vector length m 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:

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

[0010] This system adopts a uniform sampling method, so all The values ​​of are equal and .

[0011] Since there are turning parts in the discrete orchard path, in order to ensure the high accuracy of the fitting curve of the turning part, 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 path using the B-spline curve fitting method, specifically: First, the curve C(t) according to The intervals are uniformly sampled to obtain m path points, at this time 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:

[0012] Where: t i represents a node vector, which ranges from [0,1], and i =0,1,2,…, m ; After that, 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 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 beginning or end point), and use The interval (i.e., the interval formed by the curve connecting the first and the 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.

[0013] Based on the further optimization of the above scheme, 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 (for example, Beidou 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 foresight point continues to be searched until a foresight point that meets the conditions is found. Then, connect the current position and the foresight point with x Angle between axes , get heading deviation :

[0014] 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 :

[0015] Finally, the relative position between the current coordinates of the picking robot chassis and the fitting path is determined as follows: Get the nearest path point on the fitted path ( x 2 ,y 2) The next waypoint ( x 3 ,y3), 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:

[0016] like , it means 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.

[0017] Based on the further optimization of the above scheme, 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 a very small lateral deviation during the navigation process. Specifically: First, establish fuzzy immune feedback control:

[0018] Where: k 1. k 2 represent the stimulatory factor and inhibitory factor, respectively, and their signs are both positive; f(·) A nonlinear control function representing 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 Output change rate As the input of the nonlinear P controller applying fuzzy immune feedback control, the corresponding output is:

[0019]

[0020] Where: k p1 Used to control reaction speed; Indicates the effect of system stability; It represents the error change rate; , All 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:

[0021] 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 last error respectively.

[0022] 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: First, fuzzy processing is performed: the heading deviation As 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 horizontal axis is the range of the fuzzy domain, and the vertical axis is the degree of belonging to the fuzzy subset, that is, the membership degree, whose range is [0, 1]. The fuzzy subsets of the input and output variables are all seven-level fuzzy subsets, that is:

[0023] In the formula: P means positive, N means negative, Z means zero, B means large, M means medium, and S means small; Among them, the fuzzy rules are obtained based on expert experience:

[0024] 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 centroid method is used to defuzzify the output fuzzy value:

[0025]

[0026] Where: K p , K i , K d They represent the initial parameters of PID control respectively; X i represents fuzzy variable elements; express X i The corresponding membership degree.

[0027] The above only takes the heading deviation as Added to PID parameter control, without lateral deviation d The speed factor is added to the 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 through the speed factor v With lateral deviation d Obtaining regulatory factors a(v) :

[0028] Where: Indicates the number of antigens, through the speed factor v With lateral deviation d Affects the amount of antigens and thus the concentration of cells; 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; Finally, the corresponding output is obtained: .

[0029] Based on the further optimization of the above scheme, the coordinated control strategy of "one stop and 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, the target fruit tree position information, etc. 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 (i.e., a depth camera is set on the picking robot), so as 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 above fuzzy immune-PID control, 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 stops 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 identified, the picking robot arm performs the fruit picking action; if the fruit is not identified, an error instruction is sent, and the picking robot is started to navigate to the next target fruit tree position to continue the picking task; Step S5, result feedback and path adjustment: After the picking is completed, the picking mechanical arm transmits the feedback information of the successful picking back to the control system. After the picking robot receives the status signal of the picking mechanical arm, it 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 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.

[0030] The following are the technical effects of the technical solution of the present invention: The present invention is based on a picking robot, uses a teaching method to obtain a discrete orchard path, and fits the discrete path through a B-spline curve fitting method, thereby obtaining an accurate navigation path; and in the B-spline curve fitting process, the path turning point is obtained by using a uniform sampling method and a curvature comparison, thereby performing intensive collection of the path turning point, ensuring the smoothness of the turning point, and further ensuring the accuracy and no breakpoints of the fitting path. Afterwards, using the crawler chassis model of the picking robot, the position and heading information of the picking robot are used as input to establish a fuzzy immune-PID control system, so that the picking robot tracks the fitted target path with a very small lateral deviation, and uses the heading deviation as the controller error, thereby completing the accurate navigation of the picking robot on the orchard road. The navigation system can not only obtain an accurate navigation path on a complex orchard road, but also control the picking robot to accurately move along the navigation path, effectively avoid the interference of external environmental factors, and realize high-precision, high-efficiency, and automated fruit picking, with high stability and strong real-time performance, thereby saving labor costs in the fruit picking process and improving the economic benefits of fruit planting. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

[0033] Figure 3 This is a navigation flow chart of the picking robot in an embodiment of the present invention.

[0034] Figure 4 The figure is an overall operation flow chart of the picking robot in an embodiment of the present invention.

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

[0036] 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 explanation 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.

[0037] Embodiment 1: 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 robot arm adopts a set of seven-degree-of-freedom robot arms (such as Figure 5 shown); Specific navigation methods include: Firstly, the discrete orchard paths are collected through teaching, specifically: first turn on the teaching pendant device and connect it to the mobile device using the RS232 to db9 data cable; then manually control the picking robot to drive on the (predetermined) orchard road, and mark the positions of the fruit trees during driving (using conventional image marking methods); when the picking robot returns to the initial position, stop the operation of the picking robot, convert the output instructions, and obtain the coordinates of the collected path points; finally, fit the discrete path point coordinates to obtain the discrete orchard path.

[0038] The discrete orchard path is fitted to obtain the fitting path, specifically: 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:

[0039] 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 collection 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 i indivual k The B-spline basis function is:

[0040] 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), the node vector length m 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:

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

[0042] This system adopts a uniform sampling method, so all The values ​​of are equal and .

[0043] During the fitting process, the curvature is used to determine the sampling density under different path conditions, specifically: The curve C(t) according to The intervals are uniformly sampled (in this embodiment, ),get m path points, at this time node t for ; Will t Substitute into the formula , obtain the coordinates of each path point of the curve; Compute the curvature at each path point:

[0044] Where: t i represents a node vector, which ranges from [0,1], and i= 0,1,2,…, m ; 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; Find the beginning and end 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 the two adjacent path points before and after it, one is in the maximum value interval and the other is in the minimum value interval, then the path point is the beginning or end 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 L1, 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 last points) is uniformly sampled at intervals of 0.02 m, 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.

[0045] Then, construct the crawler chassis model of the picking robot (such as Figure 5 As shown), and obtain the heading deviation and lateral deviation of the picking robot chassis at the current moment, specifically: The current coordinates of the picking robot chassis are obtained by combining the antenna set on the picking robot chassis with the satellite (for example, Beidou satellite). x 1 ,y 1) And heading angle (like Figure 1 shown); Select the initial foresight point ( x 4 ,y 4) Set the foresight distance threshold D（ According to 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, continue to search for the foresight point until a foresight point that meets the conditions is found; Connect the current position and the foresight point with x Angle between axes , get heading deviation :

[0046] 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 :

[0047] Determine the relative position between the current coordinates of the picking robot chassis and the fitted path: 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 pointa , 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:

[0048] like , it means 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.

[0049] After that, 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 a very small lateral deviation during the navigation process, thus realizing the precise navigation of the picking robot. Specifically: Establish fuzzy immune feedback control:

[0050] Where: k 1. k 2 represent the stimulatory factor and inhibitory factor, respectively, and their signs are both positive; f(·) A nonlinear control function representing 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; The output of the fuzzy immune-PID controller Output change rate As the input of the nonlinear P controller applying fuzzy immune feedback control, the corresponding output is:

[0051]

[0052] Where: k p1 Used to control reaction speed; Indicates the effect of system stability; It represents the error change rate; , All are inputs of the fuzzy immune-PID controller; Adjusting the parameters of PID controller by fuzzy control k i , k d : First perform fuzzy processing: using heading deviation As 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 horizontal axis is the range of the fuzzy domain, and the vertical axis is the degree of belonging to the fuzzy subset, that is, the membership degree, whose range is [0, 1]. The fuzzy subsets of the input and output variables are all seven-level fuzzy subsets, that is:

[0053] In the formula: P means positive, N means negative, Z means zero, B means large, M means medium, and S means small; Among them, the fuzzy rules are obtained based on expert experience:

[0054] 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; Then the centroid method is used to defuzzify the output fuzzy value:

[0055]

[0056] Where: K p , K i , K d They represent the initial parameters of PID control respectively; X i represents fuzzy variable elements; express X i The corresponding membership degree.

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

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

[0059] Finally, the coordinated control strategy of "one stop and one pick" between the picking robot arm and the chassis is adopted to realize automatic picking, specifically: Step S1, path information acquisition and target fruit tree positioning: the path information of the target orchard, the target fruit tree position information, etc. 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 (i.e., a depth camera is set on the picking robot), so as 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 above fuzzy immune-PID control, and automatically decelerates and accurately docks when approaching the target fruit tree; Step S3, starting the picking mechanical arm and identifying the fruit: when the robot docks at the target fruit tree position, the picking mechanical arm is started; a visual system is installed at the end of the picking mechanical arm to identify the target fruit (the identification process can extract the characteristics of the fruit, such as shape, color, size, etc., through an image processing algorithm, and match them with the fruit model in the database, using conventional image recognition methods in the art); Step S4, fruit picking: if the fruit is identified, the picking robot arm performs the fruit picking action (i.e., after the visual system confirms the target fruit, based on the kinematic model of the seven-degree-of-freedom robot arm, the optimal path for the robot arm to reach the fruit is obtained. This method can adopt the existing conventional path calculation method and complete the picking action of the end effector of the seven-degree-of-freedom robot arm); if the fruit is not identified, an error command is sent, and the picking robot is started to navigate to the next target fruit tree position to continue the picking task; Step S5, result feedback and path adjustment: After the picking is completed, the picking mechanical arm transmits the feedback information of the successful picking back to the control system. After the picking robot receives the status signal of the picking mechanical arm, it 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 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.

[0060] Embodiment 2: The above only takes the heading deviation as Added to PID parameter control, without lateral deviation dThe speed factor is added to the 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 scheme in Example 1, the speed factor is introduced in the input process of the nonlinear P controller of the fuzzy immune-PID controller. v (Speed ​​Factor v synchronous acquisition during the movement of the picking robot), and through the speed factor v With lateral deviation d Obtaining regulatory factors a (v) :

[0061] Where: Indicates the number of antigens, through the speed factor v With lateral deviation d Affects the amount of antigens and thus the concentration of cells; 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; Finally, the corresponding output is obtained:

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

Claims

1. A fruit picking robot navigation system based on fuzzy immune-PID control, characterized by: include: Firstly, the discrete orchard paths are collected by teaching, and the discrete orchard paths are fitted to obtain the fitting paths; Then, the 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 is constructed, and the control parameters in the navigation system are adjusted in real time through the fuzzy immune-PID controller, so that the picking robot can track the target path with a very small lateral deviation during the navigation process, thereby realizing the precise navigation of the picking robot; finally, a "stop and pick" collaborative control strategy between the picking robot arm and the chassis is adopted to realize automated picking.

2. A 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 road in the orchard, and mark the position of the fruit trees simultaneously during the driving process; after that, when the picking robot returns to the initial position, stop the operation of 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. A fruit picking robot navigation system based on fuzzy immune-PID control according to claim 1 or 2, characterized in that: The specific method for fitting the discrete orchard path and obtaining the fitting path is: 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 +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 collection 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 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; 。 4. A fruit picking robot navigation system based on fuzzy immune-PID control according to claim 2 or 3, characterized in that: 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 time 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 ; After that, 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 interval 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.

5. The fruit picking robot navigation system based on fuzzy immune-PID control according to claim 4 is 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 foresight point continues to be searched until a foresight point that meets the conditions is found. Then, connect the current position and the foresight point with x Angle between axes , get 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) Consider the distance between two points as the lateral deviation d : Finally, the relative position between the current coordinates of the picking robot chassis and the fitting 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 means 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.

6. The fruit picking robot navigation system based on fuzzy immune-PID control according to claim 5, characterized in that: 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 a very small lateral deviation during the navigation process. Specifically: First, establish fuzzy immune feedback control: Where: k 1. k 2 represent the stimulatory factor and inhibitory factor, respectively, and their signs are both positive; f(·) A nonlinear control function representing 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 Output change rate As the input of the nonlinear P controller applying fuzzy immune feedback control, the corresponding output is: Where: k p1 Used to control reaction speed; Indicates the effect of system stability; It represents the error change rate; , All 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) They represent the current error, the last error and the last error respectively.

7. The fruit picking robot navigation system based on fuzzy immune-PID control according to claim 6, characterized in that: The PID controller is adjusted by fuzzy control. k i , k d The specific method is: First, fuzzy processing is performed: the heading deviation As 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 horizontal axis is the range of the fuzzy domain, and the vertical axis is the degree of belonging to the fuzzy subset, that is, the membership degree, whose range is [0, 1]. The fuzzy subsets of the input and output variables are all seven-level fuzzy subsets, that is: In the formula: P means positive, N means negative, Z means zero, B means large, M means medium, and S means small; Among them, the 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 centroid method is used to defuzzify the output fuzzy value: Where: K p , K i , K d They represent the initial parameters of PID control respectively; X i represents fuzzy variable elements; express X i The corresponding membership degree.

8. The fruit picking robot navigation system based on fuzzy immune-PID control according to claim 7 is characterized in that: The "one stop and one pick" collaborative control strategy 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, the target fruit tree position information, etc. are obtained by using the above teaching method and the 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 above fuzzy immune-PID control, 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 stops 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 identified, the picking robot arm performs the fruit picking action; if the fruit is not identified, an error instruction is sent, and the picking robot is started to navigate to the next target fruit tree position to continue the picking task; Step S5, result feedback and path adjustment: After the picking is completed, the picking mechanical arm transmits the feedback information of the successful picking back to the control system. After the picking robot receives the status signal of the picking mechanical arm, it 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 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