Deformation wheel type mobile robot path planning method

Abstract

The invention relates to a path planning method for a transformable wheel type mobile robot. The method is based on an ESDF map, abstracts a deformation moving wheeled robot into a geometric model, searches an initial path of the robot through a mixed A * algorithm, performs branch collision of the mixed A * algorithm through incremental collision detection, and then converts the initial path into a B spline curve containing position and deformation degree information of the robot along with time through fitting. And then calculating and optimizing motion feasibility and motion smoothness between control points of the B-spline curve and distance information of a geometric model to an obstacle. The invention aims to provide a smoother and safer path for the transformable wheel type mobile robot, a lightweight search process is realized, and the robot can quickly complete a path planning task on an unknown map.

Description

Technical Field

[0001] The invention relates to the technical field of path planning, and in particular to a path planning method for a deformable wheeled mobile robot. Background Art

[0002] With the development of science and technology, robotics technology is also progressing rapidly, and there are mature application cases in many fields. Different from the previous human-computer interaction mode or simple automation, the current application of robots is developing in the direction of full automation to improve work efficiency and reduce the degree of work danger. Among them, the path planning of robots plays an important role in automation. Whether it is a household robot or an industrial robot, path planning is indispensable. People's interest in the research of deformable mobile robots is gradually deepening, but the application of such robots is usually still in the prospective stage, or a lot of resources are required to achieve the expected goals. How to use deformable mobile robots is a problem that needs to be solved at present. Summary of the invention

[0003] In view of this, the purpose of the present invention is to provide a path planning method for a deformable wheeled mobile robot, which lightweights the search process and enables the robot to quickly complete path planning tasks in unknown maps, thereby compensating for the incompatibility of traditional path search algorithms for deformable mobile robots.

[0004] To achieve the above object, the present invention adopts the following technical solution: a path planning method for a deformable wheeled mobile robot, specifically comprising the following steps:

[0005] Step S1: The indoor three-dimensional environment is converted into an ESDF map, and the physical model of the deformable wheeled mobile robot is abstracted into a geometric model, which is dynamically adjusted with the deformation state of the robot; by combining the degree of deformation of the deformable mobile robot, an incremental collision detection method is used to perform branch collision detection, thereby performing an improved hybrid A* algorithm for path search;

[0006] Step S2: discretely sample the path points of the improved hybrid A* algorithm and fit them into a B-spline curve, which contains the position and deformation degree information of the robot over time;

[0007] Step S3: Taking the obtained B-spline curve as the optimization object, the path is optimized in terms of path smoothness, feasibility of motion speed, and safe distance between the geometric model of the deformable robot and obstacles.

[0008] In a preferred embodiment, in step S1, the hybrid A* algorithm robot is improved in the map model expression:

[0009] The indoor 3D environment is converted into a 3D ESDF grid map, where each grid not only stores information about whether it is occupied by an obstacle, but also stores information about the distance from the grid to the nearest obstacle in the unoccupied grid;

[0010] The real physical model of the deformable wheeled mobile robot is abstracted into a geometric model, which can truly reflect the position information, direction information and deformation degree information of the deformable wheeled mobile robot, and can always reflect the real-time state of the real deformable mobile robot. The center of the geometric model of the deformable mobile robot is projected on the map ground as the base point for the improved hybrid A* algorithm search.

[0011] In a preferred embodiment, the branch search method of the improved hybrid A* algorithm in step S1 is:

[0012] Based on the kinematics of the robot, a fixed time step is used as the search resolution to search the path. At the same time, in the hybrid A* algorithm, each fixed time step is additionally limited by acceleration, angular acceleration, and deformation acceleration to ensure the smoothness of the path and the feasibility of the motion. The heuristic function for judging each branch is:

[0013] f(n)=g(n)+h(n)

[0014] g(n)=|Δh|+λ1*Δt+g(n-1)

[0015] h(n)=λ1*t est

[0016] Where f(n) represents the cost estimation function of the deformable wheeled mobile robot from the initial state to the target state through the change of state n;

[0017] g(n) represents the actual cost of the deformable mobile robot from the initial state to state n; |Δh| is the absolute value of the difference in deformation between the current state n and the previous state n-1, h is the deformation degree of the robot, Δt is the time difference between the current state n and the previous state n-1, and g(n-1) represents the cumulative cost of the robot from the initial state to the previous state n-1;

[0018] h(n) is the best estimated cost from state n to the target state, and the time t for the robot to reach the end point is estimated by the Dubins curve est Multiply by the adjustment coefficient λ1, the adjustment coefficient λ1 adjusts the influence of time on the cost estimation function. The larger the λ1 value, the better the optimality of the resulting path time. Conversely, the smaller the λ1 value, the better the path deformation smoothness.

[0019] In a preferred embodiment, the collision detection of branches in the hybrid A* algorithm is improved in step S1:

[0020] The branch collision detection process is to detect multiple states in each search branch one by one. Specifically, in each state of the deformable mobile robot, the volume difference between the geometric model of the robot in the current state and the geometric model of the previous state is calculated, and at the same time, it is detected whether the newly added volume difference collides with the obstacle.

[0021] In a preferred embodiment, the B-spline fitting method in step S2 is:

[0022] The initial path calculated by the improved hybrid A* algorithm in step S1 is sampled discretely to obtain discrete points of the path. Then, the discrete points are fitted with a B-spline curve based on the initial state, terminal state, speed and acceleration limits of the geometric model of the deformable mobile robot to obtain a k-order B-spline curve. The expression of the curve function B(t) is as follows:

[0023]

[0024] Where P i is the control point. The information of each control point contains {x, y, z, h}, where x, y, z are the coordinates of the robot, and N i,k+1 (t) is the basis function of the B-spline curve;

[0025] The first-order derivative of the curve function with respect to time obtains the velocity information of the deformable wheeled mobile robot, and the second-order derivative obtains the acceleration information of the deformable mobile robot.

[0026] In a preferred embodiment, the optimization method of step S3 is:

[0027] According to the control points of the B-spline curve fitted in step S2, there are N+1 control points in total, the k control points at the beginning and the end are kept unchanged, and the N+1-2k control points in the middle are adjusted;

[0028] For the deformable mobile robot, the overall penalty function f for path optimization is total for:

[0029] f total =λ2f s +λ3f c +λ4f v

[0030] λ2λ3λ4 are the adjustment coefficients of each penalty function.

[0031] f s is the path smoothness penalty function, which makes the path smoother by constraining the change amplitude between the control points of the path, so as to meet the requirements of the smoothness of the robot motion. Its equation is:

[0032]

[0033] P i+1 Indicates the next control point.

[0034] f c is the penalty function of the robot's distance from obstacles, which is used to improve the safety of the path. Its equation is:

[0035]

[0036] D(P i ) reflects the robot at the control point P i The distance information from the obstacle is obtained through the control point P i The geometric model obtained in step S1 is filled with M points selected inside the geometric model. The relative positions of these filling points in the geometric model remain unchanged, which can fully reflect the distribution characteristics of the geometric model in space. The coordinates of the filling points are taken as p j , i=1,2,…,M, the shortest distance between each point and the obstacle is d(p j ), then the robot is at the control point P i The information about the nearest obstacle can be normalized as

[0037]

[0038] Therefore, the penalty function considers both the robot’s position adjustment and deformation adjustment to reduce the collision risk;

[0039] f v is a speed feasibility penalty function, which is used to constrain the speed of the path control point. When the speed of a control point exceeds the preset speed limit, f v The function value of will increase significantly, thus guiding the path optimization to adjust the equation in the direction of satisfying the speed limit:

[0040]

[0041] v is the moving speed of the robot, v h is the deformation speed of the robot, V iμ The robot is at the control point P i The movement speed or deformation speed of the

[0042] Therefore, the goal of path optimization for the deformable mobile robot is to adjust the values ​​of the control points {x, y, z, h} of the path so that the overall penalty function f total Reach the minimum value; calculate each control point P i For the penalty function f totalThe gradient direction is used as the basis for optimization and adjustment, thereby generating an optimized path that meets the requirements of path smoothness, obstacle safety and speed feasibility;

[0043] Adjust the values ​​of coefficients λ2λ3λ4 according to actual requirements. To make the path smoother, increase the proportion of coefficient λ2. To enhance the safety of the path, increase the proportion of coefficient λ3. To make the path more consistent with the kinematic laws of the robot, adjust the proportion of coefficient λ4.

[0044] Compared with the prior art, the present invention has the following beneficial effects:

[0045] (i) The present invention abstracts the physical model of the deformable wheeled mobile robot into a geometric model to improve the accuracy and reliability of the robot's expression in the map. Through this modeling method, the position, direction and morphological characteristics of the robot in different deformation states can be more accurately reflected, thereby optimizing the accuracy and effectiveness of path planning;

[0046] (ii) Using an incremental collision detection method and considering the robot's deformation degree each time the path is calculated can improve the accuracy and real-time performance of collision detection. This method can more accurately evaluate the relative position of the robot and obstacles in different deformation states, ensure that the robot does not collide with obstacles during the deformation process, and improve the safety of the robot.

[0047] (III) When optimizing the path, the path smoothness, speed feasibility, and safe distance between the robot and obstacles are comprehensively optimized to make the path not only smoother, but also to ensure that the robot's movement speed is within an acceptable range and that the path does not collide with obstacles. This comprehensive consideration allows the robot to perform tasks in more complex environments and effectively avoids conflicts and problems that may arise from single-target optimization. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 It is a schematic diagram of the planning of a preferred embodiment of the present invention;

[0049] Figure 2 It is a schematic diagram of a physical model of a modified mobile robot in a preferred embodiment of the present invention being abstracted into a geometric model;

[0050] Figure 3 is a schematic diagram of collision detection in a preferred embodiment of the present invention;

[0051] Figure 4 is a schematic diagram of path optimization in a preferred embodiment of the present invention, wherein: Figure 4 (a) is a schematic diagram of position adjustment path optimization. Figure 4 (b) is a schematic diagram of deformation degree adjustment path optimization;

[0052] Figure 5 Schematic diagram of search and optimization of a preferred embodiment of the present invention, wherein (a) is a state before the search begins, wherein the green grid represents the obstacle grid observed by the camera in real time, and the 8 red points represent the geometric model of the robot; (b) is a stage at the beginning of the search, wherein the red path is the optimized B-spline curve, that is, the local path; (c) is the robot executing the subsequent local path after a period of time; and (d) is the situation of passing through a narrow section. DETAILED DESCRIPTION

[0053] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0054] It should be noted that the following detailed descriptions are illustrative and are intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present application belongs.

[0055] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application; as used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or their combinations.

[0056] like Figure 1-5 As shown, the path planning of a deformable wheeled mobile robot described in this embodiment: Step S1, converting the indoor three-dimensional environment into an ESDF map, and abstracting the physical model of the deformable wheeled mobile robot into a geometric model, which can be dynamically adjusted with the deformation state of the robot. By combining the degree of deformation of the deformable mobile robot, an incremental collision detection method is used to perform branch collision detection, thereby performing an improved hybrid A* algorithm for path search.

[0057] Step S2, discretely sampling the path points of the improved hybrid A* algorithm and fitting them into a B-spline curve, which contains the position and deformation degree information of the robot over time.

[0058] Step S3, taking the obtained B-spline curve as the optimization object, optimizing the path in terms of path smoothness, feasibility of motion speed, and safe distance between the geometric model of the deformable robot and obstacles.

[0059] Step S1 specifically includes:

[0060] Improved hybrid A* algorithm robot's expression in map model:

[0061] The indoor 3D environment is converted into a 3D ESDF grid map, where each grid not only stores information about whether it is occupied by an obstacle, but also stores information about the distance between the grid and the nearest obstacle in the unoccupied grid;

[0062] The real physical model of the deformable wheeled mobile robot is abstracted into a geometric model, such as Figure 2 As shown, the geometric model can truly reflect the position information, direction information and deformation degree information of the deformable wheeled mobile robot, and can always reflect the real-time state of the real deformable mobile robot. The center of the obtained geometric model of the deformable mobile robot is projected on the map ground as the base point for the improved hybrid A* algorithm search;

[0063] Improve the branch search method of the hybrid A* algorithm:

[0064] Based on the kinematics of the robot, a fixed time step is used as the search resolution to search the path. At the same time, in this algorithm, each fixed time step is additionally limited by acceleration, angular acceleration, and deformation acceleration to ensure the smoothness of the path and the feasibility of the motion. The heuristic function for judging each branch is:

[0065] f(n)=g(n)+h(n)

[0066] g(n)=|Δh|+λ1*Δt+g(n-1)

[0067] h(n)=λ1*t est

[0068] Among them, f(n) represents the cost estimation function of the deformable wheeled mobile robot from the initial state to the target state through the change of state n.

[0069] g(n) represents the actual cost of the deformable mobile robot from the initial state to state n; |Δh| is the absolute value of the difference in deformation degree between the current state n and the previous state n-1, h is the deformation degree of the robot, Δt is the time difference between the current state n and the previous state n-1, and g(n-1) represents the cumulative cost of the robot from the initial state to the previous state n-1.

[0070] h(n) is the best estimated cost from state n to the target state, and the time t for the robot to reach the end point is estimated by the Dubins curve est Multiply by the adjustment coefficient λ1, which can adjust the influence of time on the cost estimation function. The larger the λ1 value, the better the optimality of the resulting path time. Conversely, the smaller the λ1 value, the better the path deformation smoothness.

[0071] Improved collision detection for branches in the hybrid A* algorithm:

[0072] The branch collision detection process is to detect multiple states in each search branch one by one, such as Figure 3 As shown, specifically: in each state of the deformable mobile robot, the volume difference of the geometric model of the robot in the current state compared with the geometric model of the previous state is calculated, and at the same time, it is detected whether the newly added volume difference collides with the obstacle. In the corresponding figure, state 2 only needs to detect the red grid additionally relative to state 1;

[0073] Step S2 specifically includes:

[0074] The initial path calculated by the improved hybrid A* algorithm in step S1 is sampled discretely to obtain discrete points of the path. Then, the discrete points are fitted with a B-spline curve based on the initial state, terminal state, speed and acceleration limits of the geometric model of the deformable mobile robot to obtain a k-order B-spline curve. The expression of the curve function is as follows:

[0075]

[0076] Where is the control point, and the information of each control point contains {x, y, z, h}, where x, y, z are the coordinates of the robot, h is the degree of deformation of the robot, and N i,k+1 (t) is the basis function of the B-spline curve;

[0077] The first-order derivative of the curve function with respect to time can obtain the velocity information of the deformable wheeled mobile robot, and the second-order derivative can obtain the acceleration information of the deformable mobile robot;

[0078] Step S3 specifically includes:

[0079] According to the control points of the B-spline curve fitted in step S2, there are N+1 control points in total, the k control points at the beginning and the end are kept unchanged, and the N+1-2k control points in the middle are adjusted;

[0080] For the deformable mobile robot, the overall penalty function f for path optimization is total for:

[0081] f total =λ2f s +λ3f c +λ4f v

[0082] λ2λ3λ4 are the adjustment coefficients of each penalty function.

[0083] f s is the path smoothness penalty function, which makes the path smoother by constraining the change amplitude between the control points of the path, so as to meet the requirements of the smoothness of the robot motion. Its equation is:

[0084]

[0085] P i+1 Indicates the next control point.

[0086] f c is the penalty function of the robot's distance from obstacles, which is used to improve the safety of the path. Its equation is:

[0087]

[0088] D(P i ) reflects the robot's position at the control point P i The distance information from the obstacle is obtained through the control point P i The geometric model obtained in step S1 is filled with M points selected inside the geometric model. The relative positions of these filling points in the geometric model remain unchanged, which can fully reflect the distribution characteristics of the geometric model in space. The coordinates of the filling points are taken as p j , i = 1, 2, ..., M, the shortest distance between each point and the obstacle is d(p j ), then the robot is at the control point P i The information about the nearest obstacle can be normalized as

[0089]

[0090] Therefore, the penalty function takes into account both the robot's position adjustment and deformation adjustment to reduce the collision risk, such as Figure 4 As shown, Figure 4 (a) is a schematic diagram of position adjustment path optimization. Figure 4 (b) is a schematic diagram of deformation degree adjustment path optimization.

[0091] f v is a speed feasibility penalty function, which is used to constrain the speed of the path control point. When the speed of a control point exceeds the preset speed limit, f v The function value of will increase significantly, thus guiding the path optimization to adjust the equation in the direction of satisfying the speed limit:

[0092]

[0093] v is the moving speed of the robot, v h is the deformation speed of the robot, V iμ The robot is at the control point P i The movement speed or deformation speed of the

[0094] Therefore, the goal of path optimization for the deformable mobile robot is to adjust the values ​​of the control points {x, y, z, h} of the path so that the overall penalty function ftotal Calculate each control point P i For the penalty function f total The gradient direction is used as the basis for optimization and adjustment, thereby generating an optimized path that meets the requirements of path smoothness, obstacle safety and speed feasibility. Figure 5 For the overall process, Figure 5 (a) is the state before the search begins, where the green grid represents the obstacle grid observed by the camera in real time, and the 8 red points represent the geometric model of the robot; Figure 5 (b) is the initial search stage, where the red path is the optimized B-spline curve, that is, the local path; Figure 5 (c) is the robot executing the subsequent local path after a period of time; Figure 5 (d) is the case of passing through a narrow section. It can be clearly seen that Figure 5 (a) Difference in deformation degree.

[0095] The values ​​of the coefficients λ2λ3λ4 can be reasonably adjusted according to actual requirements. To make the path smoother, the proportion of the λ2 coefficient needs to be increased; to enhance the safety performance of the path, the proportion of the λ3 coefficient needs to be increased; to make the path more consistent with the kinematic laws of the robot, the proportion of the λ4 coefficient needs to be adjusted.

[0096] In summary, according to steps S1, S2, and S3, the path of the deformable mobile robot can be optimized based on the improved hybrid A* algorithm, B-spline curve, and the three objectives of path smoothness, movement speed feasibility, and the safe distance between the geometric model of the deformable robot and obstacles, thereby obtaining a path planning method that can be applied to a deformable wheeled mobile robot.

Claims

1. A path planning method for a deformable wheeled mobile robot, characterized in that: The specific steps include: Step S1: convert the indoor three-dimensional environment into an ESDF map, and abstract the physical model of the deformable wheeled mobile robot into a geometric model, which is dynamically adjusted according to the deformation state of the robot; By combining the degree of deformation of the deformable mobile robot, an incremental collision detection method is used to perform branch collision detection, thereby performing an improved hybrid A* algorithm for path search; Step S2: discretely sample the path points of the improved hybrid A* algorithm and fit them into a B-spline curve, which contains the position and deformation degree information of the robot over time; Step S3: Taking the obtained B-spline curve as the optimization object, the path is optimized in terms of path smoothness, feasibility of motion speed, and safe distance between the geometric model of the deformable robot and obstacles.

2. A path planning method for a deformable wheeled mobile robot according to claim 1, characterized in that: Improve the hybrid A* algorithm robot's expression in the map model in step S1: The indoor three-dimensional environment is converted into a three-dimensional ESDF grid map, in which each grid stores information about whether it is occupied by an obstacle and the distance between the grid and the nearest obstacle in the unoccupied grid. The real physical model of the deformable wheeled mobile robot is abstracted into a geometric model, which can truly reflect the position information, direction information and deformation degree information of the deformable wheeled mobile robot, and can always reflect the real-time state of the real deformable mobile robot. The center of the geometric model of the deformable mobile robot is projected on the ground of the map as the base point for the improved hybrid A* algorithm search.

3. A path planning method for a deformable wheeled mobile robot according to claim 1, characterized in that: The branch search method of the improved hybrid A* algorithm in step S1 is: Based on the kinematics of the robot, a fixed time step is used as the search resolution to search the path. At the same time, in the hybrid A* algorithm, each fixed time step is additionally limited by acceleration, angular acceleration, and deformation acceleration to ensure the smoothness of the path and the feasibility of the motion. The heuristic function for judging each branch is: f(n)=g(n)+h(n) g(n)=|Δh|+λ1*Δt+g(n-1) h(n)=λ1*t est Where f(n) represents the cost estimation function of the deformable wheeled mobile robot from the initial state to the target state through the change of state n; g(n) represents the actual cost of the deformable mobile robot from the initial state to state n; |Δh| is the absolute value of the difference in deformation between the current state n and the previous state n-1, h is the deformation degree of the robot, Δt is the time difference between the current state n and the previous state n-1, and g(n-1) represents the cumulative cost of the robot from the initial state to the previous state n-1; h(n) is the best estimated cost from state n to the target state, and the time t for the robot to reach the end point is estimated by the Dubins curve est Multiply by the adjustment coefficient λ1, the adjustment coefficient λ1 adjusts the influence of time on the cost estimation function. The larger the λ1 value, the better the optimality of the resulting path time. Conversely, the smaller the λ1 value, the better the path deformation smoothness.

4. A path planning method for a deformable wheeled mobile robot according to claim 1, characterized in that: Improved collision detection of branches in the hybrid A* algorithm in step S1: The branch collision detection process is to detect multiple states in each search branch one by one. Specifically, in each state of the deformable mobile robot, the volume difference between the geometric model of the robot in the current state and the geometric model of the previous state is calculated, and at the same time, it is detected whether the newly added volume difference collides with the obstacle.

5. A path planning method for a deformable wheeled mobile robot according to claim 1, characterized in that: B-spline fitting method in step S2: The initial path calculated by the improved hybrid A* algorithm in step S1 is sampled discretely to obtain discrete points of the path. Then, the discrete points are fitted with a B-spline curve based on the initial state, terminal state, speed and acceleration limits of the geometric model of the deformable mobile robot to obtain a k-order B-spline curve. The expression of the curve function B(t) is as follows: Where P i is the control point. The information of each control point contains {x, y, z, h}, where x, y, z are the coordinates of the robot, and N i,k+1 (t) is the basis function of the B-spline curve; The first-order derivative of the curve function with respect to time obtains the velocity information of the deformable wheeled mobile robot, and the second-order derivative obtains the acceleration information of the deformable mobile robot.

6. A path planning method for a deformable wheeled mobile robot according to claim 1, characterized in that: Optimization method of step S3: According to the control points of the B-spline curve fitted in step S2, there are N+1 control points in total, the k control points at the beginning and the end are kept unchanged, and the N+1-2k control points in the middle are adjusted; For the deformable mobile robot, the overall penalty function f for path optimization is total for: f total =λ2f s +λ3f c +λ4f v λ2λ3λ4 are the adjustment coefficients of each penalty function; f s is the path smoothness penalty function, which makes the path smoother by constraining the change range between the control points of the path, so as to meet the requirements of the robot's motion smoothness. Its equation is: P i+1 Indicates the next control point; f c is the penalty function of the robot's distance from obstacles, which is used to improve the safety of the path. Its equation is: D(P i ) reflects the robot at the control point P i The distance information from the obstacle is obtained through the control point P i The geometric model obtained in step S1 is filled with M points selected inside the geometric model. The relative positions of these filling points in the geometric model remain unchanged, which can fully reflect the distribution characteristics of the geometric model in space. The coordinates of the filling points are taken as p j , j = 1, 2, ..., M, the shortest distance between each point and the obstacle is d(p j ), then the robot is at the control point P i The information about the nearest obstacle can be normalized as Therefore, the penalty function considers both the robot’s position adjustment and deformation adjustment to reduce the collision risk; f v is a speed feasibility penalty function, which is used to constrain the speed of the path control point. When the speed of a control point exceeds the preset speed limit, f v The function value of will increase significantly, thus guiding the path optimization to adjust the equation in the direction of satisfying the speed limit: v is the moving speed of the robot, v h is the deformation speed of the robot, V iμ The robot is at the control point P i The speed of movement or deformation; Therefore, the goal of path optimization for the deformable mobile robot is to adjust the values ​​of the control points {x, y, z, h} of the path so that the overall penalty function f total Reach a minimum value; Calculate each control point P i For the penalty function f total The gradient direction is used as the basis for optimization and adjustment, thereby generating an optimized path that meets the requirements of path smoothness, obstacle safety and speed feasibility; Adjust the values ​​of coefficients λ2λ3λ4 according to actual requirements. To make the path smoother, increase the proportion of coefficient λ2. To enhance the safety performance of the path, the ratio of the λ3 coefficient needs to be increased. To make the path more consistent with the kinematic laws of the robot, the ratio of the λ4 coefficient needs to be adjusted.

Images