Mobile robot navigation control method and system

Abstract

The application provides a kind of mobile robot navigation control method and system, comprising: based on the prior information of identification target gray image acquired in advance, real-time gray image and depth image are obtained by ToF camera, positioning is carried out in combination with the prior information of identification target, and the navigation target point pose is determined in combination with the requirement of specific task, to realize the navigation control of mobile robot.The system designed by the application includes depth progressive positioning algorithm based on the characteristics of ToF depth information to obtain the pose of mobile robot, high-order Bezier curve algorithm with attitude constraint to plan global path in real time, feedback linearization control algorithm with error compensation to carry out trajectory tracking control in combination with obstacle detection module.The application makes full use of the characteristics of image and depth information of ToF camera to realize the navigation control of mobile robot.The system structure of the application is simple, and the real-time control and accuracy are high.

Description

Technical Field

[0001] This invention relates to the field of robotics, specifically to a navigation control method for a mobile robot based on ToF camera-based target feature recognition, and more particularly to a navigation control method and system for a mobile robot. Background Technology

[0002] With the development of sensor technology, more solutions have been found for the navigation problem of mobile robots, enabling them to face more complex and challenging scenarios. Relying on methods such as multi-view vision-based navigation, laser-based navigation, and ultrasonic-based navigation, a variety of solutions have been developed for indoor navigation tasks of mobile robots.

[0003] Chinese patent CN106919171B discloses a robot indoor positioning and navigation system and method, which relies on UWB (Ultra-Wideband) wireless communication technology to locate the mobile robot in an indoor environment. An initial coordinate system is established by setting the initial coordinates of at least three base stations, and the mobile robot performs autonomous navigation within this coordinate system.

[0004] Chinese patent CN111445531B discloses a multi-camera navigation method, device, equipment, and storage medium. It uses multiple cameras to establish multiple coordinate systems, jointly determines the navigation world coordinate system of a mobile robot, and relies on this coordinate system to perform navigation tasks for the mobile robot.

[0005] Chinese patent CN215340833U discloses a laser vision navigation robot that can find disinfection points on its own. It uses a laser and vision fusion method to enable the mobile robot to create 2D and 3D maps of the environment, thereby finding disinfection locations and navigating accordingly.

[0006] The aforementioned studies have shown promising results in the field of mobile robot navigation. However, current mobile robot navigation methods, including those mentioned above, either require the cooperation of multiple sensors, resulting in high costs; or require pre-setting, making operation cumbersome; or do not consider robot load issues, leading to poor robustness. Summary of the Invention

[0007] To address the shortcomings of existing technologies, this invention provides a navigation control method and system for mobile robots.

[0008] According to the present invention, a navigation control method and system for a mobile robot are provided, the solution of which is as follows:

[0009] Firstly, a navigation control method for a mobile robot is provided, the method comprising:

[0010] Step S1: Define the coordinate system by setting the global coordinate system of the mobile robot's navigation environment, the local coordinate system of the mobile robot itself, and the coordinate system of the ToF camera's imaging plane.

[0011] Step S2: Set prior information by inputting the grayscale image of the target to the DGP algorithm as prior information for the target;

[0012] Step S3: Acquire real-time images. Use a ToF camera to capture real-time grayscale and depth images, and simultaneously input them into the DGP positioning algorithm.

[0013] Step S4: Robot localization. Real-time localization is performed using the DGP algorithm. Combining the prior information of the acquired target grayscale image with the real-time grayscale and depth image information captured by the ToF camera, semantic segmentation and localization are performed on the target to derive the pose (x, y) of the mobile robot in the global coordinate system of the navigation environment. robot ,y robot ,θ robot );

[0014] Step S5: Determine the navigation target point. Based on the specific tasks the robot needs to complete in the usage scenario, determine the pose (x, y) of the robot's navigation target point in the global coordinate system of the navigation environment. t ,y t ,θ t );

[0015] Step S6: Path planning, using the PCBC algorithm to plan a global navigation path from the robot's current localization pose to the target point pose;

[0016] Step S7: Trajectory tracking. Use the ECFL algorithm to track the trajectory and control the mobile robot to reach the target point pose.

[0017] Preferably, the global coordinate system of the mobile robot navigation environment takes the target as the origin, the frontal normal vector of the target as the X-axis, the Z-axis is perpendicular to the ground with upward as the positive direction, and the Y-axis is established by the right-hand rule;

[0018] The local coordinate system of the mobile robot takes the position of the ToF camera installed on the robot as the origin, the robot's forward direction, which is also the forward method vector of the ToF camera's viewpoint, as the X-axis, the Z-axis is perpendicular to the ground with upward as the positive direction, and the Y-axis is established by the right-hand rule.

[0019] The ToF camera's imaging plane coordinate system has the image center as the origin, the row direction of the pixels as the X direction (positive to the right), and the column direction as the Y direction (positive upwards). The depth image and grayscale image returned by the ToF camera use the same image plane coordinate system.

[0020] Preferably, the DGP algorithm is a positioning algorithm developed based on the characteristics of obtaining depth information using the active ranging principle of a ToF camera, and includes the following process:

[0021] 1) Use the DGP algorithm to identify targets and perform semantic segmentation:

[0022] The DGP algorithm performs feature matching between the prior image of the object and the real-time image returned by Time-of-Flight (ToF) to coarsely locate the object. The center coordinates of the object are denoted as (x...) in the camera imaging plane coordinate system of the grayscale image. o ,y o And obtain the depth value D of that point in the ToF depth image. init ;

[0023] A single object appears as having continuous depth information in a Time-of-F (ToF) depth image. The DGP algorithm uses this as its core principle to find abrupt changes in depth information in order to determine the object's edges.

[0024] Leveraging the high accuracy of depth information acquired by ToF cameras, the DGP algorithm can determine the edge pixel coordinates of the object to be identified by detecting abrupt changes in depth values ​​between adjacent pixels, thereby obtaining the center coordinates (x, y) of the target on the image plane. o ,y o ) and its two edge depth values ​​D LeftEdge With D RightEdge ;

[0025] 2) Using the DGP algorithm to calculate the pose of the mobile robot:

[0026] Let the actual width of the target be W0. In the global coordinate system of the mobile robot navigation environment, we have:

[0027]

[0028] From the trigonometric relationship, we know that:

[0029]

[0030]

[0031] In the global coordinate system of the mobile robot navigation environment, the mobile robot coordinates (x... robot ,y robot Let Φ be the angle between the line connecting the origin to the origin and the X-axis, and α be the angle between the line connecting the origin to the origin and the normal vector of the mobile robot. Let θ be the pose of the mobile robot. robot In the global coordinate system of the mobile robot navigation environment, it is defined as the angle between the mobile robot's normal vector and the X-axis, with counterclockwise being positive;

[0032] Let the horizontal field of view (HFOV) of the ToF camera be h, and the pixel width of the ToF returned image be w. Based on the pinhole model of the ToF camera, determine the coordinates (x, y, y) of the target in the image plane. o ,y o Calculate α:

[0033]

[0034] In the global coordinate system of the mobile robot navigation environment, Φ is calculated according to the inverse trigonometric relationship:

[0035]

[0036] Then the pose θ of the mobile robot robot Calculated by the following formula:

[0037] θ robot =π-Φ+α

[0038] Thus, the global coordinate system information of the mobile robot's navigation environment, as well as the mobile robot's pose (x, y) in the global coordinate system of the navigation environment, are obtained through the DGP algorithm. robot ,y robot ,θ robot ).

[0039] Preferably, the PCBC algorithm is a high-order Bézier curve path planning algorithm with attitude constraints, which comprehensively considers real-time performance, trajectory smoothness, and the accuracy of the starting and ending poses.

[0040] Obtain the starting pose (x) robot ,y robot ,θ robot ) and final pose (x t ,y t ,θ t At least four Bézier curve direction points should be used to plan the path, including (x... robot ,y robot ), (x1,y1), (x2,y2), (x t ,y t );

[0041] In the PCBC algorithm, (x1, y1) and (x2, y2) satisfy:

[0042]

[0043]

[0044] The PCBC algorithm allows the number of direction points between (x1, y1) and (x2, y2) to be increased to modify the navigation path;

[0045] The navigation path planned by the PCBC algorithm is a sequence of timestamped points, B(t) = (X... r (t),Y r (t)), where t∈[0,T] f ], T f This is for reference control time.

[0046] Preferably, the ECFL algorithm is a feedback linearization control algorithm with error compensation, which adds an error compensation detection mechanism to the control system and improves the trajectory tracking control performance of the mobile robot.

[0047] Modeling the kinematics of the mobile robot in the global coordinate system of the mobile robot's navigation environment:

[0048]

[0049]

[0050]

[0051] Where v and ω are the linear velocity and angular velocity of the mobile robot, respectively, the model is linearized using feedback; the ECFL algorithm takes the following linearized output constant, where L is a given parameter and satisfies L≠0:

[0052] x L =x robot +L·cos(θ robot )

[0053] y L =y robot +L·sin(θ robot )

[0054] Thus, the chain model of the ECFL algorithm can be obtained as follows:

[0055]

[0056]

[0057]

[0058]

[0059] This system has inputs (v, ω) and outputs (x, ω). L ,y L Decoupled, it becomes a linear system, combined with the reference tracking trajectory sequence points (X) obtained by the PCBC algorithm. r (t),Y r (t)), for The assignment is as follows, where k is a given parameter, (xr , y r ) to control the trajectory sequence points corresponding to the time instant T ∈ [0, T f , where the coordinates are (X r (T), Y r (T)), which is the derivative of the trajectory with respect to time at this point:

[0060]

[0061]

[0062] That is, the special control quantities (v, ω) of the ECFL algorithm are obtained:

[0063]

[0064]

[0065]

[0066] Update (x robot , y robot , θ robot ) to update the control quantity for trajectory tracking control.

[0067] Preferably, the method further includes: adding an error compensation to the feedback linearization controller, and defining the static error as:

[0068]

[0069] When the actual control time T = T f , if the static error E is greater than the acceptable error range E0, then take (X r (T f ), Y r (T f )) as the next tracking point, recalculate (v, ω), and continue tracking until the static error E satisfies E < E0, that is, the tracking can be stopped; relying on this tracking algorithm, the mobile robot can move forward along the set trajectory;

[0070] When performing each step of control, the ECFL algorithm determines whether there are obstacles in front according to the depth map and grayscale map transmitted back by the ToF, so as to decide whether to pause the forward movement. The specific steps are as follows:

[0071] Divide the real-time depth image transmitted back by the ToF into blocks and calculate the average value to obtain the depth value sequence M = {m i |i ∈ [0, 1,..., n]}, and calculate the depth value sequence and Calculate By using variables in ΔM to determine if there are depth abrupt changes, we can know whether there are obstacles moving into the reference trajectory; at the same time, by detecting the average value of the blocks representing the direction of movement in the sequence, we can determine whether there is enough distance to move forward, thus ensuring the safety of the mobile robot's operation.

[0072] Secondly, a mobile robot navigation control system is provided, the system comprising:

[0073] Module M1: Defines the coordinate system, setting the global coordinate system of the mobile robot's navigation environment, the local coordinate system of the mobile robot itself, and the coordinate system of the ToF camera's imaging plane;

[0074] Module M2: Sets prior information and inputs the grayscale image of the target to the DGP algorithm as prior information for the target;

[0075] Module M3: Acquires real-time images, uses a ToF camera to capture real-time grayscale and depth images, and synchronously inputs them into the DGP positioning algorithm;

[0076] Module M4: Robot localization. It uses the DGP algorithm for real-time localization, combining prior information from the acquired target grayscale image with real-time grayscale and depth images captured by the ToF camera. This allows for semantic segmentation and localization of the target, deriving the mobile robot's pose (x, y) in the global coordinate system of the navigation environment. robot ,y robot ,θ robot );

[0077] Module M5: Determines the navigation target point. Based on the specific tasks the robot needs to complete in the usage scenario, it determines the pose (x, y) of the navigation target point in the global coordinate system of the navigation environment. t ,y t ,θ t );

[0078] Module M6: Path planning, using the PCBC algorithm to plan the global navigation path from the robot's current localization pose to the target point pose;

[0079] Module M7: Trajectory tracking, uses the ECFL algorithm for trajectory tracking, and controls the mobile robot to reach the target point pose.

[0080] Preferably, the global coordinate system of the mobile robot navigation environment takes the target as the origin, the frontal normal vector of the target as the X-axis, the Z-axis is perpendicular to the ground with upward as the positive direction, and the Y-axis is established by the right-hand rule;

[0081] The local coordinate system of the mobile robot takes the position of the ToF camera installed on the robot as the origin, the robot's forward direction, which is also the forward method vector of the ToF camera's viewpoint, as the X-axis, the Z-axis is perpendicular to the ground with upward as the positive direction, and the Y-axis is established by the right-hand rule.

[0082] The ToF camera's imaging plane coordinate system has the image center as the origin, the row direction of the pixels as the X direction (positive to the right), and the column direction as the Y direction (positive upwards). The depth image and grayscale image returned by the ToF camera use the same image plane coordinate system.

[0083] The DGP algorithm is a positioning algorithm developed based on the characteristics of ToF camera active ranging principle to obtain depth information, and includes the following process:

[0084] 1) Use the DGP algorithm to identify targets and perform semantic segmentation:

[0085] The DGP algorithm performs feature matching between the prior image of the object and the real-time image returned by Time-of-Flight (ToF) to coarsely locate the object. The center coordinates of the object are denoted as (x...) in the camera imaging plane coordinate system of the grayscale image. o ,y o And obtain the depth value D of that point in the ToF depth image. init ;

[0086] A single object appears as having continuous depth information in a Time-of-F (ToF) depth image. The DGP algorithm uses this as its core principle to find abrupt changes in depth information in order to determine the object's edges.

[0087] Leveraging the high accuracy of depth information acquired by ToF cameras, the DGP algorithm can determine the edge pixel coordinates of the object to be identified by detecting abrupt changes in depth values ​​between adjacent pixels, thereby obtaining the center coordinates (x, y) of the target on the image plane. o ,y o ) and its two edge depth values ​​D LeftEdge With D RightEdge ;

[0088] 2) Using the DGP algorithm to calculate the pose of the mobile robot:

[0089] Let the actual width of the target be W0. In the global coordinate system of the mobile robot navigation environment, we have:

[0090]

[0091] From the trigonometric relationship, we know that:

[0092]

[0093]

[0094] In the global coordinate system of the mobile robot navigation environment, the mobile robot coordinates (x... robot ,y robot Let Φ be the angle between the line connecting the origin to the origin and the X-axis, and α be the angle between the line connecting the origin to the origin and the normal vector of the mobile robot. Let θ be the pose of the mobile robot. robot In the global coordinate system of the mobile robot navigation environment, it is defined as the angle between the mobile robot's normal vector and the X-axis, with counterclockwise being positive;

[0095] Let the horizontal field of view (HFOV) of the ToF camera be h, and the pixel width of the ToF returned image be w. Based on the pinhole model of the ToF camera, determine the coordinates (x, y, y) of the target in the image plane. o ,y o Calculate α:

[0096]

[0097] In the global coordinate system of the mobile robot navigation environment, Φ is calculated according to the inverse trigonometric relationship:

[0098]

[0099] Then the pose θ of the mobile robot robot Calculated by the following formula:

[0100] θ robot =π-Φ+α

[0101] Thus, the global coordinate system information of the mobile robot's navigation environment, as well as the mobile robot's pose (x, y) in the global coordinate system of the navigation environment, are obtained through the DGP algorithm. robot ,y robot ,θ robot );

[0102] The PCBC algorithm is a high-order Bézier curve path planning algorithm with attitude constraints, which comprehensively considers real-time performance, trajectory smoothness, and the accuracy of the starting and ending poses.

[0103] Obtain the starting pose (x) robot ,y robot ,θ robot ) and final pose (x t ,y t ,θ t At least four Bézier curve direction points should be used to plan the path, including (x... robot ,y robot ), (x1,y1), (x2,y2), (x t ,y t );

[0104] In the PCBC algorithm, (x1, y1) and (x2, y2) satisfy:

[0105]

[0106]

[0107] The PCBC algorithm allows the number of direction points between (x1, y1) and (x2, y2) to be increased to modify the navigation path;

[0108] The navigation path planned by the PCBC algorithm is a sequence of timestamped points, B(t) = (X... r (t),Y r (t)), where t∈[0,T] f ], T f For reference control time;

[0109] The ECFL algorithm is a feedback linearization control algorithm with error compensation. It adds an error compensation detection mechanism to the control system and improves the trajectory tracking control performance of the mobile robot.

[0110] Modeling the kinematics of the mobile robot in the global coordinate system of the mobile robot's navigation environment:

[0111]

[0112]

[0113]

[0114] Where v and ω are the linear velocity and angular velocity of the mobile robot, respectively, the model is linearized using feedback; the ECFL algorithm takes the following linearized output constant, where L is a given parameter and satisfies L≠0:

[0115] x L =x robot +L·cos(θ robot )

[0116] y L =y robot +L·sin(θ robot )

[0117] Thus, the chain model of the ECFL algorithm can be obtained as follows:

[0118]

[0119]

[0120]

[0121]

[0122] This system has inputs (v, ω) and outputs (x, ω). L ,y L Decoupled, it becomes a linear system, combined with the reference tracking trajectory sequence points (X) obtained by the PCBC algorithm. r (t),Y r (t)), for The assignment is as follows, where k is a given parameter, (x r ,y r ) represents the control time T∈[0,T f The coordinates (X) of the corresponding trajectory sequence points r (T),Y r (T)), The time derivative of the trajectory at that point is:

[0123]

[0124]

[0125] This yields the specific control variables (v, ω) for the ECFL algorithm:

[0126]

[0127]

[0128] Update (x) robot ,y robot ,θ robot This is used to update the control input for trajectory tracking control.

[0129] The method further includes: adding error compensation to the feedback linearization controller, defining the static error as:

[0130] E = (x robot -X r (T f )) 2 +(y robot -Y r (T f )) 2

[0131] When the actual control time T = T f If the static error E is greater than the acceptable error range E0, then (X) r (T f ),Y r (T f((v, ω)) is the next tracking point, calculate (v, ω) again, and continue tracking until the static error E satisfies E < E0, that is, the tracking can be stopped; relying on this tracking algorithm, the mobile robot can move forward along the set trajectory;

[0132] When the ECFL algorithm controls each step, it judges whether there are obstacles in front according to the depth map and grayscale map transmitted back by the ToF, so as to decide whether to pause the forward movement. The specific steps are as follows:

[0133] Divide the real-time depth image transmitted back by the ToF into blocks to calculate the mean value, and obtain the depth value sequence M = {m i |i ∈ [0, 1, …, n]}, and calculate the depth value sequence through two consecutive frames of depth images and Calculate Judge whether there is a depth mutation through the variables in ΔM, so as to know whether there are obstacles moving into the reference trajectory; at the same time, judge whether there is enough distance to move forward by detecting the block mean value representing the forward direction in the sequence, ensuring the safety of the mobile robot operation.

[0134] In the third aspect, a computer-readable storage medium storing a computer program is provided. When the computer program is executed by a processor, the steps of the mobile robot navigation control method are implemented.

[0135] In the fourth aspect, an electronic device is provided, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the computer program is executed by the processor, the steps of the mobile robot navigation control method are implemented.

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

[0137] 1. The present invention uses pure vision positioning and does not rely on other sensors, such as inertial measurement units (IMUs), lidar (LiDAR), etc., greatly reducing the usage cost;

[0138] 2. The system structure designed by the present invention is simple;

[0139] 3. The present invention can plan a global navigation path in real time under various types of target poses, and there are no special requirements for the poses of the target points;

[0140] 4. The prior information required by the present invention is to identify the object image, which is simple to obtain, and the feature matching method can be changed according to different requirements of real-time performance and hardware conditions, meeting various operating environment conditions;

[0141] 5. This invention comprehensively considers various unforeseen situations during the navigation process and develops a real-time obstacle avoidance algorithm, effectively ensuring the safety and reliability of the navigation process.

[0142] Other beneficial effects of the present invention will be explained in detail through the introduction of specific technical features and technical solutions in specific embodiments. Those skilled in the art should be able to understand the beneficial technical effects brought about by these technical features and technical solutions through the introduction of these technical features and technical solutions. Attached Figure Description

[0143] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

[0144] Figure 1 This is a flowchart of the method of the present invention;

[0145] Figure 2 A schematic diagram of the global coordinate system of the mobile robot's navigation environment and the local coordinate system of the mobile robot itself;

[0146] Figure 3 A schematic diagram illustrating the coordinates of a position calculation algorithm in the global coordinate system of a mobile robot's navigation environment;

[0147] Figure 4 A schematic diagram illustrating the coordinates for calculating the attitude algorithm in the global coordinate system of a mobile robot's navigation environment;

[0148] Figure 5 A schematic diagram of Bézier curve path planning in the global coordinate system of a mobile robot's navigation environment;

[0149] Figure 6 A schematic diagram illustrating the kinematic model of a mobile robot in the global coordinate system of its navigation environment. Detailed Implementation

[0150] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the scope of protection of the present invention.

[0151] This invention provides a navigation control method for a mobile robot, addressing the challenge of simplifying navigation operations and improving robustness while reducing costs and increasing accuracy. In indoor scenarios, this invention guides the mobile robot to a recognized object and aligns its frontal view with the object. The method primarily comprises three parts: initial visual localization, global path planning, and trajectory tracking.

[0152] Utilizing the depth information obtained through Time-of-Flight (ToF) active ranging principle, and the automatically aligned grayscale and depth images returned by ToF, this invention develops a depth progressive localization algorithm, namely the DGP algorithm, to achieve pose localization of a mobile robot. After specifying a navigation target point, a high-order Bézier curve path planning algorithm with attitude constraints is used to plan a global path. Finally, a feedback linearization control algorithm with error compensation is used to achieve trajectory tracking control. At the same time, an obstacle avoidance module is used to improve the navigation safety of the mobile robot. This invention uses only a ToF camera (Time of Flight Camera) for localization, and the designed system structure is simple, with high real-time control performance and accuracy.

[0153] Reference Figure 1 As shown, the method specifically includes the following:

[0154] Step S1: Define the coordinate system by setting the global coordinate system of the mobile robot's navigation environment, the local coordinate system of the mobile robot itself, and the coordinate system of the ToF camera imaging plane.

[0155] The global coordinate system of the mobile robot navigation environment takes the target as the origin, the target's frontal normal vector as the X-axis, the Z-axis as perpendicular to the ground with upward as the positive direction, and the Y-axis as determined by the right-hand rule.

[0156] The local coordinate system of the mobile robot is based on the position of the ToF camera installed on the robot as the origin, with the robot's forward direction, which is also the forward method vector of the ToF camera, as the X-axis. The Z-axis is perpendicular to the ground and upward as the positive direction, and the Y-axis is established by the right-hand rule.

[0157] like Figure 2 As shown, (X) w ,Y w () represents the global coordinate system of the mobile robot's navigation environment, (X) r ,Y r The image plane coordinate system represents the local coordinate system of the mobile robot itself; the ToF camera imaging plane coordinate system takes the image center as the origin, the row direction of the pixels is the X direction, which is positive to the right, and the column direction is the Y direction, which is positive upward; the depth image and grayscale image returned by the ToF camera use the same image plane coordinate system.

[0158] Step S2: Set prior information by inputting the grayscale image of the target to the DGP algorithm (Depth Gradient Positioning Algorithm, hereinafter referred to as DGP algorithm) as prior information for the target.

[0159] Step S3: Acquire real-time images. Use a ToF camera to capture real-time grayscale and depth images, and simultaneously input them into the DGP positioning algorithm.

[0160] Step S4: Robot localization. Real-time localization is performed using the DGP algorithm. Combining the prior information of the acquired target grayscale image with the real-time grayscale and depth image information captured by the ToF camera, semantic segmentation and localization are performed on the target to derive the pose (x, y) of the mobile robot in the global coordinate system of the navigation environment. robot ,y robot ,θ robot ).

[0161] The DGP algorithm is a positioning algorithm developed based on the characteristics of ToF camera active ranging principle to obtain depth information, and includes the following process:

[0162] 1) Use the DGP algorithm to identify targets and perform semantic segmentation:

[0163] The DGP algorithm performs feature matching between the prior image of the object and the real-time image returned by Time-of-Flight (ToF) to coarsely locate the object. The center coordinates of the object are denoted as (x...) in the camera imaging plane coordinate system of the grayscale image. o ,y o And obtain the depth value D of that point in the ToF depth image. init As a reference value for semantic segmentation.

[0164] In particular, a single object appears as having continuous depth information in a ToF depth image. The DGP algorithm uses this as its core to find abrupt changes in depth information in order to determine the object's edges.

[0165] Leveraging the high accuracy of depth information acquired by ToF cameras, the DGP algorithm can determine the edge pixel coordinates of the object to be identified by detecting abrupt changes in depth values ​​between adjacent pixels, thereby obtaining the center coordinates (x, y) of the target on the image plane. o ,y o ) and its two edge depth values ​​D LeftEdge With D RightEdge .

[0166] 2) Using the DGP algorithm to calculate the pose of the mobile robot:

[0167] Let W0 be the true width of the target to be identified. Figure 3 As shown, in the global coordinate system of the mobile robot navigation environment, we have:

[0168]

[0169] From the trigonometric relationship, we know that:

[0170]

[0171]

[0172] like Figure 4 As shown, in the global coordinate system of the mobile robot navigation environment, the mobile robot coordinates (x... robot ,y robot Let Φ be the angle between the line connecting the origin to the origin and the X-axis, and α be the angle between the line connecting the origin to the origin and the normal vector of the mobile robot. Let θ be the pose of the mobile robot. robot In the global coordinate system of the mobile robot navigation environment, it is defined as the angle between the mobile robot's normal vector and the X-axis, with counterclockwise being positive.

[0173] Let the horizontal field of view (HFOV) of the ToF camera be h, and the pixel width of the ToF returned image be w. Based on the pinhole model of the ToF camera, determine the coordinates (x, y, y) of the target in the image plane. o ,y o Calculate α:

[0174]

[0175] In the global coordinate system of the mobile robot navigation environment, Φ is calculated according to the inverse trigonometric relationship:

[0176]

[0177] Then the pose θ of the mobile robot robot Calculated by the following formula:

[0178] θ robot =π-Φ+α

[0179] Thus, the global coordinate system information of the mobile robot's navigation environment, as well as the mobile robot's pose (x, y) in the global coordinate system of the navigation environment, are obtained through the DGP algorithm. robot ,y robot ,θ robot ).

[0180] Step S5: Determine the navigation target point. Based on the specific tasks the robot needs to complete in the usage scenario, manually determine the pose (x, y) of the robot at the navigation target point in the global coordinate system of the navigation environment. t ,y t ,θ t ) = (0, 0, π).

[0181] Step S6: Path planning. Use the PCBC algorithm (Posture-Constrained Bezier Curve, hereinafter referred to as PCBC algorithm) to plan the global navigation path from the robot's current localization pose to the target point pose.

[0182] Specifically, path planning uses the PCBC algorithm to plan a global navigation path from the robot's current localization pose to the target point pose, obtaining the starting pose (X). robot ,Y robot ,θ robot ) and final pose (x t ,Y t ,θ t After that, at least four Bézier curve direction points should be used to plan the path, including (X). robot ,Y robot ), (X1,Y1), (x2,y2), (x t ,y t );

[0183] In the PCBC algorithm, (x1, y1) and (x2, y2) satisfy:

[0184]

[0185]

[0186] The PCBC algorithm allows the number of direction points between (x1, y1) and (x2, y2) to be increased to modify the navigation path;

[0187] The navigation path planned by the PCBC algorithm is a sequence of timestamped points, B(t) = (X... r (t),Y r (t)), where t∈[0,T] f ], T f This is for reference control time.

[0188] Furthermore, take (x) robot ,y robot The four points (x1, y1), (x2, y2), and (0, 0) are used as the four direction points for constructing the higher-order Bézier curve. Figure 5 This is a schematic diagram of Bézier curve trajectory planning in the global coordinate system of the mobile robot's navigation environment; where (x robot ,y robot Let (x2, y2) be the starting point, (0, 0) be the ending point, and (x2, y2) be the point on the x-axis at (0.5x). robot To ensure the final pose of the mobile robot is parallel to the X-axis, (x1, y1) is taken as a point on the normal vector of the mobile robot (0.5x). robot ,y robot -0.5x robot ·tan(θ robot To ensure the mobile robot can turn smoothly, the fourth-order Bézier curve formula is used:

[0189] B(t)=(1-t)3 P0+3(1-t) 2 tP1+3(1-t)t 2 P2+t 3 P3

[0190] The reference tracking trajectory sequence point B(t) = (X) is obtained. r (t),Y r (t)), such as Figure 5 As shown by the dashed line.

[0191] Step S7: Trajectory tracking. The ECFL algorithm (Error-Compensated Feedback Linearization, hereinafter referred to as ECFL algorithm) is used for trajectory tracking to control the mobile robot to reach the target point pose. This includes the following steps:

[0192] like Figure 6 As shown, the kinematic model of the mobile robot is modeled in the global coordinate system of the mobile robot navigation environment:

[0193]

[0194]

[0195]

[0196] Where v and ω are the linear velocity and angular velocity of the mobile robot, respectively, the model is linearized by feedback. Feedback linearization has the advantages of accurate linearization over a wide range, is relatively simple to implement, and is computationally fast, effectively improving the real-time performance of trajectory tracking. The ECFL algorithm takes the following linearized output constant, where L is a given parameter and satisfies L≠0:

[0197] x L =x robot +L·cos(θ robot )

[0198] y L =y robot +L·sin(θ robot )

[0199] Thus, the special chain model of the ECFL algorithm can be obtained as follows:

[0200]

[0201]

[0202]

[0203]

[0204] This system has inputs (v, ω) and outputs (x, ω). L ,y L Decoupled, it becomes a linear system, combined with the reference tracking trajectory sequence points (X) obtained by the PCBC algorithm. r (t),Y r (t)), for The assignment is as follows, where k is a given parameter, (x r ,y r ) represents the control time T∈[0,T f The coordinates (X) of the corresponding trajectory sequence points r (T),Y r (T)), The time derivative of the trajectory at that point is:

[0205]

[0206]

[0207] This yields the specific control variables (v, ω) for the ECFL algorithm:

[0208]

[0209]

[0210] Update (x) robot ,y robot ,θ robot This is used to update the control input for trajectory tracking control.

[0211] Specifically, considering the possibility that a mobile robot might stop before reaching the target point within the reference control time given by the reference trajectory due to variations in the mobile robot's load, this invention adds error compensation to the feedback linearization controller, defining the static error as:

[0212] E = (x robot -X r (T f )) 2 +(y robot -Y r (T f )) 2

[0213] When the actual control time T = T f If the static error E is greater than the acceptable error range E0, then (X) r (T f ),Y r (T f)(v, ω) is the next tracking point, calculate (v, ω) again and continue tracking until the static error E satisfies E < E0, then the tracking can be stopped; relying on this tracking algorithm, the mobile robot can move forward along the set trajectory.

[0214] In each step of control, the ECFL algorithm determines whether there is an obstacle in front according to the depth map and grayscale map transmitted by the ToF, so as to decide whether to pause the forward movement. The specific steps are as follows:

[0215] Divide the real-time depth image transmitted by the ToF into blocks and calculate the mean value to obtain the depth value sequence M = {m i |i ∈ [0, 1, …, n]}. The depth value sequence can be calculated through two consecutive frames of depth images and Calculate Judge whether there is a depth mutation through the variables in ΔM, so as to know whether there is an obstacle moving into the reference trajectory; at the same time, judge whether there is enough distance to move forward in front by detecting the block mean value representing the forward direction in the sequence, ensuring the safety of the mobile robot's operation.

[0216] The present invention also provides a mobile robot navigation control system, which can be realized by executing the process steps of the mobile robot navigation control method. That is, those skilled in the art can understand the mobile robot navigation control method as a preferred implementation manner of the mobile robot navigation control system. The system specifically includes:

[0217] Module M1: Define the coordinate system, set the global coordinate system of the mobile robot navigation environment, the local coordinate system of the mobile robot itself, and the coordinate system of the ToF camera imaging plane.

[0218] The global coordinate system of the mobile robot navigation environment takes the recognized target as the origin, the positive normal vector of the front of the recognized target as the X-axis, the Z-axis is perpendicular to the ground, and the upward direction is the positive direction. The Y-axis is established according to the right-hand rule.

[0219] The local coordinate system of the mobile robot itself takes the position of the ToF installed on the robot as the origin, the forward direction of the robot, which is also the positive normal vector in front of the ToF camera view, as the X-axis, the Z-axis is perpendicular to the ground, and the upward direction is the positive direction. The Y-axis is established according to the right-hand rule.

[0220] As Figure 2 shown, (X w , Y w ) represents the global coordinate system of the mobile robot navigation environment, (X r , Y rThe image plane coordinate system represents the local coordinate system of the mobile robot itself; the ToF camera imaging plane coordinate system takes the image center as the origin, the row direction of the pixels is the X direction, which is positive to the right, and the column direction is the Y direction, which is positive upward; the depth image and grayscale image returned by the ToF camera use the same image plane coordinate system.

[0221] Module M2: Sets prior information and inputs the grayscale image of the target to the DGP algorithm (Depth Gradient Positioning Algorithm, hereinafter referred to as DGP algorithm) as prior information for the target.

[0222] Module M3: Acquires real-time images, uses a ToF camera to capture real-time grayscale and depth images, and synchronously inputs them into the DGP positioning algorithm.

[0223] Module M4: Robot localization. It uses the DGP algorithm for real-time localization, combining prior information from the acquired target grayscale image with real-time grayscale and depth images captured by the ToF camera. This allows for semantic segmentation and localization of the target, deriving the mobile robot's pose (x, y) in the global coordinate system of the navigation environment. robot ,y robot ,θ robot ).

[0224] The DGP algorithm is a positioning algorithm developed based on the characteristics of ToF camera active ranging principle to obtain depth information, and includes the following process:

[0225] 1) Use the DGP algorithm to identify targets and perform semantic segmentation:

[0226] The DGP algorithm performs feature matching between the prior image of the object and the real-time image returned by Time-of-Flight (ToF) to coarsely locate the object. The center coordinates of the object are denoted as (x...) in the camera imaging plane coordinate system of the grayscale image. o ,y o And obtain the depth value D of that point in the ToF depth image. init As a reference value for semantic segmentation.

[0227] In particular, a single object appears as having continuous depth information in a ToF depth image. The DGP algorithm uses this as its core to find abrupt changes in depth information in order to determine the object's edges.

[0228] Leveraging the high accuracy of depth information acquired by ToF cameras, the DGP algorithm can determine the edge pixel coordinates of the object to be identified by detecting abrupt changes in depth values ​​between adjacent pixels, thereby obtaining the center coordinates (x, y) of the target on the image plane. o ,y o ) and its two edge depth values ​​D LeftEdge With DRightEdge .

[0229] 2) Using the DGP algorithm to calculate the pose of the mobile robot:

[0230] Let W0 be the true width of the target to be identified. Figure 3 As shown, in the global coordinate system of the mobile robot navigation environment, we have:

[0231]

[0232] From the trigonometric relationship, we know that:

[0233]

[0234]

[0235] like Figure 4 As shown, in the global coordinate system of the mobile robot navigation environment, the mobile robot coordinates (x... robot ,y robot Let Φ be the angle between the line connecting the origin to the origin and the X-axis, and α be the angle between the line connecting the origin to the origin and the normal vector of the mobile robot. Let θ be the pose of the mobile robot. robot In the global coordinate system of the mobile robot navigation environment, it is defined as the angle between the mobile robot's normal vector and the X-axis, with counterclockwise being positive.

[0236] Let the horizontal field of view (HFOV) of the ToF camera be h, and the pixel width of the ToF returned image be w. Based on the pinhole model of the ToF camera, determine the coordinates (x, y, y) of the target in the image plane. o ,y o Calculate α:

[0237]

[0238] In the global coordinate system of the mobile robot navigation environment, Φ is calculated according to the inverse trigonometric relationship:

[0239]

[0240] Then the pose θ of the mobile robot robot Calculated by the following formula:

[0241] θ robot =π-Φ+α

[0242] Thus, the global coordinate system information of the mobile robot's navigation environment, as well as the mobile robot's pose (x, y) in the global coordinate system of the navigation environment, are obtained through the DGP algorithm. robot ,y robot ,θ robot ).

[0243] Module M5: Determines the navigation target point. Based on the specific tasks the robot needs to complete in the usage scenario, the pose (x, y) of the robot's navigation target point in the global coordinate system of the navigation environment is manually determined. t ,y t ,θ t ) = (0, 0, π).

[0244] Module M6: Path planning, using the PCBC algorithm (Posture-Constrained Bezier Curve, hereinafter referred to as PCBC algorithm) to plan the global navigation path from the robot's current localization pose to the target point pose.

[0245] Specifically, path planning uses the PCBC algorithm to plan a global navigation path from the robot's current localization pose to the target point pose, obtaining the starting pose (X). robot ,Y robot ,θ robot ) and final pose (x t ,Y t ,θ t After that, at least four Bézier curve direction points should be used to plan the path, including (X). robot ,Y robot ), (X1,Y1), (x2,y2), (x t ,y t );

[0246] In the PCBC algorithm, (x1, y1) and (x2, y2) satisfy:

[0247]

[0248]

[0249] The PCBC algorithm allows the number of direction points between (x1, y1) and (x2, y2) to be increased to modify the navigation path;

[0250] The navigation path planned by the PCBC algorithm is a sequence of timestamped points, B(t) = (X... r (t),Y r (t)), where t∈[0,T] f ], T f This is for reference control time.

[0251] Furthermore, take (x) robot ,y robot The four points (x1, y1), (x2, y2), and (0, 0) are used as the four direction points for constructing the higher-order Bézier curve. Figure 5This is a schematic diagram of Bézier curve trajectory planning in the global coordinate system of the mobile robot's navigation environment; where (x robot ,y robot Let (x2, y2) be the starting point, (0, 0) be the ending point, and (x2, y2) be the point on the x-axis at (0.5x). robot To ensure the final pose of the mobile robot is parallel to the X-axis, (x1, y1) is taken as a point on the normal vector of the mobile robot (0.5x). robot ,y robot -0.5x robot ·tan(θ robot To ensure the mobile robot can turn smoothly, the fourth-order Bézier curve formula is used:

[0252] B(t)=(1-t) 3 P0+3(1-t) 2 tP1+3(1-t)t 2 P2+t 3 P3

[0253] The reference tracking trajectory sequence point B(t) = (X) is obtained. r (t),Y r (t)), such as Figure 5 As shown by the dashed line.

[0254] Module M7: Trajectory tracking. This module uses the ECFL algorithm (Error-Compensated Feedback Linearization, hereinafter referred to as ECFL) to track the trajectory and control the mobile robot to reach the target point pose. It includes the following steps:

[0255] like Figure 6 As shown, the kinematic model of the mobile robot is modeled in the global coordinate system of the mobile robot navigation environment:

[0256]

[0257]

[0258]

[0259] Where v and ω are the linear velocity and angular velocity of the mobile robot, respectively, the model is linearized by feedback. Feedback linearization has the advantages of accurate linearization over a wide range, is relatively simple to implement, and is computationally fast, effectively improving the real-time performance of trajectory tracking. The ECFL algorithm takes the following linearized output constant, where L is a given parameter and satisfies L≠0:

[0260] x L =x robot +L·cos(θrobot )

[0261] y L =y robot +L·sin(θ robot )

[0262] Thus, the special chain model of the ECFL algorithm can be obtained as follows:

[0263]

[0264]

[0265]

[0266]

[0267] This system has inputs (v, ω) and outputs (x, ω). L ,y L Decoupled, it becomes a linear system, combined with the reference tracking trajectory sequence points (X) obtained by the PCBC algorithm. r (t),Y r (t)), for The assignment is as follows, where k is a given parameter, (x r ,y r ) represents the control time T∈[0,T f The coordinates (X) of the corresponding trajectory sequence points r (T),Y r (T)), The time derivative of the trajectory at that point is:

[0268]

[0269]

[0270] This yields the specific control variables (v, ω) for the ECFL algorithm:

[0271]

[0272]

[0273] Update (x) robot ,y robot ,θ robot This is used to update the control input for trajectory tracking control.

[0274] Specifically, considering the possibility that a mobile robot might stop before reaching the target point within the reference control time given by the reference trajectory due to variations in the mobile robot's load, this invention adds error compensation to the feedback linearization controller, defining the static error as:

[0275] E = (x robot - X r (T f )) 2 + (y robot - Y r (T f )) 2

[0276] When the actual control time T = T f , if the static error E is greater than the acceptable error range E0, then with (X r (T f ), Y r (T f )) as the next tracking point, calculate (v, ω) again and continue to track until the static error E satisfies E < E0, that is, the tracking can be stopped; relying on this tracking algorithm, the mobile robot can move forward along the set trajectory.

[0277] When the ECFL algorithm performs each step of control, it judges whether there are obstacles in front according to the depth map and grayscale map transmitted back by the ToF, so as to decide whether to pause the forward movement. The specific steps are as follows:

[0278] Divide the real-time depth image transmitted back by the ToF into blocks and calculate the mean value, and the depth value sequence M = {m i |i ∈ [0, 1,..., n]} can be obtained. Through two consecutive frames of depth images, the depth value sequence and Calculate Judge whether there is a depth mutation through the variables in ΔM, so as to know whether there are obstacles moving into the reference trajectory; at the same time, judge whether there is enough distance to move forward by detecting the block mean value representing the forward direction in the sequence, ensuring the safety of the mobile robot operation.

[0279] The embodiment of the present invention provides a mobile robot navigation control method and system, which makes full use of the image and depth information characteristics of the ToF camera to realize the navigation control of the mobile robot, and the system structure of the present invention is simple, and the control has high real-time performance and accuracy.

[0280] Those skilled in the art will understand that, besides implementing the system and its various devices, modules, and units provided by this invention in the form of purely computer-readable program code, the same functions can be achieved entirely through logical programming of the method steps, making the system and its various devices, modules, and units of this invention function in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers. Therefore, the system and its various devices, modules, and units provided by this invention can be considered as a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; alternatively, the devices, modules, and units for implementing various functions can be considered as both software modules implementing the method and structures within the hardware component.

[0281] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A mobile robot navigation control method, characterized by, include: Step S1: Define the coordinate system by setting the global coordinate system of the mobile robot's navigation environment, the local coordinate system of the mobile robot itself, and the coordinate system of the ToF camera's imaging plane. Step S2: Set prior information by inputting the grayscale image of the target to the DGP algorithm as prior information for the target; Step S3: Acquire real-time images. Use a ToF camera to capture real-time grayscale and depth images, and simultaneously input them into the DGP positioning algorithm. Step S4: Robot positioning, real-time positioning using DGP algorithm, combining the acquired prior information of the identified target gray image with the real-time gray image and depth image information captured by the ToF camera, performing semantic segmentation and positioning on the identified target, and deducing the pose of the mobile robot in the global coordinate system of the navigation environment ; Step S5: determining the navigation target point. According to the specific task that the robot needs to complete in the use scenario, the pose of the navigation target point of the robot in the global coordinate system of the navigation environment is determined ; Step S6: Path planning, using the PCBC algorithm to plan a global navigation path from the robot's current localization pose to the target point pose; Step S7: Trajectory tracking. The ECFL algorithm is used for trajectory tracking to control the mobile robot to reach the target point pose. The ECFL algorithm is a feedback linearization control algorithm with error compensation. It adds an error compensation detection mechanism to the control system and improves the trajectory tracking control performance of the mobile robot. Modeling the kinematics of the mobile robot in the global coordinate system of the mobile robot's navigation environment: in, and The linear velocity and angular velocity of the mobile robot are given, and the model is linearized using feedback. The ECFL algorithm takes the following linearized output constants, where... Given parameters and satisfying : Thus, the chain model of the ECFL algorithm can be obtained as follows: This system input and output Decoupling, for a linear system, combined with the reference tracking trajectory sequence points obtained by the PCBC algorithm. ,right The assignment is as follows, where Given parameters, To control the timing Coordinates of the corresponding trajectory sequence points , The time derivative of the trajectory at that point is: That is, the control input of the ECFL algorithm is obtained. : renew This is used to update the control input for trajectory tracking control.

2. The mobile robot navigation control method according to claim 1, characterized in that, The global coordinate system of the mobile robot navigation environment takes the target as the origin, the frontal normal vector of the target as the X-axis, the Z-axis as perpendicular to the ground with upward as the positive direction, and the Y-axis as determined by the right-hand rule. The local coordinate system of the mobile robot takes the position of the ToF camera installed on the robot as the origin, the robot's forward direction, which is also the forward method vector of the ToF camera's viewpoint, as the X-axis, the Z-axis is perpendicular to the ground with upward as the positive direction, and the Y-axis is established by the right-hand rule. The ToF camera's imaging plane coordinate system has the image center as the origin, the row direction of the pixels as the X direction (positive to the right), and the column direction as the Y direction (positive upwards). The depth image and grayscale image returned by the ToF camera use the same image plane coordinate system.

3. The mobile robot navigation control method according to claim 1, characterized in that, The DGP algorithm is a positioning algorithm developed based on the characteristics of ToF camera active ranging principle to obtain depth information, and includes the following process: 1) Use the DGP algorithm to identify targets and perform semantic segmentation: The DGP algorithm performs feature matching between the prior image of the object and the real-time image returned by Time-of-Flight (ToF) to coarsely locate the object. The center coordinates of the object are denoted as follows in the camera imaging plane coordinate system of the grayscale image: And obtain the depth value of that point in the ToF depth image. ; A single object appears as having continuous depth information in a Time-of-F (ToF) depth image. The DGP algorithm uses this as its core principle to find abrupt changes in depth information in order to determine the object's edges. Leveraging the high accuracy of depth information acquired by ToF cameras, the DGP algorithm can determine the edge pixel coordinates of the object to be identified by detecting abrupt changes in depth values ​​between adjacent pixels, thereby obtaining the center coordinates of the target on the image plane. and the depth values ​​of its two edges and ; 2) Using the DGP algorithm to calculate the pose of the mobile robot: Let the true width of the target be denoted as . In the global coordinate system of the mobile robot navigation environment, we have: From the trigonometric relationship, we know that: In the global coordinate system of the mobile robot navigation environment, the mobile robot coordinates are ( Let the angle between the line connecting the origin to the origin and the X-axis be... Let the angle between the vector and the normal vector of the mobile robot be set as The posture of the mobile robot In the global coordinate system of the mobile robot navigation environment, it is defined as the angle between the mobile robot's normal vector and the X-axis, with counterclockwise being positive; Take the horizontal field of view (HFOV) of the ToF camera as Take the pixel width of the ToF returned image as Based on the pinhole model of the ToF camera, the coordinates of the target in the image plane are obtained. calculate : In the global coordinate system of the mobile robot navigation environment, the inverse trigonometric relationship is used for calculation. : Then the posture of the mobile robot Calculated by the following formula: Thus, the global coordinate system information of the mobile robot's navigation environment, as well as the mobile robot's pose in the global coordinate system of the navigation environment, can be obtained through the DGP algorithm. .

4. The mobile robot navigation control method according to claim 1, characterized in that, The PCBC algorithm is a high-order Bézier curve path planning algorithm with attitude constraints, which comprehensively considers real-time performance, trajectory smoothness, and the accuracy of the starting and ending poses. Obtain the starting pose and final pose At least four Bézier curve direction points are used to plan the path, including , , , ; PCBC algorithm and satisfy: The PCBC algorithm allows for... , Add more directional points between points to modify the navigation path; The navigation path planned by the PCBC algorithm is a sequence of timestamped points. ,in , This is for reference control time.

5. The mobile robot navigation control method according to claim 1, characterized in that, The method further includes: adding error compensation to the feedback linearization controller, defining the static error as: When actual control time When, if static error Greater than the acceptable error range So, with For the next tracking point, calculate again. Continue tracking until the static error is reached. satisfy This means that tracking can be stopped; relying on this tracking algorithm, the mobile robot can move along the set trajectory. At each control step, the ECFL algorithm determines whether there are obstacles ahead based on the depth map and grayscale image returned by the Time-of-Flight (ToF) system, thus deciding whether to pause the movement. The specific steps are as follows: The real-time depth image returned by Time-of-Flight (ToF) is divided into blocks, and the average value is calculated to obtain a depth value sequence. A depth value sequence is obtained by calculating two consecutive depth images. and ,calculate ,pass The system uses variables to determine if there are depth abrupt changes, thus identifying any obstacles that may have moved into the reference trajectory. Simultaneously, it uses the average value of blocks representing the direction of travel in the detection sequence to determine if there is sufficient distance to move forward, ensuring the safety of the mobile robot's operation.

6. A mobile robot navigation control system, characterized in that, The mobile robot navigation control method for performing any one of claims 1 to 5 includes: Module M1: Defines the coordinate system, setting the global coordinate system of the mobile robot's navigation environment, the local coordinate system of the mobile robot itself, and the coordinate system of the ToF camera's imaging plane; Module M2: Sets prior information and inputs the grayscale image of the target to the DGP algorithm as prior information for the target; Module M3: Acquires real-time images, uses a ToF camera to capture real-time grayscale and depth images, and synchronously inputs them into the DGP positioning algorithm; Module M4: Robot localization. It uses the DGP algorithm for real-time localization, combining prior information from the acquired grayscale image of the target with real-time grayscale and depth images captured by the ToF camera. This allows for semantic segmentation and localization of the target, deriving the mobile robot's pose in the global coordinate system of the navigation environment. ; Module M5: Determines the navigation target point. Based on the specific tasks the robot needs to complete in the usage scenario, it determines the pose of the robot at the navigation target point in the global coordinate system of the navigation environment. ; Module M6: Path planning, using the PCBC algorithm to plan the global navigation path from the robot's current localization pose to the target point pose; Module M7: Trajectory tracking, uses the ECFL algorithm for trajectory tracking, and controls the mobile robot to reach the target point pose.

7. The mobile robot navigation control system according to claim 6, characterized in that, The global coordinate system of the mobile robot navigation environment takes the target as the origin, the frontal normal vector of the target as the X-axis, the Z-axis as perpendicular to the ground with upward as the positive direction, and the Y-axis as determined by the right-hand rule. The local coordinate system of the mobile robot takes the position of the ToF camera installed on the robot as the origin, the robot's forward direction, which is also the forward method vector of the ToF camera's viewpoint, as the X-axis, the Z-axis is perpendicular to the ground with upward as the positive direction, and the Y-axis is established by the right-hand rule. The ToF camera's imaging plane coordinate system has the image center as the origin, the row direction of the pixels as the X direction (positive to the right), and the column direction as the Y direction (positive upwards). The depth image and grayscale image returned by the ToF camera use the same image plane coordinate system. The DGP algorithm is a positioning algorithm developed based on the characteristics of ToF camera active ranging principle to obtain depth information, and includes the following process: 1) Use the DGP algorithm to identify targets and perform semantic segmentation: The DGP algorithm performs feature matching between the prior image of the object and the real-time image returned by Time-of-Flight (ToF) to coarsely locate the object. The center coordinates of the object are denoted as follows in the camera imaging plane coordinate system of the grayscale image: And obtain the depth value of that point in the ToF depth image. ; A single object appears as having continuous depth information in a Time-of-F (ToF) depth image. The DGP algorithm uses this as its core principle to find abrupt changes in depth information in order to determine the object's edges. Leveraging the high accuracy of depth information acquired by ToF cameras, the DGP algorithm can determine the edge pixel coordinates of the object to be identified by detecting abrupt changes in depth values ​​between adjacent pixels, thereby obtaining the center coordinates of the target on the image plane. and the depth values ​​of its two edges and ; 2) Using the DGP algorithm to calculate the pose of the mobile robot: Let the true width of the target be denoted as . In the global coordinate system of the mobile robot navigation environment, we have: From the trigonometric relationship, we know that: In the global coordinate system of the mobile robot navigation environment, the mobile robot coordinates are ( Let the angle between the line connecting the origin to the origin and the X-axis be... Let the angle between the vector and the normal vector of the mobile robot be set as The posture of the mobile robot In the global coordinate system of the mobile robot navigation environment, it is defined as the angle between the mobile robot's normal vector and the X-axis, with counterclockwise being positive; Take the horizontal field of view (HFOV) of the ToF camera as Take the pixel width of the ToF returned image as Based on the pinhole model of the ToF camera, the coordinates of the target in the image plane are obtained. calculate : In the global coordinate system of the mobile robot navigation environment, the inverse trigonometric relationship is used for calculation. : Then the posture of the mobile robot Calculated by the following formula: Thus, the global coordinate system information of the mobile robot's navigation environment, as well as the mobile robot's pose in the global coordinate system of the navigation environment, can be obtained through the DGP algorithm. The PCBC algorithm is a high-order Bézier curve path planning algorithm with attitude constraints, which comprehensively considers real-time performance, trajectory smoothness, and the accuracy of the starting and ending poses. Obtain the starting pose and final pose At least four Bézier curve direction points are used to plan the path, including , , , ; PCBC algorithm and satisfy: The PCBC algorithm allows for... , Add more directional points between points to modify the navigation path; The navigation path planned by the PCBC algorithm is a sequence of timestamped points. ,in , For reference control time; The ECFL algorithm is a feedback linearization control algorithm with error compensation. It adds an error compensation detection mechanism to the control system and improves the trajectory tracking control performance of the mobile robot. Modeling the kinematics of the mobile robot in the global coordinate system of the mobile robot's navigation environment: in, and The linear velocity and angular velocity of the mobile robot are given, and the model is linearized using feedback. The ECFL algorithm takes the following linearized output constants, where... Given parameters and satisfying : Thus, the chain model of the ECFL algorithm can be obtained as follows: This system input and output Decoupling, for a linear system, combined with the reference tracking trajectory sequence points obtained by the PCBC algorithm. ,right The assignment is as follows, where Given parameters, To control the timing Coordinates of the corresponding trajectory sequence points , The time derivative of the trajectory at that point is: That is, the control input of the ECFL algorithm is obtained. : renew To update the control input for trajectory tracking control; The method further includes: adding error compensation to the feedback linearization controller, defining the static error as: When actual control time When, if static error Greater than the acceptable error range So, with For the next tracking point, calculate again. Continue tracking until the static error is reached. satisfy This means that tracking can be stopped; relying on this tracking algorithm, the mobile robot can move along the set trajectory. At each control step, the ECFL algorithm determines whether there are obstacles ahead based on the depth map and grayscale image returned by the Time-of-Flight (ToF) system, thus deciding whether to pause the movement. The specific steps are as follows: The real-time depth image returned by Time-of-Flight (ToF) is divided into blocks, and the average value is calculated to obtain a depth value sequence. A depth value sequence is obtained by calculating two consecutive depth images. and ,calculate ,pass The system uses variables to determine if there are depth abrupt changes, thus identifying any obstacles that may have moved into the reference trajectory. Simultaneously, it uses the average value of blocks representing the direction of travel in the detection sequence to determine if there is sufficient distance to move forward, ensuring the safety of the mobile robot's operation.

8. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the mobile robot navigation control method according to any one of claims 1 to 5.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the computer program is executed by the processor, it implements the steps of the mobile robot navigation control method according to any one of claims 1 to 5.

Images