Semi-closed-loop planning method for high-frequency planner
By introducing a trajectory planning closed-loop observer and a speed limiting closed-loop observer into the high-frequency planner, the lag problem when the high-frequency planner and the underlying controller have been solved, achieving fast response and accurate planning, and improving the system's abnormal response capability and trajectory tracking effect.
Patent Information
- Application Number
- CN202511651653.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-11-12
- Publication Date
- 2026-02-24
AI Technical Summary
In existing technologies, when the execution frequencies of the high-frequency planner and the underlying controller are close, there is a lag problem, which leads to a decrease in the performance of the planned trajectory tracking, makes it impossible to correct underlying tracking anomalies in a timely manner, and affects the system's response speed and accuracy.
By introducing a trajectory planning closed-loop observer and a speed limit closed-loop observer, and by judging the abnormal situation of the underlying controller, the planning benchmark and speed limit benchmark are updated using the actual position and speed, thus realizing semi-closed-loop planning.
It improves the system's response speed and anomaly response capability, while taking into account the accuracy of trajectory planning and the stability of closed-loop feedback, thus avoiding planning failure caused by low-level tracking lag.
Smart Images
Figure CN121552339A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to a semi-closed-loop planning method for high-frequency planners. Background Technology
[0002] In the field of robotics, hierarchical control is a common control method. Figure 1 The paper illustrates a hierarchical control framework in the prior art, which is used to perform logical judgment, planning, and prediction tasks. The upper-level planner, with a relatively low control frequency, plans based on the closed-loop feedback information of the controller, while the execution frequency of the controller is often much higher than that of the planner.
[0003] To improve task response speed and reduce replanning time after system interference, the execution frequency of the upper-level planner is continuously increased, even approaching the execution frequency of the lower-level controller. At this point, common closed-loop control methods based on deviation feedback, such as three-loop PID control for motors, are limited by the dynamic characteristics of the system and cannot completely eliminate tracking deviations, resulting in a certain degree of lag. If the planner still replans based on the actual information fed back from the controller in each cycle, the planned trajectory will also lag, ultimately leading to worse trajectory tracking performance of the actuator. This lag becomes particularly significant when the planner's execution frequency is close to that of the controller. Summary of the Invention
[0004] To address the aforementioned technical problems that urgently need to be solved in this field, the present invention provides a semi-closed-loop planning method for high-frequency planners. When the execution frequency of the upper-level planner is close to that of the lower-level controller, it can reduce the significant impact of the lag of the lower-level control on the high-frequency upper-level planner.
[0005] This application provides a semi-closed-loop planning system for high-frequency planners, including:
[0006] The trajectory planning module is used to output the desired trajectory;
[0007] The speed limiting module is used to receive the desired trajectory output by the trajectory planning module and output the command position according to the desired trajectory;
[0008] The underlying control module includes an underlying controller and sensors. The underlying controller is used to receive command positions from the speed limiting module, and the sensors are used to output the actual position and actual speed.
[0009] The trajectory planning closed-loop observer is used to receive the actual position and actual velocity from the underlying control module and the desired trajectory from the trajectory planning module. The trajectory planning closed-loop observer generates a planning benchmark based on the actual position, actual velocity, and desired trajectory, and provides the planning benchmark to the trajectory planning module for replanning.
[0010] The speed limit closed-loop observer receives the actual position from the underlying control module and the command position from the speed limit module. Based on the received actual position and command position, the speed limit closed-loop observer provides the speed limit reference to the speed limit module.
[0011] In at least one embodiment, the trajectory planning closed-loop observer determines whether the underlying controller is malfunctioning based on the actual position, actual speed, and desired trajectory, and generates a planning benchmark based on the determination result.
[0012] In at least one embodiment, when the underlying controller is operating normally, the planned value of the previous moment in the desired trajectory is directly used as the planning reference; when the underlying controller is operating abnormally, the actual speed and actual position are used as the planning reference.
[0013] In at least one embodiment, the current actual position and actual speed are compared with the planned position and planned speed of the previous moment in the desired trajectory using a trajectory planning closed-loop observer. If the lag between the actual position and actual speed and the planned position and planned speed of the previous moment is within a certain threshold range, it is determined that the underlying controller is operating normally. If the lag between the actual position and actual speed and the planned position and planned speed of the previous moment exceeds the threshold range, it is determined that the underlying controller is operating abnormally.
[0014] In at least one embodiment, the speed limit closed-loop observer determines whether the underlying controller is operating abnormally based on the actual position and the command position, and generates a speed limit reference based on the determination result.
[0015] In at least one embodiment, when the underlying controller is operating normally, the command position at the previous moment is directly used as the speed limit reference; when the underlying controller is operating abnormally, the actual position is used as the speed limit reference.
[0016] In at least one embodiment, a speed-limiting closed-loop observer is used to compare the current actual position with the commanded position at the previous moment. If the lag between the actual position and the commanded position at the previous moment is within a certain threshold range, it is determined that the underlying controller is operating normally. If the lag between the actual position and the commanded position at the previous moment exceeds the threshold range, it is determined that the underlying controller is operating abnormally.
[0017] This application also provides a semi-closed-loop planning method for high-frequency planners, utilizing the above-mentioned system for semi-closed-loop planning, the method comprising:
[0018] S1: Output the desired trajectory using the trajectory planning module:
[0019] S2: Receive the desired trajectory output by the trajectory planning module using the speed limiting module, and output the command position according to the desired trajectory;
[0020] S3: The underlying control module receives the command position from the speed limit module, and the sensor feeds back the actual position and actual speed.
[0021] S4: The trajectory planning closed-loop observer receives the actual position and actual speed from the underlying control module and the desired trajectory from the trajectory planning module. The trajectory planning closed-loop observer generates a planning benchmark based on the actual position, actual speed and desired trajectory, and provides the planning benchmark to the trajectory planning module for replanning.
[0022] S5: The speed limit closed-loop observer receives the actual position from the underlying control module and the command position from the speed limit module. Based on the received actual position and command position, the speed limit closed-loop observer provides the speed limit reference to the speed limit module.
[0023] In at least one embodiment, step S4 includes: the trajectory planning closed-loop observer determines whether the underlying controller is operating abnormally based on the actual position, actual speed, and desired trajectory, and generates a planning benchmark based on the determination result; when the underlying controller is operating normally, the planning value of the previous moment in the desired trajectory is directly used as the planning benchmark; when the underlying controller is operating abnormally, the actual speed and actual position are used as the planning benchmark.
[0024] In at least one embodiment, step S5 includes: the speed limiting closed-loop observer determines whether the underlying controller is operating abnormally based on the actual position and the command position, and generates a speed limiting reference based on the determination result; when the underlying controller is operating normally, the command position at the previous moment is directly used as the speed limiting reference; when the underlying controller is operating abnormally, the actual position is used as the speed limiting reference.
[0025] The present invention has the following beneficial effects:
[0026] (1) Due to the discrepancy in execution frequency between the planner and the controller, the existing planning method tracks an approximate open-loop planning sequence during the interval between two instructions issued by the planner, and cannot promptly correct anomalies that occur during the underlying tracking. This invention can shorten the overall system response time and improve the system's anomaly response capability by increasing the execution frequency of the planner.
[0027] (2) Existing closed-loop methods rely on the actual position of the underlying position control based on the deviation. When the execution frequency of the planner increases, the actual position will lag behind the target position. As a result, the trajectory planning will be completely dependent on the actual position. This will cause the actual trajectory to lag behind the expected trajectory, and the accuracy of the trajectory planning cannot be guaranteed. The method described in this invention can take into account both the accuracy of the trajectory planning and the stability of the closed-loop feedback. Attached Figure Description
[0028] The following description, in conjunction with the accompanying drawings, will further illustrate the above-mentioned features, technical characteristics, advantages, and implementation methods of this application in a clear and understandable manner. The accompanying drawings are for illustrative and explanatory purposes only and do not limit the scope of this application. Wherein:
[0029] Figure 1 This illustrates a hierarchical control framework in the prior art;
[0030] Figure 2 The overall structure of a semi-closed-loop planning system for high-frequency planners is shown.
[0031] Figure 3 The response characteristics of one embodiment of this application are shown;
[0032] Figures 4a-4b This is a curve showing the change in the deviation between the desired angle and the actual angle under the ramp command over time.
[0033] Figure 5 The effects of high-frequency planning are compared.
[0034] Figure 6 The feedback characteristics of a semi-closed-loop programming method are shown when an anomaly occurs.
[0035] Figures 7a-7d This demonstrates the effect of using the actual position as the speed limit reference for the difference;
[0036] Figures 8a-8d The effect of using the planning benchmark provided by the closed-loop observer as the differential speed limit benchmark is shown. Detailed Implementation
[0037] The technical solution of the present invention will be clearly and completely described below with reference to the accompanying drawings and some embodiments. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0038] One embodiment of this application provides a semi-closed-loop planning system for high-frequency planners, the overall structure of which is as follows: Figure 2 As shown, it includes:
[0039] The trajectory planning module is used to output the desired trajectory;
[0040] The speed limiting module is used to receive the desired trajectory output by the trajectory planning module and output the command position according to the desired trajectory;
[0041] The underlying control module includes an underlying controller and sensors. The underlying controller is used to receive command positions from the speed limiting module, and the sensors are used to output the actual position and actual speed.
[0042] The trajectory planning closed-loop observer is used to receive the actual position and actual velocity from the underlying control module and the desired trajectory from the trajectory planning module. The trajectory planning closed-loop observer generates a planning benchmark based on the actual position, actual velocity, and desired trajectory, and provides the planning benchmark to the trajectory planning module for replanning.
[0043] The speed limit closed-loop observer receives the actual position from the underlying control module and the command position from the speed limit module. Based on the received actual position and command position, the speed limit closed-loop observer provides the speed limit reference to the speed limit module.
[0044] The semi-closed-loop planning system for high-frequency planners provided in this embodiment introduces a trajectory planning closed-loop observer and a speed-limiting closed-loop observer. First, through mathematical analysis and experiments, the tracking characteristics (delay, deviation, etc.) of the corresponding underlying controller under controllable operating environments (load, speed) are obtained. Based on these tracking characteristics, the sensitivity parameters of the trajectory planning closed-loop observer and the speed-limiting closed-loop observer are determined to determine whether the underlying controller is malfunctioning. When the underlying controller exhibits a significant increase in deviation or severe system lag in the corresponding operating environment, it is considered that the underlying controller's tracking is malfunctioning. When the underlying controller's tracking malfunctions, the planning benchmark is updated promptly, incorporating the actual value as the planning benchmark for the planner to replan.
[0045] In the semi-closed-loop planning system for high-frequency planners provided in this embodiment, the trajectory planning closed-loop observer first determines whether the underlying controller is malfunctioning based on the actual position, actual speed, and desired trajectory, and generates a planning benchmark based on the determination result. When the underlying controller is functioning normally, the planning value (including the planned position and planned speed) of the previous moment in the desired trajectory is directly used as the planning benchmark; when the underlying controller is malfunctioning, the actual speed and actual position are used as the planning benchmark.
[0046] In the semi-closed-loop planning system for high-frequency planners provided in this embodiment, the method for determining whether the underlying controller is operating abnormally is as follows: The current actual position and actual speed are compared with the planning values (including planned position and planned speed) of the previous moment in the desired trajectory using a trajectory planning closed-loop observer. If the lag between the actual position and actual speed and the planning value of the previous moment is within a certain threshold range, it is determined that the underlying controller is operating normally, and the planning value of the previous moment is directly output as the planning benchmark. If the lag between the actual position and actual speed and the planning value of the previous moment exceeds the threshold range, it is determined that the underlying controller is operating abnormally.
[0047] In the semi-closed-loop planning system for high-frequency planners provided in this embodiment, the speed-limiting closed-loop observer first determines whether the underlying controller is malfunctioning based on the actual position and the command position, and generates a speed-limiting reference based on the determination result. When the underlying controller is operating normally, the command position at the previous moment is directly used as the speed-limiting reference; when the underlying controller is malfunctioning, the actual position is used as the speed-limiting reference.
[0048] In the semi-closed-loop planning system for high-frequency planners provided in this embodiment, the method by which the speed-limiting closed-loop observer determines whether the underlying controller is operating abnormally is as follows: The speed-limiting closed-loop observer compares the current actual position with the command position at the previous moment. If the lag between the actual position and the command position at the previous moment is within a certain threshold range, it is determined that the underlying controller is operating normally, and the command position at the previous moment is directly output as the speed-limiting reference. If the lag between the actual position and the command position at the previous moment exceeds the threshold range, it is determined that the underlying controller is operating abnormally.
[0049] According to an embodiment of this application, a semi-closed-loop planning method for high-frequency planners is also provided, which utilizes the aforementioned semi-closed-loop planning system to perform semi-closed-loop planning. The method includes:
[0050] S1: Output the desired trajectory using the trajectory planning module:
[0051] S2: Receive the desired trajectory output by the trajectory planning module using the speed limiting module, and output the command position according to the desired trajectory;
[0052] S3: The underlying control module receives the command position from the speed limit module, and the sensor feeds back the actual position and actual speed.
[0053] S4: The trajectory planning closed-loop observer receives the actual position and actual speed from the underlying control module and the desired trajectory from the trajectory planning module. The trajectory planning closed-loop observer generates a planning benchmark based on the actual position, actual speed and desired trajectory, and provides the planning benchmark to the trajectory planning module for replanning.
[0054] S5: The speed limit closed-loop observer receives the actual position from the underlying control module and the command position from the speed limit module. Based on the received actual position and command position, the speed limit closed-loop observer provides the speed limit reference to the speed limit module.
[0055] Specifically, step S4 includes: the trajectory planning closed-loop observer first determines whether the underlying controller is malfunctioning based on the actual position, actual speed, and desired trajectory, and generates a planning benchmark based on the determination result. When the underlying controller is functioning normally, the planning value of the previous moment in the desired trajectory is directly used as the planning benchmark; when the underlying controller is malfunctioning, the actual speed and actual position are used as the planning benchmark.
[0056] Specifically, step S5 includes: the speed-limiting closed-loop observer first determines whether the underlying controller is operating abnormally based on the actual position and the commanded position, and generates a speed-limiting reference based on the determination result. When the underlying controller is operating normally, the commanded position at the previous moment is directly used as the speed-limiting reference; when the underlying controller is operating abnormally, the actual position is used as the speed-limiting reference.
[0057] In the system and method provided in this application, the closed-loop observer combines the planning instruction of the previous moment with the actual feedback from the underlying layer to observe the result of the underlying control tracking. It needs to be judged in conjunction with the performance of the underlying controller. When the underlying control is running normally, the planning value of the previous moment is directly used as the planning benchmark. When the underlying control tracking is abnormal, the planning benchmark is updated in time, and the actual value is introduced as the planning benchmark so that the planner can replan.
[0058] In the system and method provided in this application, the planner parses user instructions and generates the task to be planned based on actual task requirements (such as moving towards a specified point, following a target point, moving along a determined trajectory, etc.) and planning algorithm parameters (such as maximum speed, maximum acceleration, planning time, trajectory endpoint, etc.). The planner then performs planning in conjunction with the planning benchmark provided by the closed-loop observer.
[0059] In another embodiment according to this application, trajectory planning and speed limiting are presented as typical planner tasks, a fixed-load motor is used as the controlled object, and a three-loop PI control method is used as the control method of the underlying high-frequency controller. Its response characteristics under a 1° step command and a 1° / s ramp command are as follows: Figure 3 , Figure 4a and Figure 4b As shown. Among them Figure 4b This is a curve showing the change in the deviation between the desired angle and the actual angle under the ramp command over time.
[0060] By constructing a closed-loop observer based on the tracking characteristics of the underlying controller and observing its response characteristics under ramp commands, it can be seen that its tracking error is approximately 1% of the command speed. Therefore, a sensitivity threshold parameter for the closed-loop observer is set. , The sensitivity coefficient is denoted as .
[0061] The closed-loop observer generates the planning baseline by following these steps:
[0062] Compare with current actual feedback values Compared to the instruction value at the previous moment The system determines whether an abnormal situation has occurred. If the lag of the feedback value from the previous instruction value is within a certain range, the system is considered to be without abnormality, and the previous instruction value is directly output as the planning baseline. If it exceeds this range, the task system has encountered an abnormality, and the planning baseline is updated to the current actual value. The specific execution algorithm is as follows:
[0063]
[0064] The planner plans based on the planning benchmark provided by the closed-loop observer. The planning purpose needs to be designed according to the actual working conditions and task requirements of the controlled object. Here, we will demonstrate the effect of the closed-loop observer by using the fifth-order polynomial trajectory planning method and the speed limiting method based on the numerical derivative of position.
[0065] in, Figure 5 The effects of high-frequency planning based on the planning benchmark provided by the closed-loop observer and the fully closed-loop planning based on the actual position were compared. It can be found that due to the lag of the actual position, the planned trajectory lags far behind the expected trajectory. The motor joint angle trajectory is basically inconsistent with the expected trajectory. However, the semi-closed-loop planning method can ensure the tracking effect with the expected trajectory. Figure 6 The feedback characteristics of semi-closed-loop planning are demonstrated when an anomaly occurs. The motor is hit by an external force at 10 seconds, causing a large offset. The closed-loop observer identifies that the actual tracking deviation is much larger than the normal tracking lag deviation and updates the planning starting point, so that the planning can continue smoothly. This shows that semi-closed loop can also achieve the same feedback effect as the actual closed loop.
[0066] The speed limiting method based on position numerical difference refers to approximating the speed value of the controlled object by dividing the position difference between two adjacent control cycles by the control cycle. Figure 7 shows the effect of directly using the actual position as the speed limiting reference, and Figure 8 shows the effect of using the planning reference provided by the closed-loop observer as the speed limiting reference. In both cases, the speed limiting constraint is set to 3° / s. (Figures 7 and 8 are not included in the provided text.) Figure 8a , Figure 8b The display shows the position and velocity curves of the equipment under slope commands of 2° / s and 5° / s, respectively. Figure 8c , Figure 8d The equipment was demonstrated in The position and velocity curves under the position command are shown. It can be found that the speed limit under the semi-closed-loop method is basically consistent with the set speed limit value, but the speed limit based on the actual position is much smaller than the set speed limit value due to the lag of the actual position.
[0067] The two examples above demonstrate that the semi-closed-loop planning method can both avoid planning failures caused by the lag in the underlying tracking accuracy during high-frequency planning and ensure timely closed-loop feedback of anomalies at the underlying level.
[0068] It should be understood that the sequence number of each step in the embodiments of the present invention does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.
[0069] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.
Claims
1. A semi-closed-loop planning system for high-frequency planners, comprising: The trajectory planning module is used to output the desired trajectory; The speed limiting module is used to receive the desired trajectory output by the trajectory planning module and output the command position according to the desired trajectory; The underlying control module includes an underlying controller and sensors. The underlying controller is used to receive commanded positions from the speed limiting module, and the sensors are used to output the actual position and actual speed. The trajectory planning closed-loop observer is used to receive the actual position and actual velocity from the underlying control module and the desired trajectory from the trajectory planning module. The trajectory planning closed-loop observer generates a planning benchmark based on the actual position, actual velocity, and desired trajectory, and provides the planning benchmark to the trajectory planning module for replanning. The speed limit closed-loop observer receives the actual position from the underlying control module and the command position from the speed limit module. Based on the received actual position and command position, the speed limit closed-loop observer provides the speed limit reference to the speed limit module.
2. The system according to claim 1, wherein, The trajectory planning closed-loop observer determines whether the underlying controller is malfunctioning based on the actual position, actual speed, and desired trajectory, and generates a planning baseline based on the determination result.
3. The system according to claim 1, wherein, When the underlying controller is operating normally, the planned value of the previous moment in the desired trajectory is directly used as the planning reference; when the underlying controller is operating abnormally, the actual speed and actual position are used as the planning reference.
4. The system according to claim 3, wherein, The trajectory planning closed-loop observer is used to compare the current actual position and actual speed with the planned position and planned speed of the previous moment in the expected trajectory. If the lag between the actual position and actual speed and the planned position and planned speed of the previous moment is within a certain threshold range, it is determined that the underlying controller is not operating abnormally. If the lag between the actual position and actual speed and the planned position and planned speed at the previous moment exceeds the threshold range, it is determined that the underlying controller is malfunctioning.
5. The system according to claim 1, wherein, The speed limit closed-loop observer determines whether the underlying controller is operating abnormally based on the actual position and the command position, and generates a speed limit reference based on the judgment result.
6. The system according to claim 1, wherein, When the underlying controller is operating normally, the command position of the previous moment is used as the speed limit reference; when the underlying controller is operating abnormally, the actual position is used as the speed limit reference.
7. The system according to claim 6, wherein, The current actual position is compared with the commanded position at the previous moment using a speed limit closed-loop observer. If the lag between the actual position and the commanded position at the previous moment is within a certain threshold range, it is determined that the underlying controller is operating normally. If the lag between the actual position and the command position at the previous moment exceeds the threshold range, it is determined that the underlying controller is malfunctioning.
8. A semi-closed-loop planning method for high-frequency planners, utilizing the semi-closed-loop planning system according to claim 1, the method comprising: S1: Output the desired trajectory using the trajectory planning module: S2: Receive the desired trajectory output by the trajectory planning module using the speed limiting module, and output the command position according to the desired trajectory; S3: The underlying control module receives the command position from the speed limit module, and the sensor feeds back the actual position and actual speed. S4: The trajectory planning closed-loop observer receives the actual position and actual speed from the underlying control module and the desired trajectory from the trajectory planning module. The trajectory planning closed-loop observer generates a planning benchmark based on the actual position, actual speed and desired trajectory, and provides the planning benchmark to the trajectory planning module for replanning. S5: The speed limit closed-loop observer receives the actual position from the underlying control module and the command position from the speed limit module. Based on the received actual position and command position, the speed limit closed-loop observer provides the speed limit reference to the speed limit module.
9. The method according to claim 8, wherein, Step S4 above includes: the trajectory planning closed-loop observer determines whether the underlying controller is operating abnormally based on the actual position, actual speed, and desired trajectory, and generates a planning benchmark based on the judgment result; when the underlying controller is operating normally, the planning value of the previous moment in the desired trajectory is directly used as the planning benchmark; when the underlying controller is operating abnormally, the actual speed and actual position are used as the planning benchmark.
10. The method according to claim 8, wherein, Step S5 above includes: the speed limiting closed-loop observer determines whether the underlying controller is operating abnormally based on the actual position and the command position, and generates a speed limiting reference based on the judgment result; when the underlying controller is operating normally, the command position at the previous moment is directly used as the speed limiting reference; when the underlying controller is operating abnormally, the actual position is used as the speed limiting reference.