Control method of wheeled robot with swingable trunk

Abstract

The invention relates to a control method of a wheeled robot with a swingable trunk, belongs to the technical field of robots, and solves the problem that in the prior art, a full-drive robot cannot continue to execute tasks when energy consumption is too fast or a driving system breaks down in a long-time detection task or a special operation environment. The robot comprises a wheel type driving chassis and a swingable trunk connected through a connecting rod. The method comprises the steps of obtaining a current driving mode of the robot; wherein the driving mode comprises a full-driving mode and a non-driving mode; when the robot is in a full-drive mode, based on the current motion state of the robot, a full-drive trunk control expectation state is obtained through calculation so as to drive the swingable trunk, and the swingable trunk and the wheel type drive chassis move cooperatively; and when the robot is in a non-driving mode, based on the current motion state of the robot, a non-driving trunk control expectation state is obtained through calculation, and only the swingable trunk is driven to drive the wheel type driving chassis to move.

Description

Technical Field

[0001] The invention relates to the technical field of robots, and in particular to a control method for a wheeled robot with a swingable trunk. Background Art

[0002] At present, with the rapid development of social economy, fires occur frequently, and the disasters brought to human beings are also escalating. Fire has become one of the disasters that seriously threaten human survival and development in the world today. Most places prone to fire are gathering places of flammable and explosive materials, which are at risk of explosion at any time. The high temperature, toxic and harmful gases and large-scale thick smoke generated at the fire scene seriously endanger the safety of firefighters. Therefore, the emergence of firefighting robots can effectively solve the difficulties at the above fire scene.

[0003] However, fire rescue missions usually require firefighting robots to work continuously in harsh environments such as high temperature and smoke. Existing firefighting robots generally rely on batteries for power supply, and the battery life is particularly weak under high load conditions. Although some solutions have tried to extend the working time by optimizing battery management and improving battery energy density, they have not been able to effectively solve the impact of high temperature environments on battery performance. Therefore, firefighting robots often cannot maintain sufficient working time during long rescue missions, resulting in the need for frequent charging or battery replacement, which reduces rescue efficiency. Summary of the invention

[0004] In view of the above analysis, an embodiment of the present invention aims to provide a control method for a wheeled robot with a swingable torso, so as to solve the problem that the existing full-drive robot cannot continue to perform the task when the energy is consumed too quickly or the drive system fails during long-term detection tasks or special working environments.

[0005] The purpose of the present invention is mainly achieved through the following technical solutions:

[0006] The present invention provides a control method for a wheeled robot with a swingable trunk, wherein the robot comprises a wheeled driving chassis and a swingable trunk connected by a connecting rod; the method comprises the following steps:

[0007] Acquire the current driving mode of the robot; wherein the driving mode includes a full driving mode and a non-driving mode;

[0008] When the robot is in full drive mode, based on the current motion state of the robot, a full drive trunk control desired state is calculated to drive the swingable trunk and move in coordination with the wheeled drive chassis;

[0009] When the robot is in the non-driven mode, the desired control state of the non-driven trunk is calculated based on the current motion state of the robot, and only the swingable trunk is driven to drive the wheeled drive chassis to move.

[0010] Further, the driving mode is obtained based on the current driving state of the robot; the driving state of the robot includes the battery power and health state of the robot chassis driving system;

[0011] When the battery power of the chassis driving system is higher than a preset value and the health status is normal, setting the driving mode of the robot to a full driving mode;

[0012] Otherwise, when the battery power of the chassis driving system is lower than a preset value, or the health status is abnormal, the driving mode of the robot is set to a no-driving mode.

[0013] Furthermore, when the robot is in full-drive mode, based on the current motion state of the robot, a desired state of full-drive trunk control is calculated to drive the swingable trunk and move in coordination with the wheeled drive chassis, including:

[0014] Based on the current motion state of the robot, obtaining the required power for driving the robot to move in a full-drive mode;

[0015] Based on the required power and the power preset by the wheel drive chassis, the auxiliary power of the swingable trunk in the full drive mode is calculated;

[0016] Based on the auxiliary power of the swingable trunk in the full-drive mode, calculating the desired state of full-drive trunk control;

[0017] Based on the desired control state of the fully driven trunk, a torque for driving the swingable trunk is obtained to control the movement of the swingable trunk;

[0018] The chassis is driven to move based on the preset power.

[0019] Further, the desired state of the full-drive trunk control includes a desired angle and a desired angular velocity of the swingable trunk;

[0020] When the desired state of the all-drive trunk control exceeds the state in which the swingable trunk can move, the preset power provided by the wheel-driven chassis is adjusted, and the desired state of the all-drive trunk control is calculated again.

[0021] Further, based on the desired state of the fully driven trunk control, a torque for driving the swingable trunk is obtained to control the movement of the swingable trunk, including:

[0022] Obtaining the current angle and angular velocity of the swingable torso;

[0023] Based on the current angle and angular velocity of the swingable trunk and the desired control state of the full-drive trunk, a model-based predictive control method is used to obtain the torque for driving the swingable trunk at the next moment.

[0024] Further, when the robot is in the full-drive mode, it is detected that the movement speed of the robot within a preset time is lower than a preset threshold, and it is determined that the robot encounters an obstacle;

[0025] Based on the degree of decrease in the movement speed of the robot, the desired state of the swingable trunk control is adjusted to enable the robot to cross the obstacle.

[0026] Further, adjusting the desired control state of the swingable trunk based on the decrease in the movement speed of the robot includes:

[0027] Based on the current movement speed and the expected movement speed of the robot, a movement speed difference is obtained;

[0028] Based on the movement speed difference, obtaining a difficulty level of crossing the road obstacle;

[0029] Based on the difficulty level, a swing angle and a swing frequency of the swingable trunk of the robot are adjusted to overcome obstacles.

[0030] Furthermore, obtaining the difficulty level of crossing the road obstacle based on the movement speed difference includes:

[0031] When the movement speed difference is less than the preset speed difference value of the low difficulty obstacle, the difficulty level is set to low difficulty;

[0032] When the movement speed difference is between the preset speed difference value for a low difficulty obstacle and the preset speed difference value for a medium difficulty obstacle, the difficulty level is set to medium difficulty;

[0033] When the movement speed difference is higher than the preset speed difference value of the medium difficulty obstacle, the difficulty level is set to high difficulty.

[0034] Furthermore, when the robot is in the non-driven mode, based on the current motion state of the robot, the desired control state of the non-driven trunk is calculated, and only the swingable trunk is driven to drive the wheeled drive chassis to move, including:

[0035] Based on the current motion state of the robot, the power required for the robot to move in the non-drive mode is obtained as the power of the swingable trunk in the non-drive mode;

[0036] Based on the dynamics of the swingable trunk in the non-driven mode, calculating the desired control state of the non-driven trunk;

[0037] Based on the desired control state of the undriven trunk, the torque for driving the swingable trunk is obtained to control the movement of the swingable trunk.

[0038] Further, the swingable torso is provided with a driving mechanism and a gyroscope;

[0039] The driving mechanism is used to generate a torque for driving the swingable trunk;

[0040] The gyroscope is used to obtain the angle and angular velocity of the swingable torso.

[0041] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0042] 1. The present invention designs and controls the dynamic swing of the robot trunk, so that the wheeled robot can achieve a backup walking function when the energy reserve is insufficient or the wheeled chassis drive is limited. This walking mode drives the robot forward through the swing of the trunk, reduces dependence on the wheeled chassis drive, thereby reducing energy consumption and extending working time.

[0043] 2. The trunk control of the present invention can be used together with the chassis drive system to improve the overall energy efficiency of the fire rescue wheeled robot with a trunk; during normal walking, the trunk control can assist the wheeled chassis drive system to reduce unnecessary energy consumption to improve walking efficiency. This combination can not only improve the walking speed and stability of the robot, but also extend the robot's working time in complex environments.

[0044] 3. The present invention enhances the robot's obstacle-crossing ability in complex terrain by flexibly adjusting the swing angle and frequency of the trunk. The swing of the trunk can provide additional power for the wheeled robot, helping it to cross obstacles, rubble piles, gravel areas and other uneven terrains, thereby preventing the robot from losing power or getting into trouble in difficult terrain.

[0045] 4. The present invention uses a model-based predictive control method to control trunk movement, which can predict and optimize control based on the system model, accurately adapt to the dynamic behavior of the system, and by predicting future states and control inputs, it can simultaneously process multiple control objectives and consider constraints. It is more flexible and efficient in tasks that require long-term predictions and strict constraints.

[0046] In the present invention, the above-mentioned technical solutions can also be combined with each other to achieve more preferred combination solutions. Other features and advantages of the present invention will be described in the subsequent description, and some advantages can become obvious from the description, or can be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0048] Figure 1 Schematic diagram of the structure of a wheeled robot with a swingable trunk in an embodiment of the present invention;

[0049] Figure 2 Schematic diagram of a control method for a wheeled robot with a swingable trunk according to an embodiment of the present invention;

[0050] Figure 3 This is a schematic diagram of the control flow of the robot in full drive mode according to an embodiment of the present invention;

[0051] Figure 4 Schematic diagram of the control flow of the robot in the non-driving mode according to an embodiment of the present invention. DETAILED DESCRIPTION

[0052] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0053] Wheeled robots mainly rely on wheeled chassis drive systems to achieve movement. This design has a high moving speed and good maneuverability in flat terrain, but it shows obvious limitations in the complex and dynamic environment of fire rescue sites. The ground at the fire scene is usually full of obstacles, such as collapsed buildings, debris, stairs, ramps and other irregular terrain. Traditional wheeled robots often find it difficult to cross obstacles in these environments, resulting in obstructed movement, reduced efficiency, and even loss of working ability due to inability to escape. At the same time, existing wheeled robots are also insufficient in terms of working time. Most robots rely on batteries for power, but in fire rescue missions, robots need to perform reconnaissance, search and fire extinguishing operations for a long time, and the battery life is difficult to meet the requirements of long-term missions. At the same time, complex terrain requires high energy consumption of the wheeled drive system, resulting in excessive battery energy consumption, further shortening the robot's working time.

[0054] A specific embodiment of the present invention discloses a control method for a wheeled robot with a swingable trunk, such as Figure 1 As shown, the robot includes a wheeled drive chassis and a swingable trunk connected by a connecting rod. The robot's obstacle surmounting ability and working time in complex terrain are effectively improved by the swinging of the trunk. The robot's center of gravity is adjusted by the periodic swinging of the trunk, thereby significantly enhancing the obstacle surmounting ability. The trunk assists in walking, reducing dependence on the chassis drive system, thereby reducing energy consumption and extending the robot's working time.

[0055] like Figure 2 As shown, the method includes the following steps S1-S3:

[0056] Step S1, obtaining the current driving mode of the robot; wherein the driving mode includes a full driving mode and a non-driving mode.

[0057] Furthermore, the driving mode is obtained based on the current driving state of the robot; the driving state of the robot includes the battery power and health state of the robot chassis driving system.

[0058] When the battery power of the chassis driving system is higher than a preset value and the health status is normal, the driving mode of the robot is set to a full driving mode.

[0059] Otherwise, when the battery power of the chassis driving system is lower than a preset value, or the health status is abnormal, the driving mode of the robot is set to a no-driving mode.

[0060] Specifically, in the full-drive mode, the robot uses wheel drive and trunk drive to perform tasks together. The trunk actively controls and assists the movement of the wheels to improve the maneuverability and stability of the robot in complex terrain. In this mode, the swing of the trunk reduces the robot's dependence on wheel drive by optimizing the distribution of force. In combination with the wheel drive, the movement of the trunk helps to improve the overall energy efficiency of the robot. By reducing the load of the wheel drive, the robot can obtain stronger movement performance at the same energy consumption.

[0061] When the robot can no longer rely on wheel drive, it enters the driverless mode. At this time, the robot relies only on the active swing of the trunk to achieve movement. In this mode, the robot controls the force generated by the swing of the trunk to achieve relatively low-speed walking, maintain basic mobility, and extend working time. In the driverless mode, the swing of the trunk becomes the main power source of the robot's movement. Through precise control, the swing of the trunk generates forward thrust and reaches the "gliding" state. The energy consumption of movement in this mode is extremely low, relying only on the mass and dynamic behavior of the trunk itself.

[0062] It should be noted that when the robot detects that the battery power of the chassis drive system is lower than the set threshold, for example, when the battery power is 10% of the full power, or the drive system of the chassis drive system fails, the system will automatically reduce the output of the electric drive and switch to the no-drive mode. At this time, the robot will rely on the swing of the trunk to continue walking to avoid completely stopping working. When the battery of the chassis drive system is fully charged or the drive system of the chassis drive system is repaired, the robot will automatically resume the full-drive mode. At this time, the control of the trunk continues to assist the wheel drive, enhance walking ability, and improve overall energy efficiency.

[0063] Step S2: When the robot is in full-drive mode, based on the current motion state of the robot, the full-drive trunk control desired state is calculated to drive the swingable trunk and move in coordination with the wheeled drive chassis, such as Figure 3 As shown, the steps S21 to S25 are included:

[0064] Step S21: based on the current motion state of the robot, obtain the required power for driving the robot to move in the full drive mode.

[0065] Specifically, the resistance that the robot needs to overcome when walking includes friction resistance in the horizontal direction and gravity in the vertical direction. Therefore, the total required power is the vector sum of the horizontal driving force and the vertical supporting force. Based on the speed sensor carried by the robot, the current movement speed v of the robot is obtained; based on the expected linear speed v required for the robot to perform the task desired and the expected acceleration a desired , the current movement speed v of the robot, and the required power of the robot movement are obtained using the following formula:

[0066]

[0067] F horizontal =m·a desired +μ·m·g

[0068] F vertical =m·g

[0069]

[0070] Among them, F total represents the required power for the robot's motion; F horizontal represents the horizontal driving force of the robot movement; F vertical represents the vertical driving force of the robot's movement; m represents the total mass of the robot; μ represents the friction resistance; g represents the acceleration of gravity; and t represents the preset speed time required to perform the task.

[0071] Step S22: Based on the required power and the power preset to be provided by the wheel-driven chassis, the auxiliary power of the swingable trunk in the full-drive mode is calculated.

[0072] Specifically, the total power required for the robot to move in the full-drive mode is distributed to the wheel-driven chassis and the swingable trunk to achieve joint driving.

[0073] Exemplarily, the power provided by the wheel drive chassis can be set to 30% of the required power.

[0074] Step S23: Calculate and obtain the desired state of full-drive trunk control based on the auxiliary power of the swingable trunk in the full-drive mode.

[0075] Furthermore, the desired state of the full-drive trunk control includes a desired angle and a desired angular velocity of the swingable trunk.

[0076] Specifically, based on the auxiliary power F of the swingable trunk in the full drive mode body , the connecting rod length L of the swingable trunk body According to the principle of dynamics, the desired torque τ acting on the swingable trunk is obtained using the following formula: ref :

[0077] τ ref =F body ·L body

[0078] Based on the desired torque τ of the swingable trunk body , the mass m of the swingable trunk body and the connecting rod length L of the swingable trunk body , the desired angle θ of the swingable torso is obtained using the following formula: ref :

[0079] τ ref = k·sin(θ ref )

[0080]

[0081] Based on the desired torque τ of the swingable trunk ref , the desired angular velocity ω of the swingable trunk is obtained using the following formula: ref :

[0082] ω ref =τ ref ·J body

[0083]

[0084] Among them, Jbody represents the moment of inertia of the swingable trunk.

[0085] Further, when the desired state of the all-drive trunk control exceeds the state in which the swingable trunk can move, the preset power provided by the wheel-driven chassis is adjusted, and the desired state of the all-drive trunk control is calculated again.

[0086] Specifically, when the desired state of the full-drive trunk control exceeds the state in which the swingable trunk can move, that is, the desired swing angle or angular velocity of the trunk exceeds its physical limit, it is impossible to provide sufficient auxiliary power through the swing of the trunk to meet the needs in the full-drive mode. At this time, it is necessary to adjust the preset power provided by the wheel-driven chassis and recalculate the desired state of the full-drive trunk control.

[0087] Step S24, based on the desired control state of the fully driven trunk, obtaining the torque for driving the swingable trunk and controlling the movement of the swingable trunk, comprising:

[0088] The current angle and angular velocity of the swingable torso are obtained.

[0089] Furthermore, the swingable torso is provided with a driving mechanism and a gyroscope; the driving mechanism is used to generate a torque for driving the swingable torso; and the gyroscope is used to obtain the angle and angular velocity of the swingable torso.

[0090] Specifically, the torque generated by the driving mechanism directly drives the swingable trunk to swing according to a predetermined trajectory and posture, and the driving mechanism can accurately control the swing angle of the swingable trunk according to the control instruction.

[0091] Furthermore, based on the current angle and angular velocity of the swingable trunk and the desired control state of the full-drive trunk, a model-based predictive control method is used to obtain the torque for driving the swingable trunk at the next moment.

[0092] Specifically, the model-based predictive control method is an advanced control strategy that establishes a mathematical model of the controlled object, predicts the output of the system in the future, and optimizes the control input on this basis to achieve precise control of the system behavior.

[0093] More specifically, the dynamics of the robot trunk swing is simplified to a simple pendulum model, and the relationship between the swing angle and the input torque is:

[0094]

[0095] Where c is the damping coefficient, θ is the swing angle, and τ is the input torque.

[0096] In order to facilitate control, the dynamic equations in the continuous time domain are discretized into the form in the discrete time domain. Define the state vector Among them, θ k represents the swing angle at the kth moment; represents the angular velocity at the kth moment, and the discretized dynamic equation is expressed as follows:

[0097]

[0098] Among them, T s Represents the discretized time step.

[0099] The discretized dynamic equation is converted into state space form using the following formula:

[0100] x k+1 =Ax k +Bu k +d(x k )

[0101]

[0102] In order for the robot torso angle to track the desired angle θ ref , define the following objective function:

[0103]

[0104] Among them, θ k+i represents the actual swing angle at the k+ith moment; τ k+i Represents the actual input torque at the k+ith moment; Q represents the weight of the state error in the objective function. For example, if the system has a low tolerance for swing angle errors and needs to accurately track the desired angle, Q can be set to a larger value, such as 10, otherwise it can be set to a smaller value, such as 0.1; R represents the weight of the control input torque in the objective function. For example, if the system has strict restrictions on the size of the control input torque and needs to avoid overloading the drive mechanism or reduce energy consumption, R can be set to a larger value, such as 10, otherwise it can be set to a smaller value, such as 0.1.

[0105] It should be noted that in order to ensure the physical feasibility of the swingable torso, the following constraints are set:

[0106] τ min ≤τ k+i ≤τ max ,θ min ≤θ k+i ≤θ max

[0107] Among them, τ min ,τ maxRespectively represent the minimum torque and maximum torque that the drive mechanism can output; θ min ,θ max They respectively represent the minimum swing angle and the maximum swing angle of the swingable torso.

[0108] The constraints can ensure that the control input torque and state variables are within the physical capabilities of the system, avoid overloading or damage to the actuator, and prevent the robot from entering an unsafe or unstable state.

[0109] Based on the above state space equation, the state recursion for the next N steps is as follows:

[0110] x k+i+1 =Ax k+i +Bu k+i +d(x k+i ),i=0,1,…,N-1

[0111] The swing angle and input torque of the swingable torso at all times are written in vector form as follows:

[0112] X=Sx k +TU+D

[0113] Where X represents the predicted swing angle vector, U represents the control input torque vector, S represents the state recursion coefficient matrix, which represents the matrix of how the current state affects the future state; T represents the torque state recursion coefficient matrix, which represents the matrix of how the current torque affects the future torque; D represents the cumulative effect of the nonlinear term, which represents the nonlinear term d(x k ) has a cumulative effect on the state in the prediction time domain by substituting the nonlinear term d(x k ) is accumulated in each time step.

[0114] Therefore, the objective function I can be rewritten as follows:

[0115]

[0116] Among them, X ref represents the desired swing angle vector, Q N represents the state error weight matrix; R N A weight matrix representing the control input torque.

[0117] By expressing X as a linear combination of U, the final objective function is expressed as follows:

[0118]

[0119] Among them, H is the quadratic coefficient matrix, which reflects the quadratic influence of the control input vector U on the objective function I. f is the first-order coefficient vector, which reflects the first-order influence of the control input vector U on the objective function I.

[0120] The optimal solution of the control input torque is obtained by solving the quadratic programming (QP) algorithm:

[0121]

[0122] The optimal solution The first control input torque in Applied to the system to achieve effective control of the robot's trunk swing angle.

[0123] Step S25: driving the chassis to move based on the preset power.

[0124] Furthermore, when the robot is in the full-drive mode, it is detected that the movement speed of the robot within a preset time is lower than a preset threshold, and it is determined that the robot encounters an obstacle.

[0125] Specifically, during the robot's walking process, it may encounter obstacles. At this time, obstacle detection is achieved through real-time monitoring of the robot's motion state. Based on the speed sensor carried by the robot, the robot's speed change is detected in real time. When the robot encounters an obstacle, the speed usually drops sharply, and the low-speed movement will continue for a period of time. The system determines whether an obstacle is encountered by setting a threshold for speed change. When the system determines that there is an obstacle ahead, the desired state of the trunk is adjusted so that the robot can cross the obstacle without increasing the power of the wheel-driven chassis as much as possible.

[0126] Further, based on the degree of decrease in the movement speed of the robot, the desired state of the swingable trunk control is adjusted to enable the robot to cross the obstacle, including steps S251 to S253:

[0127] Step S251: Obtain a movement speed difference based on the current movement speed and the expected movement speed of the robot.

[0128] Specifically, the current movement speed of the robot is obtained through a speed sensor carried by the robot, and the speed is subtracted from the expected movement speed of the robot preset by the system to obtain a value of the decrease in the current movement speed of the robot.

[0129] Step S252: Based on the movement speed difference, obtain the difficulty level of crossing the road obstacle.

[0130] Specifically, the speed drop values ​​are divided into different parts according to their sizes, corresponding to different obstacles. The greater the speed drop, the more difficult the obstacle is to cross. Therefore, the amplitude and frequency of the trunk swing are correspondingly enhanced at this time, thereby enhancing the influence of the trunk on the robot and helping it to successfully cross the obstacle.

[0131] Further, when the movement speed difference is less than the preset speed difference value of the low difficulty obstacle, the difficulty level is set to low difficulty;

[0132] When the movement speed difference is between the preset speed difference value for a low difficulty obstacle and the preset speed difference value for a medium difficulty obstacle, the difficulty level is set to medium difficulty;

[0133] When the movement speed difference is higher than the preset speed difference value of the medium difficulty obstacle, the difficulty level is set to high difficulty.

[0134] Specifically, based on the environment in which the task is performed, a preset speed difference for each difficulty obstacle is set at the initial stage of task execution. For example, the preset speed difference for the low difficulty obstacle is set to 1 m / s; and the preset speed difference for the medium difficulty obstacle is set to 3 m / s.

[0135] If the movement speed difference is less than the preset speed difference of the low difficulty obstacle, it means that the difference between the robot's current speed and the expected speed is small, the obstacle is a minor obstacle, and the robot can adjust the speed more easily to reach the expected state, so the difficulty level is set to low difficulty.

[0136] If the movement speed difference is greater than the preset speed difference of a low-difficulty obstacle, but less than the preset speed difference of a medium-difficulty obstacle, the obstacle is a medium obstacle, which means that the robot needs to make a certain degree of speed adjustment, but the adjustment range and difficulty are moderate, so the difficulty level is set to medium difficulty.

[0137] If the movement speed difference is greater than the preset speed difference of the medium difficulty obstacle, it means that the difference between the current speed of the robot and the expected speed is large, and the obstacle is a large obstacle, requiring a large speed adjustment, so the difficulty level is set to high difficulty.

[0138] Step S253: Based on the difficulty level, adjust the swing angle and the swing frequency of the swingable trunk of the robot to overcome the obstacle.

[0139] Specifically, set the amplitude gain factor K A and frequency gain factor K f , proportional to the difficulty level, based on the amplitude gain factor K A and the frequency gain factor K f , use the following formula to adjust the swing angle and swing frequency of the swingable torso at different difficulty levels:

[0140]

[0141] Among them, A body Indicates the swing angle of the torso after adjustment; A base Indicates the swing angle of the torso before adjustment; f body represents the swing frequency of the swingable trunk after adjustment; f base Indicates the swing frequency of the swingable torso before adjustment; represents the amplitude gain factor of the i-th level; Indicates the frequency gain factor of the i-th level, the first level is a low difficulty level, the second level is a medium difficulty level, and the third level is a high difficulty level.

[0142] For example, when the difficulty level is low (speed difference Δv<1m / s), the amplitude gain factor is set And the frequency gain factor When the difficulty level is medium (speed difference 1≤Δv<3m / s), set the amplitude gain factor And the frequency gain factor When the difficulty level is high (speed difference Δv ≥ 3m / s), set the amplitude gain factor And the frequency gain factor

[0143] When the swing angle and swing frequency of the current swingable trunk are A base =10°, f base =2Hz, and the speed drops by Δv=1.5m / s, which is a medium difficulty. It can be calculated that:

[0144] A body =10°·1.5=15°

[0145] f body =2Hz·1.3=2.6Hz

[0146] As a result, the swing amplitude and frequency of the swingable trunk are enhanced when the obstacle is a medium obstacle, which helps the robot to cross the obstacle, reduces the dependence on wheel drive, and ensures that the robot continues to walk.

[0147] Step S3: When the robot is in the non-driven mode, based on the current motion state of the robot, the desired control state of the non-driven trunk is calculated, and only the swingable trunk is driven to drive the wheeled drive chassis to move, such as Figure 4 As shown, it includes steps S31 to S33:

[0148] Step S31: based on the current motion state of the robot, the power required for the robot to move in the non-drive mode is obtained as the power for the swingable trunk in the non-drive mode.

[0149] Specifically, since the wheel-driven chassis does not provide power in the non-driving mode, the movement power of the robot in this mode is entirely provided by the swingable trunk.

[0150] Step S32: Based on the power of the swingable trunk in the non-driven mode, the desired control state of the non-driven trunk is calculated.

[0151] Specifically, the desired state of the undriven trunk control also includes the desired angle and the desired angular velocity of the swingable trunk. The method for calculating the desired state of the undriven trunk control based on the power of the swingable trunk in the undriven mode is the same as the method for calculating the desired state of the fully driven trunk control based on the auxiliary power of the swingable trunk in the fully driven mode in step S23, and will not be repeated here.

[0152] Step S33: based on the desired control state of the undriven trunk, obtain the torque for driving the swingable trunk to control the movement of the swingable trunk.

[0153] Specifically, the method for calculating the torque for driving the swingable trunk based on the desired state of the undriven trunk control is the same as the method for calculating the torque for driving the swingable trunk and controlling the movement of the swingable trunk in the full-drive mode of step S24, and will not be repeated here.

[0154] In summary, the control method of a wheeled robot with a swingable trunk according to an embodiment of the present invention has the following beneficial effects:

[0155] 1. The present invention designs and controls the dynamic swing of the robot trunk, so that the wheeled robot can achieve a backup walking function when the energy reserve is insufficient or the wheel drive is limited. This walking mode drives the robot forward through the swing of the trunk, reduces dependence on the wheel drive, thereby reducing energy consumption and extending working time.

[0156] 2. The trunk control of the present invention can be used together with the full drive system to improve the overall energy efficiency of the fire rescue wheeled robot with a trunk; during normal walking, the trunk control can assist the full drive system to reduce unnecessary energy consumption to improve walking efficiency. This combination can not only improve the walking speed and stability of the robot, but also extend the robot's working time in complex environments.

[0157] 3. The present invention enhances the robot's obstacle-crossing ability in complex terrain by flexibly adjusting the swing angle and frequency of the trunk. The swing of the trunk can provide additional power for the wheeled robot, helping it to cross obstacles, rubble piles, gravel areas and other uneven terrains, thereby preventing the robot from losing power or getting into trouble in difficult terrain.

[0158] 4. The present invention uses a model-based predictive control method to control trunk movement, which can predict and optimize control based on the system model, accurately adapt to the dynamic behavior of the system, and by predicting future states and control inputs, it can simultaneously process multiple control objectives and consider constraints. It is more flexible and efficient in tasks that require long-term predictions and strict constraints.

[0159] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A control method for a wheeled robot with a swingable trunk, characterized in that: The robot comprises a wheel-driven chassis and a swingable trunk connected by a connecting rod; the method comprises the following steps: Acquire the current driving mode of the robot; wherein the driving mode includes a full driving mode and a non-driving mode; When the robot is in full drive mode, based on the current motion state of the robot, a full drive trunk control desired state is calculated to drive the swingable trunk and move in coordination with the wheeled drive chassis; When the robot is in the non-driven mode, the desired control state of the non-driven trunk is calculated based on the current motion state of the robot, and only the swingable trunk is driven to drive the wheeled drive chassis to move.

2. The method according to claim 1, characterized in that: The driving mode is obtained based on the current driving state of the robot; the driving state of the robot includes the battery power and health state of the robot chassis driving system; When the battery power of the chassis driving system is higher than a preset value and the health status is normal, setting the driving mode of the robot to a full driving mode; Otherwise, when the battery power of the chassis driving system is lower than a preset value, or the health status is abnormal, the driving mode of the robot is set to a no-driving mode.

3. The method according to claim 2, characterized in that: When the robot is in the full-drive mode, based on the current motion state of the robot, the full-drive trunk control desired state is calculated to drive the swingable trunk to move in coordination with the wheeled drive chassis, including: Based on the current motion state of the robot, obtaining the required power for driving the robot to move in a full-drive mode; Based on the required power and the power preset by the wheel drive chassis, the auxiliary power of the swingable trunk in the full drive mode is calculated; Based on the auxiliary power of the swingable trunk in the full-drive mode, calculating the desired state of full-drive trunk control; Based on the desired control state of the fully driven trunk, a torque for driving the swingable trunk is obtained to control the movement of the swingable trunk; The chassis is driven to move based on the preset power.

4. The method according to claim 3, characterized in that: The fully-driven trunk control desired state includes a desired angle and a desired angular velocity of the swingable trunk; When the desired state of the all-drive trunk control exceeds the state in which the swingable trunk can move, the preset power provided by the wheel-driven chassis is adjusted, and the desired state of the all-drive trunk control is calculated again.

5. The method according to claim 4, characterized in that: Based on the desired control state of the fully driven trunk, a torque for driving the swingable trunk is obtained to control the movement of the swingable trunk, including: Obtaining the current angle and angular velocity of the swingable torso; Based on the current angle and angular velocity of the swingable trunk and the desired control state of the full-drive trunk, a model-based predictive control method is used to obtain the torque for driving the swingable trunk at the next moment.

6. The method according to claim 5, characterized in that: When the robot is in full-drive mode, it is detected that the movement speed of the robot within a preset time is lower than a preset threshold, and it is determined that the robot encounters an obstacle; Based on the degree of decrease in the movement speed of the robot, the desired state of the swingable trunk control is adjusted to enable the robot to cross the obstacle.

7. The method according to claim 6, characterized in that: The adjusting the desired control state of the swingable trunk based on the decrease degree of the movement speed of the robot comprises: Obtaining a movement speed difference based on a current movement speed and an expected movement speed of the robot; Based on the movement speed difference, obtaining a difficulty level of crossing the road obstacle; Based on the difficulty level, a swing angle and a swing frequency of the swingable trunk of the robot are adjusted to overcome obstacles.

8. The method according to claim 7, characterized in that: The difficulty level of crossing the road obstacle is obtained based on the movement speed difference, including: When the movement speed difference is less than the preset speed difference value of the low difficulty obstacle, the difficulty level is set to low difficulty; When the movement speed difference is between the preset speed difference value for a low difficulty obstacle and the preset speed difference value for a medium difficulty obstacle, the difficulty level is set to medium difficulty; When the movement speed difference is higher than the preset speed difference value of the medium difficulty obstacle, the difficulty level is set to high difficulty.

9. The method according to claim 2, characterized in that: When the robot is in the non-driven mode, based on the current motion state of the robot, the desired control state of the non-driven trunk is calculated, and only the swingable trunk is driven to drive the wheeled drive chassis to move, including: Based on the current motion state of the robot, the power required for the robot to move in the non-drive mode is obtained as the power of the swingable trunk in the non-drive mode; Based on the dynamics of the swingable trunk in the non-driven mode, calculating the desired control state of the non-driven trunk; Based on the desired control state of the undriven trunk, the torque for driving the swingable trunk is obtained to control the movement of the swingable trunk.

10. The method according to any one of claims 1 to 9, characterized in that: The swingable torso is provided with a driving mechanism and a gyroscope; The driving mechanism is used to generate a torque for driving the swingable trunk; The gyroscope is used to obtain the angle and angular velocity of the swingable torso.

Images