Non-holonomic mobile robot dynamic obstacle avoidance MPC control method based on approximate convexification obstacle constraint

Abstract

The application relates to the technical field of industrial control systems, and particularly discloses a nonholonomic constraint mobile robot dynamic obstacle avoidance MPC control method based on an approximately convex obstacle constraint, which comprises the following steps: obtaining the current state of a robot and a dynamic obstacle, generating an obstacle prediction boundary corresponding to an MPC prediction time and forming a reference prediction trajectory, determining a local convex direction according to the relative geometric relationship between the robot reference position and the corresponding prediction boundary, converting the non-convex obstacle avoidance condition into an approximately convex obstacle constraint, solving a prediction control sequence together with a nonholonomic motion constraint and outputting a control quantity, and updating the constraint according to the robot state and the obstacle state in the next control period. According to the application, the obstacle prediction boundary is registered, the approximately convex obstacle constraint is updated in a rolling manner, the obstacle avoidance constraint is kept corresponding to the robot prediction state, and the constraint is updated with the control period, so that the influence of constraint mismatch on MPC solving is reduced.

Description

Technical Field

[0001] This invention relates to the field of industrial control system technology, and in particular to a dynamic obstacle avoidance (MPC) control method for nonholonomically constrained mobile robots based on approximate convex obstacle constraints. Background Technology

[0002] Industrial control system technology typically revolves around the acquisition, data transmission, control calculation, and command execution of the status of industrial equipment and motion actuators. It uses position, velocity, attitude, and environmental detection information to form the basis for control, and the controller sends motion commands to the actuators. Among these, the traditional dynamic obstacle avoidance (MPC) control method for nonholonomically constrained mobile robots refers to a model-based predictive control calculation process for mobile robots subject to steering and kinematic constraints. This process uses LiDAR, vision sensors, or ranging sensors to obtain the robot's pose, velocity, and obstacle positions and motion states. It combines the robot's kinematic model, control constraints, prediction time domain, and obstacle boundaries to predict the temporal positions of the robot and obstacles within each control cycle. Based on distance constraints and trajectory tracking conditions, it calculates velocity, angular velocity, or steering control variables and sends the control commands to the motor actuators for execution.

[0003] Traditional mobile robot dynamic obstacle avoidance control often relies on establishing distance constraints based on the current or short-term predicted obstacle positions, and then solving for the control variables in the prediction time domain using the robot's kinematic model. When the obstacle is in continuous motion and the spatial relationship changes at each prediction time, uniformly or directly using non-convex distance conditions will result in insufficient correspondence between the constraint expression and the temporal state. The geometric relationship between the robot's predicted state and the predicted obstacle boundary is difficult to maintain consistency during the rolling solution process, increasing the computational burden of online optimization and making the obstacle avoidance constraint update within the control cycle lack a stable reference basis. Summary of the Invention

[0004] The purpose of this invention is to provide a dynamic obstacle avoidance (MPC) control method for nonholonomically constrained mobile robots based on approximate convex obstacle constraints. This method addresses the changes in the spatial state of obstacles at different prediction times during the continuous movement of dynamic obstacles, and the coupling between non-convex obstacle avoidance conditions and the predicted state of the nonholonomically constrained mobile robot. By establishing a correspondence between prediction times and forming approximate convex obstacle constraints based on the relative geometric relationship between the baseline predicted trajectory and the predicted boundary of the dynamic obstacle, the obstacle constraint and the nonholonomic motion constraint jointly participate in the rolling solution of MPC. Furthermore, the constraints of the next control cycle are updated based on the robot state after control execution, the reacquired dynamic obstacle state, and the predicted state of the previous control cycle, thus forming a continuous prediction, constraint, solution, execution, and feedback control process under dynamic obstacle conditions.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: a dynamic obstacle avoidance (MPC) control method for nonholonomically constrained mobile robots based on approximately convex obstacle constraints, comprising the following steps: The current motion state of the mobile robot and the current state of the dynamic obstacles are obtained. Based on the current state of the dynamic obstacles, the dynamic obstacle prediction boundary corresponding to each prediction time in the MPC prediction time domain is generated, and the reference prediction trajectory is formed based on the current motion state of the mobile robot. For each prediction time, the local convexity direction is determined based on the relative geometric relationship between the robot reference position in the baseline prediction trajectory and the corresponding dynamic obstacle prediction boundary. The non-convex obstacle avoidance condition between the robot and the dynamic obstacle is converted into an approximately convex obstacle constraint acting on the robot prediction state recursively formed in the MPC prediction time domain according to the local convexity direction, so that the robot prediction state at different prediction times has corresponding obstacle constraint conditions. The approximate convex obstacle constraint and the nonholonomic motion constraint are jointly incorporated into the MPC optimization problem. The robot's predicted state is formed according to the nonholonomic motion constraint, and the predicted control sequence is calculated. The current control quantity in the predicted control sequence is output to the motion actuator of the mobile robot. In the next control cycle, based on the robot's current motion state after execution, the newly acquired current state of the dynamic obstacle, and the predicted state of the previous control cycle, the baseline predicted trajectory and the predicted boundary of the dynamic obstacle are regenerated, and the approximate convex obstacle constraint is updated. The updated constraint participates in solving the MPC optimization problem in the next control cycle.

[0006] Furthermore, the process of generating the dynamic obstacle prediction boundary includes: obtaining the position and motion state of the dynamic obstacle according to the control cycle; using the position obtained in the current control cycle as the prediction starting position; calculating the predicted position of the dynamic obstacle sequentially according to the prediction time in the MPC prediction time domain based on the motion state; and determining the dynamic obstacle prediction boundary corresponding to each prediction position and each prediction time. When entering the next control cycle, replacing the input of the previous control cycle with the newly obtained position and motion state, and recalculating the dynamic obstacle prediction boundary corresponding to each prediction time.

[0007] Furthermore, after determining the dynamic obstacle prediction boundary for each prediction time, the robot reference position with the same prediction time in the baseline prediction trajectory is registered with the corresponding dynamic obstacle prediction boundary to determine the relative spatial direction from the robot reference position to the corresponding obstacle prediction boundary. When the robot reference position and the corresponding obstacle prediction boundary have a separation space, the local convexity direction is determined according to the relative spatial direction and an approximate convex obstacle constraint is constructed; when the robot reference position and the corresponding obstacle prediction boundary do not have a separation space, the relative spatial direction is re-determined based on the baseline prediction trajectory re-formed in the current control cycle.

[0008] Furthermore, the process of constructing approximate convex obstacle constraints includes: determining a separation boundary along the local convexity direction based on the robot reference position and the predicted boundary of the dynamic obstacle at the same prediction time as the robot reference position; projecting the predicted position in the robot prediction state onto the local convexity direction; using the positional relationship between the projected position and the separation boundary as the convex constraint condition for the corresponding prediction time; forming an approximate convex obstacle constraint sequence that corresponds one-to-one with the robot prediction state according to the order of the prediction times; and using the approximate convex obstacle constraint sequence as the obstacle constraint condition for each prediction time in the MPC optimization problem.

[0009] Furthermore, the process of incorporating the approximate convexity obstacle constraint and the nonholonomic motion constraint into the MPC optimization problem includes: recursively deriving the robot's predicted state according to the current motion state of the mobile robot and the nonholonomic motion constraint at each prediction time, so that each robot predicted state is associated with the approximate convexity obstacle constraint at the same prediction time; participating the prediction control sequence calculation for robot predicted states that satisfy the corresponding approximate convexity obstacle constraint; restricting robot predicted states that do not satisfy the corresponding approximate convexity obstacle constraint to entering the feasible state set at the corresponding prediction time; and obtaining the prediction control sequence that satisfies the nonholonomic motion constraint and the approximate convexity obstacle constraint at each prediction time in the prediction time domain.

[0010] Furthermore, after outputting the current control quantity in the predictive control sequence, the current motion state of the robot after executing the current control quantity is obtained in the next control cycle. The predicted state corresponding to the current control quantity in the previous control cycle is shifted forward according to the prediction time, and the first predicted state after the shift is replaced by the current motion state of the robot after execution, forming the reference predicted trajectory for the next control cycle. The dynamic obstacle prediction boundary is regenerated according to the current state of the dynamic obstacle obtained in the next control cycle, and the approximate convex obstacle constraint is re-determined based on the regenerated dynamic obstacle prediction boundary and the reference predicted trajectory.

[0011] Furthermore, when redetermining the approximate convex obstacle constraints, the relative geometric relationship between the robot reference position in the baseline predicted trajectory and the corresponding dynamic obstacle predicted boundary is compared at each prediction time. When the robot reference position and the corresponding dynamic obstacle predicted boundary have a separation space, the local convexity direction is updated according to the relative spatial direction, and the approximate convex obstacle constraint for the corresponding prediction time is generated; when the robot reference position and the corresponding dynamic obstacle predicted boundary do not have a separation space, the baseline predicted trajectory is re-formed, and the re-formed baseline predicted trajectory is used as the input for determining the local convexity direction in the current control cycle.

[0012] Furthermore, when forming the approximate convex obstacle constraint sequence, for the case where there are multiple dynamic obstacle prediction boundaries at the same prediction time, the relative geometric relationship between the robot reference position and each dynamic obstacle prediction boundary is determined, and corresponding approximate convex obstacle constraints are formed respectively. Each approximate convex obstacle constraint is used together as the obstacle restriction condition at the corresponding prediction time, so that the robot prediction state simultaneously satisfies each approximate convex obstacle constraint formed at the corresponding prediction time.

[0013] Furthermore, when forming the reference predicted trajectory for the next control cycle, the state deviation between the robot's current motion state after executing the current control quantity and the corresponding predicted state in the previous control cycle is calculated. When the state deviation meets a preset deviation range, the reference predicted trajectory is formed from the previously shifted predicted state; when the state deviation does not meet the preset deviation range, the robot's current motion state after executing the current control quantity is used as the prediction starting state, and the prediction state is re-produced according to the nonholonomic motion constraints to form the reference predicted trajectory.

[0014] Furthermore, after re-forming the baseline predicted trajectory, the relative spatial direction between the robot reference position and the corresponding predicted boundary of the dynamic obstacle is recalculated at each prediction time according to the re-formed baseline predicted trajectory, and the corresponding approximate convex obstacle constraint is generated based on the recalculated relative spatial direction. When the robot reference position and the corresponding predicted boundary of the dynamic obstacle have separation space at each prediction time, the generated approximate convex obstacle constraint is added to the MPC optimization problem and the predictive control sequence is calculated; when there is a prediction time without separation space, the baseline predicted trajectory formation process is re-executed within the current control cycle.

[0015] Compared with the prior art, the advantages and positive effects of the present invention are as follows: In this invention, dynamic obstacle prediction boundaries are generated at each time step according to the MPC prediction time domain, so that the robot prediction state and the obstacle motion state form a time correspondence. The local convexity direction is determined based on the relative geometric relationship between the robot reference position in the baseline prediction trajectory and the corresponding prediction boundary, and the non-convex obstacle avoidance condition is converted into an approximately convex obstacle constraint acting on the robot prediction state recursively formed in the MPC prediction time domain. The approximately convex obstacle constraint and the nonholonomic motion constraint jointly participate in the MPC solution, so that the obstacle avoidance calculation is consistent with the robot's reachable motion state. In the next control cycle, the robot state after execution, the newly acquired obstacle state and the prediction state of the previous cycle are combined to update the baseline prediction trajectory and the obstacle prediction boundary, so that the constraints are updated synchronously with the rolling control process. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0017] Figure 1 This is the main flowchart of the dynamic obstacle avoidance MPC control for the nonholonomic constraint mobile robot of the present invention; Figure 2 This is a time-series correspondence diagram between the dynamic obstacle prediction boundary and the baseline prediction trajectory of the present invention; Figure 3 This is a schematic diagram of the approximate convexity barrier constraint generation process of the present invention; Figure 4 This is a diagram showing the relationship between the co-solution of nonholonomic motion constraints and approximate convexity barrier constraints in this invention. Figure 5 This is the rolling update diagram of the baseline predicted trajectory and obstacle constraints for the next control cycle of the present invention. Detailed Implementation

[0018] The following detailed embodiments are based on the technical solutions, claims and foregoing content of this invention, to elaborate on the dynamic obstacle avoidance MPC control method for nonholonomic constraint mobile robots based on approximate convex obstacle constraints. The following terms refer to the current motion state of the mobile robot, which is the state data used to describe the motion state of the mobile robot in the current control cycle and serves as the starting point for the recursion of nonholonomic motion constraints; the current state of the dynamic obstacle refers to the position and motion state of the dynamic obstacle obtained in the current control cycle; the predicted boundary of the dynamic obstacle refers to the obstacle constraint boundary formed at different prediction times in the MPC prediction time domain based on the current state of the dynamic obstacle; the reference predicted trajectory refers to the predicted state sequence used to establish the relative geometric relationship between the robot's reference position and the predicted boundary of the dynamic obstacle; the predicted state of the robot refers to the robot state formed recursively along the prediction time domain under nonholonomic motion constraints; the local convexity direction refers to the obstacle constraint transformation direction determined based on the relative spatial relationship between the robot's reference position and the corresponding predicted boundary of the dynamic obstacle; the approximately convex obstacle constraint refers to the convex constraint condition formed by transforming non-convex obstacle avoidance conditions along the local convexity direction and acting on the predicted state of the robot; the predictive control sequence refers to the sequence of control quantities obtained by the MPC optimization problem in the prediction time domain; and the current control quantity refers to the control quantity selected from the predictive control sequence and issued to the motion actuator of the mobile robot in the current control cycle. The terms mentioned above shall retain the same technical meaning in the following steps, sub-steps and references to the accompanying drawings, and the correspondence shall not change due to changes in the description position.

[0019] Please see Figures 1 to 5 This embodiment provides a dynamic obstacle avoidance MPC control method for a nonholonomically constrained mobile robot based on approximate convex obstacle constraints. In this embodiment, a complete control cycle is used as the basic execution unit. The mobile robot first obtains the current motion state and the current state of the dynamic obstacle. Then, based on the current state of the dynamic obstacle, it generates the dynamic obstacle prediction boundary corresponding to each prediction time and forms a reference prediction trajectory based on the current motion state of the mobile robot. Subsequently, the relative geometric relationship between the robot's reference position and the corresponding dynamic obstacle prediction boundary is established for each prediction time. Based on the relative geometric relationship, the local convexity direction is determined and an approximate convex obstacle constraint is generated. The approximate convex obstacle constraint and the nonholonomic motion constraint are then added to the MPC optimization problem to form a predictive control sequence and output the current control quantity. After the current control quantity is executed, the current motion state of the robot and the current state of the dynamic obstacle are obtained again in the next control cycle. The reference prediction trajectory, the dynamic obstacle prediction boundary, and the approximate convex obstacle constraint are updated in combination with the prediction state of the previous control cycle, thereby forming a rolling closed loop consisting of state acquisition, prediction construction, constraint transformation, control solution, execution feedback, and further updates.

[0020] S1: Obtain the current motion state of the mobile robot and the current state of the dynamic obstacles. Generate the dynamic obstacle prediction boundary corresponding to each prediction time in the MPC prediction time domain based on the current state of the dynamic obstacles, and form the reference prediction trajectory based on the current motion state of the mobile robot.

[0021] The inputs to S1 include the current motion state of the mobile robot and the current state of the dynamic obstacle in the current control cycle. The current motion state of the mobile robot serves as the starting state for the robot's predicted state recursion and is also the state basis for forming the reference predicted trajectory in the current cycle. The current state of the dynamic obstacle includes at least the position and motion state explicitly stated in the claims. The position is used to determine the spatial starting point of the current obstacle, and the motion state is used to update the obstacle position according to the predicted time in the prediction time domain. The two types of state data obtained in the current control cycle should belong to the input set of the same control cycle, so that the subsequent robot reference position and the predicted boundary of the dynamic obstacle have a unified time reference.

[0022] S111: Obtain the position and motion state of the dynamic obstacle according to the current control cycle, and determine the currently obtained position as the prediction start position of the obstacle prediction process. The position of the dynamic obstacle is the spatial starting point of the obstacle prediction position at each subsequent prediction time, and the motion state of the dynamic obstacle is used to determine the state extension relationship from the prediction start position to the subsequent prediction time. After the current control cycle enters the prediction calculation, the prediction start position remains the first spatial reference of the corresponding dynamic obstacle prediction sequence within the current control cycle, and the old position of the previous control cycle is not directly used as the prediction start position of the current cycle.

[0023] S112, based on the motion state of the dynamic obstacle and the prediction times within the MPC prediction time domain, the predicted positions of the dynamic obstacle at each prediction time are sequentially formed. Each predicted position is indexed according to the order of the prediction times, so that the first prediction time corresponds to the first predicted position, and subsequent prediction times correspond to subsequent predicted positions. The predicted positions of the dynamic obstacle are only used to construct the obstacle spatial state at the corresponding prediction time within the current control cycle and are not used as fixed obstacle positions for long-term application across the current control cycle. When a new dynamic obstacle position and motion state are obtained in the next control cycle, the predicted positions formed in the previous control cycle are no longer used as direct prediction inputs for the next control cycle.

[0024] S113: Determine the corresponding dynamic obstacle prediction boundary based on the predicted positions of dynamic obstacles at each prediction time, and establish a one-to-one correspondence between the prediction time and the dynamic obstacle prediction boundary. The dynamic obstacle prediction boundary is used to express the spatially constrained objects participating in obstacle avoidance constraint calculation at each prediction time, so that the robot reference position in the subsequent baseline prediction trajectory can be matched with the corresponding obstacle boundary according to the same prediction time. In the case of multiple dynamic obstacles at the same prediction time, maintain the prediction boundary of each dynamic obstacle separately, and do not merge multiple dynamic obstacle prediction boundaries into a single boundary lacking object differentiation.

[0025] S114: A baseline predicted trajectory is formed based on the current motion state of the mobile robot. The baseline predicted trajectory includes the robot reference position corresponding to the prediction time within the MPC prediction time domain. The robot reference position serves as the robot side-space reference for subsequently determining the local convexity direction. When the baseline predicted trajectory is first formed in the current control cycle, the current motion state is used as the trajectory starting basis. In the next control cycle after entering rolling control, the baseline predicted trajectory is also adjusted according to the rolling update relationship formed in S4, using the prediction state of the previous control cycle. The baseline predicted trajectory is not directly used as the final output trajectory, but rather as an intermediate state for establishing the relative geometric relationship between the robot reference position and the predicted boundary of the dynamic obstacle.

[0026] After processing by S1, two sets of intermediate data with the same prediction time index are generated: one set is the robot reference position sequence in the baseline prediction trajectory, and the other set is the dynamic obstacle prediction boundary sequence. Both sets of intermediate data serve as input to S2. The execution result of S1 can be verified by checking whether the robot reference position and the corresponding dynamic obstacle prediction boundary exist simultaneously at each prediction time. Once the correspondence between prediction times is established, subsequent calculations of relative geometric relationships have clearly defined paired input objects.

[0027] S2, for each prediction time, determine the local convexity direction based on the relative geometric relationship between the robot reference position in the baseline prediction trajectory and the corresponding dynamic obstacle prediction boundary, and convert the non-convex obstacle avoidance condition between the robot and the dynamic obstacle into an approximate convex obstacle constraint acting on the robot prediction state recursively formed in the MPC prediction time domain according to the local convexity direction.

[0028] S2 takes the robot reference position and the predicted boundary of the dynamic obstacle generated in S1 as input, and uses the prediction time as the registration basis. The local convexity process does not use a fixed spatial direction to process the entire prediction time domain. Instead, it determines the relative relationship between the robot reference position and the predicted boundary of the obstacle for each prediction time separately, so as to avoid geometric mismatch caused by different prediction times sharing the same spatial direction.

[0029] S211, at each prediction time, the robot reference position with the corresponding prediction time in the baseline prediction trajectory is registered with the dynamic obstacle prediction boundary at the same prediction time. The registration criterion is that the prediction times are consistent, that is, when the robot reference position and the dynamic obstacle prediction boundary have the same prediction time index, they constitute a set of relative geometric relationship calculation inputs; two data items with inconsistent prediction time indices are not included in the same local convexity direction calculation. This forms a pairing relationship between the robot reference position and the dynamic obstacle prediction boundary arranged sequentially by prediction time.

[0030] S212: Based on each group of robot reference positions and the corresponding predicted boundaries of dynamic obstacles, the relative spatial orientation is determined, and it is checked whether there is a separation space between the robot reference position and the corresponding predicted boundary of dynamic obstacles. When there is a separation space, the current relative spatial orientation is used as the geometric basis for determining the local convexity orientation; when there is no separation space, the obstacle constraint is not directly constructed using the current relative spatial orientation, but instead, the process of re-forming the reference predicted trajectory is entered. The re-formed reference predicted trajectory provides a new robot reference position, and the process of determining the relative geometric relationship at the corresponding prediction time is re-executed. This ensures that the formation of the local convexity orientation has a clear geometric premise.

[0031] S213, In the case of a separation space, the local convexity direction is determined based on the relative spatial direction from the robot's reference position to the predicted boundary of the corresponding dynamic obstacle, and the separation boundary along the local convexity direction is determined accordingly. The separation boundary is used to transform the spatial relationship between the robot's predicted position and the predicted boundary of the dynamic obstacle into a constraint relationship that can be incorporated into the MPC optimization problem. The local convexity direction corresponds to the current prediction time and is not directly used as the local convexity direction for other prediction times.

[0032] S214: Project the robot's predicted position at the corresponding prediction time along the local convexity direction. Construct an approximate convex obstacle constraint for the corresponding prediction time based on the positional relationship between the projected position of the robot's predicted position in the local convexity direction and the separation boundary. The variables of the approximate convex obstacle constraint correspond to the predicted position in the robot's predicted state, allowing the obstacle constraint to directly act on the predicted state obtained recursively from the nonholonomic motion constraint, without forming an independent obstacle avoidance state separate from the robot's motion prediction. After constructing the projection relationship for each prediction time, an approximate convex obstacle constraint sequence is formed according to the order of the prediction times.

[0033] S215, for cases where multiple dynamic obstacle prediction boundaries exist at the same prediction time, the relative spatial orientation between the robot's reference position and each dynamic obstacle prediction boundary is determined, and corresponding approximate convex obstacle constraints are formed. Multiple approximate convex obstacle constraints act together on the robot's predicted state at the same prediction time, ensuring that the same robot predicted state is simultaneously subject to the spatial constraints corresponding to each dynamic obstacle at that prediction time. Here, each obstacle constraint maintains an independent object relationship, and the relative orientations of different obstacles are not mixed into a single orientation.

[0034] After processing in S2, the intermediate result is a sequence of approximately convex obstacle constraints corresponding to the prediction time. Each approximately convex obstacle constraint has a clearly defined object of robot prediction state, source of dynamic obstacle prediction boundary, and source of local convexity direction. The verification method involves checking each approximately convex obstacle constraint to see if it can be traced back to the robot reference position and dynamic obstacle prediction boundary at the same prediction time, and whether there is a clear local convexity direction and separation boundary. Constraints that satisfy the above correspondence proceed to S3.

[0035] S3 incorporates the approximate convex obstacle constraint and the nonholonomic motion constraint into the MPC optimization problem, calculates the predictive control sequence, and outputs the current control quantity in the predictive control sequence to the motion actuator of the mobile robot.

[0036] The inputs to S3 include the current motion state of the mobile robot, nonholonomic motion constraints, the approximate convex obstacle constraints formed by S2, and the MPC prediction time domain. During processing, the robot's predicted state is first recursively derived from the current motion state according to the nonholonomic motion constraints at each prediction time. Then, a correspondence is established between each robot's predicted state and the approximate convex obstacle constraint at the same prediction time, so that motion prediction and obstacle constraints share the same prediction time index.

[0037] S311, using the current motion state of the mobile robot as the starting point for recursive prediction of the robot's predicted state, a sequence of predicted robot states is formed step-by-step according to the nonholonomic motion constraints. Each predicted robot state includes the predicted robot position required for subsequent obstacle constraints, and maintains a recursive relationship with the control quantities at the corresponding time in the predicted control sequence. In this embodiment, the nonholonomic motion constraints serve to limit the reachable motion states of the mobile robot, ensuring that subsequent obstacle constraints do not filter arbitrary spatial positions, but rather constrain the predicted robot states formed under the nonholonomic motion constraints.

[0038] S312: Establish a correspondence between each robot predicted state and the approximate convex obstacle constraint at the same prediction time. If the robot predicted state satisfies the corresponding approximate convex obstacle constraint, retain the set of feasible states for the current prediction time and continue to participate in the prediction control sequence calculation. If the robot predicted state does not satisfy the corresponding approximate convex obstacle constraint, restrict the robot predicted state to outside the set of feasible states at the corresponding prediction time, preventing predicted states that do not meet obstacle space constraints from entering feasible prediction paths. The judgment process is performed in the order of prediction times, ensuring that the constraint relationships at each prediction time are consistent with the corresponding robot predicted states.

[0039] S313, considering both nonholonomic motion constraints and approximate convex obstacle constraints at each prediction time point within the prediction time domain, obtains the predictive control sequence. The control variables in the predictive control sequence correspond to the recursive order of the robot's predicted states and are uniformly formed by the MPC optimization process of the current control cycle.

[0040] In S314, the current control quantity corresponding to the current control cycle is selected from the predictive control sequence and output to the motion actuator of the mobile robot. The motion actuator changes the actual motion state of the mobile robot according to the current control quantity. The current motion state of the mobile robot after execution re-enters S1 and S4 in the next control cycle as the data input for the next round of rolling calculation. The execution state of the current control quantity constitutes the output result of the current control cycle, and also serves as the source of the physical state for forming a feedback loop in the next control cycle.

[0041] The engineering observables generated by S3 include the fact that the current control input has been determined from the predictive control sequence and fed into the motion execution process, and that a new current motion state of the robot can be obtained in the next control cycle after execution. By comparing the current motion state of the robot obtained in the next control cycle with the predicted state corresponding to the previous control cycle, the state deviation required by S4 can be generated, thereby verifying that the predictive control output has indeed entered the subsequent feedback chain.

[0042] S4. In the next control cycle, based on the robot's current motion state after execution, the newly acquired current state of the dynamic obstacle, and the predicted state of the previous control cycle, the baseline predicted trajectory and the predicted boundary of the dynamic obstacle are regenerated, and the approximate convex obstacle constraints are updated.

[0043] S4 is used to connect the prediction results of the previous control cycle, the execution results of the current control variable, and the obstacle state reacquired in the next control cycle into a new MPC input. This step does not directly modify a single value based on the old constraints of the previous control cycle, but rather reconstructs the robot reference state and dynamic obstacle temporal boundaries used in the next control cycle, and then re-executes the relative geometric relationship and approximate convex constraint generation process.

[0044] S411: In the next control cycle, the robot's current motion state after executing the current control quantity of the previous control cycle is acquired, and the current state of the dynamic obstacles in the next control cycle is also acquired. The current state of the dynamic obstacles acquired in the next control cycle replaces the current state of the dynamic obstacles in the previous control cycle and is used as the input for the next round of dynamic obstacle prediction boundary generation process; the old obstacle prediction boundary formed in the previous control cycle is not directly used as the fixed boundary in the next control cycle.

[0045] S412, the predicted state corresponding to the current control quantity in the previous control cycle is shifted forward according to the prediction time, so that the predicted state sequence of the previous control cycle corresponds again with the prediction time start point of the next control cycle. Then, the first predicted state after shifting forward is replaced with the current motion state of the robot actually obtained after executing the current control quantity, thereby forming the initial reference predicted trajectory for the next control cycle. This process enables the reference predicted trajectory to utilize both the subsequent predicted states formed in the previous control cycle and the actual robot state in the next control cycle.

[0046] S413: Calculate the state deviation between the robot's current motion state after executing the current control variable and the corresponding predicted state in the previous control cycle, and compare the state deviation with a preset deviation range. When the state deviation meets the preset deviation range, retain the reference predicted trajectory formed based on the previously shifted predicted state; when the state deviation does not meet the preset deviation range, do not use the subsequently shifted predicted state as a complete reference, but instead use the robot's current motion state after executing the current control variable as a new prediction starting state, and recursively deduce the subsequent predicted states according to the non-holonomic motion constraints to form a new reference predicted trajectory. The preset deviation range is used to determine whether the predicted state of the previous control cycle can still serve as a continuous source of the reference predicted trajectory for the next control cycle.

[0047] S414: Based on the current state of the dynamic obstacles obtained in the next control cycle, the obstacle prediction process corresponding to S111 to S113 is re-executed to form the dynamic obstacle prediction boundaries for each prediction time in the next control cycle. The regenerated dynamic obstacle prediction boundaries have the same new prediction time reference as the baseline prediction trajectory formed in the next control cycle, and the prediction time index of the previous control cycle is not directly used as the index of the new cycle.

[0048] S415, compare the relative geometric relationship between the robot reference position and the corresponding dynamic obstacle prediction boundary in the reference predicted trajectory of the next control cycle at each prediction time. When the robot reference position and the corresponding dynamic obstacle prediction boundary have a separation space, update the local convexity direction according to the new relative spatial direction and generate the approximate convex obstacle constraint for the corresponding prediction time; when there is a prediction time in which the robot reference position and the corresponding dynamic obstacle prediction boundary do not have a separation space, keep the next control cycle in the reference predicted trajectory reconstruction process, and re-execute the reference predicted trajectory formation process with the newly acquired current robot motion state as the prediction starting state until a reference predicted trajectory that can be used to further determine the relative spatial direction is formed.

[0049] In step S416, when the robot's reference position and the corresponding predicted boundary of the dynamic obstacle both have a separation space at each prediction time, the regenerated approximate convex obstacle constraints are added to the MPC optimization problem of the next control cycle, and the predictive control sequence is recalculated according to step S3. This completes a closed loop from the prediction result of the previous control cycle to the constraint update and control solution of the next control cycle. The predicted state obtained in the current control cycle then continues to serve as the feedback input for the next control cycle, causing the above process to be repeated according to the control cycle.

[0050] Through S1 to S4, the entire method establishes a clear data flow relationship. The current motion state of the mobile robot enters S1 from the actual motion state of the current control cycle, and simultaneously enters S3 as the starting point for the robot's predicted state recursion. In the next control cycle, it enters S4 to verify the predicted state of the previous control cycle. The current state of the dynamic obstacle enters S1 and generates the predicted boundary of the dynamic obstacle. The reference predicted trajectory and the predicted boundary of the dynamic obstacle together enter S2 to generate an approximately convex obstacle constraint sequence. The approximately convex obstacle constraint sequence and the nonholonomic motion constraints together enter S3 to form a predictive control sequence. The current control quantity in the predictive control sequence enters the mobile robot's motion execution process. After execution, the current motion state of the robot and the newly acquired current state of the dynamic obstacle enter S4 again to regenerate the reference predicted trajectory and the predicted boundary of the obstacle for the next control cycle.

[0051] Regarding the data timing relationship, the current motion state of the mobile robot and the current state of the dynamic obstacle used in the current control cycle are both obtained at the beginning of the current cycle. The predicted boundary of the dynamic obstacle and the reference predicted trajectory form corresponding prediction time indices in the same prediction time domain. S2 only pairs the robot reference position with the predicted boundary of the dynamic obstacle with the same prediction time, without using random pairing relationships across time. The robot predicted state formed by S3 also adopts the same prediction time order as S2, so that the approximate convex obstacle constraint can directly correspond to the robot predicted state at the same prediction time. After the next control cycle begins, the timing index of the previous control cycle is realigned as the prediction state moves forward.

[0052] In the case of multiple dynamic obstacles, multiple dynamic obstacles at the same prediction time form corresponding dynamic obstacle prediction boundaries. Each dynamic obstacle prediction boundary establishes a relative geometric relationship with the same robot reference position, and then forms corresponding approximate convex obstacle constraints. Multiple constraints jointly define the robot's predicted state at the same prediction time in S3. Different dynamic obstacles do not exchange their respective predicted positions and relative spatial orientations, nor do they use the local convex direction corresponding to one dynamic obstacle as the default local convex direction of other dynamic obstacles, thereby maintaining a clear correspondence between obstacle objects, prediction boundaries, and local convex directions.

[0053] For the rolling use of the reference predicted trajectory, the predicted state of the previous control cycle is not unconditionally carried over. After obtaining the current motion state of the actual robot in the next control cycle, the state deviation between the executed state and the corresponding predicted state of the previous cycle is first determined. Then, the formation method of the reference predicted trajectory is determined based on whether the state deviation meets the preset deviation range. When the state deviation meets the preset deviation range, the temporal continuity of the predicted trajectory is maintained by using the forward-shifted predicted state; when the state deviation does not meet the preset deviation range, the new current motion state of the actual robot is used as the prediction starting point again, and the subsequent predicted state is recursively formed based on the nonholonomic motion constraints. This ensures that the robot reference position used for convexification in the next control cycle always has a clear source of the actual state or the predicted state of the previous cycle.

[0054] For cases where there is no separation space, this state is not directly considered as normal input that can continue to generate local convex directions. S212 first identifies whether there is a separation space between the robot reference position and the corresponding predicted boundary of the dynamic obstacle; when there is no separation space, the current robot reference position does not enter the normal construction process of the corresponding approximate convex obstacle constraint, but triggers the reconstruction of the reference predicted trajectory. After reconstruction, the relative spatial direction is recalculated according to the new robot reference position and the predicted boundary of the dynamic obstacle. Only when the correspondence at each prediction time can continue to form a local convex direction, the generated approximate convex obstacle constraint enters the MPC optimization problem.

[0055] Regarding the feedback relationship of the control output, after S3 outputs the current control quantity, the current control quantity is executed by the motion actuator of the mobile robot. The current motion state of the robot obtained in the next control cycle is used to reflect the physical state after execution. This state not only serves as a normal input for the next MPC calculation, but also forms a state deviation with the predicted state corresponding to the previous control cycle. The state deviation determines whether the reference predicted trajectory in the next cycle follows the forward predicted state or is recursively calculated. Thus, the actual execution result directly affects the reference predicted trajectory used to construct the local convexity direction in the next round through the state deviation judgment, and further affects the approximate convexity obstacle constraint in the next round.

[0056] The verification in this embodiment can be performed along the data state of the control cycle. After completing S1, the robot reference position and dynamic obstacle prediction boundary corresponding to the prediction time should be obtained one-to-one; after completing S2, each approximately convex obstacle constraint should be able to trace back to the robot reference position, corresponding dynamic obstacle prediction boundary, and local convexity direction at the same prediction time; after completing S3, a predictive control sequence should be formed and the current control quantity should be output; after completing S4, a new current robot motion state and current dynamic obstacle state should be obtained, and a new baseline predicted trajectory, dynamic obstacle prediction boundary, and approximately convex obstacle constraint should be formed. When the above intermediate states and output states are established in sequence, it can be confirmed that the input, prediction, constraint transformation, solution, execution, and feedback chain of the current control cycle and the next cycle have been closed.

[0057] Therefore, this embodiment maintains the time correspondence between the robot's reference position, the predicted boundary of the dynamic obstacle, the robot's predicted state, and the approximately convex obstacle constraints through the prediction time index. It converts the non-convex obstacle avoidance conditions in the dynamic obstacle environment into convex constraint expressions directly related to the robot's predicted state through local convexity directions and separation boundaries. Furthermore, it generates a rolling update based on the robot's current motion state after actual execution, the new state of the dynamic obstacle, and the predicted state of the previous control cycle, allowing the obstacle constraints used in each control cycle to be reconstructed according to the new spatial relationships. All of the above effects can be confirmed through the data pairing state corresponding to the prediction time, the approximately convex obstacle constraint generation state, the predictive control sequence output state, and the update state of the reference predicted trajectory for the next control cycle.

[0058] The above-described structured process, processing order, state relationships, judgment conditions, data sources, and rolling feedback relationships are used to illustrate the feasible implementation methods of the technical solutions described in this invention. The core technical relationships defined in the claims and application documents are not limited to a single numerical configuration due to the detailed explanations of the prediction process, relative geometric relationships, state deviation judgment, and control cycle update process in the embodiments. Without departing from the scope supported by the technical solutions described in this invention, any equivalent adjustments made by those skilled in the art to the execution order without changing the data dependencies, equivalent transformations of data names, equivalent substitutions of the same technical relationships, and equivalent modifications to the processing organization within the control cycle do not change the technical essence of this invention: the dynamic obstacle prediction boundary corresponds to the prediction time; the local convexity direction is determined by relative geometric relationships; approximately convex obstacle constraints and non-holonomic motion constraints jointly participate in MPC solving; and the next control cycle is rolled and updated.

Claims

1. A dynamic obstacle avoidance (MPC) control method for nonholonomic mobile robots based on approximate convex obstacle constraints, characterized in that: The current motion state of the mobile robot and the current state of the dynamic obstacles are obtained. Based on the current state of the dynamic obstacles, the dynamic obstacle prediction boundary corresponding to each prediction time in the MPC prediction time domain is generated, and the reference prediction trajectory is formed based on the current motion state of the mobile robot. For each prediction time, the local convexity direction is determined based on the relative geometric relationship between the robot reference position in the baseline prediction trajectory and the corresponding dynamic obstacle prediction boundary. The non-convex obstacle avoidance condition between the robot and the dynamic obstacle is converted into an approximately convex obstacle constraint acting on the robot prediction state recursively formed in the MPC prediction time domain according to the local convexity direction. Approximate convex obstacle constraints and nonholonomic motion constraints are combined into the MPC optimization problem to calculate the predictive control sequence, and the current control quantity in the predictive control sequence is output to the motion actuator of the mobile robot. In the next control cycle, based on the robot's current motion state after execution, the newly acquired current state of the dynamic obstacle, and the predicted state of the previous control cycle, the baseline predicted trajectory and the predicted boundary of the dynamic obstacle are regenerated, and the approximate convex obstacle constraints are updated.

2. The dynamic obstacle avoidance MPC control method for nonholonomic constraint mobile robots based on approximate convex obstacle constraints according to claim 1, characterized in that, The process of generating the dynamic obstacle prediction boundary includes: obtaining the position and motion state of the dynamic obstacle according to the control cycle; using the position obtained in the current control cycle as the prediction start position; calculating the predicted position of the dynamic obstacle sequentially according to the prediction time in the MPC prediction time domain based on the motion state; and determining the dynamic obstacle prediction boundary corresponding to each prediction time according to each prediction position; when entering the next control cycle, replacing the input of the previous control cycle with the newly obtained position and motion state, and recalculating the dynamic obstacle prediction boundary corresponding to each prediction time.

3. The dynamic obstacle avoidance (MPC) control method for nonholonomic mobile robots based on approximate convex obstacle constraints according to claim 2, characterized in that, After determining the dynamic obstacle prediction boundary for each prediction time, the robot reference position with the same prediction time in the baseline prediction trajectory is registered with the corresponding dynamic obstacle prediction boundary to determine the relative spatial direction from the robot reference position to the corresponding obstacle prediction boundary. When the robot reference position and the corresponding obstacle prediction boundary have a separation space, the local convexity direction is determined according to the relative spatial direction and an approximate convex obstacle constraint is constructed. When there is no separation space, the relative spatial direction is re-determined based on the baseline prediction trajectory re-formed in the current control cycle.

4. The dynamic obstacle avoidance MPC control method for nonholonomic constraint mobile robots based on approximate convex obstacle constraints according to claim 3, characterized in that, The process of constructing approximate convex obstacle constraints includes: determining the separation boundary along the local convexity direction based on the robot reference position and the predicted boundary of the dynamic obstacle at the same prediction time as the robot reference position; projecting the predicted position in the robot prediction state onto the local convexity direction; using the positional relationship between the projected position and the separation boundary as the convex constraint condition for the corresponding prediction time; forming an approximate convex obstacle constraint sequence that corresponds one-to-one with the robot prediction state according to the order of the prediction times; and using the approximate convex obstacle constraint sequence as the obstacle constraint condition for each prediction time in the MPC optimization problem.

5. The dynamic obstacle avoidance MPC control method for nonholonomic constraint mobile robots based on approximate convex obstacle constraints according to claim 1, characterized in that, The process of incorporating the approximate convexity obstacle constraint and the nonholonomic motion constraint into the MPC optimization problem includes: recursively deriving the robot's predicted state at each prediction time according to the current motion state of the mobile robot and the nonholonomic motion constraint, so that each robot predicted state is associated with the approximate convexity obstacle constraint at the same prediction time; participating the prediction control sequence calculation for robot predicted states that satisfy the corresponding approximate convexity obstacle constraint; restricting robot predicted states that do not satisfy the corresponding approximate convexity obstacle constraint to entering the feasible state set at the corresponding prediction time; and obtaining the prediction control sequence that satisfies the nonholonomic motion constraint and the approximate convexity obstacle constraint at each prediction time in the prediction time domain.

6. The dynamic obstacle avoidance (MPC) control method for nonholonomic mobile robots based on approximate convex obstacle constraints according to claim 5, characterized in that, After outputting the current control quantity in the predictive control sequence, the current motion state of the robot after executing the current control quantity is obtained in the next control cycle. The predicted state corresponding to the current control quantity in the previous control cycle is shifted forward according to the prediction time, and the first predicted state after the shift is replaced by the current motion state of the robot after execution, forming the reference predicted trajectory for the next control cycle. The dynamic obstacle prediction boundary is regenerated according to the current state of the dynamic obstacle obtained in the next control cycle, and the approximate convex obstacle constraint is re-determined based on the regenerated dynamic obstacle prediction boundary and the reference predicted trajectory.

7. The dynamic obstacle avoidance MPC control method for nonholonomic constraint mobile robots based on approximate convex obstacle constraints according to claim 6, characterized in that, When redetermining the approximate convex obstacle constraint, the relative geometric relationship between the robot reference position in the baseline predicted trajectory and the corresponding dynamic obstacle predicted boundary is compared at each prediction time. When the robot reference position and the corresponding dynamic obstacle predicted boundary have a separation space, the local convexity direction is updated according to the relative spatial direction and the approximate convex obstacle constraint at the corresponding prediction time is generated. When the robot reference position and the corresponding dynamic obstacle predicted boundary do not have a separation space, the baseline predicted trajectory is re-formed, and the re-formed baseline predicted trajectory is used as the input for determining the local convexity direction in the current control cycle.

8. The dynamic obstacle avoidance (MPC) control method for nonholonomic mobile robots based on approximate convex obstacle constraints according to claim 4, characterized in that, When forming the approximate convex obstacle constraint sequence, for the case where there are multiple dynamic obstacle prediction boundaries at the same prediction time, the relative geometric relationship between the robot reference position and each dynamic obstacle prediction boundary is determined, and corresponding approximate convex obstacle constraints are formed respectively. Each approximate convex obstacle constraint is used as the obstacle restriction condition at the corresponding prediction time, so that the robot prediction state simultaneously satisfies each approximate convex obstacle constraint formed at the corresponding prediction time.

9. The dynamic obstacle avoidance MPC control method for nonholonomic constraint mobile robots based on approximate convex obstacle constraints according to claim 6, characterized in that, When forming the reference predicted trajectory for the next control cycle, the state deviation between the robot's current motion state after executing the current control quantity and the corresponding predicted state in the previous control cycle is calculated. When the state deviation meets the preset deviation range, the reference predicted trajectory is formed by the previously moved predicted state. When the state deviation does not meet the preset deviation range, the robot's current motion state after executing the current control quantity is used as the prediction starting state, and the prediction state is re-produced according to the non-holonomic motion constraints to form the reference predicted trajectory.

10. The dynamic obstacle avoidance MPC control method for nonholonomic constraint mobile robots based on approximate convex obstacle constraints according to claim 7, characterized in that, After the baseline predicted trajectory is re-formed, the relative spatial direction between the robot reference position and the corresponding dynamic obstacle predicted boundary is recalculated at each prediction time according to the re-formed baseline predicted trajectory. Based on the recalculated relative spatial direction, the corresponding approximate convex obstacle constraint is generated. When the robot reference position and the corresponding dynamic obstacle predicted boundary at each prediction time have separation space, the generated approximate convex obstacle constraint is added to the MPC optimization problem and the predictive control sequence is calculated. When there is a prediction time without separation space, the baseline predicted trajectory formation process is re-executed within the current control cycle.

Images