Method, device, equipment and storage medium for self-service elevator riding of walking robot

Abstract

The present invention relates to a method, device, equipment and storage medium for a self-service elevator-taking robot. The method: establish a robot kinematic model of the robot, construct a mapping relationship between the end of the wheel train and the step height, and obtain kinematic parameters; calculate the gait parameters of the elevator; generate a collision avoidance constraint set; create a time collaborative prediction and correction control law, and calculate the time collaborative control parameters; input the kinematic parameters, the collision avoidance constraint set and the time collaborative control parameters into a numerical optimal control algorithm, and use sequential quadratic programming to solve and obtain the optimal elevator trajectory; execute the self-service elevator process according to the optimal elevator trajectory and the gait parameters of the elevator, monitor the robot state and the elevator stability in real time, and when it is detected that the state deviation exceeds the preset threshold, trigger the time collaborative prediction and correction control law to adjust the trajectory in real time, and generate a dynamic elevator control strategy. The implementation of the present invention improves the control accuracy of the self-service elevator-taking robot.

Description

Technical Field

[0001] The present invention relates to the technical field of robot self-service elevator riding, and in particular to a method, device, equipment and storage medium for a mobility robot self-service elevator riding. Background Art

[0002] As a kind of intelligent mobile device, walking robots play an increasingly important role in daily life and work environment. However, in complex indoor environments, especially when facing obstacles such as steps and stairs, the autonomous mobility of walking robots still faces great challenges. Traditional walking robots often rely on manual assistance or special barrier-free facilities when riding elevators, which greatly limits their application scope and practicality.

[0003] Existing automatic elevator technology mainly focuses on wheeled and legged robots, but these methods are often difficult to directly apply to walking robots. Wheeled robots are prone to instability and safety issues when riding elevators, and although legged robots have strong obstacle-crossing capabilities, they are complex in structure and expensive, making them unsuitable as daily walking tools. In addition, most existing elevator control methods use preset fixed gaits, which are difficult to cope with complex and changeable actual environments and steps of different specifications. Summary of the invention

[0004] The main purpose of the present invention is to provide a method, device, equipment and storage medium for a mobility robot to take an elevator by itself, so as to improve the control accuracy of the mobility robot to take an elevator by itself.

[0005] To achieve the above object, the present invention provides a method for a mobility robot to take an elevator by itself, comprising the following steps:

[0006] Establish the robot kinematics model of the walking robot, construct the mapping relationship between the end of the gear train and the step height, and obtain the kinematic parameters;

[0007] Calculating the gait parameters of taking the stairs based on the kinematic parameters and the static stability criterion of the ground projection point of the center of gravity;

[0008] Constructing a nonlinear programming framework and generating a collision avoidance constraint set, wherein the collision avoidance constraint set includes a safety distance constraint between the robot body and the step edge and a robot posture adjustment constraint;

[0009] Based on the open-loop collaborative structure, a time collaborative prediction and correction control law is created, and a normalized weighted error function is introduced to calculate the time collaborative control parameters.

[0010] The kinematic parameters, the collision avoidance constraint set and the time coordinated control parameters are input into a numerical optimal control algorithm, and sequential quadratic programming is used to solve the problem to obtain an optimal elevator riding trajectory;

[0011] The self-service elevator riding process is executed according to the optimal elevator riding trajectory and the elevator riding gait parameters, and the robot state and elevator riding stability are monitored in real time. When it is detected that the state deviation exceeds the preset threshold, the time collaborative prediction and correction control law is triggered to adjust the trajectory in real time to generate a dynamic elevator riding control strategy.

[0012] The present invention also provides a self-service elevator-riding device for a walking robot, comprising:

[0013] A construction module is used to establish a robot kinematics model of the walking robot, construct a mapping relationship between the end of the gear train and the step height, and obtain kinematic parameters;

[0014] A calculation module, used for calculating the gait parameters of taking the ladder based on the kinematic parameters and the static stability criterion of the ground projection point of the center of gravity;

[0015] A generation module, used to construct a nonlinear programming framework and generate a collision avoidance constraint set, wherein the collision avoidance constraint set includes a safety distance constraint between the robot body and the step edge and a robot posture adjustment constraint;

[0016] A processing module is used to create a time collaborative prediction and correction control law based on an open-loop collaborative structure, introduce a normalized weighted error function, and calculate the time collaborative control parameters;

[0017] A solution module, used for inputting the kinematic parameters, the collision avoidance constraint set and the time coordinated control parameters into a numerical optimal control algorithm, and solving the problem by sequential quadratic programming to obtain an optimal elevator riding trajectory;

[0018] The adjustment module is used to execute the self-service elevator riding process according to the optimal elevator riding trajectory and the elevator riding gait parameters, monitor the robot state and elevator riding stability in real time, and when it is detected that the state deviation exceeds the preset threshold, trigger the time collaborative prediction and correction control law to adjust the trajectory in real time and generate a dynamic elevator riding control strategy.

[0019] The present invention also provides a computer device, comprising a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of any one of the above methods when executing the computer program.

[0020] The present invention also provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the steps of any of the above-mentioned methods are implemented.

[0021] In summary, the technical solution provided by the present invention can accurately calculate the gait parameters of the elevator by constructing an accurate kinematic model and dynamic equations, combined with the static stability criterion of the center of gravity ground projection point, to ensure that the walking robot remains highly stable during the elevator process. A nonlinear programming framework is used to generate a collision avoidance constraint set, including a safe distance constraint between the robot body and the edge of the step and a posture adjustment constraint, which effectively prevents the walking robot from colliding or tipping over during the elevator process. Based on the time-coordinated prediction and correction control law and the sequential quadratic programming algorithm, the optimal elevator trajectory can be generated, which maximizes riding comfort and minimizes energy consumption while ensuring safety. By real-time monitoring of the robot state and elevator stability, combined with a dynamic trajectory adjustment mechanism, it can cope with steps of different heights and complex environmental changes, and significantly improve the environmental adaptability of the walking robot. The entire elevator process does not require human intervention, which realizes the fully autonomous elevator riding of the walking robot and greatly improves the control accuracy of the self-service elevator riding of the walking robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 1 is a schematic diagram of the steps of a method for a mobility robot to take an elevator by itself in one embodiment of the present invention;

[0023] Figure 2 This is a structural block diagram of a self-service elevator device for a walking robot according to an embodiment of the present invention;

[0024] Figure 3 It is a schematic block diagram of the structure of a computer device according to an embodiment of the present invention.

[0025] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0026] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0027] Reference Figure 1 This embodiment provides a method for a mobility robot to take an elevator by itself, comprising the following steps:

[0028] S1, establish the robot kinematics model of the walking robot, construct the mapping relationship between the end of the gear train and the step height, and obtain the kinematic parameters;

[0029] Among them, the wheel train configuration of the walking robot is parametrically modeled, and each wheel is regarded as an independent driving unit to better describe the behavior of each wheel and its relationship with the steps. Through this process, the geometric parameters and driving parameters of the wheel train are obtained, and based on the geometric parameters and driving parameters of the wheel train, the coordinate transformation matrix from the robot base to each wheel is constructed. The coordinate transformation matrix is ​​multiplied to construct the overall transformation matrix from the robot base to the end of the wheel train. The overall transformation matrix represents the movement of each wheel as the movement relative to the robot base, realizing the comprehensive control and analysis of the entire wheel train. Based on the overall transformation matrix, the forward kinematics equation between the end position of the wheel train and the wheel angle is constructed. The forward kinematics equation maps the state (such as angle and speed) of each driving wheel of the robot to the position information of the end of the wheel train. In order to consider the influence of the step on the robot's motion, the step height parameter is introduced into the forward kinematics equation to obtain the kinematic equation containing the step information, so that the kinematic model is closer to the step environment in the real scene, and effectively describes the change law of the end position of the wheel train under different step heights. The Jacobian matrix of the kinematic equation containing step information is derived to obtain the velocity Jacobian matrix and acceleration Jacobian matrix considering the influence of steps. The Jacobian matrix describes the linear relationship between the end position and the joint variables (such as wheel angle). The velocity Jacobian matrix and the acceleration Jacobian matrix are used to describe the velocity and acceleration of the robot at different step heights, respectively. In order to determine the motion state of the end of the wheel train at a specific step height, the preset step height range parameters are input into the kinematic equation containing step information, and the inverse kinematic problem is solved by the numerical iteration method to obtain the mapping relationship function between the end of the wheel train and the step height. The numerical iteration method obtains a reasonable inverse solution through multiple approximations to ensure that the end of the wheel train can effectively match the height of the step. At the same time, based on the velocity Jacobian matrix and the acceleration Jacobian matrix, combined with the mass distribution information of the walking robot, the dynamic equation considering the influence of steps is constructed. During the robot's motion, the dynamic equation is used to describe the relationship between the external force on the robot and its motion state. In the process of riding the stairs, due to the presence of the steps, additional collision and friction will be generated between the wheels and the edge of the steps. When constructing the dynamic equation, the influence of these factors is fully considered in order to accurately describe the force state of the robot and its corresponding motion response during the elevator ride. The kinematic equation containing step information, the mapping function, the velocity Jacobian matrix, the acceleration Jacobian matrix and the dynamic equation considering the influence of the steps are combined to obtain complete kinematic parameters.

[0030] S2, based on the kinematic parameters and the static stability criterion of the center of gravity ground projection point, calculate the gait parameters of taking the ladder;

[0031] Specifically, a multi-body dynamics model of the walking robot is constructed based on the mass distribution information in the kinematic parameters and the dynamics equation considering the influence of the steps. Through the dynamics model, the coordinates of the total center of mass position of the robot under different wheel system configurations are calculated. The center of mass position is a key factor for the robot to maintain balance. In complex terrains such as going up and down stairs, it is necessary to ensure that the center of gravity of the robot as a whole is stable. Based on the coordinates of the total center of mass position of the robot under different configurations, and the mapping relationship function between the end of the wheel system and the step height, the trajectory equation of the center of gravity ground projection point is constructed to obtain the center of gravity projection trajectory of the walking robot during the entire elevator riding process. In order to facilitate control and analysis, the center of gravity projection trajectory is discretized and sampled, and the trajectory is divided into a series of sampling points in the time domain. Based on the geometric parameters of the wheel system, the support polygon corresponding to each sampling point is calculated. The support polygon is a closed polygon formed by the connecting lines between the grounded wheels, which is used to describe the stable area of ​​the robot in a specific posture. By calculating the support polygons of all sampling points, a time-varying support polygon sequence is constructed to represent how the support state of the robot changes over time during the entire elevator riding process. The time-varying support polygon sequence is substituted into the static stability criterion of the center of mass ground projection point to establish a stability constraint equation group that takes into account time-varying factors. The static stability criterion is used to determine whether the center of mass projection of the robot at a specific position and posture falls within the support polygon. Only when the center of mass projection is within the support polygon can the robot remain stable. By combining these criteria, a constraint equation group that describes the stability of the robot is established. Based on the kinematic equation containing step information and the mapping function between the end of the wheel train and the step height, the initial stair gait is designed to generate a candidate trajectory set of the wheel train motion. The design of the initial stair gait needs to consider the geometric characteristics of the wheel train and the specific conditions of the steps to ensure that each wheel can safely pass the edge of the step and remain stable. The candidate trajectory set of the wheel train motion is input into the stability constraint equation group to solve the wheel train motion trajectory that meets the stability requirements and obtain the stair gait sequence. Based on the stair gait sequence, a B-spline curve is used to fit it to obtain a continuous stair gait trajectory. The role of B-spline curve fitting is to smooth the discrete gait sequence and generate a continuous and smooth trajectory to ensure that the robot moves smoothly without sudden changes or violent oscillations during the elevator ride. The smooth trajectory helps to reduce the burden on the drive system and improve the overall motion stability and comfort. Substitute the continuous elevator gait trajectory into the dynamic equation that considers the influence of the steps, and calculate the driving torque sequence of each wheel system during the elevator ride by inverse dynamics. Inverse dynamics calculation solves the driving torque required for each wheel to achieve this motion through the known motion state. These driving torques are an important basis for the drive system to execute motion.At the same time, the velocity Jacobian matrix and the acceleration Jacobian matrix are combined to generate detailed gait parameters for taking the elevator, including the driving speed, acceleration and corresponding control input of each wheel, to ensure that the robot can smoothly complete the elevator task according to the planned trajectory.

[0032] S3, constructing a nonlinear programming framework and generating a collision avoidance constraint set, which includes a safety distance constraint between the robot body and the step edge and a robot posture adjustment constraint;

[0033] It should be noted that the three-dimensional geometric model of the walking robot is constructed based on the kinematic equations containing step information and the geometric parameters of the wheel train. The robot's shape is described by a superquadratic surface function to obtain the boundary expression of the robot body. The boundary expression is discretely sampled to generate boundary point cloud data representing the robot's shape. These point cloud data are discrete point sets used to accurately describe the robot's shape characteristics. Feature extraction is performed on the point cloud data to simplify the complexity of the boundary model. Through feature extraction, the minimum circumscribed ellipsoid model of the robot body is constructed. The minimum circumscribed ellipsoid model calculates the center coordinates and semi-axis length of the ellipsoid to approximate the boundary of the robot in three-dimensional space. According to the mapping relationship function between the end of the wheel train and the step height and the step height parameter, a three-dimensional space curve model of the step edge is established to accurately describe the step edge and obtain the parameterized equation of the step edge. Based on the robot's minimum circumscribed ellipsoid model and the parameterized equation of the step edge, the minimum distance function between the robot body and the step edge is constructed. This function is used to calculate the distance between the robot and the step, and solve the minimum distance point pair, that is, the point on the robot surface closest to the step edge and the corresponding point on the step edge. The minimum distance point pair helps determine whether the robot is too close to the edge of the step and causes a collision. In order to quantify the safe distance, an exponential potential field function is introduced as a measure of the safe distance to effectively construct a safe distance constraint between the robot body and the edge of the step, ensuring that the robot always maintains a safe distance from the step during the entire elevator riding process. The continuous gait trajectory in the gait parameters of the elevator riding is decomposed into Euler angles, and the pitch angle, roll angle and yaw angle of the robot during the elevator riding process are extracted from them. These angles describe the changes in the robot's posture during the elevator riding process and generate a continuous posture angle function. Based on the posture angle function, the stability evaluation index of the robot's posture is constructed. In order to ensure that the robot maintains a stable posture during the elevator riding process, the zero moment point theory is introduced to calculate the dynamic stability margin of the robot. The zero moment point theory is a stability evaluation method that determines whether the robot is in a stable state by calculating the torque balance of the robot in the dynamic process. In order to cope with complex terrain and uncertain factors, the fuzzy logic controller is combined to set an adaptive posture adjustment threshold. The fuzzy logic controller can handle uncertainty. By adaptively adjusting the posture threshold, the robot can perform adaptive posture adjustment in different environments and motion states to ensure that it is always in a stable state. By combining the posture angle function, dynamic stability margin calculation and fuzzy logic controller, the posture adjustment constraints of the robot in the process of taking the ladder are obtained. The safety distance constraints between the robot body and the edge of the step and the robot posture adjustment constraints are combined to obtain a complete set of collision avoidance constraints.

[0034] S4, based on the open-loop collaborative structure, a time collaborative prediction and correction control law is created, and a normalized weighted error function is introduced to calculate the time collaborative control parameters;

[0035] Specifically, a nonlinear state space model of the walking robot is constructed based on the continuous stair-riding gait trajectory and the dynamic equation considering the influence of steps. The nonlinear state space model can accurately describe the dynamic behavior of the walking robot in the process of going up and down stairs. The nonlinear state space model is linearized. The Taylor series expansion method is used to expand the nonlinear model near a certain working point to obtain the corresponding linearized system state equation and output equation. The linearized system state equation is discretized to obtain a discrete time state space model. Based on the discrete time state space model, an adaptive extended Kalman filter is designed to estimate the system state in real time and obtain a state estimation value sequence. The extended Kalman filter optimizes the state of the system and can provide more accurate state information in the presence of measurement noise and model uncertainty. Through state estimation, the real-time state of the robot in the process of taking the stairs is effectively tracked. According to the state estimation value sequence and the preset expected elevator time, a time coordination error function is constructed. The time coordination error function is used to describe the deviation between the robot and the predetermined time target during the elevator process to ensure that the robot can complete the task of going up and down stairs according to the set time. In order to improve the accuracy of time control, the remaining step number error is introduced to quantify the number of steps that the robot needs to pass before reaching the final position. The time coordination error function and the remaining step number error are combined to obtain the comprehensive time error expression. The comprehensive time error expression is normalized, and the adaptive weight coefficient based on fuzzy logic is introduced to obtain the normalized weighted error function. Through fuzzy logic control, the weights of each error term are adaptively adjusted according to the real-time state to achieve more flexible and effective time error control. Based on the normalized weighted error function, a model predictive controller is designed to construct the initial control law of the open-loop collaborative structure. Model predictive control is a model-based control method that predicts the future system behavior at each moment and finds the best control input. The design of the control law of the open-loop collaborative structure is based on the prediction of the future system behavior so that the system can run according to the set time trajectory. After constructing the initial control law, the robustness analysis is performed on it to ensure that the controller can still maintain stable and effective control effects when facing system model uncertainty and external disturbances. Through robustness analysis, the final time cooperative predictive correction control law is obtained. The time cooperative predictive correction control law is combined with the collision avoidance constraint set to construct the optimal control problem with constraints. The collision avoidance constraint set is used to ensure that the robot does not collide with the edge of the stairs and maintains a safe posture during the elevator ride. The optimal control problem with constraints combines the requirements of time cooperative control and safety constraints. By solving this optimal control problem, a control input sequence that satisfies all constraints is obtained. Based on the control input sequence, the discrete time state space model is combined for numerical integration to calculate the time cooperative control parameters.Numerical integration gradually accumulates discrete control inputs to obtain the control trajectory and motion state of the system during the entire elevator riding process.

[0036] S5, inputting kinematic parameters, collision avoidance constraint set and time coordinated control parameters into the numerical optimal control algorithm, solving it by sequential quadratic programming, and obtaining the optimal elevator riding trajectory;

[0037] Among them, the state equation of the walking robot is constructed based on the kinematic equation containing step information in the kinematic parameters. Through these state equations, combined with the time cooperative control parameters, the discrete time dynamic model of the system is established. According to the collision avoidance constraint set, the safety distance constraint between the robot body and the step edge and the robot posture adjustment constraint are converted into inequality constraints of state variables and control variables. These constraints are used to limit the posture and relative position of the robot in each time step to ensure that the robot will not collide with the steps during the elevator ride and always maintain a stable posture. By converting the physical constraints into a set of constraint functions, these safety requirements are directly taken into consideration in the optimization process, so that the optimal trajectory obtained meets the physical constraints and safety standards. Based on the normalized weighted error function in the time cooperative control parameters, the objective function of the optimal control problem is designed. In this objective function, the elevator ride time and energy consumption are taken as the main optimization indicators to obtain a multi-objective optimization expression. Through the multi-objective optimization expression, efficiency and energy consumption are taken into account when solving the optimal control input, ensuring that the walking robot can complete the elevator ride task in the shortest time with the minimum energy cost. The multi-objective optimization expression is linearized to obtain a linearized objective function for each time step. Substitute the linearized objective function and the set of constructed constraint functions into the sequential quadratic programming framework to construct quadratic programming subproblems. Under the sequential quadratic programming framework, the optimal control problem is decomposed into a series of quadratic programming subproblems. By solving each subproblem, a local optimal solution sequence is obtained. The local optimal solution sequence represents the best choice of control input and system state under the current iteration step. In order to optimize the local solution, the line search method is used to determine the optimal step size to update the state variables and control variables to obtain the iterative optimization sequence. The line search method adjusts the step size to ensure that each iteration can move in the optimal direction, thereby accelerating the convergence process. The convergence analysis of the iterative optimization sequence is performed and the termination condition is set. When the preset convergence accuracy requirements are met, the iteration process can be stopped and the optimal control input sequence is obtained. The purpose of the convergence analysis is to ensure that the error in the solution process is within an acceptable range, so as to ensure that the solution obtained is close enough to the global optimal solution and meets the accuracy requirements in practical applications. Substitute the optimal control input sequence into the discrete time dynamic model and perform numerical integration to obtain the optimal trajectory of the walking robot. The discrete control input is converted into a continuous state evolution trajectory through numerical integration to describe the motion trajectory of the walking robot in the process of taking the elevator.

[0038] S6, executes the self-service elevator riding process according to the optimal elevator riding trajectory and elevator riding gait parameters, monitors the robot status and elevator riding stability in real time, and when it is detected that the state deviation exceeds the preset threshold, triggers the time collaborative prediction and correction control law to adjust the trajectory in real time and generate a dynamic elevator riding control strategy.

[0039] Specifically, the optimal elevator trajectory and the gait parameters of the elevator are input into the trajectory tracking controller, and the sliding mode variable structure control algorithm is used to generate a real-time wheel train drive instruction sequence. Based on the real-time wheel train drive instruction sequence, the wheel train actuator of the walking robot is closed-loop controlled, and the state information of the robot is collected in real time through the sensor system to obtain state feedback data. The sensor data includes the angle, speed, acceleration of the wheel and the posture information of the robot. Through the closed-loop control method, the robot drive is adjusted in real time to ensure that it can move according to the predetermined trajectory. The robot state feedback data is processed by Kalman filtering to obtain a more accurate filtered state estimate. The Kalman filter is a filtering tool used in dynamic systems. It can make an optimal estimate of the system state in the presence of noise, thereby removing random noise in the measurement data and obtaining more reliable state information. The filtered state estimate is compared with the optimal elevator trajectory to calculate the state deviation of the robot during the elevator process. The adaptive threshold is set based on the fuzzy logic rule to obtain the evaluation result of the state deviation. The fuzzy logic controller has the ability to handle uncertainty and nonlinear systems. The evaluation threshold is dynamically adjusted according to the current robot state and environmental changes to more accurately determine whether the trajectory needs to be adjusted. According to the state deviation evaluation results, it is determined whether the trajectory adjustment needs to be triggered. When the state deviation exceeds the preset adaptive threshold, the time collaborative prediction and correction control law is activated. This control law constructs a local trajectory optimization problem by performing multi-step prediction of the current state and combining the collision avoidance constraint set. The time collaborative prediction and correction control law can effectively evaluate whether the current movement of the robot will deviate from the optimal trajectory by predicting the system state in the next few time steps, and make corresponding adjustments in advance. The collision avoidance constraint set is used to ensure that the robot always maintains a safe distance from the steps and maintains its stable posture during the trajectory adjustment process. The local trajectory optimization problem is solved to obtain the local optimal control input sequence. The local optimal control input sequence represents the optimal control instruction required to correct the deviation in the current state. The local optimal control input sequence is smoothly fused with the optimal control input sequence to generate the adjusted control instruction. The fused adjustment control instruction is input into the trajectory tracking controller to update the real-time wheel train drive instruction sequence and generate a dynamic elevator control strategy.

[0040] In one example, a robot kinematic model of a walking robot is established, a mapping relationship between the end of a wheel train and a step height is constructed, and kinematic parameters are obtained, including: parametric modeling of the wheel train configuration of the walking robot, treating each wheel as an independent drive unit, obtaining wheel train geometric parameters and drive parameters, and constructing a coordinate transformation matrix from the robot base to each wheel based on the wheel train geometric parameters and drive parameters; performing matrix multiplication operations on the coordinate transformation matrix to construct an overall transformation matrix from the robot base to the end of the wheel train, and based on the overall transformation matrix, constructing a forward kinematic equation of the wheel train end position and the wheel angle, and introducing a step height parameter to obtain a motion equation containing step information. kinematic equation; perform Jacobian matrix differentiation on the kinematic equation containing step information to obtain the velocity Jacobian matrix and acceleration Jacobian matrix considering the influence of steps, and input the preset step height range parameters into the kinematic equation containing step information, solve the inverse kinematic problem through numerical iteration method, and obtain the mapping relationship function between the end of the gear train and the step height; based on the velocity Jacobian matrix and the acceleration Jacobian matrix, combined with the mass distribution information of the walking robot, construct the dynamic equation considering the influence of steps; combine the kinematic equation containing step information, the mapping relationship function, the velocity Jacobian matrix, the acceleration Jacobian matrix and the dynamic equation considering the influence of steps to obtain the kinematic parameters.

[0041] In this example, the wheel train configuration of the walking robot is parametrically modeled, and each wheel is regarded as an independent drive unit. The motion state of each wheel is described and controlled, and the relationship between the wheels is described to facilitate the description of the geometric parameters and drive parameters of the wheel train. The geometric parameters include the position and direction of the wheel relative to the robot base, while the drive parameters involve the characteristics of the wheel such as the driving force and speed. Based on the geometric parameters and drive parameters, a coordinate transformation matrix from the robot base to each wheel is constructed. Assume that the base of the walking robot is the coordinate system , each wheel is a coordinate system , by constructing the coordinate transformation matrix of each wheel, the relative position of the base to the wheel is represented. The coordinate transformation matrix is ​​represented by homogeneous transformation, and its form is:

[0042] ;

[0043] in, From the base coordinate system To Wheel coordinate system The homogeneous transformation matrix of is the rotation matrix from the base to the wheel, describing the orientation of the wheel; and is the displacement vector from the base to the wheel, describing the position of the wheel in the base. By establishing the coordinate transformation matrix of each wheel, the position of each wheel is accurately described. By performing matrix multiplication on the coordinate transformation matrix, the overall transformation matrix from the robot base to the end of the wheel train is constructed. Assuming that the robot has multiple wheels, the overall transformation matrix It is expressed as the product of all individual wheel transformation matrices. The overall transformation matrix describes the end state of the entire wheel train of the robot, that is, the comprehensive result of the position and direction of each wheel. Based on the overall transformation matrix, the forward kinematics equation between the position of the end of the wheel train and the angle of each wheel is constructed. The forward kinematics equation calculates the position of the end of the wheel train through the known wheel angle and position parameters. Assume that the wheel angle is The gear train end position The overall transformation matrix is ​​expressed as:

[0044] ;

[0045] in, represents the forward kinematics function, which describes how each wheel angle affects the position of the end of the gear train. In order to consider the effect of the step, the step height parameter is introduced into the forward kinematics equation , and obtain the kinematic equation containing the step information. This kinematic equation relates the position of the end of the wheel train to the height of the step, so that the walking robot can adjust according to the height of the step when going up and down the steps. The kinematic equation containing the step information is derived by derivation of the Jacobian matrix to obtain the velocity Jacobian matrix and acceleration Jacobian matrix that consider the influence of the step. The Jacobian matrix is ​​used to describe the relationship between the position change of the end of the wheel train and the change of the wheel angle. Velocity Jacobian matrix It is defined as the linear mapping between the end velocity of the gear train and the angular velocity of the wheel, and the acceleration Jacobian matrix It describes the relationship between the end acceleration of the gear train and the angular acceleration of the wheel. The velocity Jacobian matrix is ​​expressed as:

[0046] ;

[0047] in, is the velocity vector at the end of the gear train, is the wheel angular velocity vector, is the velocity Jacobian matrix. The acceleration Jacobian matrix similarly describes the relationship between accelerations. By calculating the velocity and acceleration Jacobian matrices, the changing characteristics of the velocity and acceleration of the end of the gear train during step motion are obtained. In order to determine the relationship between the end of the gear train and the step height, the preset step height range parameters are input into the kinematic equation containing the step information, and the inverse kinematics problem is solved by the numerical iteration method. The purpose of the inverse kinematics problem is to find the wheel angle so that the end of the gear train can reach a specific position. Through the numerical iteration method, when the step height is known, the angles of each wheel are solved to obtain the mapping relationship function between the end of the gear train and the step height. This mapping relationship function describes how the wheel angle changes with the change of the step height. Based on the velocity Jacobian matrix and the acceleration Jacobian matrix, combined with the mass distribution information of the walking robot, a dynamic equation considering the influence of the step is constructed. The dynamic equation is used to describe the relationship between the force and torque exerted on the robot during movement. Assume that the mass of the robot is , the centroid position is , then the kinetic equation is expressed as:

[0048] ;

[0049] in, is the external force acting on the robot, is the acceleration of the center of mass, is the acceleration of gravity. By combining the acceleration Jacobian matrix, the driving force of each wheel is linked to the force relationship of the entire robot system, thereby ensuring the dynamic balance and stability of the system during the process of going up and down the stairs. The kinematic equation containing step information, the mapping function between the end of the wheel system and the step height, the velocity Jacobian matrix, the acceleration Jacobian matrix, and the dynamic equation considering the influence of the steps are combined to obtain complete kinematic parameters.

[0050] In one example, based on kinematic parameters and static stability criteria of the projection point of the center of gravity on the ground, the gait parameters of taking stairs are calculated, including: constructing a multi-body dynamics model of the walking robot according to the mass distribution information in the kinematic parameters and the dynamic equation considering the influence of steps, and calculating the total center of mass position coordinates of the robot under different wheel system configurations; constructing the trajectory equation of the projection point of the center of gravity on the ground based on the mapping relationship function between the total center of mass position coordinates and the wheel system end and the step height, obtaining the projection trajectory of the center of gravity during the taking stairs process, and discretizing and sampling the projection trajectory of the center of gravity, calculating the support polygon corresponding to each sampling point based on the wheel system geometric parameters, and constructing a time-varying support polygon sequence; substituting the time-varying support polygon sequence into the projection point of the center of gravity on the ground Static stability criterion, establish a group of stability constraint equations considering time-varying factors, and design the initial stair-taking gait based on the kinematic equation containing step information and the mapping function between the end of the wheel train and the step height, and generate a set of candidate trajectories for the wheel train motion; input the candidate trajectory set of the wheel train motion into the group of stability constraint equations, solve the wheel train motion trajectory that meets the stability requirements, obtain the stair-taking gait sequence, and perform B-spline curve fitting on the stair-taking gait sequence to obtain a continuous stair-taking gait trajectory; substitute the continuous stair-taking gait trajectory into the dynamic equation considering the influence of steps, obtain the driving torque sequence of each wheel train in the stair-taking process through inverse dynamics calculation, and generate the stair-taking gait parameters by combining the velocity Jacobian matrix and the acceleration Jacobian matrix.

[0051] In this example, a multi-body dynamics model of a walking robot is constructed based on the mass distribution information in the kinematic parameters and the dynamics equations considering the influence of steps. The multi-body dynamics model is used to describe the relative motion relationship between multiple rigid bodies of the robot. Assume that the robot consists of multiple wheels and a base, and the mass of each part is , the location is , the total center of mass position coordinates of the entire robot It is expressed as:

[0052] ;

[0053] in, is the position of the robot's total center of mass, For the The quality of each component, For the The location of the components, is the total number of components. Through this equation, the coordinates of the total center of mass position of the robot under different wheel train configurations are calculated. Different wheel train configurations mean that the positions of each wheel change. Therefore, the total center of mass is recalculated according to different configurations to ensure that the robot can maintain balance at any time. Based on the mapping relationship function between the total center of mass position coordinates and the wheel train end and the step height, the trajectory equation of the center of mass ground projection point is constructed. The center of mass ground projection point is a key indicator for the robot to maintain static stability, indicating the projection position of the robot's center of mass on the ground. If the center of mass ground projection point falls within the support polygon, the robot is stable. During the elevator ride, the center of mass projection point is constantly changing with the movement of the wheels. The trajectory equation is used to describe the changing trajectory of the center of mass projection point during the entire elevator ride. In order to better perform stability analysis, the center of mass projection trajectory is discretized and sampled, and the continuous trajectory is converted into a discrete point set. For each sampling point, the support polygon corresponding to the point is calculated based on the geometric parameters of the wheel train. The support polygon is formed by the connecting lines between the grounded wheels, and its geometric shape determines the support range of the robot in the current configuration. By calculating the support polygons of all sampling points, a time-varying support polygon sequence is constructed, which is used to describe the support state of the robot during the entire motion process. The time-varying support polygon sequence is substituted into the static stability criterion of the center of gravity ground projection point to establish a stability constraint equation group that considers time-varying factors. The static stability criterion is used to determine whether the center of gravity projection point is located within the support polygon. Only when the center of gravity projection point is always within the support polygon can the robot maintain static stability. The establishment of the stability constraint equation group aims to ensure that the robot is in a stable state at each sampling moment during the elevator riding process. On the basis of ensuring stability, the initial elevator riding gait is designed based on the kinematic equation containing step information and the mapping relationship function between the end of the wheel train and the step height, and the candidate trajectory set of the wheel train motion is generated. The design of the initial elevator riding gait needs to consider the height of the step and the motion restrictions of each wheel to ensure that each wheel can safely pass the edge of the step and maintain stability. By inputting the candidate trajectory set into the stability constraint equation group, the wheel train motion trajectory that meets the stability requirements is solved, and the initial elevator riding gait sequence is obtained. In order to make the stair gait smoother and more natural, the stair gait sequence is fitted with a B-spline curve to obtain a continuous stair gait trajectory. The B-spline curve is a mathematical method for generating smooth trajectories. By fitting discrete points to obtain a continuous curve, the robot will not experience sudden changes or violent oscillations during movement. When facing a complex step environment, a smooth gait trajectory helps reduce the impact between the wheels and the steps. The fitted continuous stair gait trajectory is substituted into the dynamic equation considering the influence of the steps, and the driving torque sequence of each wheel system during the stair gait process is calculated using the inverse dynamics method.The dynamic equation is used to describe the relationship between the force and motion between the various parts of the robot, while the inverse dynamics is to solve the torque required for driving based on the known state of motion. Assume that the acceleration of the center of mass of the robot is. , the driving torque is expressed by the following formula:

[0054] ;

[0055] in, is the driving torque vector, is the transpose of the velocity Jacobian matrix, is the total mass of the robot, is the acceleration due to gravity. Through this formula, the driving torque required for each wheel is calculated according to the acceleration of the center of mass and the mass distribution information of the system. The velocity Jacobian matrix and the acceleration Jacobian matrix are combined to generate the complete stair-riding gait parameters. The velocity Jacobian matrix is ​​used to describe how the velocity at the end of the wheel train changes with the wheel angular velocity, while the acceleration Jacobian matrix is ​​used to describe how the acceleration at the end of the wheel train changes with the wheel angular acceleration. Based on these Jacobian matrices, the drive of each wheel is finely controlled to ensure the motion stability and accuracy of the robot throughout the stair-riding process.

[0056] In one example, a nonlinear programming framework is constructed and a collision avoidance constraint set is generated. The collision avoidance constraint set includes a safety distance constraint between the robot body and the step edge and a robot posture adjustment constraint, including: constructing a three-dimensional geometric model of the walking robot body based on the kinematic equations containing step information and the geometric parameters of the wheel train, using a superquadratic surface function to describe the robot shape, and obtaining a boundary expression of the robot body; discretely sampling the boundary expression to generate boundary point cloud data, and performing feature extraction on the point cloud data to construct a minimum circumscribed ellipsoid model of the robot body, and obtaining the center coordinates and semi-axis length of the ellipsoid; establishing a three-dimensional spatial curve model of the step edge based on the mapping relationship function between the wheel train end and the step height and the step height parameters, accurately describing the step edge, and obtaining the parameters of the step edge. equation; based on the ellipsoid model and the parameterized equation of the step edge, the minimum distance function between the robot body and the step edge is constructed, the minimum distance point pair is solved, and the exponential potential field function is introduced as the safety distance metric to obtain the safety distance constraint between the robot body and the step edge; the continuous stair-riding gait trajectory in the stair-riding gait parameters is decomposed by Euler angles, the pitch angle, roll angle and yaw angle of the robot during the stair-riding process are extracted, and a continuous attitude angle function is generated; based on the attitude angle function, the robot attitude stability evaluation index is constructed, the zero moment point theory is introduced to calculate the dynamic stability margin, and the adaptive attitude adjustment threshold is set in combination with the fuzzy logic controller to obtain the robot attitude adjustment constraint; the safety distance constraint between the robot body and the step edge and the robot attitude adjustment constraint are combined to obtain a collision avoidance constraint set.

[0057] In this example, based on the kinematic equations containing step information and the geometric parameters of the gear train, a three-dimensional geometric model of the walking robot is constructed to describe the structure and appearance characteristics of the robot in three-dimensional space. In order to accurately describe the appearance of the robot, a superquadratic surface function is used for modeling. A superquadratic surface function is a function form used to represent complex geometric shapes. It describes common shapes such as cylinders and ellipsoids through different parameter combinations. Assume that the superquadratic surface function of the robot's appearance is:

[0058] ;

[0059] in, are coordinates in space, is the semi-axis length of the superquadratic surface, describing the size of the robot in three directions, It is a parameter that controls the shape of the surface, and its value is usually greater than 2 to obtain a smooth surface. Through this boundary expression, the precise boundary of the robot body in three-dimensional space is obtained. In order to better perform collision detection, the boundary expression is discretely sampled to generate boundary point cloud data. Point cloud data is a set of discrete points that represent the surface of the robot's shape. These points are used to further analyze the geometric characteristics of the robot. Feature extraction is performed on the generated point cloud data to simplify the complex boundary description and construct a minimum circumscribed ellipsoid model that can represent the overall shape of the robot. The minimum circumscribed ellipsoid is a method of describing the shape of a complex object with fewer parameters. Its formula is:

[0060] ;

[0061] in, are the center coordinates of the ellipsoid, are the semi-axis lengths of the ellipsoid in three directions respectively. These parameters are obtained by fitting the point cloud data. The minimum circumscribed ellipsoid provides a simple and effective way to describe the overall shape boundary of the robot, which is helpful for subsequent collision detection and safety distance calculation. According to the mapping relationship function between the end of the gear train and the step height and the step height parameter, a three-dimensional spatial curve model of the step edge is established. The edge of the step is the part that the robot needs to focus on when going up and down the steps, and it is accurately modeled to ensure that the robot can pass the steps safely. Through the step height parameter, the parameterized equation of the step edge is obtained, for example:

[0062] ;

[0063] in, is a parameter that describes the position change of the step edge on the two-dimensional plane. is the height of the step. This parameterized equation accurately describes the position of the step edge in three-dimensional space. Based on the minimum circumscribed ellipsoid model and the parameterized equation of the step edge, the minimum distance function between the robot body and the step edge is constructed. Assume that the center of the robot ellipsoid model is , the point on the edge of the step is , then the distance function between the two is expressed as:

[0064] ;

[0065] By solving the minimum distance point pair, the shortest distance between the robot and the steps is obtained. In order to ensure that the robot always maintains a certain safe distance during the elevator ride, an exponential potential field function is introduced as a measure of the safe distance. The potential field function is expressed as:

[0066] ;

[0067] in, For distance, is the coefficient of the potential field function, which is used to control the sensitivity of the safety distance. In this way, a safety distance constraint between the robot body and the edge of the step is constructed to ensure that the robot will not collide with the step during movement. At the same time, in order to describe the posture changes of the robot during the elevator process, the continuous elevator gait trajectory in the elevator gait parameters is decomposed by Euler angles, and the pitch angle, roll angle and yaw angle of the robot during the elevator process are extracted to describe the rotation state of the robot in three-dimensional space, and a continuous posture angle function is generated:

[0068] ;

[0069] in, is the pitch angle, is the roll angle, is the yaw angle. These angle functions are used to describe the posture changes of the robot at each moment. Based on the posture angle function, the stability evaluation index of the robot posture is constructed. In order to ensure that the robot maintains the stability of the posture during the elevator ride, the zero moment point theory is introduced to calculate the dynamic stability margin. The zero moment point theory is an important tool for judging whether the robot is in a stable state. By calculating the torque balance of the robot in the dynamic process, it is judged whether it is stable. Combined with the fuzzy logic controller to set the adaptive posture adjustment threshold, the fuzzy logic controller can dynamically adjust the adjustment amplitude and frequency of the posture according to the real-time state changes, and obtain the robot posture adjustment constraint to ensure that the robot can adaptively adjust the posture and maintain stability when facing uncertain terrain such as steps. The safety distance constraint between the robot body and the edge of the step and the robot posture adjustment constraint are combined to obtain a complete collision avoidance constraint set. This constraint set takes into account the physical distance between the robot and the step, and combines the posture adjustment requirements of the robot during the elevator ride to ensure that the robot is always in a safe and stable state during the entire elevator ride.

[0070] In one example, a time collaborative prediction and correction control law is created based on an open-loop collaborative structure, and a normalized weighted error function is introduced to calculate the time collaborative control parameters, including: constructing a nonlinear state space model of the walking robot according to the continuous stair-riding gait trajectory and the dynamic equation considering the influence of steps, and linearizing it using Taylor series expansion to obtain the linearized system state equation and output equation; discretizing the linearized system state equation to obtain a discrete-time state space model, and designing an adaptive extended Kalman filter based on the discrete-time state space model to estimate the system state and obtain a state estimation value sequence; constructing a time collaborative error correction control law according to the state estimation value sequence and the preset expected stair-riding time. Difference function is proposed, and the residual step number error is introduced to obtain the comprehensive time error expression, and the comprehensive time error expression is normalized, and the adaptive weight coefficient based on fuzzy logic is introduced to obtain the normalized weighted error function; based on the normalized weighted error function, the model predictive controller is designed, the initial control law of the open-loop collaborative structure is constructed, and the robustness analysis of the initial control law expression is performed to obtain the time collaborative predictive correction control law; the time collaborative predictive correction control law is combined with the collision avoidance constraint set to construct the constrained optimal control problem, and the control input sequence that meets the constraints is solved, and the time collaborative control parameters are calculated by numerical integration based on the control input sequence and the discrete time state space model.

[0071] In this example, a nonlinear state space model of the walking robot is constructed based on the continuous stair gait trajectory and the dynamic equation considering the influence of steps. The nonlinear state space model describes the evolution relationship of all state variables of the robot during the movement process, which is expressed as:

[0072] ;

[0073] in, is the state variable vector, including the position, speed, attitude angle, etc. of the robot during the elevator ride. is the control input vector, describing the driving force and driving torque of each wheel, is a nonlinear function that describes the relationship between the state variable and the control input. When constructing a nonlinear model, the dynamic equations of the robot on the steps are considered, such as the influence of gravity, friction, and driving force on the entire system. Due to the complexity of the nonlinear state space model, it is difficult to directly apply it to control design and linearize it. The Taylor series expansion method is used to linearize the nonlinear function near a certain working point and approximate it to a linear form. By Perform Taylor expansion to obtain the linearized system state equation:

[0074] ;

[0075] in, is the system state matrix, which represents the relationship between state variables. is the input matrix, describing how the control input affects the state change. Through linearization, the originally complex nonlinear system is simplified to a linear system, which is convenient for subsequent control design and analysis. At the same time, the output equation of the system is obtained:

[0076] ;

[0077] in, is the output variable of the system, usually some key observables, such as position or velocity, is the output matrix, is the direct transfer matrix. In order to meet the application requirements of the digital controller, the linearized system state equation is discretized to obtain a discrete time state space model. Assume that the sampling time is , the discretized state space equation is expressed as:

[0078] ;

[0079] in, and are the system state matrix and input matrix in discrete time, and For the The state variables and control inputs at each sampling moment. Based on the discrete time state space model, an adaptive extended Kalman filter is designed to estimate the system state. The adaptive extended Kalman filter is a filtering tool for nonlinear system state estimation. It can optimally estimate the system state in the presence of noise and obtain a sequence of state estimation values. According to the state estimation value sequence and the preset expected elevator riding time, a time coordination error function is constructed. The time coordination error function is used to quantify the deviation between the actual movement of the robot and the expected time to ensure that the robot can complete the action of going up and down the stairs as planned. The time coordination error function is defined as:

[0080] ;

[0081] in, is the time error, is the actual completion time, is the expected completion time. In order to improve the control accuracy, the remaining step number error is introduced to describe the number of steps the robot needs to go through before reaching the final goal. The time coordination error and the remaining step number error are combined to obtain the comprehensive time error expression. In order to reasonably weigh the impact of different error terms, the comprehensive time error expression is normalized, and the adaptive weight coefficient based on fuzzy logic is introduced to obtain the normalized weighted error function. The fuzzy logic controller can dynamically adjust the weights of different error terms according to the uncertainty of the current state and environment, so that the system has the ability to adapt to different types of errors. The normalized weighted error function is expressed as:

[0082] ;

[0083] in, and is the adaptive weight coefficient given by the fuzzy logic controller, is the remaining step number error. Based on the normalized weighted error function, a model predictive controller is designed to construct the initial control law of the open-loop collaborative structure. The model predictive controller is a model-based control strategy that finds the optimal control input by predicting the future system behavior at each moment. The control law of the open-loop collaborative structure aims to predict the future system response based on the current state and goal, and ensure that the entire system reaches the goal as expected. The initial control law is expressed as:

[0084] ;

[0085] in, is the prediction time step, For the The weighted error of the step. In order to improve the robustness of the control, the initial control law is subjected to robustness analysis to ensure that it remains effective in the face of system uncertainty and external disturbances. Through robustness analysis, the final time collaborative predictive correction control law is obtained. The time collaborative predictive correction control law is combined with the collision avoidance constraint set to construct an optimal control problem with constraints. The collision avoidance constraint set is used to ensure that the robot does not collide with the edge of the step while maintaining its posture stability during the elevator ride. The final optimal control problem is expressed as:

[0086] ;

[0087] Among them, the constraints include the safe distance constraint between the robot and the steps and the posture adjustment constraint. By solving the optimal control problem with constraints, a control input sequence that satisfies all the constraints is obtained. Based on the solved control input sequence, numerical integration is performed in combination with the discrete time state space model to calculate the time cooperative control parameters. Numerical integration is used to convert discrete control inputs into continuous system responses, thereby describing the motion trajectory of the robot during the entire elevator riding process.

[0088] In one example, kinematic parameters, collision avoidance constraint set and time cooperative control parameters are input into a numerical optimal control algorithm, and sequential quadratic programming is used to solve the problem to obtain the optimal trajectory of taking the elevator, including: constructing the state equation of the walking robot based on the kinematic equation containing step information in the kinematic parameters, and establishing a discrete-time dynamic model of the system in combination with the time cooperative control parameters; according to the collision avoidance constraint set, the safety distance constraint between the robot body and the step edge and the robot posture adjustment constraint are converted into inequality constraints of state variables and control variables, and a constraint function set is constructed; based on the normalized weighted error function in the time cooperative control parameters, the objective function of the optimal control problem is designed, and the elevator time and energy are converted into the inequality constraints of the state variables and control variables. The amount of consumption is used as the optimization indicator to obtain the multi-objective optimization expression, and the multi-objective optimization expression is linearized to obtain the linearized objective function of each time step; the linearized objective function and the constraint function set are substituted into the sequential quadratic programming framework, and the quadratic programming sub-problem is constructed to obtain the local optimal solution sequence, and based on the local optimal solution sequence, the line search method is used to determine the optimal step size, update the state variables and control variables, and obtain the iterative optimization sequence; the convergence analysis of the iterative optimization sequence is performed, and the termination condition is set. When the convergence accuracy requirement is met, the iteration is stopped to obtain the optimal control input sequence, and the optimal control input sequence is substituted into the discrete time dynamic model for numerical integration to obtain the optimal elevator trajectory of the walking robot.

[0089] In this example, the state equation of the walking robot is constructed based on the kinematic equation containing step information in the kinematic parameters. The kinematic equation describes how the geometric relationship and posture of each wheel of the robot change over time. Combining these kinematic parameters, a complete state equation is obtained, which is expressed as:

[0090] ;

[0091] in, is the system state vector, which contains the position, speed, posture and other information of the robot on the steps, and The control input vector describes the driving force and torque of each driving wheel. is a nonlinear function that describes the evolution of the state with the control input. On the basis of establishing the state equation, the system is discretized to construct a discrete time dynamic model. Assume that the sampling time of the system is , the discretized state equation is expressed as:

[0092] ;

[0093] in, and are the state matrix and input matrix of the discrete-time system, describing how the state and input interact in discrete time steps. and Respectively The model converts the system from continuous time to discrete time, which is convenient for the application of digital controllers and numerical solutions. In order to ensure the safety and stability of the robot in the process of taking the stairs, the safety distance constraint between the robot body and the edge of the step and the robot posture adjustment constraint are converted into inequality constraints of state variables and control variables according to the collision avoidance constraint set, and the constraint function set is constructed. Assume that the safety distance between the robot and the step is , while the actual distance is , then define the safety distance constraint as:

[0094] ;

[0095] Similarly, for the posture adjustment constraint, assuming the robot posture angle is , and in order to maintain stability, its angle must be within a certain range, so the constraint is expressed as:

[0096] ;

[0097] These inequality constraints describe the physical restrictions that the state variables and control variables need to meet to ensure that the robot does not collide with the steps during the entire elevator ride and that its posture remains within a safe range. Based on the normalized weighted error function in the time-cooperative control parameter, the objective function of the optimal control problem is designed. The core of time-cooperative control is to ensure that the robot can move in the expected time and energy-optimal manner during the elevator ride. The elevator ride time and energy consumption are used as optimization indicators to construct a multi-objective optimization expression. Assume that the elevator ride time is , the energy consumption is , then the multi-objective optimization expression is expressed as:

[0098] ;

[0099] in, is the objective function, and is the weight coefficient, controlling the importance of time and energy in the optimization objective, is the energy consumption function, which is proportional to the square of the control input. In order to facilitate the solution, the multi-objective optimization expression is linearized to obtain the linearized objective function of each time step, so that the entire optimization problem can be solved by linear programming. Substitute the linearized objective function and constraint function set into the sequential quadratic programming framework to construct the quadratic programming subproblem. In the sequential quadratic programming framework, the nonlinear optimization problem is decomposed into a series of quadratic programming subproblems. By solving each subproblem, the global optimal solution is gradually approached. Assume that the current state is , the control input is , then the quadratic programming subproblem is expressed as:

[0100] ;

[0101] in, is the Hessian matrix of the objective function, is the gradient vector, is the increment of the control input. By solving the quadratic programming subproblem, a sequence of local optimal solutions is obtained. In order to optimize the local solution, the line search method is used to determine the optimal step size, update the state variables and control variables, and obtain the iterative optimization sequence. The line search adjusts the step size to ensure that each iteration moves in the optimal direction, thereby accelerating the convergence process. The convergence analysis of the iterative optimization sequence is performed and the termination condition is set. When the convergence accuracy requirements are met, the iteration is stopped and the optimal control input sequence is obtained. The termination condition is that the change in the control input is less than a set threshold, or the change in the objective function is less than a set value, ensuring that the final solution is close enough to the global optimal solution. The optimal control input sequence is substituted into the discrete time dynamic model for numerical integration to obtain the optimal trajectory of the walking robot. Numerical integration is used to convert discrete control inputs into the evolution trajectory of the system state, thereby describing the specific motion path of the robot during the entire elevator riding process.

[0102] In one example, a self-service elevator riding process is executed according to the optimal elevator riding trajectory and elevator riding gait parameters, and the robot state and elevator riding stability are monitored in real time. When it is detected that the state deviation exceeds the preset threshold, the time collaborative prediction and correction control law is triggered to adjust the trajectory in real time, and a dynamic elevator riding control strategy is generated, including: inputting the optimal elevator riding trajectory and elevator riding gait parameters into the trajectory tracking controller, using the sliding mode variable structure control algorithm to generate a real-time wheel train drive instruction sequence, and based on the real-time wheel train drive instruction sequence, the wheel train actuator of the walking robot is closed-loop controlled, and the real-time state information of the robot is collected through sensors to obtain the robot state feedback data; the robot state feedback data is processed by Kalman filtering to obtain the filtered state estimate value, and the filtered state estimate value is obtained. The calculated value is compared with the optimal elevator trajectory, the state deviation is calculated, and the adaptive threshold is set based on the fuzzy logic rule to obtain the state deviation evaluation result; according to the state deviation evaluation result, it is determined whether to trigger the trajectory adjustment. When the state deviation exceeds the preset threshold, the time collaborative predictive correction control law is activated, and based on the time collaborative predictive correction control law, a multi-step prediction of the current state is performed, and a local trajectory optimization problem is constructed in combination with the collision avoidance constraint set; the local trajectory optimization problem is solved to obtain the local optimal control input sequence, and the local optimal control input sequence is smoothly merged with the optimal control input sequence to generate the adjusted control command, and the adjusted control command is input into the trajectory tracking controller, the real-time wheel train drive command sequence is updated, and a dynamic elevator control strategy is generated.

[0103] In this example, the optimal elevator trajectory and gait parameters are input into the trajectory tracking controller to ensure that the robot can take the elevator along the planned optimal path. In order to deal with uncertainty and external interference, the sliding mode variable structure control algorithm is used to generate a real-time gear train drive instruction sequence. The sliding mode variable structure control algorithm is a control method with strong robustness, which can maintain the stability of the system and accurate trajectory tracking in the case of system parameter uncertainty and external disturbance. Assume that the state of the system is And the optimal elevator trajectory is , the sliding surface is defined as:

[0104] ;

[0105] in, is the sliding surface, which describes the deviation between the current state and the reference state. By designing a suitable control law, the sliding surface Approaching zero, achieving accurate trajectory tracking. The control law adopts a discontinuous structure, for example:

[0106] ;

[0107] in, is the control input, and is the control gain, sign is a symbolic function used to switch the control input so that the system slides on the sliding surface and finally reaches the desired trajectory. Based on the generated real-time wheel train drive instruction sequence, the wheel train actuator of the walking robot is closed-loop controlled. In closed-loop control, the driving state of each wheel is adjusted in real time to keep the robot on the planned trajectory while responding to external disturbances. In order to achieve closed-loop control, the state information of the robot is collected in real time through the sensor system to obtain state feedback data. The sensor data includes the wheel speed, position, and overall attitude angle of the robot. Through these data, the current motion state of the robot can be understood. In order to improve the accuracy of state estimation, the robot state feedback data is processed by Kalman filtering. Kalman filtering is an optimal state estimation algorithm that obtains more accurate state information in the presence of noise. Assume that the feedback data is , the state is estimated to be The update formula of Kalman filter is:

[0108] ;

[0109] in, is the prior state estimate, is the Kalman gain, is the observation matrix, describing the relationship between the measurement data and the state. The Kalman gain is calculated by minimizing the estimation error covariance, so that the state estimation remains accurate in the presence of measurement noise. The filtered state estimate is compared with the optimal elevator trajectory to calculate the state deviation. The state deviation is a measure of the difference between the current state of the robot and the predetermined trajectory. Assume that the state deviation is In order to evaluate the severity of the deviation, an adaptive threshold is set based on fuzzy logic rules. The fuzzy logic controller can handle uncertainty and dynamically adjust the threshold so that the system can make judgments based on different states and environmental conditions. Assume that the state deviation threshold is , the fuzzy logic controller sets an appropriate threshold according to the state deviation and the stability of the system in order to trigger the trajectory adjustment in time. According to the state deviation evaluation result, determine whether the trajectory adjustment needs to be triggered. When the state deviation exceeds the preset adaptive threshold, the time collaborative prediction and correction control law is activated. The time collaborative prediction and correction control law is used to perform multi-step prediction and correction on the system when the deviation is large to ensure that the robot returns to the optimal trajectory as soon as possible. Based on the time collaborative prediction and correction control law, a multi-step prediction of the current state is performed, and a local trajectory optimization problem is constructed in combination with the collision avoidance constraint set. The purpose of the local trajectory optimization problem is to find a new trajectory under the premise of considering the safety distance constraints and posture stability constraints, so that the robot can safely return to the optimal path. Solve the local trajectory optimization problem to obtain the local optimal control input sequence. Assume that the local optimal control input is , and the optimal control input is In order to smoothly combine the two control sequences and avoid the instability caused by mutations, a weighted fusion method is used for smooth fusion. The fused control input is expressed as:

[0110] ;

[0111] in, is the fusion coefficient, which controls the ratio of local control input and optimal control input. Dynamic adjustments are made according to the size of the state deviation. The larger the deviation, the higher the weight of the local control, so that the trajectory can be corrected faster. The adjusted control instructions are input into the trajectory tracking controller, the real-time gear train drive instruction sequence is updated, and the dynamic elevator control strategy is generated. The dynamic control strategy ensures that the robot can make adaptive adjustments when facing an uncertain step environment through real-time state feedback, deviation evaluation, trajectory prediction and local optimization. For example, when the robot goes up and down a step with a height of 20 cm, its posture is offset due to external disturbances, and the state deviation exceeds the preset threshold. The time collaborative prediction and correction control law is activated to predict the motion within the next few time steps, and a new trajectory is found in combination with collision avoidance constraints, so that the robot can safely return to the planned optimal path. By smoothly fusing the local optimal control input and the original optimal control input, a new control instruction is generated to ensure that the entire adjustment process is smooth and stable, avoiding vibration or instability caused by sudden changes.

[0112] Reference Figure 2 This embodiment provides a self-service elevator device for a walking robot, comprising:

[0113] Building module 1, used to establish a robot kinematics model of the walking robot, construct a mapping relationship between the end of the gear train and the step height, and obtain kinematic parameters;

[0114] A calculation module 2 is used to calculate the gait parameters of taking the ladder based on the kinematic parameters and the static stability criterion of the ground projection point of the center of gravity;

[0115] A generation module 3 is used to construct a nonlinear programming framework and generate a collision avoidance constraint set, wherein the collision avoidance constraint set includes a safety distance constraint between the robot body and the step edge and a robot posture adjustment constraint;

[0116] Processing module 4 is used to create a time collaborative prediction and correction control law based on the open-loop collaborative structure, introduce a normalized weighted error function, and calculate the time collaborative control parameters;

[0117] Solving module 5, used for inputting kinematic parameters, collision avoidance constraint set and time coordinated control parameters into a numerical optimal control algorithm, solving by sequential quadratic programming, and obtaining an optimal elevator riding trajectory;

[0118] Adjustment module 6 is used to execute the self-service elevator riding process according to the optimal elevator riding trajectory and elevator riding gait parameters, monitor the robot status and elevator riding stability in real time, and when it is detected that the state deviation exceeds the preset threshold, trigger the time collaborative prediction and correction control law to adjust the trajectory in real time and generate a dynamic elevator riding control strategy.

[0119] In this embodiment, for the specific implementation of each unit in the above device embodiment, please refer to the above method embodiment, which will not be repeated here.

[0120] Reference Figure 3 In an embodiment of the present invention, a computer device is also provided. The computer device may be a server, and its internal structure may be as follows: Figure 3 As shown. The computer device includes a processor, a memory, a display screen, an input device, a network interface and a database connected through a system bus. Among them, the processor designed by the computer is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store the corresponding data in this embodiment. The network interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, the above method is implemented.

[0121] Those skilled in the art will understand that Figure 3 The structure shown in the figure is merely a block diagram of a portion of the structure related to the solution of the present invention, and does not constitute a limitation on the computer device to which the solution of the present invention is applied.

[0122] An embodiment of the present invention further provides a computer-readable storage medium on which a computer program is stored, and when the computer program is executed by a processor, the above method is implemented. It can be understood that the computer-readable storage medium in this embodiment can be a volatile readable storage medium or a non-volatile readable storage medium.

[0123] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media provided by the present invention and used in the embodiments may include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in a variety of forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double-speed data rate SDRAM (SSRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM.

[0124] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, device, article or method including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, device, article or method. In the absence of further restrictions, an element defined by the sentence "includes a ..." does not exclude the presence of other identical elements in the process, device, article or method including the element.

[0125] The above description is only a preferred embodiment of the present invention, and does not limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made by using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for a mobility robot to take an elevator by itself, characterized in that: The following steps are involved: Establish the robot kinematics model of the walking robot, construct the mapping relationship between the end of the gear train and the step height, and obtain the kinematic parameters; Calculating the gait parameters of taking the stairs based on the kinematic parameters and the static stability criterion of the center of gravity ground projection point; A nonlinear programming framework is constructed and a collision avoidance constraint set is generated, wherein the collision avoidance constraint set includes a safety distance constraint between the robot body and the step edge and a robot posture adjustment constraint; specifically, the following steps are used: based on the kinematic equations containing step information and the geometric parameters of the wheel train, a three-dimensional geometric model of the walking robot body is constructed, and the robot shape is described using a superquadratic surface function to obtain a boundary expression of the robot body; discrete sampling of the boundary expression is performed to generate boundary point cloud data, and feature extraction of the point cloud data is performed to construct a minimum circumscribed ellipsoid model of the robot body to obtain the center coordinates and semi-axis length of the ellipsoid; according to the mapping relationship between the end of the wheel train and the step height and the step height parameters, a three-dimensional spatial curve model of the step edge is established to accurately describe the step edge and obtain a parameterized equation of the step edge; based on The ellipsoid model and the parameterized equation of the step edge are used to construct the minimum distance function between the robot body and the step edge, solve the minimum distance point pair, and introduce the exponential potential field function as a safety distance metric to obtain the safety distance constraint between the robot body and the step edge; the continuous stair-riding gait trajectory in the stair-riding gait parameter is decomposed by Euler angles, the pitch angle, roll angle and yaw angle of the robot during the stair-riding process are extracted, and a continuous posture angle function is generated; based on the posture angle function, a robot posture stability evaluation index is constructed, the zero moment point theory is introduced to calculate the dynamic stability margin, and the fuzzy logic controller is combined to set the adaptive posture adjustment threshold to obtain the robot posture adjustment constraint; the safety distance constraint between the robot body and the step edge and the robot posture adjustment constraint are combined to obtain a collision avoidance constraint set; Based on the open-loop collaborative structure, a time collaborative prediction and correction control law is created, and a normalized weighted error function is introduced to calculate the time collaborative control parameters. The kinematic parameters, the collision avoidance constraint set and the time coordinated control parameters are input into a numerical optimal control algorithm, and sequential quadratic programming is used to solve the problem to obtain an optimal elevator riding trajectory; The self-service elevator riding process is executed according to the optimal elevator riding trajectory and the elevator riding gait parameters, and the robot state and elevator riding stability are monitored in real time. When it is detected that the state deviation exceeds the preset threshold, the time collaborative prediction and correction control law is triggered to adjust the trajectory in real time to generate a dynamic elevator riding control strategy.

2. The method for self-service elevator riding by a walking robot according to claim 1, characterized in that: The kinematic model of the walking robot is established, a mapping relationship between the end of the gear train and the step height is constructed, and kinematic parameters are obtained, including: The wheel train configuration of the walking robot is parametrically modeled, each wheel is regarded as an independent driving unit, the wheel train geometric parameters and driving parameters are obtained, and based on the wheel train geometric parameters and the driving parameters, a coordinate transformation matrix from the robot base to each wheel is constructed; Performing matrix multiplication operation on the coordinate transformation matrix to construct an overall transformation matrix from the robot base to the end of the wheel train, and based on the overall transformation matrix, constructing a forward kinematics equation of the end position of the wheel train and the wheel angle, and introducing a step height parameter to obtain a kinematics equation containing step information; The Jacobian matrix is ​​derived for the kinematic equation containing the step information to obtain a velocity Jacobian matrix and an acceleration Jacobian matrix that take into account the influence of the step, and a preset step height range parameter is input into the kinematic equation containing the step information, and the inverse kinematics problem is solved by a numerical iteration method to obtain a mapping relationship function between the end of the gear train and the step height; Based on the velocity Jacobian matrix and the acceleration Jacobian matrix, combined with mass distribution information of the walking robot, a dynamic equation considering the influence of steps is constructed; The kinematic equation containing the step information, the mapping relationship function, the velocity Jacobian matrix, the acceleration Jacobian matrix and the dynamic equation considering the step effect are combined to obtain kinematic parameters.

3. The method for using a walking robot to take an elevator by itself according to claim 2, characterized in that: The step of calculating the gait parameters of taking the ladder based on the kinematic parameters and the static stability criterion of the center of gravity ground projection point comprises: According to the mass distribution information in the kinematic parameters and the dynamic equation considering the influence of the steps, a multi-body dynamic model of the walking robot is constructed, and the total center of mass position coordinates of the robot under different wheel train configurations are calculated; Based on the mapping relationship function between the total center of mass position coordinates and the end of the wheel train and the step height, the trajectory equation of the center of gravity ground projection point is constructed to obtain the center of gravity projection trajectory during the elevator ride, and the center of gravity projection trajectory is discretized and sampled. The support polygon corresponding to each sampling point is calculated based on the wheel train geometric parameters to construct a time-varying support polygon sequence; Substituting the time-varying support polygon sequence into the static stability criterion of the center of gravity ground projection point, establishing a stability constraint equation group considering time-varying factors, and designing an initial stair-riding gait based on the kinematic equation containing step information and the mapping relationship function between the end of the wheel train and the step height, and generating a candidate trajectory set of the wheel train motion; Inputting the candidate trajectory set of the wheel train motion into the stability constraint equation group, solving the wheel train motion trajectory that meets the stability requirement, obtaining a ladder gait sequence, and performing B-spline curve fitting on the ladder gait sequence to obtain a continuous ladder gait trajectory; The continuous stair-riding gait trajectory is substituted into the dynamic equation considering the influence of steps, and the driving torque sequence of each wheel train during the stair-riding process is obtained by inverse dynamics calculation, and the stair-riding gait parameters are generated by combining the velocity Jacobian matrix and the acceleration Jacobian matrix.

4. The method for self-service elevator riding by a mobility robot according to claim 3, characterized in that: The time collaborative prediction and correction control law is created based on the open-loop collaborative structure, and a normalized weighted error function is introduced to calculate the time collaborative control parameters, including: According to the continuous stair-riding gait trajectory and the dynamic equation considering the influence of the steps, a nonlinear state space model of the walking robot is constructed, and a Taylor series expansion is used for linearization processing to obtain a linearized system state equation and an output equation; Discretizing the linearized system state equation to obtain a discrete-time state-space model, and designing an adaptive extended Kalman filter based on the discrete-time state-space model to estimate the system state and obtain a state estimation value sequence; According to the state estimation value sequence and the preset expected elevator riding time, a time coordination error function is constructed, and the remaining step number error is introduced to obtain a comprehensive time error expression, and the comprehensive time error expression is normalized, and an adaptive weight coefficient based on fuzzy logic is introduced to obtain a normalized weighted error function; Based on the normalized weighted error function, a model predictive controller is designed to construct an initial control law of an open-loop collaborative structure, and a robustness analysis is performed on the initial control law expression to obtain a time collaborative predictive correction control law; The time-cooperative predictive correction control law is combined with the collision avoidance constraint set to construct a constrained optimal control problem, and a control input sequence that satisfies the constraint conditions is solved. Numerical integration is performed based on the control input sequence and the discrete-time state-space model to calculate the time-cooperative control parameters.

5. The method for self-service elevator riding by a mobility robot according to claim 4, characterized in that: The kinematic parameters, the collision avoidance constraint set and the time coordinated control parameters are input into a numerical optimal control algorithm, and sequential quadratic programming is used to solve the optimal elevator riding trajectory, including: Based on the kinematic equation containing step information in the kinematic parameters, a state equation of the walking robot is constructed, and a discrete time dynamic model of the system is established in combination with the time coordinated control parameters; According to the collision avoidance constraint set, the robot body and step edge safety distance constraint and the robot posture adjustment constraint are converted into inequality constraints of state variables and control variables, and a constraint function set is constructed; Based on the normalized weighted error function in the time coordinated control parameter, the objective function of the optimal control problem is designed, the elevator time and energy consumption are used as optimization indicators, a multi-objective optimization expression is obtained, and the multi-objective optimization expression is linearized to obtain a linearized objective function for each time step; Substituting the linearized objective function and the constraint function set into a sequential quadratic programming framework, constructing quadratic programming subproblems, obtaining a local optimal solution sequence, and based on the local optimal solution sequence, using a line search method to determine an optimal step size, updating state variables and control variables, and obtaining an iterative optimization sequence; The iterative optimization sequence is subjected to convergence analysis, and termination conditions are set. When the convergence accuracy requirement is met, the iteration is stopped to obtain the optimal control input sequence, and the optimal control input sequence is substituted into the discrete-time dynamic model for numerical integration to obtain the optimal elevator trajectory of the walking robot.

6. The method for self-service elevator riding by a walking robot according to claim 5, characterized in that: The self-service elevator riding process is performed according to the optimal elevator riding trajectory and the elevator riding gait parameters, and the robot state and elevator riding stability are monitored in real time. When it is detected that the state deviation exceeds a preset threshold, the time collaborative prediction and correction control law is triggered to adjust the trajectory in real time, and a dynamic elevator riding control strategy is generated, including: The optimal elevator trajectory and the elevator gait parameters are input into a trajectory tracking controller, a sliding mode variable structure control algorithm is used to generate a real-time wheel train drive instruction sequence, and based on the real-time wheel train drive instruction sequence, a closed-loop control is performed on the wheel train actuator of the walking robot, and real-time state information of the robot is collected through a sensor to obtain robot state feedback data; Performing Kalman filtering on the robot state feedback data to obtain a filtered state estimation value, and comparing the filtered state estimation value with the optimal elevator riding trajectory to calculate the state deviation, and setting an adaptive threshold based on fuzzy logic rules to obtain a state deviation evaluation result; According to the state deviation evaluation result, determine whether to trigger trajectory adjustment, when the state deviation exceeds a preset threshold, activate the time collaborative prediction and correction control law, and based on the time collaborative prediction and correction control law, perform multi-step prediction on the current state, and combine the collision avoidance constraint set to construct a local trajectory optimization problem; Solve the local trajectory optimization problem to obtain a local optimal control input sequence, and smoothly merge the local optimal control input sequence with the optimal control input sequence to generate an adjusted control instruction, input the adjusted control instruction into the trajectory tracking controller, update the real-time wheel train drive instruction sequence, and generate a dynamic elevator control strategy.

7. A self-service elevator device for a walking robot, characterized in that: For implementing the steps of the method according to any one of claims 1 to 6, the self-service elevator riding device of the walking robot comprises: A construction module is used to establish a robot kinematics model of the walking robot, construct a mapping relationship between the end of the gear train and the step height, and obtain kinematic parameters; A calculation module, used for calculating the gait parameters of taking the ladder based on the kinematic parameters and the static stability criterion of the ground projection point of the center of gravity; A generation module, used to construct a nonlinear programming framework and generate a collision avoidance constraint set, wherein the collision avoidance constraint set includes a safety distance constraint between the robot body and the step edge and a robot posture adjustment constraint; A processing module is used to create a time collaborative prediction and correction control law based on an open-loop collaborative structure, introduce a normalized weighted error function, and calculate the time collaborative control parameters; A solution module, used for inputting the kinematic parameters, the collision avoidance constraint set and the time coordinated control parameters into a numerical optimal control algorithm, and solving the problem by sequential quadratic programming to obtain an optimal elevator riding trajectory; The adjustment module is used to execute the self-service elevator riding process according to the optimal elevator riding trajectory and the elevator riding gait parameters, monitor the robot state and elevator riding stability in real time, and when it is detected that the state deviation exceeds the preset threshold, trigger the time collaborative prediction and correction control law to adjust the trajectory in real time and generate a dynamic elevator riding control strategy.

8. A computer device comprising a memory and a processor, wherein a computer program is stored in the memory, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images