A multi-differential wheel coordinated control method

Abstract

The present invention discloses a multi-differential wheel cooperative control method. The differential steering wheel has a unique double-wheel structure and strong bearing capacity, and is particularly suitable for use in AGVs in the field of super-heavy loads. However, the special structure of the differential wheel not only restricts the left and right wheels inside the wheel system when the wheel system moves, but also the multiple sets of wheel systems equipped on the same AGV are constrained by the rigidity of the vehicle body. The mutual constraints between and within the wheel systems lead to serious coupling, which makes the tracking flexibility of the whole vehicle insufficient. Therefore, the present invention proposes a high-precision multi-differential wheel cooperative control method, which combines the control law based on deviation and the control law based on driving ability, and can effectively coordinate the posture correction requirements and the movement ability of the wheel system, realize the rapid elimination of errors and achieve a stable tracking state. This method can maximize the advantages of the omnidirectional movement of multi-differential wheel driven AGVs, so that the differential-driven AGV can achieve a tracking effect similar to that of the direct steering steering wheel driven AGV.

Description

Technical Field

[0001] The invention relates to the technical field of intelligent industrial logistics, and in particular to a multi-differential wheel cooperative control method. Background Art

[0002] In industrial fields such as aerospace manufacturing and rail transportation, the handling of large-mass and large-size workpieces is very common. In order to avoid the high risk of falling and low transfer efficiency caused by hoisting, heavy-duty AGVs are often considered for transportation. The differential wheel is a driving wheel with a double-wheel structure. This double-wheel structure makes the single set of wheel trains have a higher load-bearing capacity, which is particularly suitable for the field of super-heavy loads. In addition, both wheels of this type of wheel train are equipped with drive motors, and steering is based on the speed difference between the two wheels, which can also avoid the wear problem of large load mass on the ground. However, due to this double-wheel structure, the left and right wheels inside the wheel train restrict each other when the wheel train moves, and the multiple sets of wheel trains equipped on the same AGV are constrained by the rigidity of the vehicle body. The mutual constraints between and within the wheel trains and the serious coupling make the tracking flexibility of the whole vehicle insufficient. Summary of the invention

[0003] In order to solve the problem of insufficient tracking performance of AGV driven by multi-differential wheels, a multi-differential wheel cooperative control method under the action of hybrid control law is proposed. This method can effectively optimize the tracking performance of the vehicle, give full play to the advantages of omnidirectional movement of multi-differential wheels, and achieve a tracking effect similar to that of direct differential steering wheels.

[0004] The present application provides a multi-differential wheel cooperative control method, which combines two control laws, namely a control law based on driving capability and a control law based on deviation;

[0005] The control law based on driving capability is divided into the following steps:

[0006] S1, determine the position deviation and posture deviation of the AGV body center relative to the set trajectory line;

[0007] S2, calculating the center velocity and angular velocity of the vehicle body according to the position deviation and the posture deviation;

[0008] S3, calculating the rotation radius R of the vehicle body according to the center velocity and angular velocity of the vehicle body, and using R to represent the speed of the left and right wheels in each wheel train;

[0009] S4, under the limitation of the differential wheel driving capability, solving the inequality about the left and right wheel speeds to obtain multiple value ranges of R and taking a value within the intersection of the multiple value ranges of R as a definite solution of the rotation radius R;

[0010] S5, using the determined R value, repeat S3 to calculate the left and right wheel driving speeds of each wheel train;

[0011] The control law based on the deviation is divided into the following steps:

[0012] S6, determining the position deviation and posture deviation of the AGV body center relative to the set trajectory line;

[0013] S7, calculating the vehicle center velocity and angular velocity according to the position deviation and posture deviation;

[0014] S8, calculating the driving speed of the left and right wheels in each wheel system according to the center speed and angular velocity of the vehicle body;

[0015] The left and right wheel drive speeds to be used are determined based on the hybrid control law.

[0016] Further, the S1 is: obtaining the position deviation x of the center of the vehicle body relative to the trajectory line according to the visual sensor or magnetic sensor installed on the vehicle chassis c , with attitude deviation δ c .

[0017] Further, the S2 is:

[0018] According to the position deviation x c Determine the vehicle center velocity v c , v c The direction is parallel to the x-axis of the vehicle and points to the trajectory line, and the magnitude is |v c |=v d +k1·x c , where the root v d is the tracking speed specified by the upper scheduling system, k1 is the speed control PID parameter; according to the vehicle posture deviation δ c Determine the vehicle angular velocity ω, in the direction such that δ c Decreasing direction.

[0019] Further, the S3 is:

[0020] Assume that the rotation radius of the vehicle body is R, R is an unknown quantity and is related to the center velocity and angular velocity of the vehicle body by the formula: R = |v c | / ω; Let the center velocity vector of each gear train be v wheel_i =[v ix v iy 0], use v wheel_i =f i (R) = v c +ω·(z×P i ) can be used to solve the expression of the center speed of each gear train, where z =

[001] T , P i is the position of each wheel rotation center in the vehicle body coordinate system, P i It can be obtained according to the mechanical structure of the vehicle body, i is the wheel train number, v ix is the component of the center velocity vector of wheel train i on the x-axis of the vehicle coordinate system, v iyis the component of the center velocity vector of wheel train i on the y-axis of the vehicle coordinate system; use θ i =tan -1 (v iy / v ix ) indicates that each gear train should have a swing angle; use v iL =|v wheel_i |-d w (θ i / t)、v iR =|v wheel_i |+d w (θ i / t) indicates the driving speed of the left and right wheels in each wheel system, where t is the control period, d w It is half the distance between the two wheels of the differential.

[0021] Further, the S4 is:

[0022] Assume the maximum driving speed is v max , solve the inequality |v iL |≤v max 、|v iR |≤v max There are multiple sets of R value ranges; a value is taken in the intersection of the multiple R value ranges as the definite solution R of the rotation radius R c .

[0023] Further, the S5 is:

[0024] Let R = R c Repeat step S3 to obtain the speed v of the left and right wheels of each gear train. iL 、v iR

[0025] Further, the S6 is:

[0026] The position deviation x of the center of the vehicle relative to the trajectory line is obtained by the visual sensor or magnetic sensor installed on the vehicle chassis. c , with attitude deviation δ c .

[0027] Further, the S7 is:

[0028] According to the position deviation x c Determine the vehicle center velocity v c , v c The direction points to the trajectory line and forms a 45° angle with the vehicle's x-axis, with a magnitude of |v c |=v d +k1·x c , where v d is the tracking speed specified by the upper scheduling system, k1 is the speed control PID parameter; according to the vehicle posture deviation δc Determine the vehicle's angular velocity ω, which is |ω|=k2·δ c , the direction is such that the vehicle body δ c Decreasing direction, where k2 is the angle control PID parameter.

[0029] Further, the S8 is:

[0030] Using v wheel_i =[v ix v iy 0]=v c +ω·(z×P i ) can be used to solve the center velocity vector of each gear train, where z =

[001] T , P i is the position of the rotation center of each wheel in the vehicle coordinate system, which depends on the vehicle mechanical structure, i is the wheel train number, v ix is the component of the center velocity vector of wheel train i on the x-axis of the vehicle coordinate system, v iy is the component of the center velocity vector of wheel train i on the y-axis of the vehicle coordinate system; use θ i =tan -1 (v iy / v ix ) can solve the swing angle of each gear train; use v iL =|v wheel_i |-d w (θ i / t)、v iR =|v wheel_i |+d w (θ i / t) to solve the driving speed of the left and right wheels in each wheel system, where t is the control period, d w It is half the distance between the two wheels of the differential.

[0031] Furthermore, the hybrid control law is: if each v iL 、v iR All conditions are met|v iL |≤v max 、|v iR |≤v max The driving speeds of the left and right wheels of each wheel train are obtained from the control law based on the deviation, and flag=1 is recorded; if it is not satisfied, the driving speeds of the left and right wheels of each wheel train are obtained from the control law based on the driving capacity, and flag=2 is recorded.

[0032] The present application can effectively improve the tracking accuracy; fully consider the driving capability of the wheel train and limit the driving speed of each wheel, so that the AGV tracking process is smoother; it can avoid the problem of insufficient system control bandwidth caused by frequent forward and reverse rotation of the wheel train; it can quickly eliminate errors and achieve a stable tracking state; the number and position of wheel trains used by the AGV are not restricted, and the scalability and applicability are high. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 This is a schematic diagram of the dual-wheel differential wheel structure used in this application;

[0034] Figure 2 This is a schematic diagram of the tracking state. DETAILED DESCRIPTION

[0035] The present application provides a multi-differential wheel cooperative control method, which combines two control laws, namely a control law based on driving capability and a control law based on deviation;

[0036] The control law based on driving capability is divided into the following steps:

[0037] S1, determine the position deviation and posture deviation of the AGV body center relative to the set trajectory line;

[0038] S2, calculating the center velocity and angular velocity of the vehicle body according to the position deviation and the posture deviation;

[0039] S3, calculating the rotation radius R of the vehicle body according to the center velocity and angular velocity of the vehicle body, and using R to represent the speed of the left and right wheels in each wheel train;

[0040] S4, under the limitation of the differential wheel driving capability, solving the inequality about the left and right wheel speeds to obtain multiple value ranges of R and taking a value within the intersection of the multiple value ranges of R as a definite solution of the rotation radius R;

[0041] S5, using the determined R value, repeat S3 to calculate the left and right wheel driving speeds of each wheel train;

[0042] The control law based on the deviation is divided into the following steps:

[0043] S6, determining the position deviation and posture deviation of the AGV body center relative to the set trajectory line;

[0044] S7, calculating the vehicle center velocity and angular velocity according to the position deviation and posture deviation;

[0045] S8, calculating the driving speed of the left and right wheels in each wheel system according to the center speed and angular velocity of the vehicle body;

[0046] The left and right wheel drive speeds to be used are determined based on the hybrid control law.

[0047] Further, the S1 is: obtaining the position deviation x of the center of the vehicle body relative to the trajectory line according to the visual sensor or magnetic sensor installed on the vehicle chassis c , with attitude deviation δ c .

[0048] Further, the S2 is:

[0049] According to the position deviation x c Determine the vehicle center velocity v c , v c The direction is parallel to the x-axis of the vehicle and points to the trajectory line, and the magnitude is |v c |=v d +k1·x c , where the root v d is the tracking speed specified by the upper scheduling system, k1 is the speed control PID parameter; according to the vehicle posture deviation δ c Determine the vehicle angular velocity ω, in the direction such that δ c Decreasing direction.

[0050] Further, the S3 is:

[0051] Assume that the rotation radius of the vehicle body is R, R is an unknown quantity and is related to the center velocity and angular velocity of the vehicle body by the formula: R = |v c | / ω; Let the center velocity vector of each gear train be v wheel_i =[v ix v iy 0], use v wheel_i =f i (R) = v c +ω·(z×P i ) can be used to solve the expression of the center speed of each gear train, where z = [0 0 1] T , P i is the position of each wheel rotation center in the vehicle body coordinate system, P i It can be obtained according to the mechanical structure of the vehicle body, i is the wheel train number, v ix is the component of the center velocity vector of wheel train i on the x-axis of the vehicle coordinate system, v iy is the component of the center velocity vector of wheel train i on the y-axis of the vehicle coordinate system; use θ i =tan -1 (v iy / v ix ) indicates that each gear train should have a swing angle; use v iL =|v wheel_i |-d w (θ i / t)、v iR =|v wheel_i |+d w (θ i / t) indicates the driving speed of the left and right wheels in each wheel system, where t is the control period, d w It is half the distance between the two wheels of the differential.

[0052] Further, the S4 is:

[0053] Assume the maximum driving speed is v max , solve the inequality |v iL |≤v max 、|v iR |≤v max There are multiple sets of R value ranges; a value is taken in the intersection of the multiple R value ranges as the definite solution R of the rotation radius R c .

[0054] Further, the S5 is:

[0055] Let R = R c Repeat step S3 to obtain the speed v of the left and right wheels of each gear train. iL 、v iR

[0056] Further, the S6 is:

[0057] The position deviation x of the center of the vehicle relative to the trajectory line is obtained by the visual sensor or magnetic sensor installed on the vehicle chassis. c , with attitude deviation δ c .

[0058] Further, the S7 is:

[0059] According to the position deviation x c Determine the vehicle center velocity v c , v c The direction points to the trajectory line and forms a 45° angle with the vehicle's x-axis, with a magnitude of |v c |=v d +k1·x c , where v d is the tracking speed specified by the upper scheduling system, k1 is the speed control PID parameter; according to the vehicle posture deviation δ c Determine the vehicle's angular velocity ω, which is |ω|=k2·δ c , the direction is such that the vehicle body δ c Decreasing direction, where k2 is the angle control PID parameter.

[0060] Further, the S8 is:

[0061] Using v wheel_i =[v ix v iy 0]=v c +ω·(z×Pi ) can be used to solve the center velocity vector of each gear train, where z =

[001] T , P i is the position of the rotation center of each wheel in the vehicle coordinate system, which depends on the vehicle mechanical structure, i is the wheel train number, v ix is the component of the center velocity vector of wheel train i on the x-axis of the vehicle coordinate system, v iy is the component of the center velocity vector of wheel train i on the y-axis of the vehicle coordinate system; use θ i =tan -1 (v iy / v ix ) can solve the swing angle of each gear train; use v iL =|v wheel_i |-d w (θ i / t)、v iR =|v wheel_i |+d w (θ i / t) to solve the driving speed of the left and right wheels in each wheel system, where t is the control period, d w It is half the distance between the two wheels of the differential.

[0062] Furthermore, the hybrid control law is: if each v iL 、v iR All conditions are met|v iL |≤v max 、|v iR |≤v max The driving speeds of the left and right wheels of each wheel train are obtained from the control law based on the deviation, and flag=1 is recorded; if it is not satisfied, the driving speeds of the left and right wheels of each wheel train are obtained from the control law based on the driving capacity, and flag=2 is recorded.

[0063] Example 1

[0064] 1) If Figure 2 , the center of the vehicle has a position deviation x relative to the trajectory line c , with attitude deviation δ c When the specified distance deviation is on the left side of the track line x c <0, when x is on the right c >0; When the vehicle body posture is in a left-leaning state relative to the trajectory line, δ c <0; rightward stateδ c >0.

[0065] 2) Determine the angular velocity based on the vehicle posture deviation, ω = k2·δ c .

[0066] 3) Use the deviation-based method to distribute the gear train speed. Let flag = 1. Let the vehicle center speed vc The direction points to the trajectory line, and the angle between the velocity direction and the vehicle body is β; determine the center velocity |v c |=v d +k·|x c |, where v d is the command tracking speed.

[0067] 4) Calculate the center speed of each wheel, v ic =v c +ω×P i , where i represents the gear train number, v ic is the center velocity vector of each wheel, P i is the position vector of each wheel center in the vehicle coordinate system.

[0068] 5) Target swing angle of each round Combined with the current center speed of each gear train And swing angle The angular velocity of each wheel ω can be obtained i .

[0069] 6) Calculate the left and right wheel speeds of each wheel: Where d is half of the distance between the centers of the two wheels in each gear train.

[0070] 7) Determine whether the following conditions are met: 0≤v iL ≤v max and 0≤v iR ≤v max . Where v max It is the upper limit of the gear train driving capacity.

[0071] 8) If not satisfied, set flag = 2. Set the center velocity direction along the x-axis of the vehicle coordinate system, and the magnitude is |v c |=v d ; Assume the radius of rotation is R, then the angular velocity ω=|v c | / R.

[0072] 9) Similar to the previous method, use formula v ic =v c +ω×P, and Multiple relational expressions containing unknown variables R can be obtained, and the rotation radius can be determined under the limit of driving speed, and then re-substituted into the above formula to calculate v iL 、v iR .

[0073] 10) If flag = 2 and the direction of the vehicle's angular velocity is opposite to that of the previous moment, in order to ensure tracking efficiency and satisfy the rigid body kinematic relationship, the wheel system should be reset to zero (i.e., the swing angles of each wheel are controlled to be 0) before subsequent calculations.

[0074] The above embodiments are only for illustrating the technical idea of ​​the present invention, and cannot be used to limit the protection scope of the present invention. Any changes made on the basis of the technical solution in accordance with the technical idea proposed by the present invention shall fall within the protection scope of the present invention.

Claims

1. A multi-differential wheel coordinated control method, characterized in that: This method combines two control laws, one based on driving capability and the other based on deviation. The control law based on driving capability is divided into the following steps: S1, determine the position deviation and posture deviation of the AGV body center relative to the set trajectory line; S2, calculating the center velocity and angular velocity of the vehicle body according to the position deviation and the posture deviation; S3, calculating the rotation radius R of the vehicle body according to the center speed and angular velocity of the vehicle body, and using R to represent the speed of the left and right wheels in each wheel train; S3 is: Assume that the rotation radius of the vehicle body is R, R is an unknown quantity and has a relationship with the center velocity and angular velocity of the vehicle body: R = v c / ω; let the center velocity vector of each gear train be v wheel_i =[v ix v iy 0], use v wheel_i =f i (R) = v c +ω·(z×P i ) can be used to solve the expression of the center speed of each gear train, where z = [001] T , P i is the position of each wheel rotation center in the vehicle body coordinate system, P i It can be obtained according to the mechanical structure of the vehicle body, i is the wheel train number, v ix is the component of the center velocity vector of wheel train i on the x-axis of the vehicle coordinate system, v iy is the component of the center velocity vector of wheel train i on the y-axis of the vehicle coordinate system; use θ i =tan -1 (v iy / v ix ) indicates that each gear train should have a swing angle; use v iL =v wheel_i -d w (θ i / t)、v iR =v wheel_i +d w (θ i / t) represents the left and right wheel speeds in each wheel system, with the driving speed being the vehicle body, and the speed at the center being t; control cycle, d w is half of the distance between the two wheels of the differential wheel, ω is the vehicle body S4, under the limitation of the differential wheel driving capacity, solve the inequality about the left and right wheel speeds to obtain multiple ranges of R values ​​and take the value in the intersection of multiple ranges of R values ​​as the definite solution of the rotation radius R; S5, using the determined R value, repeat S3 to calculate the left and right wheel driving speeds of each wheel train; The control law based on the deviation is divided into the following steps: S6, determining the position deviation and posture deviation of the AGV body center relative to the set trajectory line; S7, calculating the vehicle center velocity and angular velocity according to the position deviation and posture deviation; S8, calculating the driving speed of the left and right wheels in each wheel system according to the center speed and angular velocity of the vehicle body; The left and right wheel driving speeds to be used are determined according to the hybrid control law; the hybrid control law is: if each v iL 、v iR All satisfy the condition v iL ≤v max 、|v iR ≤v max The driving speeds of the left and right wheels of each wheel train are obtained from the control law based on the deviation, and flag=1 is recorded; if it is not satisfied, the driving speeds of the left and right wheels of each wheel train are obtained from the control law based on the driving capacity, and flag=2 is recorded.

2. The multi-differential wheel coordinated control method according to claim 1, characterized in that: S1 is: obtaining the position deviation x of the center of the vehicle relative to the trajectory line according to the visual sensor or magnetic sensor installed on the vehicle chassis c , with attitude deviation δ c .

3. The multi-differential wheel coordinated control method according to claim 1, characterized in that: The S2 is: according to the position deviation x c Determine the vehicle center velocity v c , v c The direction is parallel to the x-axis of the vehicle and points to the trajectory line, and the magnitude is |v c |=v d +k1·x c , where v d is the tracking speed specified by the upper scheduling system, k1 is the speed control PID parameter; according to the vehicle posture deviation δ c Determine the vehicle angular velocity ω, in the direction such that δ c Decreasing direction.

4. The multi-differential wheel coordinated control method according to claim 1, characterized in that: S4 is: Assume the maximum driving speed is v max , solve the inequality |v iL |≤v max 、|v iR |≤v max There are multiple sets of R value ranges; a value is taken in the intersection of the multiple R value ranges as the definite solution R of the rotation radius R c .

5. The multi-differential wheel coordinated control method according to claim 1, characterized in that: The S5 is: Let R = R c Repeat step S3 to obtain the speed v of the left and right wheels of each gear train. iL 、v iR .

6. The multi-differential wheel coordinated control method according to claim 1, characterized in that: S6 is: obtaining the position deviation x of the center of the vehicle body relative to the trajectory line according to the visual sensor or magnetic sensor installed on the vehicle body chassis c , with attitude deviation δ c .

7. The multi-differential wheel coordinated control method according to claim 1, characterized in that: The S7 is: according to the position deviation x c Determine the vehicle center velocity v c , v c The direction points to the trajectory line and forms a 45° angle with the vehicle's x-axis, with a magnitude of |v c |=v d +k1·x c , where v d is the tracking speed specified by the upper scheduling system, k1 is the speed control PID parameter; according to the vehicle posture deviation δ c Determine the vehicle's angular velocity ω, which is |ω|=k2·δ c , the direction is such that the vehicle body δ c Decrease direction, where k2 is the angle control PID parameter.

8. The multi-differential wheel coordinated control method according to claim 1, characterized in that: The S8 is: using v wheel_i =[v ix v iy 0]=v c +ω·(z×P i ) can be used to solve the center velocity vector of each gear train, where z = [001] T , P i is the position of the rotation center of each wheel in the vehicle coordinate system, which depends on the vehicle mechanical structure, i is the wheel train number, v ix is the component of the center velocity vector of wheel train i on the x-axis of the vehicle coordinate system, v iy is the component of the center velocity vector of wheel train i on the y-axis of the vehicle coordinate system; use θ i =tan -1 (v iy / v ix ) can solve the swing angle of each gear train; use v iL =|v wheel_i |-d w (θ i / t)、v iR =|v wheel_i |+d w (θ i / t) to solve the driving speed of the left and right wheels in each wheel system, where t is the control period, d w It is half the distance between the two wheels of the differential.

Images