CN113843793A - Mobile redundant mechanical arm model prediction control method with obstacle avoidance function - Google Patents

Mobile redundant mechanical arm model prediction control method with obstacle avoidance function Download PDF

Info

Publication number
CN113843793A
CN113843793A CN202111117973.XA CN202111117973A CN113843793A CN 113843793 A CN113843793 A CN 113843793A CN 202111117973 A CN202111117973 A CN 202111117973A CN 113843793 A CN113843793 A CN 113843793A
Authority
CN
China
Prior art keywords
redundant
mobile
mechanical arm
model
obstacle
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
CN202111117973.XA
Other languages
Chinese (zh)
Other versions
CN113843793B (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.)
Lanzhou University
Original Assignee
Lanzhou University
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 Lanzhou University filed Critical Lanzhou University
Priority to CN202111117973.XA priority Critical patent/CN113843793B/en
Publication of CN113843793A publication Critical patent/CN113843793A/en
Application granted granted Critical
Publication of CN113843793B publication Critical patent/CN113843793B/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
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

The invention provides a mobile redundant mechanical arm model prediction control method with an obstacle avoidance function, which belongs to the technical field of mechanical arm control and comprises the following steps: s1: aiming at the mobile redundant mechanical arm, constructing a kinematic model of the mobile redundant mechanical arm; s2: according to the kinematic model of the mobile redundant manipulator, a state space model of the mobile redundant manipulator is constructed, and the future state and the future output of the system are predicted; s3: aiming at the problem of tracking the track of the mobile redundant mechanical arm, optimizing performance indexes, and designing system state constraint and input constraint according to the joint limit of the mechanical arm; s4: aiming at the problem of safe operation of the mobile redundant mechanical arm in an environment with obstacles, an obstacle avoidance scheme is designed; s5: and designing a model prediction control method of the mobile redundant mechanical arm, and completing the trajectory tracking control of the mobile redundant mechanical arm under the driving of the model prediction control method. The invention can realize the track tracking control of the mobile redundant mechanical arm in the environment with the obstacle and ensure the accuracy and the safety of the control of the mobile redundant mechanical arm.

Description

Mobile redundant mechanical arm model prediction control method with obstacle avoidance function
Technical Field
The invention relates to the technical field of mechanical arm control, in particular to a mobile redundant mechanical arm model prediction control method with an obstacle avoidance function.
Background
Redundant robotic arms with more degrees of freedom than are required are powerful tools that liberate productivity and have been widely used in the industry. For mobile redundant robotic arms, inverse kinematics studies, i.e., acquiring joint information based on the position of the end effector, are central to their motion control. Motion control based on quadratic programming is a mainstream method, but when processing joint constraints of different levels, additional parameters need to be introduced and certain margin needs to be reserved, which reduces the feasible range of joint angles. From the viewpoint of handling multiple constraints, model predictive control is applicable to motion control of a mobile redundant robotic arm. The optimization problem in model predictive control may contain a number of constraints, which may be expressed in their native form. That is to say, the joint constraint of the mobile redundant mechanical arm acts on the model predictive control scheme in the original form, and the original feasible region of the joint angle is ensured.
In order to ensure the safe operation of the mobile redundant mechanical arm, attention should be paid to avoid obstacles in a working space of the mobile redundant mechanical arm besides ensuring joint constraint. Currently, some obstacle avoidance methods suitable for redundant robotic arms have emerged. Chinese patent CN108772835A published in 2018, 11, 9, discloses an obstacle and physical limit avoidance method. Chinese patent CN112605996A published in 2021, 4/6/month discloses a model-free collision avoidance control method for redundant robot arms. It should be noted that the existing obstacle avoidance method has the defects of small joint angle feasible region, poor track tracking performance and the like, and still has a large improvement space.
Against such a background, it is urgently needed to provide a model predictive control method for a mobile redundant manipulator with an obstacle avoidance function, so as to improve the accuracy and safety of control of the mobile redundant manipulator.
Disclosure of Invention
Aiming at the defects of small joint angle feasible region, poor track tracking performance and the like in the existing mechanical arm control and obstacle avoidance technology, the invention provides a mobile redundant mechanical arm model prediction control method with an obstacle avoidance function, so as to improve the accuracy and safety of mobile redundant mechanical arm control.
The technical scheme adopted by the invention is as follows:
a mobile redundant mechanical arm model prediction control method with an obstacle avoidance function can realize the track tracking control of a mobile redundant mechanical arm in an obstacle environment, and comprises the following steps:
s1: aiming at the mobile redundant mechanical arm, constructing a kinematic model of the mobile redundant mechanical arm;
s2: according to the kinematic model of the mobile redundant manipulator, a state space model of the mobile redundant manipulator is constructed, and the future state and the future output of the system are predicted;
s3: aiming at the problem of tracking the track of the mobile redundant mechanical arm, optimizing performance indexes, and designing system state constraint and input constraint according to the joint limit of the mechanical arm;
s4: aiming at the problem of safe operation of the mobile redundant mechanical arm in an environment with obstacles, an obstacle avoidance scheme is designed;
s5: and designing a model prediction control method of the mobile redundant mechanical arm, and completing the trajectory tracking control of the mobile redundant mechanical arm under the driving of the model prediction control method.
Further, the kinematic model of the mobile redundant robot arm described in step S1 is established as
Figure BDA0003271433260000021
Wherein the content of the first and second substances,
Figure BDA0003271433260000022
to move the position of the end effector of the redundant robotic arm in a world coordinate system; w is the rotation angle of the mobile platform;
Figure BDA0003271433260000031
is the position of the end effector of the redundant robotic arm in its base coordinate system;
Figure BDA0003271433260000032
the joint angle of the redundant mechanical arm is 6 degrees of freedom;
Figure BDA0003271433260000033
the position of the connecting point of the redundant mechanical arm and the mobile platform in a world coordinate system;
Figure BDA0003271433260000034
for combined angles of rotation of the drive wheels of the mobile platform and of the joint angles of the redundant manipulator, wherein
Figure BDA0003271433260000035
Representing the rotation angle of two driving wheels of the mobile platform;
Figure BDA0003271433260000036
a mapping function for moving the joint angle of the redundant manipulator to the position of the end effector;
Figure BDA0003271433260000037
to move the velocity of the end effector of a redundant robotic arm in a world coordinate system, is
Figure BDA0003271433260000038
A derivative with respect to time t;
Figure BDA0003271433260000039
a Jacobian matrix for moving the redundant manipulator;
Figure BDA00032714332600000310
the derivative of q with respect to time t is the angular velocity at which the redundant robotic arm is moved.
Further, the step S2 is specifically
S201: constructing a state space model of the mobile redundant manipulator at a moment k according to a kinematic model of the mobile redundant manipulator, wherein k represents an update index of the current moment t being k delta seconds, and delta is a sampling interval;
s202: predicting system states in N future time domains according to a state space model of the mobile redundant mechanical arm at the k moment, wherein
Figure BDA00032714332600000311
Representing a prediction time domain;
s203: and predicting system output in N future time domains according to the state space model of the mobile redundant mechanical arm at the time k.
Further, the state space model of the mobile redundant manipulator at the time k in the step S201 is established as
Figure BDA00032714332600000312
Wherein the content of the first and second substances,
Figure BDA00032714332600000313
is a state variable of the system and is,
Figure BDA00032714332600000314
and is
Figure BDA00032714332600000315
Representing the joint angle of the movable redundant mechanical arm at the moment k; x is the number ofjAnd xj+1Respectively representing system state variables of two moments before and after;
Figure BDA0003271433260000041
is an input variable of the system;
Figure BDA0003271433260000042
is an output variable of the system and is,
Figure BDA0003271433260000043
and is
Figure BDA0003271433260000044
Indicating the position of the moving redundant robotic arm end effector at time k;
Figure BDA0003271433260000045
to move the jacobian matrix of the redundant manipulator at time k.
Further, the system status in the future N time domains is predicted as described in step S202
Figure BDA0003271433260000046
Wherein the content of the first and second substances,
Figure BDA0003271433260000047
Figure BDA0003271433260000048
Figure BDA0003271433260000049
for the system state at the future time k + i, i ═ 1, 2, …, N;
Figure BDA00032714332600000410
for future system inputs at time k + i, i is 0, 1, …, Nu-1, and
Figure BDA00032714332600000411
represents a control time domain; and I is an identity matrix.
Further, the system output in the future N time domains of step S203 is predicted as
Figure BDA00032714332600000412
Wherein the content of the first and second substances,
Figure BDA00032714332600000413
Figure BDA0003271433260000051
Figure BDA0003271433260000052
for the system output at time k + i in the future, i is 1, 2, …, N.
Further, the optimized performance index for the trajectory tracking control of the mobile redundant manipulator described in step S3 is designed to be
Figure BDA0003271433260000053
Wherein the content of the first and second substances,
Figure BDA0003271433260000054
and
Figure BDA0003271433260000055
a weight matrix which is positively definite and symmetrical;
Figure BDA0003271433260000056
and
Figure BDA0003271433260000057
the number of euclidean norms is represented,
Figure BDA0003271433260000058
i is 1, 2, …, N for the desired trajectory of the moving redundant robot arm end effector at time k + i in the future.
Further, the system state constraint and the input constraint of step S3 are designed as
Figure BDA0003271433260000059
And
Figure BDA00032714332600000510
wherein the content of the first and second substances,
Figure BDA00032714332600000511
Figure BDA00032714332600000512
and q is+And q is-The upper limit and the lower limit of the joint angle of the movable redundant mechanical arm;
Figure BDA00032714332600000513
and
Figure BDA00032714332600000514
the upper and lower limits of the joint angular velocity for moving the redundant manipulator.
Further, the step S4 is specifically
S401: a safe distance between the obstacle and the moving redundant robotic arm is determined,wherein the internal safety threshold is d1The distance of collision between the movable redundant mechanical arm and the barrier is used; an external safety threshold of d2The distance for moving the redundant mechanical arm to avoid the obstacle is generally taken2≥2d1；
S402: determining a critical point on the moving redundant manipulator whose distance from the obstacle is less than an outer safety threshold d2；
S403: and designing an obstacle avoidance method, and endowing a critical point with a proper escape speed to enable the critical point to be far away from the obstacle.
Furthermore, the obstacle avoidance method of step S403 is designed to
Figure BDA0003271433260000061
Wherein the content of the first and second substances,
Figure BDA0003271433260000062
s＝[xC-xO，yC-yO，zC-zO]T；(·)Trepresenting a transpose operation on a matrix or vector; point C is a critical point on the arm with coordinates (x)C，yC，zC) The point O is an obstacle with coordinates (x)O，yO，zO)；GCA Jacobian matrix for the critical point C;
Figure BDA0003271433260000063
d represents the distance between the critical point and the obstacle.
In particular, the obstacle avoidance method described in step S403 is superior to the conventional obstacle avoidance methods (CN112605996A, CN108772835A), and mainly includes the following two aspects: 1) the obstacle avoidance method in step S403 adopts a vector dot product form to design the escape speed of the critical point. Compared with the traditional obstacle avoidance method, the directional range of the escape speed is greatly expanded, the feasible region of the mechanical arm is also enlarged, and the mechanical arm can avoid the obstacle as much as possible without collapsing. 2) The obstacle avoidance method in step S403 abandons the use of a symbolic function in the conventional obstacle avoidance method, and further improves the tracking accuracy of the end effector of the mechanical arm in the obstacle avoidance process.
Further, the step S5 is specifically
S501: the model predictive control scheme for the track tracking control of the mobile redundant manipulator with the obstacle avoidance function is constructed by
And (3) minimizing:
Figure BDA0003271433260000071
and (3) model constraint:
Figure BDA0003271433260000072
Figure BDA0003271433260000073
Figure BDA0003271433260000074
wherein the content of the first and second substances,
Figure BDA0003271433260000075
s502: converting a model predictive control scheme of the mobile redundant mechanical arm into a standard quadratic programming form:
and (3) minimizing:
Figure BDA0003271433260000076
and (3) model constraint: sz is less than or equal to f,
wherein the content of the first and second substances,
Figure BDA0003271433260000077
Figure BDA0003271433260000078
m＝16N+16Nu+1，n＝8Nu；
s503: and converting the model predictive control scheme of the mobile redundant manipulator into a nonlinear equation by utilizing a Lagrange multiplier method and a nonlinear complementary problem function, and further designing a solver of the model predictive control scheme of the mobile redundant manipulator based on the equation.
Further, the non-linear equation in step S503 is Kχ+ ψ is 0, wherein,
Figure BDA0003271433260000081
Figure BDA0003271433260000082
ρ∈(0，1)，
Figure BDA0003271433260000083
in order to be a lagrange multiplier,
Figure BDA0003271433260000084
a+＝max{0，a}，
Figure BDA0003271433260000085
Figure BDA00032714332600000813
the product of the hadamard is represented,
Figure BDA0003271433260000086
the solver of the model predictive control scheme of the mobile redundant mechanical arm is
Figure BDA00032714332600000814
Wherein, γ ═ α τ,
Figure BDA0003271433260000087
for the design parameters, τ is the step size,
Figure BDA0003271433260000088
Figure BDA0003271433260000089
which represents a division of the hadamard,
Figure BDA00032714332600000810
Figure BDA00032714332600000811
further, for the input variables calculated by the solver
Figure BDA00032714332600000812
And taking the first input, namely the input u (k) at the moment k, and acting the first input on the system for moving the redundant mechanical arm to complete the trajectory tracking control of the movable redundant mechanical arm.
Drawings
For further illustration of the objects and technical solutions of the present invention, the present invention is illustrated by the following figures:
FIG. 1 is a flow chart of a mobile redundant manipulator model predictive control method with obstacle avoidance;
FIG. 2(a) is a front view of a mobile redundant robotic arm;
FIG. 2(b) is a side view of a mobile redundant robotic arm;
fig. 3(a) is a three-dimensional obstacle avoidance result diagram of a first simulation experiment under the condition that an obstacle avoidance method is not used;
fig. 3(b) is a graph of the distance between a critical point and an obstacle in a first simulation experiment without using an obstacle avoidance method;
FIG. 4(a) is a two-dimensional plan view of an actual trajectory and a desired trajectory of a moving redundant robotic arm end effector of simulation experiment one using the obstacle avoidance method of the present invention;
fig. 4(b) is a three-dimensional obstacle avoidance result diagram of a first simulation experiment under the condition of using the obstacle avoidance method of the present invention;
fig. 4(c) is a graph of the distance between a critical point and an obstacle in a first simulation experiment under the condition of using the obstacle avoidance method of the invention;
fig. 4(d) is a tracking error map in X, Y, Z three directions for simulation experiment one using the obstacle avoidance method of the present invention;
fig. 5(a) is a three-dimensional obstacle avoidance result diagram of a second simulation experiment under the condition of using the obstacle avoidance method of the present invention;
fig. 5(b) shows X, Y, Z tracking errors in three directions in a second simulation experiment under the condition of using the obstacle avoidance method of the present invention;
fig. 5(c) is a distance graph between a critical point and an obstacle of a second simulation experiment under the condition of using the obstacle avoidance method of the invention;
fig. 5(d) is a distance graph between a critical point and an obstacle of a second simulation experiment under the condition that an obstacle avoidance method is not used;
fig. 6(a) is a graph of the distance between a critical point and an obstacle in a first simulation experiment without using an obstacle avoidance method;
fig. 6(b) is a tracking error map in three directions of X, Y, Z for simulation experiment one without using the obstacle avoidance method;
fig. 6(c) is a graph of the distance between the critical point and the obstacle in the first simulation experiment under the condition of using the obstacle avoidance method of the national patent CN 112605996A;
fig. 6(d) is a tracking error map in three directions X, Y, Z of a first simulation experiment under the condition of using the obstacle avoidance method of the national patent CN 112605996A;
fig. 6(e) is a diagram of the distance between the critical point and the obstacle in the first simulation experiment under the condition of using the obstacle avoidance method of the national patent CN 108772835A;
fig. 6(f) is a tracking error map in three directions X, Y, Z of a first simulation experiment under the condition of using the obstacle avoidance method of the national patent CN 108772835A;
fig. 6(g) is a graph of the distance between a critical point and an obstacle in a first simulation experiment under the condition of using the obstacle avoidance method of the present invention;
fig. 6(h) is a tracking error map in X, Y, Z three directions for simulation experiment one using the obstacle avoidance method of the present invention.
Detailed Description
In order to make the purpose and technical solution of the present invention more clearly understood, the present invention will be described in detail with reference to the accompanying drawings and examples.
This embodiment is based on a mobile platform carrying a 6-degree-of-freedom redundant robot arm. Wherein, the initial joint angle of the redundant mechanical arm is set as [ pi/12, pi/2, pi/12, pi/3, -pi/12, pi/6]TRadian, initial angle of left and right driving wheels of mobile platform is set to 0, 0]TRadians, i.e. q (0) ═ 0, 0, pi/12, pi/2, pi/12, pi/3, -pi/12, pi/6]TRadian with upper and lower limits of combined angle set to-q-＝q+＝[+∞，+∞，π，π，π，π，π，π]TRadian combined with upper and lower limits of angular velocity set to
Figure BDA0003271433260000111
Radian/second; the sampling interval δ is set to 0.1 second; prediction time domain N and control time domain NuAre all set to be 3; the weight matrix Q is set to 105I; the weight matrix V is set to 10-4I; internal safety threshold d1Set to 0.1 meter; external safety threshold d2Set to 0.2 meters; the remaining relevant parameters are set as follows: ρ is 0.95, τ is 0.001, α is 300.
Considering the problems of small joint angle feasible region, poor track tracking performance and the like in the existing mechanical arm control and obstacle avoidance technology, the embodiment provides a mobile redundant mechanical arm model prediction control method with an obstacle avoidance function, and with reference to fig. 1, the method comprises the following steps:
the method comprises the following steps: according to the structure of the mobile redundant manipulator, as shown in FIG. 2, the kinematic model is established as
Figure BDA0003271433260000112
Wherein the content of the first and second substances,
Figure BDA0003271433260000113
to move the position of the end effector of the redundant robotic arm in a world coordinate system; omega is the rotation angle of the mobile platform;
Figure BDA0003271433260000114
is the position of the end effector of the redundant robotic arm in its base coordinate system;
Figure BDA0003271433260000115
the joint angle of the redundant mechanical arm is 6 degrees of freedom;
Figure BDA0003271433260000116
the position of the connecting point of the redundant mechanical arm and the mobile platform in a world coordinate system;
Figure BDA0003271433260000117
for combined angles of rotation of the drive wheels of the mobile platform and of the joint angles of the redundant manipulator, wherein
Figure BDA0003271433260000118
Representing the rotation angle of two driving wheels of the mobile platform;
Figure BDA0003271433260000119
a mapping function for moving the joint angle of the redundant manipulator to the position of the end effector;
Figure BDA00032714332600001110
to move the velocity of the end effector of a redundant robotic arm in a world coordinate system, is
Figure BDA00032714332600001111
A derivative with respect to time t;
Figure BDA00032714332600001112
a Jacobian matrix for moving the redundant manipulator;
Figure BDA00032714332600001113
the derivative of q with respect to time t is the angular velocity at which the redundant robotic arm is moved.
Step two: and constructing a state space model of the mobile redundant manipulator according to the kinematic model of the mobile redundant manipulator, and predicting the future state and the future output of the system.
In particular to
(201) According to the kinematic model of the mobile redundant manipulator, a state space model of the mobile redundant manipulator at the moment k is constructed into
Figure BDA0003271433260000121
Wherein the content of the first and second substances,
Figure BDA0003271433260000122
is a state variable of the system and is,
Figure BDA0003271433260000123
and is
Figure BDA0003271433260000124
Representing the joint angle of the movable redundant mechanical arm at the moment k; x is the number ofjAnd xj+1Respectively representing system state variables of two moments before and after;
Figure BDA0003271433260000125
is an input variable of the system;
Figure BDA0003271433260000126
is an output variable of the system and is,
Figure BDA0003271433260000127
and is
Figure BDA0003271433260000128
Indicating the position of the moving redundant robotic arm end effector at time k;
Figure BDA0003271433260000129
a Jacobian matrix of the mobile redundant mechanical arm at the moment k is obtained;
(202) predicting the system state in the future 3 time domains according to the state space model of the mobile redundant mechanical arm at the k time
Figure BDA00032714332600001210
Wherein the content of the first and second substances,
Figure BDA00032714332600001211
Figure BDA00032714332600001212
Figure BDA00032714332600001213
for the system state at the future time k + i, i is 1, 2, 3;
Figure BDA00032714332600001214
for system input at a future time k + i, i is 0, 1, 2; i is an identity matrix;
(203) predicting system output in 3 future time domains according to a state space model of the mobile redundant manipulator at the k moment
Figure BDA0003271433260000131
Wherein the content of the first and second substances,
Figure BDA0003271433260000132
Figure BDA0003271433260000133
Figure BDA0003271433260000134
for the system output at a future time k + i, i is 1, 2, 3.
Step three: aiming at the problem of tracking the track of the mobile redundant mechanical arm, the performance index is designed and optimized, and system state constraint and input constraint are designed according to the joint limit of the mechanical arm.
In particular to
(301) Aiming at the problem of track tracking of the mobile redundant mechanical arm, the performance index is designed and optimized to
Figure BDA0003271433260000135
Wherein the content of the first and second substances,
Figure BDA0003271433260000136
and
Figure BDA0003271433260000137
a weight matrix which is positively definite and symmetrical;
Figure BDA0003271433260000138
and
Figure BDA0003271433260000139
the number of euclidean norms is represented,
Figure BDA00032714332600001310
setting i to be 1, 2, 3 for the expected track of the end effector of the mobile redundant mechanical arm at the future k + i moment;
(302) based on the joint limits of the robot arm, system state constraints and input constraints are designed as
Figure BDA00032714332600001311
And
Figure BDA00032714332600001312
wherein the content of the first and second substances,
Figure BDA00032714332600001313
Figure BDA0003271433260000141
and q is+And q is-In order to move the upper and lower limits of the joint angle of the redundant manipulator,
Figure BDA0003271433260000142
and
Figure BDA0003271433260000143
the upper and lower limits of the joint angular velocity for moving the redundant manipulator.
Step four: aiming at the problem of safe operation of the mobile redundant mechanical arm in an obstacle environment, an obstacle avoidance scheme is designed.
In particular to
(401) Determining a safe distance between the obstacle and the moving redundant manipulator, wherein an inner safe threshold d10.1 m, outer safety threshold d20.2 m;
(402) determining a critical point on the moving redundant manipulator whose distance from the obstacle is less than an outer safety threshold d2；
(403) And designing an obstacle avoidance method, and endowing a critical point with a proper escape speed to enable the critical point to be far away from the obstacle.
Further, the obstacle avoidance method in step (403) is designed to be
Figure BDA0003271433260000144
Wherein the content of the first and second substances,
Figure BDA0003271433260000145
s＝[xC-xO，yC-yO，zC-zO]T；(·)Trepresenting a transpose operation on a matrix or vector; point C is a critical point on the arm with coordinates (x)C，yC，zC) (ii) a The point O is an obstacle with coordinates of (x)O，yO，zO)；GCA Jacobian matrix for the critical point C;
Figure BDA0003271433260000146
d represents the distance between the critical point and the obstacle.
Step five: and designing a model prediction control method of the mobile redundant mechanical arm, and completing the trajectory tracking control of the mobile redundant mechanical arm under the driving of the model prediction control method.
In particular to
(501) The model predictive control scheme for the track tracking control of the mobile redundant manipulator with the obstacle avoidance function is constructed by
And (3) minimizing:
Figure BDA0003271433260000151
and (3) model constraint:
Figure BDA0003271433260000152
Figure BDA0003271433260000153
Figure BDA0003271433260000154
wherein the content of the first and second substances,
Figure BDA0003271433260000155
(502) converting a model predictive control scheme of the mobile redundant mechanical arm into a standard quadratic programming form:
and (3) minimizing:
Figure BDA0003271433260000156
and (3) model constraint: sz is less than or equal to f,
wherein the content of the first and second substances,
Figure BDA0003271433260000157
Figure BDA0003271433260000158
(503) and converting the model predictive control scheme of the mobile redundant manipulator into a nonlinear equation by utilizing a Lagrange multiplier method and a nonlinear complementary problem function, and further designing a solver of the model predictive control scheme of the mobile redundant manipulator based on the equation.
Further, the nonlinear equation in step (503) is Kχ+ ψ is 0, wherein,
Figure BDA0003271433260000161
Figure BDA0003271433260000162
in order to be a lagrange multiplier,
Figure BDA0003271433260000163
a+＝max{0，a}，
Figure BDA0003271433260000164
Figure BDA0003271433260000165
the product of the hadamard is represented,
Figure BDA0003271433260000166
the solver of the model predictive control scheme of the mobile redundant mechanical arm is
Figure BDA00032714332600001612
Wherein the content of the first and second substances,
Figure BDA0003271433260000167
Figure BDA0003271433260000168
representing hadamard divisions
Figure BDA0003271433260000169
Figure BDA00032714332600001610
Further, for the input variables calculated by the solver
Figure BDA00032714332600001611
Taking the first input, namely the input u (k) at the moment k, and applying the first input to the mobile redundant mechanical arm systemAnd (4) track tracking control of the redundant mechanical arms moved in pairs.
In this embodiment, 120 seconds of simulation is performed on MATLAB software, and the simulation results are shown in fig. 3, 4, 5, and 6. The simulation example is divided into a first simulation experiment and a second simulation experiment, wherein the first simulation experiment is that the redundant mechanical arm is moved to track a kidney-shaped line, only one obstacle exists in the environment, and the coordinates of the obstacle are (3.23, 0.03 and 0.78) meters; in the second simulation experiment, the redundant mechanical arm is moved to track a Nike Menders spiral line, and the environment has two obstacles with coordinates of (2.73, -0.01, 0.78) m and (3.92, -1.93, 0.78) m. Fig. 3 is a simulation result of a first simulation experiment without using an obstacle avoidance method, where fig. 3(a) is a three-dimensional obstacle avoidance result diagram, and fig. 3(b) is a distance between a critical point and an obstacle. As can be seen from fig. 3, in the case where the obstacle avoidance method is not used, the redundant robot arm collides with the obstacle. Fig. 4 is a simulation result of a first simulation experiment in the case of using the obstacle avoidance method of the present invention, in which fig. 4(a) is a two-dimensional plan view of an actual trajectory and a desired trajectory of a mobile redundant robot arm end effector, fig. 4(b) is a three-dimensional obstacle avoidance result view, fig. 4(c) is a distance between a critical point and an obstacle, and fig. 4(d) is a tracking error in X, Y, Z three directions, respectively represented by eX、eY、eZAnd (4) showing. As can be seen from fig. 4, in the case of using the obstacle avoidance method of the present invention, the distance between the critical point and the obstacle is greater than the inner safety threshold d1I.e. the redundant robot arm avoids the collision with the obstacle. Fig. 5(a) - (c) are simulation results of a second simulation experiment in the case of using the obstacle avoidance method of the present invention, where fig. 5(a) is a three-dimensional obstacle avoidance result graph, fig. 5(b) is a tracking error in X, Y, Z three directions, and fig. 5(c) is a distance between a critical point and an obstacle; fig. 5(d) is a simulation result of the second simulation experiment without using the obstacle avoidance method, that is, the distance between the critical point and the obstacle. As can be seen from fig. 5, under the condition that the obstacle avoidance method is not used, the redundant mechanical arm collides with the obstacle for multiple times; under the condition of using the obstacle avoidance method, the distances between the critical point and the obstacle are both larger than the inner safety threshold d1I.e. the redundant mechanical arm is avoided from allCollision of obstacles.
FIG. 6 is a simulation result of a first simulation experiment under the conditions of not using an obstacle avoidance method, using an obstacle avoidance method of Chinese patent CN112605996A, using an obstacle avoidance method of Chinese patent CN108772835A, and using an obstacle avoidance method of the present invention, wherein FIGS. 6(a) - (b) are respectively a distance between a critical point and an obstacle and a tracking error in X, Y, Z three directions under the conditions of not using an obstacle avoidance method, FIGS. 6(c) - (d) are respectively a distance between a critical point and an obstacle and a tracking error in X, Y, Z three directions under the conditions of using an obstacle avoidance method of Chinese patent CN112605996A, FIGS. 6(e) - (f) are respectively a distance between a critical point and an obstacle and a tracking error in X, Y, Z three directions under the conditions of using an obstacle avoidance method of Chinese patent CN108772835A, and FIGS. 6(g) - (h) are respectively a distance between a critical point and an obstacle and X, and X are, Y, Z tracking errors in three directions. As can be seen from fig. 6, the joint angle feasible region generated by the obstacle avoidance method of chinese patent CN112605996A is small, resulting in a breakdown of the obstacle avoidance process; the obstacle avoidance method of the chinese patent CN108772835A generates a large tracking error; the obstacle avoidance method provided by the invention has small tracking error under the condition of realizing obstacle avoidance.
In conclusion, the method effectively realizes the track tracking control of the mobile redundant mechanical arm in the environment with the obstacle, and improves the accuracy and the safety of the control of the mobile redundant mechanical arm.
Finally, it should be noted that the above-mentioned preferred embodiments illustrate rather than limit the invention, and that, although the invention has been described in detail with reference to the above-mentioned preferred embodiments, it will be understood by those skilled in the art that various changes in form and detail may be made therein without departing from the scope of the invention as defined by the appended claims.

Claims (8)

1. A mobile redundant mechanical arm model prediction control method with an obstacle avoidance function is characterized by comprising the following steps:
s1: aiming at the mobile redundant mechanical arm, constructing a kinematic model of the mobile redundant mechanical arm;
s2: according to the kinematic model of the mobile redundant manipulator, a state space model of the mobile redundant manipulator is constructed, and the future state and the future output of the system are predicted;
s3: aiming at the problem of tracking the track of the mobile redundant mechanical arm, optimizing performance indexes, and designing system state constraint and input constraint according to the joint limit of the mechanical arm;
s4: aiming at the problem of safe operation of the mobile redundant mechanical arm in an environment with obstacles, an obstacle avoidance scheme is designed;
s5: and designing a model prediction control method of the mobile redundant mechanical arm, and completing the trajectory tracking control of the mobile redundant mechanical arm under the driving of the model prediction control method.
2. The method as claimed in claim 1, wherein the kinematic model of the mobile redundant manipulator in step S1 is established as
Figure FDA0003271433250000011
Wherein the content of the first and second substances,
Figure FDA0003271433250000012
to move the position of the end effector of the redundant robotic arm in a world coordinate system; w is the rotation angle of the mobile platform;
Figure FDA0003271433250000013
is the position of the end effector of the redundant robotic arm in its base coordinate system;
Figure FDA0003271433250000014
the joint angle of the redundant mechanical arm is 6 degrees of freedom;
Figure FDA0003271433250000015
is a connection point of a redundant mechanical arm and a mobile platformPosition in the frame;
Figure FDA0003271433250000016
for combined angles of rotation of the drive wheels of the mobile platform and of the joint angles of the redundant manipulator, wherein
Figure FDA0003271433250000017
Representing the rotation angle of two driving wheels of the mobile platform;
Figure FDA0003271433250000021
a mapping function for moving the joint angle of the redundant manipulator to the position of the end effector;
Figure FDA0003271433250000022
to move the velocity of the end effector of a redundant robotic arm in a world coordinate system, is
Figure FDA0003271433250000023
A derivative with respect to time t;
Figure FDA0003271433250000024
a Jacobian matrix for moving the redundant manipulator;
Figure FDA0003271433250000025
the derivative of q with respect to time t is the angular velocity at which the redundant robotic arm is moved.
3. The method as claimed in claim 1, wherein the step S2 is specifically implemented by predicting and controlling the model of the mobile redundant manipulator with obstacle avoidance function
S201: according to the kinematic model of the mobile redundant manipulator, a state space model of the mobile redundant manipulator at the moment k is constructed into
Figure FDA0003271433250000026
K represents an update index of the current time t ═ k δ seconds, and δ is a sampling interval;
Figure FDA0003271433250000027
is a state variable of the system and is,
Figure FDA0003271433250000028
and is
Figure FDA0003271433250000029
Representing the joint angle of the movable redundant mechanical arm at the moment k; x is the number ofjAnd xj+1Respectively representing system state variables of two moments before and after;
Figure FDA00032714332500000210
is an input variable of the system;
Figure FDA00032714332500000211
is an output variable of the system and is,
Figure FDA00032714332500000212
and is
Figure FDA00032714332500000213
Indicating the position of the moving redundant robotic arm end effector at time k;
Figure FDA00032714332500000214
a Jacobian matrix of the mobile redundant mechanical arm at the moment k is obtained;
s202: predicting the system state in N future time domains into
Figure FDA00032714332500000215
Wherein the content of the first and second substances,
Figure FDA0003271433250000031
Figure FDA0003271433250000032
Figure FDA0003271433250000033
for the system state at the future time k + i, i is 1, 2, …, N, and
Figure FDA0003271433250000034
representing a prediction time domain;
Figure FDA0003271433250000035
for future system inputs at time k + i, i ═ 1, 2, …, Nu-1, and
Figure FDA0003271433250000036
represents a control time domain; i is an identity matrix;
s203: predicting system output in N future time domains according to the state space model of the movable redundant mechanical arm at the k moment
Figure FDA0003271433250000037
Wherein the content of the first and second substances,
Figure FDA0003271433250000038
Figure FDA0003271433250000039
Figure FDA00032714332500000310
for the system output at time k + i in the future, i is 1, 2, …, N.
4. The method as claimed in claim 1, wherein the step S3 is specifically implemented by predicting and controlling the model of the mobile redundant manipulator with obstacle avoidance function
S301: aiming at the problem of track tracking of the mobile redundant mechanical arm, the performance index is designed and optimized to
Figure FDA0003271433250000041
Wherein the content of the first and second substances,
Figure FDA0003271433250000042
and
Figure FDA0003271433250000043
a weight matrix which is positively definite and symmetrical;
Figure FDA0003271433250000044
and
Figure FDA0003271433250000045
the number of euclidean norms is represented,
Figure FDA0003271433250000046
i is 1, 2, …, N for a desired trajectory of the moving redundant robotic arm end effector at a future time k + i;
s302: based on the joint limits of the robot arm, system state constraints and input constraints are designed as
Figure FDA0003271433250000047
And
Figure FDA0003271433250000048
wherein the content of the first and second substances,
Figure FDA0003271433250000049
Figure FDA00032714332500000410
and q is+And q is-In order to move the upper and lower limits of the joint angle of the redundant manipulator,
Figure FDA00032714332500000411
and
Figure FDA00032714332500000412
the upper and lower limits of the joint angular velocity for moving the redundant manipulator.
5. The method as claimed in claim 1, wherein the step S4 is specifically implemented by predicting and controlling the model of the mobile redundant manipulator with obstacle avoidance function
S401: determining a safe distance between the obstacle and the moving redundant robotic arm, wherein the internal safe threshold is d1The distance of collision between the movable redundant mechanical arm and the barrier is used; an external safety threshold of d2The distance for moving the redundant mechanical arm to avoid the obstacle is generally taken2≥2d1；
S402: determining a critical point on the moving redundant manipulator whose distance from the obstacle is less than an outer safety threshold d2；
S403: and designing an obstacle avoidance method, and endowing a critical point with a proper escape speed to enable the critical point to be far away from the obstacle.
6. The method as claimed in claim 5, wherein the method of step S403 is designed to avoid the obstacle by using a model predictive control method for the mobile redundant manipulator with obstacle avoidance function
Figure FDA0003271433250000051
Wherein the content of the first and second substances,
Figure FDA0003271433250000052
s＝[xC-xO，yC-yO，zC-zO]T；(·)Trepresenting a transpose operation on a matrix or vector; point C is a critical point on the arm with coordinates (x)C，yC，zC) (ii) a The point O is an obstacle with coordinates of (x)O，yO，zO)，GCA Jacobian matrix for the critical point C;
Figure FDA0003271433250000053
Figure FDA0003271433250000054
d represents the distance between the critical point and the obstacle.
7. The method as claimed in claim 1, wherein the step S5 is specifically implemented by predicting and controlling the model of the mobile redundant manipulator with obstacle avoidance function
S501: the model predictive control scheme for the track tracking control of the mobile redundant manipulator with the obstacle avoidance function is constructed by
And (3) minimizing:
Figure FDA0003271433250000055
and (3) model constraint:
Figure FDA0003271433250000061
Figure FDA0003271433250000062
Figure FDA0003271433250000063
wherein the content of the first and second substances,
Figure FDA0003271433250000064
s502: converting a model predictive control scheme of the mobile redundant mechanical arm into a standard quadratic programming form:
and (3) minimizing:
Figure FDA0003271433250000065
and (3) model constraint: sz is less than or equal to f,
wherein the content of the first and second substances,
Figure FDA0003271433250000066
Figure FDA0003271433250000067
s503: and converting the model predictive control scheme of the mobile redundant manipulator into a nonlinear equation by utilizing a Lagrange multiplier method and a nonlinear complementary problem function, and further designing a solver of the model predictive control scheme of the mobile redundant manipulator based on the equation.
8. The method as claimed in claim 7, wherein the nonlinear equation in step S503 is Kx + ψ 0, wherein,
Figure FDA0003271433250000068
Figure FDA0003271433250000069
in order to be a lagrange multiplier,
Figure FDA0003271433250000071
a+＝max{0，a}，
Figure FDA0003271433250000072
o represents HaThe product of the number of the products of the Dama,
Figure FDA0003271433250000073
the solver of the model predictive control scheme of the mobile redundant manipulator is Chij+1＝χj-γ(K+Mj)-1(Kχj+ψj) Wherein, γ ═ α τ,
Figure FDA0003271433250000074
for the design parameters, τ is the step size,
Figure FDA0003271433250000075
Figure FDA0003271433250000076
Figure FDA0003271433250000077
which represents a division of the hadamard,
Figure FDA0003271433250000078
Figure FDA0003271433250000079
further, for the input variables calculated by the solver
Figure FDA00032714332500000710
And taking the first input, namely the input u (k) at the moment k, and acting the first input on the system for moving the redundant mechanical arm to complete the trajectory tracking control of the movable redundant mechanical arm.
CN202111117973.XA 2021-09-21 2021-09-21 Mobile redundant mechanical arm model predictive control method with obstacle avoidance function Active CN113843793B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111117973.XA CN113843793B (en) 2021-09-21 2021-09-21 Mobile redundant mechanical arm model predictive control method with obstacle avoidance function

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111117973.XA CN113843793B (en) 2021-09-21 2021-09-21 Mobile redundant mechanical arm model predictive control method with obstacle avoidance function

Publications (2)

Publication Number Publication Date
CN113843793A true CN113843793A (en) 2021-12-28
CN113843793B CN113843793B (en) 2023-07-21

Family

ID=78979461

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111117973.XA Active CN113843793B (en) 2021-09-21 2021-09-21 Mobile redundant mechanical arm model predictive control method with obstacle avoidance function

Country Status (1)

Country Link
CN (1) CN113843793B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114571461A (en) * 2022-03-24 2022-06-03 合肥工业大学 Three-degree-of-freedom three-dimensional parallel robot trajectory tracking control algorithm based on Udwadia-Kalaba method

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104842355A (en) * 2015-01-20 2015-08-19 西北工业大学 Mixed-integer prediction control method for redundant space robot under obstacle avoidance restraint
CN107662211A (en) * 2017-10-16 2018-02-06 西北工业大学 A kind of robot for space forecast Control Algorithm based on quanta particle swarm optimization
WO2018107851A1 (en) * 2016-12-16 2018-06-21 广州视源电子科技股份有限公司 Method and device for controlling redundant robot arm
CN111975771A (en) * 2020-07-30 2020-11-24 华南理工大学 Mechanical arm motion planning method based on deviation redefinition neural network
CN113276121A (en) * 2021-05-31 2021-08-20 华南理工大学 Redundant manipulator moving obstacle avoidance method based on quadratic programming

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104842355A (en) * 2015-01-20 2015-08-19 西北工业大学 Mixed-integer prediction control method for redundant space robot under obstacle avoidance restraint
WO2018107851A1 (en) * 2016-12-16 2018-06-21 广州视源电子科技股份有限公司 Method and device for controlling redundant robot arm
CN107662211A (en) * 2017-10-16 2018-02-06 西北工业大学 A kind of robot for space forecast Control Algorithm based on quanta particle swarm optimization
CN111975771A (en) * 2020-07-30 2020-11-24 华南理工大学 Mechanical arm motion planning method based on deviation redefinition neural network
CN113276121A (en) * 2021-05-31 2021-08-20 华南理工大学 Redundant manipulator moving obstacle avoidance method based on quadratic programming

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
管小清;常青;梁冠豪;葛卓;: "一种冗余机械臂的多运动障碍物避障算法", 计算机测量与控制, no. 08 *
超;刘刚;王刚;杨学兵;: "基于RBF神经网络和二次规划的冗余机械臂避障问题研究", 机电工程, no. 01 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114571461A (en) * 2022-03-24 2022-06-03 合肥工业大学 Three-degree-of-freedom three-dimensional parallel robot trajectory tracking control algorithm based on Udwadia-Kalaba method
CN114571461B (en) * 2022-03-24 2023-08-11 合肥工业大学 Three-degree-of-freedom three-dimensional parallel robot track tracking control algorithm based on Udwadia-Kalaba method

Also Published As

Publication number Publication date
CN113843793B (en) 2023-07-21

Similar Documents

Publication Publication Date Title
US20210325894A1 (en) Deep reinforcement learning-based techniques for end to end robot navigation
US20220040857A1 (en) Humanoid robot and its balance control method and computer readable storage medium
US9592606B2 (en) Method and control means for controlling a robot
CN108555911A (en) Remote operating machinery arm, three-D barrier-avoiding method based on virtual thrust
WO2021065196A1 (en) Method and system for trajectory optimization for nonlinear robotic systems with geometric constraints
Sayour et al. Autonomous robotic manipulation: real‐time, deep‐learning approach for grasping of unknown objects
CN109807886A (en) A kind of space non-cooperative target based on prediction arrests strategy
Palmieri et al. Motion planning and control of redundant manipulators for dynamical obstacle avoidance
CN111522351B (en) Three-dimensional formation and obstacle avoidance method for underwater robot
CN107966907A (en) A kind of Obstacle avoidance applied to redundancy mechanical arm solves method
CN112198901B (en) Unmanned aerial vehicle autonomous collision avoidance decision method based on three-dimensional dynamic collision area
CN110095983B (en) Mobile robot prediction tracking control method based on path parameterization
US20210101286A1 (en) Robotic manipulation using domain-invariant 3d representations predicted from 2.5d vision data
CN112605996A (en) Model-free collision avoidance control method for redundant mechanical arm
CN111309002A (en) Wheel type mobile robot obstacle avoidance method and system based on vector
CN113843793A (en) Mobile redundant mechanical arm model prediction control method with obstacle avoidance function
CN114274129A (en) Mechanical arm motion planning and control method, system and medium based on visual guidance
CN115157262A (en) Obstacle avoidance and optimization control method and system for joint-limited redundant mechanical arm and robot
Vu et al. Machine learning-based framework for optimally solving the analytical inverse kinematics for redundant manipulators
Ahn et al. Robotic assembly strategy via reinforcement learning based on force and visual information
Li et al. A model predictive obstacle avoidance method based on dynamic motion primitives and a Kalman filter
CN112650217B (en) Robot trajectory tracking strategy dynamic optimization method based on evaluation function
CN116714780A (en) Rotor flying mechanical arm and planning and control method for rapid aerial grabbing
Cui et al. Fast and accurate obstacle detection of manipulator in complex human–machine interaction workspace
Saramago et al. Dynamic optimization for the trajectory planning of robot manipulators in the presence of obstacles

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