Lab Log Summer 2016 - Sheng Li

1
A Summary of Work in Spacecraft Relative Motion Control Lab
Sheng Li
Aerospace Engineering, University of Michigan, Ann Arbor, MI
This documentation keeps a record of my work in Spacecraft Relative Motion Control Lab in the
summer 2016 from May to July. This Lab is still under construction. The purpose of Spacecraft
Relative Motion Control Lab is to scale down the motion of real spacecraft to the Lab scale in both
time and length, and to emulate the motion of spacecraft with Omni-directional robot. My summer
work focuses on camera tracking system, robot control, and MPC controller. The main
achievement of my work includes realizing the multiple-object tracking function for the motion
capture codes which run on Raspberry PI, finding the method of automatic C code generation for
MPC Simulink model, building a simple version of estimator for MPC controller in Simulink, having
a primary understanding of the robot controller codes developed by Dominic Liao-McPherson and
Richard Sutherland and how to control the motion of robot with their codes. My colleague Weitao
Sun has developed the scaling blocks in Simulink. An integrated Simulink model includes scaling
blocks, MPC controller blocks and estimator blocks is to be built for the next step.

2
I. Multiple-object Tracking
The original object tracking codes were developed by Pedro Donato on the basis of the codes
developed by NaturalPoint Inc. The original object tracking codes were for single-object tracking.
In order to realize the purpose of the Spacecraft Relative Motion Control Lab, multiple-object
tracking codes must be developed. I developed the multiple-object tracking codes by modifying
the original single-object tracking codes.
The original single-object tracking codes were designed to run on Linux and read the broadcasted
position and attitude data from OptiTrack motion capture system which runs on Windows. Due to
the variable types of the original codes, the function of the codes is restricted to single-object
tracking.
The first step of realizing the multiple-tracking function is to increase the dimension of variables
defined in the header file run_motion_capture.h, to create room for multiple objects.
• The definition of structure motion_capture_obs in original header file codes:
struct motion_capture_obs{
double time;
// position (x,y,z,roll,pitch,yaw)
double pose[6];
};
struct optitrack_message {
int ID;
float x, y, z;
float qx, qy, qz, qw;
};
• The new definition of structure motion_capture_obs in the modified header file codes:
struct motion_capture_obs{
int ID[objlimit]; // The ID of objects
char name[objlimit][256];
double time;
// position (x,y,z,roll,pitch,yaw)
double pose[objlimit][6];
};
struct optitrack_message {
int ID[objlimit];
char name[objlimit][256];
float x[objlimit], y[objlimit], z[objlimit];
float qx[objlimit], qy[objlimit], qz[objlimit], qw[objlimit];

3
};
where objlimit is the upper limit of the number of object to be tracked and defined as
follows in the very beginning of the header file:
#define objlimit 20 //20 is a customized number
In addition, the definition of the global variables also should be slightly changed:
• Original codes:
#ifndef MOTION_CAPTURE_GLOBALS
#define MOTION_CAPTURE_GLOBALS
EXTERN struct motion_capture_obs mcap_obs[2];
EXTERN pthread_mutex_t mcap_mutex;
EXTERN FILE *mcap_txt;
#endif
• Modified codes:
#ifndef MOTION_CAPTURE_GLOBALS
#define MOTION_CAPTURE_GLOBALS
EXTERN struct motion_capture_obs mcap_obs[objlimit]; //increase dim.
EXTERN pthread_mutex_t mcap_mutex;
EXTERN FILE *mcap_txt;
EXTERN int nRigidBodies; //add a global counter for objects
#endif
The rest of the header file remains the same.
The second step is to modify the PacketClient.cpp file, which contains the major functions that
transfer camera data broadcasted from OptiTrack. However the only function is to be modified is
void Unpack_to_code(char* pData, struct optitrack_message *optmsg) since it is the
function which is called in run_motion_capture.c. The purpose of modification is also to increase
the dimension of variables.
• Original codes:
void Unpack_to_code(char* pData, struct optitrack_message *optmsg)
{
int major = NatNetVersion[0];
int minor = NatNetVersion[1];
char *ptr = pData;
int MessageID = 0;
memcpy(&MessageID, ptr, 2); ptr += 2;

4
int nBytes = 0;
memcpy(&nBytes, ptr, 2); ptr += 2;
if(MessageID == 7) // FRAME OF MOCAP DATA packet
{
// frame number
int frameNumber = 0; memcpy(&frameNumber, ptr, 4); ptr += 4;
// number of data sets (markersets, rigidbodies, etc)
int nMarkerSets = 0; memcpy(&nMarkerSets, ptr, 4); ptr += 4;
for (int i=0; i < nMarkerSets; i++)
{
// Markerset name
char szName[256];
strcpy(szName, ptr);
int nDataBytes = (int) strlen(szName) + 1;
ptr += nDataBytes;
// marker data
int nMarkers = 0; memcpy(&nMarkers, ptr, 4); ptr += 4;
for(int j=0; j < nMarkers; j++)
{
float x = 0; memcpy(&x, ptr, 4); ptr += 4;
float y = 0; memcpy(&y, ptr, 4); ptr += 4;
float z = 0; memcpy(&z, ptr, 4); ptr += 4;
}
}
// unidentified markers
int nOtherMarkers = 0; memcpy(&nOtherMarkers, ptr, 4); ptr += 4;
for(int j=0; j < nOtherMarkers; j++)
{
float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4;
float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4;
float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4;
}
// rigid bodies
int nRigidBodies = 0;
memcpy(&nRigidBodies, ptr, 4); ptr += 4;
if (nRigidBodies > 1) {
printf("Error: Number of rigid bodies = %dn", nRigidBodies);
}
for (int j=0; j < nRigidBodies; j++)
{

5
// rigid body position/orientation
memcpy(&(optmsg->ID), ptr, 4); ptr += 4;
memcpy(&(optmsg->x), ptr, 4); ptr += 4;
memcpy(&(optmsg->y), ptr, 4); ptr += 4;
memcpy(&(optmsg->z), ptr, 4); ptr += 4;
memcpy(&(optmsg->qx), ptr, 4); ptr += 4;
memcpy(&(optmsg->qy), ptr, 4); ptr += 4;
memcpy(&(optmsg->qz), ptr, 4); ptr += 4;
memcpy(&(optmsg->qw), ptr, 4); ptr += 4;
}
}
else
{
printf("Unrecognized Packet Type.n");
}
}
• Modified codes for multiple-object tracking:
void Unpack_to_code(char* pData, struct optitrack_message *optmsg)
{
int major = NatNetVersion[0];
int minor = NatNetVersion[1];
char *ptr = pData;
// message ID
int MessageID = 0;
memcpy(&MessageID, ptr, 2); ptr += 2;
// size
int nBytes = 0;
memcpy(&nBytes, ptr, 2); ptr += 2;
if(MessageID == 7) // FRAME OF MOCAP DATA packet
{
// frame number
int frameNumber = 0; memcpy(&frameNumber, ptr, 4); ptr += 4;
// number of data sets (markersets, rigidbodies, etc)
int nMarkerSets = 0; memcpy(&nMarkerSets, ptr, 4); ptr += 4;
for (int i=0; i < nMarkerSets; i++)
{
// Markerset name
char szName[256];
strcpy(szName, ptr);
int nDataBytes = (int) strlen(szName) + 1;

6
//add a new field “NAME” to help identify different objects
strcpy(optmsg->name[i], ptr);
/////////////////////////////
ptr += nDataBytes;
// marker data
int nMarkers = 0; memcpy(&nMarkers, ptr, 4); ptr += 4;
for(int j=0; j < nMarkers; j++)
{
float x = 0; memcpy(&x, ptr, 4); ptr += 4;
float y = 0; memcpy(&y, ptr, 4); ptr += 4;
float z = 0; memcpy(&z, ptr, 4); ptr += 4;
}
}
// unidentified markers
int nOtherMarkers = 0; memcpy(&nOtherMarkers, ptr, 4); ptr += 4;
for(int j=0; j < nOtherMarkers; j++)
{
float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4;
float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4;
float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4;
}
// rigid bodies
// int nRigidBodies = 0;
// nRigidbodies Globally declared in header
memcpy(&nRigidBodies, ptr, 4); ptr += 4;
{
// rigid body position/orientation
// forming arrays
memcpy(&(optmsg->ID[j]), ptr, 4); ptr += 4;
memcpy(&(optmsg->x[j]), ptr, 4); ptr += 4;
memcpy(&(optmsg->y[j]), ptr, 4); ptr += 4;
memcpy(&(optmsg->z[j]), ptr, 4); ptr += 4;
memcpy(&(optmsg->qx[j]), ptr, 4); ptr += 4;
memcpy(&(optmsg->qy[j]), ptr, 4); ptr += 4;
memcpy(&(optmsg->qz[j]), ptr, 4); ptr += 4;
memcpy(&(optmsg->qw[j]), ptr, 4);
ptr += 94; // 94 is a very important number, obtained from lots of test.
}

7
}
else
{
printf("Unrecognized Packet Type.n");
}
}
*The last increment of pointer ptr is 94, which exactly leads the pointer to the beginning
of the information of next object to be read. The number 94 came from multiple tests.
Therefore, optmsg contains position and orientation information of multiple objects.
The last step is to modify the top-level code motion_capture_standalone.c. This code is
executed on the top-level to start threads and store data in buffer, also to print the position and
orientation information of tracked objects. Function void *print_motion_capture(void
*userdata) needs modification.
• Original codes:
void *print_motion_capture(void *userdata){
struct motion_capture_obs mc;
/* Local copy of mcap_obs object (to update) */
while(1){
printf(stdout,"%lf,%lf,%lf,%lf,%lf,%lf,%lfn",mc.time,
mc.pose[0],mc.pose[1],mc.pose[2],mc.pose[3],mc.pose[4],mc.pose[5]);
fflush(stdout);
pthread_mutex_lock(&mcap_mutex);
memcpy(&mc, mcap_obs, sizeof(struct motion_capture_obs));
pthread_mutex_unlock(&mcap_mutex);
usleep(100000); /* 10Hz Hard coded for now */
}
return NULL;
}
• Modified codes (the dimension of mc increased):
void *print_motion_capture(void *userdata){
struct motion_capture_obs mc;
/* Local copy of mcap_obs object (to update) */
while(1){
{
printf("%lf,%d,%s,%lf,%lf,%lf,%lf,%lf,%lfn",mc.time,
mc.ID[j],mc.name[j],mc.pose[j][0],mc.pose[j][1],mc.pose[j][2],

8
mc.pose[j][3],mc.pose[j][4],mc.pose[j][5]);
fflush(stdout);
}
pthread_mutex_lock(&mcap_mutex);
memcpy(&mc, mcap_obs, sizeof(struct motion_capture_obs));
pthread_mutex_unlock(&mcap_mutex);
usleep(100000); /* 10Hz Hard coded for now */
}
return NULL;
}
The remaining file util.c does not need modification.
Method of running the object tracking codes:
1. Connect the camera system to power supply;
2. Run the Windows system and the OptiTrack software;
3. Build rigid body and broadcast its information on OptiTrack;
4. Run console on Linux (Raspbian on Raspberry PI);
5. Type command: cd ~/…/Camera/motioncapture_multi (… depends on the path of
Camera folder);
6. Type command: make clean (clean the existing binary executable files);
7. Type command: make (generate the new binary executable files);
8. Type command: cd bin;
9. Type command: ./motion_capture_standalone to run the object tracking system on
Linux.

9
II. A Simple Version of Estimator for MPC
Since there exist errors when determining the position and attitude of a satellite in the space due
to the limitation of accuracy of measurement instruments (sensors), we need an estimator in the
control model to make the measurement of the status more accurate. The estimator makes a
weighted average between the status of satellite calculated by mathematical model and the status
measured by sensors. In the Spacecraft Relative Motion Control Lab, the measurement instruments
are cameras and the corresponding software system, there are random and systematic errors
produced by the camera system. Therefore, I build a simple version of estimator for the MPC
Simulink model based on the error of camera system. The block diagram shows the working
principle of the estimator.
Fig. 1. Conceptual Estimator Block Diagram
Note that the Random block is an imitation source of measurement error produced by camera
system.
The reason for saying this is a simple estimator is that: 1. The source of inaccuracy is a simple
random function; 2. The mathematical model I used is the linear Hill-Clohessy-Wiltshire dynamic
model; 3. The method of average is a simple arithmetic average (for further study weighted average
could be applied).

10
This is the original MPC loop:
Fig. 2. Original MPC Loop by Christopher Peterson
This is the modified MPC loop with estimator:
Fig. 3. Modified MPC Loop with Estimator

11
This is the sub-system of the estimator block:
Fig. 4. Estimator Sub-system
Camera Simu function:
function x_cam = fcn(x)
x_cam = zeros(6,1);
for i = 1:1:6
if i<=3
rand_add=(rand(1,1)-.5)*2*.1; %scaled for distance
else
rand_add=(rand(1,1)-.5)*2*.0001; %scaled for speed
end
x_cam(i) = x(i) + rand_add;
end
end
Avg function:
function x_est = fcn(x_cam, x_mm)
x_est = zeros(6,1);
for i = 1:1:6
x_est(i) = (x_cam(i) + x_mm(i))/2;
end
return

12
This is the sub-system of the Hill-Clohessy-Wiltshire (linear) sub-system (MPC.A and MPC.B
are constants prescribed by the Simulink model):
Fig. 5. Hill-Clohessy-Wiltshire (linear) Sub-system
The following figures show the ideal MPC simulation results and the simulation results of MPC
with estimator. The latter results are more realistic and reasonable.
i. Ideal MPC simulation results
Fig. 6. Original MPC Simulation Trajectory

13
Fig. 7. Instantaneous Velocity Change in Radial Direction (ideal MPC)
Fig. 8. Instantaneous Velocity Change in In-Track Direction (ideal MPC)

14
ii. Simulation results of MPC with estimator
Fig. 9. Simulation Trajectory of MPC with Estimator
Fig. 10. Instantaneous Velocity Change in Radial Direction (MPC with estimator)

15
Fig. 11. Instantaneous Velocity Change in In-Track Direction (MPC with estimator)
III. Method of Running the Robot Control Code
The following steps give an instruction of the method of running the robot with the robot controller
and camera system:
1. Connect the camera system to power supply;
2. Run the Windows system and the OptiTrack software;
3. Build rigid body and broadcast its information on OptiTrack;
4. Connect the Arduino and Raspberry PI with USB cable;
5. Connect Raspberry PI to a portable power supply (5V, 1A), and wait for 30 seconds;
6. Have a computer connect to the Lab router (Jasper);
7. Remotely login to the Raspberry PI using SSH by typing ssh pi@10.0.1.4 in console
and the password is Yoda;
8. Locate the path of robot_controller folder;
9. Type cd ~/…/robot_controller/src in console;
10. Run the binary executable file by typing ./t1.out;
If the controller codes are modified, re-compiling is needed before running the robot:
1. Use console get to src level in the robot_controller folder;
2. Type make in the console and re-compiling will be accomplished;

16
For now, the robot is able to be guided to the origin of the coordinate system on the ground from
any position (only if detected by the camera system). There still exists problem guiding the robot
to move on a specific trajectory (e.g. a circular trajectory centered at origin).
VI. Automatic C Code Generation Using Simulink
Although it is very easy to auto generate C code with Simulink and a properly built model (just a
click of the Build Model button), it is the hardest part to be done. Since what we have in the
present Simulink model is a pure theoretical MPC model. If we want to have MPC C code that can
control robot, we will need to find out the interface between the robot controller and the MPC
Simulink model, and then we should be able to build the interface using s-function builder block
provided in Simulink. I need to learn more about both the s-function builder and the robot
controller code.

Lab Log Summer 2016 - Sheng Li

More Related Content

Lab Log Summer 2016 - Sheng Li