Control system and control method for reducing blind area of sensor during turning of robot

Abstract

The invention relates to a control system and a control method for reducing a blind area of a sensor during turning of a robot, and the system comprises a laser radar which is disposed at the top of the robot and is used for detecting an obstacle, and two sides of the laser radar are respectively provided with a left-side ultrasonic radar rotating holder and a right-side ultrasonic radar rotating holder. A left ultrasonic radar is arranged on the left ultrasonic radar rotating holder; a right ultrasonic radar is arranged on the right ultrasonic radar rotating holder; the ultrasonic radar holder drives the ultrasonic radar to rotate in the horizontal direction; the left and right ultrasonic radars are used for compensating the blind area of the laser radar. According to the control system and method for reducing the blind area of the sensor during robot turning, it is ensured that the robot finds obstacles in time, and the situation that the safety of the robot is affected by collision caused by the blind area is avoided.

Description

Technical Field

[0001] The present invention relates to the field of robots, and in particular to a control system and a control method for reducing a blind area of ​​a sensor when the robot turns. Background Art

[0002] Small wheeled robots generally use a lidar installed on the top of the body to sense obstacles around them. Due to the limitation of the lidar's detection range, the robot may not be able to sense obstacles in front of the robot that are beyond the detection range. Especially when the machine is turning at a high rate, due to the existence of the lidar's blind spot, the robot has a high probability of colliding with obstacles, seriously affecting the robot's safety. Summary of the invention

[0003] In view of the above-mentioned deficiencies in the prior art, the present invention provides a system and method for reducing the blind spots of sensors when the robot turns, which ensures that the robot can detect obstacles in time and avoids collisions caused by blind spots that affect the safety of the robot.

[0004] The technical solution adopted by the present invention is: A control system for reducing the blind area of ​​a sensor when a robot turns, comprising: The laser radar for detecting obstacles is installed on the top of the robot. The left ultrasonic radar rotating platform and the right ultrasonic radar rotating platform are respectively installed on both sides of the laser radar. The left ultrasonic radar is installed on the left ultrasonic radar rotating platform; the right ultrasonic radar is installed on the right ultrasonic radar rotating platform; the ultrasonic radar platform drives the ultrasonic radar to rotate in the horizontal direction; the left and right ultrasonic radars are used to make up for the blind spots of the laser radar; It also includes a left ultrasonic radar rotation angle calculation module, a right ultrasonic radar rotation angle calculation module and a trajectory calculation module. The trajectory calculation module outputs the trajectory that the robot will travel according to the detection results of the laser radar and the left / right ultrasonic radar, and the robot follows the trajectory output by the trajectory calculation module; The left and right ultrasonic radar rotation angle calculation modules receive the trajectory output by the trajectory calculation module, and calculate the rotation angles of the left and right ultrasonic radars respectively according to the curvature information of the trajectory.

[0005] Preferably, a grid map is established according to the environment in which the robot operates, and a search algorithm is used to generate a global path from the robot operation start point to the end point; The local path planning is generated using a spatial and temporal separation approach; At the spatial level, the RRT algorithm is used to randomly sample points in the space from the starting point of the local path to generate expansion nodes and build a tree structure that approaches the end point of the local path; The rough path searched by the above RRT algorithm is smoothed using an optimization method. The smoothing process considers the heading difference between the local path and the global path, the distance between the local path and the global path, and the distance between the local path and the obstacle. The cost function is written as: , in, is the heading difference between the ith local path point and the global path, is the distance difference between the ith local path point and the global path, is the distance difference between the ith local path point and the obstacle; α, β, and γ correspond to the weight coefficient of the heading cost, the weight coefficient of the distance cost between the local path and the global path, and the weight coefficient of the local path and the obstacle cost, respectively; G is a function that represents the cumulative cost of a path from the starting point to the current node, and is usually used to measure the "goodness" of a path. The smaller the G of a path, the better the path.

[0006] The velocity of the local path is smoothed using a cubic polynomial at the time level. The form of the cubic polynomial is: , is the position function, which indicates the position of the robot at time t; according to the length of the local path and the cruising speed of the robot , get the time T required for the robot to walk through the local path; set the position boundary conditions , , velocity boundary condition , , acceleration boundary condition , , solve the cubic polynomial to obtain a smooth velocity curve; a0: initial position, i.e. the position at t=0; a1: initial velocity, i.e. velocity at t=0; a2: half of the initial acceleration; a3: One sixth of the initial jerk.

[0007] Combining the local path with the above speed profile can obtain the local trajectory.

[0008] Preferably, the left and right ultrasonic radar rotation angle calculation modules determine the rotation angle of the ultrasonic radar according to the following rules; , It is stipulated that the curvature of the trajectory is positive when the robot turns left along the trajectory, and the curvature of the trajectory is negative when the robot turns right along the trajectory; k1 and k2 are two curvature thresholds corresponding to the left-turn trajectory, which are configured as needed; when the trajectory curvature k ∈ [0, k1], the rotation angle of the ultrasonic radar is 0 degrees, when the trajectory curvature k ∈ (k1, k2], the rotation angle of the ultrasonic radar is c1 * k degrees, where c1 is the rotation coefficient of the left-turn trajectory, and when k > k2, the rotation angle of the ultrasonic radar is 0 degrees; k3 and k4 are two curvature thresholds corresponding to the right-turn trajectory. When the trajectory curvature k ∈ [k3, 0], the rotation angle of the ultrasonic radar is 0 degrees. When the trajectory curvature k ∈ [k4, k3), the rotation angle of the ultrasonic radar is c2 * k degrees, where c2 is the rotation coefficient of the right-turn trajectory. When k < k4, the rotation angle of the ultrasonic radar is 0 degrees.

[0009] Preferably, determine the rotation direction and angle of the ultrasonic radar; including the following control steps: Step s100, start; Step s101, the trajectory calculation module outputs the trajectory; Step s102, judge the positive and negative of the trajectory curvature. If the curvature is positive, jump to step s103, otherwise jump to step s108; Step s103, judge whether the curvature k is in the interval [0, k1]; if k ∈ [0, k1], jump to step s105; otherwise jump to step s104; Step s104, judge whether the curvature k satisfies k > k2; if the curvature k > k2, jump to step s105, otherwise jump to s106; Step s105, the rotation angle is 0 degrees; jump to step s113; Step s106, the rotation angle is c1 * k degrees; jump to step s107; Step s107, the left ultrasonic radar pan-tilt drives the left ultrasonic radar to rotate; jump to step s113; Step s108, judge whether the curvature k is in the interval [k3, 0]; if k ∈ [k3, 0], jump to step s110; otherwise jump to step s109; Step s109, judge whether the curvature k satisfies k4 > k; if the curvature k4 > k, jump to step s110, otherwise jump to s111; Step s110, the rotation angle is 0 degrees; jump to step s113; Step s111, the rotation angle is c2 * k degrees; jump to step s112; Step s112, the right ultrasonic radar pan-tilt drives the right ultrasonic radar to rotate; jump to step s113; Step s113, end.

[0010] Preferably, including the following control steps: Start; Step s200, obtaining the current position, speed and heading of the robot; Step s201, determining the rotation direction and angle of the ultrasonic radar; Step s202, obtaining the position, speed and direction of obstacle 1 and obstacle 2; Step s203, calculate the relative speed; if the robot The speed is , is the robot's center of mass position Velocity vector in the earth coordinate system; obstacle 1 The speed is , is the center of mass position of obstacle 1 Velocity vector in the geodetic coordinate system, obstacle 2 The speed is , is the center of mass position of obstacle 2 The velocity vector in the earth coordinate system, then the velocity of obstacle 1 relative to the robot is , the speed of obstacle 2 relative to the robot is ; is the speed of obstacle 2 in the X direction; is the speed of obstacle 2 in the Y direction; Step s204, calculate the obstacle avoidance distance; if the robot's position is , the position of obstacle 1 is , the distance of obstacle 2 is , then the distance between obstacle 1 and the robot is , the distance between obstacle 2 and the robot is ; is the robot's x-coordinate; is the robot's y-coordinate; Step s205, determining whether the obstacle is within the avoidance range; Step s206, path planning; Step s207, calculating a new turning radius R; Step s208, calculate the new turning angle ; Step s209, adjusting the turning trajectory to avoid obstacles; Step s210, controlling the robot to turn; Step s211, real-time monitoring of obstacle movement trajectory; Step s212, update the path planning and jump to step s205; Step s213, continue driving along the current path; jump to step s214; Step s214, real-time feedback and path adjustment; Step s215, determine whether the path needs to be adjusted; if not, jump to step s211; if it needs to be adjusted, jump to step s216; Step s216, performing path adjustment; Step s217, the path adjustment is completed and the robot continues to drive; Finish.

[0011] Step s201, determining the rotation direction and angle of the ultrasonic radar; including the following control steps: Step s400, start; Step s401, the trajectory calculation module outputs the trajectory; Step s402, determine whether the curvature of the trajectory is positive or negative, if the curvature is positive, jump to step s403, otherwise jump to step s408; Step s403, determine whether the curvature k is in the interval [0, k1]; if k∈[0, k1], jump to step s405; otherwise, jump to step s404; Step s404, determine whether the curvature k satisfies k>k2; if the curvature k>k2, jump to step s405, otherwise jump to s406; Step s405, the rotation angle is 0 degrees; jump to step s413; Step s406, the rotation angle is c1*k degrees; jump to step s407; Step s407, the left ultrasonic radar pan / tilt drives the left ultrasonic radar to rotate; jump to step s413; Step s408, determine whether the curvature k is in the interval [k3, 0]; if k∈[k3, 0], jump to step s410; otherwise, jump to step s409; Step s409, determine whether the curvature k satisfies k4>k; if the curvature k4>k, jump to step s410, otherwise jump to s411; Step s410, the rotation angle is 0 degrees; jump to step s413; Step s411, the rotation angle is c2*k degrees; jump to step s412; Step s412, the right ultrasonic radar pan / tilt drives the right ultrasonic radar to rotate; jump to step s413; Step s413, end.

[0012] Preferably, the following control steps are included: start; Step s300, obtaining the current position, speed and heading of the robot; Step s301, determining the rotation direction and angle of the ultrasonic radar; Step s302, obtaining the position, speed and direction of obstacle 1 and obstacle 2; Step s303, calculating the relative speed; Step s304, calculating obstacle avoidance distance; Step s305, determining whether the obstacle is within the avoidance range; Step s306, path planning; Step s307, calculating a new turning radius R; Step s308, calculate the new turning angle ; Step s309, adjusting the turning trajectory to avoid obstacles; Step s310, controlling the robot to turn; Step s311, smoothing the speed of the local path; Step s312, real-time monitoring of obstacle movement trajectory; Step s313, update the path planning and jump to step s305; Step s314, continue driving along the current path; Step s315, real-time feedback and path adjustment; Step s316, determine whether the path needs to be adjusted; if not, jump to step s312; if it needs to be adjusted, jump to step s317; Step s317, perform path adjustment; re-call the RRT algorithm or dynamic window method; Step s318, the path adjustment is completed and the robot continues to travel; Finish.

[0013] The beneficial effects of the present invention compared with the prior art are as follows: The present invention reduces the blind spot of the robot sensor when turning. The trajectory calculation module outputs the trajectory that the robot will travel according to the detection results of the laser radar and the left / right ultrasonic radar, and the robot follows the trajectory output by the trajectory calculation module. The left and right ultrasonic radar rotation angle calculation modules receive the trajectory output by the trajectory calculation module, and respectively calculate the rotation angles of the left and right ultrasonic radars according to the curvature information of the trajectory, so as to ensure that the robot can detect obstacles in time and avoid collisions caused by blind spots that affect the safety of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 is a control flow chart of a control method for reducing a blind area of ​​a sensor when a robot turns according to the present invention; Figure 2 is a control flow chart of Embodiment 2 of a control method for reducing a blind area of ​​a sensor when a robot turns according to the present invention; Figure 3 is a control flow chart of Example 4 of a control method for reducing a blind area of ​​a sensor when a robot turns according to the present invention; Figure 4 is a control flow chart of Example 3 of a control method for reducing a blind area of ​​a sensor when a robot turns according to the present invention; Figure 5 It is a schematic diagram of the detection environment of the laser sensor and the ultrasonic sensor of the control system of the present invention for reducing the blind area of ​​the sensor when the robot turns; Figure 6 It is a schematic diagram of the laser sensor and ultrasonic sensor detecting obstacles when the robot turns to the left of the control system of the present invention to reduce the blind area of ​​the sensor when the robot turns; Figure 7 It is a schematic diagram of the control system of the present invention for reducing the blind area of ​​the sensor when the robot turns left, in which the laser sensor and the ultrasonic sensor detect obstacles after the ultrasonic sensor rotates a certain angle when the robot turns left; Figure 8 It is a schematic diagram of the laser sensor and ultrasonic sensor detecting obstacles when the robot turns right, which is a control system for reducing the blind area of ​​the sensor when the robot turns right; Fig. 9 It is a schematic diagram of the control system of the present invention for reducing the blind area of ​​the sensor when the robot turns. When the robot turns right, the ultrasonic sensor rotates a certain angle and then the laser sensor and the ultrasonic sensor detect obstacles. Fig.10 The present invention is a control system for reducing the blind area of ​​the sensor when the robot turns. The obstacle is a moving object. The laser sensor and the ultrasonic sensor detect obstacles when the robot turns right. Fig.11 The present invention is a control system for reducing the blind spot of the sensor when the robot turns. The obstacle is a moving object. When the robot turns right, the ultrasonic sensor rotates a certain angle and then the laser sensor and the ultrasonic sensor detect the obstacle; In the figure: 1. Left ultrasonic radar 2. Right ultrasonic radar 3. LiDAR 4. Left ultrasonic radar detection range 5. Right ultrasonic radar detection range 6. LiDAR detection range. DETAILED DESCRIPTION

[0015] The present invention is described in detail below with reference to the accompanying drawings and embodiments: See attached Figures 5 to 11 It can be seen that a control system for reducing the blind area of ​​the sensor when the robot turns includes: The laser radar for detecting obstacles is installed on the top of the robot. The left ultrasonic radar rotating platform and the right ultrasonic radar rotating platform are respectively installed on both sides of the laser radar. The left ultrasonic radar is installed on the left ultrasonic radar rotating platform; the right ultrasonic radar is installed on the right ultrasonic radar rotating platform; the ultrasonic radar platform drives the ultrasonic radar to rotate in the horizontal direction; the left and right ultrasonic radars are used to make up for the blind spots of the laser radar; It also includes a left ultrasonic radar rotation angle calculation module, a right ultrasonic radar rotation angle calculation module and a trajectory calculation module. The trajectory calculation module outputs the trajectory that the robot will travel according to the detection results of the laser radar and the left / right ultrasonic radar, and the robot follows the trajectory output by the trajectory calculation module; The left and right ultrasonic radar rotation angle calculation modules receive the trajectory output by the trajectory calculation module, and calculate the rotation angles of the left and right ultrasonic radars respectively according to the curvature information of the trajectory.

[0016] The trajectory evaluation function is used to quantitatively evaluate the degree to which a trajectory conforms to our expectations, similar to scoring. What characteristics do we want the robot's trajectory to have, such as the heading being as small as possible compared to the global path, the acceleration being as small as possible, and the distance from obstacles being as far as possible. If there are other requirements, they can also be added to the evaluation function.

[0017] Preferably, a grid map is established according to the environment in which the robot operates, and a search algorithm is used to generate a global path from the robot operation start point to the end point; The local path planning is generated using a spatial and temporal separation approach; The RRT (Rapidly-exploring Random Tree) algorithm is used at the spatial level. Starting from the starting point of the local path, points in the random sampling space are generated to expand nodes, and a tree structure is constructed to approach the end point of the local path. The selection of expansion nodes takes into account the following aspects: 1. Whether there is a collision with the obstacle perceived in real time; 2. The distance from the global path. The end point of the local path is obtained by previewing along the global path according to the current speed of the robot. The spatial path search ends when the extended node approaches the end point.

[0018] The rough path searched by the above RRT algorithm is smoothed using an optimization method. The smoothing process considers the heading difference between the local path and the global path, the distance between the local path and the global path, and the distance between the local path and the obstacle. The cost function is written as: , in, is the heading difference between the ith local path point and the global path, is the distance difference between the ith local path point and the global path, is the distance difference between the ith local path point and the obstacle; α, β, and γ correspond to the weight coefficient of the heading cost, the weight coefficient of the distance cost between the local path and the global path, and the weight coefficient of the local path and the obstacle cost, respectively; G is a function that represents the cumulative cost of a path from the starting point to the current node, and is usually used to measure the "goodness" of a path. The smaller the G of a path, the better the path.

[0019] The trajectory evaluation function can be selected according to actual needs, for example, the robot's heading is required to be as close to the global path heading as possible, the acceleration is as small as possible, and the distance from obstacles is as far as possible. The trajectory evaluation function is used to quantitatively determine the degree of conformity of a trajectory with our expectations. The weight in the trajectory evaluation function is used to quantitatively describe the importance we attach to a certain evaluation indicator. The larger the weight, the more we value a certain indicator. The core of the present invention is a system and method for reducing the blind spot of the sensor when the robot turns. The algorithm used to generate the trajectory cannot reduce the blind spot of the sensor, and can be some kind of general trajectory generation algorithm.

[0020] The cost used to select the optimal trajectory. The higher the cost, the worse the trajectory. The greater the weight of a cost, the more attention is paid to the compliance of the cost in the process of selecting the optimal trajectory.

[0021] The cost is used to quantitatively evaluate how well a trajectory matches our expectations, similar to a score.

[0022] The robot's motion trajectory is generally required to be consistent with the global path heading, with the acceleration as small as possible and to maintain a safe distance from obstacles. The trajectory evaluation function is used to evaluate the quality of a trajectory. You can choose the evaluation function according to your needs.

[0023] The velocity of the local path is smoothed using a cubic polynomial at the time level. The form of the cubic polynomial is: , is the position function, which represents the position of the robot at time t; according to the length of the local path and the cruising speed of the robot , get the time T required for the robot to walk through the local path; set the position boundary conditions , , velocity boundary condition , , acceleration boundary condition , , solve the cubic polynomial to obtain a smooth velocity curve; Combining the local path with the above speed curve can get the local trajectory; The optimal trajectory is determined according to the evaluation function, and a trajectory with the minimum cost determined by the evaluation function is selected.

[0024] Preferably, the left and right ultrasonic radar rotation angle calculation modules determine the rotation angle of the ultrasonic radar according to the following rules; , Wherein, it is stipulated that the curvature of the trajectory is positive when the robot turns left along the trajectory, and the curvature of the trajectory is negative when the robot turns right along the trajectory; k1 and k2 are respectively two curvature thresholds corresponding to the left-turn trajectory. When the trajectory curvature k ∈ [0, k1], the rotation angle of the ultrasonic radar is 0 degrees. When the trajectory curvature k ∈ (k1, k2], the rotation angle of the ultrasonic radar is c1 * k degrees, where c1 is the rotation coefficient of the left-turn trajectory. When k > k2, the rotation angle of the ultrasonic radar is 0 degrees; K1 and k2 are determined according to experiments, which are two thresholds, one large and one small, corresponding to the left-turn trajectory (the right-turn trajectory is k3, k4). The purpose is to segment the curvature, and different rotation angles are used in different segments.

[0025] k3 and k4 are respectively two curvature thresholds corresponding to the right-turn trajectory. When the trajectory curvature k ∈ [k3, 0], the rotation angle of the ultrasonic radar is 0 degrees. When the trajectory curvature k ∈ [k4, k3), the rotation angle of the ultrasonic radar is c2 * k degrees, where c2 is the rotation coefficient of the right-turn trajectory. When k < k4, the rotation angle of the ultrasonic radar is 0 degrees.

[0026] When the trajectory curvature is small, the detection range of the ultrasonic radar can still cover the trajectory points that the robot will pass through in the future for a period of time. The significance of rotating the ultrasonic radar is not great, so it is set to 0°. After the trajectory curvature is greater than a certain value, the detection range of the ultrasonic radar cannot completely cover the trajectory points that the robot will pass through. Therefore, it is necessary to rotate a certain angle to cover more trajectory points. Here, it rotates a certain angle linearly with the trajectory curvature. When the trajectory curvature is too large, if it continues to rotate linearly as above, a very large angle may be rotated, resulting in a sensor blind area in the opposite direction of rotation. Moreover, the robot generally decelerates when turning at a large curvature. When the speed is small, there is more time to detect obstacles, and the role of rotation is not so great. Therefore, the rotation angle is set to 0° here.

[0027] Appendix Figure 1 , a control method for reducing the sensor blind area when a robot turns, including the following control steps: Step s100, start; Step s101, the trajectory calculation module outputs the trajectory; Step s102, judge the positive or negative of the trajectory curvature. If the curvature is positive, jump to step s103, otherwise jump to step s108; Step s103, determine whether the curvature k is in the interval [0, k1]; if k∈[0, k1], jump to step s105; otherwise, jump to step s104; Step s104, determine whether the curvature k satisfies k>k2; if the curvature k>k2, jump to step s105, otherwise jump to s106; Step s105, the rotation angle is 0 degrees; jump to step s113; Step s106, the rotation angle is c1*k degrees; jump to step s107; Step s107, the left ultrasonic radar pan / tilt drives the left ultrasonic radar to rotate; jump to step s113; Step s108, determine whether the curvature k is in the interval [k3, 0]; if k∈[k3, 0], jump to step s110; otherwise, jump to step s109; Step s109, determine whether the curvature k satisfies k4>k; if the curvature k4>k, jump to step s110, otherwise jump to s111; Step s110, the rotation angle is 0 degrees; jump to step s113; Step s111, the rotation angle is c2*k degrees; jump to step s112; Step s112, the right ultrasonic radar pan / tilt drives the right ultrasonic radar to rotate; jump to step s113; Step s113, end. Example 1

[0028] Attached Figure 5 The small wheeled robot uses a laser radar to detect obstacles in front of it, and an ultrasonic radar is installed on the left and right front of the robot to reduce the blind spot of the laser radar. Even so, when the robot makes a large curvature turn, the obstacle may still be in the robot's perception blind spot, and the robot will risk colliding with the obstacle if it continues to drive.

[0029] The weight coefficients of the trajectory evaluation function are selected as α=0.1, β=0.2, and γ=0.5. The size of the weight reflects the aspects that are more emphasized in the selection of the trajectory.

[0030] α, β, and γ are weight coefficients for the heading difference, the distance difference between the local path and the global path, and the distance difference between the local path and the obstacle, respectively. The general range of values ​​is 0<α<1, 0<β<1, and 0<γ<1. A larger α value will tend to keep the path in the direction of the global path and avoid large heading angle changes in the path. If the task requires that the path must follow the global planning path, α can be appropriately increased. If the heading requirement is not high, the α value can be reduced. A larger β value will cause the local path to follow the global path more closely, thus making the path smoother. However, if the value is too large, it may cause the local path to be too restrained in some areas and fail to fully explore other possible paths. Therefore, the selection of β needs to be adjusted according to the quality of the global path and the need for smoothness. γ controls the safety of the local path, especially the distance to obstacles. If the presence of obstacles is critical to the task (for example, the mobile robot must avoid obstacles), a larger value should be given to γ. If the path is more important and the obstacles are relatively far away or some approach can be tolerated, γ can be appropriately reduced.

[0031] like Figure 6 As shown in the figure, there are obstacles obs1 and obs2 in front of the robot and to the left of the robot respectively. Obs1 is within the detection range of the robot's sensor, and obs2 is in the blind spot of the robot's sensor. The trajectory calculation module calculates the trajectory according to the weight coefficients selected above, which bypasses obs1 but does not bypass obs2. If the robot drives along this trajectory, there is a risk of collision with obs2 or sudden braking.

[0032] Curvature k Describes the degree of curvature of the path and is usually defined as:

[0033] in, R It is the radius of curvature of the path, that is, the bending radius of a certain section of the path, which reflects the degree of turning of the path. The larger R is, the smaller the curvature is and the flatter the path is. The smaller R is, the greater the curvature is and the path is sharply curved.

[0034] The trajectory calculated by the trajectory calculation module (curvature changes over time) is shown in the following table:

[0035] The "trajectory rotation coefficient" in the trajectory calculation module usually refers to a parameter that describes the degree of trajectory turning or rotation in path planning. It describes the rate at which the curvature changes with the path parameters over time, that is:

[0036] The value of the trajectory rotation coefficient can be any positive number, indicating the sharpness of the path turn. Its value depends on the rate of change of the trajectory curvature. The greater the curvature or the faster the change, the greater the rotation coefficient.

[0037] The curvature threshold represents the maximum allowable curvature value on the path, that is k max , and the curvature threshold is set according to the actual situation of the robot and the test requirements to prevent the robot from becoming unstable due to excessive curvature.

[0038] Let the curvature thresholds of the left and right turn trajectories be k1 = 0.1, k2 = 0.5, k3 = -0.1, k4 = -0.5, and the rotation coefficients of the left and right turn trajectories be c1 = 20, c2 = 20.

[0039] When determining the curvature k and trajectory rotation coefficient c After that, in order to smooth the speed curve of the robot, ensure that the movement of the robot is more stable, avoid sudden changes in speed or sharp changes in turning, so as to improve the comfort and stability of the robot during movement, a cubic polynomial is used to smooth the speed of the local path:

[0040] During the control process, the cubic polynomial smoothing will continuously optimize the movement speed of the robot, avoid sudden acceleration changes during turning or obstacle avoidance, so as to ensure that the robot can drive smoothly and accurately along the planned path.

[0041] For a trajectory with a certain curvature, how many degrees the radar rotates to cover the obstacles in the front blind area, and thus the rotation coefficient under a certain curvature is determined. By analogy, the rotation coefficients under other curvatures can be determined.

[0042] The larger the coefficient, the more sensitive the rotation of the ultrasonic radar is to the magnitude of the curvature. It can be understood that the larger the coefficient, the larger the rotation angle corresponding to the same curvature.

[0043] Select the curvature k = 0.25 (>0, positive) of the trajectory at t = 1.0s. According to Figure 2 the execution process, the left ultrasonic radar rotation angle calculation module needs to calculate the rotation angle and drive the left ultrasonic radar to rotate.

[0044] According to the calculation formula in Table 1, the trajectory curvature k1 (=0.1) < k (=0.25) ≤ k2 (=0.5). At this time, the rotation angle θ = c1 * k = 20 * 0.25 = 5 degrees.

[0045] Such as Figure 7As shown, after the left ultrasonic radar rotates 5°, obstacles obs1 and obs2 are both within the detection range of the sensor. The trajectory calculation module then calculates a new trajectory based on the detected obstacles obs1 and obs2, which can bypass obstacles obs1 and obs2 at the same time.

[0046] Attached Figure 2 , Embodiment 2, obstacle 1 and obstacle 2 are moving objects, including the following control steps: start; Step s200, obtaining the current position, speed and heading of the robot; Step s201, determining the rotation direction and angle of the ultrasonic radar; Step s202, obtaining the position, speed and direction of obstacle 1 and obstacle 2; Step s203, calculate the relative speed; if the robot The speed is , is the robot's center of mass position Velocity vector in the geodetic coordinate system; is the robot's speed in the X direction; is the robot's speed in the Y direction; Obstacle 1 The speed is , is the center of mass position of obstacle 1 Velocity vector in the geodetic coordinate system, obstacle 2 The speed is , is the center of mass position of obstacle 2 The velocity vector in the earth coordinate system, then the velocity of obstacle 1 relative to the robot is , the speed of obstacle 2 relative to the robot is ; is the speed of obstacle 2 in the X direction; is the speed of obstacle 2 in the Y direction; Step s204, calculate the obstacle avoidance distance; if the robot's position is , the position of obstacle 1 is , the distance of obstacle 2 is , then the distance between obstacle 1 and the robot is , the distance between obstacle 2 and the robot is ; is the robot's x-coordinate; is the robot's y-coordinate; Step s205, determining whether the obstacle is within the avoidance range; Step s206, path planning; Step s207, calculating a new turning radius R; Step s208, calculate the new turning angle ; Step s209, adjusting the turning trajectory to avoid obstacles; Step s210, controlling the robot to turn; Step s211, real-time monitoring of obstacle movement trajectory; Step s212, update the path planning and jump to step s205; Step s213, continue driving along the current path; jump to step s214; Step s214, real-time feedback and path adjustment; Step s215, determine whether the path needs to be adjusted; if not, jump to step s211; if it needs to be adjusted, jump to step s216; Step s216, performing path adjustment; Step s217, the path adjustment is completed and the robot continues to drive; Finish.

[0047] Attached Figure 3 , Embodiment 3, obstacle 1 and obstacle 2 are moving objects, including the following control steps: start; Step s300, obtaining the current position, speed and heading of the robot; Step s301, determining the rotation direction and angle of the ultrasonic radar; Step s302, obtaining the position, speed and direction of obstacle 1 and obstacle 2; Step s303, calculating the relative speed; Step s304, calculating obstacle avoidance distance; Step s305, determining whether the obstacle is within the avoidance range; Step s306, path planning; Step s307, calculating a new turning radius R; Step s308, calculate the new turning angle ; Step s309, adjusting the turning trajectory to avoid obstacles; Step s310, controlling the robot to turn; Step s311, smoothing the speed of the local path; Step s312, real-time monitoring of the obstacle movement trajectory; Step s313, update the path planning and jump to step s305; Step s314, continue driving along the current path; Step s315, real-time feedback and path adjustment; Step s316, determine whether the path needs to be adjusted; if not, jump to step s312; if it needs to be adjusted, jump to step s317; Step s317, perform path adjustment; re-call the RRT algorithm or dynamic window method; Step s318, the path adjustment is completed and the robot continues to drive; Finish.

[0048] Attached Figure 4 , Embodiment 4, step s201, determining the rotation direction and angle of the ultrasonic radar, includes the following control steps: Step s400, start; Step s401, the trajectory calculation module outputs the trajectory; Step s402, determine whether the curvature of the trajectory is positive or negative, if the curvature is positive, jump to step s403, otherwise jump to step s408; Step s403, determine whether the curvature k is in the interval [0, k1]; if k∈[0, k1], jump to step s405; otherwise, jump to step s404; Step s404, determine whether the curvature k satisfies k>k2; if the curvature k>k2, jump to step s405, otherwise jump to s406; Step s405, the rotation angle is 0 degrees; jump to step s413; Step s406, the rotation angle is c1*k degrees; jump to step s407; Step s407, the left ultrasonic radar pan / tilt drives the left ultrasonic radar to rotate; jump to step s413; Step s408, determine whether the curvature k is in the interval [k3, 0]; if k∈[k3, 0], jump to step s410; otherwise, jump to step s409; Step s409, determine whether the curvature k satisfies k4>k; if the curvature k4>k, jump to step s410, otherwise jump to s411; Step s410, the rotation angle is 0 degrees; jump to step s413; Step s411, the rotation angle is c2*k degrees; jump to step s412; Step s412, the right ultrasonic radar pan / tilt drives the right ultrasonic radar to rotate; jump to step s413; Step s413, end.

[0049] The present invention provides a system and method for reducing the blind area of ​​a sensor when a robot turns, thereby ensuring that the robot can detect obstacles in time and avoiding collisions caused by the blind area that affect the safety of the robot.

[0050] The flowchart describes in detail how the robot adjusts the path and smoothes the speed in real time according to the dynamic position, speed and direction of movement of the moving obstacle when it encounters a moving obstacle. The whole process starts with obtaining the status information of the robot and the obstacle. The robot obtains its own current position, speed and orientation, as well as the relevant information of the surrounding obstacles (such as position, speed and orientation) through sensors. Based on this information, the robot calculates the relative speed of the obstacle to itself and determines whether it enters the avoidance range. If the obstacle is within the avoidance range, the robot will use the obstacle avoidance algorithm (such as RRT or A*) to replan the path, calculate the new turning radius and angle, and ensure that it avoids collision with the obstacle. Next, the robot calculates the new turning trajectory and uses a cubic polynomial to smooth the speed of the local path in the process, thereby avoiding sudden acceleration or deceleration and making the movement smoother. Subsequently, the robot controls the steering according to the adjusted path, monitors the movement of the obstacle in real time, and dynamically adjusts the path to adapt to environmental changes. If the obstacle is not within the avoidance range, the robot continues to drive along the current path. Feedback and adjustments are continuously made throughout the process to ensure that the robot can safely and smoothly avoid obstacles and successfully complete the task, until the path adjustment is completed and the robot can continue to drive along the planned path.

[0051] The weight coefficients of the trajectory evaluation function are selected as α=0.1, β=0.2, and γ=0.5. The size of the weight reflects the aspects that are more emphasized in the selection of the trajectory.

[0052] like Figure 8 As shown, there are obstacles obs3 and obs4 in front of the robot and to the left of the robot, respectively. Obs3 is within the detection range of the robot's sensor, and obs4 is in the robot's sensor blind spot. The trajectory calculation module calculates the trajectory that bypasses obs3 but does not bypass obs4 based on the weight coefficients selected above. If the robot drives along this trajectory, there is a risk of collision with obs4 or sudden braking.

[0053] The trajectory calculated by the trajectory calculation module (curvature changes over time) is shown in the following table:

[0054] Assume that the curvature thresholds of the left and right turn trajectories are k1=0.1, k2=0.5, k3=-0.1, k4=-0.5, and the rotation coefficients of the left and right turn trajectories are c1=20, c2=20.

[0055] Select the curvature k=-0.25 (<0, negative) of the trajectory at t=1.0s, according to Figure 2 It can be seen from the execution process that the right ultrasonic radar rotation angle calculation module needs to calculate the rotation angle and drive the right ultrasonic radar to rotate.

[0056] According to the calculation formula in Table 1, the trajectory curvature k4 (=-0.5) < k (=0.25) ≤ k3 (=-0.1). At this time, the rotation angle θ = c2 * k = 20 * (-0.25) = -5 degrees.

[0057] As Fig. 9 shown, after the right ultrasonic radar rotates -5°, both the obstacle obs3 and the obstacle obs4 are within the detection range of the sensor. The trajectory calculation module then calculates a new trajectory based on the detected obstacles obs3 and obs4, which can avoid both the obstacle obs3 and the obstacle obs4 simultaneously.

[0058] The above is only a preferred embodiment of the present invention, and does not impose any form of limitation on the structure of the present invention. Any simple modification, equivalent change, and modification made to the above embodiments based on the technical essence of the present invention all fall within the scope of the technical solution of the present invention.

Claims

1. A control system for reducing the blind area of ​​a robot sensor when turning, comprising: The laser radar for detecting obstacles is installed on the top of the robot. The left ultrasonic radar rotating platform and the right ultrasonic radar rotating platform are respectively installed on both sides of the laser radar. The left ultrasonic radar is installed on the left ultrasonic radar rotating platform; the right ultrasonic radar is installed on the right ultrasonic radar rotating platform; the ultrasonic radar platform drives the ultrasonic radar to rotate in the horizontal direction; the left and right ultrasonic radars are used to make up for the blind spots of the laser radar; It also includes a left ultrasonic radar rotation angle calculation module, a right ultrasonic radar rotation angle calculation module and a trajectory calculation module, which is characterized by: The trajectory calculation module outputs the trajectory that the robot will travel according to the detection results of the laser radar and the left / right ultrasonic radar, and the robot follows the trajectory output by the trajectory calculation module; The left and right ultrasonic radar rotation angle calculation modules receive the trajectory output by the trajectory calculation module, and calculate the rotation angles of the left and right ultrasonic radars respectively according to the curvature information of the trajectory.

2. The control method of the control system for reducing the blind area of ​​the sensor when the robot turns according to claim 1, characterized in that: A grid map is created based on the robot's operating environment, and a search algorithm is used to generate a global path from the robot's operating start point to the end point. The local path planning is generated using a spatial and temporal separation approach; At the spatial level, the RRT algorithm is used to randomly sample points in the space from the starting point of the local path to generate expansion nodes and build a tree structure that approaches the end point of the local path; The rough path searched by the above RRT algorithm is smoothed using an optimization method. The smoothing process considers the heading difference between the local path and the global path, the distance between the local path and the global path, and the distance between the local path and the obstacle. The cost function is written as: ， in, is the heading difference between the ith local path point and the global path, is the distance difference between the ith local path point and the global path, is the distance difference between the ith local path point and the obstacle; α, β, and γ correspond to the weight coefficient of the heading cost, the weight coefficient of the distance cost between the local path and the global path, and the weight coefficient of the local path and the obstacle cost, respectively; G represents the cumulative cost of a path from the starting point to the current node; The velocity of the local path is smoothed using a cubic polynomial at the time level. The form of the cubic polynomial is: ， is the position function, which indicates the position of the robot at time t; according to the length of the local path and the cruising speed of the robot , get the time T required for the robot to walk through the local path; set the position boundary conditions , , velocity boundary condition , , acceleration boundary condition , , solve the cubic polynomial to obtain a smooth velocity curve; Combining the local path with the above speed curve can get the local trajectory; a0: initial position, i.e. the position at t=0; a1: initial velocity, i.e. velocity at t=0; a2: half of the initial acceleration; a3: One sixth of the initial jerk.

3. The control method of the control system for reducing the blind area of ​​the sensor when the robot turns according to claim 2, characterized in that: The left and right ultrasonic radar rotation angle calculation modules determine the rotation angle of the ultrasonic radar according to the following rules; When the robot trajectory curvature is k∈[0,k1], the ultrasonic radar rotation angle is 0 degrees. When the trajectory curvature is k∈(k1,k2], the ultrasonic radar rotation angle is c1*k degrees, where c1 is the left turn trajectory rotation coefficient. When k>k2, the ultrasonic radar rotation angle is 0 degrees. k1 and k2 are two curvature thresholds corresponding to the left-turn trajectory, k2>k1; they are configured as needed; When the curvature k of the robot's trajectory belongs to [k3, 0], the rotation angle of the ultrasonic radar is 0 degrees. When the trajectory curvature k belongs to [k4, k3), the rotation angle of the ultrasonic radar is c2 * k degrees, where c2 is the rotation coefficient for the right-turn trajectory. When k < k4, the rotation angle of the ultrasonic radar is 0 degrees; k3 and k4 are two curvature thresholds corresponding to the right-turn trajectory, k3>k4; k 3 and k 4 are all negative values; Among them, it is stipulated that the curvature of the trajectory is positive when the robot turns left along the trajectory, and the curvature of the trajectory is negative when the robot turns right along the trajectory.

4. The control method of the control system for reducing the blind area of ​​the sensor when the robot turns according to claim 3 is characterized in that: Determining the rotation direction and angle of the ultrasonic radar includes the following control steps: Step s100, start; Step s101, the trajectory calculation module outputs the trajectory; Step s102, determine the positive or negative of the trajectory curvature. If the curvature is positive, jump to step s103; otherwise, jump to step s108; Step s103, determine whether the curvature k is in the interval [0, k1]; if k ∈ [0, k1], jump to step s105; otherwise, jump to step s104; Step s104, determine whether the curvature k satisfies k > k2; if the curvature k > k2, jump to step s105, otherwise jump to s106; Step s105, the rotation angle is 0 degrees; jump to step s113; Step s106, the rotation angle is c1 * k degrees; jump to step s107; Step s107, the left ultrasonic radar gimbal drives the left ultrasonic radar to rotate; jump to step s113; Step s108, determine whether the curvature k is in the interval [k3, 0]; if k ∈ [k3, 0], jump to step s110; otherwise, jump to step s109; Step s109, determine whether the curvature k satisfies k4 > k; if the curvature k4 > k, jump to step s110, otherwise jump to s111; Step s110, the rotation angle is 0 degrees; jump to step s113; Step s111, the rotation angle is c2 * k degrees; jump to step s112; Step s112, the right ultrasonic radar gimbal drives the right ultrasonic radar to rotate; jump to step s113; Step s113, end.

5. The control method of the control system for reducing the blind area of ​​the sensor when the robot turns according to claim 2, characterized in that: Including the following control steps: Start; Step s200, obtain the current position, speed, and heading of the robot; Step s201, determine the rotation direction and angle of the ultrasonic radar; Step s202, obtain the positions, speeds, and headings of obstacle 1 and obstacle 2; Step s203, calculate the relative speed; if the robot The speed is , is the robot's center of mass position Velocity vector in the geodetic coordinate system; is the robot's speed in the X direction; is the robot's speed in the Y direction; Obstacle 1 The speed is , is the center of mass position of obstacle 1 Velocity vector in the geodetic coordinate system, obstacle 2 The speed is , is the center of mass position of obstacle 2 The velocity vector in the earth coordinate system, then the velocity of obstacle 1 relative to the robot is , the speed of obstacle 2 relative to the robot is ; is the speed of obstacle 2 in the X direction; is the speed of obstacle 2 in the Y direction; Step s204, calculate the obstacle avoidance distance; if the robot's position is , the position of obstacle 1 is , the distance of obstacle 2 is , then the distance between obstacle 1 and the robot is , the distance between obstacle 2 and the robot is ; is the robot's x-coordinate; is the robot's y-direction coordinate; Step s205, determine whether the obstacle is within the avoidance range; Step s206, path planning; Step s207, calculate the new turning radius R; Step s208, calculate the new turning angle ; Step s209, adjust the turning trajectory to avoid obstacles; Step s210, control the robot to turn; Step s211, monitor the movement trajectory of the obstacle in real time; Step s212, update the path planning, and jump to step s205; Step s213, continue to drive along the current path; Jump to step s214; Step s214, give real-time feedback and adjust the path; Step s215, determine whether the path needs to be adjusted; if it does not need to be adjusted, jump to step s211, if it needs to be adjusted, jump to step s216; Step s216, perform path adjustment; Step s217, the path adjustment is completed, and the robot continues to drive; End.

6. The control method of the control system for reducing the blind area of ​​the sensor when the robot turns according to claim 5, characterized in that: Step s201 determines the rotation direction and angle of the ultrasonic radar; it includes the following control steps: Step s400, start; Step s401, the trajectory calculation module outputs the trajectory; Step s402, determine whether the curvature of the trajectory is positive or negative, if the curvature is positive, jump to step s403, otherwise jump to step s408; Step s403, determining whether the curvature k is in the interval [0, k1]; k∈[0,k1] jump to step s405; Otherwise jump to step s404; Step s404, determine whether the curvature k satisfies k>k2; if the curvature k>k2, jump to step s405, otherwise jump to s406; Step s405, the rotation angle is 0 degrees; jump to step s413; Step s406, the rotation angle is c1*k degrees; jump to step s407; Step s407, the left ultrasonic radar pan / tilt drives the left ultrasonic radar to rotate; jump to step s413; Step s408, determine whether the curvature k is in the interval [k3, 0]; if k∈[k3, 0], jump to step s410; Otherwise jump to step s409; Step s409, determine whether the curvature k satisfies k4>k; if the curvature k4>k, jump to step s410, otherwise jump to s411; Step s410, the rotation angle is 0 degrees; jump to step s413; Step s411, the rotation angle is c2*k degrees; jump to step s412; Step s412, the right ultrasonic radar pan / tilt drives the right ultrasonic radar to rotate; jump to step s413; Step s413, end.

7. The control method of the control system for reducing the blind area of ​​the sensor when the robot turns according to claim 2, characterized in that: The control steps include: start; Step s300, obtaining the current position, speed and heading of the robot; Step s301, determining the rotation direction and angle of the ultrasonic radar; Step s302, obtaining the position, speed and direction of obstacle 1 and obstacle 2; Step s303, calculating the relative speed; Step s304, calculating obstacle avoidance distance; Step s305, determining whether the obstacle is within the avoidance range; Step s306, path planning; Step s307, calculating a new turning radius R; Step s308, calculate the new turning angle ; Step s309, adjusting the turning trajectory to avoid obstacles; Step s310, controlling the robot to turn; Step s311, smoothing the speed of the local path; Step s312, real-time monitoring of the obstacle movement trajectory; Step s313, update the path planning and jump to step s305; Step s314, continue driving along the current path; Step s315, real-time feedback and path adjustment; Step s316, determine whether the path needs to be adjusted; if not, jump to step s312; if it needs to be adjusted, jump to step s317; Step s317, adjust the path; re-call the local path planning algorithm; Step s318, the path adjustment is completed and the robot continues to drive; Finish.

Images