Multi-legged Robot Slope Travel Control Method and System Based on Bimodal Gaussian Fusion

Abstract

The present invention proposes a method and system for controlling a multi-legged robot to travel on a slope based on the fusion of bimodal Gaussian, belonging to the technical field of legged robot control; it includes constructing a foot-end plane equation by using the three-dimensional coordinates of the foot end at two touchdown moments of the multi-legged robot, and obtaining the pitch angle and the roll angle; performing feedforward calculation based on the plane angle memory update algorithm to obtain the feedforward pitch angle and the feedforward roll angle; calculating the joint angle difference based on the joint angle feedback algorithm, and then determining the real-time correction amount of the pitch angle; using the bimodal Gaussian fusion method to determine the final pitch angle of the multi-legged robot; converting the final pitch angle and the feedforward roll angle, combined with the gait and speed commands input by the user, into joint torques, desired joint positions, and desired joint speeds to control the multi-legged robot to travel on the slope. The present invention can quickly respond to terrain mutations on the basis of ensuring the lightweight of the control architecture, so as to control the multi-legged robot to travel stably on the rugged slope.

Description

Technical Field

[0001] The present invention belongs to the technical field of legged robot control, and particularly relates to a multi-legged robot slope walking control method and system based on bimodal Gaussian fusion. Background Art

[0002] The statements in this part merely provide background technical information related to the present invention and do not necessarily constitute prior art.

[0003] In recent years, multi-legged robots have become an important research direction in the field of robotics due to their excellent terrain adaptability and motion flexibility. With the expansion of application scenarios from structured environments to complex unstructured terrains, the limitations of traditional wheeled / caterpillar motion mechanisms have become increasingly prominent, while the bionic motion mechanism based on discrete foot-end touchdown provides an innovative solution for moving on irregular terrains. With the continuous progress of technology, simply achieving stable walking on flat ground can no longer meet the high-order requirements for autonomy and robustness in actual application scenarios. Therefore, the research on adaptive control technology in complex terrains - especially slope environments - is particularly important. Moreover, the current technological evolution shows a trend from static gait to dynamic gait, which poses higher technical requirements for motion control algorithms.

[0004] Currently, the research on current dynamic gait slope control technology mainly adopts control methods based on model-free deep reinforcement learning and environment perception based on 3D lidar. However, these control methods all face some technical problems, such as:

[0005] (1) Control method based on model-free deep reinforcement learning: The domain difference between simulation and reality easily leads to the decay of policy transfer performance. Minor deviations in terrain physical parameters may cause catastrophic failures, and the black-box decision-making mechanism results in the lack of system interpretability. Therefore, when foot-end slipping or attitude mutation occurs, it is difficult to trace and debug this control method in a timely manner.

[0006] (2) Control method based on 3D lidar environment perception: By constructing a terrain digital elevation model through point cloud registration and then performing offline gait planning, this leads to an exponential growth in its computational complexity, and the average delay from environment scanning to gait generation is difficult to meet the real-time control requirements of dynamic gait. Summary of the Invention

[0007] To overcome the deficiencies of the above-mentioned prior art, the present invention provides a multi-legged robot slope walking control method and system based on bimodal Gaussian fusion, which can quickly respond to terrain mutations on the basis of ensuring the lightweight of the control architecture, so as to control the multi-legged robot to walk stably on rough slopes.

[0008] To achieve the above object, one or more embodiments of the present invention provide the following technical solutions:

[0009] The first aspect of the present invention provides a method for controlling the movement of a multi-legged robot on a slope based on bimodal Gaussian fusion.

[0010] The method for controlling the movement of a multi-legged robot on a slope based on bimodal Gaussian fusion includes:

[0011] Obtain the three-dimensional coordinates of the foot end of the multi-legged robot at two touchdown moments, and construct the foot end plane equation; obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation;

[0012] Based on the plane angle memory update algorithm including the attenuation coefficient, perform feedforward calculation on the obtained pitch angle and roll angle to obtain the feedforward pitch angle and feedforward roll angle of the multi-legged robot on the slope;

[0013] Calculate the joint angle difference based on the joint angle feedback algorithm, and determine the real-time correction amount of the pitch angle of the multi-legged robot on the slope according to the difference calculation result;

[0014] Adopt the bimodal Gaussian fusion method to determine the final pitch angle of the multi-legged robot, that is: if the multi-legged robot is at any one of the two touchdown moments, use the feedforward pitch angle as the final pitch angle; otherwise, use the value obtained by weighted fusion of the feedforward pitch angle and the real-time correction amount of the pitch angle as the final pitch angle;

[0015] Combine the obtained final pitch angle and feedforward roll angle with the gait and speed commands input by the user, and convert them into joint torques, desired joint positions, and desired joint speeds through NMPC-WBC calculation to control the movement of the multi-legged robot on the slope.

[0016] Further, the two touchdown moments of the multi-legged robot are: at the 0 moment and the T / 2 moment when all legs of the multi-legged robot touch the ground simultaneously within a complete gait cycle T in the trot gait.

[0017] Further, obtaining the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation includes: setting a yaw angle threshold and a terrain flatness threshold, and when the yaw angle of the multi-legged robot is less than the set yaw angle threshold and the terrain flatness is less than the set terrain flatness threshold, use the first composite control strategy combined with the foot end plane equation to calculate the pitch angle and roll angle; otherwise, use the second composite control strategy combined with the foot end plane equation to calculate the pitch angle and roll angle.

[0018] Further, the first composite control strategy is: regard the direction of the multi-legged robot when going uphill as facing the slope directly, that is, keep the yaw angle zero, and calculate the pitch angle and roll angle according to the foot end plane equation; the second composite control strategy is: first, use the IMU to read the yaw angle and calculate the slope normal vector, and then calculate the pitch angle and roll angle according to the foot end plane equation.

[0019] Further, the planar angle memory update algorithm is as follows:

[0020] ;

[0021] Wherein, and respectively represent the feedforward roll angle and the feedforward pitch angle of the updated multi-legged robot on the slope, and respectively represent the roll angle and the pitch angle of the multi-legged robot on the slope obtained through the foot-end plane equation at the current moment, and respectively represent the roll angle and the pitch angle of the multi-legged robot on the slope obtained through the foot-end plane equation saved in the previous gait cycle; represents the attenuation coefficient.

[0022] Further, based on the joint angle feedback algorithm, the joint angle difference is calculated, including: calculating the difference between the sum of the front knee joint angles and the sum of the rear knee joint angles, and generating a real-time correction amount of the pitch angle through a PID controller.

[0023] Further, the weighted fusion algorithm for using the value after weighted fusion of the feedforward pitch angle and the real-time correction amount of the pitch angle as the final pitch angle is as follows:

[0024] ;

[0025] In the formula, represents the final pitch angle of the multi-legged robot on the slope after weighted fusion, represents the feedforward pitch angle of the multi-legged robot on the slope updated by the planar angle memory update algorithm, represents the real-time correction amount of the pitch angle generated by the PID controller; and respectively represent the weights corresponding to and ; wherein, The value of gradually decays as it moves away from the 0 moment and the T / 2 moment within the gait cycle T of the multi-legged robot.

[0026] The second aspect of the present invention provides a multi-legged robot slope traveling control system based on bimodal Gaussian fusion.

[0027] The multi-legged robot slope traveling control system based on bimodal Gaussian fusion includes:

[0028] The touchdown moment detection module is configured to: obtain the three-dimensional coordinates of the foot end of the multi-legged robot at two touchdown moments, and construct the foot end plane equation; obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation;

[0029] The feedforward module is configured to: perform feedforward calculations on the obtained pitch angle and roll angle based on a plane angle memory update algorithm including an attenuation coefficient to obtain the feedforward pitch angle and feedforward roll angle of the multi-legged robot on the slope;

[0030] The joint angle feedback module is configured to: calculate the joint angle difference based on the joint angle feedback algorithm, and determine the real-time pitch angle correction amount of the multi-legged robot on the slope according to the difference calculation result;

[0031] The bimodal Gaussian fusion control module is configured to: use the bimodal Gaussian fusion method to determine the final pitch angle of the multi-legged robot, that is: if the multi-legged robot is at any one of the two touchdown moments, use the feedforward pitch angle as the final pitch angle; otherwise, use the value obtained by weighted fusion of the feedforward pitch angle and the real-time pitch angle correction amount as the final pitch angle;

[0032] The travel control module is configured to: combine the obtained final pitch angle and feedforward roll angle with the gait and speed commands input by the user, and convert them into joint torques, desired joint positions, and desired joint speeds after NMPC-WBC solution to control the multi-legged robot to travel on the slope.

[0033] The third aspect of the present invention provides a computer-readable storage medium, on which a program is stored, and when the program is executed by a processor, it implements the steps in the method for controlling the travel of a multi-legged robot on a slope based on bimodal Gaussian fusion as described in the first aspect of the present invention.

[0034] The fourth aspect of the present invention provides an electronic device, including a memory, a processor, and a program stored on the memory and executable on the processor. When the processor executes the program, it implements the steps in the method for controlling the travel of a multi-legged robot on a slope based on bimodal Gaussian fusion as described in the first aspect of the present invention.

[0035] The above one or more technical solutions have the following beneficial effects:

[0036] (1) The present invention combines the foot-end plane equation and proposes a composite strategy of dual-method fusion, which can quickly and accurately estimate the terrain pitch angle and roll angle. At the same time, the present invention combines the adaptive attenuation coefficient in the plane angle memory update algorithm to dynamically fuse the current estimated angle and historical angle data, and uses the obtained attitude angle as feedforward control. Combining the double-peak Gaussian weight, at the moment when all legs touch the ground, it completely relies on the feedforward attitude angle generated by terrain estimation. During the swing phase, the difference between the front and rear knee joint angles is calculated in real time through the joint rotation angle feedback module, and the pitch angle correction amount is generated by the PID controller to form a transparent decision-making link. Therefore, the present invention can effectively compensate for the attitude deviation caused by terrain dynamic changes and external disturbances, achieve a rapid response to terrain mutations, and on the basis of avoiding the attitude instability problem caused by the lag of historical data in the prior art, significantly improve the interpretability.

[0037] (2) The present invention realizes the lightweight and real-time optimization of the control architecture, that is: through the construction of the foot-end plane equation (based on the coordinates at the moment when multiple feet touch the ground simultaneously) and the analytical design of the double-peak Gaussian function in the present invention, complex terrain modeling and iterative optimization calculations can be avoided, the calculation time-consuming of the algorithm can be reduced, and thus the real-time control requirements for multi-legged robots can be met. At the same time, the feedforward-feedback phased cooperation mechanism further reduces the conflict of redundant control instructions and can effectively reduce the system resource occupancy rate.

[0038] The advantages of the additional aspects of the present invention will be partly given in the following description, partly will become obvious from the following description, or will be understood through the practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The specification drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention.

[0040] Figure 1 It is a flowchart of the multi-legged robot slope traveling control method based on double-peak Gaussian fusion in Embodiment 1 of the present invention.

[0041] Figure 2 It is a gait schematic diagram of the trot gait in Embodiment 1 of the present invention.

[0042] Figure 3 It is a schematic diagram of the coordinate system of the multi-legged robot in Embodiment 1 of the present invention.

[0043] Figure 4 It is a schematic diagram of the roll angle adjustment of the multi-legged robot when climbing a slope in Embodiment 1 of the present invention.

[0044] Figure 5 It is a schematic diagram of the pitch angle adjustment of the multi-legged robot when climbing a slope in Embodiment 1 of the present invention.

[0045] Figure 6 This is a schematic diagram of the control relationship of the multi-legged robot slope traveling control system based on bimodal Gaussian fusion in the second embodiment of the present invention. Detailed implementation manners

[0046] It should be noted that the following detailed descriptions are all exemplary and are intended to provide further explanations of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs.

[0047] It should be noted that the terms used herein are only for describing specific implementation manners and are not intended to limit the exemplary embodiments according to the present invention.

[0048] Without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0049] The overall idea proposed by the present invention: The present invention provides a multi-legged robot slope traveling control method based on bimodal Gaussian fusion. This method combines the trot gait characteristics, and all legs of the multi-legged robot can be regarded as supporting legs at the 0 moment and near the T / 2 moment in each gait cycle T. A plane equation is constructed through the foot end positions. At the same time, a hybrid strategy based on the fusion of two methods is proposed for fast and high-precision terrain pitch angle and roll angle estimation; and a plane angle memory update formula is designed, and the obtained attitude angles are used as feedforward control. When away from these two moments, a knee joint angle feedback control is added to correct the pitch angle of the multi-legged robot in real time. Finally, by designing a bimodal Gaussian function to cover two touchdown events, calculating the weights, and combining the terrain estimation (feedforward control) and the knee joint angle feedback (feedback control) through weighting, fast adaptation to the terrain is achieved, with higher robustness.

[0050] Embodiment 1

[0051] This embodiment discloses a multi-legged robot slope traveling control method based on bimodal Gaussian fusion.

[0052] As Figure 1 shown, the multi-legged robot slope traveling control method based on bimodal Gaussian fusion includes:

[0053] Step S1: Obtain the three-dimensional coordinates of the foot ends of the multi-legged robot at two touchdown moments, and construct a foot end plane equation; obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation;

[0054] Step S2: Based on a plane angle memory update algorithm including an attenuation coefficient, perform feedforward calculation on the obtained pitch angle and roll angle to obtain the feedforward pitch angle and feedforward roll angle of the multi-legged robot on the slope;

[0055] Step S3, performing joint angle difference calculation based on the joint angle feedback algorithm, and determining the real-time correction amount of the pitch angle of the multi-legged robot on the slope according to the difference calculation result;

[0056] Step S4, using a bimodal Gaussian fusion method to determine the final pitch angle of the multi-legged robot, that is, if the multi-legged robot is at any one of the two ground contact moments, the feedforward pitch angle is used as the final pitch angle; otherwise, the value obtained by weighted fusion of the feedforward pitch angle and the real-time correction amount of the pitch angle is used as the final pitch angle;

[0057] Step S5: The obtained final pitch angle and feedforward roll angle are combined with the gait and speed instructions input by the user, and converted into joint torque, expected joint position and expected joint speed after NMPC-WBC solution to control the multi-legged robot to move on the slope.

[0058] Based on the above process, the present invention can quickly respond to sudden changes in terrain while ensuring the lightweight control architecture, so as to control the multi-legged robot to move stably on a rugged slope. To facilitate the understanding of the technical solution of the present invention, the specific implementation steps in the technical solution of the present invention are further explained and illustrated below.

[0059] In step S1, the three-dimensional coordinates of the foot end of the multi-legged robot at two ground contact moments are obtained, and a foot end plane equation is constructed; the pitch angle and roll angle of the multi-legged robot on the slope are obtained according to the foot end plane equation.

[0060] The moment t=0 and the moment t=T / 2 in a trot gait cycle T are regarded as all the legs of the multi-legged robot touching the ground at the same time. The three-dimensional coordinates of the foot ends of all the touching legs at these two moments are obtained to construct the foot end plane equation; and the advantages of two methods for estimating the pitch angle and roll angle of the terrain (i.e., the dual-method composite control strategy: the first composite control strategy and the second composite control strategy) are combined to achieve fast and accurate estimation of the pitch angle and roll angle in the slope terrain. Among them, the trot gait is a common gait of quadruped robots, that is, the diagonal trotting gait; its characteristic is that the two diagonal legs of the robot (such as the left front leg and the right hind leg) move synchronously, and then the other pair of diagonal legs (the right front leg and the left hind leg) move alternately; this gait strikes a balance between speed, stability and energy efficiency.

[0061] like Figure 2 As shown in the figure, in the trot gait, the left front leg and the right hind leg are swing legs before T / 2, and the right front leg and the left hind leg are swing legs after T / 2. At time T, the left front leg and the right hind leg will become swing legs again. This time T is a complete gait cycle. In more popular terms: the time it takes for all legs to start from a touchdown / swing mode, go through a full round of movement, and return to this mode is called a complete gait cycle.

[0062] Step S1 can be specifically implemented through the following steps:

[0063] Step S1-1: Construct the foot-end plane equation by obtaining the three-dimensional coordinates of the foot-end of the multi-legged robot in the world coordinate system.

[0064] Taking Figure 2 the trot gait diagram of the quadruped robot shown as an example, LF, RF, RH, and LH respectively represent the left front leg, right front leg, right hind leg, and left hind leg of the multi-legged robot. In a gait cycle T, the left front leg and the right hind leg are swing legs in the first T / 2 moment, and the right front leg and the left hind leg are swing legs in the second T / 2 moment. Only at the moment of 0 and around T / 2 can all legs be regarded as touchdown legs. By obtaining the three-dimensional coordinates of all foot-ends at these two moments, rather than storing the combination of the three-dimensional coordinates of the touchdown legs in the previous gait cycle and the touchdown legs in the current gait cycle, a plane equation that conforms to the real terrain at the current moment can be constructed more accurately. Among them, the foot-end plane equation is defined as:

[0065] (1)

[0066] Among them, represents the height offset of the plane; represents the slope of the terrain in the x direction, that is, the height change generated by the unit x axis displacement; represents the slope of the terrain in the y direction, that is, the height change generated by the unit y axis displacement; is the coordinate of a point on the terrain plane. In this embodiment, let represent the foot-end coordinate of the i th leg of the multi-legged robot. Substituting it into the foot-end plane equation, we can get:

[0067] (2)

[0068] Among them, represents the height of the foot-end of the i th leg of the robot in the z-axis direction. Since the above system of equations contains four equations and three unknowns, it needs to be solved by the least squares method. Therefore, equation (2) is denoted as:

[0069] (3)

[0070] Among them, is the set of the left column of equation (2) , representing the height of the foot-end of each leg of the robot in the z axis direction,M The four-row and three-column matrix shown in the right column of Equation (2), X in the right column of Equation (2) , , and the set of. The ultimate goal of the solution is to find a X such that the sum of the squares of the residual vector is minimized, that is, to minimize the following objective function:

[0071] (4)

[0072] Simplifying gives:

[0073] (5)

[0074] Thus, the final solution can be expressed as:

[0075] (6)

[0076] At this time, a, b, c in the foot-end plane equation have all been obtained. Next, the pitch angle and roll angle of the slope can be solved according to the foot-end plane equation. For this, the present invention provides a dual-method composite control strategy. Specifically: set a yaw angle threshold and a terrain flatness threshold. When the yaw angle of the multi-legged robot is less than the set yaw angle threshold and the terrain flatness is less than the set terrain flatness threshold, use the first composite control strategy combined with the foot-end plane equation to calculate the pitch angle and roll angle; otherwise, use the second composite control strategy combined with the foot-end plane equation to calculate the pitch angle and roll angle. The dual-method composite control strategy can be implemented by the following method:

[0077] A. The first composite control strategy.

[0078] By default, when the multi-legged robot is going uphill, it faces the slope directly, that is, the yaw angle remains zero, and the influence of the yaw angle is not considered. Calculate the pitch angle and roll angle according to the foot-end plane equation. According to the definition of b, c in the plane equation ( b represents the height change caused by the unit x axis displacement, c represents the height change caused by the unit y axis displacement), the pitch angle and roll angle of the slope can be simplified and calculated by the following formula:

[0079] (7)

[0080] Among them, represents the pitch angle of the slope, represents the roll angle of the slope.

[0081] B. The second composite control strategy:

[0082] Considering the influence of the yaw angle, first use the IMU to read out the yaw angle , and obtain the slope normal vector through Equation (1) as . Among them, the IMU is a sensor that can measure the self-acceleration and angular velocity of an object. Through the above information, the attitude angles (roll angle, yaw angle, pitch angle) of the robot can be calculated. Unitize the slope normal vector to get:

[0083] (8)

[0084] Among them, represents the slope normal vector after unitization, that is, the unit normal vector of the slope. The unit normal vector of the slope obtained by Equation (8) is the third column of the rotation matrix from the slope coordinate system to the world coordinate system. Let , and the others are similar, then there are the following equations:

[0085] (9)

[0086] Among them, respectively represent the roll angle, pitch angle, and yaw angle of the terrain estimated by the plane equation. The yaw angle is consistent with the forward direction of the robot and is directly read by the IMU. Therefore is known, has also been calculated in Equation (8). Therefore can also be calculated. Therefore, the pitch angle and roll angle of the slope estimated by the plane equation are:

[0087] (10)

[0088] Among them, represents the pitch angle estimated by the plane equation, represents the roll angle estimated by the plane equation; represents the value of the in the row, that is represents , represents , represents .

[0089] In the actual application process, the first composite control strategy is simple to calculate and does not depend on IMU data, and is more suitable for straight-line climbing, low-dynamic scenarios or resource-constrained embedded platforms; the second composite control strategy takes into account the influence of the yaw angle, corrects the coordinate system through the rotation matrix, can accurately reflect the actual attitude of the robot in three-dimensional space, has strong adaptability to complex terrains, and is more suitable for complex terrain exploration and dynamic steering tasks. The present invention adopts a composite control strategy that combines the two, that is: a yaw angle threshold and a terrain flatness threshold are set. When the yaw angle of the multi-legged robot is less than the set yaw angle threshold and the terrain flatness is less than the set terrain flatness threshold, the first composite control strategy is adopted in combination with the foot end plane equation to calculate the pitch angle and the roll angle; otherwise, the second composite control strategy is adopted in combination with the foot end plane equation to calculate the pitch angle and the roll angle.

[0090] As an optional embodiment, the yaw angle threshold can be set to 10 0 , and the terrain flatness threshold is 0.05. That is, when the yaw angle is less than 10 0 and the terrain is flat , the first composite control strategy is adopted, and the second composite control strategy is switched to at other times to better balance efficiency and accuracy.

[0091] In step S2, based on the plane angle memory update algorithm including the attenuation coefficient, feedforward calculation is performed on the obtained pitch angle and roll angle to obtain the feedforward pitch angle and feedforward roll angle of the multi-legged robot on the slope.

[0092] The plane angle memory update algorithm can be expressed as:

[0093] (11)

[0094] Wherein, and respectively represent the feedforward roll angle and feedforward pitch angle of the updated multi-legged robot on the slope, and respectively represent the roll angle and pitch angle of the multi-legged robot on the slope obtained through the foot end plane equation at the current moment, and respectively represent the roll angle and pitch angle of the multi-legged robot on the slope obtained through the foot end plane equation saved in the previous gait cycle; represents the attenuation coefficient. Among them, the attenuation coefficient The calculation formula of is:

[0095] (12)

[0096] Wherein, k is a control parameter that is as large as possible and greater than zero, and is used to adjust the steepness of the curve; is based on the time weight of the gait phase and is obtained from the bimodal Gaussian fusion function. When approaches 1, that is, at the moment when the four legs of the multi-legged robot touch the ground simultaneously; also approaches 1, completely adopting new data to ensure a rapid response when the terrain changes suddenly and ensure that the proportion of new data is not less than 0.7.

[0097] The feedforward roll angle obtained in Equation (11) is the final desired roll angle. As Figure 4 shown in the schematic diagram of the roll angle adjustment of the multi-legged robot when going uphill. For the pitch angle, since it is significantly affected by the terrain dynamic disturbance during the slope movement, it is necessary to further compensate through the feedforward-feedback hybrid control strategy to improve the body attitude stability; it should be additionally noted that Figure 4 shown in represents the body coordinate system, represents the world coordinate system.

[0098] In step S3, based on the joint angle feedback algorithm, the joint angle difference is calculated, and the real-time correction amount of the pitch angle of the multi-legged robot on the slope is determined according to the difference calculation result.

[0099] Calculating the joint angle difference based on the joint angle feedback algorithm includes: calculating the difference between the sum of the front knee joint angles and the sum of the rear knee joint angles, and generating the real-time correction amount of the pitch angle through the PID controller . As Figure 3 shown in the schematic diagram of the multi-legged robot coordinate system, are the knee joint angles of the left front leg, right front leg, left rear leg, and right rear leg respectively. When the multi-legged robot moves forward on the horizontal plane or the torso moves forward parallel to the slope, due to the symmetry of its trot gait, the difference between the sum of the front knee joint angles and the sum of the rear knee joint angles is theoretically zero. Because when far from 0 and T / 2 moments in each gait cycle, only using the plane equation will cause inaccurate terrain estimation, so this feedback algorithm is introduced to dynamically compensate for the attitude deviation caused by terrain changes and foot-end slippage to obtain the pitch angle correction amount , that is:

[0100] (13)

[0101] Among them, represents the difference between the sum of the front knee joint angles and the sum of the rear knee joint angles, , and represent the gain coefficients of proportional, integral, and differential in the PID formula respectively.

[0102] In step S4, the final pitch angle of the multi-legged robot is determined using the double-peak Gaussian fusion method, that is: if the multi-legged robot is at any of the two touchdown moments, the feedforward pitch angle is used as the final pitch angle; otherwise, the value obtained by weighted fusion of the feedforward pitch angle and the real-time pitch angle correction amount is used as the final pitch angle.

[0103] In this step, the designed double-peak Gaussian fusion formula can be expressed as:

[0104] (14)

[0105] Among them, is the time weight based on the gait phase, is the weight decay coefficient, is the current normalized phase. Among them, The calculation formula of

[0106] (15)

[0107] Among them, mod represents the modulo operation, T represents the gait cycle.

[0108] At the 0 and T / 2 moments of each gait cycle T , all legs touch the ground simultaneously, and the weight = 1. At this time, the value of the desired pitch angle depends entirely on the slope pitch angle obtained in step S2 to achieve rapid terrain tracking; when away from the 0 moment and the T / 2 moment, the weight exponentially decays. At this time, a feedback correction link is added to improve the anti-interference ability. At this time, the desired pitch angle is obtained by weighting the terrain estimation (feedforward control) in step S2 and the joint angle feedback (feedback control) in step S3, that is, the weighted fusion algorithm when the value obtained by weighted fusion of the feedforward pitch angle and the real-time pitch angle correction amount is used as the final pitch angle is:

[0109] (16)

[0110] In the formula, represents the final pitch angle of the multi-legged robot on the slope after weighted fusion, represents the feedforward pitch angle of the multi-legged robot on the slope after being updated by the plane angle memory update algorithm, represents the real-time pitch angle correction amount generated by the PID controller; and respectively represent the weights corresponding to and ; among them, The value of gradually decays as it moves away from the 0 moment and the T / 2 moment within the gait cycle T of the multi-legged robot. The schematic diagram of the pitch angle adjustment of the multi-legged robot when going uphill is asFigure 5 as shown

[0111] In step S5, the obtained final pitch angle and the feedforward roll angle are combined with the gait and speed commands input by the user, and after being solved by NMPC-WBC, they are converted into joint torques, desired joint positions, and desired joint velocities to control the multi-legged robot to move on the slope.

[0112] The final pitch angle and the feedforward roll angle are combined with the gait and speed commands input by the user and passed into the trajectory planning module. After the trajectory planning module generates a desired trajectory, it sends it to the NMPC module. The NMPC calculates the optimal system state and system input and provides them to the WBC. The WBC sends the solved joint torques, desired joint positions, and desired joint velocities to the multi-legged robot, thereby controlling the movement of the robot on the slope. It should be noted that how to convert the obtained final pitch angle and the feedforward roll angle into joint torques, desired joint positions, and desired joint velocities that can control the multi-legged robot to move on the slope is not the main innovation point of the present invention. Therefore, as long as the technical means used here can achieve the conversion required by the present invention, the present invention does not specifically limit the specific conversion method.

[0113] Embodiment 2

[0114] This embodiment discloses a multi-legged robot slope traveling control system based on bimodal Gaussian fusion.

[0115] A multi-legged robot slope traveling control system based on bimodal Gaussian fusion includes:

[0116] A touchdown moment detection module, configured to: obtain the three-dimensional coordinates of the foot end of the multi-legged robot at two touchdown moments and construct a foot end plane equation; obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation;

[0117] A feedforward module, configured to: perform feedforward calculation on the obtained pitch angle and roll angle based on a plane angle memory update algorithm including an attenuation coefficient to obtain the feedforward pitch angle and feedforward roll angle of the multi-legged robot on the slope;

[0118] A joint angle feedback module, configured to: calculate the joint angle difference based on a joint angle feedback algorithm and determine the real-time pitch angle correction amount of the multi-legged robot on the slope according to the difference calculation result;

[0119] A bimodal Gaussian fusion control module, configured to: embed a bimodal Gaussian fusion algorithm for determining the final pitch angle of the multi-legged robot by means of bimodal Gaussian fusion, that is: if the multi-legged robot is at any one of the two touchdown moments, use the feedforward pitch angle as the final pitch angle; otherwise, use the value obtained by weighted fusion of the feedforward pitch angle and the real-time pitch angle correction amount as the final pitch angle;

[0120] The traveling control module is configured to: combine the obtained final pitch angle and the feedforward roll angle, along with the gait and speed commands input by the user, and convert them into joint torques, desired joint positions, and desired joint velocities through NMPC-WBC calculation to control the multi-legged robot to travel on the slope.

[0121] The control relationship of the control system framework adopted in the present invention is as Figure 6 shown, that is: the user inputs the trot gait and speed commands, combines them with the roll angle obtained by the feedforward module and the pitch angle obtained by the bimodal Gaussian fusion control module and the current system state, generates a desired trajectory and sends it to the NMPC. The NMPC calculates the optimal system state and system input and then provides them to the WBC. Finally, the solved joint torques, desired joint positions, and desired joint velocities are sent to the multi-legged robot. The multi-legged robot sends the current sensor data and joint states to the state estimator, thereby realizing the control of the multi-legged robot during the process of traveling on the slope.

[0122] Through the above module design, the attitude adjustment during climbing can be realized only relying on proprioceptors, and a rapid response to terrain mutations can be achieved. It not only avoids the attitude instability problem caused by the lag of historical data in traditional methods, but also avoids complex terrain modeling and iterative optimization calculations, can meet the real-time control requirements of the multi-legged robot, and has strong robustness.

[0123] Embodiment III

[0124] The purpose of this embodiment is to provide a computer-readable storage medium.

[0125] A computer-readable storage medium, on which a computer program is stored, and when the program is executed by a processor, it implements the steps in the method for controlling the traveling of a multi-legged robot on a slope based on bimodal Gaussian fusion as described in Embodiment I of the present disclosure.

[0126] Embodiment IV

[0127] The purpose of this embodiment is to provide an electronic device.

[0128] An electronic device, including a memory, a processor, and a program stored on the memory and executable on the processor. When the processor executes the program, it implements the steps in the method for controlling the traveling of a multi-legged robot on a slope based on bimodal Gaussian fusion as described in Embodiment I of the present disclosure.

[0129] In the devices of the above Second, Third, and Fourth Embodiments, the steps involved correspond to those in the First Method Embodiment. For the specific implementation, reference may be made to the relevant description part of the First Embodiment. The term "computer-readable storage medium" should be understood to include a single medium or multiple media containing one or more sets of instructions; it should also be understood to include any medium that can store, encode, or carry a set of instructions for execution by a processor and cause the processor to execute any method in the present invention.

[0130] Those skilled in the art should understand that the above-mentioned modules or steps of the present invention can be implemented by a general-purpose computer device. Optionally, they can be implemented by program codes executable by a computing device. Thus, they can be stored in a storage device for execution by the computing device, or they can be separately fabricated into individual integrated circuit modules, or multiple modules or steps among them can be fabricated into a single integrated circuit module for implementation. The present invention is not limited to any specific combination of hardware and software.

[0131] Although the specific implementation of the present invention has been described above in conjunction with the accompanying drawings, it does not limit the protection scope of the present invention. Those skilled in the art should understand that based on the technical solution of the present invention, various modifications or deformations that can be made without creative efforts by those skilled in the art are still within the protection scope of the present invention.

Claims

1. A method for controlling the movement of a multi-legged robot on a slope based on the fusion of bimodal Gaussian, characterized in that, Including: Obtain the three-dimensional coordinates of the foot end of the multi-legged robot at two touchdown moments, and construct the foot end plane equation; obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation; wherein, the two touchdown moments of the multi-legged robot are: within a complete gait cycle T in the trot gait, the 0 moment and the T / 2 moment when all the legs of the multi-legged robot touch the ground simultaneously; Based on the plane angle memory update algorithm including the attenuation coefficient, perform feedforward calculation on the obtained pitch angle and roll angle to obtain the feedforward pitch angle and feedforward roll angle of the multi-legged robot on the slope; Perform joint angle difference calculation based on the joint angle feedback algorithm, and determine the real-time pitch angle correction amount of the multi-legged robot on the slope according to the difference calculation result; Adopt the double-peak Gaussian fusion method to determine the final pitch angle of the multi-legged robot, that is: if the multi-legged robot is at any one of the two touchdown moments, use the feedforward pitch angle as the final pitch angle; otherwise, use the value obtained by weighted fusion of the feedforward pitch angle and the real-time pitch angle correction amount as the final pitch angle; Combine the obtained final pitch angle and feedforward roll angle, and convert them into joint torques, desired joint positions and desired joint velocities through NMPC-WBC solution according to the gait and speed commands input by the user to control the multi-legged robot to move forward on the slope.

2. The multi-legged robot slope traveling control method based on bimodal Gaussian fusion according to claim 1, wherein Obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation, including: setting the yaw angle threshold and the terrain flatness threshold, when the yaw angle of the multi-legged robot is less than the set yaw angle threshold and the terrain flatness is less than the set terrain flatness threshold, use the first composite control strategy combined with the foot end plane equation to calculate the pitch angle and roll angle; otherwise, use the second composite control strategy combined with the foot end plane equation to calculate the pitch angle and roll angle.

3. The multi-legged robot slope traveling control method based on bimodal Gaussian fusion according to claim 2, characterized in that, The first composite control strategy is: regard the direction of the multi-legged robot when going uphill as facing the slope directly, that is, keep the yaw angle zero, and calculate the pitch angle and roll angle according to the foot end plane equation; the second composite control strategy is: first, use the IMU to read the yaw angle and calculate the slope normal vector, and then calculate the pitch angle and roll angle according to the foot end plane equation.

4. The multi-legged robot slope traveling control method based on bimodal Gaussian fusion according to claim 1, characterized in that The plane angle memory update algorithm is: ； Among them, and respectively represent the feedforward roll angle and the feedforward pitch angle of the updated multi-legged robot on the slope, and respectively represent the roll angle and the pitch angle of the multi-legged robot on the slope obtained by the foot-end plane equation at the current moment, and respectively represent the roll angle and the pitch angle of the multi-legged robot on the slope obtained by the foot-end plane equation saved in the previous gait cycle; represents the attenuation coefficient.

5. The multi-legged robot slope traveling control method based on bimodal Gaussian fusion according to claim 1, characterized in that Perform joint angle difference calculation based on the joint angle feedback algorithm, including: calculate the difference between the sum of the front knee joint angles and the sum of the rear knee joint angles, and generate the real-time pitch angle correction amount through a PID controller.

6. The multi-legged robot slope traveling control method based on bimodal Gaussian fusion according to claim 1, characterized in that, The weighted fusion algorithm when using the value obtained by weighted fusion of the feedforward pitch angle and the real-time pitch angle correction amount as the final pitch angle is: ； In the formula, represents the final pitch angle of the multi-legged robot on the slope after weighted fusion, represents the feedforward pitch angle of the multi-legged robot on the slope after being updated by the plane angle memory update algorithm, represents the real-time pitch angle correction amount generated by the PID controller; and respectively represent the weights corresponding to and ; among them, gradually decays as it moves away from the 0 moment and the T / 2 moment within the gait cycle T of the multi-legged robot.

7. A multi-legged robot slope traveling control system based on bimodal Gaussian fusion, characterized in that, Including: A touchdown moment detection module, configured to: obtain the three-dimensional coordinates of the foot end of the multi-legged robot at two touchdown moments, and construct the foot end plane equation; obtain the pitch angle and roll angle of the multi-legged robot on the slope according to the foot end plane equation; wherein, the two touchdown moments of the multi-legged robot are: within a complete gait cycle T in the trot gait, the 0 moment and the T / 2 moment when all the legs of the multi-legged robot touch the ground simultaneously; A feedforward module, configured to: perform feedforward calculations on the obtained pitch angle and roll angle based on a planar angle memory update algorithm including a decay coefficient to obtain a feedforward pitch angle and a feedforward roll angle of the multi-legged robot on a slope; A joint angle feedback module, configured to: calculate a joint angle difference based on a joint angle feedback algorithm, and determine a real-time pitch angle correction amount of the multi-legged robot on a slope according to the difference calculation result; A bimodal Gaussian fusion control module, configured to: determine the final pitch angle of the multi-legged robot by using a bimodal Gaussian fusion method, that is: if the multi-legged robot is at any one of two touchdown moments, use the feedforward pitch angle as the final pitch angle; otherwise, use the value obtained by weighted fusion of the feedforward pitch angle and the real-time pitch angle correction amount as the final pitch angle; A traveling control module, configured to: combine the obtained final pitch angle and the feedforward roll angle, together with the gait and speed commands input by the user, and convert them into joint torques, desired joint positions, and desired joint speeds after NMPC-WBC calculation to control the multi-legged robot to travel on a slope.

8. A computer-readable storage medium having a program stored thereon, characterized in that, When the program is executed by a processor, it implements the steps in the method for controlling the travel of a multi-legged robot on a slope based on bimodal Gaussian fusion according to any one of claims 1-6.

9. An electronic device, comprising a memory, a processor, and a program stored on the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps in the method for controlling the travel of a multi-legged robot on a slope based on bimodal Gaussian fusion according to any one of claims 1-6.

Images