⚡ electrical engineering

Smart Prism with Tilt Compensation for CAN bus on Mobile Machinery Using Robotic Total Stations

This paper presents and validates a Smart Prism prototype that integrates an IMU with a robotic total station to perform real-time tilt compensation, thereby enabling millimeter-to-centimeter accurate reference trajectory measurements for autonomous mobile machinery under dynamic off-road conditions via CAN bus transmission.

Original authors: Sumesh Sharma, Marcel Moll, Timo Oksanen

Published 2026-03-03

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Sumesh Sharma, Marcel Moll, Timo Oksanen

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

The Big Problem: The "Wobbly Stick" Dilemma

Imagine you are trying to measure exactly where a tractor is driving in a field using a high-tech laser camera (called a Robotic Total Station). This camera is incredibly precise; it can see a point on a glass prism with millimeter accuracy.

However, there's a catch. The tractor isn't a flat, smooth surface. It bounces, rolls, and pitches over bumps, just like a boat on choppy water.

The Setup: The laser camera sees a prism mounted high up on the tractor's roof.
The Goal: We actually want to know where the tractor's wheels or the plow on the ground are.
The Issue: Because the tractor is tilting, the point on the ground directly below the prism moves sideways. If the tractor tilts just a little bit, and the prism is 1 meter high, the point on the ground shifts by several centimeters.

The Analogy: Think of holding a long broomstick upright. If you tilt the top of the broomstick just a tiny bit, the bottom of the stick swings a long way across the floor. The laser camera sees the top of the broomstick perfectly, but it has no idea the bottom has swung sideways. This makes it useless for testing self-driving tractors, which need to know exactly where their wheels are, not where the roof is.

The Solution: The "Smart Prism"

The researchers built a device called a Smart Prism. It's a standard glass prism, but they gave it a "brain" and a "sense of balance."

The Brain (Microcontroller): A small computer chip inside the prism.
The Sense of Balance (IMU): A tiny sensor (like the one in your smartphone) that knows exactly how much the prism is tilting forward, backward, or sideways.

How it works:
Instead of just saying, "I am at coordinates X, Y, Z," the Smart Prism says:

"I am at coordinates X, Y, Z, AND I am tilting 15 degrees to the left. Therefore, the point on the ground I'm interested in is actually here."

It does this math in real-time, calculating exactly where the "virtual" point on the ground is, even while the tractor is bouncing around.

The "Translator" (CAN Bus)

There was another problem: The laser camera speaks a language only surveyors understand, but the tractor's computer (which runs the self-driving software) speaks a language called CAN bus (the standard language for cars and tractors).

The Smart Prism acts as a universal translator. It takes the laser data, does the tilt math, and then shouts the corrected position to the tractor's computer in a language it understands. This allows the tractor to treat the Smart Prism as if it were a sensor sitting directly on its rear axle, even though it's actually on the roof.

The Experiment: The "Human Wobble" Test

To prove it works, the researchers didn't just drive a tractor; they did something more controlled.

The Setup: They mounted the Smart Prism on a long pole (about 1 meter high).
The Action: A human held the pole and manually tilted it back and forth, side to side, up to 60 degrees (a very steep tilt!).
The Goal: The laser camera watched the prism, while the Smart Prism calculated where the bottom of the pole should be.
The Result: Even with the human shaking the pole wildly, the Smart Prism calculated the bottom position with an error of only 3mm to 24mm (about the width of a pencil to a small coin).

This proved that the system can handle the "wobbles" of a real tractor and still tell the computer exactly where the vehicle is.

Why This Matters

Before this, if you wanted to test a self-driving tractor, you had to drive very slowly on perfectly flat ground to avoid tilting errors, or build expensive mechanical stabilizers.

This Smart Prism changes the game:

It's cheap and modular: It's a retrofit you can add to existing equipment.
It's dynamic: You can test tractors driving fast, turning corners, and going up hills.
It's accurate: It provides the "gold standard" reference needed to prove that a self-driving tractor is actually driving correctly.

In a Nutshell

The paper describes a clever upgrade to a surveying tool. By adding a tilt sensor and a translator chip to a standard prism, the researchers created a device that can "see through" the bumps and tilts of a tractor. It tells the self-driving computer exactly where the vehicle is, turning a shaky, bouncing ride into a precise, data-rich test drive.

1. Problem Statement

Validating autonomous agricultural robots and highly automated off-road vehicles requires accurate reference trajectories under real-world field conditions. While Robotic Total Stations (RTS) provide millimeter-level accuracy for prism center coordinates, a critical limitation exists in mobile machinery applications:

The Lever Arm Effect: The point of interest (POI) on a vehicle (e.g., rear axle, tool tip) is typically displaced from the prism center by a lever arm ranging from decimeters to meters.
Tilt-Induced Errors: Off-road machinery experiences significant roll and pitch motions due to uneven terrain. Since RTS only measures 3D position (not full 6D pose), these tilts cause the POI to shift horizontally relative to the prism's vertical projection.
Consequence: Even small tilts can introduce horizontal errors in the centimeter-to-decimeter range, rendering standard RTS setups insufficient for validating centimeter-level navigation systems (like RTK-GNSS) during dynamic maneuvers (turning, slope operation).
Integration Gap: Existing RTS interfaces are surveying-specific and do not natively integrate into vehicle electronic architectures (e.g., CAN bus/ISOBUS), making real-time reference data consumption difficult.

2. Methodology

The authors propose a "Smart Prism" system that augments a standard RTS prism with an Inertial Measurement Unit (IMU) to enable real-time tilt compensation and vehicle integration.

A. Hardware Architecture

Processing Unit: STM32H723ZG microcontroller (ARM Cortex-M7, 550 MHz) featuring a double-precision FPU and hardware accelerators (CORDIC, FMAC) for real-time trigonometric calculations.
Sensors: Murata SCH16T-K01 IMU (3-axis gyroscope + 3-axis accelerometer) selected for low bias instability (0.3°/h) and wide temperature range (-40°C to 110°C).
Interfaces:
- CAN Bus: SN65HVD230 transceiver for ISO 11898-2/ISO 11783 (ISOBUS) integration, allowing the system to output data as a virtual position sensor on the vehicle network.
- RTS Connectivity: RS-232 interface to receive data from Leica CS20 field controllers.
- Power: 12V vehicle system compatible with ISO 16750-2, featuring over/under-voltage and reverse polarity protection.
Mechanical Design: A compact 70mm × 70mm PCB housed in UV-resistant PMMA. The IMU is positioned coaxially with the prism to minimize kinematic offsets.

B. Sensor Fusion and Algorithm

The core logic involves fusing RTS position data with IMU orientation data:

Attitude Estimation: An Adaptive Complementary Filter fuses gyroscope and accelerometer data to estimate roll ( $\phi$ $ϕ$ ) and pitch ( $\theta$ $θ$ ).
- The filter uses a dynamic confidence value ( $\alpha_{dyn}$ ) that adjusts based on the deviation of the measured acceleration vector from gravity ( $g$ ). This ensures stability during static periods and responsiveness during dynamic motion.
- Note: Yaw ( $\psi$ ) is currently integrated solely from the gyroscope (subject to drift) and held constant for validation purposes; it is not yet fused with a magnetometer or GNSS.
Kinematic Transformation:
- A calibrated lever arm vector ( $L_{Prism \to POI}$ ) is defined in the body frame.
- This vector is rotated into the navigation frame using the current rotation matrix derived from roll/pitch angles.
- The rotated lever arm is added to the RTS-measured prism coordinates to calculate the tilt-compensated POI coordinates.
Coordinate Systems: The system utilizes a Helmert transformation to align the RTS local system with the global navigation system (identity transformation used in validation).

C. Firmware & Data Flow

Embedded: The STM32 runs an event-driven loop, reading IMU data at ~100 Hz and RTS data at ~5 Hz. It performs bias calibration and pre-processing.
PC/Cloud: In the prototype, a Python application handles the heavy lifting of kinematic transformations and visualization, receiving data via USB. The authors note that the STM32H7 has sufficient power to run this logic entirely on-chip in future iterations.

3. Key Contributions

Integrated Hardware Platform: Development of a compact, vehicle-ready Smart Prism combining high-performance MCU, precision MEMS-IMU, automotive-grade power protection, and standard interfaces (CAN, RS-232, USB).
Real-Time Sensor Fusion: Implementation of an adaptive complementary filter that dynamically adjusts weighting based on motion dynamics to stabilize roll/pitch estimation.
Kinematic Tilt Compensation Framework: A complete mathematical method to transform body-fixed lever arms into navigation-fixed coordinates, effectively correcting for prism tilt errors.
Vehicle Integration: Bridging the gap between surveying equipment and vehicle control systems by outputting reference data directly via the CAN bus, allowing the POI to be treated as a virtual sensor co-located with the vehicle's reference point.

4. Experimental Results

The system was validated in a controlled laboratory setting using a fixed ground reference point and a lever arm of approximately 1.07 meters. The rod was manually tilted up to 60° in various directions across five test series.

Accuracy: The 3D Root-Mean-Square Error (RMSE3D) between the calculated POI and the known reference ranged from 2.9 mm to 23.6 mm.
Systematic Error: Mean coordinate errors remained within a few millimeters, indicating successful calibration of the lever arm and lack of dominant systematic bias.
Error Sources:
- The primary source of error was identified as unmodeled yaw rotation during manual handling (the rod was not perfectly constrained against rotation around the vertical axis).
- IMU noise and filter performance were found to be negligible (sub-millimeter contribution).
- Latency between asynchronous IMU and RTS data was minimal but present.

5. Significance and Future Work

Validation Capability: The system achieves centimeter-level accuracy, making it suitable for validating RTK-GNSS and other autonomous navigation systems that operate within 1–2 cm accuracy bounds.
Practical Application: By outputting data via CAN/ISOBUS, the Smart Prism allows for seamless integration into existing agricultural machinery architectures, enabling real-time validation without external surveying setups.
Future Improvements:
- Yaw Determination: Integrating a magnetometer, dual-antenna GNSS, or vehicle steering data to resolve yaw drift and further reduce errors.
- Embedded Processing: Migrating the fusion algorithms entirely to the STM32 microcontroller to remove dependency on an external PC and reduce latency.
- Field Testing: Validating the system on actual moving vehicles under dynamic field conditions.

In conclusion, the Smart Prism successfully addresses the fundamental limitation of RTS-based reference measurements in mobile machinery by compensating for tilt-induced lever arm errors, providing a robust, modular, and vehicle-integrable solution for agricultural robotics research.