CN111948624A - Tracking control method and system for non-road mobile pollution source detection laser radar - Google Patents
Tracking control method and system for non-road mobile pollution source detection laser radar Download PDFInfo
- Publication number
- CN111948624A CN111948624A CN202010733295.9A CN202010733295A CN111948624A CN 111948624 A CN111948624 A CN 111948624A CN 202010733295 A CN202010733295 A CN 202010733295A CN 111948624 A CN111948624 A CN 111948624A
- Authority
- CN
- China
- Prior art keywords
- scanning mechanism
- joint
- sliding mode
- angle
- laser radar
- 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
- 238000000034 method Methods 0.000 title claims abstract description 51
- 238000001514 detection method Methods 0.000 title claims abstract description 34
- 230000007246 mechanism Effects 0.000 claims abstract description 129
- 239000011159 matrix material Substances 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 12
- 238000004590 computer program Methods 0.000 claims description 6
- 239000000126 substance Substances 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 4
- 238000005265 energy consumption Methods 0.000 abstract description 2
- 238000012544 monitoring process Methods 0.000 description 15
- 239000013598 vector Substances 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 230000009466 transformation Effects 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 239000003344 environmental pollutant Substances 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 231100000719 pollutant Toxicity 0.000 description 2
- 230000035945 sensitivity Effects 0.000 description 2
- 238000001228 spectrum Methods 0.000 description 2
- 241000287196 Asthenes Species 0.000 description 1
- 238000003915 air pollution Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 239000010419 fine particle Substances 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000704 physical effect Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/481—Constructional features, e.g. arrangements of optical elements
- G01S7/4817—Constructional features, e.g. arrangements of optical elements relating to scanning
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
The invention discloses a tracking control method and a tracking control system for a non-road mobile pollution source detection laser radar, which utilize a DH modeling method and a base B0Azimuth rotary joint B1And roll rotary joint B2As joint points, respectively establishing joint coordinate systems; establishing a joint rotation angle-moment dynamics equation of the scanning mechanism according to the established joint coordinate system and the power parameters of the scanning mechanism; constructing an integral sliding mode surface by taking the difference value of the expected angle and the actual angle of the joint angle of the scanning mechanism as a tracking error; constructing a sliding mode angle controller by utilizing an integral sliding mode surface and a differentiator to adjust the actual rotating angle of the joint of the scanning mechanism to obtain the torque applied to the scanning mechanism by a motor, thereby realizing the detection of the laser mine on the pollution sourceAnd (4) achieving second-order integral sliding mode tracking control. Compared with the traditional method, the scheme has higher tracking performance and smaller energy consumption.
Description
Technical Field
The invention relates to the technical field of atmospheric pollution emission detection, in particular to a tracking control method and a tracking control system for a non-road mobile pollution source detection laser radar.
Background
At present, the atmospheric environment situation of China is severe, the total pollutant emission is large, and the regional atmospheric environment problem taking fine particles as characteristic pollutants is increasingly prominent. The frequently occurring problem of regional atmospheric pollution such as dust haze is closely related to the substandard emission of tail gas of urban mobile pollution sources such as motor vehicles, engineering vehicles and ships. The pollution of mobile sources has become one of the most prominent and urgent problems in the air pollution problem in China.
In the aspect of emission monitoring of non-road mobile pollution sources such as ships and the like, because an optical reflection device cannot be installed, a passive scanning observation system, namely an atmospheric pollution measurement laser radar, is adopted, and concentration information of atmospheric components is acquired by emitting laser with specific wavelength into the atmosphere and collecting and analyzing a scattering spectrum after the scattering spectrum and an atmospheric medium have physical action.
However, this monitoring and analyzing method has drawbacks in that: firstly, the measurement optical path distance is long, and the area of a monitoring area is small, so that the self-positioning of an observation target is very difficult; secondly, in the follow-up process of the observation system, the micro vibration can bring about great detection errors, and the anti-interference capability is poor.
Disclosure of Invention
The invention aims to overcome the defects in the prior art so as to improve the anti-interference capability to the external environment and quickly and stably track a target.
In order to realize the aim, the invention adopts a tracking control method of a non-road mobile pollution source detection laser radar, and a scanning mechanism of the pollution source detection laser radar comprises a base B0Azimuth rotary joint B1And roll rotary joint B2A base B0Azimuth rotary joint B1And roll rotary joint B2In turn, rotationally coupled, the method comprising:
using a DH modeling method toBase B0Azimuth rotary joint B1And roll rotary joint B2As joint points, respectively establishing joint coordinate systems;
establishing a joint rotation angle-moment dynamics equation of the scanning mechanism according to the established joint coordinate system and the power parameters of the scanning mechanism;
constructing an integral sliding mode surface by taking the difference value of the expected angle and the actual angle of the joint angle of the scanning mechanism as a tracking error;
and constructing a sliding mode angle controller by utilizing an integral sliding mode surface and a differentiator to adjust the actual rotating angle of the joint of the scanning mechanism, so as to obtain the moment applied to the scanning mechanism by the motor and realize the second-order integral sliding mode tracking control of the pollution source detection laser radar.
Further, the integral slip form surface of the construction is:
wherein λ is1And λ2And a proportional coefficient and an integral coefficient diagonal matrix respectively representing the sliding mode control rate, wherein beta is a constant coefficient, e is a tracking error, sign () is a sign function, and is 1 when the sign () is greater than 0 and is-1 when the sign () is less than 0.
Further, the method includes the steps of constructing a sliding mode angle controller by using an integral sliding mode surface and a differentiator to adjust the actual rotation angle of the joint of the scanning mechanism, obtaining the moment applied to the scanning mechanism by a motor, and realizing second-order integral sliding mode tracking control of the pollution source detection laser radar, and includes the following steps:
calculating the angular velocity and the angular acceleration of the joint rotation of the scanning mechanism by using a differentiator;
and constructing a sliding mode angle controller by utilizing the integral sliding mode surface, the rotating angular speed and the angular acceleration of the joint of the scanning mechanism so as to adjust the actual rotating angle of the joint of the scanning mechanism and obtain the moment applied to the scanning mechanism by the motor.
Further, the formula for calculating the angular velocity and the angular acceleration of the joint rotation of the scanning mechanism by using the differentiator is as follows:
wherein Σ is a differentiator parameter and satisfies
The input of the differentiator is the actual angle q of the joint angle of the scanning mechanism, and the output psi of the differentiator0，ψ1，ψ2Are used to estimate the q-values respectively,
record as
Further, the sliding mode angle control rule of the sliding mode angle controller is as follows:
wherein the content of the first and second substances,
representing the first derivative of the sliding mode surface s,
representing said desired angle qdSecond derivative of (a)1i,λ2i,eiRespectively represent matrices lambda1,λ2And the ith diagonal element of matrix e,
is the second differential of q, K1，K2Is a constant coefficient diagonal matrix, U (t-t)d) Represents tdThe moment applied to the scanning mechanism by the motor before the moment, U (t) representing time tTorque applied to the scanning mechanism by the stepper motor.
Further, the
β＝0.6，Σ＝42，
Further, the establishing of the joint rotation angle-moment dynamics equation of the scanning mechanism according to the established joint coordinate system and the dynamic parameters of the scanning mechanism includes:
establishing a positive kinematic equation of the laser radar scanning mechanism according to the DH parameters corresponding to the joint coordinate system;
and establishing a joint rotation angle-moment dynamics equation of the scanning mechanism according to the positive kinematics equation of the laser radar scanning mechanism and the power parameters of the laser radar scanning mechanism.
Further, the establishing of the joint rotation angle-moment dynamics equation of the scanning mechanism according to the positive kinematics equation of the lidar scanning mechanism and the dynamic parameters of the lidar scanning mechanism includes:
based on a positive kinematic equation of the laser radar scanning mechanism, performing forward iteration and backward iteration by using a Newton-Euler method to obtain a joint rotation angle-moment dynamic model of the laser radar scanning mechanism;
and substituting the dynamic parameters of the laser radar scanning mechanism into the joint rotation angle-moment dynamic model to obtain the moment-joint angle dynamic equation.
In a second aspect, a tracking control system for a non-road mobile pollution source detection laser radar is provided, which comprises a pollution source detection laser radar scanning mechanism and a controller, wherein the scanning mechanism comprises a base B0Azimuth rotary joint B1And roll rotary joint B3A base B0Azimuth rotary joint B1And roll rotary joint B2Are connected in turn in a rotating way, and the controller comprises a coordinate system establishing moduleThe system comprises a dynamic equation establishing module, an integral sliding mode surface constructing module and a second-order integral sliding mode tracking control module, wherein:
the coordinate system establishment module is used for utilizing a DH modeling method and using a base B0Azimuth rotary joint B1And roll rotary joint B2As joint points, respectively establishing joint coordinate systems;
the dynamic equation establishing module is used for establishing a joint rotation angle-moment dynamic equation of the scanning mechanism according to the established joint coordinate system and the power parameters of the scanning mechanism;
the integral sliding mode surface construction module is used for constructing an integral sliding mode surface by taking the difference value between the expected angle and the actual angle of the joint angle of the scanning mechanism as a tracking error;
and the second-order integral sliding mode tracking control module is used for constructing a sliding mode angle controller by utilizing an integral sliding mode surface and a differentiator so as to adjust the actual rotating angle of the joint of the scanning mechanism, obtain the moment applied to the scanning mechanism by the motor and realize the second-order integral sliding mode tracking control of the pollution source detection laser radar.
In a third aspect, there is provided a computer readable storage medium comprising a computer program for use in conjunction with a storage device, the computer program for execution by a processor, comprising:
using DH modeling method with base B0Azimuth rotary joint B1And roll rotary joint B2As joint points, respectively establishing joint coordinate systems;
establishing a joint rotation angle-moment dynamics equation of the scanning mechanism according to the established joint coordinate system and the power parameters of the scanning mechanism;
constructing an integral sliding mode surface by taking the difference value of the expected angle and the actual angle of the joint angle of the scanning mechanism as a tracking error;
and constructing a sliding mode angle controller by utilizing an integral sliding mode surface and a differentiator to adjust the actual rotating angle of the joint of the scanning mechanism, so as to obtain the moment applied to the scanning mechanism by the motor and realize the second-order integral sliding mode tracking control of the pollution source detection laser radar.
Compared with the prior art, the invention has the following technical effects: according to the invention, on the basis of a dynamic equation of the laser radar scanning mechanism, an integral terminal sliding mode surface is constructed by utilizing a difference value between an expected angle and an actual angle of each joint angle of a positioning platform, and a second-order sliding mode control rule is designed to adjust the angle of the actual joint angle according to the integral terminal sliding mode surface and a differentiator, so that a moment value is obtained, and the system state of the laser radar scanning mechanism is tracked. According to the scheme, the problems of flutter and noise sensitivity at the arrival stage are solved by adopting second-order integral sliding mode control, the uncertainty of the dynamics of a laser radar scanning system is eliminated, the robustness of the system is improved, the laser radar can track a target quickly, accurately and stably, the control cost is reduced, and the control efficiency is improved.
Drawings
The following detailed description of embodiments of the invention refers to the accompanying drawings in which:
FIG. 1 is a schematic flow diagram of a tracking control method for a non-road mobile pollution source detection lidar;
FIG. 2 is a structural diagram of a ship exhaust gas monitoring lidar provided by the embodiment;
FIG. 3 is a centroid diagram of the ship exhaust gas monitoring lidar provided by the embodiment;
FIG. 4 is a D-H link coordinate system diagram of the scanning mechanism of the laser radar for monitoring ship exhaust gas according to the present embodiment;
fig. 5 is a schematic block diagram of a second-order integral sliding mode tracking control method provided in this embodiment.
Detailed Description
To further illustrate the features of the present invention, refer to the following detailed description of the invention and the accompanying drawings. The drawings are for reference and illustration purposes only and are not intended to limit the scope of the present disclosure.
As shown in FIG. 1, the present embodiment discloses a tracking method for a non-road mobile pollution source detection lidar, the scanning of whichThe tracing mechanism comprises a base B0Azimuth rotary joint B1And roll rotary joint B2A base B0Azimuth rotary joint B1And roll rotary joint B2In turn rotatably connected, the method comprising the steps S1 to S4:
s1, using DH modeling method, using base B0Azimuth rotary joint B1And roll rotary joint B2As joint points, respectively establishing joint coordinate systems;
s2, establishing a joint rotation angle-moment dynamic equation of the scanning mechanism according to the established joint coordinate system and the power parameters of the scanning mechanism;
s3, constructing an integral sliding mode surface by taking the difference value between the expected angle and the actual angle of the joint angle of the scanning mechanism as a tracking error;
s4, constructing a sliding mode angle controller by utilizing an integral sliding mode surface and a differentiator to adjust the actual rotating angle of the scanning mechanism joint, obtaining the moment applied to the scanning mechanism by a motor, and realizing the second-order integral sliding mode tracking control of the pollution source detection laser radar.
Specifically, non-road mobile pollution source monitoring includes the emission monitoring of non-road mobile pollution sources such as boats and ships, and laser radar uses boats and ships exhaust gas monitoring laser radar to introduce as an example in this embodiment:
as shown in fig. 2 to 3, the laser radar for monitoring exhaust gas from a ship mainly includes: base B0Azimuth rotation member B1With the roll-rotating member B2Said base B0Azimuth rotation member B1With the roll-rotating member B2Are connected in turn.
As shown in FIG. 4, in the step S1, a D-H link coordinate system is established by the method of Denavit-Hartenberg (DH). Is a base B0Azimuth rotation member B1With the roll-rotating member B2Respectively constructing a coordinate system which is marked as a coordinate system 0, a coordinate system 1 and a coordinate system 2, and respectively marking the original points of the three coordinate systems as O0、O1And O2(ii) a Base B0Azimuth rotation member B1Hewei (Chinese character of 'Hewei')Rolling component B2Respectively, is recorded as S0、S1And S2(ii) a The azimuth angle and the roll angle of the non-road mobile pollution source monitoring laser radar are respectively recorded as theta1And theta2(ii) a Motor applied to azimuth rotary member B1And a roll rotating member B2Respectively is recorded as tau1And τ2；O0And O1Is recorded as d1；O1And O2Is recorded as d2。
According to the D-H method, the DH parameter table corresponding to the D-H connecting rod coordinate system constructed in this embodiment is shown in Table 1.
TABLE 1 DH parameters table
Wherein the connecting rod 1 is B1And B1Upper drive B2The connecting rod 2 is B2，aiIs an axis ziAnd zi-1The length of the male perpendicular line of (a); alpha is alphaiIs an axis zi-1And ziWhen the angle between them is around the axis xiPositive when rotating anticlockwise; diIs an axis ziAnd zi-1The common vertical line and zi-1Along z of the intersection pointi-1Length of (d); thetaiIs an axis xi-1And xiWhen the angle between them is around the axis zi-1Counterclockwise rotation is positive.
In the embodiment of the present invention, the i-0, 1,2 coordinate system i is attached to the link i and moves together with the link i, but the coordinate system 0 is fixed, and when i-0, the link i can be understood as a base.
Specifically, in the above step S2, the joint rotation angle-moment dynamics equation of the scanning mechanism is established according to the established joint coordinate system and the power parameter of the scanning mechanism, and the method includes the following sub-steps S21 to S22:
and S21, establishing a positive kinematic equation of the laser radar scanning mechanism according to the DH parameters corresponding to the joint coordinate system. The method specifically comprises the following steps:
and performing kinematic analysis on the non-road mobile pollution source monitoring laser radar according to a DH parameter table corresponding to the D-H connecting rod coordinate system, and establishing a positive kinematic equation to express as follows:
wherein the content of the first and second substances,
representing a homogeneous transformation matrix from coordinate system 0 to coordinate system 1;
represents cos θ1、cosθ2，
Denotes sin θ1、sinθ2；
Representing a one-step homogeneous transformation matrix from coordinate system 0 to coordinate system 1;
representing a one-step homogeneous transformation matrix of coordinate system 1 to coordinate system 2.
And S22, establishing a joint rotation angle-moment dynamics equation of the scanning mechanism according to the positive kinematics equation of the laser radar scanning mechanism and the power parameters of the laser radar scanning mechanism. The method specifically comprises the following steps:
deducing a dynamic model by using the result of the step S21 to obtain a corresponding forward recursion formula and a backward recursion formula; in addition, in order to make the calculation convenient, it is necessary to order
These vectors or matrices are constant and therefore cannot use the ground-fixed coordinate system, but rather the individual link coordinate systems, which require rotations in positive kinematic equations in the recursion formulaThe transformation matrix transforms the calculated intermediate vector into a coordinate system, which is as follows:
according to the Newton-Euler method, the forward recursion formula is:
wherein, i is 0,1,2,
is represented by BiThe angular velocity of (a) of (b),
a unit vector representing the z-axis direction of the coordinate system i,
represents OiAcceleration of (O)iRepresents the origin of a coordinate system that moves with the connecting rod, but coordinate system 0 is stationary;
denotes SiThe acceleration of (a) is detected,
representing a vector
Indicating motorAngular acceleration of the rotor, kriIn order to reduce the gear ratio,
a unit vector which is the direction of the rotor rotating shaft;
respectively represent thetaiFirst derivative with respect to time,
First derivative with respect to time, thetaiSecond derivative with respect to time.
The backward recursion formula is:
wherein the content of the first and second substances,
represents the force applied by the connecting rod i-1 to the connecting rod i, miIs represented by BiThe mass of (a) of (b),
representing the link i-1 to the link i with respect to the origin O of the coordinate system i-1i-1The moment of force of (a) is,
representing a vector
Is represented by BiInertia tensor with respect to coordinate system iThe matrix of quantities is then used to determine,
representing the moment of inertia, tau, of the rotor about the axis of rotationiIs composed of
The component on the z-axis of the coordinate system i, i.e. the moment.
In the present embodiment, the link 0 is assumed to be the base B0Where i is also in the range of 0 to 2, and is not calculated
Since it has no meaning, appear
An intermediate quantity with an index of 3 (i.e., index i +1 when i ═ 2) is defaulted to 0.
Carrying out derivation on parameters of the laser radar scanning mechanism for monitoring the non-road mobile pollution source to obtain a matrix form of a dynamic model of the laser radar scanning mechanism:
wherein τ ═ (τ)1τ2)T，q＝(θ1 θ2)T；
Respectively a first derivative and a second derivative of q to time; m (q) is a generalized inertia matrix,
the matrix of centripetal and Coriolis forces, G (q) is gravity, G (q), and d is external disturbance.
For example, suppose the parameters of the non-road mobile pollution source monitoring lidar scanning mechanism are: m is1＝100,m2＝46.5，r0,1＝0.18，r1,2＝0.4，
The above are coordinates in the current link coordinate system; the dynamic equation of the laser radar scanning mechanism is as follows:
specifically, in step S3, the integral sliding mode surface of the structure is:
wherein λ is1And λ2And a proportional coefficient and an integral coefficient diagonal matrix respectively representing the sliding mode control rate, wherein beta is a constant coefficient, e is a tracking error, sign () is a sign function, and is 1 when the sign () is greater than 0 and is-1 when the sign () is less than 0.
Further, the above step S4: constructing a sliding mode angle controller by utilizing an integral sliding mode surface and a differentiator to adjust the actual rotation angle of the joint of the scanning mechanism to obtain the moment applied to the scanning mechanism by a motor, and realizing the second-order integral sliding mode tracking control of the pollution source detection laser radar, wherein the method comprises the following subdivision steps S41 to S42:
s41, calculating the angular speed and the angular acceleration of the joint rotation of the scanning mechanism by using a differentiator;
it should be noted that, in order to solve the problem that the angular velocity and the angular acceleration of the joint rotation of the laser radar scanning mechanism cannot be directly measured in practice, a differentiator is added to solve the problem, and the form is as follows:
wherein Σ is a differentiator parameter and satisfies
The differentiator has an input of q and an output of psi0，ψ1，ψ2Are used to estimate the q-values respectively,
record as
Then the differentiator can obtain the first and second order differentials of q within a finite time, i.e.
S42, constructing a sliding mode angle controller by using the integral sliding mode surface, the rotating angular speed and the angular acceleration of the joint of the scanning mechanism to adjust the actual rotating angle of the joint of the scanning mechanism, and obtaining the moment applied to the scanning mechanism by the motor.
Specifically, the sliding mode angle control law of the sliding mode angle controller is as follows:
wherein the content of the first and second substances,
representing the first derivative of the sliding mode surface s,
representing said desired angle qdSecond derivative of (a)1i,λ2i,eiRespectively represent matrices lambda1,λ2And matrix e and the ith diagonal element of (1)iThe (i) th element of (a),
is the second differential of q, K1，K2Is a constant coefficient diagonal matrix, U (t-t)d) Represents tdThe moment applied to the scanning mechanism by the motor before the time, u (t), represents the moment applied to the scanning mechanism by the motor at the time.
As shown in fig. 5, the second-order integral sliding mode controller can obtain a moment U applied to the laser radar scanning mechanism, i.e., τ in the dynamic model equation, and then obtain a corresponding actual angle q according to calculation, so as to implement closed loop, and further control two rotating devices (azimuth rotating component B)1With the roll-rotating member B2) The method can realize the second-order integral sliding mode tracking control of the non-road mobile pollution source monitoring laser radar.
It should be noted that, in the embodiment, by adopting the second-order integral sliding mode control, the problems of flutter and noise sensitivity at the arrival stage are eliminated, the robustness of the system is improved, and the tracking speed and transient performance are improved. And due to the uncertainty of the dynamics of the laser radar scanning mechanism, t is introduced by adopting time delay estimation in the embodimentdMoment U (t-t) applied to laser radar scanning mechanism by motor before momentd) To eliminate the nonlinearity and uncertainty of system dynamics, such as parameter variation and interference, so that the sliding mode is independent of the system modeThe target can be tracked quickly, accurately and stably, and compared with the traditional method, the scheme of the embodiment has higher tracking performance and smaller energy consumption.
Preferably, in the selection of the parameters, the embodiment of the invention provides a group of better performance schemes by adjusting the parameters:
β＝0.6,Σ＝42,
when the set of parameters is selected, the system has smaller adjusting time and steady-state error under the proposed second-order integral sliding mode control method, and can realize better tracking performance of the angle of the rotating device.
It should be noted that, in this embodiment, compared with the scheme described in the chinese patent application publication No. CN110007599A, the sliding mode surfaces of the two control methods are different in selection, and the selection form of the sliding mode surface in this embodiment is to design the sign function in the integral sign, so that discontinuity of the sign function is eliminated, and the buffeting problem caused by sliding mode control is reduced, while another scheme is to introduce integral into the control law, which is simpler and clearer than the method in this embodiment.
In addition, the embodiment introduces the use of a differentiator to solve the problem that the angular velocity and the angular acceleration of the joint rotation of the laser radar scanning mechanism cannot be directly measured in practice. While in the other case the control law contains an angular velocity term (in the error derivative), it is necessary to actually measure the angular velocity of the rotating platform. However, this is not necessarily achieved in practice, and the present solution enables the turntable to be controlled well even if the angular velocity is not measurable by introducing a differentiator, and the output of the structure of the differentiator is added to the controller, which output can accurately track the value of the angular velocity over a period of time.
This exampleDiscloses a tracking control system of a non-road mobile pollution source detection laser radar, which comprises a pollution source detection laser radar scanning mechanism and a controller, wherein the scanning mechanism comprises a base B0Azimuth rotary joint B1And roll rotary joint B3A base B0Azimuth rotary joint B1And roll rotary joint B2The controller comprises a coordinate system establishing module, a dynamic equation establishing module, an integral sliding mode surface constructing module and a second-order integral sliding mode tracking control module, wherein:
the coordinate system establishment module is used for utilizing a DH modeling method and using a base B0Azimuth rotary joint B1And roll rotary joint B2As joint points, respectively establishing joint coordinate systems;
the dynamic equation establishing module is used for establishing a joint rotation angle-moment dynamic equation of the scanning mechanism according to the established joint coordinate system and the power parameters of the scanning mechanism;
the integral sliding mode surface construction module is used for constructing an integral sliding mode surface by taking the difference value between the expected angle and the actual angle of the joint angle of the scanning mechanism as a tracking error;
and the second-order integral sliding mode tracking control module is used for constructing a sliding mode angle controller by utilizing an integral sliding mode surface and a differentiator so as to adjust the actual rotating angle of the joint of the scanning mechanism, obtain the moment applied to the scanning mechanism by the motor and realize the second-order integral sliding mode tracking control of the pollution source detection laser radar.
It should be noted that the tracking control system for detecting the lidar by using the non-road mobile pollution source disclosed in this embodiment corresponds to the tracking control method for detecting the lidar by using the non-road mobile pollution source disclosed in the above embodiment, and the implementation and effect of the specific technical scheme refer to the description in the above method embodiment, which is not described herein again.
The present embodiments also disclose a computer-readable storage medium comprising a computer program for use in conjunction with a storage device, the computer program for execution by a processor, comprising:
using DH modeling method with base B0Azimuth rotary joint B1And roll rotary joint B2As joint points, respectively establishing joint coordinate systems;
establishing a joint rotation angle-moment dynamics equation of the scanning mechanism according to the established joint coordinate system and the power parameters of the scanning mechanism;
constructing an integral sliding mode surface by taking the difference value of the expected angle and the actual angle of the joint angle of the scanning mechanism as a tracking error;
and constructing a sliding mode angle controller by utilizing an integral sliding mode surface and a differentiator to adjust the actual rotating angle of the joint of the scanning mechanism, so as to obtain the moment applied to the scanning mechanism by the motor and realize the second-order integral sliding mode tracking control of the pollution source detection laser radar.
Through the above description of the embodiments, it is clear to those skilled in the art that the above embodiments can be implemented by software, and can also be implemented by software plus a necessary general hardware platform. Based on such understanding, the technical solutions of the above embodiments may be embodied in the form of a software product, which may be stored in a non-volatile storage medium (which may be a CD-ROM, a usb disk, a removable hard disk, etc.), and includes several instructions for enabling a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the methods according to the embodiments of the present invention, where the foregoing storage medium includes: various media that can store program codes, such as ROM, RAM, magnetic or optical disks.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.
Claims (10)
1. The tracking control method of the laser radar for detecting the non-road mobile pollution source is characterized in that a scanning mechanism of the laser radar for detecting the non-road mobile pollution source comprises a base B0Azimuth rotary joint B1And roll rotary joint B2A base B0Azimuth rotary joint B1And roll rotary joint B2In turn, rotationally coupled, the method comprising:
using DH modeling method with base B0Azimuth rotary joint B1And roll rotary joint B2As joint points, respectively establishing joint coordinate systems;
establishing a joint rotation angle-moment dynamics equation of the scanning mechanism according to the established joint coordinate system and the power parameters of the scanning mechanism;
constructing an integral sliding mode surface by taking the difference value of the expected angle and the actual angle of the joint angle of the scanning mechanism as a tracking error;
and constructing a sliding mode angle controller by utilizing an integral sliding mode surface and a differentiator to adjust the actual rotating angle of the joint of the scanning mechanism, so as to obtain the moment applied to the scanning mechanism by the motor and realize the second-order integral sliding mode tracking control of the pollution source detection laser radar.
2. The tracking control method of the non-road mobile pollution source detection lidar according to claim 1, wherein the constructed integral sliding mode surface is:
wherein λ is1And λ2And a proportional coefficient and an integral coefficient diagonal matrix respectively representing the sliding mode control rate, wherein beta is a constant coefficient, e is a tracking error, sign () is a sign function, and is 1 when the sign () is greater than 0 and is-1 when the sign () is less than 0.
3. The tracking control method of the non-road mobile pollution source detection laser radar as claimed in claim 2, wherein the second-order integral sliding mode tracking control of the pollution source detection laser radar is realized by using an integral sliding mode surface and a differentiator to construct a sliding mode angle controller to adjust the actual rotation angle of the joint of the scanning mechanism so as to obtain the torque applied to the scanning mechanism by a motor, and the second-order integral sliding mode tracking control of the pollution source detection laser radar comprises the following steps:
calculating the angular velocity and the angular acceleration of the joint rotation of the scanning mechanism by using a differentiator;
and constructing a sliding mode angle controller by using the integral sliding mode surface, the rotating angular speed and the angular acceleration of the scanning mechanism joint so as to adjust the actual rotating angle of the scanning mechanism joint and obtain the moment applied to the scanning mechanism by the motor.
4. The method as claimed in claim 3, wherein the formula for calculating the angular velocity and the angular acceleration of the joint rotation of the scanning mechanism by using the differentiator is as follows:
wherein Σ is a differentiator parameter and satisfies
The input of the differentiator is the actual angle q of the joint angle of the scanning mechanism, and the output psi of the differentiator0，ψ1，ψ2Are respectively used for estimating
Record as
5. The tracking control method of the non-road mobile pollution source detection laser radar as claimed in claim 3, wherein the sliding mode angle control law of the sliding mode angle controller is as follows:
wherein the content of the first and second substances,
representing the first derivative of the sliding mode surface s,
representing said desired angle qdSecond derivative of (a)1i,λ2i,eiRespectively represent matrices lambda1,λ2And matrix e and the ith diagonal element of (1)iThe (i) th element of (a),
is the second differential of q, K1，K2Is a constant coefficient diagonal matrix, U (t-t)d) Represents tdThe moment applied to the scanning mechanism by the motor before time, and u (t) represents the moment applied to the scanning mechanism by the motor at time t.
6. The method of claim 5, wherein the method further comprises tracking the off-road mobile pollution source detection lidar
7. The method as claimed in claim 1, wherein the step of establishing a joint rotation angle-moment dynamics equation of the scanning mechanism according to the established joint coordinate system and the dynamic parameters of the scanning mechanism comprises:
establishing a positive kinematic equation of the laser radar scanning mechanism according to the DH parameters corresponding to the joint coordinate system;
and establishing a joint rotation angle-moment dynamics equation of the scanning mechanism according to the positive kinematics equation of the laser radar scanning mechanism and the power parameters of the laser radar scanning mechanism.
8. The method as claimed in claim 1, wherein the step of establishing a joint rotation angle-moment dynamics equation of the scanning mechanism according to the positive kinematics equation of the lidar scanning mechanism and the dynamic parameters of the lidar scanning mechanism comprises:
based on a positive kinematic equation of the laser radar scanning mechanism, performing forward iteration and backward iteration by using a Newton-Euler method to obtain a joint rotation angle-moment dynamic model of the laser radar scanning mechanism;
and substituting the dynamic parameters of the laser radar scanning mechanism into the joint rotation angle-moment dynamic model to obtain the moment-joint angle dynamic equation.
9. The tracking control system for the non-road mobile pollution source detection laser radar is characterized by comprising a pollution source detection laser radar scanning mechanism and a controller, wherein the scanning mechanism comprises a base B0Azimuth rotary joint B1And roll rotary joint B3A base B0Azimuth rotary joint B1And roll rotary joint B2The controller comprises a coordinate system establishing module, a dynamic equation establishing module, an integral sliding mode surface constructing module and a second-order integral sliding mode tracking control module, wherein:
the coordinate system establishment module is used for utilizing a DH modeling method and using a base B0Azimuth rotary joint B1And roll rotary joint B2As joint points, respectively establishing joint coordinate systems;
the dynamic equation establishing module is used for establishing a joint rotation angle-moment dynamic equation of the scanning mechanism according to the established joint coordinate system and the power parameters of the scanning mechanism;
the integral sliding mode surface construction module is used for constructing an integral sliding mode surface by taking the difference value between the expected angle and the actual angle of the joint angle of the scanning mechanism as a tracking error;
and the second-order integral sliding mode tracking control module is used for constructing a sliding mode angle controller by utilizing an integral sliding mode surface and a differentiator so as to adjust the actual rotating angle of the joint of the scanning mechanism, obtain the moment applied to the scanning mechanism by the motor and realize the second-order integral sliding mode tracking control of the pollution source detection laser radar.
10. A computer-readable storage medium comprising a computer program for use in conjunction with a storage device, the computer program for execution by a processor, comprising the steps of:
using DH modeling method with base B0Azimuth rotary joint B1And roll rotary joint B2As joint points, respectively establishing joint coordinate systems;
establishing a joint rotation angle-moment dynamics equation of the scanning mechanism according to the established joint coordinate system and the power parameters of the scanning mechanism;
constructing an integral sliding mode surface by taking the difference value of the expected angle and the actual angle of the joint angle of the scanning mechanism as a tracking error;
and constructing a sliding mode angle controller by utilizing an integral sliding mode surface and a differentiator to adjust the actual rotating angle of the joint of the scanning mechanism, so as to obtain the moment applied to the scanning mechanism by the motor and realize the second-order integral sliding mode tracking control of the pollution source detection laser radar.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010733295.9A CN111948624A (en) | 2020-07-27 | 2020-07-27 | Tracking control method and system for non-road mobile pollution source detection laser radar |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010733295.9A CN111948624A (en) | 2020-07-27 | 2020-07-27 | Tracking control method and system for non-road mobile pollution source detection laser radar |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111948624A true CN111948624A (en) | 2020-11-17 |
Family
ID=73339634
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010733295.9A Pending CN111948624A (en) | 2020-07-27 | 2020-07-27 | Tracking control method and system for non-road mobile pollution source detection laser radar |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111948624A (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104049646A (en) * | 2014-06-11 | 2014-09-17 | 中国科学院长春光学精密机械与物理研究所 | Multi-directional control method for eliminating image rotation of multi-detector infusing scan imaging system |
| US20180196126A1 (en) * | 2017-01-10 | 2018-07-12 | Beijing Aerospace Institute For Metrology And Measurement Technology | Method for correcting measuring errors of long-distance scanning laser radar |
| CN109557524A (en) * | 2018-12-29 | 2019-04-02 | 安徽优思天成智能科技有限公司 | A kind of input saturation control method of marine exhaust monitoring laser radar servomechanism |
| CN109633605A (en) * | 2018-12-29 | 2019-04-16 | 安徽优思天成智能科技有限公司 | A kind of ADAPTIVE ROBUST follow-up control method of marine exhaust monitoring laser radar |
| CN109975788A (en) * | 2019-04-23 | 2019-07-05 | 中国科学技术大学 | A kind of self-adaptation control method of laser radar scanning mechanism |
| CN110007599A (en) * | 2019-04-23 | 2019-07-12 | 中国科学技术大学 | A kind of the Adaptive Integral sliding-mode control and system of laser radar scanning mechanism |
| CN110646775A (en) * | 2019-09-29 | 2020-01-03 | 凯迈（洛阳）测控有限公司 | Control method for quickly switching photoelectric radar from rotary scanning to staring mode |
-
2020
- 2020-07-27 CN CN202010733295.9A patent/CN111948624A/en active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104049646A (en) * | 2014-06-11 | 2014-09-17 | 中国科学院长春光学精密机械与物理研究所 | Multi-directional control method for eliminating image rotation of multi-detector infusing scan imaging system |
| US20180196126A1 (en) * | 2017-01-10 | 2018-07-12 | Beijing Aerospace Institute For Metrology And Measurement Technology | Method for correcting measuring errors of long-distance scanning laser radar |
| CN109557524A (en) * | 2018-12-29 | 2019-04-02 | 安徽优思天成智能科技有限公司 | A kind of input saturation control method of marine exhaust monitoring laser radar servomechanism |
| CN109633605A (en) * | 2018-12-29 | 2019-04-16 | 安徽优思天成智能科技有限公司 | A kind of ADAPTIVE ROBUST follow-up control method of marine exhaust monitoring laser radar |
| CN109975788A (en) * | 2019-04-23 | 2019-07-05 | 中国科学技术大学 | A kind of self-adaptation control method of laser radar scanning mechanism |
| CN110007599A (en) * | 2019-04-23 | 2019-07-12 | 中国科学技术大学 | A kind of the Adaptive Integral sliding-mode control and system of laser radar scanning mechanism |
| CN110646775A (en) * | 2019-09-29 | 2020-01-03 | 凯迈（洛阳）测控有限公司 | Control method for quickly switching photoelectric radar from rotary scanning to staring mode |
Non-Patent Citations (1)
| Title |
|---|
| 董天舒;何英姿;李克行;陈上上;刘贺龙;: "空间态势感知卫星非线性积分滑模控制", 航天控制, no. 06 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN100562711C (en) | Moving object method for estimating and system | |
| CN107560613B (en) | Robot indoor track tracking system and method based on nine-axis inertial sensor | |
| CN111678521B (en) | Method and system for evaluating positioning accuracy of mobile robot | |
| CN105404296A (en) | Two-wheeled self-balancing intelligent vehicle posture control method | |
| CN110007599B (en) | Self-adaptive integral sliding mode control method and system of laser radar scanning mechanism | |
| CN110209180A (en) | A kind of UAV navigation method for tracking target based on HuberM-Cubature Kalman filtering | |
| US11079366B2 (en) | Plume characterization using synchronized measurements of gas composition, wind direction, and wind speed | |
| CN111948624A (en) | Tracking control method and system for non-road mobile pollution source detection laser radar | |
| CN112147599A (en) | Spline function-based continuous-time external parameter calibration method for 3D laser radar and inertial sensor | |
| CN109807899A (en) | For the cooperation robotic friction torque compensation method of dragging teaching | |
| CN109633605A (en) | A kind of ADAPTIVE ROBUST follow-up control method of marine exhaust monitoring laser radar | |
| CN111761583B (en) | Intelligent robot motion positioning method and system | |
| Ljungberg et al. | Consistent estimators for nonlinear vessel models | |
| CN110543175A (en) | dynamic hybrid tracking control method for non-road mobile pollution source monitoring laser radar | |
| JP4015004B2 (en) | Helicopter airfoil simulation method | |
| CN115540854A (en) | Active positioning method, equipment and medium based on UWB assistance | |
| CN113514061A (en) | Geomagnetic device-based high-speed rotation system movement speed calculation method and device | |
| CN113324550A (en) | Flight path correction method based on improved flight path integral and storage medium | |
| Wu et al. | Real-time wind estimation with a quadrotor using BP neural network | |
| Nguyen et al. | Trajectory tracking for a multicopter under a quaternion representation | |
| Guo et al. | A model decomposition Kalman filter for enhanced localization of land vehicles | |
| Liu et al. | Vehicle state and parameter estimation based on adaptive robust unscented particle filter | |
| Zhang et al. | Using simulation to design an MPC policy for field navigation using GPS sensing | |
| Tsubokura et al. | HPC-LES for the Prediction of Unsteady Aerodynamic Forces on a Vehicle in a Gusty Cross-flow Condition | |
| Chen et al. | Kalman filter tracking algorithm simulation based on angle expansion |
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 |