IMAGE-BASED TRAJECTORY PLANNING METHOD AND MOVEMENT CONTROL METHOD AND MOBILE MACHINE USING THE SAME

Abstract

Image-based trajectory planning for a mobile machine having a camera is disclosed. A trajectory for the mobile machine to move to a referenced target is planned by obtaining, a desired image of the referenced target captured at a desired pose of the trajectory and an initial image of the referenced target captured at an initial pose of the trajectory through the camera of the mobile machine, calculating a homography of the desired image and the initial image; decomposing the homography into an initial translation component and an initial rotation matrix, obtaining optimized translation component(s) corresponding to constraint(s) for the mobile machine based on the initial translation component, obtaining optimized homography(s) based on the optimized translation component(s) and the initial rotation matrix, and obtaining objective image features corresponding to the trajectory based on the optimized homography(s).

Claims

1. A method for planning a trajectory for a mobile machine having a camera, wherein the trajectory comprises a plurality of poses, and the method comprises: obtaining, through the camera of the mobile machine, a desired image of a referenced target captured at a desired pose of the trajectory and an initial image of the referenced 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 of 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 for 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 objective image features corresponding to the trajectory based on the one or more optimized homographies.

2. The method of claim 1, wherein the 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) through an equation of: $H\left(t\right)=R\left(t\right)+\frac{b\left(t\right)}{d_f}{\alpha }_f;$ where, R(t) is a rotation matrix, b(t) is a translation component, d.sub.j is the depth from the origin of the desired image to a plane, and α.sub.f is the unitary 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 of claim 1, wherein the obtaining the one or more optimized translation components corresponding to the one or more constraints for the mobile machine based on the initial translation component comprises: planning one or more intermediate poses of the trajectory meeting the one or more constraints for the mobile machine 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, the one or more planned intermediate poses, and the desired pose of the trajectory.

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

5. The method of claim 1, wherein the obtaining the one or more optimized homographies based on the one or more optimized translation components and the initial rotation matrix comprises: calculating the one or more optimized homographies H.sub.r(t) through an equation of: $H_r\left(t\right)=R_0{e}^{\text{?}}+\frac{b_r\left(t\right)}{d_f}{\alpha }_f;$ $\text{?}\text{indicates text missing or illegible when filed}$ where, R.sub.0 is a rotation matrix corresponding to the initial image and the desired image, [φ.sub.0]=log(R.sub.0.sup.T)∈SO(3) ,b.sub.r(t) is an optimized translation component, d.sub.f is the depth from the origin of the desired image to a plane, and α.sub.f is the unitary normal to the plane in the desired image; t∈[0, t.sub.f], 0 is the starting time corresponding to the initial image, and t.sub.f is the ending time corresponding to the desired image.

6. The method of claim 1, wherein the obtaining the plurality of objective image features corresponding to the trajectory based on the one or more optimized homographies comprises: obtaining a plurality of feature points of 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 objective feature points of each of the initial image, the desired image, and the one or more intermediate images by projecting the plurality of obtained feature points of each of the initial image, the desired image, and the one or more intermediate images onto a virtual unitary sphere; and obtaining the plurality of objective image features of each of the initial image, the one or more intermediate images, and the desired image using the plurality of obtained objective feature points of each of the initial image, the one or more intermediate images, and the desired image.

7. A method for controlling a mobile machine having a camera to move according to a trajectory, wherein the trajectory comprises a plurality of poses, and the method comprises: obtaining, through the camera of the mobile machine, a desired image of a referenced target captured at a desired pose of the trajectory and an initial image of the referenced 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 of 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 for 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 objective image features corresponding to the trajectory based on the one or more optimized homographies; and controlling the mobile machine to move according to the objective image features corresponding to the trajectory.

8. The method of claim 7, wherein the controlling the mobile machine to move according to the objective image features corresponding to the trajectory comprises: obtaining, through the camera of the mobile machine, a current image of the referenced target captured at a current pose of the mobile machine; obtaining a plurality of feature points of the current image matched with a plurality of feature points of the desired image; obtaining a plurality of objective feature points by projecting the plurality of obtained feature points onto a virtual unitary sphere; obtaining a plurality of current image features of the current image using the plurality of obtained objective feature points: calculating a difference between the current image features of the current image and the objective image features corresponding to the current pose; and controlling the mobile machine to move according to the calculated difference.

9. The method of claim 7, wherein the 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) through an equation of: $H\left(t\right)=R\left(t\right)+\frac{b\left(t\right)}{d_f}{\alpha }_f;$ where, R(t) is a rotation matrix, b(t) is a translation component, d.sub.f is the depth from the origin of the desired image to a plane, and α.sub.f is the unitary normal to the plane in the desired image; the initial translation component is b(0), and the initial rotation matrix is R(0).

10. The method of claim 7, wherein the obtaining the one or more optimized translation components corresponding to the one or more constraints for the mobile machine based on the initial translation component comprises: planning one or more intermediate poses of the trajectory meeting the one or more constraints for the mobile machine 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, the one or more planned intermediate poses, and the desired pose of the trajectory.

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

12. The method of claim 7, wherein the obtaining the one or more optimized homographies based on the one or more optimized translation components and the initial rotation matrix comprises: calculating the one or more optimized homographies H.sub.r(t) through an equation of: $H_r\left(t\right)=R_0{e}^{\text{?}}+\frac{b_r\left(t\right)}{d_f}{\alpha }_f;$ $\text{?}\text{indicates text missing or illegible when filed}$ where, R.sub.0 is a rotation matrix corresponding to the initial image and the desired image, [φ.sub.0]=log(R.sub.0.sup.T)∈SO(3), b.sub.r(t) is an optimized translation component, d.sub.f is the depth from the origin of the desired image to a plane, and α.sub.f is the unitary normal to the plane in the desired image; t∈[0, t.sub.f], 0 is the starting time corresponding to the initial image, and t.sub.f is the ending time corresponding to the desired image.

13. The method of claim 7, wherein the obtaining the plurality of objective image features corresponding to the trajectory based on the one or more optimized homographies comprises: obtaining a plurality of feature points of 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 objective feature points of each of the initial image, the desired image, and the one or more intermediate images by projecting the plurality of obtained feature points of each of the initial image, the desired image, and the one or more intermediate images onto a virtual unitary sphere; and obtaining the plurality of objective image features of each of the initial image, the one or more intermediate images, and the desired image using the plurality of obtained objective feature points of each of the initial image, the one or more intermediate images, and the desired image.

14. A mobile machine, comprising: a camera; one or more processors; and one or more memories storing one or more programs configured to be executed by the one or more processors, wherein the one or more programs comprise instructions to: obtain, through the camera of the mobile machine, a desired image of a referenced target captured at a desired pose of a trajectory and an initial image of the referenced target captured at an initial pose of the trajectory, wherein the trajectory comprises a plurality of poses, and the initial pose is the first of the plurality of poses, and the desired pose is the last of the plurality of poses; calculate a homography of the desired image and the initial image; decompose the homography into an initial translation component and an initial rotation matrix; obtain one or more optimized translation components corresponding to one or more constraints for the mobile machine based on the initial translation component; obtain one or more optimized homographies based on the one or more optimized translation components and the initial rotation matrix; and obtaining a plurality of objective image features corresponding to the trajectory based on the one or more optimized homographies.

15. The mobile machine of claim 14, wherein the one or more programs further comprise instructions to: control the mobile machine to move according to the objective image features corresponding to the trajectory.

16. The mobile machine of claim 15, wherein the controlling the mobile machine to move according to the objective image features corresponding to the trajectory comprise: obtaining, through the camera of the mobile machine, a current image of the referenced target captured at a current pose of the mobile machine; obtaining a plurality of feature points of the current image matched with a plurality of feature points of the desired image; obtaining a plurality of objective feature points by projecting the plurality of obtained feature points onto a virtual unitary sphere; obtaining a plurality of current image features of the current image using the plurality of obtained objective feature points; calculating a difference between the current image features of the current image and the objective image features corresponding to the current pose; and controlling the mobile machine to move according to the calculated difference.

17. The mobile machine of claim 15, wherein the 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) through an equation of: $H\left(t\right)=R\left(t\right)+\frac{b\left(t\right)}{d_f}{\alpha }_f;$ where, R(t) is a rotation matrix, b(t) is a translation component, d.sub.f is the depth from the origin of the desired image to a plane, and α.sub.f is the unitary normal to the plane in the desired image; the initial translation component is b(0), and the initial rotation matrix is R(0).

18. The mobile machine of claim 15, wherein the obtaining the one or more optimized translation components corresponding to the one or more constraints for the mobile machine based on the initial translation component comprises: planning one or more intermediate poses of the trajectory meeting the one or more constraints for the mobile machine 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, the one or more planned intermediate poses, and the desired pose of the trajectory.

19. The mobile machine of claim 15, wherein the obtaining the one or more optimized homographies based on the one or more optimized translation components and the initial rotation matrix comprises: calculating the one or more optimized homographies H.sub.r(t) through an equation of: $H_r\left(t\right)=R_0{e}^{\text{?}}+\frac{b_r\left(t\right)}{d_f}{\alpha }_f;$ $\text{?}\text{indicates text missing or illegible when filed}$ where, R.sub.0 is a rotation matrix corresponding to the initial image and the desired image, [φ.sub.0]=log(R.sub.0.sup.T)∈SO(3), b.sub.r(t) is an optimized translation component, d.sub.f is the depth from the origin of the desired image to a plane, and α.sub.f is the unitary normal to the plane in the desired image; t∈[0, t.sub.f], 0 is the starting time corresponding to the initial image, and t.sub.j is the ending time corresponding to the desired image.

20. The mobile machine of claim 15, wherein the obtaining the plurality of objective image features corresponding to the trajectory based on the one or more optimized homographies comprises: obtaining a plurality of feature points of 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 objective feature points of each of the initial image, the desired image, and the one or more intermediate images by projecting the plurality of obtained feature points of each of the initial image, the desired image, and the one or more intermediate images onto a virtual unitary sphere; and obtaining the plurality of objective image features of each of the initial image, the one or more intermediate images, and the desired image using the plurality of obtained objective feature points of each of the initial image, the one or more intermediate images, and the desired image.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0005] In order to more clearly illustrate the technical solutions in this embodiment, the drawings used in the embodiments or the description of the prior art will be briefly introduced below. In the drawing(s), like reference numerals designate corresponding parts throughout the figures. It should be understood that, the drawings in the following description are Only examples of the present disclosure. For those skilled in the art, other drawings can be obtained based on these drawings without creative works.

[0006] FIG. 1A is a schematic diagram of a scenario of navigating a mobile machine according to some embodiments of the present disclosure.

[0007] FIG. 1B is a schematic diagram of navigating a mobile machine from an initial pose to a desired pose relative to a referenced target in the scenario of FIG. 1A.

[0008] FIG. 2 is a schematic block diagram illustrating a mobile machine according to some embodiments of the present disclosure.

[0009] FIG. 3A is a schematic block diagram of an example of trajectory planning for the mobile machine of FIG. 2.

[0010] FIG. 3B is a schematic diagram of a trajectory in the scenario of FIG. 1A which is planned through the example of trajectory planning of FIG. 3A.

[0011] FIG. 3C is a schematic diagram of a desired image captured by a camera of the mobile machine of FIG. 2.

[0012] FIG. 4 is a schematic block diagram of an example of trajectory optimization in the example of trajectory planning of FIG. 3A.

[0013] FIG. 5 is a schematic diagram of applying nonholonomic constraints in the example of trajectory optimization of FIG. 4.

[0014] FIG. 6A is a schematic block diagram of an example of image feature obtaining in the example of trajectory planning of FIG. 3A.

[0015] FIG. 6B is a schematic diagram of obtaining objective feature points in the example of image feature obtaining of FIG. 6A.

[0016] FIG. 7 is a schematic block diagram of an example of movement control for the mobile machine of FIG. 2.

DETAILED DESCRIPTION

[0017] In order to make the objects, features and advantages of the present disclosure more obvious and easy to understand, the technical solutions in this embodiment will be clearly and completely described below with reference to the drawings. Apparently, the described embodiments are part of the embodiments of the present disclosure, not all of the embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present disclosure without creative efforts are within the scope of the present disclosure.

[0018] It is to be understood that, when used in the description and the appended claims of the present disclosure, the terms “including”, “comprising”, “having” and their variations indicate the presence of slated features, integers, steps, operations, elements and/or components, hut do not preclude the presence or addition of one or a plurality of other features, integers, steps, operations, elements, components and/or combinations thereof.

[0019] It is also to be understood that, the terminology used in the description of the present disclosure is only for the purpose of describing particular embodiments and is not intended to limit the present disclosure. As used in the description and the appended claims of the present disclosure, the singular forms “one”, “a”, and “the” are intended to include the plural forms as well, unless the context clearly indicates otherwise.

[0020] It is also to be further understood that the term “and/or” used in the description and the appended claims of the present disclosure refers to any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0021] In the present disclosure, the terms “first”, “second”, and “third” are for descriptive purposes only, and are not to be comprehended as indicating or implying the relative importance or implicitly indicating the amount of technical features indicated. Thus, the feature limited by “first”, “second”, and “third” may include at least one of the feature either explicitly or implicitly. In the description of the present disclosure, the meaning of “a plurality” is at least two, for example, two, three, and the like, unless specifically defined otherwise.

[0022] In the present disclosure, the descriptions of “one embodiment”, “some embodiments” or the like described in the specification mean that one or more embodiments of the present disclosure can include particular features, structures, or characteristics which are related to the descriptions of the descripted embodiments. Therefore, the sentences “in one embodiment”, “in some embodiments”, “in other embodiments”, “in other embodiments” and the like that appear in different places of the specification do not mean that descripted embodiments should be referred by all other embodiments, but instead be referred by “one or more but not all other embodiments” unless otherwise specifically emphasized.

[0023] The present disclosure relates to trajectory planning and movement control or a mobile machine. As used herein, the term “mobile machine” refers to a machine such as a vehicle or a mobile robot that has the capability to move around in its environment. The term “trajectory planning” refers to find a sequence of valid configurations that moves a mobile machine from the source to the destination and is parametrized by time, where “trajectory” denotes a sequence of poses with time stamp (cf. “path” denotes a sequence of poses or position without time stamp). The term “pose” refers to position (e.g., x and v coordinates on x and y axes) and posture (e.g., a yaw angle along z axis). The term “nonholonomic constraint” refers to constraint for the movement manners of a mobile machine, for example, can move straightly and rotate while cannot move left and right directly). 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 prevent or reduce the severity of a collision. The term “sensor” refers to a device, module, machine, or subsystem such as ambient light sensor and image sensor (e.g., camera) whose purpose is to detect events or changes in its environment and send the information to other electronics (e.g., processor).

[0024] FIG. 1A is a schematic diagram of a scenario of navigating a mobile machine 100 according to some embodiments of the present disclosure. The mobile machine 100 (e.g., a sweeping robot) is navigated in its environment (e.g., an office) to, for example, performing a parking task such as auto charging, parking for pick up, parking for interaction to user, and parking for waypoints cruising, while dangerous situations such as collisions and unsafe conditions (e.g., failing, extreme temperature, radiation, and exposure) may be prevented. In this indoor navigation, the mobile machine 100 is navigated from a starting point (e.g., the location where the mobile machine 100 originally locates) to a destination the location of the goal of navigation which is indicated by the user or the navigation/operation system of the mobile machine 100), while the wall W and the obstacle O (e.g., furniture, human, pet, and garbage) may be avoided so as to prevent the above-mentioned dangerous situations. The trajectory(s) (e.g., trajectory T.sub.1 and trajectory T.sub.2) for the mobile machine 100 to move from the starting point to the destination has to be planned so as to move the mobile machine 100 according to the trajectory(s). It should be noted that, the starting point and the destination only represent the locations of the mobile machine 100 in the scenario shown in the figure, rather than the real beginning and end of the trajectory(s) (the real beginning and end of a trajectory should be a pose, respectively, e.g., initial pose S.sub.i and desired pose S.sub.d in FIG. 1B and FIG 3B). In some embodiments, for realizing the navigation and/or the trajectory planning of the mobile machine 100, the map for the environment has to be built, and the position of the mobile machine 100 in the environment may have to be determined. For example, a trajectory T.sub.1 may be planned based on the built map (and the determined position of the mobile machine 100).

[0025] FIG. 1B is a schematic diagram of navigating the mobile machine 100 from an initial pose S.sub.i to a desired pose S.sub.d relative to a referenced target T in the scenario of FIG. 1A. The initial pose S.sub.i is the beginning of a trajectory T.sub.2, and the desired pose S.sub.d is the end of the trajectory T.sub.2. In addition to the trajectory T.sub.1 planned based on the built map, another trajectory T.sub.2 may be planned (or the trajectory T.sub.1 may be replanned) in response to, for example, having detected the referenced target T through a camera C of the mobile machine 100, and the collision avoidance to the obstacle O may also considered when planning, so as to accurately navigate the mobile machine 100 according to the detected referenced target T. The referenced target T may be taken as a reference for moving the mobile machine 10 so as to determine whether the mobile machine 100 has reached the destination or not. The camera C may be a calibrated forward-facing camera which has a camera coordinate system F (see FIG. 6B), and the coordinates of the mobile machine 100 are consistent with the coordinates of the camera C.

[0026] In some embodiments, the navigation and/or the trajectory planning of the mobile machine 100 may be actuated through the mobile machine 100 itself (e.g., a control interface on the mobile machine 100) or a control device 200 such as a remote control, a smart phone, a tablet computer, a notebook computer, a desktop computer, or other electronic device by, for example, providing a request for the navigation and/or the trajectory planning of the mobile machine 100. The mobile machine 100 and the control device 200 may communicate over a network which may include, for example, the Internet, intranet, extranet, local area network (LAN), wide area network (WAN), wired network, wireless networks (e.g., Wi-Fi network, Bluetooth network, and mobile network), or other suitable networks, or any combination of two or more such networks.

[0027] FIG. 2 is a schematic block diagram illustrating the mobile machine 100 according to some embodiments of the present disclosure. 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 over one or more communication buses or signal lines L. It should be noted that, the mobile machine 100 is only one example of mobile machine, and the mobile machine 100 may have more or fewer components (e.g., unit, subunits, and modules) than shown in above or below, may combine two or more components, or may have a different configuration or arrangement of the components. The processing unit 110 executes various (sets of) instructions stored in the storage unit 120 that may be in form of software programs to perform various functions for the mobile machine 100 and to process related data, which may include one or more processors (e.g., CPU). 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 mediums (e.g., solid-state drive (SSD) or hard disk drive). The control unit 130 may include various controllers (e.g., camera controller, display controller, and physical button controller) and peripherals interface for coupling the input and output peripheral of the mobile machine 100, for example, external port (e.g., USB), wireless communication circuit (e.g., RF communication circuit), audio circuit (e.g., speaker circuit), sensor (e.g., inertial measurement unit (IMU)), and the like, 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 (and trajectory planning) of the mobile machine 100, which may be stored in the one or more memories (and the one or more non-transitory computer readable storage mediums). In other embodiments, the mobile machine 100 may be a vehicle such as a car, a drone, or a vessel.

[0028] The navigation module 121 in the storage unit 120 of the mobile machine 100 may be a software module (of the operation system of the mobile machine 100), which has instructions I.sub.n (e.g., instruction for actuating motor(s) M of the mobile machine 100 to move the mobile machine 100) for implementing the navigation of the mobile machine 100, a map builder 1211, and trajectory planners) 1212. The map builder 1211 may be a software module having instructions I.sub.b for building map for the mobile machine 100. The trajectory planner(s) 1212 may be software module(s) having instructions I.sub.p for planning path for the mobile machine 100. The trajectory planner(s) 1212 may include a global trajectory planner for planning global trajectories (e.g., trajectory TO for the mobile machine 100 and a local trajectory planner for planning local trajectories (e.g., trajectory T.sub.2) for the mobile machine 100. The global trajectory planner may be, for example, a trajectory planner based on Dijkstra's algorithm, which plans global trajectories based on map(s) built by the map builder 1211 through, for example, simultaneous localization and mapping (SLAM). The local trajectory planner may be, for example, a trajectory planner based on TEB (timed elastic band) algorithm, which plans local trajectories based on the global trajectory P.sub.g, and other data collected by the mobile machine 100. For example, images may be collected through the camera C of the mobile machine 100, and the collected images may be analyzed so as to identify obstacles, so that the local trajectory can be planned with reference to the identified obstacles, and the obstacles can be avoided by moving the mobile machine 100 according to the planned local trajectory.

[0029] Each of the map builder 1211 and the trajectory planner(s) 1212 may be a submodule separated from the instructions I.sub.n or other submodules of the navigation module 121, or a pan of the instructions I.sub.n or implementing the navigation of the mobile machine 100. The trajectory planner(s) 1212 may further have data (e.g., input/output data and temporary data) related to the trajectory planning of the mobile machine 100 which may be stored in the one or more memories and accessed by the processing unit 110. In some embodiments, each of the trajectory planners) 1212 may be a module in the storage unit 120 that is separated from the navigation module 121.

[0030] In some embodiments, the instructions I.sub.n may include instructions for implementing collision avoidance of the mobile machine 100 (e.g., obstacle detection and path replanning). In addition, the global trajectory planner may replan the global trajectory(s) (i.e., plan new global trajectory(s)) to detour in response to, for example, the original global trajectory(s) being blocked (e.g., blocked by an unexpected obstacle) or inadequate for collision avoidance (e.g., impossible to avoid a detected obstacle when adopted). In other embodiments, the navigation module 121 may be a navigation unit communicating with the processing unit 110, the storage unit 120, and the control unit 130 over the one or more communication buses or signal lines L, and may further include one or more memories (e.g., high-speed random access memory (RAM) and non-transitory memory) for storing the instructions I.sub.n, the map builder 1211, and the trajectory planner(s) 1212, and one or more processors (e.g., MPU and MCU) for executing the stored instructions I.sub.n, I.sub.b and I.sub.p to implement the navigation of the mobile machine 100.

[0031] The mobile machine 100 may further 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 over one or more communication buses or signal lines that may be the same or at least partially different from the above-mentioned one or more communication buses or signal lines L. The communication subunit 131 is coupled to communication interfaces of the mobile machine 100, for example, network interlaced) 1311 for the mobile machine 100 to communicate with the control device 200 via the network(s) N and I/O interfaced) 1312 (e.g., a physical button), and the like. The actuation subunit 132 is coupled to component(s)/device(s) for implementing the motions of the mobile machine 100 by, for example, actuating motor(s) M of wheels E (see FIG. 1B) and/or joints of the mobile machine 100. The communication subunit 131 may include controllers for the above-mentioned communication interfaces of the mobile machine 100, and the actuation subunit 132 may include controllers) for the above-mentioned component(s)/device(s) for implementing the motions of the mobile machine 100. In other embodiments, the communication subunit 131 and/or actuation subunit 132 may just abstract component for representing the logical relationships between the components of the mobile machine 100.

[0032] The mobile machine 100 may further include a sensor subunit 133 which may include a set of sensor(s) and related controller(s), for example, the camera C and an IMU U (or an accelerometer and a gyroscope), for detecting the environment in which it is located to realize its navigation. The sensor subunit 133 communicates with the control unit 130 over one or more communication buses or signal lines that may be the same or at least partially different from the above-mentioned one or more communication buses or signal lines L. In other embodiments, in the case that the navigation module 121 is the above-mentioned navigation unit, the sensor subunit 133 may communicate with the navigation unit over one or more communication buses or signal lines that may be the same or at least partially different from the above-mentioned one or more communication buses or signal lines L. In addition, the sensor subunit 133 may just abstract component for representing the logical relationships between the components of the mobile machine 100.

[0033] In some embodiments, the map builder 1211, the trajectory planner(s) 1212, the sensor subunit 133, and the motor(s) M (and wheels and/or joints of the mobile machine 100 coupled to the motor(s) M) jointly compose a (navigation) system which implements map building, (global and local) trajectory planning, and motor actuating so as to realize the navigation of the mobile machine 100. In addition, the various components shown in FIG. 2 may be implemented in hardware, software or a combination of both hardware and software. Two or more of the processing unit 110, the storage unit 120, the control unit 130, the navigation module 121, and other units/subunits/modules may be implemented on a single chip or a circuit. In other embodiments, at least a part of them may be implemented on separate chips or circuits.

[0034] FIG. 3A is a schematic block diagram of an example of trajectory planning for the mobile machine 100 of FIG. 2. In some embodiments, an image-based trajectory planning method for the mobile machine 100 is implemented in the mobile machine 100 to plan a trajectory (e.g., the trajectory T.sub.2) for the mobile machine 100 in terms of image features in image plane coordinate by, for example, storing (sets of) instructions I.sub.e corresponding to the trajectory planning method as the trajectory planner(s) 1212 in the storage unit 120 and executing the stored instructions I.sub.e through the processing unit 110, and then the mobile machine 100 may be navigated according to the planned trajectory. The trajectory planning method may be performed in response to, for example, having detected the referenced target T through the camera C of the mobile machine 100, and may simultaneously considering the obstacles (e.g., the obstacle O) detected through the camera C of the mobile machine 100, then it may also be re-performed in response to, for example, having detected unexpected obstacles. In other embodiments, the trajectory planning method may also be performed in response to a request for the navigation and/or the trajectory planning of the mobile machine 100 from, for example, (the navigation/operation system of) the mobile machine 100 itself or the control device 200.

[0035] FIG. 3B is a schematic diagram of the trajectory T.sub.2 in the scenario of FIG. 1A which is planned through the example of trajectory planning of FIG. 3A. The planned trajectory T.sub.2 includes a plurality of consecutive poses S (i.e., pose S.sub.0-pose S.sub.5) for the mobile machine 100. Each of the poses S includes a time stamp. It should be note that, the poses S shown in the figure only an example, and the planned trajectory T.sub.2 may actually have more or less poses S. In addition, the distance between the poses S can be determined according to actual needs. For example, for centimeter level accuracy, the distance may be smaller than 5 cm, 10 cm, or the like. Each of the poses S includes a position (e.g., coordinates in the world coordinate system) and a posture (e.g., an Euler angle in the world coordinate system) for the mobile machine 100. The motor(s) M of the mobile machine 100 may be actuated according to the poses S in the trajectory T.sub.2 so that the mobile machine 100 is moved according to the trajectory T.sub.2, thereby implementing the navigation of the mobile machine 100. In some embodiments, the trajectory T.sub.2 may be planned through the above-mentioned local trajectory planner by generating the trajectory while taking the identified obstacles (e.g., the obstacle O) into consideration (e.g., avoiding the identified obstacles).

[0036] According to the trajectory planning method, the processing unit 110 obtains a desired image I.sub.d of the referenced target T and an initial image I.sub.i of the referenced target T through the camera C of the mobile machine 100 (block 310 of FIG. 3A). The desired image I.sub.d of the referenced target I is captured at the desired pose S.sub.d (see FIG. 1B and FIG. 3B) of the trajectory T.sub.2, and the initial image I.sub.i of the referenced target T is captured at the initial pose S.sub.i (see FIG. 1B and FIG. 3B) of the trajectory T.sub.2. The initial pose S.sub.i is the first of the plurality of poses S in the trajectory T.sub.2, and the desired pose S.sub.d is the last of the plurality of poses S. The desired image I.sub.d and the initial image I.sub.i both include the referenced target T, so that the referenced target T can be recognized by the mobile machine 100 and feature points (e.g., feature points P.sub.0-P.sub.3 in FIG. 3C) in each image that correspond to the referenced target T can be obtained. The desired image I.sub.d and the initial image I.sub.i may be captured in advance before performing the trajectory planning method. For example, the desired image I.sub.d may be first captured (e.g., at the destination of the trajectory T.sub.2) through (the camera C of) the mobile machine 100, or through other device isolated from the mobile machine 100 and then transmitted to the mobile machine 100. then the initial image I.sub.i may be captured (e.g., at the current position of the mobile machine 100) through (the camera C of) the mobile machine 100.

[0037] FIG. 3C is a schematic diagram of the desired image I.sub.d captured by the camera C of the mobile machine 100 of FIG. 2. The desired image I.sub.d is captured when the mobile machine 100 is at the desired pose S.sub.d. In some embodiments, the referenced target T included in the desired image I.sub.d (and the initial image I.sub.i) may be an object such as a sign that has rich visual feature in the environment where the mobile machine 100 is located, so as to be easily recognized and the feature points in the image that correspond to the referenced target T can be easily obtained. The feature point may be a point (i.e., pixel) in the image that has the feature (e.g., color) different from its surrounding points, for example, the four corner points at the comer of the referenced target T in the image that can be differentiated from the wall W can be taken as the feature points. The feature points may be detected using SIFT (scale-invariant feature transform) algorithm. In other embodiments, the referenced target T may be other object, for example, a mark on the wall W or a docking station for the mobile machine 100.

[0038] The processing unit 110 may further calculate a homography H.sub.0 of the desired image I.sub.d and the initial image I.sub.i (block 320 of FIG. 3A). In one embodiment, for any two images, there is one homography (essential matrix) may be calculated as follows:

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

where, H(t) is the Euclidean homography of the two images, t∈[0, t.sub.f], 0 is the starting time corresponding to the initial image and is the ending time (which is with respect to the starting time, in seconds) corresponding to the desired image I.sub.d (the sequence of H.sub.0-H(t.sub.f) can be seen as the trajectory of homography); y.sub.i(t) is the i-th image point on the image plane coordinate (unit in pixel) of an image which is captured at the time t (the image will be the initial image I.sub.i when t=0, and will be the desired image I.sub.d when t=t.sub.f), and there are n points in the initial image h which are matched with n points in the desired image I.sub.d (n is larger than or equal to three); and y.sub.i(t.sub.f) is the i-th image point on the image plane coordinate (unit in pixel) of the desired image I.sub.d which is captured at the ending time t.sub.f, there are also n points in the desired image I.sub.d which are matched with the n points in the initial image I.sub.i. ∝ means the left-hand side is proportional to the right-hand side but they may not be equal. That is, the homography H(t) can be solved up to a scale rather than obtain its truth value. Having given those 2n points in the initial image I.sub.i and the desired image I.sub.d, the homography H(t) can be solved. For example, if there are 4 pairs of coplanar points (i.e., n=4) can be detected and matched in in the initial image I.sub.i and the desired image I.sub.d (here points are assumed in a plane Π (not shown)), the homography H(t) can be solved through a corresponding equation set containing 8 equations. Since the homography H(t) has 9 entries (3×3), the solution of equation set is linear dependent of the truth value as mentioned above. H.sub.0 is the homography corresponding to the initial image I.sub.i (captured at the starting time of 0) and the desired image I.sub.d, that is, the homography H(0).

[0039] The processing unit 110 may further decompose the homography H.sub.0 into an initial translation component b.sub.0 and an initial rotation matrix R.sub.0 (block 330 of FIG. 3A). The initial translation component b.sub.0 is a translation between the desired image I.sub.d and the initial image I.sub.i in the (three-dimensional) camera coordinate system F, and the initial translation component b.sub.0 is a rotation between the desired image I.sub.d and the initial image I.sub.i in the camera coordinate system F. By decomposing the homography H.sub.0, the initial translation component b.sub.0 and the initial rotation matrix R.sub.0 between the two image up to a scale can be obtained, and a true scale can be obtained by measuring the depth information when recording the desired image I.sub.d. In some embodiments, the homography H(t) may be decomposed into a rotation matrix R(t) and a translation component h(t) through an equation of:

[00001]$H\left(t\right)=R\left(t\right)+\frac{b\left(t\right)}{d_f}{\alpha }_f;$

where, the rotation matrix R(t) represents a rotation between the two images corresponding to the homography H(t) in the camera coordinate system F. the translation component b(t) represents a translation between the two images corresponding to the homography H(t) in the camera coordinate system F, d.sub.f is the depth from the origin of the desired image I.sub.d to the plane Π, and α.sub.f is the unitary normal to the plane Π in the desired image I.sub.d. If d.sub.f is known ahead, the truth value of the homography H(t) can be calculated, and the truth value of the rotation matrix R(t) and the translation component b(t) can be obtained too. As mentioned above, the homography H(t) is given, hence the rotation matrix R(t) and the translation component b(t) can be obtained in an analytical form. As mentioned above, H.sub.0 is the homography H(0), hence the initial translation component b.sub.0 is b(0) that is, the translation component corresponding to the initial image I.sub.i (captured at the starting time of 0) and the desired image I.sub.d, and the initial rotation matrix R.sub.0 is R(0), that is, the rotation matrix corresponding to the initial image I.sub.i and the desired image I.sub.d.

[0040] The processing unit 110 may further obtain optimized translation components B.sub.r(t) corresponding to constraints) for the mobile machine 10 based on the initial translation component b.sub.0 (block 340 of FIG. 3A). The optimized translation components b.sub.r(t), that is, a smooth trajectory of translation components which meets the constraints) for the mobile machine 100 is to be obtained. The constraints) may be, for example, nonholonomic constraint of the mobile machine 100 (e.g., the mobile machine 100 can only move straightly and rotate), kinematic constraint (e.g., the maximum allowed velocity of the mobile machine 100), shortest trajectory, and/or other constraint of the mobile machine 100 itself or the environment where it is located. FIG. 4 is a schematic block diagram of an example of trajectory optimization in the example of trajectory planning of FIG. 3A. In some embodiments, for optimizing against the constraints for the mobile machine 10 (block 340 of FIG. 3A) in the trajectory planning method, the processing unit 110 may plan intermediate pose(s) S.sub.m of the trajectory T.sub.2 that meet the constraint(s) for the mobile machine 100 based on the initial translation component b.sub.0 (block 341). The intermediate pose(s) S.sub.m (i.e., poses S.sub.1-S.sub.4 in FIG. 3B) are between the initial pose S.sub.i (i.e., pose S.sub.0 in FIG. 3B) and the desired pose S.sub.d (i.e., pose S.sub.5 in FIG. 3B) in the trajectory T.sub.2, which may include a sequence of continuous poses each having x and y coordinates and a yaw angle (e.g., angle θ.sub.2 and angle θ.sub.3 in FIG. 3B). The initial pose S.sub.i, intermediate pose(s) S.sub.m, and the desired pose S.sub.d jointly form the consecutive poses S of the trajectory T.sub.2 (see FIG. 3B).

[0041] In some embodiments, each constraint for the mobile machine 100 may be represented through one objective function of translation component b, and the sequence of intermediate pose(s) S.sub.m may be planned (in the camera coordinate system F) based on the objective function(s) by, for example, the above-mentioned local trajectory planner using TEB algorithm. In the case that TEB algorithm is used, the optimization problem will be transformed into a hyper-graph. The poses S of the mobile machine 100 and the time interval therebetween are used as nodes of the hyper-graph, and objective functions are used as edges of the hyper-graph. Each node is connected by edge(s) to form the hyper-graph, and the hyper-graph is optimized to solve the the optimization problem accordingly.

Trajectory Optimization: with Nonholonomic Constraint

[0042] FIG. 5 is a schematic diagram of applying nonholonomic constraints in the example of trajectory optimization of FIG. 4. In the case that the mobile machine 100 has the nonholonomic constraint which the mobile machine 100 cannot directly move left and right but can only move straightly (forwards and backwards) and rotate, the mobile machine 100 can only move along current heading direction, and it is approximated that between every two consecutive poses, the mobile machine 100 moves along an arc A of a circle, and γ.sub.j=γ.sub.j+1. Through cross product, there are: [0043] γ.sub.j=x.sub.j×l.sub.j; and [0044] γ.sub.j+1=l.sub.j×x.sub.j+1;
where, x=[cosθ.sub.i, sinθ.sub.j, 0].sup.Y and x.sub.j+1=[cosθ.sub.j+1, sinθ.sub.j+1, 0].sup.Y represent the heading directions in the world coordinate system, and l.sub.j is the unit vector from the origin of the x.sub.j coordinate to the origin of the x.sub.j+1 coordinate. The objective function to penalize the nonholonomic constraint is f.sub.1(b)=∥(x.sub.i+x.sub.j+1)×l.sub.i∥.sup.2.
Trajectory Optimization: with Kinematic constraint

[0045] If the mobile machine 100 has the kinematic constraint of maximum velocity, for every two consecutive poses, the linear velocity v.sub.j and the angular velocity ω.sub.j can be approximated by:

[00002]$v_j\simeq \frac{1}{{\mathrm{dt}}_j}.Math.l_j.Math.;\mathrm{and}$${\omega }_j\simeq \frac{1}{{\mathrm{dt}}_j}\left({\theta }_{j+1}-{\theta }_j\right);$

where, dt.sub.j is the time interval between the two consecutive poses. The objective function to penalize velocity which is over the maximum value v is:

[00003]$f_2\left(b\right)=\underset{j}{.Math.}{\left(g_j\left(v_j\right)+g_j\left({\omega }_j\right)\right)}^2;$

where, g.sub.f(x) is smooth function, and

[00004]$g_j\left(x\right)=\{\begin{array}{ccc}0& \mathrm{if}& x<\stackrel{\_}{v}-\zeta \\ {x-\stackrel{\_}{v}+\zeta )}^2& & \mathrm{else}\end{array}.$

Trajectory Optimization: with Fastest Trajectory Constraint

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

[00005]$f_3\left(b\right)={\left(\underset{j}{.Math.}{\mathrm{dt}}_j\right)}^2;$

where, dt.sub.j is the time interval between the two consecutive poses.

[0047] The processing unit 110 may further obtain the optimized translation components b.sub.r(t) based on the initial pose S.sub.i, the planned intermediate poses) S.sub.m, and the desired pose S.sub.d of the trajectory T.sub.2 (block 342 of FIG. 4). In some embodiments, the optimized translation components b.sub.r(t) may be obtained through weighted multi-objective optimization an equation of:

[00006]$\min \underset{k}{.Math.}{\beta }_k{f}_k\left(b\right);$

where, f.sub.k(b) is an objective function corresponding to a constraint of the mobile machine 100 (each objective function corresponds to each constraint) and β.sub.k is its weight, k∈[l, m] (m is the number of the constraints of the mobile machine 100), and b is the translation component. Upon minimizing the sum of all the objective functions f.sub.k(b) of all the constraints of the mobile machine 100. the translation component b corresponding to each of the objective functions f.sub.k(b) is taken as an optimized translation component b.sub.r(t) at time t. A sequence of the optimized translation components b.sub.r(t) at the time in-between the starting time 0 and the ending time t.sub.f are obtained. The objective function f.sub.k(b) may be solved through Quasi-Newton method to obtain the optimal solution of the translation component b.

[0048] In other embodiments, the optimization against the constraints for the mobile machine 10 (block 340 of FIG. 3A) may also be omitted by, for example, using the translation component b(t) obtained by decomposing the homography H(t) as the optimized translation components b.sub.r(t).

[0049] The processing unit 110 may further obtain optimized homography(s) H.sub.r(t) based on the optimized translation component(s) b.sub.r(t) and the initial rotation matrix R.sub.0 (block 350 of FIG. 3A). In some embodiments, the optimized homography(s) H.sub.r(t) may be calculated through an equation of:

[00007]$H_r\left(t\right)=R_0{e}^{\text{?}}+\frac{b_r\left(t\right)}{d_f}{\alpha }_f;$$\text{?}\text{indicates text missing or illegible when filed}$

where, R.sub.0 is a rotation matrix corresponding to the initial image I.sub.i and the desired image I.sub.d, [φ.sub.0]=log(R.sub.0.sup.T)∈SO(3), d.sub.f is the depth from the origin of the desired image I.sub.d to the plane ∪, and α.sub.f is the unitary normal to the plane Π in the desired image I.sub.d; t∈[0, t.sub.f], 0 is the starting time corresponding to the initial image I.sub.i, and t.sub.f is the ending time corresponding to the desired image I.sub.d.

[0050] The processing unit 110 may further obtain objective image features s.sub.r(t) corresponding to the trajectory T.sub.2 based on the optimized homographies H.sub.r(t) (block 360 of FIG. 3A). The trajectory.sub.2 in terms of image features is obtained here. FIG. 6A is a schematic block diagram of an example of image feature obtaining in the example of trajectory planning of FIG. 3A. In the trajectory planning method, for obtaining objective image features (block 360 of FIG. 3A), in some embodiments, the processing unit 110 may obtain feature points P.sub.i of each of the initial image I.sub.i, the desired image I.sub.d, and intermediate image(s) I.sub.m corresponding to the intermediate pose(s) S.sub.m of the trajectory T.sub.2 based on the optimized homographies H.sub.r(t) (block 361). The intermediate image(s) I.sub.m are pseudo images which are not captured by the camera C but correspond to the obtained feature points P.sub.i relative to the planned intermediate pose(s). In one embodiment, the feature points P.sub.i may be at least three non-collinear static points (unit in pixel) (see feature points P.sub.0, P.sub.1, P.sub.2 and P.sub.3 in FIG. 3C and feature points P.sub.0, P.sub.1 and P.sub.2 in FIG. 6B) of each image. By substituting the optimized homographies H.sub.r(t) into the above-mentioned equation [y.sub.1(t)], . . . , y.sub.n(t)[y.sub.1(t.sub.j), . . . , y.sub.n(t.sub.f)] where y.sub.1(t.sub.j) is the i-th image point of the desired image I.sub.d which is captured at the ending time t.sub.j, the feature points P.sub.i (see FIG. 3C and FIG. 6B) among points y.sub.i(t) of the initial image I.sub.j, the intermediate image(s) I.sub.mi, and the desired image I.sub.d can be obtained (a point y.sub.i(t) will belong to the initial image I.sub.i when t=0, belong to the desired image I.sub.d when t=t.sub.f, and belong to the intermediate images I.sub.m when 0<t<t.sub.f).

[0051] The processing unit 110 may further obtain objective feature points h.sub.i of each of the initial image I.sub.i, the desired image I.sub.d, and the intermediate image(s) I.sub.m by projecting the obtained feature points P.sub.i of each of the initial image I.sub.i, the desired image I.sub.d, and the intermediate image(s) I.sub.m onto a virtual unitary sphere V (see FIG. 68) (block 362). FIG. 68 is a schematic diagram of obtaining the objective feature points h.sub.i in the example of image feature obtaining of FIG. 6A. The virtual unitary sphere V is a virtual sphere centered at the origin of an image captured by the camera C, which is used in combination with a perspective projection by a unified spherical model that is a geometric formulation which models central imaging systems, so that the image measurements from any (kind of) camera which obeys the unified spherical model can all be projected onto the generalized unitary sphere. The x axis and the y axis of the camera coordinate system F of the mobile machine 100 define a motion plane, where the x axis is aligned with the forward direction of the mobile machine 100, and they axis is aligned with the axis of the wheels E of the mobile machine 100. The feature points P.sub.i (i.e., P.sub.0, P.sub.1 and P.sub.2) in the camera coordinate system F are projected onto the virtual unitary sphere V as the objective feature points h.sub.i (i.e., h.sub.0, h.sub.1, h.sub.2).

[0052] The processing unit 110 may further obtain the objective image features s.sub.r(t) of each of the initial image I.sub.i, the intermediate image(s) I.sub.m, and the desired image I.sub.d using the plurality of obtained objective feature points h.sub.i of each of the initial image I.sub.i, the intermediate image(s) I.sub.m, and the desired image I.sub.d (block 363 of FIG. 6A ). In some embodiments, the invariant visual features of image that are independent of the rotation of the camera C are used as the objective image features s.sub.r(t). Hence, the objective image features s.sub.r(f) may include centroid, moments, distances, areas, and other invariant visual features of image. In the case that the objective image features s.sub.r(t) include distances, the inverse of the distances d.sub.ab (e.g., d.sub.01 and d.sub.02 in FIG. 6B) between points h.sub.a and h.sub.b (e.g., h.sub.0 and h.sub.1, h.sub.0 and h.sub.2 in FIG. 6B) may be used as the image features S.sub.r(t) because it has the best linearizing properties between the task space and image space.

[0053] The image-based trajectory planning method plans a trajectory for a mobile machine with constraints in terms of image features in image plane coordinate. Then, during the navigation of the mobile machine, the poses of the mobile machine can be regulated according to the image features without requirements of the poses of the mobile machine relative to either inertial frame or visual target for feedback control, so that the mobile machine can eventually reach a desired pose in the trajectory without the need for pose measurements or estimations.

[0054] FIG. 7 is a schematic block diagram of an example of movement control for the mobile machine 100 of FIG. 2. In some embodiments, a movement control method for the mobile machine 100 is implemented in the mobile machine 100 to (plan a trajectory and) move the mobile machine 100 according to the planned trajectory by, for example, storing (sets of) instructions I.sub.e corresponding to the movement control method in the storage unit 120 and executing the stored instructions I.sub.e through the processing unit 110. Blocks 710-760 which plan a trajectory (e.g., the trajectory T.sub.2) for the mobile machine 100 and provide the objective image features s.sub.r(t) corresponding to the trajectory are omitted in FIG. 7 because they are the same as blocks 310-360 of FIG. 3A. respectively. In blocks 770-7120, the mobile machine 100 is controlled to move according to the obtained objective image features s.sub.r(t) corresponding to the planned trajectory.

[0055] According to the movement control method, the processing unit 110 may obtain a current image I.sub.c of the referenced target T captured at a current pose S.sub.c of the mobile machine 100 through the camera C of the mobile machine 100 (block 770). The current image I.sub.c has to include the referenced target T, so that feature points P.sub.i in the current image I.sub.c that correspond to the referenced target T can be obtained. The current pose S.sub.c is the pose of the mobile machine 100 at the time when the current image I.sub.c is captured, which may be just the initial pose S.sub.i if the mobile machine 100 has not moved yet after the initial image I.sub.i is captured (i.e., at the starting point of the trajectory 12).

[0056] The processing unit 110 may further obtain the feature points P.sub.i of the current image I.sub.c that are matched with the feature points P.sub.i of the desired image I.sub.d (block 780). The matched feature points P.sub.i in the current image I.sub.c are found so as to obtain current image features s.sub.r(t) of the current image I.sub.c for comparing with the objective image features s.sub.r(t) of the intermediate image(s) I.sub.m and the desired image I.sub.d.

[0057] The processing unit 110 may further obtain objective feature points h.sub.i by projecting the obtained feature points P.sub.i onto the virtual unitary sphere V (block 790). As mentioned above, the feature points P.sub.i are projected onto the virtual unitary sphere V as the objective feature points h.sub.i (see FIG. 6B).

[0058] The processing unit 110 may further obtain the current image features s.sub.r(t) of the current image I.sub.c using the obtained objective feature points h.sub.i (block 7100). As mentioned above, in some embodiments, the invariant visual features of image are used as the objective image features s.sub.r(t), and the objective image features s.sub.r(t) may include, for example, centroid, moments, distances, areas, and other invariant visual features of image. In the case that the obtained objective image features s.sub.r(t) include distances, the inverse of the distances d.sub.ab (e.g., d.sub.01 and d.sub.02 in FIG. 6B) between points h.sub.a and h.sub.b (e.g., h.sub.0 and h.sub.1, h.sub.0 and h.sub.2 in FIG. 6B) may be used as the objective image features s.sub.r(t).

[0059] The processing unit 110 may further calculate a difference D between the current image features s.sub.r(t) of the current image I.sub.c and the objective image features s.sub.r(t) corresponding to the current pose S.sub.c (block 7110). The current image features s.sub.r(t) of the current image I.sub.c are actual image features while the objective image features s.sub.r(t) corresponding to the current pose S.sub.c are expected image features, hence the current image features s.sub.r(t) of the current image I.sub.c may be compared with the objective image features s.sub.r(t) of the time of the current pose S.sub.c, that is, the current image features s.sub.r(t) and the objective image features s.sub.r(t) of the same time t are compared, so as to obtain the difference D by, for example, in the case that the objective image features s.sub.r(t) and the current image features s.sub.r(t) include distances, the above-mentioned inverse of distances d.sub.ab (e.g., d.sub.01 and d.sub.02 in FIG. 6B) in the current image features s.sub.r(t) may be subtracted from that in objective image features s.sub.r(t) to obtain the differences D between the distances d.sub.ab of the objective image features s.sub.r(t) and the current image features s.sub.r(t).

[0060] The processing unit 110 may further control the mobile machine 100 to move according to the calculated difference D (block 7120). In the case that the objective image features s.sub.r(t) and the current image features s.sub.r(t) include distances, since the current image features s.sub.r(t) are actual image features while the objective image features s.sub.r(t) are expected image features, the mobile machine 100 may be controlled to move according to the above-mentioned differences D between the distances d.sub.ab in the objective image features s.sub.r(t) and that in the current image features s.sub.r(t), so as to offset the differences therebetween so that the mobile machine 100 can be moved according to the planned trajectory (e.g., the trajectory T.sub.2).

[0061] The movement control method uses a trajectory for a mobile machine with constraints which is planned in terms of image features in image plane coordinate. The poses of the mobile machine will be regulated according to the image features during the movement of the mobile machine, and the mobile machine will be moved in a smoother way because the trajectory has planned with the optimization against the constraints tor the mobile machine. In addition, in the movement control method, natural backward movements are also allowed.

[0062] It can be understood by those skilled in the art that, all or part of the method in the above-mentioned embodiment(s) can be implemented by one or more computer programs to instruct related hardware. In addition, the one or more programs can be stored in a non-transitory computer readable storage medium. When the one or more programs are executed, all or part of the corresponding method in the above-mentioned embodiment(s) is performed. Any reference to a storage, a memory, a database or other medium may include non-transitory and/or transitory memory. Non-transitory memory may include read only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), Hash memory, solid-state drive (SSD), or the like. Volatile memory may include random access memory (RAM), external cache memory, or the like.

[0063] The processing unit 110 (and the above-mentioned processor) may include central processing unit (CPU), or be other general purpose processor, digital signal processor (DSP), application specific integrated circuit (ASIC), field-programmable gate array (FPGA), or be other programmable logic device, discrete gate, transistor logic device, and discrete hardware component. The general purpose processor may be microprocessor, or the processor may also be any conventional processor. The storage unit 120 (and the above-mentioned memory) may include internal storage unit such as hard disk and internal memory. The storage unit 120 may also include external storage device such as plug-in hard disk, smart media card (SMC), secure digital (SD) card, and flash card.

[0064] The exemplificative units/modules and methods/steps described in the embodiments may be implemented through software, hardware, or a combination of software and hardware. Whether these functions are implemented through software or hardware depends on the specific application and design constraints of the technical schemes. The above-mentioned trajectory planning method and mobile machine may be implemented in other manners. For example, the division of units/modules is merely a logical functional division, and other division manner may be used in actual implementations, that is, multiple units/modules may be combined or be integrated into another system, or some of the features may be ignored or not performed. In addition, the above-mentioned mutual coupling/connection may be direct coupling/connection or communication connection, and may also be indirect coupling/connection or communication connection through some interfaces/devices, and may also be electrical, mechanical or in other forms.

[0065] The above-mentioned embodiments are merely intended for describing but not for limiting the technical schemes of the present disclosure. Although the present disclosure is described in detail with reference to the above-mentioned embodiments, the technical schemes in each of the above-mentioned embodiments may still be modified, or some of the technical features may be equivalently replaced, so that these modifications or replacements do not make the essence of the corresponding technical schemes depart from the spirit and scope of the technical schemes of each of the embodiments of the present disclosure, and should be included within the scope of the present disclosure.