Rover ackermann module

[chfriedrich98]

Solved Problem

This PR adds a new module specifically for ackermann steering, continuing the rover overhaul started with #22402.

Solution

The setup requires the following geometric data for the specific rover in use:

Parameter	Description	Unit
AD_WHEEL_BASE	Wheel base of the rover	m
AD_MAX_STR_ANG	Maximum steering angle of the rover	deg

The module is structured as follows:

The commands that are sent to the actuators are generated in AckermannDriveControl from a AckermannDriveSetpoint.
This setpoint is an ackermann specific message that includes the following:

Variable	Description
float speed	Requested ground speed
float steering	Requested steering angle
bool manual	Manual control flag

This setpoint can be generated from manual inputs or a mission plan.

Manual mode
In AckermannDrive manual inputs from a controller are directly converted to a AckermannDriveSetpoint. For further development an "assisted" or "stabilized" manual mode could include features such as rate control and roll prevention.

Mission mode
AckermannDriveGuidance implements a path following algorithm called Pure pursuit.
The controller takes the intersection point between a circle around the vehicle and the line segment connecting the previous and current waypoint. The radius of the circle around the vehicle is used to tune the controller and is often referred to as look-ahead distance.

The required steering angle is calculated such that the vehicle reaches the aforementioned intersection point:

$$ \delta = \arctan\left( \frac{2Lsin(\alpha )}{l_d}\right) $$

Symbol	Description	Unit
$\delta$	Steering angle	rad
$L$	Wheelbase	m
$\alpha$	Heading error	rad
$l_d$	Lookahead distance	m

A great illustration for the derivation of this equation can be found at https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PurePursuit.html.
The look ahead distance sets how aggressive the controller behaves and is defined as follows:

$$ l_d = v \cdot k $$

It depends on the velocity $v$ of the rover and a tuning parameter $k$ that can be set with the parameter AD_LOOKAHEAD_TUN.
To deal with the edge case that the line segment is outside the look ahead radius around the rover there are 2 steps:

1. The look ahead is scaled to the crosstrack error, which creates an intersection point for the rover to drive towards.
2. Scaling the look ahead too much can have undesirable effects such as the rover driving a huge circle to return to the path, rather then taking a straight line. For this purpose the look ahead scaling can be capped at a certain value (AD_LOOKAHEAD_MAX), after which the rover will drive directly towards the current waypoint until it gets close enough to the line segment again.

To summarize, the following parameters can be used to tune the controller:

Parameter	Description	Unit
AD_LOOKAHEAD_TUN	Main tuning parameter	-
AD_LOOKAHEAD_MAX	Maximum value for the look ahead radius	m
AD_LOOKAHEAD_MIN	Minimum value for the look ahead radius	m

To enable a smooth trajectory, the acceptance radius of waypoints is scaled based on the angle between a line segment from the current-to-previous and current-to-next waypoints. The ideal trajectory would be to arrive at the next line segment with the heading pointing towards the next waypoint. For this purpose the minimum turning circle of the rover is inscribed tangentially to both line segments.

The acceptance radius of the waypoint is set to the distance from the waypoint to the tangential points between the circle and the line segments:

$$ \begin{align} r_{min} &= \frac{L}{\sin\left( \delta_{max}\right) } \\ \theta &= \frac{1}{2}\arccos\left( \frac{\vec{a}*\vec{b}}{|\vec{a}||\vec{b}|}\right) \\ r_{acc} &= \frac{r_{min}}{\tan\left( \theta\right) } \end{align} $$

Symbol	Description	Unit
$\vec{a}$	Vector from current to previous WP	m
$\vec{b}$	Vector from current to next WP	m
$r_{min}$	Minimum turn radius	m
$\delta_{max}$	Maximum steer angle	m
$r_{acc}$	Acceptance radius	m

The scaling of the acceptance radius causes the rover to "cut corners" to achieve a smooth trajectory. The degree to which corner cutting is allowed can be tuned, or disabled, with the following parameters:

Parameter	Description	Unit
AD_ACC_RAD_DEF	Default acceptance radius	m
AD_ACC_RAD_MAX	Maximum radius the acceptance radius can be scaled to	m
AD_ACC_RAD_TUN	Tuning parameter	-

The tuning parameter is a multiplicand on the calculated ideal acceptance radius to account for dynamic effects.

To smoothen the trajectory further and reduce the risk of the rover rolling over, the mission speed is reduced while the rover is within the acceptance radius. This is achieved by multiplying the inverse of the acceptance radius with a tuning parameter. This value is constrained between a minimum allowed speed and the default mission speed.
This effect can be tuned, or disabled, with the following parameters:

Parameter	Description	Unit
AD_MISS_VEL_DEF	Default mission speed	m/s
AD_MISS_VEL_MIN	Minimum the speed can be reduced to during cornering	m/s
AD_MISS_VEL_TUN	Tuning parameter	-

Lastly, a PI controller regulates the speed of the rover in AckermannDriveControl and can be tuned with the following parameters:

Parameter	Description	Unit
AD_SPEED_P	Proportional gain for ground speed controller	-
AD_SPEED_I	Integral gain for ground speed controller	-

Changelog Entry

For release notes:

Feature: Introduced AckermannDrive module

Alternatives

Open to any suggestions.

Test coverage

• SITL tested
• HITL tested (Hardware: https://www.axialadventure.com/product/1-10-scx10-ii-trail-honcho-4wd-rock-crawler-brushed-rtr/AXID9059.html)

Context

ackermann_rover_sitl.mp4

[PerFrivik]

Please check the Clang Tidy errors 👍 , otherwise this look super promising!

[PerFrivik]

Very nice! Did you check if it still works with the gazebo-classic rover?

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/rover-ackermann-oscillates-on-a-straight-track/37797/2

[Zugsepp]

Hi @chfriedrich98 ,
your module looks very interesting! I would like to test it on our AGV with a Pixhawk 6X. I added the module to the appropriate default.pxboard file, but unfortunately I receive an error when I try to make a build. With adding the differential drive module the build is successful. Nevertheless, the parameters of the module (AD_WHEEL_BASE, etc.) are not shown in the list in QGC. I tested with the generic ackermann airframe.
If you have a solution, I can provide you some logs for optimization from hardware tests.
I also opened a topic in the PX4 forum (https://discuss.px4.io/t/rover-ackermann-oscillates-on-a-straight-track/37797/4). Maybe that is a better way to communicate, as you like!

[chfriedrich98]

Hi @chfriedrich98 , your module looks very interesting! I would like to test it on our AGV with a Pixhawk 6X. I added the module to the appropriate default.pxboard file, but unfortunately I receive an error when I try to make a build. With adding the differential drive module the build is successful. Nevertheless, the parameters of the module (AD_WHEEL_BASE, etc.) are not shown in the list in QGC. I tested with the generic ackermann airframe. If you have a solution, I can provide you some logs for optimization from hardware tests. I also opened a topic in the PX4 forum (https://discuss.px4.io/t/rover-ackermann-oscillates-on-a-straight-track/37797/4). Maybe that is a better way to communicate, as you like!

Hi @Zugsepp,
I would love to see some hardware tests from different rovers, hopefully we can get yours up and running!
The PX4 forum seems like a good option, I provided a possible solution for you there.

[MaEtUgR]

Not fully through yet but those are my comments so far.