Turret Controller and CV Computations - MatthewMArnold/taproot GitHub Wiki
The turret controllers and peripheral computation surrounding computer vision automatic aiming and firing are critical to the effectiveness of a RoboMaster robot. The fundamental control system for this is the pitch and yaw gimbal turret controller.
World Frame Turret Feedback
A turret mounted RoboMaster Type C board is used to measure the turret in the world frame. Heading and gyroscope data is computed on the type C Board at 1000 Hz and sent to the main compute platform, a type A board (see [[Soldier Hardware Block Diagram]]) along with a time synchronized time stamp of the measurement data. This allows us to know at what time relative to the main compute platform we measured IMU data down to microsecond level accuracy.
Once world frame turret pitch and yaw headings are acquired, we use this data in
a turret controller command. This command runs a position to velocity cascade
PID controller. When manually or automatically controlling the turret, a
poistion target is used to run PID controllers on. The position error (actual
turret angle - desired turret angle) is ran through one
tap::algorithms::SmoothPid
object. The output is passed to a velocity PID controller, which is compared to
the actual velocity to get a velocity error, which is fed into a SmoothPid
velocity PID controller. The output of this controller is the final motor
output, which is sent to the turret gimbals.
Dynamic PID for Hero Turret Yaw Control
A cascade PID controller is not something new and is inspired by many other RoboMaster teams, being demonstrated as effective in open source materials as early as 2019 by Shenzhen University. What is a more interesting problem is PID control of a dynamic system. In particular, each hero projectile is approximately 50 grams. Our hero hopper is located on the yaw axis of the hero, which means a fuly loaded hopper of 70 golf balls weighs approximately 3.5 kg greater than an unloaded hopper. This means a normal PID algorithm tuned to be effective with a fully loaded hero hopper is unstable when the hopper weighs 3.5 kg, and a PID controller tuned for an unloaded hopper will be heavily underactuated when the hopper is full.
One solution would be to measure the number of golf balls in the hopper and make the PID gains a function of the number of balls in the hopper; however, measuring the number of balls is a nontrivial solution when you don't assume you will always start with the same number.
Thus, a novel "Fuzzy" PID approach is used to compensate for rapidly changing
unknown dynamic system without knowing anything about the system except that the
PID parameters must change. Fuzzy logic is used to adaptively change the
proportional and derivative gains at runtime, whereas they are hard-coded in the
SmoothPid object. This allows the controller to adapt to more complex situations
(such as overcoming sticktion, varying loads, etc.). Aside from adaptively
changing the gains, this controller behaves the same was as the SmoothPid
object.
For more general information about fuzzy PID, refer to this paper: https://ieeexplore.ieee.org/document/937407. You can also find others online. For a more generic look at fuzzy logic, you can refer to this paper: https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=488475.
This approach worked very well and allowed our hero turret to maintain percise and relatively rapid movement independent of the number of balls in the hopper.
Auto Aim Odometry
Because ARUW only competed in RMUL in 2022, odometry for computer vision was only performed in 2D (xy plane). This simplification was performed to speed up kalman filter performance on the main embedded compute platform (the RoboMaster Type A board). There are two parts of odometry required to locate and orient the turret in 2D space--turret heading and chassis position. Turret heading used for compter vision is the world frame turret feedback as described above.
For ground robots, chassis odometry is a 2D kalman filter that fuses the chassis mounted accelerometer with wheel odometry. The following matrices describe the kalman filter. All inputs into the kalman filter have metric units of meters/time
State transition matrix:
[ 1, DT, 0.5 * DT * DT, 0, 0 , 0 ]
[ 0, 1 , DT , 0, 0 , 0 ]
[ 0, 0 , 1 , 0, 0 , 0 ]
[ 0, 0 , 0 , 1, DT, 0.5 * DT * DT ]
[ 0, 0 , 0 , 0, 1 , DT ]
[ 0, 0 , 0 , 0, 0 , 1 ]
Observation matrix:
[ 0, 1, 0, 0, 0, 0 ]
[ 0, 0, 1, 0, 0, 0 ]
[ 0, 0, 0, 0, 1, 0 ]
[ 0, 0, 0, 0, 0, 1 ]
System noise covariance:
[ 1E1, 0 , 0 , 0 , 0 , 0 ]
[ 0 , 1E0, 0 , 0 , 0 , 0 ]
[ 0 , 0 , 1E-1, 0 , 0 , 0 ]
[ 0 , 0 , 0 , 1E1, 0 , 0 ]
[ 0 , 0 , 0 , 0 , 1E0, 0 ]
[ 0 , 0 , 0 , 0 , 0 , 1E-1 ]
Measurement noise covariance (note that 0th and 2nd columns are updated
dynamically, see below):
[ 1.0 0 0 0 ]
[ 0 1.2 0 0 ]
[ 0 0 1.0 0 ]
[ 0 0 0 1.2 ]
Predicted error covariance initial values:
[ 1E3, 0 , 0 , 0 , 0 , 0 ]
[ 0 , 1E3, 0 , 0 , 0 , 0 ]
[ 0 , 0 , 1E3, 0 , 0 , 0 ]
[ 0 , 0 , 0 , 1E3, 0 , 0 ]
[ 0 , 0 , 0 , 0 , 1E3, 0 ]
[ 0 , 0 , 0 , 0 , 0 , 1E3 ]
The odometry runs at a fixed rate of 500 Hz. Furthermore, the covarience of the measured chassis velocity was found to be a function of the robot acceleration--when you are accelerating faster, you tend to slip more, but when you are not accelerating, wheel odometry can be quite accurate. Thus, each update step, before a kalman step is taken, we compute the measure acceleration as reported the wheels and update the wheel velocity measurement covariance by inputting the wheel measured acceleration into a function whereby covariance is inversely proportional to wheel acceleration.
This odometry solution may be found in chassis_kf_odometry.hpp/cpp.
Ballistics
Once a shared reference frame is realized, computer vision techniques can be used to locate a plate in the shared frame. This data is sent across UART to the type A board (see Vision System Serial Protocol).
Once a target in space has been acquired, the embedded platform performs all required computations necessary to aim at the correct location to account for measurement latency and projectile droop.
Firstly, the target is given a type A board timestamp of when the target was seen. The difference between the current and target timestamp is computed. The target is then projeted forward to the current time using the simple known kinematic model of the system.
Once we know where the target is "now" rather than some time in the past, we perform ballistics to aim at where the target will be taking into account projectile droop. This ballistics solution takes into account both the kinematic model of the target as well as the travel time and droop of a projectile. Taproot provides this effective ballistics solution here. Furthermore, this ballistics solution is integrated into aruw-mcb's turret_cv_command.cpp, which is also available for reference.
Furthermore, this diagram shows a simple model of the steps taken when computing where to aim while taking into account target velocity and ballistics.
Shot timing
To effectively win RoboMaster 1v1 encounters in modern (post-2018) RoboMasters, it is required to be able to enact countermeasures when automatically aiming at spinning robots. This is especially important for the hero robot, which has limited ammunition and a relatively high cooldown. Thus, one of the features we implemented was computer vision based beyblade countermeasures and shot timing (which is active all the time to avoid wasting shots).
Vision based beyblade countermeasures will not be covered here because it is not something done on the embedded platform and thus is not open source. The shot timing computations; however, are open source (aside from the part of actually measuring where the plate to time a shot against is, which is performed on the vision side).
Shot timing embedded algorithm
The actual shot timing algorithm is relatively simple once the vision side is able to compute the time in the future at which point our next launched projectile will hit the robot's plate and the time between plate centers transitioning the target point. The following steps are performed on the embedded side once these values are provided by the vision system:
- Compute a ballistics solution using
tap::Ballistics, which returns a projectile time of flight. - Compute the current projected hit time as the time of flight plus the time it takes for a projectile to be launched (which accounts for physical agitator latency).
- Check if this projected hit time is within some threshold of the time in the future at which point our launched projectile will hit. You can also mod this difference by the time between plate centers transitioning the target point so if the upcoming plate will be out of range, the plate coming into view after the upcoming plate might be in range.
- If the projected hit time is close to the projected time that the target will be within the aim target, let a projectile be fired.
This shot timing algorithm only works effectively when the "time to fire" is effectively known. That is, the time it takes after a projectile is requested to be fired to the time the projectile is actually fired. Furthermore, the smaller this time is, the less potential variation and errors resulting from projectil launch timing occurs. Thus, our hero robot was designed specifically to have a "kicker" motor directly before the flywheels that pushed a queued ball into the flywheels. This meant we never wasted ammunition and that we very consistently knew that a projectile would be fired 70ms after it was requested. It is also important to note that this shot timing algorithm is only effective because of the stability and precision of the turret controller and vision software described above, without which this shot timing algorithm would be useless.
Below is a demonstration of shot timing against a beyblading robot in action. With our hero robot, we achieved 100% accuracy in all tests when firing at a 60W power limited standard robot under 3m. Above 3m, at 5m, accuracy to a lower value but is still above 50%. Most of the inaccuracy was found to be in the detection algorithm itself.