A trajectory tracking method

Abstract

The present application discloses a trajectory tracking method for controlling the linear velocity and angular velocity of a mobile robot moving on a first plane so that the mobile robot tracks a trajectory set on the first plane. The control method is characterized in that the movement of the mobile robot causes the same feature point captured by a camera fixed thereto to move in different images at different times, and the control linear velocity and control angular velocity of the mobile robot are calculated based on the movement of the feature point and the stored orientation of the camera's actual starting point in the world coordinate system, the linear velocity captured by the linear velocity sensor, and the angular velocity captured by the angular velocity sensor. The above solution solves the problem of excessive deviation in the mobile robot's own position estimation caused by relying solely on linear velocity sensors and angular velocity sensors, and achieves tracking of a fixed trajectory.

Description

Technical Field

[0001] The present application relates to the field of mobile robots, and in particular to a trajectory tracking method. Background Art

[0002] With the development of robots, robots have been widely used due to their advantages of flexibility, increased productivity, improved product quality, improved working conditions, etc. At present, mobile robots have appeared more and more in our daily life and industrial production.

[0003] To address the tracking control problem of mobile robots, researchers have developed discontinuous controllers, time-varying controllers, and hybrid controllers. However, these control methods rely on the ability to accurately determine the real-time position of the mobile robot itself, requiring the use of a global positioning system. However, such positioning is often expensive and difficult to achieve in typical working environments.

[0004] Without the support of a global positioning system, relying solely on the integration of the mobile robot's linear velocity sensor and angular velocity sensor to determine the mobile robot's position proved inaccurate and incapable of effective trajectory tracking. At the same time, it was also found that relying on the integration of the mobile robot's angular velocity sensor to determine the mobile robot's orientation was sufficiently accurate. Summary of the Invention

[0005] The purpose of this application is to overcome the above-mentioned defects or problems in the background technology and to provide a trajectory tracking method for controlling a mobile robot, which can track a pre-stored trajectory without relying on a global positioning system.

[0006] In order to achieve the above objectives, the following technical solutions are adopted:

[0007] A trajectory tracking method is used to control the linear velocity and angular velocity of a mobile robot moving on a first plane so that it tracks the trajectory set for the mobile robot on the first plane, wherein the trajectory has been stored in the mobile robot and is expressed as a position of a set starting point in a world coordinate system based on the mobile robot relative to the first plane and an expected linear velocity and an expected angular velocity at each moment; the mobile robot is fixed with a camera, which is used to photograph at least two feature points on the first plane with a height perpendicular to the first plane, and the height is allowed to be greater than or equal to zero; the direction of the actual starting point of the camera in the world coordinate system is known; the mobile robot is also equipped with a linear velocity sensor and an angular velocity sensor; the trajectory tracking method The method is as follows: sampling the current linear velocity and current angular velocity of the mobile robot at each specific interval, and taking pictures with its camera; based on the above-mentioned sampling data and the pictures, calculating the current control linear velocity and current control angular velocity of the mobile robot by the following method, and storing the relevant intermediate values ​​in the calculation process; if the current control linear velocity and the current control angular velocity cannot be obtained by calculation, then controlling the mobile robot to maintain the linear velocity and angular velocity at the previous moment or controlling the mobile robot to move arbitrarily within the allowed range; if the current control linear velocity and the current control angular velocity can be obtained by calculation, then controlling the movement of the mobile robot according to the current control linear velocity and the current control angular velocity; wherein:

[0008] and

[0009] in,

[0010]

[0011]

[0012] in, and Depend on and Points are obtained; and Obtained by the following formula:

[0013] This formula is called an observer;

[0014] In addition, it is necessary to calculate and store Depend on Points are obtained;

[0015]

[0016]

[0017] in,

[0018]

[0019]

[0020]

[0021]

[0022]

[0023]

[0024]

[0025]

[0026]

[0027]

[0028]

[0029]

[0030]

[0031]

[0032]

[0033]

[0034]

[0035] In the above formulas, v c is the control linear velocity at the current moment; ω c is the control angular velocity at the current moment; v d is the expected linear velocity corresponding to the current moment; ω d is the expected angular velocity corresponding to the current moment; k1, k2, k3 are all artificially set constants; θ e is the estimated tracking error transformed from the world coordinate system to the mobile robot coordinate system at the current moment; where the mobile robot coordinate system always uses the current camera orientation as the x-axis; θ is the angle of the mobile robot in the world coordinate system at the current moment, which is calculated by integrating the orientation of the camera's actual starting point in the world coordinate system and the angular velocity at all previous moments; is the estimated position error of the mobile robot in the world coordinate system at the current moment; Δθ is the angular error of the mobile robot in the world coordinate system at the current moment; x d ,y d is the expected position of the mobile robot in the world coordinate system at the current moment, which is calculated by integrating the position of the set starting point of the mobile robot in the world coordinate system and the expected linear velocity and expected angle at all previous moments; θ d is the desired angle of the robot at the current moment; The estimated position of the mobile robot in the world coordinate system at the current moment; The speed of the mobile robot on the x-axis and the speed of the mobile robot on the y-axis in the world coordinate system at the current moment; m×n It is used to represent a zero matrix of dimension m×n; N is the number of feature points in the picture taken at the current moment; Ψ and Φ are both diagonal parameter constant matrices set by the observer; A is the intermediate variable of the observer at the current moment; W is the transformation matrix of the overall image error of the observer at the current moment; E is the overall image error of the picture taken at the current moment; is the estimated position of all feature points in the world coordinate system at the current moment, Points are obtained; It is used to represent the estimated position of the i-th feature point in the world coordinate system at the current moment; T is used to represent the transpose of the matrix; Used to represent the estimated position of the i-th feature point on the x-axis in the world coordinate system at the current moment; Used to represent the estimated position of the i-th feature point on the y-axis in the world coordinate system at the current moment; It is used to represent the estimated position of the i-th feature point on the z-axis in the world coordinate system at the current moment; v is the linear velocity at the current moment obtained by sampling; ω is the angular velocity at the current moment obtained by sampling; is the angular velocity of the mobile robot in the world coordinate system at the current moment, which is equal to ω; is the estimated tracking error transformed from the world coordinate system to the mobile robot coordinate system at the last moment; is the estimated position error of the mobile robot in the world coordinate system at the last moment; W is the estimated position of the mobile robot in the world coordinate system at the last moment; i The transformation matrix used to represent the image error of the i-th feature point in the observer at the current moment; G is the constant coefficient matrix in W; δ i ,γ i W i The intermediate variable in δ i1,2 is δ i The first and second columns of γ i1,2 γ iThe first and second columns of m i It is used to represent the i-th row of the camera parameter matrix M; R is the rotation matrix of the mobile robot coordinate system relative to the world coordinate system; The coordinates of the i-th feature point in the image taken at the current moment in the image coordinate system; is the velocity of the i-th feature point in the image coordinate system at the current moment, taken by the current moment The coordinates of the same feature points stored previously are obtained by differential calculation; α i ,β i is the intermediate variable of the camera model; I n×n Used to represent the identity matrix of order n; E i It is used to represent the image error of the i-th feature point in the picture taken at the current moment; E i1 ,E i2 For calculating E i The intermediate variable of is the estimated depth of the i-th feature point in the picture taken at the current moment; The estimated position of the i-th feature point on the x-axis in the world coordinate system calculated at the last moment; The estimated position of the i-th feature point on the y-axis in the world coordinate system calculated at the last moment; The estimated position of the i-th feature point on the z-axis in the world coordinate system calculated at the last moment is stored.

[0036] Furthermore, the mobile robot autonomously selects feature points in the picture taken by the camera using a feature point selection algorithm.

[0037] Furthermore, the feature point selection algorithm adopts the SURF feature point capture algorithm and / or the HARIS feature point capture algorithm.

[0038] Furthermore, the matching of feature points in the two images is achieved using the optical flow method.

[0039] Furthermore, k1, k2, and k3 are all positive values. By adjusting k1, k2, and k3, the current control linear velocity and the current control angular velocity meet the maximum allowable linear velocity and the maximum allowable angular velocity.

[0040] Compared with the prior art, the above solution has the following beneficial effects:

[0041] By adopting the above control method, it is possible to track the pre-stored trajectory without relying on the global positioning system. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] In order to more clearly illustrate the technical solution of the embodiment, the following briefly introduces the drawings required for use:

[0043] Figure 1 Indicates the relationship between the world coordinate system and the mobile robot coordinate system;

[0044] Figure 2 Represents the image coordinate system on the picture;

[0045] Figure 3 It represents the operation of the mobile robot when it follows a circular trajectory;

[0046] Figure 4 Represents the operation of a mobile robot when it follows a sinusoidal trajectory.

[0047] Description of main reference numerals:

[0048] Mobile robot 1; camera 2; image 3. DETAILED DESCRIPTION

[0049] In the claims and the description, unless otherwise defined, the terms "first", "second" or "third", etc. are intended to distinguish different objects rather than to describe a specific order.

[0050] In the claims and the specification, unless otherwise specified, the terms "center", "transverse", "longitudinal", "horizontal", "vertical", "top", "bottom", "inside", "outside", "up", "down", "front", "back", "left", "right", "clockwise", "counterclockwise" and the like to indicate directions or positional relationships are based on the directions and positional relationships shown in the accompanying drawings and are only for the convenience of simplifying the description, and do not imply that the device or element referred to must have a specific direction or be constructed and operated in a specific direction.

[0051] In the claims and description, unless otherwise specified, the term "fixed connection" or "fixed connection" should be understood in a broad sense, that is, any connection method in which there is no displacement relationship or relative rotation relationship between the two parties, that is, including non-detachable fixed connection, detachable fixed connection, integral connection, and fixed connection through other devices or elements.

[0052] In the claims and the description, unless otherwise defined, the terms "include", "have" and their variations mean "including but not limited to".

[0053] The technical solutions in the embodiments will be described clearly and completely below with reference to the accompanying drawings.

[0054] See also Figure 1 , Figure 1A mobile robot 1 is shown, with a camera 2 fixed thereto. Camera 2 is used to capture at least two feature points on a first plane at a height perpendicular to the first plane, where the height is allowed to be greater than or equal to zero. Mobile robot 1 also has a linear velocity sensor and an angular velocity sensor.

[0055] In this embodiment, the mobile robot 1 is in the world coordinate system, and the origin of the world coordinate system is O w , whose X-axis is X w , whose Y axis is Y w The X-axis and Y-axis of the world coordinate system are both located on the first plane. The mobile robot 1 also has its own coordinate system, namely the mobile robot coordinate system, and the origin of the mobile robot coordinate system is O r , whose X-axis is X r , whose Y axis is Y r In this embodiment, the origin O of the mobile robot coordinate system r It is determined as the intersection of the imaging plane of camera 2 and the optical axis of camera 2, and the X-axis direction of the mobile robot coordinate system always coincides with the orientation of camera 2.

[0056] like Figure 2 As shown, the picture taken by camera 2 also has an image coordinate system, and the origin of the image coordinate system is O c , whose X-axis is X c , whose Y axis is Y c .

[0057] In this embodiment, the trajectory tracking method is used to control the linear velocity and angular velocity of the mobile robot 1 moving on the first plane so that the mobile robot 1 tracks the trajectory set on the first plane. The trajectory has been stored in the controller of the mobile robot 1 and is expressed as a desired linear velocity v based on the position of the mobile robot 1 relative to the set starting point in the world coordinate system and at each moment. d and the desired angular velocity ω d .

[0058] In this embodiment, the mobile robot 1 also knows and stores the orientation of the camera 1 in the world coordinate system at the actual starting point.

[0059] The trajectory tracking method in this embodiment is as follows: the current linear velocity v and the current angular velocity ω of the mobile robot 1 are sampled at a specific interval, and a picture is taken by the camera 2. Based on the above sampled data and the taken picture, the current control linear velocity v of the mobile robot 1 is calculated by the calculation method described below. c and the current control angular velocity ω c , and store the relevant intermediate values ​​in the calculation process; such as the current moment control linear velocity v c and the current control angular velocity ω cIf the linear velocity and angular velocity cannot be obtained by calculation, the mobile robot 1 is controlled to maintain the linear velocity and angular velocity at the previous moment or the mobile robot is controlled to move arbitrarily within the allowed range, such as controlling the linear velocity v at the current moment. c and the current control angular velocity ω c can be obtained by calculation, then the motion of the mobile robot is controlled according to the current control linear velocity and the current control angular velocity; generally, the control linear velocity v is only used when the mobile robot 1 is in the starting stage and the camera 2 cannot track the feature point at the previous moment due to the turning of the mobile robot 1. c and the current control angular velocity ω c This condition is short-lived and therefore does not affect trajectory tracking.

[0060] The above current moment control linear velocity v c and the current control angular velocity ω c The calculation method is as follows: and in,

[0061]

[0062]

[0063] in, and Depend on and Points are obtained; and Obtained by the following formula: This formula is called an observer;

[0064] In addition, it is necessary to calculate and store Depend on Points are obtained;

[0065]

[0066]

[0067] in,

[0068]

[0069]

[0070]

[0071]

[0072]

[0073]

[0074]

[0075]

[0076]

[0077]

[0078]

[0079]

[0080]

[0081]

[0082]

[0083]

[0084]

[0085] In the above formulas, v c is the control linear velocity at the current moment; ω c is the control angular velocity at the current moment; v d is the expected linear velocity corresponding to the current moment; ω d is the expected angular velocity corresponding to the current moment; k1, k2, k3 are all artificially set constants; θ e is the estimated tracking error transformed from the world coordinate system to the mobile robot coordinate system at the current moment; where the mobile robot coordinate system always uses the current camera orientation as the x-axis; θ is the angle of the mobile robot in the world coordinate system at the current moment, which is calculated by integrating the orientation of the camera's actual starting point in the world coordinate system and the angular velocity at all previous moments; is the estimated position error of the mobile robot in the world coordinate system at the current moment; Δθ is the angular error of the mobile robot in the world coordinate system at the current moment; x d ,y d is the expected position of the mobile robot in the world coordinate system at the current moment, which is calculated by integrating the position of the set starting point of the mobile robot in the world coordinate system and the expected linear velocity and expected angle at all previous moments; θ d is the desired angle of the robot at the current moment; The estimated position of the mobile robot in the world coordinate system at the current moment; The speed of the mobile robot on the x-axis and the speed of the mobile robot on the y-axis in the world coordinate system at the current moment; m×n It is used to represent a zero matrix of dimension m×n; N is the number of feature points in the picture taken at the current moment; Ψ and Φ are both diagonal parameter constant matrices set by the observer; A is the intermediate variable of the observer at the current moment; W is the transformation matrix of the overall image error of the observer at the current moment; E is the overall image error of the picture taken at the current moment; is the estimated position of all feature points in the world coordinate system at the current moment, Points are obtained; It is used to represent the estimated position of the i-th feature point in the world coordinate system at the current moment; T is used to represent the transpose of the matrix; Used to represent the estimated position of the i-th feature point on the x-axis in the world coordinate system at the current moment; Used to represent the estimated position of the i-th feature point on the y-axis in the world coordinate system at the current moment; It is used to represent the estimated position of the i-th feature point on the z-axis in the world coordinate system at the current moment; v is the linear velocity at the current moment obtained by sampling; ω is the angular velocity at the current moment obtained by sampling; is the angular velocity of the mobile robot in the world coordinate system at the current moment, which is equal to ω; is the estimated tracking error transformed from the world coordinate system to the mobile robot coordinate system at the last moment; is the estimated position error of the mobile robot in the world coordinate system at the last moment; W is the estimated position of the mobile robot in the world coordinate system at the last moment; i The transformation matrix used to represent the image error of the i-th feature point in the observer at the current moment; G is the constant coefficient matrix in W; δ i ,γ i W i The intermediate variable in δ i1,2 is δ i The first and second columns of γ i1,2 γ i The first and second columns of m i It is used to represent the i-th row of the camera parameter matrix M; R is the rotation matrix of the mobile robot coordinate system relative to the world coordinate system; The coordinates of the i-th feature point in the image taken at the current moment in the image coordinate system; is the velocity of the i-th feature point in the image coordinate system at the current moment, taken by the current moment The coordinates of the same feature points stored previously are obtained by differential calculation; α i ,β i is the intermediate variable of the camera model; IN×N Used to represent the identity matrix of order n; E i It is used to represent the image error of the i-th feature point in the picture taken at the current moment; E i1 ,E i2 For calculating E i The intermediate variable of is the estimated depth of the i-th feature point in the picture taken at the current moment; The estimated position of the i-th feature point on the x-axis in the world coordinate system calculated at the last moment; The estimated position of the i-th feature point on the y-axis in the world coordinate system calculated at the last moment; The estimated position of the i-th feature point on the z-axis in the world coordinate system calculated at the last moment is stored.

[0086] In this embodiment, the mobile robot 1 autonomously selects feature points in the image taken by the camera 2 using a feature point selection algorithm. Specifically, the feature point selection algorithm may adopt the SURF feature point capture algorithm and / or the HARIS feature point capture algorithm. Since the above algorithms are prior art, they will not be described in detail here. In this embodiment, the matching of feature points in the two images is achieved using the optical flow method. In this embodiment, k1, k2, and k3 are all positive values. By adjusting k1, k2, and k3, the current control linear velocity and the current control angular velocity meet the maximum allowable linear velocity and the maximum allowable angular velocity.

[0087] In order to verify the effectiveness of the above trajectory tracking method, two simulation experiments were conducted in Matlab software. Specifically:

[0088] Experiment 1: Tracing a Circular Trajectory

[0089] The expected linear velocity of the circular trajectory is a constant function, while the expected angular velocity is a discontinuous function, which is specifically expressed as follows:

[0090]

[0091] Applying the trajectory tracking method in this embodiment, k1, k2, and k3 are set to 0.5, 0.15, and 0.6 respectively, and the simulation results are as follows: Figure 3 As shown in Figure 2, the tracking goal is achieved. Figure 3 In FIG, the dotted line represents the actual motion trajectory of the mobile robot 1 , and the red line represents the set trajectory that the mobile robot 1 needs to track.

[0092] Experiment 2: Tracing a Sine Trajectory

[0093] The expected linear velocity and angular velocity of this sinusoidal trajectory are more complex time-varying functions, and the difficulty of tracking becomes correspondingly greater. The specific performance of the sinusoidal trajectory is:

[0094]

[0095] Applying the trajectory tracking method in this embodiment, k1, k2, and k3 are also set to 0.5, 0.15, and 0.6 respectively, and the simulation results are as follows: Figure 4 shown. Figure 4 In FIG, the dotted line represents the actual motion trajectory of the mobile robot 1 , and the red line represents the set trajectory that the mobile robot 1 needs to track.

[0096] The above description of the specification and embodiments is used to explain the scope of protection of the present application, but does not constitute a limitation on the scope of protection of the present application.

Claims

1. A trajectory tracking method for controlling the linear velocity and angular velocity of a mobile robot moving on a first plane so that the mobile robot tracks a trajectory set on the first plane, wherein the trajectory is stored in the mobile robot and expressed as a position of a set starting point of the mobile robot in a world coordinate system relative to the first plane and a desired linear velocity and desired angular velocity at each moment; Its characteristics are: The mobile robot is fixed with a camera, which is used to capture at least two feature points on a first plane with a height perpendicular to the first plane, and the height is allowed to be greater than or equal to zero; the direction of the actual starting point of the camera in the world coordinate system is known; the mobile robot is also equipped with a linear velocity sensor and an angular velocity sensor; the trajectory tracking method is: sampling the current linear velocity and current angular velocity of the mobile robot at each specific interval, and using its camera to take pictures, based on the above sampling data and the taken pictures, calculating the current control linear velocity and current control angular velocity of the mobile robot by the following method, and storing the relevant intermediate values ​​in the calculation process; if the current control linear velocity and the current control angular velocity cannot be obtained by calculation, the mobile robot is controlled to maintain the linear velocity and angular velocity of the previous moment or the mobile robot is controlled to move arbitrarily within an allowed range; if the current control linear velocity and the current control angular velocity can be obtained by calculation, the movement of the mobile robot is controlled according to the current control linear velocity and the current control angular velocity; in: and in, in, and Depend on and Points are obtained; and Obtained by the following formula: This formula is called an observer; In addition, it is necessary to calculate and store Depend on Points are obtained; in, In the above formulas, v c is the control line speed at the current moment; ω c is the control angular velocity at the current moment; v d is the expected linear velocity corresponding to the current moment; ω d is the expected angular velocity corresponding to the current moment; k1, k2, k3 are all artificially set constants; θ e is the estimated tracking error transformed from the world coordinate system to the mobile robot coordinate system at the current moment; where the mobile robot coordinate system always takes the current camera orientation as the x-axis; θ is the angle of the mobile robot in the world coordinate system at the current moment, which is calculated by integrating the orientation of the camera's actual starting point in the world coordinate system and the angular velocity at all previous moments; is the estimated position error of the mobile robot in the world coordinate system at the current moment; Δθ is the angular error of the mobile robot in the world coordinate system at the current moment; x d ,y d is the expected position of the mobile robot in the world coordinate system at the current moment, which is calculated by integrating the position of the set starting point of the mobile robot in the world coordinate system and the expected linear velocity and expected angle at all previous moments; θ d is the desired angle of the robot at the current moment; The estimated position of the mobile robot in the world coordinate system at the current moment; The speed of the mobile robot on the x-axis and the speed of the mobile robot on the y-axis in the world coordinate system at the current moment; 0 m×n Used to represent a zero matrix of dimension m×n; N is the number of feature points in the picture taken at the current moment; Ψ, Φ are both diagonal parameter constant matrices set artificially by the observer; A is the intermediate variable of the observer at the current moment; W is the transformation matrix of the overall image error of the observer at the current moment; E is the overall image error of the picture taken at the current moment; is the estimated position of all feature points in the world coordinate system at the current moment, Points are obtained; Used to represent the estimated position of the i-th feature point in the world coordinate system at the current moment; T is used to represent the transpose of the matrix; Used to represent the estimated position of the i-th feature point on the x-axis in the world coordinate system at the current moment; Used to represent the estimated position of the i-th feature point on the y-axis in the world coordinate system at the current moment; Used to represent the estimated position of the i-th feature point on the z-axis in the world coordinate system at the current moment; v is the linear velocity at the current moment obtained by sampling; ω is the angular velocity at the current moment obtained by sampling; is the angular velocity of the mobile robot in the world coordinate system at the current moment, which is equal to ω; is the estimated tracking error transformed from the world coordinate system to the mobile robot coordinate system at the last moment; is the estimated position error of the mobile robot in the world coordinate system at the last moment; The estimated position of the mobile robot in the world coordinate system at the last moment is stored; W i A transformation matrix for representing the image error of the i-th feature point in the observer at the current moment; G is the constant coefficient matrix in W; δ i ,γ i For Wi i The intermediate variables in δ i1,2 is δ i The first and second columns of γ i1,2 γ i The first and second columns of m i Used to represent the i-th row of the camera parameter matrix M; R is the rotation matrix of the mobile robot coordinate system relative to the world coordinate system; The coordinates of the i-th feature point in the image taken at the current moment in the image coordinate system; is the velocity of the i-th feature point in the image coordinate system at the current moment, taken by the current moment The coordinates of the same feature points as those stored previously are obtained by differential calculation; α i ,β i is the intermediate variable of the camera model; I n×n Used to represent the identity matrix of order n; E i Used to represent the image error of the i-th feature point in the picture taken at the current moment; E i1 ,E i2 For calculating E i The intermediate variable of is the estimated depth of the i-th feature point in the picture taken at the current moment; The estimated position of the i-th feature point on the x-axis in the world coordinate system calculated at the last moment; The estimated position of the i-th feature point on the y-axis in the world coordinate system calculated at the last moment; The estimated position of the i-th feature point on the z-axis in the world coordinate system calculated at the last moment is stored.

2. A trajectory tracking method according to claim 1, characterized in that: The mobile robot autonomously selects feature points in the image taken by the camera using a feature point selection algorithm.

3. A trajectory tracking method according to claim 2, characterized in that: The feature point selection algorithm adopts the SURF feature point capture algorithm and / or the HARIS feature point capture algorithm.

4. A trajectory tracking method according to claim 2, characterized in that: The matching of feature points in two images is achieved using the optical flow method.

5. A trajectory tracking method according to claim 1, characterized in that: k1, k2, k3 are all positive values. By adjusting k1, k2, k3, the current control linear velocity and the current control angular velocity meet the maximum allowable linear velocity and the maximum allowable angular velocity.

Images