CN112596377A - Double-drive equipment linear motion control method and device and double-drive equipment - Google Patents
Double-drive equipment linear motion control method and device and double-drive equipment Download PDFInfo
- Publication number
- CN112596377A CN112596377A CN202011481808.8A CN202011481808A CN112596377A CN 112596377 A CN112596377 A CN 112596377A CN 202011481808 A CN202011481808 A CN 202011481808A CN 112596377 A CN112596377 A CN 112596377A
- Authority
- CN
- China
- Prior art keywords
- motion
- angular acceleration
- double
- drive
- deviation value
- 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
- 230000033001 locomotion Effects 0.000 title claims abstract description 100
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000001133 acceleration Effects 0.000 claims abstract description 31
- 230000009977 dual effect Effects 0.000 claims description 20
- 238000013139 quantization Methods 0.000 claims description 8
- 238000013016 damping Methods 0.000 abstract description 5
- 230000001276 controlling effect Effects 0.000 description 13
- 238000010586 diagram Methods 0.000 description 6
- 238000006073 displacement reaction Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 2
- 230000001105 regulatory effect Effects 0.000 description 2
- 239000006227 byproduct Substances 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000004886 process control Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B11/00—Automatic controllers
- G05B11/01—Automatic controllers electric
- G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
- G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P.I., P.I.D.
Abstract
The application relates to the technical field of multi-drive equipment, and based on inertial feedback, a first-order closed-loop PID adjusts double-drive to form directional damping control linear track motion. Specifically discloses a method and a device for controlling linear motion of double-drive equipment and the double-drive equipment, wherein the method comprises the following steps: inputting an initial motion control quantity; acquiring angular acceleration components of the yaw direction of the gyroscope; carrying out quantitative processing on the angular acceleration component of the yaw direction of the gyroscope to output a deviation value of a yaw position angle; and adjusting the output motion control quantity-pwm in real time through a PID algorithm according to the deviation value so as to equalize the motion speeds of the left and right drives, so that the final motion track of the motion device is in a straight line.
Description
Technical Field
The application relates to the technical field of linear motion, in particular to a method and a device for controlling linear motion of double-drive equipment and the double-drive equipment.
Background
Due to the fact that factors such as target electric circuits, motor coefficient deviation, uneven stress of the device structure and external interference are numerous, the dual-drive (namely, the left and right independent driving sources) cannot realize relative linear motion, and a motion track of a left parabolic motion or a right parabolic motion is presented.
Referring to fig. 1, the ideal moving speed of the dual-drive R/L in the time period from t0 to t2 is v1, and the corresponding moving displacement (v1 × t2), the moving trajectory Ob, which is influenced by the above factors, is not stable continuously for v1, and is shown to change at time t 1. The left diagram v1 → b shows that the speed of the double drive (left drive and right drive) is uniform and consistent during the time period from t0 to t2, which is the ideal embodiment and corresponds to the trajectory of the segment of the right diagram Ob, i.e. linear motion; v1 → a, v1 → c, representing the velocity during the time period t0 to t2, it can be seen that the value of the velocity after t1 is not ideal and may not be very close to ideal, there is an acceleration, the displacement:
this is the embodiment of open loop control in practical application, and the motion trajectory of the open loop control will have left deviation or right deviation Oa and Oc.
Disclosure of Invention
The application provides a method and a device for controlling linear motion of double-drive equipment and the double-drive equipment, and the linear motion of the double-drive equipment is realized by a solid line.
In a first aspect, the present application provides a method for controlling linear motion of a dual drive apparatus, the method including:
inputting an initial motion control quantity;
acquiring angular acceleration components of the yaw direction of the gyroscope;
carrying out quantitative processing on the angular acceleration component of the yaw direction of the gyroscope to output a deviation value of a yaw position angle;
and adjusting the output motion control quantity in real time through a PID algorithm according to the deviation value so as to equalize the motion speeds of the left drive and the right drive, so that the final motion track of the motion device is a straight line.
In a second aspect, the present application further provides a dual drive type device linear motion control apparatus, the dual drive type vehicle model includes:
an input PWM unit for inputting an initial motion control amount;
an angular velocity acquisition unit for acquiring a gyro yaw direction angular acceleration component;
the angle deviation unit is used for carrying out vector quantization processing on the angular acceleration in the yaw direction of the gyroscope and outputting a deviation value of a yaw position angle;
and the PWM control unit adjusts the output motion control quantity in real time through a PID algorithm according to the deviation value so as to equalize the motion speeds of the left and right drives, so that the final motion track of the motion device is linear.
In a third aspect, the present application further provides a dual drive apparatus, which employs the above-mentioned method for controlling linear motion of the dual drive apparatus.
The application discloses a method and a device for controlling linear motion of double-drive equipment and the double-drive equipment, wherein angular acceleration vector data of a yaw direction of a gyroscope are acquired periodically; outputting a position angle deviation value through program operation; and regulating and outputting PWM control quantity through a PID algorithm, and adjusting linear motion. The method is based on inertial feedback, and the first-order closed-loop PID adjusts double-drive to form directional damping to control linear track motion.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings needed to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the following description are some embodiments of the present application, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
FIG. 1 is a graph of dual drive motion profile;
FIG. 2 is a schematic flow chart of a method for controlling linear motion of a dual drive device provided by an embodiment of the present application;
FIG. 3 is a flow diagram illustrating sub-steps of a dual drive device linear motion control method of FIG. 2;
fig. 4 is a schematic view of an application scenario of a linear motion control method provided in an embodiment of the present application;
fig. 5 is a schematic block diagram of a dual drive type device linear motion control apparatus provided in an embodiment of the present application.
Detailed Description
The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are some, but not all, embodiments of the present application. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
The flow diagrams depicted in the figures are merely illustrative and do not necessarily include all of the elements and operations/steps, nor do they necessarily have to be performed in the order depicted. For example, some operations/steps may be decomposed, combined or partially combined, so that the actual execution sequence may be changed according to the actual situation.
The embodiment of the application provides a method for controlling linear motion of double-drive equipment. The method for controlling the linear motion of the double-drive equipment can be applied to double-drive type vehicle models and double-drive type robots to realize the linear motion.
It should be noted that the method for controlling the linear motion of the dual-drive type equipment is used for controlling the linear motion of the dual-drive type mechanical device, and is not limited to the dual-drive type vehicle model and the dual-drive type robot, but may be used for other motion equipment, such as a floor-mopping robot, an automatic cargo-transporting robot, and the like.
Some embodiments of the present application will be described in detail below with reference to the accompanying drawings. The embodiments described below and the features of the embodiments can be combined with each other without conflict.
Referring to fig. 2, fig. 2 is a schematic flowchart of a method for controlling linear motion of a dual-drive apparatus according to an embodiment of the present application. The method for controlling the linear motion of the dual drive type equipment comprises the steps of S101 to S104.
And S101, inputting an initial motion control amount.
Specifically, an initial movement control amount such as forward, backward, movement along a route, or the like is input. After the initial motion control quantity is input, the double-drive equipment cannot realize relative linear motion due to various factors such as target electric circuits, motor coefficient deviation, uneven stress of the device structure, external interference and the like, and the motion track of a left parabolic object or a right parabolic object is presented, so that the initial motion control quantity (pwm _ input) needs to be subjected to damping adjustment, and finally, the adjusted output motion control quantity (pwm _ output) is output.
And S102, gyroscope yaw direction angular acceleration vector data.
The method is suitable for equipment with two motion drives, such as a double-drive robot and the like.
Specifically, the gyro sensor periodically collects and outputs directional vector angular acceleration.
S103, carrying out quantitative processing on angular acceleration vector data of the yaw direction of the gyroscope to output deviation values of the position angles.
Specifically, by vector conversion, and the quantization output ± 2 pi ═ 360 °. The method and the device introduce an input motion control quantity to control the linear motion of the equipment, output the angle deviation value of the position through calculating the yaw direction of the gyroscope, and then adjust and control the output of the double-drive equipment to achieve the effect of the linear motion.
And S104, adjusting the output motion control quantity in real time through a PID algorithm according to the deviation value so as to equalize the motion speeds of the left drive and the right drive, so that the final motion track of the motion device is linear.
Among them, the PID algorithm is a PID controller (also called PID regulator) that controls according to the proportion (P), integral (I) and derivative (D) of the deviation in the process control, and is the most widely used automatic controller. The method has the advantages of simple principle, easy realization, wide application range, mutually independent control parameters, simpler parameter selection and the like. And substituting the calculated yaw direction angle deviation value into a PID algorithm to adjust PWM control quantity so as to realize the linear motion of the dual-drive equipment.
The control method provided by the embodiment adjusts linear motion by introducing the input motion control quantity, firstly adopts the directional angular acceleration component of the gyroscope sensor, calculates the output directional angular deviation value by product error, and finally substitutes the calculated output directional angular deviation value into the PID algorithm to adjust the output motion control quantity, thereby realizing the linear motion of the dual-drive equipment. The invention is based on inertia feedback, and a first-order closed loop PID adjusts multi-drive to form directional damping to control linear track motion.
Referring to FIG. 3, the step of calculating the angular velocity deviation value of each driving output position includes steps S1021-S1023.
And S1021, carrying out differential control on the deviation value of the original angular acceleration quantization output position angle of the inertial gyroscope.
Outputting the angular displacement deltaS (v) of the instant timei-vi-1) Is the angular acceleration, y is the sensor's quantitative coefficient, quantified as raw position angle data (+ -360 °), dt differential time.
And S1021, integral control of the deviation value of the original angular acceleration quantization output position angle of the inertial gyroscope.
Visually outputting yaw angle data (+ -360 degrees); wherein when
When the output is zero, the low-pass filtering is performed,
and taking a low-frequency noise coefficient.
And S1021, acquiring the error of the position angle at the time t.
And (3) taking the error of the position angle at the time t:
err(t)＝yaw_erri＝(yawi-yawi-1) ③
and then substituting the formula III into the prototype PID algorithm to fuse the input control quantity, and obtaining a control equation as follows:
and performing proportional, integral and differential control on err (t), and outputting pwm R/L driving control to form rapid damping.
Referring to fig. 4, fig. 4 is a schematic view of an application scenario of a method for controlling linear motion of a dual-drive apparatus according to an embodiment of the present application. The application scene comprises a left drive, a right drive, an inertial feedback device (GY-Z), an MCU and a mobile power supply device. The inertial feedback device is used for periodically obtaining yaw direction angular acceleration vector data, the MCU is used for carrying out central processing on input transport calculation control, carrying out a series of operations such as integral of yaw angular acceleration of the inertial feedback device (gyroscope) and the like to obtain a direction deviation value, and regulating and outputting PWM (pulse width modulation) control double drive through a PID (proportion integration differentiation) algorithm, so that adjustment is in linear motion.
Referring to fig. 5, fig. 5 is a schematic block diagram of a linear motion control apparatus of a dual drive device according to an embodiment of the present application.
As shown in fig. 5, the dual drive type device linear motion control apparatus 200 includes: an input PWM unit 201, an angular acceleration acquisition unit 202, an angle deviation unit 203, and an output PWM control unit 204.
The input PWM unit 201 is used to input an initial motion control amount.
An angular velocity acquisition unit 202 for acquiring an angular acceleration of the gyroscope.
And an angle deviation unit 203 for performing quantization processing on the gyroscope yaw direction angular acceleration vector data to output a deviation value of the position angle.
And the PWM control unit 204 is used for adjusting the input motion control quantity in real time through a PID algorithm according to the deviation value so as to enable the left and right driving motions to be linear.
It should be noted that, as will be clear to those skilled in the art, for convenience and brevity of description, the specific working processes of the apparatus and the units described above may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
The invention also provides double-drive equipment, which adopts the linear motion control method of the double-drive equipment. The dual drive device may be a dual drive vehicle model, a dual drive robot, or the like. The method comprises the steps of inputting an initial motion control quantity; acquiring angular acceleration components of the yaw direction of the gyroscope; carrying out quantization processing on angular acceleration vector data of the yaw direction of the gyroscope to output a deviation value of a position angle; and adjusting the output motion control quantity in real time through a PID algorithm according to the deviation value so as to equalize the motion speeds of the left drive and the right drive, so that the final motion track of the motion device is a straight line.
While the invention has been described with reference to specific embodiments, the scope of the invention is not limited thereto, and those skilled in the art can easily conceive various equivalent modifications or substitutions within the technical scope of the invention. Therefore, the protection scope of the present application shall be subject to the protection scope of the claims.
Claims (6)
1. A method for controlling linear motion of double-drive equipment is characterized by comprising the following steps:
inputting an initial motion control quantity;
acquiring angular acceleration components of the yaw direction of the gyroscope;
carrying out quantitative processing on the angular acceleration component of the yaw direction of the gyroscope to output a deviation value of a yaw position angle;
and adjusting the output motion control quantity in real time through a PID algorithm according to the deviation value so as to equalize the motion speeds of the left drive and the right drive, so that the final motion track of the motion device is a straight line.
2. The dual drive device linear motion control method according to claim 1, wherein in the periodically acquiring the gyro yaw direction angular acceleration component, gyro yaw direction angular acceleration data is periodically acquired by an inertial feedback device.
3. The dual drive device linear motion control method according to claim 2, wherein the step of quantizing the angular acceleration component in the yaw direction of the gyroscope to output the deviation value of the yaw position angle comprises:
differential control of the deviation value of the original angular acceleration quantization output position angle of the inertial gyroscope;
integral control of the deviation value of the original angular acceleration quantization output position angle of the inertial gyroscope;
and acquiring the error of the position angle at the time t.
4. The double drive apparatus linear motion control method according to claim 1, wherein the formula of the output control amount is,
where err (t) is the error in position angle at time t, pwminputTo input a motion control amount.
5. A dual drive type equipment linear motion control device is characterized by comprising:
an input PWM unit for inputting an initial motion control amount;
an angular velocity acquisition unit for acquiring a gyro yaw direction angular acceleration component;
the angle deviation unit is used for carrying out quantitative processing on the angular acceleration component in the yaw direction of the gyroscope and outputting a deviation value of a yaw position angle;
and the PWM control unit is used for adjusting the output motion control quantity in real time through a PID algorithm according to the deviation value so as to equalize the motion speeds of the left and right drives, and the final motion track of the motion device is linear.
6. A dual drive apparatus comprising a method of controlling linear motion of the dual drive apparatus as claimed in any one of claims 1 to 4.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011481808.8A CN112596377A (en) | 2020-12-16 | 2020-12-16 | Double-drive equipment linear motion control method and device and double-drive equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011481808.8A CN112596377A (en) | 2020-12-16 | 2020-12-16 | Double-drive equipment linear motion control method and device and double-drive equipment |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112596377A true CN112596377A (en) | 2021-04-02 |
Family
ID=75195973
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011481808.8A Pending CN112596377A (en) | 2020-12-16 | 2020-12-16 | Double-drive equipment linear motion control method and device and double-drive equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112596377A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115373259A (en) * | 2022-09-23 | 2022-11-22 | 北京激扬时代健身科技有限公司 | Self-calibration method of motor-driven force instrument capable of correcting errors |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107422733A (en) * | 2017-07-27 | 2017-12-01 | 南京理工大学 | A kind of motion control method based on two wheel guide robot robot |
| CN108681329A (en) * | 2018-05-10 | 2018-10-19 | 哈尔滨工业大学 | Rocket and its posture based on controllable rudder face independently correct control method |
| CN109343539A (en) * | 2018-11-27 | 2019-02-15 | 江苏红石信息系统集成服务有限公司 | Motion control method, device, robot and storage medium |
| CN111497637A (en) * | 2020-05-29 | 2020-08-07 | 浙江同筑科技有限公司 | Motion control method for AGV with four steering wheels |
-
2020
- 2020-12-16 CN CN202011481808.8A patent/CN112596377A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107422733A (en) * | 2017-07-27 | 2017-12-01 | 南京理工大学 | A kind of motion control method based on two wheel guide robot robot |
| CN108681329A (en) * | 2018-05-10 | 2018-10-19 | 哈尔滨工业大学 | Rocket and its posture based on controllable rudder face independently correct control method |
| CN109343539A (en) * | 2018-11-27 | 2019-02-15 | 江苏红石信息系统集成服务有限公司 | Motion control method, device, robot and storage medium |
| CN111497637A (en) * | 2020-05-29 | 2020-08-07 | 浙江同筑科技有限公司 | Motion control method for AGV with four steering wheels |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115373259A (en) * | 2022-09-23 | 2022-11-22 | 北京激扬时代健身科技有限公司 | Self-calibration method of motor-driven force instrument capable of correcting errors |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112904728B (en) | Mechanical arm sliding mode control track tracking method based on improved approach law | |
| JP2011501293A (en) | Method and system for satisfying termination condition in motion control system | |
| WO2013133346A1 (en) | Robot controlling device, robot apparatus, robot control method, program for executing robot control method, and recording medium on which program is recorded | |
| US9354651B2 (en) | Sizing and tuning methodology for optimized motion control components and energy efficiency | |
| WO2013140679A1 (en) | Track control device | |
| Kumar et al. | Control and tracking of robotic manipulator using PID controller and hardware in Loop Simulation | |
| CN112596377A (en) | Double-drive equipment linear motion control method and device and double-drive equipment | |
| CN110955192B (en) | Servo control device, robot, and servo control method | |
| CN106426163A (en) | Control apparatus of motor | |
| CN109857108B (en) | Mobile robot track tracking method and system based on internal model control algorithm | |
| Schulken et al. | Model-free sliding mode control algorithms including application to a real-world quadrotor | |
| Qi et al. | Three-dimensional formation control based on filter backstepping method for multiple underactuated underwater vehicles | |
| Mathew | Design, simulation and implementation of cascaded path tracking controller for a differential drive mobile robot | |
| Huber et al. | Online trajectory optimization for nonlinear systems by the concept of a model control loop—Applied to the reaction wheel pendulum | |
| Kareem et al. | Robust stability control of inverted pendulum model for bipedal walking robot | |
| JP2019008707A (en) | Servo controller, servo control method and system | |
| Majczak et al. | Comparison of two efficient control strategies for two-wheeled balancing robot | |
| CN107290959B (en) | Robot Cartesian space velocity optimization method based on displacement equivalence | |
| US20030173928A1 (en) | Servo control method | |
| CN115179300A (en) | Flexible mechanical arm trajectory tracking control method for preset time | |
| Lamraoui et al. | Speed tracking control of unicycle type mobile robot based on LADRC | |
| CN114879508A (en) | Grinding robot path tracking control method based on model prediction control | |
| Aguilar-Ibáñez et al. | A linear differential flatness approach to controlling the Furuta pendulum | |
| CN109213002B (en) | Nonlinear dynamic inverse control variable amplitude speed regulation system simulation model and method | |
| Jung et al. | The operational space formulation on humanoids considering joint stiffness and bandwidth limit |
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 |