A cross-modal robot motion control system and variable mode control method thereof

Abstract

The present invention discloses a cross-modal robot motion control system and a variable mode control method thereof. The control system includes three levels: a software scheduling layer, a hardware driver layer, and a hardware execution layer. The hardware driver layer includes hardware driver functions of all sensors and controller peripheral interfaces, and the software scheduling layer is responsible for the task scheduling of the control system; the hardware execution layer is composed of an embedded SOC, a peripheral circuit, a program debugging interface, a variety of communication ports, an AD sampling port, an onboard sensor, a buzzer, an LED indicator light, a PWM / GPIO multiplexing interface, a power management unit, and a power interface. The hardware driver layer of the control system is the basis of the software system, the software scheduling layer is the core of the software system, and all software algorithms run on the software scheduling layer. The present invention can realize the automatic conversion of various multi-modal robot motion modes, and the present invention has significant advantages in versatility, high efficiency, reliability, etc. in the motion control of cross-modal robots.

Description

Technical Field

[0001] The invention relates to a cross-modal robot motion control system and a variable-modal control method thereof, and belongs to the field of robots. Background Art

[0002] Robot technology is developing rapidly in the world today, and various robots of different modes have emerged in the military, civilian and industrial fields, including ground crawling robots, flying robots, underwater robots, ground mobile robots, etc. At present, the control systems of these robot products of different modes do not have a unified hardware and software architecture. For example, the drone control system designed by drone manufacturers cannot be applied to unmanned vehicles, and the unmanned vehicle control system designed by unmanned vehicle manufacturers cannot be applied to drones. The reason is that different robot control systems on the current market have large differences from hardware to software systems, so it is impossible to build robots of different modes based on a set of control systems, and it is also impossible to realize the cross-modal motion control of robots (cross-modal motion control refers to allowing a robot to switch between multiple motion modes, such as flying cars have both aerial flight mode and ground driving mode, and can be mutually converted between these two modes). The present invention believes that although the appearance structures of robots of different modes are different, they have something in common, such as all of them are realized by motors or steering gears, and all of them need to mount sensors to solve their own postures. Summary of the invention

[0003] In view of the analysis of the above background technology, the present invention aims to provide a universal cross-modal robot motion control system and its variable modal control method based on the similarities of different modal robots in hardware composition, state estimation, motion control, etc., so as to accelerate the development efficiency of robots with different modalities, reduce the development cost of robots, and realize automatic conversion between different motion modes of robots.

[0004] The present invention proposes a cross-modal robot motion control system (hereinafter referred to as "control system or controller or robot controller"). Figure 1As shown, the system architecture of the cross-modal robot motion control system of the present invention includes three levels: software scheduling layer, hardware driver layer, and hardware execution layer, wherein the software scheduling layer is responsible for the task scheduling of the entire robot control system, and the scheduled tasks include all state estimation, motion control functions, and robot variable mode control algorithms; the hardware driver layer includes hardware driver functions of all sensors and controller peripheral interfaces, and the hardware driver layer converts the calculation results of the software scheduling layer into hardware signals output by the hardware execution layer, or receives signals transmitted by the hardware execution layer, converts them into state variables and transmits them to the software scheduling layer for state estimation, motion control, and variable mode control; the hardware execution layer is the hardware part of the controller, which is composed of an embedded SOC (i.e., a main control chip), peripheral circuits, a program debugging interface, a variety of communication ports, an AD sampling port, an onboard sensor, a buzzer, an LED indicator light, a PWM / GPIO multiplexing interface, an SD card slot, a power management unit, and a power interface. The hardware structure block diagram of the controller is shown in the attached figure. Figure 2 The detailed hardware interface of the controller is shown in the attached Figure 3 ~Attached Figure 6 As shown in the embodiment.

[0005] The controller peripheral circuit described in the present invention is a circuit that connects various key components of the controller; the multiple communication ports include USB interface, I2C port, SPI port, PPM / SBUS port, serial port, and CAN port. The external devices that can be connected to the controller include various sensors (for example: optical flow module, laser ranging module, gnss navigation module), electronic computers (for example: Raspberry Pi, laptop computer), single-chip microcomputer, power driver (for example: electronic speed regulator) and other electronic devices external to the controller. The program debugging interface is used to burn the program to the main control chip. The power management unit converts the external power input into 3.3v level and 5v level for powering the internal chip of the controller and external devices.

[0006] As attached Figure 3 As shown, the Micro-USB interface 13 of the controller is used for USB communication with external devices. It is a bidirectional power supply socket. The external device can supply 5V power to the controller, and the controller can also supply power to the external device. The output voltage is 5V. At the same time, the main control chip 2 can receive voltage and current monitoring signals from the external power supply device. The controller further includes on-board sensors such as accelerometer, gyroscope, magnetometer, and barometer, which are located on the back of the controller. Figure 3 As shown, there are an accelerometer and a gyroscope 91, a magnetometer 92, and a barometer 93, which are used to measure position and attitude information.

[0007] As attached Figure 3As shown, the ADC port of the controller includes two pins, Sense and GND. The GND pin 80 is a ground wire, which is used to connect the ground wire of an external device; the Sense pin 82 is an ADC sampling input pin, which supports reading an external sampling voltage signal.

[0008] As attached Figure 3 As shown, the I2C port of the controller includes four pins: GND, SCL, SDA, and 5v. Among them, the GND pin 84 is the ground line, which is used to connect the ground line of the external device; the SCL pin 86 is the clock line of the I2C bus, corresponding to the clock line connected to the external device; the SDA pin 88 is the data line of the I2C bus, corresponding to the data line connected to the external device; the 5v pin 90 is the power supply output, which can power external devices with a rated voltage of 5v.

[0009] As attached Figure 3 As shown, the SPI port of the controller includes six pins: GND, MISO, MOSI, CLK, NSS, and 5v. Among them, GND pin 41 is the ground line, which is used to connect the ground line of the external device; MISO pin 43 is the Master In Slave Out of the SPI bus; MOSI pin 45 is the Master Out Slave In of the SPI bus; CLK pin 47 is the clock line of the SPI bus; NSS pin 49 is the SPI chip select line, which is connected to the CS pin of the SPI peripheral; 5v pin 51 is the power output pin, which can power external devices with a rated voltage of 5v.

[0010] As attached Figure 3 As shown, the controller's PPM / SBUS remote control input port includes three pins: GND, 5v, and Rcin. Among them, GND pin 85 is the ground wire, which is used to connect the ground wire of the external device; 5v pin 87 is the power output, which can power the external device with a rated voltage of 5v; Rcin pin 89 is the remote control signal input pin, which supports PPM and SBus remote control signal input and is compatible with a variety of mainstream remote controls. The SBUS output port includes GND, 5v, and SBus pins, among which GND pin 79 is the ground wire, which is used to connect the ground wire of the external device; 5v pin 81 is the power output, which can power the external device with a rated voltage of 5v; SBus pin 83 is the external output pin, which can output SBus standard signals to external devices.

[0011] As attached Figure 3As shown, the power input port of the controller includes six pins: VIN, VIN, GND, GND, Vsen, and Csen. Among them, VIN pin 48 and pin 50 are power inputs, connected to the positive pole of the external power supply; GND pin 40 and pin 42 are ground wires, connected to the negative pole of the external power supply; Vsen pin 44 is the voltmeter input pin of the external power supply, used to measure the power supply voltage; Csen pin 46 is the ammeter input pin of the external power supply, used to measure the power supply current.

[0012] As attached Figure 3 As shown, the controller embodiment includes 4 groups of serial ports, each of which includes five pins: 5v, Tx, Rx, Gnd, and 3.3v. 5v pins 14, 15, 53, and 54 are power outputs, which can supply power to external devices with a rated voltage of 5v; GND pins 20, 21, 59, and 60 are ground wires, which are used to connect the ground wires of external devices; 3.3v pins 22, 23, 61, and 62 are power outputs, which can supply power to external devices with a rated voltage of 3.3v; Tx is the serial port output, and the controller embodiment of the present invention includes four serial port output pins: TX1 pin 17, TX2 pin 16, TX3 pin 55, and TX4 pin 56; Rx is the serial port input, and the controller embodiment of the present invention includes four serial port input pins: RX1 pin 19, RX2 pin 18, RX3 pin 57, and RX4 pin 58.

[0013] The CAN port included in the controller embodiment is multiplexed with the serial port in hardware design, that is, the TX3 pin 55 of the controller is multiplexed as the can tx pin, and the RX3 pin 57 is multiplexed as the can rx pin. A CAN interface chip is further connected to convert the can tx and canrx signals into canH and canL differential signals, thereby realizing CAN communication between the controller and external devices.

[0014] As attached Figure 3 As shown, the motor / servo PWM port of the controller embodiment includes pin 25, pin 27, pin 29, pin 31, pin 33, pin 35, pin 63, pin 65, pin 67, pin 69, pin 71, and pin 73, which can output PWM signals or other TTL high and low level signals to the outside, and can also read external input TTL high and low level signals.

[0015] As attached Figure 3 As shown, the GPIO port of the controller embodiment includes pin 36, pin 37, pin 38, pin 39, pin 75, pin 76, pin 77, and pin 78, which can output TTL high and low level signals to the outside, and can also read external TTL high and low level signals.

[0016] As attached Figure 5 As shown, LED indicator lights 4, 5, 6, 7, 9, 10, 11, and 12, a total of 8 LED indicator lights, indicate various states of the robot controller by flashing in different colors; the buzzer 8 is used to emit a prompt sound to indicate various states of the robot; the SD card interface 52 is used to plug in an SD memory card to record various log data of the robot controller (such as Figure 3 or Figure 4 ). Reset button 1 is used to restart the controller.

[0017] As attached Figure 3 , 4 As shown in , 5, 6, 7, and 8, the cross-modal robot controller hardware of the present invention includes two mechanical configurations: straight pin headers and curved pin headers, corresponding to two different ways of connecting external devices. Figure 3 , Attachment Figure 5 , Attachment Figure 7 As shown in the figure, the pins of the straight pin header are led out from the back of the controller. Figure 4 , Attachment Figure 6 , Attachment Figure 8 As shown in the figure, the pins of the bent pin header are led out from both sides of the controller. Figure 7 , Attachment Figure 8 As shown, in order to improve the reliability of the product, the present invention designs a shell for the hardware of these two configurations to protect the circuit board. In the actual application process, these two different mechanical configurations can be flexibly selected according to different needs to achieve the best plug-in method.

[0018] As attached Figure 4 , Attachment Figure 6 , Attachment Figure 8 As shown in the figure, the controller with bent pin header configuration leads all interfaces on both sides of the controller. This configuration is easy to connect external devices through DuPont wires on both sides of the controller in practical applications, which makes the plugging and unplugging process of connecting external devices more convenient. It is a universal version with a wide range of uses. In testing, learning, and scientific research scenarios, the bent pin header configuration is more suitable for scenarios where external electronic devices are frequently plugged and unplugged.

[0019] As attached Figure 3 , Attachment Figure 5 , Attachment Figure 7 As shown, the controller with straight pin configuration brings out all the interfaces on the back of the controller. In actual application, it can be directly plugged into the female socket of the product to achieve plug-and-play. This connection method is more secure, easy to install and does not require complicated wiring. It can be plugged into the product base plate to form a tight whole, so it is suitable for mass-produced robot products.

[0020] In the controller system architecture, the hardware driver layer is the foundation of the software system, and the software scheduling layer is the core of the software system, because all software algorithms run in the software scheduling layer. Fig. 9 The software scheduling layer includes three core algorithms: state estimation, motion control, and mode conversion. These three core algorithms together constitute the variable mode control method described in the present invention.

[0021] The state estimation algorithm described in the attached Fig. 9 As shown, it includes four steps:

[0022] (1) Multi-source sensor data acquisition;

[0023] (2) Multi-source sensor data fusion and correction;

[0024] (3) Attitude solution;

[0025] (4) Position and velocity estimation;

[0026] The above four steps estimate the robot's position, speed, acceleration, attitude angle, and angular velocity by fusing data from multiple sensors. The position, speed, acceleration, attitude angle, and angular velocity are all three-dimensional vectors.

[0027] Since a single sensor is affected by the working environment, noise, its own defects, etc., its measured data often has certain errors. Therefore, the sensor data needs to be corrected and fused after obtaining the original sensor data. The correction method is to compensate the sensor's own offset through bias compensation and filter out the noise through a digital filter. For scenarios where multiple sensors measure the same data, a logic-based fusion method can be used. Since each sensor has different characteristics, there will be different errors in different scenarios. Screening out the most accurate sensor measurement values ​​at different times based on each sensor's own characteristics will significantly improve the accuracy of the robot's state estimation results.

[0028] The attitude calculation method is to collect the data of the accelerometer, gyroscope and magnetometer on the robot controller, and calculate the rotation of the body coordinate system to the earth coordinate system in real time based on the extended Kalman filter (EKF) algorithm. The rotation adopts quaternion Indicates that because

[0029]

[0030] The rotation matrix R can be obtained. Assume that the rotation matrix Nutation angle Rotation angle Precession Angle The Euler angle can be solved. The Euler angle can be used for subsequent closed-loop control while describing the controller's posture.

[0031] The position and speed estimation method adopts a fusion algorithm based on an extended Kalman filter (EKF). The fusion algorithm based on an extended Kalman filter (EKF) is divided into two processes, namely a prediction process and an update process. The prediction process obtains the predicted speed and position by integrating the acceleration, and the update process corrects the predicted results by using position and speed sensors.

[0032] The mean and variance of the forecasting process are given by:

[0033]

[0034]

[0035] Can be obtained.

[0036] The mean μ of the update process t and variance∑ t It can be obtained by the following algorithm:

[0037]

[0038]

[0039]

[0040] By periodically iterating the above prediction process and update process, the accurate position and speed can be solved in real time.

[0041] Based on the control architecture of the present invention, no matter what form or mode the robot is in, the movement of the robot can be defined as the whole or a part of the robot moving a specific distance along a specific direction with a specific acceleration or a specific speed, and the whole or a part of the robot rotating a specific angle along a specific direction with a specific angular velocity. Obviously, based on this definition of robot movement, the basis of robot motion control is to correctly estimate the position, speed, acceleration, attitude angle and angular velocity of the whole or a part of the robot, and the controlled object is the whole or a part of the robot. The robot controller described in the present invention can estimate its own position, speed, acceleration, attitude angle and angular velocity in real time, so as long as the motion control system described in the present invention is fixedly connected to the controlled object, the position, speed, acceleration, attitude angle and angular velocity of the controlled object can be estimated in real time.

[0042] Among them, the motion control algorithm, as shown in the attached Fig. 9 As shown, it includes three steps:

[0043] (1) Position control;

[0044] (2) Posture control;

[0045] (3) Control of power actuators such as motors, steering gears, and servo motors;

[0046] The motion control algorithm is based on the position, speed, acceleration, attitude angle and angular velocity of the robot estimated by the state estimation algorithm, and performs closed-loop control on the position, speed, acceleration, attitude angle and angular velocity of the robot. Fig. 9 The position control link includes the control of the position, speed, and acceleration of the robot, and the attitude control link includes the control of the attitude angle and angular velocity of the robot. Based on the control architecture of the present invention, no matter what form and mode the controlled robot is in, each of the five control links of position, speed, acceleration, attitude angle, and angular velocity can be described as closed-loop feedback control. The control signals output by these five control links are ultimately output to power actuators such as motors, steering gears, and servo motors, thereby generating power output to cause changes in position, speed, acceleration, attitude angle, and angular velocity.

[0047] Finally, the modal conversion method is described as shown in the appendix. Fig.10 As shown. The robot needs to receive the variable mode remote control signal sent by the operator. The variable mode remote control signal is generated by, but not limited to, remote control, mobile phone APP, and computer software. The present invention divides the robot mode into three types: main mode, sub-mode, and sub-mode.

[0048] In general, the main mode only needs to be obtained once when the system is started, and then it will not change during the entire movement of the robot, because the main mode is only related to the mechanical configuration and power layout of the robot. When a robot is manufactured, its mechanical configuration and power layout are already determined. No matter which mode is changed during its movement, its mechanical configuration and power layout will not change. Therefore, the main mode is used to distinguish different types of robots. Robots with similar mechanical configurations and power layouts can be classified as the same main mode. For example, multiple main modes can be defined based on different mechanical configurations and power layouts: multi-rotor drones, fixed-wing drones, flapping-wing drones, four-wheeled unmanned vehicles, two-wheeled balancing vehicles, spider robots, humanoid robots, surface unmanned ships, underwater unmanned submarines, etc., and an infinite number of main modes can be defined.

[0049] Sub-modes and sub-modes need to be acquired in real time during the robot's motion process, and are used to control the modal transformation during the robot's motion process. Sub-modes belong to the main mode and are motion modes that the robot can maintain for a long time, while sub-modes belong to sub-modes, indicating different motion states of the robot. For example, for the main mode of a multi-rotor drone, a variety of sub-modes can be designed for its different work needs, including: fixed-altitude flight mode, fixed-point flight mode, autonomous flight mode, etc. Among them, the fixed-altitude flight mode means that the drone maintains a constant relative height to the ground during flight; the fixed-point flight mode means that the drone can obtain its own positioning in real time based on the positioning sensor during flight, and fly according to the path points specified by the operator; the autonomous flight mode means that the drone can autonomously plan the flight route and fly along the route it has planned without the operator giving instructions. For example, for the main mode of a four-wheeled unmanned vehicle, a variety of sub-modes can be designed for its different control needs, including: acceleration control mode, speed control mode, position control mode, etc. The acceleration control mode refers to the operator controlling the acceleration of the unmanned vehicle with the remote control; the speed control mode refers to the operator controlling the speed of the unmanned vehicle with the remote control; and the position control mode refers to the operator controlling the position of the unmanned vehicle with the remote control. It can be seen that the sub-mode refers to a motion mode of the robot that can be maintained for a long time. The same robot can switch to different sub-modes at any time according to work requirements, thereby producing different motion effects.

[0050] However, sub-modes are obviously too general for the motion control of robots. The robot cannot clearly execute a specific motion process based on a sub-mode. Therefore, the present invention proposes the concept of sub-modes under sub-modes. Sub-modes are further refinements of sub-modes, which are used to represent different motion states of the robot in a sub-mode. The robot can determine a specific motion process it should execute based on sub-modes. For example, for the sub-mode of multi-rotor drone fixed-altitude flight, its sub-modes can be further refined to include: locking state, take-off state, landing state, and flight state. Among them, the locking state means that the robot will cut off the output of all power systems and remain stationary; the take-off state means that the robot executes the take-off program to rise from the ground to a set take-off height; the landing state means that the robot is not in flight, but the power output is not cut off. At this time, although the robot stays on the ground, the propeller is running at a low speed, and the take-off command can be executed at any time to fly from the ground to the air; the flight state means that the robot is already in a stable flight stage in the air. For another example, for the acceleration control mode of a four-wheeled unmanned vehicle, its sub-modes can be refined according to its different motion states, including: locked state and driving state. Among them, the locked state means that the robot will cut off the output of all power systems and remain stationary; in the driving mode, the robot will drive at the corresponding acceleration according to the operator's remote control signal. From this, it can be seen that the different sub-modes of the robot correspond to a specific motion state, and the robot has only one specific control rate in a certain sub-mode, so it only needs to cyclically execute a specific motion control algorithm to maintain the current motion state for a long time. Therefore, the sub-mode is the key to the robot achieving a specific motion state.

[0051] The robot main modes described in the present invention include but are not limited to: multi-rotor drones, fixed-wing drones, flapping-wing drones, unmanned vehicles, balancing vehicles, spider robots, humanoid robots, surface unmanned ships, underwater unmanned submarines, tracked robots, robot dogs, snake-like robots, robotic arms, and combinations or variants thereof. Combination means that any two or more of the above common robot configurations can be combined together to form a new type of robot main mode, for example, a flying car is a combination of a drone and an unmanned vehicle; variant means that a certain robot main mode can have multiple similar configurations, for example, unmanned vehicles can be divided into Mecanum wheel unmanned vehicles, ordinary rubber wheel unmanned vehicles, etc. based on the number and type of wheels.

[0052] The robot sub-modes described in the present invention include but are not limited to: posture control mode, acceleration control mode, speed control mode, position control mode, autonomous control mode, target following mode and other derived modes. Among them, the posture control mode means that the operator issues a control instruction to directly set the robot's target posture; the acceleration control mode means that the operator issues a control instruction to directly set the robot's target acceleration; the speed control mode means that the operator issues a control instruction to directly set the robot's target speed; the position control mode means that the operator issues a control instruction to directly set the robot's target position; the autonomous control mode means that the robot fully autonomously plans the target and generates movement without the operator issuing a control instruction; the target following mode means that the operator gives a dynamically changing target, and the robot can move following the dynamic target. The derived mode described in the present invention refers to a combination or variant based on the posture control mode, acceleration control mode, speed control mode, position control mode, autonomous control mode, and target following mode. For example, the altitude-fixed mode of a multi-rotor UAV is a variant of the target-following mode, which is the UAV following a high-altitude target; the operator's simultaneous control of the unmanned vehicle's forward movement and turning is a combination of the attitude control mode and the acceleration control mode.

[0053] The robot sub-modalities described in the present invention include but are not limited to: locked state, air flight state, crawling state, ground driving state, navigation state, and their derivative or transition states. The locked state means that the robot will cut off the output of all power systems and remain stationary; the air flight state means that the robot is running the air flight control rate at this time and is in a stable air flight state; the crawling state has multiple derivative states, for example, wall crawling, ground crawling, and roof crawling are all different derivative states based on the crawling state; the ground driving state is mainly the ground motion state for unmanned vehicles; the navigation state is mainly the water motion state for unmanned ships or unmanned submarines; the transition state means that there may be different control rates between any two different motion states of the above-mentioned robots. If the robot cannot achieve immediate conversion between the two control rates, there must be a corresponding transition state for converting the control rate. For example, the air flight state and the locked state of the drone cannot be converted immediately, so the two transition states of the take-off state and the landing state can be set.

[0054] All sub-modes of the robot include the locked state. When the robot is in the locked state, its main mode and sub-mode can be switched freely. After switching, the robot will automatically enter the locked state under the new sub-mode.

[0055] The following is combined with Fig.10 Explain the switching method of the robot's main mode, sub-mode and sub-mode:

[0056] (1) The robot's main mode can only be switched when the sub-mode is locked;

[0057] (2) The robot's sub-mode must meet certain conditions when switching. When representing the robot's sub-mode, the sub-mode collection must be instantiated into two objects: the expected sub-mode and the actual sub-mode. The difference between the expected sub-mode and the actual sub-mode is that the robot's current sub-mode is the actual sub-mode, while the remote controller hopes that the robot will switch to the expected sub-mode. The remote controller can set the robot's expected sub-mode in real time but cannot set the robot's actual sub-mode. When the robot's actual sub-mode is the same as the expected sub-mode, the robot considers that there is no need to change modes. At this time, the robot only needs to run in the current actual sub-mode. When the robot's actual sub-mode is different from the expected sub-mode, the robot will run a set of mode change programs. The modal change program is divided into two steps: the first step is to preset the modal change conditions in the modal change program. Only when all the set modal change conditions are met, the actual sub-mode of the robot can be changed to the expected sub-mode. For example, for a flying car, when its actual sub-mode is the air flight mode and the expected sub-mode is the ground driving mode, its modal change condition should be set to whether the landing is completed. If the landing is completed, the actual sub-mode of the robot is allowed to be converted from the air flight mode to the ground driving mode, otherwise the actual sub-mode should be maintained in the air flight mode; the second step is to set the modal change flag in the modal change program. The state flag is used to guide the robot to run the corresponding sub-modal program, in which the robot's specific variable modal motion control program is run. For example, for a flying car, its actual sub-mode is the air flight mode, and the expected sub-mode is the ground driving mode. In order to complete the robot's modal conversion process, an automatic landing flag should be set in the variable modal program. This automatic landing flag can guide the robot to run a sub-modal program with the function of automatic landing. This sub-modal program can control the robot to land from the air to the ground, so that the robot meets the mode switching conditions and finally completes the mode conversion. In particular, as long as the robot's sub-mode is in a locked state, the robot's actual sub-mode can be immediately switched to any expected sub-mode.

[0058] (3) The robot's sub-mode is automatically assigned to the robot's mode by the robot system through a pre-designed mode allocation mechanism. The mode allocation mechanism will receive the remote controller's command information, but the robot's sub-mode is not directly set by the remote controller's command. Furthermore, in this mode allocation mechanism, the robot system will combine the robot's current state information and the remote controller's command information to assign the most appropriate sub-mode to the robot.

[0059] The modal allocation mechanism is a probabilistically complete judgment mechanism that covers all possible situations. When certain specific conditions are met, the system will automatically assign a sub-mode to the robot, and when these specific conditions are not met, the robot will automatically be assigned other sub-modes. Therefore, this mechanism is probabilistically complete and covers all possible motion situations of the robot. Therefore, for a sub-mode containing N sub-modal sets S = {s1, s2, s3, ..., s N},due: Where P(s i ) indicates that the robot has a sub-mode of s i probability.

[0060] For example, the main mode of flying car includes two sub-modes: fixed-altitude flight in the air and ground driving. The sub-mode set included in the fixed-altitude flight sub-mode is S1 = {lock mode, landing mode, flight mode, take-off mode}, and the sub-mode set included in the ground driving sub-mode is S2 = {lock mode, driving mode}. When the robot is in the fixed-altitude flight sub-mode, its sub-mode allocation mechanism should be designed as follows:

[0061] Step 1: First, set three variables {lock flag, take-off flag, landing flag}. The values ​​of these three variables are only 0 or 1, and are set by the remote operator through remote control commands.

[0062] Step 2, get the lock flag value, if the lock flag is 1, assign the robot to the lock state sub-mode and end the mode assignment mechanism, when the lock flag is not 1, go to step 3. As mentioned above, in the mode assignment mechanism, the most appropriate sub-mode should be assigned in combination with the current state information of the robot and the command information of the remote controller, rather than directly setting the sub-mode by the remote operator's command, for example: the operator issues an unlock command to set the lock flag from 1 to 0, but the robot controller detects that the barometer data is abnormal, so it refuses to set the flag to 0, then the lock flag will continue to remain at 1, and the robot will remain in the locked state.

[0063] Step 3, obtain the take-off flag value. If the take-off flag is 1, assign the take-off state sub-mode to the robot and end the mode assignment mechanism. If the take-off flag is not 1, proceed to step 4.

[0064] Step 4: Get the value of the landing flag. If the landing flag is 1, assign the robot to the landing state mode and end the mode assignment mechanism. If the landing flag is not 1, go to step 5.

[0065] Step 5: Assign the flight state sub-mode to the robot. After this step is completed, return to step 2 to execute the next cycle.

[0066] Similarly, when the robot is in the ground driving sub-mode, its sub-mode allocation mechanism should be designed as follows:

[0067] Step 1: First, set a variable: the lock flag. The value of this variable is only 0 or 1, and is set by the remote operator through remote control commands.

[0068] Step 2: Get the lock flag value. If the lock flag is 1, assign the robot to the lock state mode and end the mode assignment mechanism. If the lock flag is not 1, go to step 3.

[0069] Step 3: Assign the driving mode to the robot. After this step is completed, return to step 2 to execute the next cycle.

[0070] It can be seen from this that the final sub-mode allocation mechanism will definitely assign a sub-mode to the robot and no sub-mode will be left vacant.

[0071] The specific program of the sub-modal allocation mechanism in the actual project needs to be developed by the developer in accordance with the method framework given in the present invention in combination with the specific robot configuration and function. The embodiments given in the present invention should not be regarded as a restriction on the scope of protection of the present invention.

[0072] The following describes the control flow of mode conversion:

[0073] (1) Step 1: When the control system is started, it first enters the default main mode, sub-mode, and sub-mode, and remains in a locked state;

[0074] (2) Step 2: The control system obtains the main mode setting signal sent by the remote control end. If the robot sub-mode is in a locked state, it switches from the current main mode to the main mode set by the remote control end and automatically enters the default sub-mode under the main mode. Then, the mode allocation mechanism automatically allocates a sub-mode to the robot. Generally, the sub-modes allocated when the system starts are all in a locked state.

[0075] (3) Step three, the control system obtains the sub-mode setting signal sent by the remote control end, and the control system stores the newly received sub-mode in the expected sub-mode. At this time, the actual sub-mode is different from the expected sub-mode, and the current motion state of the robot is only determined by the actual sub-mode. Once the expected sub-mode is different from the actual sub-mode, the control system immediately enters the modal change program, which sets some modal conversion flags. When the modal allocation mechanism recognizes these flags, it will assign a suitable sub-mode to the robot, so that the robot performs a series of motion processes, thereby transforming the robot's motion state into a state where the sub-mode can be converted. Then the modal change program sets the actual sub-mode to the expected sub-mode, thereby completing the modal conversion. In particular, when the robot is in a locked state, the modal change program will directly set the actual sub-mode to the expected sub-mode;

[0076] (4) Step 4: The control system obtains the sub-modal command information sent by the remote control end in real time, and assigns the most appropriate sub-modal to the robot through the modal assignment mechanism;

[0077] (5) Step 5: The robot runs the corresponding motion control program according to the sub-mode to generate the corresponding motion state. After this step, it returns to step 2 and executes the loop.

[0078] Based on the technical solution described in the present invention, the automatic conversion of motion modes of various multimodal robots can be realized. The technical solution described in the present invention has significant advantages in versatility, high efficiency, reliability, etc. in the motion control of cross-modal robots. The objectives and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the specification and the drawings.

[0079] Therefore, the present invention aims to design a cross-modal robot motion control system, so as to break through the barriers of different modal robot motion control, realize cross-modal motion control of the robot, speed up the development efficiency of the robot, and reduce the development cost of the robot.

[0080] The innovation of the present invention lies in that, based on the cross-modal robot motion control system described in the present invention, robot products of different modes can be efficiently constructed, and based on the robot variable modality method described in the present invention, automatic conversion between different motion modes of the robot can be achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0082] Figure 1 This is a system architecture block diagram of the controller described in the present invention.

[0083] Figure 2The hardware structure block diagram of the controller described in the present invention.

[0084] Figure 3 This is a schematic diagram of the back side of the controller described in the present invention (straight pin header).

[0085] Figure 4 This is a schematic diagram of the back side of the controller described in the present invention (bent pin header).

[0086] Figure 5 This is a front view of the controller of the present invention (straight pin header).

[0087] Figure 6 This is a front view of the controller described in the present invention (bent pin header).

[0088] Figure 7 This is a schematic diagram of the controller described in the present invention (straight pin header with housing).

[0089] Figure 8 This is a schematic diagram of the controller described in the present invention (bent pin header with housing).

[0090] Fig. 9 It is a software scheduling block diagram of the variable mode control method described in the present invention.

[0091] Fig.10 A schematic block diagram of the modal conversion process of the present invention.

[0092] Fig.11 This is a schematic block diagram of the controller of the present invention being powered and communicated via USB.

[0093] Fig.12 This is a schematic block diagram of the controller of the present invention being powered via a pin.

[0094] Fig.13 This is a schematic block diagram of the controller of the present invention measuring the output voltage of a power supply.

[0095] Fig.14 This is a schematic block diagram of the controller of the present invention measuring the output current of a power supply.

[0096] Fig.15 This is a schematic block diagram of the controller of the present invention supplying power to an external device.

[0097] Fig.16 This is a schematic block diagram of the controller of the present invention being connected to a Wi-Fi module via a serial port.

[0098] Fig.17 This is a schematic block diagram of the controller of the present invention being connected to an optical flow module via a serial port.

[0099] Fig.18 This is a schematic block diagram of the controller of the present invention being connected to an LCD module via an I2C bus.

[0100] Fig.19 This is a schematic block diagram of the controller of the present invention being connected to a UWB module via an SPI bus.

[0101] Fig. 20 It is a schematic block diagram of the connection between the controller and the Sbus / PPM signal receiving module of the present invention.

[0102] Fig.21 This is a schematic block diagram of the controller of the present invention controlling a motor through an electric regulator.

[0103] Fig. 22 This is a schematic block diagram of the controller of the present invention connected to a temperature sensor via an ADC.

[0104] Fig.23 This is a schematic block diagram of the controller of the present invention connecting to an LED via GPIO.

[0105] Fig.24 This is a schematic block diagram of the controller of the present invention being connected to an encoder motor via GPIO.

[0106] Fig.25 This is a schematic block diagram of the controller of the present invention controlling the servo motor via the CAN bus.

[0107] Fig.26 This is a schematic diagram of the overall structure of the controller controlling the four-wheeled vehicle according to the present invention.

[0108] Fig. 27 The present invention is a schematic block diagram of the controller connected to the camera module via a serial port.

[0109] Fig.28 This is a hardware connection block diagram of the controller controlling the four-wheeled vehicle described in the present invention.

[0110] Fig.29 The figure is a schematic diagram of the overall configuration of the controller controlling the quad-rotor drone according to the present invention.

[0111] Fig.30 This is a hardware connection block diagram of the controller of the present invention controlling a quad-rotor drone.

[0112] Fig.31 This is a schematic diagram of the overall structure of a multi-legged robot controlled by the controller of the present invention.

[0113] Fig.32 The figure is a schematic block diagram of the controller controlling the steering gear according to the present invention.

[0114] Fig.33 This is a schematic diagram of the overall structure of the controller controlling the multimodal flying robot according to the present invention.

[0115] Fig.34 This is a block diagram of the closed-loop feedback algorithm based on PID regulation described in the present invention.

[0116] The meanings of the serial numbers, symbols and codes in the figure are as follows:

[0117] 1—Reset button; 2—Master chip; 3—Positioning screw hole; 4—LED indicator; 5—LED indicator; 6—LED indicator; 7—LED indicator; 8—Buzzer; 9—LED indicator; 10—LED indicator; 11—LED indicator; 12—LED indicator; 13—Micro-USB interface; 14—5v pin; 15—5v pin; 16—Tx2 pin; 17—Tx1 pin; 18—Rx2 pin; 19—Rx1 pin; 20—Gnd pin; 21—Gnd pin; 22—3.3v pin; 23—3.3v pin; 24—Gnd pin; 25—Mot or5 pin; 26—Gnd pin; 27—Motor6 pin; 28—Gnd pin; 29—Motor7 pin; 30—Gnd pin; 31—Motor8 pin; 32—Gnd pin; 33—Servo3 pin; 34—Gnd pin; 35—Servo4 pin; 36—Gpio6 pin; 37—Gpio5 pin; 38—Gpio8 pin; 39—Gpio7 pin; 40—Gnd pin; 41—Gnd pin; 42—Gnd pin; 43—MISO pin; 44—Vsen pin; 45—MOSI pin; 46—Csen pin; 47—Gnd pin; 48—Gnd pin; 49—Gnd pin; 50—Gnd pin; 51—Gnd pin; 52—Gnd pin; 53—MISO pin; 54—Vsen pin; 55—MOSI pin; 56—Csen pin; 57—Gnd pin; 58—Gpio6 pin; 59—Gpio5 pin; 60—Gnd pin; 61—Gnd pin; 62—Gnd pin; 63—MISO pin; 64—Vsen pin; 65—MOSI pin; 66—Csen pin; 67—Gnd pin; 68—Gnd pin; 69—Gnd pin; 70—Gnd pin; 71—Gnd pin; 72—Gnd pin; 73—MISO pin; 74—Vsen pin; 75—MOSI pin; 76—Csen pin; 77—Gnd pin; 78—Gnd pin; 79—Gnd pin; 80—Gnd pin; 81—Gnd pin; 82—Gnd pin; 83—MISO pin; 84—Vsen pin; 85—MOSI pin; 86—Csen pin; 8 7—CLK pin; 48—VIN pin; 49—Nss pin; 50—VIN pin; 51—5v pin; 52—SD card interface; 53—5v pin; 54—5v pin; 55—Tx3 pin; 56—Tx4 pin; 57—Rx3 pin; 58—Rx4 pin; 59—Gnd pin; 60—Gnd pin; 61—3.3v pin; 62—3.3v pin; 63—Motor1 pin; 64—Gnd pin; 65—Motor2 pin; 66—Gnd pin; 67—Motor3 pin; 68—Gnd pin; 69—Motor4 pin; 70—Gnd Pin; 71—Servo1 pin; 72—Gnd pin; 73—Servo2 pin; 74—Gnd pin; 75—Gpio1 pin; 76—Gpio2 pin; 77—Gpio3 pin; 78—Gpio4 pin 79—Gnd pin; 80—Gnd pin; 81—5v pin; 82—Sense pin; 83—SBus pin; 84—Gnd pin; 85—Gnd pin; 86—SCL pin; 87—5v pin; 88—SDA pin; 89—Rcin pin; 90—5v pin; 91—Accelerometer and gyroscope; 92—Magnetometer; 93—Barometer. DETAILED DESCRIPTION

[0118] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0119] Example 1

[0120] The embodiment configuration of the controller described in the present invention is as shown in the attached Figure 3 ~Attached Figure 6 As shown, including the attached Figure 2 All units in the controller architecture diagram shown: main control chip, peripheral circuit, program debugging interface, multiple communication ports, AD sampling port, onboard sensor, buzzer, LED indicator, PWM / GPIO multiplexing interface, SD card interface, power management unit and power interface. In this embodiment, the main control chip 2 of the controller described in the present invention adopts STM32 embedded SOC. Figure 3 ~Attached Figure 6 The illustrated controller embodiment provides a detailed description of each hardware interface.

[0121] The Micro-USB interface 13 of the controller embodiment described in the present invention is a bidirectional power supply socket, which can be powered by 5V from an external device to the controller; or the controller can be powered by an external device, and the output voltage is 5V. Furthermore, the controller can communicate with external devices via USB. At the same time, the main control chip 2 can receive voltage and current monitoring signals from an external power supply device. The external devices include various sensors (for example: optical flow module, laser ranging module, GNSS navigation module), electronic computers (for example: Raspberry Pi, laptop computer), single-chip microcomputers and other electronic devices external to the controller.

[0122] The controller embodiment of the present invention has two power supply modes, one is USB power supply, as shown in the attached Fig.11 As shown, connect the USB power cable directly to the Micro-USB port 13; another method is to use the pin power supply, as shown in the attached Fig.12 As shown, connect the positive pole of the external power supply to the power input port of the controller, VIN pin 48 or VIN pin 50, and connect the negative pole of the external power supply to the GND pin. The GND pins shown in the figure are 20, 21, 25, 27, 29, 31, 33, 35, 40, 41, 42, 59, 60, 63, 65, 67, 69, 71, 73, 79, 80, 84, and 85.

[0123] Vsen pin 44 is used to connect to the voltage meter input pin of an external power supply and can be used to measure the voltage output by the power supply. Fig.13 shown.

[0124] Csen pin 46 is used to connect to the ammeter input pin of the external power supply and can be used to measure the current output by the power supply. Fig.14 shown.

[0125] All 5V pins 14, 15, 51, 53, 54, 81, 87, 90 of the controller embodiment can provide a power supply with a rated voltage of 5V for the external device by connecting to the external device; all 3.3V pins 22, 23, 61, 62 of the controller can provide a power supply with a rated voltage of 3.3V for the external device by connecting to the external device, as shown in the attached Fig.15 shown.

[0126] like Fig.17 As shown, the Tx1 pin 17, Tx2 pin 16, Tx3 pin 55, and Tx4 pin 56 of the controller embodiment are serial port outputs, which are signal lines used to send data from the controller to an external receiving device. Tx transmits data by sending a TTL level signal, thereby transmitting binary data to an external receiving device. The Rx1 pin 19, Rx2 pin 18, Rx3 pin 57, and Rx4 pin 58 of the controller embodiment are serial port inputs, which are used to receive data from an external device. When Rx receives data, it obtains the binary data transmitted by the external device by reading the TTL level signal. During the serial communication process, the Tx pin and the Rx pin are usually used in pairs to achieve bidirectional transmission of data.

[0127] Take the controller embodiment connecting the Wi-Fi module for serial communication as an example, as shown in the attached Fig.16 As shown, the EN pin of the Wi-Fi module is connected to the GPIO port of the controller to control the switch of the Wi-Fi module, and the Reset pin is connected to the GPIO port of the controller to control the restart of the Wi-Fi module. When the EN pin receives a high-level signal, the Wi-Fi module will be in working state; when the EN pin receives a low-level signal, the Wi-Fi module will enter a sleep state. When the Reset pin receives a low-level signal, the Wi-Fi module will restart. The 5v pin and Gnd pin of the controller power the WIFI module, the Tx pin of the controller is connected to the Rx pin of the WIFI module, and the Rx pin of the controller is connected to the Tx pin of the WIFI module, thereby realizing two-way communication between the controller and the WIFI module.

[0128] The controller embodiment includes pins for I2C communication, Gnd pin 84, SCL pin 86, SDA pin 88, and 5v pin 90. I2C is a two-wire bidirectional synchronous serial bus that uses a clock line and a data line to transfer information between two devices connected to the bus, providing a simple and efficient method for data exchange between devices. Usually, one master device and multiple slave devices can be mounted on an I2C bus. The I2C communication protocol requires the use of two signal lines, SCL and SDA, which are described as follows:

[0129] SCL pin 86: SCL is the clock line in I2C communication, which is used to control the timing and rate of data transmission. When the controller acts as a master device, the clock signal is sent by the SCL pin of the controller, and all slave devices share the same SCL line. When the controller acts as a slave device, the clock line is used to receive the clock signal sent by the external master device.

[0130] SDA pin 88: SDA is the data line in I2C communication, used to transmit data and control commands. All devices can send and receive SDA signals, so the SDA line is a bidirectional signal line. Each data bit on the SDA line is controlled by the clock pulse on the SCL line, and the data transmission rate depends on the frequency of the SCL line and the timing parameters in the protocol. Take the controller connected to the LCD display for I2C communication as an example, as shown in the attached figure. Fig.18 As shown, the controller can control the content displayed on the LCD display through the I2C interface.

[0131] The controller embodiment includes pins for SPI communication: Gnd pin 41, MISO pin 43, MOSI pin 45, CLK pin 47, NSS pin 49, and 5v pin 51. SPI communication is a full-duplex, synchronous, serial communication protocol. SPI communication requires the use of multiple signal lines, including CLK, MISO, MOSI, and NSS, which are specifically described as follows: CLK pin 47 is the clock line in SPI communication. When the controller is used as a master device, the clock signal is sent by the CLK pin of the controller to control the timing and rate of data transmission. MOSI pin 45 is the data line output by the master device and input by the slave device in SPI communication, and the master device sends data to the slave device. MISO pin 43 is the data line input by the master device and output by the slave device in SPI communication, and the slave device sends data to the master device. Generally, the controller is the master device, the external device is the slave device, and multiple slave devices can be mounted on a group of SPI buses controlled. NSS pin 49 is the chip select line for SPI communication, which is connected to the CS pin of the SPI peripheral and is used to select the slave device that needs to communicate. When multiple devices are connected via the same SPI bus, a different chip select line needs to be assigned to each device. In this case, the GPIO pins of the controller can be used. Take the controller connected to the UWB module for SPI communication as an example, as shown in the attached figure. Fig.19 As shown. The controller can realize the indoor positioning function through two-way communication with the UWB module. In particular, the controller can also be configured as a slave device to accept the control of an external master device.

[0132] The remote control signal input pins of the controller embodiment include: Gnd pin 85, 5v pin 87, and Rcin pin 89. The 5v pin and Gnd pin are used to power the remote control receiver. Rcin is the remote control signal input pin, which supports the input of PPM and SBus remote control signals and is compatible with a variety of mainstream remote controls. The PPM signal is a pulse position modulation-based signal, which is commonly used for communication between the remote control and the controller. The SBus signal is a serial communication protocol, which is commonly used for communication between the remote control and the controller. Taking the connection between the controller and the SBus / PPM remote control signal receiver as an example, the hardware connection is as shown in the attached figure. Fig. 20 shown.

[0133] The pins related to the Sbus signal output of the controller embodiment include the Gnd pin 79, the 5v pin 81, and the SBus pin 83. The 5v pin and the Gnd pin are used to power the external device, and the SBus pin is an external output pin that can output the SBus standard signal to the external device. Usually, this SBus signal output interface can be connected to a remote control, an electric speed controller, a camera pan / tilt, and other devices to achieve control of the external device.

[0134] The controller embodiment includes pins for controlling motors and servos: Motor1 pin 63, Motor2 pin 65, Motor3 pin 67, Motor4 pin 69, Motor5 pin 25, Motor6 pin 27, Motor7 pin 29, Motor8 pin 31, Servo1 pin 71, Servo2 pin 73, Servo3 pin 33, Servo4 pin 35. These pins can be used as normal GPIO pins, or they can output PWM (pulse width modulation) signals, which are periodic digital signals whose levels switch rapidly between high and low levels to control the operation of external devices. In this controller, by default, the PWM frequency output by the motor pin is 400Hz, and the default pin numbers are 64, 66, 68, 70, 24, 26, 28, 30; the PWM frequency output by the servo pin is 320Hz, and the default pin numbers are 72, 74, 32, 34. In actual use, the PWM signal frequency and pulse width output by the controller can be dynamically adjusted by calling functions, and the motor control pins and servo control pins can be common. Take the connection between the controller and the ESC (electronic speed regulator) module as an example, as shown in the attached figure. Fig.21 As shown, the controller outputs a PWM signal to the electric regulator, which is connected to the motor to control the speed of the motor.

[0135] The pins related to ADC in the controller embodiment include: Vsen pin 44, Csen pin 46, and Sense pin 82. Vsen pin 44 and Csen pin 46 are used to read the voltage and current sampling signals of the external power supply. Sense pin 82 is a universal ADC sampling input pin of the controller, which supports reading the voltage sampling signal of the external device. ADC is the abbreviation of analog-to-digital converter, which is an electronic device or module used to convert analog voltage values ​​into digital signals, and is usually used to quantify and measure analog signals, such as temperature, humidity, light intensity, etc. Take the connection between the controller and the temperature sensor as an example, as shown in the attached figure. Fig. 22 As shown, the controller supplies power to the temperature sensor through the 5V and Gnd pins, and uses the Sense pin 82 to receive the voltage analog signal fed back by the temperature sensor, and then quantifies it into a temperature measurement value.

[0136] The controller embodiment includes 8 GPIO ports, namely Gpio1 pin 75, Gpio2 pin 76, Gpio3 pin 77, Gpio4 pin 78, Gpio5 pin 37, Gpio6 pin 36, Gpio7 pin 39, and Gpio8 pin 38. GPIO port stands for general purpose input and output, and it includes multiple input and output modes. In the default state, it is configured as a push-pull pull-down output mode, but the GPIO port can also be configured to other modes according to specific needs, such as changing the GPIO input and output mode to open-drain output, interrupt input, analog input and other modes. The GPIO port can connect to a variety of peripherals, such as LED lights, buzzers, LCD screens, encoders, etc. Take the connection of LED lights as an example, as shown in the attached Fig.23 As shown, the controller controls the on and off of the external LED light by controlling the high and low levels of the GPIO port.

[0137] Take the controller embodiment and the encoder motor connection as an example, as shown in the attached Fig.24 As shown, it is necessary to set GPIO1 pin 75 and GPIO2 pin 76 to push-pull output mode to output high and low levels, where the DIR pin is used to control the forward and reverse rotation of the motor, and the EN pin is an enable pin used to control whether the motor is running. GPIO3 pin 78 and GPIO4 pin 77 are configured as interrupt input mode to receive the encoder motor AB phase sampling signal to measure the motor speed. Motor1 pin 63 is configured as PWM output mode to output PWM wave to control the motor speed.

[0138] The pins of the controller embodiment related to the CAN bus are TX3 pin 55 and RX3 pin 57. Fig.25 The TX3 pin 55 and RX3 pin 57 of the configuration controller are can tx and can rx pins, which are converted into canH and canL differential signals through the CAN interface module and then connected to the canH and canL pins of the servo motor to realize the speed and angle control of the servo motor.

[0139] Embodiment 2:

[0140] This embodiment mainly describes the specific control method of the present invention for a four-wheeled unmanned vehicle.

[0141] The overall configuration of the unmanned vehicle is as follows: Fig.26 As shown, the four-wheeled vehicle is driven by encoder motors on four wheels, and the encoder motors are driven by motor drive modules. The controller is connected to the SBus remote control signal receiver, as shown in the attached embodiment 1. Fig. 20 The controller is connected to the WIFI camera module via a serial port, as shown in the attached Fig. 27 As shown, the serial port is used to transmit remote control commands and status data.

[0142] The overall connection method of the controller, encoder motor, motor drive module, and WiFi camera module is shown in the attached figure. Fig.28 As shown. The external power supply is connected to the positive and negative poles of the motor drive module to provide power for the motor operation; the positive and negative poles of the motor are connected to the M+ pin and M- pin of the motor drive module respectively; the Motor port of the controller is configured as PWM mode and connected to the PWM pin of the motor drive module to control the motor speed; the Motor port of the controller is configured as GPIO mode and connected to the DIR pin of the motor drive module, and the motor is controlled by outputting high and low levels. The motor rotates forward when the level is high, and the motor reverses when the level is low; the Servo port of the controller is configured as GPIO mode and connected to the SLP pin of the motor drive module as the enable pin of the motor drive. The motor works when the output is high, and stops when the output is low. The positive and negative poles of the motor encoder are connected to the GPIO pin of the controller, and the GPIO pin of the controller is configured as interrupt input mode to receive the pulse signal of the encoder, and then measure the speed of the motor; the VCC pin of the encoder is connected to the 3.3V pin of the controller, and the GND pin is connected to the GND pin of the controller for encoder power supply. The four encoder motors of the unmanned vehicle are connected in the same way, and the working state of each encoder motor can be controlled by the controller separately.

[0143] Embodiment 3:

[0144] This embodiment mainly describes the specific control method of the present invention for a quad-rotor drone. Fig.29 Schematic diagram of the quadrotor unmanned aerial vehicle configuration.

[0145] The overall hardware connection method is as follows Fig.30 As shown. The controller controls the external motor through the ESC, the output terminal of the ESC is connected to the motor, the power supply terminal of the ESC is connected to the power supply, the PWM input pin of the ESC is connected to the Motor pin of the controller, and the Motor pin outputs a PWM signal to control the motor speed. The controller is connected to the SBus remote control signal receiver to receive remote control signals, and the controller is connected to the WiFi camera module through a serial port to transmit remote control commands and status data.

[0146] Embodiment 4:

[0147] This embodiment mainly describes the specific control method of the multi-legged robot of the present invention. Fig.31 shown.

[0148] The power mechanism of the multi-legged robot is mainly composed of a steering gear or a servo motor. The control method of the servo motor is consistent with the control method based on the CAN bus described in Example 1, as shown in the attached Fig.25The control method of the servo is consistent with the PWM-based control method described in Example 1. The specific connection method is in the attached manual. Fig.32 The controller is connected to the SBus signal receiver to receive the control signal, which is consistent with the control method described in Example 1. Fig. 20 shown.

[0149] Embodiment 5:

[0150] The variable mode control method described in the present invention divides the robot mode into three types: main mode, sub-mode, and sub-mode. The main mode includes but is not limited to: drone mode, unmanned vehicle mode, mechanical arm mode, multi-legged robot mode, and balance vehicle mode. The present invention proposes a method for realizing the conversion of a robot between different main modes, sub-modes, and sub-modes. The following is an explanation through specific embodiments.

[0151] In order to explain the variable mode control method in detail, in an embodiment, a multi-mode flying robot having a flight mode and a ground driving mode is constructed based on the robot controller of the present invention, as shown in the attached figure. Fig.33 As shown in the figure. This flying robot is a combination of a drone and an unmanned vehicle. It drives on the ground with wheels and flies in the air with quadrotors. In order to realize variable mode control, the main mode, sub-mode and sub-mode of the robot are first divided.

[0152] The main mode is only related to the configuration of the robot, so it can be named "flying car mode". The sub-mode is subordinate to the main mode, and can be divided into "air fixed altitude flight mode" and "ground driving mode" according to the working scene of the robot. The sub-mode is subordinate to the sub-mode, so for the sub-mode of "air fixed altitude flight mode", the sub-mode can be divided into: locking mode, landing mode, fixed altitude flight mode, take-off mode, and for the sub-mode of "ground driving mode", the sub-mode can be divided into: locking mode and driving mode. In this embodiment, the remote control terminal uses a mobile phone APP.

[0153] Next, check the attached Fig.10 The robot modal conversion process shown is explained in detail.

[0154] Step 1: When the robot is started, the robot controller is first connected to the mobile phone APP. At this time, the mobile phone APP must first set the robot's main mode to "flying car mode", and then the controller software system automatically loads the sub-mode under the main mode and enters one of the default sub-modes. For example, if the default sub-mode is "air fixed altitude flight mode", then the robot's current actual sub-mode and expected sub-mode will be in "air fixed altitude flight mode". At this time, the mode allocation mechanism detects that the robot has not been unlocked, so the robot is assigned the locked mode by default.

[0155] Step 2: At this point, the operator wants the robot to take off, and uses the button on the app to send an unlock signal to the robot controller. At this point, the mode allocation mechanism detects that it has been unlocked and assigns the robot to landing mode. In landing mode, the robot's propellers begin to rotate at a low speed, indicating that the robot is no longer in locked mode, but because the propeller speed is very low, the robot will not fly.

[0156] Step 3: The operator initiates a flight command on the APP. After receiving this signal, the mode allocation mechanism of the robot controller allocates the take-off mode to the robot. At this time, the drone will fly to the pre-set take-off height.

[0157] Step 4: After the robot reaches the pre-set target take-off altitude, the mode allocation mechanism automatically allocates the fixed-altitude flight mode to the robot, that is, it stays at the current flight altitude and waits for the operator to issue other flight tasks. In order to realize the flight motion of the robot, the control system performs closed-loop feedback control on the robot's position, speed, acceleration, attitude angle, and angular velocity based on accurate state estimation. In the embodiment, a closed-loop feedback algorithm based on PID regulation is used for motion control. Finally, the robot can accurately reach the target motion state. The closed-loop feedback algorithm is shown in the attached figure. Fig.34 As shown. Each box marked with PID in the block diagram is a PID controller, and its time domain formula is:

[0158]

[0159] Among them, K P , K I , K D is the parameter to be set, K P is the proportionality coefficient, K I is the integral coefficient, K D is the differential coefficient. e(t) and e(τ) are the inputs of the PID controller, i.e., the difference between the target value and the measured value. F(t) is the output value of the PID controller. Fig.19 , Attachment Fig. 20 As shown in the figure, for the cascade PID, the output value of the upper-level PID controller is the target value of the lower-level PID controller.

[0160] The output value of the last stage PID controller is the control amount of the power actuator. For example, for the motor, the control amount can be converted into the duty cycle value of the pulse width modulation signal (PWM). The drive circuit of the motor is controlled by the pulse width modulation signal. The final result is that a larger motor control amount corresponds to a larger duty cycle, and a larger duty cycle corresponds to a larger motor speed.

[0161] Step 5: The operator wants the robot's sub-mode to change to the ground driving mode. After sending the command on the APP, the robot controller receives the command and changes the expected sub-mode to the ground driving mode. The mode allocation mechanism finds that the actual sub-mode is different from the expected sub-mode at this time, but it cannot immediately change the actual sub-mode to the expected sub-mode, so it keeps the current sub-mode unchanged and starts running the mode change program at the same time. Since the robot is in the fixed-altitude flight mode, in the mode change program, the robot's landing mark is automatically set first. After the sub-mode allocation mechanism detects the landing mark, it allocates a landing speed to the robot, guides the robot back to the ground, and then automatically switches to the locked mode.

[0162] Step 6: At this point, the modal change program believes that the robot can complete the sub-modal conversion, so it converts the actual sub-modal into the expected sub-modal and automatically ends the modal change program.

[0163] Step 7: At this point, the robot sub-mode has been transformed into the ground driving mode, so the sub-mode allocation mechanism allocates the locking mode to the robot.

[0164] Step 8: At this time, the operator wants the robot to move on the ground, and sends an unlock command through the mobile phone APP. After the mode allocation mechanism of the controller detects the unlock signal, it automatically assigns the driving mode to the robot. At this time, the robot begins to receive the remote control command issued by the APP to realize the robot's ground movement.

Claims

1. A cross-modal robot variable mode control method, the method specifically comprising: State estimation, motion control, and mode conversion; the state estimation is to estimate the robot's position, speed, acceleration, attitude angle, and angular velocity by fusing sensor data, specifically including multi-source sensor data acquisition; multi-source sensor data fusion and correction; Attitude solution; position and speed estimation; the motion control is based on the position, speed, acceleration, attitude angle and angular velocity of the robot estimated by the state estimation algorithm, and the position, speed, acceleration, attitude angle and angular velocity of the robot are closed-loop controlled; the modal conversion divides the robot mode into three types: main mode, sub-mode and sub-mode; the main mode is used to distinguish different types of robots, and robots with similar mechanical configurations and power layouts are classified as the same main mode; the sub-mode and sub-mode are acquired in real time during the robot's movement process, and are used to control the modal transformation of the robot during the movement process. The sub-mode belongs to the main mode and is the motion mode maintained by the robot for a long time, while the sub-mode belongs to the sub-mode, indicating the different motion states of the robot in a certain sub-mode. The robot determines a specific motion process it should perform based on the sub-mode; The control flow of the modal conversion is as follows: Step 1: When the control system starts, it first enters the default main mode, sub-mode, and sub-mode, and remains in a locked state; Step 2: The control system obtains the main mode setting signal sent by the remote control end; if the robot sub-mode is in a locked state, it switches from the current main mode to the main mode set by the remote control end and automatically enters the default sub-mode under the main mode, and then the mode allocation mechanism automatically allocates a sub-mode to the robot; Step 3: The control system obtains the sub-mode setting signal sent by the remote control end, and the control system stores the newly received sub-mode in the expected sub-mode. If the actual sub-mode is different from the expected sub-mode at this time, the current motion state of the robot is only determined by the actual sub-mode; once the expected sub-mode is different from the actual sub-mode, the control system immediately enters the modal change program, which sets the modal conversion flag. When the modal allocation mechanism recognizes these flags, it will allocate a suitable sub-mode to the robot, so that the robot executes a series of motion processes, thereby converting the robot's motion state into the state of the converted sub-mode, and then the modal change program sets the actual sub-mode to the expected sub-mode, thereby completing the modal conversion; Step 4: The control system obtains the sub-modal command information sent by the remote control end in real time, and allocates the most suitable sub-modal to the robot through the modal allocation mechanism; Step 5: The robot runs the corresponding motion control program according to the sub-mode to generate the corresponding motion state; after this step, it returns to step 2 and executes in a loop.

2. The method according to claim 1, characterized in that: in, The attitude calculation method is to collect the data of the accelerometer, gyroscope and magnetometer on the control system, and calculate the rotation of the body coordinate system to the earth coordinate system in real time based on the fusion algorithm of the extended Kalman filter. The rotation adopts quaternion Indicates that because Obtain the rotation matrix R, assuming that the rotation matrix Nutation angle Rotation angle Precession Angle The Euler angles are solved; the Euler angles are used for subsequent closed-loop control while describing the controller's posture.

3. The method according to claim 1 or 2, characterized in that: The position and speed estimation method adopts a fusion algorithm based on an extended Kalman filter; the fusion algorithm based on an extended Kalman filter is divided into two processes, namely a prediction process and an update process. The prediction process obtains the predicted speed and position by integrating the acceleration, and the update process corrects the prediction results through position and speed sensors. Through the periodic iteration of the prediction process and the update process, the accurate position and speed are solved in real time.

4. The method according to claim 1, characterized in that: When representing the sub-mode of the robot, the collection of sub-modes should be instantiated into two objects: the expected sub-mode and the actual sub-mode. The difference between the expected sub-mode and the actual sub-mode is that the sub-mode currently running in the robot is the actual sub-mode, and the remote controller hopes that the robot will switch to the expected sub-mode. The remote controller sets the expected sub-mode of the robot in real time but cannot set the actual sub-mode of the robot. When the actual sub-mode of the robot is the same as the expected sub-mode, the robot considers that there is no need to change the mode, and the robot runs in the current actual sub-mode. When the actual sub-mode of the robot is different from the expected sub-mode, the remote controller sets the expected sub-mode of the robot in real time but cannot set the actual sub-mode of the robot. When the actual sub-mode of the robot is the same as the expected sub-mode, the robot considers that there is no need to change the mode, and the robot runs in the current actual sub-mode. When the desired sub-mode is different, the robot will run the variable mode program; the variable mode program is divided into two steps: the first step is to preset the variable mode conditions in the variable mode program, and only when all the set variable mode conditions are met, the actual sub-mode of the robot can be changed to the desired sub-mode; the second step is to set the variable mode flag in the variable mode program, and the variable mode flag is used to guide the robot to run the corresponding sub-modal program, in which the robot's specific variable mode motion control program is run; in particular, as long as the robot's sub-mode is in a locked state, the robot's actual sub-mode is immediately switched to any desired sub-mode.

5. The method according to claim 1, characterized in that: The sub-mode of the robot is automatically assigned to the robot's mode by the robot system through a pre-designed modal allocation mechanism. The modal allocation mechanism will receive the command information of the remote controller, but the final sub-mode of the robot is not directly set by the command issued by the remote controller; further, this modal allocation mechanism is probabilistically complete. The robot system will combine the robot's current state information and the remote controller's command information to assign the most appropriate sub-mode to the robot.

6. A control system for implementing the cross-modal robot variable mode control method according to claim 1, characterized in that: The system comprises: a software scheduling layer, a hardware driver layer and a hardware execution layer, wherein the software scheduling layer is used for task scheduling of the whole robot control system, wherein the scheduled tasks include all state estimation, motion control functions and robot variable mode control algorithms; the hardware driver layer includes hardware driver functions of all sensors and controller peripheral interfaces; the hardware driver layer converts the operation results of the software scheduling layer into hardware signals outputted by the hardware execution layer, or receives signals transmitted by the hardware execution layer, converts them into state variables and transmits them to the software scheduling layer for state estimation, motion control and variable mode control; the hardware execution layer, i.e. the hardware part of the controller, is composed of an embedded SOC, i.e. a main control chip, peripheral circuits, a program debugging interface, multiple communication ports, an AD sampling port, an onboard sensor, a buzzer, an LED indicator light, a PWM / GPIO multiplexing interface, an SD card socket, a power management unit and a power interface.

7. The control system according to claim 6, characterized in that: The control system further includes onboard sensors such as accelerometers, gyroscopes, magnetometers, and barometers for measuring position and attitude information.

8. The control system according to claim 6, characterized in that: The hardware part includes two mechanical configurations, a straight pin header and a curved pin header, corresponding to two different ways of connecting external devices; the pins of the straight pin header are led out from the back of the controller, and the pins of the curved pin header are led out from both sides of the controller.

9. The control system according to claim 6, characterized in that: The power interface receives voltage and current monitoring signals from an external power supply device; the power management unit converts the external power input into 3.3v level and 5v level for powering the internal chips of the controller and external devices; the multiple communication ports include USB interface, I2C port, SPI port, PPM / SBUS port, serial port, and CAN port; the PWM / GPIO multiplexing interface supports reading or outputting various TTL level signals.

Images