CN113843803A

CN113843803A - Method and system for planning overturning real-time following track of overturning object

Info

Publication number: CN113843803A
Application number: CN202111222341.XA
Authority: CN
Inventors: 谢能达; 郭震
Original assignee: Shanghai Jingwu Intelligent Technology Co Ltd
Current assignee: Shanghai Jingwu Intelligent Technology Co Ltd
Priority date: 2021-10-20
Filing date: 2021-10-20
Publication date: 2021-12-28

Abstract

The invention provides a method and a system for planning a turnover real-time following track of a turnover object, which comprises the following steps: step S1: navigating a chassis provided with a mechanical arm to a position near a turnable object; step S2: the mechanical arm controls the vision system to take a picture, an automatic path planning method is adopted, the track of the initial section of the turnover process of the turnover object is automatically calculated and generated, and the track of the initial section is designed into a theoretical circular arc track; step S3: after the theoretical circular arc track of the initial segment is generated, a mechanical arm controller carries out speed planning and track interpolation, issues planned position points and acquires the actual gripping position of the clamping jaw in each interpolation period; step S4: and sending the actual gripping position of the clamping jaw to a mechanical arm control system at fixed intervals, and planning to obtain the overturning track of the turnable object cover by adopting a real-time track following method inside the data point set. The invention solves the blank technology in the field of automatic cleaning of the turnable objects; the intelligent cleaning machine has the advantages of intellectualization and great improvement of cleaning efficiency.

Description

Method and system for planning overturning real-time following track of overturning object

Technical Field

The invention relates to the field of intelligent robot cleaning, in particular to a method and a system for planning a turnover real-time following track of a turnover object, and more particularly relates to a method for planning a turnover real-time following track of a toilet cover during toilet cleaning.

Background

Advanced ones of existing trajectory tracking systems use sensors to track the trajectory, for example arc welding robot systems use structured light vision sensors to track the trajectory. The actuator in the track tracking system has a task track preset by means of teaching information or planning and the like, the actuator with the tool can generate driving information according to the preset task track to enable the tool to run along the preset task track, and the track tracking system can only simply feed back track deviation information collected and extracted by a sensor to the actuator directly to correct the preset task track.

The trajectory planning of the turnable object is a difficult problem all the time, and few related inventions can carry out the trajectory planning of the turnable object.

Therefore, in the cleaning field, the intelligent robot is used for cleaning the closestool, the closestool cleaning process is complex, and manual cleaning is basically carried out. The intelligent robot is used for replacing manpower, and the technology for overturning and cleaning the toilet lid is also blank. The biggest difficulty in cleaning the toilet bowl is the overturning of the toilet bowl cover, the overturning track is theoretically an arc, but the axis precision of the arc track in the overturning process is inaccurate, so that the actual overturning track and the theoretical arc track have difference, and the robot is required to be capable of adjusting the overturning track of the toilet bowl in real time, so that the overturning task of the toilet bowl cover is completed. How to provide a method capable of following and adjusting the overturning of the toilet lid in real time is a technical problem to be solved at present.

Aiming at the defects in the prior art, the technical problems to be solved by the invention are as follows: a. calculating by combining a vision system to obtain the initial position of the axis line of the overturning track of the closestool and the clamping point position of the mechanical arm, and automatically calculating the posture of the overturning action; b. meanwhile, in the overturning process, path points are obtained by calculating local map information given by vision, if a simple arc path plan is adopted, the toilet lid can be pulled to cause damage to the toilet lid, so that the overturning action needs to be adjusted in real time.

The technology of using the mechanical arm to turn the toilet lid at the present stage is blank.

Disclosure of Invention

Aiming at the defects in the prior art, the invention aims to provide a method and a system for planning a turnover real-time following track of a turnover object.

The invention provides a method for planning a turnover real-time following track of a turnover object, which comprises the following steps:

step S1: navigating a chassis provided with a mechanical arm to a position near a turnable object;

step S2: the mechanical arm controls the vision system to take a picture, an automatic path planning method is adopted, the track of the initial section of the turnover process of the turnover object is automatically calculated and generated, and the track of the initial section is designed into a theoretical circular arc track;

step S3: after the theoretical circular arc track of the initial segment is generated, a mechanical arm controller carries out speed planning and track interpolation, issues planned position points and acquires the actual gripping position of the clamping jaw in each interpolation period;

step S4: and sending the actual gripping position of the clamping jaw to a mechanical arm control system at fixed intervals, and planning to obtain the overturning track of the turnable object cover by adopting a real-time track following method inside the data point set.

Preferably, in the step S2:

after the turnable object is close to the visual system, the mechanical arm controls the visual system to take a picture, and automatically calculates and generates an initial section track of the turning process of the turnable object by adopting an automatic path planning method according to the position of the surrounding environment given by vision; designing the initial section track into a theoretical circular arc track, wherein the motion angle is act _ alpha, and the value of the motion angle is based on the total theoretical circular arc angle;

the automatic path planning method comprises the following steps:

the mechanical arm controls the vision to take a picture, the vision obtains local map information near the turnable object and comprises all position points, and point location data are sent to the mechanical arm control system through an mqtt protocol and comprise posA, posB, posC, posE, posF and posG; wherein posA represents a first point on the flipping axis of the invertible object cover; posB represents a second point on the flipping axis of the invertible object cover; posC represents the point position in front of the cover of the turnable object, is also the clamping point at the tail end of the mechanical arm and is the starting point of movement; posD represents a point in front of the invertible object cover; wherein posE and posF represent two corner points of the outer edge of the back cover of the invertible object; posD is the midpoint between posE and posF, posD ═ 0.5 ═ posE + posF; the posG is the intersection point of straight lines formed by the side wall surface of the reversible object and the posE and the posF;

calculating the total angle alpha of the whole arc after the overturning is finished, which is also a theoretical angle of the locus from posC to posC _ 1; point positions posA, posB and posC form a plane 1, the posA, the posB and the posD form a plane 2, and the included angle between the plane 1 and the plane 2 is alpha;

calculating a theoretical circular arc end point posC _1, and calculating to obtain posC _1 according to the posC, the axial lead posA-posB and an included angle alpha;

calculating the postures of posC and posC _1 in the overturning process, wherein the posture calculation firstly determines the posture information rz of the tail end of the mechanical arm, wherein rz represents a rotation matrix along the Z axis, ry represents a rotation matrix along the Y axis, and rx represents a rotation matrix along the X axis; considering collision prevention, adopting automatic search calculation, firstly calculating the distance d between a reversible object and a wall as posF-posG; and then, searching and calculating the posture by combining the mechanical structure parameters of the mechanical arm, and calculating to determine rz.

Preferably, rz is calculated as follows:

i1, selecting rz, taking a vector rz as posA-posB, wherein the vector direction is the Z-axis direction of the tail end of the mechanical arm, and after the vector is normalized, solving to obtain a rotation matrix rz; judging whether the design size of the joint of the 4,5 and 6 axes of the mechanical arm is larger than d, if the design size is not larger than d, the vector rz is posA-posB, and if the design size is larger than d, performing the step i2 to continue searching and calculating rz; (ii) a

i2, recalculating rz, the amount of orientation rz ═ posA-posC; judging whether the design size of the joint of the 4,5 and 6 axes of the mechanical arm is larger than d, if not, the vector rz is posA-posC, and if so, performing the step i3 to continue searching and calculating rz;

i3, recalculating rz, namely normalizing by the vector posC to the intermediate point of posA and posB, namely vector rz is 0.5 (posA + posB) -posC, rz, determining rz, judging whether the design size of the joint of the 4,5,6 axes of the mechanical arm at the moment is larger than d, and if the design size is not larger than d, vector rz is 0.5 (posA + posB) -posC; if the value is larger than d, the step i4 is carried out to continue searching and calculating rz;

i4, recalculating rz by passing through vector posC to posB instead, i.e. vector rz ═ posB-posC; then rz is normalized again to determine rz; the mechanical design and the deployment can ensure that the direction can not collide; determining ry as a normal vector of a plane 1 composed of posA, posB and posC, wherein ry is (posA-posB) x (posC-posB), determining rx according to a right-hand criterion, and determining the turning circular arc tracks posC to posC _1 of the turnable object through the position and posture calculation.

Preferably, in the step S3:

carrying out speed planning and track interpolation to generate a down-sending position point of each servo period; the speed planning method of the initial path adopts T-type speed planning or S-type speed planning; after moving to the act _ alpha angle, the initial path is ended; in the motion process of the mechanical arm, the mechanical arm control system controls the camera to shoot, the camera collects the clamping jaw grabbing point position posC (i) in each period and sends the clamping jaw grabbing point position posC (i) to the mechanical arm control system every other fixed period.

Preferably, in the step S4:

when the initial path is moved, the camera acquires a clamping jaw grabbing point posC (i) of each period and sends data to the mechanical arm controller system, and the mechanical arm controller system adopts a least square method to fit according to a data point set to obtain a circular track; the position points between posC and posC _ init are position points given by the camera, the positions of the fixed interpolation periods are predicted in the subsequent periods after the fitting is completed, and the position points with fixed numerical values are position points generated based on the fitting circle; calculating a planning position by adopting a real-time following adjustment method;

calculating the arc starting point posC to obtain an arc end point posC _ init according to the motion angle act _ alpha;

after data in the operation process are collected, a new circular arc is fit again, the circular arc passes through posC and posC _ init, and the position which is a posC _ n and is away from a posC _ init point by a fixed value period is taken after the posC _ init; posC _ posC _ init, posC _ n are all points on the fitting arc;

in the motion process of the mechanical arm, the camera always acquires data, and the real-time following adjustment method comprises the following steps:

the ith step: after the mechanical arm reaches the posC _ init, taking a theoretical circular arc position between the posC _ init and the posC _ n as input, wherein i is a step sequence from 1, taking the position of a fixed value period as a moving window each time, and the tail end of the mechanical arm follows the position in real time;

if the progres _ alpha is smaller than alpha, performing the step i + 1; in the (i + 1) th step, combining the new data acquired by the camera in the (i) th step with the previous camera data, and re-fitting to generate a new fitting arc and a new posC _ init;

repeating the steps until an included angle proges _ alpha of the two planes is larger than or equal to alpha to generate an end point posC _ n;

taking the position of a fixed value period as a moving window each time, enabling the mechanical arm to follow the position point in real time, and planning the real-time following track into a real-time following track planning method based on a quintic polynomial, wherein the method is specifically described as follows:

the fifth order polynomial equation used is described as follows:

q_t＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

wherein q is_tThe position of the moment t is, the moment t is the current moment, and a0, a1, a2, a3, a4 and a5 are some parameters to be solved of the description equation;

the solving process is the solution a₀、a₁、a₂、a₃、a₄、a₅；

Constraint condition initial time in the method is as follows:

q₀＝S₀

end position:

q_f＝S₁

S₀for each moving window the end position of the robot arm, V, at the initial moment₀For the end velocity of the robot arm at the initial moment of each moving window, A₀The terminal acceleration of the mechanical arm at the initial moment of each moving window is a parameter at the moment when t is 0; q. q.s₀Is the position of the initial time; q. q.s_fIs the position of the end time of each moving window; s₁For the end position of the robot arm, V, at the end of each moving window₁For the end of each moving window the end velocity of the robot arm, A₁The terminal acceleration of the mechanical arm at the end moment of each moving window;

the derivative of the initial position, also the velocity,

is the derivative of the initial velocity, also the initial acceleration;

the derivative of the end position, also the velocity of the end position,

the derivative of the ending velocity, and also the acceleration at the end;

derived from the constraints, we can obtain:

a₀＝S₀

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁

a₁＝V₀

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁

2*a₂＝A₀

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁

the above equations are solved simultaneously to obtain:

a₀＝S₀

a₁＝V₀

a₂＝A₀/2

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵

wherein t is_fIs obtained by multiplying a fixed value by a servo interpolation period;

for each moving window, S₀For each moving window posC _ init, V₀The end velocity of the robot arm at the posC _ init position, A₀The terminal acceleration of the mechanical arm at the posC _ init position; s₁For each moving window posC _ n, V₁To set the desired tip speed, A₁A set desired tip acceleration;

according to the real-time following track planning method, a track interpolation position is generated in each interpolation period, and the generated interpolation position is issued to a servo, so that the overturning track of the turnable object cover is adjusted in real time, and the overturning action of the turnable object cover is completed.

The invention provides a reversible object overturning real-time following track planning system, which comprises:

module M1: navigating a chassis provided with a mechanical arm to a position near a turnable object;

module M2: the mechanical arm controls the vision system to take a picture, an automatic path planning method is adopted, the track of the initial section of the turnover process of the turnover object is automatically calculated and generated, and the track of the initial section is designed into a theoretical circular arc track;

module M3: after the theoretical circular arc track of the initial segment is generated, a mechanical arm controller carries out speed planning and track interpolation, issues planned position points and acquires the actual gripping position of the clamping jaw in each interpolation period;

module M4: and sending the actual gripping position of the clamping jaw to a mechanical arm control system at fixed intervals, and planning to obtain the overturning track of the turnable object cover by adopting a real-time track following method inside the data point set.

Preferably, in said module M2:

the automatic path planning method comprises the following steps:

Preferably, rz is calculated as follows:

Preferably, in said module M3:

Preferably, in said module M4:

the fifth order polynomial equation used is described as follows:

q_t＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

the solving process is the solution a₀、a₁、a₂、a₃、a₄、a₅；

Constraint condition initial time in the method is as follows:

q₀＝S₀

end position:

q_f＝S₁

the derivative of the initial position, also the velocity,

is the derivative of the initial velocity, also the initial acceleration;

the derivative of the end position, also the velocity of the end position,

the derivative of the ending velocity, and also the acceleration at the end;

derived from the constraints, we can obtain:

a₀＝S₀

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁

a₁＝V₀

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁

2*a₂＝A₀

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁

the above equations are solved simultaneously to obtain:

a₀＝S₀

a₁＝V₀

a₂＝A₀/2

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵

Compared with the prior art, the invention has the following beneficial effects:

1. the invention solves the blank technology in the field of automatic cleaning of the turnable objects;

2. the mechanical arm is combined with the chassis system, so that the mechanical arm can reach each area; the vision is combined with the mechanical arm system, the mechanical arm can automatically plan a path without teaching and guiding, and the mechanical arm can automatically search and calculate the position and the posture of the turnover closestool according to the visual point position, so that the intelligent cleaning machine has the advantages of intellectualization and greatly improved cleaning efficiency;

3. the real-time track following method is adopted, so that the technical problem that the deviation between an actual track and a theoretical position is large due to the position precision error in the overturning process of the toilet lid is solved;

4. the mechanical arm is used for adjusting the track on line in real time in the process of overturning the closestool, so that the closestool cover is effectively protected from being damaged.

Drawings

Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:

FIG. 1 is a schematic view of a toilet bowl;

FIG. 2 is a schematic diagram of a toilet bowl rollover trajectory planning location point;

fig. 3 is a schematic diagram of planning location points along a trajectory in real time.

Wherein, each component in the figure is expressed as:

floor 1 toilet lid 2

Toilet rear wall 3 toilet rear cover 4

Toilet side wall 5

Detailed Description

The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.

Example 1:

the method for planning the overturning real-time following track of the invertible object, as shown in fig. 1-3, comprises the following steps:

Specifically, in the step S2:

the automatic path planning method comprises the following steps:

Specifically, rz is calculated as follows:

Specifically, in the step S3:

Specifically, in the step S4:

the fifth order polynomial equation used is described as follows:

q_t＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

the solving process is the solution a₀、a₁、a₂、a₃、a₄、a₅；

Constraint condition initial time in the method is as follows:

q₀＝S₀

end position:

q_f＝S₁

the derivative of the initial position, also the velocity,

is the derivative of the initial velocity, also the initial acceleration;

the derivative of the end position, also the velocity of the end position,

the derivative of the ending velocity, and also the acceleration at the end;

derived from the constraints, we can obtain:

a₀＝S₀

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁

a₁＝V₀

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁

2*a₂＝A₀

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁

the above equations are solved simultaneously to obtain:

a₀＝S₀

a₁＝V₀

a₂＝A₀/2

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵

Example 2:

example 2 is a preferred example of example 1, and the present invention will be described in more detail.

The person skilled in the art can understand the method for planning the overturning real-time following trajectory of the invertible object provided by the present invention as a specific implementation of the system for planning the overturning real-time following trajectory of the invertible object, that is, the system for planning the overturning real-time following trajectory of the invertible object can be implemented by executing the process of the method for planning the overturning real-time following trajectory of the invertible object.

Specifically, in the module M2:

the automatic path planning method comprises the following steps:

Specifically, rz is calculated as follows:

Specifically, in the module M3:

Specifically, in the module M4:

the fifth order polynomial equation used is described as follows:

q_t＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

the solving process is the solution a₀、a₁、a₂、a₃、a₄、a₅；

Constraint condition initial time in the method is as follows:

q₀＝S₀

end position:

q_f＝S₁

S₀for each moving window the end position of the robot arm, V, at the initial moment₀For the end velocity of the robot arm at the initial moment of each moving window, A₀The terminal acceleration of the mechanical arm at the initial moment of each moving window is a parameter at the moment when t is 0; q. q.s₀Is the position of the initial time; q. q.s_fIs the position of the end time of each moving window; s₁For the end position of the robot arm, V, at the end of each moving window₁For each moving window end time the robot armEnd velocity of A₁The terminal acceleration of the mechanical arm at the end moment of each moving window;

the derivative of the initial position, also the velocity,

is the derivative of the initial velocity, also the initial acceleration;

the derivative of the end position, also the velocity of the end position,

the derivative of the ending velocity, and also the acceleration at the end;

derived from the constraints, we can obtain:

a₀＝S₀

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁

a₁＝V₀

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁

2*a₂＝A₀

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁

the above equations are solved simultaneously to obtain:

a₀＝S₀

a₁＝V₀

a₂＝A₀/2

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵

Example 3:

example 3 is a preferred example of example 1, and the present invention will be described in more detail.

Step 1: the platform mechanical arm adopted by the technology is arranged on a mobile chassis, and the mobile chassis can carry the mechanical arm to navigate to a position near the closestool;

step 2: after the mechanical arm is close to the closestool, the mechanical arm controls the vision system to take a picture, and then the mechanical arm automatically calculates and generates an initial section track of the overturning process of the closestool according to the surrounding environment position given by vision by adopting an automatic path planning method; the initial section track is designed into a theoretical circular arc track, the motion angle is act _ alpha, the value of the motion angle is based on the total theoretical circular arc angle, and the act _ alpha is 0.1 × alpha in the scheme; where the calculated solution for α is described later, α is the total theoretical arc angle, i.e., the theoretical angle of the locus of posC to posC _1

And step 3: after an initial segment theoretical total path, namely a theoretical circular arc track is generated, a mechanical arm controller carries out speed planning and track interpolation and issues a planned position point, and meanwhile, after the mechanical arm starts to move, the mechanical arm controls a vision system to photograph and collects an actual clamping jaw grabbing position posC (i) of each interpolation period; posC (i) denotes the pinch point posC of the ith cycle; POSC is a front point of the toilet cover and is also a grabbing point of the tail end of the mechanical arm;

and 4, step 4: every fixed period 10 × servo _ cycle _ time, where servo _ cycle _ time is a servo interpolation period; the vision system sends camera photographing data posC (i) to the mechanical arm control system, and the mechanical arm control system adopts a real-time track following method inside based on the data point set to finally plan and obtain the overturning track of the toilet lid.

Further, in step 2, the automatic path planning method includes the steps of: as shown in fig. 1, the mechanical arm controls the vision to take a picture, and the vision obtains local map information near the toilet, including various position points, wherein posA represents a first point on the overturning axis of the toilet lid; posB represents a second point on the toilet lid flip axis; posC represents a clamping point of the outer edge of the toilet lid, namely a clamping point of the tail end of the mechanical arm; posD represents a point in front of the toilet lid;

the mechanical arm controls visual shooting, position points near the closestool are obtained through visual calculation, point position data are sent to the mechanical arm control system through an mqtt protocol, and the sent data comprise posA, posB, posC, posE, posF and posG; wherein posE and PosF denote two corner points of the outer rim of the toilet back cover; posD is the midpoint between posE and posF, i.e. posD ═ 0.5 × (posE + posF); the posG is the intersection point of straight lines formed by the wall surface of the side wall of the closestool and the posE and the posF;

step 2.1, calculating a theoretical arc end point posC _ 1; wherein posC is a point position in front of the toilet lid and is also a grabbing point position of the tail end of the mechanical arm, the total angle alpha of the whole arc after the overturning is completed is firstly calculated, the point positions posA, posB and posC form a plane 1, the point positions posA, posB and posD form a plane 2, the included angle between the plane 1 and the plane 2 is alpha, and the detailed calculation steps are not repeated in a deduction mode; then, calculating a theoretical circular arc end point posC _1, and calculating to obtain posC _1 according to the posC, the axial lead posA-posB and an included angle alpha;

step 2.2, calculating the postures of posC and posC _1 in the overturning process, wherein the posture calculation firstly determines the terminal rz which represents the posture information of the tail end of the mechanical arm, rz represents a rotation matrix along the Z axis, ry represents a rotation matrix along the Y axis, and rx represents a rotation matrix along the X axis; considering collision prevention, automatic search calculation is adopted, and the distance d between the toilet and the wall is calculated as posF-posG; and then, carrying out searching calculation by combining the mechanical structure parameters of the mechanical arm and the posture, firstly calculating and determining rz, wherein the calculating steps are as follows:

i 1: first, rz is selected as vector posA-posB; the vector direction is the Z-axis direction of the tail end of the mechanical arm, and after the vector is normalized, a rotation matrix rz can be obtained through solving; judging whether the design size of the joint at the axes of the

mechanical arms

4,5 and 6 is larger than d to cause collision with the wall, if not, determining that the vector rz is posA-posB, and if so, jumping to the i2 step to continue searching and calculating rz;

i2, selecting rz, wherein the orientation quantity rz is posA-posC; if the design size of the joint at the

axes

4,5 and 6 of the mechanical arm is still larger than d, causing collision, the step of skipping i3 continues to calculate rz, and if no collision occurs, rz is determined to be posA-posC;

i3 recalculating rz by the intermediate point of the vector posC to posA and posB, i.e. vector rz ═ 0.5 × (posA + posB) -posC; then rz is normalized again, namely that if the designed size of the joint connection position of the

axes

4,5 and 6 of the mechanical arm is still larger than d, collision is caused, and then the judgment of i4 is continued, and if collision is not caused, rz is determined;

i4 recalculating rz by vector posC to posB, i.e. vector rz ═ posB-posC; then rz is normalized again, namely rz is determined; the mechanical design and the deployment can ensure that the direction can not collide; then determining ry as a normal vector of a plane 1 consisting of the posA, the posB and the posC, namely ry ═ cross multiplication (posA-posB) (posC-posB); then, determining rx according to a right-hand criterion; through the position and posture calculation, the theoretical arc track of the overturning of the closestool from posC to posC _1 can be determined;

further, in step 3, after the starting position and the ending position of the total theoretical circular arc path and the posture are calculated and determined, speed planning and track interpolation are carried out, and then the issued position point of each servo period can be generated; the speed planning method of the initial path adopts the traditional T-shaped speed planning or S-shaped speed planning; locus of posC to posC _1, theoretical angle α; here, the initial path to the movement angle is act _ alpha ═ 0.1 × α; after moving to the act _ alpha angle, the initial path in the present solution is ended. In the motion process of the mechanical arm, the mechanical arm control system controls a camera to take pictures, the camera collects the position of a gripping jaw grabbing point in each period, the positions are sent to the mechanical arm control system every fixed period of 10 × servo _ cycle _ time, and then step 4 is adopted;

further, the real-time tracking method in step 4. The method comprises the following specific steps:

step 4.1, based on the description in the previous step, when the initial path is moved, the camera acquires a clamping jaw grabbing point posC (i) of each period and sends data to the mechanical arm controller system, the mechanical arm controller system fits to obtain a circular track by adopting a least square method according to the data point sets, and a schematic diagram of the fitted circular track is shown in FIG. 3; the position points between posC and posC _ init are position points given by the camera, the positions of the posC _ init to posC _ n are predicted in the following period after the fitting is completed, and the 50 position points are position points generated based on the fitting circle; then, calculating a planning position by adopting a real-time following adjustment method;

theoretical arc track, arc angle act _ alpha; if the starting point of the arc is posC, calculating to obtain the end point of the arc, namely posC _ init, according to the arc angle act _ alpha;

after data in the operation process are collected, a new circular arc is fit again and passes through the posC and the posC _ init, and the position which is 50 cycles away from a posC _ init point is taken after the posC _ init, wherein the position is posC _ n; posC _ posC _ init, posC _ n are all points on the fitting arc;

and 4.2, in the motion process of the mechanical arm, the camera always collects data. The described real-time following adjustment method is that in the step i, i is a step sequence from 1, after the mechanical arm reaches posC _ init, theoretical circular arc positions between posC _ init and posC _ n are used as input, positions of 50 periods are taken as a moving window every time, the tail end of the mechanical arm follows the positions in real time, and meanwhile, included angles proges _ alpha of posC _ n, posA and posB planes and posA, posB and posC planes are judged in each period, when the included angle proges _ alpha of the two planes is larger than or equal to alpha, a path is considered to be ended, namely, the posC _ n at the moment is an end point. If the progres _ alpha is smaller than alpha, jumping to the next step i +1, in the step i +1, finishing the motion of the step i, combining new data acquired by the camera in the previous step with previous camera data, re-fitting to generate a new fitting circular arc, and generating a new posC _ init; then repeating the step i; until the end point posC _ n is generated.

Step 4.3, taking the positions of 50 periods at each time as a moving window, taking the position points followed by the mechanical arm in real time, and planning the real-time following track as a real-time following track planning method based on a fifth-order polynomial, wherein the method is specifically described as follows:

the fifth order polynomial equation used is described as follows:

q_t＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

the solving process is the solution a₀、a₁、a₂、a₃、a₄、a₅。

Constraint condition initial time in the method is as follows: q. q.s₀＝S₀

End position: q. q.s_f＝S₁

S₀For the end position of the robot arm, V, at the initial moment of each moving window described above₀For the end velocity of the robot arm at the initial moment of each moving window described above, A₀The tip acceleration of the robot arm at the initial moment for each moving window described above; namely, t is the parameter at the time 0; q0 is the position at the initial time; q. q.s_fIs the position of the end time of each moving window;

S₁for the end position of the robot arm, V, at the end of each moving window described above₁End of arm speed for each moving window end time described above, A₁The terminal acceleration of the robot arm at the end of each moving window described above, i.e., the parameter at the time t ═ 50 × servo _ cycle _ time;

derived from the constraints, it is possible to obtain

a₀＝S₀；

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁；

a₁＝V₀；

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁；

2*a₂＝A₀；

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁；

The 6 equations are solved simultaneously to obtain:

a₀＝S₀；

a₁＝V₀；

a₂＝A₀/2；

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³；

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴；

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵；

wherein t is_f＝50*servo_cycle_time；

For each moving window

S₀For each moving window posC _ init, V₀The end velocity of the robot arm at the posC _ init position, A₀The terminal acceleration of the mechanical arm at the posC _ init position;

S₁for each moving window posC _ n, V₁To set the desired tip speed, A₁A set desired tip acceleration;

according to the real-time following track planning method, a track interpolation position can be generated in each interpolation period, the generated interpolation position is issued to a servo, and finally the overturning track of the toilet lid is adjusted in real time to complete the overturning action of the toilet lid.

Those skilled in the art will appreciate that, in addition to implementing the systems, apparatus, and various modules thereof provided by the present invention in purely computer readable program code, the same procedures can be implemented entirely by logically programming method steps such that the systems, apparatus, and various modules thereof are provided in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system, the device and the modules thereof provided by the present invention can be considered as a hardware component, and the modules included in the system, the device and the modules thereof for implementing various programs can also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.

The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.

Claims

1. A method for planning a turnover real-time following track of a turnover object is characterized by comprising the following steps:

2. The invertible object tumbling real-time follow-up trajectory planning method of claim 1, wherein in said step S2:

the automatic path planning method comprises the following steps:

3. The invertible object inversion real-time following trajectory planning method of claim 2, wherein:

rz is calculated as follows:

4. The invertible object tumbling real-time follow-up trajectory planning method of claim 1, wherein in said step S3:

5. The invertible object tumbling real-time follow-up trajectory planning method of claim 1, wherein in said step S4:

the fifth order polynomial equation used is described as follows:

q_t＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

the solving process is the solution a₀、a₁、a₂、a₃、a₄、a₅；

Constraint condition initial time in the method is as follows:

q₀＝S₀

end position:

q_f＝S₁

the derivative of the initial position, also the velocity,

is the derivative of the initial velocity, also the initial acceleration;

the derivative of the end position, also the velocity of the end position,

the derivative of the ending velocity, and also the acceleration at the end;

derived from the constraints, we can obtain:

a₀＝S₀

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁

a₁＝V₀

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁

2*a₂＝A₀

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁

the above equations are solved simultaneously to obtain:

a₀＝S₀

a₁＝V₀

a₂＝A₀/2

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵

for each moving window, S₀For each moving window posC _ init, V₀Is the end velocity of the robot arm at the posC _ init positionDegree, A₀The terminal acceleration of the mechanical arm at the posC _ init position; s₁For each moving window posC _ n, V₁To set the desired tip speed, A₁A set desired tip acceleration;

6. The utility model provides a track planning system is followed in real time to reversible object upset which characterized in that includes:

7. The invertible object tumbling real-time follow-up trajectory planning system of claim 6, wherein in said module M2:

the automatic path planning method comprises the following steps:

8. The invertible object flipping real-time following trajectory planning system of claim 6, wherein:

rz is calculated as follows:

9. The invertible object tumbling real-time follow-up trajectory planning system of claim 6, wherein in said module M3:

10. The invertible object tumbling real-time follow-up trajectory planning system of claim 6, wherein in said module M4:

the fifth order polynomial equation used is described as follows:

q_t＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

the solving process is the solution a₀、a₁、a₂、a₃、a₄、a₅；

Constraint condition initial time in the method is as follows:

q₀＝S₀

end position:

q_f＝S₁

S₀for each moving window the end position of the robot arm, V, at the initial moment₀For the end velocity of the robot arm at the initial moment of each moving window, A₀For each moving window initial moment machineThe acceleration of the end of the arm, t being a parameter at time 0; q. q.s₀Is the position of the initial time; q. q.s_fIs the position of the end time of each moving window; s₁For the end position of the robot arm, V, at the end of each moving window₁For the end of each moving window the end velocity of the robot arm, A₁The terminal acceleration of the mechanical arm at the end moment of each moving window;