CN107167141A - Robot autonomous navigation system based on double line laser radars - Google Patents

Abstract

The present invention realizes a kind of brand-new robot autonomous navigation system based on double line laser radars.Compared to existing main flow Indoor Robot navigation system, the present invention only used two line laser radars of low cost one as information source, but reach navigation and the avoidance effect of the scheme for being substantially better than existing equal cost.Based on the robot autonomous navigation system of double line laser radars, including double line radar systems, upper strata navigation system, bottom control algorithm, Motor execution system；Upper strata navigation system includes SLAM algorithms, coordinate transformation algorithm；The two-dimensional map of the original laser radar data dynamic construction current spatial gathered using SLAM algorithms according to the radar being horizontally mounted, while calculating the displacement information of robot platform.

Description

Robot autonomous navigation system based on dual-line laser radar

technical field

The invention is an autonomous navigation solution applied to indoor robots, which belongs to the field of robots.

technical background

With the continuous development of computer technology and information processing technology, robot technology based on these technologies has also been widely used in various industries, including replacing manpower to complete tasks such as household services, patrols, and goods handling that are not complex but An indoor mobile robot that needs to work continuously in an indoor space.

Navigation technology is one of the core technologies of mobile robots. In a complex and changeable indoor environment, a stable and effective navigation scheme is the key to determine whether the robot can complete its work. For mobile robot platforms that work in open outdoor spaces and do not require high precision, GPS can usually be used for positioning and navigation, but for mobile robots that work indoors, the unstable and low-precision civilian GPS navigation system cannot provide Accurate enough information for the robot to move in a small enclosed space.

At present, common low-end indoor robots mostly use radar or infrared rays for obstacle detection, and realize autonomous movement through passive obstacle avoidance when encountering obstacles. Their movement trajectories are highly random and cannot be specified in advance Destination directed movement. Some high-end robot solutions mostly use first-line laser radar or stereo vision system for obstacle avoidance and navigation. Although these two solutions can achieve directional movement and active obstacle avoidance by obtaining more environmental information, they have certain limitations in practical applications. limitations. Although the solution of navigating through the first-line lidar can achieve path planning and obstacle avoidance by obtaining the two-dimensional outline of the environment, this solution can only scan and model the plane where the first-line radar is located. Obstacles on the scanning plane are useless. Although stereoscopic vision can be used to obtain three-dimensional environmental information, due to the limitation of the viewing angle of the camera, its scanning range cannot cover a sufficient spatial area, and this solution also has disadvantages such as a large amount of data to be processed, large noise interference, and short effective distance.

Contents of the invention

In order to overcome the shortcomings of the above-mentioned existing solutions, the present invention implements a brand-new robot autonomous navigation system based on dual-line laser radar. Compared with the existing mainstream indoor robot navigation system, the present invention only uses two low-cost first-line laser radars as information sources, but achieves navigation and obstacle avoidance effects that are significantly better than existing solutions with the same cost.

The technical scheme adopted in the present invention is:

The robot autonomous navigation system based on double-line laser radar is characterized in that it includes a double-line radar system, an upper-level navigation system, a bottom control algorithm, and a motion execution system; the upper-level navigation system includes a SLAM algorithm and a coordinate transformation algorithm; the SLAM algorithm is used according to The original laser radar data collected by the horizontally installed radar dynamically constructs a two-dimensional map of the current space, and at the same time solves the displacement information of the robot platform; Coordinate transformation and superposition of the acquired original lidar data, so as to determine the three-dimensional information of obstacles in the space around the robot from the collected environmental information; carry out global path planning according to the constructed two-dimensional map , and plan a current position and destination At the same time, the real-time speed of the robot is collected; in this way, local path planning is performed according to the collected speed information , global path , and three-dimensional data , and control instructions are produced, so as to realize the upper-level navigation of the robot; The control instructions generated by the upper navigation system are analyzed and coordinate transformed, and the motion execution system is controlled by PID control to drive the robot to move.

Compared with the existing system technology, the beneficial effects of this system are:

1) This system can be flexibly applied to various indoor scenarios and is easy to deploy.

2) The combination of dual-line lidar to obtain three-dimensional information can obtain sufficient environmental information with a small amount of data, improve the utilization rate of data, and reduce the cost of data processing.

3) The navigation and obstacle avoidance system is realized based on spatial three-dimensional information, which effectively improves the robot's autonomous movement ability and obstacle avoidance effect, and increases the practicability and reliability of the system.

Description of drawings

Figure 1 is a schematic diagram of the structure of a dual-line laser radar.

Figure 2 is a schematic diagram of the radar scanning range.

Figure 3 is a schematic diagram of the structure of the robot system.

Figure 4 is a schematic diagram of the robot navigation system.

Fig. 5 is a schematic structural diagram of the bottom control unit system.

Figure 6 The movement speed of the robot chassis and the movement speed of the three wheels.

detailed description

The technical solutions of the present invention will be further introduced below in conjunction with the accompanying drawings and embodiments.

The first part introduces the principle of the technical solution:

1. Use two first-line laser radars and peripheral control circuits to form a dual-line radar system to collect environmental data. As shown in Figure 1, the two lidars are installed in different ways, one of which is fixed horizontally to obtain the plane profile of the space where the robot is located. The other radar scans up and down under the control of the radar controller powered by the steering gear 1, and the radar controller sends the angle information between the current radar scanning plane and the horizontal plane to the data processing system.

2. Use the NVIDIA Jetson TX1 embedded platform as the core controller to build a robot hardware platform. The whole platform can be divided into four main parts according to the functions, which are data acquisition unit, data processing unit, motion execution system and auxiliary components. Among them, the data acquisition unit is the data source of the whole system, and the dual-line laser radar system specially designed for this project is used to collect environmental information. The collected data will be sent to the data processing unit, which is the core of the whole system. An NVIDIA Jetson TX1 embedded platform is used as the carrier of the robot operating system and various upper-level algorithms. The motion execution system includes the mechanical platform of the robot, the underlying controller and the drive circuit. In addition, the system also includes some auxiliary components such as routers, power supplies, etc.

3. Construct the upper-level navigation system of the robot. The navigation system uses the SLAM algorithm (the prior art) to dynamically construct a two-dimensional map of the current space based on the original lidar data collected by the horizontally installed radar, and at the same time, according to the feature matching solution in the process of building the map (by the components of the SLAM algorithm , belongs to the prior art) to obtain the displacement information of the robot platform.

The coordinate transformation algorithm in the navigation system performs coordinate transformation and superposition on the original lidar data acquired by the dynamic scanning radar at each time according to the robot displacement information obtained by the SLAM algorithm, so that the obstacles in the space around the robot are finally obtained from the collected environmental information three-dimensional information .

According to the two-dimensional map of construction, utilize ROS navigation function pack (for prior art, build on the ROS robot Ubuntu operating system) to carry out global path planning , plan out a feasible path between current position and destination; At the same time, utilize The mileage information processing unit collects the real-time speed of the robot, and then uses the ROS navigation function package to perform local path planning based on the collected speed information and the three-dimensional data generated by the global path and coordinate transformation , and produces control instructions , so as to realize the upper-level navigation of the robot system.

4. The underlying control algorithm analyzes and coordinates the control commands generated by the upper navigation system, and uses PID control to control the motor speed to drive the robot to move. The underlying control unit system structure is shown in Figure 5.

The second part is further illustrated with examples

1. Dual-line laser radar system

The present invention uses two one-line laser radars for data collection. Both radars can perform 360-degree scanning in the horizontal plane, so as to obtain obstacle distance information within a 360-degree range in the plane within one cycle.

One of the radars (radar 2) is installed at the bottom of the robot 10cm from the ground. The scanning plane of the radar 2 is placed horizontally. During the operation, the basic outline information of the indoor environment can be obtained, and the user can subsequently build a global map. Limited by the structure of the robot, in order to protect the radar from being damaged by collisions during the operation of the robot, the radar 2 is embedded inside the robot as a whole, and a space of about 150 degrees is left at the front end for scanning the external environment, and the data in the rest of the angle range are discarded . After testing, this installation method can fully meet the actual data collection needs.

Another lidar (radar 1) is mounted on the scanning radar controller on top of the robot. The entire scanning radar controller is composed of a mechanical device and a control circuit. The mechanical device includes a bracket for fixing the whole device, a base for installing the radar 1, a connecting rod for transmission and a steering gear 1 for providing power, and the whole device has a quadrilateral structure. When the acquisition system is in operation, the steering gear 1 moves up and down under the control of the control circuit (that is, the radar controller), thereby driving the radar 1 to scan the space around the rotation axis of the base.

The scanning range of the radar 1 at the top of the robot is shown in FIG. 2 .

Since the radar 1 is located on the top of the robot, its scanning range is mainly distributed below the plane where the top of the robot is located. As shown in Figure 2, in order to enable the robot to bypass obstacles smoothly, a certain safety distance is reserved around. From the geometric relationship, the scanning range θ ₁ of the radar 1 below the horizontal plane can be calculated by formula (1)

Where δ is the safety distance, and H is the height of the robot. When the safety distance is 20cm and the robot height is 120cm, the scanning angle below the horizontal plane can be calculated by formula (1) to be about 80.54°. In addition, a certain safe space is also reserved above the robot, and the scanning range above the horizontal plane is set to 10°, so the radar 1 will scan in a space of about 90° above and below the horizontal plane.

The dual-line radar system consists of a radar controller and a power module. The radar controller controls the angle of the steering gear 1 through PWM. The radar 1 scans within a 90° range with a certain step size under the control of the radar controller. At the same time The radar controller will send the angle between the current radar plane and the horizontal plane to the data processing system for coordinate transformation. The power module can output two voltages of 5V and 6V, 5V provides the normal working voltage for the radar controller, and the 6V output is used for the power supply of the steering gear 1.

2 robot hardware platform

The robot hardware platform adopted by the present invention is shown in FIG. 3 .

According to the functional platform, it can be divided into data acquisition, data processing, motion execution and auxiliary components. Among them, the data acquisition part is realized through the above-mentioned dual-line laser radar system, and the data processing part adopts the JetsonTX1 embedded platform of Nvidia Company, which is equipped with the Ubuntu operating system and the ROS robot operating system to realize the processing and processing of the laser radar data. Robot Autonomous Navigation Algorithms. The motion execution part is mainly composed of Arduino controller with drive circuit, motor and steering gear, and robot chassis. The robot chassis adopts the open source robot platform HCR driven by three wheels. Its power comes from three 12V DC geared motors. Each motor drives a wheel independently. The directional movement of the robot is realized through the combination of different speeds of the three wheels. The calculation process from the upper control command to the actual speed of the motor is completed by the control algorithm running on the Arduino. The peripheral auxiliary circuit includes components such as batteries, voltage stabilizing modules and routers. The power supply cooperates with the voltage stabilizing module to provide corresponding power for the system, and the router provides support for the communication between the upper computer and the robot.

3. Robot navigation system

3.1 Coordinate transformation algorithm

The data collected by the radar that scans the space is based on the distance information of the coordinate system fixed on the radar, and there is relative motion between the radar and the robot system platform, so it is necessary to obtain the three-dimensional information of obstacles in the robot coordinate system based on the radar data. Transform the coordinates of the data.

The raw data collected by the radar is represented as (ρ, φ, z) in the cylindrical coordinate system, and converted to rectangular coordinates according to formula (2)

If the forward direction of the robot system platform is the positive direction of the X-axis, the vertical upward direction is the positive direction of the Z-axis, and the center of the robot chassis is used as the origin to establish a coordinate system, then the radar data can be calculated by the following formula in the robot coordinate system

Among them, [xyz]' is the radar data transformed to the Cartesian coordinate system, [x'y'z']' is the radar data transformed to the robot coordinate system, is the angle between the current radar scanning plane and the horizontal plane sent back by the radar controller, and [x ₀ y ₀ z ₀ ]' is the coordinate of the radar in the robot coordinate system.

Considering that the robot is constantly moving while scanning, there is relative motion between the robot coordinate system and the space coordinate system. In order to obtain more accurate spatial information, it is necessary to further convert the radar data in the robot coordinate system to the space coordinate system. It is necessary to obtain the position of the robot in the space coordinate system. The system uses the fixedly installed laser radar 1 as a data source for obtaining the position information. Unlike radars used for scanning, this radar collects environmental information within the same horizontal plane.

The data processing algorithm first performs basic coordinate translation transformation on the data and converts it to the space coordinate system, and then uses the SLAM algorithm to perform feature matching on the data collected by the robot at different times, and then calculates the displacement information of the robot in space . By accumulating displacement information and correcting the accumulative result according to the moving speed of the robot measured by the encoder, the position in the space coordinate system can be obtained according to the initial position of the robot.

After obtaining the position of the robot in the space coordinate system, the radar data in the robot coordinate system can be translated to obtain the spatial information in the space coordinate system. After the robot completes a scan within a 90° range, all scanned points are superimposed after the above transformation, and finally the three-dimensional information of the space near the current robot is obtained.

3.2 SLAM algorithm

According to actual application occasions and performance requirements, the present invention adopts the Hector SLAM algorithm (prior art) as the algorithm for constructing maps. The algorithm uses the occupancy grid map (Occupancy Grid Map) as the model of map information, and uses the feature matching method based on bilinear interpolation and Gauss-Newton method to calculate the positional relationship between each data point, so as to construct a two-dimensional environmental map and Get robot displacement information.

3.3 Upper Navigation System

Robot navigation can be divided into three processes: environment modeling, path planning, and robot control. The environment modeling process includes the coordinate transformation and map construction described above, which finally generates the required 2D environment map and 3D point cloud containing obstacle information. Path planning is the decision-making part of the whole system. This process performs multi-level path planning and motion simulation according to the environment map and obstacle information as well as the motion state of the robot, so as to generate the most reasonable expected motion state under the current state and encapsulate it into Control commands are sent to the underlying controller. Robot control takes the underlying control algorithm as the core, and actually drives the robot platform to move according to the control instructions. The complete robot navigation system is shown in Figure 4.

3.4 Bottom Control Algorithm

The underlying control algorithm is developed based on the Arduino platform, which mainly includes command analysis, coordinate transformation, PID control, and data conversion. The underlying control algorithm structure is shown in Figure 5.

After the bottom control algorithm receives the control command generated by the upper navigation system through the serial port, it first analyzes it. The control command contains the expected moving speed and rotational angular velocity of the robot. This design uses a three-wheel drive mechanical platform, and the control command provides the speed of the entire robot platform, so it is necessary to convert the expected speed of the robot into the expected speed of the three wheels.

Take the center of the robot chassis as the origin, the forward direction of the robot is the positive direction of the x-axis, and the vertical upward direction is the positive direction of the z-axis to establish a coordinate system. Set the expected speed of the robot in the directions of the x-axis and y-axis and the vector formed by the rotational speed around the z-axis is [v _x v _y ω] ^T , and the vector formed by the speeds of the three wheels is [v ₁ v ₂ v ₃ ] ^T . Suppose there is a transformation relationship between the two

The above formula can be rewritten as

In the formula, the value of [v _x v _y ω] ^T is given by the control command, so the velocity vectors of the three wheels can be obtained only by determining the parameter matrix.

The movement speed of the robot chassis and the movement speed of the three wheels are shown in Figure 6.

According to the figure above, each value in the parameter matrix can be obtained separately by using the principle of dynamics. The parameter matrix of the final parameter matrix obtained in this design (corrected according to the actual situation) is Bring it into the formula to get the relationship between the speed of the three wheels and the moving speed of the robot. After obtaining the expected speed of the three wheels, the bottom control unit will use the PID algorithm to control the three wheels respectively, and finally realize the motion control of the robot.

In addition to controlling the chassis motor, the underlying control unit also realizes the acquisition and conversion of encoder data. The encoder of the motor adopted in the present invention is an AB phase output, and the phase difference between the two phases is 90 degrees. The current speed of the motor can be obtained by counting the rising edges of any phase, and by detecting the phase between the two phases The difference can judge the rotation direction of the motor. In the present invention, the A-phase pulse of the encoder is counted through the GPIO interrupt of the bottom controller, and the phase difference between the A-phase and the B-phase is determined by judging the level of the B-phase when the rising edge of the A-phase arrives. If the rising edge of phase A arrives and phase B is at low level, the motor will rotate forward, otherwise, the motor will reverse.

After obtaining the motor speed and direction through the encoder, the speed of the three motors needs to be converted to the speed of the robot, which is the opposite of the conversion process from the desired speed of the robot to the desired speed of the motor. After the conversion is completed, the underlying control unit will use the ROS communication mechanism to publish it on the /velocity topic, and the upper-level algorithm will obtain the real-time speed information of the robot by subscribing to this topic.

Innovation of the present invention

1) Invented a new robot autonomous navigation system based on laser radar positioning technology. Through the mutual cooperation of two first-line laser radars, the indoor robot autonomous navigation function is more stable and reliable than the traditional solution.

2) Using the mechanical device and embedded controller to realize the cooperative work of dual radars.

3) The present invention has high portability and can be flexibly applied to many different indoor scenes.

Claims (8)

1. A robot autonomous navigation system based on a dual-line laser radar, characterized in that it includes a dual-line radar system, an upper-level navigation system, a bottom-level control algorithm, and a motion execution system; The upper-level navigation system includes a SLAM algorithm and a coordinate transformation algorithm; Use the SLAM algorithm to dynamically construct a two-dimensional map of the current space based on the original lidar data collected by the horizontally installed radar, and at the same time solve the displacement information of the robot platform; According to the robot displacement information obtained by the SLAM algorithm, the coordinate transformation algorithm performs coordinate transformation and superposition on the original lidar data acquired by the dynamic scanning radar at each moment, so as to determine the three-dimensional shape of the obstacles in the space around the robot from the collected environmental information. information ; Carry out global path planning based on the two-dimensional map constructed, and plan a feasible path between the current position and the destination; at the same time, collect the real-time speed of the robot; in this way, carry out local path according to the collected speed information , global path , and three-dimensional data Plan and produce control instructions to realize the upper-level navigation of the robot; The bottom-level control algorithm analyzes and transforms the coordinates of the control instructions generated by the upper-level navigation system, and uses PID control to control the motion execution system, thereby driving the robot to move. 2. The system according to claim 1, wherein the dual-line laser radar system: one of the radars (radar 2) is installed at the bottom of the robot 10cm away from the ground, and the scanning plane of the radar 2 is placed horizontally. The basic outline information of the indoor environment can be obtained, and the user can subsequently build a global map; the other laser radar (radar 1) is installed on the scanning radar controller on the top of the robot. 3. The system according to claim 2, characterized in that, limited by the structure of the robot, in order to protect the radar from being damaged by collisions during the operation of the robot, the radar 2 is embedded in the robot as a whole, and a gap of about 150 degrees is reserved at the front end. The space is used to scan the external environment, and the data in the rest of the angle range are discarded. 4. The system according to claim 2, wherein the whole scanning radar controller is made up of a mechanical device and a control circuit; the mechanical device includes a bracket for fixing the whole device, a base for installing the radar 1, a The connecting rod and the steering gear 1 for providing power, the whole device is a quadrilateral structure. When the acquisition system is in operation, the steering gear 1 moves up and down under the control of the control circuit (that is, the radar controller), thereby driving the radar 1 to scan the space around the rotation axis of the base. 5. The system according to claim 2, wherein since the radar 1 is located on the top of the robot, its scanning range is mainly distributed below the plane where the top of the robot is located. As shown in Figure 2, in order to enable the robot to bypass obstacles smoothly, a certain safety distance is reserved around. From the geometric relationship, the scanning range θ ₁ of the radar 1 below the horizontal plane can be calculated by formula (1) Where δ is the safety distance, and H is the height of the robot. 6. The system according to claim 2, wherein the coordinate conversion algorithm: The data collected by the radar that scans the space is based on the distance information of the coordinate system fixed on the radar, and there is relative motion between the radar and the robot system platform, so it is necessary to obtain the three-dimensional information of obstacles in the robot coordinate system based on the radar data. Coordinate transformation of the data; The raw data collected by the radar is represented as (ρ, φ, z) in the cylindrical coordinate system, and converted to rectangular coordinates according to formula (2) <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>x</mi> <mo>=</mo> <mi>&amp;rho;</mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;phi;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>y</mi> <mo>=</mo> <mi>&amp;rho;</mi> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&amp;phi;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>z</mi> <mo>=</mo> <mi>z</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> If the forward direction of the robot system platform is the positive direction of the X-axis, the vertical upward direction is the positive direction of the Z-axis, and the center of the robot chassis is used as the origin to establish a coordinate system, then the radar data can be calculated by the following formula in the robot coordinate system Among them, [xyz]' is the radar data transformed to the Cartesian coordinate system, [x'y'z']' is the radar data transformed to the robot coordinate system, is the angle between the current radar scanning plane and the horizontal plane sent back by the radar controller, [x ₀ y ₀ z ₀ ]' is the coordinate of the radar in the robot coordinate system; Considering that the robot is constantly moving while scanning, there is relative motion between the robot coordinate system and the space coordinate system. In order to obtain more accurate spatial information, it is necessary to further convert the radar data in the robot coordinate system to the space coordinate system. It is necessary to obtain the position of the robot in the space coordinate system. The system uses the fixedly installed laser radar 1 as the data source for obtaining the location information, and what the radar collects is the environmental information in the same horizontal plane; Firstly, the basic coordinate translation transformation is performed on the data to convert it into the space coordinate system, and then the SLAM algorithm is used to perform feature matching on the data collected by the robot at different times, and then the displacement information of the robot in space is calculated. By accumulating the displacement information and correcting the accumulated result according to the moving speed of the robot measured by the encoder, the position in the space coordinate system can be obtained according to the initial position of the robot; After obtaining the position of the robot in the space coordinate system, the radar data in the robot coordinate system can be translated to obtain the spatial information in the space coordinate system. After the robot completes a scan within a 90° range, all scanned points are superimposed after the above transformation, and finally the three-dimensional information of the space near the current robot is obtained. 7. The system according to claim 2, wherein the path planning is the decision-making part of the whole system, the process performs path planning according to the environment map and obstacle information and the motion state of the robot, and encapsulates it into a control Commands are sent to the underlying controller. 8. The system according to claim 2, characterized in that said underlying control algorithm: After receiving the control command generated by the upper navigation system through the serial port, it first analyzes it. The control command contains the expected moving speed and rotational angular velocity of the robot. This design uses a three-wheel drive mechanical platform, and the control command provides the speed of the entire robot platform, so it is necessary to convert the expected speed of the robot into the expected speed of the three wheels. Take the center of the robot chassis as the origin, the forward direction of the robot is the positive direction of the x-axis, and the vertical upward direction is the positive direction of the z-axis to establish a coordinate system. Set the expected speed of the robot in the directions of the x-axis and y-axis and the vector formed by the rotational speed around the z-axis is [v _x v _y ω] ^T , and the vector formed by the speeds of the three wheels is [v ₁ v ₂ v ₃ ] ^T . Suppose there is a transformation relationship between the two <mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>v</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mi>y</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mi>&amp;omega;</mi> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>K</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>13</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>K</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>K</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>v</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> The above formula can be rewritten as <mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>v</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>K</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>13</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>K</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>K</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>K</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>v</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mi>y</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mi>&amp;omega;</mi> </mtd> </mtr> </mtable> </mfenced> </mrow> In the formula, the value of [v _x v _y ω] ^T is given by the control command, so the velocity vectors of the three wheels can be obtained only by determining the parameter matrix. The parameter matrix of the finally obtained parameter matrix (corrected according to the actual situation) is Bring it into the formula to get the relationship between the speed of the three wheels and the moving speed of the robot. After obtaining the expected speed of the three wheels, the bottom control unit will use the PID algorithm to control the three wheels respectively, and finally realize the motion control of the robot.