Mobile robot path tracking method based on online slip identification and successive linearization model predictive control

Abstract

The application discloses a mobile robot path tracking method based on online slip identification and successive linearization model predictive control, and particularly relates to the field of mobile robot motion control and path tracking, and comprises the following steps: acquiring a robot pose and a wheel speed; an edge computing node combines a linearized motion relationship of a last period slip parameter, recursively predicts a pose along a predetermined trajectory, and solves a basic control sequence according to a position difference and a heading difference; a predicted pose is recalculated by adding a unit control amount for each control shot, a terminal change matrix is formed according to a terminal pose difference, a slip change matrix is formed according to a unit change of the slip parameter, and a cosine-sine decomposition algorithm is performed on the two matrices; an identification direction is separated from the basic control sequence, the identification direction is zero change of a terminal two-dimensional position and heading and retains a slip response, an identification increment is solved in an identification subspace, the slip parameter is updated in combination with an actual pose response, the motion relationship is linearized again by using the updated slip parameter, and a path tracking control amount is solved.

Description

Technical Field

[0001] This invention relates to the field of motion control and path tracking technology for mobile robots, and more specifically, to a path tracking method for mobile robots based on online slip identification and successive linearization model predictive control. Background Technology

[0002] Current mobile robot path tracking typically estimates motion state based on wheel speed, inertial measurement data, and pose measurement results, and uses online slip identification results to correct kinematic model. Then, the successive linearization model predicts and controls the steering and driving amounts to control two-dimensional position and heading deviations. In scenarios where warehouse mobile robots turn along narrow aisles of shelves, the robot's operating area is limited by the aisle width and shelf boundaries. During path tracking, additional wheel-to-ground responses cannot be obtained through additional turning, speed swinging, or probing actions that deviate from the predetermined trajectory. As a result, the controller will continuously adjust the steering amount based on the position deviation and heading deviation, so that the motion difference corresponding to different slip states is compressed by the control action. In the wheel speed, angular velocity and pose records collected by the edge computing node, although the position deviation and heading deviation are kept within the allowable range, the response difference corresponding to different slip parameters is weakened at the same time, making it difficult for the existing online slip identification to obtain information sufficient to distinguish slip states from the actual motion data in the path tracking process. The technical problem to be solved by this application is: how to use the existing control and motion response information in the predictive control process of successive linearization model to form an identification basis that can distinguish wheel slip state without adding additional excitation actions that deviate from the predetermined trajectory, and to complete online slip identification accordingly. Summary of the Invention

[0003] To overcome the aforementioned deficiencies of the prior art, embodiments of the present invention provide a mobile robot path tracking method based on online slip identification and successive linearized model predictive control. By separating the terminal's two-dimensional position and heading with zero change from the basic control sequence while retaining the identification direction of the slip response, the identification increment is obtained in the identification subspace and the slip parameters are updated in combination with the actual pose response. Then, the motion relationship is re-linearized with the updated slip parameters and the path tracking control quantity is solved to solve the problems mentioned in the background art.

[0004] To achieve the above objectives, the present invention provides the following technical solution: a mobile robot path tracking method based on online slip identification and successive linearized model predictive control, comprising: S1. Obtain the robot's pose and wheel speed. The edge computing nodes linearize the motion relationship by combining the sliding parameters of the previous cycle. The pose is recursively predicted along the predetermined trajectory, and the basic control sequence is solved according to the position difference and heading difference. S2. Add a unit control quantity to each control step to recalculate the predicted pose, form a terminal change matrix with the terminal pose difference, form a slip change matrix with the slip parameter unit change, perform cosine-sine decomposition algorithm on the two matrices, and take the common direction where the terminal cosine is zero and the slip sine is non-zero to form the identification subspace. S3. Based on the identification subspace, the Riemann trust region algorithm is executed. The target value is the square and the opposite of the direction of the sliding transformation matrix multiplication. The direction of the conjugate gradient is updated by truncation. If the target value decreases, it is retained; otherwise, the trust region radius is halved. When adjacent quantization directions are consistent, the identification increment sequence is formed by scaling according to the actuator control code boundary. S4. The identification increment sequence is superimposed on the basic control sequence to form the execution control sequence. The process control output is sent to the actuator. The pose is predicted recursively according to the execution control sequence. The actual pose is subtracted from the predicted pose to form the response difference sequence. The parameter change matrix is ​​formed according to the unit change of the slip parameter. The slip correction amount is obtained through QR decomposition and back substitution to form the current slip parameter. S5. Linearize the motion relationship based on the current slip parameters, recursively predict the pose along the trajectory and solve the control quantity, output it to the actuator to perform two-dimensional position or heading control, and transfer the current slip parameters to the next control cycle.

[0005] In a preferred embodiment, S1 includes: S11. Calculate the projection coefficient by connecting each adjacent trajectory point of the predetermined trajectory with the robot's current position. When the projection coefficient is less than zero, take the starting point of the connection; when it exceeds one, take the ending point of the connection; otherwise, the projection point is formed by adding the projection coefficient to the coordinates of the starting point and multiplying the difference between the coordinates of the two ends. Calculate the square of the distance from the current position to each projection point and take the first projection point corresponding to the first item in ascending order as the current trajectory position. The trajectory direction is formed by dividing the coordinate difference of the corresponding connection by the square root of the sum of the squares of the coordinate differences. The predetermined trajectory refers to a set of trajectory points recorded in the robot's working plane in the order of travel. Each adjacent trajectory point is connected by a straight line segment, and the length of the line segments is accumulated in the order of connection to form the trajectory arc length. The unit vector of the line segment coordinate difference is used as the trajectory direction of the corresponding trajectory segment. S12. Multiply the wheel speeds of the left and right drive wheels by the wheel radius, then multiply by one and subtract the slip component of the corresponding drive wheel in the slip parameter of the previous cycle to form the effective wheel speeds of the left and right. Divide the sum of the two by two to get the forward speed. Subtract the effective wheel speed of the left from the effective wheel speed of the right and divide by the wheel track to get the heading speed. The drive wheel speed refers to the angular velocity obtained by dividing the wheel rotation angle change recorded by the left and right drive wheel encoders in the same control frame by the control frame duration. The left drive wheel corresponds to the left wheel speed, and the right drive wheel corresponds to the right wheel speed. Slip parameters refer to the dimensionless parameter set that was identified based on the robot’s actual motion response in the previous control cycle and corresponds to the left and right drive wheels respectively. It is used to convert the circumferential speed of the drive wheel into the effective wheel speed after the wheel-ground interaction. Each parameter in the parameter set is converted by multiplying the circumferential speed of the drive wheel by one and subtracting the corresponding parameter. The slip component refers to the parameter value in the slip parameters that corresponds one-to-one with a single drive wheel. The left slip component acts on the circumferential speed of the left drive wheel, and the right slip component acts on the circumferential speed of the right drive wheel, and both participate in the calculation of the effective wheel speeds of the left and right wheels respectively.

[0006] In a preferred embodiment, S1 further includes: S13. Starting from the current pose, predict the pose recursively according to the control beats. For each beat, read the current control code of the corresponding left and right wheels and add the control increment of the left and right wheels in this beat to form a candidate control code. According to the wheel speed of the drive wheel corresponding to the candidate control code, form the predicted forward speed and the predicted heading speed according to S12. Using the predicted heading of the previous beat as the expansion point, substitute the heading offset into the first-order expansion term of the cosine and the first-order expansion term of the sine, and multiply the predicted forward speed and the predicted heading speed by the control beat duration to form a beat-by-beat linear recursive relationship between the predicted position and the predicted heading. At the same time, determine the reference position and reference heading of the same beat from the current trajectory position along the predetermined trajectory according to the predicted travel distance. Pose refers to the motion state formed by the horizontal and vertical coordinates of the robot body reference point in the working plane and the heading angle of the robot body's longitudinal axis relative to the working plane reference axis at the same sampling time. A control frame refers to a single calculation and execution period that an edge computing node divides according to the control cycle. The time length between adjacent control frames is the control frame duration. Each control frame corresponds to one prediction pose recursion and one control quantity calculation. Predicted heading refers to the predicted orientation angle of the robot's longitudinal axis obtained by accumulating the heading increment according to the heading speed of each control frame, starting from the current heading. The heading increment refers to the change in angle obtained by multiplying the heading speed of the current control frame by the control frame duration. It is used to predict the heading of the next frame by recursively predicting the heading of the previous frame. The predicted travel distance refers to the distance traveled along the trajectory, which is the distance obtained by multiplying the forward velocity of each control frame by the duration of the control frame, starting from the current trajectory position, and accumulated according to the order of the control frames. The process of determining the reference position and reference heading is as follows: from the current trajectory position, accumulate the length of the line segment along the predetermined trajectory in the order of connecting the trajectory points. Subtract the length of the fully traversed trajectory segment from the predicted travel distance in turn. When the remaining distance falls into the current trajectory segment, move the remaining distance along the trajectory direction from the starting point of the current trajectory segment to form the reference position. Use the trajectory direction angle of the current trajectory segment as the reference heading. S14. The single-cycle cost is formed by adding the square of the projection difference of the predicted position along the normal direction of the reference heading to the square of the difference obtained by subtracting the reference heading from the predicted heading, and then adding the square of the left and right wheel control increments. After accumulating the control increments by control cycle, a quadratic programming equation is established for the linear recursive relationship of each cycle. The left and right wheel control increments are obtained by decomposing the block LDL transpose. When the sum of the current executed control code and the control increment exceeds the actuator control code interval, the corresponding control increment is fixed as the value obtained by subtracting the current executed control code from the corresponding endpoint of the actuator control code interval, and the remaining control increments are decomposed again until all control quantities fall into the actuator control code interval. The current executed control code is added to the corresponding left and right wheel control increments, and written into a basic control sequence according to the control cycle. The process of obtaining the control increments of the left and right wheels through block LDL transpose decomposition is as follows: the control increments of the left and right wheels are alternately arranged according to the control beat order to form the vector to be determined. The single beat substitution value is expanded according to the linear recursive relationship of beat by beat and accumulated to form a quadratic term matrix and a linear term vector. The quadratic term matrix is ​​decomposed into the product of the lower triangular block matrix, the block diagonal matrix and the transpose of the lower triangular block matrix. First, the lower triangular block equation is solved by substituting forward, then the intermediate vector is solved block by block according to the block diagonal matrix, and finally the transpose equation is solved by substituting backward. The resulting vector is restored to the control increments of the left and right wheels of each control beat according to the original arrangement order. When the control increment falls outside the actuator control code interval, it is fixed to the endpoint of the corresponding interval. The corresponding component is deleted from the vector to be determined and the remaining components are decomposed repeatedly. The actuator control code range refers to the closed range of control codes that the driver control register can receive and output to the drive wheel execution circuit according to the mapping relationship between the encoding bit width and the driving direction. The two endpoints of the range correspond to the lower control code and the upper control code that the control register is allowed to write, respectively.

[0007] In a preferred embodiment, S2 includes: S21. For each control beat in the basic control sequence, read the left wheel basic control code and the right wheel basic control code of the current beat as the current control code. Rewrite the current control code as the current value plus one and the current value minus one respectively, while keeping the other basic control codes unchanged. Proceed to the prediction terminal along the linear recursive relationship of each beat. Divide the difference of the terminal horizontal coordinate, the difference of the vertical coordinate, and the difference of the heading obtained from the two recursions by two respectively, and write them into a column of the terminal change matrix according to the arrangement order of the left and right wheel basic control codes in the basic control sequence. The prediction terminal refers to the final prediction pose obtained by taking the current pose as the first prediction pose and executing all control shots in sequence according to the linear recursive relationship. The prediction terminal consists of the final prediction x-coordinate, the final prediction y-coordinate, and the final prediction heading. S22. For the current control code in S21, select a slip component in sequence, rewrite the current control code as the current value plus one and the current value minus one respectively, and rewrite the storage code corresponding to the slip component as the current value plus one and the current value minus one respectively. Under the four combinations of current control code and slip component storage code, recalculate the frame-by-frame predicted pose. First, divide the frame-by-frame pose difference between the two current control codes when the slip component storage code is plus one by two, then divide the frame-by-frame pose difference between the two current control codes when the slip component storage code is minus one by two, subtract the two results and divide by two again, and continue the slip component results in the order of horizontal coordinate, vertical coordinate and heading within the control frame to form a slip change matrix with the same column order as the terminal change matrix. The four combinations of current control code and sliding component storage code refer to the following four value combinations formed for a current control code and a sliding component, while keeping the other basic control codes and other sliding components unchanged: current control code plus one and sliding component storage code plus one, current control code minus one and sliding component storage code plus one, current control code plus one and sliding component storage code minus one, and current control code minus one and sliding component storage code minus one. The process of recalculating the step-by-step predicted pose is as follows: take the current pose as the first predicted pose, substitute the four sets of value combinations into the step-by-step linear recursion relationship formed by S1, and recursively predict the next predicted pose according to the control shot order, until the last predicted pose is formed, so that the four sets of value combinations each obtain a step-by-step predicted pose.

[0008] In a preferred embodiment, S2 further includes: S23. Multiply the terminal transformation matrix by the transpose of the terminal transformation matrix, and then multiply the transpose of the sliding transformation matrix by the sliding transformation matrix to form a joint matrix. Perform eigenvalue decomposition on the joint matrix, delete the eigenvectors with zero eigenvalues, and multiply the inverse of the square root of each retained eigenvalue by the corresponding eigenvector to form a normalized matrix. Then, multiply the terminal transformation matrix and the sliding transformation matrix by the normalized matrix on the right to form the terminal normalized matrix and the sliding normalized matrix, respectively. Perform cosine and sine decomposition algorithms on the two matrices. Transpose refers to moving the data in the i-th row and j-th column of a matrix to the j-th row and i-th column, and performing the same swap on all rows and columns of the matrix, so that the column vectors of the original matrix are transformed into the corresponding row vectors of the transposed matrix. The process of performing eigenvalue decomposition is as follows: First, Householder reflection is performed column by column on the joint matrix to form a real symmetric tridiagonal matrix by retaining only the data of the adjacent sub-diagonal outside the main diagonal. Then, implicit displacement QR iteration is performed on the real symmetric tridiagonal matrix. In each round, the orthogonal matrix obtained by QR decomposition is multiplied into the orthogonal matrix of the previous round, and the tridiagonal matrix of the next round is formed by RQ product, until the data of the non-diagonal position is written as zero after the calculation field is quantized. The data of the diagonal position is used as the eigenvalue, and the corresponding column of the multiplied orthogonal matrix is ​​used as the eigenvector. An eigenvector is a non-zero column vector that, when multiplied by a joint matrix, has the same direction as the original column vector and differs from it only by a multiple of its corresponding eigenvalue. Each eigenvector corresponds to an eigenvalue, and the combination coefficients of each control step in the corresponding direction are recorded according to the control step column order of the joint matrix. The process of executing the cosine-sine decomposition algorithm is as follows: multiply the terminal normalized matrix by the transpose of the terminal normalized matrix to form the terminal Gram matrix; perform eigenvalue decomposition on the terminal Gram matrix and arrange the eigenvectors in ascending order of eigenvalues; form a common right decomposition matrix by column using the arranged eigenvectors; take the square root of each eigenvalue to form the terminal cosine component; then take the square root of the corresponding eigenvalue minus one to form the sliding sine component; multiply the terminal normalized matrix by the corresponding column of the common right decomposition matrix to obtain the terminal direction response; multiply the sliding normalized matrix by the same column to obtain the sliding direction response; thus, each common right decomposition direction corresponds to both a terminal cosine component and a sliding sine component. S24. Read the right decomposition vectors in the cosine-sine decomposition results where the terminal cosine component is zero and the glide sine component is not zero. Multiply each right decomposition vector by the normalization matrix to obtain the control direction. Form a direction matrix by columns of each control direction. Perform column pivoting QR decomposition on the direction matrix. Delete the candidate directions corresponding to the diagonal elements of the R matrix being zero. Restore the original column order of the remaining candidate directions in the direction matrix according to the column permutation matrix to form the identification subspace. The terminal cosine component refers to the terminal response coefficient corresponding to a common right decomposition direction in the cosine-sine decomposition. The value is obtained by taking the square root of the corresponding eigenvalue of the terminal Gram matrix. When the terminal cosine component is zero, the terminal abscissa change, terminal ordinate change and terminal heading change generated by the terminal change matrix along the corresponding common right decomposition direction are all zero. The slip sine component refers to the slip response coefficient corresponding to the same common right decomposition direction in the cosine-sine decomposition. The value is obtained by subtracting the square root of the eigenvalue of the corresponding terminal Gram matrix. Under the joint normalization relation, the sum of the square of the terminal cosine component and the square of the slip sine component is one. Therefore, when the terminal cosine component is zero, the corresponding slip sine component is one. The right decomposition vector is a column vector in the common right decomposition matrix. Each element of the vector gives the combination coefficient of the corresponding control component according to the arrangement order of the left and right wheel basic control codes in the basic control sequence. The same right decomposition vector acts on the terminal normalization matrix and the slip normalization matrix at the same time, and is used to represent the correspondence of the same control direction in the terminal pose response and slip response. The process of performing column pivot QR decomposition is as follows: calculate the sum of squares of the elements in each column of the direction matrix and read the first column in descending order of the sum of squares. Move the first column to the current decomposition position and use Householder reflection to change the data below the diagonal position of the current column to zero. Then use the same reflection to update the unprocessed columns on the right. Subsequently, recalculate the sum of squares of the elements in the unprocessed columns and repeat the column swapping and Householder reflection until all columns are processed, forming a column permutation matrix, an orthogonal matrix, and an upper triangular R matrix. Then, restore the control sequence of each control direction according to the column permutation matrix. The diagonal elements of the R matrix refer to the data in the upper triangular R matrix formed by the column pivoting QR decomposition where the row number and column number are the same. Each diagonal element records the orthogonal component remaining after deducting the projection of the retained direction for the corresponding control direction. A diagonal element being zero indicates that the corresponding control direction can be obtained by linearly combining the control directions preceding it.

[0009] In a preferred embodiment, S3 includes: S31. Establish directional coordinates based on the column number of the identification subspace, read the first unit coordinate vector as the initial directional coordinates, multiply the identification subspace by the current directional coordinates to form the current control direction, multiply the vector obtained by the sliding transformation matrix by the squares of the elements and the inverse of the vector to form the current target value, subtract the product of the current directional coordinates and the derivative of the current target value with respect to the current directional coordinates to form the Riemann gradient, and use the second norm of the Riemann gradient as the radius of the first trust region. The direction coordinates refer to the column vectors that record the corresponding basic direction combination coefficients according to the arrangement order of each column in the identification subspace. Multiplying the identification subspace by the direction coordinates yields the control direction that is consistent with the arrangement order of the left and right wheel basic control codes in the basic control sequence. S32. Perform truncated conjugate gradient calculation in the orthogonal tangent space of the current direction coordinates. In the first round, the search direction is formed by the inverse of the Riemann gradient and the search quantity starts from zero. The direction curvature is formed by multiplying the second derivative of the target value by the left of the search direction and then multiplying it by the search direction. When the direction curvature is positive, the search quantity and residual are updated by dividing the sum of squared residuals by the direction curvature. When the direction curvature is not positive or the updated search quantity exceeds the trust region, the intersection point falling on the trust region boundary along the search direction is taken as the search quantity. The calculation ends when all components of the residual are zero after being quantized by the edge computing node or when the search quantity reaches the trust region boundary. The process of calculating the truncated conjugate gradient is as follows: Using the zero vector as the initial search quantity, the Riemann gradient as the initial residual, and the inverse of the Riemann gradient as the initial search direction, the direction curvature is calculated by multiplying the second derivative of the target value by the search direction. When the direction curvature is positive and the updated search quantity along the search direction is within the trust region, the step size is obtained by dividing the sum of squares of the residual elements by the direction curvature. A new search quantity is formed by adding the current search quantity to the step size and multiplying by the search direction. The current residual is then multiplied by the second derivative of the target value and the search direction. The product of the two residuals forms a new residual. The sum of squares of the elements of the new residual is divided by the sum of squares of the elements of the previous residual to obtain the conjugate coefficient. The opposite of the new residual plus the conjugate coefficient is multiplied by the search direction of the previous wheel to form the search direction of the next wheel. When the direction curvature is not positive or the updated search quantity crosses the trust region boundary, the positive scaling factor that exactly satisfies the L2 norm equal to the trust region radius is obtained by calculating the current search quantity plus the scaled search direction. The calculation ends with the corresponding boundary search quantity. When all components of the new residual are zero after being quantized by the calculation field of the edge computing node, the calculation ends with the new search quantity. The zero search quantity refers to a column vector with the same number of columns as the identification subspace, where each component is written as zero. This indicates that the truncated conjugate gradient has not yet generated any directional displacement along any basis vector in the identification subspace at the start of the first round of calculation.

[0010] In a preferred embodiment, S3 further includes: S33. Add the search quantity obtained in S32 to the current direction coordinate and divide it by the L2 norm of the resulting vector to form candidate direction coordinates. Multiply the candidate direction coordinates by the identification subspace to form candidate control directions. Multiply the vector obtained by the sliding transformation matrix by the element squares and the negative of the vector obtained by the candidate control directions to form candidate target values. When the candidate target value is lower than the current target value, replace the current direction coordinates with the candidate direction coordinates. Add the L2 norm of the current search quantity to the current trust region radius to form the trust region radius of the next round. When the candidate target value is not lower than the current target value, retain the current direction coordinates and divide the trust region radius by two. Stop the Riemann trust region algorithm when the control directions obtained by mapping the direction coordinates of two adjacent rounds are consistent bit by bit after being quantized by the executor control code. S34. The stop control direction is formed by multiplying the identification subspace by the stop control direction coordinates. The direction components of the stop control direction are read according to the arrangement order of the left and right wheel basic control codes in the basic control sequence. For control beats with positive direction components, the result is the same as the result of subtracting the basic control code from the upper endpoint of the actuator control code and dividing by the corresponding direction component. For control beats with negative direction components, the result is the same as the result of subtracting the basic control code from the lower endpoint of the actuator control code and dividing by the corresponding direction component. The result corresponding to zero direction components is deleted and the first item is read in ascending order of value as the scaling factor. The scaling factor is multiplied by the stop control direction to form the identification increment sequence. The identification increment sequence is superimposed on the basic control sequence and falls into the actuator control code interval, while maintaining the zero change relationship between the terminal two-dimensional position and heading formed in S2. The basic control code refers to the control code written to each control step and corresponding to the control register of the mobile robot actuator when S1 forms the basic control sequence. S34 reads the basic control code according to the same control step and determines the allowable scaling range of the identification increment in the corresponding direction by the difference between the upper or lower endpoint of the actuator control code and the basic control code.

[0011] In a preferred embodiment, S4 includes: S41. The identification incremental sequence and the basic control sequence are added one by one according to the control beat to form the execution control sequence. At the beginning of each control beat, the edge computing node writes the corresponding execution control code into the mobile robot actuator. At the end of the control beat, the robot's horizontal coordinate, vertical coordinate and heading are read and written into the actual pose sequence according to the control beat order. The mobile robot actuator refers to the drive control component that receives the left and right drive wheel execution control codes written by the edge computing node according to the control step, and converts the left and right drive wheel execution control codes into the corresponding drive motor drive inputs, so that the left and right drive wheels rotate according to the current control step. S42. Taking the actual pose before the first shot as the starting point of the recursion, substitute the execution control sequence and the previous cycle sliding parameters into the step-by-step linear recursion relationship to obtain the predicted pose sequence corresponding to the actual pose sequence. Then, subtract the predicted abscissa from the actual abscissa, the predicted ordinate from the actual ordinate, and the predicted heading from the actual heading for each shot, and continue in the order of the control shots to form the response difference sequence.

[0012] In a preferred embodiment, S4 further includes: S43. Select one slip component from the slip parameters of the previous cycle in sequence, rewrite the slip component as the current storage code plus one and the current storage code minus one respectively, and keep the other slip components and the execution control sequence unchanged. Recalculate the predicted pose sequence from the recursive starting point of S42. Divide the difference in horizontal coordinate, vertical coordinate and heading in the same frame of the two sets of recalculated predicted poses by two respectively, and continue to form the corresponding parameter change column according to the control frame order. Then, splice the parameter change columns according to the slip component order to form the parameter change matrix. The storage code refers to the digital code that the edge computing node stores for a single glide component according to a fixed word length. Two adjacent storage codes correspond to a glide parameter quantization step size. Increasing the current storage code by one means that the corresponding glide component increases by one quantization step size, and decreasing the current storage code by one means that the corresponding glide component decreases by one quantization step size. S44. Calculate the sum of squares of each column element in the parameter change matrix and select the first column in descending order of the sum of squares. Use Householder reflection to make the elements below the diagonal position of the first column zero and update the remaining columns simultaneously. Repeat column selection and reflection to form the column permutation matrix, orthogonal matrix, and upper triangular R matrix. Multiply the transpose of the orthogonal matrix by the response difference sequence to form the back substitution vector. Starting from the last row of the upper triangular R matrix, when the diagonal element of the current row is not zero, subtract the sum of the product of the calculated slip correction and the corresponding upper triangular element from the back substitution vector of the current row, and then divide by the diagonal element of the current row to obtain the slip correction of the current row. When the diagonal element of the current row is zero, write the slip correction of the current row to zero and move to the previous row until all rows are back substituted. Back substitute row by row and restore the slip component order according to the column permutation matrix. Multiply each slip correction by the quantization step size of the slip parameter between adjacent storage codes of the corresponding slip component to form the parameter correction value. Add the parameter correction value to the corresponding slip parameter of the previous cycle to form the current slip parameter. The Householder reflection process is as follows: starting from the diagonal position of the current processed column in the parameter change matrix, column elements are read downwards to form a column vector, and the L2 norm of the column vector is calculated. When the first element of the column vector is not negative, the inverse of the L2 norm is used as the reflection first value; when the first element of the column vector is negative, the L2 norm is used as the reflection first value. The reflection vector is formed by subtracting the unit vector at the first element position from the column vector. The reflection vector is then divided by its own L2 norm to form a unit reflection vector. The reflection matrix is ​​formed by subtracting twice the unit reflection vector from the identity matrix and its own transpose. The reflection matrix is ​​then multiplied on the left by the current column and the unprocessed columns on the right, so that the elements below the diagonal position of the current column become zero. The diagonal position refers to the data position in the parameter change matrix or upper triangular R matrix where the row number and column number are the same; during the processing of the j-th column, the position of the j-th row and j-th column is the diagonal position of the current column, and the positions below the diagonal position are the same column positions with row numbers greater than j; The slip correction is the number of slip component storage code steps obtained by performing column pivot Householder QR decomposition and triangular back substitution on the parameter change matrix and response difference sequence. Each slip correction corresponds one-to-one with a slip component. The slip correction is multiplied by the corresponding slip parameter quantization step size to form the parameter correction value.

[0013] In a preferred embodiment, S5 includes: S51. Replace the slip parameters of the previous cycle with the current slip parameters, update the forward speed and heading speed according to the effective wheel speed calculation relationship, take the actual pose at the end of this control cycle as the starting point of the recursion, and re-form the predicted pose, reference pose, position difference and heading difference according to the step-by-step linear recursion relationship. S52. Write the position difference and heading difference into the single-shot value, keep the left and right wheel control increment square terms, the step-by-step linear recursion relationship and the actuator control code interval unchanged, re-form the quadratic term matrix and the linear term vector, and obtain the path tracking control sequence of the next control cycle according to the block LDL transpose decomposition and interval endpoint fixing rules. The quadratic term matrix refers to a symmetric matrix formed by substituting the squared position difference, squared heading difference, and squared left and right wheel control increments of each control step into the linear recursive relationship for each step, and collecting the corresponding coefficients according to the product terms of the left and right wheel control increments. The row and column order of the matrix is ​​consistent with the arrangement order of the left and right wheel control increments of each control step, and it is used to characterize the quadratic effect of different combinations of control increments on the cumulative control cost. The first-order term vector refers to the column vector formed by collecting the corresponding coefficients of the first-order terms of the control increments of each left and right wheel after substituting the position difference and heading difference of each control phase into the linear recursive relationship of each phase. Each element of the vector corresponds one-to-one with the control increment sequence in the quadratic term matrix, and together with the quadratic term matrix, they constitute the quadratic programming equation for the next control cycle. S53. The edge computing node writes the path tracking control sequence to the mobile robot actuator according to the control step through process control, executes the two-dimensional position or heading control of the predetermined trajectory, and writes the current sliding parameter to the previous cycle sliding parameter position of the next control cycle at the end of the current control cycle, and writes the actual pose at the end to the recursive starting point of the next control cycle. Two-dimensional position or heading control of executing a predetermined trajectory refers to the edge computing node writing the path tracking control sequence into the mobile robot actuator according to the control frame, so that the left and right drive wheels generate corresponding speed changes, thereby changing the robot's horizontal coordinate, vertical coordinate and heading angle in the working plane, so that the actual pose is recursively derived according to the control frame and the reference pose of the same frame.

[0014] The technical effects and advantages of this invention are as follows: 1. This scheme uses cosine and sine decomposition to screen out the control direction with zero change in terminal pose and retaining the slip response, so that the existing path control quantity can also be used as identification excitation, and the slip identification basis can be obtained without increasing the off-track action. 2. By using the Riemann trust region to iteratively control the direction within the identification subspace and combining it with the actuator control code boundary scaling identification increment, the slip response search is constrained by the actual control range, reducing the interference of additional control actions on path tracking; 3. The actual pose is acquired by superimposing the identification increment with the basic control sequence, and the pose is predicted by recursively using the executed control sequence to form a response difference, so that the actual motion deviation can correspond to the change of slip parameter, thereby improving the pertinence of online slip correction. 4. The slip correction of the left and right wheels is obtained by using parameter change matrix, column pivot QR decomposition and triangular back substitution, so that the motion response of different drive wheels participates in parameter update respectively, which reduces the confusion of the state of the left and right wheels caused by uniform slip parameters. 5. Based on the current slip parameters, the motion relationship is relinearized and the control quantity for the next control cycle is solved, so that the identification results are directly incorporated into the two-dimensional position and heading control, forming a process control chain that connects slip identification and path tracking cycle by cycle. Attached Figure Description

[0015] Figure 1 This is a flowchart of the mobile robot sliding recognition and path control of the present invention. Detailed Implementation

[0016] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and 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.

[0017] Refer to the instruction manual appendix Figure 1 The present invention provides a mobile robot path tracking method based on online slip identification and successive linearized model predictive control, comprising: S1. Obtain the robot's pose and wheel speed. The edge computing nodes linearize the motion relationship by combining the sliding parameters of the previous cycle. The pose is recursively predicted along the predetermined trajectory, and the basic control sequence is solved according to the position difference and heading difference. The path tracking control is calculated based on the robot's current pose and wheel speed. First, the predetermined trajectory position corresponding to the current position is determined, and then the wheel speeds of the left and right drive wheels are corrected using the slip parameters of the previous cycle. The edge computing node then brings the control increments of the left and right wheels into the pose prediction step by step, and obtains the basic control sequence based on the deviation of the predicted pose from the reference pose. The control cycle is divided into several control steps according to the control program. Each step corresponds to one pose recursion and one calculation of the left and right wheel control quantities. The specific process is as follows: After the current position enters the trajectory corresponding calculation, the edge calculation node reads the adjacent trajectory points segment by segment in S11, takes the inner product of the vector obtained by subtracting the starting point of the trajectory segment from the current position and the coordinate difference vector of the trajectory segment, and then divides it by the sum of squares of the coordinate differences of the trajectory segments to obtain the projection coefficient. When the projection coefficient is less than zero, the starting point of the trajectory segment is taken; when it exceeds one, the ending point of the trajectory segment is taken. In other cases, the projection point is formed by adding the projection coefficient to the starting point of the trajectory segment and multiplying the coordinate difference of the trajectory segment. When the coordinates of adjacent trajectory points are the same, the corresponding trajectory segment is skipped. Then, the squared distances from the current position to each projection point are calculated and sorted in ascending order. When the values ​​are the same, the first item is determined by the absolute value of the difference between the current heading and the trajectory direction, and the trajectory segment number. The corresponding projection point is written as the current trajectory position. The trajectory direction is formed by normalizing the trajectory segment coordinate difference, and the current trajectory arc length is calculated for subsequent reading. When the current position is missing, the current trajectory record is not updated and the control code of the previous control frame is retained. In S12, the motion of the drive wheel is read according to the start and end times of the same control beat. The cumulative rotation angle of the left and right encoders is read. The rotation angle difference is divided by the control beat duration to form the wheel speed of the left and right drive wheels. Then, the wheel speed is multiplied by the wheel radius and subtracted by the corresponding slip component to form the effective wheel speed of the left and right. The sum of the two is divided by two to form the forward speed. The effective right wheel speed minus the effective left wheel speed is divided by the wheelbase to form the heading speed, and this is written into the speed record. The first control cycle writes the decoded value of the storage code corresponding to zero slip into the left and right slip components. Starting from the second control cycle, the slip parameters formed in the previous cycle are read. When the encoder data of any drive wheel is missing, the corresponding control step does not generate forward speed and heading speed. After the control increment enters the prediction process, S13 takes the current pose as the starting point for recursion, reads the current execution control code of the left and right wheels in each frame and adds the corresponding control increment to form a candidate control code, obtains the predicted wheel speed of the left and right wheels according to the calibration relationship between the driver control code and the drive wheel speed, and then forms the predicted forward speed and predicted heading speed according to the conversion relationship of S12. The predicted heading of the previous frame is used as the unfolded heading. The heading offset is formed by multiplying the predicted heading speed by the control frame duration. The predicted abscissa and ordinate are updated according to the first-order sine and cosine terms of the unfolded heading, respectively. The heading offset is added to the predicted heading of the previous frame to form the predicted pose of the current frame. The predicted forward velocity is multiplied by the control time and then accumulated step by step to form the predicted travel distance. The reference position and reference heading are obtained from the current trajectory position along the predetermined trajectory. When the predicted travel distance exceeds the remaining trajectory arc length, it is fixed to the trajectory endpoint. The heading difference is uniformly formed by the arctangent of the difference sine and cosine. After the predicted pose is formed, S14 projects the coordinate difference obtained by subtracting the reference position from the predicted position onto the reference heading normal direction to form the position difference. The position difference, heading difference and left and right wheel control increments are converted according to their respective storage resolutions and then squared and added to form the single-shot cost value, which is then accumulated according to the control shots. Based on the step-by-step linear recursion relationship, the quadratic and linear terms of the control increment are collected to form a quadratic term matrix and a linear term vector. The control increments of the left and right wheels are alternately arranged according to the control steps to form the vector to be determined. The control increments of the left and right wheels are obtained through block LDL transpose decomposition, forward substitution, block diagonal solution and backward substitution. When the sum of the current execution control code and the control increment exceeds the upper endpoint of the actuator control code or is lower than the lower endpoint, the corresponding control increment is fixed as the value obtained by subtracting the current execution control code from the endpoint, and the remaining components to be determined are decomposed again until all control codes fall into the actuator control code range, and finally the basic control sequence is written according to the control beat. After the above processing, the current trajectory position, effective wheel speed, predicted pose and basic control sequence form a unified control frame data chain. The left and right wheel control increments can directly act on the predicted position and predicted heading. The basic control sequence simultaneously satisfies the predetermined trajectory tracking relationship and the actuator control code boundary. When the robot enters the turning trajectory, the edge computing nodes sequentially complete trajectory correspondence, slip correction, frame-by-frame pose prediction, and control quantity solution. The resulting basic control sequence continues to be used for subsequent terminal change matrix and slip change matrix calculations and readings.

[0018] S2. Add a unit control quantity to each control step to recalculate the predicted pose, form a terminal change matrix with the terminal pose difference, form a slip change matrix with the slip parameter unit change, perform cosine-sine decomposition algorithm on the two matrices, and take the common direction where the terminal cosine is zero and the slip sine is non-zero to form the identification subspace. After the basic control sequence is formed, the edge computing nodes continue to use the same step-by-step linear recursive relationship to analyze the effect of each left and right wheel basic control code on the prediction terminal and slip response; When a control cycle contains N control steps, the basic control codes for the left and right wheels form 2N control components in the order of the left wheel in the first step, the right wheel in the first step, and so on to the right wheel in the Nth step. Subsequent terminal change matrices and slip change matrices establish column correspondences in this order. Increasing or decreasing the control codes by one is only used for recalculation within the edge computing nodes and is not written to the mobile robot actuator. The specific process is as follows: When establishing the terminal pose response for each control component, a basic control code for the left and right wheels is read in the calculation of S21, and the remaining 2N minus 1 basic control codes are kept at their original values. Two sets of calculation sequences are formed by adding one and subtracting one from the current control code, respectively. Then, all N control frames are recursively calculated from the current pose according to the linear recursive relationship step by step. After the two sets of calculation sequences are completed, the last frame's x-coordinate, y-coordinate, and heading are read respectively. The result of adding one code and subtracting one code is divided by two to form three terminal change quantities. The terminal change columns corresponding to the current control component are written in the order of x-coordinate, y-coordinate, and heading. After all 2N control components are completed, the terminal change matrix has 3 rows and 2N columns. The increment or decrement code values ​​used in the prediction process only participate in the step-by-step linear recursion. When the value crosses the endpoint of the actuator control code, it is still calculated according to the local linear recursion relationship and the corresponding value is not written to the driver control register. The effect of slip on the same control component is obtained through cross-difference; after entering S22, keep the current control component and column number unchanged, and select the left slip component and right slip component in sequence; For a sliding component, four combinations are formed: current control code incremented by one and sliding component storage code incremented by one; current control code decremented by one and sliding component storage code incremented by one; current control code incremented by one and sliding component storage code decremented by one; and current control code decremented by one and sliding component storage code decremented by one. Then, N control frames are recursively deduced from the current pose. The two sets of predicted poses with the slip component storage code incremented by one are first subtracted from each other and divided by two. The two sets with the slip component storage code decremented by one are processed in the same way. Then, the previous result is subtracted from the next result and divided by two to obtain the step-by-step cross-response of the current control component under the corresponding slip component change. Each slip component is connected in the order of x-coordinate, y-coordinate, and heading from the first frame to the Nth frame. The two sets of slip components are then connected in the order of left slip and right slip to form a column. Therefore, the slip change matrix has 6N rows and 2N columns, and maintains the same column control component as the terminal change matrix. The sliding parameter storage field retains one computational code bit at each end of the physical update interval, so that the endpoints of the update interval can still perform the storage code increment and decrement; the reserved code bits only participate in the differential operation and are not written to the current sliding parameter; After the two types of transformation matrices are completed, the coordinate scale is uniformly controlled, and the relevant processing is placed in S23; the terminal transformation matrix transposes and multiplies itself to obtain a 2N-order terminal Gram matrix, and the glide transformation matrix transposes and multiplies itself to obtain a 2N-order glide Gram matrix. The two are added together to form a joint matrix. The joint matrix performs Householder tridiagonalization and implicit shift QR iteration according to the fixed word length field. The result of each round of multiplication and addition is written back to the same number of decimal places. Data with all bits of zero in the rounded field is treated as zero. The eigenvalue iteration ends when all bits of the off-diagonal field are written as zero. When eigenvalues ​​are the same, they are arranged in ascending order according to the row number of the first non-zero component of the corresponding eigenvector. When the first non-zero component of the eigenvector is negative, the entire column is multiplied by negative one to fix the direction sign. Directions with zero eigenvalues ​​are deleted, and the remaining directions are scaled by the reciprocal of the square root of the eigenvalue to form a normalized matrix. The terminal transformation matrix and the sliding transformation matrix are right-multiplied by the normalization matrix, and the resulting terminal normalization matrix and sliding normalization matrix satisfy that the sum of their Gram matrices is the identity matrix. Then, eigenvalue decomposition is performed on the Gram matrix of the terminal normalized matrix. A common right decomposition matrix is ​​formed in ascending order of eigenvalues. The square root of the eigenvalue is written as the terminal cosine component, and the square root after subtracting the eigenvalue is written as the glide sine component. When fixed word length operations cause a feature value to have a last bit lower than zero or a last bit higher than one, write zero or one respectively before executing the square root to avoid the square root input exceeding the normalization range. After the common direction is decomposed, S24 restores the direction that satisfies the zero change relationship of the terminal pose to the actual control space; When all bits of the terminal cosine component field are zero after fixed word length quantization and the corresponding sliding sine component field has non-zero bits, the corresponding right decomposition vector is read and the right decomposition vector is multiplied by the normalization matrix to form a 2N-dimensional candidate control direction. The row order of the candidate control directions always maintains the left and right wheel basic control code arrangement order of the basic control sequence, and each candidate control direction forms a direction matrix by column; After performing column pivoting QR decomposition on the direction matrix, candidate directions with zero diagonal elements in the R matrix are deleted, while candidate directions with non-zero diagonal elements are retained. The original column order of the candidate directions in the direction matrix is ​​restored according to the column permutation matrix, and the retained columns form the identification subspace. If the joint matrix does not have a non-zero characteristic direction, or if the cosine-sine decomposition does not simultaneously satisfy the condition that the terminal cosine component is zero and the sliding sine component is non-zero, then the identification subspace is written as an empty record, the identification direction search is not entered in this control cycle, the identification increment sequence is written as an all-zero sequence, and the basic control sequence is directly used as the subsequent execution control sequence. Thus, the terminal change matrix records the effect of each left and right wheel control component on the final two-dimensional position and heading, and the slip change matrix records the cross response generated by the change of the left and right slip components in the step-by-step motion of the same control component. After unified normalization, the two types of matrices complete cosine and sine decomposition in the same control coordinates, so that the subsequent search is only carried out in the direction set where the terminal pose effect is eliminated and the slip response is retained. Taking a control cycle containing 4 control beats as an example, the basic control sequence contains 8 left and right wheel control components, the terminal change matrix forms 3 rows and 8 columns, and the slip change matrix forms 24 rows and 8 columns; each column always corresponds to the same left and right wheel basic control code, and the final retained identification subspace is directly used for subsequent Riemann trust region calculation and reading.

[0019] S3. Based on the identification subspace, the Riemann trust region algorithm is executed. The target value is the square and the opposite of the direction of the sliding transformation matrix multiplication. The direction of the conjugate gradient is updated by truncation. If the target value decreases, it is retained; otherwise, the trust region radius is halved. When adjacent quantization directions are consistent, the identification increment sequence is formed by scaling according to the actuator control code boundary. After the identification subspace is formed, the edge computing nodes search for the slip response direction within the subspace coordinates and convert the search results into identification increments that can be written by the actuator control code. All calculations follow the left and right wheel basic control code order and fixed word length field of S2. When the identification subspace is empty, the all-zero identification increment sequence is directly written. The specific process is as follows: Direction search is first established in the identification subspace coordinates; when processing S31, assuming that the identification subspace contains r columns, r-dimensional direction coordinates are established and the first component is written as 1 and the remaining components are written as zero to form the initial direction coordinates. The current control direction is obtained by multiplying the identification subspace by the direction coordinates. The slip change matrix is ​​multiplied by the current control direction, the sum of squares of the elements is calculated and the opposite is taken to form the current target value. Then, the Riemann gradient is obtained by subtracting the inner product of the direction coordinates from the derivative of the target value with respect to the direction coordinates. When the Riemann gradient field contains non-zero bits, its L2 norm is written into the first round of the trust region radius; when all bits are zero, the current direction coordinates are directly written as the stop control direction coordinates. The tangent space search was then completed by S32; the zero vector was used as the search quantity, the Riemann gradient was used as the residual and its negative value was taken to form the first round of search direction, and the search direction was multiplied by the second derivative of the target value on the left and then multiplied by the search direction to obtain the directional curvature. When the directional curvature is positive, the step size is formed by dividing the sum of squared residuals by the directional curvature, and the search quantity and residuals are updated accordingly. When the updated search quantity is within the trust region, the conjugate coefficient is formed by the ratio of the new sum of squared residuals to the sum of squared residuals of the front wheel, and then the next search direction is generated. When the directional curvature is not positive or the search quantity crosses the trust region boundary, the current search quantity plus the scaled search direction is substituted into the relationship that the L2 norm equals the trust region radius, the quadratic expression is expanded and the positive root is taken to obtain the boundary search quantity; the new residual is quantized with a fixed word length and all bits are zero, or the search quantity reaches the trust region boundary and the current round of calculation ends. After obtaining the search quantity, S33 adds the search quantity to the current direction coordinates and normalizes them to form candidate direction coordinates. Then, it multiplies the candidate direction coordinates by the identification subspace to form candidate control directions and recalculates the candidate target values. When the candidate target value is lower than the current target value, write the candidate direction coordinates and update the trust region radius to the current trust region radius plus the search quantity L2 norm; when the candidate target value is not lower than the current target value, retain the current direction coordinates and divide the trust region radius by two. After each round, the control direction obtained by multiplying the identification subspace by the current direction coordinate is quantized according to the actuator control code resolution. The Riemann trust region calculation is stopped when the quantized directions of two adjacent rounds are consistent in each component. When the stop direction enters the actuator boundary conversion, S34 multiplies the stop control direction coordinates by the identification subspace to form the stop control direction, and reads each direction component in the order of the left and right wheel basic control codes; When the direction component is positive, subtract the basic control code from the upper endpoint of the actuator control code and divide by the direction component. When the direction component is negative, subtract the basic control code from the lower endpoint of the actuator control code and divide by the direction component. Delete the results corresponding to zero direction component and read the first item in ascending order of the value as the scaling amount. The scaling factor is multiplied by the stop control direction and then quantized according to the control code resolution to form an identification increment sequence. The terminal change matrix is ​​then multiplied by the identification increment sequence to verify the changes in the terminal's horizontal coordinate, vertical coordinate, and heading. When all three results are zero after quantization, the identification increment sequence is retained. When there are non-zero results, the scaling amount is reduced by one control code step and recalculated until all terminal changes are zero or the scaling amount is written to zero. After the above processing, the direction search is always limited to the identification subspace, the trust region update is responsible for filtering the slip response direction, the actuator boundary conversion is responsible for converting the stop direction into an integer control code increment, and the two-dimensional position and heading zero change relationship is maintained through terminal change verification; the obtained identification increment sequence is continued to be used for the construction of subsequent execution control sequences.

[0020] S4. The identification increment sequence is superimposed on the basic control sequence to form the execution control sequence. The process control output is sent to the actuator. The pose is predicted recursively according to the execution control sequence. The actual pose is subtracted from the predicted pose to form the response difference sequence. The parameter change matrix is ​​formed according to the unit change of the slip parameter. The slip correction amount is obtained through QR decomposition and back substitution to form the current slip parameter. After the execution control sequence enters the robot, the edge computing node establishes a correspondence between the actual motion response and the prediction result of the same step according to the control step. Then, it recalculates the parameter sensitive response by storing one positive and one negative code bit of the slip component code, uses the parameter change matrix to inversely calculate the slip correction amount, and writes the updated result into the next control cycle. All pose records use the same control step number and time base to avoid step misalignment between executed control codes, actual pose, and predicted pose; the specific process is as follows: The binding of the execution control code to the actual motion record is completed in each frame; after the incremental sequence and the basic control sequence are added one by one according to the left and right wheel control components, S41 forms the execution control sequence; At the start of the control frame, the edge computing node writes the control codes for the left and right wheels into the mobile robot actuator, and simultaneously writes the control frame number and the writing time. At the end of the control frame, the node reads the robot's horizontal coordinate, vertical coordinate, and heading, and binds them to the same control frame number to form an actual pose record. All records are arranged in the order of the control frames to form an actual pose sequence. When a control cycle is missing any pose component, a pose missing record is written. No slip correction is generated in this control cycle, and the slip parameters of the previous cycle are retained until the next control cycle. The pose response difference is recalculated according to the execution control sequence to predict the baseline. After entering S42, the actual pose before the first shot is used as the starting point for recursion. The execution control codes of each shot are converted into the left and right predicted wheel speeds according to the correspondence between the control codes determined in S1 and the drive wheel speeds. Then, combined with the sliding parameters of the previous cycle, linear recursion is performed shot by shot to form a predicted pose sequence that is arranged in the same shot as the actual pose sequence. Each frame generates a lateral response difference by subtracting the predicted lateral coordinate from the actual lateral coordinate, a longitudinal response difference by subtracting the predicted ordinate from the actual ordinate, and a heading response difference by taking the arctangent of the sine and cosine of the difference after subtracting the predicted heading from the actual heading. These responses are then sequentially arranged in the order of lateral coordinate, ordinate, heading, and control frame to form a response difference sequence. Thus, a control cycle containing N control frames generates 3N response difference components. The parameter sensitivity relationship uses the same execution control sequence for central difference; when processing S43, the left sliding component and the right sliding component are read in sequence, and the current sliding component is decoded by adding one to the storage code and subtracting one from the storage code respectively, while keeping the other sliding component and all execution control codes unchanged, and recalculating the predicted pose sequence from the same recursive starting point of S42 respectively; The two sets of recalculated results are subtracted from the x-coordinate, y-coordinate and heading of the same shot and then divided by two. The result is written as the parameter change column corresponding to the current slip component. Each parameter change column is arranged in the order of left slip component and right slip component to form the parameter change matrix. Therefore, the parameter change matrix in the case of N shots is 3N rows and 2 columns. The sliding storage field retains a differential calculation code bit at each end of the parameter update interval, so that the endpoints of the update interval can still perform the storage code increment and decrement. The reserved code bit only participates in the recalculation and is not written to the current sliding parameter. The parameter writeback is completed by S44 based on the response difference sequence and the parameter change matrix. First, the column pivot Householder QR decomposition is performed in descending order of the sum of squares of the elements in each parameter change column to obtain the column permutation matrix, the orthogonal matrix and the upper triangular R matrix. Then, the response difference sequence is multiplied by the transpose of the orthogonal matrix to form the back substitution vector. Calculate row by row from the last row to the first row of the upper triangular R matrix. When the diagonal element field is non-zero, subtract the sum of the product of the already obtained slip correction and the corresponding upper triangular element from the current row's back substitution vector, and then divide by the current row's diagonal element to obtain the current row's slip correction. When the diagonal element field is zero, write the current row's slip correction to zero. After the back substitution is completed, the order of the left and right sliding components is restored according to the column permutation matrix. Each sliding correction is multiplied by the quantization step size of the sliding parameter between the corresponding adjacent memory codes to form the parameter correction value. Then, the corresponding sliding component of the previous cycle is added to form the current sliding parameter. When both diagonal elements are zero, the left and right parameter correction values ​​are written to zero and the sliding parameter of the previous cycle is maintained. After the above processing, the execution control code, actual pose, predicted pose and slip parameter changes are all entered into the calculation in the same control step order. The response difference only retains the motion differences that the execution control sequence could not explain under the slip parameters of the previous cycle. The parameter change matrix then distributes the motion differences to the corresponding slip components. When the mobile robot passes through a section of road where the attachment state changes, the motion response of the left and right wheels obtained in the current control cycle is used to form the current slip parameters, which are directly used for step-by-step linear recursion and two-dimensional position or heading control reading in the next control cycle.

[0021] S5. Linearize the motion relationship based on the current slip parameters, recursively predict the pose along the trajectory and solve the control quantity, output it to the actuator to perform two-dimensional position or heading control, and transfer the current slip parameters to the next control cycle; After the current slip parameters are formed, the path tracking calculation proceeds to the next control cycle. The edge computing nodes continue to use the established rules for effective wheel speed conversion, step-by-step linear recursion, and control quantity solution. They only replace the corresponding inputs from the previous cycle with the current slip parameters and the actual pose at the end of this control cycle, so that the slip identification results can be directly entered into subsequent two-dimensional position or heading control. The specific process is as follows: The slip parameter update is first applied to the next round of motion recursion; when executing S51, the current left slip component and the current right slip component formed in S44 are written to the slip parameter position of the previous cycle, and the actual pose at the end of the current control cycle is read as the new recursion starting point. The wheel speeds of the left and right drive wheels are recalculated according to the effective wheel speed calculation relationship determined in S12 to form forward speed and heading speed. Then, based on the step-by-step linear recursion relationship determined in S13, the predicted pose of each control frame is formed, and the reference pose of the same frame is obtained from the current trajectory position along the predetermined trajectory. The normal projection of the predicted position relative to the reference position forms the position difference, and the predicted heading and the reference heading form the heading difference according to the predetermined heading difference processing rules; when no parameter correction value is formed in S44, the current slip parameter maintains the value of the slip parameter of the previous cycle, and enters the next control cycle according to the same recursive process; After the new position difference and heading difference are written into the control solution, S52 follows the single-shot cost structure of S14, and accumulates the square of the position difference, the square of the heading difference, and the square of the left and right wheel control increments of each control shot according to the predetermined measurement scale. Then, based on the linear recursive relationship of each shot, the control increment product terms are collected again to form a quadratic term matrix, and the single control increment linear terms are collected to form a linear term vector. The quadratic term matrix is ​​decomposed by block LDL transpose according to the predetermined arrangement of the left and right wheel control increments. The left and right wheel control increments for the next control cycle are obtained by forward substitution, block diagonal solution and backward substitution. When the sum of the current execution control code and the obtained control increment touches the outside of the actuator control code interval, the corresponding control increment is processed according to the interval endpoint fixing rule of S14 and the remaining components are re-solved. After all control codes are located in the actuator control code interval, the path tracking control sequence of the next control cycle is formed according to the control beat. When the path tracking control sequence is written to the actuator, the edge computing node writes the corresponding left and right wheel execution control codes in S53 according to the starting position of the control beat, and records the execution time with the same control beat number, so that the left and right drive wheels change the speed according to the path tracking control sequence, and the robot position and heading are recursively pushed to the same beat reference pose with each control beat. At the end of a control cycle, the actual pose at the end is written to the starting point of the next control cycle, and the current sliding parameter is written to the position of the sliding parameter of the previous cycle in the next control cycle. When the control code writing fails, the corresponding control step is recorded and the actual pose generated by the control step is no longer used to update the sliding parameters. The control status is read from the position where the most recent complete execution ended in the next control cycle. At this point, the slip recognition result is used to enter the pose prediction and control quantity solution of the next control cycle through the current slip parameters. The path tracking control sequence then drives the left and right wheels to change the robot's two-dimensional position and heading. The control cycle ends and continues to enter the next round of calculation. After the robot passes through the area where the adhesion conditions change, the newly identified left and right slip components immediately participate in the calculation of effective wheel speed and predicted pose in the next control cycle, so that the subsequent control quantities correspond to the updated wheel-ground motion relationship.

[0022] exist Figure 1 It should be noted that: This figure shows the complete process of a mobile robot from generating basic control sequences, constructing identification subspace, solving identification increments, updating slip parameters, to updating path tracking control; solid arrows indicate that each step in the current control cycle transmits data and control results in sequence, and dashed arrows indicate that the current slip parameters are written back to the next control cycle for subsequent two-dimensional position or heading control.

[0023] Overall execution and implementation description: This scheme first reads the pose, wheel speed and sliding parameters of the mobile robot from the edge computing node, maps the current position to the predetermined trajectory, and corrects the effective wheel speed according to the sliding of the left and right wheels. Then, it predicts the robot's subsequent position and heading step by step according to the control steps, thereby obtaining the basic control sequence. Based on the basic control sequence, the system analyzes the influence of each left and right wheel control quantity on the predicted terminal pose and slip response. The control directions that do not change the terminal's two-dimensional position and heading but can still retain the slip response are screened out by cosine and sine decomposition. The identification increment is then obtained from these directions using the Riemann trust region algorithm. After the identification increment is superimposed with the basic control sequence, it is actually written into the actuator. The actual pose generated by the robot is compared with the pose predicted according to the original slip parameters to form a response difference. Based on this, the slip correction amount of the left and right wheels is calculated to obtain the current slip parameters. The current slip parameters then re-enter the motion prediction and control quantity solution of the next control cycle, so that slip identification and path tracking are cyclically connected in the same control process. Taking a warehouse mobile robot turning along a narrow passage as an example, after the robot enters the curve, the edge computing node first determines the corresponding trajectory segment based on the current position, then uses the encoder wheel speed and the slip parameters of the previous cycle to calculate the current effective motion speed, and predicts the position and heading captured by each control. The system then selects control change directions from the basic control sequence that will not change the target position and target heading at the end of the curve, adds identification increments in these directions, so that the left and right wheels can generate motion responses that reflect the differences in ground slip during the turning process, while keeping the predicted terminal pose unchanged. After execution, the edge computing node compares the actual pose with the predicted pose, calculates the slip correction amount of the left and right wheels, and brings the new slip parameters into the next control cycle to recalculate the path tracking control amount, so that the robot can still adjust the subsequent two-dimensional position and heading control according to the actual wheel-ground state when the road surface adhesion changes.

[0024] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A path tracking method for a mobile robot based on online slip identification and successive linearized model predictive control, characterized in that, include: S1. Obtain the robot's pose and wheel speed. The edge computing nodes linearize the motion relationship by combining the sliding parameters of the previous cycle. The pose is recursively predicted along the predetermined trajectory, and the basic control sequence is solved according to the position difference and heading difference. S2. Add a unit control quantity to each control step to recalculate the predicted pose, form a terminal change matrix with the terminal pose difference, form a slip change matrix with the slip parameter unit change, perform cosine-sine decomposition algorithm on the two matrices, and take the common direction where the terminal cosine is zero and the slip sine is non-zero to form the identification subspace. S3. Based on the identification subspace, the Riemann trust region algorithm is executed. The target value is the square and the opposite of the direction of the sliding transformation matrix multiplication. The direction of the conjugate gradient is updated by truncation. If the target value decreases, it is retained; otherwise, the trust region radius is halved. When adjacent quantization directions are consistent, the identification increment sequence is formed by scaling according to the actuator control code boundary. S4. The identification increment sequence is superimposed on the basic control sequence to form the execution control sequence. The process control output is sent to the actuator. The pose is predicted recursively according to the execution control sequence. The actual pose is subtracted from the predicted pose to form the response difference sequence. The parameter change matrix is ​​formed according to the unit change of the slip parameter. The slip correction amount is obtained through QR decomposition and back substitution to form the current slip parameter. S5. Linearize the motion relationship based on the current slip parameters, recursively predict the pose along the trajectory and solve the control quantity, output it to the actuator to perform two-dimensional position or heading control, and transfer the current slip parameters to the next control cycle.

2. The mobile robot path tracking method based on online slip identification and successive linearized model predictive control according to claim 1, characterized in that: S1 includes: S11. Calculate the projection coefficient by connecting each adjacent trajectory point of the predetermined trajectory with the robot's current position. When the projection coefficient is less than zero, take the starting point of the connection; when it exceeds one, take the ending point of the connection; otherwise, the projection point is formed by adding the projection coefficient to the coordinates of the starting point and multiplying the difference between the coordinates of the two ends. Calculate the square of the distance from the current position to each projection point and take the first projection point corresponding to the first item in ascending order as the current trajectory position. The trajectory direction is formed by dividing the coordinate difference of the corresponding connection by the square root of the sum of the squares of the coordinate differences. S12. Multiply the wheel speeds of the left and right drive wheels by the wheel radius, then multiply by one and subtract the slip component of the corresponding drive wheel in the slip parameters of the previous cycle to form the effective wheel speeds of the left and right. Divide the sum of the two by two to get the forward speed. Subtract the effective wheel speed of the left from the effective wheel speed of the right and divide by the wheel track to get the heading speed.

3. The mobile robot path tracking method based on online slip identification and successive linearized model predictive control according to claim 2, characterized in that: S1 also includes: S13. Starting from the current pose, predict the pose recursively according to the control beats. For each beat, read the current control code of the corresponding left and right wheels and add the control increment of the left and right wheels in this beat to form a candidate control code. According to the wheel speed of the drive wheel corresponding to the candidate control code, form the predicted forward speed and the predicted heading speed according to S12. Using the predicted heading of the previous beat as the expansion point, substitute the heading offset into the first-order expansion term of the cosine and the first-order expansion term of the sine, and multiply the predicted forward speed and the predicted heading speed by the control beat duration to form a beat-by-beat linear recursive relationship between the predicted position and the predicted heading. At the same time, determine the reference position and reference heading of the same beat from the current trajectory position along the predetermined trajectory according to the predicted travel distance. S14. The single-cycle cost is formed by adding the square of the projection difference of the predicted position along the normal direction of the reference heading to the square of the difference obtained by subtracting the reference heading from the predicted heading, and then adding the square of the left and right wheel control increments. After accumulating the cost per control cycle, a quadratic programming equation is established for the linear recursive relationship of each cycle. The left and right wheel control increments are obtained by decomposing the block LDL transpose. When the sum of the current executed control code and the control increment exceeds the actuator control code interval, the corresponding control increment is fixed as the value obtained by subtracting the current executed control code from the corresponding endpoint of the actuator control code interval, and the remaining control increments are decomposed again until all control quantities fall into the actuator control code interval. The current executed control code is added to the corresponding left and right wheel control increments, and the basic control sequence is written according to the control cycle.

4. The mobile robot path tracking method based on online slip identification and successive linearized model predictive control according to claim 3, characterized in that: S2 includes: S21. For each control beat in the basic control sequence, read the left wheel basic control code and the right wheel basic control code of the current beat as the current control code. Rewrite the current control code as the current value plus one and the current value minus one respectively, while keeping the other basic control codes unchanged. Proceed to the prediction terminal along the linear recursive relationship of each beat. Divide the difference of the terminal horizontal coordinate, the difference of the vertical coordinate, and the difference of the heading obtained from the two recursions by two respectively, and write them into a column of the terminal change matrix according to the arrangement order of the left and right wheel basic control codes in the basic control sequence. S22. For the current control code in S21, select a slip component in sequence, and rewrite the current control code as the current value plus one and the current value minus one respectively. At the same time, rewrite the storage code corresponding to the slip component as the current value plus one and the current value minus one respectively. Under the four combinations of current control code and slip component storage code, recalculate the frame-by-frame predicted pose. First, divide the frame-by-frame pose difference between the two current control codes when the slip component storage code is plus one by two. Then, divide the frame-by-frame pose difference between the two current control codes when the slip component storage code is minus one by two. Subtract the two results and divide by two again. Continuate the slip component results in the order of horizontal coordinate, vertical coordinate and heading within the control frame to form a slip change matrix with the same column order as the terminal change matrix.

5. The mobile robot path tracking method based on online slip identification and successive linearized model predictive control according to claim 4, characterized in that: S2 also includes: S23. Multiply the terminal transformation matrix by the transpose of the terminal transformation matrix, and then multiply the transpose of the sliding transformation matrix by the sliding transformation matrix to form a joint matrix. Perform eigenvalue decomposition on the joint matrix, delete the eigenvectors with zero eigenvalues, and multiply the inverse of the square root of each retained eigenvalue by the corresponding eigenvector to form a normalized matrix. Then, multiply the terminal transformation matrix and the sliding transformation matrix by the normalized matrix on the right to form the terminal normalized matrix and the sliding normalized matrix, respectively. Perform cosine and sine decomposition algorithms on the two matrices. S24. Read the right decomposition vectors in the cosine-sine decomposition results where the terminal cosine component is zero and the glide sine component is not zero. Multiply each right decomposition vector by the normalization matrix to obtain the control direction. Assemble the direction matrix by columns of each control direction. Perform column pivoting QR decomposition on the direction matrix. Delete the candidate directions corresponding to the diagonal elements of the R matrix being zero. Restore the original column order of the remaining candidate directions in the direction matrix according to the column permutation matrix to form the identification subspace.

6. The mobile robot path tracking method based on online slip identification and successive linearized model predictive control according to claim 5, characterized in that: S3 includes: S31. Establish directional coordinates based on the column number of the identification subspace, read the first unit coordinate vector as the initial directional coordinates, multiply the identification subspace by the current directional coordinates to form the current control direction, multiply the vector obtained by the sliding transformation matrix by the squares of the elements and the inverse of the vector to form the current target value, subtract the product of the current directional coordinates and the derivative of the current target value with respect to the current directional coordinates to form the Riemann gradient, and use the second norm of the Riemann gradient as the radius of the first trust region. S32. Perform truncated conjugate gradient calculation in the orthogonal tangent space of the current direction coordinates. In the first round, the search direction is formed by the inverse of the Riemann gradient and the search quantity starts from zero. The direction curvature is formed by multiplying the second derivative of the target value by the left of the search direction and then multiplying it by the search direction. When the direction curvature is positive, the search quantity and residual are updated by dividing the sum of squared residuals by the direction curvature. When the direction curvature is not positive or the updated search quantity exceeds the trust region, the intersection point falling on the trust region boundary along the search direction is taken as the search quantity. The calculation ends when all components of the residual are zero after being quantized by the edge computing node or when the search quantity reaches the trust region boundary.

7. The mobile robot path tracking method based on online slip identification and successive linearized model predictive control according to claim 6, characterized in that: S3 also includes: S33. Add the search quantity obtained in S32 to the current direction coordinate and divide it by the L2 norm of the resulting vector to form candidate direction coordinates. Multiply the candidate direction coordinates by the identification subspace to form candidate control directions. Multiply the vector obtained by the sliding transformation matrix by the element squares and the negative of the vector obtained by the candidate control directions to form candidate target values. When the candidate target value is lower than the current target value, replace the current direction coordinates with the candidate direction coordinates. Add the L2 norm of the current search quantity to the current trust region radius to form the trust region radius of the next round. When the candidate target value is not lower than the current target value, retain the current direction coordinates and divide the trust region radius by two. Stop the Riemann trust region algorithm when the control directions obtained by mapping the direction coordinates of two adjacent rounds are consistent bit by bit after being quantized by the executor control code. S34. The stop control direction is formed by multiplying the identification subspace by the stop control direction coordinates. The direction components of the stop control direction are read according to the arrangement order of the left and right wheel basic control codes in the basic control sequence. For control beats with positive direction components, the result is the actuator control code minus the basic control code and divided by the corresponding direction component. For control beats with negative direction components, the result is the actuator control code minus the basic control code and divided by the corresponding direction component. The result corresponding to zero direction components is deleted and the first item is read in ascending order of value as the scaling amount. The scaling amount is multiplied by the stop control direction to form the identification increment sequence.

8. The mobile robot path tracking method based on online slip identification and successive linearized model predictive control according to claim 7, characterized in that: S4 includes: S41. The identification incremental sequence and the basic control sequence are added one by one according to the control beat to form the execution control sequence. At the beginning of each control beat, the edge computing node writes the corresponding execution control code into the mobile robot actuator. At the end of the control beat, the robot's horizontal coordinate, vertical coordinate and heading are read and written into the actual pose sequence according to the control beat order. S42. Taking the actual pose before the first shot as the starting point of the recursion, substitute the execution control sequence and the previous cycle sliding parameters into the step-by-step linear recursion relationship to obtain the predicted pose sequence corresponding to the actual pose sequence. Then, subtract the predicted abscissa from the actual abscissa, the predicted ordinate from the actual ordinate, and the predicted heading from the actual heading for each shot, and continue in the order of the control shots to form the response difference sequence.

9. The mobile robot path tracking method based on online slip identification and successive linearized model predictive control according to claim 8, characterized in that: S4 also includes: S43. Select one slip component from the slip parameters of the previous cycle in sequence, rewrite the slip component as the current storage code plus one and the current storage code minus one respectively, and keep the other slip components and the execution control sequence unchanged. Recalculate the predicted pose sequence from the recursive starting point of S42. Divide the difference in horizontal coordinate, vertical coordinate and heading in the same frame of the two sets of recalculated predicted poses by two respectively, and continue to form the corresponding parameter change column according to the control frame order. Then, splice the parameter change columns according to the slip component order to form the parameter change matrix. S44. Calculate the sum of squares of each column element in the parameter change matrix and select the first column in descending order of the sum of squares. Use Householder reflection to make the elements below the diagonal position of the first column zero and update the remaining columns simultaneously. Repeat column selection and reflection to form the column permutation matrix, orthogonal matrix, and upper triangular R matrix. Multiply the transpose of the orthogonal matrix by the response difference sequence to form the back substitution vector. Starting from the last row of the upper triangular R matrix, if the diagonal element of the current row is not zero, subtract the sum of the product of the calculated slip correction and the corresponding upper triangular element from the back substitution vector of the current row, and then divide by the diagonal element of the current row to obtain the slip correction of the current row. If the diagonal element of the current row is zero, write the slip correction of the current row to zero and move to the previous row until all rows are back substituted. Back substitute row by row and restore the slip component order according to the column permutation matrix. Multiply each slip correction by the quantization step size of the slip parameter between adjacent storage codes of the corresponding slip component to form the parameter correction value, and add the parameter correction value to the corresponding slip parameter of the previous cycle to form the current slip parameter.

10. The mobile robot path tracking method based on online slip identification and successive linearized model predictive control according to claim 9, characterized in that: S5 includes: S51. Replace the slip parameters of the previous cycle with the current slip parameters, update the forward speed and heading speed according to the effective wheel speed calculation relationship, take the actual pose at the end of this control cycle as the starting point of the recursion, and re-form the predicted pose, reference pose, position difference and heading difference according to the step-by-step linear recursion relationship. S52. Write the position difference and heading difference into the single-shot value, keep the left and right wheel control increment square terms, the step-by-step linear recursion relationship and the actuator control code interval unchanged, re-form the quadratic term matrix and the linear term vector, and obtain the path tracking control sequence of the next control cycle according to the block LDL transpose decomposition and interval endpoint fixing rules. S53. The edge computing node writes the path tracking control sequence to the mobile robot actuator according to the control signal through process control, executes the two-dimensional position or heading control of the predetermined trajectory, and writes the current sliding parameter to the previous cycle sliding parameter position of the next control cycle at the end of the current control cycle, and writes the actual pose at the end to the recursive starting point of the next control cycle.

Images