Control method and system for movement of four-wheel chassis
By establishing a kinematic model and using PRM path planning and PID control algorithms, the problems of error calculation and environmental changes in the movement of four-wheel chassis are solved, dynamic adjustment and precise control are achieved, and the chassis adaptability in complex road conditions is improved.
Patent Information
- Application Number
- CN202411143070.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-20
- Publication Date
- 2024-11-26
AI Technical Summary
In the four-wheel chassis motion control, the sensor measurement data may contain errors, resulting in inaccurate calculations, which affects the adjustment of the control strategy. In addition, the dynamic changes in the environment require rapid adjustment of the control strategy to ensure control accuracy.
By establishing a kinematic model of the chassis and wheels, using the PRM path planning algorithm for path planning, and adjusting the speed of the chassis wheels in real time based on the PID control algorithm to achieve dynamic adjustment and deviation correction.
It improves the accuracy and flexibility of four-wheel chassis motion control, can quickly respond to environmental changes, ensure that the chassis moves along a predetermined path, and enhances adaptability in complex road conditions.
Smart Images

Figure CN119018183A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of motion control technology, and more particularly to a control method and system for four-wheel chassis motion. Background Art
[0002] With the development of the construction industry, the use of intelligent construction robots is becoming more and more extensive. The chassis of different robots have different operating performance on different terrains. The introduction of four-wheel independent drive (4WID) and independent steering (4WIS) technology has given the wheeled robot chassis unprecedented flexibility and maneuverability. This chassis design can achieve a variety of motion control, including but not limited to two-wheel steering, four-wheel steering, front / rear steering, turning in place, front / rear drive, four-wheel drive, part-time four-wheel drive, parking, wedge motion and lateral movement, etc., which greatly enhances its adaptability in narrow, complex and changeable road conditions. Therefore, this wheeled robot chassis is very suitable for various application scenarios such as production, inspection, security and rescue. However, this flexible and maneuverable chassis structure also increases the complexity of control accordingly. In the motion control of the four-wheel chassis, the data measured by the sensor may contain errors, which will directly lead to inaccurate deviation calculation, thereby affecting the adjustment of the control strategy. In addition, the environment in which the chassis is located during the movement often undergoes dynamic changes and adjustments. If the control strategy cannot be adjusted accordingly, the control accuracy of the chassis movement will be affected.
[0003] Therefore, it is an urgent problem for those skilled in the art to propose a control method and system for the motion of a four-wheel chassis capable of dynamic adjustment. Summary of the invention
[0004] In view of this, the present invention provides a control method and system for four-wheel chassis movement, which solves the dynamically changing environment during chassis movement by adjusting the deviation of the chassis.
[0005] In order to achieve the above object, the present invention adopts the following technical solution:
[0006] In one aspect, the present invention provides a control method for a four-wheel chassis movement, comprising:
[0007] Analyzing the motion state of the chassis wheels and the motion state of the chassis, and establishing a kinematic model of the chassis and the chassis wheels;
[0008] Acquire the target position and the initial position of the chassis, and use the PRM path planning algorithm to perform path planning on the chassis to obtain a path planning result;
[0009] Acquiring the current position information of the chassis in real time;
[0010] Based on the kinematic model, the path planning result, and the current position information, a PID control algorithm is used to adjust the speed of the chassis wheels.
[0011] Preferably, the kinematic model of the motion state of the chassis wheels and the chassis includes:
[0012] The relationship between the translation speed of the chassis and the speed of the chassis wheels is expressed as:
[0013]
[0014] Among them, v L (t) is the translation speed of the left wheel of the chassis, v R (t) is the translation speed of the right wheel of the chassis;
[0015] The relationship between the angular velocity of the chassis and the speed of the chassis wheels is expressed as:
[0016]
[0017] Where d is the distance between the left wheel and the right wheel, v L (t) is the speed of the left wheel of the chassis, v R (t) is the speed of the right wheel of the chassis;
[0018] The movement of the chassis is divided into position change and direction change, wherein the position change is expressed as:
[0019] x(t)=∫υ(t)cos(θ(t))dt;
[0020] y(t)=∫υ(t)sin(θ(t))dt;
[0021] Among them, x(t) is the displacement of the chassis on the X axis, and y(t) is the displacement of the chassis on the Y axis;
[0022] The change in direction is expressed as:
[0023] θ(t) = ∫ω(t)dt;
[0024] Where θ(t) is the chassis rotation angle.
[0025] Preferably, obtaining the target position and the initial position of the chassis, and using the PRM path planning algorithm to perform path planning for the chassis specifically includes:
[0026] Get the target position and the initial position of the chassis, the initial position is represented by P start =(x start ,y start ,θ start), the target position is represented by P goal =(x goal ,y goal ,θ goal ), where x start is the horizontal coordinate of the initial position, y start is the ordinate of the initial position, θ start is the current orientation angle of the chassis, x goal is the horizontal coordinate of the target position, y goal is the ordinate of the target position, θ goal is the chassis target orientation angle;
[0027] With the initial position P start is the starting point, the target position P goal Taking the starting point as the end point and avoiding obstacles as the constraint condition, the PRM path planning algorithm is used to find all paths connecting the starting point and the end point, and the optimal path is selected from all the paths;
[0028] The optimal path is smoothed using a Bezier curve to obtain a final path planning result.
[0029] Preferably, based on the kinematic model, the path planning result, and the current position, a PID control algorithm is used to adjust the speed of the chassis wheels, specifically including:
[0030] Compare the current position information obtained in real time with the path planning result to calculate the deviation value;
[0031] Determining whether the chassis deviates from the path planning result according to the deviation value;
[0032] If not, continue to monitor the position information of the chassis; if so, determine the cause of the deviation;
[0033] If the reason for the deviation is obstacle avoidance, the obstacle avoidance strategy is executed, and then the deviation correction adjustment is performed after the obstacle avoidance strategy is executed;
[0034] If the reason for the deviation is not for obstacle avoidance purposes, correction adjustments are made based on the deviation value.
[0035] Preferably, comparing the current position information obtained in real time with the path planning result to calculate the deviation value includes:
[0036] Determining the deviation direction: determining the deviation direction based on the relative position between the current position of the chassis and the path planning result;
[0037] Quantifying the deviation value: calculating the deviation distance and the deviation angle according to the current position of the chassis and the path planning result.
[0038] Preferably, calculating the deviation distance and the deviation angle according to the current position of the chassis and the path planning result specifically includes:
[0039] Define a global coordinate system, put the current position of the chassis and the position of the path planning result into the global coordinate system; the current position is recorded as P current =(x current ,y current );
[0040] Select the point on the path planning result closest to the current position as the reference point, and the reference point is marked as P path =(x path ,y path );
[0041] The Euclidean distance formula is used to calculate the straight-line distance between the current position and the reference point, i.e., the deviation distance d p :
[0042]
[0043] The angle between the vector pointing from the current position to the reference point and the tangent direction at the reference point, ie, the deviation angle, is calculated.
[0044] Preferably, the deviation correction adjustment is performed based on the deviation value, specifically including:
[0045] According to the deviation value, setting the chassis to return to the target position and direction of the path planning result;
[0046] Calculate the proportional term P, the integral term I and the differential term D according to the deviation value;
[0047] Calculating a control signal according to the proportional term P, the integral term I and the differential term D;
[0048] The speed adjustment and the steering angle adjustment are calculated based on the control signal.
[0049] In another aspect, the present invention provides a control system for four-wheel chassis motion, comprising:
[0050] A motion analysis module, used to analyze the motion state of the chassis wheels and the motion state of the chassis, and establish a kinematic model of the chassis and the chassis wheels;
[0051] A path planning module, used to obtain a target position and an initial position of the chassis, and use a PRM path planning algorithm to perform path planning on the chassis to obtain a path planning result;
[0052] A sensor module for acquiring current position information of the chassis in real time;
[0053] The motion control module is used to adjust the speed of the chassis wheels by using a PID control algorithm based on the kinematic model, the path planning result, and the current position information.
[0054] It can be seen from the above technical solutions that, compared with the prior art, the present invention discloses a control method and system for the movement of a four-wheel chassis, which integrates the establishment of motion relationships, path planning, and motion control, and solves the problems of difficult implementation of motion control, motion trajectory planning, chassis deviation adjustment, and dynamic changes; the present invention uses a PRM algorithm for path planning, and uses a selected path planning algorithm to search for an optimal path in the task space; the present invention uses a PID control algorithm to design control measurements, designs a feedback loop, monitors the position and speed of the chassis in real time, compares them with expected values, calculates deviations, and adjusts the control signal according to the deviations to drive the chassis to move to the target state, solving the problem of unreasonable path planning results caused by dynamic changes in the environment. BRIEF DESCRIPTION OF THE DRAWINGS
[0055] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.
[0056] Figure 1 A schematic diagram of the process provided by the present invention;
[0057] Figure 2 A schematic diagram of the structure provided by the present invention. DETAILED DESCRIPTION
[0058] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0059] The embodiment of the present invention discloses a control method for the movement of a four-wheel chassis, such as Figure 1 As shown, including:
[0060] Analyze the motion state of the chassis wheels and the motion state of the chassis, and establish the kinematic model of the chassis and the chassis wheels; Analyze the motion state of the chassis wheels and the motion state of the chassis, specifically analyze the speed and direction of the chassis wheels, and how they combine to affect the motion of the entire chassis. The specific steps are:
[0061] (1) Observe the wheels: Observe the rotation of the wheels, record the radius, rotation speed (expressed in revolutions per second or revolutions per minute), and rotation direction of the wheels, and determine whether the rotation direction of each wheel is consistent with the expected movement direction of the car.
[0062] (2) Determine the steering mechanism: In a four-wheel differential car chassis, the speeds of the front and rear wheels are synchronized. When the speeds of the wheels on the left and right sides are different, the car will move in a circle around a point called the instantaneous center of rotation (ICR). The angle of the steering wheel will affect the turning radius of the car, thereby affecting the magnitude of the angular velocity.
[0063] For a four-wheel differential chassis, a kinematic model needs to be established to describe the motion state of the chassis. This model usually involves the position of the chassis in a plane coordinate system and the motion parameters of each wheel. Specifically, it includes:
[0064] (1) Define the coordinate system: Select a fixed reference coordinate system, usually a two-dimensional rectangular coordinate system (x, y), to describe the position and movement of the chassis;
[0065] Describe the state of the chassis: Position: (x(t), y(t)) represents the plane coordinate position of the chassis at time t. Direction: θ(t) represents the orientation of the chassis at time t, that is, the angle between the chassis's forward direction and the north direction.
[0066] (2) Define the motion parameters of the wheel: use v L (t) and v R (t) represents the linear velocity of the left and right wheels respectively, and the kinematic relationship is established, including the translational velocity and angular velocity.
[0067] (3) Describe the movement of the chassis: Calculate the position and direction changes of the chassis based on the translational velocity and angular velocity.
[0068] (4) Design control strategy: Based on the kinematic model and real-time feedback of position and speed information, design a control algorithm to adjust the speed of each wheel to achieve the desired motion trajectory.
[0069] Obtain the target position and the initial position of the chassis, use the PRM path planning algorithm to plan the path of the chassis, and obtain the path planning result; select a path in the task space or configuration space so that the chassis can move from the initial position to the target position.
[0070] Acquire the current position information of the chassis in real time, collect the motion data of the chassis through sensors, and perform necessary processing such as filtering and denoising to ensure the accuracy and reliability of the data.
[0071] Based on the kinematic model, path planning results, and current position information, the PID control algorithm is used to adjust the speed of the chassis wheels. During the movement of the chassis, the robot's motion data is collected through sensors and compared with the expected goals of path planning. The PID control algorithm performs negative feedback control based on the deviation and adjusts the control command in real time to ensure that the chassis can accurately move along the predetermined path to achieve closed-loop control.
[0072] Furthermore, the kinematic model of the motion state of the chassis wheels and the chassis includes:
[0073] The relationship between the translation speed of the chassis and the speed of the chassis wheels is expressed as:
[0074]
[0075] Among them, v L (t) is the translation speed of the left wheel of the chassis, v R (t) is the translation speed of the right wheel of the chassis;
[0076] The relationship between the angular velocity of the chassis and the speed of the chassis wheels is expressed as:
[0077]
[0078] Where d is the distance between the left wheel and the right wheel, v L (t) is the speed of the left wheel of the chassis, v R (t) is the speed of the right wheel of the chassis;
[0079] The movement of the chassis is divided into position change and direction change, where the position change is expressed as:
[0080] x(t)=∫υ(t)cos(θ(t))dt;
[0081] y(t)=∫υ(t)sin(θ(t))dt;
[0082] Among them, x(t) is the displacement of the chassis on the X axis, and y(t) is the displacement of the chassis on the Y axis;
[0083] The change in direction is expressed as:
[0084] θ(t) = ∫ω(t)dt;
[0085] Where θ(t) is the chassis rotation angle.
[0086] In another embodiment, the target position and the initial position of the chassis are obtained, and the path planning of the chassis is performed using the PRM path planning algorithm, specifically including:
[0087] Get the target position and the initial position of the chassis, denoted as P start =(x start ,y start ,θ start ), the target position is denoted as P goal =(x goal ,y goal ,θ goal ), where x start is the horizontal coordinate of the initial position, y start is the ordinate of the initial position, θ start is the current orientation angle of the chassis, x goal The horizontal coordinate of the target position, y goal is the ordinate of the target position, θ goal is the chassis target orientation angle;
[0088] With the initial position P start is the starting point, the target position P goal As the end point, avoid obstacles as constraints, use the PRM path planning algorithm to find all paths connecting the start point and the end point, and select the optimal path from all paths. The specific steps are:
[0089] (1) Preliminary preparation and node sampling:
[0090] Map building and preprocessing: First, there needs to be a map representing the robot's workspace, which is usually a grid map in which the map area is divided into uniform grid cells and specific attribute information (such as obstacles, free space, etc.) is assigned to each grid cell.
[0091] Sampling: Randomly sample a set of nodes in the free space of the map, which represent possible locations of the robot. The number of sampling points can be set as needed. More sampling points may increase the possibility of finding a feasible path, but it will also increase the amount of calculation.
[0092] (2) Build a Probabilistic Roadmap (PRM):
[0093] Collision detection: For each sampled node, perform collision detection to ensure that the node is not located within an obstacle. This step is to ensure that the path does not pass through obstacles.
[0094] Connect nodes: For pairs of nodes that do not collide, try to connect them directly via a local path planning algorithm (such as straight-line motion). If the connection does not intersect with obstacles, add an edge between the two nodes.
[0095] Build a graph: Through the above steps, a graph is constructed, where nodes represent possible positions of the robot and edges represent paths that the robot can safely move between two positions. This graph is called a probabilistic roadmap (PRM).
[0096] (3) Path search and optimal path selection:
[0097] Path search: On the constructed PRM, use traditional path search algorithms (such as Dijkstra algorithm or A* algorithm) to search for all feasible paths from the start node to the end node. There may be multiple paths, each of which consists of a series of nodes and edges.
[0098] Optimal path selection: For all the searched paths, we evaluate them according to certain evaluation criteria (such as path length, time cost, safety, etc.) and select the optimal path. The selection of the optimal path can be determined according to specific needs and evaluation criteria.
[0099] The optimal path is smoothed using Bezier curves to ensure the continuity and traceability of the final planned path. The specific steps for smoothing the optimal path using Bezier curves are as follows:
[0100] (1) Selecting appropriate control points: The selection of control points is crucial for creating smooth Bezier curves. The control points define the shape of the curve, so in order to obtain a smooth curve, these points need to be carefully selected. Generally, when you want the curve to transition smoothly between two segments, the last control point of the two segments and the first control point of the next segment should be as close as possible.
[0101] (2) Adjust the position and weight of the control points: By adjusting the position and weight of the control points, the Bezier curve can better fit the desired path. The weight determines the degree of influence of the control point on the shape of the curve. Appropriate weight adjustment can make the curve smoother.
[0102] (3) Ensure continuity: In a piecewise Bezier curve, it is necessary to ensure that the parameters (usually t) between each segment are continuous. This means that between two adjacent Bezier curve segments, their tangents at the connection point should be equal to ensure a visually smooth transition.
[0103] (4) Tangent matching: In addition to positional continuity, the first-order derivative (i.e., tangent) is also required to be continuous. This can be achieved by adjusting the control points to ensure that the curve is not only positionally continuous but also tangent continuous at the connection point.
[0104] (5) Use Hermite interpolation or de Casteljau algorithm: For third-order or higher-order Bezier curves, more complex mathematical methods may be needed to ensure smooth transitions. Hermite interpolation and de Casteljau algorithm for Bezier curves are two commonly used methods that can help adjust control points to meet continuity and tangent matching requirements.
[0105] (6) Generate a smooth path: After adjusting through the above steps, use the Bezier curve to generate a smooth path. This can eliminate sharp corners and mutations in the path, making the path smoother and easier to understand.
[0106] In another embodiment, based on the kinematic model, the path planning result, and the current position, a PID control algorithm is used to adjust the speed of the chassis wheels, specifically including:
[0107] Compare the current position information obtained in real time with the path planning result and calculate the deviation value;
[0108] Determine whether the chassis deviates from the path planning result based on the deviation value;
[0109] If not, continue to monitor the position information of the chassis; if so, determine the cause of the deviation;
[0110] If the reason for the deviation is obstacle avoidance, the obstacle avoidance strategy is executed, and then the deviation correction adjustment is performed after the obstacle avoidance strategy is executed;
[0111] If the reason for the deviation is not for obstacle avoidance purposes, correction adjustments are made based on the deviation value.
[0112] Furthermore, the current position information obtained in real time is compared with the path planning result to calculate the deviation value, including:
[0113] Determine the deviation direction: Determine the deviation direction based on the relative position of the current position of the chassis and the path planning result; the deviation direction includes deviation to the left, deviation to the right, or in front of or behind the predetermined path.
[0114] Quantify the deviation value: Calculate the deviation distance and deviation angle based on the current position of the chassis and the path planning results.
[0115] Preferably, the deviation distance and the deviation angle are calculated according to the current position of the chassis and the path planning result, specifically including:
[0116] Define a global coordinate system and put the current position of the chassis and the position of the path planning result into the global coordinate system; the current position is recorded as P current =(x current ,y current );
[0117] Select the point closest to the current position on the path planning result as the reference point, and mark the reference point as P path =(x path ,y path );
[0118] Use the Euclidean distance formula to calculate the straight-line distance between the current position and the reference point, that is, the deviation distance d p :
[0119]
[0120] Calculate the angle between the vector pointing from the current position to the reference point and the tangent direction at the reference point, that is, the deviation angle θ, which can be obtained through vector operation: Determine the vector from the current position of the robot to the reference point of the predetermined path e(t) = V cp =(x path -x current ,y path -y current ), determine the tangent direction vector V of the predetermined path at the reference point tangent This is usually obtained from a path planning module, or by numerically differentiating the points on the path. The angle φ between two vectors is calculated using the dot product of the vectors:
[0121]
[0122] The deviation angle θ is 90°-φ or φ-90°, depending on the tangent direction vector and the angular relationship between the two vectors.
[0123] Preferably, the deviation correction adjustment is performed based on the deviation value, specifically including:
[0124] According to the deviation value, the chassis is set to return to the target position and direction of the path planning result;
[0125] The proportional term P, the integral term I and the differential term D are calculated based on the deviation value; where:
[0126] Proportional control (P): adjusts the output according to the size of the deviation. If the chassis deviates far from the predetermined path, the controller will increase the adjustment amount to return the chassis to the path faster.
[0127] P=K p *e(t), where K p is the proportional gain, and e(t) is the deviation value.
[0128] Integral control (I): Integral control adjusts the output by integrating the deviation. For example, if the chassis deviates from the path for a long time, the integral term will accumulate the deviation and gradually increase the control output until the deviation is eliminated.
[0129] I=∫0Ki ·e(t)dt;
[0130] Where K i is the integral gain and t is the integral variable.
[0131] Derivative control (D): Derivative control predicts future deviation changes and adjusts the output based on the rate of change of the deviation. If the chassis is rapidly deviating from the predetermined path, the differential control will make adjustments in advance to prevent the chassis from deviating too much.
[0132]
[0133] Among them, K d is the integral gain.
[0134] The control signal calculated according to the proportional term P, the integral term I and the differential term D is expressed as u(t)=P+I+D.
[0135] The speed adjustment and steering angle adjustment are calculated based on the control signal. If the output is a positive number, it means the chassis needs to accelerate; if the output is a negative number, it means the chassis needs to decelerate. If the output is a positive number, it means the chassis needs to turn left; if the output is a negative number, it means the chassis needs to turn right.
[0136] In another embodiment, implementing the control strategy includes: writing a program to implement the PID control algorithm and converting the control strategy into an actual control signal. The following steps are included in writing the program:
[0137] Create a PID controller instance;
[0138] Create a four-wheel differential chassis instance;
[0139] Set the target speed and current speed;
[0140] Calculation error, calculation speed error;
[0141] Calculate control signals;
[0142] The chassis speed is adjusted according to the control signal.
[0143] Furthermore, after implementing the control strategy, the motion state of the chassis is continuously monitored to check whether the planned path is followed successfully or the path is corrected.
[0144] Furthermore, if the expected adjustment effect is not achieved, the position and direction of the chassis are continuously monitored through the feedback loop, and the control strategy is adjusted again according to the new deviation value.
[0145] In another aspect, the present invention provides a control system for the motion of a four-wheel chassis, such as Figure 2 As shown, including:
[0146] The motion analysis module is used to analyze the motion state of the chassis wheels and the motion state of the chassis, and establish a kinematic model of the chassis and the chassis wheels;
[0147] The path planning module is used to obtain the target position and the initial position of the chassis, and use the PRM path planning algorithm to plan the path of the chassis to obtain the path planning result;
[0148] Sensor module, to obtain the current position information of the chassis in real time;
[0149] The motion control module is used to adjust the speed of the chassis wheels using a PID control algorithm based on the kinematic model, path planning results, and current position information.
[0150] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.
[0151] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.
Claims
1. A control method for the movement of a four-wheel chassis, characterized in that: include: Analyzing the motion state of the chassis wheels and the motion state of the chassis, and establishing a kinematic model of the chassis and the chassis wheels; Acquire the target position and the initial position of the chassis, and use the PRM path planning algorithm to perform path planning on the chassis to obtain a path planning result; Acquiring the current position information of the chassis in real time; Based on the kinematic model, the path planning result, and the current position information, a PID control algorithm is used to adjust the speed of the chassis wheels.
2. A control method for the movement of a four-wheel chassis according to claim 1, characterized in that: The kinematic model of the motion state of the chassis wheels and the chassis includes: The relationship between the translation speed of the chassis and the speed of the chassis wheels is expressed as: Among them, v L (t) is the speed of the left wheel of the chassis, v R (t) is the speed of the right wheel of the chassis; The relationship between the angular velocity of the chassis and the speed of the chassis wheels is expressed as: Where d is the distance between the left wheel and the right wheel, v L (t) is the speed of the left wheel of the chassis, v R (t) is the speed of the right wheel of the chassis; The movement of the chassis is divided into position change and direction change, wherein the position change is expressed as: x(t) = ∫v(t)cos(θ(t))dt; y(t)=∫v(t)sin(θ(t))dt; Among them, x(t) is the displacement of the chassis on the X axis, and y(t) is the displacement of the chassis on the Y axis; The change in direction is expressed as: θ(t) = ∫ω(t)dt; Where θ(t) is the chassis rotation angle.
3. A control method for the movement of a four-wheel chassis according to claim 1, characterized in that: Obtaining the target position and the initial position of the chassis, and using the PRM path planning algorithm to perform path planning for the chassis, specifically including: Get the target position and the initial position of the chassis, the initial position is represented by P start =(x start ,y start ,θ start ), the target position is represented by P goal =(x goal ,y goal ,θ goal ), where x start is the horizontal coordinate of the initial position, y start is the ordinate of the initial position, θ start is the current orientation angle of the chassis, x goal is the horizontal coordinate of the target position, y goal is the ordinate of the target position, θ goal is the chassis target orientation angle; With the initial position P start is the starting point, the target position P goal Taking the starting point as the end point and avoiding obstacles as the constraint condition, the PRM path planning algorithm is used to find all paths connecting the starting point and the end point, and the optimal path is selected from all the paths; The optimal path is smoothed using the Bezier curve to obtain the final path planning result.
4. A control method for the movement of a four-wheel chassis according to claim 1, characterized in that: Based on the kinematic model, the path planning result, and the current position, a PID control algorithm is used to adjust the speed of the chassis wheels, specifically including: Compare the current position information obtained in real time with the path planning result to calculate the deviation value; Determining whether the chassis deviates from the path planning result according to the deviation value; If not, continue to monitor the position information of the chassis; if so, determine the cause of the deviation; If the reason for the deviation is obstacle avoidance, the obstacle avoidance strategy is executed, and then the deviation correction adjustment is performed after the obstacle avoidance strategy is executed; If the reason for the deviation is not for obstacle avoidance purposes, correction adjustments are made based on the deviation value.
5. A control method for the movement of a four-wheel chassis according to claim 4, characterized in that: Comparing the current position information obtained in real time with the path planning result and calculating the deviation value includes: Determining the deviation direction: determining the deviation direction based on the relative position between the current position of the chassis and the path planning result; Quantifying the deviation value: calculating the deviation distance and the deviation angle according to the current position of the chassis and the path planning result.
6. A control method for the movement of a four-wheel chassis according to claim 5, characterized in that: Calculating the deviation distance and the deviation angle according to the current position of the chassis and the path planning result specifically includes: Define a global coordinate system, put the current position of the chassis and the position of the path planning result into the global coordinate system; the current position is recorded as P current =(x current ,y current ); Select the point on the path planning result closest to the current position as the reference point, and the reference point is marked as P path =(x path ,y path ); The Euclidean distance formula is used to calculate the straight-line distance between the current position and the reference point, i.e., the deviation distance d p : The angle between the vector pointing from the current position to the reference point and the tangent direction at the reference point, ie, the deviation angle, is calculated.
7. A control method for the movement of a four-wheel chassis according to claim 5, characterized in that: Correction adjustments are made based on the deviation value, including: According to the deviation value, setting the chassis to return to the target position and direction of the path planning result; Calculate the proportional term P, the integral term I and the differential term D according to the deviation value; Calculating a control signal according to the proportional term P, the integral term I and the differential term D; The speed adjustment and the steering angle adjustment are calculated based on the control signal.
8. A control system for four-wheel chassis motion, characterized in that: include: A motion analysis module, used to analyze the motion state of the chassis wheels and the motion state of the chassis, and establish a kinematic model of the chassis and the chassis wheels; A path planning module, used to obtain a target position and an initial position of the chassis, and use a PRM path planning algorithm to perform path planning on the chassis to obtain a path planning result; A sensor module for acquiring current position information of the chassis in real time; The motion control module is used to adjust the speed of the chassis wheels by using a PID control algorithm based on the kinematic model, the path planning result, and the current position information.
Citation Information
Cited By
Chassis control system for low-speed unmanned vehicle robot
CN120573120A
Control method of mobile robot and mobile robot
CN122308376A
