RoboCup Rescue Robot Sensor Head

A project of the Robotics 2020 class of the School of Information Science and Technology (SIST) of ShanghaiTech University. Course Instructor: Prof. Sören Schwertfeger.

Zeyu Zheng , Yijun Chen and Wenqian Zhang


The RoboCup Rescue Project is intended to promote research and development in disaster rescue which is a significant domain at various levels. The goal of the RoboCup Rescue competitions requires robots to show abilities of mobility, sensory perception, planning, mapping, and practical operator interfaces on searching for simulated victims in unstructured environments.

In case it’s a huge project, we divided it into two parts, Kinematics and Sensor, giving them to two different teams to finish. Our team mainly focus on Sensor part. Our robot will finish rescue task in dark labyrinth environment, recognize objects with QR code. So we need at least one light source to ensure robot could catch light information in dark environment, another challenge is cross uneven surface, in which condition keeping camera standard need using ROS to control sensor posture, mainly using rotation command to keep it standard. So how to use ROS control hardware through serial communication and sensor platform design is our main work.

Hardware Detail, Design & Real model

RoboMaster M2006 Brushless DC Gear Motor

RoboMaster C610 Brushless DC Motor Speed Controller

RelVision 1080P 10X Optical Zoom USB camera

RoboMaster Dev Board C

We use SolidWorks 2019 to draw our design of sensor head.


For convenience, we use Euler angle to describe rotation of the sensor platform. Normally a platform just need to consider yaw and pitch, so ROS need get rotation information from serial port and calculate Euler angle, then write back to serial port. The platform is divided into two parts, each part installs an electric motor , the lower part controls the Yaw Angle, upper part controls Pitch Angle, at the top of upper part surface loaded a DJI RoboMaster C-type development board, the other side loaded camera. Also, to keep the two side have same weight, we need to add one weight block( about 300 gram) on the opposite side of board.


Our software is developed based on Robot Operation System(ROS) which is very popular software architecture. We made our own ROS package, mainly packed serial communication code wrote by python. Here is our pipeline: First initialize a subscriber and publisher, then if publisher receive Quaternion from port, publish it to ROS, and if subscriber receive Quaternion from ROS, calculate it into Euler Angle and write the message to port.

System Evaluation

The project is to meet following requirements:

1 Gimbal maintain pose stable when get through different terrains.

2 Gimbal can be controlled by ROS or human operator’s willing.

3 The data gathered by different sensors can be read through ROS.

4 The data is fusioned for further works.

11Testing: The vehicle will be tested on the simulate terrains for 10 minutes

to check if the system meets the requirements above.

We use serial to report current angle and plot it during debug. Since we are not able to install on the chasis, we test the system by hand. From the video, we can see that the error of orientation for each axis under control is less than 1 deg when in static, and less that ś10 deg when we are moving. Now, the PID parameter can be more aggressive to get better response, but higher parameter will resonate with human hand’s control, so further PID tuning is needed after we can actually install it.


We design the sensor head for the rescue robot used in the RoboCup band finishes the very initial version of software design. Further more, we can:

1 Tuning PID to control more accurately

2 Apply system identification on PID parameter

3 Improve robust around 180 and -180 point

4 Use carbon fiber to reduce the weight of hardware structure