... | @@ -195,16 +195,38 @@ The following diagram shows the very simplified feedback loop of the controlled |
... | @@ -195,16 +195,38 @@ The following diagram shows the very simplified feedback loop of the controlled |
|
ctrl-->px4-->dyn-->|e|ctrl
|
|
ctrl-->px4-->dyn-->|e|ctrl
|
|
```
|
|
```
|
|
|
|
|
|
The `landing_controller` commands a velocity to a cascaded controller in the PX4 Firmware. This generates the command for the motors, which serve as input for the drone dynamics process. The output of this process is the position of the drone, relative to the aruco marker, which is the input for the `landing_controller`. To access the overall dynamics, the PX4 controller and the drone dynamics are approximated by a PT1 element:
|
|
The `landing_controller` commands a velocity to a cascaded controller in the PX4 Firmware. This generates the command for the motors, which serve as input for the drone dynamics process. The output of this process is the position of the drone, relative to the aruco marker, which is the input for the `landing_controller`. To access the overall dynamics, the PX4 controller and the drone dynamics are approximated by a PT1 element with delay:
|
|
|
|
|
|
$ F_{eu} = \frac{1}{T\cdot s + 1} $
|
|
```math
|
|
|
|
F_{sys} = \frac{1}{T_{sys} s + 1} \cdot e^{-T_{sys,delay}s}
|
|
|
|
```
|
|
|
|
|
|
|
|
The values for $`T_{sys}`$ and $`T_{sys,delay}`$ can be determined by experiments: Command a velocity using the `velocity_commander` and log the resulting velocity of the drone. The result for a custom drone in a gazebo simulation is shown below.
|
|
|
|
|
|
|
|
![system_identification.svg](uploads/d5d5ce6a9ea4211dd58ab6df5463846d/system_identification.svg)
|
|
|
|
|
|
|
|
The value of $`T_{sys,delay}`$ is the time between the step command and the first reaction of the velocity response (here $´0.25s´$) and $`T_{sys}`$ is the time from the first response until $`63%`$ of the final value is reached (here $´0.65s´$). The PT1 approximation with delay can now be simulated with exactly the same input. The result (yellow line) shows, that it is a very good representation of the dynamics. \
|
|
|
|
The transfer function now serves as the basis for tuning the PI controller (see [Controller](#controller)).
|
|
|
|
|
|
- Identification as PT1 element
|
|
- Identification as PT1 element
|
|
### Determine control deviation e
|
|
### Determine control deviation e
|
|
- internal tf tree
|
|
- internal tf tree
|
|
- determining error e
|
|
- determining error e
|
|
|
|
|
|
### Controller
|
|
### Controller
|
|
- PI controller
|
|
For the landing controller, a PI controller with the following transfer function is used:
|
|
|
|
|
|
|
|
```math
|
|
|
|
F_{ctrl} = \frac{K_I}{s} + K_P = \frac{K_I + K_Ps}{s}
|
|
|
|
```
|
|
|
|
|
|
|
|
Thus, the open control loop is determined by:
|
|
|
|
```math
|
|
|
|
L = \frac{1}{T_{sys} s + 1} \cdot \frac{K_I + K_Ps}{s} \cdot e^{-T_{sys,delay}s}
|
|
|
|
```
|
|
|
|
|
|
|
|
When the lag element is discarded, the gains $`K_I`$ and $`K_P`$ can now be calculated by a generic controller design method (e.g. [pole placement](https://de.mathworks.com/help/control/ref/place.html)). \
|
|
|
|
In the `velocity_controller` node, the controller gains are stored in the dictionary `gains`, which is a class variable. It contains the keys "P" and "I". The node subscribes to the topics which are stored under the TOPICS class variable (currently `"/landing_controller/p_gain"` and `"/landing_controller/i_gain"`). If a float is published to these topics, the local controller gains are overwritten with these values.
|
|
|
|
|
|
## Velocity Commander
|
|
## Velocity Commander
|
|
-> no arming nor changing mode |
|
-> no arming nor changing mode |
|
|
|
\ No newline at end of file |