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 = %d\n", 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; for (int j=0; j < nRigidBodies; j++) { // 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,%lf\n",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){ for (int j=0; j < nRigidBodies; j++) { printf("%lf,%d,%s,%lf,%lf,%lf,%lf,%lf,%lf\n",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

CameraSimu 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.