A Trajectory Optimization Method for Spherical Robots

Abstract

The present invention discloses a method for optimizing the trajectory of a spherical robot, which comprises the following steps: S1. Obtain target state information from the global path; obtain the roll angle response, and the current coordinates and attitude angles of the spherical robot from the IMU; S2. Initialize the prediction parameters; S3. Use the dynamic model to obtain the predicted state; S4. Use the kinematic model to obtain the predicted trajectory; S5. Calculate the error using the error function. If the error is less than the specified error, or the loop has been greater than or equal to the running duration threshold, stop the iteration and enter step S6. If the error is greater than or equal to the specified error and the loop time is less than the running duration threshold, update the prediction parameters with IPOPT and then re-enter step S3; S6. Send the attitude control parameters to the attitude controller. This solution can guide the spherical robot to always track the global path with a stable attitude, and has good stability and robustness for long-term operation.

Description

Technical Field

[0001] The present invention relates to the field of spherical robot control, and more particularly to a method for optimizing the trajectory of a spherical robot. Background Art

[0002] The spherical robot involved in the present invention is as Figure 1 shown. It can be simplified into two components: a circular spherical shell and a two-degree-of-freedom compound pendulum. The two-degree-of-freedom compound pendulum can be divided into two parts: a compound pendulum and a frame with two motors. The pendulum is driven by two motors and rotates around the horizontal and longitudinal axes of the robot. The horizontal (long) axis motor is used to control the speed of the spherical robot, and the longitudinal (short) axis motor controls the direction by changing the contact point between the spherical shell and the ground. In this way, by controlling the two motors, the spherical robot can achieve omnidirectional motion with a non-zero turning radius on a plane. Therefore, compared with wheeled and legged robots, the spherical robot has unique kinematic and dynamic characteristics. The schematic diagram of the spherical robot during movement is as Figure 2 shown. The spherical robot has characteristics such as zero turning radius and impact resistance, and has unique advantages in emergency rescue and inspection. Currently, there are no open-source papers or patents on local trajectory optimization algorithms for spherical robots. Transplanting the general TEB algorithm onto the spherical robot by simply modifying the parameters does not yield good results. Summary of the Invention

[0003] The present invention mainly solves the technical problem existing in the prior art of lacking a suitable method for optimizing the trajectory of a spherical robot, and provides a method for optimizing the trajectory of a spherical robot with good stability, low resource requirements, and high robustness.

[0004] The present invention mainly solves the above technical problems through the following technical solutions: A method for optimizing the trajectory of a spherical robot, comprising the following steps:

[0005] S1. Obtain target state information from the global path, where the target state information includes the coordinates (x goal , y goal ) and the attitude angle φ goal of the target; obtain the roll angle response from the inertial measurement unit (IMU) (including the roll angle response θ 0 at the current moment and the roll angle response θ -1 at the previous moment) and the current coordinates (x 0 , y 0 ) and the attitude angle φ 0 of the spherical robot; obtain the distance between the spherical robot and the obstacle from the distance field map;

[0006] S2. Initialize the prediction parameters, where the prediction parameters include v 1 , …, v nand θ set ; The initialization method is to randomly take values within the limit range, and the limit range is the range of v set and θ set described below;

[0007] S3. Use the kinetic model to obtain the predicted state;

[0008] S4. Use the kinematic model to obtain the predicted trajectory;

[0009] S5. Calculate the error using the error function. If the error is less than the specified error (usually set to 80 - 120), or the loop has been greater than or equal to the running duration threshold, then stop the iteration and enter step S6. If the error is greater than or equal to the specified error and the loop time is less than the running duration threshold, then update the predicted parameters using the nonlinear optimization solver (IPOPT) and re-enter step S3;

[0010] S6. Send the attitude control parameters to the attitude controller.

[0011] The nonlinear optimization solver uses the interior point method to update the predicted parameters. The value of n is determined according to actual needs, generally 15 - 25.

[0012] Preferably, the input of the kinetic model is the predicted parameters (v 1 , …, v n and θ set ), the roll angle response θ 0 at the current moment, and the roll angle response θ -1 at the previous moment. The specific formula is as follows:

[0013] θ k - 0.9182θ k-1 + 0.002835θ k-2 = - 0.3165θ 0 + 0.3695θ -1 n≥k≥1

[0014] The output of the kinetic model is the predicted state (v 1 , θ 1 ), (v 2 , θ 2 ), …, (v n , θ n ), that is, the states of the spherical robot at the subsequent n time points obtained by prediction.

[0015] Preferably, the calculation process of the kinematic model is as follows:

[0016]

[0017] In the formula, when s = 0, (x0 , y 0 ), where \((x, y)\) is the current coordinate of the spherical robot, and \(\varphi\) 0 is the current attitude angle of the spherical robot. The output of the kinematic model is the predicted trajectory \((x 1 , y 1 , \varphi 1 ), \((x 2 , y 2 , \varphi 2 ), \(\cdots\), \((x n , y n , \varphi n ), that is, the trajectories of the spherical robot at the subsequent \(n\) time points predicted later, including the coordinates \((x, y)\) and the attitude angle \(\varphi\). \(dt\) is the time difference between two adjacent sampling times.

[0018] Preferably, the error is calculated as follows:

[0019]

[0020]

[0021]

[0022] \(J\) is the error value. In the formula, \(H 1 , \(H 2 , \(H 3 and \(H 4 are coefficient matrices, \((x goal , y goal ) is the target coordinate of the spherical robot, \(\varphi goal is the target attitude angle of the spherical robot, \(v last and \(\theta last are the attitude control parameters sent to the attitude controller last time, and ESDF is the Euclidean signed distance field.

[0023] Preferably, in step S6, the attitude control parameters include \(v set and \(\theta set , where \(v set is the last \(v 1 ; the limit of \(v set is \([-0.6m / s, 0.6m / s]\), and the limit of \(\theta set is \([-15^{\circ}, 15^{\circ}]\). If the attitude control parameters are outside the limits, the attitude control parameters are set to the nearest limit edge value.

[0024] Preferably, the coefficient matrices are:

[0025] \(H1 = DIAG(3000, 3000, 2000)\), \(H2 = DIAG(30000, 1000)\), \(H3 = 1000\), \(H4 = 100\).

[0026] The substantial effect brought by the present invention is that the spherical robot can complete the challenge path with a more stable attitude and a higher success rate, thereby increasing the working range of the robot and the quality of sensor data. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 is a schematic structural diagram of the spherical robot;

[0028] Figure 2 and Figure 3 is a schematic diagram of the movement process of the spherical robot;

[0029] Figure 4 is a flowchart of a method for optimizing the trajectory of a spherical robot according to the present invention;

[0030] Figure 5 is the trajectory result of the S-bend experiment. The left side is the trajectory of the global planner, and the right side is the trajectory of this solution;

[0031] Figure 6 is the speed curve of the S-bend experiment;

[0032] Figure 7 is the roll angle curve of the S-bend experiment. DETAILED DESCRIPTION OF THE INVENTION

[0033] The technical solution of the present invention will be further specifically described below through embodiments and in conjunction with the accompanying drawings.

[0034] Embodiment: According to the relative motion relationship of the components of the spherical robot, the spherical robot can be simplified into three components: a heavy pendulum, a skeleton, and a shell, and it is modeled using the Lagrangian method.

[0035] The input of the kinematic model is (v, θ) of the robot, and the output is (x, y, ) of the robot. As Figure 2 shown, due to the limitation of the mechanical structure, the robot has a non-zero turning radius R = r / tanθ determined by θ of the robot. From Figure 3 it can be seen that the change of is actually the rotation angle of the turning radius, which can be calculated based on the turning radius and the moving distance, and the positive and negative signs depend on whether it is a left turn or a right turn.

[0036] A method for optimizing the trajectory of a spherical robot, as Figure 4 shown, includes the following steps:

[0037] S1. Obtain target state information from the global path, and the target state information includes the coordinates (x goal , y goal ) of the target and the attitude angle φgoal ; Obtain the roll angle response from the inertial measurement unit (IMU), including the roll angle response θ at the current moment 0 and the roll angle response θ at the previous moment -1 ), and the current coordinates (x 0 , y 0 ) and the attitude angle φ 0 ; Obtain the distance between the spherical robot and the obstacle from the distance field map;

[0038] S2. Initialize the prediction parameters, where the prediction parameters include v 1 , …, v n and θ set ;

[0039] S3. Use the dynamic model to obtain the predicted state;

[0040] S4. Use the kinematic model to obtain the predicted trajectory;

[0041] S5. Calculate the error using the error function. If the error is less than the specified error (usually set to 80 - 120), or the loop has been greater than or equal to the running duration threshold, then stop the iteration and enter step S6. If the error is greater than or equal to the specified error and the loop time is less than the running duration threshold, then update the prediction parameters using the nonlinear optimization solver (IPOPT) and re - enter step S3;

[0042] S6. Send the attitude control parameters to the attitude controller.

[0043] The nonlinear optimization solver uses the interior - point method to update the prediction parameters. The value of n is determined according to actual needs, generally 15 - 25.

[0044] The input of the dynamic model is the prediction parameters (v 1 , …, v n and θ set ), the roll angle response θ at the current moment 0 and the roll angle response θ at the previous moment -1 , and the specific formula is as follows:

[0045] θ k - 0.9182θ k-1 + 0.002835θ k-2 = - 0.3165θ 0 + 0.3695θ -1 n ≥ k ≥ 1

[0046] The output of the dynamic model is the predicted state (v 1 , θ 1 ), (v 2 , θ 2 ), …, (vn , θ n ), that is, the states of the spherical robot at the subsequent n time points obtained by prediction.

[0047] The calculation process of the kinematic model is as follows:

[0048]

[0049] In the formula, when s = 0, (x 0 , y 0 ) is the current coordinate of the spherical robot, φ 0 is the current attitude angle of the spherical robot, and the output of the kinematic model is the predicted trajectory (x 1 , y 1 , φ 1 ), (x 2 , y 2 , φ 2 ), …, (x n , y n , φ n ), that is, the trajectories of the spherical robot at the subsequent n time points predicted later, including the coordinates (x, y) and the attitude angle φ. dt is the time difference between two adjacent sampling times.

[0050] The specific calculation method of the error is:

[0051]

[0052]

[0053]

[0054] J is the error value. In the formula, H 1 , H 2 , H 3 and H 4 are coefficient matrices, (x goal , y goal ) is the target coordinate of the spherical robot, φ goal is the target attitude angle of the spherical robot, v last and θ last are the attitude control parameters sent to the attitude controller last time, and ESDF is the Euclidean signed distance field. Since the spherical robot is easily affected by external interference, when the target is too close to the obstacle, it needs to move away from the obstacle along the direction with the fastest gradient descent on the local ESDF map to make its value on the ESDF map less than the safety value.

[0055] In step S6, the attitude control parameters include v set and θ set , where v set is the last v1 ; v set has a limit of [-0.6 m / s, 0.6 m / s], and θ set has a limit of [-15°, 15°]. If the attitude control parameter is outside the limit, the attitude control parameter will be set to the closest limit edge value.

[0056] The coefficient matrix is:

[0057] H1 = DIAG(3000, 3000, 2000), H2 = DIAG(30000, 1000), H3 = 1000, H4 = 100. The first term in the error function makes the end state of the forward prediction tend to the target state, rather than requiring all local trajectories to be close to the global path, which gives the local motion planner more possibilities to better optimize the trajectory. The second and third terms are the limitations on the command change rate, which can make the command smoother and the attitude of the robot more stable. At the same time, the cost value of the ESDF map is added as a soft constraint to keep the robot away from obstacles.

[0058] Through the S-curve experiment, the performance of the spherical robot during the challenging continuous turning process was compared under the use of the global planner and the local planner of this scheme. Then, a long path with 10 path points similar to the real patrol scenario was used to compare the long-term stability and robustness of this scheme. The evaluation indexes include execution time, running distance, average speed, and the attitude stability index defined specifically for the spherical robot.

[0059] The global planner uses the hybrid A* algorithm, and the look-ahead distance of the global path is 2 meters. The global path planning frequency is 1 Hz, and the local planning frequency is 10 Hz. Each group of experiments is repeated 5 times to reduce randomness.

[0060] From Figure 5 it can be seen that this scheme is superior to the global trajectory planner in all indexes. This scheme can guide the robot to complete the global path with a shorter trajectory and keep the speed of the robot at 0.6 m / s.

[0061] It can also be seen that under the control of this scheme, both the swing range (reduced by 30%) and the swing speed (reduced by 15%) of the spherical robot have decreased. From Figure 7 the rolling angle curve in it, it can be clearly seen the improvement of the attitude stability of the spherical robot. The command issued by the local planner can make the rolling angle of the robot within the convergence range during the turning process, which can improve the data quality of sensors such as radar and camera on the robot.

[0062] Research shows that this solution can guide the spherical robot to complete this task. When a new target point is given, the change of the global path will not have a significant impact on the motion performance of the spherical robot. It can be seen from the trajectory that this solution can guide the spherical robot to always track the global path in a stable posture, with good stability and robustness for long-term operation. The speed curve ( Figure 6 the thick line in) and the exponent also prove this advantage. The roll angle is also relatively stable throughout the process, and the fluctuation converges quickly at 60 seconds. The slight increase in the roll angle exponent should be due to the increase in path complexity. That is, this solution enables the spherical robot to complete multi-target detection tasks in complex scenarios.

[0063] The specific embodiments described in this article are merely illustrative of the spirit of the present invention. Those skilled in the art to which the present invention pertains can make various modifications or supplements to the described specific embodiments or use similar ways to replace them, but will not deviate from the spirit of the present invention or exceed the scope defined by the appended claims.

[0064] Although terms such as prediction parameters, dynamic models, and kinematic models are used more frequently in this article, the possibility of using other terms is not excluded. The use of these terms is only to more conveniently describe and explain the essence of the present invention; interpreting them as any additional limitation is contrary to the spirit of the present invention.

Claims

1. A spherical robot trajectory optimization method, characterized in that, it includes the following steps: S1. Obtain target state information from the global path, where the target state information includes the coordinates and attitude angles of the target; obtain the roll angle response, and the current coordinates and attitude angles of the spherical robot from the inertial measurement unit; obtain the distance between the spherical robot and the obstacle from the distance field map; S2. Initialize the prediction parameters, where the prediction parameters include v 1 , …, v n and θ set ; S3. Use the dynamic model to obtain the predicted state; S4. Use the kinematic model to obtain the predicted trajectory; S5. Use the error function to calculate the error. If the error is less than the specified error, or the loop has been greater than or equal to the running time threshold, stop the iteration and enter step S6. If the error is greater than or equal to the specified error and the loop time is less than the running time threshold, update the prediction parameters using the nonlinear optimization solver and then re-enter step S3; S6. Send the attitude control parameters to the attitude controller; The input of the kinetic model is the prediction parameter, the roll angle response θ at the current moment 0 and the roll angle response θ at the previous moment -1 , and the specific formula is as follows: θ k -0.9182θ k-1 +0.002835θ k-2 = -0.3165θ 0 +0.3695θ -1 n≥k≥1 The output of the kinetic model is the predicted states (v 1 , θ 1 ), (v 2 , θ 2 ), …, (v n , θ n ).

2. A spherical robot trajectory optimization method according to claim 1, characterized in that, the calculation process of the kinematic model is as follows: where, when s = 0, (x 0 , y 0 ) is the current coordinate of the spherical robot, φ 0 is the current attitude angle of the spherical robot, and the output of the kinematic model is the predicted trajectory (x 1 , y 1 , φ 1 ), (x 2 , y 2 , φ 2 ), …, (x n , y n , φ n ).

3. A spherical robot trajectory optimization method according to claim 2, characterized in that, the specific calculation method of the error is: J is the error value, where H1, H2, H3, and H4 are coefficient matrices, (x goal , y goal ) is the target coordinate of the spherical robot, φ goal is the target attitude angle of the spherical robot, v last and θ last are the attitude control parameters sent to the attitude controller last time, and ESDF is the Euclidean signed distance field.

4. A spherical robot trajectory optimization method according to claim 3, characterized in that, In step S6, the attitude control parameters include v set and θ set , where v set is the last v 1 ; the limit of v set is [-0.6 m / s, 0.6 m / s], and the limit of θ set is [-15°, 15°]. If the attitude control parameters are outside the limits, the attitude control parameters are set to the limit edge value closest thereto.

5. A spherical robot trajectory optimization method according to claim 3 or 4, characterized in that, the coefficient matrix is: H1 = DIAG(3000, 3000, 2000), H2 = DIAG(30000, 1000), H3 = 1000, H4 = 100.

Images