Staged two-wheel maneuvering platform dynamic reconstruction method

Abstract

The invention discloses a staged two-wheeled maneuvering platform dynamic reconstruction method, which comprises the following steps of: an approaching stage: synchronizing state information among participating balance vehicles, generating an optimal docking track, approaching the participating balance vehicles in three dimensions of course angle, speed and spatial position, and tracking the optimal docking track; a contact stage: performing high-frequency synchronization on attitude information between the participating balance cars, gradually approaching the participating balance cars, keeping projections of longitudinal axes of the participating balance cars on a horizontal plane parallel, opposite pitch angles, gradually approaching the participating balance cars, gradually slowing down the speed and stabilizing to a preset value until docking mechanisms of the participating balance cars are in contact and enter a locking position; and in the locking stage, preset pressure is jointly applied to the butt joint mechanism, the butt joint mechanism is kept at the locking position till locking is completed, and dynamic reconfiguration is completed among the balance cars. According to the invention, the reliability and success rate of autonomous docking are enhanced.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of multi-machine cooperative dynamic reconfiguration, in particular to a two-wheel machine platform dynamic reconfiguration method in stages. BACKGROUND

[0002] Self-balancing robots, also known as balance cars, are first used for traffic commuting. It uses electronic gyroscopes (MEMS) to monitor the attitude information of the vehicle in real time, and adjusts the speed of the wheels to maintain longitudinal balance, and adjusts the speed difference between the two wheels to realize heading control. Compared with other multi-wheel configurations, the double-wheel balance car can flexibly turn and move in a small space.

[0003] This configuration has many advantages, but also has some limitations. Self-balancing robots have high requirements for road flatness, and it is difficult to drive on bumpy, gravel, slippery or muddy roads. And because it is a static unstable system, instability will inevitably occur when the external disturbance exceeds the control threshold.

[0004] Therefore, the concept of dynamic reconfiguration is proposed, which enables the robot to switch between self-balancing robots and multi-wheel robots in time sequence, and to switch between flexibility and stable control. However, existing research lacks research on self-balancing robot reconfiguration, especially in the field of autonomous dynamic docking of two cars, which is still a blank. SUMMARY

[0005] To solve the above technical problems in the prior art, the present application provides a two-wheel machine platform dynamic reconfiguration method in stages, which can effectively expand the functionality of the system by reorganizing multiple individuals into a new form, providing more power and flexibility, and better adapting to complex environments and diverse task requirements.

[0006] To achieve the above purpose, the present application provides a two-wheel machine platform dynamic reconfiguration method in stages, comprising:

[0007] Approach stage: synchronize the state information between the participating balance cars and generate the optimal docking trajectory, and approach and track the optimal docking trajectory in the three dimensions of heading angle, speed and spatial position;

[0008] Contact stage: synchronize the attitude information between the participating balance cars at a high frequency, and gradually approach, keep the projections of the longitudinal axes of the participating balance cars on the horizontal plane parallel to each other, the pitch angles opposite, the distance gradually close, the speed gradually slow down and stabilize to a predetermined value, until the docking mechanism of the participating balance cars contacts and enters the locking position;

[0009] Locking stage: a predetermined pressure is applied to the docking mechanism, and the docking mechanism is kept in the locking position until the locking is completed, and the dynamic reconfiguration between the participating balance cars is completed.

[0010] Wherein, in the approach stage, the contact stage and the locking stage are provided with jump-out conditions and back-off mechanisms, and when the stage task cannot be completed, it is automatically backed off to the previous stage or reattempted.

[0011] Preferably, synchronizing the state information between the balance cars comprises:

[0012] Verifying the working state of the on-board attitude sensor and motor controller of the balance car, testing the balance performance, ensuring that each participating reconstruction individual has basic maneuvering capability, and then entering a static balance state to wait for instructions;

[0013] Verifying whether the V2V network communication between different balance cars is normal, and testing the communication delay and packet loss;

[0014] After receiving the balance car docking instructions, the participating vehicle reestablishes the communication connection, synchronizes the attitude information and vehicle motion constraint information.

[0015] Preferably, the participating vehicles use a unified kinematic model for state synchronization, wherein the state space equation of the kinematic model is:

[0016] ;

[0017] In the formula, is the current system state vector, is the control vector, is the heading angle, is the heading angle velocity, a is the longitudinal acceleration, x and y are spatial coordinates, is the system state at the next time, is the linear velocity.

[0018] Preferably, the reference trajectory is estimated through bottom attitude control, wherein the bottom attitude control uses a multi-stage PID controller, including a longitudinal control loop and a heading control loop, wherein:

[0019] The longitudinal control loop includes a pitch angle velocity loop, a pitch angle loop and a linear velocity loop;

[0020] The heading control loop includes a yaw angle velocity loop;

[0021] Wherein, the running frequency of each control loop decreases step by step.

[0022] Preferably, generating the optimal docking trajectory comprises:

[0023] Determining a trajectory control point according to the initial state of the participating balance car, and determining a trajectory segment and a straight line segment based on the trajectory control point;

[0024] The quadratic Bezier interpolation is used in the trajectory segment, and the linear interpolation is used in the straight line segment, wherein the trajectory equation is:

[0025] ;

[0026] In the formula, is a trajectory vector coordinate function, t is a sampling normalized value of each stage, 、 、 、 、 、 are all vector coordinates of path control points, 、 、 、 are all path length values corresponding to each point on the trajectory, and s is a path length corresponding to the current sampling point.

[0027] Preferably, in the approaching stage, the position deviation, the angle deviation, the speed deviation and the heading angle deviation between the two vehicles for the reference trajectory tracking of the participating balance car are considered by using a cost function of an optimization control method, and the mathematical expression form of the cost function is:

[0028] ;

[0029] ;

[0030] ;

[0031] In the formula, 、 、 、 are tracking error weight parameters of the lateral coordinate, the longitudinal coordinate, the linear velocity and the yaw angle; and are single-vehicle tracking error cost functions of the two vehicles; 、 、 、 are the lateral coordinate value, the linear velocity and the yaw angle value of the vehicle numbered i, respectively; , , , are reference values of corresponding variables on the trajectory; i represents the number of rounds of corresponding time; N represents the total number of calculation rounds; is a final cost function; is a yaw angle error weight parameter corrected by scaling; T is an algorithm prediction control period, and is an adjustable hyperparameter; is the yaw angle of the No. 1 vehicle at the i time; is the yaw angle of the No. 2 vehicle at time i.

[0032] Preferably, the jump-out condition of the approaching stage is:

[0033] ;

[0034] The rollback condition of the approaching stage is:

[0035] ;

[0036] In the formula, , , The longitudinal coordinate value and the yaw angle of the vehicle numbered i, is the successful distance of the docking mechanism distance of the two vehicles, is the minimum attempt distance of the docking mechanism distance of the two vehicles, is the maximum tolerance of the vehicle body yaw angle.

[0037] Preferably, in the contact stage, an optimization control method is used to optimize a cost function considering the position deviation, heading angle deviation, pitch angle deviation and speed deviation of the two vehicles, and the participating balance vehicle attitude error weight gradually increases, specifically:

[0038] ;

[0039] ;

[0040] ;

[0041] ;

[0042] ;

[0043] ;

[0044] In the formula, the subscripts , , , , respectively represent the error weight parameters corresponding to the longitudinal coordinate, the longitudinal coordinate, the yaw angle, the pitch angle and the linear velocity; , , , , are the correction error weight parameters corresponding to the scaled longitudinal coordinate, the longitudinal coordinate, the yaw angle, the pitch angle and the linear velocity; i represents the number of wheels corresponding to the time; N represents the total number of calculation rounds; and COST is the final cost function; , , , , xi, yi, θi, φi, vi respectively represent the lateral coordinate, longitudinal coordinate, yaw angle, pitch angle and linear velocity of the No. 1 vehicle at time i, , , , , xi, yi, θi, φi, vi respectively represent the lateral coordinate, longitudinal coordinate, yaw angle, pitch angle and linear velocity of the No. 2 vehicle at time i.

[0045] Preferably, the jump-out condition of the contact phase is:

[0046] ;

[0047] The rollback condition of the contact phase is:

[0048] ;

[0049] In the formula, , , , , xi, yi, θi, φi, vi respectively represent the lateral coordinate, longitudinal coordinate, yaw angle, pitch angle and linear velocity of the No. i vehicle, is the successful distance of the docking mechanism distance of the two vehicles, is the minimum attempt distance of the docking mechanism distance of the two vehicles, is the maximum tolerance of vehicle speed, is the maximum tolerance of vehicle body yaw angle, is the maximum tolerance of vehicle body pitch angle.

[0050] Preferably, the locking phase directly controls the torque of the wheels on both sides using a parallel PID controller, and only controls the heading angle and wheel speed:

[0051] Heading controller:

[0052] ;

[0053] ;

[0054] Wheel speed controller:

[0055] ;

[0056] ;

[0057] Final control quantity:

[0058] ;

[0059] ;

[0060] In the formula, the superscript t represents the value of the current time, t-1 represents the value of the previous time, is the current yaw angle error, is the current reference yaw angle, is the current sensor observed yaw angle, pid, integral, and differential adjustment parameters, respectively, is the current target control yaw angle, is the current speed error, is the current reference linear speed, is the current left wheel speed meter equivalent linear speed, is the right wheel speed meter equivalent linear speed, is the current target control linear speed, , is the current left and right target control amount.

[0061] Compared with the prior art, the present application has the following advantages and technical effects:

[0062] (1) The present application can effectively expand the functionality of the system by reorganizing multiple individuals into a new form, providing stronger power and flexibility, and better adapting to complex environments and diverse task requirements. Through the model predictive control method, different optimization control objectives are established according to different docking stages, and local trajectory planning and control are simultaneously performed to ensure accurate matching of pitch angle, heading angle, speed, and acceleration during the reorganization into a new individual, thereby automatically completing dynamic reconfiguration, avoiding the risk of human operation failure, and improving the speed and safety of the docking reconfiguration process. Through the regulation and control method provided by the present application, autonomous dynamic autonomous docking reconfiguration can be completed, providing a new idea for multi-machine cooperative work.

[0063] (2) The present application completes planning and control by analyzing different requirements at different stages, and enhances the reliability and success rate of autonomous docking through a stage rollback mechanism. DETAILED DESCRIPTION

[0064] The accompanying drawings, which form a part of this application, are included to provide a further understanding of the application and are incorporated in and constitute a part of this application. The embodiments of the present application illustrated in the drawings and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0065] Figure 1 is a flow chart of a phased two-wheel machine maneuver platform dynamic reconfiguration method according to an embodiment of the present application;

[0066] Figure 2 is a schematic diagram of a docking controller structure according to an embodiment of the present application;

[0067] Figure 3 A schematic diagram is generated for the reference trajectory of the embodiment of the present application. DETAILED DESCRIPTION

[0068] It should be noted that the embodiments in the present application and the features in the embodiments can be combined with each other without conflict. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0069] It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although the logical order is shown in the flowchart, in some cases, the steps shown or described herein can be executed in an order different from that shown herein.

[0070] The present embodiment proposes a phased two-wheel maneuvering platform dynamic reconstruction method, as shown in Figure 1 , comprising:

[0071] Approaching phase: synchronizing the state information between the participating balance cars and generating the optimal docking trajectory, and approaching and tracking the optimal docking trajectory in the three dimensions of heading angle, speed and spatial position of the participating balance cars;

[0072] Contact phase: synchronizing the attitude information between the participating balance cars at a high frequency, and gradually approaching, keeping the projections of the longitudinal axes of the participating balance cars on the horizontal plane parallel to each other, the pitch angles opposite, the distance gradually approaching, the speed gradually slowing down and stabilizing to a predetermined value, until the docking mechanisms of the participating balance cars contact and enter the locking position;

[0073] Locking phase: applying a predetermined pressure to the docking mechanism together, keeping the docking mechanism in the locking position until the locking is completed, and then completing the dynamic reconstruction between the participating balance cars;

[0074] Among them, the jump-out condition and the back-off mechanism are provided in the approaching phase, the contact phase and the locking phase, and when the stage task cannot be completed, it is automatically backed off to the previous stage or reattempted.

[0075] Further, synchronizing the state information between the balance cars comprises:

[0076] Verify the working state of the on-board attitude sensor and motor controller of the balance car, test the balance performance, and ensure that each participating reconstruction individual has basic maneuvering capability, and then enter a stationary balance state to wait for instructions;

[0077] Verify whether the V2V network communication between different balance cars is normal, and test the communication delay and packet loss;

[0078] After receiving the balance car docking instruction, the participating vehicle re-establishes the communication connection, synchronizes the attitude information and vehicle motion constraint information.

[0079] Specifically, comprising:

[0080] Verify the working state of the vehicle-mounted attitude sensor and the motor controller, test the single vehicle balance performance, ensure that each participating individual has basic maneuvering ability, and then enter a static balance state to wait for instructions.

[0081] Verify the normal communication of the multi-vehicle V2V network, test the communication delay and packet loss.

[0082] After receiving the docking instruction of the two vehicles, the participating parties establish communication connection again, synchronize attitude information and vehicle motion constraint information.

[0083] Further, in order to facilitate different platforms to dock reconstruction, the kinematic models of various platforms are unified, so that the kinematic characteristic information of the two vehicles is synchronized when docking, and participates in docking under the unified control of the docking reconstruction controller, and the kinematic equation has the following form:

[0084] ;

[0085] In the formula, is the heading angle, is the heading angle velocity, a is the longitudinal acceleration, x and y are spatial coordinates, is the heading angle change rate, is the linear velocity change rate, , is the spatial coordinate change rate.

[0086] The acceleration of the self-balancing system has a great relationship with the current angle and speed of the platform, so the equation also needs to satisfy the constraint condition:

[0087] ;

[0088] The discrete form of the equation is:

[0089] ;

[0090] The corresponding state space equation is:

[0091] ;

[0092] In the formula, is the current system state vector, is the control vector, is the heading angle, is the heading angle velocity, a is the longitudinal acceleration, x and y are spatial coordinates, is the system state at the next time, is the linear velocity.

[0093] Further, the reference trajectory is estimated by the underlying attitude controller, wherein the underlying attitude controller employs a multi-stage PID controller including a longitudinal control loop and a heading control loop, wherein:

[0094] The longitudinal control loop includes a pitch angle velocity loop, a pitch angle loop, and a linear velocity loop;

[0095] The heading control loop includes a yaw angle velocity loop;

[0096] wherein the operating frequency of each control loop decreases step by step.

[0097] Specifically, under the docking controller, there is an underlying attitude controller of each platform, the docking controller is used to track the linear velocity and angular velocity derived by the upper layer planning, and the docking controller employs a multi-stage PID to realize the underlying attitude controller, which includes a pitch angle velocity loop, a pitch angle loop, and a linear velocity loop in the longitudinal direction, and includes a yaw angle velocity loop in the heading control. The operating frequency of each loop decreases step by step, which is manifested as high frequency of the underlying control loop and low frequency of the upper layer control loop, so as to weaken the problem of insufficient bandwidth response. The structure of the docking controller is as shown in Figure 2 .

[0098] The mathematical expression of the specific calculation method is as follows:

[0099] ;

[0100] ;

[0101] ;

[0102] ;

[0103] ;

[0104] ;

[0105] ;

[0106] ;

[0107] ;

[0108] ;

[0109] ;

[0110] In the formula, the superscript t represents the value at the current time, and t-1 represents the value at the previous time; is the linear velocity at the current time, and different subscripts represent the equivalent vehicle linear velocity values measured by the left and right wheel speed sensors; is the yaw angle error at the current time, is the current reference yaw angle, is the current sensor observed yaw angle; CLAMP is a clamp function, is the pid proportional, integral, derivative adjustment parameter, is the maximum allowable pitch angle; is the current time pitch angle error, is the current reference pitch angle, is the current sensor observed pitch angle; is the pitch angle velocity, is a remapping function of the pid proportional, integral, derivative adjustment parameter, which takes the pitch angle as the independent variable to achieve the adaptability of the controller to the nonlinear system; is the current time pitch angle velocity error, is the current reference pitch angle velocity, is the current sensor observed pitch angle velocity; is the current time yaw angle velocity error, is the current reference yaw angle velocity, is the current sensor observed yaw angle velocity; is the longitudinal control amount, is the lateral control amount; , is the current left and right target control amount.

[0111] Further, the optimal docking trajectory generation comprises:

[0112] determining a trajectory control point according to the initial state of the participating balance car, and determining a trajectory segment and a straight line segment based on the trajectory control point;

[0113] adopting quadratic Bezier interpolation in the trajectory segment and linear interpolation in the straight line segment, wherein the trajectory equation is:

[0114] ;

[0115] wherein, is a trajectory vector coordinate function, t is a sampling normalized value of each stage, , , , , , are all vector coordinates of path control points, , , , are all path length values corresponding to each point on the trajectory, and s is the path length corresponding to the current sampling point.

[0116] Specifically, the approaching stage reference trajectory for two vehicles docking should have the following characteristics:

[0117] The tangential direction of the initial stage of the trajectory should be the same as the longitudinal direction of the two vehicles in the initial state.

[0118] The initial speed of the trajectory should be the same as the output speed of the two vehicles.

[0119] The terminal speed of the trajectory is equal to the given terminal speed, and the speed gradually decreases.

[0120] The trajectory terminal should coincide and have as long a straight part as possible to facilitate docking.

[0121] Based on the above requirements, the optimal docking trajectory generation steps are as follows:

[0122] According to the initial state, determine points A and E, and construct an extension line according to the direction of the initial speed of the two vehicles, intersecting at point C, as shown in Figure 3 .

[0123] Calculate the positions of points B and D according to the predetermined value, so that they satisfy the equation:

[0124] ;

[0125] ;

[0126] Calculate the positions of points F and G so that the four points BFGD are collinear and satisfy:

[0127] ;

[0128] In the formula, represents the modulus of the AB vector, i.e. the distance between points A and B; represents the modulus of the ED vector, i.e. the distance between points E and D; represents the modulus of the BF vector, i.e. the distance between points B and F; represents the modulus of the DE vector, i.e. the distance between points D and E; represents the modulus of the DG vector, i.e. the distance between points D and G; is the vector from A to C, is the vector from E to C, and is a value in the range [0, 1], and the specific value is obtained by solving the equation.

[0129] After completing the control point calculation, perform interpolation calculation in segments, perform Bezier interpolation in the AF and GE segments, and perform direct linear interpolation in the FG stage.

[0130] Quadratic Bezier interpolation is performed according to the following formula:

[0131] ;

[0132] wherein, are three control point coordinates of the difference formula, and t is an interpolation normalization quantity.

[0133] The trajectory equation satisfies the following mathematical form:

[0134] ;

[0135] wherein, is a trajectory vector coordinate function, t is a sampling normalization value of each stage, , , , , , is a vector coordinate of a path control point as shown in Figure Three , , , , is a corresponding path length value of each point on the trajectory, and s is a path length corresponding to the current sampling point.

[0136] Further, in the approaching stage, the position deviation, the angle deviation, the speed deviation and the heading angle deviation between the two vehicles for the reference trajectory tracking of the participating balance car are considered by using a cost function of an optimization control method, and the mathematical expression form of the cost function is:

[0137] ;

[0138] ;

[0139] ;

[0140] wherein, , , , are tracking error weight parameters of the lateral and longitudinal coordinates, the linear velocity and the yaw angle; and are single-vehicle tracking error cost functions of the two vehicles; , , , are the lateral and longitudinal coordinate values, the linear velocity and the yaw angle value of the vehicle numbered i, respectively; , , , are reference values of the corresponding variables on the trajectory; i represents the number of rounds of the corresponding time; and N represents the total number of calculation rounds. is the final cost function; is the yaw angle error weight parameter corrected by scaling, T is the algorithm prediction control period, and is an adjustable hyperparameter; is the yaw angle of the first vehicle at time i; is the yaw angle of the second vehicle at time i.

[0141] According to the above form, an optimization problem is constructed and solved using a solver, and finally the control command is sent to the lower controller for execution.

[0142] Further, the spatial distance between the two vehicles and the difference between the heading angles of the two vehicles and the heading of the line connecting the two vehicles are continuously detected during the control process, and the following conditions need to be met simultaneously:

[0143] ;

[0144] If the following conditions are met within this phase time, it means that this phase has failed, and the two vehicles need to stop docking, first move away from each other, and then re-dock:

[0145] ;

[0146] wherein, , , the longitudinal and lateral coordinates and the yaw angle of the vehicle numbered i, is the successful distance of the docking mechanism distance between the two vehicles, is the minimum attempt distance of the docking mechanism distance between the two vehicles, is the maximum tolerance of the vehicle body yaw angle.

[0147] Further, in the contact phase, an optimization control method is used to optimize the cost function considering the position deviation, heading angle deviation, pitch angle deviation and speed deviation of the two vehicles, and the participation balance vehicle attitude error weight gradually increases, specifically:

[0148] ;

[0149] ;

[0150] ;

[0151] ;

[0152] ;

[0153] ;

[0154] wherein, is the corresponding error weight parameter; are the modified error weight parameters after scaling; subscripts x, y, , , v represent the weight parameters corresponding to the horizontal coordinate, the vertical coordinate, the yaw angle, the pitch angle, and the linear velocity, respectively; i represents the number of the corresponding time; N represents the total number of calculation rounds; COST is the final cost function; X, Y, , , V represent the values of the horizontal coordinate, the vertical coordinate, the yaw angle, and the pitch angle at the i-th round, and the superscript number represents the vehicle number corresponding to the parameter.

[0155] According to the above form, the optimization solving problem is constructed, and a solver is used for solving, and finally the control instructions are sent to the lower controller for execution.

[0156] Further, the spatial distance between the two vehicles and the difference between the heading angles of the two vehicles are continuously detected in the control process, and the following conditions need to be met at the same time:

[0157] ;

[0158] If the following conditions are met within this phase time, it means that this phase fails, and the two vehicles need to stop docking and re-dock:

[0159] ;

[0160] In the formula, , , , , X i, Y i, V i, Yaw i, Pitch i are the horizontal and vertical coordinate values, the linear velocity, the yaw angle, and the pitch angle of the vehicle numbered i, is the successful distance of the docking mechanism distance of the two vehicles, is the minimum attempt distance of the docking mechanism distance of the two vehicles, is the maximum tolerance of the vehicle speed, is the maximum tolerance of the body yaw angle, is the maximum tolerance of the body pitch angle.

[0161] Further, the locking phase uses a parallel PID controller to directly control the torque of the two side wheels, and only controls the heading angle and the wheel speed:

[0162] Heading controller:

[0163] ;

[0164] ;

[0165] Wheel speed controller:

[0166] ;

[0167] ;

[0168] Final control quantity:

[0169] ;

[0170] ;

[0171] In the formula, the superscript t represents the value at the current time, and t-1 represents the value at the previous time. This represents the yaw angle error at the current moment. This is the current reference yaw angle. The yaw angle observed by the current sensor. These are the proportional, integral, and derivative adjustment parameters for PID. To control the pendulum angle for the current target, This represents the current speed error. The current reference linear velocity, The current equivalent linear velocity of the left wheel speedometer. The equivalent linear velocity of the right wheel speed gauge. The current target control linear velocity, , This represents the current left and right target control values.

[0172] Lock detection and phase rollback:

[0173] The locking process is determined by detecting the speed pulse signal of the locking motor. If no motor rotation is detected within 100ms, the locking is considered successful. If no successful locking is detected for an extended period, the locking has failed, requiring reverse rotation to loosen the locking mechanism and return to the contact phase for re-engagement.

[0174] This embodiment provides a phased dynamic reconfiguration method for a two-wheeled mobility platform, dividing the entire process into three phases: approach phase, contact phase, and lock-on phase. By analyzing the different requirements of each phase, targeted planning and control are implemented, and a phase rollback mechanism enhances the reliability and success rate of the system's autonomous docking.

[0175] During the approach phase, the two vehicles are relatively far apart and may encounter obstacles. To address the dynamic environment and vehicle constraints, path planning is introduced to generate a predetermined docking path. By setting requirements for the curvature and trajectory overlap rate at the end of the path, the length of the straight overlapping path is maximized. Adjustments to speed and direction in advance ensure that the heading angles and speeds of the two vehicles are essentially consistent during subsequent assembly, reducing the probability of docking mechanism locking failure during reconstruction and significantly improving the overall operational safety.

[0176] In the contact stage, the two vehicles have reached the collinear state in the approach stage, at this time the two vehicles have the same heading angle and the same heading angle with the position line, and the spatial distance is also very close, so the two vehicles only need to keep near the current state and gradually approach. At this time, the front and rear vehicles are required to have opposite pitch angles, so that the docking mechanism can correctly enter the guide section and continue to approach. The method of generating trajectory and tracking is abandoned in this stage, and the direct control method is used to improve the response speed of the algorithm. Higher precision is achieved through higher frequency control, and the success rate of docking is improved.

[0177] In the locking stage, the docking mechanism of the two vehicles has completed the initial contact. Due to the characteristics of self-balancing configuration, when the components of the two vehicles are in contact, they will affect each other's body posture. At this time, it is not possible to effectively control the body pitch angle and speed, but the actual state of the two vehicles is already very close, so there is no need to control the posture and position. In this stage, the controller only needs to keep the heading angle and continuously apply pressure to the docking mechanism to complete the final locking with the help of the guide mechanism and the locking mechanism. PID parallel mechanism can be used to control the vehicle heading angle and wheel speed to achieve control. In this process, the pitch angle of the two vehicles is monitored in real time, and if it exceeds the tolerance, it is determined that the locking fails, and the two vehicles immediately disengage and return to the contact stage. This scheme can effectively avoid interference between the two vehicles after contact, and avoid the system entering an uncontrollable state due to unknown disturbances.

[0178] In the docking process, due to the existence of multiple participants and the direct existence of close cooperation, the errors of both parties may be superimposed to cause the final reconstruction to fail. By dividing the stage and adding a rollback mechanism, the system can be reattempted after failure, effectively improving the success rate and reliability of autonomous docking in the absence of human intervention.

[0179] The above is only the preferred specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any changes or replacements that can be easily thought of by those skilled in the art within the technical scope disclosed in the present application should be covered within the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A phased dynamic reconfiguration method for a two-wheeled mobility platform, characterized in that, include: Approach phase: Synchronize the state information of the participating balance vehicles and generate the optimal docking trajectory. Approach and track the participating balance vehicles in three dimensions: heading angle, speed and spatial position. Contact phase: The attitude information of the participating balance vehicles is synchronized at a high frequency, and they gradually approach each other, keeping the projections of the longitudinal axes of the participating balance vehicles on the horizontal plane parallel to each other, with opposite pitch angles, gradually approaching each other, gradually slowing down and stabilizing to a predetermined value, until the docking mechanism of the participating balance vehicles contacts and enters the locking position; Locking phase: A predetermined pressure is applied to the docking mechanism to keep it in the locked position until the locking is completed, then the dynamic reconfiguration between the participating balance vehicles is completed; The approach phase, contact phase, and locking phase are all equipped with exit conditions and rollback mechanisms. When a phase task cannot be completed, it will automatically roll back to the previous phase or retry.

2. The phased dynamic reconfiguration method for a two-wheeled mobility platform according to claim 1, characterized in that, Synchronizing the status information between the self-balancing vehicles includes: Verify the working status of the vehicle's onboard attitude sensor and motor controller, test the balance performance, ensure that each individual participating in the reconstruction has basic mobility, and then enter a static balance state to wait for instructions; Verify that the V2V network communication between different self-balancing scooters is normal, and test the communication latency and packet loss. Upon receiving the docking instruction from the self-balancing scooter, the participating vehicles re-establish a communication connection and synchronize attitude information and vehicle motion constraint information.

3. The phased dynamic reconfiguration method for a two-wheeled mobility platform according to claim 2, characterized in that, The participating vehicles use a unified kinematic model for state synchronization, wherein the state-space equation of the kinematic model is: ； In the formula, This is the current system state vector. For control vectors, For heading angle, Let 'a' be the angular velocity of the heading, 'a' be the longitudinal acceleration, and 'x' and 'y' be the spatial coordinates. The system state at the next moment. is the linear velocity.

4. The phased dynamic reconfiguration method for a two-wheeled mobility platform according to claim 1, characterized in that, The reference trajectory is estimated through low-level attitude control, which employs a multi-level PID controller, including a longitudinal control loop and a heading control loop, wherein: The longitudinal control loop includes a pitch rate loop, a pitch angle loop, and a linear velocity loop; The heading control loop includes a yaw rate loop; The operating frequency of each control loop decreases progressively.

5. The phased dynamic reconfiguration method for a two-wheeled mobility platform according to claim 4, characterized in that, Generating the optimal docking trajectory includes: The trajectory control points are determined based on the initial state of the self-balancing vehicle, and the trajectory segments and straight line segments are determined based on the trajectory control points. Quadratic Bezier interpolation is used for the trajectory segment, and linear interpolation is used for the straight line segment. The trajectory equation is: ； In the formula, Let be the trajectory vector coordinate function, and t be the sampled normalized value at each stage. , , , , , All are vector coordinates of path control points. , , , These are the path length values ​​corresponding to each point on the trajectory, where s is the path length corresponding to the current sampling point.

6. The phased dynamic reconfiguration method for a two-wheeled mobility platform according to claim 5, characterized in that, During the approach phase, the cost function of the optimized control method considers the positional deviation, angular deviation, speed deviation, and heading angle deviation between the two vehicles in relation to the reference trajectory. The mathematical expression of the cost function is as follows: ； ； ； In the formula, , , , The tracking error weighting parameters are the horizontal and vertical coordinates, linear velocity, and yaw angle. and The cost function for the single-vehicle tracking error between the two vehicles; , , , These are the values ​​of the horizontal and vertical coordinates, linear velocity, and yaw angle for vehicle number i. , , , This represents the reference value of the corresponding variable on the trajectory; i represents the round number at the corresponding time; N represents the total number of calculation rounds; The final cost function; The yaw rate error weighting parameter is corrected by scaling. T is the algorithm's predictive control period, which is an adjustable hyperparameter; Let yaw angle be the angle of car number 1 at time i; Let yaw angle be the yaw angle of car number 2 at time i.

7. The phased dynamic reconfiguration method for a two-wheeled mobility platform according to claim 1, characterized in that, The exit condition for the approach phase is: ； The rollback condition for the approach phase is: ； In the formula, , , The x and y coordinates and yaw angle of vehicle i are respectively. The successful distance between the docking mechanisms of the two vehicles. The minimum attempted distance for the docking mechanism between the two vehicles. This represents the maximum tolerance for the vehicle body yaw angle.

8. The phased dynamic reconfiguration method for a two-wheeled mobility platform according to claim 1, characterized in that, During the contact phase, an optimization control method is used to optimize the cost function considering the position deviation, heading angle deviation, pitch angle deviation, and velocity deviation of the two vehicles. Furthermore, the weights of the balancing vehicle's attitude error are gradually increased, specifically: ； ； ； ； ； ； In the formula, the subscript , , , , These represent the error weight parameters corresponding to the horizontal axis, vertical axis, yaw angle, pitch angle, and linear velocity, respectively. , , , , The corrected error weight parameters are the x-axis, y-axis, yaw angle, pitch angle, and linear velocity corresponding to the scaled values; i represents the number of rounds at the corresponding time; N represents the total number of calculation rounds; COST is the final cost function; , , , , Let x, y, yaw, pitch, and linear velocity of car 1 at time i be represented respectively. , , , , Let x, y, yaw angle, pitch angle, and linear velocity of car 2 at time i be represented respectively.

9. The phased dynamic reconfiguration method for a two-wheeled mobility platform according to claim 8, characterized in that, The exit condition for the contact phase is: ； The rollback condition for the contact phase is: ； In the formula, , , , , For vehicle i, the values ​​of its x and y coordinates, linear velocity, yaw angle, and pitch angle are given. The successful distance between the docking mechanisms of the two vehicles. The minimum attempted distance for the docking mechanism between the two vehicles. For the maximum tolerance of vehicle speed, This represents the maximum tolerance for the vehicle's yaw angle. This represents the maximum tolerance for the vehicle's pitch angle.

10. The phased dynamic reconfiguration method for a two-wheeled mobility platform according to claim 1, characterized in that, The locking phase employs a parallel PID controller to directly control the torque of both wheels, controlling only the heading angle and wheel speed. Heading controller: ； ； Wheel speed controller: ； ； Final control quantity: ； ； In the formula, the superscript t represents the value at the current time, and t-1 represents the value at the previous time. This represents the yaw angle error at the current moment. This is the current reference yaw angle. The yaw angle observed by the current sensor. These are the proportional, integral, and derivative adjustment parameters for PID. To control the pendulum angle for the current target, This represents the current speed error. The current reference linear velocity, The current equivalent linear velocity of the left wheel speedometer. The equivalent linear velocity of the right wheel speed gauge. The current target control linear velocity, , This represents the current left and right target control values.

Images