CN113341728B - Anti-noise type return-to-zero neural network four-wheel mobile mechanical arm trajectory tracking control method - Google Patents

Abstract

The invention discloses a four-wheel mobile mechanical arm track tracking control method of an anti-noise return-to-zero neural network, which comprises the following steps: a. measuring the data of the wheels of the four-wheel mobile mechanical arm and the mechanical arm; b. the method comprises the following steps of (1) giving a desired track of the four-wheel moving mechanical arm; c. establishing a kinematic equation by combining the kinematic characteristics of the four-wheel mobile mechanical arm; d. obtaining a mechanical arm kinematics equation through space coordinate transformation; e. establishing an overall kinematic equation of the four-wheel mobile mechanical arm based on the mobile platform and the mechanical arm model; f. defining a vector type error function aiming at the track tracking problem; g. and obtaining a kinetic equation of the anti-noise return-to-zero neural network by combining a kinematic equation, and solving the problem of track tracking of the four-wheel mobile mechanical arm under noise disturbance. The anti-noise type return-to-zero neural network controller is designed based on the difference value between the expected track and the actual motion track as an error function, noise interference in the track tracking process of the four-wheel mobile mechanical arm is inhibited, and a track tracking task is completed.

Description

Trajectory tracking control of a four-wheel mobile manipulator based on an anti-noise reset-to-zero neural network method

technical field

The invention relates to the field of mobile robots, in particular to a trajectory tracking control algorithm of a four-wheel mobile mechanical arm based on kinematics and an anti-noise type zeroing neural network.

Background technique

In recent years, my country's manufacturing industry has continued to develop rapidly and the overall scale has increased significantly, which has played a positive role in promoting the domestic economy and the world economy. The domestic manufacturing industry is still dominated by labor-intensive low-end manufacturing, with relatively low added value, and is generally only a "world factory". With the rapid development of the domestic economy and the trend of an aging population, labor costs will inevitably increase gradually, and the "demographic dividend" of China's manufacturing industry will gradually disappear. In addition, the fourth industrial revolution centered on "smart manufacturing" is sweeping the world. my country insists on focusing on the real economy for economic development, accelerates the construction of a strong manufacturing country and a strong quality country, and realizes the upgrading of the manufacturing industry. With the continuous advancement of intelligent manufacturing, "machine substitution" is gradually unfolding. When the mobile manipulator works in a dynamic, unknown and complex environment, it should have complete autonomy, that is to say, the system should have the ability to perceive, plan, maneuver and coordinate, etc. Therefore, in the theoretical research of the mobile manipulator, it is necessary to The problems addressed include trajectory planning, motion control, and cooperative control. The motion control of mobile manipulators can be divided into three types: point stabilization, path following and trajectory tracking according to different control objectives.

Most of the theoretical research on mobile manipulators at this stage is based on two or three wheels, and most of the robots are based on dynamic modeling. The dynamic modeling of four-wheel mobile manipulators is cumbersome, and it is necessary to analyze the power of the mobile platform. Dynamics model of the robotic arm. It is difficult to integrate the two models into one system, so most researchers use two control algorithms to control the two subsystems respectively, and it is difficult to realize the coordinated control of the mobile platform and the manipulator. Therefore, the present invention integrates the kinematics model of the mobile platform and the kinematics model of the manipulator into a system based on the world coordinate system through spatial coordinate transformation, and proposes an anti-noise return-to-zero neural network. The track tracking control method of the wheel moving manipulator realizes the track tracking control of the four-wheel moving manipulator.

SUMMARY OF THE INVENTION

ψ(·)代表激活函数，并选择线性激活函数ψ(e(t))＝e(t)，由此可以得到e(t)＝e(0)exp(-γt)，随着时间t变大误差函数e(t)收敛于0。结合四轮移动机械臂的整体运动学方程得到抗噪型归零神经网络动力学模型，抑制四轮移动机械臂在轨迹跟踪过程中的噪声干扰，解决了四轮移动机械臂在跟踪期望轨迹过程中受到外力碰撞、控制模块中电源电压的瞬时衰减等噪声干扰。另外，相对比系统的动力学建模，运动学建模相对简单。结合说明书附图，本发明的技术方案如下：The invention discloses a four-wheel mobile mechanical arm trajectory tracking control method based on an anti-noise type zeroing neural network. Based on the four-wheel mobile mechanical arm in the world coordinate system, the overall kinematics equation of the system is established. The desired trajectory equation is designed within the reach space, and a vector-type error function is defined based on the difference between the expected trajectory function and the actual motion trajectory function, and the differential equation of the error function e(t) is constructed to satisfy

ψ( ) represents the activation function, and chooses the linear activation function ψ(e(t))=e(t), from which we can obtain e(t)=e(0)exp(-γt), which changes with time t The large error function e(t) converges to zero. Combined with the overall kinematics equation of the four-wheeled mobile manipulator, the anti-noise zeroing neural network dynamics model is obtained, which suppresses the noise interference of the four-wheeled mobile manipulator during the trajectory tracking process, and solves the problem of the four-wheeled mobile manipulator in the process of tracking the desired trajectory. It is subject to noise interference such as external force collision and instantaneous attenuation of the power supply voltage in the control module. In addition, kinematic modeling is relatively simple compared to the dynamic modeling of the system. In conjunction with the accompanying drawings, the technical solutions of the present invention are as follows:

A four-wheel mobile manipulator trajectory tracking control method based on an anti-noise type zeroing neural network, the control method is specifically as follows:

S1: Collect the initial angle data of the four wheels of the four-wheel mobile manipulator and the initial angle data of the four-degree-of-freedom manipulator;

S2: According to the needs of the designer, the desired trajectory equation is given within the reachable space of the four-wheel mobile manipulator at the same time;

S3: The kinematic equation of the four-degree-of-freedom manipulator in the base coordinate system is obtained through spatial coordinate transformation, and the kinematic equation is obtained by analyzing the kinematic characteristics of the mobile platform. The overall kinematics equation of the mobile manipulator in the world coordinate system;

S4: In order to deal with the trajectory tracking problem of the mobile manipulator, the difference between the expected trajectory function and the actual trajectory function is designed as a vector-type error function, and an anti-noise zeroing neural network model controller is designed;

S5: Based on the parameters solved by the neural dynamics equation in step 4, the trajectory tracking task is completed by moving the robotic arm through motor control.

The specific process of step S1 is:

In this experiment, it is necessary to refer to the hardware parameters of the four-wheel mobile manipulator, measure the height of the mobile platform with a meter ruler, and measure the working range of each manipulator under the condition of power failure, and check the maximum rotation speed of each joint on the hardware official website. The parameters of each joint are as follows:

Axis 1 base Working range +90° to -90° Maximum speed (250 load) 320°/s Axis 2 Boom Working range 0° to +85° Maximum speed (250 load) 320°/s Axis 3 forearm Working range -10° to +95° Maximum speed (250 load) 320°/s Axis 4 rotation Working range +90° to -90° Maximum speed (250 load) 320°/s

The specific process of step S2 is:

According to the measured values in step S1, the desired trajectory of the end effector of the four-wheel mobile manipulator is designed to ensure that it does not exceed the reachable range of each joint of the mobile manipulator. The expected trajectory function expression is as follows:

r _xd =0.2×cos(0.1×t)

r _yd = 0.2×sin(0.2×t)

r _zd = 0.3×ones(1, size(t, 2))

The specific process of step S3 is:

S301: In order to describe the relative position and direction relationship of each link of the four-wheel moving mechanical arm, a coordinate system needs to be established on each link according to the joint structure of the mechanical arm. Using the principle of establishing the DH joint coordinate system, the homogeneous transformation ^i-1 T _i of the connecting rod coordinate system {i} relative to {i-1} is called the connecting rod transformation, in which the shaft rotation angle α _i-1 and the connecting rod length are designed a _i-1 , link offset distance d _i , joint variable θ _i , so it can be decomposed into sub-transformation problems of coordinate system {i}, each sub-transformation only depends on one link parameter, there are:

^i-1 T _i =Rot(x,α _i-1 )Trans(x,a _i-1 )Rot(z,θ _i )Trans(z,d _i )

The general transformation formula between the connected connecting rods:

The kinematic equation of the manipulator based on the base coordinate system is obtained by coordinate transformation:

Wherein, c ₁ =cosθ ₁ , s ₁ =sinθ ₁ , c ₂₃ =cos(θ ₂ +θ ₃ ), s ₂₃ =sin(θ ₂ +θ ₃ )

S302: The mobile platform chooses Mecanum wheel as the driving wheel, and adopts four-wheel all-wheel drive in terms of power. The kinematics of the Mecanum wheel chassis of the mobile platform is decomposed into three independent variables to describe; first calculate the speed of the axis position of each wheel; As a result of the second step, the actual speed of the wheel is calculated, and the inverse solution of the four-wheel omnidirectional kinematics model is obtained:

Then the positive kinematics solution of the four-wheel omnidirectional chassis can be derived:

表示X轴运动的方向，即左右方向，定义向右为正，

表示Y轴运动的方向，即前后方向，定义向前为正，ω表示yaw轴自转的角速度，定义逆时针为正，这几个量都是四个轮子的几何中心(矩形的对角线)的速度。v_ω1v_ω2v_ω3v_ω4表示每个车轮的速度。in,

Indicates the direction of X-axis movement, that is, the left and right direction, which is defined as positive to the right,

Indicates the direction of movement of the Y-axis, that is, the front and rear direction, which is defined as positive forward, ω represents the angular velocity of the yaw axis rotation, and is defined as positive counterclockwise. These quantities are the geometric centers of the four wheels (diagonals of the rectangle) speed. v _ω1 v _ω2 v _ω3 v _ω4 represents the speed of each wheel.

S303: By using the transformation matrix from the base coordinate system to the world coordinate system, the overall kinematics equation of the mobile manipulator in the world coordinate system can be obtained:

Differentiating the above formula with time t yields the following overall kinematic equation:

ν＝[θ^Τ,ω]^Τ Among them, the Jacobian matrix

ν=[θ ^Τ ,ω] ^Τ

It is finally sorted into the following simplified kinematic equations:

Among them, q=[v, θ ^Τ ] ^Τ , represents the angle vector of the mobile manipulator, including the rotation angle of the wheel of the mobile platform and the rotation angle of each joint of the manipulator.

The specific process of step S4 is:

In practical applications, there are many types of disturbances during the operation of the four-wheeled mobile manipulator. An anti-noise zeroing neural network model and related models are proposed. In order to monitor the process of solving the inverse kinematics problem of the mobile manipulator, the definition Vector error function:

e(t)=z _d (t)-z(t)

Among them, z _d (t) and z (t) represent the expected trajectory and the actual running trajectory of the mobile manipulator, respectively.

In order to obtain the exact solution of the time-varying inverse kinematics, each term of the error function is required to be close to zero. The dynamic equation of the anti-noise zeroing neural network is as follows:

Among them, ψ( ) represents the activation function of the neural network, choose a simple linear activation function ψ(e(t))=e(t), γ>0, λ>0 are adjustable parameters to change the convergence speed of the system . Introduce an integral term in the kinetic equation to remove noise.

Simultaneous overall kinematics equation and anti-noise zeroing neural network dynamic equation, the anti-noise zeroing neural network model with external disturbance is as follows:

Among them, η is the noise interference term. During the actual operation of the mobile robot, there is always external interference that affects the normal operation of the robot. For example, constant external force; transient decaying external force, etc.

Figure 1 (see accompanying drawing) presents the composition and basic principles of neural dynamics. The anti-noise zeroing neural network algorithm based on time derivative information, neural network activation function and integral term can effectively solve the time-varying inverse kinematics equation of the externally disturbed four-wheel mobile manipulator. This model can be regarded as a typical closed-loop control system in classical control theory, and as a controller system composed of a generalized proportional-integral-derivative controller.

The specific process of step S5 is:

Through the above dynamic equations, the speed of the wheels of the mobile platform and the rotation angle of each joint can be obtained in the process of tracking the desired trajectory of the four-wheel mobile manipulator. The obtained parameters can be applied to each motor to adjust each joint for trajectory tracking.

Compared with the prior art, the advantages of the present invention are:

The invention proposes an anti-noise return-to-zero neural network algorithm to deal with the trajectory tracking problem of a four-wheel mobile mechanical arm. The characteristic is that the traditional mobile manipulator control requires the establishment of a dynamic model of the system, and the need to control the mobile platform and each joint manipulator separately. The complex dynamic modeling integrates the two into a system through spatial coordinate transformation to realize the coordinated control of the mobile manipulator. The invention designs an anti-noise return-to-zero neural network control algorithm to solve the trajectory tracking problem of the four-wheel mobile manipulator, solves the control problem of the mobile manipulator in the case of external noise interference, and verifies the algorithm through simulation experiments. effectiveness.

Description of drawings

Figure 1 is a schematic diagram of an anti-noise zeroing neural network model for suppressing external time-varying disturbances;

Fig. 2 is the trajectory tracking image of controlling the end effector of the four-wheel mobile manipulator based on the anti-noise type zeroing neural network model according to the present invention;

Fig. 3 is the top view of controlling the end effector of the four-wheel mobile manipulator to track the desired trajectory based on the anti-noise return-to-zero neural network model according to the present invention;

FIG. 4 is an error image of controlling the end effector of a four-wheeled mobile manipulator to track a desired trajectory based on an anti-noise return-to-zero neural network model according to the present invention;

FIG. 5 is an image of the error rate of change for controlling the end-effector of a four-wheeled mobile manipulator to track a desired trajectory based on an anti-noise return-to-zero neural network model according to the present invention;

6 is an image of the angle change of each manipulator arm based on the anti-noise return-to-zero neural network model for controlling the end effector of the four-wheel mobile manipulator to track the desired trajectory according to the present invention;

7 is an image of the angular velocity change of each manipulator in which the end effector of a four-wheeled mobile manipulator is controlled to track a desired trajectory based on an anti-noise return-to-zero neural network model according to the present invention;

FIG. 8 is an image of the rotation angle change of each wheel based on the anti-noise return-to-zero neural network model to control the end effector of the four-wheel mobile manipulator to track the desired trajectory according to the present invention;

FIG. 9 is an image of the angular velocity variation of each wheel of the four-wheel mobile manipulator to track the desired trajectory based on the anti-noise return-to-zero neural network model according to the present invention.

Detailed ways

The invention discloses a four-wheel mobile manipulator trajectory tracking control method of an anti-noise type zeroing neural network. The method is specifically as follows:

S1: Collect the initial angle data of the four wheels of the four-wheel mobile manipulator and the initial angle data of the four-degree-of-freedom manipulator;

S2: According to the needs of the designer, the desired trajectory equation is given within the reachable space of the four-wheel mobile manipulator at the same time;

S3: The kinematic equation of the four-degree-of-freedom manipulator in the base coordinate system is obtained through spatial coordinate transformation, and the kinematic equation is obtained by analyzing the kinematic characteristics of the mobile platform. The overall kinematics equation of the mobile manipulator in the world coordinate system;

S4: In order to deal with the trajectory tracking problem of the mobile manipulator, the difference between the expected trajectory function and the actual trajectory function is designed as a vector-type error function, and an anti-noise zeroing neural network model controller is designed;

S5: Based on the parameters solved by the neural dynamics equation in step 4, the trajectory tracking task is completed by moving the robotic arm through motor control.

The specific process of step S1 is:

In this experiment, it is necessary to refer to the hardware parameters of the four-wheel mobile manipulator, measure the height of the mobile platform with a meter ruler, and measure the working range of each manipulator under the condition of power failure, and check the maximum rotation speed of each joint on the hardware official website. The parameters of each joint are as follows:

Axis 1 base Working range +90° to -90° Maximum speed (250 load) 320°/s Axis 2 Boom Working range 0° to +85° Maximum speed (250 load) 320°/s Axis 3 forearm Working range -10° to +95° Maximum speed (250 load) 320°/s Axis 4 rotation Working range +90° to -90° Maximum speed (250 load) 320°/s

The specific process of step S2 is:

According to the measured values in step S1, the desired trajectory of the end effector of the four-wheel mobile manipulator is designed to ensure that it does not exceed the reachable range of each joint of the mobile manipulator. The expected trajectory function expression is as follows:

r _xd =0.2×cos(0.1×t)

r _yd = 0.2×sin(0.2×t)

r _zd = 0.3×ones(1, size(t, 2))

The specific process of step S3 is:

S301: In order to describe the relative position and direction relationship of each link of the four-wheel moving mechanical arm, a coordinate system needs to be established on each link according to the joint structure of the mechanical arm. Using the principle of establishing the DH joint coordinate system, the homogeneous transformation ^i-1 T _i of the connecting rod coordinate system {i} relative to {i-1} is called the connecting rod transformation, in which the shaft rotation angle α _i-1 and the connecting rod length are designed a _i-1 , link offset distance d _i , joint variable θ _i , so it can be decomposed into sub-transformation problems of coordinate system {i}, each sub-transformation only depends on one link parameter, there are:

^i-1 T _i =Rot(x,α _i-1 )Trans(x,a _i-1 )Rot(z,θ _i )Trans(z,d _i )

The general transformation formula between the connected connecting rods:

The kinematic equation of the manipulator based on the base coordinate system is obtained by coordinate transformation:

Wherein, c ₁ =cosθ ₁ , s ₁ =sinθ ₁ , c ₂₃ =cos(θ ₂ +θ ₃ ), s ₂₃ =sin(θ ₂ +θ ₃ )

S302: The mobile platform chooses Mecanum wheel as the driving wheel, and adopts four-wheel all-wheel drive in terms of power. The kinematics of the Mecanum wheel chassis of the mobile platform is decomposed into three independent variables to describe; first calculate the speed of the axis position of each wheel; As a result of the second step, the actual speed of the wheel is calculated, and the inverse solution of the four-wheel omnidirectional kinematics model is obtained:

Then the positive kinematics solution of the four-wheel omnidirectional chassis can be derived:

表示X轴运动的方向，即左右方向，定义向右为正，

表示Y轴运动的方向，即前后方向，定义向前为正，ω表示yaw轴自转的角速度，定义逆时针为正，这几个量都是四个轮子的几何中心(矩形的对角线)的速度。v_ω1v_ω2v_ω3v_ω4表示每个车轮的速度。in,

Indicates the direction of X-axis movement, that is, the left and right direction, which is defined as positive to the right,

Indicates the direction of movement of the Y-axis, that is, the front and rear direction, which is defined as positive forward, ω represents the angular velocity of the yaw axis rotation, and is defined as positive counterclockwise. These quantities are the geometric centers of the four wheels (diagonals of the rectangle) speed. v _ω1 v _ω2 v _ω3 v _ω4 represents the speed of each wheel.

S303: By using the transformation matrix from the base coordinate system to the world coordinate system, the overall kinematics equation of the mobile manipulator in the world coordinate system can be obtained:

Differentiating the above formula with time t yields the following overall kinematic equation:

ν＝[θ^Τ,ω]^Τ Among them, the Jacobian matrix

ν=[θ ^Τ ,ω] ^Τ

It is finally sorted into the following simplified kinematic equations:

Among them, q=[v, θ ^Τ ] ^Τ , represents the angle vector of the mobile manipulator, including the rotation angle of the wheel of the mobile platform and the rotation angle of each joint of the manipulator.

The specific process of step S4 is:

In practical applications, there are many types of disturbances during the operation of the four-wheeled mobile manipulator. An anti-noise zeroing neural network model and related models are proposed. In order to monitor the process of solving the inverse kinematics problem of the mobile manipulator, the definition Vector error function:

e(t)=z _d (t)-z(t)

Among them, z _d (t) and z (t) represent the expected trajectory and the actual running trajectory of the mobile manipulator, respectively.

In order to obtain the exact solution of the time-varying inverse kinematics, each term of the error function is required to be close to zero. The dynamic equation of the anti-noise zeroing neural network is as follows:

Among them, ψ( ) represents the activation function of the neural network, choose a simple linear activation function ψ(e(t))=e(t), γ>0, λ>0 are adjustable parameters to change the convergence speed of the system . Introduce an integral term in the kinetic equation to remove noise.

Simultaneous overall kinematics equation and anti-noise zeroing neural network dynamic equation, the anti-noise zeroing neural network model with external disturbance is as follows:

Among them, η is the noise interference term. During the actual operation of the mobile robot, there is always external interference that affects the normal operation of the robot. For example, constant external force; transient decaying external force, etc.

Figure 1 (see accompanying drawing) presents the composition and basic principles of neural dynamics. The anti-noise zeroing neural network algorithm based on time derivative information, neural network activation function and integral term can effectively solve the time-varying inverse kinematics equation of the externally disturbed four-wheel mobile manipulator. This model can be regarded as a typical closed-loop control system in classical control theory, and as a controller system composed of a generalized proportional-integral-derivative controller.

The specific process of step S5 is:

Through the above dynamic equations, the speed of the wheels of the mobile platform and the rotation angle of each joint can be obtained in the process of tracking the desired trajectory of the four-wheel mobile manipulator. The obtained parameters can be applied to each motor to adjust each joint for trajectory tracking.

Claims (2)

1. a four-wheel mobile manipulator trajectory tracking control method of anti-noise type return-to-zero neural network, is characterized in that, described control method steps are as follows: S1: The desired trajectory equation of the four-wheel moving manipulator is given according to the requirements; S2: Given the initial rotation angle of each wheel of the four-wheel mobile manipulator, and the initial angle of the four-degree-of-freedom manipulator, and measure the length and width of the mobile platform; S3: Build the overall kinematics model of the mobile platform; S4: Design an anti-noise zeroing neural network model; S5: Based on the kinematic characteristics of the four-wheel mobile manipulator, construct the overall kinematic model of the four-wheel mobile manipulator. The specific mathematical expression is as follows:

Among them, P is the coefficient matrix of the overall kinematic model,

is the differential of the actual trajectory equation with respect to time t,

is the differential of the four wheels of the four-wheel mobile manipulator and the variables of the four manipulators to time t; According to the design formula of the anti-noise zeroing neural network model, the error function of the system is: e(t)=z _d (t)-z(t) Among them, z _d (t) is the desired trajectory equation, and z(t) is the actual trajectory equation derived from the overall kinematics model; Combined with the overall kinematics equation and the neural network model, an anti-noise reset-to-zero neural network controller is obtained. The specific mathematical expression is as follows:

Among them, γ>0, λ>0 are adjustable parameters,

is the differential of the desired trajectory with respect to time t, and η is the noise considered in the system; In the process of trajectory tracking, considering the influence of exponential decay noise, linear noise, sinusoidal noise and mixed noise on the system, the four-wheel mobile manipulator is controlled to complete the trajectory tracking task based on the anti-noise zeroing neural network model. 2. the four-wheel mobile manipulator trajectory tracking control method of a kind of anti-noise type return-to-zero neural network described in S5 in claim 1, is characterized in that, the noise considered in the system is: Linear noise is η _linear = 0.1*t; Exponential decay noise is η _{exponential decay} =e ^−0.2*t ; The sine noise is η _sine =sin(0.2*t); Mixed noise is η _mixed = η _{exponential decay} + η _linear + η _sine .

Images