BB.NSK.Sensor.Lean (bb_nsk v0.1.2)

Copy Markdown View Source

Reports how far the body has fallen away from upright, as the :lean joint's configuration.

The IMU knows which way is down and the topology knows how the chip is bolted to the body, but nothing joined the two: forward kinematics reported the robot bolt upright no matter what the accelerometer said. This closes that loop, so BB.Robot.State carries a lean that means something and anything reasoning about the robot's pose gets it for free.

Where the angle comes from

The fused orientation is a rotation from the chip's frame into the estimator's world, whose Z is up. Taking the world's up vector back through it gives up in the chip's frame, and the static rotation from imu_link to base_link — read from the topology rather than written down here — puts it in the body's. Level, that vector is the body's own +Z; leaning forward tips it towards -X. So the angle is atan2(-x, z), positive forwards, which is the sense :lean is declared with.

The rate comes from the gyroscope through the same rotation, taking the component about the body's Y. A balance controller wants both, and differentiating the angle would be a worse rate than the one the gyro measures directly. Strictly the pitch rate is wy * cos(roll) - wz * sin(roll) and this is the small-roll approximation of it, which for a robot that is supposed to stay upright is the case that matters.

What it can't tell you

Only tilt. The estimator has no magnetometer, so rotation about gravity is unobservable and its yaw wanders — but yaw doesn't reach this calculation, since it moves the up vector not at all.

It also can't tell a lean from an acceleration. An accelerometer measures specific force, so a robot accelerating forwards on the level reads as one leaning back, and the estimator only suppresses that to the extent its gyro disagrees. That is the classic failure of balancing on an IMU alone and it wants watching once the wheels are driving.

Summary

Functions

The static rotation taking a direction in imu_link's frame into body_link's.

The lean angle in radians for an orientation, positive leaning forwards.

How fast the body is leaning, in radians per second, for a gyroscope reading.

Functions

body_from_imu(robot_module, body_link, imu_link)

@spec body_from_imu(module(), atom(), atom()) :: BB.Math.Quaternion.t()

The static rotation taking a direction in imu_link's frame into body_link's.

Both links hang off the same joints above them, so those cancel and the configuration they are evaluated at doesn't matter.

lean(orientation, body_from_imu)

The lean angle in radians for an orientation, positive leaning forwards.

body_from_imu rotates a direction in the chip's frame into the body's.

Decoupled from roll, which the obvious formula is not. Pitched by theta and rolled by phi, the world's up vector in body coordinates is {-sin theta, cos theta * sin phi, cos theta * cos phi} — so taking atan2(-x, z) gives atan2(sin theta, cos theta * cos phi), and the roll shrinks the denominator. A five degree lean reads as 5.8 at thirty degrees of roll and 9.9 at sixty.

That is not a rounding error on a balance bot, it is a coupling: any rolling motion modulates the measured lean, the controller corrects a pitch error that was never there, and the oscillation is driven from an axis the pitch loop cannot see. No gain reaches it. Dividing by the whole magnitude perpendicular to X instead removes cos phi and leaves theta for any roll.

lean_rate(angular_velocity, body_from_imu)

@spec lean_rate(BB.Math.Vec3.t(), BB.Math.Quaternion.t()) :: float()

How fast the body is leaning, in radians per second, for a gyroscope reading.