Auxiliary driving control method and system for omnidirectional forklift

Abstract

The invention discloses an auxiliary driving control method and system for an omni-directional forklift, and mainly relates to the technical field of automatic control. Comprising the following steps: establishing a kinematics and dynamics model of the forklift; an MPC controller is designed according to the dynamics and kinematics model; collecting motion data when the forklift works; constructing an automatic parking algorithm and an automatic insertion algorithm for assisting driving; and real-time control over the position of the forklift is completed according to an automatic parking algorithm and an automatic inserting and taking algorithm. The system has the beneficial effects that the problem of human resources is solved, the intervention of human factors is overcome, and the efficiency of picking and placing goods is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic control, and specifically to an auxiliary driving control method and system for an omnidirectional forklift. Background Art

[0002] During railway transportation, goods need to be frequently picked up and placed, and forklifts play a very important role. A forklift consists of a vehicle body and attachments. The vehicle body is responsible for motion control, and the attachments are used for picking up goods. At the railway transportation site, the way to pick up goods is for the driver to manually drive the forklift to pick up the goods.

[0003] Since the forklift driver needs to control both the vehicle movement and the tooth insertion movement during work, the accuracy and efficiency of picking up goods often depend on the proficiency of the operator. In the railway transportation scenario, some areas for picking up / putting down goods have limited space, and the space left for the forklift on both sides is very narrow. During the process of placing goods, the stack is too high, which blocks the driver's line of sight. This situation leads to a decrease in the efficiency of the driver when driving the forklift to pick up / place goods, and even the vehicle body rubs against the walls on both sides, unable to meet the requirements of high automation, high efficiency, and high safety. The reasons for these problems and disadvantages include:

[0004] 1. The driving wheels of traditional forklifts are at the front side, and the rear wheels use Ackermann steering, with a single motion mode and poor maneuverability in narrow spaces;

[0005] 2. After the operator performs long-term goods picking up / placement, visual errors may occur;

[0006] 3. When traditional forklifts pick up / place goods in narrow environments, the driver needs to accurately judge the position of the pallet and have a strong sense of the vehicle, but this will consume a lot of the driver's energy and greatly reduce their work efficiency.

[0007] Therefore, there is an urgent need for an auxiliary driving control method and system for an omnidirectional forklift for railway gondola car loading and unloading to solve the above problems. Summary of the Invention

[0008] The purpose of the present invention is to provide an auxiliary driving control method and system for an omnidirectional forklift, which not only solves the human resource problem but also overcomes the intervention of human factors and improves the efficiency of picking up and placing goods.

[0009] To achieve the above object, the present invention is realized through the following technical solutions:

[0010] On the one hand, an auxiliary driving control method for an omnidirectional forklift is provided, including the following steps:

[0011] Establish the kinematic and dynamic models of the forklift;

[0012] Design an MPC controller based on the dynamic and kinematic models;

[0013] Collect the motion data of the forklift during operation;

[0014] Construct the automatic docking algorithm and automatic insertion and extraction algorithm for assisted driving;

[0015] Complete the real-time control of the forklift position according to the automatic docking algorithm and automatic insertion and extraction algorithm.

[0016] Preferably, establish the kinematic model of the forklift, including:

[0017] Conduct kinematic modeling for the four-steering-wheel chassis, and obtain the motion state of the forklift system by solving the motion equation of the forklift chassis. The motion state includes, but is not limited to: the position and attitude of the forklift, the speed and acceleration of the forklift;

[0018] Establish the dynamic model of the forklift, including:

[0019] Derive the kinetic energy and potential energy functions of the forklift chassis according to the Lagrange equation to obtain the motion equation of the forklift;

[0020] Design its motion mode through the geometric relationship of the four-steering-wheel chassis structure of the forklift. The motion mode includes: linear motion, in-situ rotation motion, lateral motion, rear-wheel Ackermann motion, and four-wheel Ackermann motion modes.

[0021] Preferably, the construction of the automatic docking algorithm includes the following steps:

[0022] Calculate the real-time distance between the wall and the vehicle body according to the collected forklift motion data, and set a threshold to determine the distance range;

[0023] Set a fuzzy PID controller and optimize the parameters of the fuzzy PID through a genetic algorithm;

[0024] Complete the movement control of the vehicle body towards the wall by judging different motion modes.

[0025] Preferably, the construction of the automatic insertion and extraction algorithm includes the following steps:

[0026] Design an MPC controller based on the dynamic model and kinematic model;

[0027] Calculate the yaw angle of the forklift according to the collected forklift motion data, and input the obtained yaw angle into the controller to output the steering angle and speed signals of each steering wheel;

[0028] Judge whether the forklift teeth are completely immersed in the target object.

[0029] Preferably, in the step S31, setting an MPC controller specifically includes:

[0030]

[0031] Among them, F xi (i = fl, fr, rl, rr) is the driving force in the longitudinal axis direction generated by the sliding friction between the tire and the ground, and F yi (i = fl, fr, rl, rr) is the resistance in the transverse axis direction generated by the sliding friction between the tire and the ground, (x ci , y ci ), (x cj , y cj ) are the position coordinates of the four steering wheels in the vehicle body coordinate system, m is the vehicle body mass, I is the moment of inertia, δ is the yaw angle of the vehicle body center, and F dissp is the resultant force of the resistance suffered by the vehicle in the longitudinal direction.

[0032] Preferably, outputting the steering angle and speed signals of each steering wheel, the calculation process includes:

[0033]

[0034]

[0035] Among them, δ ri , δ rr are the steering wheel deflection angles of the left rear wheel and the right rear wheel of the forklift respectively; v fl , v fr , v rl , v rr are the steering wheel speeds of the left front wheel, the right front wheel, the left rear wheel and the right rear wheel respectively; ω fl , ω fr , ω rl , ω rr , ω c are the rotational angular velocities of the left front wheel, the right front wheel, the left rear wheel, the right rear wheel and the vehicle body center respectively; and are the position change rates of the vehicle body in the transverse axis and the longitudinal axis; and are the position change rates of the vehicle body in the transverse axis and the longitudinal axis in the world coordinate system; is the included angle between the vehicle body coordinate system and the world coordinate system; is the angular change rate of the vehicle body in the world coordinate system; L and D are the wheelbase and the track width of the vehicle body respectively.

[0036] On the other hand, an auxiliary driving control system for an omnidirectional forklift is provided, including:

[0037] The data acquisition module is used for: acquiring the motion data of the forklift during operation;

[0038] The control module is used for: establishing the kinematic and dynamic models of the forklift based on the motion data of the forklift; constructing the automatic docking algorithm and automatic insertion and extraction algorithm for assisted driving based on the established kinematic and dynamic models;

[0039] The algorithm module is used for: completing the real-time control of the forklift according to the automatic docking algorithm and automatic insertion and extraction algorithm.

[0040] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0041] 1. The accuracy of automatic docking is improved: the distance between the vehicle body and the wall is accurately positioned through a laser distance sensor, and this distance value is transmitted to a controller with fuzzy rule parameters optimized by a genetic algorithm, enabling the forklift to stop at the target position more quickly and accurately;

[0042] 2. The accuracy and efficiency of automatic insertion and extraction are improved: the real-time heading angle of the vehicle is calculated based on the data measured by the laser range finder, and this heading angle is transmitted into the MPC controller, which can more accurately control the forklift to drive straight to the pallet. The automatic control logic of the hydraulic system enables the driver not to operate the insertion teeth in this mode, making the whole process more efficient and accurate.

[0043] This assisted driving control method improves the accuracy and repeatability of the forklift in inserting and extracting goods in a narrow space, saves time and labor costs, and improves the transportation efficiency of goods. Brief Description of the Drawings

[0044] Figure 1 is the flow chart of the control method of the present invention;

[0045] Figure 2 is the schematic diagram of the kinematic model of the present invention;

[0046] Figure 3 is the schematic diagram of the dynamic model of the present invention;

[0047] Figure 4 is the schematic diagram of the special motion mode of the present invention;

[0048] Figure 5 is the schematic diagram of the working scenario of the present invention;

[0049] Figure 6 is the flow chart of the automatic docking of the present invention;

[0050] Figure 7 is the flow chart of the automatic insertion and extraction of the present invention;

[0051] Figure 8 is the flow chart of the control logic of the hydraulic system of the present invention;

[0052] Figure 9 It is a schematic diagram of the system structure of the present invention. Detailed implementation manners

[0053] The present invention will be further described below in conjunction with specific embodiments. It should be understood that these embodiments are only used to illustrate the present invention and not to limit the scope of the present invention. In addition, it should be understood that after reading the content taught by the present invention, those skilled in the art can make various changes or modifications to the present invention, and these equivalent forms also fall within the scope defined by this application.

[0054] In the present invention, terms such as "upper", "lower", "left", "right", "front", "rear", "vertical", "horizontal", "side", "bottom", etc. indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings. They are only relationship terms determined for the convenience of describing the structural relationship of each component or element of the present invention, and do not specifically refer to any component or element in the present invention, and should not be construed as a limitation to the present invention.

[0055] In the present invention, terms such as "fixed connection", "connected", "connected" should be understood in a broad sense, which may mean a fixed connection, an integral connection or a detachable connection; it may be directly connected or indirectly connected through an intermediate medium. For those skilled in relevant scientific research or technology in this field, the specific meanings of the above terms in the present invention can be determined according to specific circumstances, and should not be construed as a limitation to the present invention.

[0056] Embodiment:

[0057] As Figure 1 shown, this embodiment provides an auxiliary driving control method for an omnidirectional forklift, including the following steps:

[0058] Establish the kinematic and dynamic models of the forklift;

[0059] Design an MPC controller according to the dynamic and kinematic models;

[0060] Collect the motion data of the forklift during operation;

[0061] Construct an automatic docking algorithm and an automatic picking and placing algorithm for auxiliary driving;

[0062] Complete the real-time control of the forklift position according to the automatic docking algorithm and the automatic picking and placing algorithm.

[0063] Before designing the algorithm, it is necessary to perform kinematic and dynamic analysis on the forklift to ensure the stability of the forklift during movement and to design the motion mode of the four-steering-wheel forklift. The following is the content of the model establishment and motion mode design this time:

[0064] 1) As Figure 2As shown in the figure, the kinematic model: Carry out kinematic modeling for the four-steering-wheel chassis. By solving the motion equation of the forklift chassis, the motion state of the forklift system is obtained, including the position and attitude of the forklift, as well as information such as the speed and acceleration of the forklift;

[0065] 2) As Figure 3 shown in the figure, the dynamic model: According to the Lagrange equation, the kinetic energy and potential energy functions of the forklift chassis are differentiated to obtain the motion equation of the forklift;

[0066] 3) Design a variety of motion modes through the geometric relationship of the four-steering-wheel chassis structure of the forklift: straight-line motion, in-situ rotation motion, lateral motion, rear-wheel Ackermann motion and four-wheel Ackermann motion modes, specifically as Figure 4 shown in the figure.

[0067] After establishing the model and the special motion mode model, simulate the on-site working scenario of the forklift, as Figure 5 shown in the figure.

[0068] The design of the automatic docking algorithm is the prerequisite for the smooth progress of the forklift's automatic insertion and extraction of goods, as Figure 6 shown in the figure: First, design a sensor data acquisition algorithm. Calculate the real-time distance between the wall and the vehicle body through the collected data. Through this distance value and the preset speed value of the vehicle body, use the genetic algorithm to optimize the PID parameters, so that the forklift can reach the preset value from the wall in a shorter time. The key steps in the design of the automatic docking algorithm are as follows:

[0069] 1) Collect data from the sensors to calculate the real-time distance between the wall and the vehicle body, and determine the controllable distance range by setting thresholds (minimum value 0 and maximum value 3000);

[0070] 2) Design a fuzzy PID controller and optimize the fuzzy PID parameters through the genetic algorithm. The genetic algorithm is a global optimization algorithm that simulates the natural evolution process. Through operations such as selection, crossover, and mutation, it gradually approaches the optimal solution of the problem. Through this method, the performance of the control system can be improved;

[0071] 3) Design the control logic of the hydraulic system, as Figure 8 shown in the figure;

[0072] 4) The control algorithm selects different motion modes through the judgment conditions to control the vehicle body to drive towards the wall to the preset value.

[0073] The design of the automatic insertion and extraction algorithm is the key and core to complete the automatic insertion and extraction of goods, as Figure 7 shown in the figure: After the automatic docking is completed, through the data output by the laser range finder sensor, control the forklift to stably insert the insertion teeth completely into the tray holes. After inserting into the tray holes, the hydraulic system performs a series of actions to lift the goods. The key steps in the design of the automatic insertion and extraction algorithm are as follows:

[0074] 1) Design the MPC controller based on the dynamic model, specifically as follows:

[0075]

[0076] Among them, F xi (i = fl, fr, rl, rr) is the driving force in the longitudinal direction generated by the sliding friction between the tire and the ground, and F yi (i = fl, fr, rl, rr) is the resistance in the transverse direction generated by the sliding friction between the tire and the ground. (x ci , y ci ), (x cj , y cj ) are the position coordinates of the four steering wheels in the vehicle body coordinate system, m is the vehicle body mass, I is the moment of inertia, δ is the yaw angle of the vehicle body center, and F dissp is the resultant force of the resistance suffered by the vehicle in the longitudinal direction;

[0077] 2) Calculate the yaw angle of the forklift through the double laser range sensors on one side of the vehicle body, transmit the yaw angle into the controller, and then control the steering wheels by the steering angle and speed signals output by the controller. The calculation process includes:

[0078]

[0079]

[0080] Among them, δ ri , δ rr are the steering wheel deflection angles of the left rear wheel and the right rear wheel of the forklift respectively; v fl , v fr , v rl , v rr are the steering wheel speeds of the left front wheel, the right front wheel, the left rear wheel and the right rear wheel respectively; ω fl , ω fr , ω rl , ω rr , ω c are the rotational angular velocities of the left front wheel, the right front wheel, the left rear wheel, the right rear wheel and the vehicle body center respectively; and are the position change rates of the vehicle body in the transverse and longitudinal axes; and are the position change rates of the vehicle body in the transverse and longitudinal axes in the world coordinate system; is the angle between the vehicle body coordinate system and the world coordinate system; is the angle change rate of the vehicle body in the world coordinate system; L and D are the wheelbase and track width of the vehicle body respectively;

[0081] 3) Whether the gear shaper is completely inserted into the pallet hole can be determined by the sensor signal on the push plate (at the root of the gear shaper). After this signal is triggered, the controller controls the hydraulic system to complete a series of actions, thereby realizing the automatic picking and placing of goods.

[0082] As Figure 9 shown, this embodiment also provides an auxiliary driving control system for an omnidirectional forklift, including:

[0083] A data acquisition module (including a laser range finder, a motor encoder, a limit switch, etc.), which is used to: acquire the motion data of the forklift during operation;

[0084] A control module, which is used to: establish a kinematic and dynamic model of the forklift according to the motion data of the forklift; design an MPC controller and a fuzzy PID controller according to the established kinematic and dynamic model;

[0085] An algorithm module, which is used to: input the forklift motion data into the automatic docking algorithm and the automatic picking and placing algorithm to complete the real-time control of the forklift.

[0086] The above is a specific description of the preferred embodiment of the present invention, but the present invention is not limited to the described embodiment. Those skilled in the art can also make various equivalent deformations or substitutions without departing from the spirit of the present invention, and these equivalent deformations or substitutions are all included in the scope defined by the claims of this application.

Claims

1. An assisted driving control method for an omnidirectional forklift, characterized in that: The following steps are involved: Establish kinematic and dynamic models of forklifts; Design MPC controller based on dynamic and kinematic models; Collect the motion data of the forklift when it is working; Build automatic docking and automatic insertion algorithms for assisted driving; Based on the automatic docking algorithm and automatic insertion algorithm, the real-time control of the forklift position is completed.

2. The assisted driving control method of an omnidirectional forklift according to claim 1, characterized in that: Establish the kinematic model of the forklift, including: Kinematic modeling is performed for the four-steering wheel chassis, and the motion state of the forklift system is obtained by solving the motion equation of the forklift chassis, and the motion state includes but is not limited to: the position, posture, speed and acceleration of the forklift; Establish a dynamic model of the forklift, including: According to the Lagrange equation, the kinetic energy and potential energy functions of the forklift chassis are differentiated to obtain the motion equation of the forklift; The motion mode of the forklift is designed by the geometric relationship of the four-wheel chassis structure, and the motion mode includes: linear motion, in-situ rotation motion, lateral motion, rear wheel Ackerman motion and four-wheel Ackerman motion mode.

3. The assisted driving control method of an omnidirectional forklift according to claim 1, characterized in that: The construction of the automatic docking algorithm includes the following steps: According to the collected forklift movement data, the real-time distance between the wall and the vehicle body is calculated, and a threshold is set to determine the distance range; Set up a fuzzy PID controller and optimize the parameters of the fuzzy PID through genetic algorithm; By determining different motion modes, the movement of the vehicle toward the wall is controlled.

4. The assisted driving control method of an omnidirectional forklift according to claim 1, characterized in that: The construction of the automatic insertion algorithm includes the following steps: Design MPC controller based on dynamic model and kinematic model; According to the collected forklift motion data, the yaw angle of the forklift is calculated, and the obtained yaw angle is input into the controller, and the steering angle and speed signal of each steering wheel are output; Determine whether the tines on the forklift are fully immersed in the target object.

5. The assisted driving control method of an omnidirectional forklift according to claim 4, characterized in that ,, set up the MPC controller, specifically: Among them, F xi (i = fl, fr, rl, rr) is the driving force along the longitudinal axis generated by the sliding friction between the tire and the ground, F yi (i = fl, fr, rl, rr) is the resistance along the horizontal axis generated by the sliding friction between the tire and the ground, (x ci ,y ci )、(x cj ,y cj ) is the position coordinate of the four steering wheels in the vehicle coordinate system, m is the vehicle mass, I is the moment of inertia, δ is the yaw angle of the vehicle center, F dissp It is the resultant force of the resistance acting on the vehicle in the longitudinal direction.

6. The assisted driving control method of an omnidirectional forklift according to claim 4, characterized in that: Output the steering angle and speed signal of each steering wheel. The calculation process includes: Among them, δ ri , δ rr are the steering wheel deflection angles of the left and right rear wheels of the forklift respectively; v fl 、v fr 、v rl 、v rr are the steering wheel speeds of the left front wheel, right front wheel, left rear wheel and right rear wheel respectively; ω fl ,ω fr ,ω rl ,ω rr ,ω c are the rotational angular velocities of the left front wheel, right front wheel, left rear wheel, right rear wheel and the center of the vehicle body respectively; and is the rate of change of the position of the vehicle body on the horizontal and vertical axes; and is the position change rate of the horizontal and vertical axes of the vehicle body in the world coordinate system; is the angle between the vehicle coordinate system and the world coordinate system; is the angular change rate of the vehicle body in the world coordinate system; L and D are the wheelbase and track of the vehicle body respectively.

7. An assisted driving control system for an omnidirectional forklift, characterized in that: include: The data acquisition module is used to: collect the motion data of the forklift when it is working; The control module is used to: establish a kinematic and dynamic model of the forklift according to the motion data of the forklift; According to the established kinematic and dynamic models, automatic parking and automatic insertion algorithms for assisted driving are constructed; The algorithm module is used to complete the real-time control of the forklift according to the automatic docking algorithm and the automatic insertion algorithm.

Images