A method for navigation of a channel robot along a predetermined route based on array ranging
By using array ranging sensors and PD controllers in central air conditioning ventilation ducts, combined with the intersection pass algorithm with preset speed, the problem of robot identification and passage in low-illumination environments is solved, and the robot's autonomous navigation in the channel is realized Passing with intersections reduces costs.
Patent Information
- Application Number
- CN202210380428.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-04-12
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2042-04-12
AI Technical Summary
The prior art is difficult to realize autonomous navigation of robots in central air-conditioning ventilation ducts in low-illumination environments, especially when yaw and intersection identification are encountered when crossing identification and traffic.
Using an array ranging based channel robot navigation method, by arranging 6 ranging sensors on the robot's chassis, a recognition model of straight, intersection, T-junction and 90-degree corners is established, combining PD controller and speed preset intersections. Open-loop control algorithm realizes autonomous navigation and intersection traffic of robots in channels.
It realizes intelligent road conditions identification and straight yaw control of the robot in low-illumination environments, ensuring smooth passage of the robot in the channel, reducing costs, and is suitable for autonomous navigation of narrow channels.
Smart Images

Figure CN115113620B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of robot navigation, and in particular relates to a method for navigating a channel robot along a predetermined route based on array ranging. Background Art
[0002] The ventilation ducts of central air conditioners will accumulate dirt during use, which not only reduces the working efficiency of the air conditioner, but also the unclean air affects human health, so the ventilation ducts need to be cleaned regularly. However, the ventilation ducts of central air conditioners are narrow, and considering the safety of personnel, they are generally cleaned by robots. Chinese utility model patent CN202020603888.9, named "An air conditioning duct cleaning device", discloses an air conditioning duct cleaning device. Although it can realize remote control of the robot to clean the ventilation ducts, it cannot realize autonomous navigation of the robot.
[0003] At present, the main autonomous navigation methods include visual navigation, electromagnetic navigation, laser and ultrasonic ranging navigation, etc. Due to the low illumination in the pipeline and the inconvenience of arranging electromagnetic navigation lines, visual navigation and electromagnetic navigation are not suitable for use in the ventilation ducts of central air conditioners. Laser and ultrasonic ranging sensors are low in cost, not affected by dark environments, and do not need to set navigation lines in the pipeline, so they are suitable for use in ventilation ducts. Chinese invention patent CN201310390231.3, named "An ultrasonic navigation device for autonomous mobile vehicles used in greenhouses", although this patent can solve the problem of round-trip walking and small radius in-situ right-angle turning in narrow passages, but the robot's movement along the straight road is a simple negative feedback adjustment, and it cannot effectively control the yaw amount and yaw angle synchronously, and cannot cope with the problem of identifying and passing various intersections (crossroads, T-junctions, 90-degree corners, etc.). Summary of the invention
[0004] The present invention provides a channel robot predetermined route navigation method based on array ranging in order to solve the problems that a channel robot can perform real-time road condition recognition in a low-light environment, autonomous navigation on a given route, keep the robot on a straight path in a channel without deviation, and solve the problem that the robot can pass smoothly at intersections.
[0005] The present invention adopts the following technical scheme: a method for navigating a predetermined route of a channel robot based on array ranging, comprising the following steps: S100-6 ranging sensors are arranged on the chassis of the channel robot, 4 ranging sensors are symmetrically arranged on the left and right sides of the channel robot, and 2 ranging sensors are arranged in front and rear of the chassis of the channel robot, and the ranging sensors of the right front, right rear, left front, left rear, front and rear are numbered from 1 to 6 in sequence; S200-the route of the channel robot is given, including various types of intersections and passage modes passed in sequence; S300-updates the path traversed by the channel robot; S400-a timer is started in the controller of the channel robot, with a time period as , collect the data of all ranging sensors, and get the distance data of ranging sensors 1 to 6 as follows: L 1. L 2. L 3. L 4. L 5 and L 6; S500~determine whether the channel robot is in a straight road, a crossroad, a T-junction, a 90-degree corner or other situations according to the distance data of the ranging sensor; S600~control according to the result of the judgment in step S500. When the channel robot is judged to be in a straight road, the channel robot is yaw controlled; when the channel robot is judged to be in other crossroads, T-junctions or 90-degree corners, the channel robot is controlled to pass through the intersection; when the channel robot is judged to be in other situations, the channel robot is in a transition position and maintains the original heading and speed; S700~repeat steps S300-S600 until the channel robot reaches the end point.
[0006] In step S500: S501 to S502 L 1<a, L 2<a, L 3<a, L 4<a, L 5>a, L 6>a, the robot is on a straight path. S502~When L 1>a, L 2>a, L 3>a, L 4>a, L 5>a, L 6>a, the robot is at a crossroads. S503~When L 1>a, L 2>a, L 3>a, L 4>a, L 5<a, L 6>a, or when L 1>a, L 2>a, L 3<a,L 4<a, L 5>a, L 6>a, or when L 1<a, L 2<a, L 3>a, L 4>a, L 5>a, L 6>a, the robot is at a T-junction. S504~When L 1<a, L 2<a, L 3>a, L 4>a, L 5<a, L 6>a, or when L 1>a, L 2>a, L 3<a, L 4<a, L 5<a, L 6>a, the robot is at a 90-degree corner. S505~When the data of the ranging sensor does not belong to the above four cases, it belongs to other cases, and the robot is at a transition position between a straight road and an intersection. Where a is a length constant.
[0007] In step S600, S601-When it is determined that the channel robot is on a straight path, the estimated value of the robot's yaw angle relative to the channel is calculated based on the data of the range sensors 1 to 4. and the estimated value of the yaw , ,in , ; ,in , ; The distance between the two sensors on the left or right is L, and are the yaw angles calculated by the right and left ranging sensors, respectively. and They are the distances from the right and left sides of the chassis to the wall respectively.
[0008] According to the estimated values of yaw angle and yaw amount, the angular velocity command of the robot is adjusted based on the PD controller :
[0009]
[0010] in and are the expected values of the yaw angle and yaw amount, respectively, ; The superscript “·” indicates the derivative of the corresponding physical quantity; and are the proportional and differential coefficients of the yaw angle PD controller, and are the proportional and differential coefficients of the yaw PD controller, and are the output weight values of the yaw angle and yaw amount PD controller respectively; during this process, the robot moves along Direction at constant linear speed Marching, .
[0011] S602: When the channel robot is judged to be at a crossroad, a T-junction or a 90-degree corner, the execution action of the robot at the intersection is judged:
[0012] 1) If it is a straight line, set the linear speed of the channel robot , set the angular velocity to 0, and set the time required to cross the intersection: , initialization run time: ;in is the channel width. Then, the robot travels according to the speed instruction time , in this process Record running time;
[0013] 2) If it is turning left or right, when the channel robot wants to turn right, set the turning flag ; When the channel robot wants to turn left, set the turning flag .
[0014] First, the channel robot reaches the center of the intersection from the first entrance, setting the linear speed , angular velocity is 0; set the required time: , where when the vehicle just enters the intersection, under high-frequency sampling, the distance c from the left and right rear sensors to the wall is small and can be ignored; initialization running time: , the robot travels according to the speed instruction , in this process Record the running time.
[0015] Next, the robot turns 90 degrees in the center of the intersection and sets the linear speed , the angular velocity is , As a constant, set the required time: , initialize the running time: The robot travels according to the speed command time , in this process Record the running time.
[0016] Then, the robot goes straight out of the intersection and sets the linear speed , the angular velocity is 0, and the required time is set: , initialize the running time: The robot travels according to the speed command time , in this process Record the running time.
[0017] S603-When it is determined that the channel robot belongs to other situations, the channel robot is in the transition position between the straight road and the intersection, maintaining the original heading and speed.
[0018] Compared with the prior art, the present invention discloses a method for navigation of a predetermined route for a channel robot based on array ranging. Two ranging sensors are arranged side by side on the left and right sides of the robot, and one ranging sensor is arranged on the front and rear. A ranging recognition model for straight roads, ninety-degree corner intersections, T-junctions and crossroads is established, and the robot can judge the current road condition based on the data of the ranging sensor. A closed-loop control algorithm for walking along the center line of a straight road is proposed, and an open-loop control algorithm for passing through an intersection based on a preset speed is proposed, so that the robot can automatically move along a predetermined route in the channel. The present invention can effectively solve the problems of intelligent identification of road conditions, straight road deviation and intersection passage of the robot in the channel at a low cost, and can be applied to autonomous navigation in narrow channels such as central air-conditioning ventilation ducts. BRIEF DESCRIPTION OF THE DRAWINGS
[0019] Figure 1 It is a principle diagram of the distance measuring sensor of the present invention for obtaining the yaw angle and yaw amount relative to the channel.
[0020] Figure 2 It is a flowchart of the established route navigation algorithm of the channel robot of the present invention.
[0021] Figure 3 It is the identification diagram of the array ranging sensor in a straight track;
[0022] Figure 4 It is the identification map of the array ranging sensor at the intersection;
[0023] Figure 5 This is the recognition diagram of the array ranging sensor at the T-junction where the robot can turn left or right.
[0024] Figure 6 This is the recognition diagram of the array ranging sensor at the T-junction where the robot can go straight or turn right.
[0025] Figure 7 This is the recognition map of the array ranging sensor at the T-junction where the robot can turn left or go straight;
[0026] Figure 8 This is the recognition image of the array ranging sensor when the robot turns 90 degrees left;
[0027] Fig. 9 This is the recognition image of the array ranging sensor when the robot turns 90 degrees right;
[0028] Fig.10 It is a block diagram of the closed-loop control algorithm for walking along the center line of a straight road of the present invention;
[0029] Fig.11 It is a flowchart of an open-loop control algorithm for intersection traffic based on preset speed of the present invention;
[0030] Fig.12 This is a schematic diagram of the robot reaching the center of the intersection from the moment it enters the intersection;
[0031] Fig.13 This is a schematic diagram of the robot turning right 90 degrees from the center of the intersection;
[0032] Fig.14 This is a schematic diagram of the robot leaving the intersection from the center of the intersection;
[0033] Fig.15 This is the overall picture of the robot and the channel;
[0034] Fig.16 A transparent image of the robot;
[0035] Fig.17 It is the intended route and actual trajectory of the robot in the simulation of the present invention;
[0036] Fig.18 This is the effect of the closed-loop control algorithm of the present invention for walking along the center line of a straight road. DETAILED DESCRIPTION
[0037] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings.
[0038] A method for navigating a predetermined route for a channel robot based on array ranging includes the following steps.
[0039] S100~ Figure 1 As shown, 6 distance measuring sensors are arranged on the chassis of the robot to establish a plane rectangular coordinate system Fixed to the center of the robot chassis, The axis is the robot's forward direction. Four distance measuring sensors are symmetrically arranged on the left and right sides of the robot. The distance between the two sensors on one side is L and is perpendicular to Axle, front and rear along Each axle is provided with one ranging sensor. The ranging sensors at the right front, right rear, left front, left rear, front and rear are numbered 1 to 6 in sequence, with the front ranging sensor being marked in dark color.
[0040] S200~ gives the route of the channel robot, including the types of intersections and the modes of passage (straight ahead, right turn or left turn) that it passes through in sequence, such as Figure 2 shown.
[0041] S300~Update the path traveled by the channel robot and determine the way to pass the next intersection.
[0042] S400~A timer is started in the channel robot controller, with a time period of , collect the data of all ranging sensors, and get the distance data of ranging sensors 1 to 6 as follows: L 1. L 2. L 3. L 4. L 5 and L 6.
[0043] S500~Judgment, based on the distance data of the distance measuring sensor, that the channel robot is in a straight road, a crossroad, a T-junction, a 90-degree corner, or other situations.
[0044] S501~When L 1<a, L 2<a, L 3<a, L 4<a, L 5>a, L 6>a, if Figure 3 As shown, the robot is in a straight line.
[0045] S502~When L 1>a, L 2>a, L 3>a, L 4>a, L 5>a, L 6>a, if Figure 4 As shown, the robot is at a crossroads.
[0046] S503~When L 1>a, L 2>a, L 3>a, L 4>a, L 5<a, L 6>a, or when L 1>a, L 2>a, L 3<a, L 4<a, L 5>a, L 6>a, or when L 1<a, L 2<a, L 3>a,L 4>a, L 5>a, L 6>a, the robot is at a T-junction. When the first condition is met, the robot can turn left or right. Figure 5 As shown; when the second condition is met, the robot can go straight or turn right, such as Figure 6 As shown; when the third condition is met, the robot can go straight or turn left, such as Figure 7 shown.
[0047] S504~When L 1<a, L 2<a, L 3>a, L 4>a, L 5<a, L 6>a, or when L 1>a, L 2>a, L 3<a, L 4<a, L 5<a, L 6>a, the robot is at a 90-degree corner. The robot can turn left when the first condition is met, such as Figure 8 As shown; the robot can turn right when the second condition is met, such as Fig. 9 shown.
[0048] S505~When the data of the ranging sensor does not belong to the above four situations, it belongs to other situations, and the robot is at the transition position between the straight road and the intersection.
[0049] Where a is the length constant, which should be greater than the distance between the side of the robot (in non-yaw state) and the wall of the straight road, and less than the width of the channel. The specific value needs to be adjusted according to the experiment.
[0050] S600~Control is performed according to the result of the judgment in step S500. When it is judged that the channel robot is on a straight road, the channel robot is yaw controlled; when it is judged that the channel robot is at a crossroad, a T-junction or a 90-degree corner, the channel robot is controlled to pass through the intersection; when it is judged that the channel robot is in other situations, the channel robot is in a transition position and maintains the original heading and speed.
[0051] S601: When the channel robot is judged to be on a straight path, the estimated value of the robot's yaw angle relative to the channel is calculated based on the data of the range sensors 1 to 4. and the estimated value of the yaw , ,in , ; ,in , ;in and are the yaw angles calculated by the right and left ranging sensors, respectively. and They are the distances from the right and left sides of the chassis to the wall respectively.
[0052] To make the robot move along the center line of the straight road, Fig.10 As shown, according to the estimated values of the yaw angle and the yaw amount, the angular velocity command of the robot is adjusted based on the PD controller. :
[0053]
[0054] in and are the expected values of the yaw angle and yaw amount, respectively, ; The superscript “·” indicates the derivative of the corresponding physical quantity; and are the proportional and differential coefficients of the yaw angle PD controller, and are the proportional and differential coefficients of the yaw PD controller, and are the output weight values of the yaw angle and yaw amount PD controller respectively; during this process, the robot moves along Direction at constant linear speed Marching, .
[0055] S602: When the channel robot is judged to be at a crossroad, a T-junction, or a 90-degree corner, the robot executes an open-loop control algorithm for crossing the intersection based on a preset speed, and obtains a control instruction for the travel speed, such as Fig.11 shown.
[0056] Determine the execution action of the channel robot when it encounters an intersection.
[0057] 1) If it is a straight line, set the linear speed of the channel robot , set the angular velocity to 0, and set the time required to cross the intersection: , initialize the running time: ;in is the channel width. Then, the robot travels according to the speed instruction time , in this process Record the running time.
[0058] 2) If it is turning left or right, when the channel robot wants to turn right, set the turning flag ; When the channel robot wants to turn left, set the turning flag .
[0059] First, the channel robot reaches the center of the intersection from the intersection it just entered. Fig.12 As shown, set the line speed , angular velocity is 0; set the required time: , where when the vehicle just enters the intersection, under high-frequency sampling, the distance c from the left and right rear sensors to the wall is small and can be ignored; initialization running time: , the robot travels according to the speed instruction , in this process Record the running time.
[0060] Then, the robot turns 90 degrees in the center of the intersection. Fig.13 As shown, the solid line The axis is the current heading of the robot, and the dotted line The axis is the robot's target heading, set the linear speed , the angular velocity is , As a constant, set the required time: , initialize the running time: The robot travels according to the speed command time , in this process Record running time;
[0061] Then, the robot goes straight out of the intersection, as Fig.14 As shown, set the line speed , the angular velocity is 0, and the required time is set: , initialize the running time: The robot travels according to the speed command time , in this process Record the running time.
[0062] After executing the above steps, the channel robot passed the intersection.
[0063] S603-When it is determined that the channel robot belongs to other situations, the channel robot is in the transition position between the straight road and the intersection, maintaining the original heading and speed.
[0064] S700~Repeat steps S300-S600 until the channel robot reaches the end point. Determine whether the robot has reached the end point. If yes, then end, the robot stops moving; otherwise, jump to S300. Set the end point according to actual needs. In this implementation case, the place 0.2m away from the dead end wall in front of the robot is set as the end point.
[0065] The application effect of the present invention is described in detail below in conjunction with simulation.
[0066] First, in the gazebo physics simulation software, such as Fig.15As shown, the channel is established in the gazebo software, and the channel width is 0.8m, and a two-wheel differential-driven robot is placed in it. Fig.16 This is a transparent image of the robot. The two balls at the bottom of the robot are passive support wheels. The six cubes on the side of the robot are distance sensors. The distance range of each sensor is 0.02m~1.5m. The distance between the two sensors on the left or right side is L is 0.2m.
[0067] Next, we test the channel robot’s established route navigation method based on array ranging. The starting point of the robot is the position where the center of the robot is 0.5m away from the entrance of the channel, the initial yaw is 0.05m, and the initial yaw angle is 10 degrees; given the robot’s route, the various types of intersections and traffic methods passed in sequence are straight road, crossroad (right turn), straight road, T-junction (left turn), straight road, 90-degree corner (left turn), straight road, T-junction (right turn), straight road, 90-degree corner (right turn), straight road, and the end point is set at 0.2m from the front of the robot to the dead end wall. Other simulation process parameters: sensor sampling rate and controller control rate are both 10Hz, and the threshold for road condition judgment a The yaw angle of the PD controller is 0.5 meters. and the differential coefficient 5 and 1 respectively, the ratio of the yaw PD controller and the differential coefficient 5 and 1 respectively, the output weight values of the yaw angle PD controller is 1, and the output weight value of the yaw PD controller 1, the robot's linear speed when moving straight b 0.1m / s, the angular velocity of the robot when turning in place at the intersection is 90° / s, the time it takes for the robot to reach the center of the intersection from the moment it enters the intersection Set to 3 seconds, the robot's turning time at the center of the intersection Set to 1 second, the time it takes for the robot to leave the intersection from the center Set to 6 seconds.
[0068] After the simulation is finished, export the data and draw the actual trajectory of the robot, such as Fig.17 The solid line in the middle is the planned route, and the dotted line is the planned route. It can be seen that the robot has completed the autonomous navigation based on array ranging well. When exiting the intersection, the route deviates slightly from the planned path. This is because the robot's control algorithm at the intersection is open-loop. After the robot passes the intersection, the closed-loop control algorithm that walks along the center line of the straight road quickly corrects the error of the open-loop algorithm. Fig.18Before passing the first intersection, the robot's yaw angle and yaw amount under the closed-loop control algorithm are shown as it walks along the center line of the straight road. The robot's yaw amount and yaw angle gradually converge to 0 from the initial state, indicating that the algorithm is effective.
[0069] The above are preferred embodiments of the present invention and do not limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.
Claims
1. A method for navigation of a channel robot along a predetermined route based on array ranging, characterized in that: The following steps are included: S100~Arrange 6 ranging sensors on the chassis of the channel robot, 4 ranging sensors are symmetrically arranged on the left and right sides of the channel robot, 2 ranging sensors are arranged in front and rear of the chassis of the channel robot, and the ranging sensors of the right front, right rear, left front, left rear, front and rear are numbered from 1 to 6 in sequence; S200~gives the travel route of the channel robot, including the types of various intersections and passage modes passed in sequence; S300~Update the path traveled by the channel robot; S400~A timer is started in the channel robot controller, and the data of all the distance measuring sensors are collected with a time period of Δt, and the distance data of the distance measuring sensors No. 1 to No. 6 are obtained as L1, L2, L3, L4, L5 and L6; S500: judging, based on the distance data of the distance measuring sensor, whether the channel robot is in a straight road, a crossroad, a T-junction, a 90-degree corner, or other situations; S600~control according to the result of step S500; S601-When it is determined that the channel robot is on a straight path, yaw control is performed on the channel robot; When the channel robot is judged to be on a straight path, the estimated value of the robot's yaw angle relative to the channel is calculated based on the data from the range sensors 1 to 4. and the estimated value of the yaw in in Where β1 and β2 are the yaw angles calculated by the right and left ranging sensors, respectively, and L 12 and L 34 are the distances from the right and left sides of the chassis to the wall respectively; According to the estimated values of yaw angle and yaw amount, the angular velocity command ω of the robot is adjusted based on the PD controller z : where β * and Δy * are the expected values of the yaw angle and yaw amount, β * =Δy * =0; the superscript "·" indicates the derivative of the corresponding physical quantity; and are the proportional and differential coefficients of the yaw angle PD controller, and are the proportional and differential coefficients of the yaw PD controller, λ β and λ Δy are the output weight values of the yaw angle and yaw amount PD controller respectively; during this process, the robot moves along x r Direction at constant linear speed Marching, When the channel robot is judged to be at other crossroads, T-junctions or 90-degree corners, the channel robot is controlled to pass through the intersection; S602: When the channel robot is judged to be at a crossroad, a T-junction or a 90-degree corner, the execution action of the robot at the intersection is judged: 1) If it is a straight line, set the linear speed of the channel robot Set the angular velocity to 0 and the time required to cross the intersection: Initialization running time: t c =0; where w p is the channel width, then the robot travels according to the speed instruction for time T c0 , in this process t c Record running time; 2) If it is a left turn or a right turn, when the channel robot wants to turn right, set the turning flag κ=-1; when the channel robot wants to turn left, set the turning flag κ=1; First, the channel robot reaches the center of the intersection from the first entrance, setting the linear speed Angular velocity is 0; set the required time: When the sensor just enters the intersection, the distance c from the left and right rear sensors to the wall is small under high-frequency sampling and can be ignored; initialization running time: t c = 0, the robot travels according to the speed instruction for a time T c1 , in this process t c Record running time; Then, the robot turns 90 degrees in the center of the intersection, where the solid line x r The axis is the current heading of the robot, the dotted line x r The axis is the robot's target heading, set the linear speed The angular velocity is ω z # =κω c ,ω c As a constant, set the required time: Initialization running time: t c = 0, the robot travels according to the speed instruction for a time T c2 , in this process t c Record running time; Then, the robot goes straight out of the intersection and sets the linear speed The angular velocity is 0 and the required time is set: Initialization running time: t c = 0, the robot travels according to the speed instruction for a time T c3 , in this process t c Record running time; When the channel robot is judged to be in other situations, the channel robot is in a transition position and maintains the original heading and speed; S700~Repeat steps S300-S600 until the channel robot reaches the end point.
2. The method for navigation along a predetermined route of a channel robot based on array ranging according to claim 1, characterized in that: In the step S500, S501-When L1<a, L2<a, L3<a, L4<a, L5>a, L6>a, the robot is in a straight line; S502-When L1>a, L2>a, L3>a, L4>a, L5>a, L6>a, the robot is at a crossroads; S503~When L1>a, L2>a, L3>a, L4>a, L5<a, L6>a, or when L1>a, L2>a, L3<a, L4<a, L5>a, L6>a, or when L1<a, L2<a, L3>a, L4>a, L5>a, L6>a, the robot is at a T-junction; when the first condition is met, the robot can turn left or right, when the second condition is met, the robot can go straight or turn right, and when the third condition is met, the robot can go straight or turn left; S504-When L1<a, L2<a, L3>a, L4>a, L5<a, L6>a, or when L1>a, L2>a, L3<a, L4<a, L5<a, L6>a, the robot is at a 90-degree corner; the robot can turn left when the first condition is met, and can turn right when the second condition is met; Where a is the length constant; S505~When the data of the ranging sensor does not belong to the above four situations, it belongs to other situations, and the robot is at the transition position between the straight road and the intersection.
3. The channel robot predetermined route navigation method based on array ranging according to claim 1 is characterized in that: In the step S600, S603-When it is determined that the channel robot belongs to other situations, the channel robot is in the transition position between the straight road and the intersection, maintaining the original heading and speed.
