CN103753534B - A kind of Movement Controller of Mobile Robot and control method thereof - Google Patents

Abstract

The invention discloses a mobile robot controller and a control method, including a remote controller, a wireless transceiver module, an industrial computer, a receiver, a sensor, a sensor wireless transmission system, a proportional valve, and a switch valve; The control information is sent to the main industrial computer in the form of instructions. At the same time, the industrial computer collects the information of the absolute value encoders of each joint. After the main industrial computer performs kinematic algorithm processing, the control instructions are sent to the receiver to complete the mobile assembly robot. sport control. The invention utilizes a special tool connected at its end, and adopts a master-slave plus autonomous control method to replace the human arm to complete the high-frequency automobile assembly task. On the one hand, it can reduce the labor intensity of the workers, and on the other hand, it also provides a guarantee for the safety of the workers. Mobile assembly robot controller and control method.

Description

A mobile robot controller and its control method

technical field

The invention relates to a robot technology, in particular to a mobile robot controller and a control method thereof.

Background technique

The assembly robot is the core equipment of the flexible automated assembly system, which consists of a robot manipulator, controller, end effector and sensing system. Among them, the structural types of the manipulator include horizontal joint type, rectangular coordinate type, multi-joint type and cylindrical coordinate type, etc.; the controller generally adopts multi-CPU or multi-level computer system to realize motion control and motion programming; the end effector is adapted to different The assembly object is designed into various claws and wrists, etc.; the sensing system is used to obtain the interaction information between the assembly robot, the environment and the assembly object. Commonly used assembly robots mainly include programmable general-purpose assembly operators, namely PUMA robots (first appeared in 1978, the ancestor of industrial robots) and planar double-jointed robots, namely SCARA robots. Compared with general industrial robots, assembly robots have the characteristics of high precision, good flexibility, small working range, and can be used in conjunction with other systems. They are mainly used in the manufacturing industry of various electrical appliances.

The current assembly robots are fixed manipulators, and the manipulators are installed on the mobile robot. Therefore, compared with the fixed manipulators, they have larger working space and higher flexibility, and can be used more widely. However, the structure of the mobile assembly robot is more complicated than that of the fixed manipulator arm, so it is more difficult to plan and control its end.

Contents of the invention

The purpose of the present invention is to overcome above-mentioned deficiencies in the prior art, provide a kind of mobile assembly robot controller, utilize the special tool connected at its end, adopt the master-slave plus autonomous control mode to replace the human arm to complete the assembly operation of the car, on the one hand The labor intensity of the operators can be reduced, and on the other hand, the mobile assembly robot controller and the control method can also guarantee the safety of the operators.

In order to achieve the above object, the present invention adopts the following technical solutions.

A mobile robot controller includes a remote controller, a wireless transceiver module, an industrial computer, a receiver, a sensor, a sensor wireless transmission system, a proportional valve, and a switch valve.

The remote controller communicates with the industrial computer through the wireless module, the industrial computer is connected to the receiver, the receiver is connected to the proportional valve and the switch valve, the sensor is arranged on the robot, and is connected to the industrial computer through the sensor wireless transmission system, the instructions of the remote controller , sent to the industrial computer through the wireless module, and the industrial computer sends the speed command and the switch drive command to the robot through the receiver; the remote controller includes an analog quantity acquisition module, a digital quantity acquisition module, a magnetic coupling isolation module 1, a magnetic coupling isolation module Module II, magnetic coupling isolation module III, reset circuit, JTAG circuit, microprocessor circuit, serial port drive circuit. The analog quantity acquisition module and the digital quantity acquisition module are respectively connected to the microprocessor through the magnetic coupling isolation module I and the magnetic coupling isolation module II. Connected with the microprocessor, the analog quantity acquisition module collects the data of the analog joystick of the remote control, and the digital quantity acquisition module collects the data of the key with lock and the self-resetting key, and after being processed by the microprocessor, through the serial port drive circuit and the wireless module, Send to the industrial computer. The remote controller can be provided with 7 analog sticks, namely, engine start and stop, throttle, one-key recovery, teaching, reproduction, world coordinate system movement buttons, and tool control.

The microprocessor adopts a 32-bit TMS320F2812DSP chip, and the main frequency can reach 150MHz.

The magnetic coupling isolation module selects ADuM1400/1/2 four-channel digital isolator of ADI Company. The working voltage at both ends is 2.7V ~ 5.5V, which supports low voltage operation and can realize level conversion.

The robot is provided with a mechanical arm, and the mechanical arm is a hydraulic mechanical arm with 5 degrees of freedom; the position sensor measures the angle or length of each axis of the mechanical arm; the mechanical arm can be driven by hydraulic pressure, and the hydraulic pressure source is provided with a solenoid valve for To start, stop the engine, control the size of the throttle.

The vehicle body movement control has functions such as forward, backward, speed regulation, left and right steering safety monitoring, etc.

The motion control of the lifting mechanism has the functions of single joint control, single joint position feedback and safety monitoring.

The engine and power supply power monitoring module has functions such as engine start-stop, speed regulation and other controls, as well as monitoring of engine status and power supply voltage.

The industrial computer adopts ARK-5260 small industrial computer with a main frequency of 1.66GHz, has 2 network ports, supports 4-way 232/422/485 interfaces, comes with 2-way PCI expansion slots, and is powered by 12-24V DC power supply.

The receiver outputs 7 channels of analog signals with a voltage range of 3-9V, 6V representing the median; outputs 3 channels of switching signals with a driving current of 10A and 12V.

The angular displacement sensor adopts BE1822S58, with a total accuracy of 13 bits per turn, RS485 free protocol output, no need for change, and has the functions of starting point calibration and setting.

The wireless transmission module adopts the SZ02-ZIGBEE wireless communication module, which has the advantages and characteristics of long communication distance, strong anti-interference ability, flexible networking, etc., and can realize data transfer between devices between point-to-multipoint and multipoint-to-multipoint Transparent transmission.

The hydraulic proportional valve adopts Danfoss valve, the power supply voltage is 12V, the input control signal is 3-9V, and 6V is the zero position.

The invention also discloses a mobile robot control method, comprising the following steps:

Input the initial point P ₀ (x ₀ , y ₀ , z ₀ ) and end point P _f (x _f , y _f , z _f ) of the robot motion, the speed P _v , the acceleration and deceleration time T _a and the interpolation period T _c , and run time T;

The determination of the basic parameters and the solution method of the interpolation point; before the interpolation movement, determine whether P _v meets the acceleration and deceleration requirements; the method is as follows:

From P ₀ (x ₀ , y ₀ , z ₀ ) and P _f (x _f , y _f , z _f ) get the actual moving distance P _d =|P ₀ P _f |; calculate the acceleration and deceleration from P _v and T _a required distance If C _d ≥ P _d , the actual movement speed Otherwise, C _v =P _v ; from the time T _a and the interpolation time T _c to obtain the number of acceleration steps S _a ; from P ₀ (x ₀ ,y ₀ ,z ₀ ) and P _f (x _f ,y _f ,z _f ), the parameter equation of the space line can be obtained

$\left\{\begin{array}{c}x=k(x_f-x_0)+x_0\\ \mathrm{the\; y}=k({\mathrm{the\; y}}_f-{\mathrm{the\; y}}_0)+{\mathrm{the\; y}}_0\\ z=k(z_f-z_0)+z_0\end{array}\right.$ where k is the scaling factor, 0≤k≤1(1)

Therefore, according to formula (1), the distance from each interpolation point P _i ( _xi , y _i , z _i ) to P ₀ can be obtained as

$C_{\mathrm{SD}\left(i\right)}=\left|P_i{P}_0\right|=\sqrt{{(x_i-x_0)}^2+{({\mathrm{the\; y}}_i-{\mathrm{the\; y}}_0)}^2+{(z_i-z_0)}^2}=k{P}_d$ (C _Sd(i) represents the distance from P _i ( _xi ,y _i , _zi ) to P ₀ , P _d =|P ₀ P _f |)(2)

i is the step number of each interpolation point, between 0 and All positive integers between;

Let the movement distance of the nth interpolation segment be S _d(n) , n=1,...,i, and get the distance from point P _i to P ₀

$C_{\mathrm{SD}\left(i\right)}=\underset{\mathrm{no}=1}{\overset{i}{\mathrm{\Σ}}}{S}_{d\left(\mathrm{no}\right)}=C_{\mathrm{SD}(i-1)}+S_{d\left(i\right)},$ C _Sd(i-1) represents the distance from P _i-1 ( _xi-1 , y _i-1 , z _i-1 ) to P ₀ ,

S _d(i) is the movement distance of the i-th interpolation segment, so the calculation formula of the scale factor k of each interpolation point obtained from formulas (1) and (2) is as follows:

$k=\frac{C_{\mathrm{SD}\left(i\right)}}{P_d}=\frac{\underset{\mathrm{no}=1}{\overset{i}{\mathrm{\Σ}}}{S}_{d\left(\mathrm{no}\right)}}{P_d}=\frac{C_{\mathrm{SD}(i-1)}+S_{d\left(i\right)}}{P_d}$ Where k is a scaling factor, 0≤k≤1(3)

Calculate k by formula (3), and get the Cartesian coordinates of the interpolation point.

The method for determining the moving distance S _{d (i)} of the accelerated movement segment is as follows:

The acceleration is obtained from the actual moving speed C _v and the acceleration and deceleration time T _a Therefore, the velocity S _cv(i) of the i-th interpolation point on the acceleration section =iT _c ·a, we get

${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; cv</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; cv</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; a</span>}{{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

_Scv(i-1) represents the speed of the i-1th interpolation point.

The method for determining the moving distance S _{d (i)} of the uniform motion segment is as follows:

The moving distance of each interpolation segment in the constant velocity segment S _d(i) = C _v · T _c .

The method for determining the motion distance S _{d (i)} of the deceleration motion segment is as follows:

After passing through the previous i-1 interpolation points, the remaining distance L _d(i) = P _d -C _Sd(i-1) can be obtained, so the acceleration of the deceleration section can be obtained Then the speed S _cv(m) of the mth interpolation point on the deceleration section = C _v +mT _c ·a, can be obtained

${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; cv</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; cv</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; [</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; a</span>}<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}<span\; class="notranslate">\; ]</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

The working principle of the mobile assembly robot controller of the present invention is: collect the control information of the remote control panel through the remote control acquisition board, send it to the main industrial computer in the form of instructions, and at the same time, the industrial computer collects the information of the absolute value encoders of each joint, and transmits the After processing the kinematics algorithm, the industrial computer sends the control command to the receiver to complete the motion control of the mobile assembly robot.

Adopting the above scheme, the present invention has the following advantages. First, the self-designed mobile assembly robot control system has been verified by experiments, and can replace human arms to complete automobile assembly tasks with high operating frequency; The system has low power consumption and small size, which is suitable for the application needs of mobile assembly robots. The third is that the mobile assembly robot can realize complex one-button restoration, teaching reproduction, and linear interpolation movement; the fourth is that the system adopts a modular design, which is open, readable, expandable, and maintainable for continuous development. Fifth, the main controller adopts the main industrial computer, which realizes kinematics positive and negative solution and interpolation algorithm, which can realize complex motion control and fast processing speed. Sixth, the controller has a position sensor interface and a potentiometer interface, with complete functions and high position accuracy.

Description of drawings

Fig. 1 is the general block diagram of structural principle of the present invention;

Fig. 2 is a schematic block diagram of the acquisition board of the remote controller of the present invention;

Fig. 3 is a structural diagram of the vehicle-mounted mechanical arm of the present invention;

Fig. 4 is a remote control panel diagram of the present invention;

Fig. 5 is a schematic diagram of the one-button restoration principle of the present invention;

Fig. 6 is a flow chart of trajectory planning in the world coordinate system of the present invention;

Among them, 1. Remote control; 2. Wireless sending module; 3. Wireless receiving module; 4. Industrial computer; 5. Receiver; 6. Position sensor; 7. 485 bus; 8. Proportional valve; 9. On-off valve; 10 1. Analog acquisition module; 11. Digital acquisition module; 12. Magnetic coupling isolation module I; 13. Magnetic coupling isolation module II; 14. Magnetic coupling isolation module III; 15. Reset circuit; 16. JTAG circuit; 17. Micro Processor; 18, serial port drive circuit; 19, crystal oscillator circuit.

detailed description

The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

Referring to Fig. 1, a mobile assembly robot controller includes a remote controller 1, a wireless sending module 2, a wireless receiving module 3, an industrial computer 4, a receiver 5, a position sensor 6, a 485 bus 7, a proportional valve 8, and a switch valve 9 .

The output of the remote controller 1 control panel is connected to the input of the data acquisition board, and the output of the data acquisition board is connected to the wireless transmission module 2; the output of the wireless receiving module 3 is connected to the input of the industrial computer 4 through the serial port, and the output of the industrial computer 4 is connected to the receiver through the serial port 5, the output of the receiver 5 is connected to the proportional valve 8 and the switching valve 9; the position sensor 6 is connected to the input of the industrial computer 4 through the 485 bus 7. The instructions of the remote controller 1 are sent to the industrial computer 4 through the wireless sending module 2 and the wireless receiving module 3, and the industrial computer 4 sends speed instructions and switch driving instructions to the hydraulic system through the receiver 5.

Referring to Fig. 2, described remote controller 1 acquisition board comprises analog quantity acquisition module 10, digital quantity acquisition module 11, magnetic coupling isolation module I12, magnetic coupling isolation module II13, magnetic coupling isolation module III14, reset circuit 15, JTAG circuit 16, Microprocessor 17, serial port drive circuit 18, crystal oscillator circuit 19. The analog quantity acquisition module 10 collects the data of 7 analog joysticks, and the digital quantity acquisition module 11 collects the data of the button with lock and the self-resetting button, and after being processed by the microprocessor 17, through the serial port drive circuit 18 and the wireless transmission module 2, Send to the wireless receiving module 3.

Referring to Fig. 3, the mechanical arm has 5 degrees of freedom. The mechanical structure includes a base on which a support arm is installed. The upper end of the support arm is a shoulder joint, the forearm is installed on the shoulder joint, and the front end of the forearm is connected to the platform. The platform includes a platform pitch axis and a platform rotation axis.

See Figure 4, the schematic diagram of the remote control panel. There are 7 analog joysticks, which control the front and rear of the left wheel, the front and rear of the right wheel, the left and right of the upper arm, the pitch of the upper arm, the telescopic extension of the forearm, the pitch of the platform, and the left and right of the platform; the keys with lock include tools, selection, XYZ world coordinate system, throttle size control; self-recovery buttons include reset, engine start and stop, teaching, reproduction, and start controls; indicator lights include power, wireless communication, and alarm lights.

Referring to Figure 5, when the mobile assembly robot is in the automatic motion mode, it will automatically execute in a loop according to the pre-written motion control program until the Stop button on the control panel is pressed.

Referring to Fig. 6, the present embodiment adopts the mode motion of spatial linear interpolation, can be divided into following several steps and finish:

Input the initial point P ₀ (x ₀ , y ₀ , z ₀ ) and end point P _f (x _f , y _f , z _f ) (f is the abbreviation of final) of the robot motion, the velocity P _v , the acceleration and deceleration time T _a and Interpolation cycle T _c , running time T;

The determination of basic parameters and the solution method of interpolation points. Since the linear motion of the robot in space needs to go through the acceleration and deceleration and uniform motion segments, it should be determined whether P _v meets the acceleration and deceleration requirements before interpolation motion. Methods as below:

From P ₀ (x ₀ , y ₀ , z ₀ ) and P _f (x _f , y _f , z _f ), the actual moving distance P _d = |P ₀ P _f |; from P _v and T _a can be calculated Required distance for deceleration section If C _d ≥ P _d , the actual movement speed Otherwise C _v =P _v ; the number of acceleration steps S _a is obtained from the time T _a and the interpolation time T _c . From P ₀ (x ₀ , y ₀ , z ₀ ) and P _f (x _f , y _f , z _f ), the parametric equation of the space line can be obtained

$\left\{\begin{array}{c}x=k(x_f-x_0){+x}_0\\ \mathrm{the\; y}=k({\mathrm{the\; y}}_f-{\mathrm{the\; y}}_0)+{\mathrm{the\; y}}_0\\ z=k(z_f-z_0)+z_0\end{array}\right.$ Where k is a scaling factor (0≤k≤1)(1)

Therefore, from formula (1), each interpolation point P _i ( _xi , y _i , _zi ) can be obtained (i is the step number of each interpolation point, between 0 and All positive integers between ) to P ₀ distance is

$C_{\mathrm{SD}\left(i\right)}=\left|P_i{P}_0\right|=\sqrt{{(x_i-x_0)}^2+{({\mathrm{the\; y}}_i-{\mathrm{the\; y}}_0)}^2+{(z_i-z_0)}^2}=k{P}_d$ (C _Sd(i) represents the distance from P _i ( _xi ,y _i , _zi ) to P ₀ , P _d =|P ₀ P _f |)(2)

Let the movement distance of the nth interpolation segment be S _d(n) (n=1,...,i) (n is all positive integers from 1 to i), (i is the step number of each interpolation point, at 0 and All positive integers between) can get the distance from point P _i to P ₀ (C _Sd(i-1) represents the distance from P _i-1 ( _xi-1 , y _i-1 , z _i-1 ) to P ₀ , S _d(i) is the movement distance of the i-th interpolation segment), Therefore, the calculation formula of the scale factor k of each interpolation point obtained from formulas (1) and (2) is as follows:

$k=\frac{C_{\mathrm{SD}\left(i\right)}}{P_d}=\frac{\underset{\mathrm{no}=1}{\overset{i}{\mathrm{\&Sigma;}}}{S}_{d\left(\mathrm{no}\right)}}{P_d}=\frac{C_{\mathrm{SD}(i-1)}+S_{d\left(i\right)}}{P_d}$ Where k is a scaling factor (0≤k≤1)(3)

From formula (3), k can be obtained, and the Cartesian coordinates of the interpolation points can be obtained. Therefore, the key to the spatial linear interpolation algorithm is to determine the movement distance S _d(i) of each interpolation segment. The following introduces the method of calculating S _d(i) in each segment of the movement:

Accelerate the motion segment. Since the robot acceleration section designed by the present invention is a uniform acceleration motion, the acceleration is obtained by the actual velocity C _v and the acceleration and deceleration time T _a (the unit is m/s^2), so the speed S _cv(i) of the i-th interpolation point on the acceleration section =iT _c ·a, can be obtained

$S_{d\left(i\right)}=\frac{1}{2}(S_{\mathrm{cv}\left(i\right)}+S_{\mathrm{cv}(i-1)})\&Center\; Dot;T_c=\frac{1}{2}(2i-1)a{T_c}^2$ (S _cv(i-1) indicates the speed of the i-1th interpolation point)(4)

uniform motion segment. Because the robot designed by the present invention must go through the deceleration section, and the interpolation operation is "walking while calculating", so before the start of the uniform motion section, it is necessary to calculate whether the remaining distance can meet the system deceleration requirements. The moving distance of each interpolation segment in the constant speed segment S _d(i) = C _v T _c

Deceleration segment. Since the rounding calculation is performed when calculating the number of acceleration steps S _a , it is not possible to simply invert the acceleration of the acceleration section and then plan the acceleration section. This will introduce errors, so the acceleration of the deceleration section should be recalculated. After passing through the previous i-1 interpolation points, the remaining distance L _d(i) = P _d -C _Sd(i-1) can be obtained, so the acceleration of the deceleration section can be obtained Then the speed S _cv(m) of the mth interpolation point on the deceleration section = C _v +mT _c ·a, can be obtained

${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; cv</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; cv</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; [</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; a</span>}<span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}<span\; class="notranslate">\; ]</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

Although the specific implementation of the present invention has been described above in conjunction with the accompanying drawings, it does not limit the protection scope of the present invention. Those skilled in the art should understand that on the basis of the technical solution of the present invention, those skilled in the art do not need to pay creative work Various modifications or variations that can be made are still within the protection scope of the present invention.

Claims (5)

1. A mobile robot controller is characterized in that it comprises a remote controller, a wireless transceiver module, an industrial computer, a receiver, a sensor, a sensor wireless transmission system, a proportional valve, and a switch valve; The industrial computer is connected to the receiver, and the receiver is connected to the proportional valve and the switch valve. The sensor is set on the robot and connected to the industrial computer through the sensor wireless transmission system. The instructions of the remote control are sent to the industrial control through the wireless transceiver module. The industrial computer sends speed instructions and switch drive instructions to the robot through the receiver; the remote controller includes an analog quantity acquisition module, a digital quantity acquisition module, a magnetic coupling isolation module I, a magnetic coupling isolation module II, and a magnetic coupling isolation module III , reset circuit, JTAG circuit, microprocessor circuit, serial port drive circuit; the analog quantity acquisition module and the digital quantity acquisition module are respectively connected to the microprocessor through the magnetic coupling isolation module I and the magnetic coupling isolation module II, and the microprocessor, the magnetic coupling The isolation module III and the serial port drive circuit are connected sequentially, the reset circuit and the JTAG circuit are respectively connected to the microprocessor, the analog quantity acquisition module collects the data of the analog joystick of the remote control, and the digital quantity acquisition module collects the data of the lock button and the self-reset button, After being processed by the microprocessor, it is sent to the industrial computer through the serial port drive circuit and the wireless transceiver module. 2. a mobile robot control method with mobile robot controller as claimed in claim 1, is characterized in that, comprises the steps: Input the initial point P ₀ (x ₀ , y ₀ , z ₀ ) and end point P _f (x _f , y _f , z _f ) of the robot motion, the speed P _v , the acceleration and deceleration time T _a and the interpolation period T _c , and run time T; The determination of the basic parameters and the solution method of the interpolation point; before the interpolation movement, determine whether P _v meets the acceleration and deceleration requirements; the method is as follows: From P ₀ (x ₀ , y ₀ , z ₀ ) and P _f (x _f , y _f , z _f ) get the actual moving distance P _d =|P ₀ P _f |; calculate the acceleration and deceleration from P _v and T _a required distance If C _d ≥ P _d , the actual movement speed Otherwise, C _v =P _v ; from the time T _a and the interpolation time T _c to obtain the number of acceleration steps S _a ; from P ₀ (x ₀ ,y ₀ ,z ₀ ) and P _f (x _f ,y _f ,z _f ), get the space line parametric equation $\left\{\begin{array}{c}x=k(x_f-x_0)+x_0\\ \mathrm{the\; y}=k({\mathrm{the\; y}}_f-{\mathrm{the\; y}}_0)+{\mathrm{the\; y}}_0\\ z=k(z_f-z_0)+z_0\end{array}\right.$ where k is the scaling factor, 0≤k≤1(1) Therefore, according to formula (1), the distance from each interpolation point P _i ( _xi , y _i , z _i ) to P ₀ is $C_{Sd\left(i\right)}=\left|P_i{P}_0\right|=\sqrt{{(x_i-x_0)}^2+{({\mathrm{the\; y}}_i-{\mathrm{the\; y}}_0)}^2+{(z_i-z_0)}^2}={\mathrm{kP}}_d,$ C _Sd(i) represents the distance from P _i ( _xi , y _i , _zi ) to P ₀ , P _d = |P ₀ P _f |, (2) i is the step number of each interpolation point, between 0 and All positive integers between; Let the movement distance of the nth interpolation segment be S _d(n) , n=1,...,i, and get the distance from point P _i to P ₀ C _Sd(i-1) represents the distance from P _i-1 ( _xi-1 , y _i-1 , z _i-1 ) to P ₀ , S _d(i) is the movement distance of the i-th interpolation segment, so the calculation formula of the scale factor k of each interpolation point obtained from formulas (1) and (2) is as follows: Where k is a scaling factor, 0≤k≤1, (3) Calculate k by formula (3), and get the Cartesian coordinates of the interpolation point. 3. mobile robot control method as claimed in claim 2 is characterized in that, the method for determining the moving distance S of the accelerated motion segment _(i) is as follows: The acceleration is obtained from the actual moving speed C _v and the acceleration and deceleration time T _a Therefore, the velocity S _cv(i) of the i-th interpolation point on the acceleration section =iT _c ·a, we get ${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}\mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}\mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>{{\mathrm{<span\; class="notranslate">\; aT</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$ _Scv(i-1) represents the speed of the i-1th interpolation point. 4. mobile robot control method as claimed in claim 2, it is characterized in that, the constant velocity motion segment motion distance S _{d (i)} determination method is as follows: The moving distance of each interpolation segment in the constant velocity segment S _d(i) = C _v · T _c . 5. mobile robot control method as claimed in claim 2 is characterized in that, deceleration motion section motion distance S _{d (i)} determination method is as follows: After passing through the previous i-1 interpolation points, the remaining distance L _d(i) = P _d -C _Sd(i-1) is obtained, and the deceleration section acceleration is obtained Then the speed S _cv(m) of the mth interpolation point on the deceleration section =C _v +mT _c ·a, that is, ${\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; d</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}\mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; S</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}\mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; \&lsqb;</span>\mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; a</span>}<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; c</span>}}<span\; class="notranslate">\; \&rsqb;</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; .</span>$