Obstacle avoidance method for six-wheel independent driving and independent steering robot

Abstract

The application discloses a kind of six-wheel independent drive independent steering robot obstacle avoidance method, including solving obstacle avoidance path and the obstacle avoidance strategy based on fuzzy decision;When sensor detects obstacle, obstacle information is sent to obstacle avoidance controller and obstacle avoidance planner, then the best path point is solved by the constraint of actual environment, the best path is obtained after trajectory smoothing and polynomial fitting, the best path is discretized into coordinate points after output, after the selection of obstacle avoidance strategy is carried out, obstacle avoidance is carried out according to strategy, the whole obstacle avoidance process is ended.The application solves the obstacle avoidance problem of six-wheel independent drive independent steering robot in the process of trajectory tracking when encountering static obstacle and dynamic obstacle, and the actual situation and physical model have been verified on the robot, in addition, the application also expands various different kinds of situations and new robot models in the process of robot obstacle avoidance, and provides certain reference for other obstacle avoidance methods.

Description

Technical Field

[0001] This invention belongs to the field of motion control technology for wheeled mobile robots, specifically relating to an obstacle avoidance method and strategy for wheeled robots. Background Technology

[0002] With the development of intelligent control technology, mobile robots are finding increasingly important applications in industrial production, military defense, and teaching and research. Mobile robots are mainly divided into tracked mobile robots, legged mobile robots, and wheeled mobile robots. Compared with tracked and legged robots, wheeled mobile robots are more maneuverable, have a simpler structure, are easier to control, and have a stronger load-bearing capacity. Therefore, motion control research on wheeled mobile robots is one of the important directions for future intelligent robot research.

[0003] In research on motion control of mobile robots, path planning and trajectory tracking are crucial issues. However, the appearance of static or dynamic obstacles is a common occurrence in practical control. Furthermore, autonomous obstacle avoidance capability is a key indicator of a mobile robot's intelligence level and a vital guarantee for its stable, safe, and efficient task completion in complex environments. Therefore, research on intelligent obstacle avoidance in robots has consistently been a popular topic.

[0004] The six-wheeled independently driven, independently steering robot is a novel wheeled robot model. Compared to traditional wheeled mobile robots, it represents a significant improvement in single-unit load capacity, climbing ability, and driving capability. Therefore, it is more adaptable to various terrains and is of great importance for transporting goods, relief supplies, and building materials in challenging environments. Furthermore, the six-wheeled independently driven, independently steering robot is electrically powered, greatly reducing noise, and the number of drive wheels can be adjusted according to working conditions to improve endurance. Due to its significantly enhanced load capacity, robotic arms can be added to the platform to handle various tasks. Moreover, its high flexibility and mobility allow it to adapt to various extreme working conditions, making it highly applicable in disaster relief, geological exploration, and other specialized fields.

[0005] Fuzzy decision-making refers to the mathematical theories and methods for making decisions in fuzzy environments. Strictly speaking, most real-world decisions are fuzzy decisions. Research on fuzzy decision-making started relatively late, but it covers a wide range of areas and still lacks a clearly defined scope. Commonly used fuzzy decision-making methods include fuzzy ranking, fuzzy optimization, and fuzzy strategy. Fuzzy optimization mainly refers to seeking the optimal solution given a set of solutions, various objective functions, and constraints. If the objective function or constraints are fuzzy, then the optimization is called fuzzy optimization. One way to fuzzify the objective function is to use fuzzy numbers as the objective function value and seek the conditional extrema through the analysis and calculation of fuzzy numbers. Fuzzifying constraints involves defining the constraints as fuzzy sets. In linear programming, this generalization has led to the research of fuzzy linear programming, resulting in a wider range of applications for ordinary linear programming, making it more flexible and adaptable to various situations.

[0006] The main function of obstacle avoidance strategies is to enable robots to avoid obstacles in real time after detecting them through sensors and generating an obstacle avoidance path. Since obstacle movement in real-world environments is often complex, the obstacle avoidance path needs to be adjusted at every step after its generation. Obstacle avoidance strategies are a crucial research topic in intelligent obstacle avoidance for robots, and the selection of a suitable strategy directly impacts the robot's obstacle avoidance performance.

[0007] Current obstacle avoidance research mainly focuses on situations where obstacles appear in the robot's forward direction, without analyzing other scenarios. Therefore, analyzing obstacles appearing in all directions during robot movement is of great significance for generating suitable obstacle avoidance paths and effective obstacle avoidance strategies in the field of intelligent robot obstacle avoidance research. Summary of the Invention

[0008] To address the problems of existing obstacle avoidance technologies for six-wheeled independently driven and independently steering robots during trajectory tracking, this invention provides an obstacle avoidance method for such robots. This method solves the obstacle avoidance path and the obstacle avoidance strategy based on different obstacle conditions, thereby resolving the obstacle avoidance problem when encountering different dynamic / static obstacles during movement.

[0009] The obstacle avoidance method for a six-wheeled independently driven and independently steering robot of the present invention adopts the following scheme:

[0010] This includes solving obstacle avoidance paths and developing obstacle avoidance strategies based on fuzzy decision-making. When a sensor detects an obstacle, it sends the obstacle information to the obstacle avoidance controller and the obstacle avoidance planner. Then, it solves for the optimal path point by considering the constraints of the actual environment (distance constraints, control constraints, etc.). The optimal path is obtained through trajectory smoothing and polynomial fitting. The optimal path is discretized into coordinate points and output. After outputting the coordinate points, an obstacle avoidance strategy is selected, and obstacle avoidance is performed according to the strategy. The entire obstacle avoidance process ends.

[0011] I. The process of solving the obstacle avoidance path is as follows:

[0012] During the obstacle avoidance path planning process, the robot's size and the obstacle's size are expanded. The obstacle is expanded according to the necessary safety margin for both the robot and the obstacle, as well as the obstacle's information. Because of the safety margin, the robot's expansion range is also included in this part (during the obstacle avoidance path planning process). Considering that there may be large obstacles and the robot may pass through the middle of the obstacle, the obstacle is segmented. Obstacle segmentation points are set at the obstacle outline for segmentation.

[0013] In solving the obstacle avoidance path, an obstacle avoidance penalty function and a cost function for deviating from the reference trajectory are designed. The obstacle avoidance penalty function is used in the solution and constraint of the obstacle avoidance function. The magnitude of the function is adjusted by the distance deviation between the robot's centroid coordinates and the obstacle segmentation point. The closer the distance, the larger the function value. The cost function for the robot's deviation from the reference trajectory is used as a constraint condition for the solution.

[0014] Solving for the obstacle avoidance path directly yields the coordinates of the optimal path point, therefore the following definition is provided:

[0015] ξ=[x a y a ] T ,

[0016] Where, x a y a These are the coordinates of the obstacle avoidance path.

[0017] To improve obstacle avoidance accuracy, a distance penalty function D is defined. obs Add soft constraints to the obstacle avoidance function Where K is a very large positive integer, this is for the situation where a dynamic obstacle suddenly approaches the robot during the robot's movement. When the robot is close to the obstacle, the soft constraint term is larger and has a greater weighting effect; when the robot is far from the obstacle, the soft constraint term is smaller and has a smaller weighting effect.

[0018] Based on the above penalty terms and constraints, an obstacle avoidance planner based on mathematical optimization is designed as follows:

[0019]

[0020]

[0021] Where: P is the weight matrix; The term represents the control variable of the MPC, which in the obstacle avoidance planner constrains the obstacle avoidance path, ensuring that the obstacle avoidance path obtained by the planner conforms to the constraints of the robot's kinematic model; the result ξ obtained by the obstacle avoidance planner is the robot's obstacle avoidance path; P obs For the obstacle avoidance penalty function, D obs Distance penalty function. In the constraints, γ is the MPC control variable, ensuring that the result conforms to the robot's motion model, and γ is the cost function of the deviation reference trajectory.

[0022] The obstacle avoidance path solved by the obstacle avoidance planner is processed by curve fitting. Curve fitting generates smoothed trajectory points that meet robot constraints (such as continuous robot position, first-order continuous yaw angle, and second-order continuous acceleration).

[0023] The obstacle avoidance penalty function P obs as follows:

[0024]

[0025] Where, p obs These are the weight coefficients of the obstacle avoidance penalty function, (x i y i ) represents the coordinates of the obstacle segmentation point, and ∈ is a very small positive number to avoid the denominator being 0;

[0026] The cost function of the deviation reference trajectory is as follows:

[0027]

[0028]

[0029] Where, x min x max y min y max It is the global map coordinate range, x r y r The reference trajectory coordinates, x a y a These are the coordinates of the obstacle avoidance path.

[0030] The distance penalty function is:

[0031]

[0032] Where, x obs y obs Let x be the coordinates of the obstacle's center of gravity. rob y rob Let d be the coordinates of the robot's center of mass. obs These are the weighting coefficients of the distance penalty function.

[0033] The curve fitting process uses an nth-degree polynomial as the fitting curve, and the fitting is performed based on the least squares method. The fitting function has the following form:

[0034] X = a1x n +a2x n-1 +a3x n-2 +…+a n x+a n+1 ,

[0035] Y = b1y n +b2y n-1 +b3y n-2 +…+b n y+b n+1 ,

[0036] Where a = [a1, a2, ... a n+1 ], b = [b1, b2…b n+1 [x] and [y] are the coefficients of the polynomial X and Y, respectively; n = 4 is selected, that is, the order of fitting is 4, and X and Y are the coordinates of the fitted curve.

[0037] II. The obstacle avoidance strategy based on fuzzy decision-making is as follows:

[0038] The selection of robot obstacle avoidance strategy involves predicting the obstacle trajectory and robot trajectory to determine whether a collision will occur, and then selecting a strategy. For static obstacles, which are considered to have zero speed and acceleration, obstacle avoidance trajectory planning is performed when the safe distance threshold is exceeded. After the obstacle avoidance trajectory is planned, trajectory tracking can be performed.

[0039] For dynamic obstacles, if the obstacle's trajectory is a straight line, the threshold function is used to directly determine whether a collision will occur after a certain time, and then the optimal path is solved using constraints. If the obstacle's trajectory is a curve, the trajectory after a certain time is predicted based on the differential function of the obstacle's displacement with respect to time. The robot's motion at that time is determined based on the motion time when it intersects with the robot's trajectory. The distance between the center of mass of the robot and the obstacle is used to determine whether a collision will occur, and then the optimal path is solved.

[0040] In the selection of obstacle avoidance decisions for robots, an asymmetric model from fuzzy decision-making is adopted. The robot's obstacle avoidance situation is taken as a constraint, and the selection of the robot's obstacle avoidance strategy is taken as the objective function. In the asymmetric model, accepting the constraint is a prerequisite, and the status of the objective and the constraint is not symmetrical. Given the objective function f(x) on the universe of discourse X and the fuzzy set of constraints D on X, the optimal solution m that maximizes f under constraint D is a fuzzy subset on X. It has a membership function μm(x) equal to 0 when the set on the right side of the equation is empty. The given universe of discourse X is the set of all cases, the current situation encountered by the robot is the constraint D, and the objective function is the robot's obstacle avoidance strategy.

[0041]

[0042] The robot solves the objective function by using environmental information and obstacle information as constraints when encountering obstacles, and then selects an appropriate obstacle avoidance strategy.

[0043] The dynamic obstacles are specifically divided into the following three categories:

[0044] (1) In the first case, the direction of the obstacle's movement is the same as the direction of the robot's trajectory;

[0045] At this point, the obstacle avoidance trajectory planning involves the robot selecting the optimal trajectory from the left or right side of the obstacle to overtake it, and then returning to the original trajectory for tracking. After obstacle avoidance trajectory replanning, the new trajectory points are output to the trajectory tracking controller, which calculates the robot's control input based on the MPC algorithm for trajectory tracking. However, when the robot's center of mass is on the same horizontal line as the obstacle's center of mass, a speed feedback proportional control is added to the speed term to enable the robot to perform a significant overtaking maneuver and return to the original trajectory as quickly as possible. At the same time, differential control is added to prevent overshoot.

[0046]

[0047] Among them, v mpc The velocity, k, is the robot control quantity calculated by the MPC trajectory tracking controller. p1 and k d1 It refers to the speed feedback proportional control and derivative control coefficients, e 11 It is the deviation of the robot's obstacle avoidance trajectory from the original trajectory, t k It is the current moment of the robot's movement, t d1max The robot moves to d 1max Position and time, t d1min It is d 1min The position and time shown;

[0048] (2) In the second case, the direction of the obstacle's movement intersects with the direction of the robot's movement, and the obstacle moves at a relatively fast speed;

[0049] This situation involves determining whether a collision will occur at the intersection point of the robot's tracking trajectory and the obstacle's movement trajectory. The judgment is based on the movement speed of both the robot and the obstacle. If the obstacle's movement speed is relatively fast, the robot will appropriately decelerate before reaching the collision point and then overtake the obstacle from behind after it passes the intersection point before returning to the original trajectory to continue tracking. The distance between the robot and the obstacle should be small at first and then large. When the distance is small, the speed needs to be reduced, and speed feedback proportional control should be added to the MPC control quantity. When the distance is large, the robot should revert to MPC control and add derivative control to prevent overshoot when tracking the original trajectory.

[0050]

[0051] Among them, v mpc The speed of the robot control input calculated by MPC, k p2 and k d2 It refers to the speed feedback proportional control and derivative control coefficients, e 21 e2 is the deviation of the robot's obstacle avoidance trajectory from the original trajectory, e2 is the distance between the robot's center of mass and the obstacle's center of mass, and d is the distance between the robot's center of mass and the obstacle's center of mass. dis2 d is the absolute distance between the robot's center of mass and the obstacle's center of mass in the x-direction. 2min and d 2max These are the distance thresholds between the robot's center of mass and the obstacle;

[0052] (3) In the third case, the direction of the obstacle's movement intersects with the direction of the robot's movement, and the obstacle moves at a relatively slow speed;

[0053] Before reaching the collision point, the robot accelerates appropriately and overtakes the obstacle in front of it before it crosses the intersection point, and then returns to the original trajectory to track it. The distance between the robot and the obstacle is initially small and then increases. When the distance is small, the speed needs to be increased. A speed feedback proportional control is added to the MPC control quantity to speed up obstacle avoidance. When the distance is large, the robot reverts to MPC control and adds derivative control to prevent overshoot when tracking the original trajectory.

[0054]

[0055] Among them, v mpc The speed of the robot control input calculated by MPC, k p3 and k d3 It refers to the speed feedback proportional control and derivative control coefficients, e 31 e3 is the deviation of the robot's obstacle avoidance trajectory from the original trajectory, e3 is the distance between the robot's center of mass and the obstacle's center of mass, and d is the distance between the robot's center of mass and the obstacle's center of mass. dis3 d is the absolute distance between the robot's center of mass and the obstacle's center of mass in the x-direction. 3min and d 3maxThese are the distance thresholds between the robot's center of mass and the obstacle.

[0056] The obstacle avoidance method and strategy proposed in this invention solve the obstacle avoidance problem of a six-wheeled independently driven, independently steering robot encountering both static and dynamic obstacles during trajectory tracking. Furthermore, the invention has been validated on a robot based on actual conditions and physical models. In addition, this invention expands upon various types of obstacle avoidance scenarios and introduces new robot models, providing a reference for other obstacle avoidance methods.

[0057] Compared with the prior art, the present invention has the following characteristics:

[0058] 1. This invention provides obstacle avoidance scenarios under various obstacle conditions and proposes corresponding obstacle avoidance methods and strategies. By combining the movement of the obstacle with the planned obstacle avoidance path, obstacle avoidance is achieved through comprehensive control by adjusting the robot's own speed and selecting an appropriate obstacle avoidance strategy. This approach aligns with various complex situations encountered in reality, and the proposed solution can also address various types of obstacle avoidance problems.

[0059] 2. This invention uses a six-wheeled independently driven and independently steering model. Due to its complex control characteristics and the larger control computation compared to two-wheeled and four-wheeled robots, the six-wheeled independently driven and independently steering robot has received relatively little research attention in current wheeled mobile robot research. When planning obstacle avoidance paths, the large size of the six-wheeled robot necessitates comprehensive control considering both the robot's steering and the path conditions. The robot's motion state is determined through a global positioning system and encoders on each wheel axle, and precise control of individual wheel speed and steering ensures the accuracy of the robot's movement.

[0060] 3. A fuzzy decision-making method is adopted to select the obstacle avoidance strategy of the robot. The various types of obstacle avoidance problems encountered by the robot are used as constraints, and the obstacle avoidance strategy of the robot is used as the objective function. The asymmetric model in fuzzy optimization is used to solve the obstacle avoidance problem under different types of obstacles, which reduces the computational difficulty but ensures the reliability of obstacle avoidance. Attached Figure Description

[0061] Figure 1 This is a schematic diagram of the obstacle avoidance process in this invention.

[0062] Figure 2 This is a schematic diagram of obstacle expansion segmentation.

[0063] Figure 3 This is a schematic diagram of the partial trajectory fitting results.

[0064] Figure 4 This is a schematic diagram of a robot avoiding obstacles in the same direction.

[0065] Figure 5 This is a diagram illustrating obstacle avoidance when the robot's trajectory intersects with an obstacle (at a relatively high speed).

[0066] Figure 6 This is a diagram illustrating obstacle avoidance when the robot's trajectory intersects with an obstacle (at a relatively slow speed).

[0067] Figure 7 This is a diagram illustrating a collision in the forward direction.

[0068] Figure 8 This is a schematic diagram of collisions in the intersecting trajectories.

[0069] Figure 9 This is a schematic diagram of the kinematic model of a six-wheeled omnidirectional mobile robot.

[0070] Figure 10 This is a diagram illustrating the collision state.

[0071] Figure 11 This is a schematic diagram of a potential collision state.

[0072] Figure 12 This is a diagram illustrating a safe state.

[0073] Figure 13 This is an image illustrating the obstacle avoidance effect. Detailed Implementation

[0074] This invention proposes a method for solving obstacle avoidance paths and a fuzzy decision-based obstacle avoidance strategy selection for a six-wheeled independently driven, independently steering robot. This method combines obstacle avoidance with trajectory tracking control, improving obstacle avoidance accuracy through tracking and feedback of the robot's position, and solving obstacle avoidance problems in both static and dynamic obstacle environments. It should be noted that in this invention, obstacle information is acquired by processing point cloud data obtained from depth cameras and LiDAR sensors before being sent to the robot. This invention primarily focuses on the obstacle avoidance process after acquiring obstacle information; the acquisition of obstacle information itself is not described in detail.

[0075] In addition, this invention provides a new model of a six-wheeled independently driven robot and a new scenario of three obstacle avoidance problems in the field of mobile robot obstacle avoidance. Based on this, we can further explore more types of obstacle avoidance research for mobile robots, which has high promotional value.

[0076] The obstacle avoidance method and strategy for a six-wheeled independently driven, independently steering robot proposed in this invention are as follows: Figure 1 As shown, the specific steps include:

[0077] I. Establishing a six-wheel kinematic model

[0078] After acquiring an obstacle avoidance path, the robot needs to track that path. The trajectory tracking process involves calculating the optimal control sequence based on the robot's kinematic model. Assuming the robot is a rigid body with indeformable wheels and a sideslip angle β = 0 rad, the MPC trajectory tracking controller used in this invention tracks the obstacle avoidance path. The direct control variables are the speed of the center wheel and the rotation angle of the left front wheel; therefore, in the kinematic model analysis, the speed of the center wheel and the rotation angle of the left front wheel are used as the control variables. Thus, in... Figure 9 In the coordinate system shown, the robot's kinematic model is as follows:

[0079]

[0080]

[0081]

[0082] in, Indicates the position of the robot's center and yaw angle, [v δ l ] T The control inputs are the robot speed and the left front wheel rotation angle. δ l δ r These are the left front wheel angle and the right front wheel angle, respectively. Since the left and right rear wheel angles are the same size as the front wheels, only in the opposite direction, they will not be elaborated upon here. L is the distance between the front wheel and the center wheel axle (also the distance between the center wheel and the rear wheel axle), and D is the robot width. This invention does not simplify the robot width. Based on the angular relationship, the relationship between the left and right wheel angles is as follows:

[0083]

[0084]

[0085] Therefore, the relationship between the turning radius and the front wheel deflection angle can be deduced as follows:

[0086]

[0087] The rear wheels of the six-wheeled omnidirectional mobile robot adopt a deflection direction that is the same as the front wheels but opposite in direction. Therefore, the issue of the rear wheel angle will not be discussed here.

[0088] The robot is subject to a nonholonomic constraint, meaning its wheels will not slip and it will only roll. This constraint is described by the following equation:

[0089]

[0090] For wheeled mobile robots, state-space models are best suited for model prediction. From the above, the state-space expression based on the robot's kinematic model can be derived as follows:

[0091]

[0092] in, Here, the state variables of the robot are the same as those in the kinematic model above, representing the robot's center position and yaw angle, u = [v δ l ] T , which is the control quantity for the robot.

[0093] This kinematic model is a nonlinear model. The model predictive control algorithm used in this invention is the underlying algorithm of the robot trajectory tracking controller. After planning and tracking by model predictive control, error compensation is performed by sliding mode control algorithm. Therefore, in order to improve control efficiency, the nonlinear kinematic model needs to be simplified into a linear model for processing and discretized before predictive control is performed.

[0094] The model is linearized using a first-order Taylor expansion. During the robot's motion, the desired target state is the reference state, so a first-order Taylor expansion is performed at the reference state.

[0095]

[0096]

[0097] in, X r For the robot's reference state, u r Let A be the reference state for the control quantity, and let A and B be Jacobian matrices, which are the partial derivatives of the function f with respect to the state quantity and the control quantity, respectively. Thus, the nonlinear kinematic model is transformed into a linear kinematic model.

[0098] Discretizing the formula using the forward Euler method over the sampling time T yields:

[0099]

[0100]

[0101] Where I is the identity matrix. The above are the results of linearization and discretization after kinematic modeling of the robot, which will be used as the model input for MPC.

[0102] II. Obstacle Trajectory Prediction

[0103] The obstacle trajectory prediction in this invention is only used to determine the state of the robot and the obstacle, and is not used to accurately predict the obstacle trajectory.

[0104] During collision avoidance, a step of obstacle trajectory prediction is required. If the robot and obstacle will not appear within the collision window within a certain timeframe, obstacle avoidance is unnecessary; otherwise, an appropriate obstacle avoidance strategy needs to be selected based on the positional states of the robot and obstacle. Therefore, collision prediction is performed by categorizing the positional states of the robot and obstacle into three types: collision state, potential collision state, and safe state. Figure 10 , Figure 11 and Figure 12 As shown. The robot's trajectory serves as the reference trajectory input, primarily used to determine the trajectory of obstacles. Using the Lagrange fundamental polynomial and its truncation error formula, the motion prediction calculation formula for obstacles can be derived as follows:

[0105]

[0106] Among them, [x kt ,y kt ] represents k t The predicted obstacle position after sampling time [x k ,y k ]、[x k+1 ,y k+1 ]、[x k+2 ,y k+2 ] respectively represent the existing t k Time, t k+1 Time and t k+2 The position of the obstacle is observed at any given time. Using the obstacle motion prediction formula, the future trajectory of the obstacle can be predicted based on its known position, allowing for collision detection and the selection of an obstacle avoidance strategy.

[0107] Determining the robot's state relative to obstacles through obstacle trajectory prediction primarily relies on the distance between the robot and the obstacle after a certain time interval. If the distance is less than a safety margin, the state is classified as a collision state, and an appropriate obstacle avoidance strategy is selected based on the collision type. If the distance is slightly greater than the safety margin, the state is classified as a potential collision state, and obstacle trajectory prediction and state analysis continue at the next time step. If the distance is significantly greater than the safety margin, the state is classified as safe, and a collision with the obstacle is not considered likely.

[0108] III. Using a threshold function for judgment

[0109] Different threshold functions are designed to determine whether a collision will occur for different obstacle avoidance types. A safe distance threshold function is designed for the first case (collision state), and a collision threshold function is designed for the second and third cases (potential collision state and safe state).

[0110] Safe distance threshold function:

[0111] For situations where the obstacle's direction of motion is the same as the robot's direction of motion, a safe distance threshold function is designed to determine whether a collision-free safe path needs to be planned. After completing the obstacle avoidance maneuver, the robot returns to its original trajectory to continue trajectory tracking. Figure 7 As shown, the distance threshold function is defined as follows:

[0112]

[0113] Among them, v c and v obs Representing the robot's speed and the obstacle's speed respectively, a c and a obs These represent the robot's maximum acceleration and the obstacle's maximum acceleration, respectively. c d represents the time from when the robot controller issues a command to when the underlying motion module responds. s1 This represents the minimum safe distance and the robot's safety expansion margin.

[0114] Collision threshold function:

[0115] For situations where the obstacle's motion direction intersects with the robot's motion direction, a collision threshold function is designed to determine whether a collision-free safe path needs to be planned. This is primarily determined by the distance between the robot's and the obstacle's center of gravity at the current moment and predicted for t sampling time steps in the future (time step t being the moment the obstacle reaches the point of intersection with the robot's trajectory). Figure 8 As shown. The collision threshold function is defined as follows:

[0116] D th2 =min(d i (k), d i (k+1), ...d i (k+t))+d s2

[0117] Where, d i (k) represents the distance between the robot and the obstacle at time k, and d s2 This indicates the safety margin of the robot.

[0118] IV. Solving Obstacle Avoidance Paths

[0119] The process of finding an obstacle avoidance path involves the sensor detecting an obstacle and sending the obstacle information to the obstacle avoidance controller and planner. Then, the optimal path point is calculated based on environmental constraints (distance constraints, control constraints, etc.). The optimal path is obtained through trajectory smoothing and polynomial fitting, discretized into coordinate points, and output. An obstacle avoidance strategy is then selected (specific strategies are described below), and obstacle avoidance is performed according to the strategy. The entire obstacle avoidance process then concludes. The entire obstacle avoidance process is as follows: Figure 1 As shown.

[0120] During obstacle avoidance path planning, a certain degree of expansion is required based on the dimensions of both the robot and the obstacle. The obstacle is expanded according to the necessary safety margins for both the robot and the obstacle, along with the obstacle's information. Because of the safety margin, the robot's expansion range is also included in this part, so the robot does not require expansion. Considering the possibility of large obstacles that the robot might have to pass through, the obstacle is segmented, such as... Figure 2 As shown.

[0121] In solving obstacle avoidance paths, an obstacle avoidance penalty function needs to be designed. This penalty function is mainly used in solving and constraining obstacle avoidance functions. The basic idea is to adjust the function's value by the distance deviation between the robot's center of mass coordinates and the obstacle's dividing point; the closer the distance, the larger the function value. The obstacle avoidance penalty function P is designed as follows. obs as follows:

[0122]

[0123] Where, p obs These are the weight coefficients of the obstacle avoidance penalty function, (x i y i ) represents the coordinates of the obstacle segmentation point, and ∈ is a very small positive number to avoid the denominator being 0.

[0124] In addition to the obstacle avoidance penalty function, the solution for the obstacle avoidance function also requires designing the cost of the robot deviating from the reference trajectory as a constraint. The cost function for deviating from the reference trajectory is designed as follows:

[0125]

[0126]

[0127] Where, x min x max y min y max It is the global map coordinate range, x r y r The reference trajectory coordinates, x a ya These are the coordinates of the obstacle avoidance path.

[0128] Solving for the obstacle avoidance path directly yields the coordinates of the optimal path point, therefore the following definition is provided:

[0129] ξ=[x a y a ] T ,

[0130] Where, x a y a These are the coordinates of the obstacle avoidance path.

[0131] To improve obstacle avoidance accuracy, a distance penalty function D is defined. obs Add soft constraints to the obstacle avoidance function Where K is a very large positive integer, this term mainly addresses the situation where a dynamic obstacle suddenly approaches the robot during its movement. When the robot is close to the obstacle, the soft constraint term is larger and has a greater weighting effect; when the robot is far from the obstacle, the soft constraint term is smaller and has a smaller weighting effect. The distance penalty function is:

[0132]

[0133] Where, x obs y obs Let x be the coordinates of the obstacle's center of gravity. rob y rob Let d be the coordinates of the robot's center of mass. obs The distance penalty function weights are denoted as . The main control objective of the obstacle avoidance planner is to avoid obstacles and minimize deviation from the reference path. Therefore, based on the above penalty terms and constraints, an obstacle avoidance planner based on mathematical optimization is designed as follows:

[0134]

[0135]

[0136] Where P is the weight matrix. The term represents the control variable of the MPC (Multi-Process Control Controller). Its main function in the obstacle avoidance planner is to constrain the obstacle avoidance path, ensuring that the path obtained by the planner conforms to the constraints of the robot's kinematic model. The result of the obstacle avoidance planner's solution is the robot's obstacle avoidance path.

[0137] In the constraints of the obstacle avoidance function, the first term is a hard constraint on the robot's control quantity, and combining these constraints is key to solving the problem. Based on the deviation between the obstacle avoidance penalty function and the reference path, the optimal path within this range is determined. The sampling time of the obstacle avoidance planner is the same as that of the trajectory tracking controller, and after planning the obstacle avoidance path, a trajectory tracking strategy with a short prediction step size is adopted. The model prediction trajectory tracking controller, with its better accuracy, fully meets the requirements for path replanning, thus satisfying the robot's control needs in terms of real-time performance.

[0138] Obstacle avoidance mainly involves determining the obstacle avoidance path area around the obstacle, and then finding the coordinates of the specific path to be solved within that area, which is the obstacle avoidance path.

[0139] In obstacle avoidance controller algorithms, the optimal solution sequence is the one with the smallest distance deviation from the reference point within a finite time domain, and the obtained path points are given in the form of discrete points. As the obstacle conditions and reference trajectory conditions change, the number and position of the planned local trajectory reference points will also change. If this trajectory is directly input into the trajectory tracking controller, some redundant and repetitive trajectory points will increase the computational burden of the trajectory tracking controller, and the different distribution densities of different reference points make it difficult for the trajectory tracking controller to complete the trajectory tracking task based on discrete reference points.

[0140] In summary, it is necessary to process the obstacle avoidance path planned by the obstacle avoidance controller to achieve smooth integration between the planner and the trajectory tracking controller. The obstacle avoidance path solved by the obstacle avoidance planner is mainly in the form of discrete points and does not consider requirements such as path smoothness. Therefore, it is mainly processed by curve fitting. Curve fitting can generate trajectory points that meet robot constraints (such as continuous robot position, first-order continuous yaw angle, and second-order continuous acceleration).

[0141] For curve fitting, an nth-degree polynomial fitting is selected as the fitting curve, and the fitting is performed based on the least squares method. The fitting function is as follows:

[0142] X = a1x n +a2x n-1 +a3x n-2 +…+a n x+a n+1

[0143] Y = b1y n +b2y n-1 +b3y n-2 +…+b n y+b n+1

[0144] Where a = [a1, a2, ... a n+1], b = [b1, b2…b n+1 [x] and [y] are the coefficients of the polynomial X and Y, respectively. In this paper, n=4 is selected, that is, the order of fitting is 4, and X and Y are the coordinates of the fitted curve. Figure 3 The middle part is the fitting result of a segment of the replanned trajectory. Figure 3 The numbers * represent discrete trajectory points after curve smoothing by the trajectory replanning algorithm in the obstacle avoidance planner, and the curve represents the fitted trajectory curve.

[0145] V. Selection of Obstacle Avoidance Strategies

[0146] This section mainly analyzes the obstacle avoidance strategy selection for robots facing different obstacle avoidance situations.

[0147] The obstacle avoidance problem described in this invention mainly studies the planning and tracking of obstacle avoidance trajectories for six-wheeled omnidirectional mobile robots when obstacles appear during trajectory tracking. Therefore, the robot needs to select different obstacle avoidance trajectories and strategies for different types of obstacles.

[0148] While planning the trajectory, a control strategy needs to be designed to ensure the robot performs obstacle avoidance better. For static obstacles, they can be treated as dynamic obstacles with a velocity of 0; after planning the obstacle avoidance trajectory, trajectory tracking can then be performed. For dynamic obstacles, there are three cases:

[0149] (1) In the first case, the direction of the obstacle's movement is the same as the direction of the robot's trajectory.

[0150] like Figure 4 As shown, the obstacle avoidance trajectory planning at this stage primarily involves the robot selecting the optimal trajectory from the left or right side of the obstacle to overtake it, and then returning to the original trajectory for tracking. After obstacle avoidance trajectory replanning, the new trajectory points are output to the trajectory tracking controller, which calculates the robot's control input for trajectory tracking based on the MPC algorithm. However, when the robot's center of mass is on the same horizontal line as the obstacle's center of mass, a velocity feedback proportional control is added to the velocity term to enable the robot to perform a significant overtaking maneuver and return to the original trajectory as quickly as possible. Simultaneously, differential control is added to prevent overshoot.

[0151]

[0152] Among them, v mpc The velocity, k, is the robot control quantity calculated by the MPC trajectory tracking controller. p1 and k d1 It refers to the speed feedback proportional control and derivative control coefficients, e 11 It is the deviation of the robot's obstacle avoidance trajectory from the original trajectory, t k It is the current moment of the robot's movement, t d1max The robot moved to Figure 4The d shown 1max Position and time, t d1min yes Figure 4 d 1min The position and time shown.

[0153] (2) In the second case, the direction of the obstacle's movement intersects with the direction of the robot's movement, and the obstacle moves at a relatively high speed.

[0154] like Figure 5 As shown, the main challenge in this situation is determining whether a collision will occur at the intersection of the robot's tracking trajectory and the obstacle's movement trajectory. This determination primarily depends on the movement speeds of both the robot and the obstacle. If the obstacle's speed is relatively fast, the robot slows down appropriately before reaching the collision point, then overtakes the obstacle from behind after it passes the intersection point, before returning to its original trajectory to continue tracking. The distance between the robot and the obstacle initially decreases and then increases. When the distance is small, the speed needs to be reduced, and speed feedback proportional control is added to the MPC control input. When the distance is large, MPC control is reverted. Simultaneously, derivative control is added to prevent overshoot when tracking the original trajectory.

[0155]

[0156] Among them, v mpc The speed of the robot control input calculated by MPC, k p2 and k d2 It refers to the speed feedback proportional control and derivative control coefficients, e 21 e2 is the deviation of the robot's obstacle avoidance trajectory from the original trajectory, e2 is the distance between the robot's center of mass and the obstacle's center of mass, and d is the distance between the robot's center of mass and the obstacle's center of mass. dis2 d is the absolute distance between the robot's center of mass and the obstacle's center of mass in the x-direction. 2min and d 2max These are the distance thresholds between the robot's center of mass and the obstacle.

[0157] (3) In the third case, the direction of the obstacle's movement intersects with the direction of the robot's movement, and the obstacle's movement speed is relatively slow.

[0158] like Figure 6 As shown, the robot accelerates appropriately before reaching the collision point and overtakes the obstacle before it crosses the intersection point, then returns to its original trajectory for tracking. The distance between the robot and the obstacle increases gradually; when the distance is small, the speed needs to be increased, and speed feedback proportional control is added to the MPC control to accelerate obstacle avoidance. When the distance is large, it reverts to MPC control. Simultaneously, derivative control is added to prevent overshoot when tracking the original trajectory.

[0159]

[0160] Among them, v mpcThe speed of the robot control input calculated by MPC, k p3 and k d3 It refers to the speed feedback proportional control and derivative control coefficients, e 31 e3 is the deviation of the robot's obstacle avoidance trajectory from the original trajectory, e3 is the distance between the robot's center of mass and the obstacle's center of mass, and d is the distance between the robot's center of mass and the obstacle's center of mass. dis3 d is the absolute distance between the robot's center of mass and the obstacle's center of mass in the x-direction. 3min and d 3max These are the distance thresholds between the robot's center of mass and the obstacle.

[0161] The main reason for incorporating velocity feedback proportional-derivative control into the obstacle avoidance strategy is that the MPC trajectory tracking algorithm differs between the obstacle avoidance trajectory and the original trajectory. Therefore, differential control needs to be added at the boundary to facilitate velocity transition and ensure tracking accuracy. Since the obstacle avoidance trajectory is relatively short, the MPC trajectory tracking controller employs a strategy of short prediction step size and weight matrix, which improves computational efficiency and obstacle avoidance timeliness compared to tracking ordinary trajectories.

[0162] The selection of obstacle avoidance strategies for robots mainly involves predicting the trajectories of obstacles and the robot itself, determining whether a collision will occur, and then selecting a strategy. For static obstacles, which can be considered as having zero velocity and acceleration, obstacle avoidance trajectory planning is performed when the safe distance threshold is exceeded.

[0163] For dynamic obstacles, if the obstacle's trajectory is a straight line, the calculation result of the threshold function mentioned above can be used to directly determine whether a collision will occur after a certain time, and then the optimal path can be solved using constraints. If the obstacle's trajectory is a curve, the trajectory after a certain time can be predicted based on the differential function of the obstacle's displacement with respect to time. The motion time when the obstacle intersects with the robot's trajectory can be used to determine the robot's motion at that time. The distance between the center of mass of the robot and the obstacle can be used to determine whether a collision will occur, and then the optimal path can be solved.

[0164] In robot obstacle avoidance control, this invention provides a rigorous and systematic approach that takes into account the robot's safety constraints and is used in a closed loop with a feedback controller.

[0165] In the selection of obstacle avoidance decisions for robots, an asymmetric model from fuzzy decision-making is adopted. The robot's obstacle avoidance situation is taken as a constraint, and the selection of the robot's obstacle avoidance strategy is taken as the objective function. In the asymmetric model, accepting the constraint is a prerequisite, and the objective and the constraint are not symmetrical. Given the objective function f(x) on the universe of discourse X and the fuzzy set of constraints D on X, the optimal solution M that maximizes f under constraint D is a fuzzy subset on X. It has a membership function μM(x) equal to 0 when the set on the right-hand side of the equation is empty. The given universe of discourse X is the set of all cases, the current situation encountered by the robot is the constraint D, and the objective function is the robot's obstacle avoidance strategy.

[0166]

[0167] The robot solves the objective function by using environmental information and obstacle information as constraints when encountering obstacles, and then selects an appropriate obstacle avoidance strategy.

[0168] VI. The robot performs obstacle avoidance.

[0169] After planning the obstacle avoidance path and selecting the obstacle avoidance strategy, obstacle avoidance is performed, and the effect is as follows: Figure 13 As shown in the image, the robot has solved the obstacle avoidance problem quite well, successfully avoiding obstacles when it encounters them.

Claims

1. An obstacle avoidance method for a six-wheeled independently driven, independently steering robot, characterized by: This includes solving obstacle avoidance paths and obstacle avoidance strategies based on fuzzy decision-making. When the sensor detects an obstacle, it sends the obstacle information to the obstacle avoidance controller and obstacle avoidance planner. Then, it solves for the optimal path point through the constraints of the actual environment. After trajectory smoothing and polynomial fitting, the optimal path is obtained. The optimal path is discretized into coordinate points and output. After outputting the coordinate points, the obstacle avoidance strategy is selected, and obstacle avoidance is performed according to the strategy. The entire obstacle avoidance process ends. The obstacle avoidance strategy based on fuzzy decision-making is as follows: The selection of robot obstacle avoidance strategy involves predicting the obstacle trajectory and robot trajectory to determine whether a collision will occur, and then selecting a strategy. For static obstacles, which are considered to have zero speed and acceleration, obstacle avoidance trajectory planning is performed when the safe distance threshold is exceeded. After the obstacle avoidance trajectory is planned, trajectory tracking can be performed. For dynamic obstacles, if the obstacle's trajectory is a straight line, the threshold function is used to directly determine whether a collision will occur after a certain time, and then the optimal path is solved using constraints. If the obstacle's trajectory is a curve, the trajectory after a certain time is predicted based on the differential function of the obstacle's displacement with respect to time. The robot's motion at that time is determined based on the motion time when it intersects with the robot's trajectory. The distance between the robot's center of mass and the obstacle's center of mass is used to determine whether a collision will occur, and then the optimal path is solved. In the selection of obstacle avoidance decisions for robots, an asymmetric model from fuzzy decision-making is adopted. The robot's obstacle avoidance situation is taken as a constraint, and the selection of the robot's obstacle avoidance strategy is taken as the objective function. In the asymmetric model, accepting the constraint is a prerequisite, and the status of the objective and the constraint is not symmetrical. Given the objective function f(x) on the universe of discourse X and the fuzzy set of constraints D on X, the optimal solution m that maximizes f under constraint D is a fuzzy subset on X. It has a membership function μm(x) equal to 0 when the set on the right side of the equation is empty. The given universe of discourse X is the set of all cases, the current situation encountered by the robot is the constraint D, and the objective function is the robot's obstacle avoidance strategy. ； The robot solves the objective function by using environmental information and obstacle information as constraints when encountering obstacles, and then selects an appropriate obstacle avoidance strategy.

2. The obstacle avoidance method for a six-wheeled independently driven and independently steering robot according to claim 1, characterized in that: The process of solving the obstacle avoidance path is as follows: During the obstacle avoidance path planning process, the robot's size and the obstacle's size are expanded. The obstacle is expanded according to the necessary safety margin for both the robot and the obstacle, as well as the obstacle's information. Because of the safety margin, the robot's expansion range is also included in this part. Considering that there may be large obstacles and the robot may pass through the middle of the obstacle, the obstacle is segmented. Obstacle segmentation points are set at the obstacle outline for segmentation. In solving the obstacle avoidance path, an obstacle avoidance penalty function and a cost function for deviating from the reference trajectory are designed. The obstacle avoidance penalty function is used in the solution and constraint of the obstacle avoidance function. The magnitude of the function is adjusted by the distance deviation between the robot's centroid coordinates and the obstacle segmentation point. The closer the distance, the larger the function value. The cost function for the robot's deviation from the reference trajectory is used as a constraint condition for the solution. Solving for the obstacle avoidance path directly yields the coordinates of the optimal path point, therefore the following definition is provided: in, The coordinates of the obstacle avoidance path; To improve obstacle avoidance accuracy, a distance penalty function is defined. Add soft constraints to the obstacle avoidance function Where K is a very large positive integer, this is for the case where a dynamic obstacle suddenly approaches the robot during the robot's movement. When the robot is close to the obstacle, the soft constraint term is larger and has a greater weighting effect; when the robot is far from the obstacle, the soft constraint term is smaller and has a smaller weighting effect. Based on the above penalty function and constraints, an obstacle avoidance planner based on mathematical optimization is designed as follows: + )+ ， ， Where: P is the weight matrix; The term represents the control variable for the MPC (Multi-Process Control Controller). In the obstacle avoidance planner, its role is to constrain the obstacle avoidance path, ensuring that the path obtained by the planner conforms to the constraints of the robot's kinematic model. The result of the obstacle avoidance planner's solution... This is the robot's obstacle avoidance path; For obstacle avoidance penalty function, Distance penalty function; in the constraints, This is the MPC control variable, ensuring that the result conforms to the robot's motion model. The cost function for the deviation reference trajectory; The obstacle avoidance path solved by the obstacle avoidance planner is processed by curve fitting. Curve fitting generates trajectory points that conform to robot constraints by smoothing the trajectory points.

3. The obstacle avoidance method for a six-wheeled independently driven, independently steering robot according to claim 2, characterized in that: The obstacle avoidance penalty function as follows: ， Where, p obs These are the weight coefficients of the obstacle avoidance penalty function, (x i ,y i ) represents the coordinates of the obstacle segmentation point. It is an extremely small positive number to avoid the case where the denominator is 0.

4. The obstacle avoidance method for a six-wheeled independently driven and independently steering robot according to claim 2, characterized in that: The cost function of the deviation reference trajectory is as follows: ， ， Where, x min ,x max ,y min ,y max It refers to the global map coordinate range. These are reference trajectory coordinates. These are the coordinates of the obstacle avoidance path.

5. The obstacle avoidance method for a six-wheeled independently driven, independently steering robot according to claim 2, characterized in that: The distance penalty function is: ， in, For the coordinates of the obstacle material, For the robot's center of mass coordinates, These are the weighting coefficients of the distance penalty function.

6. The obstacle avoidance method for a six-wheeled independently driven, independently steering robot according to claim 2, characterized in that: The curve fitting process uses an nth-degree polynomial as the fitting curve, and the fitting is performed based on the least squares method. The fitting function has the following form: ， ， in, , They are polynomials Coefficients; n=4 is selected, meaning the order of fitting is 4. These are the coordinates of the fitted curve.

7. The obstacle avoidance method for a six-wheeled independently driven and independently steering robot according to claim 1, characterized in that: The dynamic obstacles are specifically divided into the following three categories: (1) In the first case, the direction of the obstacle's movement is the same as the direction of the robot's trajectory movement; At this point, the obstacle avoidance trajectory planning involves the robot selecting the optimal trajectory from the left or right side of the obstacle to overtake it, and then returning to the original trajectory for tracking. After obstacle avoidance trajectory replanning, the new trajectory points are output to the trajectory tracking controller, which calculates the robot's control input based on the MPC algorithm for trajectory tracking. However, when the robot's center of mass is on the same horizontal line as the obstacle's center of mass, a speed feedback proportional control is added to the speed term to enable the robot to perform a significant overtaking maneuver and return to the original trajectory as quickly as possible. At the same time, differential control is added to prevent overshoot. ， Among them, v mpc The velocity, k, is the robot control quantity calculated by the MPC trajectory tracking controller. p1 and k d1 It refers to the speed feedback proportional control and derivative control coefficients, e 11 It is the deviation of the robot's obstacle avoidance trajectory from the original trajectory, t k It is the current moment of the robot's movement, t d1max The robot moves to d 1max Position and time, t d1min It is d 1min The position and time shown; (2) In the second case, the direction of the obstacle's movement intersects with the direction of the robot's movement, and the obstacle moves at a relatively fast speed; This situation involves determining whether a collision will occur at the intersection point of the robot's tracking trajectory and the obstacle's movement trajectory. The judgment is based on the movement speed of both the robot and the obstacle. If the obstacle's movement speed is relatively fast, the robot will appropriately decelerate before reaching the collision point and then overtake the obstacle from behind after it passes the intersection point before returning to the original trajectory to continue tracking. The distance between the robot and the obstacle should be small at first and then large. When the distance is small, the speed needs to be reduced, and speed feedback proportional control should be added to the MPC control quantity. When the distance is large, the robot should revert to MPC control and add derivative control to prevent overshoot when tracking the original trajectory. ， Among them, v mpc The speed of the robot control input calculated by MPC, k p2 and k d2 It refers to the speed feedback proportional control and derivative control coefficients, e 21 e2 is the deviation of the robot's obstacle avoidance trajectory from the original trajectory, e2 is the distance between the robot's center of mass and the obstacle's center of mass, and d is the distance between the robot's center of mass and the obstacle's center of mass. dis2 d is the absolute distance between the robot's center of mass and the obstacle's center of mass in the x-direction. 2min and d 2max These are the distance thresholds between the robot's center of mass and the obstacle; (3) In the third case, the direction of the obstacle's movement intersects with the direction of the robot's movement, and the obstacle moves at a relatively slow speed; Before reaching the collision point, the robot accelerates appropriately and overtakes the obstacle in front of it before it crosses the intersection point, and then returns to the original trajectory to track it. The distance between the robot and the obstacle is initially small and then increases. When the distance is small, the speed needs to be increased. A speed feedback proportional control is added to the MPC control quantity to speed up obstacle avoidance. When the distance is large, the robot reverts to MPC control and adds derivative control to prevent overshoot when tracking the original trajectory. ， Among them, v mpc The speed of the robot control input calculated by MPC, k p3 and k d3 It refers to the speed feedback proportional control and derivative control coefficients, e 31 e3 is the deviation of the robot's obstacle avoidance trajectory from the original trajectory, e3 is the distance between the robot's center of mass and the obstacle's center of mass, and d is the distance between the robot's center of mass and the obstacle's center of mass. dis3 d is the absolute distance between the robot's center of mass and the obstacle's center of mass in the x-direction. 3min and d 3max These are the distance thresholds between the robot's center of mass and the obstacle.

Images