Wheel-foot robot, robot system, motion control method and motion control system

Abstract

The invention discloses a wheel-foot robot, a robot system, a motion control method and a motion control system, and the motion control system comprises a track publishing module which is used for converting the expected speed and position of a robot trunk into a state track; the predictive control module is used for constructing a model predictive control algorithm which takes the expected trajectory as input and takes the system state and the control input quantity as output, and optimizing the system state and the control input quantity in real time; the state estimation module is used for estimating the motion state of the robot by fusing the sensor data; and the whole body control module is used for constructing a multi-task control algorithm which takes the expected trajectory and the motion state as input and takes the joint torque, the joint speed and the joint position as output and has priorities or weights. According to the method, global prediction and optimization can be realized, the complexity of each optimization calculation can be effectively reduced, and the response speed and the real-time processing capability of a robot system are improved.

Description

Technical Field

[0001] The present invention relates to a robot and a control system, and in particular to a wheeled robot, a robot system, a motion control method and a motion control system, and belongs to the technical field of robots. Background Art

[0002] Quadruped robots have good complex terrain obstacle crossing performance, but flat roads are more common. Quadruped robots move slowly on such terrain and make a lot of noise. For this reason, researchers have developed wheeled-legged robots. Domestic and foreign research on quadruped robots has also promoted the development of four-wheeled-legged robots. In order to meet the combination of robot obstacle crossing ability and maneuverability, researchers have developed the following four types of wheeled-legged robots: 1) deformable wheeled-legged robots; 2) special-shaped wheeled-legged robots; 3) wheel-legged robots with separated wheels and feet; 4) wheel-legged robots with tandem wheels and feet.

[0003] Among them, there are many studies on wheel-leg tandem wheel-leg robots, for example: CN115454112A involves a mobile robot with fourteen degrees of freedom and two wheels and legs intersecting each other. Although it can realize two-mode motion, it is not a true four-wheeled robot, and it is only a structural improvement, and the structure is also relatively complex; CN117556559A involves a calculation method for inverse kinematics of a four-degree-of-freedom four-legged wheel-legged robot, but this method cannot coordinate multiple tasks in real time. In addition, CN119142435A involves a gait switching control method and system for a foot-type manipulator robot, but this system and method are for four-legged robots, not for four-wheeled four-legged robots, and cannot coordinate multiple tasks in real time, and requires different control models in mode switching.

[0004] In summary, existing quadruped / wheeled-legged robot systems still have problems such as low integration, single motion mode, and low control accuracy. Summary of the invention

[0005] In order to overcome the deficiencies in the prior art, the present invention provides a wheeled robot, a robot system, a motion control method and a motion control system.

[0006] A wheeled foot robot comprises four legs mounted on a trunk; the four legs are respectively a right hind leg, a left hind leg, a right front leg and a left front leg; each leg comprises a thigh, a calf, a wheeled foot and an integrated motor module; the integrated motor module controls hip abduction and adduction joint movements, hip flexion and extension joint movements, knee flexion and extension joint movements and wheeled foot movements;

[0007] The integrated motor module comprises:

[0008] The integrated hip joint motor is mounted on the torso, and its output controls the abduction and adduction movements of the legs;

[0009] The integrated thigh motor is installed at the output end of the integrated hip joint motor, and its output controls the flexion and extension movement of the thigh;

[0010] The integrated calf motor, whose output controls the calf flexion and extension movement, is installed at the output end of the integrated thigh motor. The output end of the integrated calf motor is connected to the shaft head, which is rotatably connected to one end of a connecting rod, and the other end of the connecting rod is rotatably connected to one end of the calf. The wheel foot is rotatably arranged at the other end of the calf; the integrated wheel-foot motor is installed on a wheel frame arranged at the other end of the calf, and its output controls the movement of the wheel foot.

[0011] The effect of this solution is that the wheel-foot robot is a four-legged four-wheel-foot robot, each leg and wheel foot has four degrees of freedom, and can realize three modes of movement: pure rolling mode, roller skating mode and pure walking mode.

[0012] A robot system, comprising electronic control hardware and a wheeled robot; the electronic control hardware comprises a host computer, an industrial computer, an IMU module, an emergency stop switch, a power supply and a relay; the industrial computer, the IMU module, the emergency stop switch, the power supply and the relay are all installed on the trunk, the host computer is connected to the industrial computer via network communication, the host computer is used to control the industrial computer to control the integrated motor module and to monitor the working process and working data of the wheeled robot; the IMU module is connected to the industrial computer for communication, the power supply supplies power to the industrial computer, the emergency stop switch and the relay; the relay controls the power supply to supply power to the integrated motor module, and the emergency stop switch is electrically connected to the relay; the industrial computer 1 is connected to the integrated motor module for communication via a field bus.

[0013] The effects of this solution are: the robot system has high integration, is easy to operate and control, has a simple interface, various indicators are easy to monitor, data feedback is timely, and can complete various tasks well.

[0014] A motion control method for a wheeled robot, based on the robot system, the motion control method comprises:

[0015] Step 1: Model Predictive Control Algorithm

[0016] The desired velocity or position of the robot's trunk is converted into a state trajectory and sent to the NMPC; the NMPC sends the optimal system state and control input value evaluated and optimized to the WBC;

[0017] Step 2: Whole body control algorithm

[0018] WBC uses the system state and control input of NMPC as a high-quality motion reference to calculate joint torques;

[0019] The torque calculated by WBC is used as a feedforward term, and the low-gain joint position and joint velocity pass through the PD controller. The torque calculated by the PD controller plus the feedforward term are sent to the integrated motor module.

[0020] The effect of this solution is that it can achieve global prediction and optimization, effectively reduce the complexity of each optimization calculation, and improve the response speed and real-time processing capability of the robot system.

[0021] A motion control system of a wheeled robot, used to implement the motion control method, the motion control system comprising:

[0022] A trajectory publishing module, which converts the desired velocity and position of the robot trunk into a state trajectory;

[0023] Predictive control module, which is used to build a model predictive control algorithm with the expected trajectory as input and the system state and control input as output, and optimize the system state and control input in real time;

[0024] The state estimation module is used to estimate the robot motion state by fusing sensor data;

[0025] The whole body control module is used to build a multi-task control algorithm with priorities or weights that takes the desired trajectory and motion state as input and the joint torque, joint velocity and joint position as output.

[0026] The effect of this solution is that it can efficiently coordinate multiple tasks, and can complete grasping, obstacle avoidance, posture adjustment, etc. The control system adopts a unified dynamic model. During the switching of motion modes, there is no need to switch the kinematic model, thus realizing the unification of various motion controls.

[0027] Compared with the prior art, the present invention has the following beneficial effects:

[0028] 1. The control system can efficiently coordinate multiple tasks. For example, while the robot is performing walking tasks, it can also complete tasks such as grasping, obstacle avoidance, and posture adjustment. NMPC provides global optimal planning, while WBC ensures real-time coordination between tasks.

[0029] 2. Global prediction and optimization through NMPC, combined with the WBC real-time coordinated control strategy, can effectively reduce the complexity of each optimization calculation and improve the system's response speed and real-time processing capabilities. In MPC mode, non-holonomic rolling friction constraints can be accurately estimated without adding other algorithms.

[0030] 3. The motion control algorithm adjusts the weights through relaxation optimization, which can adjust the weights of MPC and WBC according to different terrains, and is more adaptable to the environment.

[0031] 4. Compared with traditional quadruped robots, the wheeled-legged robot of the present application can realize three movement modes using its control system. For flat terrain, it switches to pure rolling mode for fast movement; for more complex terrain, it switches to roller skating mode, that is, it can lift its legs while rolling, thus ensuring the movement speed while overcoming obstacles; in complex terrain, it switches to pure walking mode for efficient obstacle overcoming.

[0032] In addition, the motion control system is designed using a unified dynamic model. During the motion mode switching process, there is no need to switch the kinematic model, which realizes the unification of motion control, reduces the switching cycle, and improves the response speed.

[0033] The following is a further description of the scheme of the application in conjunction with the accompanying drawings and embodiments: BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 is a three-dimensional diagram of the wheel-legged robot of the present invention;

[0035] Figure 2 This is a structural diagram of the wheel-foot robot leg of the present invention;

[0036] Figure 3 The schematic diagram of the hip abduction and adduction joint arrangement of the wheeled-leg robot;

[0037] Figure 4 The schematic diagram of the hip flexion and extension joint arrangement of the wheeled-foot robot;

[0038] Figure 5 The schematic diagram of the knee flexion and extension joint arrangement of the wheeled-leg robot;

[0039] Figure 6 This is a schematic diagram of wheel and foot arrangement;

[0040] Figure 7 It is the layout diagram of the electronic control hardware of the robot system;

[0041] Figure 8 It is the logic control diagram of the robot system's electronic control hardware;

[0042] Fig. 9 This is the upper computer interface diagram combined with the robot system;

[0043] Fig.10 This is the integrated motor control block diagram;

[0044] Fig.11 is a flow chart of the motion control method;

[0045] Fig.12 Schematic diagram of rolling constraint for moving point contact with fixed joint position;

[0046] Fig.13Schematic diagram of adding a six-degree-of-freedom virtual joint between the floating base and the world coordinate system;

[0047] Fig.14 Figure 2 is the control framework diagram for robot torso motion control. DETAILED DESCRIPTION

[0048] The embodiments of the technical solution of the present invention will be described in detail below in conjunction with the accompanying drawings. Unless otherwise specified, the technical terms or scientific terms used in this application are generally understood by those skilled in the art.

[0049] The existing quadruped / wheeled-legged robot systems have low integration, single motion modes, low control accuracy, and cannot meet and adapt to a variety of tasks.

[0050] In view of this, the following specific implementation methods are proposed:

[0051] Specific implementation method 1: refer to Figure 1-Figure 2 , this embodiment provides a wheeled robot, which includes four legs mounted on a trunk 13; the four legs are respectively a right hind leg 11, a left hind leg 12, a right front leg 14 and a left front leg 15;

[0052] Each leg comprises a thigh a, a calf b, a wheel foot c and an integrated motor module d; the integrated motor module controls the hip abduction and adduction joint movements, the hip flexion and extension joint movements, the knee flexion and extension joint movements and the wheel foot c movements;

[0053] The integrated motor module d comprises:

[0054] An integrated hip joint motor 31 is mounted on the trunk 13, and its output controls the abduction and adduction movement of the legs;

[0055] The integrated thigh motor 32 is installed at the output end of the integrated hip joint motor, and its output controls the flexion and extension movement of the thigh a;

[0056] The integrated calf motor 33, whose output controls the flexion and extension movement of the calf b, is installed at the output end of the integrated thigh motor. The output end of the integrated calf motor is connected to the shaft head 23, the shaft head 23 is rotatably connected to one end of the connecting rod 22, the other end of the connecting rod 22 is rotatably connected to one end of the calf b, and the wheel foot c is rotatably arranged at the other end of the calf b;

[0057] The integrated wheel-foot motor 34 is installed on the wheel frame arranged at the other end of the calf b, and its output controls the movement of the wheel-foot c.

[0058] The hip abduction and adduction joint motors of the right hind leg 1, the left hind leg 2, the right front leg 4, and the left front leg 5 are all fixed to the front and rear panels of the trunk 13 by screw connection, and the precise motion control of the robot is achieved through the motor drive between the leg joints.

[0059] Exemplarily, all integrated motor modules are quasi-direct drive integrated motor modules.

[0060] Further, refer to Figure 3 The integrated hip joint motor 31 is connected to the integrated thigh motor 32 through the HAA-HFE motor connector 311. When the integrated hip joint motor 31 is working, it directly drives the integrated thigh motor 32 to rotate, thereby driving the hip abduction and adduction joint;

[0061] Reference Figure 4 The integrated thigh motor 32 is connected to the integrated calf motor 33 through the HFE-KFE motor connector 321, and the output end of the integrated calf motor 33 is connected to the thigh 22. When the integrated thigh motor 32 works, it indirectly drives the thigh 2 to rotate, thereby realizing the driving of the hip flexion and extension joint.

[0062] Reference Figure 5 The shaft head 23 is fixedly connected to the integrated calf motor 33 (KFE motor) by screws, and then the shaft head 23 is connected to the connecting rod 22, and the connecting rod 22 is connected to the calf b, thereby forming a parallel four-bar linkage mechanism. When the integrated calf motor 33 (KFE motor) is working, the shaft head 23 rotates synchronously, and through the transmission of the connecting rod 22, the calf b and the shaft head 23 rotate in the same phase, thereby realizing the calf flexion movement driven by the knee joint.

[0063] Reference Figure 6 The integrated wheel-foot motor 34 (WFB motor) is fixed to the side of the calf b by screw connection, and the wheel c is coaxially connected with the driving shaft of the integrated wheel-foot motor 34 (WFB motor). When the integrated wheel-foot motor 34 (WFB motor) is working, it directly drives the wheel foot c to rotate, thereby realizing the driving of the forward / backward joint of the wheel.

[0064] Specific implementation method 2, refer to Figure 7 , providing a robot system, which includes electronic control hardware and the wheeled robot; the electronic control hardware includes a host computer, an industrial computer 1, an IMU module 2, an emergency stop switch 4, a power supply 7 and a relay 8;

[0065] The industrial computer 1, the IMU module 2, the emergency stop switch 4, the power supply 7 and the relay 8 are all installed on the trunk 13, and the host computer is connected to the industrial computer 1 through network communication. The host computer is used to control the industrial computer 1 to control the integrated motor module d and to monitor the working process and working data of the wheel-foot robot;

[0066] The IMU module 2 is connected to the industrial computer 1 for communication, and the power supply 7 supplies power to the industrial computer 1, the emergency stop switch 4 and the relay 8; the relay 8 controls the power supply 7 to supply power to the integrated motor module d, and the emergency stop switch 4 is electrically connected to the relay 8;

[0067] The industrial computer 1 is communicatively connected with the integrated motor module via a field bus.

[0068] As an example, Figure 6 The following scheme is provided as shown, the industrial computer 1 is fixed to the bottom plate of the trunk 13 by screw connection, the IMU module 2 is fixed to the IMU bracket 3 by screw connection; the IMU bracket 3 is fixed to the trunk 13 by screw connection; the power supply 7 (including but not limited to batteries) is restricted on the trunk 13 by the battery bracket 6; the relay 8 is fixed to the trunk 13 by screws and the fixing frame 5.

[0069] The schematic diagram of electronic control hardware logic control is as follows: Figure 8 As shown, it includes an operating computer (host computer), a trunk 13, a left front leg 15, a right front leg 14, a left hind leg 12 and a right hind leg 11. The host computer is interconnected with the industrial computer 1 through the TCP / IP protocol, the industrial computer 1 is interconnected with the integrated motor module through the CAN protocol, and the IMU module 2 is interconnected with the industrial computer 1 through the RS232 protocol. The host computer control software in the operating computer communicates with the industrial computer in the trunk through the TCP / IP protocol; the industrial computer 1 in the trunk 13 communicates with the integrated hip joint motor 31 (HAA motor), the integrated thigh motor 32 (HFE motor), the integrated calf motor 33 (KFE motor), and the integrated wheel-foot motor 34 (WFB motor) of the left front leg 15, the right front leg 14, the left hind leg 12, and the right hind leg 11 through the CAN protocol. In addition, the 24V power supply in the trunk 13 provides power for the entire system.

[0070] Practically, the IMU module 2 in the torso 13 communicates with the industrial computer 1 via the RS232 protocol, and feeds back the robot's roll, yaw, pitch angle, angular velocity, angular acceleration and other information to the industrial computer 1, which then feeds back to the control software in the operating computer via the TCP / IP protocol; the industrial computer 1 supplies power to the IMU5V to keep it operating normally.

[0071] The 24V power supply in the torso 13 directly supplies power to the industrial computer 1, the emergency stop switch 4 and the relay 8; the relay 8 controls the power supply to the HAA motor, HFE motor, KFE motor and WFB motor of the left front leg 15, the right front leg 14, the left hind leg 13 and the right hind leg 12; at the same time, the emergency stop switch 4 is connected to the relay 8 to control the power supply to the HAA motor, HFE motor, KFE motor and WFB motor corresponding to the left front leg 15, the right front leg 14, the left hind leg 13 and the right hind leg 12 to prevent the occurrence of emergencies.

[0072] The industrial computer 1 in the torso 13 communicates with the HAA motor, HFE motor, KFE motor, and WFB motor of the left front leg 15, the right front leg 14, the left hind leg 13, and the right hind leg 12 through the CAN protocol. After the upper computer control software in the operating computer communicates with the industrial computer 1 through the TCP / IP protocol and sends instructions, the industrial computer 1 sends the instructions to the HAA motor, HFE motor, KFE motor, and WFB motor corresponding to the left front leg 15, the right front leg 14, the left hind leg 13, and the right hind leg through the CAN bus. In addition, the HAA motor, HFE motor, KFE motor, and WFB motor corresponding to the left front leg, the right front leg, the left hind leg, and the right hind leg can also send the motor position, speed and other information to the industrial computer 1 through the CAN bus, and then the industrial computer 1 sends it to the upper computer control software in the operating computer through the TCP / IP protocol.

[0073] In particular, control system software is the implementation of control system algorithms in a suitable programming language and compiled into executable files that can be run by computers. The basic requirement for building control system software is to implement control system algorithms. The key point is the interaction with different hardware. The computing power, communication capabilities and ease of use of the hardware must be fully considered.

[0074] The host computer control program is designed using QT graphical interface program to facilitate the control and data collection of the four-legged wheeled robot. The lower computer and the upper computer use TCP / IP for data communication. The controller is programmed in C++, and multi-threading technology is used to complete the construction of the overall algorithm. It also has a common interface, including the simulation environment in the development situation and the real environment in the experimental test situation. The controller designed in this way is conducive to code maintenance and upgrading, and it is convenient to switch between real experiments and simulations.

[0075] like Fig. 9 As shown, the upper computer interface includes CAN bus communication, fuselage status monitoring interface, wheel end position monitoring interface, joint status and contact monitoring interface, upper and lower computer communication module interface, state switching module interface, MPC optimization monitoring module interface and expected instruction input module interface.

[0076] Specifically: the CAN bus communication part of the upper computer software can display whether the CAN bus communication is abnormal after the robot is powered on. If the communication is normal, the set indicator light will light up green; otherwise, the set indicator light will light up red.

[0077] The fuselage state monitoring part of the host computer software can provide real-time feedback of the information data from the IMU module 2, which can help the user adjust the fuselage posture. In addition, it can also provide feedback of relevant information of the state estimator to ensure the accuracy of control.

[0078] The wheel-end position monitoring part of the host computer software can provide real-time feedback of the three-dimensional position information and XYZ speed information of the four wheels, making it easier for users to monitor the status of the robot.

[0079] The joint status and contact monitoring part of the upper computer software can detect the angle information and angular velocity information of the four joints of the four legs, which is convenient for users to debug and monitor. In addition, indicator lights are provided to monitor whether the ends of each leg of the robot are touching the ground and whether the mode switching is correct.

[0080] The upper and lower computer communication module part of the upper computer software can control whether the upper and lower computer communications are connected.

[0081] The log input module of the host computer software can display the robot usage log information.

[0082] The MPC optimization monitoring module of the host computer software can monitor whether the MPC part of the control algorithm is calculated correctly, which is convenient for debugging.

[0083] The state switching module part of the upper computer software can realize the switching functions of enable, disable, stop movement, initialization, stand up, move, squat and other modes, and detect the enable / disable state.

[0084] The desired instruction input module of the host computer software can control the desired angular displacement and desired linear speed of the XYZ axis. The user inputs relevant data through this module to control the movement of the robot.

[0085] Furthermore, the integrated motor module is a quasi-direct drive integrated motor. As mentioned above, each leg corresponds to a HAA motor, an HFE motor, a KFE motor and a WFB motor. Each motor includes a motor body, a planetary reducer and a driver. The closed-loop control block diagram of each motor is as follows: Fig.10 As shown, the mixed control of torque, position and speed can be realized. The position loop and the speed loop are in parallel form, and the control strategy is position-speed dual-loop closed-loop control:

[0086] The output values ​​of the position loop and the speed loop are added to the feedforward torque T_ff to obtain the reference torque T_ref:

[0087] T_ref=kp·(P-θ)+kd·(V-dθ)+T_ff (1)

[0088] in:

[0089] K is the desired speed of the motor output shaft; P is the desired position of the motor output shaft;

[0090] kp is the position gain; kd is the speed gain;

[0091] θ is the current position of the motor output shaft, and dθ is the current speed of the motor output shaft;

[0092] The reference torque T_ref is converted by KT_OUT to obtain the reference current iqref, which then enters the subsequent current PI controller;

[0093] in;

[0094] iqref=T-ref / KT_OUT (2)

[0095] KT_OUT=Kt·GR (3)

[0096] Kt=1.5·NPP·flux (4)

[0097] iqref is the reference current;

[0098] GR is the motor reduction ratio;

[0099] Kt is the torque constant before deceleration;

[0100] NPP is the polar pair number;

[0101] Flux is the magnetic flux.

[0102] The above scheme can realize the motion control of the robot by calculating the reference current through the parameter output by the motor chip.

[0103] Specific implementation method three: refer to Fig.11 This embodiment provides a motion control method for a wheeled robot. Based on the robot system, the motion control method includes:

[0104] Step 1: Model Predictive Control Algorithm

[0105] The desired velocity or position of the robot's trunk is converted into a state trajectory and sent to the NMPC; the NMPC sends the optimal system state and control input value evaluated and optimized to the WBC;

[0106] Step 2: Whole body control algorithm

[0107] WBC uses the system state and control input of NMPC as a high-quality motion reference to calculate joint torques;

[0108] The torque calculated by WBC is used as a feedforward item, and the low-gain joint position and joint velocity pass through the PD controller. The torque calculated by the PD controller plus the feedforward item are sent to the integrated motor module (such as the HAA motor, HFE motor, KFE motor, and WFB motor corresponding to the left front leg 15, the right front leg 14, the left hind leg 13, and the right hind leg 12, respectively) to reduce the impact when the wheels contact and obtain better tracking performance.

[0109] Specifically, combined Fig.12 and Fig.13 , and introduce the model predictive control algorithm and the whole body control algorithm respectively.

[0110] Step 1: The specific implementation process of the model predictive control algorithm is as follows: NMPC uses a simplified single rigid body model to solve the optimal system state and input over a long time range. The general NMPC formula is to find an optimal control input over a time period T based on the latest state measurement x0. Its optimal control strategy is applied to the robot in each iteration until an updated strategy is available. The formula and constraints of NMPC can be written as follows:

[0111]

[0112] Where x(t) is the state vector, u(t) is the control input vector at time t, l(·) is the time-varying operating cost, φ(·) is the cost function at the terminal state x(t), (d) in equation (5) is the state-input equality constraint, (e) is the pure state equality constraint, and (f) is the inequality constraint. The three constraints are handled by the Lagrangian method, the penalty method, and the relaxation barrier function, respectively.

[0113] First, a single rigid dynamics model is performed: adding a complete model of wheels to the traditional quadruped robot model increases the number of NMPC states and inputs by one per leg. Fig.12 As shown, the wheels of the robot are modeled as moving point contacts with fixed joint positions. Using this novel approach, the optimization time of the NMPC does not increase compared to a legged robot despite the additional degrees of freedom. Coordinate system E i Fixed at the end point of the leg, coinciding with the point of contact with the ground. Fig.12 Shows the end effector speed End effector contact position and friction cone constraints

[0114] Based on the robot's wheel modeling as a mobile point contact with a fixed joint position, the following equations for the state vector x(t) and control input vector u(t) at time t are established:

[0115]

[0116] Among them, n j =12,n e =4 are the number of leg joints and legs excluding wheel feet, Θ, q j The elements represent the posture of the torso in Euler angles and the torso in the world coordinate system. The position in the middle, the angular velocity of the center of mass COM of the trunk, the linear velocity of the center of mass COM of the trunk and the positions of the leg joints except the wheels, λ E is the contact force at the end contact point, u j Indicates the speed of the thigh and leg joints excluding the wheels;

[0117] Calculate the cost function to solve for x(t) and u(t)

[0118] The control input vector u(t) is selected so that the system state x(t) can follow the desired external command while minimizing a predefined quadratic cost function, i.e., the time-varying operation cost function. This cost function reflects the performance indicators accumulated by the system during the entire operation process, including the trade-off between state deviation and control input, which is expressed as:

[0119]

[0120] Where Q is the state vector error The positive semidefinite Hessian of R is the control input vector error The positive definite Hessian, Q and R are used to balance the contribution of state error and control input; this form of time-varying operating cost can effectively capture the dynamic changes of system performance and provide a theoretical basis for the optimization of control systems. The error vector requires a reference value for the entire body. For example, the reference position and linear velocity of the torso are calculated using the external reference trajectory of the torso. The meaning of this optimization problem is: find u(t) under constraints. First, it is necessary to ensure that x(t) is as consistent as possible with the reference x(t). The larger the value on the Q diagonal, the larger the input of the corresponding position of u(t). However, u(t) cannot be too large, otherwise, the motor will continue to run at a higher level of torque, which will cause problems such as motor heating, reduced motor accuracy, shortened mechanism life, and high noise.

[0121] The weights are designed according to the reference range of x(t) and u(t), namely:

[0122]

[0123] Calculate the equation of motion:

[0124] (b) in formula (5) is based on the single rigid body dynamics model SRBD of a four-legged wheeled robot, which defines the SRBD model and the kinematics of each leg, while considering the wheels as mobile ground contacts with fixed rotation angles. SRBD assumes that the momentum of the limb joints is negligible compared to the inertia of the trunk.

[0125] The equation of motion of the SRBD model is expressed as follows:

[0126]

[0127] represents the rotation transformation matrix of the vector from the torso coordinate system to the world coordinate system. T(Θ) is the transformation matrix from the angular velocity in the torso coordinate system to the Euler angle derivative in the world coordinate system. T(Θ) can be expressed as:

[0128]

[0129] In the above formula, m is the total mass, g is the acceleration due to gravity, is the position of the wheel base contact point of the ith leg relative to the center of mass of the trunk in the world coordinate system, which is a function of the joint position. Therefore, the dynamic model requires the formula

[0130] Θ, p com , The reference value given for trajectory planning is obtained through the planning of the swing phase and the support phase. Then, by inverse kinematics solution, we can get q j ;

[0131] Calculate the scroll constraint:

[0132] The wheel-foot robot can move in the rolling direction when in contact. Therefore, the position of the end effector of the contact leg i, that is, the constraint of the wheel bottom contact point, can be expressed as:

[0133]

[0134] in, and n are the friction coefficients μ C The friction cone and the local surface normal in the world coordinate system. The rolling constraint in the figure describes the wheel end velocity In all directions. According to the dynamic model, in the world coordinate system, Projection of the end effector velocity in the direction perpendicular to the rolling direction Through forward kinematics calculation, in this formula, the contact leg is constrained so that the speed along the rolling direction is not constrained, that is, It can be seen that, unlike quadruped robots, four-legged wheeled robots must not only consider the effect of ground reaction force, but more importantly, the rolling constraints of the four wheels. Therefore, the single rigid body dynamics model of the four-legged wheeled robot also needs to consider the kinematics of the entire robot. Compared with the SRBD model that does not consider the robot's kinematics, this method can accurately estimate the rolling constraints without introducing unnecessary heuristic directions.

[0135] When the leg is in the air, the constraints switch to:

[0136]

[0137] The leg in the air follows a predefined swing trajectory c(t) in the direction normal to the terrain n, and the ground reaction force Set to zero.

[0138] Step 2: Whole body control algorithm

[0139] In the prioritized WBC, the solution space of low-priority tasks belongs to the solution space of high-priority tasks.

[0140] The first step is to establish multi-task whole body control

[0141] 1) Speed-level full body control:

[0142] If there is n t control tasks, and the workspace position of the i-th task is represented by x i Indicates that its Jacobian matrix and null space matrix are J i 、N i , where i starts counting from 1, the smaller i is, the higher the priority;

[0143] set up To achieve the joint space speed of the first i tasks in priority, when considering the i-th task, the first i-1 tasks can be combined into a large task A i-1 ,Right now:

[0144]

[0145] Right now:

[0146]

[0147] Its null space matrix is:

[0148]

[0149] Assume that at this time Known, It can be written as:

[0150]

[0151] Multiply both sides of formula (17) by To verify and In Task A i-1 The workspace is equivalent, try changing let Implement x i Control:

[0152]

[0153] therefore for:

[0154]

[0155] Will Substituting into formula (17) we get:

[0156]

[0157] In formula (20) Through this recursive formula, the joint space velocity that satisfies all tasks can be obtained

[0158] 2) Position-level whole-body control: Formula (20) provides a mapping from workspace velocity to joint space velocity. If the i-th task has a desired position And the current position x i Given that, define the workspace position tracking error:

[0159]

[0160] And the joint space position tracking error:

[0161]

[0162] Then i / T and q i / T is approximately the workspace velocity and joint space velocity of the robot when it adjusts from the current position to the desired position. Substituting it into formula (20) yields:

[0163]

[0164] Multiplying both sides of formula (23) by T yields:

[0165]

[0166] Formula (24) is the mapping from the workspace position to the joint space position. Through this formula, Δq is obtained ntThen, substituting into formula (22), we can get the expected joint space position

[0167] 3) Acceleration-level whole-body control: For a multi-task system, the relationship between the joint space acceleration and the workspace acceleration of the i-th task is:

[0168]

[0169] Formula (25) describes the mapping relationship between different accelerations. is the result read from the sensor and used as a known quantity in the calculation to combine the first i-1 tasks into a large task A i-1 , To achieve the joint space acceleration of the first i-1 workspace acceleration tasks, if Known, then It can be written as:

[0170]

[0171] Multiply both sides of formula (26) by Add at the same time Verifiable and In Task A i-1 The effect is the same on the workspace where you are located. Change let accomplish , substitute formula (26) into formula (25):

[0172]

[0173] therefore for:

[0174]

[0175] Substituting formula (28) into formula (27), we can obtain:

[0176]

[0177] In formula (29) Through this recursive formula, the joint space acceleration that satisfies all tasks can be obtained:

[0178] Step 2: Solving priority tasks

[0179] According to the importance of the tasks that the four-legged wheeled robot needs to complete, the priority of each task is divided as shown in the following table:

[0180] Table 1 Priority tasks used in WBC

[0181]

[0182] Among them, the floating base dynamic equation constraints, torque limits and friction cone constraints can be guaranteed by the equality constraints and inequality constraints in the optimization function, and the non-holonomic rolling constraints and the corresponding tracking tasks are solved in priority order:

[0183]

[0184] end

[0185]

[0186] q cmd is the desired position in the joint space, is the desired velocity in joint space, Expected acceleration in joint space;

[0187] The third step is to establish the dynamic equations of the floating base robot in the joint space

[0188] In the above, we have fully discussed the recursive formulas of position-level, velocity-level, and acceleration-level multi-task full-body control on fixed-base serial robots. However, these conclusions cannot be directly applied to four-legged wheeled robots. Because the trunk itself is movable in the world system, it is impossible to directly solve the position and posture of the rigid body that makes up the robot in the world system through only 16 joint parameters, and it is even more impossible to control it. According to the mobility of the base, robots can be divided into fixed-base robots and floating-base robots. Four-legged wheeled robots are a typical floating-base robot.

[0189] To solve the problem that floating base robots cannot use fixed base robot algorithms, such as Fig.13 As shown in the figure, a 6-DOF joint without entity is established between the world coordinate system and the floating base robot body coordinate system, plus the 16 entity joints of the wheeled robot, a total of 22 joints are included, and the joint space vector is:

[0190]

[0191] Among them, q b is a floating base joint, q j There are 16 solid joints;

[0192] Since q contains Euler angle terms, it is not suitable for direct differential processing, so the generalized velocity is defined as With generalized acceleration for:

[0193]

[0194] The dynamic equations of the floating base robot in the joint space are expressed in the following form:

[0195]

[0196] Among them, q is the generalized joint space vector, the first 6 parameters in q describe the virtual 6-DOF joint between the world coordinate system and the body coordinate system, and the other parameters of q are the angles (rotational joints) or lengths (mobile joints) of the remaining entity joints; M(q) is the joint space mass matrix, is the bias force matrix, which includes gravity, Coriolis force and other forces that are unrelated to joint acceleration, J S and f C are the contact Jacobian and contact force respectively, τ is the generalized joint torque, and since there is no entity 6-DOF joint (virtual 6-DOF joint) can not generate torque, the selection matrix S is used j to filter out possible non-zero items in the first six elements;

[0197] Relaxation optimization: The right side of the dynamics equation (33) of the floating base robot is the force on the system, including the contact force provided by the external environment and the joint force inside the system; the left side is the state of the system and the motion achieved under the action of these forces. If only the floating base with 6 degrees of freedom is considered, that is, only the first six rows of equation (33) are considered, the force and motion of the robot trunk are analyzed. Therefore, the floating base selection matrix is ​​defined as:

[0198]

[0199] Multiplying both sides of formula (33) by formula (34) yields:

[0200]

[0201] Let S f M, S f C, M fb , C fb ,

[0202] Due to the greater focus on control for a period of time in the future, and More emphasis is placed on ensuring the priority between different tasks. In order to make the wheel bottom reaction force and the generalized acceleration satisfy the trunk dynamic equation (35), the wheel bottom reaction force and generalized acceleration Add a slack variable each:

[0203]

[0204] in, With δ f It is a slack variable added to the derivation results of the model predictive control algorithm and the whole body control algorithm. The smaller these two variables are, the better. Therefore, the following constrained optimization problem is constructed:

[0205]

[0206] in, is the wheel bottom reaction force of the ith supporting leg, Q1 and Q2 are the whole body control weight matrix and the model predictive control weight matrix, which means whether the control system trusts the whole body control more or the model predictive control more;

[0207] Since the optimization problem described by formula (37) is small in scale, the C++-based quadratic optimization library QuadProg++ is used to transform formula (37) into a standard format acceptable to QuadProg++ and construct the optimization variables:

[0208]

[0209] Then the optimization function expression in formula (37) can be converted to:

[0210]

[0211] According to the kinetic formula:

[0212]

[0213] Through the feed-forward torque τ j The torque calculated by the PD controller Add together to get the desired torque τ cmd ,

[0214]

[0215] The desired torque is sent to the integrated motor module to achieve motion state control.

[0216] Specific implementation method 4: This implementation method provides a motion control system of a wheeled robot for implementing the motion control method. The motion control system includes:

[0217] A trajectory publishing module, which converts the desired velocity and position of the robot trunk into a state trajectory;

[0218] Predictive control module, which is used to build a model predictive control algorithm with the expected trajectory as input and the system state and control input as output, and optimize the system state and control input in real time;

[0219] The state estimation module is used to estimate the robot motion state by fusing sensor data;

[0220] The whole body control module is used to construct a multi-task control algorithm with priorities or weights that takes the desired trajectory and motion state as input and outputs joint torque, joint velocity and joint position.

[0221] Fig.14 This is a block diagram of the algorithm of the control system of a four-legged wheeled robot. The control system algorithm can be divided into several modules, namely: a finite state machine for the behavior management of the four-legged wheeled robot; a terrain estimator for estimating the terrain; a contact estimator for judging whether the wheel end is in contact with the ground; a gait scheduler for switching between the support leg and the swing leg; a trajectory planner for the support phase and the swing phase, which constitutes the trajectory publishing module;

[0222] A center of mass trajectory generator receiving a host computer and generating a center of mass trajectory;

[0223] A state estimator that estimates the robot state by fusing multiple sensor data constitutes a state estimation module;

[0224] A nonlinear model predictive controller that takes into account the whole body motion state constitutes the predictive control module;

[0225] A multi-task whole-body controller with priority or weight constitutes a whole-body control module; a set of rigid body dynamics algorithms for calculating floating substrates. In the motion control system, except for the model predictive control algorithm, which runs at a frequency of 10Hz, the running frequencies of other algorithms are all 500Hz. The control period of the model predictive control is represented by ΔT, ΔT = 0.1s; the control period of other algorithms is represented by Δt, Δt = 0.002s.

[0226] The present invention has been disclosed as above with preferred implementation cases, but it is not used to limit the present invention. Any technician familiar with the profession can make slight changes or modifications to equivalent implementation cases with equivalent changes by using the above-disclosed structures and technical contents without departing from the scope of the technical solution of the present invention, which still fall within the scope of the technical solution of the present invention.

Claims

1. A wheeled robot, comprising four legs mounted on a trunk (13); the four legs are respectively a right hind leg (11), a left hind leg (12), a right front leg (14) and a left front leg (15); Features: Each leg comprises a thigh (a), a calf (b), a wheel foot (c) and an integrated motor module (d); the integrated motor module (d) controls the movement of the hip abduction and adduction joint, the movement of the hip flexion and extension joint, the movement of the knee flexion and extension joint and the movement of the wheel foot (c); The integrated motor module (d) comprises: An integrated hip joint motor (31) is mounted on the trunk (13), and its output controls the abduction and adduction movement of the legs; An integrated thigh motor (32) is installed at the output end of the integrated hip joint motor, and its output controls the flexion and extension movement of the thigh (a); An integrated calf motor (33), whose output controls the flexion and extension movement of the calf (b), is installed at the output end of the integrated thigh motor, the output end of the integrated calf motor (33) is connected to the shaft head (23), the shaft head (23) is rotatably connected to one end of the connecting rod (22), the other end of the connecting rod (22) is rotatably connected to one end of the calf (b), and the wheel foot (c) is rotatably arranged at the other end of the calf (b); The integrated wheel-foot motor (34) is installed on the wheel frame arranged at the other end of the calf (b), and its output controls the movement of the wheel-foot (c).

2. A robot system, characterized in that: It comprises electronic control hardware and the wheeled robot as claimed in claim 1; the electronic control hardware comprises a host computer, an industrial computer (1), an IMU module (2), an emergency stop switch (4), a power supply (7) and a relay (8); The industrial computer (1), the IMU module (2), the emergency stop switch (4), the power supply (7) and the relay (8) are all installed on the trunk (13); the host computer is connected to the industrial computer (1) through network communication; the host computer is used to control the industrial computer (1) to control the integrated motor module (d) and to monitor the working process and working data of the wheeled robot; The IMU module (2) is communicatively connected with the industrial computer (1), and the power supply (7) supplies power to the industrial computer (1), the emergency stop switch (4) and the relay (8); the relay (8) controls the power supply (7) to supply power to the integrated motor module (d), and the emergency stop switch (4) is electrically connected to the relay (8); The industrial control computer (1) is communicatively connected with the integrated motor module (d) via a field bus.

3. The robot system according to claim 2, characterized in that: The upper computer interface includes a fuselage status monitoring interface, a wheel end position monitoring interface, a joint status and contact monitoring interface, an upper and lower computer communication module interface, a status switching module interface, an MPC optimization monitoring module interface and an expected instruction input module interface.

4. The robot system according to claim 2, characterized in that: The integrated motor module (d) is a quasi-direct drive integrated motor, which can realize the mixed control of torque, position and speed. Its control strategy is position-speed dual-loop closed-loop control: The output values ​​of the position loop and the speed loop are added to the feedforward torque T_ff to obtain the reference torque T_ref: T_ref=kp·(P-θ)+kd·(V-dθ)+T_ff (1) in: K is the desired speed of the motor output shaft; P is the desired position of the motor output shaft; kp is the position gain; kd is the speed gain; θ is the current position of the motor output shaft, and dθ is the current speed of the motor output shaft; The reference torque T_ref is converted by KT_OUT to obtain the reference current iqref, which then enters the subsequent current PI controller; in; iqref=T-ref / KT_OUT (2) KT_OUT=Kt·GR (3) Kt=1.5·NPP·flux (4) iqref is the reference current; GR is the motor reduction ratio; Kt is the torque constant before deceleration; NPP is the polar pair number; Flux is the magnetic flux.

5. The robot system according to claim 2, characterized in that: The host computer is interconnected with the industrial computer (1) via the TCP / IP protocol, the industrial computer (1) is interconnected with the integrated motor module (d) via the CAN protocol, and the IMU module (2) is interconnected with the industrial computer (1) via the RS232 protocol.

6. A motion control method for a wheeled robot, characterized in that: Based on the robot system according to any one of claims 2 to 5, the motion control method comprises: Step 1: Model Predictive Control Algorithm The desired velocity or position of the robot's trunk is converted into a state trajectory and sent to the NMPC; the NMPC sends the optimal system state and control input value evaluated and optimized to the WBC; Step 2: Whole body control algorithm WBC uses the system state and control input of NMPC as a high-quality motion reference to calculate joint torques; The torque calculated by WBC is used as a feedforward term, and the low-gain joint position and joint velocity pass through the PD controller. The torque calculated by the PD controller plus the feedforward term are sent to the integrated motor module.

7. The motion control method of the wheeled robot according to claim 6, characterized in that: The NMPC algorithm implementation process for evaluating and optimizing the system state and control input quantity described in step 1 is as follows: S1. Single rigid body dynamics modeling Based on the robot's wheel modeling as a mobile point contact with a fixed joint position, the following equations for the state vector x(t) and control input vector u(t) at time t are established: Among them, n j =12,n e =4 are the number of leg joints and legs excluding wheel feet, Θ, q j The elements represent the posture of the torso in Euler angles, the position of the torso in the world coordinate system, the angular velocity of the center of mass of the torso, the linear velocity of the center of mass of the torso, and the positions of the leg joints except the wheels, λ E is the contact force at the end contact point, u j Indicates the speed of the thigh and leg joints excluding the wheels; S2. Calculate the cost function to solve x(t) and u(t) The control input vector u(t) is selected so that the system state x(t) follows the desired external command while minimizing a predefined quadratic cost function, also known as the time-varying operating cost function, which is expressed as: Where Q is the state vector error The positive semidefinite Hessian of R is the control input vector error The positive definite Hessian of , Q and R are used to balance the contributions of the state error and the control input; The weights are designed according to the reference range of x(t) and u(t), namely:

8. The motion control method of the wheeled robot according to claim 7, characterized in that: It also includes calculating the equations of motion and calculating the rolling constraints before establishing the state vector x(t) and the control input vector u(t) equations; 1) The process of calculating the equation of motion is: NMPC is written as follows: Where x(t) is the state vector, u(t) is the control input vector at time t, l(·) is the time-varying operating cost, φ(·) is the cost function at the terminal state x(t), (d) in equation (5) is the state-input equality constraint, (e) is the pure state equality constraint, and (f) is the inequality constraint. These three constraints are handled by the Lagrangian method, the penalty method, and the relaxation barrier function, respectively. (b) in formula (5) is based on the SRBD model of a four-legged wheeled robot. The equation of motion of the SRBD model is expressed as follows: represents the rotation transformation matrix of the vector from the torso coordinate system to the world coordinate system. T(Θ) is the transformation matrix from the angular velocity in the torso coordinate system to the Euler angle derivative in the world coordinate system. T(Θ) can be expressed as: In the above formula, m is the total mass, g is the acceleration due to gravity, is the position of the wheel base contact point of the ith leg relative to the center of mass of the trunk in the world coordinate system, which is a function of the joint position; Θ, p com , The reference value given for trajectory planning is obtained through the planning of the swing phase and the support phase. Then, by inverse kinematics solution, we can get q j ; 2) The process of calculating the rolling constraint is: The wheel-foot robot can move in the rolling direction when in contact. Therefore, the position of the end effector of the contact leg i, that is, the constraint of the wheel bottom contact point, can be expressed as: in, and n are the friction coefficients μ C The friction cone and the local surface normal in the world coordinate system. According to the dynamic model, in the world coordinate system, Projection of the end effector velocity in the direction perpendicular to the rolling direction Through forward kinematics calculation, the contact leg is constrained so that the velocity along the rolling direction is unconstrained, that is, When the leg is in the air, the constraints switch to: The leg in the air follows a predefined swing trajectory c(t) in the direction normal to the terrain n, and the ground reaction force Set to zero.

9. The motion control method of the wheeled robot according to claim 6 or 7, characterized in that: The whole body control algorithm implementation process of step 2 is: The first step is to establish multi-task whole body control 1) Speed-level full body control: If there is n t control tasks, and the workspace position of the i-th task is represented by x i Indicates that its Jacobian matrix and null space matrix are J i 、N i , where i starts counting from 1, the smaller i is, the higher the priority; set up To achieve the joint space speed of the first i tasks in priority, when considering the i-th task, the first i-1 tasks can be combined into a large task A i-1 ,Right now: Right now: Its null space matrix is: Assume that at this time Known, It can be written as: Multiply both sides of formula (17) by To verify and In Task A i-1 The workspace is equivalent, try changing let Implement x i Control: therefore for: Will Substituting into formula (17) we can get: In formula (20) Through this recursive formula, the joint space velocity that satisfies all tasks can be obtained 2) Position-level whole-body control: Formula (20) provides a mapping from workspace velocity to joint space velocity. If the i-th task has an expected position And the current position x i Given that, define the workspace position tracking error: And the joint space position tracking error: Then i / T and q i / T is approximately the workspace velocity and joint space velocity of the robot when it adjusts from the current position to the desired position. Substituting it into formula (20) yields: Multiplying both sides of formula (23) by T yields: Formula (24) is the mapping from the workspace position to the joint space position. Through this formula, Δq is obtained nt Then, substituting into formula (22), we can get the expected joint space position 3) Acceleration-level whole-body control: For a multi-task system, the relationship between the joint space acceleration and the workspace acceleration of the i-th task is: Formula (25) describes the mapping relationship between different accelerations. is the result read from the sensor and used as a known quantity in the calculation to combine the first i-1 tasks into a large task A i-1 , To achieve the joint space acceleration of the first i-1 workspace acceleration tasks, if Known, then It can be written as: Multiply both sides of formula (26) by Add at the same time Verifiable and In Task A i-1 The effect is the same on the workspace where you are located. Change let accomplish , substitute formula (26) into formula (25): therefore for: Substituting formula (28) into formula (27), we can obtain: In formula (29) Through this recursive formula, the joint space acceleration that satisfies all tasks can be obtained: Step 2: Solving priority tasks According to the importance of the tasks that the four-legged wheeled robot needs to complete, the priority of each task is divided; among them, the floating base dynamic equation constraints, torque limits and friction cone constraints can be guaranteed by the equality constraints and inequality constraints in the optimization function, and the non-holonomic rolling constraints and the corresponding tracking tasks are solved in priority order: q cmd is the desired position in the joint space, is the desired velocity in joint space, Expected acceleration in joint space; The third step is to establish the dynamic equations of the floating base robot in the joint space 1) Establish a 6-DOF joint without entity between the world coordinate system and the floating base robot body coordinate system, plus the 16 entity joints of the wheeled robot, a total of 22 joints, and its joint space vector is: Among them, q b is a floating base joint, q j There are 16 solid joints; Since q contains Euler angle terms, it is not suitable for direct differential processing, so the generalized velocity is defined as With generalized acceleration for: The dynamic equations of the floating base robot in the joint space are expressed in the following form: Among them, q is the generalized joint space vector, the first 6 parameters in q describe the virtual 6-DOF joint between the world coordinate system and the body coordinate system, and the other parameters of q are the angles or lengths of the remaining entity joints; M(q) is the joint space mass matrix, is the bias force matrix, J S and f C are the contact Jacobian and contact force respectively, τ is the generalized joint torque, and since a 6-DOF joint without a solid body cannot generate torque, the selection matrix S is used j to filter out possible non-zero items in the first six elements; 2) Analyze the force and movement of the robot trunk Define the floating base selection matrix: Multiplying both sides of formula (33) by formula (34) yields: Let S f M, S f C, M fb , C fb , Reaction force to wheel bottom and generalized acceleration Add a slack variable each: in, With δ f It is a slack variable added to the derivation results of the model predictive control algorithm and the whole body control algorithm, so the following constrained optimization problem is constructed: in, is the wheelbase reaction force of the ith supporting leg, Q1 and Q2 are the whole body control weight matrix and the model predictive control weight matrix, Using the C++ based quadratic optimization library QuadProg++, formula (37) is transformed into a standard format acceptable to QuadProg++ and the optimization variables are constructed: Then the optimization function expression in formula (37) can be converted to: According to the kinetic formula: Through the feed-forward torque τ j The torque calculated by the PD controller Add together to get the desired torque τ cmd , The desired torque is sent to the integrated motor module to achieve motion state control.

10. A motion control system for a wheeled robot, characterized in that: Used to implement the motion control method according to any one of claims 6 to 9, the motion control system comprises: A trajectory publishing module, which converts the desired velocity and position of the robot trunk into a state trajectory; Predictive control module, which is used to build a model predictive control algorithm with the expected trajectory as input and the system state and control input as output, and optimize the system state and control input in real time; The state estimation module is used to estimate the robot motion state by fusing sensor data; The whole body control module is used to build a multi-task control algorithm with priorities or weights that takes the desired trajectory and motion state as input and the joint torque, joint velocity and joint position as output.

Images