CN115755919A - Chemical inspection vehicle trajectory tracking method and system - Google Patents
Chemical inspection vehicle trajectory tracking method and system Download PDFInfo
- Publication number
- CN115755919A CN115755919A CN202211512957.5A CN202211512957A CN115755919A CN 115755919 A CN115755919 A CN 115755919A CN 202211512957 A CN202211512957 A CN 202211512957A CN 115755919 A CN115755919 A CN 115755919A
- Authority
- CN
- China
- Prior art keywords
- inspection vehicle
- vehicle
- longitudinal
- transverse
- inspection
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000007689 inspection Methods 0.000 title claims abstract description 199
- 239000000126 substance Substances 0.000 title claims abstract description 45
- 238000000034 method Methods 0.000 title claims abstract description 28
- NUFBIAUZAMHTSP-UHFFFAOYSA-N 3-(n-morpholino)-2-hydroxypropanesulfonic acid Chemical compound OS(=O)(=O)CC(O)CN1CCOCC1 NUFBIAUZAMHTSP-UHFFFAOYSA-N 0.000 claims abstract description 27
- 230000000694 effects Effects 0.000 claims abstract description 11
- 238000005457 optimization Methods 0.000 claims description 26
- 230000004927 fusion Effects 0.000 claims description 18
- 238000013528 artificial neural network Methods 0.000 claims description 13
- 239000002245 particle Substances 0.000 claims description 12
- 238000001914 filtration Methods 0.000 claims description 11
- 230000006870 function Effects 0.000 claims description 11
- 230000003993 interaction Effects 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 9
- 230000007613 environmental effect Effects 0.000 claims description 8
- 239000011159 matrix material Substances 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 7
- 238000004088 simulation Methods 0.000 claims description 6
- 238000010586 diagram Methods 0.000 claims description 5
- 230000002708 enhancing effect Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 230000005540 biological transmission Effects 0.000 claims description 3
- 230000010365 information processing Effects 0.000 claims description 3
- 238000012905 input function Methods 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 238000003889 chemical engineering Methods 0.000 claims description 2
- 238000012544 monitoring process Methods 0.000 claims description 2
- 230000004888 barrier function Effects 0.000 claims 1
- 230000008878 coupling Effects 0.000 description 4
- 238000010168 coupling process Methods 0.000 description 4
- 238000005859 coupling reaction Methods 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 230000008447 perception Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
The invention discloses a chemical inspection vehicle trajectory tracking method and a system, which consists of a sensor planning decision layer, a transverse and longitudinal control trajectory tracking layer and a chemical inspection vehicle execution layer, wherein the sensor planning decision layer realizes sensing and signal acquisition on the environment around an inspection vehicle, filters noise of the acquired signals by using a Kalman filter, generates a global map, establishes a two-degree-of-freedom inspection vehicle dynamics model, and takes a preset running trajectory of the inspection vehicle as a reference trajectory; the control track tracking layer adopts an MOPSO algorithm to jointly optimize the LQR and PID transverse and longitudinal joint controller according to the generated trackQ，R，p，i，dKey parameters are obtained to obtain the current optimal trajectory controller; the chemical industry patrols and examines the car execution layer and gives the car of patrolling and examining with horizontal vertical joint controller signal through the CAN bus, carries out the integrated control to the steering wheel of patrolling and examining the car and throttle opening and brake pressure. The invention effectively improves the track tracking effect of the chemical inspection vehicle.
Description
Technical Field
The invention relates to the field of chemical inspection vehicle trajectory tracking control, in particular to a chemical inspection vehicle trajectory tracking system based on a MOPSO algorithm combined optimization transverse and longitudinal controller.
Background
The chemical inspection vehicle is used as one of outdoor mobile robots, combines related technologies such as inspection vehicle engineering, mode recognition, artificial intelligence, automatic control and computer science, and has high self-planning, self-organizing and self-adapting capabilities. The chemical inspection vehicle system is generally divided into three blocks of perception, decision planning and control. The environmental perception is to collect the surrounding environmental characteristics through a sensor and detect and identify surrounding obstacles by utilizing a fusion algorithm; the decision planning is to carry out task decision and trajectory planning according to the current task of the inspection vehicle, the surrounding traffic conditions, the self state and other information; the inspection vehicle control is to respectively perform transverse control and longitudinal control according to reference path information, speed information and the like given by a planning layer and by combining the current state of the inspection vehicle, realize the control of an accelerator, a brake and a steering wheel, and enable the inspection vehicle to run according to a preset track under the condition of meeting the safety and comfort.
The transverse and longitudinal control problem has been receiving great attention as a core technology for the research and control of the inspection vehicle. The control capability of the system directly reflects whether the patrol vehicle smoothly and accurately tracks the expected path according to the expected speed. The existing transverse control algorithm comprises a control method based on a kinematic model, a control method based on a dynamic model and an intelligent control method without a model; PID control, fuzzy control, optimal control and model predictive control are commonly used in the longitudinal control method.
The transverse controller LQR is a linear quadratic regulator, the optimal control quantity of the LQR is determined by K, however, the matrix Q and R determine the size of the K value, so the values of the matrix Q and R directly determine the performance of the transverse control algorithm LQR, but the Q and R do not have fixed values, the selection of the two values is mutually constrained, and the determination of one value influences the selection of the other value; the PID parameters of the longitudinal controller are only debugged by experience, the sidedness is very large, the movement control of the inspection vehicle is very strong in nonlinearity, and the transverse control and the longitudinal control have very strong coupling, so that the parameter selection problem of the transverse and longitudinal combined controller becomes the key of the track tracking control problem of the inspection vehicle.
Disclosure of Invention
The invention aims to: aiming at the problems in the prior art, the invention provides a chemical industry polling car track tracking method and a chemical industry polling car track tracking system, which improve the chemical industry polling car track tracking effect.
The technical scheme is as follows: the invention provides a chemical inspection vehicle track tracking method, which comprises the following steps:
step 1: sensing the surrounding environment of the inspection vehicle through a sensor device to realize the fusion of environmental information such as traffic signs, roads, obstacles, pedestrians and the like;
and 2, step: observing and filtering the acquired signals by using Kalman filtering, removing measurement noise and system noise and enhancing the control effect;
and 3, step 3: carrying out signal fusion on the BP neural network sensor according to the filtered signals, establishing a global map, taking a preset running track of the inspection vehicle as a reference track, and establishing a two-degree-of-freedom inspection vehicle dynamic model;
and 4, step 4: establishing a Simulink simulation block diagram according to a dynamic model of the inspection vehicle, initializing various parameters of the MOPSO, performing combined optimization by taking parameters of Q, R, p, i and d in the LQR and the PID as particles in the MOPSO, and performing iterative optimization;
and 5: according to the MOPSO algorithm, jointly optimizing the parameter optimization result of the transverse and longitudinal controllers, and solving an optimal feedback gain matrix K in the transverse controller LQR to minimize the transverse error control based on the LQR performance index and obtain the optimal parameter of the PID controller, so as to achieve the purpose of accurate speed tracking;
step 6: and (3) performing track tracking control on the inspection vehicle by the transverse and longitudinal controllers after algorithm combined optimization, controlling the steering angle of a steering wheel by the transverse controller, and controlling the opening degree of a throttle (accelerator) and the pressure of braking (braking) by the longitudinal controller.
Further, in the step 1, for combinations among a plurality of sensor devices to acquire omnidirectional information, the specific steps are as follows:
step 1-1: a millimeter wave radar is arranged in front of the inspection vehicle, and a single line laser radar is arranged behind the inspection vehicle;
step 1-2: a wide-angle camera is arranged at the position of the vehicle head; the mounting positions of the two cameras are also arranged behind the inspection vehicle, so that different requirements of back monocular and binocular vision are met; the side of the inspection vehicle is respectively provided with a camera facing the back of the inspection vehicle;
step 1-3: two temperature sensors are arranged at the chassis position of the inspection vehicle and used for monitoring the temperature of an engine and the ambient temperature;
step 1-4: an information interaction mode mainly based on Ethernet is adopted between the sensor and the information processing module, data are acquired by the laser radar and the camera through an Ethernet interface, data of the millimeter wave radar are converted into an Ethernet transmission mode through CAN-to-Ethernet equipment, and data interaction is carried out between the information and the inspection vehicle through a CAN bus.
Further, the step of fusing the sensor information by using the BP neural network in the step 3 is as follows:
step 3-1: selecting a topological structure of the BP neural network according to the system requirement of the inspection vehicle and the form of sensor information fusion;
step 3-2: the input information of each sensor is comprehensively processed into a total input function, the function is mapped and defined as a mapping function of a relevant unit, and the statistical law of the environment reflects the structure of the network through the interaction of the neural network and the environment;
step 3-3: and (4) learning and understanding the output information of the sensor, determining the distribution of the weight value and finishing the fusion of the information.
Further, the step of establishing the two-degree-of-freedom inspection vehicle dynamics model in the step 3 is as follows:
step 3-4: assuming that the instantaneous turning radius of the inspection vehicle is the same as the curvature radius of the road in the driving process of the inspection vehicle, the coordinate relationship between the axle center of the rear axle and the axle center of the front axle of the inspection vehicle can be expressed as follows:
wherein (x) a ，y a ) Is the axle center coordinate of the front axle of the chemical inspection vehicle, (x) b ，y b ) The axle center coordinate of a rear axle of the chemical inspection vehicle, M is the length of the axle base, and theta is the course angle of the inspection vehicle;
step 3-5: the relation between the speed of the central point of the rear shaft of the inspection vehicle and the coordinate of the central point of the rear shaft is as follows:
wherein v is b In order to inspect the speed of the rear wheel of the vehicle,
is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
the first-order partial derivative of the longitudinal coordinate of the axle center of the rear axle of the inspection vehicle is shown as theta, and the course angle of the inspection vehicle is shown as theta;
step 3-6: assuming that the wheels of the inspection vehicle do not have sideslip phenomenon and the centroid sideslip angle is unchanged in the steering process, the kinematic constraint between the front axle and the rear axle is as follows:
wherein,
for the first-order partial derivative of the horizontal coordinate of the axis of the front axle of the inspection vehicle,
for the first-order partial derivative of the longitudinal coordinate of the axis of the front axle of the inspection vehicle,
is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
is a first-order partial derivative, delta, of the longitudinal coordinate of the axle center of a rear axle of the inspection vehicle f The front wheel deflection angle is shown, and theta is the course angle of the inspection vehicle;
step 3-7: the formula in the step 3-2 and the formula in the step 3-3 are combined to obtain:
wherein,
is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
is a first-order partial derivative, v, of a longitudinal coordinate of the axle center of a rear axle of the inspection vehicle b The speed of the rear wheel of the inspection vehicle is determined, and theta is the course angle of the inspection vehicle;
step 3-8: the inspection vehicle can know the yaw velocity
And the steering radius R of the inspection vehicle and the speed v of the central point of the rear axle b The relational expression between them is:
wherein,
for inspection vehicles yaw rate v b For the speed of the rear wheel of the inspection vehicle, R is the steering radius of the inspection vehicle, delta f Is the front wheel deflection angle, M is the wheelbase length;
step 3-9: substituting the formulas in the step 3-1 and the step 3-4 of the formula into the step 3-3 to obtain the yaw rate
Can be expressed as:
wherein,
for inspection of vehicle yaw rate, v b For inspecting the rear wheel speed, delta f Is the front wheel deflection angle, M is the wheelbase length;
step 3-10: yaw angular velocity
And the heading angle theta of the inspection vehicle
The expression of the kinematic model of the inspection vehicle is as follows:
wherein,
is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
is a first-order partial derivative of the longitudinal coordinate of the axle center of the rear axle of the inspection vehicle,
is the derivative of the course angle theta of the inspection vehicle, v b For inspecting the rear wheel speed, delta f Is a front wheel declination;
step 3-11: in the process of track following control of the inspection vehicle, the general expression of the model can be expressed as follows:
wherein the state quantity
Control quantity
Further, the step 4 of jointly optimizing the parameters of the transverse and longitudinal controllers by using the MOPSO algorithm comprises the following specific steps:
step 4-1: establishing a Simulink inspection vehicle dynamic model, and initializing an MOPSO algorithm population and parameters;
step 4-2: respectively assigning the particles to R, Q, p, i and d parameters, calling Simulink inspection vehicle dynamic model simulation through a sim () function, and calculating a fitness value;
step 4-3: updating and selecting the pbest of each particle, calculating density information, and calculating the gbest in the rank =1 set according to the density information;
step 4-4: and judging whether the conditions are met, if so, storing the optimal results of the R, Q, p, i and d parameters, calculating a feedback gain matrix K of the transverse controller LQR through the optimal R and Q matrixes, sending the optimal p, i and d parameters into a PID of the longitudinal controller to complete the optimization of the longitudinal controller, if not, updating the values of the particles according to a formula, and then returning to the step 4-2.
The invention also discloses a chemical inspection vehicle trajectory tracking system, which comprises a sensor planning decision layer, a transverse and longitudinal control trajectory tracking layer and a chemical inspection vehicle execution layer;
the sensor planning decision layer comprises a data acquisition module, a signal processing module and a model establishing module;
the transverse and longitudinal control track tracking layer comprises an optimization algorithm module;
the chemical engineering inspection vehicle execution layer comprises a transverse controller LQR, a longitudinal controller PID, a steering wheel driving module, a throttle opening and a braking pressure control module;
the data acquisition module is used for sensing the surrounding environment of the inspection vehicle through the sensor device to realize the fusion of environmental information such as traffic signs, roads, obstacles, pedestrians and the like;
the signal processing module is used for observing and filtering the acquired signals by using Kalman filtering, removing measurement noise and system noise and enhancing the control effect;
the model building module is used for fusing sensor signals according to the filtered signals, building a global map, taking a preset running track of the inspection vehicle as a reference track, and building a two-degree-of-freedom inspection vehicle dynamic model;
the optimization algorithm module is used for jointly optimizing Q, R, p, i and d key parameters of the LQR and PID transverse and longitudinal combined controller by adopting an MOPSO algorithm according to the track generated by the sensor planning decision layer so as to obtain a current track optimal controller;
the transverse controller LQR is used for controlling the steering wheel driving module to further control the steering angle of the steering wheel;
and the longitudinal controller PID is used for controlling the throttle opening control module and the brake pressure control module, and further controlling the throttle opening (accelerator) and the brake (brake) pressure.
Preferably, the data acquisition module comprises a camera, a laser radar and a temperature sensor;
a millimeter wave radar is arranged in front of the inspection vehicle, and a single line laser radar is arranged behind the inspection vehicle; two temperature sensors are arranged on the chassis of the inspection vehicle; a wide-angle camera is arranged at the head of the vehicle; the mounting positions of the two cameras are also arranged behind the inspection vehicle, so that different requirements of back monocular and binocular vision are met; the side of the inspection vehicle is respectively provided with a camera which faces the back of the inspection vehicle.
Has the beneficial effects that:
the invention effectively solves the coupling problems of signal acquisition, noise filtration, track generation, one-sidedness of manual selection of controller parameters and transverse and longitudinal control of the chemical inspection vehicle, thereby effectively improving the track tracking effect of the chemical inspection vehicle. The MOPSO intelligent algorithm is used for carrying out combined optimization on the key parameters of the transverse and longitudinal controllers, so that the one-sidedness of manual debugging and the coupling between the controllers are effectively solved, and the track tracking effect is effectively improved. Information fusion is carried out by using a BP neural network sensor, and signals are processed to ensure the accuracy of the map
Drawings
FIG. 1 is a hardware design block diagram of a chemical inspection vehicle trajectory tracking system based on a MOPSO algorithm joint optimization transverse and longitudinal controller provided by the invention;
FIG. 2 is a block diagram of the overall design of a chemical inspection vehicle trajectory tracking system based on a MOPSO algorithm joint optimization transverse and longitudinal controller provided by the invention;
FIG. 3 is a flow chart of an algorithm for jointly optimizing key parameters of a horizontal controller and a vertical controller based on an MOPSO algorithm.
Detailed Description
The invention is further described with reference to the accompanying drawings
With reference to fig. 1, fig. 2, and fig. 3, the invention discloses a chemical inspection vehicle trajectory tracking system based on a joint optimization of a MOPSO algorithm and a longitudinal-transverse controller, which comprises a sensor planning decision layer, a longitudinal-transverse control trajectory tracking layer, and a chemical inspection vehicle execution layer.
And the sensor planning decision layer is used for carrying out all-dimensional sensing and acquisition on the environmental information of the chemical inspection vehicle through a camera, a laser radar and a temperature sensor device, observing and filtering acquired signals by adopting Kalman filtering, removing measurement noise, carrying out information fusion, and generating a global map and a reference track.
And the transverse and longitudinal control trajectory tracking layer jointly optimizes key parameters such as the transverse and longitudinal controllers Q, R, p, i, d and the like through an MOPSO algorithm, and uses the optimized parameters to obtain the transverse and longitudinal controllers.
The chemical industry patrol vehicle executing layer obtains the rotation angle and the longitudinal acceleration of the steering wheel of the chemical industry patrol vehicle, and the rotation angle and the longitudinal acceleration are sent to the chemical industry patrol vehicle by the CAN communication module, so that the track tracking of the chemical industry patrol vehicle based on the MOPSO algorithm combined optimization transverse and longitudinal controller is realized; because the mobile control of the inspection vehicle has strong nonlinearity and the coupling exists between the transverse control and the longitudinal control, an LQR controller is established on the transverse control, so that the transverse error control based on the LQR performance index is minimized; in order to better decouple with the transverse control, a PID controller is established on the longitudinal control to control the longitudinal acceleration of the chemical inspection vehicle; finally, the tracking effect of the chemical inspection vehicle on the reference track is improved.
The invention is based on the chemical industry patrol vehicle track tracking system, and a chemical industry patrol vehicle track tracking method based on MOPSO algorithm combined optimization transverse and longitudinal controller, which comprises the following steps:
step 1: through various sensor devices such as a camera module, a radar module and a temperature module, the surrounding environment of the inspection vehicle is sensed, and the fusion of environmental information such as traffic signs, roads, obstacles and pedestrians is mainly realized.
Wherein, to the combination between the multiple sensor device to acquire all-round information, concrete step is as follows:
step 1-1: in order to realize the all-round perception of the surrounding environment, a millimeter wave radar is arranged in front of the inspection vehicle, and a single line laser radar is arranged behind the inspection vehicle, so that the full coverage of the surrounding environment of the inspection vehicle can be realized.
Step 1-2: in order to obtain large-scale traffic information and make more accurate decision, a wide-angle camera is installed at the position of the vehicle head so as to cover a blind area caused by vehicle head shielding; the mounting positions of the two cameras are also arranged behind the inspection vehicle, so that different requirements of back monocular and binocular vision are met; the side of the inspection vehicle is respectively provided with a camera which faces the rear side of the inspection vehicle, so that the all-around coverage of the cameras around the inspection vehicle can be basically realized.
Step 1-3: in order to ensure that the engine temperature is normal and the ambient temperature is normal, two DS18B20 temperature sensors are installed at the chassis position of the inspection vehicle. The sensor receives and transmits information through a single-wire interface without the support of other external devices, the wiring is more convenient, the data A/D conversion function is built in, and the anti-interference capability is stronger; the self-defined alarm function is provided, the alarm signal is not influenced by power supply, and if the temperature is abnormal, the alarm signal is timely sent out, so that the normal operation of the inspection vehicle can be ensured.
Step 1-4: an information interaction mode mainly based on Ethernet is adopted between the sensor and the information processing module, data are acquired by the laser radar and the camera through an Ethernet interface, data of the millimeter wave radar are converted into an Ethernet transmission mode through CAN-to-Ethernet equipment, and data interaction is carried out between the information and the inspection vehicle through a CAN bus.
Step 2: because of the objective condition of measurement, the automobile body receives external environment's interference easily, and the condition such as rugged road conditions, the interference of wind-force all can seriously influence the steady operation of inspection car, for making the more stable of system operation, uses kalman filter to survey and filter the signal collection, gets rid of measurement noise and system noise and strengthens the control effect.
And 3, step 3: and performing sensor signal fusion according to the filtered signals, performing sensor information fusion by using a BP neural network, establishing a global map, and taking the preset running track of the inspection vehicle as a reference track.
Specifically, the steps of using the BP neural network to fuse the sensor information are as follows:
step 3-1: selecting a topological structure of the BP neural network according to the system requirement of the inspection vehicle and the form of sensor information fusion;
step 3-2: the input information of each sensor is comprehensively processed into a total input function, the function is mapped and defined as a mapping function of a relevant unit, and the statistical law of the environment reflects the structure of the network through the interaction of a neural network and the environment;
step 3-3: and learning and understanding the output information of the sensor, determining the distribution of the weight and finishing the fusion of the information.
Specifically, the steps of establishing the two-degree-of-freedom inspection vehicle dynamic model are as follows:
step 3-4: assuming that the instantaneous turning radius of the inspection vehicle is the same as the curvature radius of the road in the driving process of the inspection vehicle, the coordinate relationship between the axle center of the rear axle and the axle center of the front axle of the inspection vehicle can be expressed as follows:
wherein (x) a ，y a ) Is the axle center coordinate of the front axle of the chemical inspection vehicle, (x) b ，y b ) The axis coordinate of the rear axle of the chemical inspection vehicle is shown, M is the length of the axle base, and theta is the course angle of the inspection vehicle.
Step 3-5: the relation between the speed of the central point of the rear shaft of the inspection vehicle and the coordinate of the central point of the rear shaft is as follows:
wherein v is b In order to inspect the speed of the rear wheel of the vehicle,
is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
the first-order partial derivative of the longitudinal coordinate of the axle center of the rear axle of the inspection vehicle is shown as theta, and the course angle of the inspection vehicle is shown as theta.
Step 3-6: assuming that the wheels of the inspection vehicle do not have sideslip phenomenon and the centroid sideslip angle is unchanged in the steering process, the kinematic constraint between the front axle and the rear axle is as follows:
wherein,
for the first-order partial derivative of the horizontal coordinate of the axis of the front axle of the inspection vehicle,
for the first-order partial derivative of the longitudinal coordinate of the axis of the front axle of the inspection vehicle,
is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
is a first-order partial derivative, delta, of a longitudinal coordinate of the axle center of a rear axle of the inspection vehicle f The deflection angle of the front wheel is shown, and theta is the heading angle of the inspection vehicle.
Step 3-7: the formula in the step 3-2 and the step 3-3 are combined to obtain:
wherein,
is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
is a first-order partial derivative v of the longitudinal coordinate of the axle center of a rear axle of the inspection vehicle b And theta is the course angle of the inspection vehicle.
Step 3-8: the inspection vehicle can know the yaw angular velocity
And the steering radius R of the inspection vehicle and the speed v of the central point of the rear axle b The relational expression between them is:
wherein,
for inspection vehicles yaw rate v b For inspecting the speed of the rear wheel of the vehicle, R is the steering radius of the vehicle, delta f Is the front wheel deflection angle, and M is the wheelbase length.
Step 3-9: substituting the formulas in the step 3-1 and the step 3-4 of the formula into the step 3-3 to obtain the yaw rate
Can be expressed as:
wherein,
for inspection of vehicle yaw rate, v b For inspecting the rear-wheel speed, delta f Is the front wheel slip angle and M is the wheelbase length.
Step 3-10: yaw rate
And the heading angle theta of the inspection vehicle are related
The expression of the kinematic model of the inspection vehicle is as follows:
wherein
Is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
is a first-order partial derivative of a longitudinal coordinate of the axle center of the rear axle of the inspection vehicle,
is the derivative of the course angle theta of the inspection vehicle, v b For inspecting the rear wheel speed, delta f Is the front wheel slip angle.
Step 3-11: in the patrol vehicle trajectory tracking control process, it is generally desirable to express a kinematics model in terms of control quantities and state quantities, and therefore a general expression of the model can be expressed as:
wherein the state quantity
Control quantity
And 4, step 4: establishing a Simulink simulation block diagram according to a dynamic model of the inspection vehicle, initializing various parameters of the MOPSO algorithm, performing joint optimization by taking parameters of Q, R, p, i, d and the like in the transverse controller LQR and the longitudinal controller PID as particles in the MOPSO algorithm, and performing iterative optimization.
The method comprises the following specific steps of using an MOPSO algorithm to jointly optimize parameters of the transverse and longitudinal controllers:
step 4-1: and establishing a Simulink inspection vehicle dynamic model, and initializing an MOPSO algorithm population and parameters.
Step 4-2: and respectively assigning the particles to parameters such as R, Q, p, i, d and the like, calling Simulink to examine the dynamic model simulation of the vehicle through a sim () function, and calculating the fitness value.
Step 4-3: and updating and selecting the pbest of each particle, calculating density information, and calculating the gbest in the rank =1 set according to the density information.
Step 4-4: and judging whether the conditions are met, if so, storing the optimal results of the R, Q, p, i and d parameters, calculating a feedback gain matrix K of the transverse controller LQR through the optimal R and Q matrixes, sending the optimal p, i and d parameters into a PID of the longitudinal controller to complete the optimization of the longitudinal controller, if not, updating the values of the particles according to a formula, and then returning to the step 4-2.
And 5: and jointly optimizing the parameter optimization result of the transverse and longitudinal controller according to the MOPSO algorithm, and solving the optimal feedback gain matrix K in the transverse controller LQR to minimize the transverse error control based on the LQR performance index, obtain the optimal parameter of the PID controller and achieve the aim of accurate speed tracking.
And 6: and (3) performing track tracking control on the inspection vehicle by the transverse and longitudinal controllers after algorithm combined optimization, wherein the transverse controller controls the steering angle of a steering wheel, and the longitudinal controller controls the throttle opening (accelerator) and the brake (brake) pressure so as to achieve the track tracking effect.
The present invention is not limited to the above embodiments, and any equivalent or modification of the technical solution and the inventive concept thereof according to the present invention should be included in the protection scope of the present invention within the knowledge of those skilled in the art.
Claims (7)
1. A chemical industry inspection vehicle trajectory tracking method is characterized by comprising the following steps:
step 1: sensing the surrounding environment of the inspection vehicle through a sensor device to realize the fusion of environmental information such as traffic signs, roads, obstacles, pedestrians and the like;
step 2: observing and filtering the acquired signals by using Kalman filtering, removing measurement noise and system noise and enhancing the control effect;
and step 3: performing BP neural network sensor signal fusion according to the filtered signals, establishing a global map, taking a preset running track of the inspection vehicle as a reference track, and establishing a two-degree-of-freedom inspection vehicle dynamic model;
and 4, step 4: establishing a Simulink simulation block diagram according to a dynamic model of the inspection vehicle, initializing various parameters of the MOPSO, performing combined optimization by taking parameters of Q, R, p, i and d in the LQR and the PID as particles in the MOPSO, and performing iterative optimization;
and 5: according to the MOPSO algorithm, jointly optimizing the parameter optimization result of the transverse and longitudinal controller, and solving the optimal feedback gain matrix K in the transverse controller LQR to minimize the transverse error control based on the LQR performance index, and obtaining the optimal parameter of the PID controller to achieve the purpose of accurate speed tracking;
step 6: and (3) performing track tracking control on the inspection vehicle by the transverse and longitudinal controllers after algorithm combined optimization, wherein the transverse controller controls the steering angle of a steering wheel, and the longitudinal controller controls the opening degree of a throttle (accelerator) and the pressure of a brake (brake).
2. The chemical inspection vehicle track tracking method according to claim 1, wherein in the step 1, for combinations among a plurality of sensor devices to acquire omnidirectional information, the specific steps are as follows:
step 1-1: a millimeter wave radar is arranged in front of the inspection vehicle, and a single line laser radar is arranged behind the inspection vehicle;
step 1-2: a wide-angle camera is arranged at the position of the vehicle head; the mounting positions of the two cameras are also arranged behind the inspection vehicle, so that different requirements of back monocular and binocular vision are met; the side of the inspection vehicle is respectively provided with a camera facing the back of the side of the inspection vehicle;
step 1-3: two temperature sensors are arranged at the chassis position of the inspection vehicle and used for monitoring the temperature of an engine and the ambient temperature;
step 1-4: an information interaction mode mainly based on Ethernet is adopted between the sensor and the information processing module, data are acquired by the laser radar and the camera through an Ethernet interface, data of the millimeter wave radar are converted into an Ethernet transmission mode through CAN-to-Ethernet equipment, and data interaction is carried out between the information and the inspection vehicle through a CAN bus.
3. The chemical industry patrol vehicle track tracking method according to claim 1, wherein the method comprises the following steps: the step 3 of using the BP neural network to fuse the sensor information comprises the following steps:
step 3-1: selecting a topological structure of the BP neural network according to the system requirement of the inspection vehicle and the form of sensor information fusion;
step 3-2: the input information of each sensor is comprehensively processed into a total input function, the function is mapped and defined as a mapping function of a relevant unit, and the statistical law of the environment reflects the structure of the network through the interaction of a neural network and the environment;
step 3-3: and (4) learning and understanding the output information of the sensor, determining the distribution of the weight value and finishing the fusion of the information.
4. The chemical industry patrol vehicle track tracking method according to claim 1, wherein the method comprises the following steps: the step of establishing the two-degree-of-freedom inspection vehicle dynamic model in the step 3 comprises the following steps:
step 3-4: assuming that the instantaneous turning radius of the inspection vehicle is the same as the curvature radius of the road in the driving process of the inspection vehicle, the coordinate relationship between the axle center of the rear axle and the axle center of the front axle of the inspection vehicle can be expressed as follows:
wherein (x) a ，y a ) Is the axle center coordinate of the front axle of the chemical inspection vehicle, (x) b ，y b ) The axle center coordinate of a rear axle of the chemical inspection vehicle, M is the length of the axle base, and theta is the course angle of the inspection vehicle;
step 3-5: the relationship between the speed of the central point of the rear axle of the inspection vehicle and the coordinate of the central point of the rear axle is as follows:
wherein v is b In order to inspect the speed of the rear wheel of the vehicle,
is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
the first-order partial derivative of the longitudinal coordinate of the axle center of the rear axle of the inspection vehicle is shown as theta, and the course angle of the inspection vehicle is shown as theta;
step 3-6: assuming that the wheels of the inspection vehicle do not have sideslip phenomenon and the centroid sideslip angle is unchanged in the steering process, the kinematic constraint between the front axle and the rear axle is as follows:
wherein,
for the first-order partial derivative of the horizontal coordinate of the axis of the front axle of the inspection vehicle,
for inspecting the first-order partial derivative of the longitudinal coordinate of the axle center of the front axle of the vehicle,
is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
is a first-order partial derivative, delta, of a longitudinal coordinate of the axle center of a rear axle of the inspection vehicle f The front wheel deflection angle is shown, and theta is the course angle of the inspection vehicle;
step 3-7: the formula in the step 3-2 and the formula in the step 3-3 are combined to obtain:
wherein,
is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
is a first-order partial derivative, v, of a longitudinal coordinate of the axle center of a rear axle of the inspection vehicle b The speed of the rear wheel of the inspection vehicle is determined, and theta is the course angle of the inspection vehicle;
step 3-8: the inspection vehicle can know the yaw velocity
And the steering radius R of the inspection vehicle and the speed v of the central point of the rear axle b The relational expression between them is:
wherein,
for inspection of vehicle yaw rate, v b For the speed of the rear wheel of the inspection vehicle, R is the steering radius of the inspection vehicle, delta f Is the front wheel deflection angle, M is the wheelbase length;
step 3-9: substituting the formulas in the step 3-1 and the step 3-4 of the formula into the step 3-3 to obtain the yaw angular velocity
Can be expressed as:
wherein,
for inspection of vehicle yaw rate, v b For inspecting the rear wheel speed, delta f Is the front wheel deflection angle, M is the wheelbase length;
step 3-10: yaw angular velocity
And the heading angle theta of the inspection vehicle
The expression of the kinematic model of the inspection vehicle is as follows:
wherein,
is a first-order partial derivative of the horizontal coordinate of the axle center of the rear axle of the inspection vehicle,
is a first-order partial derivative of the longitudinal coordinate of the axle center of the rear axle of the inspection vehicle,
is the derivative of the course angle theta of the inspection vehicle, v b For inspecting the rear-wheel speed, delta f Is a front wheel declination;
step 3-11: in the process of track following control of the inspection vehicle, the general expression of the model can be expressed as follows:
wherein the state quantity
Control quantity
5. The chemical industry patrol vehicle track tracking method according to claim 1, wherein the method comprises the following steps: the specific steps of jointly optimizing the parameters of the transverse and longitudinal controllers by using the MOPSO algorithm in the step 4 are as follows:
step 4-1: establishing a Simulink inspection vehicle dynamic model, and initializing an MOPSO algorithm population and parameters;
step 4-2: respectively assigning the particles to R, Q, p, i and d parameters, calling Simulink inspection vehicle dynamic model simulation through a sim () function, and calculating a fitness value;
step 4-3: updating and selecting the pbest of each particle, calculating density information, and calculating the gbest in the rank =1 set according to the density information;
step 4-4: and judging whether the conditions are met or not, if so, storing the optimal results of the R, Q, p, i and d parameters, calculating a feedback gain matrix K of the transverse controller LQR through the optimal R and Q matrixes, sending the optimal p, i and d parameters into a longitudinal controller PID, completing the optimization of the longitudinal controller, if not, updating the values of the particles according to a formula, and then returning to the step 4-2.
6. The utility model provides a chemical industry inspection vehicle trajectory tracking system which characterized in that: the system comprises a sensor planning decision layer, a transverse and longitudinal control track tracking layer and a chemical inspection vehicle execution layer;
the sensor planning decision layer comprises a data acquisition module, a signal processing module and a model establishing module;
the transverse and longitudinal control track tracking layer comprises an optimization algorithm module;
the chemical engineering inspection vehicle execution layer comprises a transverse controller LQR, a longitudinal controller PID, a steering wheel driving module, a throttle opening and a braking pressure control module;
the data acquisition module is used for sensing the surrounding environment of the inspection vehicle through the sensor device so as to realize the fusion of environmental information such as traffic signs, roads, barriers, pedestrians and the like;
the signal processing module is used for observing and filtering the acquired signals by using Kalman filtering, removing measurement noise and system noise and enhancing the control effect;
the model building module is used for fusing sensor signals according to the filtered signals, building a global map, taking a preset running track of the inspection vehicle as a reference track, and building a two-degree-of-freedom inspection vehicle dynamic model;
the optimization algorithm module is used for jointly optimizing Q, R, p, i and d key parameters of the LQR and PID transverse and longitudinal combined controller by adopting an MOPSO algorithm according to the track generated by the sensor planning decision layer so as to obtain a current track optimal controller;
the transverse controller LQR is used for controlling the steering wheel driving module to further control the steering angle of the steering wheel;
and the longitudinal controller PID is used for controlling the throttle opening control module and the brake pressure control module, and further controlling the throttle opening (accelerator) and the brake (brake) pressure.
7. The chemical inspection vehicle trajectory tracking system of claim 6, wherein: the data acquisition module comprises a camera, a laser radar and a temperature sensor;
a millimeter wave radar is arranged in front of the inspection vehicle, and a single line laser radar is arranged behind the inspection vehicle; two temperature sensors are arranged on the chassis of the inspection vehicle; a wide-angle camera is arranged at the position of the vehicle head; the mounting positions of the two cameras are also arranged behind the inspection vehicle, so that different requirements of back monocular and binocular vision are met; the side of the inspection vehicle is respectively provided with a camera which faces the back of the inspection vehicle.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211512957.5A CN115755919A (en) | 2022-11-29 | 2022-11-29 | Chemical inspection vehicle trajectory tracking method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211512957.5A CN115755919A (en) | 2022-11-29 | 2022-11-29 | Chemical inspection vehicle trajectory tracking method and system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115755919A true CN115755919A (en) | 2023-03-07 |
Family
ID=85340448
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211512957.5A Pending CN115755919A (en) | 2022-11-29 | 2022-11-29 | Chemical inspection vehicle trajectory tracking method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115755919A (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN207198663U (en) * | 2017-09-30 | 2018-04-06 | 徐工集团工程机械股份有限公司 | Road roller and road roller control system |
| CN107992051A (en) * | 2017-12-26 | 2018-05-04 | 江南大学 | Unmanned vehicle paths planning method based on improved multi-objective particle swarm algorithm |
| CN109597404A (en) * | 2017-09-30 | 2019-04-09 | 徐工集团工程机械股份有限公司 | Road roller and its controller, control method and system |
| CN110320919A (en) * | 2019-07-31 | 2019-10-11 | 河海大学常州校区 | A kind of circulating robot method for optimizing route in unknown geographical environment |
| CN111409641A (en) * | 2020-03-10 | 2020-07-14 | 厦门理工学院 | Trajectory tracking control method and system based on longitudinal and transverse coordination |
| CN113467496A (en) * | 2021-07-07 | 2021-10-01 | 东北大学 | Design method and control system of two-wheeled trolley balance track and tracking controller |
| CN113771868A (en) * | 2021-08-11 | 2021-12-10 | 佛山仙湖实验室 | Transverse control method of unmanned automobile |
| CN114564016A (en) * | 2022-02-24 | 2022-05-31 | 江苏大学 | Navigation obstacle avoidance control method, system and model combining path planning and reinforcement learning |
-
2022
- 2022-11-29 CN CN202211512957.5A patent/CN115755919A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN207198663U (en) * | 2017-09-30 | 2018-04-06 | 徐工集团工程机械股份有限公司 | Road roller and road roller control system |
| CN109597404A (en) * | 2017-09-30 | 2019-04-09 | 徐工集团工程机械股份有限公司 | Road roller and its controller, control method and system |
| CN107992051A (en) * | 2017-12-26 | 2018-05-04 | 江南大学 | Unmanned vehicle paths planning method based on improved multi-objective particle swarm algorithm |
| CN110320919A (en) * | 2019-07-31 | 2019-10-11 | 河海大学常州校区 | A kind of circulating robot method for optimizing route in unknown geographical environment |
| CN111409641A (en) * | 2020-03-10 | 2020-07-14 | 厦门理工学院 | Trajectory tracking control method and system based on longitudinal and transverse coordination |
| CN113467496A (en) * | 2021-07-07 | 2021-10-01 | 东北大学 | Design method and control system of two-wheeled trolley balance track and tracking controller |
| CN113771868A (en) * | 2021-08-11 | 2021-12-10 | 佛山仙湖实验室 | Transverse control method of unmanned automobile |
| CN114564016A (en) * | 2022-02-24 | 2022-05-31 | 江苏大学 | Navigation obstacle avoidance control method, system and model combining path planning and reinforcement learning |
Non-Patent Citations (1)
| Title |
|---|
| 徐明泽 等: "基于LQR和PID的智能车轨迹跟踪控制算法设计与仿真", 太原理工大学学报, vol. 53, no. 5, pages 877 - 885 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113386795B (en) | Intelligent decision-making and local track planning method for automatic driving vehicle and decision-making system thereof | |
| CN109263639B (en) | Driving path planning method based on state grid method | |
| CN111142091B (en) | Automatic driving system laser radar online calibration method fusing vehicle-mounted information | |
| CN113635897B (en) | Safe driving early warning method based on risk field | |
| CN110764507A (en) | Artificial intelligence automatic driving system for reinforcement learning and information fusion | |
| CN115257724A (en) | Safety and energy-saving decision control method and system for plug-in hybrid electric vehicle | |
| CN111452786A (en) | Obstacle avoidance method and system for unmanned vehicle | |
| Huang et al. | Path tracking based on improved pure pursuit model and pid | |
| CN115018353A (en) | Intelligent network-connected automobile decision planning method under heterogeneous traffic flow | |
| CN114047003B (en) | Human-vehicle difference data trigger record control method based on dynamic time warping algorithm | |
| CN111231984B (en) | Four-wheel steering intelligent automobile pseudo-decoupling controller and control method thereof | |
| CN115755919A (en) | Chemical inspection vehicle trajectory tracking method and system | |
| Joo et al. | Real time traversability analysis to enhance rough terrain navigation for an 6× 6 autonomous vehicle | |
| CN115476881B (en) | Vehicle track tracking control method, device, equipment and medium | |
| CN115107904B (en) | Reconfigurable wheel-track universal distributed driving unmanned vehicle and control method thereof | |
| CN114074672B (en) | Method for identifying cornering stiffness of a tyre of a vehicle and related device | |
| CN108501948A (en) | A kind of pilotless automobile control method based on virtual reality | |
| Wang et al. | Stable Obstacle Avoidance Strategy for Crawler-Type Intelligent Transportation Vehicle in Non-Structural Environment Based on Attention-Learning | |
| CN110329028B (en) | Hybrid electromagnetic suspension mode switching system and method based on highway grade recognition | |
| Mohamed-Ahmed et al. | Non-linear control based on state estimation for the convoy of autonomous vehicles | |
| Wu et al. | Simulation and experimental study of nonlinear characteristics for multi-mode driving of intelligent vehicles | |
| CN112965494A (en) | Control system and method for pure electric automatic driving special vehicle in fixed area | |
| CN112351407A (en) | AEB strategy method based on 5G hierarchical decision | |
| Hua et al. | Intelligent vehicle human-simulated steering characteristics access and control strategy | |
| CN117406754B (en) | Logistics robot environment sensing and obstacle avoidance method and system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20230307 |
|
| RJ01 | Rejection of invention patent application after publication |