When modeling, we broke down the system into three layers to interface with our PID controller: Camera transformation, Platform angles, and Motor angles.
We referenced the math derived by [1] and determined the mapping from the platform coordinates to the motor angles. However, due to the complexity of the math involved and the concerns of runtime, we decided to make a few assumptions and approximations to simplify the calculation:
We assumed that the change in heights due to the two rotations about the x- and the y-axis of the platform can be decoupled.
We assumed that the rods move perpendicularly to the base by a small angle approximation on the motor angles. This approximation is estimated to have less than a 2% of error in the height.
We assumed the length of the rods is much longer than the length of the shaft arm.
The first is to convert the image frame into a real-world frame. We determined the coordinates through experiments and figured out the transformation matrices. Note that in this method, we neglected the effect of axis skew viewed from camera.
From the first assumption we made, we can calculate the change in height at the three joints on the platform due to the rotation about the x- and y-axis. The height can be found by using the similar triangle theorem.
From assumption 3, we can infer that changes in height at the joints are equal to the changes in height of the top of the shaft arm by a small angle approximation. With this knowledge in mind, we can directly find the change in height using Trigonometry.
We simulated the system response to validate our system model and to tune the PID controller. In our simulation, we used Euler discretization and a double integrator to synthesize our state evolution over time.