System for monitoring stability of operation of autonomous robots

11886200 ยท 2024-01-30

Assignee

Inventors

Cpc classification

International classification

Abstract

System for monitoring stability of autonomous robot, including a GNSS navigation receiver including antenna, analog front end, plurality of channels, inertial measurement unit (IMU) and a processor, all generating navigation and orientation data for the robot; based on the navigation and orientation data, calculating position and direction of movement for the robot; calculating spatial and orientation coordinates z.sub.1, z.sub.2 of the robot, relating to the position and direction of movement; continuing with programmed path for the robot for any spatial and orientation coordinates z.sub.1, z.sub.2 within an attraction domain, where a measure V(z) of distance from zero in z.sub.1, z.sub.2 plane are defined by Lurie-Postnikov functions and is less than 1; for spatial and orientation coordinates outside the attraction domain with V(z)>1, terminating the programmed path and generating notification.

Claims

1. A system for monitoring stability of an autonomous robot, the system comprising: a Global Navigation Satellite System (GNSS navigation receiver including an antenna, an analog front end, a plurality of channels, an inertial measurement unit (IMU) and a processor programmed with a path for the autonomous robot, all generating navigation and orientation data for the autonomous robot; based on the navigation and the orientation data, the system calculating a position and a direction of movement for the autonomous robot; the system calculating spatial and orientation coordinates z.sub.1, z.sub.2 of the robot, which relate to the position and the direction of movement; the system continuing with a programmed path for the robot for any spatial and orientation coordinates z.sub.1, z.sub.2 within an attraction domain, such that a measure V(z) of distance from zero in a z.sub.1, z.sub.2 plane defined by V ( z ) = z T Pz + 0 c T z ( z 2 , ) d , where P is a 2 by 2 matrix z.sup.T is (z.sub.1, z.sub.2).sup.T, T stands for transposition, is a scalar coefficient, is a control function defined by ( z 2 , ) = s u _ ( ( 1 + z 2 2 ) 3 2 ) ( 1 + z 2 2 ) 3 2 , s u _ ( u ) = { - u _ for u - u _ , u for .Math. "\[LeftBracketingBar]" u .Math. "\[RightBracketingBar]" < u , u _ for u u _ , = 2 z 1 + 2 z 2 , is a desired rate of exponential decrease of lateral displacement; and for any spatial and orientation coordinates outside the attraction domain, the system terminating the programmed path.

2. The system of claim 1, wherein, for any spatial and orientation coordinates within the attraction domain with a 0.75<V(z)1, continuing the programmed path with a be on the look-out status.

3. The system of claim 1, wherein, for any spatial and orientation coordinates within the attraction domain with a V(z)0.75, continuing the programmed path.

4. The system of claim 1, where the spatial and orientation coordinates z.sub.1, z.sub.2 are subject to a dynamic model that uses z 1 = z 2 , z 2 = u ( 1 + z 2 2 ) 3 2 . where u is a desired curvature of a path of a target point of the autonomous robot.

5. The system of claim 1, wherein the attraction domain is defined by ()={z:V(z).sup.2}, wherein 1 is a measure of how deep into the attraction domain the autonomous robot is at present.

6. A system for monitoring stability of an autonomous robot, the system comprising: a Global Navigation Satellite System (GNSS}navigation receiver including an antenna, an analog front end, a plurality of channels, an inertial measurement unit (IMU) and a processor programmed with a path for the autonomous robot, all generating navigation and orientation data for the autonomous robot; based on the navigation and the orientation data, the system calculating a position and a direction of movement for the autonomous robot; the system calculating spatial and orientation coordinates zi, z2 of the autonomous robot, which relate to the position and the direction of movement; the system continuing with a programmed path for any spatial and orientation coordinates z.sub.1, z.sub.2 within an attraction domain, such that a measure V(z) of distance from zero in a z.sub.1, z.sub.2 plane are defined by Lurie-Postnikov functions and is less than 1; and for any spatial and orientation coordinates outside the attraction domain with V(z)>1, the system terminating the programmed.

7. The system of claim 6, wherein the function V(z) is defined by V ( z ) = z T Pz + 0 c T z ( z 2 , ) d , where P is a 2 by 2 matrix z.sup.T is (z.sub.1, z.sub.2).sup.T, T stands for transposition, is a scalar coefficient, is a control function defined by: ( z 2 , ) = s u _ ( ( 1 + z 2 2 ) 3 2 ) ( 1 + z 2 2 ) 3 2 , s u _ ( u ) = { - u _ for u - u _ , u for .Math. "\[LeftBracketingBar]" u .Math. "\[RightBracketingBar]" < u , u _ for u u _ , = 2 z 1 + 2 z 2 , is a desired rate of exponential decrease of lateral displacement.

8. The system of claim 6, wherein, for any spatial and orientation coordinates within the attraction domain with a 0.75<V(z)1, continuing the programmed path with a be on the look-out status.

9. The system of claim 6, wherein, for any spatial and orientation coordinates within the attraction domain with a V(z)0.75, continuing the programmed path.

10. The system of claim 6, where the spatial and orientation coordinates z.sub.1, z.sub.2 are subject to a dynamic model that uses z 1 = z 2 , z 2 = u ( 1 + z 2 2 ) 3 2 . where u is a desired curvature of a path of a target point of the autonomous robot.

11. The system of claim 6, wherein the attraction domain is defined by ()={z:V(z).sup.2}, wherein 1 is a measure of how deep into the attraction domain the autonomous robot is at present.

Description

BRIEF DESCRIPTION OF THE ATTACHED FIGURES

(1) The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the principles of the invention.

(2) In the drawings:

(3) FIG. 1 illustrates the kinematic schematic of the wheeled robot.

(4) FIG. 2 illustrates the equivalent kinematic schematic with the single steering wheel.

(5) FIG. 3 illustrates the exemplary target path and the target point of the robot.

(6) FIG. 4 illustrates the attraction domain constructed for the wheeled robot.

(7) FIG. 5 illustrates the curvilinear path comprised of several segments.

DETAILED DESCRIPTION OF EMBODIMENTS OF THE INVENTION

(8) Reference will now be made in detail to the embodiments of the present invention, examples of which are illustrated in the accompanying drawings.

(9) A detailed description will be given for the wheeled robot model (agricultural tractor, lawn mower), although it will be understood the concept is applicable more broadly than just these types of devices. Consider the kinematic model of a wheeled robot, described, for example, in [2]. The model describes the platform with four wheels moving without lateral slippage (FIG. 1). The rear wheels are assumed to be driving and the front wheels responsible for steering of the platform. Two steering wheels can be schematically replaced with the one equivalent, see FIG. 2. The planar motion is described by position , of the target point of the platform and angle describing the orientation of the platform with respect to the fixed reference system O. The target point is taken located in the middle of the rear axle, is the angle between the direction of the velocity vector of the target point and the -axis. The set of GNSS and inertial sensors (accelerometers, gyroscopes) is used to estimate position of the target point in real time. The GNSS antenna or plurality of antennas is located on the body of AMR with best reception of the GNSS signal. The antenna position is then recalculated to the target point.

(10) In the exemplary simple case the kinematic equations of the wheeled robot are:
{dot over ()}=v cos ,
{dot over ()}=v sin ,
{dot over ()}=vu.(1)

(11) Here, the dot over a symbol denotes differentiation with respect to time, vv(t) is a scalar linear velocity of the target point. In the general case the model may include description of the internal dynamics of actuators, interaction of tires with soil, backlash the steering shaft and other mechanical joints, other physical phenomena. What is significant, the description of the robot must be done in terms of differential equation, similar to (1), but probably more complex, including greater number of state variables.

(12) The control variable u is the instantaneous curvature of the trajectory circumscribed by the target point, which is related to the angle of rotation of the front wheels. The control goal is to stabilize the motion of the target point along the -axis, when the lateral deviation and the angular deviation are equal to zero. The constraining condition of the control resources can be written as the two-sided constraints:
u,(2)

(13) where =1/R.sub.min is the maximum possible curvature of the actual trajectory circumscribed by the target point and R.sub.min represents the minimum possible turning radius of the platform. Values R.sub.min and .sub.max are the maximum turning angle of the steering wheel, are related as R.sub.min=L/tan .sub.max, where L is the distance between axles, see FIG. 1 and FIG. 2.

(14) The model (1) can be further generalized for the case of any oblique straight line. It has been shown (see [2] for example) that, by changing the state variables and the independent variable, the straight path following problem can be written in the canonical form:

(15) z 1 = z 2 , z 2 = u ( 1 + z 2 2 ) 3 2 . ( 3 )

(16) In (3), z.sub.1 is the lateral deviation of the robot from the target path and z.sub.2=tan , where is the angle between the direction of the velocity vector and the target path. The prime denotes differentiation with respect to the new independent variable .

(17) Ignoring two-sided constraints on control and using a feedback linearization technique [1] leads to the choice of control u in the form

(18) u = - ( 1 + z 2 2 ) 3 2 for some > 0 and = 2 z 1 + 2 z 2 , ( 4 )

(19) where is desired rate of exponential decrease of the lateral displacement. Then the system (3) closed by the control function (4) in the feedback loop takes the form
z.sub.1=z.sub.2,
z.sub.2=.(5)

(20) This system is equivalent to z.sub.1+2.sub.1+.sup.2z.sub.1=0, which implies the exponential convergence with rate of both z.sub.1 and z.sub.2. The particular form of function (4) determines poles of the closed-loop system (5). For the given function, the system has one pole of multiplicity 2. The other forms of the function will imply different poles. The problem of pole placement is beyond the scope of this discussion, but generally a discussion may be found in literature, e.g., [9], incorporated by reference herein in its entirety.

(21) However, in general, control (4) does not satisfy the two-sided constraints (2). Taking control in the form

(22) u = - s u _ ( ( 1 + z 2 2 ) 3 2 ) , where ( 6 ) s u _ ( u ) = { - u for u - u , u for | u | < u , u for u u , ( 7 )

(23) may not guarantee that z.sub.1 and z.sub.2 decrease exponentially with given rate of exponential stability. Moreover, undesirable overshoot in variations of the variables is possible. Especially dangerous is overshoot in the variable z.sub.1. This means that even starting sufficiently close to the target trajectory, the robot may perform large oscillations going far enough even if the closed loop system is stable [2].

(24) Thus, even if the closed loop system maintains stable behavior, overshoots in the lateral displacement may happen, which may violate local geometry constraints. It means that the whole motion of the robot must be inscribed into guaranteed invariant domain, which is also the attraction domain.

(25) Thus, we have finally arrived at the problem: how to describe the attraction domain inscribed into the band of certain width around axes z.sub.1, z.sub.2 and guaranteeing a prescribed exponential convergence rate. Equations in (3) take the form

(26) z 1 = z 2 , z 2 = - s u _ ( ( 1 + z 2 2 ) 3 2 ) ( 1 + z 2 2 ) 3 2 = - ( z 2 , ) , ( 8 ) ( z 2 , ) = { u _ ( 1 + z 2 2 ) 3 2 for u _ ( 1 + z 2 2 ) 3 2 , for .Math. "\[LeftBracketingBar]" .Math. "\[RightBracketingBar]" < u _ ( 1 + z 2 2 ) 3 2 , - u _ ( 1 + z 2 2 ) 3 2 for - u _ ( 1 + z 2 2 ) 3 2 . ( 9 )

(27) This is everything needed to explore the absolute stability approach [4] for analysis of the nonlinear system (8), (9). The control system can now be rewritten in the form of closed-loop system with nonlinear feedback

(28) z = Az + b ( z 2 , ) , ( 10 ) = c T z , where ( 11 ) A = [ 0 1 0 0 ] , b = [ 0 - 1 ] , c = [ 2 2 ] , z = [ z 1 z 2 ] . ( 12 )

(29) Given a positive value .sub.0, assume that the variable takes values from the interval
.sub.0.sub.0.(13)

(30) Then the function (9) satisfies the sector constraints

(31) u _ 0 ( z 2 , ) 1. ( 14 )

(32) The absolute stability means the asymptotic stability of the system (8), (9), whatever nonlinearity there exists, can exist only if it satisfies the sector condition (14). In the absolute stability theory traditionally two classes of nonlinearities are considered: (a) stationary (or time invariant) nonlinearity of the form (), (b) non-stationary (or time-varying) nonlinearity of the form (,).

(33) As shown in [4], the systems (a) are usually analyzed using Lyapunov functions from the Lurie-Postnikov class. The systems (b) are treated using quadratic Lyapunov functions. Lurie-Postnikov functions allow for less conservative absolute stability conditions comparing with those obtained by using quadratic functions. The problem with using Lurie-Postnikov functions for analysis of systems (b) is the need to take derivative over explicitly presented variable . Here, we are analyzing the absolute stability of the system (8), with nonlinearity (9), depending on variables z.sub.2 (implicitly depending on ) and . Thus, use of the Lurie-Postnikov function is extended for the class of nonlinearities (9). Using this approach, we obtain estimates of the attraction domain wider than estimates previously obtained using quadratic Lyapunov functions. The candidate for the Lyapunov function is taken in the form

(34) V ( z ) = z T Pz + 0 c T z ( z 2 , ) d . ( 15 )

(35) The symmetric positively definite 22 matrix P and the scalar must be chosen in such a way, that

(36) V ( z ) = dV ( z ) d < 0 for all z 0. ( 16 )

(37) Even more strong inequality guarantees exponential stability:
V(z)+2V(z)<0(17)

(38) with exponential rate , 0<<.

(39) As already mentioned, we are looking for attraction domain of the form
()={z:V(z).sup.2},(18)

(40) the constant must be chosen to maximize the size of the set (). Given P and the maximum size will be reached at the maximum possible for which the condition (17) holds. The function (16) is homogeneous with respect to P and . Therefore, we can set =1 and choose unknown parameters P and in such a way that the volume of (1) will be maximized.

(41) As an example of geometric constraints, we take a rectangular
(a.sub.1,a.sub.2)={z:|z.sub.1|.sub.1,|z.sub.2|.sub.2}.(19)

(42) Constraints (13) define the strip |c.sup.Tz|.sub.0 whose intersection with (19) gives
(a.sub.1,a.sub.2,.sub.0)={z:|z.sub.1|.sub.1,|z.sub.2.sub.2,|c.sup.Tz|.sub.0}.(20)

(43) Further, given .sub.0 the sector constraint (14) can be rewritten in the form

(44) ( u _ 0 c T z - ( z 2 , ) ) ( ( z 2 , ) - c T z ) 0. ( 21 )

(45) The function (9) satisfies (21) if only satisfies (13).

(46) The search for parameters P and ensuring the condition (17) in the domain (1) which, in turn, is inscribed in the region (19) is very complicated due to the complexity of the nonlinear function (9). Using the results of the theory of absolute stability, we replace a specific function (9) with a whole class of functions satisfying only condition (21). The plan is to construct the attraction domain, satisfying the geometry constraints, which is as follows. Find such parameters P and , that: (a) condition (17) is satisfied in (20) for every function (z.sub.2,) satisfying quadratic constraint (21), (b) the domain .sub.0(1) is inscribed in (a.sub.1, a.sub.2,.sub.0).

(47) We want to optimize the choice of parameters in such a way that the attraction domain is as wide as possible can be formulated in terms of minimization of the matrix trace
tr(P).fwdarw.min(22)

(48) under constraints (a) and (b) that, in turn, can be written as Linear Matrix Inequalities (LMI, see [6]) with respect to parameters P and .

(49) After a series of intermediate algebraic computations, which we omit, we present the final algorithm, including semidefinite optimization for which any of convex optimization packages can be used (see for example [7]). Note that convex optimization is numerically tractable and allows a very efficient computer implementation.

(50) 0 Denote 0 = u _ 0 , e - ( 0 , 1 ) T , ( 23 ) 0 = u _ 0 , e = ( 0 1 ) , T = [ - 0 cc T 1 2 ( 0 + 1 ) c 1 2 ( 0 + 1 ) c T - 1 ] , E = 1 2 [ ce T + ec T - e - e T 0 ] and sgn = { 1 for 0 , - 1 for < 0.

(51) Let the matrices Q(,P) and M(,P) are defined by quadratic forms depending on the vector

(52) Y = ( z ) R 3 :

(53) Y T Q ( , P ) Y = { z T Pz + ( c T z - 1 2 ) z T Pz + ( c T z ) 2 2 for 0 , z T Pz for 0 , ( 25 ) z T M ( , P ) z = { z T Pz for 0 , z T Pz + ( c T z ) 2 2 for < 0. ( 26 )

(54) Assume that for given numbers >0, .sub.0>, >0, 0<, a.sub.1>0, a.sub.2>0, and .sub.0=1+a.sub.2.sup.2; the following Linear Matrix Inequalities (LMIs) in variables P,, and .sub.i0, i=1, 2, 3, are feasible:

(55) R ( , P ) + 2 Q ( , P ) + 1 T - ( 3 u _ 2 - 2 sgn ) E 0 , ( 27 ) R ( , P ) + 2 Q ( , P ) + 1 T - ( 3 u _ 2 0 2 + 3 sgn ) E 0 , ( 28 ) [ M ( , P ) c c T 0 2 ] 0 , ( 29 ) M ( P , ) - [ 1 a 1 2 0 0 0 ] 0 , M ( P , ) - [ 0 0 0 1 a 2 2 ] 0 , ( 30 )

(56) then the domain (1).Math.(a.sub.1, a.sub.2,.sub.0) is an invariant estimate of the attraction domain of the system (3) under control (6). Moreover, along the trajectories of the system starting at z(0)(1), the solution z() decreases exponentially at the rate .

(57) LMIs (27)-(30) define constraints of the semidefinite convex optimization problem. The cost function is defined as

(58) min P , tr ( M ( , P ) ) . ( 31 )

(59) FIG. 4 shows an example of the attraction domain (1) obtained for certain values, describing the problem.

(60) Different wider class of geometry constraints, except those described in (19), can be described in terms of LMI's and included into convex optimization problem similar to the one described above.

(61) The method proposed in this invention can be generalized to the case of curvilinear target paths (see [8]), like shown in FIG. 5 where the path consisting of several segments is shown, while the single straight line segment is considered in the invention description. Generalization to the case of several segments is straightforward.

(62) Parameters P, , and probably 1 define the invariant attraction domain () which is stored in the internal memory of the onboard controller. The real state of the robot measured by GNSS and inertial sensors is further recalculated into variables z.sub.1, z.sub.2 which are further checked for condition

(63) ( z 1 z 2 ) ( ) . ( 32 )

(64) If (32) holds, then the onboard computer takes decision status OK and the robot continues autonomous operation. Otherwise, if the condition (32) fails, the onboard computer decides to take the status alarm. The system identifies itself as being in unsafe condition and therefore, additional actions must be taken. For example: the engine must be stopped and the flashing light is on, operation must be switched to the backup algorithm or the search mode (must be a subject of a separate patent).

(65) If the robot is equipped with the light tower allowing multiple color lights, the status OK or status alarm decision can be further quantified in the following way: if (z.sub.1, z.sub.2).sup.T() where 0.75 (for example), the status is OK and the light has the green color, if (z.sub.1, z.sub.2).sup.T() where 0.75<1, the status is OK but wide-awake and the light has the yellow color, if (32) fails, the status is alarm, the light is red, engine stopped, the message is sent to the central facility.

(66) As will be appreciated by one of ordinary skill in the art, the various blocks and components of the receiver and robot control logic shown in the figures can be implemented as discrete hardware components, as an ASIC (or multiple ASICs), as an FPGA, as either discrete analog or digital components, and/or as software running on a processor.

(67) Having thus described the different embodiments of a system and method, it should be apparent to those skilled in the art that certain advantages of the described method and apparatus have been achieved. It should also be appreciated that various modifications, adaptations, and alternative embodiments thereof may be made within the scope and spirit of the present invention. The invention is further defined by the following claims.

REFERENCES

Incorporated Herein by Reference in their Entirety

(68) 1. H. Khalil, Nonlinear Systems. Michigan State University, East Lansing. 2002. ISBN-13: 978-0130673893. 2. Rapoport L. B.: Estimation of Attraction Domains in Wheeled Robot Control. Automation and Remote Control. 67. (9), 1416-1435 (2006). DOI: 10.1134/S0005117906090062 3. U.S. Pat. No. 8,224,525B1 to Rapoport et al., Indication of Attraction Domains for Controlled Planar Motion of a Ground Vehicle. Jul. 17, 2012 4. Aizerman, M. A. and Gantmacher, F. R.: Absolute Stability of Regulation Systems. Holden Day, San Francisco (1964). 5. Cui J. and Sabaliauskaite G. On the alignment of safety and security for autonomous vehicles. In: Proceedings of IARIA CYBER, pp. 59-64. Barcelona (2017), ISBN: 978-1-61208-605-7 6. S. Boyd, S., Ghaoui, L. E., Feron, E., and Balakrishnan, V.: Linear Matrix Inequalities in System and Control Theory. SIAM, Philadelphia (1994). 7. Grant, M. and Boyd, S.: Graph implementations for nonsmooth convex programs. In: Recent Advances in Learning and Control (a tribute to M. Vidyasagar), Blondel, V., Boyd, S., and Kimura, H. (eds.), Lecture Notes in Control and Information Sciences, pp. 95-110. Springer (2008). 8. Pesterev A. V., Rapoport L. B.: Stabilization Problem for a Wheeled Robot Following a Curvilinear Path on Uneven Terrain. Journal of Computer and Systems Sciences International. 49. (4). 672-680 (2010). DOI: 10.1134/S1064230710040155 9. Polderman, J. W., A state space approach to the problem of adaptive pole placement, Math. Control Signals Systems. 2: 71-94 (1989). DOI:10.1007/BF02551362