CN113433820B - A control system of a six-rotor spherical robot and its trajectory control method - Google Patents

Abstract

The invention discloses a control system of a six-rotor spherical robot, which is characterized by comprising a position controller, an attitude controller, a control distributor and a spherical rolling dynamics module, wherein the position controller is connected with the control distributor through a control signal line; the position controller inputs the track waypoint and the current position information received by the sensor, calculates deviation, outputs expected rolling angular velocity and transmits the expected rolling angular velocity to the attitude controller; the attitude controller inputs the expected rolling angular speed and the angular speed data measured by the sensor, calculates the angular speed deviation, outputs the expected driving moment and transmits the expected driving moment to the control distributor; the control distributor resolves the expected driving torque into a control signal of the motor according to the spatial configuration of the motor arrangement in the six-rotor spherical robot and the current attitude information. The invention adopts the cascade PID algorithm to carry out the track control, solves the control problem of the six-rotor spherical robot, and realizes the complex track tracking and the accurate and stable control of the spherical robot.

Description

A control system of a six-rotor spherical robot and its trajectory control method

technical field

The invention relates to the technical field of spherical robot control, in particular to a control system of a six-rotor spherical robot and a speed position control method thereof.

Background technique

Most of the existing spherical robot control technologies are based on the gravity pendulum and drive wheel driving methods, and use the control strategy of linear rolling and yaw steering decoupling, which is difficult to achieve accurate and stable control effects for complex trajectory tracking problems.

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide a control system and a trajectory control method of a six-rotor spherical robot in view of the above-mentioned problems, so as to realize the precise and stable control of the six-rotor spherical robot.

The technical scheme adopted in the present invention is as follows:

The present invention is a control system of a six-rotor spherical robot, comprising a position controller, an attitude controller and a control distributor;

The position controller inputs the trajectory waypoint and the current position information received by the sensor, calculates the deviation, outputs the expected roll angular velocity, and transmits it to the attitude controller;

The attitude controller inputs the expected rolling angular velocity and the angular velocity data measured by the sensor, calculates the angular velocity deviation, outputs the expected driving torque, and transmits it to the control distributor;

The control distributor, according to the spatial configuration of the motor arrangement in the six-rotor spherical robot and the current attitude information, calculates the desired driving torque into the control signal of the motor.

Preferably, a spherical rolling dynamics module is also included, and the spherical rolling dynamics module outputs the position information, angular velocity data, and attitude information measured by the corresponding sensors to the position controller, the attitude controller, and the control distributor, respectively.

The invention also discloses a trajectory control method of a six-rotor spherical robot, which is based on the control system of the six-rotor spherical robot and adopts a cascade PID algorithm for trajectory control.

Preferably, the cascade PID algorithm specifically includes the following steps:

Step 1: Obtain the position information obtained by the trajectory waypoint and the position sensor as the first-level PID input, and use the PID algorithm to perform integral separation to obtain the desired rolling angular velocity;

Step 2: Take the expected rolling angular velocity and the measured angular velocity data as the input of the second-stage PID, calculate the angular velocity deviation, and output the expected driving torque;

Step 3: According to the spatial configuration of the motor arrangement in the six-rotor spherical robot and the detected current attitude information, the expected driving torque is calculated into the control signals of the six motors.

Preferably, the position information obtained by the position sensor in the first step is subjected to mean filtering processing and Kalman filtering processing in sequence.

Preferably, in the first step, the PID algorithm is used to perform integral separation to obtain the desired rolling angular velocity: use the product of the proportional coefficient and the derivative value of the input value to replace the original differential, and perform integral separation on the deviation differential, and output based on the calculation result. Desired roll angular velocity.

Preferably, the calculation formula of the expected rolling angular velocity is:

为期望滚动角速度。Among them, K _p is the proportional coefficient, K _I is the integral coefficient, K _D is the differential coefficient, e is the deviation between the desired position and the actual position,

is the desired roll angular velocity.

Preferably, the step 2 specifically includes: constructing the angular velocity control signal described in the inertial coordinate system through the PI controller

为期望滚动角速度，ω_e为传感器测得的角速度数据在惯性坐标系下的描述；所述ω_e的计算公式为：in,

is the expected rolling angular velocity, ω _e is the description of the angular velocity data measured by the sensor in the inertial coordinate system; the calculation formula of the ω _e is:

ω _e =R _b2e ω _B

θ、ψ是3个Euler角；Among them, ω _B is the description of the angular velocity data measured by the sensor in the body coordinate system as the reference frame, c represents cos, s represents sin,

θ, ψ are 3 Euler angles;

解算为机体坐标系下描述的驱动力矩控制信号

其中，I为惯性矩阵。The angular acceleration control signal described in the inertial coordinate system

The solution is the driving torque control signal described in the body coordinate system

Among them, I is the inertia matrix.

Preferably, the step 3 specifically includes: setting the rotational speed of the rotor _i ( _i =1, ₂ , _. =k _F ω _i ² , M _i =k _M ω _i ² , where k _F is the motor rotational force coefficient, and k _M is the motor rotational torque coefficient;

Design the control distributor as follows:

为机体坐标系下描述的驱动力矩控制信号，

为六个电机的期望滚动角速度。Among them, L is the wheelbase between the two motors arranged in the same direction,

is the driving torque control signal described in the body coordinate system,

is the desired roll angular velocity for the six motors.

Preferably, a torque-driven control distribution strategy is adopted in the third step, that is, two motors arranged on the same diameter take control signals of equal magnitude and in the same direction.

To sum up, due to the adoption of the above-mentioned technical solutions, the beneficial effects of the present invention are:

1. The present invention realizes the control of the six-rotor spherical robot. First, input the obtained trajectory waypoints and the position information obtained by the position sensor into the position controller to obtain the desired rolling angular velocity; then input the expected rolling angular velocity and the measured angular velocity data into the attitude controller, calculate the angular velocity deviation, and output the expected driving torque; control According to the spatial configuration of the motor arrangement in the six-rotor spherical robot and the detected current attitude information, the distributor calculates the expected driving torque into the control signals of the six motors, so as to drive the spherical robot to move according to the predetermined trajectory through the control of the motors.

2. The present invention simplifies the control model and reduces the difficulty of control. According to the spatial configuration of the motors, in order to avoid the excessively strong model coupling caused by the anti-torque moment generated by the rotor during the rotation process, the present invention adopts a torque-driven control distribution strategy, that is, the two motors arranged on the same diameter are of equal size. And the control signals in the same direction, under this control distribution strategy, the control signals of the 6 rotors can actually be equivalent to 3 speed control signals to achieve the effect of simplification.

3. The present invention designs a control strategy for the motor according to the spatial configuration of the motor arrangement in the six-rotor spherical robot. Through the control cooperation between the motors, different actions of the robot can be realized, and the six-rotor spherical robot can be accurately controlled.

Description of drawings

The invention will be described by way of example and with reference to the accompanying drawings, in which:

FIG. 1 is a schematic diagram of the overall structure of the six-rotor spherical robot on which the hardware platform of the present invention is based.

Figure 2 is a schematic diagram of the layout of motors and rotors in a six-rotor spherical robot.

3 is a schematic diagram of a control system of a six-rotor spherical robot in an embodiment of the present invention.

FIG. 4 is a schematic diagram of the inertial coordinate system and the body coordinate system established in the present invention.

FIG. 5 is a curve diagram of the trajectory control effect realized by the present invention.

Fig. 6 is the control effect diagram of the pitch channel in the actual test of the present invention.

Fig. 7 is the effect diagram of rolling channel control in the actual test of the present invention.

Marked in the figure: 1 is the spherical shell, 10 is the upper hemispherical shell, 11 is the lower hemispherical shell, 12 is the hole, 20 is the X-axis bracket, 21 is the Y-axis bracket, 22 is the Z-axis bracket, 40 is the lower motor, 41 is the upper motor, 42 is the right motor, 43 is the left motor, 44 is the front motor, 45 is the rear motor, 5 is the rotor, and 6 is the blade.

Detailed ways

All features disclosed in this specification, or all disclosed steps in a method or process, may be combined in any way except mutually exclusive features and/or steps.

Any feature disclosed in this specification (including any accompanying claims, abstract), unless expressly stated otherwise, may be replaced by other equivalent or alternative features serving a similar purpose. That is, unless expressly stated otherwise, each feature is but one example of a series of equivalent or similar features.

As shown in FIG. 1 , the shell of the six-rotor spherical robot is a spherical shell 1, and the spherical shell 1 has holes 12; in the embodiment, the hollow spherical shell is preferably hollowed out, which can facilitate the airflow of the blades. The spherical casing is divided into two hemispherical casings, namely an upper hemispherical casing 10 and a lower hemispherical casing 11, and the two hemispherical casings can be easily opened and merged.

A bracket is fixed in the spherical shell, and a driving mechanism for driving the spherical shell to roll forward, spin and balance left and right is installed on the bracket; the driving mechanism includes a motor and a rotor mounted on the motor. At least six drive mechanisms are installed on the bracket, and the six drive mechanisms include six motors and six rotors; the rotors include at least one blade. The bracket includes an X-axis bracket, a Y-axis bracket and a Z-axis bracket that pass through the same midpoint.

As shown in FIG. 2 , two ends of the X-axis support 20 , the Y-axis support 21 and the Z-axis support 22 are respectively symmetrically installed with drive mechanisms; the drive mechanisms include a motor and a rotor 5 mounted on the motor. Motors and rotors 5 are respectively installed on the X-axis brackets 20 , Y-axis brackets 21 and Z-axis brackets 22 in opposite directions. The upper motor 41 and the lower motor 40 are respectively installed on the X-axis bracket 20 , and the rotors 5 are installed on the upper motor 41 . , generate an upward force, the rotor 5 is installed on the lower motor 40 to generate a downward force, and by controlling the upper and lower motors, the left and right leveling of the spherical robot is realized; the Y-axis bracket 21 is respectively installed with a left motor 43 and a right motor 42. A rotor 5 is installed on the left motor 43 to generate a leftward force, and a rotor 5 is installed on the right motor 42 to generate a rightward force. By controlling the left and right motors, the rotation of the spherical robot is realized; on the Z-axis bracket 22 The front motor 44 and the rear motor 45 are respectively installed, the rotor 5 is installed on the front motor 44 to generate a forward force, and the rotor 5 is installed on the rear motor 45 to generate a backward force, and the spherical robot is realized by controlling the front and rear motors. rolls back and forth.

A camera is also installed in the spherical shell, which acts as a payload to detect the surrounding environment. An autopilot, an ESC, a power source, a locator and a task computer are also installed in the spherical shell; the autopilot is used to control the movement of the spherical robot; the ESC is used to convert the direct current provided by the power source into a drive The alternating current of the motor; the locator for collecting positioning information; the mission computer for collecting, processing and transmitting the load data, and realizing autonomous planning and re-planning tasks. The spherical shell is also equipped with data transmission and image transmission for communication with the ground station and image transmission.

As shown in FIG. 3 , the present invention provides a control system of a six-rotor spherical robot, including a position controller, an attitude controller, a control distributor and rigid body dynamics (spherical rolling dynamics module); the position controller, Input the trajectory waypoint and the current position information received by the sensor, calculate the deviation, output the expected roll angular velocity, and transmit it to the attitude controller; the attitude controller, input the expected roll angular velocity and the angular velocity data measured by the sensor, calculate the angular velocity deviation, Output the desired driving torque and transmit it to the control distributor; the control distributor, according to the spatial configuration of the motor arrangement in the six-rotor spherical robot and the current attitude information, calculates the desired driving torque into the control signal of the motor; so The spherical rolling dynamics module, respectively, outputs the position information, angular velocity data, and attitude information measured by the corresponding sensors to the position controller, the attitude controller, and the control distributor.

The invention provides a trajectory control method for a six-rotor spherical robot, comprising the following steps:

The waypoint data of the trajectory planner and the position information obtained by the position sensor are obtained as the first-stage input; the PID algorithm is used for integral separation, and the desired roll angular velocity is obtained through the position controller as the input of the second-stage PID;

Input the expected roll angular velocity and the angular velocity data measured by the gyroscope to the attitude controller, calculate the angular velocity deviation, output the expected driving torque, and transmit it to the control distributor;

The control distributor calculates the desired driving torque into the control signals of the six motors according to the spatial configuration of the motor arrangement and the current attitude information in the six-rotor spherical robot;

Based on the spherical rolling dynamics model, the spherical robot can achieve precise and stable control according to the predetermined trajectory under the synergy of the six rotors.

In an embodiment, a trajectory control method for a six-rotor spherical robot is disclosed, comprising the following steps:

S101: Acquire the waypoint data and the position information obtained by the position sensor as the first-stage input, use the PID algorithm to perform integral separation, and obtain the desired roll angular velocity through the position controller as the input of the second-stage PID;

Specifically, the waypoint data is obtained by discretizing a predetermined trajectory, and the target waypoint on each route segment switches to the tracking process of the next route segment when the current position crosses the route method plane where the waypoint is located. The position sensor uses the indoor optical motion capture system OptiTrack to obtain position information. Due to certain fluctuations, it needs to be average filtered; the processed data is subjected to Kalman filtering to make the position data more real and reliable. Then, the desired roll angular velocity is controlled by the discretized position PID algorithm, and the integral separation is used to avoid integral saturation. The calculation formula is:

为位置控制器计算得到的期望滚动角速度；Among them, K _p is the proportional coefficient, K _I is the integral coefficient, K _D is the differential coefficient, e is the deviation between the desired position and the actual position,

the desired roll angular velocity calculated for the position controller;

S102: Input the expected roll angular velocity and the angular velocity data measured by the gyroscope to the attitude controller, calculate the angular velocity deviation, output the expected driving torque, and transmit it to the control distributor;

θ、ψ。Specifically, the spherical robot can be simplified as an underactuated rolling sphere with two orthogonal inputs. As shown in Figure 4, the inertial coordinate system OXYZ and the body coordinate system of the spherical robot are established with the motion origin O and the center point C as the origin respectively. CX _B Y _B Z _B , you can choose 5 parameters to describe the pure rolling motion of the spherical robot along a smooth horizontal plane: the coordinates (x, y) of the center C and the 3 Euler angles:

θ, ψ.

陀螺仪传感器测得的角速度数据是以飞控本身所在的机体坐标系CX_BY_BZ_B为参考系，记为ω_B；通过PI控制器构造角加速度控制信号

The expected rolling angular velocity data calculated by the position controller is based on the inertial coordinate system OXYZ as the reference system, which is recorded as

The angular velocity data measured by the gyroscope sensor is based on the body coordinate system CX _B Y _B Z _B where the flight control itself is located, and is denoted as ω _B ; the angular acceleration control signal is constructed by the PI controller.

为位置控制器计算得到的期望滚动角速度，ω_e为陀螺仪传感器测得的角速度数据在惯性坐标系下的描述，其计算公式为：in,

is the expected rolling angular velocity calculated by the position controller, ω _e is the description of the angular velocity data measured by the gyro sensor in the inertial coordinate system, and its calculation formula is:

ω _e =R _b2e ω _B

Among them, c stands for cos, s stands for sin.

According to the kinematic characteristics of the sphere, the angular acceleration control signal described in the inertial coordinate system is solved into the driving torque control signal described in the body coordinate system

Among them, I is the inertia matrix;

S103: The control distributor calculates the desired driving torque into control signals of the six motors according to the spatial configuration of the motor arrangement in the six-rotor spherical robot and the current attitude information;

Specifically, the control assignment is performed according to the motor space arrangement configuration of the six-rotor spherical robot shown in FIGS. 1 and 2 . For the six-rotor spherical robot, our control method is to control the rotation speed of the six rotors. First, analyze the effect of the rotational speed ω, assuming that the rotational speed of the rotor i (i=1,2,...,6) is expected to reach ω _i , then its effect can decompose the lift F _i and the rotational torque in the direction of the motor axis M _i , whose relation is:

F _i =k _F ω _i ²

M _i =k _M ω _i ²

Among them, k _F is the motor rotational force coefficient, and k _M is the motor rotational torque coefficient.

According to the spatial configuration of the motors, in order to avoid the excessively strong model coupling caused by the anti-torque moment generated by the rotor during the rotation process, the present invention adopts a torque-driven control distribution strategy, that is, the two motors arranged on the same diameter are of equal size. And the control signals in the same direction, under this control distribution strategy, the control signals of the six rotors can actually be equivalent to three speed control signals. The control distributor is designed as follows:

Among them, L is the wheelbase between the two motors arranged in the same direction.

As shown in FIG. 5 , the trajectory control effect achieved by the present invention in the actual test is very good, and the control accuracy of the spherical robot is improved. As shown in FIG. 6 , by comparing the pitch input angular rate and the actual angular rate, the difference between the two is relatively small, which proves that the present invention has a good pitch channel control effect in the actual test. As shown in FIG. 7 , by comparing the roll input angular rate with the actual angular rate, the two are relatively close, indicating that the present invention has a good effect of rolling channel control in the actual test. Through the actual test, the test result proves that under the control of the trajectory control method of the present invention, the six-rotor spherical robot can realize precise and stable control according to the predetermined trajectory.

The present invention is not limited to the foregoing specific embodiments. The present invention extends to any new features or any new combination disclosed in this specification, as well as any new method or process steps or any new combination disclosed.

Claims (8)

1. a trajectory control method of a six-rotor spherical robot, is characterized in that, adopts cascade PID algorithm to carry out trajectory control; Described cascade PID algorithm specifically comprises the following steps: Step 1: Obtain the position information obtained by the trajectory waypoint and the position sensor as the first-level PID input, and use the PID algorithm to perform integral separation to obtain the desired rolling angular velocity; Step 2: Take the expected rolling angular velocity and the measured angular velocity data as the input of the second-stage PID, calculate the angular velocity deviation, and output the expected driving torque; Step 3: According to the spatial configuration of the motor arrangement in the six-rotor spherical robot and the detected current attitude information, the expected driving torque is calculated into the control signals of the six motors; The step 2 specifically includes: constructing the angular velocity control signal described in the inertial coordinate system through the PI controller

in,

is the expected rolling angular velocity, ω _e is the description of the angular velocity data measured by the sensor in the inertial coordinate system; the calculation formula of the ω _e is: ω _e =R _b2e ω _B

Among them, ω _B is the description of the angular velocity data measured by the sensor in the body coordinate system as the reference frame, c represents cos, s represents sin,

θ, ψ are 3 Euler angles; The angular acceleration control signal described in the inertial coordinate system

The solution is the driving torque control signal described in the body coordinate system

Among them, I is the inertia matrix. 2 . The trajectory control method of a six-rotor spherical robot according to claim 1 , wherein the position information obtained by the position sensor in the step 1 is subjected to mean filter processing and Kalman filter processing in turn. 3 . 3. the trajectory control method of the six-rotor spherical robot according to claim 1 and 2, is characterized in that, utilizes PID algorithm to carry out integral separation in described step 1, obtains the method for expected rolling angular velocity: utilize proportional coefficient and input value to find The product of derivative values replaces the original differential, and the deviation differential is integrated and separated, and the expected roll angular velocity is output based on the calculation result. 4. the trajectory control method of the six-rotor spherical robot according to claim 3, is characterized in that, the calculation formula of expected rolling angular velocity is:

Among them, K _p is the proportional coefficient, k _I is the integral coefficient, k _D is the differential coefficient, e is the deviation between the desired position and the actual position,

is the desired roll angular velocity. 5. The trajectory control method of a six-rotor spherical robot according to claim 1, wherein the step 3 specifically comprises: setting the rotational speed of the rotor i to reach ω _i , i=1, 2, . . . , 6, and the effect It is decomposed into the lift force F _i and the rotational torque M _i in the direction of the motor axis, F _i =k _F ω _i ² , M _i =k _M ω _i ² , where k _F is the motor rotational force coefficient, and k _M is the motor rotational torque coefficient; Design the control distributor as follows:

Among them, L is the wheelbase between the two motors arranged in the same direction,

is the driving torque control signal described in the body coordinate system,

is the desired roll angular velocity for the six motors. 6. The trajectory control method of the six-rotor spherical robot according to claim 1, characterized in that, in the step 3, a torque-driven control distribution strategy is adopted, that is, two motors arranged on the same diameter are of equal size and in the same direction. control signal. 7. A control system of a six-rotor spherical robot, characterized in that, adopting the trajectory control method of the six-rotor spherical robot as claimed in one of claims 1-6, comprising a position controller, an attitude controller, and a control distributor; The position controller inputs the trajectory waypoint and the current position information received by the sensor, calculates the deviation, outputs the expected roll angular velocity, and transmits it to the attitude controller; The attitude controller inputs the expected rolling angular velocity and the angular velocity data measured by the sensor, calculates the angular velocity deviation, outputs the expected driving torque, and transmits it to the control distributor; The control distributor, according to the spatial configuration of the motor arrangement in the six-rotor spherical robot and the current attitude information, calculates the desired driving torque into the control signal of the motor. 8. The control system of the six-rotor spherical robot according to claim 7, further comprising a spherical rolling dynamics module, wherein the spherical rolling dynamics module is directed to a position controller, an attitude controller, and a control distributor respectively. , and output the position information, angular velocity data, and attitude information measured by the corresponding sensor.

Images