Motion control methods, motion devices and storage media

Abstract

This disclosure provides a motion control method, a motion device, and a storage medium. In the control method, based on scene information of the motion device, the uncertainty factor values ​​of various uncertainties affecting the behavior of the motion device are calculated. A fuzzy decision model is used to determine the motion speed and behavior of the motion device based on these uncertainty factor values. Based on the motion closed-loop control rules for achieving the desired speed under the determined behavior, a corresponding acceleration control command is obtained; wherein the desired speed is obtained based on the determined motion speed. Based on the acceleration control command of the motion device, a corresponding target posture is determined, and control of the drive motor is executed to move the motion device to the target posture. By considering various uncertainties affecting the motion speed of the motion device in a scene, making corresponding decisions, and employing multimodal control, safe and fast motion is achieved in complex and uncertain environments.

Description

Technical Field

[0001] This disclosure relates to the field of motion control technology, and in particular to motion control methods, motion devices and storage media. Background Technology

[0002] In recent years, drones have demonstrated enormous potential in search and rescue, inspection and surveillance. Due to their agility, drones can easily reach locations inaccessible to humans, allowing them to perform dangerous tasks in their place.

[0003] Rapid flight of unmanned aerial vehicles (UAVs) has always been a key research focus. Because tasks in these fields, such as search and rescue and detection, are often urgent, and UAVs have limited endurance, rapid flight is of significant research value. While simple rapid flight has become achievable with the advancement of UAV technology, more complex and uncertain environments still present greater challenges to ensuring the autonomy and safety of UAV flight.

[0004] The main challenge facing drones in high-speed flight in complex environments is the widespread uncertainty inherent in perception, localization, and control. This includes issues such as perception ambiguity, localization drift, and control errors, and these uncertainties become more pronounced as drone speed increases. Current research falls into two categories: one focuses on overcoming the challenges of high-speed flight, but these methods face significant safety risks when the environment changes; the other prioritizes safety and employs conservative flight strategies, but these approaches are still insufficient for improving drone flight efficiency.

[0005] Therefore, finding an environmental behavior decision-making scheme suitable for drones that balances flight safety and efficiency has become a pressing technical problem for the industry. Summary of the Invention

[0006] In view of the shortcomings of the prior art described above, the purpose of this disclosure is to provide a motion control method, motion device and storage medium to solve the problems in the related art.

[0007] The first aspect of this disclosure provides a motion control method for a motion device, comprising: calculating uncertainty factor values ​​of various uncertain factors affecting the behavior of the motion device based on scene information of the motion device; determining the decision motion speed and decision behavior of the motion device based on the multiple uncertainty factor values ​​using a fuzzy decision model; obtaining a corresponding acceleration control command based on a motion closed-loop control rule for the motion device to reach a desired speed under the decision behavior; wherein the desired speed is obtained based on the decision motion speed; determining a corresponding target posture based on the acceleration control command of the motion device, and controlling the motion device to move to the target posture.

[0008] In an embodiment of the first aspect, based on scene information of the motion device, the uncertainty factor values ​​of various uncertainties affecting the behavior of the motion device are calculated, including at least: calculating a first uncertainty factor value representing the uncertainty of the perceived target position based on the prior position of the target and the state information of the motion device; calculating a second uncertainty factor value representing the uncertainty of the gap between obstacles for the motion device to pass through based on the relationship between the passable width between obstacles in the motion direction of the motion device within its field of vision and the preset passable width required by the motion device; and calculating a third deterministic factor value representing the uncertainty of control precision based on the ratio of the current speed to the maximum speed of the motion device.

[0009] In a first aspect embodiment, the calculation of a first uncertainty factor value representing the uncertainty of the perceived target position based on the prior position of the target and the state information of the motion device includes:

[0010] According to the calculation formula μ1=p1*f(x) / f m (x) Calculate the value of the first uncertainty factor; f(x) is the probability density function of x, f m (x) is a probability density function that follows a multidimensional normal distribution with x as the variable and xm as the expectation; where p1 is a constant coefficient, x is the motion state information of the motion device, and xm is the expected value. m The prior location information of the target;

[0011] And / or, the calculation of a second uncertainty factor value characterizing the uncertainty of the clearance between obstacles for the motion device, based on the relationship between the passable width between obstacles in the motion direction within the motion device's field of vision and the preset passable width required by the motion device, includes:

[0012] Taking the direction of motion of the device as To the left is To the right is Location is The location of obstacles within the field of view is Calculate the value of the second uncertainty factor, u2:

[0013]

[0014] Where, d vision Indicates the field of vision sensed by the motion device; l body Indicates the minimum safe passage width, l safe This indicates the minimum free pass width for the drone. And satisfy or c1 is a constant; E is (1, 1, 1). T;

[0015] And / or, based on the ratio of the current speed to the maximum speed of the motion device, calculate a third uncertainty factor value characterizing the uncertainty of control precision, including:

[0016] According to the formula

[0017] Among them, v s c2 and c3 are constants, representing the lower limit threshold for safe speed.

[0018] In an embodiment of the first aspect, the step of determining the motion speed of the motion device using a fuzzy decision model based on the values ​​of the multiple uncertainty factors includes: obtaining the motion speed based on the influence of the combined effects of the multiple uncertainty factors on the preset maximum motion speed of the motion device.

[0019] In a first aspect embodiment, obtaining the decision movement speed based on the multiple uncertainty factors using a fuzzy decision model includes:

[0020] calculate Among them, u i Let wj be the value of each uncertainty factor, m be the weight of each uncertainty factor value, and m be the number of uncertainty factor values, where m ≥ 3;

[0021] Keep u constant, based on the formula The velocity v of the decision-making motion is calculated.

[0022] In an embodiment of the first aspect, the step of determining the decision behavior of the motion device based on the values ​​of the multiple uncertainty factors using a fuzzy decision model includes: obtaining a set of combined effects formed by combining each uncertainty factor with each of the other uncertainty factors; wherein each combined effect is calculated using the factor values ​​of two uncertainty factors; determining the uncertainty factor value that has the greatest impact on the decision behavior as the target uncertainty factor value; calculating the corresponding overall combined effect value based on the set of combined effects of the target uncertainty factor value, and determining the associated decision behavior.

[0023] In a first aspect embodiment, obtaining the decision-making behavior of the motion device based on the values ​​of the multiple uncertainty factors using a fuzzy decision model includes:

[0024] Define a fuzzy matrix, where the element in the i-th row and j-th column is defined as r. ij =ρ ij w i w j h(μ i )h(μ j );

[0025] Where, ρ ij The row-column correlation coefficient representing the elements, w i and w j Let represent the row and column weights respectively. The h function characterizes the standard for whether the impact of uncertain factors is positive or negative, and its calculation formula is:

[0026]

[0027] The overall combined impact value I is calculated based on the following formula:

[0028]

[0029] The associated decision-making behavior is determined based on the value of I.

[0030] In the first aspect of the embodiment, the decision-making behavior includes at least one of: visual tracking behavior, obstacle avoidance behavior for dense obstacles, and prior navigation behavior; the obtaining of the corresponding acceleration control command based on the motion closed-loop control rule that the motion device achieves the desired speed under the decision-making behavior includes at least one of the following:

[0031] When the decision-making behavior is visual tracking, the object detection algorithm is executed to obtain the updated object position x. d And determine the position information in the state information x of the motion device, and calculate the acceleration command according to the following formula.

[0032]

[0033] Among them, v d The desired velocity is obtained by averaging and filtering multiple decision velocities; Kp is the proportional gain of the position loop, K vp K is the proportional gain of the velocity loop. vd K is the differential gain of the velocity loop. vi The integral gain of the velocity loop; A represents the world coordinate system, and B represents the motion device coordinate system. and These represent the rotation matrices for the transformation from the world coordinate system / motion device coordinate system to the motion device coordinate system / world coordinate system, respectively.

[0034] When the decision-making behavior is obstacle avoidance behavior with dense obstacles, the point cloud information is perceived and the obstacle center is fitted with a Gaussian model, and the decision-making motion speed is added to the obstacle avoidance planning algorithm corresponding to the obstacle.

[0035] When the decision-making behavior is a priori navigation behavior, navigation route planning is performed based on prior information, and acceleration commands are calculated according to the following formula.

[0036]

[0037] The second aspect of this disclosure provides a motion device, comprising: an environmental sensor for point cloud information acquisition, a positioning sensor for self-positioning, and a drive motor system for driving the motion device; a memory and a processor; the memory storing program instructions; the processor being communicatively connected to the environmental sensor, the positioning sensor, the drive motor system, and the memory, for running the program instructions to implement the motion control method as described in any one of the first aspects;

[0038] A third aspect of this disclosure provides a computer-readable storage medium storing program instructions that, when executed, perform the motion control method as described in any one of the first aspects.

[0039] As described above, this disclosure provides a motion control method, a motion device, and a storage medium. In the control method, based on scene information of the motion device, uncertainty factor values ​​of various uncertainties affecting the behavior of the motion device are calculated. A fuzzy decision model is used to determine the motion speed and behavior of the motion device based on these uncertainty factor values. A corresponding acceleration control command is obtained based on the motion closed-loop control rule that allows the motion device to reach the desired speed under the decision behavior. The desired speed is obtained based on the decided motion speed. A corresponding target posture is determined based on the acceleration control command of the motion device, and the motion device is controlled to move to the target posture. By considering various uncertainties affecting the motion speed of the motion device in a scene, making corresponding decisions, and employing multimodal control, safe and fast motion is achieved in complex and uncertain environments. Attached Figure Description

[0040] Figure 1 A flowchart illustrating the motion control method in an embodiment of this disclosure is shown.

[0041] Figure 2 A schematic diagram illustrating the principle of obstacle density and the passage of the moving device in the embodiments of this disclosure is shown.

[0042] Figure 3 A schematic diagram of the system structure corresponding to the motion control method in the embodiments of this disclosure is shown.

[0043] Figure 4 A schematic diagram of the motion control system modules in an embodiment of this disclosure is shown.

[0044] Figure 5 A schematic diagram of the motion device in an embodiment of this disclosure is shown. Detailed Implementation

[0045] The following specific examples illustrate the implementation of this disclosure. Those skilled in the art can easily understand other advantages and effects of this disclosure from the information disclosed herein. This disclosure can also be implemented or applied through other different specific embodiments, and various details in this disclosure can be modified or changed according to different viewpoints and application modules without departing from the spirit of this disclosure. It should be noted that, unless otherwise specified, the embodiments and features in the embodiments of this disclosure can be combined with each other.

[0046] The embodiments of this disclosure will now be described in detail with reference to the accompanying drawings, so that those skilled in the art to which this disclosure pertains can readily implement it. This disclosure may be embodied in many different forms and is not limited to the embodiments described herein.

[0047] In this disclosure, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic represented in connection with that embodiment or example is included in at least one embodiment or example of this disclosure. Furthermore, the specific features, structures, materials, or characteristics represented may be combined in any suitable manner in any one or a group of embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples represented in this disclosure, as well as the features of those different embodiments or examples.

[0048] Furthermore, the terms "first" and "second" are used for illustrative purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the representation of this disclosure, "a set" means two or more, unless otherwise explicitly specified.

[0049] For the purpose of clarity, devices unrelated to the description are omitted, and the same or similar components throughout the specification are given the same reference numerals.

[0050] Throughout this specification, when it is said that a device is "connected" to another device, this includes not only "direct connection" but also "indirect connection" by placing other components in between. Furthermore, when it is said that a device "comprises" a certain constituent element, unless otherwise stated otherwise, this does not exclude other constituent elements, but rather implies that other constituent elements may be included.

[0051] While the terms first, second, etc., are used in some examples herein to refer to various elements, these elements should not be limited by these terms. These terms are used only to distinguish one element from another. For example, first interface and second interface, etc., are used. Furthermore, as used herein, the singular forms “a,” “an,” and “the” are intended to also include the plural forms unless the context indicates otherwise. It should be further understood that the terms “comprising,” “including,” indicate the presence of the stated feature, step, operation, element, module, item, kind, and / or group, but do not exclude the presence, occurrence, or addition of one or more other features, steps, operations, elements, modules, items, kinds, and / or groups. The terms “or” and “and / or” as used herein are interpreted as inclusive, or mean any one or any combination thereof. Thus, “A, B, or C” or “A, B, and / or C” means “any one of: A; B; C; A and B; A and C; B and C; A, B, and C.” Exceptions to this definition will only occur if the combination of elements, functions, steps, or operations is inherently mutually exclusive in some way.

[0052] The technical terms used herein are for reference only to specific embodiments and are not intended to limit the scope of this disclosure. The singular form used herein includes the plural form unless the statement explicitly indicates otherwise. The word "comprising" as used in this specification means to specify a particular characteristic, region, integer, step, operation, element, and / or component, and does not exclude the presence or addition of other characteristics, regions, integers, steps, operations, elements, and / or components.

[0053] Although not explicitly defined, all terms, including technical and scientific terms used herein, shall have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure pertains. Terms defined in commonly used dictionaries shall be further interpreted as having a meaning consistent with the relevant technical literature and the message of the present disclosure, and shall not be over-interpreted as having an ideal or overly formulaic meaning unless otherwise defined.

[0054] The main challenge for drones flying rapidly in complex environments is the widespread uncertainty inherent in various aspects, including perception, localization, and control. These uncertainties include perceptual ambiguity, localization drift, and control errors, and they become more pronounced as drone speed increases. Because of these uncertainties, drones struggle to balance safety and speed in complex environments.

[0055] In view of this, the present disclosure provides a motion control method for a motion device, which evaluates the uncertainty factors of the motion of the motion device to make fuzzy decisions and performs motion in a corresponding behavior pattern, taking into account both fast and safe motion.

[0056] It should be noted that the motion device may be an aircraft, such as a drone or similar device that moves in space; or it may be, for example, an unmanned vehicle, an unmanned boat, or other mobile robot.

[0057] like Figure 1 The diagram shown illustrates the structure of the motion device 1 in one embodiment of this disclosure.

[0058] The motion device 1 includes sensors for external perception, such as an environmental sensor for point cloud information acquisition and a positioning sensor 103 for self-positioning. In some embodiments, the environmental sensor may include an image sensor 101 and a depth sensor 102. The image sensor 101 is used to acquire images, i.e., RGB information of target points in the environment, while the depth sensor 102 is used to acquire depth information, i.e., D information, of obstacles at the target points in the environment. In some embodiments, the depth sensor 102 may be based on time-of-flight (ToF). In some embodiments, the image sensor 101 and the depth sensor 102 may be integrated into a depth camera module. In some embodiments, the positioning sensor 103 may include, for example, a GPS module.

[0059] The motion device 1 includes a drive motor system 107 for driving the motion device 1. In some embodiments, the drive motor system 107 may be connected to and drive the motion mechanism 108 of the motion device 1, such as the rotor of a drone, or the wheelset of a vehicle or robot. In some embodiments, the drive motor system 107 may include one or more servo motors.

[0060] The motion device 1 includes a controller 100, such as an onboard computer of a drone. The controller 100 includes a memory and a processor. The memory stores program instructions; the processor is communicatively connected to the environmental sensor, positioning sensor 103, drive motor system, and memory, and is used to execute the program instructions to perform motion control methods. The controller 100 can communicate with the image sensor 101, depth sensor 102, positioning sensor 103, drive motor system, etc., for example, through a serial bus interface, general purpose input / output interface, etc., so that the processor can receive data from these components or send instructions to them.

[0061] As an example, Figure 1 The diagram illustrates a possible communication architecture between the memory and the processor. The processor 104 and the memory 109 can communicate via bus 105. The memory 109 can store program instructions. The processor 104 implements the steps in the camera calibration method of the previous embodiment by executing the program instructions stored in the memory 109.

[0062] Bus 105 can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. Buses can be categorized as address buses, data buses, control buses, etc. For ease of representation, although only one thick line is used in the diagram, this does not indicate that there is only one bus or one type of bus.

[0063] In some embodiments, the processor 104 may be implemented as a central processing unit (CPU), a microprocessor unit (MCU), a system-on-chip (System-on-Chip), or a field-programmable array (FPGA). The memory 109 may include volatile memory for temporary data storage during program execution, such as random access memory (RAM).

[0064] The memory 109 may also include non-volatile memory for data storage, such as read-only memory (ROM), flash memory, hard disk drive (HDD), or solid-state disk (SSD).

[0065] In some embodiments, the controller 100 may further include a communicator 106. The communicator 106 is used for communication with external devices. In specific examples, the communicator 106 may include one or more wired and / or wireless communication circuit modules. For example, the communicator 106 may include one or more of the following: a wired network card, a USB module, a serial interface module, etc. The wireless communication protocols followed by the wireless communication module include, for example, Nearfield Communication (NFC) technology, Infrared (IR) technology, Global System for Mobile Communications (GSM), General Packet Radio Service (GPRS), Code Division Multiple Access (CDMA), Wideband Code Division Multiple Access (WCDMA), Time-Division Code Division Multiple Access (TD-SCDMA), Long Term Evolution (LTE), Bluetooth (BT), Global Navigation Satellite System (GNSS), etc.

[0066] like Figure 2 The diagram shown illustrates a flow chart of a motion control method applied to a motion device according to an embodiment of this disclosure.

[0067] The motion control method includes:

[0068] Step S201: Based on the scene information of the motion device, calculate the uncertainty factor values ​​of various uncertainty factors affecting the behavior of the motion device.

[0069] In some embodiments, the scene information may include point cloud information from image sensors and depth sensors, self-position information obtained by positioning sensors, and may also include prior information, such as prior velocity and volume of the target, prior map information, etc.

[0070] In some embodiments, the plurality of uncertainties may include: a first uncertainty factor value characterizing the uncertainty of the perceived target position, a second uncertainty factor value characterizing the uncertainty of the clearance between obstacles for the motion device, and a third deterministic factor value characterizing the uncertainty of control precision. Of course, the plurality of uncertainties may also include, for example, weather conditions, the precision of components of the motion device, etc.

[0071] As an example, several methods for calculating the values ​​of uncertainty factors are provided.

[0072] For example, a first uncertainty factor value representing the uncertainty of the perceived target position is calculated based on the prior position of the target and the state information of the motion device.

[0073] In one example, the calculation can be performed according to the formula μ1=p1*f(x) / f m (x) Calculates the value of the first uncertainty factor; f(x) is the probability density function of x. x represents the state information of the motion device, including position and attitude information. For example, X of a quadcopter drone is (x, y, z, q1, q2, q3, q4), where (x, y, z) represents the position information, and (q1, q2, q3, q4) represents the attitude information of the four rotors. p1 is a constant coefficient, x... m The prior location information of the target.

[0074] f m (x) represents x as a variable, and x m The expected probability density function conforming to a multidimensional normal distribution is:

[0075]

[0076] For example, based on the relationship between the passable width between obstacles in the direction of movement of the motion device within its field of vision and the preset passable width required by the motion device, a second uncertainty factor value characterizing the uncertainty of the gap between obstacles in the motion device is calculated.

[0077] In one example, the calculation method for the second uncertainty factor value can be referred to Figure 3 As shown.

[0078] like Figure 3 As shown, with the direction of motion of the moving device as... To the left is To the right is Location is The location of obstacles within the field of view is Indicates the direction of movement of the moving device Obstacle on the left Indicates the direction of movement of the moving device Obstacles on the right. These obstacles can be constructed by fitting point cloud data; the center point of each fitted obstacle point set can be used as... and Based on the above, calculate the value of the second uncertainty factor, u2:

[0079]

[0080] Where, d vision Indicates the field of vision sensed by the motion device; l body Indicates the minimum safe passage width, l safe This indicates the minimum free pass width for the drone. This is the sum of the narrowest width 'a' to the left and the narrowest width 'b' to the right in the direction of travel, which represents the passable width in the direction of travel. Furthermore, it satisfies... or c1 is a constant; E is (1, 1, 1). T .

[0081] For example, based on the ratio of the current speed to the maximum speed of the motion device, a third uncertainty factor value is calculated to characterize the uncertainty of control precision.

[0082] In some embodiments, the value of the third uncertainty factor can be calculated according to the following formula.

[0083] According to the formula

[0084] Among them, v s c2 and c3 are constants, representing the lower limit threshold for safe speed.

[0085] In some embodiments, in addition to u1 to u3, the other uncertainties are u i (i>3) can be extended to include these uncertainties, which are treated as normal distributions and can be substituted with constant values.

[0086] Step S202: Based on the values ​​of the various uncertainty factors, the motion device is determined by a fuzzy decision model to determine its motion speed and behavior.

[0087] In some embodiments, the decision motion speed can be obtained based on the influence of the combined effects of the various uncertainty factors on the preset maximum motion speed of the motion device.

[0088] As a calculation example, establish the velocity decision correlation. This can be calculated first. Among them, u i For each uncertainty factor, w i Let m be the weight of each uncertainty factor value, and m be the number of uncertainty factor values, where m ≥ 3;

[0089] Keep u constant, based on the formula The calculated decision-making speed v is the maximum speed v under the combined effect of the various uncertain factors. max Under the influence of factors such as large target perception deviation and dense obstacle distribution, the flight speed of the drone will be reduced for safety reasons, resulting in a suitable decision-making motion speed v.

[0090] In some embodiments, the decision-making behavior of the motion device based on the values ​​of the multiple uncertainty factors using a fuzzy decision model can be exemplarily determined by the magnitude of the combined influence of the multiple uncertainty factors. For example, a set of combined influences formed by each uncertainty factor and each of the other uncertainty factors can be obtained. Each combined influence is calculated using the factor values ​​of two uncertainty factors. The uncertainty factor value with the greatest combined influence on the decision-making behavior is determined as the target uncertainty factor value; the overall combined influence value is calculated based on the combined influence of the target uncertainty factor value, and the associated decision-making behavior is determined.

[0091] As an example for intuitive illustration, the process of obtaining the decision-making behavior of the motion device based on the values ​​of the various uncertainty factors using a fuzzy decision model includes:

[0092] Define a fuzzy matrix, where the element in the i-th row and j-th column is defined as r. ij =ρ ij w i w j h(μ i )h(μ j );

[0093] Where, ρ ij The row-column correlation coefficient representing the elements, w i and w j Let represent the row and column weights respectively. The h function characterizes the standard for whether the impact of uncertain factors is positive or negative, and its calculation formula is:

[0094]

[0095] The overall combined impact value I is calculated based on the following formula:

[0096]

[0097] The decision-making behavior associated with the value of I can be determined as follows, for example:

[0098] When I=1, the decision-making behavior is visual tracking behavior;

[0099] When I=2, the decision-making behavior is obstacle avoidance behavior with dense obstacles;

[0100] When I=3, the decision-making behavior is a priori navigation behavior. ...

[0102] More decision-making actions can be set based on the results of I. The above are just some examples and are not limited to this.

[0103] Step S203: Based on the motion closed-loop control rules for the motion device to reach the desired speed under the decision-making behavior, obtain the corresponding acceleration control command.

[0104] The motion closed-loop control rule can be PID control of the drive motor system, including speed loop and position loop control. For example, the control parameters involved in the acceleration control command may include, for instance, the proportional gain Kp of the position loop and the proportional gain K of the speed loop. vp The differential gain K of the velocity loop vd K is the integral gain of the velocity loop. vi wait.

[0105] In the first aspect of the embodiment, obtaining the corresponding acceleration control command based on the motion closed-loop control rule that the motion device achieves the desired speed under decision-making behavior includes at least one of the following:

[0106] When the decision-making behavior is visual tracking, the object detection algorithm is executed to obtain the updated object position x. d And determine the position information in the motion device's state information x. Calculate the acceleration command according to the following formula.

[0107]

[0108] ; where v d The desired velocity is obtained by averaging and filtering multiple decision velocities; Kp is the proportional gain of the position loop, K vp K is the proportional gain of the velocity loop. vd K is the differential gain of the velocity loop. vi This is the integral gain of the velocity loop; This represents the estimated value of x, i.e., the estimated location. The velocity represents the derivative of the estimated position. A represents the world coordinate system, and B represents the coordinate system of the moving device. and These represent the rotation matrices for the transformation from the world coordinate system / motion device coordinate system to the motion device coordinate system / world coordinate system, respectively; diag([0, 1, 1]) represents the construction of a diagonal matrix with diagonals of 0, 1, and 1. In some embodiments, the target detection algorithm can employ known algorithms, such as relatively fast one-stage target detection algorithms, such as OverFeat, YOLOv1, YOLOv2, YOLOv3, SSD, and RetinaNet.

[0109] When the decision-making behavior is obstacle avoidance behavior involving dense obstacles, point cloud information is perceived and the obstacle center is fitted using a Gaussian model, and the decision-making motion velocity is incorporated into the obstacle avoidance planning algorithm corresponding to the obstacle. In some embodiments, the obstacle avoidance planning algorithm can be a known algorithm, such as the UAV 3D obstacle avoidance algorithm described in Chinese patent application CN202110567745.6.

[0110] When the decision-making behavior is a priori navigation behavior, navigation route planning is performed based on prior information (such as a priori map), and acceleration commands are calculated according to the following formula.

[0111]

[0112] Step S204: Determine the corresponding target posture based on the acceleration control command of the motion device, and control the motion device to move to the target posture.

[0113] In some embodiments, acceleration commands can be converted into attitude information through the dynamic model of the UAV, and then converted into motor speed values ​​through a low-level conversion model (such as the UAV flight control software Pixhawk), so as to use as control commands to drive the drive motor system.

[0114] like Figure 4 The diagram shows a system architecture schematic for implementing a motion control method in one embodiment of this disclosure.

[0115] The system architecture consists of two parts. The first part is fuzzy decision-making, which uses fuzzy models to assess uncertainties based on sensor data and prior information to determine velocity and mode. By fuzzy modeling the uncertainties affecting rapid flight, mode decisions are determined based on the dominant uncertainties, and velocity decisions are established through the relationships between these uncertainties. The second part is multimodal control. Based on the fuzzy decision-making results, a control strategy is determined based on the dominant mode decisions, and desired velocity control is added based on the velocity decisions, ultimately forming multimodal control. This multimodal control is used for navigation route planning and switching control in behaviors such as visual tracking, prior navigation, and obstacle avoidance. Finally, the acceleration commands from the multimodal control are fed into the dynamics system and converted into target attitude information. The desired velocity corresponding to the decided velocity is also incorporated, and this information can be converted into control commands for the drive motor system to complete motion control.

[0116] In some embodiments, the software platform in the system architecture is implemented based on distributed nodes in ROS. Sensors involved include image sensors, depth sensors, and positioning sensors. Decision and control commands can be sent from the onboard computer of the motion device (such as a drone) to the underlying Pixhawk hardware via the MAVROS communication protocol, where they are converted into motor speed control commands.

[0117] like Figure 5 The diagram shows a schematic representation of the motion control device in an embodiment of this disclosure. It should be noted that the principle and technical implementation of the motion control device can be referenced from the motion control methods in previous embodiments; therefore, they will not be repeated in this embodiment.

[0118] The motion control device 500 includes:

[0119] Uncertainty factor assessment module 501 is used to calculate the uncertainty factor values ​​of various uncertain factors affecting the behavior of the motion device based on the scene information of the motion device.

[0120] Decision module 502 is used to determine the motion speed and decision behavior of the motion device based on the values ​​of the various uncertainty factors using a fuzzy decision model;

[0121] The instruction acquisition module 503 is used to obtain the corresponding acceleration control instruction based on the motion closed-loop control rule of the motion device reaching the desired speed under the decision behavior; wherein, the desired speed is obtained based on the decision motion speed.

[0122] The motion control module 504 is used to determine the corresponding target posture based on the acceleration control command of the motion device, and control the motion device to move to the target posture.

[0123] It should be noted that, in Figure 5 The various functional modules in the embodiments can be implemented, in whole or in part, through software, hardware, firmware, or any combination thereof. When implemented in software, they can be implemented, in whole or in part, in the form of a program instruction product. A program instruction product includes one or a set of program instructions. When the program instructions are loaded and executed on a computer, all or part of the flow or function according to this disclosure is generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The program instructions can be stored in a computer-readable storage medium or transferred from one computer-readable storage medium to another.

[0124] and, Figure 5 The apparatus disclosed in the embodiments can be implemented through other modular division methods. The apparatus embodiments shown above are merely illustrative. For example, the module division is only a logical functional division, and in actual implementation, there may be other division methods. For example, a group of modules or modules may be combined or dynamically integrated into another system, or some features may be ignored or not executed. Furthermore, the shown or discussed mutual coupling, direct coupling, or communication connection may be through some interfaces, and the indirect coupling or communication connection between devices or modules may be electrical or other forms.

[0125] in addition, Figure 5 The functional modules and sub-modules in the embodiments can be dynamically integrated within a single processing unit, or each module can exist physically independently, or two or more modules can be dynamically integrated within a single unit. These dynamic units can be implemented in hardware or as software functional modules. If these dynamic units are implemented as software functional modules and sold or used as independent products, they can also be stored in a computer-readable storage medium. This storage medium can be a read-only memory, a hard disk, or an optical disk, etc.

[0126] It should be specifically noted that the flowchart representations of the embodiments described above in this disclosure can be understood as representing modules, segments, or portions of code comprising one or more sets of executable instructions configured to implement specific logical functions or processes. Furthermore, the scope of the preferred embodiments of this disclosure includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved.

[0127] For example, Figure 2 The order of the steps in the method embodiments may vary in specific scenarios and is not limited to the above representation.

[0128] This disclosure also provides a computer-readable storage medium storing program instructions that, when executed, implement the motion control method of any of the previous embodiments.

[0129] That is, the method steps in the above embodiments are implemented as software or computer code that can be stored in a recording medium (such as CD ROM, RAM, floppy disk, hard disk or magneto-optical disk), or implemented as computer code that is originally stored in a remote recording medium or a non-transitory machine-readable medium and will be stored in a local recording medium after being downloaded via a network, so that the method represented herein can be stored in such software processing on a recording medium using a general-purpose computer, a special processor or programmable or special hardware (such as ASIC or FPGA).

[0130] In summary, the embodiments of this disclosure provide a motion control method, a motion device, and a storage medium. In the control method, based on scene information of the motion device, the uncertainty factor values ​​of various uncertainties affecting the behavior of the motion device are calculated. A fuzzy decision model is used to determine the motion speed and behavior of the motion device based on these multiple uncertainty factor values. Based on the motion closed-loop control rules for the motion device reaching the desired speed under the decision behavior, a corresponding acceleration control command is obtained; wherein the desired speed is obtained based on the decided motion speed. Based on the acceleration control command of the motion device, a corresponding target posture is determined, and the motion device is controlled to move to the target posture. By considering multiple uncertainties affecting the motion speed of the motion device in a scene, making corresponding decisions, and employing multimodal control, safe and fast motion is achieved in complex and uncertain environments.

[0131] The above embodiments are merely illustrative of the principles and effects of this disclosure and are not intended to limit this disclosure. Any person skilled in the art can modify or alter the above embodiments without departing from the spirit and scope of this disclosure. Therefore, all equivalent modifications or alterations made by those skilled in the art without departing from the spirit and technical concept disclosed in this disclosure should still be covered by the claims of this disclosure.

Claims

1. A motion control method applied to a motion device, characterized in that, include: Based on scene information of the motion device, the uncertainty factor values ​​of various uncertainties affecting the behavior of the motion device are calculated, including at least: a first uncertainty factor value representing the uncertainty of the perceived target position based on the prior position of the target and the state information of the motion device; a second uncertainty factor value representing the uncertainty of the gap between obstacles for the motion device to pass through based on the relationship between the passable width between obstacles in the motion direction of the motion device within its field of view and the preset passable width required by the motion device; and a third deterministic factor value representing the uncertainty of control precision based on the ratio of the current speed to the maximum speed of the motion device. The motion device's decision speed and behavior are determined using a fuzzy decision model based on the values ​​of the various uncertainty factors. The determination of the decision speed using the fuzzy decision model based on the values ​​of the various uncertainty factors includes: calculating... ;in, For each uncertainty factor, The weights of each uncertainty factor value are given, where m is the number of uncertainty factor values, and m ≥ 3; Maintain Constant, based on formula The velocity v of the decision-making motion is calculated; Based on the motion closed-loop control rules for the motion device to achieve the desired speed under decision-making behavior, the corresponding acceleration control command is obtained; wherein, the desired speed is obtained based on the decision-making motion speed; Based on the acceleration control command of the motion device, the corresponding target posture is determined, and the motion device is controlled to move to the target posture.

2. The motion control method according to claim 1, characterized in that, The calculation of the first uncertainty factor value, which characterizes the uncertainty of the perceived target position based on the prior position of the target and the state information of the motion device, includes: According to the calculation formula Calculate the value of the first uncertainty factor; f(x) is the probability density function of x. Let x be the variable, and x m Let be the expected probability density function conforming to a multidimensional normal distribution; where, Let x be a constant coefficient, and let x be the motion state information of the motion device. m The prior location information of the target; And / or, the calculation of a second uncertainty factor value characterizing the uncertainty of the clearance between obstacles for the motion device, based on the relationship between the passable width between obstacles in the motion direction within the motion device's field of vision and the preset passable width required by the motion device, includes: Taking the direction of motion of the device as To the left is To the right is The location is The location of obstacles within the field of view is Calculate the value of the second uncertainty factor. : ； in, Indicates the field of vision sensed by the motion device; Indicates the minimum safe passage width. Minimum free pass width for drones And satisfy , or c1 is a constant; E is (1, 1, 1). T ; And / or, based on the ratio of the current speed to the maximum speed of the motion device, calculate a third uncertainty factor value characterizing the uncertainty of control precision, including: According to the formula ; in, c2 and c3 are constants, representing the lower limit threshold for safe speed.

3. The motion control method according to claim 1, characterized in that, The process of determining the motion speed of the motion device using a fuzzy decision model based on the values ​​of the various uncertainty factors includes: The decision motion speed is obtained based on the combined effect of the various uncertain factors on the preset maximum motion speed of the motion device.

4. The motion control method according to claim 3, characterized in that, The decision-making behavior of the motion device based on the values ​​of the multiple uncertainty factors using a fuzzy decision model includes: Obtain a set of combined effects formed by combining each uncertainty factor with each of the other uncertainty factors; wherein each combined effect is calculated using the factor values ​​of the two uncertainty factors; The set of uncertainties that has the greatest impact on decision-making behavior is identified as the target uncertainty value. Based on a set of combined effects of the target uncertainty factors, calculate the corresponding overall combined effect value and determine the associated decision-making behavior.

5. The motion control method according to claim 3, characterized in that, The process of obtaining the decision-making behavior of the motion device based on the values ​​of the various uncertainty factors using a fuzzy decision model includes: Define a fuzzy matrix, where the element in the i-th row and j-th column is defined as... ; in, ij The row-column correlation coefficient representing the elements, w i and w j Let represent the row and column weights respectively. The h function characterizes the standard for whether the impact of uncertain factors is positive or negative, and its calculation formula is: ， The overall combined impact value I is calculated based on the following formula: ; The associated decision-making behavior is determined based on the value of I.

6. The motion control method according to claim 1 or 5, characterized in that, The decision-making behavior includes at least one of: visual tracking behavior, obstacle avoidance behavior in dense obstacles, and prior navigation behavior; based on the motion closed-loop control rules for the motion device to achieve the desired speed under the decision-making behavior, the corresponding acceleration control command is obtained, including at least one of the following: When the decision-making behavior is visual tracking, the object detection algorithm is executed to obtain the updated object position x. d And determine the position information in the state information x of the motion device, and calculate the acceleration command according to the following formula. : ； Among them, v d The desired velocity is obtained by averaging and filtering the velocities of multiple decision-making movements; K p K is the proportional gain of the position loop. vp K is the proportional gain of the velocity loop. vd K is the differential gain of the velocity loop. vi The integral gain of the velocity loop; A represents the world coordinate system, and B represents the motion device coordinate system. and These represent the rotation matrices for the transformation from the world coordinate system / motion device coordinate system to the motion device coordinate system / world coordinate system, respectively. When the decision-making behavior is obstacle avoidance behavior with dense obstacles, the point cloud information is perceived and the obstacle center is fitted with a Gaussian model, and the decision-making motion speed is added to the obstacle avoidance planning algorithm corresponding to the obstacle. When the decision-making behavior is a priori navigation behavior, navigation route planning is performed based on prior information, and acceleration commands are calculated according to the following formula. : 。 7. A motion device, characterized in that, include: Environmental sensors for point cloud information acquisition, positioning sensors for self-positioning, and drive motor systems for driving motion devices; Memory and processor; The memory stores program instructions; The processor is communicatively connected to the environmental sensor, positioning sensor, drive motor system, and memory, and is used to run the program instructions to implement the motion control method as described in any one of claims 1 to 6.

8. A computer-readable storage medium, characterized in that, The device stores program instructions that are executed to perform the motion control method as described in any one of claims 1 to 6.

Images