TowardsaModel-BasedMotionControlDesignfora7-axisRoboticArmLWA4DbySchunk U U

(1)

U

MEÅ

U

NIVERSITY

M

ASTER

T

HESIS

Towards a Model-Based Motion

Control Design for a 7-axis Robotic

Arm LWA4D by Schunk

Author:

Roger Vila Cano

(2)

ii

Supervisors:

Leonid Friedovich and Szabolcs Fodor

(3)

iii

“The apple cannot be stuck back on the Tree of Knowledge; once we begin to see, we are doomed and challenged to seek the strength to see more, not less. ”

(4)

(5)

v

Abstract

This thesis presents an overview and realization of several steps to-wards system integration and the control design for the robot arm LWA4D by Schunk using MATLAB/Simulink blocks through the adopted hardware and software from dSpace company.

The dynamical equations for the manipulator using three and seven degrees-of-freedom will be derived and implemented using MATLAB/Simulink blocks. Moreover, the implementation of model-based controllers based on the system dynamics is implemented and tested both in ideal conditions and under the delays related to CAN communication protocol.

In addition, in order to visualize the motion of the manipulator under the model-based controllers, SimMechanics is used alongside plots showing the end-effector position and the generated torques. Information regarding the first steps on how the hardware and software should be set up in or-der to establish communication between the manipulator and the Simulink blocks is provided as well.

(6)

(7)

vii

Acknowledgements

I would like to start thanking one of my supervisors, Leonid Friedovich who gave me very good advices to improve the quality of this thesis, I highly appreciate his predisposition for discussion during all the thesis pe-riod.

Also, I would like not only to thank Szabolcs Fodor about all the support and the patience he had with me during the thesis period but also during all my master studies.

Thanks to all the friends that I have met during this period which we have shared experiences that I will remember all my life.

And last but not least to my family, for all their support during this pe-riod and specially to my grandma who have been and will be a source of inspiration to all my future projects.

(8)

(9)

ix

List of Figures

1.1 System Integration Schematic . . . 2

1.2 The LWA4D by Schunk (from the laboratory at Umeå Uni-versity) . . . 4

1.3 MicroAutobox II (from the laboratory at Umeå University) . 4 2.1 CANopen Network Example . . . 8

2.2 Schematic diagram of a calibration system . . . 9

3.1 ISO 7-Layer Reference Model for CANopen . . . 11

3.2 CANopen Reference Model . . . 12

3.3 Communication State Machine operation . . . 15

3.4 State Machine in System Context . . . 16

3.5 State Machine Block Diagram . . . 17

3.6 Robot Control Schematic . . . 18

4.1 Denavit-Hartenberg kinematic parameters . . . 21

4.2 The 3DOF manipulator structure . . . 23

4.3 The 7DOF manipulator structure . . . 24

4.4 Block scheme of joint space PD control with gravity compen-sation . . . 27

4.5 Block scheme of task space PD control with gravity compen-sation . . . 28

4.6 Block scheme of joint space computed torque method . . . . 29

4.7 Block scheme of task space computed torque method . . . . 30

4.8 Link modeled as a cylinder . . . 31

5.1 2DOF Planar Robot structure . . . 34

5.2 Dynamic Parameter estimation 2DOF planar Robot . . . 37

5.3 Data transmission in MATLAB/Simulink . . . 38

5.4 The 3DOF module configuration . . . 39

5.5 SimMechanics 3DOF Block Diagram . . . 42

5.6 SimMechanics representation for the 3DOF model . . . 42

5.7 PD+G Control in Joint Space results for T = 5 and p = 2.5 . . 44

5.8 PD+G Control in Joint Space results for T = 5 and p = 1.25 . 45 5.9 PD+G Control in Task Space results for T = 5 and p = 40 . . 46

5.10 PD+G Control in Task Space results for T = 5 and p = 20 . . 47

5.11 CTM Control in Joint Space results for T = 1.5 and p = 10 . 49 5.12 CTM Control in Joint Space results for T = 5 and p = 10 . . . 50

5.13 CTM Control in Task Space results for T = 1.5 and p = 10 . . 51

5.14 CTM Control in Task Space results for T = 5 and p = 10 . . . 52

5.15 Dynamic S-Function blocks . . . 54

(14)

xiv

5.19 7DOF CTM Control in Joint Space results for T = 3 and p = 25 58 5.20 7DOF CTM Control in Task Space results for T = 3 and p = 25 59

6.1 Schunk LWA4D connectors . . . 61

6.2 Dimensions of Schunk LWA4D power supply socket (Both sides) . . . 62

6.3 DIP switches in PRL+ driver . . . 63

6.4 DIP switches in ERB+ driver . . . 64

6.5 DIP switches in PG+ gripper . . . 65

6.6 Positions of Schunk LWA4D CAN and Serial sockets . . . 65

6.7 CAN terminator adaptor in accordance with CiA . . . 66

6.8 CAN assignment of 9-pin SUB D Socket . . . 66

(15)

xv

List of Tables

3.1 How to Map (or Remap) a PDO . . . 14

3.2 How to reach Operation Enable state . . . 17

3.3 General steps to perform robot control . . . 19

4.1 DH parameters for the 3DOF model . . . 23

4.2 DH parameters for the 7DOF model . . . 25

5.1 Module specifications . . . 33

5.2 Link dimensions . . . 33

5.3 2DOF Planar Robot parameters . . . 35

5.4 Estimated parameters vs Real value . . . 38

5.5 3DOF CAD Model Parameters . . . 40

5.6 7DOF CAD Model Parameters . . . 53

6.1 Pin allocation of Schunk LWA4D power supply connector . 62 6.2 DIP-Switch Configuration for PRL+ . . . 63

6.3 DIP-Switch Configuration for ERB 115 . . . 64

6.4 DIP-Switch Configuration for PG+ . . . 64

6.5 Pin allocation of Schunk LWA4D CAN connector . . . 65

(16)

(17)

1

Chapter 1

Introduction

1.1 Motivation and Preview

Industrial robotic manipulators are changing the concept of industrial pro-duction with the so called fourth industrial revolution (Industry 4.0) [1]. With more robots in our industries year after year [2], new methods of cooperation [3] and interactive programming [4],[5] are being developed. Beyond the fact that humans and robots will soon coexist in a daily ba-sis (human-robot co-existence society is already being prepared in many countries [6]), there is an increasing need for more user-friendly ways to program and optimize robot manipulators operations in the industries. Robotics is an interdisciplinary field that contains elements of mechanical engineering, electrical engineering and computer science. The process of building a robot contains elements related to the design, construction and computer systems for the control, sensory feedback and information pro-cessing [7].

Despite a lot of open source materials for implementing different type of robots systems are available on Internet such as ROS [8], in order to modify the provided code and implement new types of controllers (i.e model based controllers) one requires a deep understanding of the codes, advanced knowl-edge about the communication protocols and a good understanding about control theory and how this could be applied on a real system.

This thesis work is focused on the robot manipulator LWA4D produced by Schunk and on the development of an easy-to-use rapid control proto-typing software/hardware bundle and exploration of the possibility of im-plementing a model based motion control based on its dynamical model. Furthermore, in order to decrease the complexity of implementing differ-ent controllers in C++ or ROS programming, the possibility of using MAT-LAB/Simulink to perform fast prototyping and testing such controllers is also considered and studied. The proposed solution is using the hardware MicroAutobox II from dSpace to send the control signals, designed using Simulink blocks, to the arm modules on-the-fly using the CANopen proto-col implemented on the LWA4D drivers and to actively visualize the signals and interact with control system’s parameters on-line.

(18)

2 Chapter 1. Introduction

1.2 Goals and proposed solutions

This project aims to perform system identification experiments with a Schunk robotic arm and after that, test some model-based controllers. The results should be analysed both by simulations and using the physical system. The goals defined for this project are listed below.

1. Set up and adapt the hardware and software from the Schunk robotic arm: perform wiring for connecting the robot to a power source and to dSpace software/hardware bundle. Study the dSpace software as an example of a rapid control prototyping system.

2. Develop a dynamical model for the reduced 3-axis arm (with four out of seven motors either mechanically blocked of frozen by high-gain regulators) in Matlab/Simulink.

3. Propose experiments for verification of the dynamic model for the reduced system using recorded experiment data. If necessary, suggest and perform calibration experiments.

4. Propose a model-based stabilizing controller for a tracking task for the system with 3-axis and verify it in simulation and experiment. 5. Prepare literature survey on identification of parameters and

calibra-tion for industrial robotic manipulators, especially for similar robotic arms.

1.3 System Integration

This section shows how all the elements mentioned before should be con-nected. Figure 1.1 gives an overview to the user on how the system integra-tion should be performed. Notice that more detailed explanaintegra-tion on how the elements should be set up is described in Chapter 6.

FIGURE1.1: System Integration Schematic

When using MATLAB to design a controller capable to communicate with the Schunk arm, one can follow the next steps:

(19)

1.4. The LWA4D by Schunk 3

2. Using Control Desk, the code from MATLAB is converted to a real-time application linked to the MicroAutobox II microprocessor (ECU) through the Ethernet card.

3. Once the application is in Online mode, the signal can be sent to the MicroAutobox through the Ethernet cable.

4. The MicroAutobox then converts the signal into CANopen format and sends it to the LWA4D through a CANopen cable.

5. The LWA4D receives the signal and responds according to the CANopen communication protocol pre-installed on each of its modules.

1.4 The LWA4D by Schunk

The Schunk LWA4D Dexterous Lightweight Arm is one of the most com-pact and powerful modular lightweight arms. The arm can be adapted to various tasks depending on which application is required being able to move heavy loads of up to 10 kg [9].

Moreover, the arm can be combined with extra sensors and gripping sys-tems making it an ideal candidate for several applications. Checking the documentation [9], some of the main advantages for this arm model are listed below.

• Suitable for mobile applications using low energy consumption at 24 V DC.

• High torque, speed and repeat accuracy for rapid acceleration. • Short cycle times and high process stability.

• Complete integration of control and power electronics with already integrated protocols for drivers such as CANopen.

• Internal cabling with free wires and able to incorporate new mod-ules without disruptive cables.

• Manoeuvrable and flexible where the 7 axes provide exploitable re-dundancy for kinematic calculations.

1.4.1 Hardware for the LWA4D by Schunk

The arm LWA4D requires 24V using an average intensity of 5A on its mod-ules reaching maximum values of 15A. For that reason, in order to provide enough power, a powerful power supply PULS CPS20 24V AC/DC 20A [10] is used.

1.5 The MicroAutoBox II

MicroAutoBox R _{II [11] is a real-time system provided by dSPACE for}

per-forming fast function prototyping. It can be used for many rapid control prototyping applications such as motor speed control.

(20)

FIGURE1.2: The LWA4D by Schunk (from the laboratory at Umeå University)

Using the MicroAutoBox II R _{hardware, it is possible to program some}

of its functionalities with MATLAB/Simulink blocks making it ideal for real-time applications that uses controllers designed in Simulink blocks for instance.

FIGURE1.3: MicroAutoBox II (from the laboratory at Umeå

University)

1.5.1 Hardware for the MicroAutobox II

In order to use the MicroAutobox II, we require some extra hardware: a suitable power supply and an Ethernet PCI card plugged into the host PC that will handle the communication between the dSpace software (through MATLAB/Simulink blocks) and the MicroAutobox II.

(21)

1.6. System Set Up Overview 5

is used to power up the MicroAutoBox II in order that the power require-ments are satisfied. For the Ethernet card, the model D-Link DFE-530TX

PCI Fast Ethernet Adapter [13] is used to connect the MicroAutoBox II with

the host PC. How the card is integrated with the other system elements is explained in further sections.

1.5.2 Software for the MicroAutobox II

In order to use the MicroAutobox II with MATLAB/Simulink, some MAT-LAB toolboxes and extra software from dSPACE should be installed in ad-vance.

MATLAB Software

The required MATLAB software that should be installed on the host PC is listed below.

• Simulink

• Simulink R _CoderTM

• Compatible toolboxes might be installed depending on the used dSPACE release such as Aerospace Blockset, Communications System Toolbox, Control System Toolbox, DSP System Toolbox, Fixed Point Designer, Fuzzy Logic Toolbox or Simulink Control Design to name a few. Refer to [14] for more information.

dSPACE Software

In addition, the following software provided by dSPACE should be in-stalled on the host PC as well.

• ControlDesk Next Generation: (dSPACE ControlDesk 5.1 & dSPACE MCD3Client 5.1).

• dSPACE Application: CANopen Master (64-bit). • dSPACE Common: dSPACE CAN API 2.7.

• dSPACE Firmware Manager: dSPACE Firmware Archives 1.0 & dSPACE Firmware Manager 1.0.

• dSPACE HIL API .NET: dSPACE HIL API .NET 1.5.

• dSPACE Python Extensions: dSPACE Python Extension 1.5. • RCP & HIL: dSPACE RCP & HIL x64 Software RLS2013-B. • Real-Time Testing: dSPACE Real-Testing 2.2.

1.6 System Set Up Overview

This section provides an overview of all the required steps for develop-ment of a working rapid control prototyping system such as the LWA4D by Schunk. In addition, we will mention which of them have been fully and partially finished and described in the thesis’s chapters.

(22)

1. The first step is to connect the arm to a suitable power supply and check the correct functionality of its modules. In addition, some wiring is required regarding the CAN connector. This steps are described in Chapter 6, Section 6.1.

2. After that, we should set up the MicroAutoBox II installing all the required software (Section 1.5.2) and setting up communication with the host PC (Section 6.2).

3. Build the CANopen Blockset Blockset shown in Section 6.3 before establishing communication with the arm modules. The CANopen communication protocol is explained in Chapter 3.

4. Initialize the arm modules according to the device profiles for robot applications explained in Section 3.2 (Communication between code generated with MATLAB/Simulink and the arm modules needs to be fixed in further studies).

5. Derive a solution for the forward kinematics problem for the arm. In Section 4.1.2, a reduced model kinematics (using three of the seven modules) is shown. If we want to use all the modules, observe that the forward kinematics for the complete model is shown in Section 4.1.3. 6. Derive a solution for the inverse kinematics. This thesis only provides

the inverse kinematic solution for the reduced model (Section 4.1.2). 7. Using the general kinematic expressions, the next step is to derive a

dynamic model for the arm using the information from Section 4.2. 8. Perform parameter identification experiments on the arm either by

shape approximation (Section 4.3.4) or using dynamic parameter iden-tification (Section 4.3.4). A dynamic parameter ideniden-tification example based on the LWA4D (using 2DOF) is provided in Section 5.1.

9. Build the model-based controllers in MATLAB/Simulink according to Section 4.3 using the previously identified parameters.

10. In order to avoid undesirable behaviours from the LWA4D due to communication limitations (check Section 5.2), analyse the performance of the motion control using simulations of a dynamic model in MAT-LAB/Simulink. This is shown in Section 5.3 for the reduced model and in Section 5.4 for the complete model (further studies on the pos-sible effects due to the communication are required for the 7DOF case).

(23)

7

Chapter 2

Literature Review

2.1 Robot Manipulators

Robot manipulators are mechanical devices automatically controlled, ver-satile enough to be programmed to perform in different applications, re-programmable, with several degrees of freedom and the ability to use them with different tooling [15].

They appeared during the industrial revolution due to an increasing de-mand for production and therefore, motivation for automation. The inven-tion of the first numerical controlled machines, the arrival of computers and integrated systems made possible the development of the first industrial robots. Joseph Engelberger [16] is often called the father of robotics that developed the first industrial robot called the Unimate in 1954 originally patented by George Charles Devol [17]. In early 1980s, robot arms were pro-claimed as the ultimate solution to automated manufacturing. However, at that time the savings in labor costs often did not outweigh the creation of robot systems [18].

In recent years, new technologies and deeper understanding of the subjects made robotics a key element in modern industries and nowadays robots are commonly used in applications such as welding, material handling, drilling, assembly or laser cutting for instance [19].

2.2 Communication protocols

Robot applications use a wide range of communication protocols. The Com-mon Industrial Protocol (CIP) family is managed by ODVA [20] (global as-sociation whose members comprise the world’s leading automation com-panies) and integrates control services, communication services [21], and routing capabilities based on Ethernet network and the Internet. The main industrial communication protocols are:

1. Ethernet/IP: Protocol built on standard TCP/IP (IEEE 802.3) and com-munications use existing network infrastructure. The main advantage is the continuous progress of physical Ethernet technology. Commu-nication speed goes from 10Mb/s to 1Gb/s or more. Moreover, this communication eases remote control through Ethernet network. 2. ControlNet: It is built on its own physical and data link layer. It

(24)

8 Chapter 2. Literature Review

3. DeviceNet: It uses the Controller Area Network [22] (CAN bus) as a support for physical and data link layer. Speeds goes from 1Mb/s at 40m to 20Kb/s at 1200m. CANopen (higher layer protocol based on CAN protocol) uses the master/slave mode and can have up to 127 nodes. Figure 2.1 shows the master/slave architecture used by the DeviceNet communication technique.

FIGURE2.1: CANopen Network Example

2.3 Embedded Control Systems

By definition an embedded system is a computer system with a dedicated function within a larger mechanical or electrical system such as a robotic manipulator. One of the many applications are the embedded control sys-tem that compute control algorithms implemented into such robotic appli-cations.

In robotics, we can differentiate two general domains regarding design re-quirements [23]:

1. Close integration: Involving planning, sensing, control and mod-elling.

2. Interactions: Related to task and its environment.

An illustrated example of an embedded control system can be observed from Figure 1.1 where the host PC controls the manipulator through the external hardware called MicroAutoBox.

2.4 Calibration for Industrial Robots

(25)

2.4. Calibration for Industrial Robots 9

model-less and model-based. Those two methods do not require any exter-nal sensor device but they do require measurements from the end-effector to a target with a known position.

1. Model-less method: The actual pose error is investigated by point to point in the robot’s workspace to set up the error compensation model depending on a set of a measuring system.

2. Model-based method: Can be used to improve the accuracy in the en-tire workspace. The four main procedures are: modelling, measuring, identification and implementation.

All those calibration techniques are based on adding an extra kinematic factor called kinematic error into the conventional kinematic equations. Mod-elling the error of a system is rather complicated and there are many ways of doing so, such as using the generalized kinematic error model by Oka-mura and Park in 1996 [26]. Keep in mind that the end-effector position is found using direct kinematics (using the joint variables) which has many parameters that could be responsible for the inaccuracies. Figure 2.2 shows a possible set up for calibrating a manipulator.

FIGURE2.2: Schematic diagram of a calibration system

Another calibration technique suggested by [24] uses a laser tracker placed on the end-effector that measures the rate of change in XYZ coor-dinates and compare it with the expected value. The error from each joint is measured trough independent joint rotations and therefore, new models incorporating those errors shall be derived.

(26)

(27)

11

Chapter 3

CANopen Communication

Protocol

3.1 CANopen Elements

CANopen is a communication protocol for embedded systems used in au-tomation [22]. This protocol implements the so called network layer accord-ing to the OSI model [27]. The addressaccord-ing Scheme 3.1 is constituted usaccord-ing several small communication protocols and an application layer defined by a device profile.

FIGURE3.1: ISO 7-Layer Reference Model for CANopen

CAN-in-Automation (CiA) is a non-profit organization controlled by several companies around the world. This company has been publishing CANopen standards along with diverse device profiles. The basic CANopen application and communication profiles are given in the CiA 301 and CiA 302 specification release by CAN in Automation [28][29].

Furthermore, devices incorporate a higher level protocol called device pro-files. Those device profiles are based on the application layer and com-munication profile DS301 [28] and/or the framework for programmable CANopen devices DSP 302 [29]. Therefore, depending on the type of de-vice that it is connected (i.e I/O modules, drives and motion control...etc) this higher layer called device profile is incorporated on top of the commu-nication protocol profile.

(28)

12 Chapter 3. CANopen Communication Protocol

FIGURE3.2: CANopen Reference Model

3.1.1 Device Profiles

The CANopen Device Profiles provide a high level of standardization be-tween devices of the same nature [22]. The Device Profile objects may in-clude process commands, process data or executable functions. Some of the Device Profiles developed by CiA are:

• DS401: Generic I/O Modules • DSP402: Drives and Motion Control

• DS405: IEC 61131-3 Programmable Devices • DS406: Encoders

• DS412: Medical Devices • Others

3.1.2 Object Dictionary

The Object Dictionary (OD) is the main element in all the CANopen de-vices. These ordered grouping of objects that can be accessed from the net-work through 2-byte index plus a 1-byte sub-index via receive and transmit Service Data Objects [28]. In addition, some part or all the object can be mapped to transmit and receive Process Data Objects in a predefined man-ner explained in Section 3.1.6.

The OD contains all the parameters describing the device and its network behaviour such as [30]:

• Index: The object dictionary index.

• Object: The object type such as variable, array...etc. • Name: The name of the entry.

(29)

3.1. CANopen Elements 13

• Category: Indicating if the entry is mandatory, optional or condi-tional.

An example for the LWA4D manipulator is the object called

communi-cation_ modestored at index 0x200c/0 used to define the type of

commu-nication that should be used by each module (CANopen in our case).

3.1.3 Service Data Objects

Service Data Objects also known as SDOs are principal objects in CANopen, which allow to access the data kept in a node’s object dictionary. Further-more, an SDO can be used to write the configuration of data to a node’s object dictionary [30].

3.1.4 Process Data Objects

Process Data Objects or PDOs are used to transfer real-time data from one producer to one or more consumers. There are two types of PDOs: the Rx-PDO for received and TxRx-PDO for transmitted data.

3.1.5 Transmission methods

There are three methods to arrange transmission:

• Event drive: The transmission of a PDO is triggered by the occurrence of an event. The definition of an ”event” is usually defined by the device profile. A typical event is a change in the process data.

• Timer driven: If an event has not occurred within a specified time frame, an event is triggered by a timer and a PDO is transmitted.

• Synchronized: The synchronized polling mode is intended for ap-plications where it is crucial to get all inputs at the same time and apply all outputs at the same time. To achieve this, a SYNCH signal is transmitted, usually by the network management master, on a fixed time basis. All synchronized nodes keep their transmit buffers up-dated with the current data. Reception of a SYNCH message triggers transmission of the data.

The synchronous method is used in the considered arm.

3.1.6 Mapping Parameters

(30)

TABLE3.1: How to Map (or Remap) a PDO

Stage Step Description

1 Disable the PDO In the PDO’s mapping object (i.e 0x1601), set the sub-index 0 to zero.

2 Set the communication pa-rameters

If necessary, set PDO’s ID and transmis-sion type (i.e 0x1401) using sub-index 1 and 2 respectively.

3 Map the data Using PDO’s mapping object (i.e 0x1601), set sub-indexes 1-4. A maximum of 4 ob-jects can be mapped containing data no longer than 8 bytes.

4 Set the number of mapped objects and enable the PDO

In the PDO’s mapping object (i.e 0x1601), set the sub-index 0 to the actual number of objects mapped.

Notice that the structure of the predefined PDO’s varies between Rx-PDO1 and RxPDO2 or TxRx-PDO1 and TxPDO2 for instance. Therefore, it is essential to check the documentation [28] to see how many sub-index has each predefined PDO and if it is required to remap it or on the other hand, if it is possible to use some predefined mapping.

(31)

3.2. Device Profiles for Robot Applications 15

3.1.7 Network Management

Network management (NMT) [22] in CANopen determines the master/slave relationship controlling the state transitions. The NMT state machine de-termines the state of communication level enabling the SDO and PDO ob-jects sequentially. According to [28], the following network communication states are supported:

• Initialization: The node performs its internal initialization and will not respond to communication nor transmit anything.

• Pre-operational: The node’s internal configuration has finished and will only respond to SDO and NMT messages.

• Operational: The node is fully operational and will also respond to PDO messages.

• Stopped: SDO and PDO messages are disabled. The node will only respond to NMT messages.

Using the states mentioned above, the state machine in Figure 3.3 (cour-tesy from Elmo Motion Control) shows the allowed state transitions.

FIGURE3.3: Communication State Machine operation [28]

When powering on the servo drive, the initialization state is reached. After the boot sequence is completed, the NMT moves to pre-operational state. Notice that the transitions between pre-operational, operational and stopped is carried out with NMT messages. For more information, the com-mands used for this purpose are specified in [28].

3.2 Device Profiles for Robot Applications

(32)

3.2.1 Device Profile for Drives and Motion Control DS402

The CiA Draft Standard Proposal DSP402 provides standardized CANopen device profile for motion control products such as servo controllers, fre-quency converters or stepper motor controllers [22].

In order to control the servos (acting as slaves) from Schunk LWA4D, the DSP402 protocol should be also used by the CANopen Blockset (see Sec-tion 6.3) using the MicroAutoBox II (acting as a master).

The most characteristic objects used for device control are the controlword and statusword. The controlword is used to control the state of the device while the statusword describes the actual status of the device. The access to both objects should be always allowed and therefore they must be mapped as PDO messages.

FIGURE3.4: State Machine in System Context [31]

(33)

FIGURE3.5: State Machine Block Diagram [31]

Figure 3.5 (courtesy from Elmo Motion Control) shows the allowed tran-sitions between states. In order to operate using a point-to-point motion, the state Operation Enable should be reached using the commands Shut Down, Switched ON and Enable Operation corresponding to the bits 0-3 from the controlword. The Table 3.2 shows how to reach the state Operation Enable once the device is initialized and no errors are detected.

TABLE3.2: How to reach Operation Enable state

Step Description ControlWord (0x6040)

0 Start→ Not Ready to Switch ON Reached when power on. 1 Not Ready to Switch ON→ Switch

ON Enabled

Reached when power on.

2 Switch ON Enabled → Ready to Switch ON

Shut Down (0x06) command sent.

3 Ready to Switch ON → Switched ON

Switch ON (0x07) command sent.

4 Switched ON→ Operation Enable Enable Operation (0x0F) command sent.

(34)

3.2.2 Robot Control according to DS402

One of the most common modes of operation used by Schunk LWA4D is called Interpolated Position Mode. It is designed for the coordinated control of multi-axis systems in the sense of a robot control system. The Interpolated Position Mode is described in more detail by [31]. However, in Figure 3.6 the general steps for performing such control are shown.

FIGURE3.6: Robot Control Schematic [33]

(35)

TABLE3.3: General steps to perform robot control

Step NMT State Description

1 Initialization Configure desired PDOs. As the use of ControlWord and StatusWord is required, 2 RxPDO and 2 TxPDO need to be used. 2 Initialization Configure parameters. The sync time,

baud rate or node guarding among other parameters might be selected in this stage.

3 Pre-operational Select Mode of Operation. Via SDO, the mode of operation (0x6060) Interpolated Position Mode (0x07) is selected.

4 Pre-operational Get device into state Operational through NMT messages.

(36)

(37)

21

Chapter 4

Dynamic Modeling and Motion

Control

4.1 Kinematic analysis

Before the dynamic model equations can be derived, the kinematic proper-ties need to be extracted. In order to successfully implement a model-based controller, the forward kinematics from the manipulator shall be described first. For this purpose and as it is widely used in many applications, the Denavit Hartenberg (DH) parameters are defined in the following sections.

4.1.1 General Kinematics Expressions

In order to derive the forward kinematic expressions, the DH convention is going to be used. Figure 4.1 (courtesy from Leonid Freidovich) shows the kinematic parameter notations used by the following sections.

FIGURE4.1: Denavit-Hartenberg kinematic parameters [34]

Forward Kinematics

Forward kinematics define the end-effector position and orientation from specific values for the joint variables.

(38)

22 Chapter 4. Dynamic Modeling and Motion Control i−1_T i =    

cos(θi) − sin(θi) cos(αi) sin(θi) sin(αi) aicos(θi)

sin(θi) cos(θi) cos(αi) − cos(θi) sin(αi) aisin(θi)

0 sin(αi) cos(αi) di 0 0 0 1     (4.1)

Once the respectivei−1Timatrices are found, the different matrices shall

be combined in order to obtain the homogeneous transformation from base to end-effector such as

0_T

i =0 T1. . .i−1Ti. (4.2)

The position and rotation from the end-effector are extracted from (4.2) and as a result solving the forward kinematics problem.

Velocity Kinematics

The velocity relations between the end-effector and joints is established by the manipulator Jacobian. This matrix is very important in the analysis and control of robot motion as it is used to determine singular configurations (when det(J) = 0), to find the dynamic expressions or for the forward and inverse kinematics in terms of velocities. In (4.3), the analytical Jacobian expression is shown. J =      ∂x ∂θ1 · · · ∂x ∂θn ∂y ∂θ1 · · · ∂y ∂θn ∂z ∂θ1 · · · ∂z ∂θn ρ0z00 · · · ρn−1zn−10      (4.3)

Where n is the total number of joints and ρ = 0 for prismatic joints and

ρ = 1 for revolute joints. Notice that the position   x y z 

is taken from the last

column of0Tn.

From [35] the end-effector can be represented as a function of joint coordi-nates such as X = X(q) ˙ X = J(q) ˙q ¨ X = J(q)¨q + ˙J(q) ˙q. (4.4)

4.1.2 Reduced Model Kinematics

(39)

4.1. Kinematic analysis 23

FIGURE4.2: The 3DOF manipulator structure

Direct kinematics for the reduced system

From Figure 4.2, the DH parameters used for the direct kinematic analysis are obtained in Table 4.1.

TABLE4.1: DH parameters for the 3DOF model

Jointi αi ai di θi

1 ₋π

2 0 L1 θ∗1

2 0 L2 0 θ∗2

4 0 L3+ L4 0 θ∗4

Inverse kinematics for the reduced system

The use of the inverse kinematics is required in order to work with positions in world frame. In further sections, we will see that the motion control can be performed either in joint or task space. In both cases, the provided ref-erences should be the same with the difference that when using joint space controllers, the reference position should be converted into joint space vari-ables in advance.

The expressions used for the 3DOF model inverse kinematics from [34] will be used in simulations.

Using the expressions represented by (4.5), θ1, θ2 and θ4 are obtained for a

given position (x, y, z) of the end-effector or tool-center point (TCP).

θ1= tan−1

y x

(40)

24 Chapter 4. Dynamic Modeling and Motion Control cos θ4 = x2+ y2+ (z_{− L}1)2− L22− (L3+ L4)2 2L2(L3+ L4) θ4 = tan−1 ±√1− cos θ4 cos θ4 θ2 = tan−1 p x2_{+ y}2 z_{− L}1 ! − tan−1 (L3+ L4) sin θ4 L2+ (L3+ L4) cos θ4 (4.5)

4.1.3 Complete Model Kinematics

The Schunk LWA4D robotic arm is composed of seven rotational modular joints. An illustration on how the joint are distributed is shown in Fig-ure 4.3.

FIGURE4.3: The 7DOF manipulator structure

Observe that, in order to be able to follow the DH rules, origins of the frames for some links shall be combined (dashed lines) with each other.

Direct kinematics for the complete system

Considering the base coordinate frame (x0, y0, z0), the lengths used for the

(41)

4.2. Dynamic Robot Modelling 25

TABLE4.2: DH parameters for the 7DOF model

Jointi αi ai di θi 1 ₋π₂ 0 L1 θ∗1 2 π 2 0 0 θ∗2 3 ₋π 2 0 L2 θ ∗ 3 4 π₂ 0 0 θ∗₄ 5 ₋π 2 0 L3 θ ∗ 5 6 π₂ 0 0 θ∗₆ 7 0 0 L4 θ∗7

4.2 Dynamic Robot Modelling

The dynamical model of a nthDOF manipulator under some standard as-sumptions, in particular neglecting friction, can be represented as

Mn×n(θ)¨θn×1+ Cn×n(θ, ˙θ) ˙θn×1(θ) + Gn×1(θ) = τn×1 (4.6)

where M (θ) is a positive Inertia matrix, C(θ, ˙θ) is the Coriolis matrix, G(θ) is the Gravity vector and τ is the vector of joint torques. Furthermore, notice that Mn×n(θ) and Cn×nare not independent from each other and can

be chosen so that the matrixM˙n×n− 2Cn×n

is skew symmetric.

4.2.1 Computation of the Inertia Matrix

The inertia matrix derived in [35.p 199] is shown by (4.7).

M (θ) = n X i=1 J_ωT_iRiIiRiTJωi + mi J T viJvi (4.7)

M (θ) is (n× n) and Symmetric, Positive definite and Configuration depen-dent (in general). In addition the elements contained in that matrix are listed below.

• Jviand Jωi: Obtained from the Jacobian matrix.

• Ri: Rotation matrix.

• Ii: Inertia matrix for each link.

Notice that for dynamic purposes the Jacobians, rotations and inertias should be calculated with respect to the center of gravity of each link.

4.2.2 Computation of the Coriolis Matrix

(42)

26 Chapter 4. Dynamic Modeling and Motion Control Cij(θ, ˙θ) = 1 2 n X k=1 ∂Mij ∂θk +∂Mik ∂θj − ∂Mkj ∂θi ˙θk (4.8)

From (4.8), the Christoffel symbols corresponding to the matrix M (θ) are used to find the C(θ, ˙θ) depending on θ and ˙θ.

4.2.3 Computation of the Gravity Vector

The remaining term is the so called gravity vector that relates to the po-tential energy of the manipulator. Using (4.9), the popo-tential energies are computed. U =₋ n X i=1 mighi (4.9)

where mi is the mass of each link, g is the gravitational constant value

(i.e g = 9.81m/s2 for Earth) and hi corresponds to the z coordinate taken

from each0TCGi derived from each link center of gravity.

G(θ) =    ∂U ∂θ1 .. . ∂U ∂θn    (4.10)

From (4.9), the gravity vector (4.10) for the robot manipulator is ob-tained.

4.3 Motion Control

Several motion control techniques can be applied on a robot manipulator. In this section two methods are shown: PD controller with gravity compen-sation and the computed torque method. Both are model-based techniques that use the dynamical equations in order to find suitable controllers. The equations derived in the following section are provided by [35], [36] and [37].

4.3.1 PD Control with Gravity Compensation

The control law for the ith_{joint will have the following form}

τP DGi = KPi ei(t) + Tdi dei dt + Gi(θi) (4.11)

where KPi > 0 represents the proportional gain and Tdiis the derivative

time and ei is an appropriately defined error signal. Remember that the

dynamical model of an nthDOF manipulator is shown in (4.6). When using (4.11) with (4.6), the dynamical expression (4.12) for the PD control with gravity compensation (PD+G) is obtained.

Mn×n(θ)¨θn×1+ Cn×n(θ, ˙θ) ˙θn×1(θ) + Gn×1(θ) = τP DGn×1. (4.12)

(43)

4.3. Motion Control 27 τP DG =    τ1 .. . τn   = (KP)n×n       e1 .. . en   + (TD)n×n    ˙e1 .. . ˙en      +    G1 .. . Gn    (4.13)

where (KP)n×nand (TD)n×n contain the controller gains on the

diago-nal elements of the matrices.

KP KD MANIPULATOR ˜ θ θref τ ˙θ θ G(·)

FIGURE 4.4: Block scheme of joint space PD control with gravity compensation

Based on the Lyapunov’s stability theorem, an energy like function [38] for the dynamical system described in (4.12) is defined by (4.14).

V (x) = 1 2˙θ T_{M (θ) ˙θ +}1 2e T_K Pe (4.14)

where M (θ) > 0, KP > 0, e = θ− θref, θref is a reference joint position

and x = e_˙θ

. In order to show stability, it is required that ˙θ → 0 and the positioning error e→ 0 as t → ∞, therefore it is needed to show that ˙V ≤ 0. Using product rule of differentiation and the identity ˙θ = ˙e we obtain

˙

V (x) = ˙θTM (θ)¨θ +1 2 ˙θ

T_{M (θ) ˙θ}_˙ _{− ˙θ}T_K

Pe. (4.15)

Substituting (4.11) into (4.12), M (θ)¨θ is defined in the next expression. M (θ)¨θ =_{−C(θ, ˙θ) ˙θ + K}Pe + KPTD˙e (4.16)

Finally, (4.15) is rewritten after substituting M (θ)¨θ from (4.16). ˙

V (x) =_{− ˙θ}TKPTD˙θ

In ˙V (x) if we chose KPTD as a diagonal matrix with positive elements,

(44)

28 Chapter 4. Dynamic Modeling and Motion Control

˙

V (x) =− ˙θT_K

PTD˙θ ≡ 0 ⇒ ˙θ ≡ 0, ˙e ≡ 0, ¨θ ≡ 0 ⇒ KPe≡ 0. PD Control with Gravity Compensation in task space

Similarly with the joint space stability analysis, given a desired end-effector position Xref, it is desired to find the control structure with a task space

error

˜

X = Xref − X = Xref − k(θ) (4.17)

that tends asymptotically to zero. In this case, k(θ) corresponds to the direct kinematic calculations while J(θ) is the Jacobian matrix used to find the velocity ˙X in task space from ˙X = J(θ) ˙θ.

The control law used for this method is defined by

τ = JThKP(Xref − X) − KDX˙ i + G(θ). (4.18) KP KD MANIPULATOR ˜ X Xref τ ˙θ θ G(·) J(θ) k(·) JT_(θ) X ˙ X

FIGURE 4.5: Block scheme of task space PD control with gravity compensation

4.3.2 Computed torque method

The PD+G controller can be used only in the case of point to point (PTP) tra-jectories without having an influence to the transient effects. On the other hand, a model-based controller such as the computed torque method [39] can be used for trajectory tracking where it is possible to adjust the be-haviour of the transients.

In this method, the control law used for (4.6) is defined by (4.19).

τ = M (θ)τlinear+ C(θ, ˙θ) ˙θ + G(θ) (4.19)

With the control law from (4.19), the manipulator dynamical model (4.6) is modified in equation (4.20).

M (θ)¨θ + C(θ, ˙θ) ˙θ + G(θ) = M (θ)τlinear+ C(θ, ˙θ) ˙θ + G(θ) (4.20)

(45)

4.3. Motion Control 29

τlinear = ¨θref+ KD( ˙θref − ˙θ) + KP(θref − θ) (4.21)

where ¨θ = τlinearand the tracking error is defined as e = θref − θ.

The stability conditions will be achieved when ¨e + KD˙e + KPe = 0 is

sta-ble, that is when both matrices are positive definite, for example diagonal matrices with positive elements on the diagonals. In addition, in order not to excite additional oscillatory transients (not advised in robotic systems) the system roots should not contain complex parts. Therefore, the roots can have the form of (4.22).

s2+ KDis + KPi = (s + p) 2 _{= s}2_{+ 2ps + p}2 _(4.22) where KDi = 2p, KPi = p 2_{and p > 0.} KP KD MANIPULATOR ˜ θ θref τ ˙θ θ G(·) ˙θref ˜˙θ M (·) C(·) × × ¨ θref ˙θ θ τlinear

FIGURE 4.6: Block scheme of joint space computed torque method

Computed torque method in task space

The computed torque method can also be used when the reference trajec-tory is given in the task space. In this case, the reference trajectrajec-tory is a component vector Xref. The tracking error will be

eX = Xref − X

where Xref is the reference trajectory and X is the measured position of

the end-effector which is calculated with the help of the inverse kinematics. Similarly as with (4.20), the dynamic equation for the computed torque in workspace controller [34] is shown in (4.23).

(46)

30 Chapter 4. Dynamic Modeling and Motion Control KP KD MANIPULATOR ˜ X Xref τ ˙θ θ G(·) ˙ Xref ˜˙ X M (·) C(·) × × ¨ Xref ˙ X X τX × J(·) k(·) × ˙ J(θ) × ˙θ J−1(θ)

FIGURE4.7: Block scheme of task space computed torque method

4.3.3 Linear Trajectory with Fifth Order Polynomial

In Section 4.3.2 we have seen that in order to use the computed torque method, it is required to provide θref, ˙θref and ¨θref. Therefore, three

func-tions of time related with desired (reference) position, velocity and acceler-ation need to be generated [40] for each joint variable with specific initial and final conditions.

The linear trajectories with fifth order polynomials allow us to generate tra-jectories with the six predefined conditions. The following matrix can be used A =          1 t0 t20 t30 t40 t50 0 1 2t0 3t20 4t30 5t40 0 0 2 6t0 12t20 20t30 1 tf t2f t3f t4f t5f 0 1 2tf 3t2_f 4t3_f 5t4_f 0 0 2 6tf 12t2f 20t3f          . (4.24)

In order to generate the trajectories within a period of time, the matrix A (4.24) shall be generated where t0 is the initial time and tf the final time.

The size of the matrix A has to be as large as many conditions we would like to introduce (i.e when using jerk conditions, the matrix A should be 8_{× 8 for that case).}

a =    a0 .. . a5   = A −1          ¨ θf ˙θf θf ¨ θ0 ˙θ0 θ0          (4.25)

Using (4.24), the polynomial constant elements are obtained using (4.25).

(47)

4.3. Motion Control 31

˙θref = 5a0t4+ 4a1t3+ 3a2t2+ 2a3t + a4

¨

θref = 20a0t3+ 12a1t2+ 6a2t + 2a3 (4.26)

Finally, as the name of the trajectory specifies, the reference trajectory is in the form of a fifth order polynomial. Using (4.26), the reference trajecto-ries for position, velocity and acceleration can be obtained either in joint or task space.

4.3.4 Parameter Identification

In order to derive the dynamical equations to design a model-based con-troller, parameter identification needs to be performed. There are several ways to perform parameter identification and in this section some possible approaches will be presented.

Parameter Estimation through Shape Approximation

One of the advantages of the LWA4D is the cylindrical shape for most of its modules (Figure 1.2). Having such shape and joint distribution, the param-eter estimation becomes simplified.

The center of gravity of a cylinder with uniform mass density is placed on the geometric center of the cylinder and therefore, some values used in fur-ther modelling are extracted from Figure 4.8. This case can be applied for all Schunk modules.

FIGURE4.8: Link modeled as a cylinder

Using this link approximation, both centres of gravity and inertia matri-ces are obtained. The inertia matrix [41] for each link according to Figure 4.8 is defined by (4.27). I =   1 12mh2+14mr2 0 0 0 ₁₂1mh2+1₄mr2 0 0 0 1₂mr2   (4.27)

Dynamic Parameter Identification

(48)

32 Chapter 4. Dynamic Modeling and Motion Control

Consider that measurements of torques, positions, velocities and accel-erations have been obtained using either sensors, filters (for velocities and accelerations) or observers at given time instants t1· · · tN.

¯ τ =    τ (t1) .. . τ (tN)   =    Y (t1) .. . Y (tN)   π = ¯Y π (4.28)

Furthermore, the number of measurements to perform shall be large enough to avoid ill-conditioning of matrix ¯Y (matrix ¯Y should have full column rank). Solving (4.28) by a least-squares techniques leads to

π = ¯YTY¯−1 ¯

YTτ¯ (4.29)

where ¯YT_Y_¯−1 ¯

(49)

33

Chapter 5

Results and Simulations

In this section, the results obtained using simulations in MATLAB/Simulink are shown. First, the dynamic parameter identification technique is shown for a 2DOF planar robot and afterwards, the dynamical models for both 3DOF and 7DOF systems will be derived first and after that, the results for the two proposed model-based controllers shown in Section 4.3 will be tested and analysed.

All the simulations will be based on the parameters from the LWA4D mod-ules [42] and [43]. Therefore, the modmod-ules specifications are established in the Table 5.1. Notice that the dimensions are given for the real manipulator and the CAD model provided by [44] used for simulations.

TABLE5.1: Module specifications

Joint Type Mass

[kg] Radius (Real/CAD) [m] Peak Torque [N_{· m]} 1 P RL120 3.6 0.066/0.065 372 2 P RL120 3.6 0.066/0.065 372 3 P RL100 2.0 0.056/0.054 176 4 P RL100 2.0 0.056/0.054 176 5 P RL80 1.2 0.0445/0.044 41.4 6 ERB115 0.9 0.058/0.0572 19 7 ERB115 0.9 0.058/0.0572 19

TABLE5.2: Link dimensions

Lengthi Real Lengths [m] CAD Lengths [m]

1 0.380 0.300

2 0.328 0.328

3 0.3230 0.320

(50)

34 Chapter 5. Results and Simulations

5.1 Dynamic Parameter Identification for a 2DOF

Pla-nar Robot

The dynamic parameter identification technique from Section 4.3.4 has been tested for a 2DOF planar robot based on the original LWA4D. In this section we see that in order to estimate the system parameters successfully, some grouping of parameters need to be done. Moreover, the given trajectories need to be exciting enough [45][46][47] in order to obtain non singular ma-trices.

FIGURE5.1: 2DOF Planar Robot structure

Figure 5.1 shows which motors should be frozen from the LWA4D arm in order to perform a dynamic parameter identification using a 2DOF pla-nar robot structure along the (x0, y0) plane.

5.1.1 Parameters for the 2DOF Planar Robot

The values used for simulations of the 2DOF manipulator are shown in Ta-ble 5.3. With the specifications from the LWA4D and the PRL/ERB modules [42], the masses for the 2DOF model are considered as

(

m1 = 1₂mP RL120+ mP RL100 +

1

2mP RL100

m2 = 1₂mP RL100+ mP RL80+ mERB115

and the radius used for the inertia components are

(

r1= rP RL100

(51)

5.1. Dynamic Parameter Identification for a 2DOF Planar Robot 35

TABLE5.3: 2DOF Planar Robot parameters

Parameter Value l2 0.328 [m] l3+ l4 0.397 [m] lc2 0.164 [m] lc3+ lc4 0.1985 [m] m1 4.8 [kg] m2 4 [kg] Izz1 0.0070 [kg· m2] Izz2 0.0039 [kg· m2] g 9.81 [m/s2]

5.1.2 Building the System Regressor

The elements from the dynamic matrices (4.7), (4.8) and (4.10) are defined below.            m11= m2l22+ 2m2cos(θ4)l2(lc3+ lc4) + m1l2c2+ m2(lc3+ lc4)2+ Izz1 + Izz2 m12= m2(lc3+ lc4)2+ l2m2cos(θ4)(lc3+ lc4) + Izz2 m21= m2(lc3+ lc4)2+ l2m2cos(θ4)(lc3+ lc4) + Izz2 m22= m2(lc3+ lc4)2+ Izz2            c11=−l2(lc3+ lc4) ˙θ4m2sin(θ4) c12=−l2(lc3+ lc4)m2sin(θ4)( ˙θ2+ ˙θ4) c21= l2(lc3+ lc4) ˙θ2m2sin(θ4) c22= 0 (

g1 = gm2((lc3+ lc4) cos(θ2+ θ4) + l2cos(θ2)) + lc2gm1cos(θ2)

g2 = (lc3+ lc4)gm2cos(θ2+ θ4)

Looking at the elements from the dynamic equations, it is highly recom-mended to find combinations of variables in order to reduce the number of parameters that should be estimated by (4.29).

In this case, the combinations are

(52)

After substituting (5.1) into the dynamic equations, using the Symbolic Toolbox from MATLAB, the symbolic expressions for τ1and τ2are computed

from (4.6) such as      τ1 =(p1+ p2+ 2p3cos(θ4))¨θ2+ (p2+ p3cos(θ4))¨θ4+

(−2p3sin(θ4)) ˙θ2˙θ4+ (−p3sin(θ4)) ˙θ42+ p5cos(θ2+ θ4) + p4cos(θ2)

τ2= (p2+ p3cos(θ4))¨θ2+ p2θ¨4+ (p3sin(θ4)) ˙θ22+ p5cos(θ2+ θ4).

(5.2) Looking at (4.29), π refers to the system parameters [p1, p2, p3, p4, p5]T

and the regressor elements are defined in (5.3).

                                         y11= ¨θ2 y12= ¨θ2+ ¨θ4

y13= 2 cos(θ4)¨θ2+ cos(θ4)¨θ4− 2 sin(θ4) ˙θ2˙θ4− sin(θ4) ˙θ24

y14= cos(θ2) y15= cos(θ2+ θ4) y21= 0 y22= ¨θ2+ ¨θ4 y23= cos(θ4)¨θ2+ sin(θ4) ˙θ22 y24= 0 y25= 0 (5.3)

5.1.3 Parameter Identification under Finite Fourier series

trajecto-ries

In [45] it is shown that one way to provide sufficiently enough exciting trajectories to estimate the parameters is through Finite Fourier Series as reference trajectories in joint space. Using the values specified on the list below, the reference trajectories are generated.

• F requency → ω = 1[Hz] • Joint2 → a21 = 0.156 , a22 = −0.478 , a23 = 0.078, a24 = −0.388 , a25 = −0.070 , b21 = 0.088 , b22 = 0.253 , b23 =−0.207 , b24 = 0.549 , b25= 0.150, θ20 = 0. • Joint4 → a41 = 0.064 , a42 = −0.335 , a43 = 0.451 , a44 = 0.292 , a45 = 1.046, b41 = −0.125 , b42 = 0.292 , b43 =−0.369 , b44 = 0.557 , b45= 0.964, θ40 = 0.

Using the previous values, the expression used to generate the Finite Fourier Series is θiref = θi0 + 5 X n=1 ain nω sin(nωt)− bin nωcos(nωt) (5.4)

where i is the joint index, θ0 the initial position and t the simulation

(53)

5.1. Dynamic Parameter Identification for a 2DOF Planar Robot 37

(A) Reference trajectories

(B) Angular Joint Positions

(C) Parameter Identification Result

FIGURE 5.2: Dynamic Parameter estimation 2DOF planar

(54)

Using a simple PD controller with gains KP = 30 and KD = 5 with

θ0 = 0, 0, 0, −π₂, 0, 0, 0 under the reference trajectories from Figure 5.2a,

the dynamic parameters are identified. In addition, Figure 5.2b shows the joint positions during the simulation time. Finally, from Figure 5.2c we see that all the estimated values for the system parameters are converging to certain values after iterating (4.29) over the simulation time. In the Table 5.4 there is a comparison between the estimated parameter value and the real value used for the robot dynamical model.

TABLE5.4: Estimated parameters vs Real value

Parameter Estimated Parameter Real Value

p1 0.54630 0.56644

p2 0.14777 0.16151

p3 0.29674 0.26043

p4 20.4553 20.5931

p5 7.95738 7.78914

This results might be improved changing the controller gains or modi-fying the parameters used to define the reference trajectories.

5.2 Data Transmission of LWA4D

The effects from the delays expected on the torque and joint position trans-mission using CANopen communication will be considered for simula-tions.

The Schunk LWA4D rate transmission is 10ms and therefore, meaning that every 10ms all inputs and outputs should be updating according to the syn-chronized transmission method mentioned in Section 3.1.5.

In order to test the CAN communication effects on the suggested control motion controllers, we use zero order hold blocks on the inputs and out-puts of the manipulator such as in Figure 5.3.

MANIPULATOR τ θ s ˆ˙θ (1 + 0.01s)2 10 ms 10 ms ZOH ZOH SAT

FIGURE5.3: Data transmission in MATLAB/Simulink

(55)

5.3. Results for the 3DOF Model 39

In the next sections we will analyse the performance of the motion con-trollers from Section 4.3 under ideal conditions (no sampling errors or er-rors due to approximations in computation of derivatives) and a more re-alistic scenario where the data transmission limitations from the real set up will be considered.

5.3 Results for the 3DOF Model

In this section, four motors from the Schunk manipulator are assumed to be frozen in order to analyse the behaviour with 3 degrees of freedom (3DOF). The solutions for the forward and inverse kinematics are found using the lengths and constraints from the LWA4D arm. One way to visualize the ef-fects of the controller on the manipulator is through the MATLAB toolbox Simscape that contains the tool SimMechanics, which uses the CAD model [44] from the LWA4D.

5.3.1 Parameters for the 3DOF Model

The reduced model parameters are based on the given characteristics from the LWA4D documentation. Using the information from Schunk documen-tation [9] and the PRL modules [42], the masses mi, radius ri and lengths li

are found for the different modules: P RL120, P RL100, P RL80and ERB115.

(56)

Based on the module configuration from Figure 5.4, the masses for the 3DOF are considered to be

     m1 = mP RL120 + 1 2mP RL120 m2 = 1₂mP RL120 + mP RL100+ 1 2mP RL100 m3 = 1₂mP RL100 + mP RL80+ mERB115

while the radius used to compute the inertia elements approximating the shape of the links to cylinders according to (4.27) are

     r1 = rP RL120 r2 = rP RL100 r3 = rP RL80.

In order to simulate this model, a CAD model provided by Schunk is used. Although the appearance of the CAD model is the same compared to the real manipulator, the dimensions differ a bit. For that reason, the simulation parameters shown in Table 5.5, will be assigned according to the CAD model.

TABLE5.5: 3DOF CAD Model Parameters

Parameter Link1 Link2 Link3

l [m] 0.3 0.328 0.320+0.077 lc[m] 0.15 0.164 0.1600 + 0.0385 m [kg] 5.4 4.8 4 Ixx[kg· m2] 0.0462 0.0465 0.0545 Iyy[kg· m2] 0.0462 0.0465 0.0545 Izz [kg· m2] 0.0114 0.0070 0.0039

While the length of the links and the masses are provided by the docu-mentation, the inertia elements have been approximated considering mod-ules almost with a cylinder shape. Another technique could be using dy-namic parameter identification taking measurements from the arm sensors substituting all the known parameters (lengths and masses) and leaving the inertias as unknown parameters.

5.3.2 Dynamical Model for the 3DOF Model

(57)

The M (θ) matrix elements are

                                       m11=Iyy1 + Iyy2 + m3((lc3+ lc4)c24+ l2c 2 2+ lc22 m2+ Iyy3c 2 24+ Ixx3s 2 24+ Ixx2s 2 2− Iyy2s2 2_{− l}2 c2m2s22 m12= 0 m13= 0 m21= 0 m22= m3l22+ 2m3c4l2(lc3+ lc4) + m2lc22 + m3(lc3+ lc4)2+ Izz2+ Izz3 m23= m3(lc3+ lc4)2+ l2m3c4(lc3+ lc4) + Izz3 m31= 0 m32= m23 m33= m3(lc3+ lc4)2+ Izz3

where ci = cos(θi), si = sin(θi), cij = cos(θi+ θj) and sij = sin(θi+ θj).

Moreover, the C(θ, ˙θ) elements are defined as

                                                                 c11=−1 2˙q2(m3s22l 2 2+ 2m3s224l2(lc3+ lc4) + m2s22l2c2+ m3s2244(lc3+ lc4)2− Ixx3s2244+ Iyy3s2244− Ixx2s22+ Iyy2s22)− ˙q4s24(c24m3(lc3+ lc4) 2₊ l2c2m3(lc3+ lc4)− Ixx3c24+ Iyy3c24) c12=− 1 2˙q1(m3s22l 2 2+ 2m3s224l2(lc3+ lc4) + m2s22l2c2+ m3s2244(lc3+ lc4)2− Ixx2s22− Ixx3s2244+ Iyy2s22+ Iyy3s2244)) c13=− ˙q1s24(c24m3(lc3+ lc4)2+ l2c2m3(lc3+ lc4)− Ixx3c24+ Iyy3c24) c21= 1 2˙q1(m3s22l 2 2+ 2m3s224l2(lc3+ lc4) + m2s22l2c2+ m3s2244(lc3+ lc4)2− Ixx2s22− Ixx3s2244+ Iyy2s22+ Iyy3s2244)) c22=−l2(lc3+ lc4) ˙q4m3s4 c23=−l2(lc3+ lc4)m3s4( ˙q2+ ˙q4) c31= ˙q1s24(l2(lc3+ lc4)c2m3− l2(lc3+ lc4)3m23s4( ˙q2+ ˙q4) + Ixx3l2(lc3+ lc4) m3s4( ˙q2+ ˙q4)− Iyy3l2(lc3+ lc4)m3s4( ˙q2+ ˙q4)) c32= l2(lc3+ lc4) ˙q2m3s4 c33= 0

where sii = sin(2θi), siij = sin(2θi + θj) and siijj = sin(2θi + 2θj). And

finally, the G(θ) vector elements are defined as

     g1 = 0 g2 =−gm3((lc3+ lc4)c24+ l2c2) + lc2gm2c2 g3 =−(lc3+ lc4)gm3c24.

5.3.3 SimMechanics for the 3DOF Model

(58)

g = [0 0 -g] Link1 Link2 Link3

θ1 ˙θ1 ¨ θ1 θ2 ˙θ2 ¨ θ2 θ3 ˙θ3 ¨ θ3

FIGURE5.5: SimMechanics 3DOF Block Diagram

Using SimMechanics, the joint actuators can be controller using Gener-alized forces or Motion. In this case, the motion variables θi, ˙θi and ¨θi are

introduced from the dynamic computations. Once the CAD pieces from each link are attached together respecting the rotation axis (always along z axis), the model should look similar to Figure 5.6.

FIGURE 5.6: SimMechanics representation for the 3DOF

model

5.3.4 Motion Control for the 3DOF Model

The results obtained from the different controllers described in Chapter 4 are shown here. Several experiments will be conducted in order to ver-ify the correct functionality of such controllers both in ideal conditions and under the corresponding effects due to the CANopen protocol used by the Schunk LWA4D alongside with the corresponding angular velocity estima-tions.

In all tests, the manipulator should reach four reference points placed in a 3D space. The points are the same for all the controllers but for PD+G con-trol they will be given one after the other (after predefined periods of time T ) as it is a point-to-point (PTP) motion control while in CTM the trajecto-ries from point to point will be given according to Section 4.3.3.

The used reference points are

Xref1 = [−0.5, −0.3, 0.6]

Xref2 = [−0.5, −0.3, 0.05]

Xref3 = [0.5,−0.4, 0.05]

(59)

starting at the initial position associated to the joint configuration θinit =

−π

2,−π2,π2, which is Xinit= [0,−0.397, 0.628].

PD+G Control in Joint Space for 3DOF

The PD+G controller in joint space is shown in Section 4.3.1 in Figure 4.4. In order to convert Xref to θref, the inverse kinematics for the 3DOF model

shown in Section 4.1.2 is used.

Moreover, the stability analysis (4.15) based on the Lyapunov’s theorem de-termines that the error will converge to zero as long as KPTD is a diagonal

matrix has positive elements. Ideally, without sampling errors, quantiza-tion errors and errors due to approximaquantiza-tions in computaquantiza-tion of derivatives, this controller works for the mentioned conditions as long as the motors can provide the required input torque. However, in this section we will see that the set up limitations mentioned in Section 5.2 create undesirable be-haviours making the stability analysis not valid anymore.

Finding suitable gains under such conditions can be tricky and from the following experiments we will see that they should remain under a certain value.

Using the controller gains from (4.22), the PD+G controller performs poorly. For that reason, we found through experimentation that defining the con-troller gains such as (5.6) improves the simulation results.

(

KP = p

KD = 5₃p

(5.6)

The conducted tests are

• Test 1 → T = 5 and p = 2.5 • Test 2 → T = 5 and p = 1.25

where T is the period of time from point to point and p defines the con-trol gains.

Using trial and error we find that for big p values, the motion control is destroyed with CAN communication and estimating the angular velocities. From Figure 5.7a it can be observed that the end-effector trajectory XCAN

for the first three points, is the same as the trajectory of the end-effector X performed under ideal conditions. However, looking at Figure 5.7b we see that at some point τ2CAN and τ3CAN start oscillating making the end-effector

(60)

(A) End-effector position

(B) Applied Torques

FIGURE5.7: PD+G Control in Joint Space results for T = 5

and p = 2.5

For the second test, the p value has been decreased until XCAN stops

oscillating and behaves similarly as X (see Figure 5.8a). Using Figure 5.8b, it can be also verified that τ and τCAN do not have undesirable behaviours.

(61)

(A) End-effector position

(B) Applied Torques

FIGURE5.8: PD+G Control in Joint Space results for T = 5

and p = 1.25

PD+G Control in Task Space for 3DOF

Similarly as the PD+G control in joint space, the task space version of the same controller shows the effects of giving the reference points directly in the Cartesian space. In order to implement such a controller, the use of the direct kinematics and the Jacobian transposed is introduced such as in Figure 4.5.

The same control gain condition (5.6) is used in this case. However, in order to make the results between controllers easier to compare, keep in mind that KP should have been scaled by an average value of the Jacobian matrix and

KD should have been scaled by an average of the derivative of the Jacobian

TowardsaModel-BasedMotionControlDesignfora7-axisRoboticArmLWA4DbySchunk U U

U

MEÅ

U

NIVERSITY

M

T

Towards a Model-Based Motion

Control Design for a 7-axis Robotic

Arm LWA4D by Schunk

Author:

Roger Vila Cano

Supervisors:

Leonid Friedovich and Szabolcs Fodor

Abstract

Acknowledgements

Contents

List of Figures

List of Tables

Chapter 1

Introduction

1.1

Motivation and Preview

1.2

Goals and proposed solutions

1.3

System Integration

1.4

The LWA4D by Schunk

1.5

The MicroAutoBox II

1.6

System Set Up Overview

Chapter 2

Literature Review

2.1

Robot Manipulators

2.2

Communication protocols

2.3

Embedded Control Systems

2.4

Calibration for Industrial Robots

Chapter 3

CANopen Communication

Protocol

3.1

CANopen Elements

3.2

Device Profiles for Robot Applications

Chapter 4

Dynamic Modeling and Motion

Control

4.1

Kinematic analysis

4.2

Dynamic Robot Modelling

4.3

Motion Control

Chapter 5

Results and Simulations

5.1

Dynamic Parameter Identification for a 2DOF

Pla-nar Robot

5.2

Data Transmission of LWA4D

5.3

Results for the 3DOF Model