Image-based trajectory planning method and motion control method, and mobile machine using these methods

Abstract

An image-based trajectory planning method, a motion control method, and a mobile machine (100) using these methods are applied to the field of trajectory planning technology. The trajectory (T2) for moving the mobile machine (100) to a reference target (T) is planned through the following steps: obtaining a desired image (Id) of the reference target (T) captured at a desired pose (Sd) of the trajectory (T2) and an initial image (Ii) of the reference target captured at an initial pose (Si) of the trajectory through a camera of the mobile machine (100); calculating a homography (H0) between the desired image (Id) and the initial image (Ii); decomposing the homography (H0) into an initial translation component (b0) and an initial rotation matrix (R0); obtaining one or more optimized translation components (br(t)) corresponding to one or more constraint conditions of the mobile machine (100) based on the initial translation component (b0); obtaining one or more optimized homographies (Hr(t)) based on the one or more optimized translation components (br(t)) and the initial rotation matrix (R0); and obtaining a plurality of target image features (sr(t)) corresponding to the trajectory (T2) based on the one or more optimized homographies (Hr(t)).

Description

Technical Field

[0001] This application relates to trajectory planning, and in particular, to an image-based trajectory planning method, a motion control method, and a mobile machine using these methods. Background Art

[0002] With the mature development of artificial intelligence (AI) technology, mobile machines such as mobile robots and cars have been applied in various daily life scenarios to provide services such as housework, medical care, and transportation. In order to provide services in a more mobile manner, an automatic navigation function is required. Trajectory planning is one of the key technologies for realizing automatic navigation. It provides a trajectory for a mobile machine to move to a destination while considering factors such as kinematic constraints, path length, and obstacle avoidance.

[0003] The most common trajectory planning technology is implemented in Cartesian coordinates (with x, y, and z axes). Although it is straightforward and widely used, the states of not all mobile machines in Cartesian coordinates can be directly or accurately measured. For example, when the pose (i.e., position and orientation) of a mobile machine can only be measured by a single on-board camera. One solution, a technology called "image-based", is to use the image features of the images captured by the camera, and these features are represented in image plane coordinates. The image features can be calculated from multiple feature points fixed in the environment and processed as the state of the mobile machine in the image plane coordinates.

[0004] As a newly developed technology, the existing image-based trajectory planning in image plane coordinates still focuses on omnidirectional robotic arms and three-dimensional drones (with "holonomic" constraint conditions), and the planned trajectories may not be achievable and applicable to mobile machines with nonholonomic and underactuated constraint conditions. Summary of the Invention

[0005] This application provides an image-based trajectory planning method, a motion control method, and a mobile machine using these methods, so as to use image features to plan the trajectory of a mobile machine with constraint conditions and solve the problem in the prior art that trajectory planning for a mobile machine with constraint conditions is not achieved.

[0006] An embodiment of this application provides a trajectory planning method for planning a trajectory for a mobile machine with a camera, where the trajectory includes multiple poses, and the method includes:

[0007] Through the camera of the mobile machine, an expected image of a reference target captured at an expected pose of the trajectory and an initial image of the reference target captured at an initial pose of the trajectory are obtained, where the initial pose is the first of the multiple poses and the expected pose is the last of the multiple poses;

[0008] Calculate the homography between the expected image and the initial image;

[0009] Decompose the homography into an initial translation component and an initial rotation matrix;

[0010] Based on the initial translation component, obtain one or more optimized translation components corresponding to one or more constraints of the mobile machine;

[0011] Based on the one or more optimized translation components and the initial rotation matrix, obtain one or more optimized homographies; and

[0012] Based on the one or more optimized homographies, obtain multiple target image features corresponding to the trajectory.

[0013] Embodiments of the present application also provide a mobile machine control method for controlling a mobile machine with a camera to move along a trajectory, where the trajectory includes multiple poses, and the method includes:

[0014] Through the camera of the mobile machine, an expected image of a reference target captured at an expected pose of the trajectory and an initial image of the reference target captured at an initial pose of the trajectory are obtained, where the initial pose is the first of the multiple poses and the expected pose is the last of the multiple poses;

[0015] Calculate the homography between the expected image and the initial image;

[0016] Decompose the homography into an initial translation component and an initial rotation matrix;

[0017] Based on the initial translation component, obtain one or more optimized translation components corresponding to one or more constraints of the mobile machine;

[0018] Based on the one or more optimized translation components and the initial rotation matrix, obtain one or more optimized homographies;

[0019] Based on the one or more optimized homographies, obtain multiple target image features corresponding to the trajectory; and

[0020] Control the mobile machine to move according to the target image features corresponding to the trajectory.

[0021] Embodiments of the present application also provide a mobile machine, including:

[0022] Multiple sensors;

[0023] One or more processors; and

[0024] One or more memories storing one or more computer programs;

[0025] Wherein, the one or more computer programs include a plurality of instructions for:

[0026] Obtaining, via the camera of the mobile machine, a desired image of a reference target captured at a desired pose of the trajectory and an initial image of the reference target captured at an initial pose of the trajectory, wherein the initial pose is the first of the plurality of poses and the desired pose is the last of the plurality of poses;

[0027] Calculating the homography between the desired image and the initial image;

[0028] Decomposing the homography into an initial translation component and an initial rotation matrix;

[0029] Obtaining one or more optimized translation components corresponding to one or more constraints of the mobile machine based on the initial translation component;

[0030] Obtaining one or more optimized homographies based on the one or more optimized translation components and the initial rotation matrix; and

[0031] Obtaining a plurality of target image features corresponding to the trajectory based on the one or more optimized homographies.

[0032] As can be seen from the embodiments of the present application above, the image-based trajectory planning method, motion control method, and mobile machine using these methods provided by the present application utilize image features to plan the trajectory of a mobile machine with constraints, thereby solving the problems in the prior art such as the inability to achieve trajectory planning for a mobile machine with constraints. During the navigation of the mobile machine, the pose of the mobile machine can be adjusted according to image features, without the need to use the pose of the mobile machine relative to the inertial system or visual target for feedback control, so that the mobile machine can finally reach the desired pose in the trajectory without pose measurement or estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] To more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings used in the description of the embodiments or the prior art. In the following drawings, similar reference numerals refer to corresponding components in these figures. It should be understood that the drawings in the following description are only examples of the present application. For those skilled in the art, other drawings can be obtained based on these drawings without creative efforts.

[0034] Figure 1A It is a schematic diagram of a scenario for navigating a mobile machine in some embodiments of the present application.

[0035] Figure 1B It is in Figure 1A the scenario of

[0036] Figure 2 a schematic block diagram of a mobile machine in some embodiments of the present application.

[0037] Figure 3A a schematic block diagram of an example of trajectory planning of the mobile machine in FIG. 1.

[0038] Figure 3B It is planned through Figure 3A an example of trajectory planning of Figure 1A the scenario of

[0039] Figure 3C It is Figure 2 a schematic diagram of a desired image captured by a camera of a mobile machine in

[0040] Figure 4 It is Figure 3A a schematic block diagram of an example of trajectory optimization in an example of trajectory planning of

[0041] Figure 5 It is in Figure 4 the scenario of applying non - holonomic constraint conditions in an example of trajectory optimization.

[0042] Figure 6A It is in Figure 3A the scenario of an example of obtaining image features in an example of trajectory planning.

[0043] Figure 6B In Figure 6A the scenario of an example of obtaining image features of

[0044] Figure 7 It is Figure 2 a schematic block diagram of an example of motion control of a mobile machine in Detailed implementation manners

[0045] To make the objectives, features, and advantages of this application more apparent and understandable, the technical solutions in the embodiments of this application will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this application, rather than all of them. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative efforts fall within the scope protected by this application.

[0046] It should be understood that when used in the description and appended claims of this application, the terms "include", "comprise", "have" and their variants mean the presence of the described features, wholes, steps, operations, elements, and / or components, but do not exclude the presence or addition of one or more other features, wholes, steps, operations, elements, components, and / or their combinations.

[0047] It should also be understood that the terms used in the description of this application are only for the purpose of describing specific embodiments and do not limit the scope of this application. As used in the description and appended claims of this application, unless the context clearly indicates otherwise, the singular forms "a", "an", and "the" are intended to include the plural forms.

[0048] It should be further understood that the term "and / or" used in the description and appended claims of this invention refers to any combination and all possible combinations of one or more of the related listed items, and includes these combinations.

[0049] The terms "first", "second", and "third" in this application are only for the purpose of description and should not be construed as indicating or implying relative importance or the number of the technical features referred to. Thus, the features defined by "first", "second", and "third" may explicitly or implicitly include at least one of the technical features. In the description of this application, the meaning of "a plurality" is at least two, such as two, three, etc., unless otherwise clearly defined.

[0050] The statement "in one embodiment" or "in some embodiments" etc. described in the description of this application means that in one or more embodiments of this application, specific features, structures, or characteristics related to the description of this embodiment may be included. Thus, statements such as "in one embodiment", "in some embodiments", "in other some embodiments", "in still other embodiments" etc. that appear in different places in the description do not mean that the described embodiments should be cited by all other embodiments, but rather by "one or more but not all other embodiments", unless otherwise specifically emphasized.

[0051] This application relates to trajectory planning and motion control for mobile machines. As used herein, the term "mobile machine" refers to a machine capable of moving around in its environment, such as a vehicle or a mobile robot. The term "trajectory planning" refers to finding a series of time-parametrized valid configurations that move a mobile machine from a source to a destination, where a "trajectory" represents a sequence of poses with timestamps (for reference, a "path" represents a series of poses or positions without timestamps). The term "pose" refers to a position (e.g., x and y coordinates on the x and y axes) and an orientation (e.g., yaw angle along the z axis). The term "nonholonomic constraint" refers to a constraint on the way a mobile machine moves, such as being able to move straight and rotate but not directly move left or right). The term "navigation" refers to the process of monitoring and controlling the movement of a mobile machine from one place to another, and the term "collision avoidance" refers to preventing collisions or reducing the severity of collisions. The term "sensor" refers to a device, module, machine, or subsystem (such as a camera) that aims to detect events or changes in its environment and send relevant information to other electronic devices (such as a processor).

[0052] Figure 1A is a schematic diagram of a scenario for navigating a mobile machine 100 in some embodiments of this application. The mobile machine 100 (such as a floor cleaning robot) is navigated in its environment (such as an office) to perform docking tasks (such as automatic charging, docking for pick-up and drop-off, docking for user interaction, and docking for waypoint cruising), while preventing dangerous situations such as collisions and unsafe states (such as falling, extreme temperatures, radiation, and exposure). In this indoor navigation, the mobile machine 100 is navigated from a starting point (such as the position where the mobile machine 100 is initially located) to a destination (such as the navigation target position indicated by the user or the navigation / operating system of the mobile machine 100), and can avoid walls W and obstacles O (such as furniture, people, pets, and trash) to prevent the above-mentioned dangerous situations. The trajectory of the mobile machine 100 moving from the starting point to the destination (such as trajectory T 1 and trajectory T 2 ) must be planned so that the mobile machine 100 moves according to this trajectory. It should be noted that the starting point and the ending point only represent the positions of the mobile machine 100 in the scenario in the figure, rather than the true starting point and ending point of the trajectory (the true starting point and ending point of a trajectory should be a pose respectively, such as Figure 1B and Figure 3B the initial pose S i and the desired pose S d)。In some embodiments, to implement the navigation and / or trajectory planning of the mobile machine 100, it is necessary to construct an environmental map and it may be necessary to determine the position of the mobile machine 100 in the environment. For example, a trajectory T can be planned based on the constructed map (and the determined position of the mobile machine 100). 1 。

[0053] Figure 1B is in Figure 1A the scenario of navigating the mobile machine 100 from the initial pose S to the desired pose S relative to the reference target T d is a schematic diagram. The initial pose S i is the starting point of the trajectory T 2 and the desired pose S d is the end point of the trajectory T 2 1 2 In addition to the trajectory T planned based on the constructed map, in order to respond to, for example, the reference target T detected by the camera C of the mobile machine 100, another trajectory T can also be planned (or the trajectory T can be replanned), and collision avoidance with respect to the obstacle O can also be considered during the planning so as to accurately navigate the mobile machine 100 according to the detected reference target T. The reference target T can be used as a reference for moving the mobile machine 10 so as to determine whether the mobile machine 100 has reached the destination. The camera C can be a calibrated front camera having a camera coordinate system F (see 1 (or the trajectory T can be replanned 1 ), and collision avoidance with respect to the obstacle O can also be considered during the planning so as to accurately navigate the mobile machine 100 according to the detected reference target T. The reference target T can be used as a reference for moving the mobile machine 10 so as to determine whether the mobile machine 100 has reached the destination. The camera C can be a calibrated front camera having a camera coordinate system F (see Figure 6B ). The coordinates of the mobile machine 100 are consistent with those of the camera C.

[0054]

[0055] Figure 2 In some embodiments, the navigation and / or trajectory planning of the mobile machine 100 can be actuated by causing the mobile machine 100 itself (e.g., a control interface on the mobile machine 100) or a control device 200 such as a remote controller, a smartphone, a tablet computer, a laptop computer, a desktop computer, or other electronic devices to provide a request for the navigation and / or trajectory planning of the mobile machine 100, for example. The mobile machine 100 and the control device 200 can communicate via a network, which can include, for example, the Internet, an intranet, an extranet, a local area network, a wide area network, a wired network, a wireless network (e.g., a Wi-Fi network, a Bluetooth network, and a mobile network), or other suitable networks, or any combination of two or more such networks.FIG. 0 is a schematic block diagram of a mobile machine 100 in some embodiments of the present application. The mobile machine 100 may be a mobile robot such as a wheeled robot, which may include a processing unit 110, a storage unit 120, and a control unit 130 that communicate via one or more communication buses or signal lines L. It should be noted that the mobile machine 100 is merely an example of a mobile machine, and the mobile machine 100 may have more or fewer components (e.g., units, subunits, and modules) than shown above or below, may combine two or more components, or may have different component configurations or arrangements. The processing unit 110 executes various instructions (or groups of instructions) stored in the storage unit 120, which may be in the form of software programs to perform various functions of the mobile machine 100 and process related data, and may include one or more processors (e.g., a central processing unit). The storage unit 120 may include one or more memories (e.g., high-speed random access memory (RAM) and non-transitory memory), one or more memory controllers, and one or more non-transitory computer-readable storage media (e.g., a solid-state drive or a hard disk drive). The control unit 130 may include various controllers (e.g., a camera controller, a display controller, and a physical button controller) and a peripheral device interface for coupling external ports (e.g., USB) of the mobile machine 100, a wireless communication circuit (e.g., an RF communication circuit), an audio circuit (e.g., a speaker circuit), sensors (e.g., an inertial measurement unit, etc.) input and output peripheral devices to the processing unit 110 and the storage unit 120. In some embodiments, the storage unit 120 may include a navigation module 121 for implementing navigation functions (e.g., map building and trajectory planning) related to the navigation of the mobile machine 100, which may be stored in one or more memories (and one or more non-transitory computer-readable storage media). In other embodiments, the mobile machine 100 may be a vehicle, such as a car, a drone, or a boat.

[0056] The navigation module 121 in the storage unit 120 of the mobile machine 100 may be a software module (of the operating system of the mobile machine 100) having instructions I for implementing the navigation of the mobile machine 100 n (e.g., instructions for actuating a motor M of the mobile machine 100 to move the mobile machine 100), a map builder 1211, and a trajectory planner 1212. The map builder 1211 may be a software module having instructions I for building a map for the mobile machine 100 b The trajectory planner 1212 may be a software module having instructions I for planning a path for the mobile machine 100 p The trajectory planner 1212 may include instructions for planning a global trajectory (e.g., trajectory T for the mobile machine 100 1)'s global trajectory planner and a local trajectory planner for planning a local trajectory (e.g., trajectory T) for the mobile machine 100 2 )'s local trajectory planner. The global trajectory planner can be, for example, a trajectory planner based on the Dijkstra algorithm, which plans a global trajectory based on the map constructed by the map builder 1211 through methods such as simultaneous localization and mapping (SLAM). The local trajectory planner can be, for example, a trajectory planner based on the timed elastic band (TEB) algorithm, which plans a local trajectory based on the global trajectory and other data collected by the mobile machine 100. For example, an image can be collected by the camera C of the mobile machine 100, and the collected image can be analyzed to identify obstacles. Thus, the local trajectory can be planned with reference to the identified obstacles, and the mobile machine 100 can be moved according to the planned local trajectory to avoid the obstacles 100.

[0057] Each of the map builder 1211 and the trajectory planner 1212 can be a sub-module separated from the instructions I of the navigation module 121 n or other sub-modules, or a part of the instructions I for implementing the navigation of the mobile machine 100 n The trajectory planner 1212 can also have data related to the trajectory planning of the mobile machine 100 (e.g., input / output data and temporary data), which can be stored in one or more memories and accessed by the processing unit 110. In some embodiments, each trajectory planner 1212 can be a module separated from the navigation module 121 in the storage unit 120.

[0058] In some embodiments, the instructions I n can include instructions for implementing collision avoidance (e.g., obstacle detection and path replanning) of the mobile machine 100. In addition, the global trajectory planner can replan the global trajectory (i.e., plan a new global trajectory) in response to, for example, the original global trajectory being blocked (e.g., blocked by an unexpected obstacle) or being insufficient to avoid a collision (e.g., unable to avoid the detected obstacle when adopting the original global trajectory). In other embodiments, the navigation module 121 can be a navigation unit that communicates with the processing unit 110, the storage unit 120, and the control unit 130 through one or more communication buses or signal lines L. In other embodiments, the navigation module 121 can be a navigation unit that communicates with the processing unit 110, the storage unit 120, and the control unit 130 through one or more communication buses or signal lines L, and can also include one or more memories (e.g., high-speed random access memory and non-temporary memory) for storing the instructions I n, a map builder 1211 and a trajectory planner 1212, and one or more processors (such as MPU and MCU) for executing stored instructions I n , I b and I p to achieve navigation of the mobile machine 100.

[0059] The mobile machine 100 may also include a communication subunit 131 and an actuation subunit 132. The communication subunit 131 and the actuation subunit 132 communicate with the control unit 130 via one or more communication buses or signal lines that are the same as or at least partially different from one or more communication buses or signal lines L described above. The communication subunit 131 is coupled to the communication interface of the mobile machine 100, such as a network interface 1311 that enables the mobile machine 100 to communicate with the control device 200 via a network N and an I / O interface 1312 (such as a physical button), etc. The actuation subunit 132 is coupled to components / devices for achieving the movement of the mobile machine 100 by, for example, actuating motors M (see Figure 1B ) of the wheels E and / or joints of the mobile machine 100. The communication subunit 131 may include a controller for the above-mentioned communication interface of the mobile machine 100, and the actuation subunit 132 may include a controller for the above-mentioned components / devices for achieving the movement of the mobile machine 100. In other embodiments, the communication subunit 131 and / or the actuation subunit 132 may be merely abstract components for representing the logical relationship between the components of the mobile machine 100.

[0060] The mobile machine 100 may also include a sensor subunit 133, which may include a set of sensors and associated controllers, such as a camera C and an inertial measurement unit U (or an accelerometer and a gyroscope), for detecting its surrounding environment to achieve its navigation. The sensor subunit 133 communicates with the control unit 130 via one or more communication buses or signal lines that are the same as or at least partially different from one or more communication buses or signal lines L described above. In other embodiments, in the case where the navigation module 121 is the above-mentioned navigation unit, the sensor subunit 133 may communicate with the navigation unit via one or more communication buses or signal lines that are the same as or at least partially different from one or more communication buses or signal lines L described above. Additionally, the sensor subunit 133 may be merely an abstract component for representing the logical relationship between the components of the mobile machine 100.

[0061] In some embodiments, the map builder 1211, the trajectory planner 1212, the sensor subunit 133, and the motor M (and the wheels and / or joints of the mobile machine 100 coupled to the motor M) together form a (navigation) system to achieve map building, (global and local) trajectory planning, and motor drive to achieve navigation of the mobile machine 100. Figure 2The various components shown can be implemented using hardware, software, or a combination of hardware and software. Two or more processing units 110, storage unit 120, control unit 130, navigation module 121, and other units / sub-units / modules can be implemented on a single chip or circuit. In other embodiments, at least a portion of them can be implemented on separate chips or circuits.

[0062] Figure 3A is a schematic block diagram of an example of trajectory planning for the mobile machine 100 in FIG. 1. In some embodiments, on the mobile machine 100, for example, instructions (group) I corresponding to an image-based trajectory planning method e are stored as the trajectory planner 1212 in the storage unit 120, and the stored instructions I are executed by the processing unit 110 e to implement the trajectory planning method, thereby using image features in the image plane coordinates to plan the trajectory of the mobile machine 100 (e.g., trajectory T 2 ), and then the mobile machine 100 can navigate according to the planned trajectory. In response to, for example, a reference target T having been detected by the camera C of the mobile machine 100, the trajectory planning method can be executed, and obstacles detected by the camera C of the mobile machine 100 (e.g., obstacle O) can be considered simultaneously. Then, in response to, for example, detecting an unexpected obstacle, the trajectory planning method can also be re-executed. In other embodiments, in response to a request for navigation and / or trajectory planning of the mobile machine 100 from, for example, the mobile machine 100 itself or the control device 200 (navigation / operating system), the trajectory planning method can also be executed.

[0063] Figure 3B is planned through an example of Figure 3A trajectory planning Figure 1A in the scenario of 2 the trajectory T 2 The planned trajectory T 0 includes multiple consecutive poses S of the mobile machine 100 (i.e., pose S 5 - pose S 2 ). Each of the multiple poses S includes a timestamp. It should be noted that the pose S in the figure is only an example, and the planned trajectory T 2 may actually have more or fewer poses S. Additionally, the distance between poses S can be determined according to actual needs. For example, for centimeter-level accuracy, the distance can be less than 5 cm, 10 cm, etc. Each pose S includes the position of the mobile machine 100 (e.g., coordinates in the world coordinate system) and the orientation (e.g., Euler angles in the world coordinate system). The motor M of the mobile machine 100 can be driven according to the poses S 2 in the trajectory T 2Move to achieve the navigation of the mobile machine 100. In some embodiments, the above local trajectory planner can generate a trajectory T while considering the identified obstacles (e.g., obstacle O) 2 (e.g., avoiding the identified obstacles) to plan the trajectory T 2 .

[0064] According to this trajectory planning method, the processing unit 110 can obtain the desired image I of the reference target T through the camera C of the mobile machine 100 d and the initial image I of the reference target T i ( Figure 3A of the frame 310)). The desired image I of the reference target T d is taken at the desired pose S 2 of the trajectory T d (see Figure 1B and Figure 3B ), while the initial image I of the reference target T i is taken at the initial pose S 2 of the trajectory T i (see Figure 1B and Figure 3B ). The initial pose S i is the first of the multiple poses S in the trajectory T 2 , and the desired pose S d is the last of the multiple poses S. Both the desired image I d and the initial image I i include the reference target T, so that the reference target T can be recognized by the mobile machine 100, and the feature points corresponding to the reference target T in each image (e.g., Figure 3C the feature points P 0 -P 3 ) can be obtained. The desired image I d and the initial image I i can be pre-shot before executing this trajectory planning method. For example, the desired image I 2 (e.g., at the destination of the trajectory T d ) can be first taken by the mobile machine 100 (through its camera C) and then transmitted to the mobile machine 100. Then, the initial image I i (e.g., at the current position of the mobile machine 100) can be taken by the mobile machine 100 (through its camera C).

[0065] Figure 3C is Figure 2 a schematic diagram of the desired image I captured by the camera C of the mobile machine 100 in d . When the mobile machine 100 is at the desired pose S d , the desired image I is takend . In some embodiments, the reference target T included in the desired image I d (and the initial image I i ) can be an object such as a sign with rich visual features in the environment where the mobile machine 100 is located, so that the feature points corresponding to the reference target T in the image can be easily obtained. The feature point can be a point (i.e., pixel) in the image that has different features (such as color) from the surrounding points. For example, the four corner points at the corners of the reference target T in the image that can be distinguished from the wall W can be used as feature points. The SIFT (scale-invariant feature transform) algorithm can be used to detect the feature points. In other embodiments, the reference target T can be other objects, such as a mark on the wall W or a docking station of the mobile machine 100.

[0066] The processing unit 110 can also calculate the homography H d between the desired image I i and the initial image I 0 ( Figure 3A block 320 of). In one embodiment, for any two images, a homography (essential matrix) can be calculated as follows:

[0067] [y 1 (t),..., y n (t)] ∝ H(t)[y 1 (t f ),..., y n (t f )];

[0068] where H(t) is the Euclidean homography of the two images, t ∈ [0, t f , 0 is the starting time corresponding to the initial image II, and t f is the ending time (in seconds relative to the start time) corresponding to the desired image I d (the sequence of (H(0) - H(t f ) can be regarded as the trajectory of the homography); y i (t) is the i-th image point on the image plane coordinates (in pixels) of the image taken at time t (when t = 0, this image will be the initial image I i , and when t = t f it will be the desired image I d ), and there are n points in the initial image I i corresponding to the desired image I dn points matching in (where n is greater than or equal to 3); y i (t f ) is the i-th image point on the image plane coordinates (in pixels) of the expected image I f captured at the end time t d . There are also n points in the expected image I d that match the n points in the initial image I i . ∝ means that the left side is proportional to the right side, but they may not be equal. That is, the homography H(t) can be solved in terms of scale rather than obtaining its true value. Given the 2n points in the initial image I i and the expected image I d , the homography H(t) can be solved. For example, if 4 pairs of coplanar points (i.e., n = 4) can be detected and matched in the initial image I i and the expected image I d (assuming the points are in a plane П (not shown in the figure)), then the homography H(t) can be solved through the corresponding system of equations with 8 equations. Since the homography H(t) has 9 entries (3×3), the solution of the system of equations is linearly related to the above true value. H 0 is the homography corresponding to the initial image I i (captured at the starting time of 0) and the expected image I d , that is, the homography H(0).

[0069] The processing unit 110 can also decompose the homography H 0 into an initial translation component b 0 and an initial rotation matrix R 0 ( Figure 3A in the box 330). The initial translation component b 0 is the translation between the expected image I d and the initial image I i in the (three-dimensional) camera coordinate system F. The initial rotation matrix R 0 is the rotation between the expected image I d and the initial image I i in the camera coordinate system F. By decomposing the homography H 0 , the initial translation component b 0 and the initial rotation matrix R 0 regarding the scale between the two images can be obtained. The true scale can be obtained by measuring the depth information when recording the expected image I D . In some embodiments, the homography H(t) can be decomposed into a rotation matrix R(t) and a translation component b(t) by the following equation:

[0070]

[0071] Among them, the rotation matrix R(t) represents the rotation between two images corresponding to the homography H(t) in the camera coordinate system F, the translation component b(t) represents the translation between two images corresponding to the homography H(t) in the camera coordinate system F, and d f is the depth from the origin of the desired image I d to the plane П, and α f is the unit normal to the plane П in the desired image I d . If d f is known in advance, the true value of the homography H(t) can be calculated, and the true values of the rotation matrix R(t) and the translation component b(t) can also be obtained. As described above, the homography H(t) is given, so the rotation matrix R(t) and the translation component b(t) can be obtained in an analytical form. As described above, H 0 is the homography H(0), so the initial translation component b 0 is b(0), that is, the translation component corresponding to the initial image I i (captured at the start time of 0) and the desired image I d , and the initial rotation matrix R 0 is R(0), that is, the rotation matrix corresponding to the initial image I i and the desired image I d .

[0072] The processing unit 110 can also obtain an optimized translation component b 0 corresponding to the constraint conditions of the mobile machine 100 based on the initial translation component b r (t) ( Figure 3A in the box 340). To obtain the optimized translation component b r (t), that is, to obtain a smooth trajectory of the translation component that satisfies the constraint conditions of the mobile machine 100. The constraint conditions can be, for example, non-holonomic constraints of the mobile machine 100 (such as the mobile machine 100 can only move and rotate in a straight line), kinematic constraints (such as the maximum allowable speed of the mobile machine 100), the shortest trajectory, and / or other constraints of the mobile machine 100 itself or its environment. Figure 4 is Figure 3A a schematic block diagram of an example of trajectory optimization in an example of trajectory planning. In some embodiments, in order to optimize for the constraints of the mobile machine 10 in the trajectory planning method ( Figure 3A in the block 340), the processing unit 110 can plan a trajectory T 0 that conforms to the constraint conditions of the mobile machine 100 according to the initial translation component b 2 for the intermediate pose S m (box 341). In the trajectory T 2 , the intermediate pose S m (that isFigure 3B Pose S in 1 -S 4 ) is between the initial pose S i (i.e., Figure 3B Pose S in 0 ) and the desired pose S d (i.e., Figure 3B Pose S in 5 ), which may include a series of continuous poses, each having x and y coordinates and a yaw angle (e.g., Figure 3B Angle θ in 2 and angle θ 3 ). The initial pose S i , the intermediate pose S m and the desired pose S d together form a trajectory T 2 of continuous poses S (see Figure 3B ).

[0073] In some embodiments, each constraint of the mobile machine 100 can be represented by an objective function of the translational component b, and the sequence of intermediate poses S m can be planned (in the camera coordinate system F) based on the objective function, such as the local trajectory planner using the TEB algorithm mentioned above. In the case of using the TEB algorithm, the optimization problem will be transformed into a hyper-graph. The pose S of the mobile machine 100 and its time interval are used as the nodes of the hyper-graph, and the objective function is used as the edge of the hyper-graph. Each node is connected by an edge to form a hyper-graph, and then the hyper-graph is optimized to solve the optimization problem.

[0074] Trajectory Optimization: Case with Nonholonomic Constraints

[0075] Figure 5 is a schematic diagram of applying nonholonomic constraints in an example of trajectory optimization of Figure 4 . In the case where the mobile machine 100 has nonholonomic constraints that cannot move directly left and right, but can only move linearly (forward and backward) and rotate, the mobile machine 100 can only move along the current forward direction and is approximated to move along an arc A of a circle between every two consecutive poses, and γ j = γ j+1 . Through the cross product, we have:

[0076] γ j = x j × l j ; and

[0077] γ j+1 = l j × x j+1 ;

[0078] where x j = [cosθ j , sinθ j , 0] T and x j+1 = [cosθ j+1 , sinθ j+1 , 0] T represent the forward direction in the world coordinate system, and l j is the unit vector from the origin of the x j coordinates to the origin of the x j+1 coordinates. The objective function used to penalize this non-holonomic constraint is f 1 (b) = ||(x j + x j+1 ) × l j || 2 .

[0079] Trajectory Optimization: Case with Kinematic Constraints

[0080] If the mobile machine 100 has a maximum speed as a kinematic constraint, for every two consecutive poses, the linear velocity v j and the angular velocity ω j can be approximated as:

[0081] and

[0082]

[0083] where dt j is the time interval between two consecutive poses. The objective function used to penalize the speed exceeding the maximum speed is:

[0084]

[0085] where g j (x) is a smoothing function, and

[0086] Trajectory Optimization: Case with Fastest Trajectory Constraint

[0087] Regarding the constraint of the fastest trajectory for the mobile machine 100, for every two consecutive poses, the objective function of the fastest trajectory constraint is:

[0088]

[0089] where dt j is the time interval between two consecutive poses.

[0090] The processing unit 110 can further obtain an optimized translation component br(t) based on the initial pose Si, the planned intermediate pose Sm, and the desired pose Sd of the trajectory T2 ( Figure 4 of the frame 342). In some embodiments, the optimized translation component br(t) can be obtained by a weighted multi-objective optimization formula:

[0091]

[0092] where f k (b) is an objective function corresponding to the constraint conditions of the mobile machine 100 (each objective function corresponds to each constraint condition), and β k is its weight, k ∈ [1, m] (m is the number of constraint conditions of the mobile machine 100), b is the translation component. When minimizing the sum of all objective functions of all constraint conditions of the mobile machine 100, the translation component b corresponding to each objective function is taken as the optimized translation component b r (t) at time t. A sequence of the optimized translation component b r (t) between the start time 0 and the end time is obtained. The objective function can be solved by the quasi-Newton method to obtain the optimal solution of the translation component b.

[0093] In other embodiments, the optimization of the constraint conditions for the mobile machine 10 ( Figure 3A of the frame 340) can also be omitted. For example, the translation component b(t) obtained by decomposing the homography H(t) is taken as the optimized translation component b r (t).

[0094] The processing unit 110 can also obtain an optimized homography H r (t) based on the optimized translation component b 0 and the initial rotation matrix R r ( Figure 3A of the frame 350). In some embodiments, the optimized homography H r (t) can be calculated by the following equation:

[0095]

[0096] where R 0 is the rotation matrix corresponding to the initial image I i and the desired image I d , d f is the depth from the origin of the desired image I d to the plane П, and α f is the unit normal to the plane П in the desired image I d ; t ∈ [0, tf , where 0 is the start time corresponding to the initial image Ii, and t f is the end time corresponding to the desired image I d .

[0097] The processing unit 110 can also obtain the target image feature s r corresponding to the trajectory T based on the optimized homography H 2 (t)( r (the box 360 of Figure 3A ). Here, the obtained trajectory T is related to the image feature 2 . Figure 6A is a schematic block diagram of an example of obtaining an image feature in an example of trajectory planning of Figure 3A . In this trajectory planning method, in order to obtain the target image feature ( Figure 3A the block 360 of r ), in some embodiments, the processing unit 110 can obtain the initial image I i , the desired image I d and the intermediate pose S 2 corresponding to the trajectory T m and the feature points Pi (box 361) of each of the intermediate images I m . The intermediate image I m is a pseudo-image, not captured by the camera C, but corresponding to the obtained feature points P i related to the planned intermediate pose Figure 3C . In one embodiment, the feature points Pi can be at least three non-collinear static points (in pixels) of each image (see 0 the feature points P 1 , P 2 and P 3 in Figure 6B and the feature points P 0 , P 1 and P 2 ). By substituting the optimized homography H r (t) into the above equation: [y 1 (t),..., y n (t)] ∝ H(t)[y 1 (t f ),..., y n (t f )], where y i (t f ) is the i-th image point of the desired image I f captured at the end time t d , the feature points in the initial image Ii and the intermediate image I mand the desired image I d at the point y i in (t) the feature point Pi (see Figure 3C and Figure 6B ). (The point y i in (t) will belong to the initial image I when t = 0 i , and will belong to the desired image I when t = t f , and will belong to the intermediate image I when 0 < t < t d ). f ) m )

[0098] The processing unit 110 can also project the feature points P i in each of the acquired initial image I d , desired image I m and intermediate image I i onto a virtual single sphere V (see Figure 6B ) to obtain the target feature points h i in each of the initial image I d , desired image I m and intermediate image I i (block 362). Figure 6B In the example of obtaining the image features which is Figure 6A , the schematic diagram of obtaining the target feature point h i is shown. The virtual single sphere V is a virtual sphere centered at the origin of the image captured by the camera C and is used in combination with the perspective projection of the unified sphere model. The unified sphere model is a geometric formula that can simulate a central imaging system so that image measurements from any (type) camera following the unified sphere model can be projected onto a generalized single sphere. The x-axis and y-axis of the camera coordinate system F of the mobile machine 100 define the motion plane, where the x-axis is aligned with the forward direction of the mobile machine 100 and the y-axis is aligned with the axis of the wheel E of the mobile machine 100. The feature points P i (i.e., P 0 , P 1 and P 2 ) in the camera coordinate system F are projected onto the virtual single sphere V as the target feature points h i (i.e., h 0 , h 1 and h 2 ).

[0099] The processing unit 110 can also use the multiple target feature points h i in each of the acquired initial image I m , intermediate image I d and desired image I i to obtain the initial image I i, intermediate image I m and desired image I d for each of the target image features s r (t)( Figure 6A of block 363). In some embodiments, invariant visual features of the image that are independent of the rotation of camera C are used as the target image feature s r (t), so the target image feature s r (t) may include the centroid, moment, distance, area, and other invariant visual features of the image. The distance d Figure 6B between points ha and hb (e.g., ab h0 and h1, h0 and h2 in Figure 6B ) (e.g., 01 d 02 and d r ) can be used as the image feature s

[0100] This image-based trajectory planning method uses image features in the image plane coordinates to plan a trajectory for a mobile machine with constraints. Then, during the navigation of the mobile machine, the pose of the mobile machine can be adjusted according to the image features without using the pose of the mobile machine relative to the inertial system or the visual target for feedback control, so that the mobile machine can finally reach the desired pose in the trajectory without pose measurement or estimation.

[0101] Figure 7 is Figure 2 a schematic block diagram of an example of the motion control of the mobile machine 100 in e In some embodiments, on the mobile machine 100, for example, the instruction(s) I e corresponding to the motion control method are stored in the storage unit 120 and the stored instruction I 2 is executed by the processing unit 110 r to (plan the trajectory and) move the mobile machine 100 according to the planned trajectory. Boxes 710 - 760 plan a trajectory for the mobile machine 100 (e.g., trajectory T Figure 3A ) and provide the target image feature s Figure 7 c

[0102] According to this motion control method, the processing unit 110 can obtain the current image I c of the reference target T taken at the current pose S c of the mobile machine 100 through the camera C of the mobile machine 100 (box 770). The current image Ic It must include the reference target T so as to obtain the current image I c and the feature point P corresponding to the reference target T in it i . The current pose S c is the pose of the mobile machine 100 when the current image I c is captured. If the mobile machine 100 has not moved after the initial image I i is captured (i.e., located at the starting point of the trajectory T 2 ), it can be the initial pose S i .

[0103] The processing unit 110 can also obtain the feature point P of the current image I c that i matches the feature point P of the expected image I d in the box 780. Search for the matching feature point P in the current image I i to obtain the current image feature s c of the current image I i in order to compare it with the sum of the intermediate image I c and the target image feature s r (t) of the expected image I m and the target image I d target image feature s r (t).

[0104] The unit 110 can also obtain the target feature point h i by projecting the obtained feature point P i onto the virtual single sphere V (box 790). As described above, the feature point P i is projected onto the virtual single sphere V as the target feature point h i (see Figure 6B ).

[0105] The processing unit 110 can also use the obtained target feature point h i to obtain the current image feature s c of the current image I r (t) (box 7100). As described above, in some embodiments, the invariant visual feature of the image is used as the target image feature s r (t), and the target image feature s r (t) can include, for example, the centroid, moment, distance, area, and other invariant visual features of the image. In the case where the obtained target image feature s r (t) includes distance, the points h a and h b (for example, Figure 6B the h in 0 and h 1 , h0 and h 2 the distance d between ab (e.g., Figure 6B the d in 01 and d 02 ) the reciprocal of which can be used as the target image feature s r (t).

[0106] The processing unit 110 may also calculate the difference D (block 7110) between the current image feature s c (t) of the current image I r and the target image feature s c (t) corresponding to the current pose S r . The current image feature s c (t) of the current image I r is the actual image feature, while the target image feature s c (t) corresponding to the current pose S r is the desired image feature. Thus, the current image feature s c (t) of the current image I r can be compared with the target image feature s c (t) at the time of the current pose S r , that is, compare the current image feature s r (t) and the target image feature s r (t) at the same time t, so as to obtain the difference D. For example, in the case where the target image feature s r (t) and the current image feature s r (t) include distances, the distances d r in the above-mentioned current image feature s ab (e.g., Figure 6B the d in 01 and d 02 ) the reciprocal of which can be subtracted from the reciprocal in the target image feature s r (t) to obtain the difference D between the distance d r of the target image feature s r (t) and the current image feature s ab (t).

[0107] The processing unit 110 may also control the mobile machine 100 to move according to the calculated difference D (block 7120). In the case where the target image feature s r (t) and the current image feature s r (t) include distances, since the current image feature s r (t) is the actual image feature, while the target image feature s r (t) is the desired image feature, it can be based on the above-mentioned target image feature sr The distance d in (t) ab and the current image feature s r The distance d in (t) ab The difference D between them is used to control the movement of the mobile machine 100 to offset the difference between them, so that the mobile machine 100 can move according to the planned trajectory (such as trajectory T 2 )

[0108] The motion control method uses the trajectory of the mobile machine with constraints, and the trajectory is planned according to the image features in the image plane coordinates. During the movement of the mobile machine, the pose of the mobile machine will be adjusted according to the image features. Since the trajectory has been optimized for the constraints of the mobile machine during planning, the mobile machine will move in a smoother manner. In addition, natural backward movement is also allowed in this motion control method.

[0109] Those skilled in the art can understand that all or part of the methods in the above embodiments can be implemented by one or more computer programs to instruct relevant hardware. In addition, one or more programs can be stored in a non-volatile computer-readable storage medium. When one or more programs are executed, all or part of the corresponding methods in the above embodiments are executed. Any reference to storage, memory, database or other media may include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), flash memory, solid state drive (SSD), etc. Volatile memory may include random access memory (RAM), external cache memory, etc.

[0110] The processing unit 110 (and the above-mentioned processor) may include a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate, transistor logic devices, and discrete hardware components. The general-purpose processor may be a microprocessor or any conventional processor. The storage unit 120 (and the above-mentioned memory) may include internal storage units such as hard disks and internal memories. The storage unit 120 may also include external storage devices such as plug-in hard disks, smart media cards (SMCs), secure digital (SD) cards, and flash memory cards.

[0111] The exemplary units / modules and methods / steps described in the embodiments may be implemented by software, hardware, or a combination of software and hardware. Whether these functions are implemented by software or hardware depends on the specific application and design constraints of the technical solution. The above-mentioned anti-collision method and mobile machine may be implemented in other ways. For example, the division of units / modules is only a logical functional division, and other division methods may be adopted in actual implementation, that is, multiple units / modules may be combined or integrated into another system, or some features may be ignored or not executed. In addition, the above-mentioned mutual coupling / connection may be a direct coupling / connection or communication connection, or an indirect coupling / connection or communication connection through some interfaces / devices, and may also be in electrical, mechanical, or other forms.

[0112] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit the technical solutions of the present application. Although the present application has been described in detail in combination with the above embodiments, the technical solutions in the above embodiments can still be modified, or some of the technical features can be equivalently replaced, so that these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included in the protection scope of the present application.

Claims

1. A trajectory planning method for planning a trajectory for a mobile machine with a camera, wherein the trajectory includes a plurality of poses, the method comprises: acquiring, by the camera of the mobile machine, a desired image of a reference target captured at a desired pose of the trajectory and an initial image of the reference target captured at an initial pose of the trajectory, wherein the initial pose is the first of the plurality of poses and the desired pose is the last of the plurality of poses; calculating a homography between the desired image and the initial image; decomposing the homography into an initial translation component and an initial rotation matrix; obtaining one or more optimized translation components corresponding to one or more constraints of the mobile machine based on the initial translation component; obtaining one or more optimized homographies based on the one or more optimized translation components and the initial rotation matrix; and obtaining a plurality of target image features corresponding to the trajectory based on the one or more optimized homographies.

2. The method according to claim 1, wherein decomposing the homography into the initial translation component and the initial rotation matrix comprises: decomposing the homography H(t) into a rotation matrix R(t) and a translation component b(t) by the following equation: and where \(R(t)\) is the rotation matrix, \(b(t)\) is the translation component, \(d\) f is the depth from the origin of the desired image to the plane, and \(\alpha\) f is the unit normal to the plane in the desired image; the initial translation component is \(b(0)\), and the initial rotation matrix is \(R(0)\).

3. The method according to claim 1, wherein obtaining the one or more optimized translation components corresponding to the one or more constraints of the mobile machine based on the initial translation component comprises: planning one or more intermediate poses of the trajectory for the mobile machine that satisfy one or more constraints based on the initial translation component, wherein the one or more intermediate poses are between the initial pose and the desired pose in the trajectory; and obtaining the one or more optimized translation components based on the initial pose of the trajectory, the one or more planned intermediate poses, and the desired pose.

4. The method according to claim 3, wherein each constraint of the mobile machine is represented by an objective function of a translation component, and obtaining the one or more optimized translation components based on the initial pose of the trajectory, the one or more planned intermediate poses, and the desired pose comprises: minimizing the sum of the objective functions of the one or more constraints of the mobile machine; and taking the translation component corresponding to each objective function as an optimized translation component.

5. The method according to claim 1, wherein obtaining the one or more optimized homographies based on the one or more optimized translation components and the initial rotation matrix comprises: Calculate the one or more optimized homographies H r (t): where, R 0 is the rotation matrix corresponding to the initial image and the desired image, b r (t) is the optimized translation component, d f is the depth from the origin of the desired image to the plane, α f is the unit normal to the plane in the desired image; t ∈ [0, t f , 0 is the start time corresponding to the initial image, t f is the end time corresponding to the desired image.

6. The method according to claim 1, wherein obtaining the plurality of target image features corresponding to the trajectory based on the one or more optimized homographies comprises: obtaining a plurality of feature points for each of the initial image, the desired image, and one or more intermediate images corresponding to one or more intermediate poses of the trajectory based on the one or more optimized homographies, wherein the one or more intermediate poses are between the initial pose and the desired pose in the trajectory; Obtaining a plurality of target feature points of each of the acquired initial image, the desired image, and the one or more intermediate images by projecting the plurality of feature points of each of the acquired initial image, the desired image, and the one or more intermediate images onto a virtual single sphere; and Using the plurality of target feature points of each of the acquired initial image, the one or more intermediate images, and the desired image to obtain the plurality of target image features of each of the initial image, the one or more intermediate images, and the desired image.

7. A mobile machine control method for controlling a mobile machine having a camera to move along a trajectory, where the trajectory includes a plurality of poses, the method comprises: Obtaining, through the camera of the mobile machine, a desired image of a reference target captured at a desired pose of the trajectory and an initial image of the reference target captured at an initial pose of the trajectory, where the initial pose is the first of the plurality of poses and the desired pose is the last of the plurality of poses; Calculating the homography between the desired image and the initial image; Decomposing the homography into an initial translation component and an initial rotation matrix; Obtaining one or more optimized translation components corresponding to one or more constraint conditions of the mobile machine based on the initial translation component; Obtaining one or more optimized homographies based on the one or more optimized translation components and the initial rotation matrix; Obtaining a plurality of target image features corresponding to the trajectory based on the one or more optimized homographies; and Controlling the mobile machine to move according to the target image features corresponding to the trajectory.

8. The method according to claim 7, wherein controlling the mobile machine to move according to the target image features corresponding to the trajectory comprises: Obtaining, through the camera of the mobile machine, a current image of the reference target captured at the current pose of the mobile machine; Obtaining a plurality of feature points of the current image that match the plurality of feature points of the desired image; Obtaining a plurality of target feature points by projecting the obtained plurality of feature points onto a virtual single sphere; Obtaining a plurality of current image features of the current image using the obtained plurality of target feature points; Calculating the difference between the current image features of the current image and the target image features corresponding to the current pose; and Controlling the movement of the mobile machine according to the calculated difference.

9. The method according to claim 7, wherein decomposing the homography into the initial translation component and the initial rotation matrix comprises: Decomposing the homography H(t) into a rotation matrix R(t) and a translation component b(t) by the following equation: and where \(R(t)\) is the rotation matrix, \(b(t)\) is the translation component, \(d\) f is the depth from the origin of the desired image to the plane, and \(\alpha\) f is the unit normal to the plane in the desired image; the initial translation component is \(b(0)\), and the initial rotation matrix is \(R(0)\).

10. A mobile machine, comprises: A plurality of sensors; One or more processors; and One or more memories storing one or more computer programs; wherein the one or more computer programs include a plurality of instructions for: Using the camera of the mobile machine, an expected image of a reference target captured at an expected pose of the trajectory and an initial image of the reference target captured at an initial pose of the trajectory are obtained, where the initial pose is the first of the plurality of poses and the expected pose is the last of the plurality of poses; Calculate the homography between the expected image and the initial image; Decompose the homography into an initial translation component and an initial rotation matrix; Based on the initial translation component, obtain one or more optimized translation components corresponding to one or more constraints of the mobile machine; Based on the one or more optimized translation components and the initial rotation matrix, obtain one or more optimized homographies; and Based on the one or more optimized homographies, obtain a plurality of target image features corresponding to the trajectory.

Images