CN112276911A - Grab arm type trash cleaning robot motion control system - Google Patents

Grab arm type trash cleaning robot motion control system Download PDF

Info

Publication number
CN112276911A
CN112276911A CN202011246801.8A CN202011246801A CN112276911A CN 112276911 A CN112276911 A CN 112276911A CN 202011246801 A CN202011246801 A CN 202011246801A CN 112276911 A CN112276911 A CN 112276911A
Authority
CN
China
Prior art keywords
cleaning robot
track
acceleration
angle
joint
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
Application number
CN202011246801.8A
Other languages
Chinese (zh)
Inventor
黄振东
高云峰
陈荣娜
王亚
王之辉
袁家宝
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Anhui Lu'an Hengyuan Machinery Co ltd
Original Assignee
Anhui Lu'an Hengyuan Machinery Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Anhui Lu'an Hengyuan Machinery Co ltd filed Critical Anhui Lu'an Hengyuan Machinery Co ltd
Priority to CN202011246801.8A priority Critical patent/CN112276911A/en
Publication of CN112276911A publication Critical patent/CN112276911A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/0081—Programme-controlled manipulators with master teach-in means
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1612—Programme controls characterised by the hand, wrist, grip control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • General Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses a grab arm type trash cleaning robot motion control system, which is used for thoroughly cleaning up garbage and dirt adhered and wound on a grid, wherein grab bucket claw teeth of a trash cleaning machine are required to extend into the grid of a trash rack, and meanwhile, the grab bucket cannot be scraped and collided with the trash rack and runs along the grid along a set track from top to bottom. The operation track of the grab bucket is required to be smooth and continuous in the work engineering of the trash remover, and the continuous change of the speed, the acceleration and the pulsation of each joint node and the execution efficiency of the joint node are well controlled. The cleaning robot is a planar three-degree-of-freedom series robot, the tail end posture rotates around an axis vertical to the plane, the track planning method of the tail end executor is that a track planning model is constructed according to the actual cleaning path of the cleaning robot, the whole path is divided into 5 stages, path interpolation is respectively carried out by adopting interpolation modes such as sine acceleration and deceleration and quintic polynomial, and the track of each hydraulic cylinder is smooth and continuous after the track planning.

Description

Grab arm type trash cleaning robot motion control system
Technical Field
The invention relates to a motion control system, in particular to a motion control system of a grab arm type trash cleaning robot, and belongs to the technical field of motion control of trash cleaning robots.
Background
The trash cleaning robot is equipment for intercepting and cleaning sundries in front of a water retaining gate, a water retaining dam and a water inlet of hydroelectric power generation equipment in a water conservancy facility. The in-process of decontaminating relies on the grapple to push down by force, can realize simultaneously that the clearance of filth on the trash rack and the snatching of filth before the trash rack compare in other equipment of decontaminating: the grab arm type trash cleaning robot can effectively improve the trash cleaning effect and the trash cleaning efficiency, and particularly can effectively clean high-strength upstream trash jammed in front of a trash rack of a water taking and discharging port in time.
In order to thoroughly clean up the garbage and dirt adhered to and wound on the grid bars, claw teeth of a grab bucket of the trash remover are required to extend into a grid groove of the trash rack, and meanwhile, the grab bucket must be ensured not to be scraped and collided with the trash rack and to move along the grid bars from top to bottom according to a set track. The operation track of the grab bucket is required to be smooth and continuous in the work engineering of the trash remover, and the continuous change of the speed, the acceleration and the pulsation of each joint node and the execution efficiency of the joint node are well controlled. However, at present, no research on the control system of the cleaning robot exists, and no mature product which can be applied to the market exists, so that a motion control system of the grab arm type cleaning robot is urgently needed, and the motion tracks and the speeds of all hydraulic cylinders are respectively planned, so that the positions and speed curves of all joints are continuous without sudden change, and the transition between path points is smooth.
In order to solve the above technical problems, the present invention provides the following technical solutions.
Disclosure of Invention
The invention aims to provide a motion control system of a grab arm type trash cleaning robot, which plans motion tracks and speeds of all hydraulic cylinders respectively, so that positions and speed curves of all joints are continuous without sudden change, and transition among path points is smooth. According to the motion requirement of the cleaning robot, the motion track is smooth, continuous and has no pause in the cleaning process, so that the motion path from P1 to P3 is divided into 5 stages. A P1-A acceleration stage, an A-B first straight line uniform speed stage, a B-C straight line inter-segment track transition stage, a C-D second straight line uniform speed stage and a D-P3 deceleration stage. Therefore, the azimuth angle phi and the end positions y and z adopt the same track planning mode. In order to smooth start and stop of acceleration and deceleration, a sinusoidal acceleration and deceleration track planning method is adopted in the acceleration section and the deceleration section. In order to ensure that the motion track is continuous and has no pause, a 5-degree polynomial is adopted to perform transition of two linear tracks, namely a transition starting point B position P1, a speed V1, acceleration 0, a transition ending point C position P2, a speed V2 and acceleration 0.
The purpose of the invention can be realized by the following technical scheme:
the motion control system hardware of the decontamination robot mainly comprises an embedded controller, a servo proportional valve, a hydraulic pump, a proximity sensor, a dynamic inclinometer and the like. The main control equipment is an embedded controller used for industrial control tasks, can provide a real-time running environment with a period of 2ms, realizes establishment of kinematics, a track model and an online correction model, and realizes acquisition of sensor information and control of a servo proportional valve through an I/O module of the main control equipment, so that the decontamination robot is controlled to move according to a planned track and perform real-time online correction on a tail end track.
A motion control system of a grab arm type trash cleaning robot comprises a database management module, a motion control module, an operation module, an abnormality detection module and a data acquisition module; the motion control module comprises a data storage unit, a track planning unit, a kinematics unit, a communication unit, a teaching unit, a safety management unit and a human-computer interaction unit;
the track planning unit is used for carrying out grabbing arm space track planning on the grabbing arm type trash cleaning robot, and the specific planning process comprises the following steps:
the method comprises the following steps: setting the initial position of the cleaning robot as P1, the upper end point of the trash rack as P2 and the river surface as P3 according to the actual cleaning path of the cleaning robot;
step two: in the process of cleaning, the cleaning robot moves downwards from an initial position P1 to reach an upper end point P2 of the trash rack, then moves downwards along the trash rack, the claw teeth of the grab bucket at the tail end of the cleaning robot coincide with the trash rack, and the cleaning robot reaches the river surface P3 to grab dirt;
step three: performing transition of two linear tracks by using a polynomial of degree 5, wherein the polynomial is at a transition starting point B position P1, a velocity V1, an acceleration 0, a transition ending point C position P2, a velocity V2 and an acceleration 0;
step four: constructing a track planning model, dividing a whole course path into 5 stages, respectively performing path interpolation by adopting interpolation modes such as sine acceleration and deceleration, quintic polynomial and the like, performing a P1-A acceleration stage, a A-B first stage straight line constant speed stage, a B-C straight line inter-stage track transition stage, a C-D second stage straight line constant speed stage, a D-P3 deceleration stage and an azimuth angle
Figure RE-GDA0002856903880000031
The same track planning mode is adopted for the terminal positions y and z;
step five: planning the actual motion track of the cleaning robot according to the planning method, setting the constant speed vc and the maximum acceleration amax according to the data of the positions and the azimuth angles of the points P1, P2 and P3 obtained by actual teaching, planning the motion track of the tail end, resolving through inverse kinematics to obtain the motion track of each hydraulic cylinder, and respectively planning the motion track and the speed of the grab bucket, so that the position and the speed curve of each joint are continuous without sudden change, and the transition between path points is smooth.
Preferably, the database management module is used for realizing the establishment and the storage of data variables and program texts; the motion control module is used for realizing point-to-point motion, joint interpolation, terminal linear interpolation and track point transition interpolation, and controlling the position, speed, acceleration and terminal attitude of each shaft; the operation module is used for realizing teaching, reproduction and program compiling operation, manually controlling single-axis inching, linearly moving the tail end and automatically operating a program; the abnormity detection module is used for realizing sudden stop abnormity, servo abnormity and limit abnormity; the data acquisition module is used for integrating a distance measuring sensor and detecting and correcting the position of the tail end in real time.
Preferably, the data storage unit is used for storing the taught position point and track program and the parameter template established by the user, so that the repeated calling performance of the program is improved; the track planning unit is used for carrying out interpolation on acceleration, speed, position and posture by adopting an interpolation algorithm in a joint space and a terminal Cartesian space to form a smooth motion curve; the kinematics unit is used for establishing a coordinate system of each joint axis, measuring DH parameters and compiling positive and negative kinematics of the mechanical arm; the communication unit is used for realizing data transmission between the demonstrator and the hydraulic servo controller through an industrial Ethernet; the teaching unit is used for manually controlling the single-axis point motion and the tail end linear motion of the mechanical arm through a demonstrator, teaching a required motion point and forming a motion track and a program; the safety management unit is used for feeding back abnormal conditions in real time in the motion process, reporting errors, responding to an emergency stop button and initializing; the man-machine interaction unit is used for completing the operation, display and interaction functions of all the modules.
Preferably, the trash cleaning robot is used for cleaning garbage and dirt attached and wound on the grid bars, the trash cleaning robot extends claw teeth of the grab bucket into the grid bars of the trash rack, the grab bucket does not scrape and collide with the trash rack, and the grab bucket runs along the grid bars from top to bottom according to a set track; the cleaning robot is a planar three-freedom-degree series robot, and the tail end posture rotates around an axis vertical to the plane.
Preferably, the kinematics unit is used for solving the position and the posture of the comb teeth in the motion space according to the parameter size of the trash cleaning robot; the specific solving process is as follows:
s1: solving the Q position and the large arm joint angle theta according to the plane geometric relation1Angle theta of the forearm joint2The functional relationship with the Q position (y, z) is as follows:
y＝lAEcosθ1+lEQcos(θ1+θ2)
z＝lAEsinθ1+lEQsin(θ1+θ2)
in the formula IAEIs a big armLength of connecting rod lEQThe length of the small arm connecting rod;
s2: solving the attitude of RS, the angles in the plane are directly added, so the angle theta of the large arm joint1Angle theta of the forearm joint2Angle theta of grab bucket joint3Angle theta of comb teeth4The sum is the azimuth angle of RS
Figure RE-GDA0002856903880000041
Figure RE-GDA0002856903880000042
In the formula, theta4Is the angle of the comb teeth with respect to the grab bucket, and theta4Is a constant value;
s3: obtaining the functional relation between the angle of the large arm joint, the angle of the small arm joint, the angle of the grab bucket joint and the angle of the comb teeth, wherein the stroke of the corresponding hydraulic cylinder is measured by each encoder, and the nonlinear relation is needed to be solved between the angle of each rotary joint and the stroke of the corresponding driving hydraulic cylinder;
according to the cosine theorem, the stroke l of the large-arm hydraulic cylinderBDConverting into angle DAB:
Figure RE-GDA0002856903880000051
in the formula IAD、lABThe distance between two end points of the large-arm hydraulic cylinder and the original point A is lBDForming a triangle;
s4: big arm joint angle theta1Relation with angle ≈ DAB:
θ1＝∠DAB+∠CAB-π/2-∠DAE
in the formula, the angle CAB and the angle DAE are joint angles of two end points of the large-arm hydraulic cylinder;
s5: in the same way, the angle theta of the forearm joint can be obtained2And the stroke l of the small arm hydraulic cylinderFGThe relationship of (1):
Figure RE-GDA0002856903880000052
θ2＝π-∠GEF-∠GEA-∠FEQ
in the formula IEG、lEFThe distance between two end points of the small arm hydraulic cylinder and the original point E is lFGForming a triangle; the angle GEA and the angle FEQ are joint angles of two end points of the small arm hydraulic cylinder;
forearm joint angle theta3Relationship to the boom cylinder stroke:
Figure RE-GDA0002856903880000053
θ3＝∠NQE+∠NQR-π
in the formula IQN、lQRThe distance between two end points of the grab bucket hydraulic cylinder and the original point Q is lNRThe angle NQE is the joint angle of one end point of the grab bucket hydraulic cylinder.
Preferably, the track planning method of sinusoidal acceleration and deceleration is adopted in the acceleration stage and the deceleration stage:
labeling the acceleration as a (t);
Figure RE-GDA0002856903880000054
in the formula, amax is the maximum acceleration, omega is an unknown parameter, 0-T1 is acceleration time, and T2-T is deceleration time;
integrating the acceleration formula with respect to time t to obtain a velocity v (t);
Figure RE-GDA0002856903880000061
integrating the velocity formula with respect to time t again to obtain displacement s (t);
Figure RE-GDA0002856903880000062
step five: the motion track enters a constant speed stage after being accelerated in a sine way, the constant speed is Vc, when t is t1, a (t) is 0, v (t) is Vc, and the motion track can be obtained
Figure RE-GDA0002856903880000063
In the same way, the deceleration time can be obtained;
step six: performing transition of two linear tracks by using a polynomial of degree 5, wherein the polynomial is at a transition starting point B position P1, a velocity V1, an acceleration 0, a transition ending point C position P2, a velocity V2 and an acceleration 0; the mathematical expression is as follows:
m0＝p1 m1＝v1 m2＝0
m3＝(20(p2-p1)-(12v1+8v2)Te)/(2Te 3)
m4＝(-30(p2-p1)+(16v1+14v2)Te)/(2Te 4)
m5＝(12(p2-p1)-(6v1+6v2)Te)/(2Te 5)
s(t)＝m5t5+m4t4+m3t3+m2t2+m1t+m0
in the formula, Te is the transition trace time.
Compared with the prior art, the invention has the beneficial effects that:
1. the motion control system hardware of the decontamination robot mainly comprises an embedded controller, a servo proportional valve, a hydraulic pump, a proximity sensor, a dynamic inclinometer and the like. The main control equipment is an embedded controller used for industrial control tasks, can provide a real-time running environment with a period of 2ms, realizes establishment of kinematics, a track model and an online correction model, and realizes acquisition of sensor information and control of a servo proportional valve through an I/O module of the main control equipment, so that the decontamination robot is controlled to move according to a planned track and perform real-time online correction on a tail end track.
2. In order to thoroughly clean up the garbage and dirt adhered to and wound on the grid bars, claw teeth of a grab bucket of the trash remover are required to extend into the grid bars of the trash rack, and meanwhile, the grab bucket must be ensured not to scrape and touch the trash rack and runs along the grid bars from top to bottom according to a set track. The operation track of the grab bucket is required to be smooth and continuous in the work engineering of the trash remover, and the continuous change of the speed, the acceleration and the pulsation of each joint node and the execution efficiency of the joint node are well controlled. The robot for cleaning the sewage is a planar three-degree-of-freedom series robot, the tail end posture rotates around an axis vertical to the plane, the track planning method of the tail end actuator is that a track planning model is constructed according to the actual sewage cleaning path of the robot for cleaning the sewage, the whole path is divided into 5 stages, path interpolation is carried out by adopting interpolation modes such as sine acceleration and deceleration and quintic polynomial, and the track of each hydraulic cylinder is smooth and continuous after the track planning.
3. According to the motion requirement of the cleaning robot, the motion track is smooth, continuous and has no pause in the cleaning process, so that the motion path from P1 to P3 is divided into 5 stages. A P1-A acceleration stage, an A-B first straight line uniform speed stage, a B-C straight line inter-segment track transition stage, a C-D second straight line uniform speed stage and a D-P3 deceleration stage. Therefore, the azimuth angle phi and the end positions y and z adopt the same track planning mode. In order to smooth start and stop of acceleration and deceleration, a sinusoidal acceleration and deceleration track planning method is adopted in the acceleration section and the deceleration section. In order to ensure that the motion track is continuous and has no pause, a 5-degree polynomial is adopted to perform transition of two linear tracks, namely a transition starting point B position P1, a speed V1, acceleration 0, a transition ending point C position P2, a speed V2 and acceleration 0.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
Fig. 1 is a block diagram of a motion control system of a trash cleaning robot of the present invention.
Detailed Description
The technical solutions of the present invention will be described clearly and completely with reference to the following embodiments, and it should be understood that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. 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 invention.
Referring to fig. 1, a motion control system of a grab arm type trash cleaning robot includes a database management module, a motion control module, an operation module, an abnormality detection module, and a data acquisition module; the motion control module comprises a data storage unit, a track planning unit, a kinematics unit, a communication unit, a teaching unit, a safety management unit and a human-computer interaction unit;
the track planning unit is used for carrying out grabbing arm space track planning on the grabbing arm type trash cleaning robot, and the specific planning process comprises the following steps:
the method comprises the following steps: setting the initial position of the cleaning robot as P1, the upper end point of the trash rack as P2 and the river surface as P3 according to the actual cleaning path of the cleaning robot;
step two: in the process of cleaning, the cleaning robot moves downwards from an initial position P1 to reach an upper end point P2 of the trash rack, then moves downwards along the trash rack, the claw teeth of the grab bucket at the tail end of the cleaning robot coincide with the trash rack, and the cleaning robot reaches the river surface P3 to grab dirt;
step three: performing transition of two linear tracks by using a polynomial of degree 5, wherein the polynomial is at a transition starting point B position P1, a velocity V1, an acceleration 0, a transition ending point C position P2, a velocity V2 and an acceleration 0;
step four: constructing a track planning model, dividing a whole course path into 5 stages, respectively performing path interpolation by adopting interpolation modes such as sine acceleration and deceleration, quintic polynomial and the like, performing a P1-A acceleration stage, a A-B first stage straight line constant speed stage, a B-C straight line inter-stage track transition stage, a C-D second stage straight line constant speed stage, a D-P3 deceleration stage and an azimuth angle
Figure RE-GDA0002856903880000081
The same track planning mode is adopted for the terminal positions y and z;
step five: planning the actual motion track of the cleaning robot according to the planning method, setting the constant speed vc and the maximum acceleration amax according to the data of the positions and the azimuth angles of the points P1, P2 and P3 obtained by actual teaching, planning the motion track of the tail end, resolving through inverse kinematics to obtain the motion track of each hydraulic cylinder, and respectively planning the motion track and the speed of the grab bucket, so that the position and the speed curve of each joint are continuous without sudden change, and the transition between path points is smooth.
Preferably, the database management module is used for realizing the establishment and the storage of data variables and program texts; the motion control module is used for realizing point-to-point motion, joint interpolation, terminal linear interpolation and track point transition interpolation, and controlling the position, speed, acceleration and terminal attitude of each shaft; the operation module is used for realizing teaching, reproduction and program compiling operation, manually controlling single-axis inching, linearly moving the tail end and automatically operating a program; the abnormity detection module is used for realizing sudden stop abnormity, servo abnormity and limit abnormity; the data acquisition module is used for integrating a distance measuring sensor and detecting and correcting the position of the tail end in real time.
Preferably, the data storage unit is used for storing the taught position point and track program and the parameter template established by the user, so that the repeated calling performance of the program is improved; the track planning unit is used for carrying out interpolation on acceleration, speed, position and posture by adopting an interpolation algorithm in a joint space and a terminal Cartesian space to form a smooth motion curve; the kinematics unit is used for establishing a coordinate system of each joint axis, measuring DH parameters and compiling positive and negative kinematics of the mechanical arm; the communication unit is used for realizing data transmission between the demonstrator and the hydraulic servo controller through an industrial Ethernet; the teaching unit is used for manually controlling the single-axis point motion and the tail end linear motion of the mechanical arm through a demonstrator, teaching a required motion point and forming a motion track and a program; the safety management unit is used for feeding back abnormal conditions in real time in the motion process, reporting errors, responding to an emergency stop button and initializing; the man-machine interaction unit is used for completing the operation, display and interaction functions of all the modules.
Preferably, the trash cleaning robot is used for cleaning garbage and dirt attached and wound on the grid bars, the trash cleaning robot extends claw teeth of the grab bucket into the grid bars of the trash rack, the grab bucket does not scrape and collide with the trash rack, and the grab bucket runs along the grid bars from top to bottom according to a set track; the cleaning robot is a planar three-freedom-degree series robot, and the tail end posture rotates around an axis vertical to the plane.
Preferably, the kinematics unit is used for solving the position and the posture of the comb teeth in the motion space according to the parameter size of the trash cleaning robot; the specific solving process is as follows:
s1: solving the Q position and the large arm joint angle theta according to the plane geometric relation1Angle theta of the forearm joint2The functional relationship with the Q position (y, z) is as follows:
y＝lAEcosθ1+lEQcos(θ1+θ2)
z＝lAEsinθ1+lEQsin(θ1+θ2)
in the formula IAELength of the connecting rod of the big arm lEQThe length of the small arm connecting rod;
s2: solving the attitude of RS, the angles in the plane are directly added, so the angle theta of the large arm joint1Angle theta of the forearm joint2Angle theta of grab bucket joint3Angle theta of comb teeth4The sum is the azimuth angle of RS
Figure RE-GDA0002856903880000103
Figure RE-GDA0002856903880000104
In the formula, theta4Is the angle of the comb teeth with respect to the grab bucket, and theta4Is a constant value;
s3: obtaining the functional relation between the angle of the large arm joint, the angle of the small arm joint, the angle of the grab bucket joint and the angle of the comb teeth, wherein the stroke of the corresponding hydraulic cylinder is measured by each encoder, and the nonlinear relation is needed to be solved between the angle of each rotary joint and the stroke of the corresponding driving hydraulic cylinder;
according to the cosine theorem, the stroke l of the large-arm hydraulic cylinderBDConverting into angle DAB:
Figure RE-GDA0002856903880000101
in the formula IAD、lABThe distance between two end points of the large-arm hydraulic cylinder and the original point A is lBDForming a triangle;
s4: big arm joint angle theta1Relation with angle ≈ DAB:
θ1＝∠DAB+∠CAB-π/2-∠DAE
in the formula, the angle CAB and the angle DAE are joint angles of two end points of the large-arm hydraulic cylinder;
s5: in the same way, the angle theta of the forearm joint can be obtained2And the stroke l of the small arm hydraulic cylinderFGThe relationship of (1):
Figure RE-GDA0002856903880000102
θ2＝π-∠GEF-∠GEA-∠FEQ
in the formula IEG、lEFThe distance between two end points of the small arm hydraulic cylinder and the original point E is lFGForming a triangle; the angle GEA and the angle FEQ are joint angles of two end points of the small arm hydraulic cylinder;
forearm joint angle theta3Relationship to the boom cylinder stroke:
Figure RE-GDA0002856903880000111
θ3＝∠NQE+∠NQR-π
in the formula IQN、lQRThe distance between two end points of the grab bucket hydraulic cylinder and the original point Q is lNRThe angle NQE is the joint angle of one end point of the grab bucket hydraulic cylinder.
Preferably, the track planning method of sinusoidal acceleration and deceleration is adopted in the acceleration stage and the deceleration stage:
labeling the acceleration as a (t);
Figure RE-GDA0002856903880000112
in the formula, amax is the maximum acceleration, omega is an unknown parameter, 0-T1 is acceleration time, and T2-T is deceleration time;
integrating the acceleration formula with respect to time t to obtain a velocity v (t);
Figure RE-GDA0002856903880000113
integrating the velocity formula with respect to time t again to obtain displacement s (t);
Figure RE-GDA0002856903880000114
step five: the motion track enters a constant speed stage after being accelerated in a sine way, the constant speed is Vc, when t is t1, a (t) is 0, v (t) is Vc, and the motion track can be obtained
Figure RE-GDA0002856903880000115
In the same way, the deceleration time can be obtained;
step six: performing transition of two linear tracks by using a polynomial of degree 5, wherein the polynomial is at a transition starting point B position P1, a velocity V1, an acceleration 0, a transition ending point C position P2, a velocity V2 and an acceleration 0; the mathematical expression is as follows:
m0＝p1 m1＝v1 m2＝0
m3＝(20(p2-p1)-(12v1+8v2)Te)/(2Te 3)
m4＝(-30(p2-p1)+(16v1+14v2)Te)/(2Te 4)
m5＝(12(p2-p1)-(6v1+6v2)Te)/(2Te 5)
s(t)＝m5t5+m4t4+m3t3+m2t2+m1t+m0
in the formula, Te is the transition trace time.
The above formulas are all quantitative calculation, the formula is a formula obtained by acquiring a large amount of data and performing software simulation to obtain the latest real situation, and the preset parameters in the formula are set by the technical personnel in the field according to the actual situation.
The working principle of the invention is as follows: the track planning unit is used for carrying out grabbing arm space track planning on the grabbing arm type trash cleaning robot, and setting the starting position of the trash cleaning robot to be P1, the upper end point of the trash rack to be P2 and the river surface to be P3 according to the actual trash cleaning path of the trash cleaning robot; in the process of cleaning, the cleaning robot moves downwards from an initial position P1 to reach an upper end point P2 of the trash rack, then moves downwards along the trash rack, the claw teeth of the grab bucket at the tail end of the cleaning robot coincide with the trash rack, and the cleaning robot reaches the river surface P3 to grab dirt; performing transition of two linear tracks by using a polynomial of degree 5, wherein the polynomial is at a transition starting point B position P1, a velocity V1, an acceleration 0, a transition ending point C position P2, a velocity V2 and an acceleration 0; constructing a track planning model, dividing a whole course path into 5 stages, respectively performing path interpolation by adopting interpolation modes such as sine acceleration and deceleration, quintic polynomial and the like, performing a P1-A acceleration stage, a A-B first stage straight line constant speed stage, a B-C straight line inter-stage track transition stage, a C-D second stage straight line constant speed stage, a D-P3 deceleration stage and an azimuth angle
Figure RE-GDA0002856903880000121
The same track planning mode is adopted for the terminal positions y and z; according to the planning method, the cleaning robot is actually subjected toPlanning a motion track, wherein data of positions and azimuth angles of P1, P2 and P3 points are obtained through practical teaching, setting a constant speed vc and a maximum acceleration amax, planning a tail end motion track, solving through inverse kinematics to obtain motion tracks of all hydraulic cylinders, and respectively planning the motion tracks and the speeds of the grab buckets, so that the position and speed curves of all joints are continuous without sudden change, and the transition between path points is smooth.
In the description herein, references to the description of "one embodiment," "an example," "a specific example" or the like are intended to mean that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, the schematic representations of the terms used above do not necessarily refer to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.
The preferred embodiments of the invention disclosed above are intended to be illustrative only. The preferred embodiments are not intended to be exhaustive or to limit the invention to the precise forms disclosed. Obviously, many modifications and variations are possible in light of the above teaching. The embodiments were chosen and described in order to best explain the principles of the invention and the practical application, to thereby enable others skilled in the art to best utilize the invention. The invention is limited only by the claims and their full scope and equivalents.

Claims (6)

1. A motion control system of a grab arm type trash cleaning robot is characterized by comprising a database management module, a motion control module, an operation module, an abnormality detection module and a data acquisition module; the motion control module comprises a data storage unit, a track planning unit, a kinematics unit, a communication unit, a teaching unit, a safety management unit and a human-computer interaction unit;
the track planning unit is used for carrying out grabbing arm space track planning on the grabbing arm type trash cleaning robot, and the specific planning process comprises the following steps:
the method comprises the following steps: setting the initial position of the cleaning robot as P1, the upper end point of the trash rack as P2 and the river surface as P3 according to the actual cleaning path of the cleaning robot;
step two: in the process of cleaning, the cleaning robot moves downwards from an initial position P1 to reach an upper end point P2 of the trash rack, then moves downwards along the trash rack, the claw teeth of the grab bucket at the tail end of the cleaning robot coincide with the trash rack, and the cleaning robot reaches the river surface P3 to grab dirt;
step three: performing transition of two linear tracks by using a polynomial of degree 5, wherein the polynomial is at a transition starting point B position P1, a velocity V1, an acceleration 0, a transition ending point C position P2, a velocity V2 and an acceleration 0;
step four: constructing a track planning model, dividing a whole course path into 5 stages, respectively performing path interpolation by adopting interpolation modes such as sine acceleration and deceleration, quintic polynomial and the like, performing a P1-A acceleration stage, a A-B first stage straight line constant speed stage, a B-C straight line inter-stage track transition stage, a C-D second stage straight line constant speed stage, a D-P3 deceleration stage and an azimuth angle
Figure FDA0002770298020000011
The same track planning mode is adopted for the terminal positions y and z;
step five: planning the actual motion track of the cleaning robot according to the planning method, wherein the data of the positions and the azimuth angles of the P1, P2 and P3 points are obtained by actual teaching, and the set constant speed v is givencMaximum acceleration amaxPlanning the motion trail of the tail end, obtaining the motion trail of each hydraulic cylinder through inverse kinematics calculation, and respectively planning the motion trail and the speed of the grab bucket, so that the position and speed curves of each joint are continuous without sudden change, and the transition between path points is smooth.
2. The motion control system of the grabbing arm type trash cleaning robot as claimed in claim 1, wherein: the database management module is used for realizing the establishment and the storage of data variables and program texts; the motion control module is used for realizing point-to-point motion, joint interpolation, terminal linear interpolation and track point transition interpolation, and controlling the position, speed, acceleration and terminal attitude of each shaft; the operation module is used for realizing teaching, reproduction and program compiling operation, manually controlling single-axis inching, linearly moving the tail end and automatically operating a program; the abnormity detection module is used for realizing sudden stop abnormity, servo abnormity and limit abnormity; the data acquisition module is used for integrating a distance measuring sensor and detecting and correcting the position of the tail end in real time.
3. The motion control system of the grabbing arm type trash cleaning robot as claimed in claim 1, wherein: the data storage unit is used for storing the taught position points and track programs and the parameter templates established by the users, and the repeated calling performance of the programs is improved; the track planning unit is used for carrying out interpolation on acceleration, speed, position and posture by adopting an interpolation algorithm in a joint space and a terminal Cartesian space to form a smooth motion curve; the kinematics unit is used for establishing a coordinate system of each joint axis, measuring DH parameters and compiling positive and negative kinematics of the mechanical arm; the communication unit is used for realizing data transmission between the demonstrator and the hydraulic servo controller through an industrial Ethernet; the teaching unit is used for manually controlling the single-axis point motion and the tail end linear motion of the mechanical arm through a demonstrator, teaching a required motion point and forming a motion track and a program; the safety management unit is used for feeding back abnormal conditions in real time in the motion process, reporting errors, responding to an emergency stop button and initializing; the man-machine interaction unit is used for completing the operation, display and interaction functions of all the modules.
4. The motion control system of the grabbing arm type trash cleaning robot as claimed in claim 1, wherein: the trash cleaning robot is used for cleaning garbage and dirt attached to and wound on the grid bars, the claw teeth of the grab bucket extend into the grid bars of the trash rack, the grab bucket does not scrape and collide with the trash rack, and the trash cleaning robot runs along the grid bars from top to bottom according to a set track; the cleaning robot is a planar three-freedom-degree series robot, and the tail end posture rotates around an axis vertical to the plane.
5. The motion control system of the grabbing arm type trash cleaning robot as claimed in claim 1, wherein: the kinematics unit is used for solving the position and the posture of the comb teeth in the motion space according to the parameter size of the trash cleaning robot; the specific solving process is as follows:
s1: solving the Q position and the large arm joint angle theta according to the plane geometric relation1Angle theta of the forearm joint2The functional relationship with the Q position (y, z) is as follows:
y＝lAEcosθ1+lEQcos(θ1+θ2)
z＝lAEsinθ1+lEQsin(θ1+θ2)
in the formula IAELength of the connecting rod of the big arm lEQThe length of the small arm connecting rod;
s2: solving the attitude of RS, the angles in the plane are directly added, so the angle theta of the large arm joint1Angle theta of the forearm joint2Angle theta of grab bucket joint3Angle theta of comb teeth4The sum is the azimuth angle of RS
Figure FDA0002770298020000034
Figure FDA0002770298020000031
In the formula, theta4Is the angle of the comb teeth with respect to the grab bucket, and theta4Is a constant value;
s3: obtaining the functional relation between the angle of the large arm joint, the angle of the small arm joint, the angle of the grab bucket joint and the angle of the comb teeth, wherein the stroke of the corresponding hydraulic cylinder is measured by each encoder, and the nonlinear relation is needed to be solved between the angle of each rotary joint and the stroke of the corresponding driving hydraulic cylinder;
according to the cosine theorem, the stroke l of the large-arm hydraulic cylinderBDConverting into angle DAB:
Figure FDA0002770298020000032
in the formula IAD、lABThe distance between two end points of the large-arm hydraulic cylinder and the original point A is lBDForming a triangle;
s4: big arm joint angle theta1Relation with angle ≈ DAB:
θ1＝∠DAB+∠CAB-π/2-∠DAE
in the formula, the angle CAB and the angle DAE are joint angles of two end points of the large-arm hydraulic cylinder;
s5: in the same way, the angle theta of the forearm joint can be obtained2And the stroke l of the small arm hydraulic cylinderFGThe relationship of (1):
Figure FDA0002770298020000033
θ2＝π-∠GEF-∠GEA-∠FEQ
in the formula IEG、lEFThe distance between two end points of the small arm hydraulic cylinder and the original point E is lFGForming a triangle; the angle GEA and the angle FEQ are joint angles of two end points of the small arm hydraulic cylinder;
forearm joint angle theta3Relationship to the boom cylinder stroke:
Figure FDA0002770298020000041
θ3＝∠NQE+∠NQR-π
in the formula IQN、lQRThe distance between two end points of the grab bucket hydraulic cylinder and the original point Q is lNRThe angle NQE is the joint angle of one end point of the grab bucket hydraulic cylinder.
6. The motion control system of the grabbing arm type trash cleaning robot as claimed in claim 1, wherein: the track planning method adopting sine acceleration and deceleration in the acceleration stage and the deceleration stage comprises the following steps:
labeling the acceleration as a (t);
Figure FDA0002770298020000042
in the formula, amaxThe acceleration is the maximum acceleration, omega is an unknown parameter, 0-T1 is acceleration time, and T2-T is deceleration time;
integrating the acceleration formula with respect to time t to obtain a velocity v (t);
Figure FDA0002770298020000043
integrating the velocity formula with respect to time t again to obtain displacement s (t);
Figure FDA0002770298020000044
step five: the motion track enters a constant speed stage after being accelerated in a sine way, the constant speed is Vc, when t is t1, a (t) is 0, v (t) is Vc, and the motion track can be obtained
Figure FDA0002770298020000045
In the same way, the deceleration time can be obtained;
step six: performing transition of two linear tracks by using a polynomial of degree 5, wherein the polynomial is at a transition starting point B position P1, a velocity V1, an acceleration 0, a transition ending point C position P2, a velocity V2 and an acceleration 0; the mathematical expression is as follows:
m0＝p1 m1＝v1 m2＝0
m3＝(20(p2-p1)-(12v1+8v2)Te)/(2Te 3)
m4＝(-30(p2-p1)+(16v1+14v2)Te)/(2Te 4)
m5＝(12(p2-p1)-(6v1+6v2)Te)/(2Te 5)
s(t)＝m5t5+m4t4+m3t3+m2t2+m1t+m0
in the formula, Te is the transition trace time.
CN202011246801.8A 2020-11-10 2020-11-10 Grab arm type trash cleaning robot motion control system Pending CN112276911A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011246801.8A CN112276911A (en) 2020-11-10 2020-11-10 Grab arm type trash cleaning robot motion control system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011246801.8A CN112276911A (en) 2020-11-10 2020-11-10 Grab arm type trash cleaning robot motion control system

Publications (1)

Publication Number Publication Date
CN112276911A true CN112276911A (en) 2021-01-29

Family

ID=74351124

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011246801.8A Pending CN112276911A (en) 2020-11-10 2020-11-10 Grab arm type trash cleaning robot motion control system

Country Status (1)

Country Link
CN (1) CN112276911A (en)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20070030271A1 (en) * 2005-08-05 2007-02-08 Denso Wave Incorporated Robot controller and robot control method using spline interpolation
CN102509025A (en) * 2011-11-25 2012-06-20 苏州大学 Method for quick solution of six-degree-of-freedom humanoid dexterous arm inverse kinematics
CN107717985A (en) * 2017-09-22 2018-02-23 深圳星河智能科技有限公司 A kind of space multi straight smooth motion control method applied to six axis robot arm
CN108147143A (en) * 2017-12-19 2018-06-12 合肥泰禾光电科技股份有限公司 A kind of automatic stacking machine people's control system
CN109623820A (en) * 2018-12-25 2019-04-16 哈工大机器人（合肥）国际创新研究院 A kind of robot space tracking transition method

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20070030271A1 (en) * 2005-08-05 2007-02-08 Denso Wave Incorporated Robot controller and robot control method using spline interpolation
CN102509025A (en) * 2011-11-25 2012-06-20 苏州大学 Method for quick solution of six-degree-of-freedom humanoid dexterous arm inverse kinematics
CN107717985A (en) * 2017-09-22 2018-02-23 深圳星河智能科技有限公司 A kind of space multi straight smooth motion control method applied to six axis robot arm
CN108147143A (en) * 2017-12-19 2018-06-12 合肥泰禾光电科技股份有限公司 A kind of automatic stacking machine people's control system
CN109623820A (en) * 2018-12-25 2019-04-16 哈工大机器人（合肥）国际创新研究院 A kind of robot space tracking transition method

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
罗霄等: "《工业机器人技术基础与应用分析》", 31 March 2018, 北京理工大学出版社 *
黄忠明等: "基于正弦加减速控制算法的工业机器人位姿轨迹规划研究", 《制造业自动化》 *

Similar Documents

Publication Publication Date Title
CN104647331B (en) A kind of principal and subordinate is servo-actuated teaching industrial robot system
CN103147577B (en) Control method, equipment, system and construction machinery for multi-joint mechanical arm support
CN105033996B (en) Control system based on hand-push teaching type five-shaft horizontal joint robot
CN103176409B (en) Method for fast and accurately realizing concrete pump truck cantilever crane movement locus
Zhang et al. Research on trajectory planning and autodig of hydraulic excavator
Li et al. Development of articulated robot trajectory planning
CN111152226A (en) Robot working track planning method and system
CN204856122U (en) Punching press robot control system
CN114265364A (en) Monitoring data processing system and method for industrial Internet of things
Xiao et al. Simulation research of a six degrees of freedom manipulator kinematics based On MATLAB toolbox
CN112276911A (en) Grab arm type trash cleaning robot motion control system
Yanai et al. Feedback control for wire-suspended mechanism with exact linearization
CN207058563U (en) A kind of SCARA industrial robot control systems
Niola et al. Robot trajectory planning by points and tangents
Dai Collision-free motion of an articulated kinematic chain in a dynamic environment
CN115544768A (en) Autonomous excavation operation track generation method and system
Adar et al. Real time visual servoing of a 6-dof robotic arm using fuzzy-pid controller
CN114505853A (en) Remote layered management and control method and system for industrial robot
Zhang et al. Design of an industrial robot polishing control system
CN204585243U (en) A kind of unpowered joint arm structure
Wang et al. Automatic steel grabbing robot system for scrap steel processing production line
CN206348641U (en) The omni-directional mobile robots control system developed based on dSPACE controllers
Jianbo et al. Dynamic coordination of uncalibrated hand/eye robotic system based on neural network
Yang et al. Design of third-order uniform acceleration and deceleration trajectory based on Simulink StateFlow
Zhang et al. Research Article Research on Trajectory Planning and Autodig of Hydraulic Excavator

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: 20210129

RJ01 Rejection of invention patent application after publication