操控性橢圓理論於兩足機器人步態規劃之應用研究

全文

(1)國立交通大學工學院精密與自動化工程學程. 碩士論文. 操控性橢圓理論於兩足機器人步態規劃之應用研究. The Study on the Gait Generation of Biped Robot Using the Manipulability Ellipsoid Algorithm. 研究生 : 指導教授 :. 李駿榮鄭璧瑩博士. 中華民國九十五年六月.

(2) 操控性橢圓理論於兩足機器人步態規劃之應用研究 The Study on the Gait Generation of Biped Robot Using the Manipulability Ellipsoid Algorithm 研究生：李駿榮. Student：Jiung-Rong Lee. 指導教授：鄭璧瑩博士. Advisor：Dr. Pi-Ying Cheng. 國立交通大學. 工學院精密與自動化工程學程碩士論文 A Thesis Submitted to Degree Program of Automation and Precision Engineering. College of Engineering National Chiao Tung University in Partial Fulfillment of the Requirements for the Degree of Master of Science in Automation and Precision Engineering November 2005 Hsinchu, Taiwan, Republic of China 中華民國九十五年六月.

(3) 操控性橢圓理論於兩足機器人步態規劃之應用研究. 研究生：李駿榮. 指導教授：鄭璧瑩博士. 國立交通大學工學院精密與自動化工程學程. 摘要本論文以具有十個自由度之空間並聯式機構模擬 KHR-1 兩足機器人之下半身，並以操控性橢圓理論為核心，設計並模擬兩足機器人由蹲姿轉換至直立站姿及由站立姿態開始完成以兩左右跨步為一基礎步伐組之關節運動軌跡做為範例，進行電腦輔助軌跡規劃法的研發。在以程式完成軌跡設計後，透過控制器的數據傳輸介面將關節運動軌跡數據轉入 KHR-1 兩足機器人控制系統中，並藉由機器人執行動作範例以驗證理論推導之關節運動軌跡之實用性與穩定性。本研究所建立的兩足機器人步態規劃之流程，可簡化傳統程序的繁瑣耗時，同時提供一較為穩定可靠之動作模式。本研究所提出的方法為先推導出終端效應器特定點在卡氏座標中之線速度單位圓與關節角速度橢圓之對應轉換模型，經由計算找出終端效應器在往某特定方向移動時所對應各關節驅動器之最小角速度組合，以此最小角速度組合做為機械手作動規劃時之依據。本研究所採用的原理，在靜力學之觀點來看，每一單位時間間隔之角加速度變化較小。這 i.

(4) 使得兩足機器人運作時能獲得較高之穩定性。在機器人的姿態平衡的維持方面，關節角位移的計算規劃時輔以零矩點理論(Zero-Moment Point)建立機器人平衡機制。應用零矩點理論在兩足機器人的運動平衡控制方面，為維持機器人姿態對地面形成之合作用力向量通過零矩點且作用力接觸座標落在單腳腳底或雙腳間區域內便能使機器人獲得穩定的運動與姿態。本論文提出以操控性橢圓理論及零矩點理論作為核心概念建立輔助規劃與模擬系統規劃出兩足機器人之動作並進行穩定性與效率之分析。此外，本研究也實際組裝 KHR1 多關節人形兩足機器人，經傳入所規劃的控制軌跡數據，進行作動測試。經實作測試，可確信本研究成果不僅可將多自由度的兩足機器人的控制軌跡規劃過程系統化與簡化，同時也可經由理論與模擬方法進行最佳化的規劃與應用。. 關鍵字: 操控性橢圓理論、兩足機器人、步態規劃、零矩點理論. ii.

(5) The Study on the Gait Generation of Biped Robot Using the Manipulability Ellipsoid Algorithm. Student：Jiung-Rong Lee. Advisor：Dr. Pi-Ying Cheng. Department of Automation and Precision Engineering National Chiao Tung University ABSTRACT. Based on the manipulability ellipsoid algorithm, the research develops a gait planning system to generate the gait for the spatial parallel/serial type manipulator with ten or more degree of freedom. The research develops systematic methods for joint trajectory planning and simulation of the biped robot motion. The proposed methodology can be successfully applied to generate but not limited to the gaits of squat to stand and two walking strides. The desired joint trajectory of biped robot produced by developed program with the kernel of the manipulability ellipsoid algorithm. An appropriate procedure has been set up to transform the generated joint trajectory data to the control unit of the KHR-1 biped robot, after the simulation to test and verification of the robot on the motion stability. This proposed method can save the cycle-time on trajectory generation of the biped robot. The velocity ellipsoid method is currently adopted for finding the minimum combination of angular velocity of the joints of the robot. Based on the static kinematics, the manipulability ellipsoid method can provide the choice of the minimum variation of the joint velocity for the biped robot to reach the more stable motion status. Furthermore, Zero-Moment point algorithm is applied as the constraint of the restriction area of the projection point of the mass center of the robot in the state of trajectory planning, thus the high stability of the robot can be afforded. A. iii.

(6) biped robot KHR-1 with ten more degree of freedom has also been assembled and installed for proving the advantage and usefulness of the proposed method. The illustrated examples have demonstrated the benefit and the advantage on the gait planning of biped robot.. KEY WORDS: Manipulability Ellipsoid Algorithm, Biped Robot, Gait Generation, Zero-Moment Point Algorithm. iv.

(7) Acknowledgement. I would like to express the gratitude to my advisor, Dr. Pi-Ying Cheng, for his excellent guidance in my research and his sincere concern about my personal life throughout these years. Dr. Pi-Ying Cheng is an amiable and decent professor, and he is also a person who has integrity and responsibility. His outstanding personality gives me a deep impression, and he is a role of model for me. There are so many teachers who gave me the precious advices and guidance so that I can have the continuously improvement and progress with my research and study in Chaio Tung University. The NCTU has been instructed me the most valuable knowledge. The absolute support and constant encouragement from my family and my girlfriend, Debby, keep me move on whether my mind was weighed down or up. I would like to dedicate this thesis to my parents and my girlfriend for their consistent care and the sacrifice that they made for me in the past years.. v.

(8) Table of Contents ABSTRACT. i. ACKNOLEDGEMENT. v. TABLE OF CONTENTS. vi. LIST OF TABLE. ix. LIST OF FIGURE. x. NOMENCLATURE CHAPTER 1. xiv. Introduction. 1. 1.1 Literature Reviews. 1. 1.2 Motive. 3. 1.2.1 Target of Research. 4. 1.2.2 Assumption and Basic Constraint of Research. 4. 1.2.3 Kernel Conception. 5. 1.3 Thesis Overview CHAPTER 2. 6. The Kinematical Model of KHR-1 Robot. 9. 2.1 Introduction. 9. 2.2 KHR-1 Robot System. 9. 2.2.1 Hardware Interface. 11. 2.2.1.1 KRS-784 Servo Motors. 11. 2.2.1.2 RCB-1 Circuit Boards. 11. 2.2.1.3 Passive Transmission Parts. 12. 2.2.1.4 Supporting Skeleton Parts. 12. 2.2.2 Communicated Interface. 12. 2.2.3 Software Interface. 12. 2.2.4 The Control System of KHR-1. 14. 2.2.5 Procedure of Motion design. 14. 2.2.6 Example of Motion Create by Try and Error. 16. 2.3 Kinematical Model of KHR-1 Robot. 20 vi.

(9) 2.3.1 Denevit-Hartenberg Homogeneous Transformation Matrices. 21. 2.3.2 Link Parameter and Link Coordinate System. 21. 2.3.3 Closure Loop Equation. 25. 2.4 Jacobian Analysis of Three Links Planar Serial Type Manipulator 2.4.1 Mathematical model of Jacobian Matrice 2.5 The Transforming Procedure with KHR-1. 26 26 29. 2.5.1 The Coordinate System. 29. 2.5.2 Definition of Link Three if Kinematical Model. 32. CHAPTER 3 Introduction of Main Algorithm. 35. 3.1 Introduction. 35. 3.2 Manipulability Ellipsoid Algorithm. 35. 3.2.1 Condition Number. 35. 3.2.2 Velocity Ellipsoid Model. 36. 3.3 Force Ellipsoid Model. 40. 3.3.1 Application of the Principle of Virtual Work. 40. 3.3.2 Force Ellipsoid. 41. 3.4 Inverse Kinematics. 45. 3.5 ZMP (Zero-Moment Point) Algorithm. 50. CHAPTER 4 Result of Simulation and Theoretical Analysis. 53. 4.1 Introduction. 53. 4.2 Introduction of Envelope One. 54. 4.2.1 Compound Estimate Function in Envelope One. 57. 4.2.2 Process Flows of Two Different Methods in Envelope One. 58. 4.2.3 The Result of Simulation of Envelope One. 62. 4.3 Introduction of Envelope Two and Three. 72. 4.3.1 Process Flows of Envelope Two and Three. 72. 4.3.2 The Result of Simulation of Envelope Two and Three. 73. CHAPTER 5 Conclusion and Future Work. 90. 5.1 Conclusion. 90 vii.

(10) 5.2 Future Work. 91. REFERENCE. 92. viii.

(11) List of Tables Table 2.1 D-H parameters of a three link planar serial type manipulator. 28. Table 2.2 Coordinate system transformation between kinematical model and KHR-1. 33. Table 4.1 Detail process of biped gait generation in envelope one. 55. Table 4.2 Comparison of method one and two. 69. Table 4.3 Detail process of biped gait generation in envelope two. 76. Table 4.4 Detail process of biped gait generation in envelope two. 77. Table 4.5 Detail process of biped gait generation in envelope two. 78. Table 4.6 Detail process of biped gait generation in envelope two. 79. Table 4.7 Detail process of biped gait generation in envelope two. 80. Table 4.8 Detail process of biped gait generation in envelope two. 81. Table 4.9 Detail process of biped gait generation in envelope two. 82. ix.

(12) List of Figures Fig.1.1 The envelops of biped gait. 7. Fig.1.2 The procedure of gait generation. 7. Fig.2.1 (a) KHR-1 biped robot top view. 10. (b) KHR-1 biped robot front view. 10. (c) KHR-1 biped robot right view. 10. Fig.2.2 (a) KRS-784ICS Servo Motor. 13. (b) RCB-1 Circuit Boards. 13. (c) Passive Transmission Parts. 13. (d) RS232C cable. 13. Fig.2.3 Communicated software, Heart to Heart. 15. Fig.2.4 (a) The circuit I/O set configuration on RCB-1. 17. (b) The comparison of number of servo motors and I/O on RCB-1. 17. (c) The comparison of number of servo motors and the location on KHR-1.. 17. Fig 2.5 Current procedure of design motion. 18. Fig 2.6 Example motion, prostrate to stand up, created by try and error (a) Initial posture. 19. (b) Support by arms. 19. (c) Curling the thigh towards to body. 19. (d) Support by arms and thigh. 19. (e) Arms getting away from ground. 19. (f) Goal posture, stand up. 19. Fig.2.7 A three link planar serial type manipulator. 22. Fig.2.8 Definition of link parameters. 24. Fig.2.9 (a) Complete order number of each servo motor on KHR-1 robot. 30. (b) The coordinate system of right thigh (No.20~22 motors) of robot. 30. (c) The coordinate system of left thigh (No.14~16 motors) of robot. 30. x.

(13) Fig.2.10 The coordinate system comparison of physical model and KHR-1. 31. Fig.2.11 The shift angle between motors number 21 and 20. 33. Fig.2.12 The distance between joint of motor number 20 and canter of mass of upper body. 34. Fig.3.1 (a) Unity circle of velocity of end-effector in Cartesian coordinate (b) Velocity ellipsoid. 39 39. Fig.3.2 (a) Unity circle of joint torque. 44. (b) Force ellipsoid of end-effector. 44. Fig.3.3 Two possible inverse kinematics solution. 48. Fig.3.4 Inverse kinematics implement for simplifying the kinematical model (a) X-Y plane. 49. (b) Z-Y plane. 49. Fig.3.5 (a) The Single sole support area with tipping moment. (b) General configuration of a biped robot under interaction force/torque from ground and environment Fig.3.6 (a) Double support phase. 51. (b) Single support phase. 51. Fig.4.1 (a) Initial posture right view and 3-D view (b) Goal posture right view and 3-D view. 56 56. Fig 4.2 Process flow chart of method one on envelope one. 60. Fig 4.3 Process flow chart of method two on envelope one. 61. Fig. 4.4 (a) Angular displacement of method one. 63. (b) Angular displacement of method two. 63. Fig. 4.5 (a) X-Y distribution of angular displacement of method one. 64. (b) X-Y distribution of angular displacement of method two. 64. Fig. 4.6 (a) Joint one location of method one. 65. (b) Joint one location of method two. 65. Fig. 4.7 (a) Joint two location of method one. 66. (b) Joint two location of method two. 66 xi.

(14) Fig. 4.8 (a) Angular velocity of method one. 67. (b) Angular velocity of method two. 67. (c) Comparison of angular velocity. 67. Fig. 4.9 (a) Joint torque of method one. 68. (b) Joint torque of method two. 68. (c) Comparison of Joint torque. 68. Fig. 4.10 Comparison of compound estimate function. 69. Fig. 4.11 Manipulability ellipsoid of method one in first interval.. 70. (a) Angular velocity ellipsoid. 70. (b) Force ellipsoid. 70. Fig. 4.12 Manipulability ellipsoid of method two in first interval.. 71. (a) Angular velocity ellipsoid. 71. (b) Force ellipsoid. 71. Fig. 4.13 Consequently locomotion of all envelopes. 74. Fig. 4.14 Consequently locomotion of all envelopes. 75. Fig 4.15 Process Flows of Envelope Two and Three. 83. Fig 4.16 (a) Angular displacement of front leg. 84. (b) Angular displacement of back leg. 84. Fig 4.17 (a) Location of front ankle. 85. (b) Location of back ankle. 85. Fig. 4.18 (1) Practical simulation of envelope two and three.. 86. Fig. 4.18 (2) Practical simulation of envelope two and three.. 87. Fig. 4.18 (3) Practical simulation of envelope two and three.. 88. Fig. 4.18 (4) Practical simulation of envelope two and three.. 89. xii.

(15) Nomenclature. a1 , a 2 , a3. = Length of link in physical model. ai. = The displaced (i − 1) th coordinate system is translated along the xi axis a distance = The displaced (i − 1) th coordinate system is rotated about the xi axis. αi. an angle 0. A1. = The transformation matrix of link one respected with base. 1. A2. = The transformation matrix of link two respected link one. 2. A3. = The transformation matrix of link three respected link two. i −1. Ai. = The transformation matrix of link i respected with link i-1. c. = Condition number. C1. = Initial configuration in procedure of design motion. CN. = Goal configuration in procedure of design motion. Cr. = Reasonable configuration in procedure of design motion. CH i. = The number of servo motor of KHR-1 robot. di. = The (i − 1) th coordinate system is translated along the z i −1 axis a distance. e j (ti ). = Compound estimate function. g. = The velocity of gravity. Hg. = The rate of change of angular momentum at center of mass of a robot. J. = Jacobain matrix of physical model which define the relation of velocity of End-effector and the angular velocity of each joint. K. = Arbitrary point on sole of robot. λ1. = Eigenvalue of J T J. λ2. = Eigenvalue of J T J. M. = Joint two of Physical model. M Kf n. = Moment of friction force. M Pn. = Moment of pressure force. xiii.

(16) n. = Unit vector which is normal to the sole. O. = Joint one (base) of Physical model. Oi. = The origin of link i. P. = Joint three of Physical model. P&x. = The linear velocity of joint two in x coordinate. P&y. = The linear velocity of joint two in y coordinate. q&. = N dimensional velocity vector mapped by Jacobian matrix. 0. q. = Position vector of Q respected to the base coordinate system. 3. q. = Position vector of end-effector coordinate system. Q. = End-effector of Physical model. Qx. = Position of end-effector in x coordinate. Qy. = Position of end-effector in y coordinate. Q& x. = Linear velocity of end-effector in x coordinate (Vqx ). Q& y. = Linear velocity of end-effector in y coordinate (Vqy ). RPn. = Pressure force. Rfn. = Friction force. θ1 , θ 2 , θ 3. = Angle of links respected with its coordinate system in physical model. θ1b. = The θ 1 value on boundary. θ 2b. = The θ 2 value on boundary. θ1c. = The current θ 1 in time t i. θ 2c. = The current θ 2 in time t i. θ1v. = The next θ 1 value in time t i +1. θ 2v. = The next θ 2 value in time t i +1. θ1 min. = The boundary of minimum θ 1. θ1 max. = The boundary of maximum θ 1. θ 2 min. = The boundary of minimum θ 2. θ 2 max. = The boundary of maximum θ 2. θi. = Angle of arbitrary link respected with its coordinate system in physical model or KHR-1 robot xiv.

(17) θ iHOME. = The data value of home position in. θ iKHR. = The actual data value executed by KHR-1. θ imotion. = The data value of motion file. θ&i. = Angular velocity of arbitrary link with its coordinate system in physical model or KHR-1 robot. ∆θ 1. = The displacement of θ 1 in time t i. ∆θ 2. = The displacement of θ 2 in time t i. ∆t. = The segment of time constant. T (z, d ). = The transformation matrix of distance d i. T (z ,θ i ). = The transformation matrix of angle θ i. T (z , a ). = The transformation matrix of angle ai. T (z, α ). = The transformation matrix of angle α i. τc. = The constraint output force of each joint. τi. = The output force of arbitrary joint in physical model of KHR-1 robot. τ max. = The maximum output force supported of each joint. τ1 , τ 2 , τ 3. = The output force of arbitrary joint in physical model. X 0 , Y0. = Coordinate system of base in physical model. X 1 , Y1. = Coordinate system of joint two in physical model. X 2 , Y2. = Coordinate system of joint three in physical model. Y14 , Z 14. = Coordinate system of servo motor number 14 (left thigh on KHR-1). Y15 , Z 15. = Coordinate system of servo motor number 15 (left thigh on KHR-1). Y16 , Z 16. = Coordinate system of servo motor number 16 (left thigh on KHR-1). Y20 , Z 20. = Coordinate system of servo motor number 20 (right thigh on KHR-1). Y21 , Z 21. = Coordinate system of servo motor number 21 (right thigh on KHR-1). Y22 , Z 22. = Coordinate system of servo motor number 22 (right thigh on KHR-1). xv.

(18) CHAPTER 1 Introduction. 1.1 Literature Reviews The Biped robot motion design and trajectory planning based on novel and various theories had developed for more complex motion requirement. For optimal control of biped robots, Fujimoto [1] proposed a general formulation of optimal control for biped robots based on numerical representation of motion equation, which can solve exactly the minimum energy consumption trajectory for a biped running motion. The main contribution of his idea is: it is useful to know the lower boundary of the consumption energy when we design the biped robot and select actuators. He fined input joint torques and initial posture that minimize input energy. Tang, Zhou and Sun [2] adopted third-order spline interpolation based trajectory planning and zero moment point method which is aiming to achieve smooth biped swing leg trajectory by reducing the instant velocity change which occurs at the time of collision of the biped swing leg with the ground. McGeea and Spong [3] discuss the control of the biped robot which consists of two legs connected to a cylindrical torso by DC-motors. A key problem that exists with this design is the generation of stable gaits while simultaneously guarantee bounded torso velocity. He presented a solution to this problem by controlling the velocity of the hub through design of the leg trajectory, which in turn determines the zero dynamics governing the hub velocity. In the manipulability ellipsoids implementation research, such as Chiacchio [4] proposed a formal definitions of force and velocity manipulability ellipsoids for multiple cooperative arms are established according to the global task space formulation that regards the 1.

(19) closed-chain system as a whole, at the object level and then independently from the number of arms involved in the cooperation. The proposed manipulability ellipsoids can be conveniently utilized to determine optimal postures for redundant multiple arm systems. Furthermore, the formulation adopted in this work allows the derivation of a dynamic manipulability ellipsoid. Tao [5] developed the optimal force distribution under the known posture. Two separate coordinating robots whose end-effectors firmly grasp a common inertial load, from a closed-chain structure. In these closed kinematical chains, the two robots must observe a set of constraint conditions on end-effectors’ position, orientation, linear and angular velocities, as well as the forces and torques inserted onto the end-effectors by the load. Although the manipulability ellipsoid, force and velocity ellipsoid are widely used technique in visualizing the dexterousness of robot motion, it has some problems that it does not transform the exact joint constraints into end-effector space. Therefore, Lee [6] explored the relation between manipulability ellipsoid and manipulability polytope for handling this problem. Another implementation of manipulability ellipsoid is adopted for developing the multi coordinates of robotic system, Lee [7] presented the static characteristics of a three degrees of freedom in parallel actuated manipulator and its desired static actuator characteristics for clamping and bracing applications. In his paper, highlights the influences of the over constraining forces on the manipulator performance using the presentation of the velocity, and force ellipsoids which were often employed in robotics for prediction of singularity, graphical representation of static characteristics, and for optimization of task performance. In terms of robotic motion design, except the various trajectory planning methods, Yamamoto [8] presented the global dynamics, which include two kernels: zero moment point and passive dynamics. He pointed out most commonly used strategy for trajectory generation, the high-gain servo method [9] is basically playing back of the predefined ideal trajectory.. 2.

(20) Then if we expect a robot to behave as various as the human, almost infinite number of ideal trajectory may be required. Yamamoto’s global dynamics theory is base on the explored phase space of robot motion. In the phase space, the envelopes of motion are connected by an unstable region which is called node. The unstable region is the exchanging point between motions transitions, then the controlling parameters can input into the node the cause the motion change from current to next one. The particular contribution of global dynamics is to minimize the control parameter and to simply the procedure of motion design. In legged robots development, especially the biped robots, the balance control is the particular and the first affected problem. Zero moment point is just the concept to explore the balance control issue. Goswani [10] studied the fundamental mechanics of rotational stability of multi-body systems with the goal to identify a general stability criterion. He focused on H g , the rate of change of angular momentum in center of mass of a robot, as the physical quantity containing its stability information. He proposed three control strategies using H g that can be used for stability recapture of biped robot.. 1.2 Motive In various theory of trajectory planning for biped robot motion we can see different complexity in mathematical model and experimental procedure. In terms of a specific motion design, the complexity depends on the number of parameters of the desired motion, the more parameters the more difficult to accomplish it. There are several theories such as impedance control, torque control, zero moment point, energy control, are developed for a long period. A biped robot problem comes out first is the balance maintenance, a loss of stability might result in a fall with potentially disastrous consequence for both robots and animals. Furthermore, a procedure of design robot motion will be another issue when the degree of freedom is steamily high so that the cycle-time of trajectory planning can not be reduced. 3.

(21) Due to these reasons, we proposed a velocity ellipsoid theory, in terms of the static kinematics, study the better procedure which can provide the shorter cycle-time for motion design, can produced a stable manipulating environment and produced the less executing force of each joint .. 1.2.1 Target of Research (1) Developing a biped robotic gait generation procedure which can provide a shorter cycle-time for specific robotic motion design. (2) Based on the static kinematics, the manipulability ellipsoid method can provide the choice of the minimum variation of the joint rate to reach the more stable motion status for the biped robot gait. (3) Reducing the executing force of each joint. A valuable advantage of using manipulability ellipsoid is eliminating the energy consumption when manipulating.. 1.2.2 Assumption and Basic Constraint of Research Several necessary assumption and constraint need to be defined before we discuss the main object in this thesis, and they are: (1) Base on static kinematics to study the biped gait generation issue, and do not consider the effects of dynamic parameters such as inertial force, linear acceleration, angular acceleration and so on. Trying to extend the limitation of manipulability ellipsoid algorithm in static kinematics sphere. (2) By first assumption, according to manipulability ellipsoid algorithm, dividing the entire process of biped locomotion to infinity of interval of time, let the angular velocity in arbitrary interval is defined by the minimum combination of angular velocity from last configuration.. 4.

(22) (3) Only consider how to design desired joint trajectories and ignore the parameters of servo motor control system such as rotation rate and output torque. (4) The kinematical model in this thesis is a spatial parallel type manipulator, for the purpose to reduce the kinematical model to two planer serial type manipulators, first we calculate the coordinate of end-effector from leg A, where the end-effecotor is also belong to another leg B, and obtain the configuration of leg B through inverse kinematics.. 1.2.3 Kernel Conception Manipulability ellipsoid algorithm content velocity ellipsoid and force ellipsoid, these two sub algorithm were adopted to implement and produce joint trajectories in most process of locomotion. In this thesis we design continuously biped gaits which include three envelopes [8], as shown in Fig. 1.1. The first envelop is from initial posture, squat, to goal posture, stand. In this envelop, we implement both the velocity ellipsoid and force ellipsoid algorithm to obtain a stable and less energy consumption gait. The second envelop is from initial posture, stand, to goal posture, first stride; and the third envelop is also the second stride, from the initial posture, stride, to the goal posture, stand. The two strides in last two envelops defined as a humanoid biped robot is walking slowly as human being. When a joint trajectory is produced by programming, next we have to do is transfer the trajectory data and implement into KHR-1 biped robot. When find out the invalid or unstable situation, trying to modify the program code and execute the experiment again until the situation of experiment is satisfied the principle we set. The entire procedure of biped gait generation is described in Fig.1.2.. 5.

(23) 1.3 Thesis Overview This thesis is base on the manipulability ellipsoid theory to develop a simply procedure of biped robot motion design. The construction of our experimentation includes derive the manipulability ellipsoid model of planar serial manipulator, producing the desired joint trajectory by program, implement the desired joint trajectory by KHR-1 simulation and finally analysis the θ i , θ&i , τ i of each joint. The content of each chapter are described below: (1) Chapter one is the introduction of research background, targets of research, assumption of research, review the relative literatures and a brief chapter by chapter of this thesis. (2) In the first part of chapter two, we discuss the kinematical model of KHR-1 biped robot. The KHR-1 robot system is presented with its hardware, software, communication interface and procedure of motion design. Through this chapter, one can realize the whole picture of practical KHR-1 robot, the virtual model which used for simulating the KHR-1 robot in programming, and how equilibrium of these two objects constructed. The second part of this chapter is to derive the mathematical equation of kinematical model. (3) In chapter three, there are three main algorithms to be introduced. The manipulability ellipsoid equation, which include velocity and force ellipsoid. And base on this theory we can define the arbitrary coordination on the unity velocity circle is corresponding to the specific coordination of angular velocity on the velocity ellipsoid, or on the unity output force circle is corresponding to the specific coordination of joint torque on the joint torque ellipsoid. The second algorithm is inverse kinematics, which defined the configuration of manipulator by a given location and direction of end-effector. The third algorithm is Zero-Moment point algorithm, which used to constraint the location of projection which belongs to center of mass of biped upper body on the single or double support phase, so that the resultant moment affect on center of mass of biped upper body will be zero, and the stability of biped motion will be guaranteed.. 6.

(24) Walk slowly Squat. Stand. Envelope 1. First stride. Envelope 2. Second stride. Envelope 3. Fig.1.1 The envelops of biped gait.. MATLAB Program. Desired joint trajectories. KHR-1 Biped Robot. Modify. Fig.1.2 The procedure of gait generation.. 7.

(25) (4) In chapter four, we present the result of experimentation envelops by envelop. In first envelop we compare the two different method of designing the gait, squat to stand. In second and the third envelop we compare the difference between the motion which design by KONDO company and by this thesis. (5) In chapter five, we integrate the main target of research, kernel of algorithm, procedure of gait generation to construct a complete picture, conclude the differences and advantage in these objects. Besides, several new vision of research in the future will be presented for extending the current progress of research to more and better achievements.. 8.

(26) CHAPTER 2 The Kinematical Model of KHR-1 Biped Robot. 2.1 Introduction In this chapter we are going to present the KHR-1 biped robot system which includes hardware, software, control system and procedure of motion design. Furthermore, the kinematical model of KHR-1 biped robot which is a simulator of KHR-1 biped robot in the programming is introduced too. The desired joint trajectories are based on the kinematical model and produced by MATLAB programming which is shown in Fig. 1.2. We use this virtual kinematical model as an intermediary; all programs were designed according to this kinematical model.. 2.2 KHR-1 Robot System The KHR-1 biped robot is shown in Fig. 2.1, it is manufactured by Kondo Kagaku Co. Japan. It was constructed from four particular components, servo motors, RCB-1 circuit boards, plastic passive transmission parts and aluminum alloy supporting skeleton parts. We will have the detail discuss in next section about these particular components. One of objects of this research is develop an ideal joint trajectory planning procedure, which is evaluated by the simulation of KHR-1. Via the practical experimentation that include visualize observation on moving robot, the stability and efficiency can be guarantee to proof.. 2.2.1 Hardware Interface In this section we are going to introduce the particular hardware components of KHR-1. All of the components are manufactured by Kondo Kagaku Co. Japan. There are four particular hardware components: servo motors, RCB-1 circuit boards, plastic passive. 9.

(27) (a). (b). (c). Fig.2.1 (a) KHR-1 biped robot top view. (b) KHR-1 biped robot front view. (c) KHR-1 biped robot right view.. 10.

(28) transmission parts and aluminum alloy supporting skeleton parts.. 2.2.1.1 KRS-784ICS Servo Motors KRS-784ICS servo motor is shown in Fig.2.2 (a), it is the digital FET (Field Effect Transtor) servo motor. It has been developed to drive joint of robot. Here are the specifications of KRS-784ICS: Size: 41 × 35 × 21(mm) excluded projections Weight: 45g Torque: 8.7 kg/cm (using 5N600 power cell) Speed: 0.17 sec/60 degree (using 5N600 power cell) Reasonable Voltage: 6V. 2.2.1.2 RCB-1 Circuit Boards RCB-1 is shown in Fig.2.2 (b), it has been developed for KHR-1 kit as the robot control board. A board can control twelve servo motors. It can control using all functions of the robot servo motors as KRS-784ICS. Since KHR-1 constructed from seventeen servo motors, in other words, there are necessary two pieces of RCB-1 boards for link the motors. Note that the number of motors link to RCB-1 is higher than the exactly number content of KHR-1, therefore, there are few non function I/O on RCB-1 without linking with servo motor. Here are the specifications of RCB-1: Size: 45 × 35 (mm) Weight: 12 g (one board) Possible number of servo motors: 12 Reasonable Voltage: 6V There is an EEPROM (Electrically Erasable Programmable ROM) embedded in the RCB-1,. 11.

(29) all the command data will be saved in the EEPROM. The topology of command data is: The maximum a hundred position data accomplished a motion. The maximum two hundred motion data accomplished a scenario. The maximum four scenario data could be edited and saved in EEPEROM.. 2.2.1.3 Passive Transmission Parts The passive transmission parts is shown in Fig.2.2 (c), when assemble KHR-1, there should be transmission parts for delivering the power which produced from servo motors into the supporting skeleton parts and accomplishing the motion.. 2.2.1.4 Supporting Skeleton Parts The skeleton parts support the servo motors and other components so that all the different components will be located at the fixed position on robot. The other function of skeleton parts are endowed the robot a humanoid body and be brawnier.. 2.2.2 Communicated Interface A RS232C cable transmits the command signal from the software in computer into the RCB-1, then a motion command is executed and a new motion is produced. The RS232C cable is shown in Fig.2.2 (d).. 2.2.3 Software Interface The Heart to Heart is used to produce the motion command which defined by adjusting the rotational value of joint, and the motion commands transmit by the RS232C cable into the RCB-1. It is shown in Fig. 2.3, each channel determines the rotational angle of servo motor of KHR-1, when a new angle value is produced by adjusting the angle bar, a new configuration. 12.

(30) (a). (b). (c). (d). Fig.2.2 (a) KRS-784ICS Servo Motor. (b) RCB-1 Circuit Boards. (c) Passive Transmission Parts. (d) RS232C cable.. 13.

(31) of robot is produced. Besides, one can adjust the desired speed by choosing the button zero to seven, which the smaller number determine the higher speed.. 2.2.4 The Control System of KHR-1 The RCB-1 can connect maximum twenty four servo motors, the particular function is motion command processor, all of the command signal will be send out there and receive by chip on RCB-1. The result of motion depends on the command which defined by modifying the angle value of motor in Heart to Heart. Therefore, different command causes the different motion. The Fig.2.4 (a), (b) and (c) determine the complete circuit configuration of servo motors with signal cables and power cables. Due to only seventeen motors used in KHR-1, there are several I/O do not connect with motors and act as no function I/O.. 2.2.5 Procedure of Motion design A robot motion is produced by user who adjusts the configuration of robot through the Heart to Heart. A complete configuration is determined by a group value include seventeen angular magnitudes of servo motors. Unfortunately, this inefficiently and non-systematic procedure will be the serious obstruct when we develop the biped gait. The current procedure of design the robot motion is shown in Fig.2.5, because a group value include seventeen angular magnitudes of servo motors as previous description, it means, one has to adjusts the each value one by one until all the motors are in the new position. One can easy to perceive that from step two to step five are presented a try and error process. The result brings from this try and error process is procrastinate and tardy progress when design a robot motion. Besides, there is no other way can guarantee the stability, reliability and efficiency of the new motion except execute the motion and observe it. Since there are several drawbacks in the current procedure of motion design for biped. 14.

(32) Fig.2.3 Communicated software, Heart to Heart.. 15.

(33) robot locomotion, we have to develop a new procedure which can eliminate these drawbacks and make the stability, reliability and efficiency to be guaranteed. The procedure we develop in this research will be discussed in next chapter.. 2.2.6 Example of Motion Create by Try and Error Here we are going to introduce the sample of motion which is created by the current procedure, try and error. The motion is called prostrate to stand up, which include several postures transmission. When the motion is very complicated or involve the transmission between several different motions, it will cause the difficulty of designing procedure to be more and more complex. The initial posture is shown in Fig.2.6 (a), the robot lies on the ground, the first transmission between motions is shown in Fig.2.6 (b), swing the arms and prop body by arms support on the ground. The second transmission is shown in Fig.2.6 (c), cower the thighs and legs towards the body so that the robot can try to make the center of mass moves to the lower body. The third transmission is shown in Fig.2.6 (d), when the thighs and legs are moving toward the body and the soles are supporting on the ground, the reaction force present the kinetic energy and the robot starts moving body and its body becomes backward, the arms are more and more distant from the ground. We called the posture in this moment, squat. The last transmission is shown in Fig.2.6 (e) and (f), the position of center of mass is located in lower body so that the robot can ascend the body into the goal posture, stand.. 16.

(34) (a). (b). (c). Fig.2.4 (a) The circuit I/O set configuration on RCB-1. (b) The comparison of number of servo motors and I/O on RCB-1. (c) The comparison of number of servo motors and the location on KHR-1. 17.

(35) (1)Define initial posture and goal posture (2) Adjust configuration 1, C1 with group value, CH 11 ~ CH 241 (3) Add the group value of C1 to motion editor (4) Adjust configuration N, C N with group value, CH 1N ~ CH 24 N. (5) Add the group value of C N to motion editor. (9) Back to adjust C r. (8) Check which configuration is reasonable, C r. (6) Produce the motion, M 1 .. (7) Experiment Check if the motion M 1 is desired target.. No. Yes (10) Produce the desire trajectory. Fig 2.5 Current procedure of design motion. 18.

(36) (a). (b). (c). (d). (e). (f). Fig 2.6 Example motion, prostrate to stand up, created by try and error. (a) Initial posture. (b) Support by arms. (c) Curling the thigh towards to body. (d) Support by arms and thigh. (e) Arms getting away from ground. (f) Goal posture, stand up.. 19.

(37) 2.3 Kinematical Model of KHR-1 Robot A kinematical model has been considered as a spatial parallel type manipulator, which is adopted to simplify the KHR-1 biped robot system. This kinematical model is an intermediary when design the joint trajectories by programming. We constraint the upper body of KHR-1 robot is immobile; therefore we need not consider the upper body in the kinematical model. Besides, via appropriate assumptions to simplify the kinematical model, the original characteristic of model will transform to planar serial type manipulator. We are going to present appropriate assumptions in chapter three. A serial type manipulator consists of several links connected in series by various types of joints, typically revolute and prismatic joints. One end of the manipulator is attached to the ground and the other end is free to move in space. For this reason a serial type manipulator is sometimes called an open-loop manipulator. We call the fixed link the base, and the free end where a gripper or a mechanical hand is attached, the end-effector. We use this planar serial type manipulator, is shown in Fig. 2.7 (a), which has three degree of freedom to be a kinematical model and simulate a KHR-1 biped robot which is introduced in next section. The object of making analogy between this three link serial manipulator and KHR-1 biped robot are: (1) Simplifying the multi rigid body of KHR-1 biped robot and replace it by an extremely simple constitution to be the kinematical model in the whole experimentation. The contribution of this vehicle can produce the higher efficiency when design the robot motion. (2) Via solving the direct kinematics and inverse kinematics problem of serial type manipulator, we can find a solution of producing ideal trajectory for robotic motion planning. In solving the inverse kinematics problems, we often interested in obtaining a closed-form solution that is, in reducing the problem to an algebraic equation relating the. 20.

(38) end-effector location to a single joint variable. The number of possible inverse kinematics solutions depends on the type and location of a robot manipulator. In general, closed-form solutions can be found for manipulators with simple geometry, such as manipulators with three consecutive joint axes intersecting at the common point or three consecutive joint axes parallel to another. For a manipulator of general geometry, the inverse kinematics problem becomes a very difficult task. Therefore, we use the Denavit and Hartenberg’s method to solve the inverse kinematics problem in this research.. 2.3.1 Denavit-Hartenberg Homogeneous Transformation Matrices The method created by Denavit and Hartenberg in year 1955 is systematic in nature and more suitable for the kinematics analysis of serial manipulator. First of all we have to take a look for the basic conception of D-H method. We are going to establish a coordinate system to each link of a manipulator, a 4 × 4 transformation matrix relating two successive coordinate systems can be established. The definition of link parameters is shown in Fig.2.8.. 2.3.2 Link Parameter and Link Coordinate System Here are the 4 × 4 transformation matrix of D-H method: (1) The (i − 1) th coordinate system is translated along the z i −1 axis a distance d i . This brings the origin Oi −1 into coincidence with H i −1 . This corresponding transformation matrix is ⎡1 ⎢0 T (z, d ) = ⎢ ⎢0 ⎢ ⎣0. 0 1 0 0. 0 0⎤ 0 0 ⎥⎥ 1 di ⎥ ⎥ 0 1⎦. (2) The displaced (i − 1) th coordinate system is rotated about the z i −1 axis an angle θ i , which brings the xi −1 axis. The corresponding transformation matrix is. 21.

(39) Q(CoM). a3 X2. θ3 P. Y2. θ2. a2. Y0. O. X1. ′ a2. θ17. θ13 ′ a2. M a1. ′ a1. θ1. ′ a1. X0. Y. Y. ′ a0. θ 23. Sole X. Z. Fig.2.7 A three link planar serial type manipulator.. 22. ′ a0. θ19.

(40) Zi-1. Zi Joint i. Joint i+1. Link i. αi. Xi. ai Oi Zi-2. θi Hi-1. di. Oi-1. Xi-1. Hi-2. Fig.2.8 Definition of link parameters.. 23.

(41) ⎡cθ i ⎢ sθ T (z,θ ) = ⎢ i ⎢ 0 ⎢ ⎣ 0. − sθ i cθ i 0 0. 0 0 1 0. 0⎤ 0⎥⎥ 0⎥ ⎥ 1⎦. (3) The displaced (i − 1) th coordinate system is translated along the xi axis a distance ai . This brings the origin Oi −1 into coincidence with Oi . The corresponding transformation matrix is. ⎡1 ⎢0 T ( x, a ) = ⎢ ⎢0 ⎢ ⎣0. 0 1 0 0. 0 ai ⎤ 0 0 ⎥⎥ 1 0⎥ ⎥ 0 1⎦. (4) The displaced (i − 1) th coordinate system is rotated about the xi axis an angle α i , which brings the two coordinate system into complete coincident. The corresponding transformation matrix is. ⎡1 0 ⎢0 cα i T ( x, α ) = ⎢ ⎢0 sα i ⎢ ⎣0 0. 0 − sα i. cα i 0. 0⎤ 0⎥⎥ 0⎥ ⎥ 1⎦. We may think of the transformations above as four basic transformations about the moving coordinate axes. Therefore, the resulting transformation matrix, i −1 Ai , is given by i −1. Ai = T ( z, d )T ( z,θ )T ( x, a )T ( x, α ) .. (2.1). Expanding Eqs. (2.1), we obtain ⎡cθi ⎢ sθ i −1 Ai = ⎢ i ⎢0 ⎢ ⎣0 where. i −1. − cα i sθ i cα i cθi sα i 0. sα i sθi − sα i cθ i cα i 0. ai cθ i ⎤ ai sθi ⎥⎥ di ⎥ ⎥ 1 ⎦. (2.2). Ai define arbitrary link with the four parameters, d i , θ i , ai , α i . Equation (2.2) is. called Denavit-Hartenberg (D-H) transformation matrix.. 24.

(42) 2.3.3 Closure-loop Equation. In this section we are going to formulate the entire coordinated system of three link planar serial type manipulator by D-H method. According to the definition of D-H method, we can list out the D-H parameters of a 3 link manipulator is shown in Table 2.1. Substituting the D-H parameters in to Eqs. (2.2), we obtain ⎡cθ1 − sθ 1 ⎢sθ cθ1 0 A1 = T (Z0 , 0)R(Z0 ,θ1 )T ( X1, a1 )R( X1, 0) = ⎢ 1 ⎢0 0 ⎢ 0 ⎣0 ⎡cθ 2 ⎢ sθ 1 A2 = T (Z 1 , 0)R(Z 1 , θ 2 )T ( X 2 , a 2 )R( X 2 , 0) = ⎢ 2 ⎢ 0 ⎢ ⎣ 0. − sθ 2 cθ 2. ⎡cθ 3 ⎢ sθ 2 A3 = T (Z 2 , 0)R(Z 2 , θ 3 )T ( X 3 , a 3 )R( X 3 , 0 ) = ⎢ 3 ⎢ 0 ⎢ ⎣ 0. − sθ 3. 0 0. cθ 3 0 0. 0 a1cθ1 ⎤ 0 a1sθ1 ⎥⎥ 1 0 ⎥ ⎥ 0 1 ⎦. (2.3). 0 a 2 cθ 2 ⎤ 0 a 2 sθ 2 ⎥⎥ 1 0 ⎥ ⎥ 0 1 ⎦. (2.4). 0 a 3 cθ 3 ⎤ 0 a 3 sθ 3 ⎥⎥ 1 0 ⎥ ⎥ 0 1 ⎦. (2.5). Furthermore, we can use the loop-closure Equation 0. A1 1 A2 2 A3 = 0 A3. (2.6). then substituting the Eqs. (2.3), (2.4) and (2.5) to (2.6), we obtain ⎡cθ123 − sθ123 ⎢sθ cθ123 0 A3 = ⎢ 123 ⎢ 0 0 ⎢ 0 ⎣ 0. 0 a1cθ1 + a2cθ12 + a3cθ123⎤ 0 a1sθ1 + a2sθ12 + a3sθ123⎥⎥ ⎥ 1 0 ⎥ 0 1 ⎦. (2.7). One can compute the position of the end-effector point by using the Eqs. (2.7), which is direct kinematics as ⎡0⎤ ⎡a1cθ1 + a2cθ12 + a3cθ123⎤ ⎡qx ⎤ ⎥ ⎢ ⎥ ⎢ ⎢q ⎥ ⎢ y ⎥=0A3 ⎢0⎥ = ⎢a1sθ1 + a2 sθ12 + a3 sθ123 ⎥ ⎥ ⎢0⎥ ⎢ ⎢qz ⎥ 0 ⎥ ⎢ ⎥ ⎢ ⎢ ⎥ 1 ⎦ ⎣1⎦ ⎣ ⎣1⎦. 25. (2.8).

(43) [. ]. where qx qy qz 1 =0q is position vector of Q with respect to the base coordinate system; T. and the position vector of the end-effector coordinate system is given by 3q = [0,0,0,1] . T. Suppose given θ 1 , θ 2 and θ 3 , the position of point Q can be computed by Eqs. (2.8).. 2.4 Jacobian Analysis of Three Links Planar Serial Type Manipulator. In the last section we have discussed about the position analysis of the three links planar serial type manipulator. This knowledge enables us to bring the end-effector to some desired locations in space. In this section we are going to extend the position analysis problem with our kinematical model, the three links serial type manipulator, to a velocity analysis problem of serial manipulator. For our trajectory planning, it is necessary to move the end effector of a manipulator along some desired paths with a prescribed speed. To achieve this goal, the motion of the individual joints of a manipulator must be carefully coordinated. For the robot manipulators, the Jacobian matrix is defined as the matrix that transforms the joint rates in the actuator space to the velocity state in the end-effector space. The Jacobian matrix is a critical component for generating trajectories of prescribed geometry in the end-effector space.. 2.4.1 Mathematical model of Jacobian Matrix. According to the three links planar serial manipulator is shown in Fig. 2.7, the position of end effector, Q, can the determined as Qx = a1cθ1 + a2cθ12 + a3cθ123. (2.9). Q y = a1 sθ1 + a 2 sθ12 + a3 sθ123. (2.10). ⎡0⎤ Z 0 = Z 1 = Z 2 = ⎢⎢0⎥⎥ ⎢⎣1⎥⎦. (2.11). 26.

(44) From Eqs. (2.9) and (2.10), take the derivative with respect to the time, we obtain dQˆ x = Q& x = Vqx = − a1 sθ1θ&1 − a 2 sθ12θ&12 − a3 sθ123θ&123 dt dQˆ y dt. = Q& y = Vqy = a1cθ1θ&1 + a2cθ12θ&12 + a3cθ123θ&123. (2.12). (2.13). Write the Eqs. (2.12) and (2.13) into the matrix form as ⎡Q& x ⎤ ⎡Vqx ⎤ ⎡− a1sθ1 − a2 sθ12 − a3 sθ123 ⎢& ⎥ ⎢ ⎥ ⎢ ⎢Qy ⎥ = ⎢Vqy ⎥ = ⎢ a1cθ1 + a2cθ12 + a3cθ123 ⎢Q& z ⎥ ⎢ωqz ⎥ ⎣⎢ 1 ⎣ ⎦ ⎣ ⎦ ⎡Vqx ⎤ ⎢ ⎥ ⎢Vqy ⎥ = ⎢ω qz ⎥ ⎣ ⎦ where the J determined the Jacobian matrix.. 27. − a2 sθ12 − a3 sθ123 a2cθ12 + a3cθ123. ⎡θ&1 ⎤ ⎢ ⎥ J ⎢θ&2 ⎥ ⎢θ&3 ⎥ ⎣ ⎦. 1. − a3 sθ123 ⎤ ⎡θ&1 ⎤ ⎢ ⎥ a3cθ123 ⎥⎥ ⎢θ&2 ⎥ (2.14) 1 ⎦⎥ ⎢⎣θ&3 ⎥⎦. (2.15).

(45) Table.2.1. D-H parameters of a three link planar serial type manipulator.. Joint i 1 2 3. αi. ai. di. θi. 0 0 0. a1. 0 0 0. θ1 θ2 − θ3. a2 a3. 28.

(46) 2.5 The Transforming Procedure with KHR-1. The purpose of using transforming procedure is transform the coordinate system of kinematical model as same as KHR-1 biped robot. Due to there are differences of construction between KHR-1 biped robot and kinematical model , it is necessary to transform the data of desired joint trajectory to a proper data file of KHR-1.. 2.5.1 The Coordinate System Transformation. The three links planar serial manipulator is adopted to be a kinematical model to simulate the KHR-1 robot, and we have been introduced the kinematical model in section 2.3. A serious problem after the desired trajectory design is how we transform the data file of desired trajectory into KHR-1 robot by an exact, correct and simple procedure. The first part of this issue is transforming the coordinate system between the kinematical model and KHR-1 robot. The KHR-1 robot constructed from seventeen servo motors, and each motor marked with an individual number, which is shown in Fig.2.9 (a), and we has the analogy of kinematical model to the right thigh of KHR-1, which is shown in Fig.2.9 (b), and the left thigh in Fig.2.9 (c). Comparing the kinematical model and right thigh of KHR-1, we can find out that the coordinate system of motor number twenty one and twenty of right thigh, and link two, link three of kinematical model are quite different from each other. The comparison result is shown in Fig.2.10. In terms of link one of kinematical model, coordinate system x0 − y 0 is completely the same with coordinate system x 22 − y 22 of motor number twenty two, so that need not any transformation between these two coordinate systems. In terms of link two of kinematical model, the coordinate system x1 − y1 needs to be rotated 90 degree to meet the coordinate system x 21 − y 21 , in terms of link three of kinematical model; coordinate system x 2 − y 2 needs to be rotated 90 degree to meet coordinate system x 20 − y 20 . 29.

(47) (a). Yr. 6 9. 8. 1. 7 19. 13. 20. 14. 21. 15. 22. 16. 23 (b). 2. 3 Xr. 17 (c). Z20 Y21 Y20. Z14. Y15 Z21. Y14. Z15 Y16. Y22 Z22. Z16. Fig.2.9 (a) Complete order number of each servo motor on KHR-1 robot. (b) The coordinate system of right thigh (No.20~22 motors) of robot. (c) The coordinate system of left thigh (No.14~16 motors) of robot.. 30.

(48) Q (CoM). a3 x2. Motor No.21 Z 20. y1. y2. a2. y 20. x1. y 21. y 22. Motor No.20. a1. y0. Z 22. Motor No.22. x0 c 20. Z 21 c 21. Fig.2.10 The coordinate system comparison of physical model and KHR-1. 31.

(49) Besides, a shift angle is produced by the arrangement distance when assemble the motors number twenty one and twenty. Therefore, we must consider the shift angle and find the mathematical relation between link two and these two motors. In Fig.2.11, the shift angle is calculated with distance between joints of the motors and is 27.78°. This shift angle cause the link two constructed with the conceal distance from joint of motor number twenty one to twenty. For this reason, we can integrate the coordinate system and describe them as Table 2.2. According to Table 2.2, the data file of desired trajectory transformation can be accomplished.. 2.5.2 Definition of Link Three of Kinematical Model. The shift angle we discussed in last section cause the conceal distance, and the distance is actually determined the link two of kinematical model. In terms of kinematical model, link three is a special case when match the configuration with KHR-1 robot. Because we constraint the configuration of upper body is fixed during the biped gait is manipulating, therefore, we can simplify the upper body of KHR-1 biped robot to a particle as the end-effector. In other words, the definition of link three of kinematical model is determined as the distance from joint of motor number twenty to center of mass of upper body. The canter of mass can be calculated is shown in Fig.2.12, measured the vector from joint of motor twenty to global origin of KHR-1, and measured the vector from center of mass of upper body to global origin. We can obtain the distance between joint of motor twenty to CoM of upper body.. 32.

(50) Table 2.2 Coordinate system transformation between kinematical model and KHR-1. Physical Model. Coordinate System. Transform. Coordinate System. KHR-1. Link 1. x0 − y 0. 0. z 22 − y 22. Motor 22. Link 2. x1 − y1. 90-27.78=62.22. z 21 − y 21. Motor 21. Link 3. x2 − y 2. 90-27.78=62.22. z 20 − y 20. Motor 20. 46.34mm 27.78∘. 24.41mm. Fig.2.11 The shift angle between motors number 21 and 20.. 33.

(51) 48.37 mm Global origin of KHR-1. 19.22 mm. 31.37 mm. 79.14 mm. 18.56 mm. joint of motor 20. Th. l. Fig.2.12 The distance between joint of motor number 20 and canter of mass of upper body.. 34.

(52) CHAPTER 3 Introduction of Main Algorithm 3.1 Introduction In this chapter we are going to introduce the main algorithms and the assumptions of simplify the kinematical model in the research. There are three different algorithms were adopted for designing the joint trajectory. Manipulability ellipsoid algorithm which includes velocity ellipsoid and force ellipsoid, ZMP (Zero-Moment Point) algorithm and inverse kinematics. For the purpose of simplifying the kinematical model of KHR-1 robot, we must set several assumptions which associate with these algorithms.. 3.2 Manipulability Ellipsoid Algorithm The manipulability ellipsoid algorithm is a kernel of research for developing stable condition of biped robot gait, where velocity ellipsoid is for creating the stable locomotion by defining the minimum combination of angular velocity of each joint in arbitrary interval of time, and the force ellipsoid is for reducing the output force consumption. We develop a new strategy which can improve the stability and reduce the energy consumption via establishing both two ellipsoids.. 3.2.1 Condition Number In Last chapter we have discussed about the Jacobian matrix of three links serial manipulator. The Jacobian matrix, J, transform the joint rate in n-dimensional space into the end-effector velocity in m-dimensional space. We have the kinematical model which includes all joint are revolute, therefore the first three rows of Jacobian matrix, J, have the dimension of length, whereas the last three rows are dimensionless. For those manipulators with only one type of joint and for one type of task, either point positioning or body orienting but not both, 35.

(53) the Jacobian matrix can be characterized by a measure called the condition number, c [11]. The condition number of a matrix A is defined as. c = A A −1. (3.1). Where the norm of A is defined as A = max x≠0. Ax. (3.2). x. In other word, the norm of A bounds the amplifying power of the matrix: Ax ≤ A x. for all vector x,. (3.3). The condition number of the Jacobian matrix depends on the link lengths and the manipulator configuration. As the end-effector moves from location to location, the condition number will assume different values. The minimum condition number of any matrix is 1. Those points in the works space of a manipulator where the condition number of the Jacobian matrix is equal to 1 are called isotropic points [12].. 3.2.2 Velocity Ellipsoid Model For the target of comparing the characteristics to compare the joint rates required to produce a unit end-effector velocity in all possible directions. To achieve this goal, we confine the end-effector velocity vector on an m-dimensional unit sphere,. x& T x& = 1. (3.4). x& = Jq&. (3.5). Where. and q& is an n-dimensional velocity vector which mapped by Jacobian matrix into m-dimensional velocity vector x& . Compare the corresponding joint rates in the n-dimensional joint space. Substituting Eq. (3.5) into (3.4) yields 36.

(54) q& T J T Jq& = 1. (3.6). Equation (3.6) represents an ellipsoid in the n-dimensional joint space. Because the product J T J is symmetric positive semi definite, its eigenvectors are orthogonal, The principle axes of the ellipsoid coincide with the eigenvectors of J T J , and the length of its principle axes are equal to the reciprocals of the square roots of the eigenvectors of J T J . Since the Jacobian matrix is configuration dependent, the ellipsoid is also configuration dependent. As the end-effector moves from one location to another, the shape and orientation of the ellipsoid will also change accordingly. The closer the velocity ellipsoids to a sphere, the better the transformation characteristics are. The transformation is said to be isotropic when the principle axes are all of equal length. At an isotropic point, a unit sphere in the n-dimensional joint space. On the other hand, at a singular point, one or more of the principle axes becomes infinitely long and the ellipsoid degenerates into a cylinder. Under such a condition, the end-effector will not be able to move in some directions. Consider a three links planar serial manipulator is shown in Fig.2.7, apply the restricted condition which present in chapter one, reduce the Jacobian matrix of Eq.(2.14) to ⎡− a sθ − a 2 sθ12 J =⎢ 1 1 ⎣ a1cθ 1 + a 2 cθ12. − a 2 sθ 12 ⎤ a 2 cθ 12 ⎥⎦. (3.7). and. ⎡a12 + a22 + 2a1a2 cθ 2 a1a2 cθ 2 + a22 ⎤ J J =⎢ ⎥ 2 a22 ⎦ ⎣ a1a2 cθ 2 + a2 T. Substituting the length of link one. 2 m and link two 1 m into Eq.(3.8), we obtain. ⎡3 + 2cθ 2 JT J = ⎢ ⎣ 2cθ 2 + 1 Let us assume the θ 2 =. π 2. (3.8). 2cθ 2 + 1⎤ ⎥ 1 ⎦. , then the matrix product J T J becomes. 37. (3.9).

(55) ⎡3 1⎤ JT J = ⎢ ⎥ ⎣1 1⎦. (3.10). The eigenvalues of J T J are. λ1 = 0.5858 , λ 2 = 3.4142 and the eigenvector is. ⎡ 0.3827 - 0.9238⎤ ⎢- 0.9238 0.3827 ⎥ ⎣ ⎦ Substituting Eq. (3.10) to (3.6) yields. (0.3827θ& − 0.9238θ& ) + (0.9238θ& + 0.3827θ& ) = 2. 3θ& + 2θ&1θ&2 + θ 2 1. 2 2. 1. 2. 1.30652. 2. 1. 2. 0.54152. =1. (3.11). Equation (3.12) represents an ellipse is shown in Fig.3.1 (b), the joint rates required to. (. ). produce a unity end-effector velocity are θ&1 ,θ&2 = (− 0.500,1.207 ) rad/s along the major axis,. (. ). and θ&1 ,θ&2 = (0.500,0.207 ) rad/s along the minor axis. Without loss the generality, we assume that θ 1 = 0 . Then the Jacobian matrix becomes ⎡ − 1 − 1⎤ J =⎢ ⎥ ⎣ 2 0⎦. (3.12). Hence the corresponding end-effector velocities are (v& x , v& y ) = (− 0.707,−0.707 ) m/s along. the major axis and (v&x , v& y ) = (− 0.707,0.707 ) m/s along the minor axis, respectively, is shown in Fig.3.1 (a). We notice that to produce the same end-effector speed along the principle axes, one requires the largest joint rates while the other requires the smallest joint rates.. 38.

(56) (a). (b). θ&2. VY. M (-0.500, 1.207). P (-0.707, 0.707) P (0.500, 0.207) VX. M (-0.707, -0.707). Fig.3.1 (a) Unity circle of velocity of end-effector in Cartesian coordinate. (b) Velocity ellipsoid.. 39. θ&1.

(57) 3.3 Force Ellipsoid Model. Similar to the transformation of velocities, the transformation of forces for manipulators with only one type of joints and for one type of tasks can be characterized by a comparison of the end-effector force produced by a unity joint torque.. 3.3.1 Application of the Principle of Virtual Work. For a serial manipulator, the virtual displacements at the joints can be written as. δq = [δq1 , δq 2 , Kδq n ]T , and the virtual displacement of the end-effector can be expressed as. δx = [δx, δy, Kδψ ]T . Let the end-effector output force and moment be denoted by ⎡f⎤ F= ⎢ ⎥ ⎣n⎦. (3.13). Also the vector of joint torques be denoted by ⎡τ 1 ⎤ ⎢τ ⎥ τ = ⎢ 2⎥ ⎢M⎥ ⎢ ⎥ ⎣τ n ⎦. (3.14). Assuming the frictional forces at the joints are negligible, the virtual work produced by the force of constraint at the joints is zero. Hence, by neglecting the gravitational effect, the virtual work, δW , define by all active forces is given by. δW = τ T δq − F T δx. (3.15). The principle of virtual work states that a system is under equilibrium if and only if the virtual work vanishes for any infinitesimal virtual displacement. This is true if the virtual displacements are compatible with the constraints imposed on the system. In Eq. (3.15), however, the virtual displacement δq and δx are not independent, In fact, they are related by the conventional Jacobian matrix as follows:. δq = Jδq 40. (3.16).

(58) Substituting Eq.(3.16) into (3.15) yields. (τ. T. ). − F T J δq = 0. (3.17). Since Eq.(3.17) holds for any arbitrary virtual displacement, δq , we conclude that. τ T − FT J = 0. (3.18). τ = JTF. (3.19). Taking the transpose of Eq.(3.18) yields. Equation (3.19) maps an m-dimensional end-effector output force into an n-dimensional joint torques. Since the Jacobian matrix is configuration dependent, the mapping is also configuration dependent.[11]. 3.3.2 Force ellipsoid. Similar to transformation of velocities, the transformation of force for manipulators with only one type of joints and for one type of tasks can be characterized by a comparison of the end-effector force produced by a unit joint torque. Substituting Eq. (3.19) into τ T τ = 1 yields F T JJ T F = 1. (3.20). At a given manipulator configuration, Eq. (3.20) represents an m-dimensional ellipsoid. Because the product JJ T is symmetric positive semi definite, its eigenvectors are orthogonal. The principle axes of the ellipsoid coincide with the eigenvectors of JJ T , and their lengths are equal to the reciprocals of the square roots of the eigenvalues. Since the Jacobian matrix is configuration dependent, the force ellipsoid is also configuration dependent. As the end-effector moves from one location to another, the shape and orientation of the force ellipsoid will also change accordingly. The closer the transmission is said to be isotropic when the principle axes are of equal lengths. At an isotropic point, an n-dimensional unit sphere in the joint torque space maps onto m-dimensional sphere in the end-effector force space. On the other hand, at a singular point, and n-dimensional unit sphere 41.

(59) in the joint torque space maps onto an m-dimensional cylinder in the end-effector force space. Thus the mechanical advantage of the manipulator becomes infinitely large in some direction. Consider a three links planar serial manipulator is shown in Fig.2.7, apply the restricted condition which present in chapter one, reduce the Jacobian matrix of Eq.2.14 to Eq. (3.7), and yield a 2-dof manipulator. For this 2-dof manipulator, the end-effector output force and. [. ]. [. input joint torques can be written as f = f x , f y and τ = τ x ,τ y T. ]. T. , respectively. Substituting. the Jacobain matrix, Eq. (3.7), into (3.19), we obtain ⎡τ 1 ⎤ ⎡− a1 sθ1 − a 2 sθ12 ⎢τ ⎥ = ⎢ − a 2 sθ 12 ⎣ 2⎦ ⎣. a1cθ1 + a 2 cθ12 ⎤ ⎡ f x ⎤ ⎥⎢ f ⎥ a 2 cθ12 ⎦⎣ y ⎦. (3.21). Let the link lengths be a1 = 2 m and a 2 = 1 m. At the posture where θ 1 = 0 and θ1 = π / 2 , the Jacobian matrix reduces to ⎡ − 1 − 1⎤ J =⎢ ⎥. ⎣ 2 0⎦ Hence ⎡ 2 JJ T = ⎢ ⎣− 2 The. eigenvalues. of. − 2⎤ ⎥. 2 ⎦. JJ T are λ1 = 2 − 2 = 0.5858 and λ2 = 2 + 2 = 3.4142 .. The. corresponding eigenvectors, normalized to unit length, are (0.707, 0.707) and (-0.707, 0.707), respectively. These two eigenvectors are at 45∘angles with the f x and f y axes, respectively, and they are lined up with the principle axes of the ellipse. Substituting JJ T into (3.20), we obtain 2. 2 fx − 2 2 fx f y + 2 f y 2. 2. 2. fy ⎞ f ⎞ ⎛ f ⎛ f ⎟⎟ + 3.4142⎜⎜ x − y ⎟⎟ = 1 = 0.5858⎜⎜ x + 2⎠ 2⎠ ⎝ 2 ⎝ 2. Figure 3.2 shows the ellipse and its principal axes. The end-effector forces produced by a unit. 42.

(60) joint torque are ( f x , f y ) = (0.924,0.924) N along the minor axis. The corresponding joint torques are (τ 1 ,τ 2 ) = (0.383,−0.924 )N ⋅ m along the major axis and (τ 1 ,τ 2 ) = (0.924,0.383) N ⋅ m along the minor axis. We note that the mechanical advantage along the major axis is. larger than that along the minor axis.. 43.

(61) (a). (b). τy. fy M (0.924, 0.924) P (0.924, 0.383) P (-0.383, 0.383). τx. fx. M (0.383, -0.924). Fig.3.2 (a) Unity circle of joint torque. (b) Force ellipsoid of end-effector.. 44.

(62) 3.4 Inverse Kinematics. For the inverse kinematics problem, the location of the end-effector is given and the problem is to find the joint angles θ i , i = 1, 2, 3, necessary to bring the end-effector to the desired location. For a planar 3-dof manipulator, the end-effector can be specified in terms of the position of point Q and orientation angle Φ of the end-effector. Hence the overall transformation matrix from the end-effector coordinate system to the base coordinate system, 0. A3 , is given by ⎡cΦ − sΦ ⎢ sΦ cΦ 0 A3 = ⎢ ⎢0 0 ⎢ 0 ⎣0. 0 qx ⎤ 0 q y ⎥⎥ 1 0⎥ ⎥ 0 1⎦. (3.21). Inverse kinematics solution can be obtained by equating the elements of Eq. (2.7) to that of (3.21). To find the orientation of the end-effector, we equate the (1, 1) and (2, 1) elements of Eq. (2.7) to that of (3.21): cθ123 = cΦ,. (3.22). sθ123 = sΦ,. (3.23). θ123 = θ1 + θ 2 + θ3 = Φ. (3.24). Hence. Next we equate the (1, 4) and (2, 4) elements of Eq. (2.7) to that of (3.21): PX = a1cθ1 + a2cθ12 ,. (3.25). PY = a1sθ1 + a2 sθ12 ,. (3.26). where PX = q X − a3cΦ and PY = qY − a3 sΦ denote the position vector of the point P located at the third joint axis is shown in Fig. 2.7. Note that by using this substitution θ3 disappears from Eq. (3.25) and (3.26). From Fig. 2.7 we observe that the distance from point O to P is independent of θ1 . Hence we can eliminate θ1 by summing the squares of Eq. 45.

(63) (3.25) and (3.26); that is, PX + PY = a1 + a2 + 2a1a2cθ 2 2. 2. 2. 2. (3.27). Solving Eq. (3.27) for θ 2 , we obtain. θ 2 = cos −1 k ,. (3.28). where. PX + PY − a1 − a2 2a1a2 2. k=. 2. 2. 2. Equation (3.28) yields (1) two real roots if k < 1 , (2) one double root if k = 1 , and (3) ∗. ∗. no real roots if k > 1 , In general, if θ 2 = θ 2 is a solution, θ 2 = −θ 2 is also a solution, where. π ≥ θ 2∗ ≥ 0 . We call θ 2 = θ 2∗ the elbow-down solution and θ 2 = −θ 2∗ the elbow-up solution. If k = 1 , the arm is in a fully stretched or folded configuration. If k > 1 , the position is not reachable. Corresponding to each θ 2 , we can solve θ1 by expending Eq. (3.25) and (3.26) as follows:. (a1 + a2cθ 2 )cθ1 − (a2 sθ 2 )sθ1 =. pX ,. (3.29). (a2 Sθ 2 )cθ1 + (a1 + a2cθ 2 )sθ1 =. pY ,. (3.30). Solving Eq.(3.29) and (3.30) for cθ1 and sθ1 , yields cθ1 = sθ1 =. p X (a1 + a2cθ 2 ) + PY a2 sθ 2 , ∆ − PY a2 sθ 2 + p y (a1 + a2cθ 2 ) ∆. ,. where ∆ = a1 + a2 + 2a1a2cθ 2 . Hence, corresponding to each θ 2 , we obtain a unique 2. 2. solution for θ1 :. θ1 = A tan 2(sθ1 , cθ1 ) . 46. (3.31).

(64) In a computer program we may use the function A tan 2( x, y ) to obtain a unique solution for θ1 . However, the solution may be real or complex. A complex solution corresponds to an end-effector location that is not reachable by the manipulator. Once θ1 and. θ 2 are known, Eq. (3.24) yields a unique solution for θ3 . Hence, corresponding to a given end-effector location, there are generally two real inverse kinematics solution, one being the reflection of the other about a line connecting points O and P, is illustrated in Fig. 3.3 For the purpose of simulation of human being’s motion, we have to constraint the work space of joint of KHR-1 biped robot and kinematical model into a valid sphere. In other words, when we have two possible inverse kinematics solutions, each one must be satisfied with the constraint of human being’s motion, any invalid posture such as elbow up in Fig. 3.3, will be ignored in our biped robot gait generation procedure. We adopted the inverse kinematics to solve the problem of two legs locomotion of biped robot. The original characteristic of kinematical model of KHR-1 biped robot is a spatial parallel type manipulator, is shown in Fig. 3.4, we can see the front leg with angles, θ1F and. θ 2 F , back leg with angles, θ1B and θ 2 B . Based on the inverse kinematics, we can separate these two legs into two planar serial type manipulators, and then simplify the original characteristic of kinematical model. Assuming the posture of front leg in arbitrary interval of time is given by defining the coordinate system, therefore, the end-effecotr, point H, is given, too. Notice that the end-effector of front leg is also the one of back leg, according to this correlation; one can find out the configuration of back leg and obtain the solutions of angles of θ1B and θ 2 B by inverse kinematics.. 47.

(65) Q(CoM), end-effector. −θ∗. θ∗. (Elbow up). Y0. (Elbow down). O. X0. Sole. Fig.3.3 Two possible inverse kinematics solution. 48.

(66) (a). (b). Q. Q. H. H. θ13. θ17. θ2F. ′ a2. ′ a2. θ2B. θ1B. ′ a1. ′ a1. θ1F. ′ a0. Y. θ 23. ′ a0. Y. X. Z. Fig.3.4 Inverse kinematics implement for simplifying the kinematical model. (a) X-Y plane (b) Z-Y plane. 49. θ19.

(67) 3.5 ZMP (Zero-Moment Point) Algorithm. ZMP (Zero-Moment Point) algorithm was presented by M. Vukobratovich in 1970’s. The ZMP algorithm has been presented over thirty five years. Its first practical demonstration was announced at Waseda University, Laboratory of Ichiro Kato in Japan in year 1984. It is also the first demonstration of dynamically balanced case of the WL-10RD robot which was the member of the robot family WABOT [13]. Referring to Fig. 3.5 (a), the ZMP is the point on the ground where the tipping moment, M P n , acting on the biped, due to gravity and inertia forces, equals zero, the tipping moment being defined as the component of the moment that is tangential to the supporting surface. It should be noted that the term ZMP is not a perfectly exact expression because the normal component of the moment generated by the inertia forces acting on the biped is not necessarily zero. If we bear in mind, however, that ZMP abridges the exact expression “zero tipping moment point,” then the term becomes perfectly acceptable [14]. The robot is subjected to a Ground Reaction Force (GRF), Rˆ , at the point P, which is the center of pressure (CoP). Due to unilaterality of the GRF, P is always located within the convex hull of the foot support area. In Fig. 3.5 (b) left part, the GRF passes through the CoM at point G and consequently generates a zero moment. Thus H& G = 0 and the robot will not be rotated. In Fig. 3.5 (b) right part, the GRF does not pass through the CoM thus generating a net clockwise moment around the CoM, i.e. H& G = GP × Rˆ ≠ 0 . This implies the tendency of the robot to tip forward. In current demonstrated example, based on the ZMP algorithm, the X-Z plane projection of zero moment point, Q′ , was constricted in the convex hull which was formed by single leg or two legs, the schematic area illustrated in Fig. 3.6 (a) and (b). Therefore, the point, Q in X-Y plane, Q′ in Z-Y plane must be satisfied based on this theory so that the biped gait will be kept in the stable condition. 50.

(68) (a). R Pn M Kf n. Rfn. K K. M Pn. (b). H& G = GP × R. a. G. H& G = 0. a. G. m⋅ g m⋅ g. P. P R. R. A. Fig.3.5 (a) The Single sole support area with tipping moment. (b) General configuration of a biped robot under interaction force/torque from ground and environment.. 51.

(69) (a) C. B Z. Back leg D. UBX LBX. WP. A. Q′. Front leg. WS. E. X. F. LSOLE. (b). Z. Q′. X. Fig.3.6 (a) Double support phase. (b) Single support phase.. 52.

(70) CHAPTER 4 Result of Simulation and Theoretical Analysis. 4.1 Introduction In this research we design a sequent biped gait which includes three envelopes (Fig. 1.1), the first envelope was defined as a robot or a human being is trying to approach to posture of standing from initial posture of squatting. The second envelope was defined as making first stride from initial posture of standing and the last envelope was defined as making another stride from the initial posture of previous stride. Envelope two and three were connected as a cycle of walking motion, walk slowly, which is compared with a human being’s action in the reality. We had been mentioned assumptions in chapter one, base on the static kinematics, in a specific locomotion which manipulate very slowly, however, in arbitrary interval of time, the robot can be kept in stable condition. In this chapter we are going to present the result of experiments of KHR-1 robot locomotion. Notice the manipulation of KHR-1 robot with the joint trajectories was yielded from the main algorithms which were mentioned in chapter three. The presentation will be divided to two parts: (1) Envelope one, two methods were implemented in the biped gait experimentation, the first method is only consider the effect of minimum combination of angular velocity through velocity ellipsoid, the second method is consider both the effects of minimum combination of angular velocity and minimum combination of joints torque through velocity ellipsoid and force ellipsoid. A combined estimate function, e1 (ti ) and e2 (ti ) were adopted to estimate the difference of these two methods. (2) Envelope two and three, compare the sample motion which is developed by supplier, 53.