CN113843790B - Robot, control method thereof, and readable storage medium - Google Patents
Robot, control method thereof, and readable storage medium Download PDFInfo
- Publication number
- CN113843790B CN113843790B CN202111067173.1A CN202111067173A CN113843790B CN 113843790 B CN113843790 B CN 113843790B CN 202111067173 A CN202111067173 A CN 202111067173A CN 113843790 B CN113843790 B CN 113843790B
- Authority
- CN
- China
- Prior art keywords
- robot
- speed
- output value
- driving wheels
- determining
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/60—Other road transportation technologies with climate change mitigation effect
- Y02T10/72—Electric energy management in electromobility
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a control method of a robot, which comprises the following steps: acquiring a current running instruction and attitude parameters of a robot; determining a target control quantity according to the operation instruction and the attitude parameter; and determining a PWM value of a motor of the robot according to the sum of the target control quantity and the speed output value set in the operation instruction so as to control the operation of the robot. The invention also discloses a robot and a readable storage medium. The invention ensures that the robot can keep stable balance under various movement modes.
Description
Technical Field
The present invention relates to the field of robots, and in particular, to a robot, a control method thereof, and a readable storage medium.
Background
With the progress of society and the development of network technology, more and more forms of robots are generated, and in recent years, spherical robots are receiving more and more attention. The spherical robot is a robot with a driving system positioned in a spherical shell (or a sphere) and realizing the movement of the sphere in an internal driving mode, and the robot has good dynamic and static balance and good sealing property, so that the robot can run in a severe environment without people, sand and dust, humidity and corrosiveness. The method can be applied to the fields of planetary detection, environment monitoring, national defense equipment, entertainment and the like.
The spherical robot has a structure that two driving wheels are connected through a main body in the middle, and the movement form of the spherical robot, such as linear movement, curve movement and the like, is controlled by controlling the running speed of the two driving wheels. In the related art, the operation control method of the spherical robot is to control the rotation speeds of the two driving wheels according to the set speed in the operation instruction after the operation instruction is acquired through Bluetooth, so that the spherical robot operates according to the instruction action. However, the spherical robot controlled to operate by the control method has a problem of poor balance.
Disclosure of Invention
The invention mainly aims to provide a robot, a control method thereof and a readable storage medium, and aims to ensure that the robot can keep stable balance under various movement modes.
In order to achieve the above object, the present invention provides a control method of a robot, the robot including a main body and two driving wheels rotatably provided at both sides of the main body, respectively; the control method of the robot comprises the following steps:
acquiring a current running instruction and attitude parameters of the robot;
determining a target control amount according to the running instruction and the attitude parameter;
and determining a PWM value of a motor of the robot according to the sum of the target control quantity and the speed output value set in the running instruction so as to control the running of the robot.
Optionally, the running instruction comprises a stop instruction, a straight running instruction and a steering instruction; the step of determining the target control amount according to the operation instruction includes:
if the running instruction is a stop instruction or a straight running instruction, acquiring the attitude parameter of the robot, and determining a target control amount according to the attitude parameter and the running instruction;
and if the running instruction is a steering instruction, the target control quantity is 0.
Optionally, the posture parameters include a current inclination angle, an inclination angle speed, a Z-axis angular speed of the main body, and rotational speeds of the two driving wheels; the step of determining a target control amount according to the attitude parameter and the operation instruction includes:
determining an angle loop output value for maintaining balance of the body based on the tilt angle and the tilt angle speed;
when the running instruction is a straight running instruction, determining a straight running ring output value for maintaining synchronous rotation of the two driving wheels according to the rotating speeds of the two driving wheels and the Z-axis angular speed;
the target control quantity is the sum of the angle ring output value and the straight ring output value;
when the running instruction is a stop instruction, determining a speed ring output value for accelerating the speed reduction of the driving wheels according to the rotating speeds of the two driving wheels;
the target control amount is a sum of the angle loop output value and the speed loop output value.
Optionally, the step of determining an angle loop output value for maintaining balance of the body according to the tilt angle and the tilt angle speed includes:
at intervals of a first time, the inclination angle Roll and the inclination angle speed gx are acquired regularly;
and determining the angle ring output value Out_A by adopting a proportional differential closed-loop control algorithm according to the inclination angle and the inclination angle speed, wherein a calculation formula of Out_A=roll_AP+gx_AD is Out_A, wherein AP is an angle proportion parameter, and AD is an angular speed proportion parameter.
Optionally, the step of determining an angle loop output value out_a for maintaining the balance of the body according to the tilt angle and the tilt angle speed further comprises:
judging whether the inclination angle Roll exceeds a first threshold angle or not;
if so, the angle ring output value is out_a=roll_ap+gx+ad+ (angle-Roll) AP.
Optionally, the step of determining the output value of the straight running ring for maintaining the synchronous rotation of the two driving wheels according to the rotation speeds of the two driving wheels and the Z-axis angular velocity includes:
the second time is spaced, the rotating speeds of the two driving wheels and the Z-axis angular speed gz of the robot are obtained at fixed time, and the rotating speeds of the two driving wheels are left_v and right_v respectively;
and determining the output value out_T of the straight-going ring by adopting a proportional differential closed-loop control algorithm according to the rotating speed of the driving wheel and the Z-axis angular speed, wherein a calculation formula of out_T=TP (left_v-right_v) +TD gz, wherein TP is a straight-going speed proportional parameter, and TD is a steering angular speed proportional parameter.
Optionally, the step of determining a straight-going ring output value for maintaining synchronous rotation of the two driving wheels according to the rotational speeds of the two driving wheels and the Z-axis angular velocity further includes:
judging whether the Z-axis angular velocity exceeds a second threshold value;
if so, gz is 0.
Optionally, the step of determining a straight-going ring output value for maintaining synchronous rotation of the two driving wheels according to the rotational speeds of the two driving wheels and the Z-axis angular velocity further includes:
judging whether the rotation speeds of the two driving wheels are both larger than a third threshold value;
if not, out_T is 0.
Optionally, the step of determining a speed loop output value for accelerating the deceleration of the drive wheels based on the rotational speeds of the two drive wheels comprises:
the rotation speeds of the two driving wheels are acquired at regular time intervals at third time, wherein the rotation speeds are left_v and right_v respectively;
determining the speed loop output value out_s according to the rotation speed of the driving wheel by adopting a proportional closed-loop control algorithm, wherein the calculation formula of the out_s is out_s=vp_v_old ', wherein v_old ' = (left_v+right_v) ×a+v_old_b, v_old is the previous value of v_old ', VP is a speed proportion parameter, 0 < a < 1,0 < B < 1, and a+b=1.
In addition, in order to achieve the above object, the present invention also provides a robot including a memory, a processor, and an operation control program of the robot stored on the memory and operable on the processor, the processor implementing the steps of the control method of the robot as described above when executing the operation control program of the robot.
In addition, in order to achieve the above object, the present invention also provides a readable storage medium having stored thereon an operation control program of a robot, which when executed by a processor, implements the steps of the control method of the robot as described above.
In the embodiment of the invention, the current running instruction and the attitude parameters of the robot are obtained; determining a target control quantity according to the operation instruction and the attitude parameter; the sum of the target control quantity and the speed output value set in the running instruction is used as the PWM value of the motor of the robot, so that the robot can be controlled to reach an equilibrium state, the possible deviation in the running process of the robot is eliminated, the running state of the robot accords with the running instruction, and the accuracy of a running path is ensured.
Drawings
FIG. 1 is a schematic diagram of a robot architecture for a hardware operating environment according to an embodiment of the present invention;
FIG. 2 is a flow chart of a first embodiment of a control method of the robot of the present invention;
FIG. 3 is a flow chart of a second embodiment of a control method of the robot of the present invention;
FIG. 4 is a flow chart of a third embodiment of a control method of the robot of the present invention;
FIG. 5 is a flow chart of a fourth embodiment of a control method of the robot of the present invention;
FIG. 6 is a flowchart of a fifth embodiment of a control method of a robot according to the present invention;
FIG. 7 is a flowchart of a control method of a robot according to a sixth embodiment of the present invention;
FIG. 8 is a flowchart of a seventh embodiment of a control method of a robot according to the present invention;
FIG. 9 is a flowchart of an eighth embodiment of a control method of a robot according to the present invention;
fig. 10 is a schematic flow chart of a ninth embodiment of a control method of the robot according to the present invention;
fig. 11 is a flowchart illustrating a control method of a robot according to a tenth embodiment of the present invention.
The achievement of the objects, functional features and advantages of the present invention will be further described with reference to the accompanying drawings, in conjunction with the embodiments.
Detailed Description
It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
The main solution of the invention is as follows: acquiring a current running instruction and attitude parameters of a robot; determining a target control quantity according to the running instruction and the attitude parameters of the robot; and determining a PWM value of a motor of the robot according to the sum of the target control quantity and the speed output value set in the running instruction so as to control the running state of the robot to accord with the running instruction.
The existing robot directly controls the operation of the driving wheel according to the set speed in the operation command, so that the problem of poor balance exists. Thus, the above solution proposed by the present invention aims to improve the running stability of the robot.
Referring to fig. 1, fig. 1 is a schematic structural diagram of a wearable device of a hardware running environment according to an embodiment of the present invention.
As shown in fig. 1, the robot may include: a communication bus 1002, a processor 1001, such as a CPU, a user interface 1003, a network interface 1004, a memory 1005. Wherein the communication bus 1002 is used to enable connected communication between these components. The user interface 1003 may include a Display, an input unit such as a Keyboard (Keyboard), and the optional user interface 1003 may further include a standard wired interface, a wireless interface. The network interface 1004 may optionally include a standard wired interface, a wireless interface (e.g., WI-FI interface). The memory 1005 may be a high-speed RAM memory or a stable memory (non-volatile memory), such as a disk memory. The memory 1005 may also optionally be a storage device separate from the processor 1001 described above.
Those skilled in the art will appreciate that the robot configuration shown in fig. 1 is not limiting of the robot and may include more or fewer components than shown, or may combine certain components, or may be arranged in different components.
In the robot shown in fig. 1, the network interface 1004 is mainly used for connecting to a background server and performing data communication with the background server; the user interface 1003 is mainly used for connecting a client (user side) and performing data communication with the client; and the processor 1001 may be used to call a control program of the robot stored in the memory 1005 and perform the relevant steps of the respective embodiments of the control method of the robot described below.
Referring to fig. 2, fig. 2 is a flowchart of a first embodiment of a control method of a robot according to the present invention, and in this embodiment, the control method of the robot includes the following steps:
step S10: acquiring a current running instruction and attitude parameters of the robot;
the control method of the robot provided by the embodiment is applied to the robot. Wherein, the robot can be a spherical robot, a balance trolley and the like; the robot comprises a main body and two driving wheels which are respectively arranged on two sides of the main body in a rotating mode, and the driving wheels are driven to rotate through motors. The running instruction can be sent by user operation, the way of the robot receiving the running instruction comprises but is not limited to Bluetooth, infrared, wiFi and the like, and the running instruction can comprise but is not limited to a straight running instruction, a stopping instruction and a steering instruction, wherein the straight running instruction can be a forward running instruction or a backward running instruction; the steering command may be a left turn command, a right turn command, a left turn command, or a right turn command. When the running command is a steering command, a speed difference of the two driving wheels, and a duration of the speed difference, may be set. The steering of the robot is achieved by making the output speeds of the two drive wheels different, i.e. there is a speed difference in the output speeds of the two drive wheels. The duration time of the speed difference is controlled, so that the holding time of the steering action can be realized, and the walking of the robot in different postures can be realized. When the running command is a straight running command, the speed difference of the two driving wheels is 0.
Meanwhile, the running instruction can also comprise a set speed, for example, a user can select a speed gear (a high gear corresponds to a high speed, a middle gear corresponds to a medium speed, and a low gear corresponds to a low speed) or the user can set a straight running speed by himself; when the user selects the stop instruction, the set speed is zero. The corresponding set parameter in the running instruction is the target parameter.
The robot is in an upright state in a stop state, and at the moment, the robot is a balance point, and when the robot runs, the main body can incline forwards or backwards, namely a certain inclination angle is generated.
The robot is provided with a gesture detection module, and gesture parameters of the robot can be detected through the gesture detection module. The attitude parameters comprise speed parameters and angle parameters of the robot, the attitude detection module comprises a gyroscope and an encoder, the angle parameters are detected through the gyroscope and an acceleration sensor, the angle parameters refer to the angle condition of the robot in an earth coordinate system, and the angle parameters comprise an inclination angle and an inclination angle speed. The tilt angle of the robot, that is, the tilt angle of the body may be a roll angle (rotation about the X axis) or a pitch angle (rotation about the Y axis), and the tilt angle speed may be an angular speed of the X axis or an angular speed of the Y axis, and the Z axis angular speed may be measured when the robot is steered as a whole. The angular velocity signals on the two driving wheels are measured by using a gyroscope, the acceleration signals of the main body are obtained by using an acceleration sensor, and then the data obtained by the gyroscope and the acceleration sensor are fused in a complementary filtering mode to obtain the inclination angle and the inclination angle velocity. At the same time, the speed parameter is detected by the encoder. The two driving wheels are respectively and correspondingly provided with an encoder for detecting the rotating speeds of the two driving wheels.
After the robot is initialized, the speed parameter and the angle parameter of the current robot are continuously received, and after an operation instruction is received, the target parameter is obtained.
Step S20: and determining a target control quantity according to the running instruction and the attitude parameter.
Step S30: and determining a PWM value of a motor of the robot according to the sum of the target control quantity and the speed output value set in the running instruction so as to control the running of the robot.
The robot is provided with a closed-loop control system, and different closed-loop control methods are provided for different gesture parameters. After receiving the operation command, the corresponding target control quantity can be calculated according to the operation command and the gesture parameter by a corresponding closed-loop control method, and the target control quantity is a dynamic value, so that the running of the robot can be controlled by the PWM value of the dynamic motor, and the self-adaptive control of the robot can be realized.
Specifically, the current state of the main body can be judged through the inclination angle and the inclination angle speed, and in order to keep the balance of the main body, a target control amount can be determined to perform fine adjustment control on the inclination angle of the main body, so that the robot cannot generate larger deviation and cannot turn over or have unstable speed.
Because the running path of the robot is controlled by the rotating speeds of the two driving wheels, the current state of the driving wheels can be judged by the rotating speeds of the two driving wheels, and in order to ensure that the running path of the robot and a preset route cannot deviate greatly, a target control amount can be determined to finely control the speeds of the two driving wheels so as to ensure that the rotating speeds of the two driving wheels cannot deviate greatly. When the running instruction is taken as a straight running instruction, the speed difference of the two driving wheels is zero through the fine adjustment control of the target control quantity on the speeds of the two driving wheels, and the running track of the robot can be ensured to be a straight line.
The embodiment obtains the current running instruction and the attitude parameters of the robot; determining a target control quantity according to the operation instruction and the attitude parameter; the sum of the target control quantity and the speed output value set in the running instruction is used as the PWM value of the motor of the robot, so that the robot can be controlled to reach an equilibrium state, the possible deviation in the running process of the robot is eliminated, the running state of the robot accords with the running instruction, and the accuracy of a running path is ensured.
A second embodiment of the control method of the robot of the present invention is proposed based on the above-described embodiment. Referring to fig. 3, in this embodiment, the step of determining the target control amount according to the operation instruction includes:
step S21: if the running instruction is a stop instruction or a straight running instruction, executing and determining a target control amount according to the attitude parameter and the running instruction;
step S22: and if the running instruction is a steering instruction, determining that the target control quantity is 0.
It can be understood that when the robot receives the steering instruction, the speed difference of the two driving wheels can be controlled to realize the steering of the robot, and the main body cannot influence the steering control process because the inclination angle of the main body is not greatly influenced in the steering process, so that the influence on the steering process is very tiny even if the target control quantity is determined to perform fine adjustment control on the robot according to the attitude parameter and the running instruction. Therefore, when the running instruction is a steering instruction, the target control amount is determined to be 0. The PWM value of the motor can be directly used as the PWM value of the motor according to the speed output value set in the running instruction, so that the control process is simplified, unnecessary operation is reduced, and idle work is avoided.
A third embodiment of the control method of the robot of the present invention is proposed based on the above-described embodiment. Referring to fig. 4, in this embodiment, the step of determining the target control amount according to the attitude parameter and the operation instruction includes:
step S211: determining an angle loop output value for maintaining balance of the body based on the tilt angle and the tilt angle speed;
step S212: when the running instruction is a straight running instruction, determining a straight running ring output value for maintaining synchronous rotation of the two driving wheels according to the rotating speeds of the two driving wheels and the Z-axis angular speed; the target control quantity is the sum of the angle ring output value and the straight ring output value;
step S213: when the running instruction is a stop instruction, determining a speed ring output value for accelerating the speed reduction of the driving wheels according to the rotating speeds of the two driving wheels; the target control amount is a sum of the angle loop output value and the speed loop output value.
It can be understood that when the robot receives the straight running command, the balance stability of the main body needs to be controlled and the speed difference of the two driving wheels is zero. And acquiring the rotating speeds of the two driving wheels and the Z-axis angular speed in real time through a closed-loop control system to acquire a target control quantity for controlling the robot to move straight. Whether the driving wheels are synchronous and whether the main body turns or not is judged by whether the speed difference of the two driving wheels reaches zero and whether the Z-axis angular speed is equal to zero. When the speed difference is not zero, a straight-going loop output value for controlling the speed difference is determined to control the speed difference of the two driving wheels. And meanwhile, an angle ring output value for controlling the inclination angle of the main body is obtained according to the inclination angle and the inclination angle speed through a closed-loop control system. And then, superposing the output value of the straight-going ring, the output value of the angle ring and the set speed output value to obtain the PWM value of the motor, so that the aim that the main body can keep balance in the straight-going process is fulfilled. The main body keeps balanced, so that the rolling or shaking of the main body can be prevented from influencing the rotating speed of the driving wheel. Meanwhile, the speed difference of the two driving wheels is not zero, the angle parameter of the main body can be influenced by the speed parameter of the driving wheels, so that the speed difference is eliminated to be the stable influence of the main body in the process of controlling the inclination angle of the main body, and the stable influence of the main body can be avoided when the speed difference of the two driving wheels is zero.
When the robot receives the stop command, the speed output value set in the stop command is zero, and the rotation speed of the driving wheel needs to be reduced to zero from the current rotation speed. The output value of the speed ring for controlling the rotation speed of the driving wheel is obtained through the closed-loop control system so as to enable the driving wheel to be stably decelerated, and meanwhile, the balance stability of the main body is required to be ensured in the process of controlling the rotation speed of the driving wheel, so that the output value of the angle ring for controlling the inclination angle of the main body is required to be obtained through the closed-loop control system according to the inclination angle and the inclination angle speed. And then, superposing the speed ring output value, the angle ring output value and the set speed output value to obtain the PWM value of the motor, so that the aim that the main body can keep balance in the speed reduction process is fulfilled. The main body keeps balance, so that the rolling or shaking of the main body can be prevented from influencing the speed reduction process of the driving wheel. Meanwhile, the balance speed reduction of the driving wheel can ensure that the stability of the main body is not influenced.
A fourth embodiment of the control method of the robot of the present invention is proposed based on the above-described embodiment. Referring to fig. 5, in the present embodiment, the step of determining an angle loop output value for maintaining balance of the body according to the tilt angle and the tilt angle speed includes:
step S2111: at intervals of a first time, the inclination angle Roll and the inclination angle speed gx are acquired regularly;
step S2112: and determining the angle ring output value Out_A by adopting a proportional differential closed-loop control algorithm according to the inclination angle and the inclination angle speed, wherein a calculation formula of Out_A=roll_AP+gx_AD is Out_A, wherein AP is an angle proportion parameter, and AD is an angular speed proportion parameter.
It can be understood that the update time interval of the angle ring output value out_a is the first time, the angle ring output value out_a calculated by the proportional differential closed-loop control algorithm is overlapped with the set speed output value, and then fine adjustment control can be performed on the tilting posture of the main body, so that the stability of the main body is ensured.
A fifth embodiment of the control method of the robot of the present invention is proposed based on the above-described embodiment. Referring to fig. 6, in this embodiment, the step of determining an angle loop output value out_a for maintaining the balance of the main body according to the tilt angle and the tilt angle speed further includes:
step S2113: judging whether the inclination angle Roll exceeds a first threshold angle or not;
step S2114: if so, the angle ring output value is out_a=roll_ap+gx+ad+ (angle-Roll) AP.
When the robot moves, the inclination angle of the main body forward and backward is not easy to be too large, and if the inclination angle is too large, the driving force of the motor is insufficient. For example, when the robot advances on the ground with high friction, the backward inclination angle of the main body may be excessively large, and the speed of the robot is affected, so that the motor is difficult to drive the robot to move.
In order to solve the above problem, a threshold value of the inclination angle, i.e. a first threshold value, is set so that the inclination angle of the main body is within a certain range, and when the inclination angle Roll is smaller than the first threshold angle, the angle loop output value is out_a=roll×ap+gx×ad. When the tilt angle Roll exceeds the first threshold angle, a compensating force add_v, add_v= (angle-Roll) AP is provided by the motor, i.e. the angle loop output value is out_a=roll ap+gx ad+ (angle-Roll) AP.
Specifically, when the robot advances, the main body is tilted backward, the tilt angle Roll of the main body is negative in the earth coordinate system, the first threshold of the tilt angle is defined as run_angle when the robot advances, and when the tilt angle Roll < run_angle, that is, the tilt angle Roll exceeds the first threshold, a supplementary force add_v= (run_angle-Roll) = (AP) is provided;
when the robot is retreating, the main body is tilted forward, the tilt angle Roll of the main body is positive in the earth coordinate system, the first threshold of the tilt angle is defined as back_angle when the robot is retreating, and when the tilt angle Roll > back_angle, that is, the tilt angle Roll exceeds the first threshold, a supplementary force add_v= (run_angle-Roll) AP is provided.
It should be noted that, when the operation command of the carriage is a stop command, it may not be determined whether or not the inclination angle Roll exceeds the first threshold, that is, when the operation command is a stop command, the angle loop output value is out_a=roll×ap+gx×ad. This is because the stop command itself requires that the robot stop, on the one hand, if the tilt angle exceeds the first threshold, causing the robot to stop faster, and, on the other hand, if the speed of the robot is decreasing during the stop, the tilt angle of the body gradually decreases, typically not exceeding the first threshold.
A sixth embodiment of the control method of the robot of the present invention is proposed based on the above-described embodiment. Referring to fig. 7, in this embodiment, the step of determining the output value of the straight running ring for maintaining the synchronous rotation of the two driving wheels according to the rotational speeds of the two driving wheels and the Z-axis angular velocity includes:
step S2121: the second time is spaced, the rotating speeds of the two driving wheels and the Z-axis angular speed gz of the robot are obtained at fixed time, and the rotating speeds of the two driving wheels are left_v and right_v respectively;
step S2121: and determining the output value out_T of the straight-going ring by adopting a proportional differential closed-loop control algorithm according to the rotating speed of the driving wheel and the Z-axis angular speed, wherein a calculation formula of out_T=TP (left_v-right_v) +TD gz, wherein TP is a straight-going speed proportional parameter, and TD is a steering angular speed proportional parameter.
It will be appreciated that the straight travel command requires that the speed differential of the two drive wheels be zero and that the robot not turn. The update time interval of the straight-going loop output value Out_T is the second time, the straight-going loop output value Out_T calculated through a proportional differential closed-loop control algorithm is overlapped with the set speed output value, and then fine adjustment control can be carried Out on the rotating speeds of the two driving wheels and the steering of the robot, so that the synchronous rotation of the two driving wheels and the steering are ensured, the fact that the steering does not occur is ensured, the straight line is kept when the robot moves forwards or backwards is ensured, and the situation that the two driving wheels have errors in speed in actual movement to cause a curved path is prevented.
A seventh embodiment of the control method of the robot of the present invention is proposed based on the above-described embodiment. Referring to fig. 8, in this embodiment, the step of determining the output value of the straight running ring for maintaining the synchronous rotation of the two driving wheels according to the rotational speeds of the two driving wheels and the Z-axis angular velocity further includes:
step S2123: judging whether the Z-axis angular velocity exceeds a second threshold value;
step S2124: if so, gz is 0.
In general, when the robot performs the straight-line command, the Z-axis angular velocity will not be too high, if the Z-axis angular velocity exceeds the second threshold, it is considered that an abnormal situation occurs in the robot itself, for example, the robot may be directly toggled by a person to turn, or an abnormality occurs in the sensor, and if the operation of the robot is controlled according to the gz of the abnormal situation, it is likely that an operation deviation occurs in the robot, so that in order to avoid the above problem, when the Z-axis angular velocity exceeds the second threshold, the Z-axis angular velocity gz is directly determined as 0, and a larger deviation in the output value of the straight-line loop due to the abnormal situation can be eliminated, that is, the influence on the PWM value of the motor can be eliminated.
An eighth embodiment of the control method of the robot of the present invention is presented based on the above-described embodiment. Referring to fig. 9, in this embodiment, the step of determining the output value of the straight running ring for maintaining the synchronous rotation of the two driving wheels according to the rotational speeds of the two driving wheels and the Z-axis angular velocity further includes:
step S2125: judging whether the rotation speeds of the two driving wheels are both larger than a third threshold value;
step S2126: if not, out_T is 0.
It should be noted that, when the rotation speeds of the two driving wheels are small, the speed difference is very small, if the closed loop control system performs the straight loop control and the angle loop control at the same time, the robot may shake, so if the rotation speed of the driving wheels is less than the third threshold value, the output value of the straight loop is 0, that is, when the running speed of the robot is reduced to be less than the third threshold value, the robot only performs the angle loop control, that is, only superimposes the output value of the angle loop to the set speed output value, so as to control the balance of the robot.
Meanwhile, the rotation speeds of the two driving wheels are required to be larger than the third threshold value in order to eliminate the abnormal condition of one driving wheel, such as no starting or being jammed.
A ninth embodiment of the control method of the robot of the present invention is proposed based on the above-described embodiment. Referring to fig. 10, in this embodiment, the step of determining the output value of the speed loop for accelerating the deceleration of the driving wheels according to the rotational speeds of the two driving wheels includes:
step S2131: the rotation speeds of the two driving wheels are acquired at regular time intervals at third time, wherein the rotation speeds are left_v and right_v respectively;
step S2132: determining the speed loop output value out_s according to the rotation speed of the driving wheel by adopting a proportional closed-loop control algorithm, wherein the calculation formula of the out_s is out_s=vp_v_old ', wherein v_old ' = (left_v+right_v) ×a+v_old_b, v_old is the previous value of v_old ', VP is a speed proportion parameter, 0 < a < 1,0 < B < 1, and a+b=1.
It can be understood that the update time interval of the speed loop output value out_s is the third time, and after the speed loop output value out_s calculated by the proportional closed-loop control algorithm is overlapped with the set speed output value, the running speed of the robot can be finely tuned to ensure that the robot can balance and decelerate.
Where a and B are parameters of the filter, 0 < a < 1,0 < B < 1, e.g. a=0.7, b=0.3.
The initial value of V_old in the proportional closed-loop control algorithm is 0.
A tenth embodiment of the control method of the robot of the present invention is presented based on the above-described embodiment. Referring to fig. 11, in this embodiment, after initialization, attitude parameters (including angle parameters and speed parameters) of the robot are received at regular time, and if no external operation command is received, the robot is controlled to remain stationary, i.e., the robot is in an upright state. In order to maintain the robot in a balanced state at this time, the inclined posture of the PD control main body and the posture of the P controller control driving wheel may be maintained in an initialized state.
After receiving an external operation instruction, calculating a PWM output value OUT of the motor, wherein the calculation formula is as follows: out=out_a+out_s+out-t+blue_speed, where blue_speed is the steering speed output value set in the steering command.
Specifically, if it is a steering command, out_a+out_s+out_t=0, i.e., out=blue_speed;
if a straight-going instruction, out_s=0, i.e., out=out_a+out_t+blue_speed;
if the run instruction is a stop instruction, then out_t=0, i.e. out=out_a+out_s+blue_speed;
after the PWM output value OUT of the motor is acted on two driving wheels of the robot through the motor, the balance control of the robot can be realized.
In this embodiment, the update interval time of out_a is the first time, the update interval time of out_s is the third time, and the update interval time of Out-T is the second time, which are respectively greater than the first time, because the straight loop control may affect the angle loop control, in order to ensure the stability of the robot, the update interval time of the straight loop needs to be shorter than the update time of the angle loop, for example, the first time may be 5ms, the second time and the third time may be 40ms, meaning that the update frequency of the PWM value of the motor is also 5ms once, that is, out_a is different every 8 times, but out_t and out_s are the same.
The robot includes a memory, a processor, and a robot operation control program stored on the memory and operable on the processor, the processor implementing the steps of the robot control method as described above when executing the robot operation control program.
In addition, the embodiment of the invention also provides a readable storage medium, wherein the readable storage medium is stored with a running control program of the robot, and the running control program of the robot realizes the steps of the control method of the robot when being executed by a processor.
It should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or system. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or system that comprises the element.
The foregoing embodiment numbers of the present invention are merely for the purpose of description, and do not represent the advantages or disadvantages of the embodiments.
From the above description of the embodiments, it will be clear to those skilled in the art that the above-described embodiment method may be implemented by means of software plus a necessary general hardware platform, but of course may also be implemented by means of hardware, but in many cases the former is a preferred embodiment. Based on this understanding, the technical solution of the present invention may be embodied essentially or in a part contributing to the prior art in the form of a software product stored in a storage medium (e.g. ROM/RAM, magnetic disk, optical disk) as described above, comprising instructions for causing a terminal device (which may be a mobile phone, a computer, a server, a television, or a network device, etc.) to perform the method according to the embodiments of the present invention.
The foregoing description is only of the preferred embodiments of the present invention, and is not intended to limit the scope of the invention, but rather is intended to cover any equivalents of the structures or equivalent processes disclosed herein or in the alternative, which may be employed directly or indirectly in other related arts.
Claims (9)
1. A control method of a robot comprises a main body and two driving wheels which are respectively and rotatably arranged at two sides of the main body; the control method of the robot is characterized by comprising the following steps of:
acquiring a current running instruction and attitude parameters of the robot;
determining a target control amount according to the running instruction and the attitude parameter;
determining a PWM value of a motor of the robot according to the sum of the target control quantity and the speed output value set in the operation instruction so as to control the operation of the robot;
the running instructions comprise a stop instruction, a straight running instruction and a steering instruction; the step of determining the target control amount according to the operation instruction includes:
if the running instruction is a stop instruction or a straight running instruction, executing and determining a target control amount according to the attitude parameter and the running instruction;
if the running instruction is a steering instruction, determining that the target control quantity is 0;
the attitude parameters comprise the current inclination angle, the inclination angle speed, the Z-axis angular speed and the rotating speeds of the two driving wheels of the main body; the step of determining a target control amount according to the attitude parameter and the operation instruction includes:
determining an angle loop output value for maintaining balance of the body based on the tilt angle and the tilt angle speed; the updating interval time of the angle ring output value is the first time;
when the running instruction is a straight running instruction, determining a straight running ring output value for maintaining synchronous rotation of the two driving wheels according to the rotating speeds of the two driving wheels and the Z-axis angular speed; the updating interval time of the execution ring output value is second time, and the second time is longer than the first time;
the target control quantity is the sum of the angle ring output value and the straight ring output value;
when the running instruction is a stop instruction, determining a speed ring output value for accelerating the speed reduction of the driving wheels according to the rotating speeds of the two driving wheels;
the target control amount is a sum of the angle loop output value and the speed loop output value.
2. The method of controlling a robot according to claim 1, wherein the step of determining an angle loop output value for maintaining balance of the body according to the tilt angle and the tilt angle speed comprises:
at intervals of a first time, the inclination angle Roll and the inclination angle speed gx are acquired regularly;
and determining the angle ring output value Out_A by adopting a proportional differential closed-loop control algorithm according to the inclination angle and the inclination angle speed, wherein a calculation formula of Out_A=roll_AP+gx_AD is Out_A, wherein AP is an angle proportion parameter, and AD is an angular speed proportion parameter.
3. The method of controlling a robot according to claim 2, wherein the step of determining an angle loop output value out_a for maintaining the balance of the body according to the tilt angle and the tilt angle speed further comprises:
judging whether the inclination angle Roll exceeds a first threshold angle or not;
if so, the angle ring output value is out_a=roll_ap+gx+ad+ (angle-Roll) AP.
4. The method of controlling a robot according to claim 1, wherein the step of determining a straight traveling ring output value for maintaining the synchronous rotation of the two driving wheels according to the rotational speeds of the two driving wheels and the Z-axis angular velocity comprises:
the second time is spaced, the rotating speeds of the two driving wheels and the Z-axis angular speed gz of the robot are obtained at fixed time, and the rotating speeds of the two driving wheels are left_v and right_v respectively;
and determining the output value out_T of the straight-going ring by adopting a proportional differential closed-loop control algorithm according to the rotating speed of the driving wheel and the Z-axis angular speed, wherein a calculation formula of out_T=TP (left_v-right_v) +TD gz, wherein TP is a straight-going speed proportional parameter, and TD is a steering angular speed proportional parameter.
5. The method of controlling a robot according to claim 4, wherein the step of determining a straight traveling ring output value for maintaining the synchronous rotation of the two driving wheels according to the rotational speeds of the two driving wheels and the Z-axis angular velocity further comprises:
judging whether the Z-axis angular velocity exceeds a second threshold value;
if so, gz is 0.
6. The method of controlling a robot according to claim 4, wherein the step of determining a straight traveling ring output value for maintaining the synchronous rotation of the two driving wheels according to the rotational speeds of the two driving wheels and the Z-axis angular velocity further comprises:
judging whether the rotation speeds of the two driving wheels are both larger than a third threshold value;
if not, out_T is 0.
7. The method of controlling a robot according to claim 1, wherein the step of determining a speed loop output value for accelerating deceleration of the driving wheels according to rotational speeds of the two driving wheels includes:
the rotation speeds of the two driving wheels are acquired at regular time intervals at third time, wherein the rotation speeds are left_v and right_v respectively;
determining the speed loop output value out_s according to the rotation speed of the driving wheel by adopting a proportional closed-loop control algorithm, wherein the calculation formula of the out_s is out_s=vp_v_old ′ Wherein v_old' = (left_v+right_v) ×a+v_old×b, v_old is the previous v_old ′ VP is a speed ratio parameter, 0 < a < 1,0 < B < 1, a+b=1.
8. A robot comprising a memory, a processor and a robot operation control program stored on the memory and operable on the processor, the processor implementing the steps of the robot control method according to any one of claims 1-7 when executing the robot operation control program.
9. A readable storage medium, characterized in that the readable storage medium has stored thereon a robot operation control program, which when executed by a processor, implements the steps of the robot control method according to any one of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111067173.1A CN113843790B (en) | 2021-09-13 | 2021-09-13 | Robot, control method thereof, and readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111067173.1A CN113843790B (en) | 2021-09-13 | 2021-09-13 | Robot, control method thereof, and readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113843790A CN113843790A (en) | 2021-12-28 |
| CN113843790B true CN113843790B (en) | 2023-07-14 |
Family
ID=78973792
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111067173.1A Active CN113843790B (en) | 2021-09-13 | 2021-09-13 | Robot, control method thereof, and readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113843790B (en) |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006136962A (en) * | 2004-11-11 | 2006-06-01 | Hitachi Ltd | Mobile robot |
| CN101554726B (en) * | 2009-05-15 | 2011-01-19 | 北京工业大学 | Flexible two-wheel self-balance robot system and motion control method thereof |
| CN205679999U (en) * | 2016-06-22 | 2016-11-09 | 浙江机电职业技术学院 | A kind of balanced bi-wheel vehicle |
| CN108806689A (en) * | 2018-07-30 | 2018-11-13 | 苏州工业职业技术学院 | Acoustic control self-balancing trolley and its control method |
| CN109240305B (en) * | 2018-10-19 | 2022-02-22 | 广州大学华软软件学院 | Two-wheeled robot motion control system and method based on complementary filtering |
| CN110001840B (en) * | 2019-03-12 | 2021-01-01 | 浙江工业大学 | Two-wheeled self-balancing vehicle motion control method based on visual sensor under various road conditions |
-
2021
- 2021-09-13 CN CN202111067173.1A patent/CN113843790B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113843790A (en) | 2021-12-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8543294B2 (en) | Coaxial two-wheeled vehicle and its control method | |
| KR101156822B1 (en) | Mobile and control method of mobile | |
| US8352170B2 (en) | Traveling apparatus and method of controlling parallel two-wheeled vehicle | |
| JP2008059497A (en) | Mobile body and its control method | |
| US8751110B2 (en) | Inverted pendulum type vehicle | |
| US20140222267A1 (en) | Apparatus and methods for control of a vehicle | |
| US11975787B2 (en) | Control device for mobile body | |
| EP2083340B1 (en) | Inverted type movable body and control method thereof | |
| JP2011500423A (en) | Apparatus and method for controlling vehicle motion | |
| US8843271B2 (en) | Inverted pendulum type vehicle | |
| JPWO2007013282A1 (en) | Inverted two-wheeled traveling robot and control method thereof | |
| JP2007011634A (en) | Control method of mobile carriage and mobile carriage | |
| KR20130127372A (en) | Inverted pendulum type vehicle | |
| US9128488B2 (en) | Movement direction control apparatus and computer program | |
| JP2010082717A (en) | Inverted pendulum type movement mechanism | |
| CN113843790B (en) | Robot, control method thereof, and readable storage medium | |
| US10981617B2 (en) | Inverted pendulum type vehicle | |
| US8678124B2 (en) | Inverted pendulum type vehicle | |
| CN113031585B (en) | Control device for moving body | |
| US8874319B2 (en) | Inverted pendulum type vehicle | |
| US20210031855A1 (en) | Control device of a rideable mobile body | |
| JP2016088307A (en) | Inverted two-wheel type moving body |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |