Disclosure of Invention
The invention aims to: the flexible joint control method based on the sliding-mode observer is provided, and aims to solve the problems that a flexible joint mechanical arm system in the prior art is high in nonlinearity, difficult to model and prone to external disturbance.
The technical scheme adopted by the invention is as follows:
a flexible joint control method based on a sliding-mode observer comprises the following steps:
step 1: establishing a flexible joint mechanical arm dynamic model:
wherein (1) represents a load side dynamic model, (2) represents a motor side dynamic model,
indicating the load side angleThe angular velocity and the angular acceleration,
representing motor side angle, angular velocity and angular acceleration, m (q) representing the inertia matrix of the robot arm,
representing a nonlinear term consisting of centrifugal force and Copenforces, G (q) representing a gravity matrix of the robot arm, J representing a motor-side moment of inertia, K
sDenotes joint stiffness, λ denotes joint reduction ratio, τ
extRepresenting external disturbance torque, τ
eAnd τ
mRespectively representing the load side and the motor driving torque;
step 2: designing a sliding-mode observer:
wherein x in (3)1,x2Denotes the generalized state of the system,. tau.denotes the control input of the system, f (x)1,x2T) is the nonlinear dynamics of the system, including the internal dynamics of the system and the external disturbance torque; assuming that the internal dynamic model structure and model parameters of the system are unknown, and considering both the internal dynamic model and the external disturbance torque as unknown disturbance, a sliding mode disturbance observer pair f (x) shown below can be adopted1,x2T) estimating:
wherein (A, B, L > 0) are all gain coefficients, sigmoid (e)z) Is expressed as a saturation function as follows:
a is more than 0 and is a control parameter, and the problem of vibration of the sliding mode observer can be effectively inhibited by introducing a saturation function;
and step 3: applying a sliding-mode observer to a flexible joint mechanical arm system:
order to
And
respectively load side and motor side trajectory commands,
and
for the corresponding track tracking error, the flexible joint mechanical arm error dynamics model can be organized as follows:
take the load side as an example, let x
lq=e
q,
Representing a system disturbance, then (6) may be arranged as:
comparing (3) and (8), the following sliding mode disturbance observer can be designed for the load side:
in the same way, the motor side sliding mode disturbance observer can be designed as follows:
and 4, step 4: designing a flexible joint mechanical arm control law:
dividing a joint model of a flexible joint mechanical arm into a motor side and a load side, adopting a layered cascade control strategy, introducing a virtual controller and the motor side controller together at the load side, and forming a distributed cascade control architecture at each joint of the mechanical arm, wherein the load side virtual controller calculates a control moment actually required by the joint load side by combining a user track command and system state information, and maps the load side control moment into a motor side control command through an elastic dynamics mapping formula as follows:
then, a motor side controller outputs a motor side control torque by combining a motor side track command and motor side state information, so that the joint is driven to track a given track; the control law structures of the load side and the motor side are as follows:
in the formula (12) (K)
pq,K
dq) (K) in equation (13) for position and velocity gain of the load side controller
pθ,K
dθ) Is a motor sideThe position and velocity gains of the controller,
and
estimating system disturbances on a load side and a motor side by sliding mode observers shown in equations (9) and (10), respectively;
and 5: updating the control output of the mechanical arm:
acquiring real-time angle parameters of the mechanical arm in the motion process through a load side and motor side angle encoder arranged in the mechanical arm joint, and acquiring angular velocity information of the mechanical arm joint motion through first-order differential processing; obtaining elastic parameters of each joint of the mechanical arm through external calibration or parameter identification; and finally, calculating the control input of each joint of the mechanical arm and using the control input for controlling the mechanical arm by combining the state feedback information and the joint elastic parameters.
In summary, due to the adoption of the technical scheme, the invention has the beneficial effects that:
1. the sliding mode observer designed by the invention estimates the dynamic characteristics and the external disturbance of the system, and can accurately estimate the system disturbance without depending on the internal model structure and related parameters of the system.
2. According to the invention, a saturation function is introduced into the sliding mode observer, so that the chattering phenomenon of the traditional sliding mode observer based on the sign function can be effectively inhibited.
3. According to the invention, a layered control architecture is adopted, a load side virtual controller is introduced, and load side control input is mapped into a motor side control command through joint elastic dynamic mapping, on the basis, a sliding mode observer is utilized to effectively estimate system disturbance, model parameters except joint rigidity are not needed, complex dynamic modeling and dynamic parameter identification processes are avoided, and the method has the characteristics of simplicity and easiness in use.
4. The invention has the capability of effectively estimating and compensating the unknown dynamic behavior and the external disturbance of the system, thereby improving the control precision and the anti-interference performance of the flexible joint mechanical arm.
Detailed Description
The invention will be further explained in detail with reference to the drawings and technical solutions.
Referring to fig. 1, a flexible joint control method based on a sliding-mode observer includes the following steps:
step 1: establishing a flexible joint mechanical arm dynamic model:
wherein (1) represents a load side dynamic model, (2) represents a motor side dynamic model,
representing the load side angle, angular velocity and angular acceleration,
representing motor side angle, angular velocity and angular acceleration, m (q) representing the inertia matrix of the robot arm,
representing a nonlinear term consisting of centrifugal force and Copenforces, G (q) representing a gravity matrix of the robot arm, J representing a motor-side moment of inertia, K
sDenotes joint stiffness, λ denotes joint reduction ratio, τ
extRepresenting external disturbance torque, τ
eAnd τ
mRespectively representing the load side and the motor driving torque;
step 2: designing a sliding-mode observer:
wherein x in (3)1,x2Denotes the generalized state of the system,. tau.denotes the control input of the system, f (x)1,x2T) is the nonlinear dynamics of the system, including the internal dynamics of the system and the external disturbance torque;
assuming that the internal dynamic model structure and model parameters of the system are unknown, and considering both the internal dynamic model and the external disturbance torque as unknown disturbance, a sliding mode disturbance observer pair f (x) shown below can be adopted1,x2T) estimating:
wherein (A, B, L > 0) are all gain coefficients, sigmoid (e)z) Is expressed as a saturation function as follows:
a is more than 0 and is a control parameter, and the problem of vibration of the sliding mode observer can be effectively inhibited by introducing a saturation function;
and step 3: applying a sliding-mode observer to a flexible joint mechanical arm system:
order to
And
respectively load side and motor side trajectory commands,
and
for the corresponding track tracking error, the flexible joint mechanical arm error dynamics model can be organized as follows:
take the load side as an example, let x
1q=e
q,
Representing a system disturbance, then (6) may be arranged as:
comparing (3) and (8), the following sliding mode disturbance observer can be designed for the load side:
in the same way, the motor side sliding mode disturbance observer can be designed as follows:
and 4, step 4: designing a flexible joint mechanical arm control law:
dividing a joint model of a flexible joint mechanical arm into a motor side and a load side, adopting a layered cascade control strategy (as shown in figure 1), introducing a virtual controller and a motor side controller together at the load side, and forming a distributed cascade control framework at each joint of the mechanical arm, wherein the load side virtual controller calculates a control moment actually required by the joint load side by combining a user track command and system state information, and maps the load side control moment into a motor side control command by the following elastodynamics mapping formula:
then, a motor side controller outputs a motor side control torque by combining a motor side track command and motor side state information, so that the joint is driven to track a given track;
the control law structures of the load side and the motor side are as follows:
in the formula (12) (K)
pq,K
dq) (K) in equation (13) for position and velocity gain of the load side controller
pθ,K
dθ) For the position and speed gains of the motor side controller,
and
estimating system disturbances on a load side and a motor side by sliding mode observers shown in equations (9) and (10), respectively;
and 5: updating the control output of the mechanical arm:
acquiring real-time angle parameters of the mechanical arm in the motion process through a load side and motor side angle encoder arranged in the mechanical arm joint, and acquiring angular velocity information of the mechanical arm joint motion through first-order differential processing; obtaining elastic parameters of each joint of the mechanical arm through external calibration or parameter identification; finally, calculating the control input of each joint of the mechanical arm and using the control input for controlling the mechanical arm by combining the state feedback information and the joint elastic parameters;
fig. 2 is a schematic view of the working principle of a flexible joint robot arm to which the control method designed by the present invention is applied, and the control simulation results are shown in fig. 3-7; as can be seen from fig. 3 and 4, the control method can achieve good trajectory tracking and disturbance estimation performance; fig. 5 and 6 compare the control inputs of the sliding mode observer using the saturation function and the sliding mode observer using the conventional sign function, and it can be seen that the chattering phenomenon of the motor-side control signal is effectively suppressed due to the introduction of the saturation function in the present invention, and this characteristic can improve the capability of the control system to cope with the sensor noise, and enhance the robustness and stability of the system; fig. 7 compares the trajectory tracking accuracy with or without disturbance compensation, and it can be seen that the flexible joint mechanical arm control method based on the sliding-mode observer provided by the invention has an obvious advantage in control accuracy compared with the conventional PD controller.
In conclusion, the system disturbance can be accurately estimated under the condition of not depending on the internal model structure and related parameters of the system; a saturation function is introduced into the sliding mode observer, so that the chattering phenomenon of the traditional sliding mode observer based on the sign function can be effectively inhibited; the method can avoid complex dynamics modeling and dynamics parameter identification processes, and has the characteristics of simplicity and easiness in use; the flexible joint mechanical arm has the capability of effectively estimating and compensating unknown dynamic behaviors and external disturbance of a system, and can improve the control precision and the anti-interference performance of the flexible joint mechanical arm.