CN104216406B - The control device and control method on a kind of four-wheel drive omnidirectional chassis - Google Patents

Abstract

The control device and control method on a kind of four-wheel drive omnidirectional chassis, suitable for robot chassis movement control field, wireless serial communication module is used for receiving the instruction from the wireless serial communication module being connected with computer, command information includes the positional value and attitude angle value of the four-wheel drive omnidirectional chassis target point to be moved to, and the instruction received is transmitted into host computer ARM；FPGA merges counting circuit and information acquisition circuit, the data of coded disc counting module are acquired and handled by FPGA, then ARM is transferred to by general purpose I/O Interface, FPGA also serves as interface circuit simultaneously, ARM control instruction is transferred to DC MOTOR CONTROL module, the rotating speed to direct current generator and the control in direction is completed；ARM is the kernel control module of whole control device, to be merged to the data received, realizes and carries out real time settlement to the pose on four-wheel drive omnidirectional chassis, completes the real-time control to four direct current generators.

Description

The control device and control method on a kind of four-wheel drive omnidirectional chassis

Technical field

The present invention is the control device and control method on a kind of four-wheel drive omnidirectional chassis, is primarily adapted for use in robot chassis Move control in mobile control field, the chassis available for domestic robot, industrial robot, military robot.Machine can be made People is all the time along straight line moving, while can change body attitude, greatly shortens the travel distance of robot, improves robot Flexibility and mobility.In this way, robot can be made preferably to complete task in small space.

Background technology

The U.S. have developed First industrial robot with 1962, have passed through the development of nearly 50 years, various countries are in machine People technically puts into know clearly substantial amounts of human and material resources, financial resources, and the development of robot technology has gone through earth-shaking change Change.From initial generation industrial robot, third generation intelligent robot by now, robot technology level there has been to be carried significantly Rise, wherein U.S. of several science and technology poweies, day, moral, English, method etc. are just taken its place in the front ranks of the world, its Sino-U.S., the scientific and technological superpower of day two are more It is to hold a safe lead in all fields.At present, the robot technology of China is reducing and developed country also in stable development Gap.

At present, robot ambulation movement control has become an important branch in robot field, has obtained people Increasing concern.Flexibility, mobility when how to improve robot movement, are the heat that people study in robot field Point.

Currently, to be broadly divided into the sufficient formula of leg, crawler type, wheeled etc. several for the move mode of ground robot.Leg foot formula is main Bionics is relied on, it imitates the biological walking manner such as people, animal, insect, and development is swift and violent at present, the free degree of its body Height, is adapted to complicated landform, it might even be possible to speeling stairway, has the disadvantage that speed is slower；Crawler type is applied to more complicated landform, such as Mountain region etc., translational speed formula more sufficient than leg is fast；Wheeled robot translational speed is most fast, but is typically only capable to applied to relatively flat Landform.

For wheeled robot, what its most of chassis was used is the driving wheel of two-wheel drive, i.e., two, and remaining is Driven pulley, but the chassis of two-wheel drive can not directly carry out horizontal translation, can only carry out longitudinal translation, itself rotation, i.e., Two-wheeled chassis only possesses two frees degree, and this type of drive strongly limit the flexibility of robot.Thus, to improve machine The flexibility of device people, it is necessary to be improved on this basis.

The content of the invention

The present invention is the control device and control method on a kind of four-wheel drive omnidirectional chassis, using this control device and control Method can make the Robot any direction straight line equipped with four-wheel omnidirectional chassis advance, while itself rotates, that is, be expert at The attitude of itself can be arbitrarily adjusted during sailing, that is, has three degree of freedom, this type of drive has stronger compared with two-wheel drive Flexibility.

Four-wheel drive is that flexibility is high compared to the advantage of two-wheel drive, and the free degree is more, but for being required in control also more Height, is the big difficult point in control for the coordination control that four-wheel is moved.The core technology of the present invention is exactly to solve four-wheel motion Coordinate this problem of control.

A kind of control device on four-wheel drive omnidirectional chassis, including main control module（ARM and FPGA）, wireless serial communication mould Block, coded disc counting module, DC MOTOR CONTROL module.Wireless serial communication module is used for receiving from the nothing being connected with computer The instruction of line serial communication modular, the positional value of command information including the four-wheel drive omnidirectional chassis target point to be moved to And attitude angle value, the instruction received is transmitted to host computer ARM；FPGA merges counting circuit and information acquisition circuit, by FPGA is acquired and handled to the data of coded disc counting module, is then transferred to ARM by general purpose I/O Interface, while FPGA Also serve as interface circuit, ARM control instruction be transferred to DC MOTOR CONTROL module, complete to the rotating speed of direct current generator and The control in direction；ARM is the kernel control module of whole control device, to be merged to the data received, realization pair The pose on four-wheel drive omnidirectional chassis carries out real time settlement, and completes the real-time control to four direct current generators.

A kind of control method on four-wheel drive omnidirectional chassis, including control device as used above, and for the device Core control algolithm.Comprise the following steps that：

Set the positional value of the four-wheel drive omnidirectional chassis target point to be moved to, i.e. X, Y coordinate, and body itself The angle, θ to be rotated, starts start button, by wireless serial communication mode, the master installed on four-wheel drive omnidirectional chassis Control module is received after instruction, according to the control algolithm of four-wheel drive omnidirectional set in advance bobbin movement, passes through direct current generator Control module, controls four direct current generator co-ordinations, and the navigation mode used passes through coded disc counting for four code-discs navigation mode Module real-time counting, can record the distance of each wheel walking, this data is transmitted into main control module, main control module is straight to four again Flow motor and carry out real-time PID control, the deviation between current kinetic track and desired guiding trajectory is calculated in real time, constantly carry out inclined Difference is corrected, and whole body is moved along default track, is realized and is changed along straight line moving and in real time certainly in automatic direction Body attitude angle.

Brief description of the drawings

In order to illustrate more clearly about the embodiment of the present invention or technical scheme of the prior art, below will be to embodiment or existing The accompanying drawing to be used needed for having technology description is briefly described, it should be apparent that, drawings in the following description are only this Some embodiments of invention, for those of ordinary skill in the art, without having to pay creative labor, may be used also To obtain other accompanying drawings according to these accompanying drawings.

Fig. 1 is the structural representation on four-wheel omnidirectional provided in an embodiment of the present invention chassis；

Fig. 2 is four-wheel omnidirectional bobbin movement mode figure provided in an embodiment of the present invention；

Fig. 3 is four-wheel omnidirectional bobbin movement analysis chart provided in an embodiment of the present invention；

Fig. 4 is omnidirectional plate wheel scheme of installation provided in an embodiment of the present invention；

Fig. 5 is control device schematic diagram provided in an embodiment of the present invention；

Fig. 6 is four-wheel omnidirectional bobbin movement track provided in an embodiment of the present invention schematic diagram；

Fig. 7, Fig. 8 are bobbin movement deviation schematic diagram provided in an embodiment of the present invention；

Fig. 9 is PID control schematic diagram provided in an embodiment of the present invention；

Figure 10 is four-wheel omnidirectional provided in an embodiment of the present invention chassis program control flow chart；

Embodiment

Below in conjunction with the accompanying drawing in the embodiment of the present invention, the technical scheme in the embodiment of the present invention is carried out clear, complete Site preparation is described, it is clear that described embodiment is only a part of embodiment of the invention, rather than whole embodiments.It is based on Embodiment in the present invention, it is every other that those of ordinary skill in the art are obtained under the premise of creative work is not made Embodiment, belongs to the scope of protection of the invention.

Fig. 1 is the structural representation on four-wheel omnidirectional provided in an embodiment of the present invention chassis.Compared with two-wheeled chassis, four-wheel is complete There is the characteristics of flexibility is high to chassis.The chassis of two-wheel drive can not directly carry out horizontal translation, can only carry out longitudinal translation, Itself rotation, i.e., two-wheeled chassis only possesses two frees degree.Four-wheel drive chassis can then walk along any direction, and also can be certainly Body rotates, i.e. four-wheel drive chassis has three degree of freedom, and flexibility is higher for this compares two-wheel drive.

Fig. 2 is four-wheel omnidirectional bobbin movement mode figure provided in an embodiment of the present invention.As shown in Fig. 2 four-wheel omnidirectional chassis Moved linearly along any direction, and body attitude changes during this period, and this is the characteristics of wheel chassis is protruded the most, is also control Absolute difficult point in system.The advantage of the mode of this movement is to perform complex task（Require that body barycenter is moved by A points B points are moved, while requiring that car body rotates some angle）When highly shortened the move distance of body, determined in body speed In the case of can shorten the time arrived at, and the chassis that this task is two-wheel drive can not be completed.

Fig. 3 is four-wheel omnidirectional bobbin movement analysis chart provided in an embodiment of the present invention.Four-wheel omnidirectional chassis working space For a plane, absolute coordinate system x as shown in Figure 3 is set up_a-y_aWith the coordinate system x on four-wheel omnidirectional chassis_γ-y_γ, wherein, four-wheel is complete Overlapped to chassis coordinate origin with four-wheel omnidirectional center chassis.A be four-wheel omnidirectional center chassis to four wheel between distance, in It is L that the heart moves to B points distance from A points, and θ hopes deviation absolute coordinate system transverse axis x from A points to B points exercise sessions for barycenter_aAngle Degree, γ is x at the end of motion_γWith x_aExpectation angle（I.e. the angle of rotation is expected on four-wheel omnidirectional chassis at the end of motion）, γ ' is the angle of rotation in real time in motion process,For the speed and spin velocity of desired motion, v_iFor institute It is required that wheel i provide along driving direction speed.

By kinematics analysis, following kinematical equation can be obtained：

Aforementioned four equation, then be that 4 direct current generators distinguish corresponding rotating speed equation, willDesired motion Speed and spin velocity are substituted into aforementioned four equation, and we are that can obtain 4 direct current generator theoretical rotational speeds to be controlled v_i。

Therefore, aforementioned four equation is the core algorithm of the present invention.

Fig. 4 is omnidirectional plate wheel scheme of installation provided in an embodiment of the present invention.As illustrated, being connected on omnidirectional plate wheel One incremental coded disc counting module, records the turned number of turns of code-disc, this data is passed into main control module, to omnidirectional in real time The distance passed by is taken turns to be settled accounts.

Fig. 5 is control device schematic diagram provided in an embodiment of the present invention.As shown in Figure 5, wireless serial communication module with ARM is connected, and for receiving the instruction from the wireless serial communication module being connected with computer, command information includes four-wheel drive The positional value and attitude angle value of the omnidirectional chassis target point to be moved to, host computer ARM is transmitted to by the instruction received； FPGA is connected with ARM, and counting circuit and information acquisition circuit are merged, the data of coded disc counting module are acquired by FPGA With processing, ARM is then transferred to by general purpose I/O Interface, while FPGA also serves as interface circuit, ARM control instruction passed It is defeated to arrive DC MOTOR CONTROL module, complete the rotating speed to direct current generator and the control in direction；ARM is the core of whole control device Heart control module, to be merged to the data received, realizes and the pose on four-wheel drive omnidirectional chassis is tied in real time Calculate, and complete the real-time control to four direct current generators；Coded disc counting module is connected with FPGA, when code-disc wheel, which rotates 1, to be enclosed, code Disk counting module can record 1200 numbers, therefore can realize that the distance walked to code-disc wheel is remembered by coded disc counting module The ratio between record, then the radius by code-disc wheel and driving wheel, then it can obtain the travel distance of driving wheel；Motor control module and FPGA It is connected, it is to carry out closed-loop control to the electric current loop of direct current generator that it, which is acted on,.

Fig. 6 is four-wheel omnidirectional bobbin movement track provided in an embodiment of the present invention schematic diagram.As shown in figure 9, by certain week Phase is to the track differential of car, phase this week initial time, the pose of car（X1, y1, theta1）（It is known）, at the end of car pose For（X2, y2, theta2）（It is unknown）.The displacement of revolver is sl within this cycle, and the displacement of right wheel is sr, and the displacement of front-wheel is Sh, the displacement of trailing wheel is sb, and the sideslip respectively taken turns during due to four-wheel omnidirectional moving is serious, what the displacement that code-disc is remembered was taken turns only along each Positive direction, will not record the displacement that each wheel breaks away and produced, so each wheel displacement of code-disc note（dl,dr,dh,db）It is that each wheel is real The component of the border direction of motion（The component of wheel direction of advance）, according to experimental result, find to resolve the barycenter of car as follows Displacement it is more accurate：

The angle that car is turned over is:

In formula (4.27), L_lrFor the code-disc spacing of car left and right wheels, L_bhFor the code-disc spacing of Chinese herbaceous peony trailing wheel.

By Δ θ can in the hope of phase this week at the end of the angle of car be：

theta2=theta1+Δθ

When this section is moved it is sufficiently small when, track with horizontal direction angle can be the straight of VelTheta similar to one Line.Wherein:

Veltheta=theta1+arctan((db+dh)/(dr+dl))

Then have:

x₂=x₁+d*cos(Veltheta)

y₂=y₁+d*sin(Veltheta)

So far, by（X1, y1, theta1）It is proposed（X2, y2, theta2）, so in theory, in plane motion, knowing The road initial pose of body, and have recorded the displacement of each wheel of body, then the pose of four-wheel omnidirectional chassis at any time is all It can obtain.

Fig. 7,8 are bobbin movement deviation schematic diagram provided in an embodiment of the present invention.Straight line fortune is carried out on four-wheel omnidirectional chassis , thus can be straight by this, it is necessary to angle to the coordinate points and straight line set the goal and place coordinate system x-axis positive direction when dynamic Line is decided.Robot coordinate value and course angle of itself according to this target line and currently, judges and target line Position and angular deviation, while controlling four-wheel differentia amendment travel track.

As shown in fig. 7, it is necessary to which two offsets of regulation are laterally inclined in the makeover process of four-wheel omnidirectional chassis track Difference and course deviation, if it is desired to the two variables are adjusted simultaneously, then situation can be more complicated, it is necessary to according to body not Same situation assigns different Signed Domination motor differentials regulations to regulation parameter., body ideal trajectory is straight along y direction Line is run, and lateral deviation and course deviation shown in Fig. 7 are certainly existed when body and ideal trajectory generation deviation.

As shown in figure 8, in order to adjust lateral deviation and the course deviation that four-wheel omnidirectional chassis is advanced simultaneously, improving linear rows Walk convergent rapidity, reduce overshoot, introduce a preposition sensing point, i.e., on a certain position in front of body direct of travel, A virtual point is placed, the deviation of this point and target line is calculated, and sets suitable feedback oscillator to count according to this deviation Calculate the differential value of four wheels of amendment, the straight path that amendment body is advanced.The selection of preposition sensing point length and feedback oscillator Coefficient needs to be adjusted according to the debugging effect of the actual walking of robot, as shown in Figure 8., can be with by adding preposition sensing point Lateral deviation and course deviation are incorporated into together, while equivalent to anticipatory control link is introduced, walking precision can be improved. The control of attitude angle during for body movement, as long as adding the drift correction of attitude angle on this basis.

Fig. 9 is PID control schematic diagram provided in an embodiment of the present invention.

Traditional pid control algorithm is divided into position type PID control and incremental PID control.

The basic formula of position type pid control algorithm is：

The basic formula of incremental pid control algorithm is：

Δu(k)=K_p{[e(k)-e(k-1)]+K_Ie(k)+K_D[e(k)-2e(k-1)+e(k-2)]}

Wherein, computer export value during u (k) --- kth time sampling；Deviation when e (k) --- n-th is sampled；Δu (k) --- the difference of the computer of -1 sampling of kth time sampling and kth.

By observing above formula, the output of Position Form PID algorithm is relevant with whole past state, to be used in calculating formula The accumulated value of deviation is removed, larger accumulated error is easily produced, integration saturation is easily produced.And increment type need to only calculate increment, When there is calculation error or precision is not enough, the influence calculated controlled quentity controlled variable is smaller.

When execution unit is without integrating unit, the digital quantity of its position and computer export is to correspond（Watched Ru electro-hydraulic Take valve）, generally positional PID control calculation is used in engineering.If execution unit band integrating unit（Such as stepper motor, stepping electricity Machine band movable valve drives multiturn potentiometer）When, generally select incremental timestamp algorithm.

For being used in the present invention to be used in closed loop PID control mode principle of the direct current generator with incremental encoder Position type PID or incremental PID, but because incremental encoder is integrating unit, so from incremental PID control It is even more ideal.

Figure 10 is four-wheel omnidirectional provided in an embodiment of the present invention chassis program control flow chart.The traveling process on chassis has Three phases, respectively boost phase, constant velocity stage and decelerating phase.It will be calculated in real time and target point in each stage Distance, if reaching, target point just stops.In accelerating sections, the control mode of even acceleration can be used, but needs what a sets first Maximal rate, when speed reaches maximal rate, then at the uniform velocity area, into after at the uniform velocity area, need to detect whether to reach deceleration area, When entering deceleration area, used in order to avoid the mode due to the next motor oscillating problem of unexpected deceleration strip, deceleration area deceleration It is to be multiplied by present speed from target range and deceleration area length ratio, obtains a smooth attenuation process, final speed meeting It is slow to reach zero.The effect of this deceleration is pretty good, slows down also quickly.In real process, it is contemplated that the requirement of time and anti- Only motor slowly runs, and reduces to 10% maximal rate when speed and is walked with this uniform velocity, until reach target point stop or Person directly enters next section of path in program.

In summary, useful achievement of the invention is：There is provided a kind of control method on four-wheel drive omnidirectional chassis.It is this The move mode on chassis can make Robot any direction straight line advance, while itself rotates, i.e., in the process of moving The attitude of itself can be arbitrarily adjusted, is compared compared with two-wheel drive, there is higher flexibility, mobility.

It is an advantage of the invention that：

1. creative use four-wheel omnidirectional driving chassis.Four-wheel drive has flexibility, mobility compared with two-wheel drive High the characteristics of.The chassis of two-wheel drive can not directly carry out horizontal translation, can only carry out longitudinal translation, itself rotation, i.e., two Wheel chassis only possesses two frees degree.Four-wheel drive chassis can then walk along any direction, and also can itself rotation, i.e. four-wheel Driving chassis has three degree of freedom, and this is higher compared to flexibility, mobility for two-wheel drive.

2. establishing the kinematical equation and kinetics equation on four-wheel drive omnidirectional chassis, kinematics and dynamics have been carried out Analysis, and emulated, the feasibility of this control mode is demonstrated in theory.

3. using four omnidirectional plate wheels navigation mode, the attitude angle and coordinate of body are gone out using four code-disc real-time resolvings.This The mode of kind is while precision is ensured, it is to avoid with low cost using the higher gyro of price.

4. adding preposition sensing point, lateral deviation and course deviation are incorporated into together, while advanced equivalent to introducing Correction link, can improve traveling precision.

Apply specific embodiment in the present invention to be set forth the principle and embodiment of the present invention, above example Explanation be only intended to help and understand the method for the present invention and its core concept；Simultaneously for those of ordinary skill in the art, According to the thought of the present invention, it will change in specific embodiments and applications, in summary, in this specification Appearance should not be construed as limiting the invention.

Claims (2)

1. the control device on a kind of four-wheel drive omnidirectional chassis, it is characterised in that including the main control module being made up of ARM and FPGA, Wireless serial communication module, coded disc counting module, DC MOTOR CONTROL module；Wireless serial communication module is connected with ARM, sets Wireless serial communication module in chassis is used for receiving the wireless serial communication mould at the main control module for coming from computer connection The instruction of block, instruction includes the positional value and attitude angle value of the four-wheel drive omnidirectional chassis target point to be moved to, and will connect The instruction received is transmitted to host computer ARM；FPGA is connected with ARM, and FPGA merges counting circuit and information acquisition circuit, by FPGA Data to coded disc counting module are acquired and handled, and then ARM are transferred to by the interface of general purpose I/0, while FPGA also makees For interface circuit, ARM control instruction is transferred to DC MOTOR CONTROL module, the rotating speed to direct current generator and direction is completed Control；ARM is the kernel control module of whole control device, to be merged to the data received, is realized to four-wheel Drive the pose on omnidirectional chassis to carry out real time settlement, and complete the real-time control to four direct current generators；

Coded disc counting module is connected with FPGA, and when code-disc wheel, which rotates 1, to be enclosed, coded disc counting module can record 1200 numbers, therefore Realize that the distance walked to code-disc wheel is recorded by coded disc counting module, then by the radius of code-disc wheel and driving wheel it Than obtaining the travel distance of driving wheel；Motor control module is connected with FPGA, and it is that the electric current loop of direct current generator is carried out that it, which is acted on, Closed-loop control；

By track differential of some cycles to car, phase this week initial time, the pose of car is known x1 and y1 and thetal, knot The pose of car is unknown x2 and y2 and theta2 during beam；The displacement of revolver is dl within this cycle, and the displacement of right wheel is dr, The displacement of front-wheel is dh, and the displacement of trailing wheel is db, and the sideslip respectively taken turns during due to four-wheel omnidirectional moving is serious, and the displacement of code-disc note is only Along each wheel positive direction, will not record it is each wheel break away and produce displacement, so code-disc note each wheel displacement dl and dr and Dh and db are the components for each wheel actual motion direction for representing wheel direction of advance, and the position of the barycenter of car is resolved as follows Move：

$d=\sqrt{{\left(\frac{dl+dr}{2}\right)}^2+{\left(\frac{dh+db}{2}\right)}^2}$

The angle that car is turned over is：

$\mathrm{\Δ}\mathrm{\θ}=\frac{1}{2}(\frac{dr-dl}{L_{lr}}+\frac{dh-db}{L_{bh}})$

In above formula, L_1rFor the code-disc spacing of car left and right wheels, L_bhFor the code-disc spacing of Chinese herbaceous peony trailing wheel；

By Δ θ can in the hope of phase this week at the end of the angle of car be：

Theta2=thetal+ Δs θ

When this section move it is sufficiently small when, track can similar to one be VelTheta with horizontal direction angle straight line；Its In：

Veltheta=thetal+arctan ((db+dh)/(dr+dl))

Then have：

x₂=x1+d*cos (Veltheta)

y₂=y1+d*sin (Veltheta)

So far, in plane motion, it is known that the initial pose of body, and it have recorded the displacement of each wheel of body, then four-wheel is complete It can be obtained to the pose of chassis at any time；

When four-wheel omnidirectional chassis is moved along a straight line, it is necessary to the coordinate points and straight line and place coordinate system x-axis set the goal just The angle in direction；Robot coordinate value and course angle of itself according to this target line and currently, judges and target line Position and angular deviation, while controlling four-wheel differentia amendment travel track；

, it is necessary to which two offsets of regulation are lateral deviation and course deviation in the makeover process of four-wheel omnidirectional chassis track, When the two variables being adjusted simultaneously, then different symbol controls are assigned to regulation parameter in different situations according to body Motor differential regulation processed；

A preposition sensing point is introduced, i.e., on a certain position in front of body direct of travel, places a virtual point, calculates this The deviation of point and target line, and set feedback oscillator to calculate four of the amendment differential values taken turns, amendment according to this deviation The straight path that body is advanced；The selection of preposition sensing point length and feedback gain are needed according to the actual walking of robot Effect is debugged to adjust；

The basic formula of position type pid control algorithm is：

$u\left(k\right)=K_p\{e\left(k\right)+K_I\underset{j=0}{\overset{k}{\Σ}}e\left(j\right)+K_D\[e\left(k\right)-e(k-1)\]\}$

The basic formula of incremental pid control algorithm is：

△ u (k)=K_p{[e(k)-e(k-1)]+K_Ie(k)+K_D[e(k)-2e(k-1)+e(k-2)]}

Wherein, computer export value during u (k) --- kth time sampling；Deviation during e (k) --- kth time sampling；

The difference of the computer of -1 sampling of Δ u (k) --- kth time sampling and kth；

When execution unit is without integrating unit, the digital quantity of its position and computer export is to correspond, using Position Form PID Control algolithm；When execution unit band integrating unit, from incremental timestamp algorithm；

The traveling process on chassis has three phases, respectively boost phase, constant velocity stage and decelerating phase；In each stage The distance with target point will be calculated in real time, if reaching, target point just stops；In accelerating sections, the controlling party of even acceleration can be used Formula, but need what a maximal rate set first, when speed reaches maximal rate, then at the uniform velocity area, into after at the uniform velocity area, It need to detect whether to reach deceleration area, when entering deceleration area, the mode that deceleration area slows down is used from target range with slowing down The ratio between section length is multiplied by present speed, obtains a smooth attenuation process, and final speed can slowly reach zero；Actual mistake Cheng Zhong, it is contemplated that the requirement of time and prevent that motor from slowly running, when speed reduces to 10% maximal rate i.e. with this at the uniform velocity Speed is walked, and next section of path is stopped or directly enter in program until reaching target point.

2. the control method on a kind of four-wheel drive omnidirectional chassis, it is characterised in that comprise the following steps that：

Set the positional value of the four-wheel drive omnidirectional chassis target point to be moved to, i.e. X, Y coordinate, and body itself institute

The angle, θ to be rotated, starts start button, by wireless serial communication mode, is installed on four-wheel drive omnidirectional chassis Main control module is received after instruction, according to the control algolithm of four-wheel drive omnidirectional set in advance bobbin movement, passes through direct current Machine control module, controls four direct current generator co-ordinations, and the navigation mode used passes through code-disc meter for four code-discs navigation mode Digital-to-analogue block real-time counting, can record the distance of each wheel walking, this data is transmitted into main control module, main control module is again to four Direct current generator carries out real-time PID control, and the deviation between current kinetic track and desired guiding trajectory is calculated in real time, is constantly carried out Bias correction, makes whole body be moved along default track, realizes automatic direction is along straight line moving and changes in real time Itself attitude angle；

Wireless serial communication module is connected with ARM, for receiving the finger for the wireless serial communication module for coming from computer connection Order, command information includes the positional value and attitude angle value of the four-wheel drive omnidirectional chassis target point to be moved to, and will receive To instruction be transmitted to host computer ARM；FPGA is connected with ARM, counting circuit and information acquisition circuit is merged, by FPGA to code-disc The data of counting module are acquired and handled, and then ARM are transferred to by the interface of general purpose I/0, while FPGA also serves as interface Circuit, DC MOTOR CONTROL module is transferred to by ARM control instruction, completes the rotating speed to direct current generator and the control in direction System；ARM is the kernel control module of whole control device, to be merged to the data received, is realized to four-wheel drive The pose on omnidirectional chassis carries out real time settlement, and completes the real-time control to four direct current generators；Coded disc counting module and FPGA It is connected, when code-disc wheel, which rotates 1, to be enclosed, coded disc counting module can record 1200 numbers, therefore can be realized by coded disc counting module The ratio between the distance that code-disc wheel is walked is recorded, then the radius by code-disc wheel and driving wheel, then it can obtain the row of driving wheel Walk distance；Motor control module is connected with FPGA, and it is to carry out closed-loop control to the electric current loop of direct current generator that it, which is acted on,；

By track differential of some cycles to car, phase this week initial time, the pose of car is known x1 and y1 and thetal, knot The pose of car is unknown x2 and y2 and theta2 during beam；The displacement of revolver is dl within this cycle, and the displacement of right wheel is dr, The displacement of front-wheel is dh, and the displacement of trailing wheel is db, and the sideslip respectively taken turns during due to four-wheel omnidirectional moving is serious, and the displacement of code-disc note is only Along each wheel positive direction, will not record it is each wheel break away and produce displacement, so code-disc note each wheel displacement dl and dr and Dh and db are the components for each wheel actual motion direction for representing wheel direction of advance, and the position of the barycenter of car is resolved as follows Move：

$d=\sqrt{{\left(\frac{dl+dr}{2}\right)}^2+{\left(\frac{dh+db}{2}\right)}^2}$

The angle that car is turned over is：

$\mathrm{\Δ}\mathrm{\θ}=\frac{1}{2}(\frac{dr-dl}{L_{lr}}+\frac{dh-db}{L_{bh}})$

In above formula, L_1rFor the code-disc spacing of car left and right wheels, L_bhFor the code-disc spacing of Chinese herbaceous peony trailing wheel；

By Δ θ can in the hope of phase this week at the end of the angle of car be：

Theta2=thetal+ Δs θ

When this section move it is sufficiently small when, track can similar to one be VelTheta with horizontal direction angle straight line；Its In：

Veltheta=thetal+arctan ((db+dh)/(dr+dl))

Then have：

x₂=x1+d*cos (Veltheta)

y₂=y1+d*sin (Veltheta)

So far, in plane motion, it is known that the initial pose of body, and it have recorded the displacement of each wheel of body, then four-wheel is complete It can be obtained to the pose of chassis at any time；

When four-wheel omnidirectional chassis is moved along a straight line, it is necessary to the coordinate points and straight line and place coordinate system x-axis set the goal just The angle in direction；Robot coordinate value and course angle of itself according to this target line and currently, judges and target line Position and angular deviation, while controlling four-wheel differentia amendment travel track；

, it is necessary to which two offsets of regulation are lateral deviation and course deviation in the makeover process of four-wheel omnidirectional chassis track, The two variables are adjusted when simultaneously, then assign different Signed Dominations to regulation parameter in different situations according to body Motor differential is adjusted；

A preposition sensing point is introduced, i.e., on a certain position in front of body direct of travel, places a virtual point, calculates this The deviation of point and target line, and set feedback oscillator to calculate four of the amendment differential values taken turns, amendment according to this deviation The straight path that body is advanced；The selection of preposition sensing point length and feedback gain are needed according to the actual walking of robot Effect is debugged to adjust；

The basic formula of position type pid control algorithm is：

$u\left(k\right)=K_p\{e\left(k\right)+K_I\underset{j=0}{\overset{k}{\Σ}}e\left(j\right)+K_D\[e\left(k\right)-e(k-1)\]\}$

The basic formula of incremental pid control algorithm is：

△ u (k)=K_p{[e(k)-e(k-1)]+K_Ie(k)+K_D[e(k)-2e(k-1)+e(k-2)]}

Wherein, computer export value during u (k) --- kth time sampling；Deviation during e (k) --- kth time sampling；

The difference of the computer of -1 sampling of Δ u (k) --- kth time sampling and kth；

When execution unit is without integrating unit, the digital quantity of its position and computer export is to correspond, using Position Form PID Control algolithm；When execution unit band integrating unit, from incremental timestamp algorithm；

The traveling process on chassis has three phases, respectively boost phase, constant velocity stage and decelerating phase；In each stage The distance with target point will be calculated in real time, if reaching, target point just stops；In accelerating sections, the controlling party of even acceleration can be used Formula, but need what a maximal rate set first, when speed reaches maximal rate, then at the uniform velocity area, into after at the uniform velocity area, It need to detect whether to reach deceleration area, when entering deceleration area, the mode that deceleration area slows down is used from target range with slowing down The ratio between section length is multiplied by present speed, obtains a smooth attenuation process, and final speed can slowly reach zero；Actual mistake Cheng Zhong, it is contemplated that the requirement of time and prevent that motor from slowly running, when speed reduces to 10% maximal rate i.e. with this at the uniform velocity Speed is walked, and next section of path is stopped or directly enter in program until reaching target point.