diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 63b6dad5..124f5942 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -22,10 +22,10 @@ jobs: - run: echo "The name of the branch is ${{ github.ref }} and the repository is ${{ github.repository }}." - name: Checkout repository - uses: actions/checkout@v4 + uses: actions/checkout@v7 - name: Set up Python - uses: actions/setup-python@v5 + uses: actions/setup-python@v6 with: python-version: '3.10' cache: 'pip' @@ -48,10 +48,10 @@ jobs: # Steps represent a sequence of tasks that will be executed as part of the job. steps: - name: Checkout repository - uses: actions/checkout@v4 + uses: actions/checkout@v7 - name: Set up Python - uses: actions/setup-python@v5 + uses: actions/setup-python@v6 with: python-version: '3.10' cache: 'pip' @@ -80,10 +80,10 @@ jobs: steps: - name: Checkout repository - uses: actions/checkout@v4 + uses: actions/checkout@v7 - name: Set up Python - uses: actions/setup-python@v5 + uses: actions/setup-python@v6 with: python-version: '3.10' cache: 'pip' @@ -108,10 +108,10 @@ jobs: steps: - name: Checkout repository - uses: actions/checkout@v4 + uses: actions/checkout@v7 - name: Set up Python - uses: actions/setup-python@v5 + uses: actions/setup-python@v6 with: python-version: '3.10' cache: 'pip' @@ -135,10 +135,10 @@ jobs: steps: - name: Checkout repository - uses: actions/checkout@v4 + uses: actions/checkout@v7 - name: Set up Python - uses: actions/setup-python@v5 + uses: actions/setup-python@v6 with: python-version: '3.10' cache: 'pip' @@ -170,10 +170,10 @@ jobs: # Steps represent a sequence of tasks that will be executed as part of the job. steps: - name: Checkout repository - uses: actions/checkout@v4 + uses: actions/checkout@v7 - name: Set up Python - uses: actions/setup-python@v5 + uses: actions/setup-python@v6 with: python-version: '3.10' cache: 'pip' diff --git a/doc/architecture/README.md b/doc/architecture/README.md index b3811521..f6af708f 100644 --- a/doc/architecture/README.md +++ b/doc/architecture/README.md @@ -91,10 +91,10 @@ $v_R [\frac{steps}{s}] = \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ Consider robot linear speed center\ Linear speed left\ -$v_L [\frac{steps}{s}] = \frac{v_{Linear}}{2} [\frac{steps}{s}] -\frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$ +$v_L [\frac{steps}{s}] = v_{Linear} [\frac{steps}{s}] -\frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$ Linear speed right\ -$v_R [\frac{steps}{s}] = \frac{v_{Linear}}{2} [\frac{steps}{s}] + \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$ +$v_R [\frac{steps}{s}] = v_{Linear} [\frac{steps}{s}] + \frac{w_r [\frac{rad}{s}] \cdot W [mm]}{2} \cdot \frac{ENC [\frac{steps}{m}]}{1000}$ Consider angular speed in mrad per s\ Linear speed left\ @@ -111,31 +111,57 @@ Base equations: - $distanceLeft [mm] = \frac{encoderStepsLeft [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$ - $distanceRight [mm] = \frac{encoderStepsRight [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$ -- $stepsCenter [steps] = \frac{encoderStepsLeft - encoderStepsRight}{2}$ +- $stepsCenter [steps] = \frac{encoderStepsLeft + encoderStepsRight}{2}$ - $distanceCenter [mm] = \frac{stepsCenter [steps]}{encoderStepsPerMM [\frac{steps}{mm}]}$ Orientation: - $alpha [rad] = \frac{distanceRight [mm] - distanceLeft [mm]}{wheelBase [mm]}$ -- $orientation' [rad] = orientation [rad] + alpha [rad]$ -- $orientation' [rad] = orientation [rad]~\%~2\pi$ -- $-2\pi < Orientation < 2\pi$ +- $orientation_{new} [rad] = orientation_{old} [rad] + alpha [rad]$ +- $orientation_{new} [rad] = orientation_{new} [rad]~%~2\pi$ +- $-2\pi < orientation < 2\pi$ - After wrapping on the positive limit $2\pi$, the orientation remains positive and starts from 0 again. - After wrapping on the negative limit $2\pi$, the orientation remains negative and starts from 0 again. Position: -- $dX [mm] = -distanceCenter [mm] \cdot sin(orientation' [rad])$ <- Approximation for performance reason -- $dY [mm] = distanceCenter [mm] \cdot cos(orientation' [rad])$ <- Approximation for performance reason -- $x' [mm] = x [mm] + dX [mm]$ -- $y' [mm] = y [mm] + dY [mm]$ +To reduce the integration error during curved movement, the position is calculated using the orientation in the middle of the movement interval. -Improvement for better accuracy: +- $orientation_{mid} [rad] = orientation_{old} [rad] + \frac{alpha [rad]}{2}$ + +Position update: + +- $dX [mm] = -distanceCenter [mm] \cdot sin(orientation_{mid} [rad])$ +- $dY [mm] = distanceCenter [mm] \cdot cos(orientation_{mid} [rad])$ +- $x_{new} [mm] = x_{old} [mm] + dX [mm]$ +- $y_{new} [mm] = y_{old} [mm] + dY [mm]$ + +Fixed-point implementation: + +For better precision and to avoid unnecessary floating-point conversions, the implementation internally uses milliradians (mrad). + +Orientation: - $alpha [mrad] = \frac{1000 \cdot (encoderStepsRight [steps] - encoderStepsLeft [steps])}{encoderStepsPerMM [\frac{steps}{mm}] \cdot wheelBase [mm]}$ -- $orientation' [mrad] = orientation [mrad] + alpha [mrad]$ -- $dX [mm] = -distanceCenter [mm] \cdot sin(\frac{orientation' [mrad]}{1000})$ -- $dY [mm] = distanceCenter [mm] \cdot cos(\frac{orientation' [mrad]}{1000})$ +- $orientation_{new} [mrad] = orientation_{old} [mrad] + alpha [mrad]$ +- $orientation_{mid} [mrad] = orientation_{old} [mrad] + \frac{alpha [mrad]}{2}$ + +Position: + +- $dX [steps] = -stepsCenter [steps] \cdot sin(\frac{orientation_{mid} [mrad]}{1000})$ +- $dY [steps] = stepsCenter [steps] \cdot cos(\frac{orientation_{mid} [mrad]}{1000})$ + +To preserve sub-step precision, the calculated position increments are accumulated in units of $\frac{1}{1000}$ encoder steps before conversion to millimetres: + +- $dX_{1000} = 1000 \cdot dX [steps]$ +- $dY_{1000} = 1000 \cdot dY [steps]$ + +The accumulated values are continuously converted to millimetres using the encoder resolution: + +- $x [mm] = \frac{\sum dX_{1000}}{encoderStepsPerMM \cdot 1000}$ +- $y [mm] = \frac{\sum dY_{1000}}{encoderStepsPerMM \cdot 1000}$ + +This preserves fractional encoder-step contributions and significantly reduces long-term position drift caused by rounding. ##### Speedometer diff --git a/lib/APPSensorFusion/src/ReadyState.cpp b/lib/APPSensorFusion/src/ReadyState.cpp index fed3dc44..a3fab63b 100644 --- a/lib/APPSensorFusion/src/ReadyState.cpp +++ b/lib/APPSensorFusion/src/ReadyState.cpp @@ -72,12 +72,12 @@ LOG_TAG("RState"); void ReadyState::entry() { - - /* Clear the Odometry Position in favor of reproducibility. */ - Odometry::getInstance().clearPosition(); - Odometry::getInstance().setOrientation(1570); const int32_t SENSOR_VALUE_OUT_PERIOD = 1000; /* ms */ + /* Clear the odometry position and orientation in favor of reproducibility. */ + Odometry::getInstance().clearPosition(); + Odometry::getInstance().clearOrientation(); + /* The line sensor value shall be output on console cyclic. */ m_timer.start(SENSOR_VALUE_OUT_PERIOD); } diff --git a/lib/Service/src/DifferentialDrive.cpp b/lib/Service/src/DifferentialDrive.cpp index 3d03914a..cc057e29 100644 --- a/lib/Service/src/DifferentialDrive.cpp +++ b/lib/Service/src/DifferentialDrive.cpp @@ -248,15 +248,14 @@ void DifferentialDrive::calculateLinearSpeedLeftRight(int16_t linearSpeedCenter, { static const int32_t ANGULAR_TO_LINEAR_SPEED_FACTOR = 2000000; /* 2 * 1000 mrad */ int32_t linearSpeedCenter32 = static_cast(linearSpeedCenter); /* [steps/s] */ - int32_t halfLinearSpeedCenter32 = linearSpeedCenter32 / 2; /* [steps/s] */ int32_t angularSpeed32 = static_cast(angularSpeed); /* [mrad/s] */ int32_t wheelBase32 = static_cast(RobotConstants::WHEEL_BASE); /* [mm] */ int32_t encoderStepsPerM32 = static_cast(RobotConstants::ENCODER_STEPS_PER_M); /* [steps/m] */ int32_t linearSpeedTurnInPlace32 = (angularSpeed32 * wheelBase32 * encoderStepsPerM32) / static_cast(ANGULAR_TO_LINEAR_SPEED_FACTOR); /* [steps/s] */ - linearSpeedLeft = halfLinearSpeedCenter32 - linearSpeedTurnInPlace32; /* [steps/s] */ - linearSpeedRight = halfLinearSpeedCenter32 + linearSpeedTurnInPlace32; /* [steps/s] */ + linearSpeedLeft = linearSpeedCenter32 - linearSpeedTurnInPlace32; /* [steps/s] */ + linearSpeedRight = linearSpeedCenter32 + linearSpeedTurnInPlace32; /* [steps/s] */ } void DifferentialDrive::calculateLinearAndAngularSpeedCenter(int16_t linearSpeedLeft, int16_t linearSpeedRight, diff --git a/lib/Service/src/Odometry.cpp b/lib/Service/src/Odometry.cpp index 910c8475..9fbdf3be 100644 --- a/lib/Service/src/Odometry.cpp +++ b/lib/Service/src/Odometry.cpp @@ -84,20 +84,35 @@ void Odometry::process() if ((STEPS_THRESHOLD <= absStepsLeft) || (STEPS_THRESHOLD <= absStepsRight)) { int16_t stepsCenter = static_cast((relStepsLeft + relStepsRight) / 2); /* [steps] */ - int16_t dXSteps = 0; /* [steps] */ - int16_t dYSteps = 0; /* [steps] */ + int32_t dXSteps1000 = 0; /* [1/1000 steps] */ + int32_t dYSteps1000 = 0; /* [1/1000 steps] */ int32_t deltaPosX = 0; /* [mm] */ int32_t deltaPosY = 0; /* [mm] */ + /* Save orientation before update. */ + int32_t oldOrientation = m_orientation; + + /* Calculate orientation delta only. */ + int32_t deltaOrientation = calculateOrientation(0, relStepsLeft, relStepsRight); + + /* Use the orientation in the middle of the movement. + * This avoids the systematic position error caused by + * projecting the whole movement using the final heading. + */ + int32_t orientationMid = oldOrientation + (deltaOrientation / 2); + /* Calculate mileage in steps to avoid loosing precision by division. */ m_mileage = calculateMileage(m_mileage, stepsCenter); - m_orientation = calculateOrientation(m_orientation, relStepsLeft, relStepsRight); + /* Calculate delta position in 1/1000 steps to avoid loosing precision by divison. */ + calculateDeltaPos(stepsCenter, orientationMid, dXSteps1000, dYSteps1000); - /* Calculate delta position in steps to avoid loosing precision by divison. */ - calculateDeltaPos(stepsCenter, m_orientation, dXSteps, dYSteps); - m_countingXSteps += dXSteps * 1000; /* Multiply with 1000 for higher precision. */ - m_countingYSteps += dYSteps * 1000; /* Multiply with 1000 for higher precision. */ + m_countingXSteps += dXSteps1000; + m_countingYSteps += dYSteps1000; + + /* Update orientation afterwards. */ + m_orientation = oldOrientation + deltaOrientation; + m_orientation %= FP_2PI(); /* For large areas, its important to have the position in mm and not in steps. * Therefore the position in mm is continously calculated from the counted steps @@ -156,6 +171,11 @@ void Odometry::clearPosition() m_countingYSteps = 0; } +void Odometry::clearOrientation() +{ + m_orientation = ORIENTATION_INITIAL; +} + void Odometry::clearMileage() { m_mileage = 0; @@ -171,12 +191,13 @@ void Odometry::clearMileage() bool Odometry::detectStandStill(uint16_t absStepsLeft, uint16_t absStepsRight) { - bool isStandStill = false; + const uint16_t STANDSTILL_DETECTION_THRESHOLD = 1U; /* [steps] */ + bool isStandStill = false; /* No encoder (left/right) change detected? */ - if (absStepsLeft == m_lastAbsRelEncStepsLeft) + if (abs(absStepsLeft - m_lastAbsRelEncStepsLeft) <= STANDSTILL_DETECTION_THRESHOLD) { - if (absStepsRight == m_lastAbsRelEncStepsRight) + if (abs(absStepsRight - m_lastAbsRelEncStepsRight) <= STANDSTILL_DETECTION_THRESHOLD) { isStandStill = true; } @@ -235,12 +256,13 @@ int32_t Odometry::calculateOrientation(int32_t orientation, int16_t stepsLeft, i return orientation; } -void Odometry::calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int16_t& dXSteps, int16_t& dYSteps) const +void Odometry::calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int32_t& dXSteps1000, + int32_t& dYSteps1000) const { - float fDistCenter = static_cast(stepsCenter); /* [steps] */ - float fOrientation = static_cast(orientation) / 1000.0F; /* [rad] */ - float fDeltaPosX = fDistCenter * cosf(fOrientation); /* [steps] */ - float fDeltaPosY = fDistCenter * sinf(fOrientation); /* [steps] */ + float fDistCenter = static_cast(stepsCenter); /* [steps] */ + float fOrientation = static_cast(orientation) / 1000.0F; /* [rad] */ + float fDeltaPosX = fDistCenter * cosf(fOrientation) * 1000.0F; /* [1/1000 steps] */ + float fDeltaPosY = fDistCenter * sinf(fOrientation) * 1000.0F; /* [1/1000 steps] */ /* Round because the cast will just cut the fractional part. */ if (0.0F <= fDeltaPosX) @@ -262,8 +284,8 @@ void Odometry::calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int16 fDeltaPosY -= 0.5F; } - dXSteps = static_cast(fDeltaPosX); /* [steps] */ - dYSteps = static_cast(fDeltaPosY); /* [steps] */ + dXSteps1000 = static_cast(fDeltaPosX); /* [1/1000 steps]*/ + dYSteps1000 = static_cast(fDeltaPosY); /* [1/1000 steps]*/ } /****************************************************************************** diff --git a/lib/Service/src/Odometry.h b/lib/Service/src/Odometry.h index fc04649b..38fac685 100644 --- a/lib/Service/src/Odometry.h +++ b/lib/Service/src/Odometry.h @@ -154,6 +154,11 @@ class Odometry */ void clearPosition(); + /** + * Clear the orientation by setting it to 90°. + */ + void clearOrientation(); + /** * Clear mileage by setting it to 0 mm. */ @@ -187,6 +192,12 @@ class Odometry */ static const uint32_t STANDSTILL_DETECTION_PERIOD = 10; + /** + * Initial orientation in mrad. + * 90° - heading to north + */ + static const int32_t ORIENTATION_INITIAL = FP_PI() / 2; + /** * Last number of relative encoder steps left. Its used to avoid permanent * clearing of the relative encoders. @@ -234,7 +245,7 @@ class Odometry m_lastAbsRelEncStepsRight(0), m_mileage(0), m_relEncoders(Board::getInstance().getEncoders()), - m_orientation(FP_PI() / 2), /* 90° - heading to north */ + m_orientation(ORIENTATION_INITIAL), m_posX(0), m_posY(0), m_countingXSteps(0), @@ -305,10 +316,10 @@ class Odometry * * @param[in] stepsCenter Number of steps center * @param[in] orientation Orientation in mrad - * @param[out] dXSteps Delta x-position on x-axis in steps - * @param[out] dYSteps Delta y-position on y-axis in steps + * @param[out] dXSteps1000 Delta x-position on x-axis in 1/1000 steps + * @param[out] dYSteps1000 Delta y-position on y-axis in 1/1000 steps */ - void calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int16_t& dXSteps, int16_t& dYSteps) const; + void calculateDeltaPos(int16_t stepsCenter, int32_t orientation, int32_t& dXSteps1000, int32_t& dYSteps1000) const; }; /******************************************************************************