CN113547501B - SLAM-based mobile mechanical arm cart task planning and control method - Google Patents

Abstract

The invention provides a mobile mechanical arm cart task planning and control method based on SLAM, which comprises the following steps: step S1: drawing an environment map by using SLAM and Yolo, acquiring barrier information, and planning a path on the environment map in real time by using a navigation function; step S2: establishing a cart mobile mechanical arm overall dynamic model with incomplete constraint and complete constraint; and step S3: and controlling the mobile mechanical arm by adopting an adaptive neural network control method based on an integral Lyapunov function. The invention provides a whole body control framework based on Yolo and ORB-SLAM2 aiming at the high nonlinearity and model uncertainty of a flexible joint robot model, which can enable a mobile mechanical arm to execute operation in an unknown environment and integrate Yolo obstacle detection, ORB-SLAM2 perception and an integral Lyapunov controller into the whole body movement operation of the mobile mechanical arm.

Description

SLAM-based mobile mechanical arm cart task planning and control method

Technical Field

The invention relates to a robot control method, in particular to a mobile mechanical arm cart task planning and control method based on SLAM.

Background

Mobility, awareness, and intelligence greatly expand the capabilities of mobile operating systems in executing various applications, such as context awareness, complex mobility tasks, human-like tasks, and the like. Among them, the mobile robot arm, as a typical operating system, shows great potential in combination with the flexibility of the mobile base and the diversity of the end-effectors. Compared with a fixed mechanical arm, the movable mechanical arm can perform extensive and flexible operation by virtue of the mechanical arm and the movable platform. The use of the mobile platform greatly expands the operating space, so that the manipulator can perform tasks similar to human tasks, such as pushing a trolley, opening a door, lifting and transporting loads and cleaning tasks. Due to the additional degrees of freedom (DOF) of the mobile platform, more tasks become available.

Despite the tremendous potential of mobile robotic arms, challenges remain in achieving autonomous mobile operational tasks. First, autonomic motion control is critical to task perception and operation in unknown environments. The related work of mobile operations mostly assumes that the environment is known, focusing on controller design, the framework of this type of system lacks the autonomy to perform the operational tasks.

Conventional object detection systems employ a deformable element model (DPM) approach. And (4) extracting the target area by using a sliding box method, and then realizing identification by using a classifier. The method has the problems of low speed and difficult training.

Compared with a basic fixed mechanical arm, the movable mechanical arm is combined with the movable platform, so that flexible operation can be realized. The dynamics of mobile platforms are quite different from robotic arms, and it is a challenge to combine robotic arms and mobile platforms into a comprehensive control system. The dynamic model of the moving mechanical arm system cannot be accurately obtained, and the problem of limitation of an actuating mechanism also exists.

Disclosure of Invention

Aiming at the defects in the prior art, the invention aims to provide a mobile mechanical arm cart task planning and control method based on SLAM.

The invention provides a mobile mechanical arm cart task planning and control method based on SLAM, which is characterized by comprising the following steps:

step S1: drawing an environment map by using SLAM and Yolo, acquiring barrier information, and planning a path on the environment map in real time by using a navigation function;

step S2: establishing an integral dynamic model of the cart mobile mechanical arm with incomplete constraint and complete constraint;

and step S3: and controlling the mobile mechanical arm by adopting an adaptive neural network control method based on an integral Lyapunov function.

Preferably, in step S1, the obstacle function β (r) and the target point position r are determined according to the mobile platform position r, the obstacle function β (r), and the target point position r _d A sufficiently large constant value κ>0, establishing a navigation function

By using the navigation function gradient descent method, the minimum value can be approached without passing through the boundary of the obstacle, and the navigation and path planning from the mobile platform to the target point are realized.

Preferably, the barrier function β (r) is:

wherein beta is _i And (r) is an obstacle function of the ith circular obstacle on the two-dimensional plane, and m is the number of the obstacles. For i e {1, …, m },

wherein r = (x, y) ^T Is the position of the mobile platform, q _i ＝(x _i0 ,y _i0 ) ^T The coordinate position of the centre of a circle, ρ _i Is a radius.

Preferably, the mobile mechanical arm comprises an upper body and a base, the base is provided with two independent driving wheels and four supporting legs, the upper body comprises a trunk, two arms, two hands and a visual sensor arranged on the head, and the visual sensor is used for visual operation of the working space.

Preferably, the motor and the driver of the mobile mechanical arm are powered by a 24V battery, and different motors are provided with different incremental encoders and harmonic reducers, so that accurate joint speed and position control is realized.

Preferably, the vision sensor acquires a real-time image of the surrounding environment, then uses the frame and category probability of the obstacle in the Yolo predicted image, uses ORB-SLAM2 to extract ORB features in the image, encodes the environment in sparse representation, simultaneously performs fast extraction and matching, generates a sparse feature point map, and combines with the obstacle detection result in Yolo to generate an environment map containing obstacle position information.

Preferably, in the step S2, the value of n isThe freedom degree mechanical arm is provided with 2+n flexible joints, and the generalized coordinate of the moving platform is q _s ＝[X,Y,φ] ^T . According to the Lagrange formula of the mobile platform and the mechanical arm, the dynamic equation of the incomplete mobile mechanical arm with the flexible joint can be expressed as

Wherein

For uniform generalized coordinates of the moving arm, q _m ＝[θ _r ,θ _l ,θ ₁ ,…,θ _n ]Angular displacement of the joint, theta, in relation to the wheel and arm joints for a generalized coordinate _m ＝[θ _rm ,θ _lm ,θ _1m ,…,θ _nm ] ^T Is the angular displacement of the trolley. M (q) epsilon R ^(n+5)×(n+5) In order to move the inertial matrix of the robot arm,

is a matrix of nonlinear terms related to the Coriolis/centrifugal force vector, G (q) ∈ R ^(n+5)×1 Is the gravity vector, B ∈ R ^(n+5)×(n+2) Is a full rank input transformation matrix, K ∈ R ^(n+2)×(n+2) Is a diagonal matrix of known joint stiffness, tau epsilon R ^(n+2)×1 Is the input of the mobile platform and the manipulator joint, A (q) belongs to R ^3×(n+5) Is a constrained correlation matrix, J ∈ R ^(n+2)×(n+2) For a diagonal matrix of motor inertia, λ ∈ R ^3×1 Is a lagrange multiplier.

Preferably, the non-complete constraint and the complete constraint of the mobile mechanical arm are

Wherein

Wherein, b is the distance from the driving wheel to the geometric center of the mobile platform, d is the distance from the geometric center of the mobile platform to the mass center of the mobile platform, and r is _w Is the radius of each wheel.

Preferably, the (n + 5) × (n + 2) -dimensional matrix S (q) is defined so that

A(q)S(q)＝0

Wherein c = r _w /2b, therefore

Can be expressed as

The time is derived to obtain

Substituting into kinetic equation and multiplying by S ^T (q) obtaining a kinetic model after transformation of

Preferably, in step S3, the uncertainty of the dynamic model of the mobile manipulator and the external interference are solved by designing an adaptive neural network.

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

1. the invention establishes a map containing the position information of the obstacle based on the Yolo and the SLAM, and carries out real-time path planning and navigation by utilizing the navigation function, thereby improving the autonomy of the system to execute the operation task in an unknown environment.

2. Aiming at the high nonlinearity and model uncertainty of a flexible joint robot model, the invention provides a whole body control framework based on Yolo and ORB-SLAM2, which can enable a mobile mechanical arm to execute operation in an unknown environment, and integrates Yolo obstacle detection, ORB-SLAM2 perception and integral Lyapunov controller into the whole body movement operation of the mobile mechanical arm.

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 system diagram of a mobile robot cart in a task planning and control method of a mobile robot cart based on SLAM according to an embodiment of the present invention;

fig. 2 is a system diagram of a SLAM-based mobile robotic arm cart task planning and control method according to an embodiment of the present invention;

fig. 3 is a schematic diagram of task path planning and navigation of a mobile robotic arm cart in a SLAM-based method for task planning and control of a mobile robotic arm cart according to an embodiment of the present invention.

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.

A task planning and control method of a mobile mechanical arm cart based on SLAM is disclosed, wherein a mobile mechanical arm is composed of an upper body and a base. Referring to fig. 1, the base is a wheeled platform with incomplete constraints, the actual configuration and corresponding speed needs to be estimated for the force control of the vehicle, since the vehicle is not equipped with sensors, sensors on the mobile robot, such as laser rangefinders, encoders and force/torque sensors are used to estimate the configuration of the vehicle, and laser rangefinders are used to estimate the orientation of the vehicle within the mobile frame on the mobile platform.

The upper half of the robot is arranged on a base, and the base is provided with two independent driving wheels and four supporting legs. The upper half body is composed of a trunk, two arms, two hands and a vision sensor arranged on the head, and the vision sensor is used for the vision operation of a large working space. The motors and drives of the system are powered by a 24V battery. In addition, different motors are provided with different incremental encoders and harmonic reducers to realize accurate joint speed and position control, and the subsystems and the control module have 34 degrees of freedom.

A SLAM-based mobile robotic arm cart mission planning and control method, as shown in fig. 2, comprises the following steps: step S1: drawing an environment map by using SLAM and Yolo, acquiring barrier information, and planning a path on the environment map in real time by using a navigation function; step S2: establishing an integral dynamic model of the cart mobile mechanical arm with incomplete constraint and complete constraint; and step S3: and controlling the mobile mechanical arm by adopting an adaptive neural network control method based on an integral Lyapunov function. Specific examples are as follows.

As shown in fig. 3, in step S1, a real-time image of the surrounding environment is acquired by a vision sensor moving the head of the robot arm, and then the border and category probability of an obstacle in the image is predicted by Yolo, ORB features in the image are extracted by ORB-SLAM2 to encode the environment in sparse representation, and at the same time, a sparse feature point map is generated by fast extraction and matching, and an environment map containing obstacle position information is generated in combination with the obstacle detection result in Yolo.

Suppose that the mobile mechanical arm works in space (two-dimension plane)

In which some isolated obstacles exist

i ∈ {1, …, m }, and for each i ≠ j (i, j ∈ [0,m })]) Is provided with

Where index 0 represents a workspace boundary, defined as an obstacle.

Let r = (x, y) ^T Is the position of the moving platform, beta _i (r) is the barrier function of the ith circular barrier in a two-dimensional plane, for i e {1, …, m },

wherein q is _i ＝(x _i0 ，y _i0 ) ^T Is the coordinate position of the center of a circle of a circular obstacle, rho _i Is the radius of a circular obstacle.

In the form of a matrix

The barrier function for multiple barriers is

Wherein m is the number of obstacles according to the position r of the mobile platform, the obstacle function beta (r) and the position r of a target point _d A sufficiently large constant value κ>0, establishing a navigation function

When the mobile platform is in the free configuration space

Is easy to obtain

Only at the target point r _d A unique minimum value of 0 and a normalized maximum value of 1 at the boundary of the obstacle, so

Can approach a minimum without crossing the obstacle boundary, and closed-loop feedback allows the robot to always follow this behavior. Therefore, the navigation function gradient descent method can approach the minimum value without passing through the barrier boundary, and navigation and path planning from the mobile platform to the target point are realized.

In step S2, a cart movement robot global dynamics model having incomplete constraints and complete constraints is established. Considering the flexibility of each joint of the mobile mechanical arm, for the mechanical arm with n degrees of freedom, 2+n flexible joints exist, and the generalized coordinate of the mobile platform is q _s ＝[X，Y,φ] ^T . According to the Lagrange formula of the mobile platform and the mechanical arm, the dynamic equation of the incomplete mobile mechanical arm with the flexible joint can be expressed as

Wherein

For uniform generalized coordinates of the moving arm, q _m ＝[θ _r ,θ _l ,θ ₁ ,…,θ _n ]Angular displacement of the joint, theta, in relation to the wheel and arm joints for a generalized coordinate _m ＝[θ _rm ，θ _lm ,θ _1m ,…,θ _nm ] ^T Is the angular displacement of the trolley. M (q) is belonged to R ^(n+5)×(n+5) In order to move the inertial matrix of the robot arm,

is a matrix of nonlinear terms related to the Coriolis/centrifugal force vector, G (q) ∈ R ^(n+5)×1 Is the gravity vector, B ∈ R ^(n+5)×(n+2) Is a full rank input transformation matrix, K ∈ R ^(n+2)×(n+2) Is a diagonal matrix of known joint stiffness, tau epsilon R ^(n+2)×1 Is the input of the mobile platform and the manipulator joint, A (q) belongs to R ^3×(n+5) Is a constrained correlation matrix, J ∈ R ^(n+2)×(n+2) For a diagonal matrix of motor inertia, λ ∈ R ^3×1 For Lagrange multipliers, τ represents the input moment, R ^nxn A matrix of real numbers representing n x n.

The non-complete constraint and the complete constraint of the mobile mechanical arm are

Wherein

Wherein, b is the distance from the driving wheel to the geometric center of the mobile platform, d is the distance from the geometric center of the mobile platform to the mass center of the mobile platform, and r _w Is the radius of each wheel.

By defining a (n + 5) × (n + 2) -dimensional matrix S (q) such that

A(q)S(q)＝0

Therein is provided with

Wherein c = r _w /2b, therefore

Can be expressed as

The time is derived to obtain

Substituting into kinetic equation and multiplying by S ^T (q) obtaining a kinetic model after transformation of

In step S3, the mobile manipulator is controlled by using an adaptive neural network control method based on the integral lyapunov function, and the process returns to (1) to continue execution. Due to model uncertainty and external interference, it is difficult to obtain M (q),

actual value of G (q). Considering that modeling errors are not available but bounded, a self-adaptive neural network is designed to solve uncertainty and interference, and a high-dimensional integral Lyapunov self-adaptive control framework suitable for a mobile mechanical arm is provided. The goal is to design an input τ so that the reference trace can be tracked by the output.

First, define

M(q)＝S ^T (q)M(q)S(q),

G(q)＝S ^T (q)G(q),B＝S ^T (q)B,U＝Bτ

Then the system

Can be rewritten as

Let M _d (q)∈R ^(n+2)×(n+2) Is M _dii(q) Diagonal matrix of not equal to 0, M _dii(q) Represents M _d (q)(i,i)。

Define M = n +2 and divide M (q) into the following two parts:

M(q)＝M _d (q)+Δ _M

wherein Δ _M The matrix is unknown, available

Wherein

Considering D as a disturbance in the system, an observer was designed to estimate D.

Considering the kinetic model, there are

F (x) cannot be obtained due to model uncertainty. As a class of multi-layer feedforward networks, any continuous function can be approximated as a radial basis function neural network. Thus, F (x) is approximated as

F(x)＝W ^*T R(x)+∈

Where x is the neural network input vector,

is the optimal weight vector of the weight vector,

given a bounded approximation error with node number l > 1, R (x) = [ R = ₁ (x),…,R _l (x)]Is the radial basis function.

Assuming unknown external disturbance D and its derivatives

Are bounded. I.e. the existence of the inequality

Is unknown constant d _M And ρ. The filtered tracking error E and error E are defined as follows, E = [ r ] ₁ ，…，r _M ] ^T ：

e＝y-y _d

Can be obtained where Λ is a positive definite symmetric matrix, y _d Is a reference track. Obtaining a series of linear differential equations when r _i When =0, its solution e _i (i =1, …, m) converges to zero.

So that there are

Wherein

E＝[r ₁ ，…,r _M ] ^T ,v＝[v ₁ ,…,ν _m ] ^T ；

The input U is to ensure E → 0. Consider the following integral Lyapunov function candidate

Wherein

And the matrix alpha is formed by R ^m×m Is a positive definite diagonal matrix.

B _α Is represented by B _α ＝M _d (q)α＝diag[M _d (q)(i，i)*α(i，i)] _m*m ，B _αii Is represented by B _α (i，i)，r _i Is represented by r _i ＝E _i (E＝[r ₁ ,…,r _M ] ^T )，v _i To represent

α _ii Represents α (i, i).

Setting alpha ₁₁ ＝…＝α _mm ,

Where xi = [ xi ] ₁ ，ξ ₂ ,…,ξ _m ] ^T ∈R ^m ,ξ _i ＝λ _i e _i I =1,2, …, m _i (i =1,2, …, m0, may be such that e → 0 is true when v → 0,

is a scalar independent of E, x and v, and selects the appropriate M _d (q) and alpha to ensure M _dii (q)α _ii Is greater than 0. The integral Lyapunov function can be re-expressed as

Based on B _α And

the fact of symmetry, taking the time derivative of the Lyapunov function, has

And is

Is provided with

Can obtain

Is provided with

Wherein

Is a scalar, independent of v, because

Is provided with

And

then is provided with

Due to M _d (q), α and M _d (q) α is symmetrical, having

Wherein

Definition of

Is provided with

To achieve the estimation of D, an auxiliary variable z is designed:

z＝D-K _d x ₂

wherein

Is the matrix to be designed.

Differentiating z with time t to obtain

To achieve the estimation of the system disturbance D, there is first an estimation of z, an estimation variable

The update rule of (D) is described as follows, and the estimated value of D is defined as

Estimate of disturbance D

Can be obtained from the following formula

Definition of

An estimation error expressed as a perturbation, having

Differentiated with respect to time t, then combined, one can obtain

Based on the above analysis, the adaptive control input is designed as follows:

wherein

Is W ^* Estimation of, K ₁ ∈R ^m×m Is a positive definite diagonal matrix and,

is a neural network updating law, and omega is a diagonal element of omega _i A matrix of directly opposing angular constants (i =1,2, …, m),

is a constant number of times that the number of the first,

is W ^* Is estimated.

The following Lyapunov function candidates are designed as

Wherein

By differentiation with time t, can be obtained

Substituting into the controller to obtain

Taking into account the fact that

Is provided with

Because of the fact that

Substituting into the controller input formula, have

Wherein

Wherein λ is _max (. Cndot.) and λ _min (. Cndot.) are the maximum and minimum eigenvalues of (. Cndot.) respectively. Controller parameter K ₁ And K _d Must satisfy (K) ₁ -I _m×m ) Not less than 0 and

two sides of the same time e ^κt Multiplication and integration over time t can result in

From the above inequality, V ₂ Finally, bounded when t → ∞, which means that E,

and

and is also bounded.

Those skilled in the art will appreciate that, in addition to implementing the system and its various devices, modules, units provided by the present invention as pure computer readable program code, the system and its various devices, modules, units provided by the present invention can be fully implemented by logically programming method steps in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system and various devices, modules and units thereof provided by the present invention can be regarded as a hardware component, and the devices, modules and units included therein for implementing various functions can also be regarded as structures within the hardware component; means, modules, units for realizing various functions can also be regarded as structures in both software modules and hardware components for realizing the methods.

In the description of the present application, it is to be understood that the terms "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", and the like indicate orientations or positional relationships based on those shown in the drawings, merely for convenience of description and simplicity of description, and do not indicate or imply that the device or element being referred to must have a particular orientation, be constructed in a particular orientation, and be operated, and therefore, are not to be construed as limiting the present application.

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 (10)

1. A task planning and control method of a mobile mechanical arm cart based on SLAM is characterized by comprising the following steps:

step S1: drawing an environment map by using SLAM and Yolo, acquiring barrier information, and planning a path on the environment map in real time by using a navigation function;

step S2: establishing a cart mobile mechanical arm overall dynamic model with incomplete constraint and complete constraint;

and step S3: and controlling the mobile mechanical arm by adopting an adaptive neural network control method based on an integral Lyapunov function.

2. The SLAM-based mobile robotic arm cart mission planning and control method of claim 1, wherein: in the step S1, according to the position r of the mobile platform, the barrier function beta (r) and the position r of the target point _d A sufficiently large constant value κ>0, establishing a navigation function

By using the navigation function gradient descent method, the minimum value can be approached under the condition that the boundary of the obstacle does not pass, and the navigation and the path planning from the mobile platform to the target point are realized.

3. The SLAM-based mobile robotic arm cart mission planning and control method of claim 2, wherein: the barrier function β (r) is:

wherein beta is _i (r) is an obstacle function of the ith circular obstacle on the two-dimensional plane, and m is the number of the obstacles; for i e {1, …, m },

wherein r = (x, y) ^T Is the position of the mobile platform, q _i ＝(x _i0 ,y _i0 ) ^T The coordinate position of the centre of a circle, ρ _i Is a radius.

4. The SLAM-based mobile robotic arm cart mission planning and control method of claim 1, wherein: the movable mechanical arm comprises an upper body and a base, the base is provided with two independent driving wheels and four supporting legs, the upper body comprises a trunk, two arms, two hands and a visual sensor arranged on the head, and the visual sensor is used for visual operation of a working space.

5. The SLAM-based mobile robotic arm cart mission planning and control method of claim 4, wherein: the motor and the driver of the mobile mechanical arm are powered by a 24V battery, and different motors are provided with different incremental encoders and harmonic reducers, so that accurate joint speed and position control is realized.

6. The SLAM-based mobile robotic arm cart mission planning and control method of claim 4, wherein: the visual sensor acquires a real-time image of the surrounding environment, then the frame and category probability of an obstacle in a picture are predicted by using the Yolo, ORB features in the image are extracted by using ORB-SLAM2 to encode the environment in sparse representation, meanwhile, the sparse feature point map is quickly extracted and matched to generate a sparse feature point map, and the sparse feature point map is combined with an obstacle detection result in the Yolo to generate an environment map containing obstacle position information.

7. The SLAM-based mobile robotic arm cart mission planning and control method of claim 1, wherein: in the step S2, for the n-degree-of-freedom mechanical arm, 2+n flexible joints are arranged, and the generalized coordinate of the mobile platform is q _s ＝[X,Y,φ] ^T (ii) a According to the Lagrange formula of the mobile platform and the mechanical arm, the dynamic equation of the incomplete mobile mechanical arm with the flexible joint can be expressed as

Wherein

For uniform generalized coordinates of the moving arm, q _m ＝[θ _r ,θ _l ,θ ₁ ,…,θ _n ]Angular displacement of the joint, theta, in relation to the wheel and arm joints for a generalized coordinate _m ＝[θ _rm ,θ _lm ,θ _1m ,…,θ _nm ] ^T Is the angular displacement of the trolley; m (q) epsilon R ^(n+5)×(n+5) In order to move the inertial matrix of the robot arm,

is a matrix of nonlinear terms related to the Coriolis/centrifugal force vector, G (q) ∈ R ^(n+5)×1 Is the gravity vector, B ∈ R ^(n+5)×(n+2) Is a full rank input transformation matrix, K ∈ R ^(n+2)×(n+2) Is a diagonal matrix of known joint stiffness, tau epsilon R ^(n+2)×1 Is the input of the mobile platform and the manipulator joint, A (q) belongs to R ^3×(n+5) Is a constrained correlation matrix, J ∈ R ^(n+2)×(n+2) For a diagonal matrix of motor inertia, λ ∈ R ^3×1 Is a lagrange multiplier;

R ^nxn a matrix of real numbers representing n x n.

8. The SLAM-based mobile robotic arm cart mission planning and control method of claim 7, wherein: the non-complete constraint and the complete constraint of the mobile mechanical arm are

Wherein

Wherein, b is the distance from the driving wheel to the geometric center of the mobile platform, d is the distance from the geometric center of the mobile platform to the mass center of the mobile platform, and r is the radius of each wheel.

9. The SLAM-based mobile robotic arm cart mission planning and control method of claim 8, wherein: by defining a (n + 5) × (n + 2) -dimensional matrix S (q) such that

A(q)S(q)＝0

Wherein c = r/2b, thus

Can be expressed as

The time is derived to obtain

Substituting into kinetic equation and multiplying by S ^T (q) obtaining a kinetic model after transformation of

10. The SLAM-based mobile robotic arm cart mission planning and control method of claim 1, wherein: in the step S3, the uncertainty and the external interference of the dynamic model of the mobile manipulator are solved by designing the adaptive neural network.

Images