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

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

Info

Publication number
CN113547501A
CN113547501A CN202110865506.9A CN202110865506A CN113547501A CN 113547501 A CN113547501 A CN 113547501A CN 202110865506 A CN202110865506 A CN 202110865506A CN 113547501 A CN113547501 A CN 113547501A
Authority
CN
China
Prior art keywords
mechanical arm
mobile
slam
control method
cart
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110865506.9A
Other languages
Chinese (zh)
Other versions
CN113547501B (en
Inventor
李智军
周丙睿
李国欣
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Science and Technology of China USTC
Original Assignee
University of Science and Technology of China USTC
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Science and Technology of China USTC filed Critical University of Science and Technology of China USTC
Priority to CN202110865506.9A priority Critical patent/CN113547501B/en
Publication of CN113547501A publication Critical patent/CN113547501A/en
Application granted granted Critical
Publication of CN113547501B publication Critical patent/CN113547501B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J5/00—Manipulators mounted on wheels or on carriages
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

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 an integral dynamic model of the cart mobile mechanical arm with incomplete constraint and complete constraint; 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 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 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 a load 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;
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 rdA sufficiently large constant value κ>0, establishing a navigation function
Figure BDA0003187179570000021
Figure BDA0003187179570000022
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:
Figure BDA0003187179570000023
wherein beta isiAnd (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 },
Figure BDA0003187179570000024
where r ═ (x, y)TIs the position of the mobile platform, qi＝(xi0,yi0)TIs the center of a circular obstacleCoordinate position of (1), ρiIs 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, and then uses the Yolo to predict the frame and category probability of the obstacle in the 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 the sparse feature point map with the obstacle detection result in Yolo to generate an environment map containing obstacle position information.
Preferably, in step S2, for the n-degree-of-freedom mechanical arm, there will be 2+ n flexible joints, and the generalized coordinate of the mobile platform is qs＝[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
Figure BDA0003187179570000031
Figure BDA0003187179570000032
Wherein
Figure BDA0003187179570000033
For uniform generalized coordinates of the moving arm, qm＝[θr,θl,θ1,…,θn]Is the angular displacement of the connecting joint with the wheel and arm joint in a generalized coordinate,θm＝[θrm,θlm,θ1m,…,θnm]Tis 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,
Figure BDA0003187179570000034
is a matrix of nonlinear terms related to the Coriolis/centrifugal force vector, G (q) e R(n+5)×1Is 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)×1Is the input of the mobile platform and the robot joint, A (q) epsilon R3×(n+5)Is a constrained correlation matrix, J ∈ R(n+2)×(n+2)For a diagonal matrix of motor inertia, λ ∈ R3×1Is a lagrange multiplier.
Preferably, the incomplete constraint and the complete constraint of the mobile mechanical arm are
Figure BDA0003187179570000035
Wherein
Figure BDA0003187179570000036
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 rwIs the radius of each wheel.
Preferably, the (n +5) × (n +2) -dimensional matrix S (q) is defined
A(q)S(q)＝0
Figure BDA0003187179570000037
Wherein c is rw/2b, therefore
Figure BDA0003187179570000038
Watch capable of showingShown as
Figure BDA0003187179570000039
The time is derived to obtain
Figure BDA00031871795700000310
Substituting into the kinetic equation and multiplying it by ST(q) obtaining a kinetic model after transformation of
Figure BDA0003187179570000041
Figure BDA0003187179570000042
Preferably, in step S3, the adaptive neural network is designed to solve the uncertainty and the external disturbance of the dynamic model of the mobile manipulator.
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. The invention provides a whole body control framework based on Yolo and ORB-SLAM2 aiming at 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 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; 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 of the moving robot head, and then the border and class probability of an obstacle in the image is predicted using Yolo, ORB features in the image are extracted using ORB-SLAM2 to encode the environment in a sparse representation, while fast extraction and matching are performed, a sparse feature point map is generated, 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)
Figure BDA0003187179570000051
In which some isolated obstacles are present
Figure BDA0003187179570000052
i ∈ {1, …, m }, and for each i ≠ j (i, j ∈ [0, m })]) Is provided with
Figure BDA0003187179570000053
Where index 0 represents a workspace boundary, defined as an obstacle.
Let r be (x, y)TIs the position of the moving platform, betai(r) is the barrier function for the ith circular barrier in the two-dimensional plane, for i e {1, …, m },
Figure BDA0003187179570000054
wherein q isi＝(xi0，yi0)TIs the coordinate position of the center of a circle of a circular obstacle, rhoiIs the radius of a circular obstacle.
In the form of a matrix
Figure BDA0003187179570000055
The barrier function for multiple barriers is
Figure BDA0003187179570000056
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 pointdA sufficiently large constant value κ>0, establishing a navigation function
Figure BDA0003187179570000061
Figure BDA0003187179570000062
When the mobile platform is in the free configuration space
Figure BDA0003187179570000063
Is easy to obtain
Figure BDA0003187179570000064
Only at the target point rdA unique minimum value of 0 and a normalized maximum value of 1 at the boundary of the obstacle, so
Figure BDA0003187179570000065
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 SAnd 2, establishing a cart moving mechanical arm overall dynamic model with incomplete constraint and complete constraint. 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 qs＝[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
Figure BDA0003187179570000066
Figure BDA0003187179570000067
Wherein
Figure BDA0003187179570000068
For uniform generalized coordinates of the moving arm, qm＝[θr,θl,θ1,…,θn]Angular displacement of a joint, theta, in a generalized coordinate relation to the wheel and arm jointsm＝[θrm，θlm,θ1m,…,θnm]TIs 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,
Figure BDA0003187179570000069
is a matrix of nonlinear terms related to the Coriolis/centrifugal force vector, G (q) e R(n+5)×1Is 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)×1Is the input of the mobile platform and the robot joint, A (q) epsilon R3×(n+5)Is a constrained correlation matrix, J ∈ R(n+2)×(n+2)For a diagonal matrix of motor inertia, λ ∈ R3×1For Lagrange multipliers, τ represents the input moment, RnxnA matrix of real numbers representing n x n.
The non-complete constraint and the complete constraint of the mobile mechanical arm are
Figure BDA00031871795700000610
Wherein
Figure BDA00031871795700000611
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 rwIs 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
Figure BDA0003187179570000071
Wherein c is rw/2b, therefore
Figure BDA0003187179570000072
Can be expressed as
Figure BDA0003187179570000073
The time is derived to obtain
Figure BDA0003187179570000074
Substituting into the kinetic equation and multiplying it by ST(q) obtaining a kinetic model after transformation of
Figure BDA0003187179570000075
Figure BDA0003187179570000076
In step S3, the moving robot arm is controlled by the adaptive neural network control method based on the integral lyapunov function, and the process returns to (1) to continue the execution. Due to model uncertainty and external interference, it is difficult to obtain m (q),
Figure BDA0003187179570000077
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
Figure BDA0003187179570000078
M(q)＝ST(q)M(q)S(q),
Figure BDA0003187179570000079
G(q)＝ST(q)G(q),B＝ST(q)B,U＝Bτ
Then the system
Figure BDA00031871795700000710
Figure BDA00031871795700000711
Can be rewritten as
Figure BDA00031871795700000712
Let Md(q)∈R(n+2)×(n+2)Is Mdii(q)Diagonal matrix of not equal to 0, Mdii(q)Represents Md(q)(i,i)。
Define m ═ n +2, and divide m (q) into the following two parts:
M(q)＝Md(q)+ΔM
wherein ΔMThe matrix is unknown, available
Figure BDA0003187179570000081
Wherein
Figure BDA0003187179570000082
Figure BDA0003187179570000083
Considering D as a disturbance in the system, an observer was designed to estimate D.
Considering the kinetic model, there are
Figure BDA0003187179570000084
Due to the uncertainty of the model, f (x) cannot be obtained. As a class of multi-layer feedforward networks, any continuous function can be approximated as a radial basis function neural network. Therefore, F (x) is approximated as
F(x)＝W*TR(x)+∈
Where x is the neural network input vector,
Figure BDA0003187179570000085
is the optimal weight vector of the weight vector,
Figure BDA0003187179570000086
given a bounded approximation error with node number l > 1, R (x) ═ R1(x),…,Rl(x)]Is the radial basis function.
Assuming unknown external disturbance D and its derivatives
Figure BDA0003187179570000087
Are bounded. Namely that there is a satisfaction of the inequality
Figure BDA0003187179570000088
Figure BDA0003187179570000089
Is unknown constant dMAnd ρ. The filtered tracking error E and error E are defined as follows, E ═ r1，…，rM]T：
Figure BDA00031871795700000810
e＝y-yd
Can be obtained where Λ is a positive definite symmetric matrix, ydIs a reference track. Obtaining a series of linear differential equations when riWhen equal to 0, its solution ei(i ═ 1, …, m) converges to zero.
So that there are
Figure BDA00031871795700000811
Wherein
E＝[r1，…,rM]T,v＝[v1,…,νm]T；
Figure BDA00031871795700000812
Input U is to ensure E → 0. Consider the following integral Lyapunov function candidate
Figure BDA00031871795700000813
Wherein
Figure BDA0003187179570000091
And the matrix alpha is formed by Rm×mIs a positive definite diagonal matrix.
BαIs represented by Bα＝Md(q)α＝diag[Md(q)(i，i)*α(i，i)]m*m，BαiiIs represented by Bα(i，i)，riIs represented by ri＝Ei(E＝[r1,…,rM]T)，viTo represent
Figure BDA0003187179570000092
αiiRepresents α (i, i).
Setting alpha11＝…＝αmm,
Figure BDA0003187179570000093
Where xi is ═ xi1，ξ2,…,ξm]T∈Rm,ξi＝λieiI-1, 2, …, m. by means of a design parameter λi(i ═ 1,2, …, m0, can be such that e → 0 is true when v → 0,
Figure BDA0003187179570000094
is a scalar independent of E, x and v, selecting the appropriate Md(q) and alpha to ensure Mdii(q)αiiIs greater than 0. The integral lyapunov function can be re-expressed as
Figure BDA0003187179570000095
Based on BαAnd
Figure BDA0003187179570000096
the fact of symmetry, taking the time derivative of the Lyapunov function, has
Figure BDA0003187179570000097
And is
Figure BDA0003187179570000098
Figure BDA0003187179570000099
Figure BDA00031871795700000910
Is provided with
Figure BDA00031871795700000911
Can obtain
Figure BDA00031871795700000912
Figure BDA00031871795700000913
Is provided with
Figure BDA00031871795700000914
Wherein
Figure BDA00031871795700000915
Is a scalar, independent of v, because
Figure BDA00031871795700000916
Is provided with
Figure BDA00031871795700000917
Figure BDA00031871795700000918
And
Figure BDA00031871795700000919
then is provided with
Figure BDA0003187179570000101
Due to Md(q), α and Md(q) α is symmetrical, having
Figure BDA0003187179570000102
Wherein
Figure BDA0003187179570000103
Definition of
Figure BDA0003187179570000104
Is provided with
Figure BDA0003187179570000105
Figure BDA0003187179570000106
To achieve the estimation of D, an auxiliary variable z is designed:
z＝D-Kdx2
wherein
Figure BDA0003187179570000107
Is the matrix to be designed.
Differentiating z with time t to obtain
Figure BDA0003187179570000108
To achieve the estimation of the system disturbance D, there is first an estimation of z, an estimation variable
Figure BDA0003187179570000109
The update rule of (D) is described as follows, and the estimated value of D is defined as
Figure BDA00031871795700001010
Figure BDA00031871795700001011
Estimate of disturbance D
Figure BDA00031871795700001012
Can be obtained from the following formula
Figure BDA00031871795700001013
Definition of
Figure BDA00031871795700001014
An estimation error expressed as a perturbation, having
Figure BDA00031871795700001015
Differentiated with respect to time t, then combined, one can obtain
Figure BDA00031871795700001016
Based on the above analysis, the adaptive control input is designed as follows:
Figure BDA0003187179570000111
Figure BDA0003187179570000112
wherein
Figure BDA0003187179570000113
Is W*Estimation of, K1∈Rm×mIs a positive definite diagonal matrix and,
Figure BDA0003187179570000114
is the neural network update law, omega is the diagonal element omegai(i is 1,2, …, m) and a positive angle constant matrix,
Figure BDA0003187179570000115
is a constant number of times that the number of the first,
Figure BDA0003187179570000116
is W*Is estimated.
The following Lyapunov function candidates are designed as
Figure BDA0003187179570000117
Wherein
Figure BDA0003187179570000118
By differentiation with time t, can be obtained
Figure BDA0003187179570000119
Substituting into the controller to obtain
Figure BDA00031871795700001110
Taking into account the fact that
Figure BDA00031871795700001111
Figure BDA00031871795700001112
Figure BDA00031871795700001113
Is provided with
Figure BDA00031871795700001114
Because of the fact that
Figure BDA00031871795700001115
Substituting into the controller input formula, have
Figure BDA0003187179570000121
Wherein
Figure BDA0003187179570000122
Figure BDA0003187179570000123
Wherein λmax(. and λ)min(. cndot.) are the maximum and minimum eigenvalues of (. cndot.) respectively. Controller parameter K1And KdMust satisfy (K)1-Im×m) Not less than 0 and
Figure BDA0003187179570000124
two sides of the same time eκtMultiplication and integration over time t can result in
Figure BDA0003187179570000125
From the above inequality, V2Finally, bounded when t → ∞, which means that E,
Figure BDA0003187179570000126
and
Figure BDA0003187179570000127
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 invention can be regarded as a hardware component, and the devices, modules and units included in the system for realizing various functions can also be regarded as structures in the hardware component; means, modules, units for performing the various functions may also be regarded as structures within both software modules and hardware components for performing the method.
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, and are only for convenience in describing the present application and simplifying the description, but do not indicate or imply that the referred device or element must have a specific orientation, be constructed in a specific orientation, and be operated, and thus, should not 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 an integral dynamic model of the cart mobile mechanical arm with incomplete constraint and complete constraint;
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 step S1, the obstacle function β (r) and the target point position r are determined according to the mobile platform position rdWith a sufficiently large constant value k > 0, a navigation function is established
Figure FDA0003187179560000011
Figure FDA0003187179560000012
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.
3. The SLAM-based mobile robotic arm cart mission planning and control method of claim 1, wherein: the barrier function β (r) is:
Figure FDA0003187179560000013
wherein beta isiAnd (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 },
Figure FDA0003187179560000014
where r ═ (x, y)TIs the position of the mobile platform, qi＝(xi0，yi0)TIs the coordinate position of the center of a circle of a circular obstacle, rhoiIs 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 vision sensor acquires a real-time image of the surrounding environment, then uses the Yolo to predict the frame and the class probability of the obstacle in the image, uses ORB-SLAM2 to extract ORB features in the image, encodes the environment in sparse representation, simultaneously and quickly extracts and matches, generates a sparse feature point map, and combines with the 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, there will be 2+ n flexible joints, and the generalized coordinate of the mobile platform is qs＝[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
Figure FDA0003187179560000021
Figure FDA0003187179560000022
Wherein
Figure FDA0003187179560000023
For uniform generalized coordinates of the moving arm, qm＝[θr，θl，θ1，…，θn]Angular displacement of a joint, theta, in a generalized coordinate relation to the wheel and arm jointsm＝[θrm，θlm，θ1m，…，θnm]TIs 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,
Figure FDA0003187179560000024
is a matrix of nonlinear terms related to the Coriolis/centrifugal force vector, G (q) e R(n+5)×1Is 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)×1Is the input of the mobile platform and the robot joint, A (q) epsilon R3×(n+5)Is a constrained correlation matrix, J ∈ R(n+2)×(n+2)For a diagonal matrix of motor inertia, λ ∈ R3×1Is a lagrange multiplier.
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
Figure FDA0003187179560000025
Wherein
Figure FDA0003187179560000026
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
Figure FDA0003187179560000031
Where c is r/2b, and thus
Figure FDA0003187179560000032
Can be expressed as
Figure FDA0003187179560000033
The time is derived to obtain
Figure FDA0003187179560000034
Substituting into the kinetic equation and multiplying it by ST(q) obtaining a kinetic model after transformation of
Figure FDA0003187179560000035
Figure FDA0003187179560000036
10. The SLAM-based mobile robotic arm cart mission planning and control method of claim 1, wherein: in step S3, the adaptive neural network is designed to solve the uncertainty of the dynamic model of the mobile robot arm and the external interference.
CN202110865506.9A 2021-07-29 2021-07-29 SLAM-based mobile mechanical arm cart task planning and control method Active CN113547501B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110865506.9A CN113547501B (en) 2021-07-29 2021-07-29 SLAM-based mobile mechanical arm cart task planning and control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110865506.9A CN113547501B (en) 2021-07-29 2021-07-29 SLAM-based mobile mechanical arm cart task planning and control method

Publications (2)

Publication Number Publication Date
CN113547501A true CN113547501A (en) 2021-10-26
CN113547501B CN113547501B (en) 2022-10-28

Family

ID=78133351

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110865506.9A Active CN113547501B (en) 2021-07-29 2021-07-29 SLAM-based mobile mechanical arm cart task planning and control method

Country Status (1)

Country Link
CN (1) CN113547501B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117506939A (en) * 2024-01-04 2024-02-06 华中科技大学 Mechanical arm motion planning method based on state triggering self-adaptive gradient neural network

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5739657A (en) * 1995-05-10 1998-04-14 Fujitsu Limited Apparatus for controlling motion of normal wheeled omni-directional vehicle and method thereof
CN108520554A (en) * 2018-04-12 2018-09-11 无锡信捷电气股份有限公司 A kind of binocular three-dimensional based on ORB-SLAM2 is dense to build drawing method
CN110378997A (en) * 2019-06-04 2019-10-25 广东工业大学 A kind of dynamic scene based on ORB-SLAM2 builds figure and localization method
US10827322B1 (en) * 2019-07-05 2020-11-03 Charter Communications Operating, Llc Augmented reality signal visualization
CN112288857A (en) * 2020-10-30 2021-01-29 西安工程大学 Robot semantic map object recognition method based on deep learning

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5739657A (en) * 1995-05-10 1998-04-14 Fujitsu Limited Apparatus for controlling motion of normal wheeled omni-directional vehicle and method thereof
CN108520554A (en) * 2018-04-12 2018-09-11 无锡信捷电气股份有限公司 A kind of binocular three-dimensional based on ORB-SLAM2 is dense to build drawing method
CN110378997A (en) * 2019-06-04 2019-10-25 广东工业大学 A kind of dynamic scene based on ORB-SLAM2 builds figure and localization method
US10827322B1 (en) * 2019-07-05 2020-11-03 Charter Communications Operating, Llc Augmented reality signal visualization
CN112288857A (en) * 2020-10-30 2021-01-29 西安工程大学 Robot semantic map object recognition method based on deep learning

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117506939A (en) * 2024-01-04 2024-02-06 华中科技大学 Mechanical arm motion planning method based on state triggering self-adaptive gradient neural network
CN117506939B (en) * 2024-01-04 2024-03-29 华中科技大学 Mechanical arm motion planning method based on state triggering self-adaptive gradient neural network

Also Published As

Publication number Publication date
CN113547501B (en) 2022-10-28

Similar Documents

Publication Publication Date Title
Kheddar et al. Humanoid robots in aircraft manufacturing: The airbus use cases
Zhang et al. Visual motion planning for mobile robots
Wang et al. A hybrid visual servo controller for robust grasping by wheeled mobile robots
Slotine et al. On the adaptive control of robot manipulators
Bobrow et al. Time-optimal control of robotic manipulators along specified paths
Andaluz et al. Adaptive unified motion control of mobile manipulators
Souzanchi-K et al. Robust impedance control of uncertain mobile manipulators using time-delay compensation
Xu et al. Motion planning of manipulators for simultaneous obstacle avoidance and target tracking: An RNN approach with guaranteed performance
Wang et al. A modified image-based visual servo controller with hybrid camera configuration for robust robotic grasping
Maravall et al. Vision-based anticipatory controller for the autonomous navigation of an UAV using artificial neural networks
Andaluz et al. Visual control with adaptive dynamical compensation for 3D target tracking by mobile manipulators
Kim et al. Localization of a mobile robot using images of a moving target
Fang et al. Type design and behavior control for six legged robots
Spong et al. Control in robotics
Vladareanu et al. The navigation of mobile robots in non-stationary and non-structured environments
Pajak et al. Point-to-point collision-free trajectory planning for mobile manipulators
Lee et al. Reinforcement learning and neural network-based artificial intelligence control algorithm for self-balancing quadruped robot
Xu et al. Uncalibrated visual servoing of mobile manipulators with an eye-to-hand camera
Farooq et al. Fuzzy logic based path tracking controller for wheeled mobile robots
CN113547501B (en) SLAM-based mobile mechanical arm cart task planning and control method
Nagarajan et al. Integrated motion planning and control for graceful balancing mobile robots
Apurin et al. Comparison of ROS Local Planners for a Holonomic Robot in Gazebo Simulator
Ismail et al. Collision-free and dynamically feasible trajectory of a hybrid cable–serial robot with two passive links
Li et al. Kinodynamics-based pose optimization for humanoid loco-manipulation
Tan et al. Neural-network-based control of wheeled mobile manipulators with unknown kinematic models

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant