search

PX4 / PX4-Autopilot

PX4 Autopilot Software
https://px4.io
BSD 3-Clause "New" or "Revised" License
8.59k stars 13.56k forks source link

[Bug] Singularities in control allocation's pseudo inverse #23606

Open Donecle opened 3 months ago

Donecle commented 3 months ago

Describe the bug

While trying some new quad frame geometry parameters today, we had a bad crash at takeoff (2 front motors full throttle and 2 back motors idle).

To Reproduce

In simulation,

  1. choose any quad frame, e.g. iris
  2. set CA_ROTOR* parameters as in the flight log below
  3. print the result of the pseudo-inverse

Expected behavior

The geometry in the log is quite common and should result in a correct mixing matrix.

Screenshot / Media

No response

Flight Log

https://logs.px4.io/plot_app?log=4679a30d-9709-4950-b57f-9b47603b4fa3

Software Version

main, on August 23rd, 2023

Flight controller

px4_sitl

Vehicle type

Multicopter

How are the different components wired up (including port information)

Default

Additional context

No response

bresch commented 3 months ago

@Donecle Could you please show the params and resulting control allocation matrix directly here (copy or screenshot)?

Donecle commented 3 months ago

Sure.

x CA_ROTOR0_AX [123,242] : -0.0612 x CA_ROTOR0_AY [124,243] : 0.0000 x CA_ROTOR0_AZ [125,244] : -1.0000 x + CA_ROTOR0_PX [128,247] : 0.2410 x CA_ROTOR0_PY [129,248] : 0.2380 x CA_ROTOR1_PX [152,274] : -0.2240 x CA_ROTOR1_PY [153,275] : -0.2390 x + CA_ROTOR2_PX [160,283] : 0.2410 x + CA_ROTOR2_PY [161,284] : 0.2380 x CA_ROTOR3_PX [168,292] : -0.2240 x CA_ROTOR3_PY [169,293] : -0.2390

EFFECTIVENESS:

0|-1.52425 1.55350 -1.54700 1.55350 1| 1.56357 -1.45600 1.56650 -1.45600 2| 0.41893 0.32500 -0.32500 -0.32500 3|-0.39723 0 0 0
4| 0 0 0 0
5|-6.48785 -6.50000 -6.50000 -6.50000

MIX:

0|-4.6e+02 -5.3e+02 5.2e+02 -4.6e+01 0 -1.8e+04 1|-4.4e+02 -5.2e+02 5.2e+02 -4.2e+01 0 -1.8e+04 2|-4.0e+02 -4.8e+02 5.2e+02 -3.6e+01 0 -1.8e+04 3|-4.1e+02 -4.9e+02 5.2e+02 -4.5e+01 0 -1.8e+04

bresch commented 3 months ago

If I understand correctly, you have 2 pairs of coaxial rotors, which is quite weird. Maybe check if it fails at the pseudo-inverse level or if it's due to the normalization.

Donecle commented 3 months ago

Apologies, copy paste issues for the parameters, here are the correct ones: x CA_ROTOR0_AX [123,242] : -0.0612 x CA_ROTOR0_AY [124,243] : 0.0000 x CA_ROTOR0_AZ [125,244] : -1.0000

x CA_ROTOR0_PX [128,247] : 0.2410 x CA_ROTOR0_PY [129,248] : 0.2380 x CA_ROTOR1_PX [152,274] : -0.2240 x CA_ROTOR1_PY [153,275] : -0.2390 x CA_ROTOR2_PX [160,283] : 0.2410 x CA_ROTOR2_PY [161,284] : -0.2380 x CA_ROTOR3_PX [168,292] : -0.2240 x CA_ROTOR3_PY [169,293] : 0.2390

Thanks for your help!

Donecle commented 3 months ago

Nevermind, this is still not enough to reproduce, I'll give you the correct set later today.

bresch commented 3 months ago

@Donecle Maybe you could add your case in an unit test to debug it more easily if you can reproduce it: https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp

Donecle commented 3 months ago

Mmh I've never done this but I could try. Here is a set of param to change in startup script to have a failure:

param set-default CA_ROTOR_COUNT 4 param set-default CA_ROTOR0_PX 0.241 param set-default CA_ROTOR0_PY 0.238 param set-default CA_ROTOR0_AX -0.0616628 param set-default CA_ROTOR0_AY 0.0616628 param set-default CA_ROTOR0_AZ -0.996195 param set-default CA_ROTOR0_CT 36.0 param set-default CA_ROTOR0_KM 0.001632 param set-default CA_ROTOR1_PX -0.224 param set-default CA_ROTOR1_PY -0.238 param set-default CA_ROTOR1_AX 0.0616628 param set-default CA_ROTOR1_AY -0.0616628 param set-default CA_ROTOR1_AZ -0.996195 param set-default CA_ROTOR1_CT 36.0 param set-default CA_ROTOR1_KM 0.001632 param set-default CA_ROTOR2_PX 0.241 param set-default CA_ROTOR2_PY -0.238 param set-default CA_ROTOR2_AX -0.0616628 param set-default CA_ROTOR2_AY -0.0616628 param set-default CA_ROTOR2_AZ -0.996195 param set-default CA_ROTOR2_CT 36.0 param set-default CA_ROTOR2_KM -0.001632 param set-default CA_ROTOR3_PX -0.224 param set-default CA_ROTOR3_PY 0.238 param set-default CA_ROTOR3_AX 0.0616628 param set-default CA_ROTOR3_AY 0.0616628 param set-default CA_ROTOR3_AZ -0.996195 param set-default CA_ROTOR3_CT 36.0 param set-default CA_ROTOR3_KM -0.001632

EFFECTIVENESS: | 0 | 1 | 2 | 3 0|-8.53174 8.53174 8.53174 -8.53174 1| 8.63933 -8.02966 8.63933 -8.02966 2| 1.12184 1.08410 -1.12184 -1.08410 3|-2.21985 2.21985 -2.21985 2.21985 4| 2.21985 -2.21985 -2.21985 2.21985 5|-3.6e+01 -3.6e+01 -3.6e+01 -3.6e+01 MIX: | 0 | 1 | 2 | 3 | 4 | 5 0|-0.07477 -6.8e+03 0.22655 -1.2e+04 -0.37924 -5.3e+06 1|-0.02525 -7.1e+03 0.22655 -1.3e+04 -0.41406 -5.3e+06 2|-0.02083 -6.8e+03 -0.22678 -1.2e+04 -0.39331 -5.3e+06 3|-0.08107 -7.1e+03 -0.22678 -1.3e+04 -0.39950 -5.3e+06

Donecle commented 3 months ago

Here's a flight log on this setup : https://logs.px4.io/plot_app?log=86e11219-3fcc-46c5-8068-f377ecfd2419

Donecle commented 3 months ago

Looks like the fullRankCholesky function founds a rank of 5 instead of 4 on this param set.

Donecle commented 3 months ago

If I force 10 times higher value here : https://github.com/PX4/PX4-Autopilot/blob/main/src/lib/matrix/matrix/PseudoInverse.hpp#L83, it founds a rank of 4 as it should. This looks like a numerical issue.