Intelligent Robotics and Applications: 14th International Conference, ICIRA 2021, Yantai, China, October 22–25, 2021, Proceedings, Part IV (Lecture Notes in Computer Science) 3030890910, 9783030890919

Citation preview

LNAI 13016

Xin-Jun Liu · Zhenguo Nie · Jingjun Yu · Fugui Xie · Rui Song (Eds.)

Intelligent Robotics and Applications 14th International Conference, ICIRA 2021 Yantai, China, October 22–25, 2021 Proceedings, Part IV

123

Lecture Notes in Artificial Intelligence Subseries of Lecture Notes in Computer Science Series Editors Randy Goebel University of Alberta, Edmonton, Canada Yuzuru Tanaka Hokkaido University, Sapporo, Japan Wolfgang Wahlster DFKI and Saarland University, Saarbrücken, Germany

Founding Editor Jörg Siekmann DFKI and Saarland University, Saarbrücken, Germany

13016

More information about this subseries at http://www.springer.com/series/1244

Xin-Jun Liu · Zhenguo Nie · Jingjun Yu · Fugui Xie · Rui Song (Eds.)

Intelligent Robotics and Applications 14th International Conference, ICIRA 2021 Yantai, China, October 22–25, 2021 Proceedings, Part IV

Editors Xin-Jun Liu Tsinghua University Beijing, China

Zhenguo Nie Tsinghua University Beijing, China

Jingjun Yu Beihang University Beijing, China

Fugui Xie Tsinghua University Beijing, China

Rui Song Shandong University Shandong, China

ISSN 0302-9743 ISSN 1611-3349 (electronic) Lecture Notes in Artificial Intelligence ISBN 978-3-030-89091-9 ISBN 978-3-030-89092-6 (eBook) https://doi.org/10.1007/978-3-030-89092-6 LNCS Sublibrary: SL7 – Artificial Intelligence © Springer Nature Switzerland AG 2021 This work is subject to copyright. All rights are reserved by the Publisher, whether the whole or part of the material is concerned, specifically the rights of translation, reprinting, reuse of illustrations, recitation, broadcasting, reproduction on microfilms or in any other physical way, and transmission or information storage and retrieval, electronic adaptation, computer software, or by similar or dissimilar methodology now known or hereafter developed. The use of general descriptive names, registered names, trademarks, service marks, etc. in this publication does not imply, even in the absence of a specific statement, that such names are exempt from the relevant protective laws and regulations and therefore free for general use. The publisher, the authors and the editors are safe to assume that the advice and information in this book are believed to be true and accurate at the date of publication. Neither the publisher nor the authors or the editors give a warranty, expressed or implied, with respect to the material contained herein or for any errors or omissions that may have been made. The publisher remains neutral with regard to jurisdictional claims in published maps and institutional affiliations. This Springer imprint is published by the registered company Springer Nature Switzerland AG The registered company address is: Gewerbestrasse 11, 6330 Cham, Switzerland

Preface

With the theme “Make Robots Infinitely Possible”, the 14th International Conference on Intelligent Robotics and Applications (ICIRA 2021) was held in Yantai, China, during October 22–25, 2021, and designed to encourage advancement in the field of robotics, automation, mechatronics, and applications. The ICIRA series aims to promote top-level research and globalize quality research in general, making discussions and presentations more internationally competitive and focusing on the latest outstanding achievements, future trends, and demands. ICIRA 2021 was organized by Tsinghua University, co-organized by Beihang University, Shandong University, YEDA, Yantai University, and IFToMM ChinaBeijing, undertaken by the Tsingke+ Research Institute, and technically co-sponsored by Springer. On this occasion, three distinguished plenary speakers and 10 keynote speakers delivered their outstanding research works in various fields of robotics. Participants gave a total of 186 oral presentations and 115 poster presentations, enjoying this excellent opportunity to share their latest research findings. The ICIRA 2021 proceedings cover over 17 research topics, with a total of 299 papers selected for publication in four volumes of Springer’s Lecture Note in Artificial Intelligence. Here we would like to express our sincere appreciation to all the authors, participants, and distinguished plenary and keynote speakers. Special thanks are also extended to all members of the Organizing Committee, all reviewers for peer review, all staff of the conference affairs group, and all volunteers for their diligent work. October 2021

Xin-Jun Liu Zhenguo Nie Jingjun Yu Fugui Xie Rui Song

Organization

Honorary Chair Youlun Xiong

Huazhong University of Science and Technology, China

General Chair Xin-Jun Liu

Tsinghua University, China

General Co-chairs Rui Song Zengguang Hou Qinchuan Li Qinning Wang Huichan Zhao Jangmyung Lee

Shandong University, China Institute of Automation, CAS, China Zhejiang Sci-Tech University, China Peking University, China Tsinghua University, China Pusan National University, South Korea

Program Chair Jingjun Yu

Beihang University, China

Program Co-chairs Xin Ma Fugui Xie Wenguang Yang Bo Tao Xuguang Lan Naoyuki Kubota Ling Zhao

Shandong University, China Tsinghua University, China Yantai University, China Huazhong University of Science and Technology, China Xi’an Jiatong University, China Tokyo Metropolitan University, Japan Yantai YEDA, China

Publication Chair Zhenguo Nie

Tsinghua University, China

Award Chair Limin Zhu

Shanghai Jiao Tong University, China

viii

Organization

Advisory Committee Jorge Angeles Jianda Han Guobiao Wang Tamio Arai Qiang Huang Tianmiao Wang Hegao Cai Oussama Khatib Tianran Wang Tianyou Chai Yinan Lai Yuechao Wang Jie Chen Jangmyung Lee Bogdan M. Wilamowski Jiansheng Dai Zhongqin Lin Ming Xie Zongquan Deng Hong Liu Yangsheng Xu Han Ding Honghai Liu Huayong Yang Xilun Ding Shugen Ma Jie Zhao Baoyan Duan Daokui Qu Nanning Zheng Xisheng Feng Min Tan Xiangyang Zhu Toshio Fukuda Kevin Warwick

McGill University, Canada Shenyang Institute of Automation, CAS, China National Natural Science Foundation of China, China University of Tokyo, Japan Beijing Institute of Technology, China Beihang University, China Harbin Institute of Technology, China Stanford University, USA Shenyang Institute of Automation, CAS, China Northeastern University, China National Natural Science Foundation of China, China Shenyang Institute of Automation, CAS, China Tianjin University, China Pusan National University, South Korea Auburn University, USA King’s College London, UK Shanghai Jiao Tong University, China Nanyang Technical University, Singapore Harbin Institute of Technology, China Harbin Institute of Technology, China The Chinese University of Hong Kong, China Huazhong University of Science and Technology, China Harbin Institute of Technology, China Zhejiang University, China Beihang University, China Ritsumeikan University, Japan Harbin Institute of Technology, China Xidian University, China SIASUN, China Xi’an Jiatong University, China Shenyang Institute of Automation, CAS, China Institute of Automation, CAS, China Shanghai Jiao Tong University, China Nagoya University, Japan Coventry University, UK

Contents – Part IV

Novel Mechanisms, Robots and Applications Optimal Design of Cam Curve Dedicated to Improving Load Uniformity of Bidirectional Antagonistic VSA . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Fanghua Mei, Shusheng Bi, Chang Liu, and Qing Chang

3

Research on Foot Slippage Estimation of Mammal Type Legged Robot . . . . . . . Yufei Liu, Boyang Xing, Zhirui Wang, Lei Jiang, Ruina Dang, Peng Xu, and Tong Yan

14

Design of Deformable Flapping Structure of Bat-Like Flapping Air Vehicle . . . . Bosong Duan, Chuangqiang Guo, Kening Gong, and Hong Liu

24

An Autonomous Flight Control Strategy Based on Human-Skill Imitation for Flapping-Wing Aerial Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Yihong Li, Juntao Liu, Hui Xu, and Wenfu Xu

34

A New Moving Target Interception Algorithm for an AUV in the Ocean Current Environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Xiangqiao Meng, Wei Zhang, Bing Sun, and Daqi Zhu

45

Design and Control of In-Pipe Inspection Robot for Pre-commissioning . . . . . . . Hui Li, Ruiqin Li, and Yuan Wang

56

Research on Technology of Pipeline Detection Robot Based on Spiral Propulsion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ZhiQian Wang, ZhenBo Deng, and Pei Lei

67

Dynamic Characteristics of Two-Mass Inertial Pipeline Robot Driven by Noncircular Gears . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Dawei Liu and Jiarui Lu

79

Kinematic Performance Analysis and Optimization of a 3-DOF Parallel Mechanism for Ankle Rehabilitation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Shuai Yang, Chenglei Liu, Xiaohui Wang, and Jianjun Zhang

91

Configuration Synthesis of Multi-mode Ankle Rehabilitation Robot . . . . . . . . . . 103 Weihan Jia, Chenglei Liu, Jun Wei, and Jianjun Zhang

x

Contents – Part IV

Hydrodynamic Coefficient Estimation of Small Autonomous Underwater Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115 Gan Zhang, Yinfu Lin, Ji Huang, Lin Hong, and Xin Wang Stiffness Modeling of a Novel 2-DOF Solar Tracker with Consideration of Universal Joint Stiffness . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 Yuyao Song and Jun Wu A Kirigami-Inspired Transformable Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137 Cheng Wang, Junlan Li, Usman Haladu Garba, Ping Wang, Zhaocen Guan, Jiahao Zhang, and Dawei Zhang Design and Simulation Analysis of a Magnetic Adsorption Mechanism for a Wall-Climbing Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147 Jiankun Chen, Kai He, Haitao Fang, Junnian Liu, and Hailin Ma Design of a Variable Compliance Mechanism with Changeable Compliance Orientation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158 Junjie Du, Xianmin Zhang, Benliang Zhu, Hai Li, and Weijian Zhong Design and Analysis of a Rolling Joint Based on Gear Tooth for Continuum Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169 Shengge Cao, Jingjun Yu, Guoxin Li, Jie Pan, and Xu Pei Design of Continuum Robot Based on Compliant Mechanism . . . . . . . . . . . . . . . . 180 Guoxin Li, Jingjun Yu, Yichao Tang, Jie Pan, Shengge Cao, and Xu Pei Biologically Inspired Planning and Optimization of Foot Trajectory of a Quadruped Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192 Senwei Huang and Xiuli Zhang Intensity Distribution Estimation of Radiation Source for Nuclear Emergency Robot in 3D Environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 204 Zhiyu Zhang, Zhengya Guo, and Zhenhua Xiong Design and Development of Multi-arm Cooperative Rescue Robot Actuator Based on Variant Scissor Mechanism . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215 Yu Shan, Yanzhi Zhao, Kaida Guo, Dongyang Xu, Wannan Zhao, and Haibo Yu WPFBot: A Novel and Highly Mobile Amphibious Robot . . . . . . . . . . . . . . . . . . . 226 Fusheng Liu, Lei Jiang, Xuelong Li, Xinxin Liu, Ruina Dang, and Wei Wang

Contents – Part IV

xi

A Class of Double-Delta-Based 6-DOF Pick-and-Place Robots with Integrated Grippers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 236 Xiaodong Jin, Yuefa Fang, and Fuqun Zhao Motion Planning of Docking Process for Underwater Vehicle Based on Manipulator Aided Grasping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 246 Yang Zhang, Zongyu Chang, Kunfan Shen, Jinyi Li, and Zhongqiang Zheng A Novel Control Approach of the Flexible Servo Riveting Gun Based on Self-pierce Riveting Technology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 257 Yan Liu, Qiu Tang, Xincheng Tian, and Lixin Ma Path Synthesis Optimization Model of Planar Five-Bar Metamorphic Mechanism . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 268 Xinyuan Yao, Xingdong Wang, Wei Sun, and Haoke Bai Impedance Control for Underwater Gripper Compliant Grasping in Unstructured Environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 279 Liang Lu, Qi Liu, Chengyuan Liang, Xiaofan Tang, Denan Xu, Xin Yang, and Bin Han Design and Kinematic Analysis of a Novel 7-DoF Dual-Arm Parallel Manipulator for 3C Products Assembly . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 290 Xiansheng Yang, Zhenyu Zou, Jinmin Zhang, and Yunjiang Lou Multi-objective Lightweight Design for the Forearm of a Palletizing Robot . . . . 302 Ying He, Han Gao, Chao Ma, Yihui Xiao, and Dan Wang Kinematic Analysis of a Novel 4-DOF 3T1R Parallel Manipulator . . . . . . . . . . . . 316 Jie Zhao, Guilin Yang, Dexin Jiang, Tianjiang Zheng, Yingzhong Tian, Silu Chen, and Chi Zhang A Calibration Method of Robots by Eliminating Redundant Parameters Based on Jacobian Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 327 Zhenya He, Huaiyan Tang, Guojian Huang, Xianmin Zhang, Zhong Chen, and Mingjing Song An Online Intelligent Kinematic Calibration Method for Quadruped Robots Based on Machine Vision and Deep Learning . . . . . . . . . . . . . . . . . . . . . . . 338 Handing Xu, Zhenguo Nie, Xinyu Cui, ShiKeat Lee, Qizhi Meng, Fugui Xie, and Xin-Jun Liu

xii

Contents – Part IV

Space Robot and On-orbit Service Non-singular Fast Terminal Sliding Mode Fuzzy Adaptive Control of Floating-Based Three-Link Space-Robot with Dead-Zone . . . . . . . . . . . . . . . . . 347 Zhihao Zhang, Xiaoyan Yu, and Jiabao Gong Regulation of Space Manipulators with Free-Swinging Joint Failure Based on Iterative Learning Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 360 Yingzhuo Fu, Qingxuan Jia, Gang Chen, and Yifan Wang A Trajectory Planning Method for Load-Carrying Capacity Improvement of Redundant Space Manipulator with Large External Force . . . . . . . . . . . . . . . . . 371 Jingdong Zhao, Xiaohang Yang, Zhiyuan Zhao, and Hong Liu Inverse Kinematics of a Novel SSRMS-Type Reconfigurable Manipulator with Lockable Passive Telescopic Links . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 383 Jingdong Zhao, Zhiyuan Zhao, Guocai Yang, Xiaohang Yang, and Hong Liu A Trajectory Optimization Method for Stabilizing a Tumbling Target Using Dual-Arm Space Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 396 Lei Yan, Bingshan Hu, and Sethu Vijayakumar Error Analysis of Space Manipulator Joint Module Based on the SDT Theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 405 Wei Tang, Xingdong Wang, Jiabo Zhang, and Shengli Liu Toolkit for Dynamic Control Rapid Prototype Simulation System of Robots Applied in Space Experimental Cabin . . . . . . . . . . . . . . . . . . . . . . . . . . . 417 Ning Li, Xiaolong Ma, Chongfeng Zhang, Huaiwu Zou, and Feng Li LSTM Based Model Predictive Control for Flying Space Robot to Track Uncooperative Target . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 428 Qiang Liu, Minghe Jin, Bin Wang, and Hong Liu Design and Engineering Realization of the Joint for Chang’E-5 Surface Sampling Arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 440 Kang Wang, Wencheng Ni, Zhijun Zhao, Xin Liu, Yaobing Wang, and Shuiqing Jiang Thermal-Mechanical Coupling Analysis and Structural Optimization of a Deployable Grasping Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 451 Weida Cheng, Hailin Huang, Bing Li, Guanglu Jia, Fujun Peng, and Aiguo Wu

Contents – Part IV

xiii

Wheel and Track Hybrid Planetary Surface Locomotion Mechanism . . . . . . . . . . 463 Zhijun Zhao, Yun Zhang, Kang Wang, Yaobing Wang, and Wencheng Ni Optimization Algorithm for Cooperative Assembly Sequence of Truss Structure Based on Reinforcement Learning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 474 Jie Yin, Meng Chen, and Tao Zhang A Coordination Planning Method of the Hybrid Two-Arm Space Robot for On-Orbit Servicing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 485 Zhonghua Hu, Wenfu Xu, Taiwei Yang, Han Yuan, and Huaiwu Zou Design and Kinetostatic Analysis of a Legged Robot for Asteroid Exploration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 498 Qingpeng Wen, Jun He, Zhicong Wang, Jiaze Sun, and Feng Gao Effect Analysis of Initial Conditions on Landing Performance of Vertical-Landing Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 510 Yingchao Wang, Haitao Yu, Haibo Gao, Zhen Liu, and Zongquan Deng Neural Learning Enhanced Motion Planning and Control for Human Robot Interaction Comparison of GBNN Path Planning with Different Map Partitioning Approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 523 Mingzhi Chen, Daqi Zhu, and Zhenzhong Chu The Review of Image Processing Based on Graph Neural Network . . . . . . . . . . . 534 Jinji Nie, You Xu, Yichuan Huang, and Jiehao Li Artificial Intelligence in Education: Origin, Development and Rise . . . . . . . . . . . 545 Ruixing Ye, Fuhai Sun, and Jiehao Li A Path Planning Algorithm for Mobile Robots Based on DGABI-RRT . . . . . . . . 554 Qingdang Li, Hui Zhao, Mingyue Zhang, and Zhen Sun Research on Tool Wear Detection Method Using Deep Residual Network . . . . . . 565 Qian Zhou, Kai Guo, Jie Sun, and Vinothkumar Sivalingam Color Recognition Method Based on Image Segmentation . . . . . . . . . . . . . . . . . . . 576 Minghui Hua, Haixiang Zhou, Wanlei Li, and Yunjiang Lou Adaptive Echo State Network Robot Control with Guaranteed Parameter Convergence . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 587 Ruihong Wu, Zhiwen Li, and Yongping Pan

xiv

Contents – Part IV

Design of the Autonomous Path Planning System for Mining Robots Based on Stereo Vision . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 596 Youge Su, Wanghui Bu, Jing Chen, Sihang Li, and Mingzhi Liu A Human-Like Learning Framework of Robot Interaction Skills Based on Environmental Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 606 Hanzhong Liu, Chenguang Yang, and Shi-Lu Dai An AUV Path Planning Algorithm Based on Model Predictive Control and Obstacle Restraint . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 617 Zhaoyang Liu, Daqi Zhu, and Mingzhong Yan Quantum-Behaved Particle Swarm Optimization Fault-Tolerant Control for Human Occupied Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 628 Huapeng Zhang and Daqi Zhu Path Planning Algorithm of Mobile Robot Based on Dichotomy of Thermal Conduction Topology Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . 638 Yuanyang Fang, Pengyuan Qi, Baotong Li, and Xiaohu Li An RBFNN-Informed Adaptive Sliding Mode Control for Wheeled Mobile Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 649 Quan Liu, Zhao Gong, Fugui Xie, Shuzhan Shentu, and Xin-Jun Liu Robust Formation Tracking of Multiple Wheeled Mobile Robots Under External Disturbance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 659 Kanyang Jiang, Xiaoxiao Li, Zhihao Xu, Xuefeng Zhou, and Shuai Li A Novel Path Planning Method Based on Regional Index Algorithm for Hyper-redundant Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 669 Longfei Jia, Yaxing Guo, Yunfei Tao, He Cai, Tianhua Fu, and Yuping Huang Path Planning for Nuclear Emergency Robot in Radiation Environment with Uneven Terrain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 680 Ying Huang, Yan Zhou, and Zhenhua Xiong A Multi-sensing Input and Multi-constraint Reward Mechanism Based Deep Reinforcement Learning Method for Self-driving Policy Learning . . . . . . . 691 Zhongli Wang, Hao Wang, Xin Cui, and Chaochao Zheng Intelligent Safety Decision-Making for Autonomous Vehicle in Highway Environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 702 Zhenyu Jiang, Zhongli Wang, Xin Cui, and Chaochao Zheng

Contents – Part IV

xv

A “Look-Backward-and-Forward” Adaptation Strategy of NN Model Parameters for Prediction of Motion Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . 714 Yisha Liu, Silu Chen, Yufan Zhu, Yu Du, Chi Zhang, Guilin Yang, and Chenguang Yang Velocity Constraints Based Online Trajectory Planning for High-Speed Parallel Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 725 Di Yang, Fugui Xie, and Xin-Jun Liu Medical Engineering Ultra-sensitive and Stretchable Optical Fiber Respiratory Belt Sensor . . . . . . . . . 737 Tianliang Li, Yifei Su, Hui Cheng, Fayin Chen, Yuegang Tan, and Zude Zhou Filter Pruning Using Expectation Value of Feature Map’s Summation . . . . . . . . . 748 Hai Wu, Chuanbin Liu, Fanchao Lin, and Yizhi Liu Muscle Tension Analysis Based on sEMG Signal with Wearable Pulse Diagnosis Device . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 756 Xin Chang, Xinyi Li, Jian Li, Guihua Tian, Hongcai Shang, Jingbo Hu, Jiahao Shi, and Yue Lin Author Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 767

Novel Mechanisms, Robots and Applications

Optimal Design of Cam Curve Dedicated to Improving Load Uniformity of Bidirectional Antagonistic VSA Fanghua Mei1 , Shusheng Bi1(B) , Chang Liu1 , and Qing Chang2,3 1 School of Mechanical Engineering and Automation, Bei Hang University,

Beijing 100191, China [email protected] 2 School of Mechanical Engineering, Tianjin University of Commerce, Tianjin 300134, China 3 Tianjin Chuangju Technology Co., Ltd., Tianjin 300392, China

Abstract. Adjustable stiffness actuator (VSA) sets adjustable series flexible unit between traditional rigid motor and load to enhance the actuator performance. With a bidirectional connection of each motor to the output link, the bidirectional antagonistic VSA enables the motors to support each other to generate a higher torque at the link joint. However, the load distribution is not ideal. In this paper, a novel bidirectional antagonistic VSA is proposed. The torque of the antagonistic motors is coupled and decomposed by the symmetrical cam structure to provide the actuator with stiffness adjustment reaction and output torque. This paper focus on studies of the stiffness nonlinear compensation method to improve accuracy and cam optimization method for a small load coefficient. The optimization results are verified by MATLAB. The output load is evenly transmitted to the antagonistic motor in the high stiffness and large load working space. Keywords: Bidirectional antagonistic VSA · Load coefficient · Cam curve optimization · Coupled and decomposed · Stiffness nonlinear compensation

1 Introduction Compared with the traditional rigid actuator, VSA introduces an adjustable elastic structure between the rigid motor and the load, which endows it with excellent performance [1]. Although it has great potential and advantages in non-traditional industrial fields, VSA’s commercial applications are limited by the design contradictions between its size, weight, cost and output power, potential energy storage, stiffness range, efficiency, power and accuracy. At present, there have numerous researches to address the above problems based on their respective demands, but no general solution. According to the principle of stiffness adjustment [2], it can be divided into variable impedance[3, 4],variable structure [5–8], variable transmission ratio [9–13], single spring preload [14, 15] and antagonistic VSA [16, 17]. © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 3–13, 2021. https://doi.org/10.1007/978-3-030-89092-6_1

4

F. Mei et al.

The common problem of the above VSA is that the introduction of the stiffness adjusting mechanism increases the cost, weight, and size of the actuator, but fails to improve the continuous output power and torque of the actuator. The bidirectional VSA has great advantages in the load distribution of the antagonistic motor, which provides a good design idea for improving the output capacity of the actuator. The output capacity of the actuator can be effectively improved by combining the antagonistic motor setup with other types of stiffness adjustment. However, the above structure has the following shortcomings: 1) stiffness adjustment precision, stiffness adjustment time and the load coefficient of antagonistic motor are not specially designed; 2) There is a certain nonlinear coupling relationship between stiffness and torque, which is difficult to quantitatively compensate; 3) The structure is relatively complex, which is not conducive to miniaturization and lightweight design. To address the above problems, a new type of actuator is proposed by combining bidirectional antagonistic the motor structure with the transmission ratio VSA. The actuator not only inherits the advantages of transmission ratio VSA, but also works in help mode as bidirectional antagonistic VSA. The key point of the actuator is the optimization design of cam curve, which makes the output load evenly distributed to the motor. In Section two, the overall mechanism of the transmission ratio VSA based on bidirectional antagonistic motor structure is introduced. In Section three, the paper introduces the principle of transmission ratio VSA based on variable link length, and puts forward the nonlinear compensation method of stiffness. In Section four, the constraints of the motor load coefficient, the stiffness adjustment stroke, local stiffness adjustment speed variation and bearing size on the cam profile is discussed and the optimization design method of cam contour is put forward. The optimization results show that the load coefficient of the motor is uniform in the whole stiffness range and the continuous output power of the actuator can be approximately the sum of two motors. The conclusion is made in Section five.

2 Overall Structural Design The actuator consists of four parts: cam coupling mechanism driven by antagonistic motor (dotted line layer), stiffness adjusting mechanism based on variable guide bar mechanism (solid line layer), internal spring and output link OD (Fig. 1). By controlling the polar angleκ, the transmission ratio between actuator deformation θ and spring deformation angle α can be changed. Another important function of cam mechanism is to couple the torque TM 1 and TM 2 from antagonistic motors, and decompose the coupling force to balance the e reaction force of stiffness adjustment and external load τload . According to the structural dimensions of the actuator, the key structural parameters are shown in Table 1 (excluding cam curve parameters):

Optimal Design of Cam Curve Dedicated to Improving Load

5

Fig. 1. Overall structural of transmission ratio VSA based on bidirectional antagonistic motor

Table 1. Key structural parameters of VSA Description

Index

Value

Spring stiffness

k

205 Nm/rad

Elastic proportional deformation capacity of spring

α

0.22 rad

Maximum output load

τload

20 Nm

Out link length

od

0.04 m

Maximal input link length

amax

0.065 m

Minimal input link length

amin

0.012 m

Bearing static load capacity

Fbear

0.95 KN

Bearing diameter

Dbear

19 mm

Maximum polar angle stroke of stiffness adjustment

κmax K˜ vrm

3π/2

Stiffness adjustment speed stability

5

3 Stiffness Adjustment Principle and Nonlinear Compensation 3.1 Stiffness Adjustment Principle and Nonlinear Analysis The force diagram of the stiffness adjusting mechanism is shown in Fig. 2. The pressure angle of rod DP to roller P is β, the actuator output stiffness K of the actuator is the derivative of the output torque τload to the deformation angle θ .

Fig. 2. Force diagram of stiffness adjusting mechanism

6

F. Mei et al.

According to the principle of virtual work, the actuator stiffness K is given by the formula (1). ⎧ 2 ⎪ ⎨ K = k 1 − tan (θ − α)α cot α + α tan(θ − α) (1) [tan(θ − α) cot α + 1)]2 ⎪ ⎩ OD sin α = OP sin(θ − α) Obviously, there is a certain nonlinear relationship between output stiffness K and actuator deformation θ . In order to know the nonlinearity of the actuator more clearly, the paper uses the index lnon to express the nonlinear relationship between output stiffness K and joint deformation. In the formula (2), Kθmax represents the output stiffness with the maximal deformation θmax , Kθ0 represents the actuator stiffness with zero deformation θ0 . lnon = Kθ max /Kθ0

Fig. 3. Stiffness characteristics of actuator

(2)

Fig. 4. Nonlinear analysis of stiffness

The ideal range of the non-linear index lnon is 0.95–1.05, which may be more accord with the requirements of industrial applications. According to Fig. 4, it can be seen clearly that the output stiffness is nonlinear, especially in the low stiffness area, and its adverse effects are the decrease of torque control accuracy, torque output ability and deformation ability in the low stiffness. The reason is the nonlinear trigonometric function between actuator deformation and flexible spring deformation. 3.2 Stiffness Nonlinear Compensation Here we define the input link length OP as a. The physical meaning of stiffness nonlinear compensation is: when the joint deformation is θ, the output stiffness K remains unchanged by adjusting input link a0 to aθ for compensation. Then d θ, da should satisfy the following relationship: ⎧ ⎨ ∂K(a, θ ) da + ∂K(a, θ ) d θ = 0 ∂a ∂θ (3) ⎩ K(a0 , θ0 ) = K(aθ , θ )(boundary)

Optimal Design of Cam Curve Dedicated to Improving Load

7

The nonlinear stiffness compensation rate of input link length is defined as formula (4), which represents the input link length adjustment ratio. Cr = (aθ −a0 )/a0

(4)

MATLAB tool is used to obtain the numerical solution of the formula (3), and the calculation result is shown in Fig. 5. It can be seen from the calculation results that the stiffness nonlinear compensation mainly occurs in the low stiffness region, and the compensation rate is relatively high, which is consistent with the analysis results in the Fig. 4. The deformation ability of the actuator before and after stiffness nonlinear compensation is compared, as shown in the Fig. 6. In the Fig. 6, OPinitial is the initial length of the input link before compensation, θs represents the maximum deformation angle of the actuator under different stiffness, θlnon =0.95 represents the maximum deformation angle of the actuator when the non-linear index lnon equals 0.95, θcr represents the deformation capacity of the actuator after nonlinear compensation. According to Fig. 6, in the low stiffness region (OP < 0.03 m), θlnon =0.95 is greatly reduced due to the limitation of nonlinear stiffness index lnon . After the nonlinear compensation of the stiffness, θcr is close to the upper limit of θs .The deformation ability of the elastic element is brought into full play, and the effective deformation ability of the actuator in the low stiffness arrange is greatly improved.

Fig. 5. Input link length compensation Cr

Fig. 6. Result of nonlinear compensation

4 Optimization Design of Cam Drive Profile 4.1 Constraint Analysis of Cam Curve Design Parameters Constraint of Motor Load Coefficient ψ on Cam Press Angle γ (a ). The roller can carry out compound motion under the differential drive of any two intersecting curves. Firstly, the paper derives the analytic relation between cam press angle γ (a) and motor load coefficient by using the simplest linear cam, which provides basis for cam curve design. In the Fig. 7, the linear cam curve C1’s polar angle is κ and applies force FM 1 on roller P under the driving torque TM 1 . Compared with cam C1, cam C2 has a completely symmetrical mechanical structure. The force exerted on the roller by the connecting rod DP can be decomposed into the reaction force of stiffness adjustment Fa and the output force Fd , and the pressure angle of rod DP to roller P is β.The pressure

8

F. Mei et al.

Fig. 7. Linear cam drive structure

angle of cam to roller P is γ ,which is determined by cam curve. TM 1 , TM 2 are the driving torque of motor M1 and M2 respectively. According to the Fig. 7, the driving torque of motor M1 and M2 is given by the formula (5). ⎧ τout (1 + tan β tan γ ) ⎪ ⎨ TM 1 = FM 1 sin γ OP = 2 (5) τ − tan β tan γ ) (1 ⎪ out ⎩ T = F sin γ OP = M2 M2 2 Define motor load coefficient ψ as: ψ = tan β tan γ

(6)

The pressure angle β is determined by the stiffness adjusting mechanism, and satisfy the following geometric relationship: β = arcsin(OD sin α/OP)

(7)

The relationship between ψ and motor working mode is as follows: Table 2. The relationship between ψ and motor working mode ψ value

T M 1 , TM 2

Mode

ψ =0

Completely helping

0 τout /2, τout /2 > TM 2 > 0

ψ =1

TM 1 = τout , TM 2 = 0

Independent

ψ >1

TM 1 > τout , TM 2 < 0

Consuming

Helping antagonistic

The main design goal of cam curve structure is to achieve reasonable pressure angle γ (a) function to make ψ value as small as possible, and ensure the motors in the helping antagonistic mode. Constraint of Stiffness Adjustment on Cam Press Angle γ (a ). The nonlinear compensation of the stiffness is realized by adjusting the input link length a. Therefore, the

Optimal Design of Cam Curve Dedicated to Improving Load

9

sensitivity of the actuator stiffness K to the cam curve polar angle κ is an important parameter. According to above discussion, Kκθ is defined as follows: Kκθ =

dK da da d κ

(8)

If the Kκθ value is too large, we need a high accuracy the motor; If the Kκθ value is too small, the response speed of stiffness adjustment cannot meet the requirements. So, we need a relatively stable Kκθ value. In fact, both of the above situations exist in the whole stiffness range in many actuators. According to the precise stiffness expression, a is not explicitly expressed in the stiffness formula (1). In order to simplify the analysis process and make the relationship between γ (a) and Kκθ more intuitive, the stiffness formula (1) is simplified as follows: 2  1 − tan2 (θ − α)α cot α + α tan(θ − α) a = k K˜ = lim k θ→0 od + a [tan(θ − α) cot α + 1)]2

(9)

Although formula (9) is not very accurate, it is enough for us to analyze the influence trend of cam curve pressure angle γ (a) on Kκθ . In the above formula, od is the length of the output link. Correspondingly, Kκθ can be simplified to K˜ κθ as follows: od ∗ a d K˜ da = 2k tan(γ (a)) K˜ κθ = da d κ (od + a)3

(10)

In order to evaluate the variation of stiffness adjustment with the difference angle, parameter K˜ vrm is defined as formula (11). The pressure angle γ (a) should be inversely proportional to a for a small K˜ vrm . max(K˜ κθ ) K˜ vrm = min(K˜ κθ )

(11)

When the maximum speed of the motor is determined, the maximum pole angle κmax of cam curve can be used to express the stiffness adjustment time or the stiffness adjustment stroke.  amax κmax = (12) (a tan(γ (a)))−1 da amin

Constraint of Bearing Structural on Cam Press Angle γ (a ). In order to reduce the friction between cam groove and roller, roller is usually replaced by bearing. The curve pressure angle γ (a) should not be too small, otherwise the bearing will be overloaded. Fbear is rated static load of the bearing, the constraint conditions of bearing strength is as follows: ⎧ ⎪ ⎨ FM 1 = τout (1 + tan β tan γmin )/2 sin γmin a ≤ Fbear β = arcsin(od sin α/a) (13) ⎪ ⎩ α ≤ 0.22rad 0.012 ≤ a ≤ 0.065

10

F. Mei et al.

In addition, in order to avoid the distortion of cam contour, the minimum curvature radius should not be less than the bearing radius.

3/2 a 1 + tan2 (γ (a))  ≥ Dbear /2 (14) ρr =  2 tan (γ (a))−a sec2 (γ (a))γ  (a) tan(γ (a)) + 1

4.2 Cam Curve Design Optimization and Result Analysis It can be seen from Sect. 4.1 that the design goal of cam profile is to obtain the minimum pressure angle γ (a) to reduce ψ value and make it enter help mode. The above constraints cannot be solved analytically. Firstly, the formula (13) is solved numerically, and the result in the Fig. 8 shows that the curve has a sharp inflection point, which is not conducive to data fitting. The reason is that the constraint factors limiting the pressure angle near the inflection point have changed. Therefore, the low stiffness interval data can be deleted for better fitting data (Fig. 8). The corresponding fitting result γ˜min is given by formula (15). γ˜min (a) = 2083a4 − 530310a3 + 531.5a2 − 27.36 ∗ a + 0.854

(15)

Considering that the torque output capacity of the actuator is limited in the low stiffness region, a single motor can provide enough driving torque in the, which can allow appropriately higher ψ in the low stiffness. Here correction function γˆc (a) is introduced for compromise design as formula (16) shows. By adjusting the coefficient bc and nc , the pressure angle γ (a) in the low stiffness region can be appropriately increased, and the influence on the design value of the pressure angle in the high stiffness region can be minimized. The coefficient ac can adjust the pressure angle of the whole stiffness range. γˆc (a) = ac + bc (amin /a)nc

(16)

In summary, the correction function should satisfy the following constraints: ⎧ ⎪ minψ ⎪ ⎪ ⎪ ⎪ (Bearing load constraint) ⎨ γˆmin (a) = γˆc (a)+γ˜min (a) ≥ γmin (a) (Stiffness adjustment stroke constraint) κmax ≤ 4.71rad ⎪ ⎪ ⎪ (Variation of stiffness adjustment speed) K˜ vrm < 5 ⎪ ⎪ ⎩ (Contour distortion constraint) ρr min ≥ D/2 (17) The parameters are optimized iteratively according to the formula (17), the results of parameter design meet the requirements of bearing load (Fig. 9), contour fidelity (Fig. 10) and stiffness adjustment index (Fig. 11). Compared with Fig. 8 and Fig. 9, the optimization result of pressure angle only slightly deviates from the bearing load pressure angle constraint curve. Therefore, in order to obtain smaller pressure angle, the bearing with larger bearing capacity should be selected. The final curve optimization result is given by formula (18), as Fig. 12 shows. γˆmin (a) = 20830a4 − 5303a3 + 531.5a2 − 27.36a + 0.864−0.25(amin /a)5

(18)

Optimal Design of Cam Curve Dedicated to Improving Load

Fig. 8. γmin (a) under the bearing constraint

Fig. 9. Correcting results of γmin (a)

Fig. 10. Minimum curvature radius ρr check

Fig. 11. K˜ κθ check (K˜ vrm < 5)

Fig. 12. Optimal design results of cam curve

11

Fig. 13. Motor load coefficient ψ

In general, the ψ value meets the design goal (Fig. 13). In the working range of high stiffness and large external load, the ψ value is close to 0.1, and the motor can bear the external load evenly; in the low stiffness range, the cam structure avoids bearing overload at the expense of load coefficient, but the motor is still in helping mode. The stiffness adjustment stroke κmax is 4.18 rad, and the stiffness adjustment index K˜ κθ is stable. Further reduction of the load coefficient ψ of the motor will increase the stiffness adjustment stroke and increase the stiffness adjustment time.

12

F. Mei et al.

5 Conclusion In order to improve the output power of the actuator, a new type of actuator is proposed by combining the motor structure with the transmission ratio VSA. The core of the actuator is the cam curve design. The cams have are two main functions: 1) Torque coupling and decomposition, which ensure the motors to work in helping mode; 2) Stiffness adjustment and position control, which can be realized simultaneously by motors differential control. The key of the design is to optimize the cam press angle γ (a) and get a smaller motor load coefficient ψ. The relationship between cam profile and motor load coefficient ψ, stiffness adjustment speed stability K˜ vrm , stiffness adjustment stroke κmax and bearing structure (size and strength) is established quantitatively. Through the cam curve optimization design, the output load is evenly transmitted to the antagonistic motor in the high stiffness, which improves the utilization rate of the antagonistic motor and increase the continuous output power and torque of the actuator. By adjusting the parameters of the cam curve base on the bearing rated load in the low stiffness, the tradeoff between load coefficient and the structural strength can be well achieved. In fact, the stiffness adjustment stroke κmax is inversely proportional to the load coefficient ψ. This paper focuses on reducing the load coefficient of the motor, so that the motor can bear the external load more evenly. Therefore, the stiffness adjustment stroke or speed stability is not the final design goal, but the constraint factor of the cam parameter design. In fact, the cam parameter design can adjust the design objective and constraint function according to the needs. Another innovation of this paper is the stiffness nonlinear compensation. The precondition of nonlinear compensation is that the stiffness adjusting mechanism and cam coupling structure are designed independently. So, the nonlinear relationship between output stiffness and load can be quantified and compensated. The accuracy and deformation ability of actuator can be improved by transmission ratio compensation. A potential advantage of the actuator is its energy efficiency. For the antagonistic motor structure, the stiffness adjustment action does not need to take a single action to complete. Therefore, the optimal energy consumption control strategy of VSA based on the antagonistic motor structure is very worthy of study in the future. Acknowledgements. This work was supported by the National Natural Science Foundation of China (Grant No. 52075013 and No. 51675015).

References 1. Bi, S., Liu, C., Zhou, X.: Review of research on adjustable stiffness actuator structure. Chin. J. Mech. Eng. 54(13), 34–46 (2018) 2. Vanderborght, B., Albu-Schäffer, A., Bicchi, A., et al.: Variable impedance actuators: a review. Robot. Auton. Syst. 61(12), 1601–1614 (2013) 3. Vallery, H., Veneman, J., Asseldonk, E.V., et al.: Compliant actuation of rehabilitation robots. IEEE Robot. Autom. Mag. 15(3), 60–69 (2008) 4. Song, Z., Lan, S., Dai, J.S.: A new mechanical design method of compliant actuators with non-linear stiffness with predefined deflection-torque profiles . Mech. Mach. Theory 133, 164–178 (2019)

Optimal Design of Cam Curve Dedicated to Improving Load

13

5. Li, X., Chen, W., Lin, W., et al.: A variable stiffness robotic gripper based on structurecontrolled principle. IEEE Trans. Autom. Sci. Eng. 1–10 (2017) 6. Morrison, T., Li, C., Pei, X., et al.: A novel rotating beam link for variable stiffness robotic arms. In: 2019 International Conference on Robotics and Automation (ICRA), pp. 9387–9393. IEEE (2019) 7. Verstraten, T., López-García, P., Lenaerts, B., et al.: Improving the performance of industrial machines with variable stiffness springs. Mech. Based Des. Struct. Mach. 1–20 (2020) 8. Liu, C., Bi, S., Zhao, H.: Novel variable stiffness actuator based on folded series leaf springs. Chin. J. Mech. Eng. 17, 84–91 (2017) 9. Jafari, A., Saganakis, N.G., Caldwell, D.G.: A Novel intrinsically energy efficient actuator with adjustable stiffness (AwAS). IEEE/ASME Trans. Mechatron. 18(1), 355–365 (2013) 10. Cong, P.V., Phan, V.D., Nguyen, T.H., et al.: A compact adjustable stiffness rotary actuator based on linear springs: working principle, design, and experimental verification. Actuators 9(4), 141 (2020) 11. Deman, M., Gams, A.: Rotatable cam-based variable-ratio lever compliant actuator for wearable devices. Mech. Mach. Theory 130, 508–522 (2018) 12. Sun, J., Guo, Z., Zhang, Y., et al.: A novel design of serial variable stiffness actuator based on an archimedean spiral relocation mechanism. IEEE/ASME Trans. Mechatron. (2018) 13. Shao, Y., Zhang, W., Ding, X.: Configuration synthesis of variable stiffness mechanisms based on guide-bar mechanisms with length-adjustable links. Mech. Mach. Theory 156, 104153 (2021) 14. Wolf, S., Eiberger, O., Hirzinger, G.: The DLR FSJ: energy based design of a variable stiffness joint. In: 2011 IEEE International Conference on Robotics and Automation. Shanghai, pp. 5082–5089. IEEE (2011) 15. Zhu, Y., Wu, Q., Chen, B., et al.: Design and evaluation of a novel torque-controllable variable stiffness actuator with reconfigurability. IEEE/ASME Trans. Mechatron. (2021) 16. Yigit, C.B., Bayraktar, E., Boyraz, P.: Low-cost variable stiffness joint design using translational variable radius pulleys. Mech. Mach. Theory 130, 203–219 (2018) 17. Bilancia, P., Berselli, G., Palli, G.: Virtual and physical prototyping of a beam-based variable stiffness actuator for safe human-machine interaction. Robot. Comput. Integrated Manuf. 65 (2020)

Research on Foot Slippage Estimation of Mammal Type Legged Robot Yufei Liu1 , Boyang Xing1 , Zhirui Wang1 , Lei Jiang1,2(B) , Ruina Dang1 , Peng Xu1 , and Tong Yan1 1 Unmanned Center, China North Vehicle Research Institute, Beijing 100072, China

[email protected] 2 College of Computer Science and Technology, Zhejiang University, Hangzhou 310027, China

Abstract. The accurate of the velocity and posture data of legged robot such as hexapod and quadruped robots can not be obtained by body sensors only due to the influence of the sensor error itself and the installation error, as well as the foot slippage or mechanical error during the foot-terrain interaction. In this paper, the data fusion method based on Extended Kalman Filter (EKF) is adopted, and the data of accurate velocity and posture of trunk body are from the kinematics and IMU information. The single leg foot-terrain slippage model for mammal type legged robot based on leg dynamics model is proposed. The single leg foot-terrain slippage estimation method is presented. The method can detect the slippage rate of the single leg and the state of the slippage can be determined. The estimated value is compared with data obtained by the motion capture system (Mocap system), and the experimental results have demonstrated the effectiveness of the proposed method and realize the single leg slippage estimation of mammal type hexapod and quadruped robots. Keywords: Foot slippage · Extended Kalman Filter · Slippage estimation · Mammal type legged robot

1 Introduction Legged robots exhibit potentials in rough and uneven terrains. To control autonomous walking of a legged robot, it is essential to obtain the instantaneous velocity and posture of the hexapod and quadruped robots. The precise estimation of foot slippage is indispensable for the functioning of legged robots without any geometric knowledge of the environment. Measurement sensors for legged robots include encoders, cameras, and global positioning system (GPS) devices [1]. Different integrations of the perception devices have been explored on legged robots [2, 3]. Roston et al. [4] presented a navigation system which extracts data from the leg kinematics and derive the slippage detection method which relies on the foot-terrain interaction. There are many approaches to estimate the velocity using the data from joint position sensors and force sensors of legged robot. Lin presented an algorithm for the posture © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 14–23, 2021. https://doi.org/10.1007/978-3-030-89092-6_2

Research on Foot Slippage Estimation of Mammal Type Legged Robot

15

estimation based on leg kinematics for a hexapod robot RHex which was supported by three or six legs at all times [5]. The author [6] also presented a full body state estimator for legged robot. A posture estimator was implemented on a quadruped robot based on a particle filter [7] and the leg kinematics and the IMU data were used to calculate the location of legged robot. Reinstein et al. [8] presented a navigation method using state estimation method. The method combines state estimation methods with machine learning to be immune to feet slippage. The ETH laboratory developed ANYmal quadruped robot with robust controller for blind walking. The robustness of quadruped robots is trained in simulation through reinforcement learning methods. The controller is generated by fusing proprioceptive sensors according to neural network strategy, which can maintain the robustness under mud, snow and gravel terrains [9, 10]. Precise velocity and slippage estimation of legged robots are important for machine learning, and the control strategy can update based on collected data samples and estimates autonomously [11, 12]. The orientation of the legged robot body is estimated using the joint variables and high-order sliding-mode observer. Rotella [13] presented an approach for velocity and body pose estimation by joint encoders and the IMU data calculation based on state estimator. Due to the feet of legged robots were not in contact with the ground at all times. The estimation algorithm can be applied in different terrains and is immune to feet slippage. A robust estimation method of walking legged robots was presented based on the proprioceptive sensors data fusion [14]. Okita presented slippage estimation algorithm based on the Unscented Kalman Filter, and the data are from the IMU and accelerometer data [15]. The impact of walking gait and energy efficiency on the performance of the robot under different friction environments are analyzed [16]; and also proposed two adaptive slippage strategies to achieve the walking of quadruped robot. However, the general problem is to estimate instantaneous velocity and slippage of the hexapod and quadruped robots without any assumption about the different terrains; and the velocity and slippage estimation for legged robots has been rarely studied. The main contribution of this paper is to propose foot-terrain slippage model for mammal type legged robot based on leg dynamics model with foot slippage. The footterrain slippage estimation method based on state estimation is presented. The method can detect the slippage rate of the single leg and the state of the slippage can be determined. This paper is organized as follows: Sect. 2 introduces the foot terrain interaction for slippage. Data fusion method based on EKF are presented in Sect. 3. Section 4 introduces slippage estimation and state determination. Finally, comprehensive experiments are presented in Sect. 5. The conclusions are given in Sect. 6.

2 Foot Terrain Interaction for Slippage Foot terrain interaction for slippage is mainly divided into normal and tangential directions, but the previous researches mainly focused on static foot terrain loading experiments. The contact model is analyzed for the foot terrain dynamic contact, and the impact model of the foot terrain mechanics is obtained. The process of foot terrain interaction for forward and lateral directions is shown in Fig. 1.

16

Y. Liu et al. Normal z

FN

a

f

n O

σ Fx Forward x

τ

v Mz

z

C

βF

Fy FT

Lateral y

Fig. 1. The foot-terrain interaction for forward and lateral directions

When the foot contacts with terrain, the fixed ground gives the leg a restraining force to prevent the foot from sinking into the ground. The normal force along the vertical contact direction can be calculated as:  (1) FN = Fp ds − GRA L

where GRA carries the gravity of the foot active area. The velocity vector v of tangential movement has a velocity component in the normal direction of the tangential contact line. Assuming that the state of movement is a low velocity condition, and it has no effect on the slippage area. The passive pressure generated by each vertical edge element is F N . The tangential force F T along the lateral contact direction can be obtained as:       3K FN tan ϕ · th j, dK FT = (2) K 2 K where κ is the contact surface between the flat foot and soft terrain, j is the tangential displacement of the foot, K is the shear deformation of the soft terrain. According to the normal and tangential force equations, the authors can analyze the influence of normal vertical load and slip velocity load on the experimental results of foot-terrain slippage mechanics, and solve the problem of difficulty in establishing the theoretical model.

3 Data Fusion Method Based on EKF 3.1 Calibration for Sensor Data In order to obtain the velocity information of the robot body, it is necessary to integrate the acceleration signal of sensor, and the velocity error gradually increases with time. Therefore, it is necessary to calibrate the acceleration data to suppress the error of velocity calculation. The acceleration error of the IMU can be calculated by the scaling factor and the deviation value of each axis. The three-axis static model can be expressed as: ⎧ ⎪ ⎨ fx = Gax fˆx + Bax (3) fy = Gay fˆy + Bay ⎪ ⎩ f = G fˆ + B z az z az

Research on Foot Slippage Estimation of Mammal Type Legged Robot

17

where Gax , Gay and Gaz are the scaling factors of each axis, Bax , Bay and Baz are the deviation values of acceleration for each axis. The estimated values of each axis acceleration are:  ˆ x,k−1 fˆx,k−1 = fx − Bˆ x,k−1 · G (4) When the estimated value does not reach the true value, there exists acceleration error. In order to compensate the error in real time, an error model needs to be established, and the error value between the calculated value and the true value can also be calculated. The static model of acceleration can be transformed as: β1 fˆx2 + β2 fˆx + β3 fˆy2 + β4 fˆy + β5 fˆz2 + β6 fˆz + 1 = 0

(5)

Inertial navigation acceleration measurement value after calibration can be calculated as:  −1 β = ST S ST v

(6)

The inertial navigation sensor data in the static state can be collected, and the calibration factor and deviation value of the three-axis acceleration data are obtained by parameters solving. 3.2 State Estimation by Data Fusion In order to obtain the optimal estimation value of the robot state, it is necessary to fuse the kinematics and inertial navigation data. This paper adopts the Extended Kalman Filter (EKF) method to fit different data to obtain the optimal velocity and pose estimated value of robot. The state variables of the system include the position, velocity and pose of the robot, and the equation of state variable is defined as:

 (7) yk = px , py , pz , vx , vy , vz , φ, θ, ψ The state variable differential equation of the prediction model is derived, and the equations of the velocity, acceleration and angular acceleration of the trunk body are obtained as: ⎧ ⎪ ⎨ p˙ = v  ˆ v˙ = T N f − b − w (8) f f B ⎪

 ⎩ N ε˙ = T B wˆ − bw − ww In order to ensure the convergence of the EKF, the error values of the position, velocity, pose and inertial navigation deviation are taken as the state variables of the system. T  yk = δp δv δε δbf δbw

(9)

18

Y. Liu et al.

The measurement variables of the measurement model can be expressed as: ⎤ ⎡ δ ⎤ ⎡ ⎡ ⎤⎢ p ⎥ pˆ − pc I 0 0 0 0 ⎢ δv ⎥ ⎥ ⎢ Zk = ⎣ vˆ − vc ⎦ = ⎣ 0 I 0 0 0 ⎦⎢ δε ⎥ ⎥ ⎢ εˆ − ε c 0 0 I 0 0 ⎣ δbf ⎦ δbw

(10)

where pc is the position correction of robot, vc is the velocity correction of robot, εc is the pose correction of robot. A feedback correction method is adopted to estimate the state of hexapod robot [17]. The errors of the deviation values from EKF will feedback to the prediction model; and the feedback correction method prevents the drift of the prediction calculation. In accordance with the feedback correction principle, the estimated deviation values can be feedback to the calculation process. The correction vectors of the position, velocity and attitude are obtained as: ⎤ ⎤ ⎡ ⎤ ⎡ ⎡ pI MU (k − 1) δ pˆ k−1 pc (k − 1) ⎣ vc (k − 1) ⎦ = ⎣ vIMU (k − 1) ⎦ − ⎣ δˆvk−1 ⎦ (11) εc (k − 1) εIMU (k − 1) δ εˆ k−1 Thereafter, the estimations of the state are formulated as:  ⎤ ⎡ ⎤ ⎡ ⎡ ⎤ W f˜ − b − w t 2 /2 − 1) t + C v (k c f f pc (k − 1) pˆ (k)  B ⎥ ⎢ ⎥ ⎣ vˆ (k) ⎦ = ⎣ vc (k − 1) ⎦ + ⎢ W f˜ − b − w t C ⎦ ⎣ f f B εˆ (k) εc (k − 1) ˜ CW − w w − b ( ) t w w B

(12)

The state estimators provided position, velocity and attitude vectors which were further compared to the values obtained by Mocap system.

4 Slippage Estimation and State Determination In order to prevent and compensate for the invalid slippage of the foot terrain contact process, the slippage estimation method for the mammal type hexapod and quadruped robots based on the dynamic model. The body velocity estimation value and the foot tip velocity estimation value are fused based on the EKF, and the foot slippage of the robot can be estimated. 4.1 Acceleration Model of Foot Slippage for Mammal Type Walking When the hexapod robot walking by the mammal type, the robot can walk in forward direction. Slippage estimation method is proposed based on the single leg dynamics model. Suppose that the robot walks in the forward direction and do not consider the turn, and it relies on the movement of the hip joint and knee joint simplified, as shown in Fig. 2.

Research on Foot Slippage Estimation of Mammal Type Legged Robot

Fig. 2. Slip model of trunk body for mammal type

19

Fig. 3. Slip model of single leg for mammal type

The two control variables of a single leg with three links are defined as shown in Fig. 3. The angle between the hip joint and the normal direction is qH , and the angle between the knee joint and the normal direction is qK . The foot terrain contact point C s is defined as supporting position, the mass of coxa is M A , the length of coxa is L A , and the moment of inertia based on the center of mass of the coxa is J A . The mass of thigh is M H , the length of thigh is L H , and the moment of inertia based on the center of mass of the thigh is J H . The mass of calf is M K , the length of calf is L K , and the moment of inertia based on the center of mass of the calf is J K . The position of the foot tip for the supporting phase represents the slippage displacement. The input slippage variables for dynamic system are defined. According to the single-leg slippage model for mammal-type legged robot walking, the position vector of the two-link can be obtained as:  L P + LK sin qK pk = L sy Psz + LK cos qK (13)  L Psy + LK sin qK + LH sinqH pH = L Psz + LK cos qK + LH cosqH The kinetic energy of the robot’s single-leg movement for mammal-type legged robot walking is: T=

1 1 1 2 2 MK p˙ Tk p˙ k + MH p˙ TH p˙ H + (JK q˙ K + JH q˙ H ) 2 2 2

(14)

The potential energy of the hexapod robot for the mammal type legged robot walking can be expressed as:   (15) V = MK g L Psz + LK cos qK + MH g L Psz + LK cos qK + LH cosqH The general form of the equation for single leg movement according to Lagrange equation: ˙ q˙ + Gd (q) − Cd u − Ed F = 0 Dd (q)q¨ + Bd (q, q)

(16)

20

Y. Liu et al.

where the Dd (q) is Inertia matrix, and the Bd (q) is Coriolis matrix, and the Gd (q) is Gravity matrix and u is joint torque. According to the single leg Lagrange equation, the forward slippage acceleration model is obtained as:    2 2 Psy = (MK LK + LK MH ) q˙ K sin qK − q¨ K cos qK + MH LH q˙ H sin qH − q¨ H cos qH + FT /(MK + MH )

L¨

(17)

4.2 Foot Slippage State Analysis Based on EKF The state variables for forward foot slippage displacement, knee joint and hip joint changing angle for the slip estimation can be obtained as: ysk =



L

Psy , qk , qH

T

Foot acceleration for mammal type legged robot walking is defined as: ⎡ ⎤   afx ys1 = 1 0 0 ⎣ afy ⎦ = fs (ys , ws1 ) afz

(18)

(19)

where ws1 is defined as process noise, and variance is Qs1. The foot velocity of mammal-type legged robot walking along the forward direction is: ys2 = vs (ys , ws2 ) = L P˙ sy + LK q˙ K cos qK + LH q˙ H cos qH − vyc

(20)

where vyc is the forward velocity of the coxa obtained from the state estimation, ws2 is defined as process noise, and variance is Qs2. The foot position of mammal-type legged robot walking along the forward direction is: ys3 = ps (ys , ws3 ) = L Psy + LK (sin qK − sin qK0 ) + LH (sin qH − sin qH 0 ) − vyc t (21) where qA0 is the initial angle of the coxa joint when the foot touches the terrain. The slippage state equation for the mammal type legged robot walking is: ⎡ ⎤ fs (ys , ws1 ) ysk = ⎣ vs (ys , ws2 ) ⎦ + wsy (22) ps (ys , ws3 ) According to the slippage state equation, the slippage rate of the robot for the traction direction is defined as: SI (t) =

ps (ys , ws3 ) LP L yei − Pysi

(23)

Research on Foot Slippage Estimation of Mammal Type Legged Robot

21

where L Pyei is the position calculated by positive kinematics at the end of control cycle, LP ysi is the position calculated by positive kinematics at the beginning of control cycle. When the support leg makes effective movement, the slip rate satisfies as follows: SI (t) ∈ [0, 1)

(24)

According to the calculated foot slippage rate, the foot slippage can be judged by comparing with the slip threshold value.

5 Experimental Verification The experimental systems of hexapod and quadruped robots include proprioceptive and external measurement equipments. The IMU has an advantage of dependence on realtime gravity. The accelerations and angular velocities can be integrated over time to derive the position, velocity of the trunk body. The acceleration in the IMU frame have been periodically measured in the experiments; and forward kinematic is adopted to calculate the velocity to further determine the measurement model in the data fusion method. Mocap system consists of four cameras and six position markers; furthermore, a rigid body model in Mocap frame is incorporated. The cameras are placed in the four corners of the experimental field and they are immobile during the whole experiment. The experimental measurement systems for hexapod and quadruped robots are shown in Fig. 4.

The Motion Capture System

The Motion Capture System The ElSpider Robot

Markers

Passive compliant ankle

Markers

Q-Dog Robot

Fig. 4. The internal and external measurement system for hexapod and quadruped robots

The hexapod robot adopts the tripod gait walking, and slippage estimation data of hexapod trunk body and stance feet for hexapod robot are analyzed on the tile terrain. The slippage estimation of stance feet for hexapod and quadruped body are analyzed on soft terrain as shown in Fig. 5. The estimated velocity of trunk body in the forward direction is shown in Fig. 6. The trajectory of the trunk body is adopted and the velocity of trunk body is found to have a periodical fluctuation. By comparing the slipping displacement calculated by kinematics and the data obtained by state estimation, the occurrence of the hexapod and quadruped body slippage

22

Y. Liu et al. 0.12

0.12

0.08 0.06

Slippage estimation based on EKF Stance feet slippage (m)

Stance feet slippage (m)

Slippage estimation based on EKF 0.1

Foot displacement by Mocap system Normal Walking Foot-terrain slippage

0.04 0.02 0 0

0.1

Foot displacement by Mocap system

0.08

Normal Walking

0.06

Foot-terrain slippage

0.04 0.02

7

21

14 Time (s)

0 0

28

7

14 Time (s)

21

28

Fig. 5. Comparison results of the slipping displacement for the stance feet of the hexapod and quadruped robot 4 IMU data

Forward velocity(×10-1m/s)

3.5

Kinematics computation

3

Mocap data Fusion data

2.5 2

1.5 1

0.5 0

Forward velocity(×10-2m/s)

-0.5

0

4 3.5

3

6

9

12 15 Time(s)

18

21

24

Mocap data Fusion data

3 2.5 2

1.5 1 0.5

0 -0.5 6

9 Time(s)

12

Fig. 6. The estimated velocity for trigonometric trajectory of trunk body in forward direction measured by Mocap data (dash - dotted line), IMU data (dash - two dotted line), kinematics computation data (dashed line), and fusion data (solid line).

relative to the terrain can be determined; The inertial navigation data, kinematics and foot force data can be fused based on EKF framework. According to the calculated foot slip acceleration, the foot-terrain slippage displacement of hexapod and quadruped robots can be estimated and the comparison data by the Mocap system verifies the effectiveness of the slippage estimation algorithm.

6 Conclusion In this study, we investigate a data fusion method based on EKF. The data of accurate velocity and slippage estimation of hexapod and quadruped robots are from the kinematics, IMU and foot force information. The foot-terrain slippage models for mammal type

Research on Foot Slippage Estimation of Mammal Type Legged Robot

23

legged robot based on leg dynamics model with foot slippage are proposed. And the footterrain slippage estimation method based on state estimation is presented. The method can detect the slippage rate and the state of the stance feet slippage can be determined. The verification experiments for the velocity and slippage estimation of mammal type hexapod and quadruped robots are performed and the experimental results have demonstrated the effectiveness of the proposed method. Future work will aim to fuse the state estimator with the visual information to detect the foot slippage more accurate in the walking of the legged robot.

References 1. Lijn, D., Lopes, G.A.R.: Motion estimation based on predator/prey vision. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3435–3440 (2010) 2. Ma, J.S.: Robust multi-sensor, day/night 6-DOF pose estimation for a dynamic legged vehicle in GPS-denied environments. In: IEEE/RSJ International Conference on Robotics and Automation (ICRA), pp. 619–626 (2012) 3. Stelzer, A., Heiko, H., Martin, G.: Stereo-vision-based navigation of a six-legged walking robot in unknown rough terrain. Int. J. Robot. Res. 31(4), 391–402 (2012) 4. Roston, G.P., Krotkov, E.P.: Dead reckoning navigation for walking robots. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 607–612 (1991) 5. Lin, P.C., Komsuoglu, H., Koditschek, D.E.: A leg con-figuration measurement system for full-body pose estimates in a hexapod robot. IEEE Trans. Rob. 21(3), 411–422 (2005) 6. Lin, P.C., Komsuoglu, H.: Sensor data fusion for body state estimation in a hexapod robot with dynamical gaits. IEEE Trans. Rob. 22(5), 932–943 (2006) 7. Chitta, S., Vemaza, P.: Proprioceptive localization for a quadrupedal robot on known terrain. In: IEEE/RSJ International Conference on Robotics and Automation (ICRA), pp. 4582–4587 (2007) 8. Reinstein, M., Hoffmann, M.: Dead reckoning in a dynamic quadruped robot: inertial navigation system aided by a legged odometer. In: IEEE/RSJ International Conference on Robotics and Automation (ICRA), pp. 617–624 (2011) 9. Lee, J., Hwangbo, J.: Learning quadrupedal locomotion over challenging terrain. Sci. Robot. 5(47), eabc5986 (2020) 10. Hwangbo, J., Lee, J.: Learning agile and dynamic motor skills for legged robots. Sci. Robot. 4(26), eaau5872 (2019) 11. Wawrzy´nski, P., Tanwani, A.K.: Autonomous reinforcement learning with experience replay. Neural Netw. 41(5), 156–167 (2013) 12. Wawrzy´nski, P.: Reinforcement learning with experience replay for model-free humanoid walking optimization. Int. J. Humanoid Rob. 11(3), 137–2187 (2014) 13. Rotella, N., Bloesch, M., Righetti, L.: State estimation for a humanoid robot. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 952–958 (2014) 14. Wawrzy´nski, P., Mo˙zaryn, J., Klimaszewski, J.: Robust estimation of walking robots velocity and tilt using proprioceptive sensors data fusion. Robot. Auton. Syst. 66, 44–54 (2015) 15. Okita, N., Sommer H.J.: A novel foot slip detection algorithm using unscented Kalman Filter innovation. In: American Control Conference, pp. 5163–5168 (2012) 16. Takemura, H., Deguchi, M., Ueda, J.: Slip-adaptive walk of quadruped robot. Robot. Auton. Syst. 53(2), 124–141 (2005)

Design of Deformable Flapping Structure of Bat-Like Flapping Air Vehicle Bosong Duan , Chuangqiang Guo(B) , Kening Gong, and Hong Liu Harbin Institute of Technology, Harbin 150001, China [email protected]

Abstract. This paper designs a deformable flapping wing structure based on the bat’s flight motion law. The reciprocating flapping mechanism is coupled with the folding and unfolding mechanism to achieve the goal of unfolding the wings to increase the lift during the downward and folding the wings to reduce the resistance during the upward. The motion function of 3D modeling flapping mechanism is verified by ADAMS. The results show that the structure meets the design requirements and can achieve the expected functions. Keywords: Bat-like flapping air vehicle · Structural coupling design · Kinematics · Three-dimension modeling

1 Introduction Since the 1990s, with the development of aerodynamics theory, new materials and micro electro mechanical technology, micro air vehicle (MAV) has entered the trend of development, and various types of MAV have been researched all over the world [1]. At present, MAV can be divided into three types: fixed wing, rotary wing and flapping wing according to different flight principles [2]. Among them, the fixed wing MAV is limited by the minimum flight speed and wingspan, and has poor maneuverability and flexibility, and it is difficult to achieve miniaturization; rotary wing MAV relies on the rotation of the wing paddle to generate power, and its efficiency is relatively low. Flying creatures in nature, without exception, all use flapping wings, and flapping wing is considered to be the optimal flying method for biological evolution [3]. Bionic flapping micro air vehicle (FMAV)has great development prospects in military investigation, scientific research and civil fields because of its good concealment, low noise and high mobility [4]. Bat flapping is similar to birds, but it can produce higher lift than birds when flying at medium and low speeds; it can hover and fly, and its maneuverability is stronger than that of birds of the same size, which is in line with MAV research requirements [5, 6]. At present, most of the flapping mechanisms of bat-like FMAV use crank rocker mechanism to achieve two-dimensional flapping, such as “microbat” bat simulation vehicle [7] studied by the University of California Institute of technology, and a FMAV of Brown University in the United States [8] But for bionic bat-like FMAV, the key technology of bionics is the folding and unfolding movement of wings during wing © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 24–33, 2021. https://doi.org/10.1007/978-3-030-89092-6_3

Design of Deformable Flapping Structure of Bat-Like Flapping Air Vehicle

25

flapping. There are also bat-like FMAV with a mechanism for folding and unfolding, such as the large-scale bionic flying fox FMAV designed by the German Festo company, and the Bat Bot2 bat-like FMAV developed by the University of Illinois in the United States [9]. Although these two bat-like FMAV have joined the movement of folding and unfolding, they are limited by the mismatch between the flapping frequency and the folding and unfolding frequency, which makes it difficult to achieve synchronized movement, which increases the difficulty of control and makes it difficult to achieve the desired goal. In order to solve the above problems, this paper designs a structure which can realize the coupling between the flapping mechanism and the folding and unfolding mechanism of the wings, so as to match the flapping frequency with the folding and unfolding frequency of the wings.

2 Structural Design The mechanical structure of the bat-like FMAV can be divided into three parts: the flapping mechanism with the wings folding and unfolding, the leg swing mechanism and the fuselage. The degree of freedom of the whole machine includes: two wings synchronized and symmetrical flapping, that is, the flapping degree of freedom of the two wings are related, and there is only 1 independent flapping degree of freedom; coupling the folding and unfolding mechanism to the flapping mechanism, that is, there are 2 passive movement degrees of freedom on the shoulders of the wings on both sides; the elbow and wrist on each side have a passive rotation degree of freedom; since the angle between the fingers changes little during the flight, the three fingers can be fixed to remove the relative movement between the fingers [10]; each leg is provided with a degree of freedom of rotation in the abdominal-back direction swing. In summary, there are three independent active degrees of freedom: one flapping degree of freedom, two leg swing degrees of freedom, and six passive degrees of freedom: two shoulder movement degrees of freedom, two elbow rotation degrees of freedom, and two wrist rotation degrees of freedom, as shown in Fig. 1. This paper only studies the deformable flapping structure of the wings, and only considers the flapping structure of the wings and the deformable structure of the wing skeleton.

Fig. 1. The degree of freedom of the whole machine

26

B. Duan et al.

2.1 Flapping Mechanism The existing flapping mechanism of FMAV mainly includes double crank-double rocker mechanism [11] and single crank-double rocker mechanism [12]. The mechanism of single crank-double rocker will cause a phase difference between the left and right wings in the process of synchronous flapping, which will lead to the instability of the FMAV. Therefore, the double crank-double rocker mechanism is selected in this design to realize the synchronous symmetrical reciprocating flapping of two wings. The schematic diagram of crank rocker mechanism with one wing is shown in Fig. 2.

Fig. 2. The crank rocker mechanism

In Fig. 2, L1 is crank, L 2 is rocker, L 3 is flapping link, θ1 is the crank angle, θ0 is the flapping angle. From the geometric relationship in the figure, it can be got:  π π θ1  (0, 2π ), θo  − , (y − L1 sinθ1 + L3 sinθo )2 + (x + L1 cosθ1 − L3 cosθo )2 = L22 2 2

(1)

Assume that: D1 = x2 + y2 + L21 − L22 + L23 + 2L1 (xcosθ1 + ysinθ1 )

(2)

D2 = −2L3 (y − L1 sinθ1 )

(3)

D3 = 2L3 (x + L1 cosθ1 )

(4)

ϕ1 = arcsin 

D2

(5)

D2 + D3 2 2

The relationship between the flapping angle θ0 and the crank angle θ1 can be obtained: θo = ϕ1 ± arccos 

D1 D2 + D3 2

2

= arcsin 

D2 D2 + D3 2

2

± arccos 

D1 D2 + D3 2 2

(6)

Design of Deformable Flapping Structure of Bat-Like Flapping Air Vehicle

27

Through calculation, it is found that when the calculation formula takes the negative sign, the value range of θ0 is (0, π), which does not meet the conditions, while the calculation formula takes a positive sign, the value range of θ0 (−π/2, π/2) meets the conditions, so the formula always takes a positive sign, that is: θo = ϕ1 + arccos 

D1

= arcsin 

D2

+ arccos 

D1

(7) D2 + D3 D2 + D3 2   Take the parameters of flapping mechanism as x, y, L1 , L2 , L3 = [24, 30, 9, 32.82, 19.69] and θ1 (0, 2π ), the curve of flapping angle changing with crank angle is obtained, as shown in Fig. 3. D2 + D3 2

2

2

2

2

Fig. 3. The curve of flapping angle changing with crank angle

Asymmetrical flapping of the wings will produce additional effects. When the upward angle is greater than the downward angle, it will enhance the pitch stability of the FMAV; when the downward angle is greater than the upward angle, it will increase the average lift in a cycle. In order to increase the lift of bat-like FMAV, this design adopts an asymmetric flapping mechanism. From the flapping angle curve shown in Fig. 3, it can be seen that the downward angle is about 35° (the downward angle is positive), the upward angle is about 20°, and the downward angle is about 1.75 times of the upward angle, which can realize a wider range of down flapping motion and increase the lift. 2.2 Deformable Mechanism The wing skeleton is a plane mechanism, and the rotation of the elbow and the wrist is linked with the swing rotation of the shoulder to realize the active deformation movement of folding and unfolding the wings. The degree of freedom of the skeleton deformation mechanism is 1. In order to realize the rotation of three joints, watt six-bar linkage mechanism is considered. The schematic diagram of the wing deformation mechanism is shown in Fig. 4. Point B2 is equivalent to the driving point of the whole joint structure, and its displacement along the y-axis is: yB2 = y0 + f (θo )

(8)

28

B. Duan et al.

Fig. 4. Diagram of the wing deformation mechanism

Where, yB2 is the vertical distance from point B2 to fixed point B1 , y0 is the initial value of vertical distance, θo is the flapping angle, f (θo ) is the liner function of θo . According to the structural relationship in the figure, the following formula can be obtained: ⎧ 2 2 B3 −(yB2 2 +(x1 −x2 )2 ) ⎪ ⎪ θ1 = arccos B2 B3 +B12·B ⎪ ⎪ 2 B3 ·B1 B3 ⎪ 2 2 ⎪ ⎪ B2 B3 +(yB2 2 +(x1 −x2 )2 )−B1 B3 ⎪ √ θ = arccos ⎪ 2 ⎪ 2 2 ⎪ ⎪ 1 −x2 )  2B2 B3 · yB2 +(x ⎪ ⎪ · sinθ2 x = B B + B B ⎪ B2B4 2 3 3 4 ⎪

⎪ ⎪ 2 2 ⎪ ⎪ ⎪ B4 B6 = B3 B6 + B3 B4 − 2 · B3 B6 · B3 B4 cosθ1 ⎪ ⎪ 2 2 2 ⎪ +B4 B6 −B3 B6 ⎨ θ3 = arccos B3 B42·B 3 B4 ·B4 B6 (9) 2 2 2 ⎪ B4 B5 +B4 B6 −B5 B6 ⎪ θ = arccos ⎪ 4 ⎪ 2·B4 B5 ·B4B6  ⎪ ⎪ ⎪ yB2 ⎪ θ5 = π − atan( x1 −x2 ) − θ2 − θ3 − θ4 ⎪ ⎪ ⎪ ⎪ ⎪ xB4B5 = B4 B5 · cosθ5 ⎪ ⎪ ⎪ ⎪ ⎪ xB5 = x1 + xB2B4 ⎪  + xB4B5 ⎪ ⎪ ⎪ yB5 = yB2 + B4 B5 · sinθ5 − B2 B3 + B3 B4 · cosθ2 ⎪ ⎪ ⎩ θ7 = θ6 + θ5 The displacement equation of fingertip can be obtained:  xB9 = xB5 + B5 B9 · cosθ7 yB9 = yB5 + B5 B9 · sinθ7

(10)

The deformation movement curve of the wingtip can be obtained by formula (10). The length of the link is optimized by genetic algorithm with the constraint of lift and skeleton weight. The results are [B2 B3 , B1 B3 , B3 B6 , B3 B4 , B6 B5 , B4 B5 , B5 B9 , x1 , θ6 ] = [0.0383, 0.0348, 0.008, 0.068, 0.065, 0.014, 0.1724, 0.017, π/6], and the displacement projection of the wingtip deformation is obtained, as shown in Fig. 5. Since the flapping mode is asymmetric, the corresponding wing deformation motion mode is also asymmetric. The points A-D marked in Fig. 5 represent the highest point in the upward process, the farthest point in the downward process (the distance is the projection distance in the horizontal direction, not the distance amplitude), the lowest

Design of Deformable Flapping Structure of Bat-Like Flapping Air Vehicle

29

Fig. 5. The displacement projection of the wingtip deformation

point in the downward process and the farthest point in the upward process (compared with Fig. 7, it is convenient to understand the meaning of the four points). Point B is higher than point D, which means that the wingspan in the process of downward is larger than that in the process of upward; point A is lower than point C, which means that the projection length of the lowest point of downward is greater than that of the highest point of upward, while the downward angle is 1.75 times of the upward angle, which means that the wingspan at the lowest point is far greater than that at the highest point, that is, the lowest point is in a fully unfolding state, and the highest point is in a fully folding state. Based on the above two points, it is proved that the deformation mechanism can cooperate with the flapping mechanism to realize the function of unfolding when flapping downward and folding when flapping upward. 2.3 Coupling Design Mechanism In this section, a coupling mechanism is designed to realize the coupling of flapping mechanism and deformation mechanism, so as to ensure that a motor can drive the reciprocating flapping and deformation movement at the same time. This design mechanism uses crank rocker, bevel gear and rack-and-gear cooperation to achieve coupling, as shown in Fig. 6. The reduction gear 1 and the reduction gear 2 are fixedly connected by a shaft, so that the flapping mechanism link 3 and the coupling mechanism link 5 rotate at the same time to ensure that the front shoulder (to control flapping) and the bevel gear 1 have the same rotational angular velocity; The two bevel gears 6, 7 mesh with each other and the central axis is arranged vertically to change the direction of movement; The rackand-gear mechanism is used to turn the rotation into a translational motion to control the b reciprocating linear motion of the shoulders, thereby driving the entire wing joints folding and unfolding. In summary, the 2.1 flapping mechanism is coupled with the 2.2 deformable mechanism through the coupling design mechanism. The obtained wingtip motion curve is shown in Fig. 7, where the horizontal axis represents the deformation movement displacement of the wingtip (the value represents the distance from the flapping axis), and the vertical axis represents the rotation angle of the wingtip around the flapping axis.

30

B. Duan et al.

7

10 8 5

6

11

4 9

3

2

1

1 - reduction gear 1; 2 - reduction gear 2; 3 - flapping mechanism link; 4 - front shoulder; 5 - coupling mechanism link;6-bevel gear 1; 7-bevel gear 2; 8-gear; 9-rack; 10 body fixed rod; 11 back shoulder Fig. 6. The coupling design mechanism

Fig. 7. The wingtip motion curve

As can be seen from Fig. 7, the curve shape is a crescent shape. When it moves to point A, the wingtip moves to the position closest to the flapping axis, that is, the wings are in a folding state; When moving to point B, the wingtip moves to the farthest position from the flapping axis (absolute distance, not projection distance), that is, the wings are in an unfolding state. The shape of the trajectory shows that this structure can actually meet the requirements of the expected motion function.

3 Simulation Combining the size of the mechanism designed in the previous section and the actual model for three-dimensional modeling, the overall model is shown in Fig. 8.

Design of Deformable Flapping Structure of Bat-Like Flapping Air Vehicle

31

Fig. 8. The overall three-dimension model

In order to verify whether the designed mechanism can correspond to the theoretical calculation and meet the expected movement goal. Import the 3D model into Adams and analyze its kinematics. The obtained flapping trajectory is shown in Fig. 9.

Fig. 9. The wingtip trajectory

The simulation trajectory curve is the same as the wingtip movement trajectory calculated by the theory in Fig. 7. Both are crescent-shaped curves, which meet the expected motion law, which can prove the accuracy of the simulation model. Judging the accuracy of the model from the trajectory is only a qualitative judgment and lacks a quantitative basis. Next, we will compare the flapping angle and the wingtip deformation and displacement curve to quantitatively illustrate the comparison. Measure the flapping angle of the flapping mechanism and the deformation movement curve of the deforming mechanism in two cycles, as shown in Fig. 10. Comparing Fig. 3 and Fig. 10(a) and Fig. 4 and Fig. 10(b), the flapping angle and wingtip deformation curves simulated by Adams are completely consistent with the theoretical calculation results in the trend and specific values. Through the above results, it can be proved that the structure design is correct and the structure can realize the movement of folding when flapping upward and unfolding when flapping downward.

32

B. Duan et al.

(a) Flapping angle

(b) Wingtip deformation displacement Fig. 10. Simulation model flapping angle and wingtip deformation displacement

4 Conclusion In this paper, from the perspective of bionics, the structure of bat-like FMAV is designed. The flapping mechanism is coupled with the deformation mechanism of the wing skeleton, so that it can use one motor to drive the flapping mechanism and deformation mechanism at the same time, which can reduce the weight of the aircraft, make the overall layout compact and easy to control, and realize the optimization design of bat-like FMAV structure. Adams is used to analyze the motion of the three-dimensional model. By comparing the theoretical calculation results with the simulation results, it is proved that the mechanism can realize the motion of folding when flapping upward and unfolding when flapping downward, which meets the requirements of the expected model function, and verifies the rationality and feasibility of the mechanism design.

References 1. Chen, L.L., Song, W.P., et al.: Research on aerodynamic-structural coupling of flexible flapping wings. Chin. J. Aeronaut. 34(12), 2668–2681 (2013) 2. Fang, R.J., Wu, F.J.: Development of micro air vehicles. Electromech. Technol 36(05), 158– 160 (2013) 3. Keennon, M., Klingebiel, K., Won, H.: Development of the nano hummingbird: a tailless flapping wing micro air vehicle. In: 50th AIAA Aerospace Sciences Meeting including the New Horizons Forum and Aerospace Exposition (2012) 4. Han, J.K, Hui, Z., Tian, F.B., et al.: Review on bio-inspired flight systems and bionic aerodynamics. Chin. J. Aeronautics (2020)

Design of Deformable Flapping Structure of Bat-Like Flapping Air Vehicle

33

5. Riskin D.K, Willis, D.J, Iriarte-Díaz, J., et al.: Quantifying the complexity of bat wing kinematics. J. Theoretical Biol. 254(3), 604–615 (2008) 6. Norberg, U.M., Rayner, J.M.V.: Ecological Morphology and flight in bats (Mammalia; Chiroptera): wing adaptations, flight performance, foraging strategy and echolocation. Philosoph. Trans. Roy. Soc. B Biolog. Sci. 316(1179), 335–427 (1987) 7. Bahlman, J.W, Swartz, S.M., Breuer, K.S.: Design and characterization of a multi-articulated robotic bat wing. Bioinspirat. Biomimet. 8(1), 016009 (2013) 8. Colorado, J., Rossi, C., Barrientos, A., et al.: The role of massive morphing wings for maneuvering a bio-inspired bat-like robot. In: IEEE International Conference on Robotics and Automation (2018) 9. Ramezani, A., Chung, S.-J., Hutchinson, S.: A biomimetic robotic platform to study flight specializations of bats. Sci. Robot. 3(2), eaal2505 (2017) 10. Von Busse, R., Hedenstrom, A., Winter, Y., et al.: Kinematics and wing shape across flight speed in the bat Leptonycteris yerbabuenae. Biology Open 1(12), 1226–1238 (2012) 11. Huang, M.Y., Xiao, T.H., Ang, H.S.: Design of multi-stage flexible flapping wing aircraft. Chin. J. Aeronaut. 31(08), 1838–1844 (2016) 12. Zhou, K., Fang, Z.D., Cao, X.M., Zhang, M.W.: Optimization design of single crank double rocker flapping wing driving mechanism. J. Aeronautical power 01, 184–188 (2008)

An Autonomous Flight Control Strategy Based on Human-Skill Imitation for Flapping-Wing Aerial Vehicle Yihong Li, Juntao Liu, Hui Xu, and Wenfu Xu(B) School of Mechanical Engineering and Automation, Harbin Institute of Technology, Shenzhen 518055, China [email protected]

Abstract. Flapping wing aerial vehicle (FAV) is an aircraft that imitate the flying of birds or insects. Comparing with fixing wing and rotary wing vehicles, FAV is hard to control precisely, because of its inherent vibration and complex dynamic analysis. Therefore, most FAV are controlled by human manipulators. In this paper, we mainly focus on studying the experience of human manipulators, using the experience to control our FAV, also known as HITHawk. We call this control strategy human-skill imitation. Human-skill imitation divides flying stages into three parts, which are takeoff, landing, and cruise. Each stage has its own control logic to fly HITHawk in better performance and safer situation. After that, we test each stage of HITHawk to fly in the air. The takeoff stage needs to climb to expect altitude as soon as possible. The cruise stage needs to fly in a set circle track. HITHawk knows how to keep in the trajectory and go back in case of flying away from the circle. Due to HITHawk’s characteristic, it cannot be controlled the pitch downward to prevent diving while landing. Therefore, the landing stage needs to control the throttle to float and descent until touching the ground. Keywords: FAV · Control strategy · Human-skill imitation

1 Introduction Bionics is the study of the habits, movements, and instincts of animals to imitate, adapt, innovate, and invent new equipment and tools. Flapping wing aerial vehicle (FAV) is a major part of bionics. Unlike conventional flying machines like fixing wing and rotary wing, FAV has the advantages of excellent concealment, maneuverability, and less energy consumption. The majority of FAV generate lift and thrust through wings flapping meanwhile change position and attitude through tails. For example, like Phoenix in MIT [1], SmartBird in Festo [2] and DelFly in TUD [3], they produced those mature structures of vehicles which can fly stable and complete the mission. Some flapping-wing micro aerial vehicle (FWMAV) have a little difference with larger size FAVs. FWMAV use wings to generate upward lift and adjust attitude by the difference in frequency, amplitude, and flapping angle between two wings. Nano hummingbird [4] and KUBeetle-S in Konkuk © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 34–44, 2021. https://doi.org/10.1007/978-3-030-89092-6_4

An Autonomous Flight Control Strategy Based on Human-Skill

35

University [5] are typical FWMAV using leading edges and roots to control the shape of wings and attitude of the vehicle themselves. HITHawk, with two wings of wingspan range from 1.2 m to 2.2 m and a tail has 2 DOF, is a typical kind of FAV (see Fig. 1) [6]. Unlike fixing wing and rotary wing flying vehicles having mature dynamic analysis and control strategies, it is relatively difficult to control a FAV. Therefore, most FAVs are using manual control. The manipulator stays on the ground and visually judge the environment and attitude of the aircraft, sending command during the whole flight. We developed a control strategy call human-skill imitation, which can simulate the strategies while manual flying. This strategy does not need a detailed analysis of dynamic models but uses the experience of the human manipulator, which can be more adaptive to specific FAV. Besides, we used the human-skill imitation and onboard data recorder to record the flight data during different flying stages including takeoff, landing and cruise. Flight data can help us to analyze the feasibility of human-skill imitation.

Fig. 1. HITHawk

Fig. 2. Drive system.

2 Control Strategy Control of the FAV is not as straightforward as fixing wing and rotary wing vehicle because of the inherent vibrations that occur during flapping. Therefore, we developed a control strategy call human-skill imitation, to simulate the flying experience while manually controlling HITHawk. 2.1 Manual Control HITHawk has one motor and two servos to be controlled during flying. The motor drives the reduction gears and the crank-rocker mechanism, which ultimately makes the wings flap. The motor, reduction gears and the crank-rocker mechanism consist of the drive system (see Fig. 2). Two servos control two DOF of the tail, generating pitch and roll torque to control HITHawk to climb, descent and turn (see Fig. 3). While HITHawk is flying in the air, the vertical mechanics model including gravity, lift, and resistance generated by tail is shown in Fig. 4. The amount of lift generated by wings is equal to the sum of gravity and resistance. When HITHawk pitches up, due to the angle of the tail, the resistance will reduce, and the gravity will generate a larger torque to heads down the HITHawk. On the other hand, when HITHawk pitches down, the

36

Y. Li et al.

resistance will increase because of larger windward area of the tail, generating a torque heads up the HITHawk. The above is the simplified self-balance model of HITHawk. The detail dynamic model of HITHawk is involved in Ref [7].

Fig. 3. The tail controlled by two servos.

Fig. 4. Vertical mechanics model of HITHawk in flight

Manipulator normally uses model airplane remote control and receiver to directly control the HITHawk. Three channels of the remote control are used to send PWM signals to adjust the motor speed and servos’ angles, which finally change the flapping frequency of the wings and tail position. Motor speed relates to the throttle channel and two servos’ angles relate to the pitch and roll channel (see Fig. 5). 2.2 Human-Skill Imitation Fixing wing vehicle normally has three DOF, which includes pitch, roll and yaw, to control attitude during flight. And the lift is corresponding to airspeed, angle of attack and wing shape. However, HITHawk has two DOF of attitude control (pitch and roll) and has different procedures between climbing and descending because the lift is mainly related to flapping frequency. Therefore, human-skill imitation is divided into stages of takeoff, landing and cruise, working with sensor data processing and altitude control. Sensor Data Processing. Due to the need to obtain the status and attitude, HITHawk has several sensors that measure airspeed, attitude, position, groundspeed etc.

An Autonomous Flight Control Strategy Based on Human-Skill

37

Fig. 5. Remote control with three command channel and receiver

Airspeed data is obtained by Pitot tube, which can measure both static pressure ps and stagnant pressure pt in the air stream. With the density of air ρ, Airspeed V can be calculated by Eq. (1) according to Bernoulli’s principle (see Fig. 6).  2(pt − ps ) (1) V = ρ

Fig. 6. Pitot tube on HITHawk.

Attitude data is obtained by the Inertial Measurement Unit (IMU), including gyroscopes, accelerometers, and magnetometers. Gyroscopes can directly measure angular velocity, which can obtain angle data by integrating the angular velocity. However, as the Gyroscope works for a long time, the integration error accumulates and becomes more and more unreliable. Therefore, IMU uses accelerometers and magnetometers to obtain acceleration on 3 axes and yew angle according to magnetic field intensity. After processing the data through a complementary filter, PI controller and extended Kalman filter (EKF), attitude data can be more precise (see Fig. 7) [8]. Position and groundspeed data from GPS and RTK module (see Fig. 8). RTK base station can receive position data by GPS satellite, sending relative location to computer

38

Y. Li et al.

Fig. 7. The process of attitude data obtained by IMU.

and calculate the error between GPS on vehicle and base station. GPS and RTK module give position accuracy to the centimeter level. Besides, altitude data, also known as position data on the z-axis, is mainly obtained from both onboard barometer and GPS module.

Fig. 8. RTK base station (Left) and GPS module on vehicle (Right).

With sensor data of airspeed, attitude, position, and groundspeed, it is feasible to make a control strategy during different stages of HITHawk’s flight. Altitude Control. HITHawk has inherent vibration because of the wing flapping, which means its pitch angle oscillates at the same frequency as the flapping of the wings. Thus, we cannot deploy a precise PID controller on pitch control, which is useless and energy consumption. Pitch control mainly works for altitude control, so we directly combine altitude data and pitch servo output into a closed-loop system. Through human manipulator’s flying experience, HITHawk has several characteristics during altitude changing: 1. While flying with a high airspeed, it can climb with an upward pitch command. 2. While flying with low airspeed, it will stall with an upward pitch command due to pitch command will bring greater resistance. 3. Except for emergency descent, the downward pitch command normally unused because it will cause a big change in pitch attitude and even dive to the ground.

An Autonomous Flight Control Strategy Based on Human-Skill

39

4. Descending can be achieved by reducing airspeed, which causes a lower lift comparing to its gravity and thus a slow descent.

Fig. 9. Block diagram of altitude control.

In summary, we can design a block diagram of altitude control (see Fig. 9). After setting an expected height value (Exp_Height) and obtaining the estimated height value (Est_Height) from the barometer and GPS module, we can calculate the error of altitude control (Err_Height). In case of emergency descent, we can set a maximum downward pitch command (Pitch_Com), looking forward to the fastest way of descent. In normal altitude control, we can decide it is climbing or descending depend on whether Err_Height is positive or not. While descending, we can set the throttle value to a specific descending value and keep the pitch value zero. While climbing, depending on the current airspeed, we can decide to increase the throttle value to increase airspeed or set an upward pitch command to satisfy climbing. Takeoff. Without landing gear, HITHawk is unable to accelerate and takeoff from a runway. Therefore, the usual way for HITHawk to takeoff is using a launcher or throwing from hand. Both of those methods give HITHawk an initial velocity to generate enough lift to climb.

40

Y. Li et al.

Fig. 10. Block diagram of takeoff control.

Figure 10 shows the logic of takeoff control. After detected acceleration from a launcher or throwing from the hand, we set the throttle to a takeoff value (normally 30%– 50%), generating enough thrust and lift to climb up. Then we calculate the climbing rate, also known as the value of change in height. If it is a negative climbing rate but not a touchdown, it means the vehicle is hard to climb up, thus we set a maximum throttle to gain enough lift. We also cannot more set pitch command because it will cause bigger resistance to the vehicle. If it is a positive climbing rate, we can switch to altitude control mode and continue flying to estimate height for the cruise. Finally, if it is unfortunately touchdown, we must set the throttle to zero to avoid damaging the structure by flapping on the ground. Landing. Due to unable to set a downward pitch command, the usual way to landing is like gliding. Figure 11 shows how to control the vehicle land. Firstly, setting the throttle to land value (normally 5%–10%) and detect the climbing rate. If it is still a positive climbing rate, it means the throttle is not less enough or too much wind. Thus, we need to set throttle to a minimum, looking for less lift to descent. After descending to a safe altitude, we need to double-check the decline rate to avoid too much landing impact. If the decline rate is bigger than the save value, we must go around to protect the vehicle and try to land again. Otherwise, it is safe to land so that we can set the throttle to zero to touchdown. Cruise. Among the many tasks of the FAV, cruising is essential, especially cruising in circles. As shown in Fig. 12, when starting the circle cruising, we initially set the radius and center of the circle, normally center will be placed on the left or right side of the vehicle, at the same distance as the radius. Therefore, the vehicle will start cruising along

An Autonomous Flight Control Strategy Based on Human-Skill

41

Fig. 11. Block diagram of landing control.

Fig. 12. Block diagram of cruise.

with the tangent of the circle. While flying, the vehicle will be unavoidable deviate from the circle. When the deviation is not such big (r1 < r < r2 , where r is the distance to the center of the circle), we can control the vehicle fly to the tangent. But if the deviation is too big (r > r2 ), it is primarily to fly back to the circle, aiming to the center. If it is in the circle (r < r1 ), all we need to do is to hold the position. All orientational control is

42

Y. Li et al.

based on yaw angle from GPS and IMU data, as well as the roll servo to generate roll torque. Both form a closed-loop control system [9].

3 Experiment After the basic framework of human-skill imitation has been built, we use HITHawk to fly and test the takeoff, cruise, and lading procedure, verifying the feasibility of human-skill imitation.

Fig. 13. Raw acceleration during takeoff.

Fig. 14. Altitude and thrust data during takeoff.

3.1 Takeoff Flowing the standard takeoff procedure, we threw HITHawk with hand, generating an acceleration in the direction of the tail. As we can see in Fig. 13, at the time of about 0:59, hand throwing causing a huge acceleration. At the same time, detecting the acceleration, HITHawk set the throttle to takeoff value (55%), generating enough thrust and lift to climb (see Fig. 14). The altitude gradually increases with the climb and enter the cruise stage after reaching the expected height. 3.2 Cruise After takeoff, HITHawk entered the cruise stage and automatically flew in a circle. Refer to Fig. 15, HITHawk set the start point as the center of the circle, then flew straight along the radius, finally turn left to join the circle. Whenever HITHawk flew away from the circle because of wind or other interference, it would fly to the tangent of the circle. And when HITHawk flew into the circle, it would try to turn in the opposite direction to fly away from the circle.

An Autonomous Flight Control Strategy Based on Human-Skill

43

Fig. 15. Position and Expect Trajectory of HITHawk during cruise.

Fig. 16. Altitude and thrust data during landing.

3.3 Landing To prevent losing control of diving, the landing procedure mainly control by reducing throttle. Figure 16 shows the data of altitude and throttle during landing. The throttle value is high initially, but the altitude has not dropped very much, even increase a little, which is mainly caused by a strong wind. Therefore, HITHawk chose to reduce the throttle to descent. When HITHawk descent to a safe altitude and the descent rate is also safe, it would set throttle value to zero to land on the ground. The little impulse of altitude data is the moment of touching the ground. After all, the landing mission is completed.

44

Y. Li et al.

4 Conclusions In this paper, we developed a new control strategy call human-skill imitation to control a kind of FAV, also known as HITHawk. We imitate the performance and experience of human manipulator who can manipulate HITHawk skillfully. We mainly divided it into three stages during flying, which is takeoff, landing, and cruise. Different stages have different control logic to fly better and prevent danger. After all, we conducted experiments on different stages of human-skill imitation, testing the performance of HITHawk while flying in the air.

References 1. Jackowski, Z.J.: Design and construction of an autonomous ornithopter. Massachusetts Institute of Technology (2009) 2. Mackenzie, D.: A flapping of wings. Science 335(6075), 1430–1433 (2012) 3. Karásek, M., Muijres, F.T., De Wagter, C., et al.: A tailless aerial robotic flapper reveals that flies use torque coupling in rapid banked turns. Science 361(6407), 1089–1094 (2018) 4. Keennon, M., Karl, K., Henry, W.: Development of the nano hummingbird: a tailless flapping wing micro air vehicle. In: 50th AIAA Aerospace Sciences Meeting Including the New Horizons Forum and Aerospace Exposition, p. 588 (2012) 5. Phan, H.V., Aurecianus, S., Kang, T., Park, H.C.: KUBeetle-S: an insect-like, tailless, hovercapable robot that can fly with a low-torque control mechanism. Int. J. Micro Air Vehicl. 11, 1–10 (2019) 6. Pan, E., et al.: Two experimental methods to test the aerodynamic performance of HITHawk. In: International Conference on Intelligent Robotics and Applications, pp. 386–398. Springer, Cham (2019). https://doi.org/10.1007/978-3-030-27535-8_35 7. Xu, W., et al.: Flight control of a large-scale flapping-wing flying robotic bird: system development and flight experiment. Chin. J. Aeronaut. 1–7 (2021) 8. Mi, G., Tian, Z., Jin, Y., Li, Z., Zhou, M.: MIMU update algorithm based on the posture and magnetometer. Chin. J. Sensors Actuat. 28(1), 43–48 (2015) 9. Wang, Y.: Research on autonomous formation flight control method of large bionic flappingwing robot. Harbin Inst. Technol. 39–40 (2020)

A New Moving Target Interception Algorithm for an AUV in the Ocean Current Environment Xiangqiao Meng1 , Wei Zhang2 , Bing Sun1(B) , and Daqi Zhu1 1

2

Shanghai Engineering Research Center of Intelligent Maritime Search and Rescue and Underwater Vehicles, Shanghai Maritime University, Haigang Avenue 1550, Shanghai 201306, China Department of Electrical Engineering and Automation, Shanghai Dianji University, Shanghai 201306, China

Abstract. For the interception problem of autonomous underwater vehicle (AUV) in the ocean current environment, it is not only necessary to plan the intercept path of the AUV but also to determine the moving trajectory of the target. In this paper, an artificial potential field method integrating Kalman filter and velocity vector synthesis (APF-VS-KF) is proposed to solve this problem. For the underwater environment with ocean current, artificial potential field method integrated velocity synthesis (APF-VS) can modify the moving direction of AUV that is affected by ocean current, so that AUV can intercept target faster. Kalman filter (KF) approach can filter out the underwater noise and make the target move along the ideal trajectory. The simulation results show that the AUV can quickly intercept the moving target and the noise has little effect on the moving trajectory of the target through APF-VS-KF.

Keywords: Moving target interception Kalman filter · Velocity synthesis

1

· Artificial potential field ·

Introduction

At present, there have been some applications in real life for tracking or intercepting a moving target by robots. For example, the missiles intercept invading the hostile target in the military. In the robot soccer game, the robots [1,2] intercept the moving footballs or the opponent’s robots. But there are few applications in the underwater environment. In recent years, the development and utilization of marine resources are increasing day by day, but there are still some scarce resources to be protected. For the complex and changeable marine environment, it is still unsolved problem Supported by the National Natural Science Foundation of China (61873161, U1706224), Shanghai Rising-Star Program (20QA1404200) and National Key Research and Development Plan (2017YFC0306302). c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 45–55, 2021. https://doi.org/10.1007/978-3-030-89092-6_5

46

X. Meng et al.

for the AUV to intercept the hostile target and this paper will try to solve this problem. So far, there have been some interception algorithms, but few interception algorithms are suitable for interception problem of the AUV. Hence, it is necessary and important to study the interception technology of the AUV. The guidance law [3,4] is one of interception methods. It uses the navigation and guidance theory and takes the distance of the robot and the target as the optimization goal and applies the optimal control method to design the guidance rate. Active prediction planning execution (APPE) [5,6] method is also an interception method, but it is difficult to predict the interception point for the inertial constraints. This paper proposes a new interception algorithm, it can solve interception problem of the AUV in the ocean current environment. The innovation points of this paper mainly include the following two aspects. 1) For the underwater environment with noise, the target cannot follow the ideal trajectory. KF can effectively filter out the noise, and the error between its trajectory and the real trajectory tends to zero after some iterations. 2) For the underwater environment with ocean current, the moving direction of the AUV will be affected. APF-VS can change the moving direction of AUV to the direction calculated by artificial potential field (APF) method, so that the AUV can intercept target faster. This paper is organized as follows. Section 2 describes the basic model of an AUV intercepting moving target. Section 3 introduces the proposed APF-VS-KF algorithm. Simulation results are presented in Sect. 4. Finally, the conclusion is given in Sect. 5.

2 2.1

The Basic Model for an AUV Intercepting Moving Target The Overview of Interception Problem

The AUV intercepts the target moving with uniform speed, which mainly means that the AUV can avoid obstacles and overcome the influence of ocean current, and finally intercept the target quickly in the underwater environment. It believes that the interception task is completed when the Euclidean distance between the AUV and moving target is less than or equal to a certain threshold. Three problems need to be solved. Firstly, it avoids obstacles and plans the intercept path of the AUV [7,8]. Secondly, it filters out the effect of underwater noise on the moving target. Thirdly, it increases interception efficiency of the AUV. Figure 1 shows that schematic diagram of an AUV intercepts target moving with uniform velocity in the 3-D underwater environment with ocean current. 2.2

A Model of the Underwater Environment

The underwater environment is a three-dimensional space. In most cases, when the AUV is navigating underwater, its motion control is usually decoupled to

Moving Target Interception for AUV in Ocean Current Environment

47

Fig. 1. Target interception of AUV in the 3-D underwater environment with ocean current

the horizontal plane and the vertical plane. Except for recycling and lifting, AUV usually works in the same horizontal plane, so the modeling of underwater environment can be simplified appropriately and expressed by two-dimensional environment map model. It is assumed that there are some static obstacles, one AUV and one moving target in the working field. The speed of the AUV and the moving target are denoted as Vr and Vg respectively. Assuming that the unit sampling time is T seconds. Once the moving target is detected by the AUV, the current time is reported t0 . The current position of the AUV and the moving target are regarded as their starting points which are denoted by R0 and G0 respectively.

3

The Proposed APF-VS-KF Algorithm

The marine environment is an underwater environment with ocean current. The ocean current will cause AUV to deviate from the moving direction determined only by APF, which makes AUV cannot intercept the moving target in a shorter path. Hence, it is of great significance to study and eliminate the adverse effects of the ocean current on the AUV. The proposed APF-VS algorithm can modify the moving direction of AUV, so that AUV can intercept the moving target quickly. In interception process, motion position of the target is determined by KF algorithm. Figure 2 shows the flowchart of APF-VS-KF method. 3.1

Planning Intercept Path of the AUV

Artificial potential field [9,10] algorithm is widely used in robot path planning. In the process of intercepting the target by an AUV, the surrounding environment

48

X. Meng et al.

Fig. 2. Flowchart of the APF-VS-KF method

is abstracted into a kind of artificial potential field. The potential field generated by obstacles is called the repulsion potential field, where it is not zero only within the certain range of obstacles and the rest of position is zero. The potential field generated by moving target is called the gravitational potential field, which fills the whole environment and the potential field value is the global minimum. The potential field at any point in the space is the superposition of gravitational potential field and the repulsion potential field. AUV determines the next moving position by searching for the fastest decreasing direction of the potential field, and the potential field generated by the target is the global minimum, so the AUV will move towards the target. Marine environment is an underwater environment full of ocean current, which will have a certain influence on the course of AUV and reduce the interception efficiency, the APF-VS method can overcome this effect. The idea of APF-VS method is to control the velocity direction of AUV, so that the

Moving Target Interception for AUV in Ocean Current Environment

49

direction of AUV combined with the velocity vector of ocean current is exactly the moving direction of AUV calculated by APF method. The 2-D Synthetic Model of APF-VS. Figure 3 is 2-D model of APF-VS. The little green rectangle at the origin represents AUV, D(t) is the velocity vector of AUV calculated by APF, Vc (t) represents the velocity vector of ocean current. In the environment without ocean current, AUV can move along D(t) to obtain the optimal path. But when there is ocean current Vc (t) in the environment, the AUV will deviate from D(t). Only when the AUV is adjusted to vector Vr (t), the AUV can follow D(t).

Fig. 3. The 2-D model of APF-VS

The following is the principle of synthesis. D(t) = {D(t)| min U (t) = Uatt (t) + Urep (t)}

(1)

Vr (t) = {Vr (t)| Vr (t ) sin a1 = Vc (t ) sin(a1 + a3 )}

(2)

Among them, Uatt , Urep , U (t) respectively represent the gravitational potential field, the repulsive potential field and the total potential field. a1 is the angle between velocity vector Vr (t) and D(t). a2 is the angle between D(t) and positive direction of x-axis. a3 is the angle between Vc (t) and positive direction of x-axis. The restriction provided by Eq. 2 can determine Vr (t).

50

X. Meng et al.

Fig. 4. The 3-D model of APF-VS

The 3-D Synthetic Model of APF-VS. The integrated model in a 3-D environment with ocean current is as follows. D(t) = {D(t)| min U (t) = Uatt (t) + Urep (t)}

(3)

Vr (t) = {Vr (t)|T (t) • Vr (t) = 0, Vr (t ) sin(β1 ) = Vc (t ) sin(β2 ), β1 + β2 = arccos(Vc (t) • Vr (t)/ Vc (t ) Vr (t ) )}

(4)

The restriction provided by Eq. 4 can determine Vr . Figure 4 is a 3-D schematic diagram of APF-VS. Among them, T (t) is defined as T (t) = Vc (t) × D(t), β1 is the angle between Vr and D(t), β2 is the angle between Vc and D(t). 3.2

Update Moving Position of Target Based on KF

In this paper, the position coordinates of the target after each unit sampling time T are predicted by KF algorithm. For the underwater environment with noise, the target cannot follow the ideal path. KF approach can effectively decrease the effect of the noise on the moving trajectory of the target. The main idea is to predict the current state according to the target state of the previous time, and then modify the predicted value of the current time according to the observed value and obtain the optimal estimated value of the current target state. The five core equations of KF algorithm [11,12] are given below. −

xt = Axt−1 + But−1

(5)

Moving Target Interception for AUV in Ocean Current Environment

51

Pt − = APt−1 AT + Q

(6)

Kt = APt−1 AT + Q

(7)

xt = xt − + Kt (zt − Hxt − )

(8)

Pt = (I − Kt H)Pt −

(9)

−

Among them, xt is the predicted value of the target state at time t, A is the − state transition matrix, B is the control matrix, Pt is the matrix of control variables, B is the predictive value of covariance matrix at time t , Q is the covariance matrix generated by the noise of the system itself, Kt is Kalman coefficient, H is the observation matrix, R is a covariance matrix caused by observation noise, xt is modified state of the target at time t. On the basis of known target state xt−1 and covariance matrix Pt−1 , the target state and covariance matrix at time t can be predicted by Eqs. 5 and 6. Based on the observed value zt of the target state at time t, the state of the target and covariance matrix at time t are modified by Eqs. 7, 8, 9. In this paper, for 2-D environment with ocean ⎡ ⎤ current, the state dimension 1010 ⎢0 1 0 1⎥ ⎥ is set to 4, state transition matrix A = ⎢ ⎣ 0 0 1 0 ⎦, covariance matrix is identity 0 0 0 1 1000 matrix of 4-D, observation matrix H = , Q is set to e−7 I4 , R is set to 0100 e−1 I2 , ut−1 is a column vector of 4-D whose elements are all 0. By the above method, the target position can be accurately determined after each unit sampling time T in the process of intercepting the moving target by an AUV.

4

Simulation Results and Analysis

Simulations are conducted on the MATLAB platform. To verify the effectiveness of algorithm, the underwater environment is simplified, the AUV and moving target are both regarded as particles. It assumes that AUV is represented by a little green rectangle, the obstacles are represented by solid black dots and the intercept point is represented by solid blue dot. The green trajectory represents the intercept path of the AUV, the trajectory of the red pentagram represents the trajectory of the target obtained by KF. The trajectory formed by the black hollow dot represents the actual motion trajectory of the target. Suppose the unit sampling time T is 1 s, the speed of the AUV and the moving target are denoted as 2 m/s and 1 m/s respectively.

52

4.1

X. Meng et al.

The AUV Intercepts a Target Moving Along a Constant Straight Line in a 2-D Environment with Ocean Vurrent of 90◦

Assume that the ocean current is a constant current with a velocity of 0.8 m/s and direction of 90◦ . In a 2-D ocean current environment of 50 m ∗ 50 m, the initial position of AUV is [0, 0]. Because the historical state of the target is needed to determine the motion position of the target, for accuracy of prediction, the initial position of the target is set to [15.5660, 28.8966]. Figure 5(a) and Fig. 5(b) show the simulation diagrams by APF and by APF-VS respectively. The intercept point of Fig. 5(a) is located in [45.1253, 44.9240], the intercept point is located in [43.7015, 44.4620] after integrating velocity synthesis

Fig. 5. An AUV intercepts a target moving along a constant straight line (a) APF (b) APF-VS

Moving Target Interception for AUV in Ocean Current Environment

53

in Fig. 5(b). Hence, AUV can successfully intercept the target moving along a constant straight line whether or not velocity synthesis is integrated in the environment with ocean current.

Fig. 6. Path length and time of AUV intercepting target A. APF B. APF-VS

As can be seen from Fig. 6, the intercept path length of AUV has been reduced by two meters and intercept time saved 1 s after the integrated velocity synthesis. Hence, the AUV intercepts moving target faster by APF-VS.

Fig. 7. The error between the actual trajectory of the target and the Kalman trajectory

Figure 7 shows the error between the real trajectory of target and the trajectory obtained by the KF in the 2-D environment. As can be seen from the Fig. 7, KF can effectively filter out noise and make the error between its trajectory and the real trajectory approach zero after some iterations.

54

4.2

X. Meng et al.

The AUV Intercepts a Target Moving Along a Constant Straight Line in a 2-D Environment with Ocean Current of 180◦

Fig. 8. An AUV intercepts a target moving along a constant straight line (a) APF (b) APF-VS

Without changing the environment and parameter settings in Fig. 5, the velocity vector of ocean current is only changed to V c(t) = [−0.5, 0]. Figure 8(a) and Fig. 8(b) show the intercept path of AUV planned by APF and by APF-VS respectively. The simulation results show that the intercept path length has been reduced by ten meters and intercept time saved 5 s after the integrated velocity synthesis. Hence, the effectiveness of APF-VS is proved again.

Moving Target Interception for AUV in Ocean Current Environment

5

55

Conclusion

This paper mainly studies interception problem of the AUV in the underwater environment with ocean current and the APF-VS-KF method is proposed. The APF-VS method is used to plan the intercept path of AUV and avoid obstacles. The KF approach is used to filter out the effect of noise on moving target trajectory. Simulation results show that APF-VS algorithm can avoid obstacles and overcome the effect of ocean current on moving direction of AUV, and make AUV intercept target faster. Experiments verify that KF method can reduce the effect of the noise on moving target trajectory.

References 1. Sun, W.C., Tang, S.Y., Gao, H.J., Zhao, J.: Two time-scale tracking control of nonholonomic wheeled mobile robots. IEEE Trans. Control Syst. Technol. 24(6), 2059–2069 (2016) 2. Zhu, Q.B., Hu, J., Henschen, L.: A new moving target interception algorithm for mobile robots based on sub-goal forecasting and an improved scout ant algorithm. Appl. Soft Comput. 13(1), 539–549 (2013) 3. Li, B., Lin, D.F., Wang, J., Tian, S.: Guidance law to control impact angle and time based on optimality of error dynamics. Proc. Inst. Mech. Eng. Part G J. Aerospace Eng. 233(10), 3577–3588 (2019) 4. Lee, C.H., Ryu, M.Y.: Practical generalized optimal guidance law with impact angle constraint. Proc. Inst. Mech.l Eng. Part G J. Aerospace Eng. 233(10), 3790– 3809 (2019) 5. Park, J., Choi, J.S., Kim, J., Ji, S.H., Lee, B.H.: Long-term stealth navigation in a security zone where the movement of the invader is monitored. Int. J. Control Autom. Syst. 8(3), 604–614 (2010) 6. Croft, E.A., Fenton, R.G., Benhabib, B.: An on-line robot planning strategy for target interception. J. Robot. Syst. 15(2), 97–114 (1998) 7. Chen, M.Z., Zhu, D.Q.: Multi-AUV cooperative hunting control with improved Glasius bio-inspired neural network. J. Navig. 72(3), 759–776 (2019) 8. Cao, X., Sun, C.Y., Chen, M.Z.: Path planning for autonomous underwater vehicle in time-varying current. Path Plan. Auton. Underwater Vehi. Time Vary. Curr. 13(8), 1265–1271 (2019) 9. Wang, S.M., Fang, M.C., Hwang, C.N.: Vertical obstacle avoidance and navigation of autonomous underwater vehicles with h infinity controller and the artificial potential field method. J. Navig. 72(1), 207–228 (2019) 10. Sun, B., Zhu, D., Yang, S.X.: An optimized fuzzy control algorithm for threedimensional AUY path planning. Int. J. Fuzzy Syst. 20(2), 597–610 (2017). https://doi.org/10.1007/s40815-017-0403-1 11. Zhu, D.Q., Cheng, C.L., Sun, B.: An integrated AUV path planning algorithm with ocean current and dynamic obstacles. Int. J. Robot. Autom. 31(5), 382–389 (2016) 12. Jin, X., Yang, J., Li, Y., Zhu, B., Wang, J., Yin, G.: Online estimation of inertial parameter for lightweight electric vehicle using dual unscented Kalman filter approach. IET Intell. Transp. Syst. 14(5), 412–422 (2020)

Design and Control of In-Pipe Inspection Robot for Pre-commissioning Hui Li1,2(B)

, Ruiqin Li1(B) , and Yuan Wang1

1 School of Mechanical Engineering, North University of China, Taiyuan 030051, China

[email protected], [email protected] 2 Department of Mining Engineering, Lvliang University, Lvliang 033001, Shanxi, China

Abstract. This paper presents the design, control, and implementation of a new in-pipe inspection robot named LH-6. This robot can spontaneously adapt its configuration while it is rolling in the pipeline. The constant contact of all wheels with the ground is ensured by a series of elastic spring in the middle of each supporting arm. By analyzing the wheel kinematics model of the supporting arms mechanism of rolling, the dynamic equation for the contact drive of the in-pipe robot is established, and according to the requirements of pre-commissioning condition, optimization of its control inputs is performed using optimal control approach. It provides speed and force control that allows the robot to properly balance its posture while the load can be distributed as evenly as possible on each contact wheel on the bottom of the robot for any situation of the pipeline. Finally, the robot is tested on different pipeline shapes in the field and proves its capability to adapt in real-time to the pre-commissioning pipeline. Keywords: Large in-pipe robot · Pre-commissioning · Long-distance pipeline inspection · Speed and force control · Multi-contact interaction

1 Introduction The pipelines have been known as high efficiency, low cost, more secure, and fast-flowing transportation lines [1]. The rapid development of the global oil and gas transmission industry has led to a significant increase in the number of pipelines. At present, a large part of the energy needs of many countries is transported by pipelines [2]. The economies of many countries depend on the smooth and uninterrupted operation of these pipelines, so ensuring their safe and trouble-free operation is becoming increasingly important [3]. The long-distance transportation of hydrocarbons such as oil and gas pipelines are large diameter (≥711 mm) steel pipes [4]. The long diameter of large diameter steel pipes is produced by helical welding in the manufacturing process, and the connection between pipe segments is usually made by ring welding [5]. Pre-commissioning refers to the period between the completion of the pipeline, and its being put into use, in which many key preparations are carried out to ensure the pipeline can meet the requirements of its owner or the operator [6]. The pipeline in the construction phase is affected by © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 56–66, 2021. https://doi.org/10.1007/978-3-030-89092-6_6

Design and Control of In-Pipe Inspection Robot for Pre-commissioning

57

various factors. A larger defect may exist in the pre-commissioning stage, such as geometric deformation of pipeline caused by collision or extrusion, pipe internal mechanical scratch, pipe sundry legacy, etc. These defects will affect the formal running pipeline integrity management. In the pre-commissioning stage, these defects need to be detected and dealt with in time to eliminate the risk. The commonly used pre-commissioning equipment for pipeline testing at the stage is mainly Pig or caliper with the diameter measuring device, and the air compressor is used as the power source to press air into the pipeline and promote the operation of the detector strongly in the pipeline [7]. For the pre-commissioning stage of large-diameter pipelines, on the one hand, there is a risk of Pig jamming and stagnation due to lack of power; on the other hand, the speed is difficult to control, such as the risk of super-highspeed advance, so that the detector cannot collect data normally [8]. Sometimes, the detector may be damaged, and the pipeline may be seriously or even injured. There are obvious defects in using such pigs in this stage. Most of the pipelines are installed in the field, underground, or in places with limited space [9]. It is difficult and costly to conduct the pre-commissioning and maintenance of the pipeline by the above externally driven and passively operated detector [10]. Therefore, it is necessary to design an autonomous pipeline robot with internal energy to deal with this problem. It can enter the pipeline to complete the inspection and maintenance of the pipeline instead of the diggers and other working tools, which greatly saves the cost of manpower and material resources. Because of the error of machining dimension, even under the same driving current, the front and rear wheels motion of the robot cannot be completely consistent, and the performance of the same motor also has a certain difference. The rotation speed of all driving motors cannot be kept the same at any time. These nuances are hard to spot in a short mobile robot, but when the robot in the pipe runs a few kilometers, the tiny difference of the influence of the error of the robot is more serious. Even the same mechanical properties of the drive motor in the straight pipe will inevitably result in a small error in speed. The long-distance movement causes the robot to rotate around the X/Y axis, causing blockage problems, and random rotation around the centerline of the pipe. When the pipe diameter changes or runs through the weld bulge, it will also affect the rotation speed of the driving wheel, and there will be differentiation, which may lead to wheel slippage or wheel strong push to rotate the axis of the robot around the pipe. In this paper, the universal joint motor layout, and the Quadratic planning (QP) control method to improve the driving process of mechanical load and extra force consumption, reduce the possibility of motor burning, reduce energy consumption, solve the bend turning problem and straight pipe drive unbalanced load problems.

2 Kinematic Properties of HL-6 Robot in Pipes 2.1 Overview of the LH-6 Robotic System The architecture of the LH-6 robot will directly affect its motion performance and detection ability. According to the testing requirements before the large-diameter pipeline is put into production, LH-6 adopts a wheeled mechanism as the mobile mechanism of the robot, including six groups of walking mechanism, battery compartment, control

58

H. Li et al.

box, sensing detection unit, control arm, and other components. Its appearance and configuration are shown in Fig. 1.

Fig. 1. (a) The naked robot from lateral view, (b) Focus on the naked robot from front view, (c) The LH-6 robot with an operating arm and sensing detection unit from a three-quarter view.

The robot has 6 groups of supporting arms and 12 supporting wheels. The supporting wheel installed with the driving motor is the driving wheel, otherwise, it is the driven wheel. The hub of the LH-6 supporting wheels is 7075 aluminum alloy, and the outer ring is coated with 90A polyurethane of 30 mm thickness, which is wear-resistant and shock-absorbing. The robot is divided into six equal parts to arrange the supporting arms, each supporting arm is installed on both ends of a wheel, and each supporting arm is equipped with a spring expansion element. The walking mechanism includes a supporting arm, a supporting wheel, a driving motor, etc., which supports and pulls the robot. Driving force of the supporting wheel pipeline inspection robot, the essence is the friction between the driving wheels and pipe wall, therefore the driving wheels and pipe wall always stay in touch, and generate enough clamping force, namely closing force. And on this basis, it is necessary to have a strong traction force to drive robot motion. The walking mechanism needs to provide closing force and variable diameter ability and can also provide traction before and after movement walk institutions. Now, a description of the actuator’s arrangement of the robot is being presented. LH-6 has six actuators, four of which are rated at 1 kw and two of which are rated at 2 kw. RE40 precision drive motors provide enough power for the operation of the vehicle. Motor#1, #2, #3, and #4 are 1 KW drive motors, Motor#5 and #6 are 2 kw compensation power motors, Motor#1 and #2, Motor#3 and #4 are arranged in 180°, Motor#1 and #2 are installed in the front end of the robot, Motor#3 and #4 are installed in the rear end of the robot, Motor#5 and #6 are installed on the telescopic mechanism.

Design and Control of In-Pipe Inspection Robot for Pre-commissioning

59

2.2 Kinematics of HL-6 Robot in Pipes A wheel moving in the pipe must have contact with the pipe wall, which may form a contact area with complex geometry. This contact area depends on the surface geometry of the inner wall surface, such as coating, curved surface curvature, surface morphology, and whether it is attached to sundries; It depends on the shape and material of the wheel. The wheel is made of a steel frame with high strength polyurethane attached, and the wheel wall can be regarded as the hard wall of the soft wheel. Therefore, the contact point between the wheel and the pipe wall is approximated as the contact tangent point between the equivalent wheel surface and the wall surface where the symmetry plane of the wheel center is located.

Fig. 2. (a) Definition of the wheel-pipe-wall contact frame. αi is defined as the angle between pipeline generatrix and τi , βi is defined as the angle between the radius of the wheel and the radius of the pipeline, both pass the point of contact. (b) Coordinate system for wheel–pipe-wall interaction mechanics in the pipeline, ωi is the rotation speed of the wheel, v is the speed of the LH-6 robot, FN is the normal force and FT is the tangential force.

As shown in Fig. 2(a), Ci is the center of the wheel. yi is the unit vector of the axis of the rolling wheel; Pi is the center point of the contact area; Pi − τi ni bi is the contact(frenet) frame for the contact point movement. O − XYZ is the inertial reference frame; Ci − xi yi zi refers to the coordinate frame attached to the center of the wheel; O1 − X1 Y1 Z1 is the moving coordinate frame attached to the center of mass (COM) of the robot, the COM passes through the center axis of the pipeline, ni is the normal unit vector of the contact point, perpendicular to the section of the pipe wall at the contact point, τi is the longitudinal unit vector along the direction of the wheel movement, bi is the lateral unit vector, determined by ni = bi × τi . Vector bi lies on the extension line of the projection of a vector yi onto the contact plane [11], then y × ni  and |Ci Pi |Pi = ri · ni . bi =  i (1) yi × ni  The diameter of the pipeline and wheel is 2R and 2r, respectively, the rotational speed of the ith wheel is ωi , at time t, the coordinates of point Pi in the frame {O1 } as follows: Pi (t) = [ pix1 piy1 piz1 ]T ,

(2)

60

H. Li et al.

where     pix1 = R sin ωi trRsin α , piy1 = r sin −ωi trRsin α , piz1 = −ωi tr cos α. according to the properties of the frenet frame as follows: P˙ i (t) P˙ × P¨ i  , bi =  i  , n = b × τ, τi =  P˙ i (t) P˙ i × P¨ i  Then, the rotation matrix of the ith contact frame {Pi } with respect to the robot’s moving frame {O1 } as follows: Ri = [τ Ti nTi bTi ], the contact point Pi in frame {O1 } is given by: (3) PiO1 = Ri · PPi ,  T with PiO1 = [ xO1 yO1 z O ]T , Ppi = 0 r cos βi r sin βi . 1 The centroid velocity of the robot is expressed as v ∈ R3 in the inertial frame {O}, and the rotational velocity of the robot is expressed as ωe ∈ R3 in frame {O1 } [12]: [V Pi ]O1 = RT v + ωe × l i ,

(4)

where [V Pi ]O1 is the velocity of the contact point Pi in frame {O1 }, li ∈ R3 is the distance vector of the robot’s COM to the ith contact point, and R is the rotation matrix between frame {O} and {O1 }, which can be obtained by using the Euler Angle formula through rotation angles φ = (θ, ψ, ϕ)T . φ˙ can be calculated as a function of rotational speed ωe . This relation has the following matrix form: φ˙ = Hφ ωe , where

⎡

1

Sψ Sϕ Sψ Cϕ Cψ Cψ

(5)

⎤

⎥ ⎢ Hφ (φ) = ⎣ 0 Cϕ −Sϕ ⎦, with Cθ = cos(θ ), Sθ = sin(θ ), etc. Sϕ Cϕ 0 Cψ Cψ substitute Eq. (5) into Eq. (4) to obtain: ˙ [V Pi ]O1 = RT v − [˜li ]Hφ−1 φ,

(6)

where [˜li ] ∈ R3×3 is the skew matrix corresponding to the cross-product operation, which lead to following equation in matrix form:  S˙ 

G −1 T ˜ = Li x˙ , (7) [V Pi ]O1 = R −[li ]Hφ φ˙ where Li = [ RT −[˜li ]Hφ−1 ] ∈ R3×6 is called locomotion matrix, x˙ = [ S˙ G φ˙ ]T ∈ R6 is the COM velocity twist of with respect to the frame {O1 }. the position of COM of HL-6 in frame {O} is denoted by SG , then v = S˙ G . In these equations, li depends on the wheel radius r, the normal contact ni : li = ki − ri n

(8)

where ki is the position of the center of the wheel expressed in the frame {O1 } and is obtained from the kinematic model.

Design and Control of In-Pipe Inspection Robot for Pre-commissioning

61

3 Dynamical Model of HL-6 Robot in Pipes The movement of the robot in the pipeline is restricted by the space of the pipeline, which is a kind of space-restricted movement [13]. The robot movement consists of the curve movement along the pipe axis and the possible rotation around the pipe axis. The former is the content of this paper, while the latter is the problem of robot attitude deflection, which is not discussed here. As shown in Fig. 2(b), Two reference frames appear here, the inertial frame {O} whose origin is on the central axis of the pipeline and the frame {O1 } whose origin is on the geometric center of HL-6. It is stipulated herein that the corresponding coordinates of the two coordinate systems are parallel to each other. The velocities twist V of COM point G of HL-6 robot and the wrench F acting on COM in frame {O1 }:     vG FG ∈ R6 and F = ∈ R6 . V = (9) ωG MG The net external force of the robot in the pipeline can be expressed as follows:   FG = (Fdi + FCi + FRi ) + mg = Fi + mg (10) where Fdi is the driving force at the contact point between the ith wheel and the pipe wall surface when the robot is moving; mg robot gravity, g ∈ R3 gravity acceleration; FCi is the closing force between the ith wheel and the pipe wall. As can be seen from δ ∈ R3 is the vector from the COM point G to the origin of frame {O1 }, the velocity of the COM point G in the inertial frame is v = [vG ]O1 , then vG and v˙ G as follows: vG = [vG ]O1 = v + [ω˜ e ]δ,

(11)

v˙ G = v˙ + [ω˙˜ e ]δ + [ω˙˜ e ]2 δ,

(12)

where [ω˜ e ] ∈ R3×3 is the skew matrix, the relationship between the force FG and the acceleration v˙ G is written as: FG = m˙vG = m[˙v + [ω˙˜ e ]δ + [ω˙˜ e ]2 δ], then, Eq. (10), (12) and (13) can be solved jointly:  Fi = m˙vG − mg = m[˙v + [ω˙˜ e ]δ + [ω˙˜ e ]2 δ] − mg.

(13)

(14)

The net external moment of the robot in the pipeline can be expressed as follows:   MG = (Mdi + MCi + MRi ) + Mg = Fi pi,G + Mg (15) where Mdi is the torque generated by the ith driving force, MCi is the torque generated by the ith closing force, MRi is the torque generated by the ith motion resistance [15], M g is the torque generated by gravity, and pi,G ∈ R3 is the vector from the ith contact point to the COM of the robot.

62

H. Li et al.

The relationship between the moment MG and the angular acceleration ω˙ e is written as: MG =



Fi pi,G + Mg = IG ω˙ e ,

(16)

where, IG ∈ R3 is the moment of inertia of the robot with respect to the CoM, since the direction of gravity of the robot may pass through the CoM, in general, M g = 0, then.  (17) Fi pi,G ≈ IG ω˙ e , according to Eq. (14) and (17), the following formula can be obtained: ⎡ ⎤   F1   I3×3 · · · I3×3 ⎢ . ⎥ m[˙v + [ω˜˙ e ]DG + [ω˜˙ e ]2 DG ] − mg ⎣ .. ⎦ = IG ω˙ e [˜p1,G ] · · · [˜pu,G ]       Fu    A b

(18)

F

where, [˜p1,G ] is the skew symmetric matrix, and the robot wheel has u contact points with the inner wall of the pipeline.

4 Control of HL-6 Robot Based on Kinetic Model 4.1 Control of CoM’s Position and Trunk’s Orientation We compute the desired acceleration of the CoM v˙ d ∈ R3 using a PD control law:     v˙ d = Kpc G d − G + Kdc vd − v , (19) where G d ∈ R3 is the desired position of the CoM, whereas Kpc ∈ R3×3 and Kdc ∈ R3×3 are positive define diagonal matrices of proportional and derivative gains, respectively. Similarly, we compute the desired angular acceleration of the robot’s trunk ω˙ e ∈ R3 as:     (20) ω˙ ed = Kpe e Rd RT + Kde ωed − ωe , where R ∈ R3×3 and Rd ∈ R3×3 are rotation matrices representing the actual and desired orientation of the trunk with respect to the inertial frame {O}, respectively, e(·) : R3×3 → R3 is a mapping from a rotation matrix to the associated rotation vector, ω˙ e ∈ R3 is the angular velocity of the trunk, whereas Kpe ∈ R3×3 and Kde ∈ R3×3 are positive define diagonal matrices of proportional and derivative gains, respectively.

Design and Control of In-Pipe Inspection Robot for Pre-commissioning

63

4.2 Computation of the Desired Supporting Force Given a desired value of the acceleration of the CoM and theangular acceleration of the robot’s trunk, we want to compute the desired supporting force Fi . We rewrite Eq. (18) in matrix form as: ⎡ ⎤    F1  I3×3 · · · I3×3 ⎢ . ⎥ m[˙vd + [ω˜˙ ed ]DG + [ω˜˙ ed ]2 DG ] − mg . = (21) ⎣ . ⎦ IG ω˙ ed [˜p1,G ] · · · [˜pu,G ]       Fu    b1 A1 F

where we replaced the actual accelerations with the desired accelerations. This system has 6 equations and k = 3u unknowns; since in our experiments 3 ≤ u ≤ 12, typically the system has infinite solutions. We exploit this redundancy to satisfy the inequality constraints imposed by the friction cones. At every control loop we solve the following quadratic program (QP): N d = argminN ∈Rk (A1 F − b1 )T S(A1 F − b1 ) + αF T WF s.t. dmin < CF < dmax

(22)

where S ∈ R6×6 and W ∈ Rk×k are positive-definite weight matrices, α ∈ R weighs the secondary objective, C ∈ Rp×k is the inequality constraint matrix, dmin ,dmax ∈ Rp the lower/upper bound respectively, with p being the number of inequality constraints. These ensure that the supporting force lie inside the friction cones and the normal components of the supporting force stay within some user-defined values. According to literature [16, 17], C, dmin and dmax can be calculated. The regularisation term F T WF was used to enforce the torque constraint indirectly. 4.3 Details of Control In the straight pipe, the HL-6 robot detects the robot’s running attitude by using a precise nine-axis gyroscope in real-time and then transmits the collected robot attitude data to the main controller. When the robot rotates clockwise, the running speed of motor#2 remains unchanged, but the running speed of motor#1 should be reduced to make the robot gradually recover the horizontal posture. When the robot rotates counterclockwise, the running speed of motor#1 remains unchanged, but the running speed of motor#2 should be reduced to make the robot gradually recover its horizontal posture. The specific speed is reduced by a related algorithm [18]. When the robot moves forward, motor#1 and #2 execute speed mode, motor#3 and #4 execute torque mode, and motor#5 and #6 do not work. The robot driving wheel #1-#4 presents a cross arrangement. The advantage is

64

H. Li et al.

that no matter what the robot encounters is uphill, downhill, or left-right rotation, it can be had at least three wheels in contact with the pipe wall, which can effectively ensure that the robot has enough driving force. When the robot encounters obstacles in the pipeline, to ensure the robot can run smoothly in the pipeline, a safety protection strategy in the robot control system should be set up. When the robot encounters an insurmountable obstacle, the current of the robot will instantly increase beyond the threshold of the blocking current, and the robot will judge that it has encountered a large obstacle. As a result, the robot will retreat 10 m, and then rush forward again, if the three consecutive sprints are still out of the way, it means that the pipeline has a major problem. At this point, the robot will actively withdraw from the pipe. When the robot enters the curved pipe, the IMU detects that the robot is in turning. At this time, the control system of the robot will actively enter the turning state and select the two driving wheels in the speed mode. The one that contacts the outer bend wall will increase the current and speed. The other contact with the inner bend wall will appropriately reduce the speed and improve the driving torque. Thus, the problem that the arc length of the robot is inconsistent between the inner and outer bending is solved. If the value fed back by the robot decreases or not within a certain time, it indicates that the robot is sliding. At this point, the driving wheels with motor#5 and #6 press down on the pipe wall to prevent slippage and provide sufficient power for the robot.

5 Experiments A field experiment was implemented to observe the running state of the HL-6 inspection robot inside the natural gas pipeline above the field nearby Dragon town, Heihe City, Heilongjiang Province, China. The tests were conducted three times over half a year, each lasted a week. The length of the pipeline under construction is 5 km. The thicknesses of the pipelines(#LY3407358) are 21.4 mm, its diameter is 1422 mm. Figure 3 is five pictures of the field experiment. In Fig. 3(a), the horizontal bend section of the pipeline; in Fig. 3(b), the pipeline bends the climbing section of the pipeline; in Fig. 3(c), the horizontal section of the pipeline; in Fig. 3(d), the HL-6 robot is ready to enter the pipeline at the transfer station; in Fig. 3(e), the HL-6 robot is preparing to enter the pipeline in the field. The results of the field experiment show that the HL-6 robot can work well in the natural gas transmission pipeline, and can pass the horizontal section, the horizontal bending section, and the climbing section of the pipeline smoothly. For more details, we can refer to Reference [19].

Design and Control of In-Pipe Inspection Robot for Pre-commissioning

65

Fig. 3. Field experiment inside pre-commissioning pipeline: (a) the horizontal bend section of the pipeline; (b) the climbing section of the pipeline; (c) the horizontal section of the pipeline; (d) the HL-6 robot is ready to enter the pipeline at the transfer station; (e) the HL-6 robot is preparing to enter the pipeline in the field.

6 Conclusions This paper presents a new cross-driven pre-commissioning pipeline testing robot with active adaptability to the pipeline environment and automatic tractive force adjusting for long-distance inspection in natural gas pipelines with large diameter series. Based on the dynamic model, our attitude control algorithm enables the robot to move autonomously over long distances without human intervention. This allows the robot to extend the detection range of a single job in the pipeline. The self-inspection system combines data returned by various sensors to provide flexible responses to various situations in the pipeline. Field tests show that the robot can work well in the pre-commissioning pipeline, and can pass through inclined pipeline and pipeline with complex terrain smoothly.

References 1. Sambasivan, M., Gopal, S.: Handbook of Oil and Gas Piping: A Practical and Comprehensive Guide. CRC Press, Boca Raton (2018) 2. Dey, P.K.: A risk-based model for inspection and maintenance of cross-country petroleum pipeline. J. Qual. Maint. Eng. 7(1), 25–43 (2001) 3. Roslin, N.S., Anuar, A., et al.: A review: hybrid locomotion of in-pipe inspection robot. Proc. Eng. 41, 1456–1462 (2012) 4. Iqbal, H., Tesfamariam, S., et al.: Inspection and maintenance of oil & gas pipelines: a review of policies. Struct. Infrastruct. Eng. 13(6), 794–815 (2017)

66

H. Li et al.

5. Papadaki, C.I., Chatzopoulou, G., et al.: Buckling of internally-pressurized spiral-welded steel pipes under bending. Int. J. Press. Vessels Pip. 165, 270–285 (2018) 6. Ab Rashid, M.Z., Yakub, M.F.M., et al. Modeling of the in-pipe inspection robot: a comprehensive review. Ocean Eng. 203, 107206 (2020) 7. Shen, H.Y., Phan, H.T.: Blind faith gas export pipeline pre-commissioning and commissioning (PCC) challenges. In: Offshore Technology Conference. Offshore Technology Conference (2010) 8. Russell, D.: Pigging in pipeline pre-commissioning. Pigging Prod. Serv. Assoc. 9 (2005) 9. Chatzigeorgiou, D., Youcef, T., et al.: MIT leak detector: modeling and analysis toward leak-observability. IEEE/ASME Trans. Mechatron. 20(5), 2391–2402 (2015) 10. Zaman, D., Tiwari, M.K., Gupta, A.K., Sen, D.: A review of leakage detection strategies for pressurised pipeline in steady-state. Eng. Failure Anal. 109, 104264 (2020) 11. Song, Z., Ren, H., Zhang, J., et al.: Kinematic analysis and motion control of wheeled mobile robots in cylindrical workspaces. IEEE Trans. Autom. Sci. Eng. 13(2), 1207–1214 (2015) 12. Grand, C., Benamar, F., Plumet, F.: Motion kinematics analysis of wheeled–legged rover over 3D surface with posture adaptation. Mech. Mach. Theory 45(3), 477–495 (2010) 13. Zhang, Y., Yan, G.: In-pipe inspection robot with active pipe-diameter adaptability and automatic tractive force adjusting. Mech. Mach. Theory 42(12), 1618–1631 (2007) 14. Kwon, Y.S., Yi, B.J.: Design and motion planning of a two-module collaborative indoor pipeline inspection robot. IEEE Trans. Rob. 28(3), 681–696 (2012) 15. Ding, L., Gao, H., Deng, Z., et al.: Foot–terrain interaction mechanics for legged robots: Modeling and experimental validation. Int. J. Robot. Res. 32(13), 1585–1606 (2013) 16. Focchi, M., del Prete, A., Havoutis, I., Featherstone, R., Caldwell, D.G., Semini, C.: Highslope terrain locomotion for torque-controlled quadruped robots. Auton. Robot. 41(1), 259– 272 (2016). https://doi.org/10.1007/s10514-016-9573-1 17. Siciliano, B., Khatib, O. (eds.): Springer handbook of robotics. Springer, Cham (2016). https:// doi.org/10.1007/978-3-319-32552-1 18. Featherstone, R. (eds): Rigid Body Dynamics Algorithms Springer Boston (2008).https://doi. org/10.1007/978-1-4899-7560-7 19. Li, H., Li, R., Zhang, J., et al.: Development of a pipeline inspection robot for the standard oil pipeline of China national petroleum corporation. Appl. Sci. 10(8), 2853 (2020)

Research on Technology of Pipeline Detection Robot Based on Spiral Propulsion ZhiQian Wang1,2(B) , ZhenBo Deng1,2 , and Pei Lei1,2 1 Aviation Industry Chengdu Aircraft Industry Group

Co., Ltd. (Liability), Chengdu 610037, China 2 Sichuan Aviation Intelligent Manufacturing Equipment Engineering

Technology Research Center, Chengdu 610037, China

Abstract. Aiming at the space curved pipeline with an inner diameter of less than 15 cm, in order to realize its internal detection work, this paper studies a compact, functionally integrated micro pipeline inspection robot and the corresponding pipeline inspection and scanning strategy. First, the robot adopts the driving principle based on spiral propulsion, and uses only one power source to realize the robot’s forward, backward, and stop motions in the pipeline, and adapt to the bending and spatial posture of the pipeline. Ensure the robot’s traveling ability and traveling efficiency in the pipeline. Second, the robot is equipped with a camera and a variety of measuring sensors to realize the detection of the inside of the pipeline. With the help of the robot’s spiral propulsion action, the laser ranging sensor can scan the inside of the pipeline at 360°. And based on the robot’s helix angle and coordinate conversion algorithm, the Cartesian coordinate system point cloud data set of the pipeline inner wall is obtained, and the point cloud data is processed based on the Poisson surface reconstruction algorithm to realize the visual surface reverse modeling of the pipeline inner wall, thereby Realize the foreign body inspection inside the pipeline. Third, this paper proposes a pipeline roundness detection algorithm based on point cloud data, which realizes the pipeline roundness detection. Fourth, this paper proposes a method for determining the location of foreign objects based on point cloud data. Finally, this paper designs a verification experiment to verify the robot’s traveling mode and detection ability. Keywords: Pipeline robot · Spiral propulsion · Point cloud processing · Roundness detection

1 Introduction As a device for transporting gas, liquid or fluid with solid particles, pipelines have become one of the most important systems in modern engineering. Pipelines are mainly used in water supply, drainage, heating, gas supply, and long-distance transportation of oil and natural gas, Agricultural irrigation, hydraulic engineering and various industrial installations [1–6]. Therefore, the health of the pipeline directly affects the normal operation of the engineering system. Due to the tightness of the pipeline, the remote control robot is often used to detect the internal state of the pipeline. In order to make the robot © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 67–78, 2021. https://doi.org/10.1007/978-3-030-89092-6_7

68

Z. Wang et al.

move smoothly in the pipeline to perform detection operations, the way the robot moves in the pipeline is very important. According to the different driving principles of the robot, it can be divided into mobile type, wheel type, crawler type, abdominal wall type, peristaltic type, etc. [7–14]. At present, it is very difficult to detect the inside of pipelines with an inner diameter of less than Φ15 cm. The main reason is that the robot cannot realize the miniaturization design of the body under the existing driving principle. For this reason, this paper proposes a driving principle of pipeline robot based on spiral propulsion. The robot developed in this paper is equipped with a camera and a variety of measuring sensors. With the help of the robot’s spiral propulsion action, the laser ranging sensor can scan the inside of the pipeline at 360°. And get the point cloud data inside the pipeline. Then, based on the Poisson surface reconstruction algorithm, the reverse modeling inside the pipeline is realized and the roundness detection algorithm based on the point cloud realizes the roundness measurement inside the pipeline. Finally, the internal detection of the pipeline is realized.

2 Pipeline Robot Design 2.1 Structure Composition of Pipeline Robot The robot consists of two parts: the front part of the car body and the rear part of the car body. The front part of the car body is used to carry the control system and power system, and the rear part of the car body is used as the rotating propulsion unit and the measuring unit. as shown in Fig. 1. As shown in Fig. 2. The robot outputs the power of the geared motor in the front section of the car body to the rear section of the car body through the slip ring, and transmits the signal of the rear section of the car body to the control system of the front section of the car body to avoid wire entanglement when the car body rotates. The slip ring is the key bridge connecting the front and rear ends of the car body.

Fig. 1. Spiral-propelled pipeline robot

The guide wheels located in the front section of the car body are similar in structure to the drive wheels located in the rear section of the car body. They are composed of guide

Research on Technology of Pipeline Detection Robot Based on Spiral Propulsion

69

shafts, linear bearings, springs, wheel feet, and bearing wheels. The springs enable the robot to move the wheels in the pipeline. Contact with the inner wall of the pipeline at a certain pressure to ensure friction. And it can adapt to changes in pipeline inner diameter and bending curvature within a certain range. Regardless of the pipeline’s attitude, it can climb and descend vertically along the pipeline. The range of adaptable pipeline inner diameter changes is: D = ±5 mm, D is the inner diameter of the pipeline.

1. Guide wheel; 2. Front section of car body; 3. Slip ring; 4. Drive wheel; 5. Linear bearing; 6. Bearing wheel; 7. Guide shaft; 8. Camera; 9. Geared motor; 10. Output shaft; 11. The rear section of the car body; 12. Wheel feet; 13. Spring Fig. 2. Robot structure

2.2 Robot Travel Method The orientation of the guide wheels in the front section of the car body is parallel to the pipeline axis, which can ensure that the robot maintains its orientation and attitude angle when traveling in the pipeline, prevents the car body from overturning, and provides a stable platform for measuring equipment. The driving wheels of the rear section of the car body and the pipeline axis present a certain angle α, as shown in Fig. 3. The robot simulates the motion form of the screw rod, and realizes the robot traveling in the pipeline by relying on the rotary motion of the rear section of the car body skeleton. The traveling trajectory is shown in Fig. 4. The angle between the driving wheel of the rear section of the car body and the axis of the pipeline is The helix angle of the robot, so the rotation lead of the robot is: K = D · π · tan α

(1)

Where D is the inner diameter of the pipeline. The rotation lead of the robot can be adjusted by adjusting the angle between the driving wheel and the pipeline axis, thereby adjusting the motion parameters such as the power and travel speed of the robot. Under the condition of constant speed, the relationship between the included angle and the motion parameters is: the larger the included angle α, the greater the robot power, and the lower the traveling speed; the smaller the included angle α, the smaller the robot power, and the greater the traveling speed.

70

Z. Wang et al.

Fig. 3. Robot helix angle and travel trajectory

3 Robot Control System The robot uses the Arduino MCU as the main control chip and the Arduino IDE as the integrated development platform. The control program is written and developed based on the C language. The control logic is shown in Fig. 4.

Fig. 4. Circuit control logic design

The robot uses 7.2 V lithium battery as the energy system. The robot control system is also divided into front-end and back-end hardware. The front-end hardware includes the main control chip, power supply module, lighting module, motor drive module, SD card storage module, Oled display screen, and WI-Fi communication module. The lighting module can ensure normal video monitoring in the dark pipeline, the SD card storage module can save point cloud scanning data, and the Oled display can display the internal operating status of the robot; the rear hardware part includes the laser ranging sensor, MPU gyroscope, and data Collection module. The latter part is mainly sensor equipment, which is used to scan the inner wall of the pipeline and generate point cloud data. The front and rear hardware is used as a connection bridge through slip rings. The

Research on Technology of Pipeline Detection Robot Based on Spiral Propulsion

71

slip rings can prevent the wire harness from being twisted when the car body rotates at the back, and realize energy and data transmission.

4 Pipeline Inner Wall Scanning Detection Strategy The laser ranging sensor carried by the robot in this paper is used to scan the inner wall of the pipeline and generate its point cloud data, which is used to reconstruct the visual surface of the inner wall of the pipeline to facilitate the detection of obstacles and the accuracy of the inner wall of the pipeline. 4.1 Point Cloud Data Acquisition The acquisition of the point cloud relies on the laser ranging sensor and MPU gyroscope located in the rotating part of the rear end of the robot, and the sensor is driven by the rotating motion when moving forward to achieve a 360° scan of the inner wall of the pipeline. The distance value L is measured by the laser ranging sensor, and the real-time attitude angle θ of the back-end rotating part is measured by the MPU gyroscope, that is, the point coordinates (L, θ ) in the polar coordinate system of the scanning point are obtained. And convert it to space rectangular point coordinates. As shown in Fig. 5, the coordinate conversion algorithm is: Scanning points L and θ at time T1, we get:   (3) X1,2.3 = L1,2.3 + r · sin θ   Y1,2,3 = L1,2.3 + r · cos θ

(4)

In the formula, r is the distance from the laser ranging sensor to the central axis of rotation. Assuming that the attitude angle of the scanning point at T0 is θ 0 and the Z coordinate value is 0 at the beginning, then: Z = (θ − θ0 ) · K

(5)

Among them, K is the lead.Finally, the X, Y, and Z coordinate values of the scanning point are obtained. a set of initial data contains one angle and three distance values, which can be converted into three-dimensional point coordinates in space under three rectangular coordinate systems. 4.2 Poisson Surface Reconstruction After the robot completes the acquisition of the point cloud inside the pipeline, and based on the obtained point cloud data, it reconstructs the point cloud inside the pipeline to visualize the surface, thereby allowing the user to obtain various information inside the pipeline, including: welding at the pipe connection. The size and shape of the scar, whether there is a foreign body and the location of the foreign body, etc. In this paper, the

72

Z. Wang et al.

1. Protective case; 2. Laser ranging sensor; 3. MPU gyroscope Fig. 5. Schematic diagram of sensor layout and scanning

Poisson surface reconstruction algorithm, which is very effective in the fitting method, is used to visualize the surface impact on the inner wall of the pipeline. The Poisson surface reconstruction algorithm is a triangular mesh intermediate algorithm based on implicit functions. This algorithm combines the advantages of global and local methods, integrates all point cloud information, and can well reconstruct the inner wall details of the pipeline [15–22].

Fig. 6. Poisson surface reconstruction algorithm

4.3 Pipe Roundness Detection The pipelines are mostly thin-walled shapes, so they are easy to be compressed and deformed during pipeline processing and transportation. This chapter studies the pipeline roundness detection algorithm based on scanning point clouds. Since the driving wheel and guide wheel assembly of the robot in contact with the pipeline has elastic characteristics, the rotation center of the robot (scanning center of gravity) and the axis of the pipeline will be misaligned when the robot is traveling and scanning. Therefore, the roundness accuracy of the pipeline cannot be directly obtained through the L 1 , L 2 , L 3 measured by the laser ranging sensor. This chapter processes the distance value scanned by the laser distance sensor based on the principle of external

Research on Technology of Pipeline Detection Robot Based on Spiral Propulsion

73

triangles, and obtains the circumscribed circle of the scanning point, and judges the roundness accuracy of the pipe by comparing the diameter of the circumscribed circle and the diameter of the pipe, as shown in Fig. 7. In Fig. 7, L 1 , L 2 , L 3 is the three distance values measured by the robot in a single measurement, and they are vectors. R1 is the radius of the correct contour circle of the pipeline,. R2 is the radius of the circumscribed circle obtained from L 1 , L 2 , L 3 measured by the laser distance sensor, the three points scanned are on the actual contour of the pipeline, and r is the correct contour circle of the pipeline The distance between the center of the circle and the scanning center of the robot. This value is an unknown quantity. If the pipeline is not deformed, the circumscribed circle obtained from the three scanning points will coincide with the correct contour circle of the pipeline. If the pipeline is deformed, the circumscribed circle obtained from the three scanning points will coincide with the correct contour of the pipeline The circles are not centered or have unequal radii. R represents the radius difference between the circumscribed circle and the correct contour circle of the pipeline, and O represents the center distance between the circumscribed O2 and the correct contour circle O1 of the pipeline. Define: φ = m · R + n · O

(16)

φ is the accuracy error of the pipeline circle. Among them, m、n is an empirical constant, which is related to the elastic characteristics of the robot’s elastic feet.

Fig. 7. Principle of pipe roundness measurement

74

Z. Wang et al.

5 Experimental Verification In this chapter, an aluminum tube with an inner diameter of 10 cm is used as the test environment of the robot to verify the effectiveness of the robot driving principle based on screw propulsion and the traveling efficiency of the robot. In addition, foreign objects such as screws, nuts, and rivets are placed in the pipeline to verify the effectiveness of the pipeline inspection and scanning strategy. When the robot is traveling, the robot collects and saves the initial data of the sensor. After that, the data is exported to the computer, and the computer processes the initial data to obtain the point cloud data inside the pipeline to verify the accuracy of the pipeline roundness detection algorithm. Reverse modeling based on point cloud data to verify the accuracy of foreign body recognition.

Fig. 8. Pipeline robot for testing

5.1 Robot Pipeline Travel Test In order to test the effect of the robot’s spiral propulsion structure on traveling in the pipeline. In this paper, two aluminum tubes with an inner diameter of Φ10cm are used as the test scene. The bending degree and length of the two aluminum tubes are different to verify the effectiveness of the robot’s walking structure and signal shielding performance.

Pipeline1: Length 120cm

Pipeline2: Length 75cm

Fig. 9. Aluminum pipe for test

As shown in Fig. 9. The total length of pipeline 1 is 120 cm, with a 90° bend, and its turning radius is: 15 cm. The total length of pipeline 2 is 75 cm, and it has two bends with an angle of 150°, and its turning radius is: 20 cm. Place the pipes 1 and 2 on the horizontal plane for testing as shown in the figure, and then place the pipes 1 and 2

Research on Technology of Pipeline Detection Robot Based on Spiral Propulsion

75

Fig. 10. Robot travel speed

perpendicular to the horizontal plane and test. The traveling speed of the robot in the four test environments is shown in Fig. 10. The test results are shown in Table 1. the robot speed is calculated based on the point cloud data. Through the analysis of the table, it can be obtained that when the robot travels in the pipeline, in addition to overcoming friction, it will also experience additional resistance caused by the bending of the pipeline. When the pipeline is in a vertical posture, it will also be subject to additional resistance caused by the robot’s own gravity. Table 1. Robot pipeline travel experiment results Test scene

Maximum speed

Minimum speed

Passing time

Remote control distance

Level pipeline1

1.2 cm/s

0.7 cm/s

111.5 s

~35 m

Level pipeline2

1.2 cm/s

1.0 cm/s

65.8 s

~35 m

Vertical pipeline1

1.2 cm/s

0.4 cm/s

164.6 s

~35 m

Vertical pipeline2

0.8 cm/s

0.6 cm/s

102.1 s

~35 m

5.2 Point Cloud Surface Reconstruction A section of aluminum tube with an inner diameter of 10 cm and a length of 150 cm was used as a test site to verify the point cloud scanning strategy of the robot. When the robot travels in the aluminum tube, it collects the data of the laser ranging sensor and the MPU gyroscope, and uses the coordinate conversion algorithm of Sect. 5.1 to convert the initial data into spatial rectangular coordinate points to obtain the scanning point cloud data inside the pipeline. Based on the Poisson surface algorithm of Sect. 5.2, the point cloud is visualized surface reconstruction. First, the point cloud is meshed, and then the reverse modeling of the inner wall of the pipeline is obtained based on the Poisson algorithm, as shown in Fig. 11.

76

Z. Wang et al.

Step 1: Point cloud data acquisition

Step 2: Point cloud meshing

Step 3: Visualize surface reconstruction

Fig. 11. Surface reconstruction process

In Fig. 12, A nut is placed in the pipeline as a foreign object in the pipeline to verify whether the point cloud scanning strategy can scan the foreign object. The nut is placed about 65 cm away from the entrance of the aluminum tube, and the outline size of the nut is about 1 × 0.8 × 1 cm.

Fig. 12. Point cloud foreign body scanning

It can be obtained from the experimental data that the presence of foreign objects can be clearly observed in the point cloud data display, as shown in the red circle in Fig. 12. However, the presence of foreign objects cannot be seen in the reconstructed surface. This is because the density of the point cloud is relatively sparse, which is not conducive to the reverse reconstruction of the details of the surface.

6 Conclusion Aiming at the detection problems of space bending pipelines, the characteristics and detection requirements of the pipelines are comprehensively analyzed. In order to realize the detection work in the interior of small and curved pipes, this paper proposes a robot based on spiral propulsion. The axis of the guide wheel and the pipe are parallel, and there is an angle between the drive wheel and the axis of the pipe, that is, the helix angle,

Research on Technology of Pipeline Detection Robot Based on Spiral Propulsion

77

which determines the helix lead of the robot. In addition, as a detection platform, the robot is equipped with a camera, a wireless control module and various sensors. With the help of the robot’s spiral propulsion action, the laser ranging sensor can scan the inside of the pipeline 360°. The robot obtains the distance value and angle value through the laser ranging module and the MPU gyroscope, and obtains the point cloud data based on the coordinate transformation algorithm. This paper proposes a pipeline roundness detection algorithm based on point cloud data to judge the deformation of the pipeline. Based on the Poisson surface reconstruction algorithm, the reverse surface modeling of the pipeline is obtained, and the presence and location of foreign objects are judged based on this.

References 1. Kim, J.H., Sharma, G., Iyengar, S.S.: Design concept and motion planning of a single-moduled autonomous pipeline exploration robot. In: Conference of the IEEE Industrial Electronics Society. IEEE (2010) 2. Zhang, Y.F., et al.: Principle analysis and structure design of the pipeline robot driven by fluid medium pressure difference. J. Jilin Inst. Chem. Technol. (2009) 3. Dewei, T.A.N.G., Qingkai, L.I., Shengyuan, J.I.A.N.G., et al.: Differential property and traction force of triaxial differential pipeline robot in elbow. Robot 32(1) 91–96 (2010) 4. Zhang, X., Chen, H.: Independent wheel drive and fuzzy control of mobile pipeline robot with vision. In: Conference of the IEEE Industrial Electronics Society. IEEE (2003) 5. Xiao-Yun, X.U., et al.: Comparative study of three regulating mechanisms for pipeline robot to suit different pipe diameters. Optics Precis. Eng. (2004) 6. Fukuda, T., Hosokai, H., Otsuka, M.: Autonomous pipeline inspection and maintenance robot with inch worm mobile mechanism. In: IEEE International Conference on Robotics & Automation. IEEE (2003) 7. Choi, Y.H., Yang, H.S., Park, N.C.: Development of a pipeline robot like foxtail. 식물병연구 17, 129–135 (2007) 8. Wang, Y.X., Su, J.: Movability of the tracked pipeline-robot based on hierarchical fuzzy control. High Technol. Lett. 17(2), 166–172 (2011) 9. Li, Z.X., et al.: Development of the self-adaptive pipeline cleaning robot. Adv. Mater. Res. 97–101, 4482–4486 (2010) 10. Guo, F., et al.: Research on driving force of pipeline robot in elbow of pipeline. J. Harbin Inst. Technol. 38(8), 1264–1266 (2006) 11. Chen, H.J., Gao, B.T., Zhang, X.H.: Drive control system for pipeline crawl robot based on CAN bus. J. Phys: Conf. Ser. 48(1), 1233 (2006) 12. Jun-Yuan, L.I., et al.: Localization technique of pipeline robot based on multi-sensor data fusion. Control Decis. (2006) 13. Wang, Z.W., et al.: Precision location technology of pipeline robot based on multi-sensor data fusion. Robot 30(3), 238–241 (2008) 14. Feng-Ping, X.U., Deng, Z.Q.: Research on traveling-capability of pipeline robot in elbow. Robot (2004) 15. Yu, Q., et al.: Incremental poisson surface reconstruction for large scale three-dimensional modeling. In: Chinese Conference on Pattern Recognition and Computer Vision, (PRCV) Springer, Cham (2019). https://doi.org/10.1007/978-3-030-31726-3_38 16. Estellers, V., et al.: Robust poisson surface reconstruction. In: International Conference on Scale Space and Variational Methods in Computer Vision Springer, Cham (2015). https://doi. org/10.1007/978-3-319-18461-6_42

78

Z. Wang et al.

17. Kazhdan, M.M., Bolitho, M., Hoppe, H.: Screened poisson surface reconstruction. In: Proceedings of the Fourth Eurographics Symposium on Geometry Processing, Cagliari, Sardinia, Italy, 26–28 June 2006. DBLP (2006) 18. Kazhdan, M., Bolitho, M., Hoppe, H.: Poisson surface reconstruction. The Japan Institute of Energy (2013) 19. Nie, J., et al.: Rapid surface reconstruction algorithm from dense point cloud. Jisuanji Fuzhu Sheji Yu Tuxingxue Xuebao/J. Comput. Aided Des. Comput. Graph. 24(5), 574–582 (2012) 20. Zeng, Q., et al.: Simple building reconstruction from LIDAR point cloud. In: International Conference on Audio IEEE (2008) 21. Wang, J., Li, Z.: 3D building facade reconstruction based on image matching-point cloud fusing. Chin. J. Comput. 35(10), 2072 (2012) 22. Lin, C.H., Kong, C., Lucey, S.: Learning efficient point cloud generation for dense 3D object reconstruction. arXiv (2017)

Dynamic Characteristics of Two-Mass Inertial Pipeline Robot Driven by Noncircular Gears Dawei Liu1,2(B) and Jiarui Lu1 1 College of Mechanical Engineering, Yanshan University, Qinhuangdao 066004, China

[email protected] 2 National Engineering Technology Research Center of Cold Rolling Strip Equipment

and Technology, Yanshan University, Qinghuagndao 066004, China

Abstract. Considering small diameter pipes, a two-mass inertia pipeline robot driven by a non-circular gear is proposed to study the walking dynamics of the robot in an isotropic Coulomb friction environment. The mechanism principle of the two-mass inertia pipeline robot is given to illustrate the walking mechanism of the robot under the combined action of the internal force and the external isotropic coulomb friction force. A dynamic model of a two-mass impact pipeline robot driven by non-circular gear is established. The dynamic response of the robot is calculated to reveal the walking characteristics of the pipe-impact robot under nonharmonic inertial force by the numerical method. The results show that the moving speed of the robot wavelike decreases with the increase of the input speed. Increasing the eccentricity of the non-circular gear or the motor speed in an appropriate range can be effectively improved the moving speed of the robot. Keywords: Pipeline robot · Inertial drive · Noncircular gear · Two-mass system · Walking dynamics

1 Introduction The pipeline network is a significant infrastructure for water transmission, communications, petrochemical industry, and nuclear industry [1]. In the huge pipe network system, pipeline robots have become indispensable intelligent equipment for intelligent management of the pipeline network. There are crawlers, legged, and wheeled robots with simple manipulation and high reliability in conventional pipeline systems [2]. However, for small diameter pipes and special pipes conveying corrosive or flammable medium, more stringent requirements are put forward for the size, sealing and corrosion resistance of robots. Unidirectional movement of a body can be generated by the interaction of internal inertia force and environmental friction force. Provatidis studied the mechanisms with two eccentrics that rotate in opposite directions and proved the possibility of a linear motion driven by periodic inertial forces [3]. Two single-mass robots can be connected by elastic elements to form a two-mass robot. Zimmermann studied the linear motion of a two-element system connected by a spring and introduced that the average velocity could © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 79–90, 2021. https://doi.org/10.1007/978-3-030-89092-6_8

80

D. Liu and J. Lu

be controlled by changing the phase of the eccentric mass block [4]. Besides, Muscia investigated ships in water driven by inertia force and concluded that the speed of the ship can be increased when the two contra-rotating masses are driven by elliptical gears [5]. As can be seen from the above research, the rotational speed law of the eccentric block has a significant influence on the mobile performance of the inertial robot. Two plastids with relative motion will form the simple two-mass movement system. Chernous’ko carried out an in-depth and systematic study on the two-mass moving system. The kinematic parameters of the system were optimized in the dry and viscous friction environment to improve its average speed [6, 7]. Considering the rectilinear motion of a two-mass system in a resistive medium, Yegorov and Zakharova obtained the periodic law of motion of the internal mass which ensures the shell moves at a specified average velocity and minimum energy consumption [8]. Fang and Xu investigated the creep dynamics of a vibration-driven system from the perspective of sliding bifurcation [9, 10]. Chen and Xu studied the dynamics of a new vibration-driven system with two modules connected by a mechanical position limiter under isotropic dry friction [11]. Liu used four different friction models to describe the movement of the capsule system in different environments to obtain the maximum speed of the capsule robot [12]. These studies show that speed of the two-mass system can control more flexibly, which is conducive to improving the moving speed of the inertial robot. In conclusion, using non-circular gear to modify the simple harmonic inertia force or using two (multiple)-mass mechanism both can improve the moving performance of the inertial robot. Therefore, this paper proposes an inertial pipeline robot driven by a non-circular gear.

2 Mechanism Principle of Two-Mass Inertial Pipeline Robot The two-mass impact robot consists of an internal oscillator and a housing. The feature of the two-mass inertia robot in this paper is that the internal mass makes a non-sinusoidal oscillation through the action of the contra-rotating masses driven by a pair of noncircular gears. The mechanism principle of the two-mass pipeline robot is illustrated in Fig. 1. The internal oscillator consists of the motor, non-circular gears, synchronous gears, and eccentric block. The motor drives the active non-circular gear 1, with the driven noncircular gear 2 is rigidly secured to synchronous gear 3. The centerline of the non-circular gear is parallel to the movement direction of the robot. The two synchronous circular gears 3 and 4 are the same geometric dimension, with the connecting line between their rotating center is perpendicular to the direction that the robot moves. Two identical eccentric blocks are respectively secured to the synchronous circular gear. The angle between the eccentric block and the non-circular gear is ϕ. The internal oscillator is attached to the housing by a spring and damping. There are multiple rows of elastic supporting feet install on the housing, which can cause pressure on the pipeline. The friction force between the robot and the pipe can be controlled by adjusting the pressure value. In the internal oscillator, the transmission ratio of the non-circular gear and the angle ϕ between the eccentric block and the non-circular gear determines the vibration

Dynamic Characteristics of Two-Mass Inertial Pipeline Robot Driven Spring

Non-circular ggear

81

Synchrronous gear E Eccentric block

Δϕ

Inteernal oscillator

External shell

Sup pporting feet

Fig. 1. Mechanism diagram of two-mass inertia pipeline robot

Internal inertial force Fc

mode of the internal oscillator. Figure 2 shows the inertial force of the internal vibrator driven by a pair of non-circular gear. The work done by the combination of internal inertia force and external friction force f is shown in the shaded part of Fig. 2. When the positive work is greater than the negative work, the robot will move towards one direction. The transmission ratio of the non-circular gear is the key to improve the speed and stability of the robot in the isotropic dry friction environment.

+ f

-

-

-f

Time

Fig. 2. Schematic diagram of anisotropic symmetry inertial force

3 Dynamic Modeling of Two-Mass Inertial Pipeline Robot 3.1 Transmission Ratio of Non-circular Gear The transmission ratio of non-circular gear has an important effect on the mobility performance of the robot. The nonlinear angle relation between two non-circular gears can be obtained by integrating the transmission ratio. Therefore, the method of noncircular gear transmission ratio construction based on the Fourier series is adopted to obtain the analytical expression of the Angle relation. It is easy to satisfy the closure condition of the pitch curve of non-circular gear.

82

D. Liu and J. Lu

Making the polar angle of driving non-circular gear as the independent variable, the transmission ratio function of non-circular gear is constructed as follows: ∞

i21 =

ω2 1  = + [an cos(nn1 ϕ1 ) + bn sin(nn1 ϕ1 )] ω1 ij

(1)

n=1

where i21 is the angular velocity ratio of driven non-circular gear to the driving one. ω1 and ω2 are angular velocities of the driving and driven non-circular gear respectively. n1 and n2 are the orders of the driving and driven non-circular gear. ϕ1 is the polar angle of the driving non-circular gear. ij is the ratio of the order of the driven non-circular gear to that of the driving non-circular gear, with ij = n2 /n1 , n is the number of terms of Fourier series. an and bn is the coefficients of each term respectively. According to Eq. (1), the expressions of the rotation angle and radius vector of non-circular gears can be derived: ⎧ Ai21 ⎪ ⎪ ⎪ r1 = ⎪ ⎪ 1 + i21 ⎪ ⎪ ⎪ ⎨ A r2 = (2) 1 + i21 ⎪ ⎪   ⎪ ∞ ⎪ ⎪ ϕ1  an sin(nn1 ϕ1 ) bn cos(nn1 ϕ1 ) bn ⎪ ⎪ ϕ = + − + 2 ⎪ ⎩ ij nn1 nn1 nn1 n=1

where r 1 and r 2 are the radius vector of the driving and driven non-circular gears respectively. ϕ2 is the polar angle of driven non-circular gear. A is the center distance of two non-circular gears. When the non-circular gear rotates from the position with a polar angle of 0, the rotation angle and polar angle of the non-circular gear are equal. In the following descriptions, it is considered that the rotation angle and the polar angle of the non-circular gear are equal. According to the definition of the intersecting angle ϕ between the driven noncircular gear and the eccentric block, the expression of the phase angle of the eccentric block can be obtained as follows: ϕ0 = ϕ2 + ϕ

(3)

For inertial robots, the input angular velocity of the system is known, so the angle ϕ1 of the driving non-circular gear can be expressed as a linear function of time t. Through the expression of ϕ2 in Eq. (2), the expression of ϕ0 can be deduced with independent variable t. Then the kinematic model of eccentric block can be expressed by analytical function, which is the basis of dynamical modeling for the inertial robot. Thus, Fourier series is used to construct non-circular gear transmission ratio. 3.2 Dynamic Equation In Fig. 1 the eccentric block is simplified into a particle of m0 /2 and a massless connecting rod of length R. In the x-axis direction, the displacements of the centroids of the eccentric

Dynamic Characteristics of Two-Mass Inertial Pipeline Robot Driven

83

block, the internal vibrator, and the outer shell are x 0 , x 1 , and x 2 respectively. The relationship between x0 and x1 can be represented as follows: x0 = R sin ϕ0 + x1

(4)

Taking the second derivative of x 0 concerning t could result in the acceleration of eccentric block in x-direction as the following equation: x¨ 0 = Rϕ¨0 cos ϕ0 − Rϕ˙02 sin ϕ0 + x¨ 1

(5)

The force analysis of eccentric block, internal oscillator, and outer shell is carried out as shown in Fig. 3. Where F 10 (F 01 ) is the force of the internal oscillator (eccentric block) on eccentric block (internal oscillator) in x-direction, F 12 (F 21 ) is the force of the internal oscillator (outer shell) on the outer shell (internal oscillator) in the x-direction and F f is the friction force between the pipe and the robot. m0/2 y

F10

x

F21

F01 m1

(a)

y

Ff

F12

y

x

(b)

x m (c)

Fig. 3. Force analysis diagram of pipeline robot

According to the force analysis of eccentric block in Fig. 3 (a), the differential equation of motion of the two eccentric blocks in the x-direction can be obtained: m0 x¨ 0 = −F10

(6)

According to the force analysis of the internal oscillator in Fig. 3 (b), the differential equation of motion of the internal oscillator in the x-direction can be obtained: m1 x¨ 1 = F01 − F21 where F 01 and F 21 satisfy the following equation: F01 = F10 F21 = k1 (x1 − x2 ) + c1 (˙x1 − x˙ 2 )

(7)

(8)

where k 1 and c1 are the spring stiffness and damping between the internal oscillator and the external shell. By combining Eqs. (5)–(8), the dynamic equation of the internal oscillator can be obtained: (m1 + m0 )¨x1 = −m0 (Rϕ¨0 cos ϕ0 − Rϕ˙02 sin ϕ0 ) − k1 (x1 − x2 ) + c1 (˙x1 − x˙ 2 )

(9)

84

D. Liu and J. Lu

where ϕ0 is a function of time t. According to the force analysis of the outer shell in Fig. 3 (c), the differential equation of motion of the outer shell can be obtained: m¨x2 = F12 − Ff

(10)

where F 12 and F f satisfy the following equation: ⎧ F12 = F21 ⎪ ⎪ ⎧ ⎪ ⎨ ⎪ ⎨ sgn(˙x2 )f ⎪ Ff = sgn(F12 )F12 ⎪ ⎪ ⎪ ⎩ ⎩ sgn(F12 )f

x˙ 2 = 0 x˙ 2 = 0 and f > |F12 | x˙ 2 = 0 and f ≤ |F12 |

(11)

where f is the Coulomb’s friction force between the robot and the pipe. It is assumed that the maximum static friction force is equal to the sliding friction force. Equations (9) and (10) are the two degrees of freedom dynamic models of the two-mass inertial pipeline robot in Fig. 1, where the generalized coordinates are x 1 and x 2 .

4 Simulation Results and Discussion 4.1 System Parameters and Motion Characteristics Based on the dynamics model of the two-mass pipeline robot proposed above, the mobile performance of the robot is simulated. Because the velocity of the pipeline robot is periodic, the average velocity va in the whole cycle is used to measure the performance of the robot. In the simulation example, the initial design parameters of the pipeline robot are shown in Table 1. The orders of the non-circular gears are 1, which means that ij = 1. We take the first term in Eq. (1) as the transmission ratio of the non-circular gears in the robot and let b1 = 0. Table 1. System parameters of pipeline robot Parameters

Value

Eccentric blocks mass m0 /(kg)

0.3

Ratio of internal vibrator mass to eccentric blocks mass δ1

3

Ratio of outer shell mass to eccentric blocks mass δ2

1

Eccentricity of eccentric blocks R/(m)

0.0135

Ratio coefficient of non-circular gear a1

0.3

Motor speed ω1 (rad/s)

1000

Connecting spring stiffness k 1 /(N/m)

2 × 104

Connection damping c1 /(Ns/m)

10

Coefficient of sliding friction μ

0.15

supporting force N/(N)

200

Dynamic Characteristics of Two-Mass Inertial Pipeline Robot Driven

85

Displacement of housing x/(m)

Based on the numerical method, the dynamic responses of the pipeline robot are calculated. Figure 4 shows the displacement curve of the pipeline robot. It can be seen from the figure that the robot can realize one-way step movement, and there will be a small amount of backward phenomenon in each movement cycle. From the speed curve in Fig. 5, the speed of the robot changes periodically and the speed has a negative value. Therefore, the robot will retreat in each movement cycle. In Fig. 5, the average velocity va of the robot is 0.0954 m/s. 0.19 0.18 0.17 0.16 0.15 0.1 0.13 1.5

1.6

1.7 1.8 Time t/(s)

1.9

2

Speed of housing v2/(m/s)

Fig. 4. Displacement curve of robot

0.6

average velocity va

0.0954m/s

0.4 0.2 0 1.05

1.1

1.15

1.2

Time t/(s) Fig. 5. Velocity curve of robot

4.2 Influence of System Parameters on Average Speed To improve the mobile performance of the robot, the influence of the main parameters of the robot on average velocity va is studied. The control variable method is introduced to research the effect of the stiffness of the connecting spring k 1 , eccentricity a1 , and the motor speed n1 on the velocity of the robot. Influence of Spring Stiffness on Average Speed of Robot When the stiffness of the connecting spring k 1 rises from 1 × 104 to 10 × 104 , the average velocity va curve of the robot is obtained, as shown in Fig. 6. When the stiffness

86

D. Liu and J. Lu

Average speed va /(m/s)

of the connecting spring k 1 changes from 1 × 104 to 5 × 104 , the average velocity va takes on a tendency to decrease first and then increase. As the stiffness of the connecting spring k 1 is in the interval 5 × 104 to 10 × 104 , the rule of the average velocity va is similar to that of the interval 1 × 104 to 5 × 104 , but the maximum va of each interval decreases with k 1 . 0.2 (11840,0.2064) 0.14

(10000,0.1528) (41220,0.0522)

0.08 0.02 1×104

5×104 Spring stiffness k1/(N/m)

10×104

Fig. 6. The influence of spring stiffness on average moving speed of robots

acting force

In the interval 1 × 104 to 5 × 104 , the force between internal oscillator and housing is calculated when k 1 is 10000, 11840 and 41220 respectively which is the particular value marked in Fig. 6, as shown in Fig. 7. When the stiffness of spring increases from 10000 to 11840, since the inertial force is the force generated by the forward motion of the robot shell to overcome the external friction, the average moving speed of the robot increases with the increase of k 1 . When the stiffness of the spring increases to 41220, the vibration frequency of the inner vibrator decreases, and the reverse inertial force of the internal oscillator in the outer shell increases. Therefore, the retrogression of the robot is aggravated, which leads to the decrease of the robot’s moving speed. The large spring stiffness results in the increase of the opposite inertia force of the internal oscillator, and then robot retrogression occurs frequently, which is the reason for the decrease of average velocity va . 50 0 -50 1 50 0 -50 1 50 0 -50 1

(a) 1.5

2

2.5 (b)

1.5

2

2.5 (c)

2

2.5

1.5

Time

(a) k1=10000;(b) k1=11840;(c) k1=41220 Fig. 7. Relative forces with different k 1

Dynamic Characteristics of Two-Mass Inertial Pipeline Robot Driven

87

From the above analysis, va first increases and then decreases with the increase of k 1 . It is suggested that increasing the spring stiffness in a certain speed range may reduce the average velocity va of the robot. Hence, we cannot improve the average velocity va of the robot by increasing the spring stiffness continuously.

Average speed va/(m/s)

Influence of Spring Stiffness on Average Speed of Robot Considering the actual machining of non-circular gear, non-circular gear eccentricity a1 is limited in the interval 0 to 0.7. Keeping other parameters in Table 1 unchanged, the curve of the average velocity va of the robot with a1 is obtained, as shown in Fig. 8. 0.16

0.3

0.12

0.6

0.08 0.04 0

0 0

0.4 0.6 0.2 Eccentricity of non-circular gear

0.8

Fig. 8. Influence of non-circular gear eccentricity on average moving speed of robot

When non-circular gear eccentricity a1 increases from 0 to 0.7, the average velocity va first rises from 0 to 0.1729 m/s and then falls to 0.141 m/s. When eccentricity a1 equals 0, 0.3, and 0.6 respectively, the force between the internal oscillator and housing is calculated to research the variation of the average velocity va with eccentricity a1 , as shown in Fig. 9. As eccentricity a1 starts to increase, the eccentric block rotates with variable speed, which results from the asymmetrical inertia force excited by noncircular gears. The positive inertial force is greater than the opposite inertial force, which improves the average velocity va of the robot. As eccentric ratio a1 starts to increase, the eccentric block rotates with variable speed, which results from the asymmetrical inertia force excited by non-circular gears. The positive inertial force is greater than the opposite inertial force, which improves the average velocity va of the robot. When the eccentricity a1 continues to increase, the α decreases. The negative work growth rate of the inertial force is gradually greater than the positive work growth rate, with the robot recoils more intensively, the speed of movement slows down. Therefore, the appropriate value of eccentricity a1 should be taken in the design. Influence of Motor Speed on Average Speed of Robot The curve of average velocity va with speed n1 is obtained by rising speed of motor n1 from 100 r/min to 3000 r/min, which is shown in Fig. 10. The average velocity va of the robot first increases and then decreases.

88

D. Liu and J. Lu

(a)

50 Acting force F12/N

0 -50 50 0 -50 50 0 -50

1

1.5

2

2.5 (b)

1

1.5

2

2.5 (c)

1

1.5

2.5

2 Time t/s (a) a1=0; (b) a1=0.3; (b) a1=0.6

Average speed va/(m/s)

Fig. 9. Relative forces with different a1

0.3

140

0.2 0.1

280

0 -0.1

0

50

1000 1500 2000 2500 3000 Motor speed n1(r/min)

Fig. 10. The influence of motor speed on average moving speed of robot

Figure 11 shows the force between the inner vibrator and outer shell when speed n1 is 1400 r/min and 2800 r/min respectively. When speed n1 is smaller than 410 r/min, the inertia force excited by the eccentric block is not enough to overcome the friction between the outer shell and the inner wall of the pipe, which keeps the robot still. When speed n1 rises from 410 r/min to 1406 r/min, the average velocity va of the robot will increase, which enhances the exciting force of the eccentric block gradually. In the same friction environment, the work done by the forward inertial force is greater than that by the backward inertial force. However, when the speed n1 of the motor is greater than 1406 r/min, the work done by the backward inertial force increases gradually with the increase of speed n1 , which leads to the decrease of the average velocity va and even causes the robot to retreat slightly.

Acting force F12/N

Dynamic Characteristics of Two-Mass Inertial Pipeline Robot Driven

100 50 0 -50

89

(a)

1.5

1

200

Time

2

2.5 (b)

0 -200 0.4

0.6

0.8 Time (a) n1=1400r/min;(b) n1=2800r/min

1

Fig. 11. Relative forces with different n1

5 Conclusion (1) Considering small diameter pipes, a two-mass impact pipeline robot driven by a non-circular gear is proposed to combine a non-circular gear excitation mechanism with a two-mass mobile robot. (2) With the increase of spring stiffness k 1 , the moving speed of the robot decreases in a fluctuating manner. In the local stiffness region, the moving speed of the robot increases first and then decreases. Therefore, when the spring stiffness range is fixed, the appropriate spring stiffness can be selected to improve the robot’s moving speed. (3) Increasing the a1 of the non-circular gear can increase the inertia force, thus improving the moving speed of the robot. The larger eccentricity can be selected appropriately in the range allowed by the geometrical conditions of non-circular gears. (4) The mobile speed of the robot increases first and then decreases with the increase of n1 . In a certain range, increasing the motor speed can significantly improve the moving speed of the robot.

Acknowledgment. Innovative ability training for graduate students supported by Hebei Provincial Department of Education (CXZZSS2021057).

References 1. Li, Z.: Research on micro pipe robot with adjustable supporting mechanism based on piezoelectric inertial impact drive. East China University of Science and Technology (2016) 2. Li, T., Ma, S.G., Li, B.: Adaptive motion mechanism and mechanism design of screw driven in pipe robot. Chin. J. Mech. Eng. 52(9), 9–17 (2016) 3. Provatidis, C.G.: A study of the mechanics of an oscillating mechanism. Int. J. Mech. Sci. 5(4), 263–274 (2011) 4. Zimmermann, K., Zeidis, I., Bolotnik, N.: Dynamics of a two-module vibration-driven system moving along a rough horizontal plane. Multibody Syst. Dyn. 22(2), 199–219 (2009) 5. Muscia, R.: Performance improvement of a vibration driven system for marine vessels. Multibody Syst. Dyn. 36(2), 169–194 (2016)

90

D. Liu and J. Lu

6. Chernous’ko, F.L.: Analysis and optimization of the rectilinear motion of a two-body system. J. Appl. Math. Mech. 75(5), 493–500 (2011) 7. Chernous’ko, F.L.: On the optimal motion of a body with an internal mass in a resistive medium. J. Vibr. Control, 14(1–2), 197–208 (2008) 8. Yegorov, A.G., Zakharova, O.S.: The energy-optimal motion of a vibration-driven robot in a resistive medium. J. Appl. Math. Mech. 74(2), 443–451 (2010) 9. Fang, H.B., Xu, J.: Dynamics of a mobile system with an internal acceleration-controlled mass in a resistive medium. J. Sound Vibr. 330(16), 4002–4018 (2011) 10. Fang, H.B., Xu, J.: Dynamics of a three-module vibration-driven system with non-symmetric Coulomb’s dry friction. Multibody Syst. Dyn. 27(4), 455–485 (2012) 11. Chen, Q., Xu, J.: Locomotion of two vibration-driven modules connected by a mechanical position limiter. Int. J. Mech. Sci. 137, 252–262 (2018) 12. Liu, Y., Pavlovskaia, E., Wiercigroch, M.: Forward and backward motion control of a vibroimpact capsule system. Int. J. Non-Linear Mech. 70, 30–46 (2015)

Kinematic Performance Analysis and Optimization of a 3-DOF Parallel Mechanism for Ankle Rehabilitation Shuai Yang1,2 , Chenglei Liu1,2 , Xiaohui Wang1,2 , and Jianjun Zhang1,2(B) 1 School of Mechanical Engineering, Hebei University of Technology, Tianjin 300401, China

[email protected] 2 Key Laboratory of Robot Perception and Human-Machine Fusion, Hebei Province,

Tianjin 300130, China

Abstract. The rotation center of the existing ankle rehabilitation robot is inconsistent with the actual rotation center of the human ankle, which will influence the rehabilitation effect. This paper proposes a 3-DOF parallel mechanism for ankle rehabilitation based on a UR equivalent model with respect to the physiological structure and motion characteristics of the human ankle. The inverse kinematics of the mechanism is established in terms of the analytical method. The workspaces of the mechanism are obtained by the search method and compared with the motion range of the human ankle, which shows that the mechanism could meet the motion demand of ankle rehabilitation. To make the mechanism have an excellent kinematic performance within the rehabilitation motion range, the ratio of the high-quality transmission workspace is greater than 0.5 as the optimization goal, the parameters of the mechanism are optimized based on the motion/force transmissibility. The results show that the mechanism can meet the motion requirements of ankle rehabilitation and have an excellent kinematic performance in its rehabilitation motion range. Keywords: Ankle rehabilitation · Parallel mechanism · Kinematic performance analysis · Parameter optimization

1 Introduction As one of the three joints of the human lower extremity, the ankle is the primary loading weight joint and the hub that contacts the ground. Ankle injuries are the most common sports injury and can lead to traumatic arthritis and joint instability if left untreated [1]. The traditional treatment of ankle injury highly depends on the one-by-one physical therapy by the doctor, which requires a lot of human resources and heavy labor intensity, and the rehabilitation physician cannot objectively evaluate the rehabilitation status. However, machine-assisted treatment can effectively improve the rehabilitation effect compared to traditional treatment. At present, many researchers have carried out research on ankle rehabilitation robots. Girone [2] developed a 6-DOF ankle rehabilitation robot based on the Stewart platform, © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 91–102, 2021. https://doi.org/10.1007/978-3-030-89092-6_9

92

S. Yang et al.

which has redundant DOFs and difficult control. Dai [3] proposed a 3-UPS/U (U, P and S stand for universal, prismatic, and spherical pair, respectively) parallel mechanism for ankle rehabilitation, which has insufficient DOFs. Malosio [4, 5] designed a spherical ankle rehabilitation robot with a remote center of rotation based on the 3-RRR (R stands for revolute pair). Li [6–8] presented 3-RRS, 2-UPS/RRR and 3-UPS/RRR parallel mechanisms for ankle rehabilitation. These three mechanisms all equated the ankle with an S pair, and all had a remote rotation center. Fang [9] also equivalent the ankle to S pair and proposed a rope-driven ankle rehabilitation robot. Zhao [10, 11] developed a biological fusion ankle rehabilitation mechanism that used the motion characteristics of the ankle to constrain the movement freedom degree of the mechanism. Chen [12] equated the ankle with a spatial RR model and proposed an ankle rehabilitation mechanism based on 3-UPU. To improve the fitting accuracy of human-machine motion, this paper equates the ankle with the UR model based on the ankle anatomy and motion characteristics and proposes a novel ankle rehabilitation mechanism. In Sect. 2, the UR model is selected with respect to the physiological structure and motion characteristics of the ankle, and the mechanism is proposed. In Sect. 3, the inverse kinematic of the mechanism are analyzed and the workspace of the mechanism is compared with the motion range of the ankle. In Sect. 4, the parameters of the mechanism are optimized. The conclusions are drawn in Sect. 5.

2 Ankle Equivalent Model and Mechanism Design 2.1 Ankle Structure and Equivalent Model As shown in Fig. 1(a), the ankle is a highly adaptive multi-axis joint, mainly composed of the tibiotalar joint and the subtalar joint. The tibiotalar joint is composed of the tibia, fibula and talus, and the subtalar joint is composed of the talus and calcaneus. Both the tibiotalar joint and the subtalar joint collectively bear the weight and movement of the lower extremity. The main motion forms of the ankle include dorsiflexion/plantar flexion, inversion/eversion, adduction/abduction. The axes of motion of the ankle are not the fixed axes and have no confluence but changes constantly with the movement of the ankle [13].

Tibia

Fibula

Tibiotalar joint

Talus Subtalar joint

Tibiotalar joint

Navicula

Subtalar joint

Calcaneus

(a)

(b)

Fig. 1. (a) The structure of ankle (b) The equivalent model of ankle

Kinematic Performance Analysis and Optimization of a 3-DOF Parallel Mechanism

93

At present, scholars in the field of ankle rehabilitation generally consider that the equivalent model of the ankle is the spatial RR model [14] or S pair [15]. The two rotation axes of the spatial RR model correspond to the rotation axes of the tibiotalar joint and the subtalar joint of the ankle, but it ignores the motion forms of the ankle. The S pair considers the motion forms of the ankle and ignores the real structure of the ankle. Zhang [16] proposed a series of ankle equivalent models with respect to the physiological structure and motion characteristics of the ankle. Among them, the UR equivalent model equated the tibiotalar joint with the U pair and the subtalar joint with the R pair. The distance from the geometric center of the U pair to the R pair was the size of the human talus, as shown in Fig. 1(b). 2.2 Structure of the Ankle Rehabilitation Mechanism Figure 2 shows the structure of the 2-UPU/[RR][RRR]/PRPS parallel mechanism for ankle rehabilitation based on the UR equivalent model. The mechanism has two rotation centers, namely fixed spherical center O which is equivalent to the rotation center of the tibiotalar joint, and moving spherical center O1 which is equivalent to the rotation center of the subtalar joint. Point O is located at the intersection of the rotation axes on the fixed platform and point O1 is located at the intersection point of the rotation axes on the moving platform. The line between point O and point O1 is called the “doublecentered line”. The mechanism has four limbs between the fixed platform and the moving platform. Limb 1 and limb 2 are the UPU limbs, and the rotation axes connected to the fixed platform are perpendicular to each other and intersect at point O. The rotation axes connected to the moving platform intersect at point O1 . The rotation axes connected to the P pair are parallel to each other. Limb 3 is the [RR][RRR] limb in which the first two rotation axes intersect at the point O and the remaining rotation axes intersect at the point O1 . Limb 4 is an unconstraint PRPS limb in which the axis of the R pair coincides with the direction of the guide rail. The P pairs of limb 1 and limb 2 and the slider of limb 4 are the drives.

Fixed platform Limb 3

Limb 1

Limb 4

Limb 2

Moving platform

Fig. 2. The 3D CAD model

Fig. 3. The kinematic structure

94

S. Yang et al.

The number of degree of freedoms for the parallel mechanism can be obtained by the modified G-K formula [17] F = d (n − g − 1) +

g 

fi + ν − ζ = 6(13 − 15 − 1) + 21 + 0 − 0 = 3

(1)

i=1

3 Kinematic Analysis 3.1 Inverse Kinematic Analysis As shown in Fig. 3, point Ai (i = 1, 2) is the center of the U pair connected to the fixed platform in the ith limb. Point Bi (i = 1, 2) is the center of the U pair connected to the moving platform in the ith limb. Points A3 and A3  are the centers of the R pairs connected to the fixed platform in limb 3. Point E 3 is the center of the R pair connected to the moving platform in limb 3. Point H is the center of the S pair in limb 4. Point M is the center of the guide rail. Let the distance from point O to point A1 is. The distance from point O to point A2 is. The distance from point O1 to point B1 is. The distance from point O1 to point B2 is. The length of the double-centered line is l. The distance from point O1 to point E 3 is. The distance from point E 3 to point H is lEH . The angle between the axis of the U pair connected to the moving platform and the moving platform is ϕ. lOA1 = lOA2 , lO1 B1 = lO1 B2 . The rotation transformation matrix can be expressed as ⎡ ⎤ CβCγ SαSβCγ − CαSγ CαSβCγ + SαSγ O ⎣ CβSγ SαSβSγ + CαCγ CαSβSγ − SαCγ ⎦ (2) O1 R(α, β, γ ) = −Sβ SαCβ CαCβ where, S and C present sine and cosine, respectively. Let the normal vectors of plane OA1 B1 O1 and plane OA2 B2 O1 be n1 and n2 , respectively   n1 · OA1 = 0 n2 · OA2 = 0 , n2 : (3) n1 : n1 · O1 B1 = 0 n2 · O1 B2 = 0 The intersection line of plane OA1 B1 O1 and plane OA2 B2 O1 is the straight line where the double-centered line OO1 is located with respect to the structural characteristics of the mechanism. Thus, the double-centered line OO1 can be obtained as  OO1 · n1 = 0 OO1 : (4) OO1 · n2 = 0 Then the coordinate of the origin of the moving coordinate system in the fixed coordinate system can be obtained as follow O1 = (aO1 , bO1 , cO1 )T

(5)

Kinematic Performance Analysis and Optimization of a 3-DOF Parallel Mechanism

95

The coordinates of B1 , B2 and H in the moving coordinate system can be written as b1 = (

lO1 B1 lO1 B1 l O B lO B , , −lO1 B1 Sϕ)T , b2 = (− 1 2 , 1 2 , −lO1 B2 Sϕ)T , h = (0, lEH , −lO1 E3 )T 2 2 2 2

(6)

The coordinates of B1 , B2 and H in the fixed coordinate system can be expressed as O O B1 = OO1 + O O1 R(α, β, γ )b1 , B2 = OO1 + O1 R(α, β, γ )b2 , H = OO1 + O1 R(α, β, γ )h

(7) Then, the lengths of limb 1 and limb 2, l1 and l2 can be obtained as l1 = |A1 B1 |, l2 = |A2 B2 |

(8)

The slider movement distance l3 can be obtained as l3 = MH · X

(9)

3.2 The Workspace Analysis Workspace is a necessary condition to measure whether the mechanism can meet the rehabilitation requirements of the human ankle. In this paper, the orientational workspace, moving spherical center and moving platform workspaces of the parallel mechanism for ankle rehabilitation are solved. The structural parameters of the mechanism are shown in Table 1. Table 1. The structural parameters of the mechanism Parameters

Number

lOA1

275 mm

lO1 B1

180 mm

lO1 E3

180 mm

l

30 mm

ϕ

45º

Under the condition that the moving distances of the P pairs in limb 1 and limb 2 are limited to ±100 mm and the rotation ranges of the U pair are ±45º, the workspaces of the mechanism are solved by using the search method with the help of MATLAB. The results are shown in Fig. 4. According to the literature [18], the motion range of the human ankle and the motion range of the mechanism are listed in Table 2. It can be seen from the data in Table 2 that the workspace of the mechanism fully meets the motion range requirements of the human ankle. So, the parallel mechanism for ankle rehabilitation can complete the rehabilitation exercise of the ankle.

96

S. Yang et al. Table 2. The motion range of human ankle and the motion range of mechanism

Movement type

The motion range of human ankle/° [18]

The motion range of mechanism/°

Dorsiflexion

20.3–29.8

34

Plantarflexion

37.6–45.7

51

Inversion

14.5–22.0

45

10.0–17.0

45

Adduction

22.0–36.0

76

Abduction

15.4–25.9

76

γ/( )

β/( )

Eversion

α/( )

α/( )

(a)

Z/mm

Z/mm

(b)

(c)

(d)

Fig. 4. (a) Projection of the orientational workspace onto the XY plane; (b) Projection of the orientational workspace onto the XZ plane; (c) The workspace of moving spherical center; (d) The workspace of moving platform.

4 Performance Analysis and Optimization At present, the performance evaluation indexes applied to parallel mechanisms mainly include the condition number of the Jacobian matrix [19], dexterity [20], and motion/force transmissibility [21]. Based on the screw theory, the motion/force transmissibility reflects the transmission efficiency of the motion and force of the parallel mechanism from input to output. In this paper, motion/force transmissibility is used as the evaluation standard of the kinematic performance of the ankle rehabilitation parallel mechanism, and the parameters are optimized based on this index.

Kinematic Performance Analysis and Optimization of a 3-DOF Parallel Mechanism

97

4.1 The Motion/Force Transmissibility The motion/force transmissibility can be divided into input transmissibility and output transmissibility with respect to transfer objects between the input and output of the parallel mechanism. The input and output transmissibility of a single limb of the parallel mechanism can be expressed as     $Ti ◦ $Ii  $Ti ◦ $Oi      λi = , ηi = (10) $Ti ◦ $Ii  $Ti ◦ $Oi  max

max

where λi is input transmissibility, ηi is output transmissibility, $Ii is the input screw of the ith driving limb, $Ti is the transmission force screw of the ith driving limb, $Oi is the output screw of the ith driving limb. Both λi and ηi are within the range of [0, 1]. To ensure that each limb has excellent input and output transmissibility, the local transmission index (LTI) of the parallel mechanism is defined as

= min{λi , ηi }

(11)

where Γ is a dimensionless index independent of the coordinate system. The closer Γ is to 1, the better the motion/force transmissibility of the mechanism. The closer Γ is to 0, the closer the mechanism is to the singularity. When the driving joint of a limb is locked, a constraint screw that is reciprocity to all motion screws except the driving screw will be added. This screw is called the transmission force screw. Then the transmission force screws can be obtained as $T 1 = (A1 B1 ; rA1 × A1 B1 ), $T 2 = (A2 B2 ; rA2 × A2 B2 ), $T 3 = (X; rH × X)

(12)

According to the reciprocity of the motion screw and the force screw, the output motion screw of each limb can be obtained as  $Tj ◦ $Oi = 0 i, j = 1, 2, 3 i = j (13) $Cj ◦ $Oi = 0 The LTI of the mechanism can be obtained by putting the output motion screws into Eqs. (10)–(11)). The distribution diagrams of LTI of the mechanisms in pure dorsiflexion/plantarflexion (α = 0º), inversion/eversion (β = 0º) and adduction/abduction (γ = 0º) are drawn, respectively, as shown in Fig. 5. It can be seen from the figures that the LTI of the mechanism in the central area of the workspace is greater than 0.7, and its value decreases progressively as it leans on the boundary of the workspace. There is no Γ = 0 position in the target workspace, which indicates that the mechanism does not have a singular configuration in the workspace. 4.2 Parameter Optimization From the above analysis, LTI can only reflect the motion/force transmissibility of the mechanism in a particular configuration, but not the motion/force transmissibility of the mechanism in the whole workspace. So, the global motion/force transmissibility ζ is

S. Yang et al.

γ/( )

98

β/( )

β/( )

γ/( )

(a)

α/( )

(b)

α/( )

(c)

Fig. 5. (a) Distribution diagram of LTI when α = 0º; (b) Distribution diagram of LTI when β = 0º; (c) Distribution diagram of LTI when γ = 0º.

introduced. When G ≥ 0.7, the configuration sets of the mechanism are defined as the high-quality transfer workspace. The ratio of the high-quality transfer workspace to the whole workspace is the global motion/force transmissibility ζ dW (14) ζ = SG dW S where W is the whole workspace, SG is the volume of the high-quality transfer workspace, S is the volume of the whole workspace, ζ is within the range of [0, 1]. The closer ζ is to 1, the better the kinematic performance of the mechanism. Substituting the structural parameters in the previous section into Eq. (14), the global motion/force transmissibility of the mechanism is obtained to be 0.33. It shows that the motion/force transmissibility of the mechanism is not excellent under such structural parameters. So further optimization is necessary. In this paper, the space model method is used to optimize three structural parameters: the distance from the point O to the point AOA , l OA ; the distance from the point O1 to the point Bi , l O1B ; the distance from the point H to the point E 3 , l EH . Firstly, the structural parameters are dimensionless treated as follows ⎧ lOA + lO1 B + lEH ⎪ ⎨D = 3 (15) ⎪ ⎩ r = lOA , r = lO1 B , r = lEH 1 2 3 D D D where D is the normalized factor, r i is the dimensionless parameter of the three optimization parameters.

Kinematic Performance Analysis and Optimization of a 3-DOF Parallel Mechanism

99

In consideration of the interference between the various components of the mechanism and the stroke of the push link, the dimensionless parameters shall meet the following conditions ⎧ ⎪ ⎨ r 1 + r2 + r3 = 3 r3 ≤ r2 (16) ⎪ ⎩ r2 ≤ r1 According to Eq. (16), the parameter optimization region of the mechanism can be obtained, as shown in Fig. 6(a). It is projected onto the s-t coordinates to obtain Fig. 6(b), thereby the three-dimensional space is reduced to the two-dimensional space to reduce the optimization parameters. Its mapping relationship can be obtained as follows ⎧ √ ⎪ 3 6 ⎪ ⎪ r1 = − s+t+ ⎪ ⎪ 2 ⎪ ⎪ √6 ⎨ 3 6 (17) s−t+ r2 = − ⎪ 6 2 ⎪ ⎪ √ ⎪ ⎪ ⎪ ⎪ ⎩ r3 = 6 s 3

r3

r1

0 t

3

1

3

A

1.5

1.5

r2 C

1 3

3

B A

r1

r2

(a)

B

(b)

C

s

Fig. 6. (a) 3D optimization space; (b) 2D optimization space

MATLAB is used to conduct an iterative search on the entire s-t region in Fig. 6(b). The global motion/force transmissibility of the corresponding size is calculated, and the performance graphs are drawn, as shown in Fig. 7(a). It can be seen from the figure that the global motion/force transmissibility of the mechanism in the range of 0.6 ≤ s ≤ 1.22 and 0 ≤ t ≤ 0.2 is greater than 0.5. It shows that the parameters of the mechanism have an excellent kinematic performance within this range. Considering the rationality of the layout between the links of the mechanism and the workspace, s = 0.8, t = 0.2, D = 200 mm are selected. The corresponding mechanism parameters are lOA = D·r 1 ≈ 255 mm, and lEH = D·r 3 ≈ 131 mm. Then the parameters are put into Eqs. (10)–(11), and the distribution diagrams of its LTI are drawn, as shown in Fig. 7. By compared the distribution diagrams of LTI of the mechanism before optimization, the area of the high-quality transfer workspace after optimization is significantly

S. Yang et al.

γ/(º)

100

β/(º)

(b)

γ/(º)

β/(º)

(a)

α/(º)

(c)

α/(º)

(d)

Fig. 7. (a) The performance graph; (b) The distribution diagram of LTI of the optimized mechanism when α = 0º; (c) The distribution diagram of LTI of the optimized mechanism when β = 0º; (d) The distribution diagram of LTI of the optimized mechanism when γ = 0º.

increased, and the ratio of high-quality transmission workspace after optimization reaches 0.59. It indicates that the kinematic performance of the optimized mechanism has been significantly improved.

5 Conclusions In this paper, a 2-UPU/[RR][RRR]/PRPS parallel mechanism for ankle rehabilitation has been proposed, and its kinematics performance has been analyzed and optimized. The conclusions are as follows: (1) UR equivalent model has been selected as the equivalent model of the human ankle with respect to the physiological structure and motion characteristics of the human ankle and a 3-DOF 2-UPU/[RR][RRR]/PRPS parallel mechanism for ankle rehabilitation has been proposed. (2) The inverse kinematics of the mechanism has been solved by the analytical method. Comparing the workspace of the mechanism with the motion range of the human ankle showed that the mechanism can meet the rehabilitation motion requirements of the ankle. (3) Based on the motion/force transmissibility and space model method, the parameters of the mechanism have been optimized. After optimization, the ratio of high-quality transfer workspace reached 0.59, which indicated that the mechanism has an excellent kinematic performance in the ankle rehabilitation motion range.

Kinematic Performance Analysis and Optimization of a 3-DOF Parallel Mechanism

101

So, the 2-UPU/[RR][RRR]/PRPS parallel mechanism can be used for human ankle rehabilitation. Acknowledgments. This work is supported by the National Natural Science Foundation of China (Grant No.52075145), Major Scientific and Technological Achievements Transformation Project of Hebei Province (Grant No.20281805Z), and the Central Government Guides Local Science and Technology Development Funds (Grant No.206Z1801G).

References 1. Yao, T.S., Meng, X.J.: Ankle Surgery. China Press of Traditional Chinese Medicine, Beijing (1998) 2. Girone, M., Burdea, G., Bouzit, M.: A stewart platform-based system for ankle telerehabilitation. Auton. Robot. 10(2), 203–212 (2001) 3. Saglia, J.A., Tsagarakis, N.G., Dai, J.S.: Control strategies for patient-assisted training using the ankle rehabilitation robot (ARBOT). IEEE/ASME Trans. Mechatron. 18(6), 1799–1808 (2013) 4. Malosio, M., Negri, S.P., Pedrocchi, N.: A spherical parallel three degrees-of-freedom robot for ankle-foot neuro-rehabilitation. In: 2012 Annual International Conference of the IEEE Engineering in Medicine and Biology Society, pp. 3356–3359. IEEE, San Diego (2012) 5. Malosio, M., Caimmi, M., Ometto, M.: Ergonomics and kinematic compatibility of PKankle, a fully-parallel spherical robot for ankle-foot rehabilitation. In: 5th IEEE RAS/EMBS International Conference on Biomedical Robotics and Biomechatronics, pp. 497–503. IEEE, Sao Paulo (2014) 6. Li, D.S., Li, J.F., Wang, S.: 3-RRS Parallel ankle rehabilitation mechanism and motion analysis. Mach. Des. Manuf. 8, 4–8 (2015) 7. Li, J.F., Li, S.C., Tao, C.J.: Parallel 2-UPS/RRR ankle rehabilitation mechanism and sports performance analysis. Robot 38(02), 144–153 (2016) 8. Li, J.F., Xu, C.H., Tao, C.J.: Performance analysis of 3-UPS/RRR parallel ankle rehabilitation mechanism. Acta Automatica Sinica 42(12), 1794–1807 (2016) 9. Yu, R.T., Fang, Y.F., Guo, S.: Design and kinematic performance analysis of rope driven parallel ankle rehabilitation mechanism. Robot 37(01), 53–62 (2015) 10. Bian, H., Zhao, T.S., Tian, X.B.: Biological fusion rehabilitation institution and its application. Robot 32(04), 470–477 (2010) 11. Bian, H., Liu, Y.H., Zhao, T.S.: Mechanism and kinematics of parallel 2-RRR/UPRR ankle rehabilitation robot. Robot 32(01), 6–12 (2010) 12. Chen, Z.M., Yin, T., Pan, H.: A .3-DOF parallel ankle rehabilitation mechanism. J. Mech. Eng. 56(21), 70–78 (2020) 13. Mao, B.Y., Pang, Q.J., Dai, K.R.: Artificial Ankle Surgery. People’s Military Medical Publishing House, Beijing (2015) 14. Dul, J., Johnson, G.E.: A kinematic model of the human ankle. J. Biomed. Eng. 7(2), 137–143 (1985) 15. Wang, C.Z., Fang, Y.F., Guo, S.: Design and kinematic analysis of redundantly actuated parallel mechanism for ankle rehabilitation. Robotica 33(2), 366–384 (2015) 16. Liu, C.L., Zhang, J.J., Qi, K.C.: Type synthesis of generalized spherical parallel mechanisms for ankle rehabilitation. J. Mech. Eng. 56(19), 79–91 (2020) 17. Huang, Z., Liu, J.F., Li, Y.W.: On the DOF of Mechanism — Searching for the General Formula of DOF for 150 Years. Science Press, Beijing (2011)

102

S. Yang et al.

18. Siegler, S., Chen, J., Schneck, C.D.: The three-dimensional kinematics and flexibility characteristics of the human ankle and subtalar joints — part I: kinematics. J. Biomech. Eng. 110(4), 364–373 (1988) 19. Gosselin, C., Angeles, J.: The optimum kinematic design of a spherical three-degree-offreedom parallel manipulator. J. Mech. Des. 111(2), 202–207 (1989) 20. Stoughton, R.S., Arai, T.: A modified Stewart platform manipulator with improved dexterity. IEEE Trans. Robot. Autom. 9(2), 166–173 (1993) 21. Wang, J.S., Wu, C., Liu, X.J.: Performance evaluation of parallel manipulators: motion/force transmissibility and its index. Mech. Mach. Theory 45(10), 1462–1476 (2010)

Configuration Synthesis of Multi-mode Ankle Rehabilitation Robot Weihan Jia1,2 , Chenglei Liu1,2 , Jun Wei1,2 , and Jianjun Zhang1,2(B) 1 School of Mechanical Engineering, Hebei University of Technology, Tianjin 300130, China

[email protected] 2 Key Laboratory of Robot Perception and Human-Machine Fusion, Tianjin 300130,

Hebei Province, China

Abstract. The existing ankle rehabilitation robots are a mediocre specificity and detach from the human motion axis, in which the result of the ankle rehabilitation effect is not obvious. This paper investigates a kind of multi-mode parallel mechanisms, which accord with the movement requirements, with respect to the early-middle-late period of rehabilitation. First, based on the distribution characteristics of the axis of ankle motion, LBAs (limbs of the bio-syncretic axis) are given to satisfy the DOFs and anatomical characteristics of the ankle joint. An UPS limb is universally expressed as a muscle, which is transformed into an UPR limb that is connected with the screw theory of the LBA 1. Second, depending on the principle of human muscle movement, the equivalent models of inversion/eversion and abduction/adduction are obtained. We deduct five types of mechanisms based on these conditions, which is verified that the LBAs 1 and 3 mechanisms meet the movement requirements. Meanwhile, in order to realize the simple control, 32 types of driving limbs are configured in terms of the enumeration, and 128 types of mechanisms are obtained. The multi-mode ankle rehabilitation robot is adjusted to the corresponding rehabilitation mode according to the patient’s situation, which has a wider application scope, stronger specificity and simple control. Keywords: Ankle rehabilitation robot · Parallel mechanism · Multi-mode mechanism · Type synthesis

1 Introduction The ankle sprain is one of the most common symptoms of the ankle joint. Nevertheless, the postoperative recovery from ankle sprain is not ideal, resulting in ligament relaxation [1]. During the 6–12 weeks postoperative rehabilitation of the ankle joint, it is necessary to keep the movement of 1-DOF to ensure joint correction and avoid secondary injury as far as possible [2]. The multi-mode rehabilitation mechanism is a branch of reconfigurable mechanisms [3, 4]. However, reconfigurable rehabilitation ankle does not universally exist, which was first proposed by Yoon [5, 6]. The robot allowed ankle and foot movements, including toe, heel elevation and traditional ankle rotation. ZENG et al. [7] proposed a new reconfigurable parallel mechanism for ankle rehabilitation. It consisted of linear actuators, reconfigurable organization, connecting rod, rocker arm, and © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 103–114, 2021. https://doi.org/10.1007/978-3-030-89092-6_10

104

W. Jia et al.

the platform. Based on the mathematical model and motion trajectory of the ankle joint, LI [8] and MARIA [9] designed a reconfigurable ankle rehabilitation robot. NURAHMI et al. [10] proposed two-mode rehabilitation through a 3-RPS parallel manipulator and applied it to the rehabilitation of ankle joint. From the characteristics of the reconfigurable mechanism, we can use it to obtain high precision rehabilitation of ankle joint. Parallel mechanisms are used for most of the domestic ankle rehabilitation robots [11–13]. Several classes of reconfigurable mechanisms have been proposed, such as metamorphic mechanisms, kinematotropic mechanisms, mechanisms with variable topology and so on, leading to the study of reconfigurable mechanisms in a new and interesting territory [3]. The motion of the ankle sprains patient is not completely consistent with the state of movement of the uninjured ankle joint when considering the state of movement of the injured ankle joint. Therefore, the movement requirements are discussed in this paper. Movement requirement 1: Most ankle sprains are ligament injuries in clinical treatment. The dorsiflexion/plantarflexion of the ankle is accompanied by inversion/ eversion or abduction/adduction when entering the cavity. Once it touches the ground, the ankle joint will become excessive inversion/eversion [2]. Ankle-Foot orthosis is required at the beginning of rehabilitation when ankle injuries results joint instability [1]. The objective of the early phase is to move the ankle joint motion in a single plane and limit the inversion/eversion of the ankle [1]. Subsequently, dorsiflexion/plantarflexion and abduction/adduction (movement within a small range) are maintained in the sagittal plane and the horizontal plane respectively. Movement requirement 2: The tibiotalar joint plays a key role in the gait phase. It is essential that patients are able to walk steadily as quickly as possible during the intermediary phase of rehabilitation [1]. During this time, the main joint of motion is the tibiotalar joint, and its main form of movement is dorsiflexion/plantarflexion. The axis of the tibiotalar joint is projected into the coronal plane, leading to the included angle that is 10° between the horizontal shaft in the coronal plane and the motion axis of the joint [1]. The abduction/adduction has more influence on the tibiotalar joint than inversion/eversion [1]. Based on this point, the rehabilitation middle model is dorsiflexion/plantarflexion and abduction/adduction, and its type of movement is 2-DOF spherical movement. Movement requirement 3: In order to avoid the inconsistent of the DOF of the ankle in the later phase of rehabilitation, inversion/eversion is added to the motion type of rehabilitation [2]. Based on movement requirement 2, the main motion type of the subtalar joint is inversion/eversion. The axis of the subtalar joint is projected into the sagittal plane, creating a 42° angle between the horizontal shaft in the sagittal plane and the joint motion axis. Abduction/adduction has a greater effect on the subtalar joint than dorsiflexion/plantarflexion [1]. Therefore, the rehabilitation mode is inversion/eversion and abduction/adduction, and its motion type is 2-DOF spherical motion.

Configuration Synthesis of Multi-mode Ankle Rehabilitation Robot

105

To solve the above issues, this paper proposes multi-mode rehabilitation parallel mechanisms. The mechanisms are divided into early-middle-later periods of rehabilitation, and the DOF of the ankle joint rehabilitation will change in different periods. Section 2 introduces the ankle joint and LBA configurations. Section 3 presents the constraint limb configuration and analysis of movement requirements. The overall configurations of the mechanisms and calculation of coupling degree k are reported in Sect. 4. The conclusions are discussed in Sect. 5.

2 The Ankle Joint Structure and The Axis LBAs Configuration 2.1 The Ankle Joint Structure The ankle joint is one of the main weight-bearing parts of the human body, which includes double joints: the tibiotalar joint and the subtalar joint as shown as Fig. 1(a). 2.2 The LBAs Configurations The axis of revolute joint is required to coincide with the axis of the ankle joint. The rotation motion is equivalent to the revolute pair, as shown in Table 1. Table 1. The relationship between ankle movement type and matching kinematic pair Axis A

Axis B

Axis C

Ankle movement type

Dorsiflexion/plantarflexion

Abduction/adduction

Inversion/eversion

Matching kinematic pair

Ra

Rb

Rc

We fix the calf on the base so that the axes of the LBAs rotation pairs coincide with the axes of the ankle joints, which can ensure that the ankle joint remains in an arbitrary position during the rehabilitation process. In other words, the point M and the point N of the LBA limbs coincide with the points M and point N of the human ankle. Therefore, the limbs are called LBAs, and their configuration diagrams are shown in Fig. 1. We take all the actuated joints of LBA to apply on the base for the convenience of control. Based on this point, let Ri be actuated joints. There are six types of LBA based on a random combination of Ra , Rb and Rc .

106

W. Jia et al. Axis A

Sagittal plane

Axis A

Sagittal plane M

Axis C

Z

Y N

Coronal plane

Axis C

M

N Coronal plane X Axis B

Axis B

(a)

(b)

Fig. 1. The real anatomical diagram of human ankle joint and axes of human ankle

3 The Configurations of Constraint Limb and the LBAs Mechanisms 3.1 The Configuration of the Constraint Limb of the LBA 1 Component M

1

M The

N

The

N

platform

platform

Component 1

M The

Component platform

Component 2

N Component

2

Component

1

2

(a)

(b)

(c)

M

1 The

N

The

platform

N

Component

platform

Component

Component

Component M

M

2 The Component platform 1

2

N Component

2

1

(d) The base

(e) Ri

Component

Rm

(f) Component 2

1

Rn

The platform

(g)

Fig. 2. (a) The LBA 1 (b) The LBA 3 (c) The LBA 5 (d) The LBA 2 (e) The LBA 4 (f) The LBA 6 (g) The general configurations diagram of LBAs. Arbitrarily, A element from a, b, andc is placed in i, m or n. The Axes of the LBAs are orthogonal to each other.

To realize movement requirements 2 and 3, other limbs need to be further configured. Muscle tissue is driven by muscle contraction. In space, the UPS limb is generally represented as muscles [15]. According to Fig. 1, the ankle joint can be projected onto the sagittal plane, resulting in only dorsiflexion/plantarflexion of the ankle joint. The UPS limb can be equivalent to the RPR limb in the sagittal plane, as shown in Fig. 3. The RPR

Configuration Synthesis of Multi-mode Ankle Rehabilitation Robot Rb Axis C LBA The

107

M(Ra) Constraint limb N

Rc

platform

Fig. 3. The equivalent model of sagittal dorsiflexion/plantarflexion.

limb is fixed in the sagittal plane of the mechanism so that the axis of the Rc coincides with the axis of the U joint, which is the axis of the end rotation pair. However, the constraint limb is constructed to satisfy the prerequisites that dorsiflexion/plantarflexion and inversion/eversion cannot occur simultaneously. The dorsiflexion/plantarflexion of the human ankle joint is driven by the contraction of the muscles [1, 14]. Therefore, we first regard the UPS limb as the constraint limb. The RPR limb makes the Rc axis coincide with the axis of the rotating pair that is the end of the U pair linking with the base at the static position. On the contrary, the RPR limb is equivalent to the UPS limb in space. As shown in Fig. 4, the constraint limb is replaced with the RPR limb to match. As shown in Fig. 4, the mechanism achieves the dorsiflexion/plantarflexion at an arbitrary position. Abduction/adduction is not considered in this subsection. Since the UPS limb is an unconstrained limb, the constraint performance of component 2 is unrelated to the UPS limb. Therefore, the constraint screw of LBA on component 2 is   ⎧ r $021 = 0 1 0 ; −b022 0 0 ⎪ ⎪ ⎪ r   ⎪ ⎨$ = 0 0 1; 0 0 0 022 (1)   ⎪ $r023 = 1 0 0 ; 0 0 0 ⎪ ⎪ ⎪   ⎩ r $024 = 0 0 0 ; 0 0 1 To achieve movement requirements 2 and 3, Eq. (1) need to add $r 025 = (0 0 0; 0 1 0) for dorsiflexion/plantarflexion and $r 026 and $r 027 for inversion/eversion, respectively. $r 026 = (0,0,0;1,0,0) and $r 027 = (0,1,0;0,0,0) are supplied by the constraint limb, while $r 022 , $r 023 and $r 024 are redundant constraints. To obtain the independent kinematic pair of UPS, we consider the single case of dorsiflexion/plantarflexion, as shown in Fig. 4. The motion screw of the UPS is obtained as   ⎧ $011 = 0 m011 n011 ; a022 0 0 ⎪ ⎪ ⎪   ⎪ ⎪ ⎪ $ = ; c 1 0 0 0 b 012 012 012 ⎪ ⎪ ⎪   ⎪ ⎨ $013 = 0 0 0; 0 b013 c013 (2)   ⎪ $014 = 0 1 0 0 ; a014 0 ⎪ ⎪ ⎪   ⎪ ⎪ ⎪ 0; 0 b015 c014 ⎪ $015 = 1 0 ⎪ ⎪   ⎩ $016 = 0 m016 n016 ; a016 0 0

108

W. Jia et al. $023

M Z $011 $012

Y

N

$ $014 013 $015

$016

$021

X $022

Fig. 4. The UPS dorsiflexion/plantarflexion mechanism

It can be seen that the reciprocal product of $011 , $014 , and $016 with $r 025 is not zero. These kinematic pairs correspond to the motion screw unrelated to the dorsiflexion/plantarflexion. Let $011 , $014 , and $016 be set S1 . In addition, we need to set the driving pair on the end of the UPS limb that closes to the base in the case of inversion/eversion, as shown in Fig. 5. At this time, the inversion/eversion is performed alone, and the constraint screw of LBA on component 2 is shown in Eq. (1). In order to avoid redundant constraints, the driving pairs in the UPS are kept. In this case, the motion screw of the UPS limb is   ⎧ $111 = 0 1 0 ; 0 0 0 ⎪ ⎪ ⎪   ⎪ ⎪ ⎪ $112 = 1 0 0 ; 0 0 c112 ⎪ ⎪ ⎪   ⎪ ⎨ $113 = 0 0 0 ; 0 b113 c113 (3)   ⎪ = $ ; 0 0 0 1 0 a ⎪ 014 114 ⎪ ⎪   ⎪ ⎪ ⎪ $115 = 1 0 0 ; 0 b115 c114 ⎪ ⎪ ⎪   ⎩ $116 = 0 m016 n016 ; a116 0 0 Because the reciprocal product of $112 , $113 , $114 , $115 , $116 with $112 , $113 , $114 , $115 , $116 is not zero, these kinematic pairs corresponding to the motion screw uncorrelated to the dorsiflexion/plantarflexion. Let $112 , $113 , $114 , $115 and $116 be set S2 . We need to exclude kinematic pairs that belongs to S2 ∩ S1 . Ultimately, the constraint limb is UPR. 3.2 The Configurations of Constraint Limbs of Other LBAs Mechanisms The LBA needs to meet the proposed movement requirements. Because the human body produces joint movement through muscle contraction, we can also apply the RPR to the inversion/eversion on the coronal plane or abduction/adduction on the horizontal plane, respectively. In this way, the equivalent models of inversion/eversion and abduction/adduction can be obtained, as shown in Fig. 6 and Fig. 7.

Configuration Synthesis of Multi-mode Ankle Rehabilitation Robot Z Driving pair $111 $112

M

$022

X Y

N

$021

$113

$114 $116

109

$023

$115

Fig. 5. The UPS inversion/eversion mechanism Rb

Ra M Rc LBA

The platform Constraint limb

N

Constraint limb

LBA

N

Rc

The platform

Axis C

Fig. 6. The Coronal plane equivalent model inversion/eversion

Fig. 7. The horizontal plane equivalent of model of abduction/adduction

According to the equivalent models in Fig. 3, Fig. 6 and Fig. 7, the method for configuration of the LBA mechanisms is shown in Fig. 9. Due to the screw theory, the UPS limb is equivalent to the UPR limb. Similarly, the manufacturing accuracy is reduced to ensure that the M and N points coincide with the M and N points of the ankle joint. Therefore, the UPR limb is adopted for the synthesis of the other five LBA mechanisms. Single plane To achieve movement requirements

Constraint limb In plane

Objection and requirements

Considering human muscle driving

RPR limb

Project to plane

Equivalent model

Screw theory

Deduced to space

LBA mechanisms

UPS limb In space

Fig. 8. The mind mapping of the LBAs mechanisms

The LBAs 2, 3, 4, 5, and 6 are synthesized as shown in Fig. 9. Nonetheless, we must analyze whether their configuration meets movement requirements. Based on the screw theory analysis, the movement modes of the five LBAs are different, as shown in Table 2. The movement modes of the LBA 1 and the LBA 3 are analyzed to meet the movement requirements.

110

W. Jia et al. Table 2. The five movement modes corresponding to the LBAs

Modes Dorsiflexion/plantarflexion Inversion/eversion

LBA 2

LBA 3

LBA 4

LBA 5

LBA 6

1 √

2

1 √

2

1 √

1 √

2

1 √

√

√

√

√

√

√

√

√

Abduction/adduction

Z Y

M

N

Z

M

Y

N

X

√

√

√

X Y

X

Z Y

√

M

√

M

X Y

N

N

The LBA 3 mechanism Z

Y

X

N

The LBA 4 mechanism Z

Y X

Z Y

M

M N

Y

2 √

M

M N

Z

X

The LBA 2 mechanism Z

2 √

N

X

The LBA 5 mechanism Z

M

Y X

X N

M N

The LBA 6 mechanism Fig. 9. The configurations of five LBAs

4 The Overall Configurations of Mechanisms and Calculation of Coupling Degree k 4.1 The Configurations of Driving Limbs The LBAs 1 and 3 mechanisms are underactuated, which satisfies the dorsiflexion/plantarflexion and inversion/eversion of movement requirements 2 and 3. In order to install the motor on the base, the limb is linked to the base with the platform.

Configuration Synthesis of Multi-mode Ankle Rehabilitation Robot

111

Thus, the mechanisms fully meet movement requirements 2 and 3. From a perspective configuration of mechanism, movement requirements 1 need to achieve decoupling of dorsiflexion/plantarflexion and abduction/adduction in a single plane. To realize movement requirement 1, it is necessary to decouple abduction/adduction and dorsiflexion/plantarflexion. The revolute joints on both ends of the driving limbs are R1 and R2 , respectively. Furthermore, R1 is the driving pair, and its axis coincides with the axis of the Rb . Meanwhile, the axis of R2 coincides with the Ra or Rc , as shown as Fig. 9(a). Figure 9(b) and Fig. 9(c) is both other possibilities for driving limbs. Table 3. The table of unconstrained limbs Limb types

Special position limbs configuration table

R1 PPRy PR2 R1 Rx PRy Pz R2

C1 PRy PR2, C1 Cy PR2, R1 PCPR2 Uxz PRy PR2, Uxz Cy PR2, R1 Rx Cy PR2

R1 PRx Ry Pz R2

Cz Rx Ry PR2, Cz Uxy PR2, R1 PUxy PR2

R 1 R x R x R y Pz R 2 R1 PPRy Rx R2

Uxz Rx Ry PR2, Uxz Uxy PR2, R1 Rx Uxy PR2 Cz PRy Rx R2, Cz Cy Rx R2, R1 PCy Rx R2

R1 Rx PRy Rx R2

Uxz PRy Rx R2, Uxz Cy Rx R2, R1 Rx Cy Rx R2

R1 PRx Ry Rx R2

Cz Rx Ry Rx R2, Cz Uxy Rx R2, R1 PUxy Rx R2 Uxz Rx Ry Rx R2, Uxz Uxy Rx R2, R1 Rx Uxy Rx R2

R1 Rx Rx Ry Rx R2

The mechanism has three DOFs in movement requirement 1 and two DOFs in movement requirements 2 and 3. One of the two DOFs in movement requirements 2 and 3 is fixed axis rotation, and the other is provided by a plane linkage mechanism that is the equivalent model of dorsiflexion/plantarflexion, inversion/eversion or abduction/adduction. However, this configuration of limbs is R1 Ry Pz R2 or R1 Ry Rx R2 . In a single mode, it is great high for the degree of coupling between the two DOFs. Hence, two DOFs are added to the driving pair, which leads the constraint degree of the limb is zero [17, 18]. In this way, the permutation and combination of P and R is four results: PP, Rx P, PRx , Rx R. The motion axis of the P is kept in the sagittal plane. Then, there are 8 species for the driving limbs, based on the enumeration as shown in Table 3. Excluding repeat limbs, the total number is 32 types of 6-DOF limbs. For LBA 1, there are two situations regarding the arrangement of the driving limbs. One is that the axis of the revolute joint R2 at the end of the driving limbs is colinear with the axis of the Rc . The other is that the axis of the R2 is colinear with the axis of the Ra . In the conclusion, there are 128 types of mechanisms that can satisfy the movement requirements by enumeration, and all the limb types of mechanisms have the same constraint performance. 4.2 The Coupling Degree Analysis of the Multi-mode Parallel Mechanism The coupling degree k of the ankle rehabilitation robot represents the complexity of kinematics and dynamics problems of the multi-loop mechanism [18]. When k = 0,

112

W. Jia et al. Axis B

Axis A

Sagittal plane

Z

M P

Axis C

Z

M

X

N

R2

Sagittal plane

Rx

N

Y

X

Sagittal plane M

Y N Horizontal plane

Horizontal plane

Horizontal plane Ry

R1

(a)

Ry

(b)

(c)

Fig. 10. The configuration method of driving limbs Z M X Y

Z M

N

Diving limb

Y

N X

Fig. 11. The configuration of the LBAs 1 and 3 mechanism

the kinematics and dynamics analysis of each loop can be solved separately, so that the analytical solution of the position solution can be obtained. The coupling degree k expresses the complexity of the kinematics and dynamics of the multi-loop mechanism [17, 18]. For the LBA 1 mechanism, in the mode of dorsiflexion/plantarflexion and abduction/adduction, the closed chain is composed of constraint limb and LBA 1, which is the equivalent model of dorsiflexion/plantarflexion. Let mj be number of kinematic pairs in the jth single open chain. Let f i be DOFs of ith joints of single open chain. Let I j be number of actuated pairs in the jth single open chain. Let ξ Lj be number of independent equations of the jth single open chain. Therefore, the constraint degree  is =

mj 

fi − Ij − ξLj

(4)

i=1

Based on the Eq. (4), the LBA 1 mechanism has zero coupling degree. For the LBA 3, the constraint degree of this mechanism does not change, so it is a mechanism with a zero coupling degree for these two types of mechanisms.

5 Conclusions In this paper, a multi-mode parallel mechanism for ankle joint rehabilitation was synthesized according to the movement requirements. The conclusions are shown as follows:

Configuration Synthesis of Multi-mode Ankle Rehabilitation Robot

113

(1) Three rehabilitation modes were proposed. They are dorsiflexion /plantarflexion and abduction/adduction in a single plane, dorsiflexion/plantarflexion and abduction/adduction or inversion/eversion and abduction/adduction. These modes satisfy the movement requirements. (2) From the constraint screw of the proposed LBAs and the movement requirements, the redundant screw of the UPS limb was eliminated, and the UPR limb was obtained. Under this guidance, it has obtained that the LBAs 1 and 3 mechanisms realize the movement requirements with three equivalent models. (3) The coupling degree of the mechanism is zero, so the kinematics and control is simple. The ankle rehabilitation robot for the rehabilitation of ankle sprain has stronger adaptability. The ankle joint has 128 configurations. Different configurations have potential research value on the matching degree of ankle joint. Acknowledgments. This work is supported by the National Natural Science Foundation of China (Grant No.52075145), Major Scientific and Technological Achievements Transformation Project of Hebei Province (Grant No.20281805Z), and the Central Government Guides Local Science and Technology Development Funds (Grant No.206Z1801G).

References 1. Donalda, N.: Kinesiology of the Musculoskeletal System Foundations for Rehabilitation. People’s Military Medical Press, Beijing (2014) 2. Jeme, C.: Postsurgical rehabilitation guidelines for the orthopedic clinician. Science & Technology Translation & Publishing Co., Tianjin (2009) 3. Wei, J., DAI, J.S.: Reconfiguration-aimed and manifold-operation based type synthesis of metamorphic parallel mechanisms with motion between 1R2T and 2R1T. Mech. Mach. Theory 139(1), 66–80 (2019) 4. Wei, J., Dai, J.S.: Lie Group based type synthesis using transformation configuration space for reconfigurable parallel mechanisms with bifurcation between spherical motion and planar motion. J. Mech. Des. 142(6), 1–13 (2020) 5. Yoon, J.W., Ryu, J.H.: A novel reconfigurable ankle/foot rehabilitation robot. In: 2005 IEEE International Conference on Robotics and Automation Barcelona, pp. 2290–2295. Institute of Electrical and Electronics Engineers Inc., Spain (2005) 6. Yoon, J.W., Ryu, J.H., Byung, L.: Reconfigurable ankle rehabilitation robot for various exercises. J. Robot. Syst. 22, 15–33 (2006) 7. Zeng, S.L., Yao, L.G., Guo, X.N., et al.: Kinematics analysis and verification on the novel reconfigurable ankle rehabilitation robot based on parallel mechanism. Mech. Mach. Sci. 24, 195–202 (2015) 8. Li, Y., Yao, L.G.: Mathematical modeling and kinematics analysis for a novel ankle rehabilitation. Mech. Mach. Sci. 36, 571–580 (2016) 9. Maria, B., Paola, A., Edgar, A., et al.: Reconfigurable mechanical system design for tracking an ankle trajectory using an evolutionary optimization algorithm. IEEE Access 5, 5480–5492 (2017) 10. Nurahmi, L., Caro, S., Solichin, M.: A novel ankle rehabilitation device based on a reconfigurable 3-RPS parallel manipulator. Mech. Mach. Theory 134(1), 135–150 (2019)

114

W. Jia et al.

11. Li, J.F., Zhang, K., Zhang, L.Y.: Design and kinematic performance evaluation of parallel ankle rehabilitation robot. J. Mech. Eng. 55, 29–39 (2019) 12. Liu, Q.Q., Zhang, X., Shang, W.F.: Design and characterization of the MKA-IV robot for ankle rehabilitation. In: Proceedings of the 2018 IEEE International Conference on Realtime Computing and Robotics, pp. 544–549, Institute of Electrical and Electronics Engineers Inc., Shen Zhen (2018) 13. Jamwal, K.P., Xie, S.Q., Kean, C.A.: Kinematic design optimization of a parallel ankle rehabilitation robot using modified genetic algorithm. Robot. Auton. Syst. 57, 1018–1027 (2009) 14. Yu, X.J., Liu, K., Kong, X.W.: State of the art of multi-mode mechanism. J. Mech. Eng. 56, 14–27 (2020) 15. Niu, J.Y.: Research on wheel-legged quadruped walking robot based on series-parallel mechanism. Yan Shan University (2018) 16. Ke, X., Deng, J.M., Wu, G.L., et al.: Design of 3-DOF zero coupling degree planar parallel manipulator based on the coupling-reducing and its kinematic performance improvement. Mech. Mach. Sci. 59, 400–408 (2018) 17. Shen, H.P., Cheng, Q.W., Wu, G.L., et al.: Topological design of an asymmetric 3-translation parallel mechanism with zero coupling degree and motion decoupling. In: Proceedings of the ASME 2018 International Design Engineering, pp. 1–7, American Society of Mechanical Engineers (2018) 18. Yang, T.L.: Topology Structure Design of Robot Mechanisms. China Machine Press, Beijing (2002) 19. Dai, J.S., Huang, Z., Harvey, L.K.: Mobility of overconstrained parallel mechanisms. J. Mech. Des. 128(1), 220–229 (2006) 20. Liu, C.L., Zhang, J.J., Qi, K.C., et al.: Synthesis of generalized spherical parallel manipulations for ankle rehabilitation. J. Mech. Eng. 56(19), 79–91 (2020) 21. Leardini, A., Connor, J.J., Catani, F., et al.: A geometric model of the human ankle joint. J. Mech. Eng. 32(6), 585–591 (1999)

Hydrodynamic Coefficient Estimation of Small Autonomous Underwater Vehicle Gan Zhang, Yinfu Lin, Ji Huang, Lin Hong, and Xin Wang(B) Harbin Institute of Technology, Shenzhen, Guangdong, China [email protected]

Abstract. Autonomous underwater vehicles (AUVs) have been widely used in many aspects of the underwater world for their autonomy and robust flexibility. However, the dynamic performance of the small AUVs is strongly affected by hydrodynamic effects. In this work, based on a novel designed small AUV, we conduct a hydrodynamic coefficients estimation work by adopting the computational fluid dynamics (CFD) method. Firstly, the resistance to AUV under different propeller layout conditions is analyzed. Secondly, to study the hydrodynamic coefficients of the AUV at different pitch angles, we simulated the movement of the AUV in the Fluent module and determined the thrust of the propeller by estimating the axial and radial forces. Simulation results show that the resistance and sinking force coefficients of the designed AUV are positively correlated with pitch angle and negatively correlated with Reynolds number. This work provides a vital reference for designing effective control strategies for small underwater vehicles. Keywords: Computational fluid dynamic · Autonomous underwater vehicle · Hydrodynamic coefficients

1 Introduction Due to the complex underwater environment, the water flow has a great impact on autonomous underwater vehicles. Therefore, in order to control AUVs’ motion efficiently and stably, it is necessary to analyze the force conditions and hydrodynamic coefficients of the robot in the underwater intervention process. Nowadays, in the motion simulation of AUV, many research works have been conducted by using CFD methods. In order to study AUV’s hydrodynamic effects, Zheng et al. [1] used computational fluid dynamic method to analyze the resistance and hydrodynamic coefficients of AUV at different speeds. Some fluid simulations were performed on AUV’s external attachments. Wang et al. [2] optimized the hydrodynamic performance of AUV with various appendages. The optimization results showed that the influence of attachments on the hydrodynamic performance was proportional to their size, and attachments’ distributed layout was beneficial to reduce the resistance. Zhao et al. [3] established a motion simulation platform for AUV attachments. Maneuverability of AUV was simulated in horizontal and vertical planes. In order to evaluate the influence of the attachments, the fluid properties of AUV with or without attachments were compared. Julian Hoth et al. [4] determined the © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 115–125, 2021. https://doi.org/10.1007/978-3-030-89092-6_11

116

G. Zhang et al.

flow parameters around AUV. He used different methods to perform CFD simulations on the AUV and analyzed the pressure data. The results showed that the method which used vector machine could produce more accurate results. A. Muthuvel et al. [5] used a commercial CFD package called STAR-CCM+ to simulate the flow around AUV with different attachments. Ammar Amory et al. [6] used ANSYS Fluent software to simulate fluid properties of a fish-like AUV. Its fluid properties were compared with the torpedo AUV to demonstrate that the fish-like AUV had less resistance when running. Madan A. D et al. [7] used semi-empirical formula to analyze the relationship between sizes of AUV and lift and drag forces. Ali Nematollahi et al. [8] analyzed the influence of different submergence depths and vehicle speeds on the hydrodynamic performance of underwater vehicles, and summarized some conclusions. Different pitch angles have a great impact on the motion of AUV. A. Mitra et al. [9] conducted simulations and experiments on the hull of AUV. He explored the flow characteristics of AUV with different submerged depths and angles of attack. It provided a method to improve the design of AUV. Su et al. [10] designed an AUV with vector thruster and calculated the hydrodynamic coefficients of AUV at different angles of attack by CFD software. P. Jagadeesh et al. [11] analyzed the hydrodynamic coefficients of the torpedo-type AUV under different pitch angles by both CFD simulation and experiment. The results helped with the design of AUV. In this paper, Sect. 2 introduces the fluid dynamics model used in fluid simulation and presents the influence of different propeller layouts on resistance coefficient of AUV. Section 3 analyzes resistances and sinking forces of AUV with different pitch angles and Reynolds numbers. Finally, the conclusion of this article is in the Sect. 4.

2 Fluid Dynamics Model In this work, three assumptions are made in the process of our CFD simulation: (1) A fixed-size cuboid is used as the simulation area, and its boundary effect is ignored for simulating an infinite AUV working area. (2) We take the resistance in still water into consideration but ignore the influence of currents, since this AUV only works in lakes and rivers. (3) The torques generated by their rotation can cancel each other due to the symmetrical placement of propellers. Meanwhile, the thrust is considered. 2.1 Fluid Flow Model Before establishing computational model, Reynolds number can be determined as follows: Re =

ρuL μ

(1)

where the density of water ρ equals 998.2 kg/m3 , dynamic viscosity coefficient μ equals 1.003 * 10−3 kg/m·s, the effective length of AUVL equals 0.2 m, and u is the velocity of AUV. When the AUV travels at the speed of 0.1 m/s, the Reynolds number is 1.99 * 104 , so it should be considered as a turbulence model. However, the analysis of turbulent

Hydrodynamic Coefficient Estimation of Small Autonomous Underwater Vehicle

117

flow field is very complicated. There may be eddy currents of different sizes interfering with each other, which makes the simulation more difficult. Here, we choose standard k-epsilon model to simulate AUV’s movement. It is a common turbulence model, which can be used to solve the external flow problem of complex geometry. It can be described as follows: ∂ μt ∂k ∂(ρk) ∂(ρkui ) + = [(μ + ) ] + Gk + Gb − ρε − YM + Sk ∂t ∂xi ∂xj σk ∂xj

(2)

ε ∂(ρε) ∂(ρεui ) ∂ μt ∂ε ε2 = [(μ + ) ] + C1ε (Gk + C3ε Gb ) − C2ε ρ + Sε (3) + ∂t ∂xi ∂xj σε ∂xj k k where k is kinetic energy, ε is turbulent dissipation rate, α is turbulent Prandtl number, Gk and Gε are kinetic energy changes caused by velocity gradient and buoyancy, Y M is fluctuations caused by transitional diffusion in compressible turbulence, C is empirical coefficient, S is custom constant. 2.2 3D Modeling and Meshing We design a small AUV with flexibility and portability, which can work in lakes and rivers. When AUV is working underwater, different layouts of propellers may have different effects on the resistance. In this section, the influence of propeller layout on its resistance is analyzed through CFD simulation. The 3D models of AUV with different propeller layouts are shown in Fig. 1.

Fig. 1. 3D models of AUV. (a) AUV with no propeller. (b) AUV with external propellers. (c) AUV with internal propellers.

Due to the small size of AUV, it is meshed with the highest-quality unstructured grid in ANSYS software, which is a good way to improve the accuracy of the simulation. The finite element models after meshing are shown in Fig. 2. We use a 20D*10D*10D cuboid as the computational domain where D is the diameter of AUV (D = 160 mm). The computational domain after meshing and its boundary conditions are shown in Fig. 3. The whole computational domain including AUV is symmetrical about the middle plane, so half of the domain is used to improve the calculation efficiency. During the simulation, AUV is static and different speeds of water are used to simulate different forward speeds of AUV.

118

G. Zhang et al.

Fig. 2. AUV after meshing. (a) AUV with no propeller. (b) AUV with external propellers. (c) AUV with internal propellers.

Fig. 3. Computational domain and boundary conditions.

2.3 Calculation Results AUV is set to travels at the speed of 0.6 m/s in water. As shown in the following equation, the resistance to AUV can be calculated: FD = 0.5ρCD v2 A

(4)

where C D denotes the resistance coefficient, F D denotes the resistance, ν denotes the speed of AUV, ρ denotes the density of the fluid, and A denotes the cross-sectional area. The resistance coefficients and resistances are shown in Table 1. From Table 1, we can find that AUV without propeller has the least resistance when running in water. After AUV is equipped with external propellers or internal propellers, its resistance will double. However, the difference between them is small. It can be clearly seen that their efficiency of underwater motion is similar. For the convenience of control, AUV with external propellers is used for the following simulation experiment.

3 Simulation Experiment AUVs with different pitch angles will be subjected to different forces from water flow. The forces on AUV are calculated by CFD software, and they are analyzed to obtain the hydrodynamic coefficients as well as axial and radial forces of AUV.

Hydrodynamic Coefficient Estimation of Small Autonomous Underwater Vehicle

119

Table 1. Resistance coefficients and resistances to AUV. Propeller layout

Cross-sectional area

Resistance coefficient

Resistance

No propeller

0.0064π

0.5523

1.9989N

Internal propellers

0.01π

0.6796

3.8431N

External propellers

0.01π

0.7227

4.0870N

3.1 Initial Conditions and Force Analysis The length-diameter ratio (L/D) of this AUV is 2.1875, and Table 2 shows the initial conditions of the simulation. Table 2. Initial conditions. Parameter

Unit

Numerical description

Reynolds number

(×105 )

0.4, 0.8, 1.2, 1.6, 2.0

Pitch angle (α)

°

0, 5, 10, 15, 20

Hydrodynamic coefficient

--

Resistance and sinking force

Fig. 4. Force analysis of AUV.

As shown in Fig. 4, F L denotes the sinking force, F D denotes the resistance, F A denotes the axial force, F N denotes the radial force, and α denotes the pitch angle. The relationship among them can be described as follows:  FA = FD cos α − FL sin α (5) FN = − FD sin α − FL cos α

120

G. Zhang et al.

According to Eq. (4), we can obtain the resistance and sinking force coefficient: CD = FD /(0.5ρv2 A)

(6)

CL = FL /(0.5ρv2 A)

(7)

3.2 Analysis of Simulation Results When the AUV is moving at different speeds and pitch angles, the resistance and sinking force can be simulated. Simulation results are shown in Table 3 and Table 4. Table 3. Resistances to AUV with different Reynolds numbers and pitch angles. Pitch angles

Reynolds numbers (×105 ) 0.4

0.8

1.2

1.6

2.0

0°

0.46N

1.82N

4.08N

7.22N

11.16N

5°

0.51N

2.02N

4.53N

7.96N

12.33N

10°

0.56N

2.18N

4.88N

8.64N

13.45N

15°

0.66N

2.64N

5.91N

10.48N

16.18N

20°

0.73N

2.91N

6.46N

11.47N

17.84N

Table 4. Sinking forces of AUV with different Reynolds numbers and pitch angles. Pitch angles

Reynolds numbers (×105 ) 0.4

0.8

1.2

1.6

2.0

5°

0.08N

0.30N

0.65N

1.15N

1.61N

10°

0.12N

0.45N

0.98N

1.67N

2.52N

15°

0.16N

0.62N

1.32N

2.25N

3.39N

20°

0.23N

0.88N

1.92N

3.25N

4.87N

After visualizing the data, the resistance and sinking force diagrams are shown in Fig. 5 and Fig. 6. From Fig. 5 and Fig. 6, it is clearly that resistance and sinking force both have a quadratic relationship with Reynolds number. This result is consistent with Eq. (6) and Eq. (7). Meanwhile, the resistance and sinking force are positively correlated to the pitch angle. Based on the estimated force, the resistance coefficients and the sinking force coefficients can be calculated separately through Eq. (6) and Eq. (7), as shown in Table 5 and Table 6.

Hydrodynamic Coefficient Estimation of Small Autonomous Underwater Vehicle

121

Fig. 5. Resistances to AUV with different Reynolds numbers and pitch angles.

Fig. 6. Sinking forces of AUV with different Reynolds numbers and pitch angles.

Table 5. Resistance coefficients of AUV with different Reynolds numbers and pitch angles. Pitch angles

Reynolds numbers (×105 ) 0.4

0.8

1.2

1.6

2.0

0°

0.731

0.723

0.722

0.717

0.711

5°

0.814

0.805

0.800

0.792

0.784

10°

0.888

0.868

0.863

0.859

0.857

15°

1.056

1.049

1.045

1.040

1.030

20°

1.172

1.157

1.143

1.141

1.136

122

G. Zhang et al.

Table 6. Sinking force coefficients of AUV with different Reynolds numbers and pitch angles. Pitch angles

Reynolds numbers (×105 ) 0.4

0.8

1.2

1.6

2.0

5°

0.131

0.121

0.116

0.114

0.102

10°

0.186

0.179

0.173

0.166

0.160

15°

0.258

0.245

0.234

0.223

0.216

20°

0.364

0.349

0.339

0.323

0.310

The resistance and sinking force coefficients diagrams of AUV are shown in Fig. 7 and Fig. 8. When the Reynolds number is constant, the larger the pitch angle is, the greater the resistance coefficient and sinking force coefficient become. It is because the contact area between AUV and the incoming flow increases. When the pitch angle is constant and the Reynolds number increases, the resistance coefficient and sinking force coefficient tend to be smaller. This is because the resistance coefficient includes the pressure coefficient and the viscous coefficient. The pressure coefficient decreases with the increase of Reynolds number, but the viscous coefficient is almost unchanged. When the pitch angle is 0° and the Reynolds number increases, the relationship among the resistance, pressure force and viscous force is shown in Fig. 9.

Fig. 7. Resistance coefficients of AUV with different Reynolds numbers and pitch angles.

To obtain the thrust of propellers, we convert the resistance and sinking force into axial and radial forces with Eq. (5). The axial and radial forces can be offset by the thrust generated by the horizontal propellers and the vertical propellers. It provides data reference for controlling the thrust when AUV is moving at different pitch angles. The axial and radial forces of AUV are shown in Table 7 and Table 8 respectively.

Hydrodynamic Coefficient Estimation of Small Autonomous Underwater Vehicle

123

Fig. 8. Sinking force coefficients of AUV with different Reynolds numbers and pitch angles.

Fig. 9. Relationship between resistance, pressure force, and viscous force.

Table 7. Axial forces of AUV. Pitch angles

Reynolds numbers (×105 ) 0.4

0.8

1.2

1.6

2.0

0°

0.46N

0.82N

4.08N

7.22N

11.16N

5°

0.50N

1.99N

4.45N

7.83N

12.15N

10°

0.53N

2.07N

4.64N

8.22N

12.81N

15°

0.60N

2.39N

5.37N

9.55N

14.75N

20°

0.61N

2.43N

5.42N

9.67N

15.10N

124

G. Zhang et al. Table 8. Radial forces of AUV.

Pitch angles

Reynolds numbers (×105 ) 0.4

0.8

1.2

1.6

2.0

0°

0N

0N

0N

0N

0N

5°

−0.13N

−0.48N

−1.05N

−1.84N

−2.68N

10°

−0.21N

−0.82N

−1.81N

−3.15N

−4.82N

15°

−0.33N

−1.28N

−2.80N

−4.88N

−7.46N

20°

−0.47N

−1.82N

−4.01N

−6.97N

−10.68N

4 Conclusion In this paper, we apply CFD method to estimate the hydrodynamic coefficients and axial and radial forces of a small AUV. Firstly, we establish a simulation model of the designed AUV with Fluent and analyze the resistances to AUV under different propeller layouts and decide to choose AUV with external propellers. Next, hydrodynamic coefficients of AUV moving in different pitch angles are given by simulation. And we discuss the influence of the Reynolds number and pitch angle on hydrodynamic coefficients. The results show that the resistance and sinking force coefficients of AUV will increase with the increase of the pitch angle and decrease with the increase of Reynolds number. Finally, the axial and radial forces of AUV are calculated, which provides data reference for stability control of AUV. In the future, we plan to verify the effectiveness of the fluid dynamic model by field experiments. Acknowledgment. This work was supported in part by the R \& D plan of key areas in Guangdong Province (No.2019B090920001), and Shenzhen Bureau of Science Technology and Information of China (No.JCYJ20180306172134024).

References 1. Zheng, H., Wang, X., Xu, Z.: Study on hydrodynamic performance and CFD simulation of AUV. In: 2017 IEEE International Conference on Information and Automation. ICIA (2017) 2. Wang, Y., Gao, T., Pang, Y., et al.: Investigation and optimization of appendage influence on the hydrodynamic performance of AUVs. J. Mar. Sci. Technol. 24, 297–305 (2019) 3. Zhao, J., Su, Y., Ju, L., et al.: Hydrodynamic performance calculation and motion simulation of an AUV with appendages. In: Proceedings of 2011 International Conference on Electronic & Mechanical Engineering and Information Technology, pp. 657–660 (2011) 4. Hoth, J., Kowalczyk, W.: Determination of flow parameters of a water flow around an AUV body. Robotics 8(1), 5 (2019) 5. Muthuvel, A., Babu, M., Purandarraj, S., et al.: Numerical simulation of flow over an autonomous underwater vehicle with appendages for various turbulence models. In: AIP Conference Proceedings 2039, p. 020038 (2018) 6. Amory, A., Maehle, E.: Modelling and CFD simulation of a micro autonomous underwater vehicle SEMBIO. In: OCEANS 2018 MTS/IEEE Charleston, pp. 1–6 (2018)

Hydrodynamic Coefficient Estimation of Small Autonomous Underwater Vehicle

125

7. Madan, A.D., Issac, M.T.: Hydrodynamic analysis of AUV hulls using semi-empirical and CFD approach. Univ. J. Mech. Eng. 5(5), 137–143 (2017) 8. Nematollahi, A., Dadvand, A., Dawoodian, M.: An axisymmetric underwater vehicle-free surface interaction: a numerical study. Ocean Eng. 96(mar.1), 205–214 (2015) 9. Mitra, A., Panda, P., Warrior, H.V.: The effects of free stream turbulence on the hydrodynamic characteristics of an AUV hull form. Ocean Eng. 174, 148–158 (2019) 10. Su, H., Chen, C., Cai, Q., et al.: Computational fluid dynamics study of autonomous underwater vehicle with vectorial thrusters. In: 2018 OCEANS - MTS/IEEE Kobe Techno-Oceans (OTO), pp. 1–5 (2018) 11. Jagadeesh, P., Murali, K., Idichandy, V.G.: Experimental investigation of hydrodynamic force coefficients over AUV hull form. Ocean Eng. 36(1), 113–118 (2009)

Stiffness Modeling of a Novel 2-DOF Solar Tracker with Consideration of Universal Joint Stiffness Yuyao Song1,2

and Jun Wu1,2(B)

1 The State Key Laboratory of Tribology and Institute of Manufacturing Engineering,

Department of Mechanical Engineering, Tsinghua University, Beijing 100084, China [email protected] 2 Beijing Key Lab of Precision/Ultra-Precision Manufacturing Equipment and Control, Beijing 100084, China

Abstract. This paper deals with stiffness modeling of a novel parallel 2-DOF solar tracker with high rigidity and large workspace to track the sun. An analytical method to derive the stiffness matrix of universal joint in non-diagonal form is proposed. Considering the influence of universal joint stiffness on the deformation of solar tracker, the stiffness matrix of the solar tracker is derived with Matrix Structural Analysis method. The simulation on the stiffness of the solar tracker is carried out. A stiffness measuring mechanism is designed to measure the stiffness of the solar tracker and the experimental results are compared with the stiffness value from the stiffness model to validate the effectiveness of the modeling method. Keywords: Parallel solar tracker · Stiffness modeling · Universal joint stiffness

1 Introduction The two-axis solar energy systems have emerged as a viable source of renewable energy over the past several decades, and are now widely used in a variety of industrial and domestic applications. The predominant two-axis trackers are commonly designed based on serial architectures which usually has a heavy structure in order to keep enough rigidity. This results in larger actuator requirements and high power consumption far from optimal. From this point of view, a novel two-axis mechanism with parallel structure is an ideal choice for solar tracker due to its high stiffness to inertia ratio [1]. Stiffness is one of the most significant performance of a solar tracker [2], so it is essential to evaluate the stiffness of solar tracker precisely and efficiently. The Finite Element Analysis (FEA), the Matrix Structural Analysis (MSA) and the Virtual Joint Method (VJM) are the most common approaches to model the stiffness of a mechanism. The FEA can get fairly accurate result at the expense of high computational cost [3–5], so the FEA is usually applied at the final design stage. The MSA computes in the same way as the FEA does, but the elements used in the MSA are much bigger and possess clear physical meanings. The MSA consumes less time in computation but its accuracy © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 126–136, 2021. https://doi.org/10.1007/978-3-030-89092-6_12

Stiffness Modeling of a Novel 2-DOF Solar Tracker

127

is usually a little bit lower compared with the FEA [6]. In the VJM, the deformation of a mechanism is attributed to joints compliance. So links are regarded as rigid body while joints are regarded to be elastic. The VJM is the most effective one in all three methods but has some difficulties when dealing with parallel mechanisms [9]. Different from the condition of serial mechanism, universal joint is widely used in the parallel mechanism. However, there still exist a few defects in stiffness modeling of universal joint, leading to accuracy decreasing of the above three methods when the stiffness of parallel mechanism is studied. The stiffness matrix of universal joint is usually simplified as a diagonal matrix in the existing works [2], ignoring the size effect if the two groups of bearing in Universal joint installed with some distance away from each other. To model the stiffness of parallel mechanism containing universal joint more accurately, a new modeling method is essentially required to establish a non-diagonal stiffness matrix of universal joint. In this paper, a novel solar tracker with parallel structure is proposed. An approach to formulate stiffness matrix of universal joint in non-diagonal form is presented, and Matrix Structural Analysis is used to derive stiffness matrix of solar tracker with consideration of universal joint stiffness. The remaining sections of this paper is organized as follows. In Sect. 2, the structure of the solar tracker is introduced. In Sect. 3, the stiffness matrices of revolute joint and universal joint are derived, based on which the stiffness of the solar tracker is obtained. In Sect. 4, the simulation and experimental result of the solar tracker stiffness are derived, and the effectiveness of the modeling method is also validated. In Sect. 5, some conclusions are given.

2 Structure of the Solar Tracker The 2-DOF solar tracker considered in this paper is shown in Fig. 1 and its schematic diagram is shown in Fig. 2. The solar tracker consists of a moving platform, a pole, a 5R parallel mechanism and a fixed base. The 5R parallel mechanism includes two kinematic chains and a base link. Two kinematic chains are connected to the base link by revolute joints which are active joints actuated by motors. The 5R mechanism is connected to the moving platform by a revolute joint and a universal joint in sequence. The pole is fixed on the base and connected to the moving platform by a universal joint. Therefore, the solar tracker possesses two rotational DOF whose direction coincide with the axes of universal joint connecting the moving platform and the pole.

Fig. 1. 2-DOF parallel solar tracker

128

Y. Song and J. Wu C

w

B C D

D'

ψ5

o

D

v

u

F1

u'

E1

z

F2 H

v'

d

l4 ψ4

b w' G

l3

ψ3

E2 E1

h

A

r

O

x

l1

w ψ2

ψ1 F1 u

y

a

a1 G

a2

E2

l2 F2

b

Fig. 2. Schematic diagram of solar tracker and 5R parallel mechanism (a. solar tracker b. 5R mechanism).

3 Stiffness Modeling of the Solar Tracker The Finite Element Analysis (FEA), the Matrix Structural Analysis (MSA) and the Virtual Joint Method (VJM) are the most common approaches to model the stiffness of a mechanism. Compared to the others, the MSA method is a good compromise between computational efficiency and accuracy. Thus, the MSA method is used in this study to establish the stiffness model of the solar tracker. The stiffness of revolute joints is introduced first, based on which the non-diagonal stiffness matrix of universal joint is then derived. The stiffness of the solar tracker is finally obtained by furtherly considering the actuator stiffness and beam element. The whole solar tracker is decomposed into 18 elements and 20 nodes, as shown in Fig. 3. Refer to the Appendix for the specific type of each element. 3.1 Stiffness of Revolute Joint Deformation of revolute joint is treated as the result of bearing compliance. All the bearings used in the novel solar tracker are deep-groove ball bearing. According to work by Li [11], the relationship between axial/radial displacement and corresponding forces of the bearing can be expressed as Fa = 3.462

p  1/2  Dδa3

(1)

i=1

Fr = 3.462

p 

1/2  fpu (ϕi ) Dδr3 cos3 ϕi cos ϕi

(2)

i=1

where p is the number of balls. D is the diameter (mm) of balls. δa , δr are the axial/radial displacement (μm) of bearing, respectively. ϕi is the angle between the ith ball and central line of the bearing, as shown in Fig. 4. fpu (ϕi ) is a function used to figure out if the ith ball is compressed. It is expressed as  1 (cos ϕi > 0) (3) fpu (ϕi ) = 0 (cos ϕi ≤ 0)

Stiffness Modeling of a Novel 2-DOF Solar Tracker

129

Fig. 4. Structure of deep-groove ball bearing

Fig. 3. 2-DOF parallel solar tracker

Moreover, the deformations of balls are central symmetric when the bearing has rotational deformation under a torque. Considering the loading-deformation relationship of the ball, the relationship between the reaction torque M and the rotational deformation θ can be expressed as M =

l 

1/2  3.462 D(rθ cos φi )3 r cos φi

(4)

i=1

Equations (1)(2)(4) are the loading-deformation relationships of deep-groove ball bearing with non-linear characteristic. Based on the stiffness of bearing, the stiffness of revolute joint can be obtained. Revolute joint is modeled as 3-dimensional element with two nodes. In the novel solar tracker, revolute joints are separated into two categories- single-span revolute joint and double-span revolute joint. The single-span revolute joint has only one bearing, so its stiffness is the same as that of deep-groove ball bearing. This can be expressed as ps

ps

ps

b ka = kab kr = krb krot = krot ps

ps

(5)

ps

where ka , kr , krot are the axial, radial, rotation stiffness of single-span Revolute joint, b are the axial, radial, rotation stiffness of bearing. Double-span revolute and kab , krb , krot joint has the same axial stiffness as the bearing ignoring the effect of preload. If the radial force is uniformly loaded, the two bearings deform equally on radial direction. Thus, the radial stiffness of double-span Revolute joint is twice as much as that of the bearing. The above relationships can be expressed as pd

pd

ka = kab kr = 2krb pd

(6)

pd

where ka , kr are axial and radial stiffness of double-span revolute joint. When torque is loaded on the Revolute joint, two bearings have equal radial deformations, but the direction is opposite. Then, the rotation stiffness of double-span Revolute joint is derived as pd

krot =

lp2

kb 2 r where lp is the distance between the two bearings.

(7)

130

Y. Song and J. Wu

3.2 Stiffness of Universal Joint Universal joint can be seen as the assembly of two revolute joints Rb and Ru , whose axes are perpendicular to each other. It is also modeled as 3-dimensional element with two nodes. The node displacements of Rb and Ru are expressed as (b)

xr =



(b)



x r1

(b)

x r2

(u)

xr =



(u)



x r1

(8)

(u)

x r2

The connection structure between two Revolute joints are treated as rigid body, so node (b) (u)

displacements x r , x r have to satisfy the deformation compatibility condition (u)

(b)

x r1 = G x r2

where

⎡

100 0 ⎢0 1 0 0 ⎢ ⎢ ⎢ 0 0 1 du G=⎢ ⎢0 0 0 1 ⎢ ⎣0 0 0 0 000 0

(9)

⎤ 0 −du 0 0 ⎥ ⎥ ⎥ 0 0 ⎥ ⎥ 0 0 ⎥ ⎥ 1 0 ⎦ 0 1

and du is the distance between two Revolute joints. Based on Sect. 3.1, the loadingdeformation relationship of each Revolute joint can be expressed as ⎡ (b) ⎤   (b)   (b) 0 xr r ⎦ = Kr ⎣ f (u) (10) (u) (u) u   0 Kr xr fr (b)

(u)

(b)

(u)

where f r and f r are node forces of two Revolute joints, and Kr , Kr are the stiffness matrices of two Revolute joints in local coordinate system of Universal joint. It should (b)

(u)

be noticed that f r and f r are not the external forces/torques loaded on the nodes of Universal joints. Substituting (10) into (9) leads to ⎡

⎤

⎡

K11 ⎢ K21 f r ⎦=⎢ ⎣ (u) ⎣ 0  fr 0 (b)

K12 K22 K32 K42

⎤⎡ (b) ⎤  0 x r1 ⎥ ⎢ 0 ⎥⎢ (b) ⎥ ⎥ r2 ⎦ K33 ⎦⎣ x (u)  x r2 K43

(11)

Stiffness Modeling of a Novel 2-DOF Solar Tracker

Besides, the external loadings of nodes can be divided as ⎡ (b) ⎤ ⎡ (b) ⎤ ⎡ (b) ⎤ ⎡ (b) ⎤ ⎡ (b) ⎤      o o i r1 ⎦ = ⎣ f r1 ⎦, ⎣ f r2 ⎦ = ⎣ f r2 ⎦ + ⎣ f r2 ⎦ ⎣ f (u)  (u) (u) (u) (u) of of if f r2 f r1 r2 r1 r1 (∗)

131

(12) (∗)

where o f rj (j = 1, 2) stands for the external loadings on each node and i f rj (j = 1, 2) stands for the forces/torques caused by rigid connection structure. The equilibrium equation of the rigid connection structure can be expressed as i

where

(b)

(u)

f r2 = J i f r1

⎡

1 ⎢ 0 ⎢ ⎢ ⎢ 0 J = −⎢ ⎢ 0 ⎢ ⎣ 0 −du

(13)

⎤ 000 0 0 0⎥ ⎥ ⎥ 0 0 0⎥ ⎥ 1 0 0⎥ ⎥ 0 1 0⎦ 001

0 0 1 0 0 1 0 du 0 0 0 0

Substituting (10) (13) into (12) leads to o

(b)

(u)

f r2 − J f r1 = o

⎡



(b)

of





K21 0 − J 0 K33

⎤



r1 ⎦ = K11 0 ⎣ (u)  0 K43 of r2

 (b)   x r1 (u)

x r2

 (b) x r1 (u)

x r2

(b)

+ (K22 − JK32 ) x r2

 K12 (b) x + K42 r2

(14)



(15)

Internal nodes of universal joint, that is node 2 of Rb and node 1 of Ru , usually do not have external loadings, so the following condition can be obtained o

(b)



(u)



f r2 = 0, o f r1 = 0

(16)

(b)

By substituting (16) into (14), x r2 can be written as (b)

x r2 = −(K22 − JK32 )

−1







K21 0 − J 0 K33

 (b)   x r1 (u)

x r2

(17)

132

Y. Song and J. Wu

By substituting (17) into (15), the loading-deformation condition of universal joint can be derived ⎡ (b) ⎤  (b)   of r1 r1 ⎣ (u) ⎦ = Ku x (u) (18)   of x r2 r2

where Ku is the stiffness matrix of universal joint, which can be expressed as         K12 K11 0 − Ku = (K22 − JK32 )−1 K21 0 − J 0 K33 K42 0 K43

(19)

Ku is a symmetric matrix with non-diagonal elements named coupling stiffness. Rank of this matrix is 4 which equals to the number of constraints that a universal joint offers in a mechanism. 3.3 Stiffness of Actuators The stiffness of actuators also plays a key role in the stiffness of solar tracker. They are treated as actuator elements, that is elements (14) and (15) in Fig. 3, of which only the rotational stiffness along the axis direction is considered and others are treated as rigid. Thus the stiffness of actuators can be expressed by rotation stiffness which satisfies the relationship Mi = ki  θi

(20)

where Mi is the constraining torque along the axis of revolute joint is the rotational deformation on the same direction. Ignoring compliance caused by electrical characteristic of motors, stiffness of the actuator equals to the rotational stiffness of motor axis, which can be expressed as ki =

Gi Iip li

(21)

where Gi is the shear modulus of the axis. Iip is polar moment of inertia. li is the effective length of axis which is half of the length of axis. 3.4 Stiffness of the Solar Tracker The solar tracker has many parts whose length is much bigger than width. These parts are modeled as 3-dimensional Eula-Bernoulli beam elements with two nodes located at the ends. In the novel parallel solar tracker, the parts corresponding to elements (5) and (16) have complicated shapes and they have relative high stiffness. To make modeling process simpler and more effective, these parts are treated as rigid body accompanied with the moving platform. Then, based on the stiffness of beam, actuators, revolute joint and universal joint elements, and by using substructure synthesis method, the stiffness matrix of the solar tracker can be obtained.

Stiffness Modeling of a Novel 2-DOF Solar Tracker

133

4 Simulation and Validation of the Stiffness Model 4.1 Stiffness Simulation Due to non-linear loading-deformation characteristic of revolute joints and universal joints on the novel parallel solar tracker, external loadings have to be determined according to weight and possible wind load on moving platform before stiffness simulation. Forces on X and Y directions are estimated to be 300 N while force on Z direction to be 600 N, and torques on X,Y and Z directions are all about 30 Nm. Node 4 is chosen as the loading point. The range of θo and ψo is [−70◦ , 0◦ ] and [−60◦ , 60◦ ], respectively, and the step of each angle is set to be 5◦ . ψo is rotational displacement of moving platform along w axis, and θo is rotational displacement of moving platform along u axis, as shown in Fig. 2. Newton-Euler irritation method is used to solve the non-linear problem. The stiffness of the novel parallel solar tracker at node 4 is shown in Fig. 5.

Fig. 5. Stiffness of the novel parallel solar tracker (a. translational stiffness on X direction b. translational stiffness on Y direction c. translational stiffness on Z direction d. rotational stiffness on X direction e. rotational stiffness on Y direction f. rotational stiffness on Z direction).

From Fig. 5, one may see that all the translational and rotation stiffness are symmetric about the plane ψo = 0. This coincides with the symmetric characteristic of structure. Besides, the stiffness of solar tracker is critically influenced by the configuration. This influence can be illustrated by changing-ratio which is defined as    kmax − kmin   × 100%  (22) hk =   kmax where kmax is the maximum stiffness and kmin is the minimum one in the whole workspace. Changing-ratio of stiffness on different directions is shown in Table 1. As Table 1 shows, each changing-ratio of stiffness is larger than 78% which proves the huge influence of configuration on stiffness.

134

Y. Song and J. Wu Table 1. Changing ratio of stiffness.

Stiffness direction

Changing-ratio/%

X-translational

78.3

Y-translational

93.9

Z-translational

99.3

X-rotational

85.2

Y-rotational

92.3

Z-rotational

88.0

4.2 Stiffness Experiment Stiffness experiment is carried out to evaluate the stiffness of the solar tracker. The experiment mainly measures the translational stiffness on Z direction. A loading mechanism is designed to exert specific force on the point corresponding to node 4 and displacement is measured by dial gage. The loading device consists of positioning mechanism, locking mechanism, loading mechanism and auxiliary loading platform, as shown in Fig. 6. Force monitor is not illustrated in the figure. Loading mechanism consists of loading screw and force sensor which are assembled together with screw pair. Loading screw exerts force on the solar tracker through auxiliary loading platform and the force sensor is installed on the slide of positioning mechanism which can move freely in the horizontal plane. Locking mechanism can prevent the motion of positioning mechanism which guarantee the accuracy and safety during loading process. Six configurations are chosen to compare the loading-deformation relationship between modeling and experiment, as shown in Fig. 7. From Fig. 7, the experimental result is close to the modeling one. The result proves the effectiveness of this method as the difference between experimental and modeling stiffness is in the acceptable extent. 1

3

2

6

4 5

Fig. 6. Stiffness of the novel parallel solar tracker (1. positioning mechanism 2. locking mechanism 3. force sensor 4. loading screw 5. auxiliary loading platform 6. dial gage).

Stiffness Modeling of a Novel 2-DOF Solar Tracker

135

Fig. 7. Stiffness of the novel parallel solar tracker

5 Conclusion In this paper, the stiffness modeling of a novel parallel solar tracker is investigated. An analytical method is introduced to consider coupling stiffness and derive non-diagonal stiffness matrix of universal joint. Besides, the MSA method is used to get the stiffness matrix of solar tracker. The simulation result illustrates the significant effect of configuration on the stiffness of solar tracker. The modeling method is validated by experiment whose loading-deformation relationship fit relatively well with the simulation one.

136

Y. Song and J. Wu

Appendix See Table 2. Table 2. Type of each element. Element number

Element type

(2) (4)

Universal joint element

(6) (7) (10) (11) (17) (18)

Revolute joint element

(3) (5) (16)

Rigid body

(14) (15)

Actuator element

(1) (8) (9) (12) (13)

Beam element

References 1. Anatol, P., Damien, C., Philippe, W.: Stiffness analysis of over constrained parallel manipulator. Mech. Mach. Theory 44(5), 966–982 (2009) 2. Selcuk, E., Selim, D., Saban, U.: Effects of joint clearance on the dynamics of a partly compliant mechanism: numerical and experimental studies. Mech. Mach. Theory 88, 125–140 (2015) 3. Wu, J., Yu, G., Gao, Y., Wang, L.P.: Modal analysis of practical quartz resonators using finite element method. IEEE Trans. Ultrason. Ferroelectr. Freq. Control 57, 292–298 (2010) 4. Liu, Y., Nikolay, V., Yu, ZP.: Dynamics and control of a planar 3-DOF parallel manipulator with actuation redundancy. Mech. Mach. Theory 44(4), 835–849 (2009) 5. Zhang, X., Zhang, X., Fu, W.N: Fast numerical method for computing resonant characteristics of electromagnetic devices based on finite-element method. IEEE Trans. Magn. 53, 1–4 (2017) 6. Liu, XJ., Bonev, I.A.: Orientation capability, error analysis, and dimensional optimization of two articulated tool heads with parallel kinematics. J. Manuf. Sci. Eng. Trans. ASME 130(1) (2008) 7. Gosselin, C.: Stiffness mapping for parallel manipulator. IEEE Trans. Robot. Autom. 6(3), 377–382 (1990) 8. Li., W.M.: Rigidity calculation of axial preload taper roller bearings. Bearing 5, 1–3 (2003)

A Kirigami-Inspired Transformable Robot Cheng Wang, Junlan Li(B) , Usman Haladu Garba, Ping Wang, Zhaocen Guan, Jiahao Zhang, and Dawei Zhang Key Laboratory of Mechanism Theory and Equipment, Design of Ministry of Education, School of Mechanical Engineering, Tianjin University, Tianjin 300054, China [email protected]

Abstract. This paper reports a novel kirigami-inspired transformable robot with the structure design and gait analysis. The transformation movement of this robot is inspired by the 10R thick panel kirigami model, which is based on the thick panel origami with cutting common crease lines proposed in our previous work. Due to the removal of common crease line, this kirigami robot can extend the range of movement and accomplish more missions. Based on the D-H notations, the kinematic model of the kirigami robot is established. Three kinds of robot gait are proposed, corresponding movement simulation and numerical results are discussed. The structure details are designed, and its prototype is fabricated by 3D printing. Furthermore, the experimental verification is carried out, which demonstrate that the kirigami robot can accomplish the transformable movement of gaits and traverse through the narrow and low-slung spaces. Keywords: Transformable robot · Thick panel kirigami · Kinematic model · Gait analysis

1 Introduction Transformable robots have drawn growing attention to researchers recently, which can adjust its structural morphology to adapt to complex tasks in multiple working environments and used for different applications such as medical, rescue, reconnaissance and space missions [1–3]. The transformable folding and deploying method is the key point to this kind of robot, which determines its movement form. Origami, a branch of transformable structure, provides new ideas for designing transformable robot [4, 5]. Up to date, many origami patterns have been used to develop transformable robots due to their large deployment ratio [6]. Felton et al. [7] proposed a three-dimensional self-folding crawling robot based on the origami folding method, which uses the heating element in printed circuit board (PCB) to drive the robot to accomplish self-folding movement. Lee et al. [8] designed and manufactured a variable-diameter wheel robot based on the Waterbomb origami pattern with large deployment ratio. The diameter of the robot wheels can be driven by shape memory alloy (SMA), which make the robot adapt to different working environments. Wood et al. [9] focused on a worm crawling robot, whose external skeleton is designed by Waterbomb origami pattern. This robot is also driven by SMA and accomplish the crawling gait through different supply plans. © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 137–146, 2021. https://doi.org/10.1007/978-3-030-89092-6_13

138

C. Wang et al.

Pagano et al. [10] designed a bionic crawling robot based on Kresling origami pattern. The body of this robot is an origami structure, which is driven by motors to accomplish overall folding and deploying, thereby completing the movement of the robot. Li et al. [11] proposed architecture for fluid-driven origami-inspired artificial muscles. Based on origami skeleton, these artificial muscles can be programmed to produce not only a single contraction but also complex multi-axial actuation, and even controllable motion with multiple degrees of freedom. Although the origami folding method enriches the transformable robot design, most of the existing researches focus on soft materials, rarely considering the rigidity and thickness of materials. In this paper, a novel kirigami transformable robot with rigid and thick panel is proposed based on the thick panel kirigami method, which is introduced in our previous work [12]. The paper is organized as follows. Section 2 introduces the 10R thick panel kirigami pattern and corresponding robot structures. Section 3 establishes the kinematic model of this kirigami model. The gaits analysis and numerical results are proposed in Sect. 4, and experimental test is conducted in Sect. 5. Finally, the conclusion is drawn in Sect. 6.

2 Kirigami Robot A novel kirigami robot with ten rotational (10R) hinges (which is called kirigami robot for short in this paper) is proposed in this paper, as shown in Fig. 1. This kirigami robot is inspired by thick panel kirigami method [12]. Different from origami, kirigami method allows the removal of crease lines, which increases the flexibility of the robot transformation.

Fig. 1. 3D schematic diagram of kirigami robot

Corresponding zero-thickness kirigami pattern is shown in Fig. 2a, where the kirigami pattern has 10 crease lines and is constructed by removing the common crease line A-B of a four-vertex origami pattern (combined by four one-vertex origami patterns with four crease lines and same sector angle γ ). The sector angles of each vertex are

A Kirigami-Inspired Transformable Robot

139

chosen to be same for the common collinear crease lines A-B. According to the axis-shift method for thick panel origami [13] and the removal of the common crease A-B, the corresponding thick panel kirigami model with 10 rotational hinges is determined as shown in Fig. 2b. The hinges for mountain and valley crease lines are arranged at the bottom and top surface of panel, respectively.

Fig. 2. Kirigami pattern with zero-thickness sheet (a) and thick panel (b).

In corresponding thick panel kirigami robot, the constraint of common hinges is released due to the removal of the crease line A-B, which lead to uniform panel thickness for all parts, defined as t. Thus, the characteristics of this thick panel kirigami model is that it has completely flat surface at fully deployed configuration, but more than one degree of freedom (DOF) during the movement.

3 Kinematic Modeling This kirigami robot can be regarded as a close-loop 10R spatial linkage in kinematic analysis, which is best illustrated by transformation matrix based on D-H notations. The transformation matrix between joint i and i + 1 is expressed as. ⎤ ⎡ cosθ i −cosα i(i+1) sinθ i sinα i(i+1) sinθ i ai(i+1) cosθi ⎢ sinθ i cosα i(i+1) cosθ i −sinα i(i+1) cosθ i ai(i+1) sinθi ⎥ ⎥ (1) Ti(i+1) = ⎢ ⎦ ⎣ 0 sinα i(i+1) cosα i(i+1) di 0

0

0

1

The kirigami model and its equivalent 10R linkage are shown in Figs. 3 and 4. Specifically, the geometry parameters γi(i+1) (i=1,2…,9,10, and i+1 is replaced by 1 when i+1 is greater than 10) represent the sector angle and t(i+1) represent the panel thickness, which are equal to γ and t, respectively. The kinematic parameters αi(i+1) represent the twist angles between axes Zi and Zi(i+1) , positive alone axis Xi ; ai(i+1) represent the distance between axes Zi and Zi(i+1) , positive along axis Xi ; θi is the angle from Xi to Xi(i+1) along the positive direction of Zi ; di is the normal distance from axis Xi to Xi(i+1) along the positively direction of Zi and ϕi represent the dihedral angle. Accordingly, the relationships of the robot geometry and kinematic parameters are determined in Table 1. Based on the relationship in Table 1, the closure equation of this close-loop spatial linkage can be expressed as: 1 T21 • T32 • T43 • T54 • T65 • T76 • T87 • T98 • T10 9 • T10 = I

(2)

140

C. Wang et al.

Fig. 3. Schematic diagram of kirigami robot with geometry parameters.

Fig. 4. Equivalent 10R spatial linkage with kinematic parameters

which can also be written as: 1 2 T76 • T87 • T98 • T10 9 • T10 = (T1 )

−1

• (T32 )

−1

• (T43 )

−1

• (T54 )

−1

• (T65 )

−1

(3)

where Ti(i + 1) (=T−1 i(i + 1) ) is the 4 × 4 transformation matrix between the coordinate system of link i(i−1) and link i(i + 1) for spatial linkages, and I 4 is a 4 × 4 unit matrix. By analytical calculation, it can be notice that the existing conditions in Table 1 cannot determine the unique solution of Eq. 2, therefore, this 10R kirigami robot has more than one DOF.

A Kirigami-Inspired Transformable Robot

141

Table.1 Kinematic and structure parameter relationships of kirigami robot. αi(i+1)

a i(i + 1)

1

α12 = π −γ 12

2

α23 = 0

a12 = t 12 a23 = t 23

3 4

α34 = γ 34 α45 = 0

a45 = t 45

5

α56 = π + γ 56

a56 = t 56

6

α56 = π −γ 67

7

α78 = 0

a67 = t 67 a78 = t 78

8

α89 = γ 89

a89 = 0

9

α910 = 0

a910 = t 910 a101 = t 101

Joint

a34 = 0

α101 = π + γ 101

10

4 Gait Analysis Gait analysis is the basis of robot movement. For this multi-DOF kirigami robot, based on the kinematic model and simulation method (using ADAMS simulation software), three kinds of gait and their numerical results are proposed. 4.1 Gait I Gait I is the fundamental gait for kirigami robot, which can accomplish the crawling movement along one direction by transforming its body. Here, in order to obtain the numerical result for Gait I, the following constrains are added: ϕ2 = ϕ3 = ϕ4 = ϕ5 = ϕ7 = ϕ8 = ϕ9 = ϕ10 = η1GaitI , ϕ1 = ϕ6 = η2GaitI

(4)

Substitute Eq. 4 into closure Eq. 2, the matrix can be simplified, but the elements in the matrix are still complex, thus, the analytical results are not listed here. Because the common hinge A-B is removed, there is no direct relationship between angles η1GaitI and η2GaitI , and the kirigami robot has two DOFs now. Since our thick panel kirigami method is derived from the corresponding thick panel origami, the movement of vertices A and B can be kept in the same movement form of the origami, which leads to a composite Gait I with the following constrain: ηGaitI

η2GaitI

tan( 22 ) ) = −2arctan( sin(γ − π2 )

(5)

where η1GaitI is regarded as the input value and η2GaitI is regarded as the output value. Then, the kirigami robot has only one degree of freedom, which can accomplish crawling movement. Based on the simulation, the numerical results are proposed in Fig. 5a, and

142

C. Wang et al.

corresponding moving sequence is shown in Figs. 5b−5d. It can be notice that the red curve represents the origami movement form because of Eq. 5. Furthermore, the kirigami robot can accomplish a wider movement range than the origami movement form because the hinge is removed, and considering the self-intersection during the folding sequence, the feasible region of kirigami movement is given as the red region in Fig. 5a.

Fig. 5. (a) Numerical result and (b)-(d) moving sequence of kirigami robot Gait I.

4.2 Gait II and Gait III On the basis of the extra kirigami movement in the feasible region, the vertical movement Gait II and horizontal movement Gait III can be determined. According to Eq. 4, when one of the dihedral angle parameters η1GaitI or η2GaitI is defined as a constant, the kirigami robot has only one DOF during movement. Specifically, the dihedral angles for Gait II satisfy: η2GaitII = −2arctan(

tan( c2II ) ) sin(γ − π2 )

(6)

π cIII )sin(γ − )) 2 2

(7)

and for Gait III satisfy: η2GaitIII = −2arctan(tan(

where cII and cIII represent the constant value of η2GaitII and η1GaitIII , respectively. Similarly, the numerical results and moving sequences of Gait II and Gait III can be obtained through the simulation method, as shown in Figs. 6 and 7, respectively. The parameters

A Kirigami-Inspired Transformable Robot

Fig. 6. (a) Numerical result and (b)-(d) moving sequence of kirigami robot Gait II.

Fig. 7. (a) Numerical result and (b)-(d) moving sequence of kirigami robot Gait III.

143

144

C. Wang et al.

W min , W max H min H max are proposed to describe the width and height range of vertical and horizontal movement of robot gait. These three gaits proposed above can accomplish different movements. Specifically, Gait II drives the robot to move in the vertical direction without changing the width and height of the robot, which is suitable to traverse the narrow space; Gait III leads the robot redefine the working space and move along the horizontal direction; Gait I can be regard as the combination of the Gaits II and III, which satisfies origami constraints and is the boundary movement that will not self-intersection. This kirigami robot with multi-DOF can also accomplish more movement gait, which will be discussed in our future work.

5 Experiment The kirigami robot consist of different components, where the mechanical components include body parts, crawling foots, bearings and connecting pins; the electrical components include motor, control board and power supply. The prototype of kirigami robot is fabricated by 3D printing, as shown in Fig. 8a. Specifically, each “leg” is connected by a steel bearing and supported by one “foot”, which has an uneven bottom surface to increase friction as shown in Fig. 8b. There are ten DC-motors mounted on the robot, where six of them are attached to the rotational joint to drive or to provide auxiliary torque for gait movement, and the other four motors are located on the feet to control the posture. Power, control and transmission systems are contained within the body of the kirigami robot.

Fig. 8. (a) Prototype of the kirigami robot, (b) detail of robot “Foot”.

The corresponding movement range of the robot is Max{W = 50 mm, H = 25 mm} and Min{W = 25 mm, H = 15 mm}. By using its gaits, the kirigami robot is required to traverse through vertical and horizontal narrow gaps, which are set to [W = 30 mm, H = 30 mm] and [W = 45 mm, H = 20 mm] (slightly bigger than the maximum size), respectively, to confirm the gait movement range of the kirigami robot. To traverse the vertical gap, Fig. 9 (a), the robot first folds its body to P1Gait II configuration then utilizes the locomotion Gait II to crawl through the gap. Similarly, for the horizontal gap, Fig. 9 (b), the robot deploys from P3Gait III configuration and then moves with only parameter η1Gait I varying, just like Gait II. The experiments demonstrated that the robot is capable of traversing through confined spaces.

A Kirigami-Inspired Transformable Robot

145

Fig. 9. (a) Traversing through a vertical gap with narrow space, and (b) traversing through a horizontal gap with low-slung space.

6 Conclusion In this paper, a novel kirigami-inspired transformable robot has been presented. The kinematic model is established by using the D-H notation. Based on the movement simulation and numerical results, three gaits are proposed and discussed that can change the configuration of robot and drive robot adapt to narrow and low-slung spaces. The prototype has been fabricated and experiments conducted show that the robot can traverse through a vertical gap with W = 50 mm, H = 20 mm and a horizontal gap with W = 30 mm, H = 30 mm. Future work will focus on multi robot gait movement and structure optimization of robot to adapt to more complex working environment. Moreover, due to the extensibility of kirigami pattern, the combination and network of the robots will be implemented to expand the application of robots.

References 1. Salemi, B., Moll, M., Wei-min, S.: SUPERBOT: a deployable, multi-functional, and modular self-reconfigurable robotic system. In: Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots & Systems, pp. 3636–3641. IEEE (2006) 2. Schmitz, D., Khosla, P., Kanade, T.: The CMU reconfigurable modular manipulator system. In: Carnegie Mellon University, 1988 Technical Report CMU-RI-TR-88–7 (1988) 3. Yim, M.: Locomotion With A Unit-Modular Reconfigurable Robot. In: Ph. D. Thesis, Department of Mechanical Engineering, Stanford University (1994) 4. Turner, N., Goodwine, B., Sen, M.: A review of origami applications in mechanical engineering. Proc. Inst. Mech. Eng. C J. Mech. Eng. Sci. 230(14), 2345–2362 (2016)

146

C. Wang et al.

5. Feng, H., Yang, M., Yao, G., Chen, Y., Dai, J.S.: Origami robots. . Scientia Sinica Technologica 48(12), 1259–1274 (2018) 6. Rus, D., Sung, C.: Spotlight on origami robots. Sci. Robot. 3(15), eaat0938 (2018) 7. Felton, S., Tolley, M., Demaine, E., et al.: A method for building self-folding machines. Science 345, 644–646 (2014) 8. Lee, D.Y., Jung, G.P., Sin, M.K., et al.: Deformable wheel robot based on origami structure. In: IEEE International Conference on Robotics & Automation, pp. 5612–5617. IEEE (2013) 9. Onal, C.D., Wood, R.J., Rus, D.: An origami-inspired approach to worm robots. IEEE/ASME Trans. Mechatron. 18(2), 430–438 (2013) 10. Pagano, A., Yan, T., Chien, B., et al.: A crawling robot driven by multi-stable origami. Smart Mater. Stru. 26(9), 094007 (2017) 11. Li, S., Vogt, D.M., Rus, D., et al.: Fluid-driven origami-inspired artificial muscles. In: Proceedings of the National Academy of Sciences of the United States of America, pp. 13132–13137 (2017) 12. Cheng, W., Junlan, L., Zhong, Y.: A kirigami-inspired foldable model for thick panels. In: The 7th international meeting on origami in science, mathematics and education, vol. 3, pp. 715–730. 7OSME (2018) 13. Lang, R.J., Tolman, K.A., Crampton, E.B., Magleby, S.P., Howell, L.L.: A review of thicknessaccommodation techniques in origami-inspired engineering. Appl. Mech. Rev. 70(1), 1–20 (2018)

Design and Simulation Analysis of a Magnetic Adsorption Mechanism for a Wall-Climbing Robot Jiankun Chen1,2,4 , Kai He1,2,4(B) , Haitao Fang1,2,4 , Junnian Liu3,4 , and Hailin Ma3,4 1 Shenzhen Institute of Advanced Technology, Chinese Academy of Sciences, Shenzhen 518055, China [email protected] 2 University of Chinese Academy of Sciences, Beijing 100049, China 3 Shenzhen Key Laboratory of Precision Engineering, Shenzhen, Guangdong, China 4 Yiulian Dockyards (Zhoushan) Limited, Zhoushan 316281, China

Abstract. In order to ensure the cargo clean and improve the service life of the cargo hold, the cargo hold needs to be cleaned and maintained regularly. In this paper, a wheeled wall-climbing robot for cargo hold cleaning is proposed, which can walk on multiple surfaces of cargo hold. Firstly, this paper introduces the mechanical design of the wall-climbing robot. To prevent the robot from skidding or overturning on the wall, the force analysis of the robot is carried out to make sure it can walk steadily. In addition, the characteristics of the magnetic attraction force when the robot transitions on the wall are analyzed, and the relationship between the magnetic attraction force of the front wheel and the rear wheel is determined. In order to obtain reliable magnetic adsorption force, the arrangement and installation position of magnets are optimized through simulation and analysis based on the Halbach array. Finally, a new magnetic adsorption device was developed for wall transition. Keywords: Wall-climbing robot · Magnetic adsorption · Wall transition · Magnetic simulation

1 Introduction At present, cleaning and maintenance of bulk carriers is one of the most important works in the shipping industry [1]. when the cargo ship is docked, it is necessary to clean the corroded cabin surface of the bulk carrier. Cargo hold cleaning mainly relies on shipyard workers standing on scaffolding with high-pressure water jets to clean the corroded walls, which is costly and inefficient [2]. Therefore, how to realize the automatic cleaning of cargo hold is an urgent task in the shipping industry. In recent years, magnetic adsorption wall-climbing robot has been widely used in ship cleaning industry, which has excellent reliability and high efficiency [3]. Considering the discontinuity of the interior surface of the cargo compartment and the angle between the walls, the wall-climbing robot is required to have the ability to crawl between different © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 147–157, 2021. https://doi.org/10.1007/978-3-030-89092-6_14

148

J. Chen et al.

walls. Today, most wall-climbing robots can only crawl on walls with low curvature or angle, and they cannot accommodate the large incline angle between different walls in the cargo hold [4].Therefore, it is very important to optimize the magnetic adsorption structure of the existing wall-climbing robots so that they can adapt to the more complex wall environment. At present, the wall-climbing robots used in metal wall cleaning are mainly composed of tracked robots and wheeled robots [5]. C. Park et al. designed and fabricated a separable modular wall-climbing robot called R-Track [6]. R-Track consists of three tracked wallclimbing robots, each of which is connected by magnetic connectors. Each module of the R-Track is securely connected by the magnetic force of the connector. And the modules can detach themselves by creating a velocity difference between them with the traction force from the magnetic tracks. Through the cooperation between modules, R-Track can perform all kinds of wall-to-wall transitions between perpendicular walls. However, R-Track does not have enough load capacity to install equipment for cabin cleaning and cannot achieve the purpose of engineering application. To adapt the robot to crawl on surfaces with small curvature, Kosuke Nagaya et al. developed a tracked vehicle whose main body is made of the right and left independent crawler which relates to the turn hinge of two degrees of freedom [7]. This magnetic track robot can only walk on the uniform wall surface, cannot transit on different walls. However, wheeled robots have more advantages than tracked robots because of their locomotion: fast travel speed, simple and rigid structure, control simplicity, etc. [8] For the robot to crawl on all kinds of curved surfaces, Haruhiko Eto and H. Harry Asada designed a wheeled wall-climbing robot with a magnetic adsorption mechanism that automatically adapts to the shape of the wall [9]. The magnetic adsorption device has two degrees of freedom and can change direction so that the magnetic force is always perpendicular to the contact surface. The control of the robot is further complicated by the fact that four servo motors are mounted beside each magnet to drive the rotation of each magnetic adsorption device. Wei Song et al. developed a wheeled wall-climbing robot with vacuum absorption [10]. By optimizing the magnet arrangement and magnetizing direction in the magnetic adsorption mechanism, the robot can obtain more efficient magnetic adsorption force, which can reduce the size of the magnet and the weight of the robot. However, the magnetic adsorption structure does not ensure that the robot can walk from one surface to another. To sum up, first, the robot used for cabin cleaning needs to be able to walk on all the walls of the cabin without slipping or overturning. The magnetic adsorption mechanism must provide sufficient force and the magnet should be as small as possible. In the paper, based on the Halbach array of the magnet, the shape, size, magnetization direction and arrangement of each magnet are readjusted. A new magnetic adsorption mechanism was developed. Compared with other magnet arrangement, the utilization of magnetic energy of the device is significantly improved. At the same time, a cargo hold cleaning robot (CHR) is designed, which has the characteristics of compact structure, flexible movement and strong load capacity. Finally, through the simulation and analysis of CHR’s magnetic adsorption force and wall transition process, CHR can stably crawl on each wall of the cargo hold.

Design and Simulation Analysis of a Magnetic Adsorption Mechanism

149

2 Robot Design and Static Analysis The interior of the ship’s cargo hold has a complex wall surface, including topside tank, bottom side tank, hopper tank and frames, shown in Fig. 1(a),(c). The widths between the frames are 600 mm to 1000 mm and the angle between the bevel and the vertical plane is up to 120° as shown in Fig. 1(b). Facing the harsh environment in the cabin, CHR needs to overcome the difficulties of the transition of multiple walls. Topside

Hopper

Tank

Tank

Frames

120°

Bottom side tank

600~1000mm (b)

(a)

(c)

Fig. 1. Cargo hold structure. (a) The top. (b) Cabin side. (c) Frame structure.

2.1 Design of Cargo Hold Cleaning Robot Figure 2 illustrates the design of CHR, including magnetic adsorption mechanism, walking mechanism, cleaning mechanism, etc. The walking mechanism is composed of two servo motors connected to a reducer. Two wheels with a diameter of 300mm are connected to the reducer through shafts. The magnetic adsorption mechanism is installed below the driving mechanism. It consists of several magnets arranged in a certain direction of magnetization. This mechanism can change the magnetic adsorption force by adjusting its height and angle to adapt to different metal wall surface. A universal wheel, made of permanent magnets, is installed at the bottom of the robot control box. The size of CHR is 1100 × 520 × 540 (mm), shown in Fig. 3. The width of the CHR is very narrow so that the robot can walk between the frames of the cabin. rocker

control box

walking mechanism 540mm

cleaning mechanism

magnetic adsorption mechanism

universal wheel

1100mm

520mm

Fig. 2. The 3D model of the CHR

The CHR is equipped with a rocker cleaning mechanism. The rocker arm has two degrees of freedom. First, its tail is connected to the motor and the reducer to achieve

150

J. Chen et al.

horizontal rotation. Second, it has an electric push rod on its head, which allows it to rotate vertically. When CHR passes through the wall at a certain angle, to prevent the cleaning head from hitting the wall, it can lift the cleaning mechanism through the electric push rod, shown in Fig. 3. wall

electric push rod

(c)

(b)

(a)

(d)

Fig. 3. The process of lifting the rocker arm. (a) Activate the electric putter. (b) Lift the rocker. (c) CHR climbs the wall. (d) Down rocker.

2.2 Static Analysis To determine the required magnetic adsorption force, a mechanical model of CHR was established, shown in Fig. 4. y

f1

F1

Fj

x

N1 f2

l1

F2

l2

l3 β

A

G

N2

l4

Fig. 4. Force analysis of CHR on wall surface

To avoid skidding, the friction force should conform to the following formula: f1 + f2 ≥ G cos β

(1)

f1 = μN1 , f2 = μN2

(2)

The force of CHR on the x-axis direction is: F1 + F2 = Fj + N1 + N2 + G sin β

(3)

√ Fj = 0.745Q pj

(4)

Design and Simulation Analysis of a Magnetic Adsorption Mechanism

151

According to the formula (1), (2) and (3), the adsorption force satisfies the following formula: 1 (5) F1 + F2 ≥ G cos β + G sin β + Fj μ where F 1 is the sum of adhesive forces from magnetic adsorption mechanism of CHR, F 2 is magnetic adsorption force from the universal wheel, f1 is the sum of friction force between driving wheels and surface, f 2 is the sum of friction force between the universal wheel and surface, G is the gravity of CHR, N 1 is the sum of supporting force of at driving wheels, N 2 is the sum of supporting force at the universal wheel, and F j is the water jet reaction force, pj (MPa) is operation pressure of water jet, Q(L/min) is the flow rate of water jet, β is the inclination angle between surface and vertical direction, μ is the friction coefficient between CHR and the surface. When CHR climbs on inclined walls for cleaning, the robot is prone to overturning at point A. CHR is required to have the ability to resist the overturning. The following formula should be conformed: F1 l1 ≥ Fj l2 + Gl3 sin β + Gl4 cos β + N1 l1

(6)

When the robot is about to overturn, it is known that value of N 1 equals zero. Formula (6) simplified to: F1 ≥

Fj l2 + Gl3 sin β + Gl4 cos β l1

(7)

where l1 is the distance between the contact point of the driving wheel on the wall surface and the overturning point A; l2 is the distance between the reaction (F j ) point and point A along the Y-axis direction; l3 and l 4 are the distance between the center of gravity and point A along the Y-axis direction and the X-axis direction, respectively. Table 1. The Parameters of the CRH Parameter

Value

G

800 N

pj

280 MPa

Q

40 L/min

μ

0.3

l1

633 mm

l2

1014 mm

l3

456 mm

l4

271 mm

Table1 lists the values of various parameters for CHR. The relationship between the adsorption force (i = 1,2) and inclination β can be obtained by substituting the values of these parameters into formula (5) and (7), as shown in Fig. 5.

152

J. Chen et al.

According to the two extreme points A and B on the adsorption force change curve as shown in Fig. 5, the magnetic adsorption force shall conform to the following formula: F1 + F2 ≥ 3284N , F1 ≥ 1471N

A (18,3284)

B (60,1471)

(8)

F1+F2 F1

Fig. 5. Change curve of adsorption force with β.

3 Wall Transition Analysis When CHR is working in the cargo hold, as shown in Fig. 6 (a), it can walk on multiple surfaces of the cargo hold, such as hopper tanks, bottom side tanks, and topside tanks. Especially, it can walk on the wall with a maximum angle of 120°. Therefore, CHR must overcome the difficulty of transition between multiple walls. The magnetic adsorption capacity of CHR should be optimized so that the magnetic force on CHR not only acts on the walking wall but also on the front transition wall. To determine the magnetic force of the robot transitioning between the various walls, a mechanical analysis is carried out on the transition process of the robot on the wall with a maximum angle of 120°, shown in Fig. 6 (b).

Topside

Frames

120°

Tank

Hopper Tank

v Botttom side tank (a)

(b)

Fig. 6. Wall transition analysis of robot. (a) The walking process of CHR in the cargo hold. (b) Mechanical analysis of CHR transition on surface

Design and Simulation Analysis of a Magnetic Adsorption Mechanism

153

The CHR should be subjected to a magnetic force F a along the normal line of the front wall, which provides a sufficient friction force f a to drive the robot upwards. During the transition process, the robot mainly moves along the y-axis. Ignore the displacement of the robot in the x-axis direction. The magnetic force F a conforms to the following formulas (9) and (10). fa + N1 cos 30◦ + f1 sin 30◦ + N2 cos 30◦ ≥ F1 cos 30◦ + F2 cos 30◦ + f2 sin 30◦ + G sin 30◦ Fa + f1 cos 30◦ + F1 sin 30◦ + F2 sin 30◦ = Na + N1 sin 30◦ + N2 sin 30◦ + f2 cos 30◦ + G cos 30◦

(9) (10)

Considering the critical situation, the driving wheel of the CHR will be separated from the vertical wall. Currently, the supporting force N 1 is equal to zero. The supporting force N 2 received by the universal wheel is approximately equal to the adsorption force F 2 . Simplify formulas (9) and (10) to obtain the following formula: fa ≥ F1 cos 30◦ + f2 sin 30◦ + G sin 30◦

(11)

fa = μa Fa , f2 = μF2

(12)

The value of μa is 0.7, which is the coefficient of friction between the drive wheel and the wall. Finally, F a should conform to the following formula: Fa ≥ 1.24F1 + 0.21F2 + 400

(13)

4 Design of Magnetic Circuit 4.1 Design of Magnetic Attraction Mechanism The design goal of the magnetic attraction mechanism is to make the attraction force F a generated by the magnet satisfy the formula (13). Therefore, it is necessary to optimize the arrangement and magnetization direction of the magnets. The Halbach array has extremely high magnet utilization, and one side of it can generate a very strong magnetic field. Based on this magnet structure, a magnetic attraction mechanism is designed, as shown in Fig. 7. Because of its circular shape as shown in Fig. 7(a),The mechanism can generate an adsorption force to the front wall. According to formula (13), this force is much greater than F 1 . The magnetizing direction of the magnet is determined based on the characteristics of Halbach array as shown in Fig. 7(b). The size of the magnets on the head and tail of the magnetic attraction mechanism are different. Three lengths of magnets are designed, and the size of the head is larger than the tail, as shown in Fig. 7(c).

154

J. Chen et al. magnet shell driving wheel

yoke

head

Fa

F1

tail

magnetization direction

(a)

(c)

(b)

Fig. 7. Magnet arrangement and magnetizing direction. (a) Installation position of mechanism. (b) The magnetization direction. (c) Dimensional characteristics of the magnets

rotate metal wall

A

100

θ

F

F

Fig. 8. Simulation process of the maximum position of the magnetic force of the Halbach array

4.2 Installation Position of the Magnet On the other hand, considering the influence of the installation angle of the magnet on the adsorption force, it is important to find the position with the highest magnetic field strength in the Halbach array. By building a simple Halbach array, as shown in Fig. 8, simulate it in ANSYS to find the relationship between the rotation angle θ and the adsorption force F. Five different magnets are arranged as a Halbach array in the magnetizing direction shown in Fig. 8. The entire array rotates counterclockwise around point A. The angle of each magnet is 20°, and the angle of the entire array is 100°. The iron plate is placed below the magnet. There is a magnetic force F between the iron plate and the magnet. Finally, the function curve of force F changing with angle θ is obtained, as shown in Fig. 9. The magnetic adsorption mechanism designed in this paper is composed of two groups of curved Halbach array. In order to make each group produce the maximum magnetic adsorption force on the wall, the installation position of the mechanism needs to be determined. According to Fig. 9, when the rotation angle of the magnet is 20°, the magnetic attraction force has a minimum value, and when the rotation angle is 50°, the magnet attraction force reaches the maximum value. When the middle position of the Halbach array is perpendicular to the wall, the magnetic field intensity is the largest. Therefore, the magnet is installed in the position shown in Fig. 10. The middle positions of the two Halbach arrays are respectively perpendicular to the 120° wall.

Design and Simulation Analysis of a Magnetic Adsorption Mechanism

155

50° maximum

20° minimum

Fig. 9. Change curve of magnetic attraction force on Halbach array.

120°

Fig. 10. The installation position of the attraction mechanism on the wheel

5 Simulation Results By simulating the magnetic attraction mechanism at this position, the magnetic field density distribution is obtained, as shown in Fig. 11(a). The magnetic field intensity is mainly concentrated in the position close to the wall, and the magnetic intensity density at the front is greater. The distribution of the magnetic field intensity is consistent with the magnetic characteristics when the robot is transitioning on the wall. In the process of wall transition, there are two main adsorption forces on the transition wall. It can be seen from Fig. 11 that the magnetic flux density on the front wall is larger than that on the bottom wall. So, the force F a is greater than the force F 1 .

Fa F1 (a)

(b)

Fig. 11. (a) Magnetic field density distribution; (b) Change curve of magnetic attraction force with distance x.

156

J. Chen et al.

The distance between the magnet and the wall also affects the magnetic attraction force. If this distance is too close, the flexibility of the climbing robot will be reduced. However, if the distance is too large, the adsorption force between the robot and the wall surface decreases, resulting in the robot falling off from the wall surface. The relationship between magnet attraction force F a , F 1 and distance x is obtained through simulation, as shown in Fig. 11(b). F a and F 1 gradually decrease as the distance x increase as shown in Fig. 11(b). To determine the optimal distance x, the value of F 2 is set to 2000N. According to formula (13), F a and F 1 conform to the following relationship: Fa ≥ 1.24F1 + 820

(14)

In addition, a gap between the magnet and the wall is needed to ensure that the CHR has a certain ability to overcome obstacles. In summary, when the distance x is equal to 8 mm, the above conditions can be met. The value of F a is 6674N, and the value of F 1 is 4552N. The forces F a and F 1 are the magnetic adsorption forces generated at the beginning of the transition. In the process of wall transition, the robot crawled to the front of the wall and the magnetic adsorption mechanism was farther and farther from the bottom of the wall, while the adsorption force with the front of the wall was also changing. Therefore, it is necessary to carry out simulation experiments on the force of magnetic adsorption mechanism in the transition process. Through simulation, it can be seen that the relationship between the adsorption force on the wall and the transition Angle α is shown in Fig. 12.

Fig. 12. Adsorption force during wall transition

In Fig. 12, F a2 is the adsorption force on the front wall and F 12 is the adsorption force on the bottom wall during the transition process. According to the simulation results, both F 12 and F a2 are decreasing steadily. When the transition Angle α is equal to 5°, the force F 12 decreases to zero. When the robot is transitioning on the wall, F a2 is always a safe force, which is maintained at about 4000N. Therefore, under the action of this magnetic adsorption mechanism, the robot can complete the transition of the wall surface and the robot will not slip or fall off during this process.

Design and Simulation Analysis of a Magnetic Adsorption Mechanism

157

6 Conclusion CHR is a wheeled wall-climbing robot aimed at cleaning cargo holds of ships. CHR has the ability to walk on walls with an included angle of up to 120°. By establishing the mechanical model of the robot, the relationship of the magnetic attraction force required for the robot to walk is obtained. Based on this relationship, this paper designs a new type of magnetic attraction mechanism and optimizes the arrangement and installation position of the magnets. Through the magnetic simulation, the best adsorption force required by the robot when the robot transitions on the wall is finally obtained. So far, the robot has completed the theoretical design. In the next step, a prototype of CHR will be made for experiments. The actual change process of magnetic attraction force will be studied through experiments. And the robot wall transition ability will be confirmed. Acknowledgement. This paper is partially supported by Science and Technology Planning Project of Guangdong Province (2017B090914004), CAS-HK Joint Laboratory of Precision Engineering, and NSFC-Shenzhen Robot Basic Research Center project (U1713224).

References 1. Adland, R., Cariou, P., Jia, H., Wolff, F.-C.: The energy efficiency effects of periodic ship hull cleaning. J. Clean. Prod. 178, 1–13 (2018) 2. Eich, M., Vögele, T.: Design and control of a lightweight magnetic climbing robot for vessel inspection. In: 2011 19th Mediterranean Conference on Control & Automation (MED), Corfu, Greece, pp. 1200–1205 (2011) 3. Prabakaran, V., et al.: Hornbill: a self-evaluating hydro-blasting reconfigurable robot for ship hull maintenance. IEEE Access 8, 193790–193800 (2020) 4. Xu, Z., Xie, Y., Zhang, K., Hu, Y., Zhu, X., Shi, H.: Design and optimization of a magnetic wheel for a grit-blasting robot for use on ship hulls. Robotica 35(3), 712–728 (2017) 5. Iborra, A., et al.: A cost-effective robotic solution for the cleaning of ships’ hulls. Robotica 28(3), 453–464 (2010) 6. Park, C., Bae, J., Ryu, S., Lee, J., Seo, T.: R-track: separable modular climbing robot design for wall-to-wall transition. IEEE Robot. Autom. Lett. 6(2), 1036–1042 (2021) 7. Nagaya, K., Yoshino, T., Katayama, M., Murakami, I., Ando, Y.: Wireless piping inspection vehicle using magnetic adsorption force. IEEE/ASME Trans. Mechatron. 17(3), 472–479 (2012) 8. Xu, Y., et al.: A novel design of a wall-climbing robot and experimental study on magnetic wheels. In: 2021 International Conference on Computer, Control and Robotics (ICCCR), Shanghai, China, pp. 60–65 (2021) 9. Eto, H., Asada, H.H.: Development of a wheeled wall-climbing robot with a shape-adaptive magnetic adhesion mechanism. In: 2020 IEEE International Conference on Robotics and Automation (ICRA), Paris, France, pp. 9329–9335 (2020) 10. Song, W., Jiang, H., Wang, T., Ji, D., Zhu, S.: Design of permanent magnetic wheeltype adhesion-locomotion system for water-jetting wall-climbing robot. In: Advances in Mechanical Engineering, July 2018

Design of a Variable Compliance Mechanism with Changeable Compliance Orientation Junjie Du, Xianmin Zhang(B) , Benliang Zhu, Hai Li, and Weijian Zhong South China University of Technology, Guangzhou 510640, China [email protected]

Abstract. Variable compliance mechanisms are considered as an effective way for interaction with changeable environments. Existing designs of variable compliance mechanisms can only change the amplitude of compliance rather than the orientation of compliance. In this paper, a novel hybrid mechanism is designed to realize changeable compliance orientation in a large range. The mechanical structure and working principle of the designed mechanism are firstly presented. Then, the closed-form relationship between compliance orientation and configuration of the designed mechanism is analyzed. Spherical coordinates are used to characterize and evaluate the orientation of compliance. Theoretical analysis shows that the orientation of compliance can be changed in a large range spatially. Finite element analysis is also performed to verify the theoretical model. It shows that the orientations of deformation during simulations are close to the theoretical compliance orientations. Under unchanged loading conditions, a large change in deformation orientation can be achieved by changing the mechanism’s configuration. This indicates that the designed mechanism can change the compliance orientation in a large range. Keywords: Variable compliance mechanisms · Changeable compliance orientation · Flexure mechanisms

1 Introduction Interaction with different environments has become the major topic for robotics research [1]. Although Traditional robotics researchers develop rigid robots to ensure motion accuracy and avoid deformation, compliance seems to be an efficient solution for interaction with environments [2], especially when deformation occurs. Therefore, integrating compliance into robot designs has become a promising way to increase robustness. For example, series elastic actuators use compliant elements to realize stable force control during contacts [3, 4]. Soft robots use fully compliant structures to increase the robustness and safety during grasping tasks [5]. One major limitation on applying compliance in robotics is that environments would generally change but the compliance of structures cannot be changed after manufacture. This greatly limits the application of compliant robots since different compliance is needed for different environments [6]. Variable compliance mechanisms offer an elegant way to solve this problem. By changing some property (e.g. length, position and © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 158–168, 2021. https://doi.org/10.1007/978-3-030-89092-6_15

Design of a Variable Compliance Mechanism

159

orientation) of internal parts, the compliance of such mechanisms can be changed to fit the environments [7]. Several principles are used to design variable compliance mechanisms. The antagonistic variable compliant mechanisms combine different nonlinear springs to imitate the basic structures of human muscles [8]. By controlling the preloads on these springs, the output compliance can be controlled. Another kind of variable compliance mechanism utilizes the principle of variable effective lengths [9]. Such kind of mechanisms mainly includes several beams where the compliance is largely related to their effective lengths. Similarly, by controlling effective lengths, the compliance of the mechanisms can be controlled. Mechanisms designed using such principle have been widely applied in human-robot interactions [10, 11], where larger compliance is needed when interacting with human objects to ensure safety and medium compliance is needed during manipulations. The compliance property of a general spatial mechanisms includes amplitude (the force-deformation relationship) and orientation (the orientations where deformation occurs). Although mechanisms designed utilizing the above principles can generally control the amplitude of compliance, the orientation of compliance cannot be precisely controlled. For applications where orientation of compliance matters (e.g. motion-guided compliant mechanisms [12] and compliant joints [13]), such design principles cannot be simply applied. It seems that designing mechanisms with variable compliance orientation is of great importance to increase the flexibility of existing variable compliance mechanisms. In this study, we present the design and conceptual analysis of a variable compliance mechanism that can change the orientation of compliance. Cross-spring flexures (CSFs) and parallelogram flexures (PFs) are used as compliant joints. Rigid joints are integrated such that internal degree-of-freedoms (DOFs) exist. By using the internal DOFs to control the positions and orientations of the CSFs, compliance orientation of the designed mechanism can be controlled. Conceptual analysis and finite element analysis (FEA) are performed for the designed mechanism to evaluate the ability of changing the orientation of compliance. Result shows that the designed mechanisms reserve one compliant DOF and the orientation of the compliant DOF can be controlled in a spatial manner with large range.

2 Mechanical Design and Basic Principle 2.1 Mechanical Design The Designed Mechanism is a Rigid-Compliant Coupling System Which Contains Both Compliant Flexures and Rigid Joints. The Meanings of DOF Are Different for RigidBody Mechanisms and Compliant Mechanisms. To Avoid Confusion, in This Paper, We Use Rigid-Degree-Of-Freedom (RDOF) to Represent the Motion Achieved by Rigid Joints Without Deformation. The Compliant-Degree-Of-Freedom (CDOF) is Used to Represent the Motion Caused by Materials Deformation with Large Compliance. With these notations, the designed mechanism is a 1-CDOF prismatic mechanism where the orientation of CDOF can be changed in a spatial manner. The schematic diagram of the designed mechanism is shown in Fig. 1. It is composed of two identical 2-DOFs prismatic compliant mechanisms (named 1/2-mechanisms as

160

J. Du et al.

shown in Fig. 1). Each of the 1/2 mechanism is composed of two identical serialconnected mechanisms (named 1/4-mechanisms as shown in Fig. 1). Exploded view of the 1/4-mechanism is shown in Fig. 2 to illustrate the basic structure. A 1/4-mechanism consists of two rigid joints and two compliant joints which are serially connected. Those joints include the adjusting revolute joint for changing the orientation of compliance, the CSFs as compliant revolute joint [14], passive revolute joint and the PFs as compliant prismatic joint [15]. The axes of adjusting revolute joint and passive revolute joint are colinear. This introduces internal rotational DOF such that the orientation of the CSFs (or the compliant revolute joint) can be changed. 2.2 Basic Principle The schematic diagram of a 1/2-mechanism is shown in Fig. 3. Since two 1–4 mechanisms are parallel connected in a 1/2-mechanism, there are two adjusting revolute joints, two passive revolute joints and two compliant revolute joints. The axes of all these joints are parallel and form a planar antiparallelogram mechanism (see Fig. 4). It is known that when ϕ1 = ϕ2 = θ , the planar antiparallelogram mechanism has a instantaneous translational CDOF S1 perpendicular to all the revolute joints.

Fig. 1. Schematic diagram of the variable compliance mechanism and the CDOFs orientation.

Further, the PFs works similarly to a translation joint and the corresponding joint axis is colinear with those revolute joints. This introduce another translational CDOF S2 which is perpendicular to the aforementioned translational CDOF S1 formed by the antiparallelogram mechanism. Geometrically, these two CDOFs form a plane named CDOF plane P and represent all the available translational orientation of the 1/2-mechanism. The orientation of S2 remains constant while the orientation of S1 is controlled by the adjusting angle θ . In coordinate frame {B}, S1 and S2 can be represented as B

S 1 = (cosθ, sinθ, 0)T

(1)

Design of a Variable Compliance Mechanism

161

Fig. 2. Exploded view of the 1/4-mechanism

B

S 2 = (0, 0, 1)T

(2)

The plane P is formed by S1 and S2 . The governing equations of P are P : xsinθ + ycosθ = 0

(3)

Fig. 3. The CDOFs of a 1/2-mechanism. When ϕ1 = ϕ2 = θ, the 1/2 mechanism contains two translational CDOFs S 1 and S 2 .

The proposed mechanism is connected by two 1/2-mechanisms in parallel. θ1 and θ2 are used to denote the corresponding adjusting joint angles of the first and second 1/2 mechanism, respectively. The freedom and constraint topology [16, 17] of general parallel compliant mechanisms states that the DOF set of the end platform is the intersection of the DOF sets of each kinematic chain. Although the proposed mechanism is not a purely compliant mechanism, this principle can be applied. Therefore, the CDOF of the end platform is a translational motion along the intersection line of the two CDOF planes (Fig. 5). A global coordinate frame {O} is attached at the middle of the end platform. {B1 } and {B2 } are used to denote the corresponding coordinate frames in two 1/2 mechanisms.

162

J. Du et al.

Fig. 4. The antiparallelogram mechanism formed by the revolute joints of a 1/2 mechanism. When ϕ1 = ϕ2 = θ , the resultant CDOF S1 is a translational motion.

Fig. 5. The coordinate system and CDOF orientation of the proposed variable compliance mechanism.

The corresponding coordinate transformation matrices are   O RO p O Bi Bi , i = 1, 2 TBi = 0 1

(4)

Design of a Variable Compliance Mechanism

163

Fig. 6. The principle of changing the compliance orientation by changing θ1 and θ2 (The red arrow denotes the translational CDOF of the mechanism). (Color figure online) π π π T T O O O Where RO B1 = Rz (− 2 )Rx ( 2 ), pB1 = (0, 0, d ) , RB2 = Ry ( 2 ), pB2 = (0, 0, d ) . In the coordinate frame {O}, the corresponding CDOF orientation of the first 1/2 mechanism, S 1 and S 2 are O

S 1 = (RO B1 ) O

TB

S 1 = (sinθ1 , 0, cosθ1 )T

S 2 = (RO B1 )

TB

S 2 = (0, 1, 0)T

(5) (6)

Similarly, the CDOF orientation of the second 1/2 mechanism, S 3 and S 4 are O

TB

S 3 = (RO B2 ) O

S 1 = (0, sinθ2 , cosθ2 )T

S 4 = (RO B2 )

TB

S 2 = (1, 0, 0)T

(7) (8)

The corresponding CDOF planes of the two 1/2 mechanisms P1 and P2 are P1 : xcosθ1 + zsinθ1 = 0

(9)

P2 : ycosθ2 + zsinθ2 = 0

(10)

It can be seen from Eq. (13) that the orientation of the two CDOF planes are governed by the adjusting joint angle θ1 and θ2 . Further, P1 rotates about the y-axis in {O} as θ1 changes while P2 rotates about the x-axis as θ2 changes. The intersection line of P1 and P2 , which is denoted as S, can be found as O

T 1 S= (−tanθ1 , −tanθ2 , 1, 0, 0, 0) tan2 θ1 + tan2 θ2 + 1

(11)

164

J. Du et al.

Similarly, the CDOF orientation of the end platform is governed by both ϕ1 and ϕ2 . As θ1 and θ2 change, S rotates around the origin of {O} in a spatial manner. Figure 6 shows how the CDOF orientation can be changed by changing θ1 and θ2 . To make geometrical intuition of the orientation of CDOF, spherical parameters α and β are used to parametrize the resulting CDOF, as shown in Fig. 7. For − π2 < θ1 < π2 and − π2 < θ2 < π2 , the relationship between the adjusting angles θ1 , θ2 and the CDOF orientation parameters α and β are α = atan2(−tanθ2 , −tanθ1 )  β = acos

1 2 tan (θ1 ) + tan2 (θ2 ) + 1

(12)  (13)

And θ1 = −atan2(cosβ, sinβcosα)

(14)

θ2 = −atan2(cosβ, sinβsinα)

(15)

Fig. 7. CDOF orientation of the variable compliance mechanism (red arrow) and the spherical parameters for parametrize the CDOF orientation.

3 Finite Element Analysis Finite element analysis is performed using Abaqus to validate the proposed mechanism. The corresponding geometry parameters for CSFs are thickness tC = 0.2 mm, width bC = 7.7 mm and length lC = 22 mm. The geometry parameters of PFs are tp = 0.2 mm, width bp = 20 mm length lb = 20 mm (see Fig. 8). Other geometry parameters are α = π4 , l0 = 30 mm, l1 = 13 mm, l2 = 12 mm, l3 = 28 mm. Other parts of the mechanism are assumed not to contribute a lot to the deformation. The Young’s module

Design of a Variable Compliance Mechanism

165

Fig. 8. The geometry parameters of CSFs and PFs used in simulations.

of materials used in the simulation is 192GPa while the poison’s ratio is assumed to be 0.35. In the finite element model, a constant force is applied at the middle of the end platform along the −z axis. The amplitude of the applied force is 10N . Static force analysis is performed and the deformation twist of the end platform is collected. Also, simulations are performed for different combinations of adjusting angles θ1 and θ2 as shown in Fig. 9.

Fig. 9. Simulation configuration for the variable compliant mechanism.

After obtaining the deformation of the end platform δ = (δx , δy , δz ), the correspond spherical coordinate αD and βD can be obtain as αD = atan2(δy , δx )

(16)

βD = acos(δz /δ)

(17)

166

J. Du et al.

π rad , θ = π rad and Fig. 10. Deformed state of the variable compliant mechanism for θ1 = 12 2 12 π rad , θ = − π rad . The deformations of end platform are in different orientations (the θ1 = 12 2 6 deformation is scaled by 20).

The spherical coordinates of the computed compliance orientation are then obtained according to Eq. (14–15) based on θ1 and θ2 . For each combination of θ1 and θ2 , the spherical coordinates of the computed compliance orientation and simulation deformation are compared. The results are shown in Table 1 while the deformed configurations are shown in Fig. 10. Noted that the deformation orientation and compliance orientation would not generally equal since the compliance of all directions would influence the deformation. In Table 1, it is found that for the same applied force (along −z axis), the deformation orientation changes largely for different adjusting angles. For θ1 = π/12 and θ2 = π/6, αD = −2.076 while for θ2 = −π/6, αD changes to 1.926. Noted that for θ1 = π/12 and θ2 = π/2, the compliance orientation computed using Eq. (13) is (010000)T and force along −z axis should contribute no deformation. The applied force in this simulation is changed to along the y axis. Simulation results shows that (1) spherical parameters of the deformation orientations αD and βD change when the adjusting angles change, which verifies that the designed mechanism has changeable compliance orientation. (2) the deformation orientations obtained by simulation are closed to the compliance orientations computed using Eq. (14–15), which verifies that the orientation of compliance can be controlled

Design of a Variable Compliance Mechanism

167

by changing the adjusting angles. (3) the orientation of compliance can be controller in a large range (α ∈ [0, 2 π ) and β ∈ [0, π2 ]). Table 1. Simulation displacements and corresponding theoretical orientations. Adjusting angles Simulation displacement (rad ) (mm)

Simulation Computed deformation compliance orientation (rad ) orientation (rad )

π θ1 = 12 θ2 = π6

αD = −2.076 βD = 0.540

αC = −2.005 βC = 0.567

αD = 1.926 βD = 0.476

αC = 2.005 βC = 0.567

αD = 1.936 βD = 0.766

αC = 1.832 βC = 0.803

αD = −1.570 βD = 1.502

αC = −1.571 βC = 1.571

π θ1 = 12 θ2 = − π6 π θ1 = 12 θ2 = − π4 π θ1 = 12 θ2 = π2

x

4.73 × 10−2

y

8.51 × 10−2

z

−1.62 × 10−1

x

3.25 × 10−2

y

−8.88 × 10−2

z

−1.84 × 10−1

x

2.38 × 10−2

y

6.25 × 10−2

z

−7.03 × 10−2

x

−1.50 × 10−6

y

7.26 × 10−2

z

5.00 × 10−6

4 Conclusion In this paper, the design of a variable compliance mechanism with variable compliance orientation is presented. Degree-of-freedom analysis are performed to formalize the relationship between the adjusting angles and the orientation of compliance. Finite element analysis is also performed to verify the correctness of theoretical results. It is found that by appropriate reorienting the compliant joints, the orientation of compliance can be controlled in a large range. Acknowledgements. This research was supported by the National key technologies R&D program of China (Grant No. 2018YFB1307800), the Guangdong Basic and Applied Basic Research Foundation (Grant Nos. 2021B1515020053, 2021A1515012418).

References 1. Goodrich, M.A., Schultz, A.C.: Human-Robot Interaction: A Survey. Now Publishers Inc., Hanover (2008)

168

J. Du et al.

2. Kau, N., Schultz, A., Ferrante, N., et al.: Stanford doggo: an open-source, quasi-direct-drive quadruped. In: 2019 International Conference on Robotics and Automation (ICRA), pp. 6309– 6315. IEEE (2019) 3. Hutter, M., Gehring, C., Bloesch, M., et al.: StarlETH: a compliant quadrupedal robot for fast, efficient, and versatile locomotion. In: Adaptive Mobile Robotics, pp. 483–490 (2012) 4. Pratt, G.A., Williamson, M.M.: Series elastic actuators. In: Proceedings 1995 IEEE/RSJ International Conference on Intelligent Robots and Systems. Human Robot Interaction and Cooperative Robots, vol. 1, pp. 399–406. IEEE (1995) 5. Wang, N., Ge, X.D., Guo, H., et al.: A kind of soft pneumatic actuator based on multi-material 3D print technology. In: 2017 IEEE International Conference on Robotics and Biomimetics (ROBIO), pp. 823–827. IEEE (2017) 6. Aktas, B., Howe, R.D.: Flexure mechanisms with variable stiffness and damping using layer jamming. In: Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems (2019) 7. Morrison, T., Su, H.J.: Stiffness modeling of a variable stiffness compliant link. Mech. Mach. Theory 153, 104021 (2020) 8. Petit, F., Chalon, M., Friedl, W., et al.: Bidirectional antagonistic variable stiffness actuation: analysis, design & implementation. In: 2010 IEEE International Conference on Robotics and Automation, pp. 4189–4196. IEEE (2010) 9. Xie, Z., Qiu, L., Yang, D.: Design and analysis of a variable stiffness inside-deployed lamina emergent joint. Mech. Mach. Theory 120, 166–177 (2018) 10. Hyun, D., Yang, H.S., Park, J., et al.: Variable stiffness mechanism for human-friendly robots. Mech. Mach. Theory 45(6), 880–897 (2010) 11. Eiberger, O., Haddadin, S., Weis, M., et al.: On joint design with intrinsic variable compliance: derivation of the DLR QA-joint. In: 2010 IEEE International Conference on Robotics and Automation, pp. 1687–1694. IEEE (2010) 12. Zhu, B., Chen, Q., Li, H., et al.: Design of planar large-deflection compliant mechanisms with decoupled multi-input-output using topology optimization. J. Mech. Robot. 11(3) (2010) 13. Zhang, H., Zhang, X., Zhu, B.: A novel flexural lamina emergent spatial joint. Mech. Mach. Theory 142, 103582 (2019) 14. Su, H.J., Shi, H., Yu, J.J.: A symbolic formulation for analytical compliance analysis and synthesis of flexure mechanisms. J. Mech. Des. 134(5), 51009-NaN (2010) 15. Hao, G., Li, H.: Conceptual designs of multi-degree of freedom compliant parallel manipulators composed of wire-beam based compliant mechanisms. Proc. Inst. Mech. Eng. Part C J. Mech. Eng. Sci. 229(3), 538–555 (2015) 16. Hopkins, J.B., Culpepper, M.L.: Synthesis of multi-degree of freedom, parallel flexure system concepts via Freedom and Constraint Topology (FACT)–Part I: Principles. Precis. Eng. 34(2), 259–270 (2010) 17. Hopkins, J.B., Culpepper, M.L.: Synthesis of multi-degree of freedom, parallel flexure system concepts via freedom and constraint topology (FACT). Part II: Practice. Precis. Eng. 34(2), 271–278 (2010)

Design and Analysis of a Rolling Joint Based on Gear Tooth for Continuum Robots Shengge Cao, Jingjun Yu, Guoxin Li, Jie Pan, and Xu Pei(B) School of Mechanical Engineering and Automation, Beihang University, Beijing 100191, China [email protected]

Abstract. Continuum robots can work in narrow and complicated space well because they are more flexible and more adaptable. The joint of the continuum robot is an important factor affecting its performance. However, it is difficult for existing joints to reconcile between load capacity and flexibility. Besides, they also suffer from complex assembly and big friction. This paper presents a rolling joint for continuum robots that has one gear tooth and one tooth space in each rolling elements. The curvature diameter of the rolling elements is equal to the reference diameter of the gear and the centers of the two circles coincide. The joint can rotate with no slip and small friction. The use of just one gear tooth solves the misalign of the gear teeth. In addition, it also has the advantages of simple assembly, large axial bearing capacity and strong torsional stiffness. The kinematics and statics model of the joint was established based on homogeneous transformation and Hertz Formula, respectively. The results of simulations and tests show that the rolling joint made of structural steel can at least bear the axial load of 1000 N and the torsion load of 3 N·m, the gear tooth can ensure the two rolling elements rotate without slip, and the torsional stiffness of the continuum robot can be strengthened and stabilized because of the gear teeth in its joints. Keywords: Continuum robot · Rolling joint · Gear tooth · Torsional stiffness

1 Introduction Continuum robots are the new type of robot which can change uniformly and continuously [1]. They can work in narrow and complicated space such as industrial small space operations [2, 3], nuclear maintenance [4] and minimally invasive surgery [5] without destroying environment because they are more flexible and more adaptable than traditional mechanical arms. The joint of the continuum robot is an important factor affecting its performance. With the deepening research on continuum robots, more and more joints are researched and designed. According to the types of unit joints, continuum robots can be divided into two types: rigid unit continuum robots and flexible unit continuum robots [6]. The joints of rigid unit continuum robots are rigid rotational pairs [7–9]. They have large load, high precision and good stability. But the accuracy depends on machining and control precision, which makes the control complicated and the environment compliance bad © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 169–179, 2021. https://doi.org/10.1007/978-3-030-89092-6_16

170

S. Cao et al.

[10]. The joints of flexible unit continuum robots are flexure hinge [11–13]. They are usually controlled in sections and each section has multiple flexible joints. They have good flexibility and simple control because they are usually controlled in sections and have simple structure. But they are weak in load capacity, especially in the direction of axis [14]. The joints based on rolling contact have the advantages of both compliant and rigid rotational pairs, but have few applications in continuum robots. Currently, there are three types rolling joints used in continuum robots: cables, elastic rods and gear teeth. A few rolling elements both assembling and driving by wire cable or elastic rod are the most common rolling continuum robots [15–17]. They are strong in axial load but weak in torsional stiffness. They also have big slip between rolling elements. Two rolling elements assemble by ribbons or cables, which prevent slip of the rolling elements, insure a no-slip condition. Ribbons or cables have two ways to assemble: two pairs crossing and one pair. Two pairs cables crossing rolling joints can rotate without friction but require additional tensioning or fixing elements [10, 18]. One pair cables crossing rolling joints solved this problem but the friction is big [19]. Two gears can rotate with no slip, so the rolling joints using gear teeth can both prevent the slip of the rolling elements and have simple structure [20, 21]. The friction between the gear teeth is also small. But the gear teeth are prone to misalign when there is no axial restraint because of not using the entire gear. In our study, a rolling joint that has one gear tooth in each rolling elements was presented. The joint can rotate with no slip and the continuum robot using them have strong torsional stiffness. The teeth misaligning problem of multiple gear teeth joint is solved. The structure or the rolling joint was designed. The kinematics model was obtained based on homogeneous transformation. The statics was analyzed. The rest of this paper is organized as follows: Sect. 2 describes the structure design of the rolling joint. Section 3 introduces the kinematics and statics model of the rolling joint. The simulations and experiments are demonstrated in Sect. 4 and the conclusions are given in Sect. 5.

2 Design of the Rolling Joint Continuum robots are often used in narrow or complex spaces so they require larger lengths and smaller diameters, which makes the structure design of continuum robots more important. As mentioned above, the joints of continuum robot require some special properties. For example, the joints need to be strong enough to bear the axial force, because the driving mechanism of continuum robot is mostly separated from the arm which generate large axial force. The introduction of rolling contact has solved many problems of rigid rotational pairs and flexure hinge, but there are still many problems such as the slide between two rolling elements in a joint, the complex assembly, the friction between rolling elements, the weak torsional stiffness, etc. In our study, as shown in Fig. 1, the rolling elements are very similar to the common ones. Taking standard involute cylinder gear as an example, just one gear tooth and tooth space are used in each of rolling elements. The rolling element is obtained by truncating a cylinder with two equal standard pitch circles because of the continuum robot need 2

Design and Analysis of a Rolling Joint Based on Gear Tooth

171

DOF to work. That is to say, when the diameter of curvature is equal to the reference diameter of the gear and the centers of the two circles coincide, the two rolling elements can be guaranteed to rotate with no slip. This is because the pitch circles of the two gears rotate with no slip if the gears are installed according to standard centre distance. Using just one gear tooth can also solve the misalign of the gear teeth. The joint has both simple assembly and strong torsional stiffness. It also has large axial bearing capacity which is the advantage of common rolling contact joints. Gear modulus can be selected according to parameters of the rolling element parameters.

(a)

(b)

Curvature diameter of rolling elements = Reference diameter of the gear

Gear tooth space

Gear tooth

(c) Fig. 1. Structure of the rolling element and the rolling joint: (a) a common rolling element; (b) the rolling element that have one gear tooth and tooth space; (c) the rolling joint.

As shown in Fig. 2, an example is a pipe-detecting robot that used this joint. the robot need to clean up inside a cylindrical tank after passing through a 50 mm diameter pipe. The outer diameter of the rolling element is 40 mm, the inner diameter is 30 mm, the high is 20 mm, and the diameter of curvature is 50 mm. The gear corresponding to the gear tooth is standard involute cylinder gear. The modulus of the gear is 5 and the number of teeth is 10. The parameters of the rolling elements are shown in Table 1. There are 12 axial through holes evenly distributing on the rolling element, some for the driving ropes and the other for the elastic rods.

172

S. Cao et al.

Fig. 2. A pipe-detecting robot that used the gear tooth rolling joint

Table 1. Parameters of the rolling element in the pipe-detecting robot Parameters

Symbol (unit)

Value

Outer diameter

d o (mm)

40

Inner diameter

d i (mm)

30

High

h (mm)

20

Diameter of curvature

d c (mm)

50

Modulus of the gear

m

5

Number of gear teeth

z

10

3 Analyses 3.1 Kinematics Analysis Kinematics for the rolling joint is based on homogeneous transformation [22]. As shown in Fig. 3, The kinematic parameters are defined. The local coordinate system (righthand system, which only drawn two axes) Oi (xi , yi , zi ) is established on the bottom of the Number i rolling element in the continuum robot and the zi -axis coincides with the initial rotation axis. Assuming there is no slip between the two rolling elements, the transform matrix between the two coordinate system on the two elements in one rolling joint is derived on the basis of the geometric relationship: π = Rot(x, − )Trans(x, mi−1 )Trans(z, ni−1 )Rot(z, θ ) 2 ⎤ ⎡ cos θ − sin θ 0 R − d + 2 · cos θ2 · (R − Rcos θ2 ) ⎥ ⎢ 0 0 −1 0 ⎥ =⎢ θ θ ⎦ ⎣ sin θ cos θ 0 2 · sin 2 · (R − Rcos 2 ) 0 0 0 1

i−1 i T

(1)

Design and Analysis of a Rolling Joint Based on Gear Tooth

173

⎡ Where

Trans(x, mi−1 )

⎤ 1 0 0 R − d + 2 · cos θ2 · (R − Rcos θ2 ) ⎢0 1 0 ⎥ 0 ⎢ ⎥, ⎣0 0 1 ⎦ 0 000 1 ⎤

=

⎡

100 ⎢0 1 0 Trans(z, ni−1 ) = ⎢ ⎣ 0 0 1 2 · sin

0 ⎥ 0 ⎥ θ θ ⎦, R is the diameter of curvature, d 2 · (R − Rcos 2 ) 000 1 is the distance between the origin of the local coordinate system and the centre of the standard pitch circle, and θ is the joint angle.

xi yi

Oi xi-1

Oi-1

zi-1

d

R

Fig. 3. Geometric description of the rolling joint

Based on the transform matrix, the point on the rolling element i for subsequent verification can be derived: ⎤ ⎡ R − d + 2 cos θ2 (R − R cos θ2 ) + x0 cos θ − y0 sin θ ) ⎥ ⎢ 0 ⎥ (2) q = Tp = ⎢ θ θ ⎦ ⎣ 2 sin (R − R cos ) + x0 sin θ + y0 cos θ ) 2

2

1 ⎤ x0 ⎢ y0 ⎥ ⎥ Where q is the coordinate in the coordinate system i − 1 and p = ⎢ ⎣ 0 ⎦ is the ⎡

1 coordinate in the coordinate system i. 3.2 Strength Analysis Statics analysis of axial load for the rolling joint is based on Hertz Formula [23]. The rolling elements only contact at the teeth, because the pitch circles of gears are tangent. As shown in Fig. 4, the force N at the gear teeth can be derived: 2N sin α = FL

174

S. Cao et al.

Where FL is the axial load of the rolling joint and α is the working pressure angle. Futher, the contact stress at the gear teeth can be derived on the basis of Hertz Formula:   1 F FL E ρ · (3) = σH max =

2 2 1−μ 1−μ πb 4π bmz(1 − μ2 ) cos2 α 1 2 + E1 E2 cos α 1 1 1 Where, ρ1 = ρ2 = tanrbα = mzsin α , ρ = ρ1 + ρ2 , rb is the base circle radius; m and z represent the module of the gear and the number of the teeth, respectively; b is the thickness of tooth; E and μ is the Young’s modulus and the Poisson’s ratio, respectively. Taking the structural steel as an example, the Young’s modulus and the Poisson’s ratio are E = 20060 Mpa and μ = 0.3. In our study, the thickness of tooth, the module of the gear and the number of the teeth are b = 5 mm, m = 5 and z = 10. The axial load of the rolling joint is FL = 1000 N, which is evenly distributed on the upper surface of the joint. The maximum contact stress can be calculated: 2

σH max = 89.14 Mpa

FL α N

Fig. 4. Force analysis diagram of the rolling joint

4 Simulations and Experiments 4.1 Kinematics Simulation and Test The kinematics of the rolling joint was simulated in ADAMS and tested. In the simulation, the two rolling elements were rotated at the same speed and in opposite direction to simulate the real rotation. The trajectory of the marker point relative to the bottom element was measured. As shown in Fig. 5(c), the bottom rolling element was fixed on the test bench together with graph paper and the top rolling element was driven by two wire cables. One wire cable was driven by a fixed nut and a rotating screw and the other was kept tensioned by a spring. The trajectory of the marker point was recorded on the graph paper. As shown in Fig. 5(a), the results of the test and simulation are compared with the calculated values in Eq. (7), in which R = 50 mm, d = 5 mm, x0 = 20 mm,

Design and Analysis of a Rolling Joint Based on Gear Tooth

175

y0 = 20 mm, and θ = 0◦ , 3◦ , 6◦ , 9◦ , 12◦ , 15◦ , 18◦ , 21◦ , 24◦ , 27◦ , 30◦ . The result shows that the calculated values are same as the simulation values and the result of the test is close to them, which means the gear tooth can ensure the two rolling elements rotate without slip. The joint achieves pure rolling with small friction through simple structure and assembly, which is beneficial for continuum robots. 14.0 test simulation caculation

12.0 10.0 y/mm

8.0

(b)

6.0 4.0 2.0 0.0 0.0

2.0

4.0 x/mm

(a)

6.0

8.0

(c)

Fig. 5. The kinematics simulation and test: (a) Comparison of test, simulation and calculation results; (b) Recording of simulation process; (c) Recording of test process.

4.2 Statics Simulation The load capacity in the direction of axis and torsional stiffness of the rolling joint were simulated in ANSYS WORKBENCH. In the simulations, the properties of the rolling elements were set to structural steel whose Young’s modulus and Poisson’s ratio are E = 206000 Mpa and μ = 0.3. The bottom rolling element is fixed and the contact type between the top element and the bottom element is no separation. The mesh size is 0.5 mm. The y-axis load FL = 1000 N and the y-axis torsion load TL = 3 N · m are applied to the upper rolling element individually and simultaneously. The simulation results are shown in Fig. 6. The result shows that the maximum contact stress is smaller than the yield limit of structural steel, which means the rolling joint can at least bear the axial load of 1000 N and the torsion load of 3 N·m. The simulation value of maximum contact stress is σH max = 84.61 Mpa, which is similar to the calculated value σH max . The big axial and torsion load capacity of the joint can improve the operating performance of the continuum robots. 4.3 Torsional Stiffness Test Taking a 200 mm long continuum robot, whose elements is fabricated based on 3D printing, as an example. If structural steel is needed to be used as the material in practical application, the same way as gear machining can be used. The torsional stiffness of the continuum robot using the rolling joint were tested by the test apparatus in Fig. 7. There

176

S. Cao et al.

Fig. 6. The stress cloud of the rolling joint: (a) The axial load FL = 1000 N; (b) The torsion load TL = 3 N · m. (c) The axial load FL = 1000 N and the torsion load TL = 3 N · m.

are 10 joints in the continuum robot, which is fixed between the base and the torque sensor by bolts and a coupling. The torque sensor is fixed to the motor and the motor is fixed on the base. In the continuum robot, there are 8 superelastic Ni-Ti rods, which are distributed at 30°, 60°, 120°, 150°, 210°, 240°, 300°, 330° to avoid the driving wire cables. The influence of the gear teeth on the torsional stiffness of the continuum robot was investigated by comparing with the continuum using no gear tooth rolling elements which have the same parameters. Each continuum robot was tested with three different diameters of Ni-Ti rods.

Rolling joints

Torque sensor DC power supply Stepper motor driver

Arduino UNO

Stepper motor

Reading dispaly

Fig. 7. The device of torsional stiffness test

During the test, the motor was turned 3.6° and held for 10 s to stabilize, then it was turned in the opposite direction 7.2°. It is worth noting that the rotation angle of the motor does not represent the actual torsion angle of the continuum robot, but the supporting torque can reflect the trend of torsional stiffness of the continuum robot within the allowable error range. The result is shown in Fig. 8. The result shows that

Design and Analysis of a Rolling Joint Based on Gear Tooth

177

the supporting torque of the continuum robot using the joints with gear tooth is about 10 times greater than that of using the joints with no gear tooth and more stable in the process of motor rotation. Further, the supporting torque of the continuum robot using the joints with gear tooth does not depend much on the diameter of the Ni-Ti rods, but the supporting torque of the no gear tooth one varies certainly with the diameter of the Ni-Ti rods. 0.25 0.2 0.15 0.1 0.05 0 -0.05 -0.1 -0.15 -0.2 -0.25 0

Supporting torque/N·m

using gear tooth & 1mm SMA rods using gear tooth & 0.8mm SMA rods using gear tooth & 0.6mm SMA rods no gear tooth & 1mm SMA rods no gear tooth & 0.8mm SMA rods no gear tooth & 0.6mm SMA rods

2

4

6

8

10 12 14 16 18 20 Time /s

22 24 26

28 30

Fig. 8. The result of the first test of supporting torque

Supporting torque /N·m

Considering that the performance of the continuum robot using no gear tooth rolling joint just torqued small angle is greatly affected by the clearance, to test the continuum robot using no gear tooth rolling joint more intuitively, the motor was turned 14.4°, which is the angle that the robot with gear tooth cannot reach, and held for 10 s, then it was turned in the opposite direction 28.8°. As shown in Fig. 9, the supporting torque not only varies greatly with the diameter of the Ni-Ti rods, but also fluctuates greatly, which may be caused by the friction due to dislocation between adjacent rolling elements. These mean that the torsional stiffness of the continuum robot can be strengthened because of the gear teeth in its joints and the introduction of the gear tooth can stabilize the torsional stiffness of the continuum robot. 0.1 0.08 0.06 0.04 0.02 0 -0.02 -0.04 -0.06

1mm SMA rods 0.8mm SMA rods 0.6mm SMA rods

0

5

10

15

20 25 Time /s

30

35

40

Fig. 9. The result of the second test of supporting torque

45

178

S. Cao et al.

5 Conclusions In this paper, a rolling joint for continuum robots that has one gear tooth and one tooth space in each rolling elements was presented. The joint can rotate with no slip and small friction because the curvature diameter of the rolling elements is the reference diameter of the gear. The joint has both simple assembly, large axial bearing capacity and strong torsional stiffness. Using just one gear tooth solved the misalign of the gear teeth. The kinematics and statics model of the joint was established based on homogeneous transformation and Hertz Formula, respectively. The results of statics simulation show that the rolling joint made of structural steel can at least bear the axial load of 1000 N and the torsion load of 3 N·m. The results of kinematics simulation and test show that the gear tooth can ensure the two rolling elements rotate without slip. The results of torsional stiffness test show that the torsional stiffness of the continuum robot can be strengthened because of the gear teeth in its joints and the introduction of the gear tooth can stabilize the torsional stiffness of the continuum robot. Finally, future work will establish the kinematics model of the continuum robot and build a continuum robot prototype to verify kinematics model and operational performance. Acknowledgments. The authors gratefully acknowledge the financial support of National Key Research and Development Program of China (Grant No. 2019YFB1311200) and National Natural Science Foundation of China (Grant No. U1813221). We thank Yunxian Gao for helpful suggestions.

References 1. Robinson, G., Davies, J.B.C.: Continuum robots-a state of the art. In: IEEE International Conference on Robotics and Automation, Detroit, Michigan, vol. 4, pp. 2849–2854 (1999) 2. Axinte, D., Dong, X., Palmer, D., et al.: MiRoR - miniaturised robotic systems for holistic in-situ repair and maintenance works in restrained and hazardous environments. IEEE/ASME Trans. Mechatron. 23(2), 1 (2018) 3. Buckingham, R.: Snake arm robots. Ind. Robot. Int. J. 29(3), 242–245 (2002) 4. Buckingham, R., Graham, A.: Snaking around in a nuclear jungle. Indus. Robot Int. J. 32(2), 120–127 (2005) 5. Watanabe, H., Kanou, K., Kobayashi, Y., et al.: Development of a “steerable drill” for ACL reconstruction to create the arbitrary trajectory of a bone tunnel. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, San Francisco, California, pp. 25–30 (2011) 6. Dong, X., Raffles, M., Guzman, S.C., et al.: Design and analysis of a family of snake arm robots connected by compliant joints. Mech. Mach. Theory 77, 73–91 (2014) 7. Anderson, V.C., Horn, R.C.: Tensor arm manipulator design. Trans. ASME 57, 1–12 (1967) 8. OC Robotics: A World First-Laser cutting for nuclear decommissioning at Sellafield. https://www.ocrobotics.com/lasersnake2--news--a-world-first--laser-cutting-for-nuc lear-decommissioning-at-sellafield. Accessed 01 Nov 2016 9. Deashapriya, K.P., Sampath, P.A.G., Wijekoon, W., et al.: Biomimetic flexible robot arm design and kinematic analysis of a novel flexible robot arm. In: International Moratuwa Engineering Research Conference, Moratuwa, Sri Lanka, pp. 385–390 (2016)

Design and Analysis of a Rolling Joint Based on Gear Tooth

179

10. Halverson, P.A., Howell, L.L., Magleby, S.P.: Tension-based multi-stable compliant rollingcontact elements. Mech. Mach. Theory 45(2), 147–156 (2010) 11. Mcmahan, W., Jones, B.A., Walker, I.D.: Design and implementation of a multi-section continuum robot: Air-Octor. In: IEEE/RSJ International Conference on Intelligent Robots and Systems. Edmonton, pp. 2578–2585 (2005) 12. Yoon, H.S., Yi, B.J.: Design of a master device for controlling multi-moduled continuum robots. Proc. Inst. Mech. Eng. Part C J. Mech. Eng. Sci. 231(10), 1921–1931 (2017) 13. Jiang, S., Chen, B., Qi, F., et al.: A variable-stiffness continuum manipulators by a SMA-based sheath in minimally invasive surgery. Int. J. Med. Robot. Comput. Assist. Surg. 16(2), e2081 (2020) 14. Suh, J.W., Lee, J.J., Kwon, D.S.: Underactuated miniature bending joint composed of serial pulleyless rolling joints. Adv. Robot. 28(1), 1–14 (2014) 15. Jeanneau, A., Herder, J., Laliberté, T., et al.: A compliant rolling contact joint and its application in a 3-dof planar parallel mechanism with kinematic analysis. In: International Design Engineering Technical Conferences and Computers and Information in Engineering Conference, Salt Lake City, Utah, pp. 689–698 (2004) 16. Buckingham, R.O., Graham, A.C.: Robotic arms with coaxially mounted helical spring means. U.S. Patent, 8, 069, 747 (2011) 17. Hwang, M., Kwon, D.: K-FLEX: a flexible robotic platform for scar-free endoscopic surgery. Int. J. Med. Robot. Comput. Assist. Surg. 16 (2020) 18. Chironis, N.P., Sclater, N.: Mechanisms and Mechanical Devices Sourcebook. McGraw-Hill, New York (1996) 19. Suh, J., Kim, K., Jeong, J., et al.: Design considerations for a hyper-redundant pulleyless rolling joint with elastic fixtures. IEEE/ASME Trans. Mechatron. 20(6), 2841–2852 (2015) 20. Cooper, T.G., Wallace, D.T., Chang, S., et al.: Surgical tool having positively positionable tendon-actuated multi-disk wrist joint. U.S. Patent, 6817974 B2 (2004) 21. Jelínek, F., Pessers, R., Breedveld, P.: DragonFlex – smart steerable laparoscopic instrument. J. Med. Dev. 7(2), 020911 (2014) 22. Denavit, J., Hartenberg, R.S.: A kinematic notation for lower-pair mechanisms based on matrices. J. Appl. Mech. 21(5), 215–221 (1995) 23. Hertz, H.: On the contact of elastic solids. Journal für die reine und angewandte Mathematik (Crelles Journal) 92, 156 (1880)

Design of Continuum Robot Based on Compliant Mechanism Guoxin Li1 , Jingjun Yu1 , Yichao Tang2 , Jie Pan1 , Shengge Cao1 , and Xu Pei1(B) 1 School of Mechanical Engineering and Automation, Beihang University,

Beijing 100191, China [email protected] 2 Beijing Institute of Specialized Machinery, Beijing 100143, China

Abstract. As a new type of bionic special robot, continuum robot has the characteristics of multi-degrees of freedom, good flexibility, large length-diameter ratio and strong adaptability to the environment. The compliant mechanism has the advantages of simple structure, free from assembly, and has a certain stiffness, which is very suitable for the design of continuum robot. However, due to the limitation of its own structure, its bending angle is small, it cannot fully meet the requirements of the joint bending angle of continuum robot. In this study, a continuum robot design based on compliant mechanism was proposed. The remote center motion mechanism was used as the continuum robot joint, and the joint configuration was dominated by trapezoidal leaf in series. The structural dimensions of the leaf were optimized. Then the kinematics model of the robot is established based on piecewise constant curvature theory. Finally, the compressive stiffness, joint stiffness of the compliant joint, and the kinematic performance of the continuum robot were explored through simulation and experiment. The rationality of joint configuration design and stiffness design, as well as the applicability of the kinematic model were verified. Keywords: Compliant mechanism · Continuum robot · Compliant joint · Kinematics model

1 Introduction Since the early 1960s, continuum robot has been widely used in narrow, complex and confined environments, such as aviation maintenance [1, 2], nuclear industry [3], medical [4, 5], space [6] and ocean [7], due to their large length-diameter ratio, high compliance and super-redundancy structure characteristics. Because of these advantages, researchers have proposed many structural designs of continuum robots. According to the skeleton design method, continuum robot can be divided into two categories: rigid skeleton continuum robot and compliant skeleton continuum robot. The most representative of rigid skeleton is a series of continuum robots designed by OC Robotics [8, 9]. Different from rigid skeleton, the compliant skeleton continuum robot uses spring [10], pneumatic device [5], elastic rod with base plate [11], compliant joint [12] and concentric pipe structure [13, 14] as motion units to achieve continuous bending deformation. © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 180–191, 2021. https://doi.org/10.1007/978-3-030-89092-6_17

Design of Continuum Robot Based on Compliant Mechanism

181

As an important branch of this field, the continuum robot with compliant joint as the skeleton realizes bending by using its own structural characteristics. In compliant joints, compliant rotation pair is the most common, mainly including notch-type [15] and leaf-type [16, 17]. In order to meet the movement requirements of compliant continuum robot, researchers usually use different form of compliant Hooke hinge as joint [18, 19], such as notch-type and leaf-type Hooke hinge, which are mostly used in the field of medical surgery. Another good example of a continuum robot with compliant joint is that the whole Nitinol alloy is used as skeleton [20, 21], the motion of the continuum robot is realized through the compliant joint made by laser cutting. However, due to the structural limitation of the compliant joint mentioned above, the bending angle of a single joint is small, so a longer arm is needed for the same bending angle, which increases the difficulty of modeling and controlling such continuum robot. Based on this, the trapezoidal leaf based on the same direction series was designed as a compliant joint, and the optimization of each structure size was completed, which increases the bending angle of the compliant joint and retains a certain joint stiffness; Then, the kinematics analysis of compliant continuum robot was carried out; Finally, the compressive strength and joint stiffness of compliant mechanism and the kinematic performance of the continuum robot were explored.

2 Design and Modeling 2.1 The Design of Compliant Joint For a bending joint that can be used in a continuum robot, the bending angle and compressive capacity will be taken into account. Therefore, the bending angle of compliant joint can be increased by series, the leaf can be designed as a tension structure. Its basic configuration consists of two trapezoidal leaves in series with the same direction and approximately equal stiffness (Fig. 1a), and the instantaneous center of rotation (ICR) of two trapezoidal leaves is located on the bottom surface with one degree of freedom (Fig. 1b), and the rotation stiffness is Kseries = 1/2Kone and the maximum bending angle is φseries = 2φone . With each joint having the same configuration in parallel, the final joint stiffness is equal to the single trapezoidal leaf, and the maximum rotation angle is doubled.

ϕ1 ϕ2

H1 H2

(a)

hf 2

hf 1

(b)

Fig. 1. The design of the compliant joint. (a) The design of joint structure. (b) The dimensional parameters of joint structure.

182

G. Li et al.

In the trapezoidal leaf, according to the force analysis of the pseudo-rigid-body model, the model can be simplified to the pin-joint model of the pivot [22]. The stiffness of the pin-joint model can be given by K=

2Ebt 3 n(1 − 3n + 3n2 ) cos ϕ 3H

(1)

When the material is determined, the stiffness K is related to the width b, thickness t, angle ϕ between the leaves and the position H from the bending center. Where n is the ratio of the instantaneous rotation radius R to the length l of the leaf and represents the relative position of the ICR. Without the interference of external structure, the maximum rotation angle of the leaf is only related to the internal stress. Since the internal stress produced by bending moment is much greater than the shear stress, only the effect of bending moment is considered. The maximum stress on the leaf can be given by σ=

Et(3n − 1) mmax t = α 2I l

(2)

Where I and l are the moment of inertia and length of the leaf respectively. When its maximum stress reaches the yield strength (S y ) of the material, the maximum rotation angle of the leaf is αmax =

l Sy Et(3n − 1)

(3)

2.2 The Size Design of Leaf First, according to the principle of approximate equality of stiffness, the joint length was determined: hf 1 = 17 mm, H1 = 55 mm, hf 2 = 14 mm, H2 = 46 mm. Then the thickness t, width b and installation angle ϕ of the leaf were designed respectively. The initial thickness was set as 0.6 mm, the width was 10 mm, and the initial angles were ϕ1 = 10◦ , ϕ2 = 30◦ respectively. Taking the single trapezoidal leaf as an example, the influence of the change of various parameters on the stiffness value and the maximum bending angle was verified. Specifically, the maximum rotation angle of the structure is the intersection point of the allowable stress value and the simulation curve. As can be seen from Fig. 2, the theoretical value of stiffness is approximately equal to the simulated value below 0.1 rad, the change of thickness has a great influence on the performance of the joint, especially in the aspect of stiffness, the change of installation angle has little influence on joint stiffness and maximum rotation angle, which can be almost ignored, and the change of the width only changes the stiffness value of the joint, but has no effect on the maximum rotation angle. Therefore, the thickness should be taken as the top consideration in joint design. At the same time, the contradiction between the stiffness and the maximum rotation angle of the joint is most obvious in the choice of the thickness. After comprehensive consideration, the specific parameters are shown in Table 1. According to the calculation equation of single hinge, the stiffness of the two leaves are

Design of Continuum Robot Based on Compliant Mechanism pivot 1 width b = 10mm φ1=10

4000

2000

0 0.05

0.15

0.2

0.25

0.3

600 400 200

(b1)

pivot 1 width b = 10mm, thickness t = 0.6mm stress(max) (MPa)

simulation-13° simulation-15°

1500

theory-10° theory-13°

1000

theory-15° 500

0.05

0.1

0.15

0.2

0.25

0.3

rotation angle (rad) pivot 1 width b = 10mm, thickness t = 0.6mm simulation-10° simulation-13°

800

simulation-15° allowable stress

600

theory-10° theory-13°

400

theory-15° 200 0

0 0

(a2)

0

1000

simulation-10°

2000

M (N·mm)

0.1

rotation angle (rad)

2500

0.05

0.1

0.15

0.2

0.25

0.3

ratation angle (rad) pivot 1 thickness t = 0.6mm φ1=10

2500

stress(max) (MPa)

simulation-12mm theory-8mm theory-10mm

1000

0

0.05

0.1

0.15

0.2

0.25

0.3

ratation angle (rad) pivot 1 thickness t = 0.6mm φ1=10 simulation-8mm

simulation-10mm

1500

(b2) 1000

simulation-8mm

2000

M (N·mm)

800

pivot 1 width b = 10mm φ1=10

0 0

(a1)

theory-12mm 500

800

simulation-10mm simulation-12mm

600

allowable stress 400

theory

200 0

0

(a3)

simulation-0.4mm simulation-0.6mm simulation-0.8mm simulation-1.0mm allowable stress theory-0.4mm theory-0.6mm theory-0.8mm theory-1.0mm

1000

simulation-0.4mm simulation-0.6mm simulation-0.8mm simulation-1.0mm theory-0.4mm theory-0.6mm theory-0.8mm theory-1.0mm

stress(max) (MPa)

M (N·mm)

6000

183

0

0.05

0.1

0.15

0.2

ratation angle (rad)

0.25

0.3

(b3)

0

0.05

0.1

0.15

0.2

0.25

0.3

ratation angle (rad)

Fig. 2. Variation curve of the stiffness and maximum rotation angle when the structural parameters change. (a1–a3) Stiffness value curve. (b1 –b3) Maximum stress value curve corresponding to rotation angle.

Table 1. The structural parameter values of compliant joint Parameter The thickness

0.6 mm

The width

9 mm

Pivot 1 included angle ϕ 1

12°

H1

55 mm

H f1

17 mm

Pivot 2 included angle ϕ 2

34

H2

46 mm

H f2

14 mm

6.97 Nm/rad and 6.88 Nm/rad respectively, and the maximum rotation angle are 7.11° and 7.13° respectively. As a result, the designed joint stiffness is about 7 Nm/rad and the maximum rotation angle is about 14.2°.

184

G. Li et al.

3 The Kinematic Analysis 3.1 The Joint Bending Angle and Direction Each joint is composed of two 1-DOF bending joints (each bending joint is called 1/2 joint of the continuum robot), and the rotation axis of each 1/2 joint is concentrated on the same plane. The phase difference of the adjacent 1/2 joint is 90°, so that each joint has 2 DOF, as shown in Fig. 3a. Set the bending angle of the first 1/2 joint as α and the bending angle of the second 1/2 joint as β (Fig. 3b). X0 Xl

X2 Pi

Y2

Pi ' ϕ

Z 0 Zl

(b)

ϕ'

Pi 0

Y0

(a)

ϕ

Yl Z2

(c)

Fig. 3. Diagram of joint bending and coordinate system. (a) The way of joint connection. (b) The transformation of coordinate system. (c) The rope length change when the 1/2 joint is bent.

As shown in Fig. 3b, according to the geometric relationship, it can be obtained by ⎧ ⎨ sin β = sin θ cos η (4) sin θ sin η = sin α cos β ⎩ cos θ = cosαcosβ Therefore, when the bending angle θ and bending direction η are known, the first 1/2 joint bending angle α and the second 1/2 joint bending angle β are α = arctan(tan θ sin η) β = arcsin(sin θ cos η)(α, β ∈ [−20◦ ∼ 20◦ ])

(5)

On the contrary, θ and η can be obtained from bending angle α and β by Eq. (4). θ = arccos(cosαcosβ) η = arctan(

sin α )(θ ∈ [−20◦ ∼ 20◦ ], η ∈ [0◦ ∼ 360◦ ]) (6) tan β

When the 1/2 joint is bent by a certain angle ϕ (Fig. 3c), the 1/2 joint’s rope length is l. Assume that the bending angle of each rope is ϕ, ignoring ϕ  . The relationship between the bending angle and the rope length is l(i) = r sin γ sin ϕ

(7)

Where i represents the serial number of driving rope. γ is the included angle between the driving rope position and the joint rotation axis.

Design of Continuum Robot Based on Compliant Mechanism

185

3.2 The Continuum Robot Bending Configuration The variation of a single joint is extended to a single segment robot, and each segment consists of n joints. It is assumed that each joint in a single segment of the continuum robot is uniformly bent, that is, each joint has the same bending direction and the same bending angle. Therefore, the total bending angle θs = nθj , bending direction ηs = ηj and the variation of the driving rope length ls (i) = nlj (i) in a single segment can be got. Z A

Z4

Z3

X3

X4 Z0 Z2

Rbend

O Z1 X0

X2

X1

Y

θsegment ηsegment

X

B

Fig. 4. The schematic diagram of single-segment bending coordinate system transformation.

The continuum robot in this segment can be abstract as the model of Hooke Hinge translational joint - Hooke Hinge. According to the corresponding relationship, both points O and A are Hooke Hinge, and translational joint is between points O and A (Fig. 4). The transformation matrix Ts is established with bending angle, bending direction and joint length as parameters.



T  R ( z , ) R ( x , ) 2

1 0





T  R ( z , - ) R ( x, - ) 2 2

2 1





(8)

T  T ( z , 2 R sin ) R( x, ) 2 2

3 2





T  R ( z , - ) R ( x, - ) 2 2

4 3

Ts = 40 T = 10 T 21 T 32 T 43 T

(9)

When the circular arc D-H matrix is used, the end coordinate system of the first segment is not coincide with the initial reference coordinate system of the second segment, there is a phase difference between them along the Z axis of the end coordinate system of the first segment which is equal to the bending direction angle of the first segment. Therefore, the intra-segment transformation matrix needs to be modified. Ts_ cor = Ts R(z, −η)

(10)

186

G. Li et al.

When a single segment is extended to the whole, different coordinate transformation matrices can be obtained according to different bending angle and bending direction of each segment. When the continuum robot includes three segments, there is 1 2 3 Ttotal = Ts_ cor Ts_ cor Ts_ cor

(11)

4 Performance Validation 4.1 The Compressive Strength The axial compressive stiffness of the compliant joint was verified by simulation. The bottom surface was fixed, and the deformation and maximum stress were detected when 1000 N pressure was applied vertically to the top surface. According to the simulation results shown in Fig. 5, the total deformation is 1.53 mm, and the deformation is mostly concentrated on both sides of the top surface, with a deformation ratio of about 2.3% (joint length lj = 66 mm). Besides, the axial deformation is about 0.5 mm, the deformation is about 0.76%, so the axial stiffness value can reach 2000 N/mm. The maximum stress is 229 MPa. According to the yield strength of 7050 aluminum (455 MPa), its compressive capacity is at least more than 2000 N, which ensures that the continuum robot can have a considerable load capacity.

Fig. 5. The axial compression simulation of compliant joint. (a) The stress nephogram. (b) The deformation nephogram.

4.2 The Joint Stiffness As shown in Fig. 6a. The whole device was installed on the vibration isolation table. The displacement was applied through the handwheel of the sliding platform and the movement distance was monitored by a laser displacement sensor. Besides, the force sensor was fixed on the sliding table, which was used to measure the loading force in the loading process, and the direction of the loading force was ensured to be vertically downward through the sliding table. The stabilized power supply was used to supply power to the displacement sensor.

Design of Continuum Robot Based on Compliant Mechanism

187

The experimental object was the 1/2 joint model processed by 3D printing, the thickness of the printed part were changed to 1 mm and 1.2 mm. The installation location was shown in Fig. 6b. The joint stiffness can be obtained by K=

Fr arctan( l/r)

(12)

Where r is the moment arm of loading force F and also the radius of the circle where the driving rope hole is located. l is the displacement of the force sensor. It can be seen from the Table 2 that the relative error of the stiffness value of the experimental sample is large. This is because the elastic modulus of 3D printed material has a wide range of values, leading to large fluctuation of experimental values. In addition, the actual structure is arc structure, not the plane structure calculated, so there are some differences with the calculation model.

(a)

(b)

Force sensor

Sliding table

Displacement sensor Stabilified-voltage power

r

F

Fig. 6. The joint stiffness experiment. (a) The experimental equipment. (b) The experimental process.

Table 2. The stiffness measurement results Experiment 1

2

3

Stiffness value (t = 2.23 1.2 mm)

2.20

2.17

Linear correlation coefficient

0.993

0.998

Stiffness value (t = 1.07 1.0 mm)

1.24

1.24

Linear correlation coefficient

0.999

0.995

0.996

0.997

Average

Theoretical value

Relative error

2.20

1.99

9.5%

1.18

1.14

3.4%

188

G. Li et al.

4.3 The Kinematic Performance The measuring instrument was DXL360 digital angle ruler, which takes the horizontal direction of the instrument installation as the X axis and the vertical direction as the Y axis to measure the tilt angle of the two axes. Its measuring range is ±40°, resolution is 0.02° and accuracy is ±0.1°. The bending angle and direction of the two axes can be obtained by the following transformation.  ⎧ ⎪ ⎨ θ = arcsin( sin2 ϕX + sin2 ϕY ) (13) sin ϕX ⎪ ⎩ η = arctan( ) sin ϕY The experimental effect is shown in Fig. 7. The continuum robot was controlled to complete the actions: C-curve, S-curve and circle drawing. In the process of drawing a circle, the experimental values of bending direction and bending angle were measured, which were compared with the preset values to verify the correctness of the kinematics model. The arrangement of the experiment is shown in Table 3. (a)

(b)

(c)

(d)

η=0° θ=30°

η=45° θ=30°

η=90° θ=30°

η=135° θ=30°

η=180° θ=30°

Fig. 7. The schematic diagram of bending motion of continuum robot. (a) Experimental initial prototype. (b) C-curve. (c) S-curve. (d) Circle drawing (0–180°).

Design of Continuum Robot Based on Compliant Mechanism

189

Table 3. The arrangement of the experiment Number

0

The bending direction (°)

3

4

5

6

0

30

60

90

120

150

180

30

30

30

30

30

30

30

Prototype experiment-Bending angle

35.00 34.00 33.00 32.00 31.00 30.00 29.00 28.00 27.00 26.00 25.00

(a)

2

0

1

Prototype experiment-Bending direction

180.00 )

experiment 1 experiment 2 experiment 3 preset value

Bending direction (

Bending angle (

)

The bending angle (°)

1

2

3 Number

4

5

6

experiment 1 experiment 2 experiment 3 preset value

150.00 120.00 90.00 60.00 30.00 0.00

(b)

0

1

2

3 Number

4

5

6

Fig. 8. The comparison of single-segment bending preset values and experimental values. (a) The comparison of bending angles. (b) The comparison of bending directions.

The results are shown in Fig. 8. The error of bending angle is about ±2°, and the maximum error of bending direction is about ±4°, its error is mainly derived from the non-uniform bending of each joint. Since the design stiffness of each joint is equal, when the joint is bent, it is not only affected by the tension of the rope, but also by its own weight and load, its bending angle is slightly larger than the preset value usually, which is consistent with the extension of Fig. 8a. Further, there is a certain degree of impact on the bending direction.

5 Conclusion In this paper, the design of continuum robot based on compliant mechanism was proposed. The advantages of compliant mechanism being free from assembly and having a certain stiffness were integrated into the structural design of continuum robot. The problem of small bending angle caused by structural interference of compliant mechanism was improved by connecting compliant mechanism in series in the same direction, and the structural size parameters of compliant joint were optimized. Finally, the compressive strength, joint stiffness, joint maximum bending angle and kinematic performance of continuum robot were validated by experiment and simulation. The whole continuum robot is composed of compliant joints which are vertically staggered and connected in series (length-diameter ratio ≥5, diameter 115 mm, length 792 mm). There are three segments in total, each of which has two joints and each joint has two degrees of freedom (12 DoFs). In addition, the results show that the axial compression capacity of the compliant mechanism is above 2000 N, the axial stiffness is about 2000N/mm which indicates that it has considerable load capacity. While its joint stiffness is about 7 N/mm, and the bending angle can reach 15°. In the later stage,

190

G. Li et al.

the stiffness design will be optimized by taking the joint as the unit to ensure uniform bending. Acknowledgements. This work was supported in part by the National Key Research and Development Program of China (Grant No. 2019YFB1311200), and the National Natural Science Foundation of China (Grant No. U1813221).

References 1. Axinte, D., et al.: MiRoR-miniaturized robotic systems for holistic in-situ repair and maintenance works in restrained and hazardous environments. IEEE/ASME Trans. Mechatron. 23(2), 978–981 (2018) 2. Dong, X., et al.: Development of a slender continuum robotic system for on-wing inspection/repair of gas turbine engines. Robot. Comput. Integrat. Manufact. 44, 218–229 (2017) 3. Buckingham, R., Graham, A.: Snaking around in a nuclear jungle. Indus. Robot 32(2), 120– 127 (2015) 4. Watanabe, H., Kanou, K., Kobayashi, Y., Fujie, M.G.: Development of a ‘Steerable Drill’ for ACL reconstruction to create the arbitrary trajectory of a bone tunnel. In: IEEE International Conference on Intelligent Robots and Systems, pp. 955–960 (2011) 5. De Volder, M., Moers, A., Reynaerts, D.: Fabrication and control of miniature McKibben actuators. Sens. Actuat. A Phys. 166, 111–116 (2011) 6. Nahar, D., Yanik, P.M., Walker, I.D.: Robot tendrils: long, thin continuum robots for inspection in space operations. In: 2017 IEEE Aerospace Conference, IEEE, pp. 1–8 (2017) 7. Liljebäck, P., Mills, R.: Eelume: a flexible and subsea resident imr-vehicle. In: Oceans 2017Aberdeen. IEEE, pp. 1–4 (2017) 8. Buckingham, R., Graham, A.: Nuclear snake-arm robots. Indus. Robot 39(1), 6–11 (2012) 9. Buckingham, R., et al.: Snake-arm robots: a new approach to aircraft assembly. SAE Tech. Paper Ser. 1, 724 (2007) 10. Kim, Y., Cheng, S.S., Diakite, M., et al.: Toward the development of a flexible mesoscale MRIcompatible neurosurgical continuum robot. IEEE Trans. Robot. 33(6), 1386–1397 (2017) 11. Xu, K., Liu, H.: Multi-backbone continuum mechanisms: forms and applications. J. Mech. Eng. 54(13), 25–33 (2018) 12. Daniel, B., Roppenecker, P.: Multi arm snake-like robot kinematics. In: Intelligent Robots and Systems, pp. 5040–5045 (2013) 13. Penning, R.S., Jung, J., Borgstadt, J.A., Ferrier, N.J., Zinn, M.R.: Towards closed loop control of a continuum robotic manipulator for medical applications. In: Proccedings of EEE International Conference on Robotics and Automation, Shanghai, pp. 4822–4827 (2011) 14. Dupont, P.E., Lock, J., Itkowitz, B., et al.: Design and control of concentric-tube robots. IEEE Trans. Robot 26(2), 209–225 (2010) 15. Howell, L.L., Midha, A.: A method for the design of compliant mechanisms with small-length flexural pivots. ASME J. Mech. Des. 11(6), 280–290 (1994) 16. Yu, J., Hao, G., Chen, G., Bi, S.: State-of-art of compliant mechanisms and their applications. J. Mech. Eng. 51(13), 53–68 (2015) 17. Awtar, S., Slocum, A.H.: Constraint-based design of parallel kinematic XY flexure mechanisms. ASME J. Mech. Des. 129(8), 816–830 (2007) 18. Dong, X., Raffles, M., Guzman, S.C., et al.: Design and analysis of a family of snake arm robots connected by compliant joints. Mech. Mach. Theory 77, 73–91 (2014)

Design of Continuum Robot Based on Compliant Mechanism

191

19. Roppenecker, D.B., Schuster, L., Coy, J.A., et al.: Modular body of the multi arm snake-like robot. In: International Conference on Robotics and Biomimetics, pp. 374–379 (2014) 20. Kato, T., Okumura, I., Kose, H., Takagi, K., Hata, N.: Tendon-driven continuum robot for neuroendoscopy: validation of extended kinematic mapping for hysteresis operation. Int. J. Comput. Assist. Radiol. Surg. 11(4), 589–602 (2015). https://doi.org/10.1007/s11548-0151310-2 21. Tian, J., Wang, T., Fang, X., et al.: Design, fabrication and modeling analysis of a spiral support structure with superelastic Ni-Ti shape memory alloy for continuum robot. Smart Mater. Struct. 29(4), 045007 (2020) 22. Xu, P., Yu, J., Zong, G., et al.: An effective pseudo-rigid-body method for beam-based compliant mechanisms. Precis. Eng. 34(3), 634–639 (2010)

Biologically Inspired Planning and Optimization of Foot Trajectory of a Quadruped Robot Senwei Huang

and Xiuli Zhang(B)

School of Mechanical, Electronic and Control Engineering, Beijing Jiaotong University, Beijing 100044, China [email protected]

Abstract. Foot trajectory plays an important role in high speed running of cheetahs, so it is very significant to design an appropriate foot trajectory by mimicking cheetahs to improve the performance of quadruped robots. This paper presents a Bézier curve based foot trajectory planning approach for a quadruped robot. Four 5th-order Bézier curves are employed to plan the robot’s foot trajectory in the sagittal plane, which is inspired by the foot trajectory characteristics of cheetahs. Particle swarm optimization algorithm is used to determine the foot trajectory parameters with the aim of minimizing the jerk of the trajectory and impact as the feet touch the ground. Finally, the joints trajectories are computed through the inverse kinematic of the quadruped robot. Moreover, the foot trajectory is parametrized for applying to robots with different sizes. The dynamic simulation results show that the generated foot trajectory is effective to approximately achieve the desired speeds. Keywords: Foot trajectory planning · Bio-inspired · Particle swarm optimization · Bézier curve · Quadruped robot

1 Introduction In recent years, quadruped robot develops rapidly and has shown great control performance [1–5]. Foot trajectory plays an important role in the motion performance of the quadruped robot because the foot trajectory determines the interaction between the foot and the ground. An inappropriate foot trajectory will lead to bad impact force, which will greatly influence the continuity, stability, and efficiency of the quadruped robot. As a solution to this problem, many kinds of foot trajectory planning methods were proposed, such as rectangular foot trajectory [6], elliptical foot trajectory [7], composite cycloid foot trajectory [8], composite foot trajectory [9], polynomial foot trajectory [10], and Bézier foot trajectory [11].

© Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 192–203, 2021. https://doi.org/10.1007/978-3-030-89092-6_18

Biologically Inspired Planning and Optimization of Foot Trajectory

193

The composite cycloid foot trajectory which was proposed by Sakakibara et al. [7] takes the step length, height, and step phase times as parameters. HyQ [12] achieves approximately a 2 m/s trot by applying the composite cycloid foot trajectory. Scalf-1 [13] robot uses composite cycloid foot trajectory to avoid foot-dragging when walking. Wang et al. [14] modified the method of composite cycloid to plan the foot trajectory with zero impact. Yin et al. [15] use a composite cycloid to plan the foot trajectory and realize the tracking of the foot trajectory by combining neural networks and reinforcement learning methods. The Bézier foot trajectory is widely used for its advantages over the past few years. The foot trajectories of Cheetah-Cub [16] were generated by four quadratic Bézier curves. The stability of the Cheetah-Cub is improved by adjusting the leg angle at the moment of take-off and touch-down. Serval [17] achieves many motion patterns, simply by replaying parameterized foot trajectory which is generated by fitting four cubic Bézier curves to the foot trajectory data of dog. MIT Cheetah2 [18] robot’s foot trajectory of swing phase is based on three 5th-order Bézier curves. Han et al. [19] used an 11th order Bézier curve to plan the foot trajectory of a quadruped robot and improved the walking stability by optimizing the step height and step length. Various foot trajectories have been developed and studied to improve the locomotion performance of the quadruped robot. However, there is still a lot of work that has to be done in the field of quadruped robot trajectory planning. This paper deals with the foot trajectory design for a quadruped robot. We propose here an approach: we define in the sagittal plane our desired foot trajectory, we parameterize trajectory that is close to the cheetah, and the joints trajectory are computed through the inverse kinematic. Instead of designing a fixed trajectory, we aim to parameterize it with variables that are meaningful for locomotion. The following section introduces the quadruped robot model. Section 3 introduces foot trajectory planning and optimization based on Bézier curve and particle swarm optimization. Section 4 verifies the effectiveness of the foot trajectory in simulation, and Sect. 5 concludes the paper and discusses future work.

2 Quadruped Robot By mimicking quadruped mammals, we constructed a quadruped robot model in the Adams simulation environment. As shown in Fig. 1, the robot has a trunk and four legs. Each leg includes a femur, a tibia, and a foot. The hip and knee joints have one active pitch degree of freedom respectively. The foot is fixed at the lower end of the tibia. The material of the foot is defined as rubber, which reduces impact and increases friction. The main mechanical parameters of the quadruped robot are given in Table 1. The robot has dimensions of 1.05 m × 0.56 m × 0.7 m, and a mass of 23.85 kg. The following sections introduce the foot trajectory planning and optimization of the quadruped robot.

194

S. Huang and X. Zhang Table 1. Mechanical quadruped robot

Fig. 1. 3D model of the quadruped robot

parameters

of

the

Link

Length (m)

Mass (kg)

Inertia (kg·m2 )

Trunk

0.8

10

0.534

Femur

0.4

2

0.006

Tibia

0.34

0.2

0.00002

Foot

0.046

0.0001

/

3 Planning and Optimization of the Foot Trajectory 3.1 Biological Observation and Analysis The paths followed by the feet and leg joints of a cheetah in relation to fixed points at the shoulder and the hip were depicted by Hildebrand [20, 21]. Figure 2 shows the hindfoot and joints trajectories of a cheetah. There is a retraction segment in the foot trajectory before the foot touches the ground. After the foot leaves the ground, there is a backswing segment, which is symmetrical with the retraction segment. In this paper, the foot trajectory is divided into one stance phase (ST) and one swing phase (SW) with reference to [18]. The swing phase is divided into three sub-phases, namely, a follow-through (FT), a protraction (PR), and a swing-leg-retraction (SR), which happen in sequence. Four transitional points of the four phases are defined, as shown in Fig. 2(a). To simplify the design and optimization process, each leg of the cheetah is treated as a virtual leg, which has only two important parameters: length (distance from foot to shoulder or hip) and angle (angle between the virtual leg and the horizontal plane). The posterior extension point (pep) means that the virtual leg has reached the maximum angle when swinging backward. The anterior extension point (aep) means that the virtual leg has reached the maximum angle when swinging forward. The data presented in [21] was used to analyze the foot trajectory characteristics of the cheetah through the kinematic method. The locomotion of the virtual leg at pep and aep has the following characteristics: (1) the angle reaches the maximum or minimum; (2) the angular velocity is close to 0; (3) the contraction speed of the virtual leg is close to the maximum. Take-off is where the foot begins to leave the ground. Touch-down is where the foot begins to touch the ground. The locomotion of the virtual leg at take-off and touch-down has the following characteristics: (1) the length reaches the maximum; (2) the contraction speed is close to 0.

Biologically Inspired Planning and Optimization of Foot Trajectory

Posterior extension point

195

Anterior extension

Protraction

point

Follow-through

Swing-leg-retraction

Ground Stance

Take-off

Touch-down

(a)

(b)

Fig. 2. (a) Hindfoot trajectory in the sagittal plane of a cheetah galloping at 50 to 60 mph. The foot trajectory is divided into FT, PR, SR, and ST phases, which are represented by green, blue, yellow, and black lines, respectively. (b) Joint angles and successive positions of hindlimb segments of a cheetah galloping at roughly 56 mph. Arrows show the approximate times of take-off and touch-down. (Color figure online)

3.2 Foot Trajectory Planning Based on Bézier Curves The desired foot trajectory can be generated and parameterized using the Bézier curve. The points that are used to define a Bézier curve are called control points. A Bézier curve of degree n can be represented as C(u) =

n 

Bn,i (u)Pi

(1)

i=0 n! ui (1−u)n−i , u ∈ [0, 1] where Bn,i (u) is a Bernstein Polynomial given by Bn,i (u) = i!(n−i)! is a parameter of the Bézier curve, and Pi are the Bézier curve control points. To plan the time of each phase of the foot trajectory, time coefficient a is introduced to map phase duration t into the parameter of the Bézier curve u, as shown in Eq. (2).

u = at, t ∈ [0, tmax ]

(2)

where a is the time coefficient given by a = 1/tmax . tmax is phase duration required for the foot to move from the first to the last point of the Bézier curve. The first derivative of a Bézier curve of degree n is a Bézier curve of degree n − 1, C  (at) =

n−1 

  Bn−1,i (at) na(Pi+1 − Pi )

(3)

i=0

It is easy to obtain C(0) = P0 and C  (0) = na(P1 − P0 ) at t = 0, C(1) = Pn and C  (1) = na(Pn − Pn−1 ) at t = tmax . The first and last points on the curve are coincident with the first and last control points. The speed of the start (end) of a Bézier curve can be controlled by the position of the first (last) two control points and time coefficient. A foot trajectory is generated by connecting four 5th-order Bézier curves. We plan the foot trajectory in the polar coordinate system and take the center of the hip joint as the origin. The coordinates of the control points of the foot trajectory are defined as, j

j

j

Pi = [φi , li ]T , i ∈ {0, 1, · · · , 5}, j ∈ {FT,PR,SR,ST}

(4)

196

S. Huang and X. Zhang j

Here, Pi represent the coordinate of the ith control point of the jth segment of the foot j j trajectory, φi and li is the polar angle and diameter of the control point. Subscripts i ∈ {0, 1, · · · , 5} and j ∈ {FT,PR,SR,ST} indicate control point and phase index. 3.3 Definition of Constraints and Optimization Variables The connecting points of the foot trajectory are selected in order to provide continuous and smooth transitions by subject to the following constraints P5FT = P0PR , P5PR = P0SR , P5SR = P0ST , and P5ST = P0FT aFT (P5FT − P4FT ) = aPR (P1PR − P0PR ), aPR (P5PR − P4PR )

= aSR (P1SR − P0SR )

aSR (P5SR − P4SR ) = aST (P1ST − P0ST ), aST (P5ST − P4ST )

= aFT (P1FT − P0FT )

Combining Eq. (5) and (6) yields   aFT  FT aSR  SR P5 − P4FT , P4PR = P0SR + P0 − P1SR P1PR = P5FT + aPR aPR    a aFT  FT SR P1ST = P5SR + P5SR − P4SR , P4ST = P0FT + P0 − P1FT aST aST

(5)

(6)

(7) (8)

According to the conclusion that the angular velocity of the virtual leg at pep and aep is close to 0 (φ˙ 0FT = φ˙ 5SR = 0). We obtain the following constraints φ4FT = φ5FT = φ0PR = φ1PR , φ4PR = φ5PR = φ0SR = φ1SR

(9)

According to the conclusion that the contraction speed of the virtual leg at take-off and touch-down is close to 0 (˙l0FT = ˙l5SR = 0). We obtain the following constraints l0FT = l1FT = l4ST = l5ST , l0ST = l1ST = l4SR = l5SR

(10)

The step length (L) and hip height (H) is introduced into the foot trajectory planning, see Fig. 3. Virtual leg angle and length at take-off and touch-down can be obtained by φ0FT = π + arctan(2H /L) l0FT = −H/sin(φ0FT )

(11)

φ5SR = 2π − arctan(2H /L) l5SR = −H/sin(φ5SR )

(12)

The research of Haberland et al. [22], Karssen et al. [23], and Park et al. [18] show that the relative speed of the foot with respect to the ground at the moment of touchdown can be reduced by retracting the swing leg. And the reduced foot speed can help reduce impact energy losses, decrease peak forces, and minimize foot slipping, as well as improve the stability and disturbance rejection of running robots. The optimal retraction rate,

Biologically Inspired Planning and Optimization of Foot Trajectory φ0FT φ5SR

Virtual leg

vxrobot O0

x

l0FT

0°

l5SR

φ FT

φSR

0

vto = l0FTφ0FT

197

H

5

l0FT

l5SR

Ground Take-off

vtd = l5SRφ5SR

Touch-down

L

Fig. 3. Design of foot trajectory in the polar coordinate system with the origin (O0 ) at the hip.

for which the impact force and impact losses are minimal, is the retraction at which the tangential foot speed (magnitude of the velocity of the foot in the direction perpendicular to the leg) is zero. According to this conclusion, the swing leg retraction rate φ˙ 5SR will be calculated to satisfy φ˙ 5SR l5SR − vxrobot sin(φ5SR ) = 0; therefore the following constraint equation can be derived, 5aSR (φ5SR − φ4SR )l5SR − vxrobot sin φ5SR = 0

(13)

The horizontal velocity of the foot vxrobot is equal to the horizontal velocity of the robot at the moment of take-off, expressed as vxrobot − vtto sin φ0FT = 0. The constraint equation can be expressed as vxrobot − 5aFT (φ1FT − φ0FT )l0FT sin φ0FT = 0

(14)

Therefore, the coordinates of the control points that are needed to generate a complete foot trajectory are defined by,   FT φ0 f1 x1 x2 x3 x3 FT P = FT FT l0 l0 x11 x12 x13 x14   x3 x3 x4 x5 x6 x6 P PR = x14 f2 x15 x16 f3 x17 (15)   x6 x6 x7 x8 f4 φ5SR SR P = x17 x18 x19 x20 l5SR l5SR   SR φ5 f5 x9 x10 f6 φ0FT P ST = SR l5 l5SR x21 x22 l0FT l0FT where xi are the parameters to be determined by optimization. The coordinates of the take-off, [φ0FT , l0FT ]T , and touch-down, [φ5SR , l5SR ]T , can be calculated by Eq. (11) and (12). f1 can be calculated by Eq. (13). f2 and f3 can be calculated by Eq. (7). f4 can be calculated by Eq. (14). f5 and f6 can be calculated by Eq. (8). The duration of the FT, PR, SR, and ST phases are defined by tFT , tPR , tSR , and tST , respectively. The total duration is given by T = tFT + tPR + tSR + tST . The ratio of the

198

S. Huang and X. Zhang

duration of FT, PR, SR, and ST phases to the total duration is defined as x23 , x24 , and x25 , respectively. Therefore the following equation can be derived, tFT = x23 T , tSR = x24 T , tST = x25 T , and tPR = (1 − x23 − x24 − x25 )T

(16)

To sum up, only 25 parameters need to be determined to generate the desired foot trajectory. Vector x ∈ R25 , which parameterizes the desired foot trajectory, is defined as,   x = x1 x2 · · · x25 (17) The first 22 elements of x, x1···22 , are used to parameterize the desired trajectory of the FT, PR, SR, and ST phases. The last three elements of x, x23···25 determine the durations of the FT, PR, and SR phases. 3.4 Parameter Optimization Based on Particle Swarm Optimization This paper uses the particle swarm optimization (PSO) algorithm to determine these 25 parameters. PSO is a bionic optimization algorithm proposed by Kennedy and Eberhart [24]. Here, the position of each particle represents a foot trajectory, and the 25 dimensions of the particle represent 25 parameters that are used to determine a foot trajectory. The speed and position update methods are given by Eq. (18) and (19), respectively.     k+1 k k k k k + c p (18) = ωvid + c1 r1 pid − x r − x vid 2 2 ,pbest id d ,gbest id k+1 k+1 k xid = xid + vid

(19)

Where i = 1, 2, · · · , N is particle number, N is particle swarm size. d = 1, 2, · · · , D is particle dimension number. k is iterations. ω is inertia weight. c1 and c2 are individual k and x k are the and group learning factors. r1 and r2 are random numbers in [0,1]. vid id k velocity and position vector of the particle i in the k iteration. pid ,pbest and pdk ,gbest are the historical optimal position of the individual and group in the k iteration. Objective: Minimize norm of the jerk of the trajectory, j∈{FT,PR,SR,ST}



3  d 3 j j∈{FT,PR,SR,ST} d j φ + l dt 3 dt 3

(20)

In order to generate an available foot trajectory for a quadruped robot, optimization should subject to constraints shown in Table 2. The initial conditions are shown in Table 3. We optimize three foot trajectories that have expected speeds of 1 m/s, 2 m/s, and 3 m/s respectively. After 100 iterations, the particle fitness value has reached a stable value. The foot trajectories are shown in Fig. 4.

Biologically Inspired Planning and Optimization of Foot Trajectory Table 2. The constraints of the virtual leg

199

Table 3. Initial conditions

Parameters

Value range

Parameters

Value

Leg angle (rad)

3.752–5.498

Total duration (s)

1, 0.75, and 0.6

Leg length (m)

0.363–0.785 −25–25

Horizontal velocity (m/s)

1, 2, and 3

Leg angular velocity (rad/s) Leg stretching speed (m/s)

−4–4

Hip height (m)

0.55

Step length (m)

0.4

Fig. 4. The foot trajectories with expected speeds of 1 m/s, 2 m/s, and 3 m/s.

4 Simulations In this section, the effectiveness of foot trajectories is verified in several simulation tests. We control the quadruped robot to gallop three times for 20 s in the Adams simulation environment with three types of foot trajectories, respectively. Figure 5 shows snapshots within one gait cycle when the quadruped robot gallops at 1 m/s. The video shows the robot can walk steadily when controlled by the designed joint trajectories. The horizontal speeds of the center of mass (CoM) when using three foot trajectories respectively, see Fig. 6. The speed of CoM fluctuates periodically along with this robot’s galloping locomotion. And the fluctuation of speed increases with the increase of speed. The first foot trajectory v1 which desired speed is 1 m/s achieves an average speed of 1.287 m/s. The second foot trajectory v2 which desired speed is 2 m/s achieves an average speed of 1.716 m/s. The third foot trajectory v3 which desired speed is 3 m/s achieves an average speed of 2.239 m/s. The results show the desired speeds can be roughly achieved. But the differences between the actual speeds and the expected speeds are becoming greater as the running speed increases. This is mainly due to the inaccurate position control, which leads to the leg touching the ground at an unexpected joint position and speed. Besides, the ground reaction force (GRF) increases gradually as the running speed increase, as shown in Fig. 7. The increased impact forces cause unstable locomotion, which makes the desired velocity cannot be fully achieved. Since the robot does not have an elastic element, the GRF is fairly large. So we consider adding an elastic element to each leg in future work.

200

S. Huang and X. Zhang

Fig. 5. Snapshots of the simulated quadruped robot galloping at 1 m/s. The red arrows represent the direction of GRF. (Color figure online)

Fig. 6. The velocity of CoM of the simulated quadruped robot.

Fig. 7. Ground reaction forces in the vertical direction within one gait cycle. Solid cyan line, dashed blue line, dotted black line, and dash-dotted red line represents front left foot (FL), front right foot (FR), hind right foot (HR), and hind left foot (HL), respectively. (Color figure online)

Figure 8 and Fig. 9 show the front left leg joint torques within one gait cycle. The torque acting on each joint changes periodically and fluctuates while the phase of trajectory is changing. And the joint torque reaches the maximum when the foot is touching the ground or crossing the phase transition points of four phases. Because the acceleration at these positions changes dramatically, the jerk of the actuator will be very large. It is necessary to consider the change of acceleration at the phase transition point in future work when connecting multiple Bézier curves to generate foot trajectory. The

Biologically Inspired Planning and Optimization of Foot Trajectory

201

joint torque is smooth while the foot is in other positions of the foot trajectory, so the jerk of the actuator can be reduced.

Fig. 8. The torque acting on the hip joint of front left leg.

Fig. 9. The torque acting on the knee joint of front left leg.

5 Conclusions We proposed a parameterized approach using hip height, step height, horizontal velocity of the robot, and step phase times for trajectory generation of a quadruped robot. The desired foot trajectories are planned using four 5th-order Bézier curves in which the control points are constrained by referring to the cheetah’s biological data. Particle swarm optimization algorithm is applied to determine the foot trajectory parameter. The foot trajectories are effective and the desired speeds can be roughly achieved on a simulated quadruped robot. As the robot has no elastic component, the GRF is quite large. The joint torque is comparatively large when the foot is touching the ground or crossing the connection points of the Bézier curves. For the future work, we can improve the constraint conditions of trajectory planning when connecting multiple Bézier curves.

202

S. Huang and X. Zhang

References 1. Yang, C., Yuan, K., Zhu, Q., Yu, W., Li, Z.: Multi-expert learning of adaptive legged locomotion. Sci. Robot. 5(49), 1–14 (2020) 2. Shandong youbaote intelligent robotics co., ltd. http://www.yobotics.cn. Accessed 20 Apr 2021 3. WEILAN. http://www.weilan.com. Accessed 20 Apr 2021 4. Bledt, G., Kim, S.: Extracting legged locomotion heuristics with regularized predictive control. In: IEEE International Conference on Robotics and Automation, pp. 406–412. IEEE Press, Paris (2020) 5. Lee, J., Hwangbo, J., Wellhausen, L., Koltun, V., Hutter, M.: Learning quadrupedal locomotion over challenging terrain. Sci. Robot. 5(47), 1–13 (2020) 6. Yin, P., Wang, P., Li, M., Sun, L.: A novel control strategy for quadruped robot walking over irregular terrain. In: IEEE Conference on Robotics. Automation and Mechatronics (RAM), pp. 184–189. IEEE Press, Qingdao (2011) 7. Kim, K.Y., Park, J.H.: Ellipse-based leg-trajectory generation for galloping quadruped robots. J. Mech. Sci. Technol. 22(11), 2099–2106 (2008) 8. Sakakibara, Y., Kan, K., Hosoda, Y., Hattori, M., Fujie, M.: Foot trajectory for a quadruped walking machine. In: EEE International Workshop on Intelligent Robots and Systems. Towards a New Frontier of Applications, pp. 315–322. IEEE Press, Ibaraki (1990) 9. Rong, X., Li, Y., Ruan, J., Li, B.: Design and simulation for a hydraulic actuated quadruped robot. J. Mech. Sci. Technol. 26(4), 1171–1177 (2012) 10. Yuan, L., Zhang, Z., Ouyang, R.: Trajectory planning of quadruped robot based on the principle of optimal power. Int. J. Mech. Res. 5(4), 138–147 (2016) 11. Hyun, D.J., Seok, S., Lee, J., Kim, S.: High speed trot-running: Implementation of a hierarchical controller using proprioceptive impedance control on the MIT Cheetah. Int. J. Rob. Res. 33(11), 1417–1445 (2014) 12. Semini, C., Tsagarakis, N.G., Guglielmino, E., Focchi, M., Cannella, F., Caldwell, D.G.: Design of HyQ - a hydraulically and electrically actuated quadruped robot. Proc. Inst. Mech. Eng. Part I J. Syst. Control Eng. 225(6), 831–849 (2011) 13. Li, Y., Li, B., Rong, X., Meng, J.: Mechanical design and gait planning of a hydraulically actuated quadruped bionic robot. J. Shandong Univ. Sci. 41(5), 32–36 (2011) 14. Wang, L., Wang, J., Wang, S., He, Y.: Strategy of foot trajectory generation for hydraulic quadruped robots gait planning. J. Mech. Eng. 49(1), 39–44 (2013) 15. Yin, J., Li, H., Dai, Z.: Quadruped robot harmonious control based on RBF-Q learning. Appl. Res. Comput. 30(8), 2349–2352 (2013) 16. Tuleu, A.: Hardware, software and control design considerations towards low-cost compliant quadruped robots. École Polytechnique Fédérale de Lausanne (2016) 17. Eckert, P., et al.: Towards Rich Motion Skills with the Lightweight Quadruped Robot Serval - A Design, Control and Experimental Study. In: Manoonpong, P., Larsen, J.C., Xiong, X., Hallam, J., Triesch, J. (eds.) SAB 2018. LNCS (LNAI), vol. 10994, pp. 41–55. Springer, Cham (2018). https://doi.org/10.1007/978-3-319-97628-0_4 18. Park, H.W., Kim, S.: Quadrupedal galloping control for a wide range of speed via vertical impulse scaling. Bioinsp. Biomimet. 10(2), 1–20 (2015) 19. Han, B., Zhu, C., Luo, Q., Zhao, R., Wang, Q.: Key gait parameter optimization for hydraulic quadruped robot. Trans. Beijing Inst. Technol. 38(10), 1056–1060 (2018) 20. Hildebrand, M.: Further studies on locomotion of the cheetah. J. Mammal. 42(1), 84–91 (1961) 21. Hildebrand, M., Hurley, J.P.: Energy of the oscillating legs of a fast-moving Cheetah, Pronghorn, Jackrabbit, and Elephant. J. Morphol. 184, 23–31 (1985)

Biologically Inspired Planning and Optimization of Foot Trajectory

203

22. Haberland, M., Karssen, J.G.D., Kim, S., Wisse, M.: The effect of swing leg retraction on running energy efficiency. In: IEEE International Conference on Intelligent Robots & Systems, pp. 3957–3962. IEEE Press, San Francisco (2011) 23. Karssen, J.G.D., Haberland, M., Wisse, M., Kim, S.: The effects of swing-leg retraction on running performance: analysis, simulation, and experiment. Robotica 33(10), 2137–2155 (2015) 24. Kennedy, J., Eberhart, R.: Particle swarm optimization. Swarm Intell. 1(1), 33–57 (1995)

Intensity Distribution Estimation of Radiation Source for Nuclear Emergency Robot in 3D Environment Zhiyu Zhang, Zhengya Guo, and Zhenhua Xiong(B) State Key Laboratory of Mechanical System and Vibration, School of Mechanical Engineering, Shanghai Jiao Tong University, Shanghai 200240, China [email protected]

Abstract. There is generally strong radioactivity during nuclear emergency. When the nuclear emergency robots work in this case, many devices will fail because of the high radiation dose. Thus, it is very important for the robot to sense the location and intensity of radiation sources in the surrounding environment to avoid excessive radiation. The maximum likelihood expectation maximization (MLEM) iterative algorithm can be used to estimate the distribution of source intensity based on the data measured by gamma camera at multiple detection points. The system matrix (SM) needed to be calculated by geometric relation to achieve in-situ reconstruction. In this paper, a model-based SM calculation method is proposed. The model consists of two parts: the camera imaging model and the environmental attenuation model. In the camera imaging model, the detector pixels are virtually divided into smaller units to improve the calculation accuracy. Parametric effects of the division number on the calculation of SM are analyzed. The improved model is verified by two simulation experiments, whose results suggest that multiple point sources in the 3D environment with obstacles can be accurately located with their intensity estimation difference less than 5% after several iterations. Keywords: Radiation sensing · Gamma camera · Intensity distribution · System matrix

1 Introduction The development of nuclear energy brings economic and social benefits to people. However, in case of the nuclear emergency, it also brings security risk, and the environment generally has strong radioactivity. The charged particles and high-energy gamma-rays bring many challenges to robots. The computer systems, sensors, and other equipment carried by the robots may face the risk of failure after absorbing excessive radiation [1]. Thus, if the robot can sense the location and intensity of radiation sources in the surrounding environment, it will be of great importance to make decisions on subsequent actions and avoid excessive radiation exposure. The traditional equipment for gamma-ray detection is called Geiger counter. Some research on estimating the intensity distribution has been carried out by equipping a © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 204–214, 2021. https://doi.org/10.1007/978-3-030-89092-6_19

Intensity Distribution Estimation of Radiation Source

205

Geiger counter on the robot [2, 3]. However, because the data obtained by the Geiger counter is not directional, the robot needs to go to a large number of discrete detection points before it can accurately estimate the location of the sources. The robot would inevitably receive more radiation. More recently, many studies have developed gamma cameras to measure the distribution of radioactive materials in high-dose environments [4–6]. The position of the radiation sources can be observed visually by overlaying the gamma image with the optical image. However, a gamma camera can only provide the direction of the incident gamma-ray instead of the location coordinates of the source. In addition, the measured value is the intensity at the camera position rather than the activity of the source. Some studies [7–11] introduce the maximum likelihood expectation maximization (MLEM) algorithm, which is widely used in medical image reconstruction, to locate the radiation sources. In this algorithm, the system matrix (SM) constructs the relationship between sources and the detected data. It is necessary to establish an accurate SM to estimate the intensity of sources. Generally, there are three ways to get the SM: analytical calculation, Monte Carlo simulation, and experimental measurement. In the study [8], the researchers obtained SM by Monte Carlo simulation after the camera acquired data at multiple detection points and reconstructed the radiation field off-line. SM gained by this way is only applicable for off-line reconstruction since it’s very time-consuming. To achieve in-situ reconstruction, the SM needs to be obtained by analytical calculation. The analytical calculation methods are different for varying equipment because of the different imaging principles. The SM calculation method of self-developed equipment [10] and Compton camera [11] is studied. The SM calculation method for the gamma camera with collimator in medical image reconstruction is called ray-driven. The research on optimizing the ray-driven method mainly focused on 2D environment [12–14]. In addition, many differences between reconstruction on the nuclear industry and medical image affect the quality. Hence, in this paper, an improved ray-driven method is proposed, which can estimate the intensity distribution of the radiation source in the 3D obstacle environment more accurately without losing too much computing speed. The overall framework of the proposed method and the specific calculation method of the model-based SM is explained in Sect. 2. In Sect. 3, we analyze the influence of the number of divisions on the calculation results. At the same time, reconstruction cases are investigated to demonstrate the effectiveness of this method. Conclusions are drawn in Sect. 4.

2 The Proposed Method Measured values of the camera is determined by the attenuation and the geometric relationship. In this study, we assume that the pose of gamma camera and the 3D environment information have been obtained by SLAM algorithm before reconstructing. The position relationship between the obstacles and the detector can be mastered through the 3D map model established by SLAM. Based on that, the attenuation of gamma-ray emitted by the source to the sensor can be calculated. The overall framework of this method is shown in Fig. 1. The environment is discretized into N voxels. The intensity of radiation in each voxel is represented by vector

206

Z. Zhang et al.

x = [xj ] (j = 1, 2, . . . , N ). At this time, the distribution of 3D intensity is transformed into vector. Similarly, 2D image is transformed into vector for representation. If the number of detected values could be obtained at each point is I . After measuring at m points, all the detected data are combined to form the vector b = [bi ] (i = 1, 2, . . . , M ), where M = m × I . The SM is used to represent the projection process from voxels to detection values. Each element of SM represents the contribution of a voxel to a detector pixel. It consists of the calculation results of two models: the camera imaging model and the environmental attenuation model. SM is represented by matrix A = [aij ]. Then, the detection process could be expressed as an equation of Ax = b. The MLEM iterative method will be used to solve the equation. Finally, the intensity distribution of the radiation source on each voxel can be estimated.

Fig. 1. The overall framework of the proposed method for estimating the intensity distribution of radiation sources.

2.1 Camera Imaging Model In this study, we use a pixelated gamma camera with a collimator to obtain radiation data. The collimator is made of metal material, and there is a pinhole in it. The schematic diagram of the imaging principle is shown in Fig. 2. The radiation source in the voxel emitted gamma-rays in any direction. A part of the rays pass through the pinhole and reach the detector plane. Others would be absorbed by the collimator. Considering that the pinhole has a certain diameter, the area covered by blue could collect gamma-rays emitted from the voxel in the figure. When calculating the weight of each voxel to each detector pixel, we further subdivide the detector pixels into n × n units. The parameter n is called as the number of divisions. On the left side of Fig. 2 is the result of dividing the red pixel into 5 × 5. After dividing the pixel into small units, we could connect the center of each unit with the center of the voxel and calculate the intersections of lines and collimator plane. According to the geometric relationship between intersections and pinhole, the proportion of lines that could pass through the pinhole is calculated. The value is written as wij , which is regarded as the contribution of the jth voxel to the ith detector pixel in imaging. Because the camera is located in the region to be reconstructed, many voxels are outside the camera’s field of view (FOV). For these voxels, wij is equal to 0. Therefore, these voxels could be excluded in advance to shorten the calculation time.

Intensity Distribution Estimation of Radiation Source

207

Fig. 2. Imaging principle of single-pinhole collimator

2.2 Environmental Attenuation Model The data measured by gamma cameras are the intensity of the gamma-rays transported to the sensor. The number of photons received by the detector is less than that emitted by the source. It is necessary to establish the environmental attenuation model to estimate the radiation intensity at the source. The attenuation of gamma-rays is mainly affected by two factors: distance and shielding. Distance refers to the attenuation of gamma-ray intensity caused by the propagation distance during transport. This part of attenuation obeys the inverse square law, as shown in Eq. (1). I1 = I0 /4π r 2

(1)

where I 1 is the intensity at the detector (or the number of photons), I 0 is the intensity of the radiation source, r is the distance between source and detector. Shielding refers to the attenuation of gamma-ray intensity caused by the physical obstacles that gamma-ray passes through. When gamma-rays reach an object, there is a certain probability of absorption and scattering. It can block or reduce the number of photons passing through the object. The following exponential equation can express the attenuation of this part: I2 = I0 e−μ(E)ρd

(2)

where I 2 is the intensity after passing through the object, μ(E) is the attenuation coefficient, ρ is the object’s density, d is the distance that the ray travels through the object. The density could be obtained from the previously determined environment information. The attenuation coefficient is a function of gamma-ray energies, so we can set up a lookup table in advance. In the calculation, the attenuation coefficient could be found according to the energy of gamma-rays. In realistic environments, gamma-rays emitted by the source may pass through more than one object before reaching the detector. Therefore, when the initial intensity of the source in the jth voxel is I 0 , the intensity measured by the ith detector can be calculated as: I = I0 Sij 

Sij = e k

−μk dijk

/rij2

(3)

208

Z. Zhang et al.

where Sij is regarded as the contribution of the jth voxel to the ith detector pixel after attenuation, k is the index of the voxel where the obstacle is located, rij is the distance between the jth voxel and the ith pixel. 2.3 MLEM Iterative Algorithm By combining the results of models proposed in the first two sections, we could get the weight of the jth voxel to the ith pixel, and then calculate the SM. It is known that each pixel value bi represents the weighted sum of contributions of all voxels in the environment, as Eq. (4). If the number of projection data is M , then a system of M equations can be formed. The intensity distribution of radiation sources could be calculated by solving the system of equations. Because of the high dimension of SM, it is difficult to obtain an analytical solution. MLEM iterative algorithm [15] is usually used to find the approximate solution of this kind of equations. bi =

N 

aij xj

(4)

j=1

aij = wij Sij The principle of MLEM algorithm will not be introduced in detail in this paper, and the iterative equation is as follow: (k+1) xj

(k)

xj = i

aij

 i

aij  j

bi a x(k) ij j 

(5)



where k is the number of iterations.

3 Simulation Experiment and Results Discussion 3.1 Design of the Simulation Experiment The Experiment of SM Calculation. To analyze the influence of pixel subdivision on the SM calculation results, a simple SM calculation experiment is set up. We calculate the SM between 30 × 30 voxels (5 × 5 cm2 per voxel) and 20 × 20 detector pixel array (2 × 2 mm2 per pixel) when the number of divisions n takes different values. The schematic diagram of the experiment is shown in Fig. 3. All voxels are located on the plane 2 m away from the collimator plane. The simulated camera focal length is 5 cm. MATLAB is used to calculate SM and analyze results. At the same time, the Monte Carlo simulation result of SM is also calculated by MCNP5 code for comparison. The Experiment of Reconstruction. For evaluating the feasibility of this method, two experimental scenarios are set up: 1) Single point source in an open environment; 2) Multiple point sources in an obstacle environment. The size of the space to be reconstructed is set as 8 × 5 × 3 m3 , and the size of each voxel is 10 × 10 × 10 cm3 . The

Intensity Distribution Estimation of Radiation Source

209

Fig. 3. Model of the SM calculation experiment.

objects made of concrete and iron are placed in the obstacle environment, as shown in Fig. 4. The parameters of radiation sources are shown in Table 1. The intensity is defined as the total number of particles emitted per unit of time, which is proportional to the activity.

Fig. 4. Material and distribution of objects in the obstacle environment.

Table 1. The nuclide, coordinate, and intensity of point source. Type

Position (x, y, z) (cm)

Intensity

Scenario 1

Cs-137

(405 405 155)

1010

Scenario 2

Cs-137

(535 435 55)

4 × 109

Cs-137

(535 235 105)

2 × 109

Co-60

(375 375 105)

4 × 109

In each scenario, we use MCNP5 to simulate the camera measurement results. And the parameter settings of the pinhole camera during simulation are shown in Table 2. After that, the simulation result are inputted into the reconstruction program written by MATLAB2020a. The reconstruction program outputs an estimate of the source intensity in each voxel in the environment.

210

Z. Zhang et al. Table 2. Simulated camera parameters in MCNP5.

Parameter

Value

Size of the detector plane

4 × 4 cm2

Detector pixel array

40 × 40

Distance between detector plane and pinhole

5 cm

Pinhole diameter

0.5 cm

3.2 Results and Discussion SM Calculation Experiment When the pinhole diameter is set to 5 mm, the calculated SMs of n = 1 and n = 10 are shown in Fig. 5. The result of n = 1 is equivalent to the ray-driven algorithm. After enlarging the red part of the image, we could notice that the value of each element of SM is almost the same when n = 1. Because in this case, the weight calculated through the camera imaging model is either 0 or 1. At the same time, many elements that should have positive values are considered as 0.

Fig. 5. The calculated value of SM when n was 1 (left), 10 (right).

We further calculate the relationship between n and relative error. For long-distance imaging, the SM is greatly affected by the diameter of pinhole and the length of detector pixel. Keeping the detector length unchanged and changing the pinhole diameter, the relative errors between the calculated results and the MCNP simulation results of the three specific elements (a19,1 , a10,15 , a190,435 ) in SM under different parameter n are calculated. The results are shown in Fig. 6. The black dashed in the figure indicated the relative error of ±5%. We could notice that with the increase of n, the error between the calculated results and the simulation results converged. We could know that the larger the parameter k is, the more accurate the result is. But it also takes more time to calculate. Table 3 shows the time required for calculation when parameter n takes different values (the size of SM is 400 × 900). We found that the time was increased dramatically by increasing n. Therefore, it is necessary to find a suitable value of n to balance the calculation speed and accuracy. We

Intensity Distribution Estimation of Radiation Source

211

Fig. 6. The relation curve between n and relative error.

Table 3. The relationship between calculation time of SM and parameter n. n

1

2

3

4

5

10

20

50

100

Time (sec)

0.095

0.107

0.123

0.132

0.158

0.300

0.819

4.111

15.71

define parameter k to represent the ratio between the length of the unit after subdivision and the pinhole diameter: k=

d l/n

(6)

where d is the diameter of pinhole, l is the length of detector pixel. k is further calculated when the error is less than 5% in each above four cases. The results are 13, 12, 12, and 10, respectively. From the above experiments, we could consider that when the length of the subdivided unit is about 1/10 of the pinhole diameter, the calculated SM has high accuracy and relatively fast speed. Reconstruction Experiment Single Point Source in an Open Environment. The actual location of the source in the environment is shown in Fig. 7a. In this scenario, four detection points are selected to obtain projection data. The black voxels indicate the location of each detection point, and the lines indicate the direction of measurement. The red voxel is the actual location

212

Z. Zhang et al.

of the source. When the number of division n is 1 and 10, the reconstruction results after 100 iterations are shown in Fig. 7b and Fig. 7c. The size of SM is 6400 × 120000, and the calculation time is 57 s and 140 s, respectively (without environmental attenuation). It is noted that the resolution is higher when n is 10, and the intensity is mainly distributed in the correct voxel. We sum the intensities of all voxels around the correct voxel in the case of n = 1, and the result is 9.27 × 109 . The intensity of red voxel is estimated to be 9.82 × 109 when n is 10. The relative error is 1.8%. Therefore, the pixel subdivision could improve the accuracy of the results but cost more time.

Fig. 7. Actual location and reconstruction results of a single point source in the open environment. (a) The actual location of the source and the detection points; The reconstruction result when n = 1 (b) and n = 10 (c).

It is noted that some voxels far away from the real position in the results are also considered to have a source whose intensity is several orders of magnitude lower than the maximum. The insufficient number of detection points may cause this phenomenon. Some voxels only have projections in one direction so that accurate values couldn’t be determined. This problem can be solved by optimizing the detection point selection strategy. Multiple Point Sources in an Obstacle Environment. Due to the limitation of FOV, only part of the sources could be detected at some positions. We select seven points for reconstruction. The actual location of the radiation source and the reconstruction results after 300 iterations are shown in Fig. 8, where gray voxels represent the obstacles. The voxels with an intensity greater than 109 in the results are marked in red. When n is 1, the reconstruction result is very poor. From the result in the case of n = 10, we could find that there are three red voxels, which correspond to the real positions of the three radiation sources. The intensities of the three red voxels are estimated to be 3.94 × 109 , 1.97 × 109 , and 4.13 × 109 , respectively. The relative error is less than

Intensity Distribution Estimation of Radiation Source

213

5%. Similarly, if the selection of detection points is optimized, the reconstruction results could be improved.

Fig. 8. Actual location and reconstruction results of multiple point sources in the obstacle environment. (a) The actual location of the sources and the detection points; the reconstruction result when n = 1 (b) and n = 10 (c).

4 Conclusions In this paper, we propose an optimization method of analytic calculation of SM. The accuracy of estimation of intensity distribution is improved by dividing the detector pixels into smaller units. The influence of the number of division n on the calculation of SM is studied. The result shows that the calculation is more accurate by increasing n, but it will spend more time. When the length of the subdivided unit is about 1/10 of the pinhole diameter, the reconstruction results show high accuracy, and the increase of calculation time is acceptable. Simulation experiments are carried out in several scenarios to prove the effectiveness of the method. This method could locate the unknown number of radiation sources in the correct voxels. The relative error of estimation of intensity is less than 5% after iterations. In future work, we will consider optimizing the selection of detection points. Furthermore, the choice of subsequent detection points could be optimized according to the reconstruction results estimated from the existing data so that the robot could complete the high-accuracy reconstruction task and receive less radiation at the same time. Acknowledgement. This work is jointly supported by National Natural Science Foundation of China (U1813224) and National Key R&D program of China (2019YFB1310801).

214

Z. Zhang et al.

References 1. Nagatani, K., Kiribayashi, S., Okada, Y., et al.: Emergency response to the nuclear accident at the Fukushima Daiichi Nuclear Power Plants using mobile rescue robots. J. Field Robot. 30(1), 44–63 (2013) 2. Andersonl, R.B., Pryor, M., Landsberger S.: Mobile robotic radiation surveying using recursive bayesian estimation. In: IEEE International Conference on Automation Science and Engineering, pp. 1187–1192. IEEE, Vancouver (2019) 3. Peterson, J., Li, W., Cesar-Tondreau, B., et al.: Experiments in unmanned aerial vehicle/unmanned ground vehicle radiation search. J. Field Robot. 36(4), 818–845 (2019) 4. Okada, K., Tadokoro, T., Ueno, Y., et al.: Development of a gamma camera to image radiation fields. Prog. Nucl. Sci. Tech. 4(1), 14–17 (2014) 5. Ueno, Y., Takahashi, I., Ishitsu, T., et al.: Spectroscopic gamma camera for use in high dose environments. Nuclear Instr. Methods Phys. Res. Sect. A Accelerat. Spectromet. Detect. Assoc. Equip. 822, 48–56 (2016) 6. Sueoka, K., Kataoka, J., Takabe, M., et al.: Development of a new pinhole camera for imaging in high dose-rate environments. Nuclear Instr. Methods Phys. Res. Sect. A Accelerat. Spectromet. Detect. Assoc. Equip. 912, 115–118 (2018) 7. Mihailescu, L., Vetter, K., Chivers, D.: Standoff 3D gamma-ray imaging. IEEE Trans. Nuclear Sci. 56(2), 479–486 (2009) 8. Kim, D., Woo, H., Ji, Y., et al.: 3D radiation imaging using mobile robot equipped with radiation detector. In: 2017 IEEE/SICE International Symposium on System Integration, SII, pp. 444–449. IEEE, Taipei (2017) 9. Wang, R., Wang, S., Fan, P., et al.: Absolute gamma source positioning with position-sensitive scintillation detector arrays. In: 2018 IEEE Nuclear Science Symposium and Medical Imaging Conference Proceedings. NSS/MIC, pp. 1–4. IEEE, Sydney (2018) 10. Hellfeld, D., Barton, P., Gunter, D., et al.: Real-time free-moving active coded mask 3D gamma-ray imaging. IEEE Trans. Nuclear Sci. 66(10), 2252–2260 (2019) 11. Lee, M.S., Shy, D., Whittaker, W.R., et al.: Active range and bearing-based radiation source localization. In: 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. IROS, pp. 1389–1394. IEEE, Madrid (2018) 12. Wietholt, C., Hsiao, T., Chen, C.T.: New ray-driven system matrix for small-animal pinholeSPECT with detector blur, geometric response and edge penetration modeling. In: 2006 IEEE Nuclear Science Symposium Conference Record, vol. 6, pp. 3414–3419. IEEE, San Diego (2006) 13. Jeong, M., Hammig, M.D.: Comparison of gamma ray localization using system matrixes obtained by either MCNP simulations or ray-driven calculations for a coded-aperture imaging system. Nuclear Instr. Methods Phys. Res. Sect. A Accelerat. Spectromet. Detect. Assoc. Equip. 954, 161353 (2020) 14. Moslemi, V., Erfanian, V., Ashoor, M.: Estimation of optimized timely system matrix with improved image quality in iterative reconstruction algorithm: a simulation study. Heliyon 6(1), e03279 (2020) 15. Shepp, L.A., Vardi, Y.: Maximum likelihood reconstruction for emission tomography. IEEE Trans. Med. Imaging 1(2), 113–122 (1982)

Design and Development of Multi-arm Cooperative Rescue Robot Actuator Based on Variant Scissor Mechanism Yu Shan1,2 , Yanzhi Zhao1,2(B) , Kaida Guo3 , Dongyang Xu1,2 , Wannan Zhao1,2 , and Haibo Yu1,2 1 Hebei Provincial Key Laboratory of Parallel Robot and Mechatronic System,

Yanshan University, Qinhuangdao 066004, China [email protected] 2 Key Laboratory of Advanced Forging and Stamping Technology and Science, Ministry of Education of China, Yanshan University, Qinhuangdao 066004, China 3 Overseas Manufacturing Management Department, CITIC Dicastal Co. Ltd., Qinhuangdao 066011, China

Abstract. Search and rescue has become a key technology to be solved, but it is difficult for most rescue robots to implement safe and effective rescue for survivors. Based on two degree of freedom five bar variant scissor mechanism, this paper proposes a design scheme of combined bionic multi-arm cooperative rescue robot actuator. The actuator was designed to imitate elephant trunk, octopus and armadillo and the working track of rescue manipulator is designed by the cross section of human body. The kinematics of the actuator is analyzed based on the vector method, and the positive position solution and Jacobian matrix are obtained. The optimal design parameters of the actuator are determined, and the experimental prototype of the rescue robot actuator is developed, which lays the foundation for the development of the new rescue robot. Keywords: Rescue robot · Variant scissor mechanism · Kinematics analysis

1 Introduction Global accidents and natural disasters occur frequently, which seriously threaten human security and social stability. In order to save the lives of the survivors, it is so important to reach and rescue the survivors within 48 h after the disaster. At present, the rescue robot technology is developing rapidly, and various innovative researches are applied to the end-effector of the rescue robots, aiming at providing assistance and safety to the survivors in emergency. The end-effector is similar to “robot’s hand” and is used to grasp objects. In the rescue, these machines can replace human beings into complex and dangerous areas, and rescue the injured person. In the United States “911” terrorist attack disaster relief, the successful application of rescue robots [1–4] has triggered a research boom of rescue robots in universities, companies and research institutions around the world. In fact, the © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 215–225, 2021. https://doi.org/10.1007/978-3-030-89092-6_20

216

Y. Shan et al.

research of rescue robots mainly includes chassis walking part and the end-effector part. In order to make the robot move better in complex terrain, researchers have successfully developed various chassis walking mechanisms, such as wheeled type [5, 6], tracked type [7, 8] and legged type [9, 10]. However, how to rescue the injured person safely and effectively, that is, the research of the end-effector, has become a major challenge at present. Most of the existing and available end-effector in the market are not safe, which puts forward higher requirements for the power supply and control system. The new rescue robot BEAR [11] developed by Vecna Robotics is mainly used for the rescue and transportation of the wounded after disaster. The upper limb is equipped with two arm actuators, which are driven by hydraulic devices. The actuator has the advantages of high power and large bearing capacity. The robot RoVA [12] developed by Hstar Technologies is designed to meet the needs of nursing service, with powerful arms and many intelligent sensors. South Korea’s national defense development agency has developed a human rescue robot HURCULES [13], which is equipped with an electric end-effector. Ahalya Ravendran [14] of Thammasat University in Thailand has designed and developed a mobile robot with gripper and driller. At present, scholars from various countries have studied the end-effector of rescue robots accordingly, but there are still some problem: for example, they are noisier, more difficult to maintain, and have poor ring closure, etc. At present, it is difficult for most rescue robots to implement safe and effective rescue for survivors. In order to significantly improve the safety and overall carrying capacity of rescue robots, this paper designs a safe and effective rescue robot end-effector based on variant scissor mechanism that is applicable to the survivors, so as to achieve the purpose of rescue for survivors.

2 Design of Robot Actuator Nature has been the source of various scientific and technological principles and major inventions. In recent years, more and more scientific researchers focus on biology and propose many bionic robots. In the design process of rescue manipulator, we found some unique biological forms. In the ocean, the octopus arm can move flexibly, and its arms cooperate with each other to grasp objects better (Fig. 1). The elephant trunk is soft and nimble, enabling flexible grasping of large loads, and the armadillo hard-shell is retractable and foldable, reducing the space constraints on some other sports. Based on the combined bionics principle, the overall design scheme imitates the shape characteristics of octopus multi-arm cooperation, and the design of the rescue robot arm is inspired by the elephant trunk and armadillo, as shown in Fig. 1. Imitation octopus multi-arm collaborative rescue actuator design effectively reduces the weight of the human body apportioned to each rescue robot arm, improving the overall load-bearing capacity of the actuator while reducing the local force on the human body, improving contact safety and comfortable. At the same time, the combination of octopus and elephant trunk can simulate the cooperative transportation of multiple elephant trunks, which can realize large carrying capacity. The elephant trunk shape and its gripping movement can enable to hold the human body safely and stably, and the folding form of armadillo can increase the operating space of the actuator.

Design and Development of Multi-arm Cooperative Rescue Robot Actuator

217

Fig. 1. Combined bionic design inspiration

So this paper proposes a design scheme of multi-arm cooperative rescue robot actuator, which is composed of three pairs of rescue manipulator, as shown in Fig. 2.

Fig. 2. Overall design of rescue robot actuator

By combining the above three organisms in a biomimetic manner, this paper designs the rescue end-effector. As an important part of the rescue robot actuator, the structure of the rescue manipulator framework directly determines the performance of the rescue robot actuator.

Fig. 3. Schematic diagram of operation range of rescue robot actuator

In order to design a reasonable manipulator framework mechanism, the first consideration is that it must meet the operational requirements, followed by the need to have a

218

Y. Shan et al.

certain load-bearing capacity and a small space scale, and finally, safety and reliability, processing economy and other factors must be regarded. This paper designs a rescue robotic arm skeleton mechanism scheme based on two degree of freedom five bar variant scissor mechanism, and its overall structure and operation range are shown in Fig. 3. The scheme can meet the action requirements of rescue operation and bear the load of the whole human body and has good stiffness, large working space, convenient driving. At the same time, it has good telescoping and folding characteristics of the scissor-fork mechanism, the unfolding state can meet the large-scale range of operation when working, and the folding state can save space when not working, which is convenient for transportation. The design of less degrees of freedom makes the drive setting and its control easy to realize, and the work is stable and reliable. The skeleton mechanism of rescue manipulator has multiple revolute and prismatic pairs. In the drive selection, priority should be given to setting the drive on the prismatic pair connected with the frame, which can effectively reduce the motion inertia of the mechanism and improve the flexible response performance of the mechanism. Due to the limitation of the mechanism layout space, it is difficult to set the drive motor and reducer at the revolute pair, so the prismatic pair connected with the frame are selected as the drive joints.

3 Kinematics Analysis of the Actuator 3.1 Establish the Kinematic Model The executive mechanism of rescue robot is composed of several pairs of rescue manipulators, and the kinematic performance of the rescue manipulator is determined by the internal rigid skeleton. According to the skeleton configuration of the two degree of freedom variant scissor rescue executive mechanism, the kinematic model of the skeleton mechanism is established. First, a global coordinate system O − XY is established, as shown in Fig. 4. The origin O is located on the axis of the first branch prismatic pair, the axis X coincides with the axis of the first branch, and the coordinate system O1 − XY of the first branch coincides with the global coordinate system. A branch coordinate system O2 − XY is established on the second branch. The second branch of the axis X moves the axis of the secondary axis coincides, and is parallel to the global coordinate system axis X . The distance between the two axis is e, and the axis Y coincides with the axis Y of the global coordinate system. Therefore, the transformation matrix from the first branch coordinate system O1 −XY to the global coordinate system O −XY is a 3 × 3 unit matrix. The transformation matrix o T from the second branch coordinate system O − XY to the global coordinate system 2 2 O − XY is determined by the structural parameters e of the mechanism, namely: ⎡ ⎤ o o  100 R2 t2 o = ⎣0 1 e⎦ T2 = (1) 0 1 001 Where o T2 denotes the transformation matrix from coordinate system O2 − XY to coordinate system O − XY . o R2 is the direction transformation matrix from coordinate

Design and Development of Multi-arm Cooperative Rescue Robot Actuator

219

system O2 − XY to coordinate system O − XY . o t2 is the position transformation matrix from coordinate system O2 − XY to coordinate system O − XY . e is the distance between the axis X of the coordinate system O2 − XY and the axis X of the coordinate system O − XY .

Fig. 4. Framework coordinate system of rescue manipulator

Through the transformation matrix, the point coordinates in the second branch coordinate system can be transformed into the global coordinate system, that is, there is   T T xo , yo , 1 = o T2 x2 , y2 , 1 . According to the relationship between the connecting rods in Fig. 3, ρ1 and ρ2 are the input variable of the prismatic pair in the first branch and the second branch respectively, and the length of the passive connecting rod in the first branch is l1 ,the length of the passive link in the second branch is l2 , and other structural parameters are l3 = 100 mm, l4 = 200 mm, l5 = 250 mm, θ = π9 , respectively. Point P is the operating point at the end of the manipulator. As can be seen from Fig. 4, there is the following vector relationship:       ρ1 l1 cos α1 xB = + yB 0 l1 sin α2 (2)         xB 0 l cos α2 ρ = − 2 + 2 yB 0 l2 sin α2 e Where xB denotes the abscissa of point B in the coordinate system O − XY . yB is the ordinate of point B in the coordinate system O − XY . ρ1 is the input displacement of the slider A1 , l1 is the length of 1st rod, α1 is the angle between 1st rod and the axis X positive direction of the coordinate system O − XY . ρ2 is the input displacement of the slider A2 , l2 is the length of 2nd rod, α2 is the angle between 2nd rod and the axis X positive direction of the coordinate system O − XY . The algebraic equations are as follows:

ρ1 − ρ2 + l1 cos α1 − l2 cos α2 = 0 (3) l1 sin α1 − l2 sin α2 − e = 0

220

Y. Shan et al.

3.2 Positive Solution of Position According to the geometric position of the mechanism, the coordinate of the manipulator end operation point P rod can be obtained:   ρ1 + (l1 + l4 ) cos α1 + l3 cos α2 + l5 cos(α2 − θ ) xP (4) = yP (l1 + l4 ) sin α1 + l3 sin α2 + l5 sin(α2 − θ ) Where xP is the abscissa of point P in the coordinate system O − XY . yP is the ordinate of point P in the coordinate system O − XY . θ is the angle between 3rd rod and 5th rod. l5 is the length of 5th rod. In order to get the mapping relationship between the position of the manipulator end operation point P and the position of the input terminals ρ1 and ρ2 , the sum of independent variables α1 and α2 in the middle should be eliminated. Then rewrite Eq. (3) as:

l2 cos α2 = ρ1 − ρ2 + l1 cos α1 (5) l2 sin α2 = l1 sin α1 − e The expression of α1 can be obtained by square elimination of α2 on both sides of the equation: A sin α1 + B cos α1 + C = 0

(6)

Where: A = −2el1 B = 2(ρ1 − ρ2 )l1 C = (ρ1 − ρ2 )2 + e2 + l12 − l22 . Let x = tan(α1 /2), then sin α1 = 2x/1 + x2 cos α1 = 1 − x2 /1 + x2 , substitute formula (6), the equation about α1 can be transformed into quadratic equation about x: (B − C)x2 − 2Ax − (B + C) = 0

(7)

√ A± A2 +B2 −C 2 , B−C

according to the arrangement of The solution is α1 = 2 arctan connecting rod, the “+” sign is taken here. √ 2 +E 2 −F 2 In the same way, α2 = 2 arctan D± DE−F , according to the arrangement of connecting rod, we can get the “-” sign here. Where: D = 2el2 , E = −2(ρ1 − ρ2 )l2 , F = (ρ1 − ρ2 )2 + e2 + l22 − l12 . By substituting equations α1 and α2 into Eq. (4), the forward position solution of the rescue manipulator can be obtained:   ρ1 + f xP = g yP

(8) f = f (ρ1 − ρ2 ) g = g(ρ1 − ρ2 ) It can be seen from Eq. (8) that the ordinate of the end P of the manipulator is only related to the difference ρ1 − ρ2 of the two input parameters, and has nothing to do with the specific value of a single input variable. The function with abscissa ρ1 − ρ2 is the sum of one of the inputs.

Design and Development of Multi-arm Cooperative Rescue Robot Actuator

221

3.3 Velocity Analysis The first derivative of time can be obtained by formula (5):

−l3 sin α2 ω2 = v1 − v2 − l2 sin α1 ω1 l2 cos α2 ω2 = l2 cos α1 ω1

(9)

Where v1 is the input speed of slider A1 , v2 is the input speed of slider A2 . ω1 is the angular speed of 1st rod. ω2 is the angular speed of 2nd rod, the solution is as follows: ⎧ (v1 −v2 ) cos α2 ⎪ ⎪ ⎨ ω1 = l1 sin(α1 − α2 ) (10) (v −v ) cos α1 ⎪ ⎪ ⎩ ω2 = 1 2 l2 sin(α1 − α2 ) 

The first derivative of time can be obtained by formula (4):   vPx x˙ P v1 − (l1 + l4 ) sin α1 ω1 − l3 sin α2 ω2 − l5 sin(α2 − θ)ω2 = = (11) vPy y˙ P (l1 + l4 ) cos α1 ω1 + l3 cos α2 ω2 + l5 cos(α2 − θ )ω2

Where vPx is the component of point P velocity in axis X direction and vPy is the component of point P velocity in axis Y direction. It can be further expressed as √

2 +B2 −C 2 α1 = 2 arctan A+√AB−C (12) 2 +E 2 −F 2 α2 = 2 arctan D− DE−F Where:

A = −2el1 , B = 2(ρ1 − ρ2 )l1 , C = (ρ1 − ρ2 )2 + e2 + l12 − l22 , D = 2el2

E = −2(ρ1 − ρ2 )l2 , F = (ρ1 − ρ2 )2 + e2 + l22 − l12 By substituting Eq. (10) into Eq. (11), we can get   vPx v1 + u = vPy w

u = u(v1 − v2 ) w = w(v1 − v2 )

.

(13)

It can be seen from Eq. (13) that the velocity in Y direction at the end P of the manipulator is only related to the difference between the two input parameters, and has nothing to do with the specific value of a single input variable. By calculating the first derivative of Eq. (8), we can get   x˙ P ρ˙1 + f˙ = y˙ P g˙ ⎧ df ⎪ (14) ⎪ (ρ˙1 − ρ˙2 ) ⎨ f˙ = d (ρ1 − ρ2 ) dg ⎪ ⎪ ⎩ g˙ = (ρ˙1 − ρ˙2 ) d (ρ1 − ρ2 )

222

Y. Shan et al.

Therefore, the Jacobian matrix of rescue manipulator can be obtained  1 + d (ρ1df−ρ2 ) − d (ρ1df−ρ2 ) J = dg − d (ρ1dg−ρ2 ) d (ρ1 −ρ2 )

(15)

3.4 Trajectory Simulation The established kinematics model of rescue manipulator is analyzed, and the relevant conclusions of position solution and velocity solution are obtained. Based on ADAMS software, the simulation platform of rescue robot actuator is established. The variables of minimum mean square error of l1 , l2 and e are analyzed respectively, and three groups of optimal solutions are obtained. Then the motion simulation of the three groups of solutions is carried out, and the trajectory simulation results are obtained and compared with the theoretical analysis results. The 3D model of rescue manipulator is established in SolidWorks software. The 3D model is imported into Adams, and then the constraint conditions and motion pairs are set, as shown in Fig. 5. 300 325

Py(mm)

350 375 400 425 450 575

Fig. 5. ADAMS simulation model

550

525

500

475

Px(mm)

450

425

400

375

Fig. 6. Trajectory diagram of rescue manipulator end

In the ADAMS software, three groups of rescue manipulator arm end point operation trajectory diagram are shown in Fig. 6. Compared with the simulation results in the Fig. 6, the simulation results are basically consistent with the theoretical results, which verifies the correctness of the theoretical analysis, and its trajectory is basically the same as that of human body contour.

4 Prototype Development Based on the theoretical design and kinematics analysis, this paper developes the experimental prototype of the multi-arm cooperative rescue robot’s end-effector. The prototype scheme of rescue robot actuator is shown in Fig. 7. This scheme mainly includes three parts: frame, rescue manipulator and drive transmission mechanism. Among them, rescue manipulators have three groups and include rigid framework and flexible safety cushion, and each group contains two mirror manipulator frameworks. In the development of experimental prototype, on the premise of

Design and Development of Multi-arm Cooperative Rescue Robot Actuator

223

Fig. 7. Prototype drawing of rescue actuator

meeting the requirements of mechanism principle verification and experiment, the manufacturing process feasibility, assembly feasibility, cost economy and other factors should be fully considered. After comprehensive consideration of experimental requirements and research resources, the principle of prototype development with standard parts as the main part and non-standard parts as the auxiliary part is formulated. Based on this principle, the prototype is designed and manufactured.

Fig. 8. Rescue actuator

The rescue robot arm multi-link skeleton is made of aluminum profiles and aluminum alloy connectors spliced together, with good rigidity and standard profiles for easy connection. As a direct contact part with human body, the end drag reduction mechanism chooses a 3D printing shell made of high-strength nylon material and an active rotating conveyor belt to give consideration to the bearing capacity and experimental safety, which can reduce the resistance in the process of contact between the rescue robot actuator and human body and effectively avoid the secondary injury of the actuator to the rescued human body. The outer flexible coating is made of soft rubber material with moderate hardness based on 3D printing. It is wrapped outside the rigid framework and inflated in the cavity. It separates the human body from the rigid rescue manipulator framework and stabilizes the position of the human body. In the process of flexible contact with the human body, the pressure on the human body is evenly distributed, which effectively improves the safety of human-computer interaction. The prototype of the developed rescue actuator is shown in Fig. 8.

224

Y. Shan et al.

5 Conclusion In the rescue process, in order to achieve low friction to hold and support the human body, the paper designs a multi-arm cooperative rescue robot actuator based on the combined bionic principle, which can safely and effectively carry the injured people. The position of the actuator and the relationship between the end velocity and the input velocity are obtained based on vector method. Finally, the design and manufacture of the experimental prototype of the rescue robot actuator are completed, and the basic action of encircling rescue is realized through motion control. The design theory of the robot provides a reference for the research and development of new rescue robot. Acknowledgements. This work was supported in part by the National Key Research and Development Program under Grant (No. 2017YFE0112200), in part by the Science and Technology Department of Hebei Science and Technology Research and Development Plan under Grant (No. 206Z1807G), in part by the Joint Fund Project of Liaoning Natural Science Foundation under Grant (No. 2021KF2206), and in part by Guangdong Province Jihua Laboratory under Grant (No. Y80311w180).

References 1. Murphy, R.R.: Rescue robotics for homeland security. Commun. ACM 47(3), 66–68 (2004) 2. Snyder, R.: Robots assist in search and rescue efforts at WTC. IEEE Robot. Automat. Mag. 8, 26–28 (2001) 3. Casper, J., Murphy, R.R.: Human-robot interaction during the robot-assisted urban search and rescue effort at the world trade center. IEEE Trans. Syst. Man Cybern. Part B Cybern. 33(3), 367–385 (2003) 4. Davids, A.: Urban search and rescue robots: from tragedy to technology. IEEE Intell. Syst. 17(2), 81–83 (2002) 5. Chiu, Y., Shiroma, N., Igarashi, H., Sato, N., Inami, M., Matsuno, F.: FUMA: environment information gathering wheeled rescue robot with one-DOF arm. IEEE Int. Saf. Secur. Rescue Robot. 12(5), 81–86 (2005) 6. Yokokohji, Y., Tubouchi, T., Tanaka, A.: Guidelines for human interface design of rescue robots. In: 2006 SICE-ICASE International Joint Conference, Busan, South Korea, pp. 3455– 3460 (2006) 7. Albiez, J., Ilg, W., Luksch, T., Berns, K., Dillmann, R.: Learning a reactive posture control on the four-legged walking machine BISAM. In: 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems, Maui, United States (2001) 8. Yamauchi, B.M.: PackBot: a versatile platform for military robotics. In: 2004 Defense and Security Society For Optics And Photonics, Florida, United States, pp. 228–237 (2004) 9. Murphy, R.: Marsupial and shape-shifting robots for urban search and rescue. IEEE Intell. Syst. 15(3), 14–19 (2000) 10. Hunt, A.J., Bachmann, R.J., Murphy, R.R.: A rapidly reconfigurable robot for assistance in urban search and rescue. In: 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, San Francisco, United States, pp. 209–214 (2011) 11. Leddy, R., Irshad, A., Metcalfe, A.: Comparative accuracy of preoperative tumor size assessment on mammography, sonography, and MRI: is the accuracy affected by breast density or cancer subtype? J. Clin. Ultrasound 44(1), 17–25 (2016)

Design and Development of Multi-arm Cooperative Rescue Robot Actuator

225

12. Ding, J., Lim, Y.-J., Solano, M., Shadle, K.: Giving patients a lift - the robotic nursing assistant (RoNA). In: 2014 IEEE International Conference on Technologies for Practical Robot Applications, Miami, United States, pp. 1–5 (2014) 13. Choi, B., Lee, W., Park, G., Lee, Y., Min, J., Hong, S.: Development and control of a military rescue robot for casualty extraction task. J. Field Robot. 36(4), 656–676 (2019) 14. Ravendran, A., Ponpai, P., Yodvanich, P., Faichokchai, W., Hsu, C.-H.: Design and development of a low cost rescue robot with environmental adaptability. In: 2019 International Conference on System Science and Engineering (ICSSE), Dong Hoi, Vietnam, pp. 57–61 (2019)

WPFBot: A Novel and Highly Mobile Amphibious Robot Fusheng Liu1 , Lei Jiang2 , Xuelong Li1 , Xinxin Liu1 , Ruina Dang3 , and Wei Wang1(B) 1 Wuhan University, Wuhan 430072, Hubei Province, People’s Republic of China

[email protected]

2 College of Computer Science and Technology, Zhejiang University, Hangzhou 310027,

People’s Republic of China 3 China North Vehicle Research Institute, Beijing 100070, People’s Republic of China

Abstract. This paper introduces WPFBot (Wheel–Paddle–Fin composite robot), an amphibious robot that features novel but simple mechanisms, versatile mobility both on terrestrial and underwater circumstances. It integrates six actuation modules which can be categorized into two kinds of structure, one is compliant propelling paddles used for thrusting underwater, the other is a wheel-fin composite structure for land moving and underwater ascent and descent control. It needs no additional operation for switching between land and water environments. We introduced in this paper the design of the prototype and demonstrated its rich motion functions through simulation and experiment methods. Keywords: Amphibious robot · Prototype design · Amphibious function realization

1 Introduction Amphibious robots in the past two decades have been developed a lot from the inspirations of amphibious creatures [12]. There are many previous robots which are designed with one kind of motion mode, such as walking, swimming or flying. All these robots can adapt solely to one specific type of environment. However, the working circumstances of robots in most cases are complex and challengeable. Therefore, many researchers tend to explore amphibious/triphibious robots which generally have two or three of air, land or water maneuvers. Researchers from all over the world have developed series of amphibious/triphibious robot prototypes. For example, in the field of water-land amphibious robots, there already have snake-like robot ACM-R5 [1], AQUA with alternative limbs to maneuver on land and underwater [2], and Salamandra Robotica II integrated with four legs and an actuated spine [3]. In the field of land-air amphibious robot, there’s deformable amphibious robot which achieve two kind of locomotion modes via deforming itself [4]. In the field of air-water robot, there are MUWA [5], CRACUNS [6]. In addition, the triphibious robot concept also was explored [7]. In general, the multi-dwelling robots have more excellent ability of motion than traditional mobile robots which has only one type of motion in view of their adaptability © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 226–235, 2021. https://doi.org/10.1007/978-3-030-89092-6_21

WPFBot: A Novel and Highly Mobile Amphibious Robot

227

to many kinds of environment. Motivated by these amazing robots and its application prospects, our focus in this paper was put on the water-land amphibious robot. We developed an amphibious robot WPFBot (Fig. 1). The robot we created has many advantages as follows:

Fig. 1. WPFBot tests underwater

1) Easy switching amphibious motion modes: Our robot achieves different modes of locomotion needs no deforming itself or other superfluous operations. Whereby it integrates two locomotion modes, the switching between them is zero delayed. 2) Versatile ability of motion: The robot integrates two different kinds of actuator structure. Therefore, it could move on the ground like wheeled mobile robots and is capable of five-degree-of-freedom motion in the water via the cooperation of paddles and wheel-fin actuators. 3) Simple drive architecture: The robot was designed for achieving excellent ability of amphibious maneuvers. Complicated mechanisms would increase difficulty of control and meanwhile, error tolerance decreases. Therefore, on the premise that the movement function can be realized, we emphasize the simplicity of robot design by integrating solely six actuators which were divided into two different forms to get amphibious locomotion. 4) Energy-efficient motions: Our robot integrated six actuators, however, moving on the land and underwater just need four actuators working at a time with the other two remain silent. 5) Low-cost: Controlling the cost in the lab is important, we have carried out many explorations on reducing the cost. And in the end, our robot was mainly manufactured using machining which is more economical than other processing methods. Also, nearly all of components are standard commercial off-the-shelf parts to reduce cost. Our robot is derived from AQUA [2, 8], a robot whose switching mode of locomotion was achieved by replacing legs and paddles, which considered has the minimal configuration and excellent adaptability to volatile environments. WPFBot inherits the amphibious ability and simple mechanisms of AQUA. Besides, the integration of two kinds of actuators makes amphibious conversion easier, and novel locomotion modes

228

F. Liu et al.

underwater and on land are more energy-efficient. However, saving energy also means decline of drive energy. For that, the maximum velocity of robot would decrease. In this paper, Sect. 2 describes the design concepts of the robot, motion principles and provides preliminary control strategy. Section 3 completes the system construction of our robot, including sealing design and control system with a focus on simple constructions and low-cost. In Sect. 4, experimental and simulation results are presented. Section 5 provides a conclusion and outlook on future application and improvement of the robot.

2 Principle and Preliminary Control Strategy 2.1 Thrusting Principle For fish and cetaceans, oscillating tails produce propulsive and maneuvering force. Researchers have employed some theoretical and experimental methods and proved that high-aspect-ratio foils would achieve analogous function [14]. In general, a foil moving at constant forward speed U performs a synthetic motion of harmonic transverse motion and pitching motion, which could be analytically depicted as follows: h(t) = h0 sin (ωt). θ(t) = θ0 sin (ωt + ϕ). Where h0 , θ0 are the amplitude of the heaving and pitching oscillations respectively, ω is the angular frequency of the oscillations, and ϕ is the phase angle between the two oscillatory motions, the point C is pivot point and foil chord is denoted by c, the definition of parameters was shown in Fig. 2. One cycle of the paddle generates two reverse vortices through periodic oscillation, thereby producing a jet of high propulsive efficiency [13]. In this paper, we just used its harmonic pitching motion with reduced propulsion efficiency which will be discussed in Sect. 4.

y

z x

Fig. 2. The compliant paddle model

Fluid

Fig. 3. The morphology of amphibious robot

Our robot morphology is as depicted in Fig. 3. It consists of a rigid body with six actuators, two compliant paddles (foils) in the middle and four wheel-fin (wheel-foil) structures in the front and rear. Each actuation architecture is only composed of one revolute degree of freedom (DOF). The attachment points of the actuation architectures are all fixed relative to the body and the compliance of paddles is mainly determined by their materials. The configuration admits movements of five DOF, two paddles provide power of thrusting via periodic oscillations and wheel-fin mechanisms adjust directions by changing attack angles.

WPFBot: A Novel and Highly Mobile Amphibious Robot

229

2.2 Underwater Motion To achieve precise motion control of the amphibious robot, we used a 4-parameterfamily controller for submarine running, which enables the robot to robustly execute a variety of different gaits like moving forward and backward, lift-up, descending, and turning. The robot’s terrestrial locomotion is in analogy with the wheeled mobile robot, which implement moving and turning using wheels with differential capacity. Different locomotion was employed depending on the environment. In this paper, we emphasize on exploring the versatile locomotion ability of the robot. As described above, our controller can be parameterized by four variables: f m (equals 2π/ω), A, ratio, and θ. The oscillatory frequency and amplitude are f m , A respectively, Ratio denotes the time-consuming T1/T2 value of the fast phase and the slow phase in a cycle as shown in Fig. 4, θ is a parameter used for tuning attack angle of the wheelfin structure. In this paper’s simulation and experiments, we used a joint space closed loop but task space open loop control strategy. The controller generates clock driven desired trajectory for each actuator (Fig. 4), which are then tracked by PD controller for each individual revolute joint while moving in one direction (forward/backward). The robot actuators can be divided into three groups, only one of the wheel-fin groups keeps working to tune pitch angle to avoid going off courses, the other keeps silence and even causes no energy consumption as described in Sect. 1. Turning has used the same strategy. The parameter f m and A play critical roles because high f m and A generally means higher propulsion power. In Sect. 4, our simulation studies reveal in detail the correlations of these parameters with certain attributes of moving forward and turning behaviors. — left paddle --- right paddle

Fig. 4. The designed displacement-time curve of paddles’ oscillating

2.3 Lift-Up and Descending Our robot adopted six actuators configuration giving the robot direct control over 5 of its 6 degrees of freedom (DOF): surge (forth and back), heave (up and down), pitch, roll, and yaw. The robot can complete ascent and descend movements forward or backward due to its symmetry (Fig. 3). The ascent/descent motion controller is the same as forward/backward gait, the two paddles oscillate periodically to provide thrusting power and the wheel-fin mechanism changes the angle of attack to tune directions.

230

F. Liu et al.

3 System Design WPFBot (Wheel–Paddle–Fin composite robot) is specifically built for dexterous motion both on land and underwater circumstances with the minimal configuration. Generally, mechanical complexity is considered as one of the major sources of failure [11]. Therefore, we paid our attention to simplifying the mechanical structures and thereby improving the reliability while usually working on terrains and underwater. As described above, our focus was put on large mobility, versatile locomotion skills, environment endurance, low cost. 3.1 The Overall Design and Specifications In robot’s design, rational placing actuators is one of the cores. Our design emphasizes achieving versatile performance by using the minimum amount of actuator and thereby promoting control robustness. Most existing similar amphibious robots are actuated by not only six actuators, where extra actuators are mounted to get amphibious capability and effective locomotion. Our developed wheel-paddle-fin integrated amphibious robot adopted completely symmetrical structures with the simplest modular mechanisms. Figure 5 shows the overall mechanical structure and main components of the wheel-paddle-fin integrated amphibious robot. It is composed of four wheel-fin integrated drive modules and two paddles in the middle. It integrates commonly used wheels and biomimetic fins together in the front and rear, used to move on the terrain and adjust direction while working underwater respectively. The middle two paddles are mainly used to provide power for swimming underwater. The remaining components also include AL frame, energy unit, control unit. The specifications of the amphibious robot are listed in Table 1. Main body

Fairwater

Paddle

Battery

fin Wheel-fin architecture

Fig. 5. Main components of WPFBot

One of advantages of our robot is that it would need no additional operations, such as exchange components, when converting from land moving to underwater swimming, which completed by automatically switching propelling method(wheel-moving and paddle-thrusting) according to the operation environment. In the wheel-moving locomotion mode, the wheel-fin integrated driving devices in the front and rear are used as driving wheels, while the middle two paddles enter no power consumption mode. In the swimming locomotion mode, the middle two paddles are used to provide forward

WPFBot: A Novel and Highly Mobile Amphibious Robot

231

Table 1. Specifications of the amphibious robot Items

Value

Dimensions (mm)

609 × 251 × 70

Weight (kg)

9.1

Maximum ground speed (m/s)

4

Maximum underwater speed (m/s)

0.34

Maximum work depth (m)

5.1

Payload capacity (kg)

2.5

thrust or adjust motion direction, while the front and rear wheel-fin integrated structures control the depth of our robot dives by providing pitch control torque. The utilization of these novel integrated deriving devices, making our amphibious robot mechanism much simplest and with excellent adaptability for the complicated amphibious environments. 3.2 Shell Sealing Design Shell for the amphibious robot is also one of core components. Excellent shell design allows for more even distribution of impact loads, reduces hydrodynamics drag while swimming, promotes environments obstacles endurance and provides a pleasing aesthetic more reminiscent of biological organisms [8]. The design of shell provides internal electronics with protection from impact, dust, moisture and other harmful environmental damage. In addition, reasonable shell design could lead to saving of cost. Low cost is one of our aspirations, composites were chosen as the shell materials at first, in view of their impact resistance, corrosion resistance performance and easy to process, which needs a little cost on conforming them to complex curvatures and any designed curvatures as needed are possible. However, it’s expensive to custom a specific shape. High-precision molds required to fabricate 3D seals are hard to get. Then to reduce mold costs, split design was adopted. The shell is composed of two AL lateral plates for mounting drive devices, three carbon fiber plates and one transparent PVC plate for easily observing the internal situation, with parts can be obtained by machining. Then Silicone Rubber was used to static sealing. One of the biggest troubles we encounters is it’s hard to maintain, disassembling the sealing shell and resealing takes so much time. As a result, a small opening is left out, which is convenient for us to inspect the robot and observe the state of running, and does not affect our static sealing. A small plate was used to seal it. This kind of sealing has the characteristics of anti-vibration, high temperature and corrosion resistance. 3.3 Shaft Sealing Shaft sealing could be complex compared with shell design because the shaft is spinning. There are many methods for sealing, and we did an evaluation of the diverse type of watertight sealing. Of course, standard commercial off-the-shelf (COTS) seal would be

232

F. Liu et al.

preferred. Among them, the mechanical seal has complex structure, high manufacturing requirements and difficult installation which is against our original intention, low-cost and simple mechanism. O-ring seal is mechanism simplified, however, it’s hard to lubricate and would cause a lot of wear and tear after long-term use. Oil seal at final became our best choice due to its easy to assembly and low friction properties. After choosing the type of sealing, the detailed sealing structure is needed. We used oil sealing rings, the bearings were used to protect sealing rings from harmful compression and sealing rings allow lubrication to prolong service life. The sealing assembly of drive module is composed of five major components: motor assembly, bearings, oil sealing rings, housing and output shaft. 3.4 Electric System Integration To minish size and weight of our robot, we utilize two 6 cell lithium-polymer battery as robot power, each with a nominal voltage of 24 V and a mass of 0.465 kg. On the motor driver, Elmo was chosen in view of its tolerance to high current. SAM9X35 was chosen to run main controller program, which has the ability of high-speed computing and multi-threaded task distributing. In terms of communication, we built Control Area Network (CAN) to transmit control signals and feedback information between the main controller and Elmo drivers.

4 Simulation and Experiments 4.1 Underwater Simulation Our simulation studies in this section were implemented for proving the robot’s locomotion controllability. We first present a simulation strategy using the control variate method for revealing the qualitative relationship among forward speed v, frequency f m , ratio and amplitude A. Figure 6 (a) shows the forward moving speed as a function of ratio T1/T2, other variates in this simulation have kept constant. We found that forward speed of the robot is greatly affected by the ratio of the fast phase and the slow phase within a certain range depending on T and A of the paddle movement, and then the ratio will hardly have any effect on the speed. Figure 6 (b) told that oversized oscillating period will decrease the robot’s forward speed while the other parameters remain invariable. Figure 6 (c) shows the relationship between the forward speed and amplitude A under the situation that the other variates remain constant. It resembles the ratio-velocity curve and oversized amplitude to some extent would even reduce the forward speed in view of the imperfect control system. Figure 7 depicts the change of robot depth when the attack angle of the fin is changed. It can be seen that θ = 45° is a demarcation point, subsequent parameter change has little impact on the ascent or descent locomotion. Therefore, when the robot needs to float to water surface, we use a 45° angle of attack to realize the operation. These simulation results prove that robot can achieve a variety of different motions by modifying different parameters under water, and different variates may lead to the same operation. In a word, the robot locomotion underwater is controllable.

WPFBot: A Novel and Highly Mobile Amphibious Robot

(a) Ratio-velocity

(b) Period-velocity

233

(c) Amplitude-velocity

Fig. 6. The influence of parameters on robot movement

Fig. 7. The ascent efficiency with angle of attack changing

4.2 Experiments In addition to the simulation, we conducted some experiments to verify the robot’s motion performance. In the land performance test, we carried out the maximum stable speed test in three kinds of different environment: rubber ground, bumpy grass and even artificial turf. As shown in Fig. 8, the maximum stable speed occurred in the artificial turf experiment and up to 2.5 m/s, it would be unstable if exceeded this speed. This speed is far from reaching the limit of our actuators, the uneven pressure distribution on the four wheels originated from no suspension mechanism and low friction coefficient should be blamed for the phenomenon. To prove the amphibious ability of our robot, underwater tests also were performed. Forward moving, turning and heaving motion all show the same effect as simulation. The forward maximum speed occurs in T = 800 ms, A = π/4, ratio = 2. Turning operations can be achieved by different amplitude and frequency of two paddles. Here turning locomotion was controlled by changing frequency. Heaving motion was executed in θ = 45°, where high efficiency could be realized. The results of underwater experiments were shown in Fig. 9.

234

F. Liu et al.

Fig. 8. Speed in different environments, denoted by the time of 20 m stable running

(a) Descent motion

(b) Turning motion

Fig. 9. Underwater experiments, purple line in (b) is the guide line and red the turning angle (Color figure online)

5 Conclusion and Future Work A new morphology of amphibious robot is proposed and realized in this paper. The design of morphology and prototype was reported and our focus was put on the design of the sealing structure. The simulation and experiments executed in Sect. 4 proved that our robot has excellent motion capabilities on the ground and underwater, and a series of different kind of motion, like forward and backward motion, turning and heaving underwater, could be achieved via changing the four parameters: f m , A, ratio, and θ. This work completed a preliminary prototype design and experiments. The future work include: 1) To distribute the wheel pressure evenly, so that the robot could track the designed trajectory better, we would integrate a suspension system to the robot. 2) Although we have used the current controller for basic motion tests, this is a task space open-loop controller. A better control strategy needs to be developed. 3) We hope to further improve the performance of the robot, including operating speed and flexibility, and strive to apply it to actual environments.

WPFBot: A Novel and Highly Mobile Amphibious Robot

235

References 1. Yamada, H.: Development of amphibious snake-like robot acm-r5. In: The 36th International Symposium on Robotics, Tokyo (2005) 2. Georgiades, C., et al.: AQUA: an aquatic walking robot. In: 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), vol. 4, pp. 3525–3531 (2004) 3. Crespi, A., et al.: Salamandra robotica II: an amphibious robot to study salamander-like swimming and walking gaits. IEEE Trans. Robot. 29(2), 308–320 (2013) 4. Feng, Y., Wang, J., Shi, J.: Control technologies of deformable air-land amphibious vehicle flight system. IOP Conf. Ser. Earth Environ. Sci. 252, 022075 (2019) 5. Kawasaki, K., et al.: MUWA: Multi-field universal wheel for air-land vehicle with quad variable-pitch propellers. In: 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE (2013) 6. Brown, G.: New UAV can launch from underwater for aerial missions. New UAV Can Launch from Underwater for Aerial Missions. Np, nd Web 4 (2016) 7. Zhong, G., et al.: Design and performance analysis of a triphibious robot with tilting-rotor structure. IEEE Access 9, 10871–10879 (2021) 8. Dudek, G., et al.: Aqua: an amphibious autonomous robot. Comput. 40(1), 46–53 (2007) 9. Anderson, J.M., et al.: Oscillating foils of high propulsive efficiency (1998) 10. Guglielmini, L., Blondeaux, P., Vittori, G.: A simple model of propulsive oscillating foils. Ocean Eng. 31(7), 883–899 (2004) 11. Saranli, U., Buehler, M., Koditschek, D.E.: Design, modeling and preliminary control of a compliant hexapod robot. In: Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation, vol. 3. IEEE (2000) 12. Rafeeq, M., et al.: Locomotion strategies for amphibious robots-a review. IEEE Access 9, 26323–26342 (2021) 13. Triantafyllou, M.S., George, T.S.: An efficient swimming machine. Sci. Am. 272(3), 64–70 (1995) 14. Kudo, T., et al.: Study on propulsion by partially elastic oscillating foil (1st report) analysis by linearized theory. J. Soc. Nav. Archit. Japan 156, 82–91 (1984)

A Class of Double-Delta-Based 6-DOF Pick-and-Place Robots with Integrated Grippers Xiaodong Jin, Yuefa Fang(B) , and Fuqun Zhao Beijing Jiaotong University, Beijing 100044, China [email protected]

Abstract. The pick-and-place robots with high speed, high precision and high carrying capacity are demanded in industry and logistics. However, there are few 6-DOF parallel mechanisms that can be used for grasping and sorting. In order to design 6-DOF high performance parallel robots for the above purpose, a structural synthesis method is presented. First, the articulation modes of the APM with integrated grippers are determined based on the motion transmission characteristics of the common single-DOF grippers, and the required motion type of the limbs is obtained by velocity vector mapping analysis. Subsequently, a class of doubleDelta-based robots with four types of grippers is presented, and their DOF are verified and demonstrated by means of the displacement group theory. The proposed robots have the characteristics of high speed, large workspace of the Delta robot, as well as high carrying capacity and grasping force, which are suitable for high performance pick-and-place applications. Keywords: Pick-and-place · Delta robot · Parallel mechanism · Integrated gripper

1 Introduction The parallel robot for pick-and-place is of practical importance in industry, depending on its architectural advantages of high speed, high precision and high carrying capacity. The most common application currently is the Delta robot [1, 2], which is widely used in the sorting operation of production and logistics lines thanks to its relatively large workspace and excellent dynamic capabilities. Since the Delta robot has three pure translational degree of freedom (DOF), it can only translate rather than rotate the target object, which leads to the inability to some pick-and-place tasks that need to adjust the object’s orientation. Some other traditional 4–6 DOF parallel mechanisms (PMs) with 1–3 rotational DOF, such as the 3T1R (T and R denote translation and rotation, respectively) 4-UPU PM [3], the 3T2R 5-RPUR PM [4], and the 3T3R Stewart platform [5], are difficult to apply to pick-and-place task due to the limited position and orientation workspace. Therefore, PMs with rotational DOF and large workspace is demanded for expanding the application of the PMs in the pick-and-place field. In order to design 3T1R PMs with relatively large workspace, Pierrot and Krut et al. proposed the H4 [6], Part 4 [7, 8] and I4 [9] family for high speed pick-and-place, and © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 236–245, 2021. https://doi.org/10.1007/978-3-030-89092-6_22

A Class of Double-Delta-Based 6-DOF Pick-and-Place Robots

237

presented the concept of articulated moving platform (AMP). The AMP can be seen as a kinematic chain that embedding single-DOF joints into the rigid moving platform to reduce the interference between moving platform and limbs, and to increase the maximum range of motion in rotation. Since then, some new 3T1R PMs with AMPs have been proposed, such as the X4 developed by Xie [10, 11] and the C4 designed by Huang et al. [12]. To obtain 3T2R configuration, Krut et al. [13] designed a novel AMP with 2-dimension (2D) rotational axes and evolved Eureka PM based on the basic limb architecture of H4 series. Besides, Wang et al. [14, 15] made multi-dimensional improvements to the AMP of the H4 to derive new AMPs with 2D and 3D independent rotation axes, and synthesized a class of 3T2R and 3T3R configurations. In a word, involving an AMP into a PM can help improve the orientation workspace due to the independent rotational axes in the AMP, and adopting limb structure similar to Delta helps to realize large position workspace and better dynamic capabilities. Wu [16] designed a 3-DOF gearbox and used double Delta robots that stacked up and down as limbs to obtain a 6-axis 3T3R PM with high acceleration, in 2017, which is the first conception design to obtain multi-rotation using double-Delta-based robot, to the authors’ knowledge. Otherwise, there is an incompatible issue in the pick-and-place application of the above mechanisms, i.e. one or more additional grippers have to be rigid installed on the end of the moving platform to perform the clamping operation, which needs additional motors on the moving platform. If a large clamping force is required, the motors on the moving platform need to be large and heavy, which drastically increased inertia and reduces the dynamic performance of the mechanism; if a small mass motor is used, the clamping force is too small to hold relatively heavy objects. Therefore, the endeffectors used by Delta and H4 series robots at present are mainly pneumatic suction cups and electromagnetic suction cups, to avoid using the motors to affect the speed and carrying capacity of the mechanisms. However, the pneumatic suction cups can only absorb objects with flat surface and small mass and the electromagnetic suction cups can only absorb the ferromagnetic materials, which limits the application of these type of mechanisms. As a result, it is worth studying a new connection mode of moving platform and gripper to address the above problems. We assume that if the gripper is integrated into the AMP as a kinematic pair or a kinematic chain, i.e. the gripper is part of the PM instead of rigid connecting on the moving platform and its DOF becomes part of DOF of the AMP, then the gripper is actuated by the actuators installed on the base and there is no need to install actuator on the moving platform any more. Since the gripper is coupling actuated by the motors that installed on the base, it can provide greater clamping force. Besides, since the actuators are no longer installed on the moving platform, the dynamic performance and carrying capacity of the mechanism will not be affected too much, as long as the AMP and gripper are made of lightweight materials. According to the above factors, this paper aims to find an articulation mode between moving platform and the common single-DOF grippers to make the gripper be part of the AMP. Meanwhile, the limb structure of Delta robot is still used to maintain excellent working space and dynamic performance. Since two Delta robots provide six actuators, the DOF of the proposed PM should be six, where three translation DOF, two rotation DOF and one clamping DOF of the integrated gripper. First, the articulation modes of the APM with integrated grippers are determined based on the motion transmission

238

X. Jin et al.

characteristics of the common single-DOF grippers, and the required motion type of the limbs is obtained by velocity vector mapping analysis. Then, a class of double-Deltabased robots with four types of grippers is presented, and their DOF are verified and demonstrated.

2 Architecture Design 2.1 Basic Structures of AMP The single-DOF grippers have been well researched and their structures have been well synthesized, as Refs. [17, 18]. It can be found that there are two types of input motion (driving motion) of the common grippers, which are rotation provided by motor directly and translation provided by linear actuation assembly made of motor and lead screw. We equate these two kinds of input motion to a revolute joint and a prismatic joint respectively, as I R(i) and I P(j) shown in Figs. 1(a) and 1(b). If I R(i) and I P(j) are used as the output joints (end-effectors) of the AMPs in PMs, and if they are also the suppliers of the input motion of the mechanical grippers, then the grippers can be integrated into the AMPs, which eliminates the need for an additional drive motor on the gripper.

Fig. 1. Examples of AMP: (a) R(k)R(i)-I R(i)-R(i)R(k); (b) R(k)R(i)-I P(j)-R(i)R(k)

The whole mechanism has six DOF, in which one DOF is the motion of clamping, i.e., the rotational DOF of I R(i) or the translational DOF of I P(j), as a result, the remaining DOF should be the 3T2R operational motion that can be performed on the grippers. Concretely, one gripper DOF actuated by I R(i) or I P(j) is used to pinch object, and the remaining 3T2R DOF are used to manipulate the object that being pinched by gripper. In order to produce 2D rotation, two sets of orthogonal rotational axes should be arranged symmetrically on both sides of I R(i) and I P(j), which are provided by two sets of orthogonal arranged revolute joints, as the joints R(i)R(k) that shown in Fig. 1. When taking into account combinations of the direction of the input joint and the direction of the 2D rotation, the basic structures of AMP can be obtained as listed in Table 1. The AMPs can be divided into two categories according to the input joint of gripper, one is revolute joint as input joint of gripper, as the example R(k)R(i)-I R(i)-R(i)R(k) AMP shown in Fig. 1(a), and another is prismatic joint as input joint of gripper, as the example R(k)R(i)-I P(j)-R(i)R(k) AMP shown in Fig. 1(b). It can be found from Table 1 that there are only two structures of AMP if taking revolute joint as input joint of gripper, because

A Class of Double-Delta-Based 6-DOF Pick-and-Place Robots

239

there must be co-directional revolute joints on both sides of this revolute joint, otherwise it is immovable. For example, if the input revolute joint of gripper is I R(i), each side of I R(i) must exist a co-directional revolute joint R(i) to ensure that I R(i) is movable, meanwhile, to provide a rotational DOF for the gripper. Besides, direction of rotation of the gripper only presents i&k and i&j considering the situation of j&k is isomorphic to i&k, which depends on the direction of view. Table 1. Synthesis of AMPs. IJG Revolute joint

DIJ

IJGD

DI

RJIJ

AMP

i

I R(i)

i&k

R(i)R(k)

R(k)R(i)-I R(i)-R(i)R(k)

i&j

R(i)R(j)

R(j)R(i)-I R(i)-R(i)R(j)

i&k

R(i)R(k)

R(k)R(i)-I P(i)-R(i)R(k)

i Prismatic joint

i

I P(i)

j

I P(j)

R(k)R(i)-I P(j)-R(i)R(k)

k

I P(k)

R(k)R(i)-I P(k)-R(i)R(k)

i

I P(i)

j

I P(j)

k

I P(k)

R(i)R(j) i&j

R(j)R(i)-I P(i)-R(i)R(j) R(j)R(i)-I P(j)-R(i)R(j) R(j)R(i)-I P(k)-R(i)R(j)

IJG: input joint of gripper (output joint of AMP); DIJ: direction of input joint; IJGD: input joint of gripper associated with direction; DI: direction of rotation; RJIJ: revolute joints on one side of input joint. It should be mentioned again that the middle I R(i) and I P(j) are used to provide input motion for the grippers and the remaining joints in the AMPs are used to provide rotational axes for the 2D rotation of the grippers. If connecting two appropriate parallel limbs to the end revolute joints (R(j) or R(k)) of the AMP to provide the needed motion that can trigger the movement of all the joints in the AMP, the 2D rotational motion and one clamping motion can be produced. 2.2 Deduction of Delta Robot as Limb This part aims at determining the limb structure that connected to both side of the AMPs by analyzing the mapping relationship of velocity vectors. As shown in Fig. 1, the end link Q is input link of AMP (output link of limb) to provide necessary motion for the passive joints I R(i)/I P(j), R(i) and R(k). In order to investigate the demanded input motion of the end link to make all kinematic joints in the AMP movable, we in turn assume that I R(i)/I P(j), R(i) and R(k) are active joints rather than passive joints and the end link Q is output link of AMP. If the angular velocity vectors of I R(i), R(i) and R(k) are represented as w1 , w2 and w3 in the R(k)R(i)-I R(i)-R(i)R(k) AMP, and the distance vectors between rotational axes and end link Q are represented as r1 , r2 and r3 , the mapping velocity vectors of the end link Q can be calculated as D1 = w1 × r1 = w1 i × (r1y j + r1z k) = w1 r1y k − w1 r1z j

(1)

240

X. Jin et al.

D2 = w2 × r2 = w2 i × (r2y j + r2z k) = w2 r2y k − w2 r2z j

(2)

D3 = w3 × r3 = w3 k × (r3x i + r3y j) = −w3 r3x j + w3 r3y i

(3)

where w1 , w2 and w3 are angular velocity scalars of I R(i), R(j) and R(k), r ix , r iy , r iz (I = 1, 2, 3) are the component of the distance scalar in the x, y, z direction, and i, j, k are direction vectors. Then the composition velocity vector of the end link Q of the R(k)R(i)-I R(i)-R(i)R(k) AMP can be obtained as D = D1 + D2 + D3 = w3 r3y i − (w1 r1z + w2 r2z + w3 r3x )j + (w1 r1y + w2 r2y )k (4) The mapping velocity vectors D2 and D3 of the R(k)R(i)-I P(j)-R(i)R(k) AMP are the same as that of the R(k)R(i)-I R(i)-R(i)R(k) AMP. The mapping velocity vector D1 of the R(k)R(i)-I P(j)-R(i)R(k) AMP becomes D1 = v1 = v1 j

(5)

where v1 is the velocity vector of I P(j), and v1 is linear velocity scalar of I P(j). The composition velocity vector of the end link Q of the R(k)R(i)-I P(j)-R(i)R(k) AMP is D = D1 + D2 + D3 = w3 r3y i + (v1 − w2 r2z − w3 r3x )j + w2 r2y k

(6)

Equations (4) and (6) indicate that if I R(i)/I P(j), R(i) and R(k) are seen as the active joints and the end link Q is seen as output link, it will produce three directions of linear displacement, which are actually the parasitic translation of the rotation of the revolute joints. Further, if only investigating the orthogonal joints R(i) and R(k), D2 + D3 = w3 r3y i − (w2 r2z + w3 r3x )j + w2 r2y k

(7)

It shows that whether the input joint of gripper is revolute joint I R(i) or prismatic joint I P(j), the end link Q will produce three directions of parasitic translation. As a result, it proves that if the passive joints I R(i)/I P(j), R(i) and R(k) in the AMPs perform the corresponding motions of rotation or translation, the end link Q of limb must have 3D translational DOF, because movement is relative. In other words, Eqs. (4), (6) and (7) prove the limb that connected to the AMP must have three translational DOF to guarantee that all joints in the AMP are movable to generate two rotational DOF and one clamping DOF. All PMs with three translational DOF can satisfy the requirement of the limb structure, we select the Delta robot as the parallel limb of the PMs designed in this paper, considering it has competitive kinematic and dynamic characteristics. 2.3 AMPs with Integrated Single-DOF Grippers As mentioned above, the revolute joint I R(i) and the prismatic joint I P(i) (or I P(j), are used as the input joints of the single-DOF grippers. The motion transmission mechanisms should be constructed to convert the input rotation or input translation into I P(k))

A Class of Double-Delta-Based 6-DOF Pick-and-Place Robots

241

the gripping action. Considering the different demands of grabbing tasks and the different transmission ways, four kinds of grippers are designed; a. For subject that needs to be held as a whole, such as a sphere, the rotational input I R(i) is generalized as a gripper directly, as shown in Fig. 2(a), which provides a rotational clamping action; b. For most shapes, especially regular shapes like cubes, the rotational input I R(i) provides a translational clamping action by a transmission assembly that connected by I R(i) joint, gear-gear train and double parallelogram mechanisms, as shown in Fig. 2(b); c. The motion transmission is realized by I P(j) joint, rack-gear-gear train and double parallelogram mechanisms, as shown in Fig. 2(c); d. The motion transmission is realized by a set of four-bar mechanisms with a common input I P(k), as shown in Fig. 2(d), which can construct multi-finger clamping action to provide more stable pick-and-place operation. We define these four grippers as GA, GB, GC, GD, respectively. It can be found from Table 1 and Fig. 2(a) and 2(b) that if revolute joint is used as input joint of gripper, there must be a set of co-directional revolute joints in the AMP, i.e., if I R(i) is selected as input joint of gripper, each side of I R(i) has to connect a revolute joint R(i), which results in the clamping direction having to be related to one of the rotational directions. For the translational input I P(i) (or I P(j), I P(k)), there is no correlation between the clamping direction and the rotation direction of the gripper, as illustrated in Figs. 2(c) and 2(d). As a result, comparing the two input joints I R(i) and I P(i) (or I P(j), I P(k)), it is found that if the latter is selected as input joint of gripper, the mechanism is easier to realize the selectivity of movement and has a stronger adaptability to the task.

Fig. 2. AMPs with integrated single-DOF grippers: (a) R(k)R(i)-[I R(i)]GA -R(i)R(k); (b) R(k)R(i)-[I R(i)]GB -R(i)R(k); (c) R(k)R(j)-[I P(j)]GC -R(j)R(k); (d) R(k)R(j)-[I P(k)]GD -R(j)R(k)

242

X. Jin et al.

3 Double-Delta-Based Robots After designing the AMPs with integrated single-DOF grippers and selecting the Delta robot as parallel limb, a class of double-Delta-based robots with integrated grippers can be obtained, as listed in Table 2. The superscripts GA, GB, GC and GD of IJGD in AMPIG represent the grippers that have been classified in Sect. 2.3. Figure 3 shows the example robots of 2[Delta-R(k)R(j)]-[I P(j)]GC , 2[Delta-R(k)R(i)]-[I R(i)]GB , 2[DeltaR(k)R(i)]-[I R(i)]GA and 2[Delta-R(k)R(j)]-[I P(k)]GD . It should be mentioned that since R(i)R(k) and R(j)R(k) are isomorphic as stated in Sect. 2.1, only the structures with RJIJ of R(i)R(k) are listed in Table 2, and the structures with RJIJ of R(j)R(k) are not shown redundantly. Therefore, 2[Delta-R(k)R(j)]-[I P(j)]GC in Fig. 3(a) and 2[Delta-R(k)R(i)][I P(i)]GC in Table 2 represent the same robot structure, and 2[Delta-R(k)R(j)]-[I P(k)]GD in Fig. 3(d) and 2[Delta-R(k)R(i)]-[I P(k)]GD also represent the same robot structure. Table 2. List of robots structure. PL

IJGD

Delta robot

I R(i)

RJIJ

AMPIG

RS

R(i)R(k)

R(k)R(i)-[I R(i)]GA -R(i)R(k)

2[Delta-R(k)R(i)]-[I R(i)]GA

R(k)R(i)-[I R(i)]GB -R(i)R(k)

2[Delta-R(k)R(i)]-[I R(i)]GB

R(j)R(i)-[I R(i)]GA -R(i)R(j)

2[Delta-R(j)R(i)]-[I R(i)]GA

R(j)R(i)-[I R(i)]GB -R(i)R(j)

2[Delta-R(j)R(i)]-[I R(i)]GB

R(i)R(j) I P(i)

R(k)R(i)-[I P(i)]GC -R(i)R(k)

2[Delta-R(k)R(i)]-[I P(i)]GC

I P(j)

R(k)R(i)-[I P(j)]GC -R(i)R(k)

2[Delta-R(k)R(i)]-[I P(j)]GC

I P(k)

R(k)R(i)-[I P(k)]GC -R(i)R(k)

2[Delta-R(k)R(i)]-[I P(k)]GC

R(i)R(k)

R(k)R(i)-[I P(k)]GD -R(i)R(k)

2[Delta-R(k)R(i)]-[I P(k)]GD

I P(i)

R(j)R(i)-[I P(i)]GC -R(i)R(j)

2[Delta-R(j)R(i)]-[I P(i)]GC

IP(j)

R(j)R(i)-[I P(j)]GC -R(i)R(j)

2[Delta-R(j)R(i)]-[I P(j)]GC

IP(k)

R(j)R(i)-[I P(k)]GC -R(i)R(j)

2[Delta-R(j)R(i)]-[I P(k)]GC

R(j)R(i)-[I P(k)]GD -R(i)R(j)

2[Delta-R(j)R(i)]-[I P(k)]GD

R(i)R(j)

PL: parallel Limb; AMPIG: AMP with integrated gripper; RS: robot structure. Here, we are going to verify the mobility property to prove that the proposed pickand-place robots have one DOF of clamping motion (or pick-DOF) and 3T2R DOF of manipulation (or place-DOF), using the displacement group theory. The displacement group theory has been well-researched and become a mature method in structural synthesis and DOF analysis and verification of PMs, see Refs. [19–21]. The proposed grippers can actually be regarded as single-DOF non-rigid moving platforms, whose input joints I R(i) and I P(i) (or I P(j), I P(j)) are associated with the displacement subgroups (DSGs) of {IG R(N, u)} and {IG T (u)} (or {IG T (v)}, {IG T (w)}), respectively, where the superscript IG in DSG denotes the input joint of gripper. And the kinematic chain on each side of the input joint of gripper become hybrid limb that connected by a parallel Delta limb and a R(i)R(k) (or R(i)R(j)) kinematic chain. Since DSG of the pure translational Delta robot

A Class of Double-Delta-Based 6-DOF Pick-and-Place Robots

243

can be represented as {T }, when taking 2[Delta-R(k)R(i)]-[I R(i)]GA shown in Fig. 3(c) as example, the displacement sub manifold (DSM) of the hybrid limb with the revolute input joint I R(i) can be written as IG {LR i } = {T }{R(Ni1 , w)}{R(Ni2 , u)}{ R(N3 , u)}

(8)

Fig. 3. Double-Delta-based robots with integrated grippers; (a) 2[Delta-R(k)R(j)]-[IP(j)]GC; (b) 2[Delta-R(k)R(i)]-[IR(i)]GB; (c) 2[Delta-R(k)R(i)]-[IR(i)]GA; (d) 2[Delta-R(k)R(j)]-[IP(k)]GD

where i = 1, 2, is the number of the hybrid limb. Taking 2[Delta-R(k)R(j)]-[I P(j)]GC shown in Fig. 3(a) as example, the DSM of the hybrid limb with the prismatic joint I P(j) is {LPi } = {T }{R(Ni1 , w)}{R(Ni2 , v)}{IG T (v)}

(9)

The DSM of the example 2[Delta-R(k)R(i)]-[I R(i)]GA robot can be calculated as R IG {DR } = {LR 1 } ∩ {L2 } = {T }{R(N1 , w)}{R(N2 , u)}{ R(N3 , u)}

(10)

And the DSM of the example 2[Delta-R(k)R(j)]-[I P(j)]GC robot can be obtained as {DP } = {LP1 } ∩ {LP2 } = {T }{R(N1 , w)}{R(N2 , v)}{IG T (v)}

(11)

244

X. Jin et al.

Equation (10) represents that the double-Delta-based robot with revolute joint as input joint of the integrated gripper has three pure translational DOF, two rotational DOF, and a pick-DOF that realized by a revolute joint in the middle of AMP. Similarly, Eq. (11) illustrates that the double-Delta-based robot with prismatic joint as input joint of the integrated gripper has three pure translational DOF, two rotational DOF, and a pick-DOF that realized by a prismatic joint in the middle of AMP. As a result, it proves that the proposed pick-and-place robots have 6-DOF of 3T2R place-DOF and one pickDOF. Since there are six actuators on the base of the double-Delta robot, the mechanism is fully controllable. More specifically, when the integrated gripper holds an object, the developed robot is able to manipulate the object by three translation and two rotation in space. It is worth emphasizing again that these architectures have two intuitive functional advantages: one is the clamping action is actuated by the actuators on the base, and the drive motor of gripper does not need to be installed on the moving platform, which avoids seriously increasing the quality of moving platform and thus has little effect on the dynamic performance of Delta robot, and can potentially improve the holding force thanks to the coupling actuators on the base; another is the stronger ability to manipulate object, with 5-DOF of 3T2R.

4 Conclusions This paper presents a structural synthesis procedure of the double-Delta-based 6-DOF pick-and-place robots. It is proved that the proposed method can be used to design 6-DOF parallel mechanisms with integrated grippers. This structural synthesis method provides the mathematical model of the connection between the traditional translational parallel mechanisms and the articulated grippers, which becomes the theoretical basis for the design of 6-DOF pick-and-place robots with integrated grippers. The main body of the proposed 6-DOF robot is two delta robots, so its translational performance is similar to that of delta robot. Future work will focus on kinematics and dynamics, including workspace, clamping force, velocity, inertia and stiffness. Acknowledgments. The authors gratefully acknowledge the financial support of the Fundamental Research Funds for the Central Universities under grant no. 2021JBM022, the China Postdoctoral Science Foundation under grant no. 2020M680008 and National Natural Science Foundation of China under grant no. 51975039.

References 1. Clavel, R.: A fast robot with parallel geometry. In: Proceedings International Symposium on Industrial Robots, Lausanne, CiNii, pp. 91–100 (1988) 2. Park, S.B., Kim, H.S., Song, C., Kim, K.: Dynamics modeling of a delta-type parallel robot. In: IEEE ISR 2013, Seoul, Korea, pp. 1–5. IEEE (2013) 3. Gabardi, M., Solazzi, M., Frissoli, A.: An optimization procedure based on kinematics analysis for the design parameters of a 4-UPU parallel manipulator. Mech. Mach. Theory 133, 211–228 (2019)

A Class of Double-Delta-Based 6-DOF Pick-and-Place Robots

245

4. Yuefa, F., Lung-Wen, T.: Structure synthesis of a class of 4-DoF and 5-DoF parallel manipulators with identical limb structures. Int. J. Robot. Res. 21(9), 799–810 (2002) 5. Rui, Y., Wenbai, Z., Peng, H.: Accuracy analysis of Stewart platform based on interval analysis method. Chin. J. Mech. Eng. 26, 29–34 (2013) 6. Company, O., Marquet, F., Pierrot, F.: A new high-speed 4-DOF parallel robot synthesis and modeling issues. IEEE Trans. Rob. Autom. 19(3), 411–420 (2003) 7. Pierrot, F., Nabat, V., Company, O.: Optimal design of a 4-DOF parallel manipulator: from academia to industry. IEEE Trans. Rob. 25(2), 213–224 (2009) 8. Nabat, V., Rodriguez, M., Company, O.: Par4: very high speed parallel robot for pickand-place. In: 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, Edmonton, AB, Canada, pp. 553–558. IEEE (2005) 9. Krut, S., Benoit, M., Ota, H., Pierrot, F.: I4: a new parallel mechanism for Scara motions. In: 2003 IEEE International Conference on Robotics and Automation, Taipei, Taiwan, China, pp. 1875–1880 (2003) 10. Xie, F., Liu, X.-J.: Design and development of a high-speed and high-rotation robot with four identical arms and a single platform. J. Mech. Robot. 7(4), 41015 (2015) 11. Jiao, M., Zhufeng, S., Liwen, G., Fuigui, X., Xiaoqiang, T.: Dynamic performance analysis of the X4 high-speed pick-and-place parallel robot. Robot. Comput. Integr. Manuf. 46, 48–57 (2017) 12. Liu, S., Huang, T., Mei, J., Zhao, X., Wang, P.: Optimal design of a 4-DOF SCARA type parallel robot using dynamic performance indices and angular constraints. J. Mech. Robot. 4, 031005 (2012) 13. Krut, S., Company, O., Rangsri, S., Pierrot, F.: Eureka: a new 5-degree-of-freedom redundant parallel mechanism with high tilting capabilities. In: Proceedings of the 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems, Las Vegas, Nevada, pp. 3575– 3580. IEEE (2003) 14. Wang, C., Fang, Y., Guo, S.: Design and analysis of 3R2T and 3R3T parallel mechanisms with high rotational capability. J. Mech. Robot. 8, 011004 (2016) 15. Congzhe, W., Yuefa, F., Hairong, F.: Novel 2R3T and 2R2T parallel mechanisms with high rotational capability. Robotica 35(02), 401–418 (2017) 16. Wu, G.: Conceptual design and analysis of a 6-axis double delta robot towards high acceleration. In: Zhang, X., Wang, N., Huang, Y. (eds.) Mechanism and Machine Science. LNEE, vol. 408, pp. 389–401. Springer, Singapore (2017). https://doi.org/10.1007/978-981-10-28755_33 17. Fanyu, C.: Gripping mechanisms for industrial robots: an overview. Mech. Mach. Theory 17(5), 299–311 (1982) 18. Giuseppe, C. (ed.): Grasping in Robotics. Springer, London (2012). https://doi.org/10.1007/ 978-1-4471-4664-3 19. Hervé, J.M.: The Lie group of rigid body displacements, a fundamental tool for mechanism design. Mech. Mach. Theory 34(5), 719–730 (1999) 20. QinChuan, L., Zhen, H., Herve, J.M.: Type synthesis of 3R2T 5-DOF parallel mechanisms using the Lie group of displacements. IEEE Trans. Robot. Autom. 20(2), 173–180 (2004) 21. Jingjun, Y., Jiansheng, D., Shusheng, B., GuangHua, Z.: Type synthesis of a class of spatial lower-mobility parallel mechanisms with orthogonal arrangement based on Lie group enumeration. Sci. China Ser. E: Technol. Sci. 53(2), 388–404 (2010)

Motion Planning of Docking Process for Underwater Vehicle Based on Manipulator Aided Grasping Yang Zhang1,2 , Zongyu Chang1,2(B) , Kunfan Shen1 , Jinyi Li1 , and Zhongqiang Zheng1 1 College of Engineering, Ocean University of China, Qingdao 266100, China

[email protected] 2 Key Laboratory of Ocean Engineering of Shandong Province, Qingdao 266100, China

Abstract. Underwater vehicle-manipulator system (UVMS) is a useful tool for underwater inspection, maintenance, and repair task. And underwater docking systems can fulfill underwater charging and data exchange for UVMS. However, docking operation is difficult due to complex underwater environment. The manipulator can be applied to aid the docking mission. In this study, the dynamic model of UVMS including vehicle and manipulator is proposed, and the relationships of kinematics about end effector and system velocity are established. Based on three possible tasks, singularity-robust task priority redundancy resolution method is modified considering joint limits. Two cases using direct pseudoinverse method and task priority method are compared. The results of numerical simulation show that our proposed method can coordinate the motion of UVMS and complete docking mission successfully. Keywords: Underwater vehicle-manipulator system · Docking · Motion Planning · Task priority

1 Introduction Underwater docking systems can fulfill underwater charging, data exchange, and other tasks for autonomous underwater vehicle (AUV) or remotely operated vehicle (ROV). It also increases working time, efficiency, region, and so on. Previous works on ROV or AUV docking mainly focused on some simplicity task, such as data and power transmission. Some projects are for underwater docking, including EURODOCKER [1], SWIMMER [2], REMUS [3, 4], ALIVE [5], MARINE BIRD [6], FREESUB [7], DIFIS [8], TRITON [9], etc. For general torpedo-shaped AUV, the dock is cone-shaped [10]. The docking design has been optimized by Page and Mahmoudian [11]. However, conventional docking systems are usually designed to meet specific AUV outlines and structures, and actual underwater docking operation is difficult. Therefore, an AUV-based or ROVbased manipulator system using its own manipulator to aid docking is meaningful and promising [12, 13]. © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 246–256, 2021. https://doi.org/10.1007/978-3-030-89092-6_23

Motion Planning of Docking Process for Underwater Vehicle

247

Underwater vehicle-manipulator system (UVMS) is a useful underwater tool that mostly utilized for underwater inspection, maintenance, and repair tasks. The difference between industrial robot, space robots, and UVMS is that the mathematical model of the latter is more complex because of buoyancy, drag, added mass and so on. UVMS is strong nonlinear, redundant, and parameter uncertainty. It also has disturbance caused by currents, wave, etc. Several methods have been used to derive the dynamics equation of the UVMS, such as Newton–Euler method [14–16], Kane’s method [17, 18], and Lagrange method [19]. Kinematic analysis, motion control, and visual navigation are also some vital aspects for underwater vehicle autonomous docking. Li et al. [13] utilized a task-priority redundancy inverse kinematic solution method based on weighted-least norm solution for ROV manipulators system docking. Teo et al. [20] presented a robust AUV docking approach under unknown currents. Zhang et al. [21] researched the impact process in AUV docking. Yazdani et al. [22] presented a novel inverse dynamic in the virtual domain method for underwater docking operations. Meng et al. [23] studied AUV docking process and collision problems. Considering unknown environment and obstacles, Cui et al. [24] presented an integrated approach to plan smooth docking path. Thomas et al. [25] proposed a unified guidance and control method for AUV using task priority control approach. The application of underwater vision for underwater vehicle docking has a lot of potential, such as vision positioning [26], visual navigation [27], visual pose estimation [28], and so on. Wang et al. [29] proposed an omnidirectional planar type AUV docking and charging platform which has unconstraint on AUV structures, and the adopted monocular Simultaneous Localization and Mapping (SLAM) program and visual servo are utilized for docking process. Vallicrosa et al. [30] proposed a method for AUV docking by combing acoustic and optical sensing. Anderlini et al. [31] applied reinforcement learning for AUV docking control. UVMS is a kinematically redundant system because it has more degrees of freedom (DOFs) than those required to execute a given task. Generally, a manipulation task is given in terms of trajectories for the end effector, specially position and orientation. Although the vehicle is floating and has 6 DOFs motions in the water, it is inefficient to use vehicle thrusters to move the end effector of manipulator due to the difficulty of vehicle control. Moreover, the inertia between vehicle and manipulator are different. In the normal case, movement of the manipulator is more efficient. Therefore, motion planning of docking process for UVMS based on manipulator aid is considered and simulated in this study. The reminder of this paper is structured as follows. Section 2 briefly describes the dynamic model of UVMS. Section 3 introduces the kinematics of end effector and task priority method, and three possible tasks are listed. Two numerical cases are studied in Sect. 4, direct pseudoinverse method and singularity-robust task priority redundancy resolution method are compared. Finally, Sect. 5 concludes the paper.

248

Y. Zhang et al.

2 Dynamic Modeling of UVMS Figure 1 shows the scheme of docking of UVMS and underwater fixed structure. The equation of motions of the UVMS including 6 DOFs vehicle and n DOFs manipulator in the body-fixed frame can be written in the following form [14, 32] M (q)ζ˙ + C(q, ζ )ζ + D(q, ζ )ζ + g(q, η) = τ (1)  T  T where generalized velocity ζ = ν T , q˙ T , ν = u υ ω p q r is the (6 × 1) vector of vehicle linear and angular velocities expressed in the body-fixed frame, q is the (n× 1) vector of joint angles, n is the number of joints, M (q) is the ((6 + n)× (6 + n)) inertia and added mass matrix of the UVMS, C(q, ζ ) is the matrix of Coriolis and centripetal terms which also include added mass, D(q, ζ ) is the matrix of friction and hydrodynamic damping terms, g(q, η) is the vector of restoring forces, generalized T  forces τ = τvT , τmT , where τv is the (6×1) vector of forces and moments acting on the vehicle and τm is the (n×1) vector of joint torques.

Fig. 1. The coordinate frame of UVMS and underwater fixed structure.

T   T η = η1T η2T = x y z φ θ ψ is the vector of vehicle position (η1 ) and Euler angles (η2 ) in the earth-fixed frame, The linear and angular velocity of vehicle η˙ is related to the body-fixed linear and angular velocity ν by a Jacobian matrix   I RB O3.×3 ν (2) η˙ = Jv ν = O3.×3 Jko where Jv is the (6 × 6) matrix between the body velocity and global velocity, ⎡ ⎤ cψcθ −sψcφ + cψsθ sφ sψsφ + cψsθ cφ O3×3 is the null (3 × 3) matrix, RIB = ⎣ sψcθ cψcφ + sψsθ sφ −cψsφ + sψsθ cφ ⎦ −sθ sφcθ cφcθ ⎡ ⎤ cθ sφsθ cφsθ and Jko = cθ1 ⎣ 0 cφcθ −sφcθ ⎦, c· and s· are short notations for cos(·) and sin(·), 0 sφ cφ respectively.

Motion Planning of Docking Process for Underwater Vehicle

249

3 Kinematics of End Effector and Redundancy Resolution In UVMS, direct kinematics is the process of computing the end effector configuration by knowing the vehicle pose (position and orientation) and joint positions. ηee1 and ηee2 represent the position and orientation of the end effector in the earth-fixed frame, respectively. The relation between the end effector velocity and generalized velocity can be expressed by the following form [32]     η˙ J η˙ ee = ee1 = Je ζ = pos ζ (3) η˙ ee2 Jor where Je is the (6×(6 + n)) matrix between generalized velocity and end effector velocity, and Jpos , Jor ∈ R3×(6+n) . UVMS is kinematically redundant with respect to the end effector task. As a given end effector motion, to achieve an effective coordinated motion of the vehicle and manipulator using the redundant DOFs, the generalized velocity of UVMS should be planned and improved during grasping and docking operation. 3.1 Possible Task A possible task σx = σx (η, q), which has m dimensions, is given when needed. Its time derivative is related to the corresponding Jacobian Jx (η, q) and system generalized velocity ζ . σ˙ x = Jx (η, q)ζ

(4)

To obtain the reference generalized velocity ζr , the simplest solution is to calculate the pseudoinverse. (5) ζr = Jx† (η, q)σ˙ x

−1 ∈ R(6+n)×m because m < (6 + n). where Jx† (η, q) = JxT (η, q) Jx (η, q)JxT (η, q) This solution, by using least-square method, corresponds to the minimization of the vehicle and manipulator joint velocities. In this study, three tasks are considered during UVMS grasping and docking process. Task 1 End Effector Configuration (m1 = 6) The precise control of the end effector can make it grasp the fixed object. Therefore, the position and orientation of end effector is needed. First-priority task is σx1 = ηeed ∈ Rm1 , and the Jacobian matrix is Jx1 = Je ∈ Rm1 ×(6+n) . Task 2 Vehicle Orientation (m2 = 3) Because the base of UVMS is floating, the pose of vehicle also need to be controlled. Vehicle orientation is chosen as a second task during motion planning. The second task T  function is given by σx2 = η2d = φd θd ψd ∈ Rm2 , and corresponding Jacobian   m ×(6+n) matrix is Jx2 = O3×3 RIB O3×3 ∈ R 2 . Task 3 Vehicle Position Norm (m3 = 1) In the process of docking, it is necessary that control of the position of vehicle. The

250

Y. Zhang et al.

distance between desired and actual position of vehicle can be chosen as a third task. m3 and corresponding Jacobian matrix is J T σ x3 =  x3 = (η1d T− Iη1 ) (η1d −η1 ) ∈m R ×(6+n) 3 . −(η1d − η1 ) RB O1×(3+n) ∈ R 3.2 Joint Limits Constraints and Velocity Saturation Manipulator exhibits mechanical limits for their structures, so a simple function including joints positions and its limits is defined. H (q) =

n

1 qi,max − qi,min ci (qi,max − qi )(qi − qi,min )

(6)

i=1

where coefficient ci > 0, and the subscript max and min correspond maximum and minimum joint values, respectively. The partial derivative of this function with respect to (q −q )(2qi −qi,max −qi,min ) = c1i i,max(q i,min . Wq is chosen as a diagonal the joint positions is ∂H∂q(q) 2 2 i,max −qi ) (q  i  i −qi,min)   ∂H (q)   ∂H (q)  matrix, if  ∂qi  > 0, then Wqi,i = 1 +  ∂qi , or else Wqi,i = 1. The weights consider the direction of the joint angle change. Letting the weights of vehicle are always 1, the full weight matrix is obtained. W = blockdiag{I6 , Wqn×n } ∈ R(6+n)×(6+n)

(7)

Combined with weighted coefficient, weighted pseudoinverse can be obtained.  −1 † Jx,W = W −1 JxT Jx W −1 JxT (8) With this approach, the weighting of task variable can be handled. Because of the limitations of the actual situation, generalized velocity ζ in body-fixed frame may also be saturated. Therefore the desired velocity should satisfy the following conditions. ⎧ ⎨ ζi,max , ζi,max ≤ ζi ζir = Sat(ζi ) = (9) |ζi | < ζi,max (i = 1, 2, · · · , 6 + n) ζi , ⎩ −ζi,max , ζi ≤ −ζi,max

3.3 Singularity-Robust Task Priority Redundancy Resolution Considering multiple tasks, singularity-robust task priority technique is an effect method [33, 34]. For three tasks and joint limits, reference generalized velocity ζr can be obtained. † † † ζr = Jx1,W x1 σ˜ x1 + N1 Jx2,W x2 σ˜ x2 + N12 Jx3,W x3 σ˜ x3

(10)

where x1 ∈ Rm1 ×m1 , x2 ∈ Rm2 ×m2 and x3 ∈ Rm3 ×m3 are diagonal positive defi

−1 † nite matrix of gains, the task error σ˜ xi = σxi,d − σxi and Jxi,W = W −1 JxiT Jxi W −1 JxiT , † Jx1 and N12 = I6+n − (i = 1, 2, 3.). And null space projector N1 = I6+n − Jx1,W   Jx1 † ∈ R(m1 +m2 )×(6+n) . Jx12,W Jx12 , matrix Jx12 = Jx2

Motion Planning of Docking Process for Underwater Vehicle

251

4 Case Studies A 9 DOFs UVMS with a 3 DOFs planar manipulator arm was considered in this study (Fig. 1). For UVMS underwater docking process, two cases are analyzed. The main dimension parameters of UVMS are listed in Table 1, the vehicle is considered as a box and the manipulator is cylindrical rod. The joint position limits and the saturation value of velocity of UVMS are listed in Table 2. Table 1. Main dimension parameters of UVMS. Vehicle Dimensions (m)

2.5 × 1.2 × 1.0

Manipulator Link 1

Link 2

Link 3

L = 0.8, r = 0.16

L = 0.8, r = 0.13

L = 0.8, r = 0.1

Table 2. Joint limits constraints and velocity saturation.

Vehicle Joint 1 Joint 2 Joint 3

Limits

Velocity saturation

 [−90, 50] × π 180 rad  [−90, 150] × π 180 rad  [−150, 150] × π 180 rad

[0.2, 0.2, 0.2, 0.2, 0.2, 0.2]T m/s or rad/s  q1 max = π 9 rad/s  q1 max = π 9 rad/s  q1 max = π 9 rad/s

4.1 Case l: Direct Pseudoinverse Method In this section, motion trajectory is planned and only task 1 is considered by using direct pseudoinverse method. Of course, there are including joint limits and velocity saturation. Two main steps are included. Firstly, starting from the initial UVMS configuration (Fig. 2), UVMS is controlled to move so that its manipulator can grasp the handle which fixed in docking structure. Secondly, after the end effector of manipulator is fixed, vehicle and manipulator joints will change so that vehicle can move to appointed position. Figure 2 shows the initial configuration of the UVMS. The initial  configuration  of  the vehicle and manipulator are η = [0, 0, 0, 0, 0, 0]T m or rad and q = −45 45 45 ×π 180 rad, respectively. The handle is located in [2, 0, 2.5] m and vehicle docking position is in [2, 0, 1] m. The total simulation time is 30 s. Joint limits coefficient c = [ 1 1 1 ]T . Figure 3 shows the scene that end effector of manipulator is fixed to the handle (time = 15 s). Figure 4 shows the final configuration of UVMS and underwater scene (time = 30 s). Figure 5 shows the time history of the pose of the end effector in the earthfixed frame. And Fig. 6 shows the time history of the pose of vehicle in the earth-fixed frame. Obviously, although direct pseudoinverse method can realize end effector grasp the handle, the pose of vehicle cannot be guaranteed to the desired configuration.

252

Y. Zhang et al.

Fig. 2. Initial scene.

Fig. 3. Grasped the handle (case l).

(a) Position

Fig. 4. Final scene (case l).

(b) Orientation

Fig. 5. Time history of the position and orientation of the end effector (case l).

(a) Position

(b) Orientation

Fig. 6. Time history of the position and orientation of the vehicle (case l).

Motion Planning of Docking Process for Underwater Vehicle

253

4.2 Case ll: Task Priority Method The basic settings are the same as in case l except planning method. In the first stage (time = 0–15 s), x1 = I6 , x2 = 30I3 and x3 =I1 . End effector moves to the desired handle position. Figure 7 shows the scene that end effector of manipulator is fixed to the handle (time = 15 s). In the second stage (time = 15–30 s), x1 = 5I6 , x2 = 15I3 and x3 = 5I1 . The body of UVMS moves to docking position with the help of the manipulator. Figure 8 shows the final configuration of UVMS and underwater scene (time = 30 s). Figure 9 shows the time history of the pose of the end effector in the earth-fixed frame. And Fig. 10 shows the time history of the pose of vehicle in the earth-fixed frame. Figure 11 shows the time history of three joints angles.

Fig. 7. Grasped the handle (case ll).

(a) Position

Fig. 8. Final scene (case ll).

(b) Orientation

Fig. 9. Time history of the position and orientation of the end effector (case ll).

(a) Position

(b) Orientation

Fig. 10. Time history of the position and orientation of the end effector (case ll).

From Figs. 9 and 10, the values of position and orientation of vehicle and end effector are moving slowly. After grasped the handle, the values of position and orientation of end

254

Y. Zhang et al.

(a) Joint 1

(b) Joint 2

(c) Joint 3

Fig. 11. Time history of three joints angles (case ll, dashed line: joint limits).

effector are not changed. Moreover, the manipulator joints are not over the limit (Fig. 11). By comparing two methods, the method proposed in this study can accomplish the underwater grasping and docking operation task well. However, the prototype experiment is lacking to verify the simulation results. And some external factors in the grasping process are ignored in this study. Therefore, more specific process should be considered, and dynamic simulation and pool experiment should be completed in the future.

5 Conclusions In this study, a UVMS including 6 DOFs vehicle and 3 DOFs manipulator is proposed for underwater docking operation mission. Singularity-robust task priority redundancy resolution method is modified considering joint limits. Two cases are compared with direct pseudoinverse method and task priority method. The simulation results show that our proposed method can coordinate the motion of UVMS well and complete docking mission successfully. In the future, dynamic simulation considering more specific grasping process and water pool experiment will be verified.

References 1. Brighenti, A., Zugno, L., Mattiuzzo, F., Sperandio, A.: Eurodocker - a universal docking - downloading - recharging system for auvs: Conceptual design results. In: IEEE Oceanic Engineering Society. OCEANS’98. Conference Proceedings, pp. 1463–1467. IEEE, Nice (1998) 2. Evans, J.C., Keller, K.M., Smith, J.S., Marty, P., Rigaud, O.V.: Docking techniques and evaluation trials of the swimmer auv: An autonomous deployment auv for work-class rovs. In: MTS/IEEE OCEANS 2001. An Ocean Odyssey. Conference Proceedings, pp. 520–528. IEEE, Honolulu (2001) 3. Stokey, R., et al.: Enabling technologies for remus docking: An integral component of an autonomous ocean-sampling network. IEEE J. Oceanic Eng. 26(4), 487–497 (2001) 4. Allen, B., et al.: Autonomous docking demonstrations with enhanced remus technology. In: OCEANS 2006, pp. 1–6. IEEE, Boston (2006) 5. Evans, J., Redmond, P., Plakas, C., Hamilton, K., Lane, D.: Autonomous docking for intervention-auvs using sonar and video-based real-time 3d pose estimation. In: Proceedings of the MTS/IEEE Oceans 2003, pp. 2201–2210. IEEE, San Diego (2003) 6. Kawasaki, T., Noguchi, T., Fukasawa, T., Baino, M.: Development of auv “marine bird” with underwater docking and recharging system. In: The 3rd International Workshop on Scientific Use of Submarine Cables and Related Technologies, IEEE, Tokey (2003)

Motion Planning of Docking Process for Underwater Vehicle

255

7. Weiss, P., Vicente, J., Grossnet, D., Brignone, L., Labbe, D.F.L., Wilson, P.: Freesub: Dynamic stabilization and docking for autonomous underwater vehicles. In: 13th International Symposium on Unmanned Untethered Submersible Technology, pp. 1–6. IEEE, Durham (2003) 8. Sotiropoulos, P., Tosi, N., Andritsos, F., Geffard, F.: Optimal docking pose and tactile hooklocalisation strategy for auv intervention: The difis deployment case. Ocean Eng. 46, 33–45 (2012) 9. Palomeras, N., et al.: I-auv docking and panel intervention at sea. Sensors-Basel 16(10), 1673 (2016) 10. Wu, L., Li, Y., Su, S., Yan, P., Qin, Y.: Hydrodynamic analysis of auv underwater docking with a cone-shaped dock under ocean currents. Ocean Eng. 85, 110–126 (2014) 11. Page, B.R., Mahmoudian, N.: Simulation-driven optimization of underwater docking station design. IEEE J. Oceanic Eng. 45(2), 404–413 (2020) 12. Chen, J., et al.: A novel autonomous docking method of unmanned marine vehicle based on manipulator. Appl. Mech. Mater. 364, 370–374 (2013) 13. Li, Y., Zhang, Q., Feng, X.: Inverse kinematic solution and simulation for rovs-manipulators system. In: 2008 Chinese Control and Decision Conference, pp. 3345–3348. IEEE, Yantai (2008) 14. Schjølberg, I., Fossen, T.I.: Modelling and control of underwater vehicle-manipulator systems. In: Proceedings of the 3rd Conference on Marine Craft Maneuvering and Control, pp. 45–57. IFAC, Southhampton (1994) 15. Mohan, S., Kim, J.: Modelling, simulation and model reference adaptive control of autonomous underwater vehicle-manipulator systems. In: 2011 11th International Conference on Control, Automation and Systems, pp. 643–648. ICROS, Kintex, Gyeonggi-do (2011) 16. Huang, H., Tang, Q., Li, H., Liang, L., Li, W., Pang, Y.: Vehicle-manipulator system dynamic modeling and control for underwater autonomous manipulation. Multibody Sys.Dyn. 41(2), 125–147 (2016). https://doi.org/10.1007/s11044-016-9538-3 17. Tarn, T.J., Shoults, G.A., Yang, S.P.: A dynamic model of an underwater vehicle with a robotic manipulator using kane’s method. Auton. Robot. 3(2), 269–283 (1996). https://doi. org/10.1007/BF00141159 18. Yang, K., Wang, X., Ge, T., Wu, C.: A dynamic model of rov with a robotic manipulator using kane’s method. In: 2013 Fifth Conference on Measuring Technology and Mechatronics Automation, pp. 9–12. IEEE, Hong Kong (2013) 19. From, P.J., Duindam, V., Pettersen, K.Y., Gravdahl, J.T., Sastry, S.: Singularity-free dynamic equations of vehicle–manipulator systems. Simul. Model. Pract. Th. 18(6), 712–731 (2010). https://doi.org/10.1016/j.simpat.2010.01.012 20. Teo, K., An, E., Beaujean, P.-P.J.: A robust fuzzy autonomous underwater vehicle (auv) docking approach for unknown current disturbances. IEEE J. Oceanic Eng. 37(2), 143–155 (2012) 21. Zhang, T., Li, D., Yang, C.: Study on impact process of auv underwater docking with a cone-shaped dock. Ocean Eng. 130, 176–187 (2017) 22. Yazdani, A.M., Sammut, K., Yakimenko, O.A., Lammas, A., Tang, Y., Mahmoud Zadeh, S.: Idvd-based trajectory generator for autonomous underwater docking operations. Robot. Auton. Syst. 92, 12–29 (2017) 23. Meng, L., Lin, Y., Gu, H., Su, T.-C.: Study on dynamic docking process and collision problems of captured-rod docking method. Ocean Eng. 193, 106624 (2019) 24. Cui, P., Yan, W., Cui, R., Yu, J.: Smooth path planning for robot docking in unknown environment with obstacles. Complexity 2018(6), 1–17 (2018) 25. Thomas, C., Simetti, E., Casalino, G.: A unifying task priority approach for autonomous underwater vehicles integrating homing and docking maneuvers. J. Mar. Sci. Eng. 9(2), 162 (2021)

256

Y. Zhang et al.

26. Li, Y., Jiang, Y., Cao, J., Wang, B., Li, Y.: Auv docking experiments based on vision positioning using two cameras. Ocean Eng. 110, 163–173 (2015) 27. Li, D., Chen, Y., Shi, J., Yang, C.: Autonomous underwater vehicle docking system for cabled ocean observatory network. Ocean Eng. 109, 127–134 (2015) 28. Trslic, P., et al.: Vision based autonomous docking for work class rovs. Ocean Eng. 196, 106840 (2020) 29. Wang, T., Zhao, Q., Yang, C.: Visual navigation and docking for a planar type auv docking and charging system. Ocean Eng. 224, 108744 (2021) 30. Vallicrosa, G., Bosch, J., Palomeras, N., Ridao, P., Carreras, M., Gracias, N.: Autonomous homing and docking for auvs using range-only localization and light beacons. IFACPapersOnLine 49(23), 54–60 (2016) 31. Anderlini, E., Parker, G.G., Thomas, G.: Docking control of an autonomous underwater vehicle using reinforcement learning. Appl. Sci-Basel 9(17), 3456 (2019) 32. Antonelli, G.: Underwater robots, 4th edn. Springer International Publishing AG, Gewerbestrasse (2018). https://doi.org/10.1007/978-3-319-77899-0 33. Chiaverini, S.: Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators. IEEE T. Robotic. Autom. 13(3), 398–410 (1997) 34. Antonelli, G.: Stability analysis for prioritized closed-loop inverse kinematic algorithms for redundant robotic systems. IEEE Trans. Robot. 25(5), 985–994 (2009)

A Novel Control Approach of the Flexible Servo Riveting Gun Based on Self-pierce Riveting Technology Yan Liu1(B) , Qiu Tang1 , Xincheng Tian1 , and Lixin Ma2 1

2

School of Control Science and Engineering, Shandong University, 17923 Jingshi Road, Jinan 250061, China ly [email protected] CODESYS Software System (Beijing) Co., Ltd., Beijing, China

Abstract. Lightweight vehicle bodies have become the development trend in auto industry, so the self-pierce riveting (SPR) technology has attracted extensive attention. This paper proposes a novel control approach of the flexible servo riveting gun based on SPR technology. First, this paper briefly introduces the riveting procedure based on the mechanical structure of servo riveting gun. Then the riveting quality inspection rules of traditional riveting guns are used for reference to plan the forceposition curve in riveting process autonomously, and the cubic spline curve is adopted to construct the force trajectory. To control this planned force trajectory, riveting force measured by sensor is fed back into the closed-loop control. According to the deviation between theoretical and actual riveting force, the set value of riveting speed is changed based on admittance control algorithm, so the servo motors torque is indirectly controlled to make the riveting force reach the set value. Finally, the control approach is applied to the servo riveting system designed by our lab. Riveting results show the feasibility of proposed control approach.

Keywords: Self-pierce riveting and control

1

· Servo riveting gun · Force planning

Introduction

Many countries have made commitments to reduce carbon emissions and formulated a series of standards to limit the exhaust emissions of fuel vehicles [1]. Studies have shown that reducing the entire vehicles quality can improve the driving performance, such as fuel economy, carbon oxide emissions, braking distance and other indicators [2]. Typically, an important way to lighten the vehicles quality is to use lots of lightweight materials in the white-body, such as aluminum alloy, magnesium alloy, and reinforced plastic. For the connection Supported by the Shandong Provincial Key Research and Development Program under Grant No. 2019JZZY010441. c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 257–267, 2021. https://doi.org/10.1007/978-3-030-89092-6_24

258

Y. Liu et al.

between these materials, spot welding method cannot meet the performance requirements. Compared with traditional connection methods, self-pierce riveting (SPR) has the advantages of high energy efficiency, environmental protection and no material restrictions particularly [3]. Therefore, SPR has become the major approach of body sheet joining for new energy vehicles. SPR technology is a cold joining technique to form a stable connection, which using the riveting gun to directly press rivets into sheets to be riveted [4]. The basic structure of the riveting gun is: controller and power system, mechanical frame, rivets supply system and riveting die. The quality of SPR is closely related to the shape of rivet and die, the material of sheet workpiece, and the riveting force [5]. When the shape of rivet, die and the sheets are matched, how to integrate the SPR process to control the riveting force will directly determine the riveting quality. Therefore, the research on riveting gun control is of great significance to the development of automobile lightweight. Many studies on SPR technology has been carried out, mainly in directions of the performance analysis of riveted joints including the static performance and fatigue analysis, the monitoring method of riveting quality, and the influence analysis of SPR process parameters on riveting quality. Paper [6] shows that the specimen configuration of aluminum alloys has a significant effect on the strength and failure mechanism of riveted joints by fatigue analysis. Paper [7] describes several key parameters of the joint shape that can evaluate the SPR joint quality by static performance analysis, which provides guidance for the adaptation between rivets and SPR process parameters. In riveting quality monitoring, paper [8,9] show that the force-displacement curve is a reliable method to analyze the riveting quality and divide it into four stages. Sehyeok et al. develops the supervised deep-learning architecture for SPR process to predict the cross-sectional shape from the riveting force [10], these methods will be performed after the riveting is completed. For a given material combination, many researchers focus on the rivet and die designation using simulation and experimental approach. For example, paper [11] systematically studies the influence of die type, diameter, depth and tip height on joint formation, paper [12] analyzes the influence of half-hollow rivet geometric structure on the SPR quality through the joint cross-section observation and the mechanical characteristic test. Zhao et al. established a joint shape prediction model using regression analysis and FEA [13], including the regression models for the interlock and the minimum remaining bottom sheet thickness prediction. There have been some self-pierce riveting guns on production line, such as hydraulic self-pierce riveting guns represented by Henrob and electric servo selfpierce riveting guns represented by Tucker. The former is mainly based on constant force punching, and the latter is speed control during the riveting. However, neither can plan and control the riveting force during riveting process, the riveting process cannot be integrated to interfere with the riveting quality. Furthermore, it is essential to determine whether the riveting joints is qualified by adding an additional riveting quality monitoring system.

The exible servo riveting gun control based on SPR technology

259

To solve the problems of high cost, complicated operation and uncontrollable riveting quality of existing servo riveting gun, this paper aims to propose a new control method of flexible servo riveting gun integrating SPR technology and riveting force feedback. First, the riveting procedure based on the mechanical structure of servo riveting gun is introduced briefly, which is the basis of riveting control. Then, the riveting quality inspection rules of traditional riveting guns are used for reference to autonomously plan the force-position curve in riveting process, which is used as the riveting control goal. To achieve this goal, the actual riveting force is fed back into the system closed-loop control, and the speed reference value is given based on admittance control algorithm. Finally, a servo riveting gun is developed and the feasibility of proposed control approach is verified by riveting experiments.

2

The Working Principle of Servo Riveting System

There are two main power sources for self-pierce riveting guns: hydraulic cylinder and servo motor. The hydraulic riveting gun can provide a large punch force instantaneously, its mechanical structure is simple, but the controllability is poor and there is a risk of oil leakage. The mechanical structure of servo riveting gun is relatively complicated, and it usually includes transmission mechanism such as the reducer and lead screw to increase the riveting torque, but its advantage lies in its good controllability. With the development of manufacturing technology, the servo motor performance has been improved while its volume has been gradually reduced, and its shortcomings have been gradually overcome. Therefore, this paper studies the control method of riveting gun powered by servo motor.

Fig. 1. The SPR process

Taking a two-layer joint as an example, the SPR process can be roughly summarized into four stages (as shown in Fig. 1): (a) The blank holder moves downwards to compress the two sheets to prevent the surrounding sheets from flowing into the die under the riveting force. (b) The rivet rod moves towards the die according to the setting law, pushing the rivet to pierce the upper sheet. (c) As the riveting process progresses, the rivet shank first pierces through the top sheet and gradually expands to the surroundings. (d) The rivet shank and the

260

Y. Liu et al.

sheets gradually form an interlocking structure. This four stages of the riveting process mentioned above are called a riveting cycle.

3

The Force-Displacement Trajectory Planning During Riveting

The self-pierce riveting process determines that riveting quality is affected by the material of sheets and rivets, die and other factors, and these effects are ultimately reflected in the changing trend of riveting force with the rivet displacement. In this article, the riveting force-displacement curve is called the riveting force trajectory. Research and experiment show that: the characteristics of riveting force trajectory are different under different process parameters. Therefore, the mainstream riveting equipment manufacturer, such as Henrob and Emhart, determines the riveting quality by detecting the riveting force trajectory. Typically, designing and controlling the riveting force trajectory is the key to riveting quality control. This article takes the riveting force as control target, adapts the process changes of different sheets, rivets, and dies by planning the riveting force trajectory, so as to achieve the best riveting quality and realize the flexibility of riveting system. 3.1

The Analysis of Riveting Force Trajectory

There are two methods of self-pierce riveted joint quality online inspection: one is to judge the joint quality by monitoring the change trend of force-displacement trajectory during the riveting process. The other is to judge the joint quality by observing whether the rivet shank is normally expanded by ultrasound. In view of the equipment cost and system integration, the current riveting quality inspection systems generally adopt the first method. This also indirectly proves the importance of riveting force to riveting quality, and provides a basis for the idea of controlling riveting quality by controlling riveting force in this article.

Fig. 2. The schematic diagram of riveting force trajectory

The exible servo riveting gun control based on SPR technology

261

For the research on riveting quality determination, one part is judged by setting error band for the riveting force trajectory, the other part is judged by setting different windows at the critical stages. This article will combine the above two methods: setting error bands during the riveting force control to reduce the frequency of speed command changes, and planning the riveting force trajectory during the critical stage to ensure the joint quality. Through analysis and summary, the three critical stages of the self-pierce riveting process are: the rivet penetrates the upper sheet, the rivet penetrates the lower sheet, and the rivet shank expands to the die. The schematic diagram of riveting force trajectory curve is shown in Fig. 2. According to Fig. 2, the change trend of riveting force in critical stages is determined by the riveting force and its derivative (to position) in the key nodes (x = x0 , x1 , x2 · · · ). Therefore, the mathematical expression of riveting force trajectory can be determined by setting the force and its derivative information of key nodes. 3.2

The Construction of Riveting Force Trajectory

Commonly used curve construction methods include polynomial fitting, spline fitting and NURBS fitting etc. However, the calculation of polynomial fitting is complicated, it is necessary to construct node vector U for NURBS fitting, and the mathematical expression of fitted curve is parametric equation with node variable u as independent variable, which has changed the original functional relationship [14]. To make the riveting force trajectory smooth and the 2ndorder derivative between nodes continuous, this paper takes the cubic spline curve as an example to design and fit the force trajectory. Assuming that the sheet material, rivet, and die have been determined and successfully matched according to automobile production process, the position information of key nodes is [x0 , x1 , · · · , xn ], the riveting force information is [fr0 , fr1 , · · · , frn ] and its derivative information is [f  r0 , f  r1 , · · · , f  rn ]. According to the definition of cubic spline, function Sc (x) is C 2 [ha , hb ], hr = hb − ha is defined as rivet height, and these variables satisfy the relationship ha = x0 < x1 < · · · < xn = hb . Set the 2nd-order derivative of spline function Sc (x) at the key node is  Sc (xj ) = Mj , (j = 0, 1, · · · , n), select nodes xj and xj+1 , then on interval  [xj , xj+1 ], Sc (x) is a linear function, which can be expressed as:  Δj = xj+1 − xj  (1) −x x x−x , j = 0, 1, · · · , n − 1 Sc (x) = Mj j+1 + Mj+1 Δj j Δj The force and its derivative information of key nodes can be expressed as:  Sc (xj ) = frj , Sc (xj+1 ) = frj+1   Sc (xj − ) = Sc (xj + ) = f  rj

(2)

262

Y. Liu et al. 

Integrating Sc (x) and combining condition 1 in Eq. 2 to calculate the expression of cubic spline Sc (x) on interval [xj , xj+1 ]: Sc (x) = Mj

Mj Δ2j xj+1 − x Mj+1 Δ2j x − xj (xj+1 − x)3 (x − xj )3 ) ) + Mj+1 + (frj − + (frj+1 − 6Δj 6Δj 6 Δj 6 Δj

(3) 

Taking the derivative of Eq. 3, the expression of Sc (x) is: 

Sc (x) = −Mj

2

2

(xj+1 − x) (x − xj ) frj+1 − frj Mj+1 − Mj Δj (4) +Mj+1 + − 2Δj 2Δj Δj 6 

Similarly, the expression of Sc (x) on interval [xj−1 , xj ] can be calculated: 2

2

(xj − x) (x − xj−1 ) frj − frj−1 Mj − Mj−1 Δj−1 + Mj + − 2Δj−1 2Δj−1 Δj−1 6 (5) According to condition 2 in Eq. 2, we can get:  2Mj + Mj+1 = kj , j = 1, 2, · · · n − 1 (6) μj Mj−1 + 2Mj + λj Mj+1 = mj 

Sc (x) = −Mj−1

Where symbols μj = kj and mj are:

Δj−1 Δj−1 +Δj ,



λj =

Δj Δj−1 +Δj ,

and the expressions of parameters

6  Δj (f [xj , xj+1 ] − f rj ) f [xj ,xj+1 ]−f [xj−1 ,xj ] mj = 6 Δj−1 +Δj

kj =

(7)

Where f [xj , xj+1 ] is the first-order average difference of fr (x) with respect 6 (f  rn − to points xj and xj+1 , let k0 = Δ60 (f [x0 , x1 ] − f  r0 ) and kn = Δn−1 f [xn−1 , xn ]), then Eq. 6 can be written in matrix form: ⎡ ⎤ ⎤ ⎡ k0 2 1 ⎡ ⎤ ⎢ m1 ⎥ ⎥ M0 ⎢μ1 2 λ1 ⎢ ⎥ ⎥ ⎢ ⎥ ⎢ M1 ⎥ ⎢ k1 ⎥ ⎢ 2 1 ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ .. ⎥ ⎢ .. ⎥ ⎢ .. (8) ⎥⎢ . ⎥=⎢ . ⎥ ⎢ . ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎢ ⎥ ⎥ ⎢ ⎣ ⎦ μn−1 2 λn−1 ⎥ Mn−1 ⎢mn−1 ⎥ ⎢ ⎣ kn−1 ⎦ ⎣ 2 1 ⎦ Mn kn 1 2 Solving Eq. 8 can get the unknown vector [Mj ], and substituting it into Eq. 3 can get the segment expression of the function Sc (x). As is shown in Fig. 3, the blue windows are quality monitoring window, in which the overall slope of force trajectory curve must meet the limited conditions given according to quality criteria. It is commonly found in traditional riveting quality monitoring systems. This paper will find the optimal force and its derivative information of the key nodes by delimiting this window, and then construct the riveting force trajectory.

The exible servo riveting gun control based on SPR technology

263

Fig. 3. The trajectory fitting curve and its node information

Taking the key node xj for example, (x, fr ) is one point on the riveting force trajectory fr (x). In Fig. 3, point (xj , fj ) is the center of the monitor window, which can be expressed as x ∈ [xj − ςj,x , xj + ςj,x ]and f ∈ [fj − ςj,y , fj + ςj,y ], where 2ςj,x and 2ςj,y represent the length and width respectively. So the limited conditions can be listed as: ⎧ 0 ≤ fr  (x) ≤ frd max , x ∈ [xj − ςj,x , xj + ςj,x ] ⎪ ⎪ ⎨ ∃x ∈ (xj − ςj,x , xj + ςj,x ), letfr = fj − ςj,y (9) ∃f ⎪ r ∈ (fj − ςj,y , fj + ςj,y ), letx = xj + ςj,x ⎪ ⎩  |fr (xj + ςj,x ) − fr  (xj+1 − ςj+1,x )| ≤ ςd where ςd is the max value of derivative difference between adjacent key nodes. Then selecting the force and its derivative (fr (xj ∓ ςj,x ), fr  (xj ∓ ςj,x )) at node xj ∓ ςj,x to construct the riveting force trajectory according to the computation of function Sc (x). Considering the acceleration characteristics of servo motor, this paper takes the square sum of riveting force derivative as the optimization goal, which is n  2 fr  (xi ) (10) Fgoal (x) = i=1

The optimal key node information (x, fr , fr  ) can be calculated using advanced optimization algorithms, such as GA and NSGA2 [15,16]. Then submitting (x, fr , fr  ) into the calculation algorithm of cubic spline, the force trajectory fr (x) is obtained eventually.

4

The Design of Riveting Gun Controller Based on Admittance Control

This paper proposes a servo riveting gun control method based on a fully closedloop of riveting force. The system control block diagram is shown in Fig. 4.

264

Y. Liu et al.

The speed controller is integrated in servo driver and the controller should give the set value of riveting speed refer to rivet current position and force. Admittance control is one of the robot compliance control method [17], whose basic principle is to calculate the motion trajectory of robot TCP according to the deviation of contact force between robot and workpiece.

Fig. 4. The control scheme proposed in this paper

The riveting gun is essentially a single-axis robot, and the general expression of admittance control algorithm is: ef = M e¨ + B e˙ + Ke

(11)

Where ef is the contact force deviation, ξf ≥ 0 is the force deviation threshold, and e is the position increment of robot. In the servo riveting system designed in this paper: ⎧ ef = Fref − Fpush ⎪ ⎪ ⎨ e˙ = ωref − ω0 (12) e = edt ˙ ⎪ ⎪ ⎩ e¨ = ddte˙ Fref is the set value of riveting force obtained according to fr (x). Substituting Eq. 12 into Eq. 11 and performing Laplace transformation to Eq. 11, the transfer function of admittance controller can be expressed as:  Eω (s) 0, |ef | ≤ ξf = (13) Gc (s) = s Ef (s) M s2 +Bs+K , |ef | > ξf The schematic diagram of admittance controller is shown in Fig. 5. The speed increment Δω output by the admittance controller is applied to the initial speed value ω0 , the target speed ωref of the speed controller can be set. Referring related papers to adjust parameters M , B and K to obtain a favorable system response.

The exible servo riveting gun control based on SPR technology

265

Fig. 5. The schematic diagram of controller designation

5

Experiments

To verify the feasibility of the planning and control methods proposed in previous chapters, related experiments are designed and carried out. The riveting experiment is carried out on the riveting system designed by our lab, which uses an industrial computer as the control core. 5.1

The Experiment Platform

The riveting system is composed of the control system, the servo drive system, the mechanical body and the rivets feeding device, as is shown in Fig. 6. The drive system uses Panasonic bus-type, high-performance drive and motor. The industrial computer is embedded with TwinCAT motion control software and connects to the driver, coupler and IO terminals through EtherCAT bus. Rivets are stored in the rivet cassette, and are pushed to the designated position by cylinder during riveting. The driver works in cyclic synchronous velocity (CSV) mode, and the planning and control algorithms are implemented in TwinCAT. 5.2

Results

The servo riveting gun have been used to conduct riveting experiments on aluminum alloy multi-layer sheets with different dies, and the experimental results are shown in the Fig. 7. It shows that the riveting system performs well in the SPR experiments of 2-layer sheets connection with different types of riveting die, and the riveting force trajectory deviation is within the tolerance of following error, which has met the control requirements designed in this paper. In the riveting gun control system, the advantages of adopting this novel control approach proposed in this paper are: 1) Riveting can be carried out in strict accordance with the planned force trajectory, which is beneficial to improve the yield rate of riveted joints and production efficiency. 2) The riveting quality monitoring system can be saved, the resource utilization rate can be improved, then the equipment cost can be reduced.

266

Y. Liu et al.

Fig. 6. The servo riveting gun prototype developed by our lab

Fig. 7. The riveting results. (a) (b) Riveted joints. (c) Riveting force trajectory

6

Conclusions

In this paper, a novel control approach of the flexible servo riveting gun based on SPR technology is proposed, and the prototype of servo riveting gun is developed to conduct the experimental verification. The major contributions of this work include: 1) The riveting quality inspection rules of traditional riveting guns are used for reference to plan the force-position curve in riveting process autonomously, and the cubic spline curve is adopted to construct the force trajectory. 2) For the control of planned force trajectory, riveting force measured by sensor is fed back into the closed-loop control. And the admittance control model is adopted to give the set value of riveting speed according to the deviation between theoretical and actual riveting force. Riveting results have shown the feasibility of proposed control approach.

The exible servo riveting gun control based on SPR technology

267

References 1. Wang, T.T., Wagner, J.: Advanced engine cooling system subjected to ram air effect-nonlinear adaptive multiple input and multiple output (NAMIMO) control. IEEE Trans. Veh. Technol. 66(9), 7730–40 (2017) 2. Briskham, P., Blundell, N., Han, L., Hewitt, R., Young, K., Boomer, D.: Comparison of Self-Pierce Riveting. Resistance Spot Welding and Spot Friction Joining for aluminium automotive sheet, SAE Technical Papers (2006) 3. Han, L., Thornton, M., Shergold, M.: A comparison of the mechanical behaviour of self-piercing riveted and resistance spot welded aluminium sheets for the automotive industry. Mater. Eng. 31(3), 1457–67 (2010) 4. He, X., Pearson, I., Young, K.: Self-pierce riveting for sheet materials: state of the art. J. Mater. Proces. Techno. 199(1), 27–36 (2008) 5. Mori, K., Abe, Y., Kato, T.: Self-pierce riveting of multiple steel and aluminium alloy sheets. J. Mater. Proces. Technol. 214(10), 2002–2008 (2014) 6. Zhang, X., He, X., Xing, B., Wei, W., Lu, J.: Quasi-static and fatigue characteristics of self-piercing riveted joints in dissimilar aluminium-lithium alloy and titanium sheets. J. Mater. Res. Technol. 9(3), 5699–5711 (2020) 7. Haque, R.: Quality of self-piercing riveting (SPR) joints from cross-sectional perspective: a review. Arch. Civil Mech. Eng. 18(1), 83–93 (2017). https://doi.org/ 10.1016/j.acme.2017.06.003 8. Hou, W., Mangialardi, E., Hu, S.J., Wang, P.C., Menassa, R.: Characterization for quality monitoring of a self-piercing riveting process. In: Proceedings of the Sheet Metal Welding Conference XI (2004) 9. Haque, R., Durandet, Y.: Investigation of self-pierce riveting (SPR) process data and specific joining events. J. Manuf. Process. 30, 148–160 (2017) 10. Oh, S., Kim, H.K., Jeong, T., Kam, D., Ki, H.: Deep-learning-based predictive architectures for self-piercing riveting process. IEEE Access 8, 116254–116267 (2020) 11. Liu, Y., Li, H., Zhao, H., Liu, X.: Effects of the die parameters on the self-piercing riveting process. Int. J. Adv. Manuf. Technol. 1, 3353–3368 (2019). https://doi. org/10.1007/s00170-019-04567-4 12. Jiang, H., Gao, S., Li, G., Cui, J.: Structural design of half hollow rivet for electromagnetic self-piercing riveting process of dissimilar materials. Mater. Des. 183, 108141 (2019) 13. Zhao, H., Han, L., Liu, Y., Liu, X.: Modelling and interaction analysis of the selfpierce riveting process using regression analysis and FEA. Int. J. Adv. Manuf. Technol. 1, 159–176 (2021). https://doi.org/10.1007/s00170-020-06519-9 14. Piegl, L., Tiller, W.: The NURBS Book, 2nd edn. Springer, Berlin (2003) 15. Deb, K., Pratap, A., Agarwal, S., Meyarivan, T.: A fast and elitist multiobjective genetic algorithm: NSGA-II. IEEE Tans. Evol. Comput. 6(2), 182–197 (2002) 16. Amar, J., Nagase, K.: Genetic-algorithm-based global design optimization of treetype robotic systems involving exponential coordinates. Mech. syst. sig. Process. 156, 107461 (2021) 17. Calanca, A., Muradore, R., Fiorini, P.: A review of algorithms for compliant control of stiff and fixed-compliance robots. IEEE/ASME Trans. Mech. 21(2), 613–624 (2016)

Path Synthesis Optimization Model of Planar Five-Bar Metamorphic Mechanism Xinyuan Yao, Xingdong Wang, Wei Sun(B) , and Haoke Bai School of Mechanical Automation, Wuhan University of Science and Technology, Wuhan, China [email protected]

Abstract. The path synthesis of metamorphic mechanism is an essential problem in kinematics. This paper develops a new optimization model for planar five-bar metamorphic mechanism with two sub-configurations. As different configurations may produce large differences in the target points, the utilization of two configurations can help to preserve good results and avoid bad results. Comparative study between metamorphic mechanism and the models using one subconfiguration shows that trajectory of metamorphic mechanism by the proposed model is closer to the target point. In addition, an example demonstrates the comprehensive capabilities of metamorphic mechanisms in the face of trajectories with change mutation points. This optimization model has potential reference value for the optimization of other metamorphic mechanisms. Keyword: Metamorphic mechanism · Path synthesis · Optimization algorithm · Kinematics · Five-bar mechanism

1 Introduction Mechanisms is an important basic discipline of robotics, including kinematics and dynamics of robot mechanisms. One of the important topics of mechanism research is to synthesize robot mechanisms to improve their kinematic and dynamic performance. Compared with traditional mechanisms, metamorphic mechanisms are multi-topological structures which give good solutions to the contradiction among the economy, adaptability and efficiency. Metamorphic mechanisms [1] were proposed by Dai et al. according to the idea of reconfiguration. As the metamorphic property can adapt to different tasks, it can be applied to different situations. Hence, it has become increasingly important in industrial production, especially for reconfigurable robots and aerospace. Specifically, in the case of a plurality of different stages of work, the mechanism is always used to change the mechanism topology to exhibit different mechanism types or kinematic performance to achieve functional requirements. As the skeleton of modern mechanical systems, the mechanism has attracted considerable attentions from researchers due to its accuracy and reliability. Kim et al. [2] introduced a new design method for crank-rocker four-bar linkages according to differential objective functions. Cabrera et al. [3] presented a MUMSA algorithm for synthesis of mechanisms. An imperialist competitive algorithm was introduced by Ebrahimi et al. © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 268–278, 2021. https://doi.org/10.1007/978-3-030-89092-6_25

Path Synthesis Optimization Model

269

[4], which is utilized to optimize the link lengths of four-bar mechanism. Gogater et al. [5] utilized evolutionary methods to optimize the trajectory generation of a four-bar mechanism. Meanwhile, three different error functions were presented. A different error function was proposed by Matekar et al. [6], which can be utilized to optimize path generation of four-bar mechanisms. However, the research related to trajectory optimization of metamorphic mechanisms has not attracted attention compared to traditional mechanisms. So far, researches on metamorphic mechanisms have focused on configuration description [7, 8], transformation analysis [9–11] and structural synthesis [12–14]. An optimization method, multidisciplinary design optimization, was first proposed by Zhang et al. [15] for optimum synthesis of metamorphic mechanisms. Besides, the trajectory optimization of the adjustable linkage [16] was analyzed. The controlled five-bar mechanism [17] was studied by Kong in the 1990s. The dimensional synthesis of metamorphic mechanisms is different from the customary regular mechanisms [18, 19]. The main reason is that metamorphic mechanisms exist many configurations. Hence, this research presents two possible contributions as follows. Firstly, this study develops a new optimization model for planar five-bar metamorphic mechanism with two sub-configurations. This model may provide a reference for the optimization of other metamorphic mechanisms. Secondly, a planar five-bar metamorphic mechanism is proposed, which has the potential to generate paths with multiple change points due to different configurations may produce large differences in the achieved target points. This feature makes the mechanism potentially valuable in the design of advanced robot structures.

2 The Configurations of Metamorphic Mechanism This section brings an example of a five-bar metamorphic mechanism as shown in Fig. 1, using the metamorphic principle, to highlight the features of path synthesis of metamorphic mechanism. The variation of configurations is performed by changing properties of the kinematic pair.

Fig. 1. Schematic diagram of five-bar metamorphic mechanism.

A schematic diagram of the proposed planar five-bar metamorphic mechanism is shown in Fig. 1, where the link 1 is the input link and the link 4 is the frame link. Links

270

X. Yao et al.

2 and 3 form a dyad connecting the end of the input link and the slider 5 which sliding along frame link 4 in a restricted range. In addition, the symbols x A , yA and θ 4 denote the offset and rotation angle of the frame link 4 in the global coordinate system XOY, respectively. The symbol ϕ 10 means the initial angle of the input link with respect to the X-axis. Configuration analysis of a metamorphic mechanism is illustrated in Fig. 2, including two sub-configurations.

Fig. 2. (a) Configuration 1: Crank-slider mechanism. (b) Configuration 2: Four-bar mechanism.

It is worth noting that the joint C is a controlled joint, and its state is affected by its angle and the state of slider 5. In configuration 1, since slider is not touching its boundary, the joint C is locked and links 2 and 3 are fixed together as shown in Fig. 2(a). The locking angle of joint C is a constant θ 10 . The mechanism is a slider-crank mechanism with 1 DOF, and this configuration can also be regarded as the original metamorphic mechanism. Once the slider touches its boundary, the joint C is unlocked to ensure that the input link can be rotated the whole cycle. Then the mechanism will transform into a four-bar mechanism in configuration 2, as shown in Fig. 2(b), and its DOF is also 1. And then, the joint C locks when its angle θ 1 reaches the locking angle θ 10 again, and the mechanism changes again to configuration 1. The locking of joint C can be achieved by an electromagnetic brake. Metamorphic mechanisms change continuously in two configurations during a cycle. At the same time, the point P fixed on the link 2 of the metamorphic mechanism generates a coupler path. The path generation problem of metamorphic mechanism is transformed into the optimization of dimensional parameters of the mechanism. The aim is to minimize the error between the target points Q(xi , yi ) and actual output points P(x i , yi ). The actual output points can be obtained when linkage lengths and other parameters are determined. The coordinates of coupler point P are shown in Fig. 1 expressed as follows:  xP = xA + l1 cos(ϕ10 + ϕ1 ) + r2 cos(ϕ2 + θ2 ) (1) yP = yA + l1 sin(ϕ10 + ϕ1 ) + r2 sin(ϕ2 + θ2 ) where, the variable l1 represents the length of link 1. Note that ϕ 1 refers to the rotation angle of the input link 1 relative to its initial angle ϕ 10 . The variable ϕ 2 is the angle of link 2 relative to the X-axis. The variables r 2 and θ 2 define the position of the point P on the link 2.

Path Synthesis Optimization Model

271

In configuration 1, the mechanism is a crank-slider mechanism. The parameters of the mechanism are calculated as follows:  ⎧ ⎪ l l22 + l32 − 2l2 l3 cos θ10 = ⎪ 50 ⎪ ⎪  ⎪ ⎪ 2 − l 2 sin2 (ϕ + ϕ − θ ) ⎪ ⎪ l4 = l1 cos(ϕ10 + ϕ1 − θ4 ) + l50 10 1 4 ⎪ 1 ⎨  2 2 2 l2 +l50 −l3 (2) 2l2 l50 ⎪ ϕCBD = arccos ⎪ ⎪ 

⎪ ⎪ ⎪ ϕ5 = arcsin l4 sin θ4 −l1l50sin(ϕ10 +ϕ1 ) ⎪ ⎪ ⎪ ⎩ ϕ2 = ϕ5 + ϕCBD where, the constant l50 is the length of the equivalent links of link 2 and link 3 after revolute pair B is locked. The variables l2 and l 3 represent the length of link 2 and link 3, respectively. In addition, l4 represent the distance of the slider relative to the fixed pivot A. In configuration 2, the mechanism becomes a four-bar linkage. The parameters of the mechanism are calculated as follows. ⎧  2 − 2l l cos(ϕ + ϕ − θ ) ⎪ ⎪ l5 = l12 + l40 1 40 10 4 ⎪ ⎪ 1

⎪ ⎪ l40 sin θ4 −l1 sin(ϕ10 +ϕ1 ) ⎪ ⎨ ϕ5 = arctan l40 cos θ4 −l1 cos(ϕ10 +ϕ1 )  (3) l22 −l52 −l32 ⎪ + ϕ5 = arccos ϕ ⎪ 3 ⎪ 2l l 3 5 ⎪ ⎪ 

⎪ ⎪ ⎩ ϕ2 = arctan l5 sin ϕ5 +l3 sin ϕ3 l5 cos ϕ5 +l3 cos ϕ3

where, l5 indicates the distance between the joint B and the joint D, and the constant l 40 means the length of the frame of the four-bar mechanism, which is also the maximum sliding distance of the slider. In addition, it’s important to note that when use the arctan function to compute the values of ϕ 5 and ϕ 2 , the result should add π if the denominator is negative.

3 Optimization Model The path generation problem might be regarded as an optimization problem to minimize the error between the target trajectory and actual output points. However, the path generation of metamorphic mechanisms is different from the traditional regular mechanisms due to it exists many configurations. The transformation of the configurations affects the coupler curves of metamorphic mechanism. The objective function is the positional error, which is the distance between desired trajectory Q(xi , yi ) and the obtained coupler curves P(x i , yi ). 3.1 Optimization Model of Configuration 1 Design variables of configuration 1 are described as: X1 = [l1 , l2 , l3 , θ10 , ϕ10 , θ4 , r2 , θ2 , xA , yA , ϕ11 , ϕ12 · · · · · · ϕ1N ]

(4)

272

X. Yao et al.

where, N represents the number of the target points. The objective function is expressed as: min f1 =

N  (xi − xi )2 + (yi − yi )2

(5)

i=1

The constraints of the optimized model are as follows: (1) Conditions for the existence of crank: l1 − l50 ≤ 0

(6)

(2) Conditions for crank continuity: ϕ1i+1 > ϕ1i (i = 1, 2, · · · , N − 1) (3) Boundary constraints of input variables as: ⎧ 0 ≤ θ10 ≤ 180 ⎪ ⎪ ⎪ ⎨ 0 ≤ li ≤ 300 ⎪ 0 ≤ ϕ1i ≤ 360 ⎪ ⎪ ⎩ −50 ≤ xA , yA ≤ 50

(7)

(8)

3.2 Optimization Model of Configuration 2 Design variables of configuration 2 are described as: X2 = [l1 , l2 , l3 , l40 , ϕ10 , θ4 , r2 , θ2 , xA , yA , ϕ11 , ϕ12 · · · · · · ϕ1N ]

(9)

The objective function is expressed as: min f2 =

N  (xi − xi )2 + (yi − yi )2

(10)

i=1

The constraints of the optimized model are as follows: (1) Conditions for the existence of crank: ⎧ l1 − l2 − l3 + l40 ≤ 0 ⎪ ⎪ ⎪ ⎨l − l + l − l ≤ 0 1 2 3 40 ⎪ l + l − l − l 2 3 40 ≤ 0 ⎪ 1 ⎪ ⎩ l1 − min(l2 , l3 , l40 ) ≤ 0

(11)

(2) Conditions for crank continuity: ϕ1i+1 > ϕ1i i = 1, 2, · · · , N − 1 (3) Boundary constraints of input variables as: ⎧ ⎪ ⎨ 0 ≤ li ≤ 300 0 ≤ ϕ1i ≤ 360 ⎪ ⎩ −50 ≤ xA , yA ≤ 50

(12)

(13)

Path Synthesis Optimization Model

273

3.3 Optimization Model of Metamorphic Mechanism One of the greatest challenges when an evolutionary algorithm is utilized for dimensional synthesis of metamorphic mechanisms is the establishment of an optimization model. Design variables of metamorphic mechanism are described as: X = [l1 , l2 , l3 , l40 , θ10 , ϕ10 , θ4 , r2 , θ2 , xA , yA , ϕ11 , ϕ12 · · · · · · ϕ1N ]

(14)

The objective function is expressed as: min f =

N

(xi − xi )2 + (yi − yi )2

 (15)

i=1

In order to ensure the rationality of the two configurations of the metamorphic mechanism, the constraints of the metamorphic mechanism will include all the constraints of configurational 1 and configurational 2, as shown in Eq. (6–8) and (11–13). In addition, the metamorphic mechanism also needs to satisfy the following constraints to ensure that it has two configurations: ⎧  ⎨ l40 < l1 + l 2 + l 2 − 2l2 l3 cos θ10 3 2 (16) ⎩ l > l − l 2 + l 2 − 2l l cos θ 40

1

2

3

2 3

10

The optimization model of the metamorphic mechanism is established. In particular, different configurations may produce large differences for generated curve. The traditional four-link mechanism can match up to nine target points accurately. However, since the metamorphism mechanism has more design variables and multi-configurations, it has the potential to match more target points.

4 Comparative Study This section provides two types of comparative study cases. The first case is to compare the performance of the metamorphic mechanism and its two configurations for the same path synthesis task. The second case shows the performance of metamorphic mechanism in the path synthesis task with multiple change points under different optimization algorithms. 4.1 Comparison of Path Synthesis Results of Different Mechanisms The target points Q(xi , yi ) are listed in Table 1. Figure 3 shows the trajectory synthesis results of the metamorphic mechanism and its two configurations. In this case, PSO algorithm is used to optimize the solution. The parameters of PSO algorithm: Population size is 40, and the number of iterations is 200. The optimization results of the metamorphic mechanism are shown in Table 2. The specific steps of the trajectory optimization of the metamorphic mechanism are as follows:

274

X. Yao et al. Table 1. The target points [15].

No.

1

2

3

4

5

6

x coordinate (mm)

90.6370

76.8194

60.1725

45.4632

36.8254

35.4143

y coordinate (mm)

51.7388

59.6564

61.1179

59.2460

42.3444

28.9602

No.

7

8

9

10

11

12

x coordinate (mm)

41.3717

50.1560

70.3132

85.8555

95.8937

97.6261

y coordinate (mm)

18.4987

12.3348

11.8870

17.2435

27.5398

40.1082

Fig. 3. The best paths generated by different mechanisms.

Step 1: Calculate the objective function f 1 of configuration 1. Because the mechanism is a crank-slider mechanism, the link l40 does not exist. The result of PSO algorithm is 14.8431. Step 2: Calculate the objective function f 2 of configuration 2. Because the metamorphic mechanism becomes a four-bar linkage, the limiting angle θ 10 does not exist. The result of PSO algorithm is 14.8380. Step 3: Calculate the objective function f of the metamorphic mechanism. The ultimate result of PSO algorithm is 7.1813. The optimized coupler curves of configuration 1, configuration 2 and metamorphic mechanism are shown in Fig. 3, respectively. In the optimized trajectory of the metamorphic stage, the trajectory consists of two mutations. The trajectory of metamorphic mechanism is closer to the target point. As can be seen from Table 2, the error of the metamorphic mechanism between desired and obtained coupler curves by PSO algorithm is 7.1813, which is prior to that of each individual optimization model. Clearly, this illustrated example demonstrates the superiority of the proposed model with two sub-configurations.

Path Synthesis Optimization Model

275

Table 2. Optimized results of the target trajectory in Table 1. Items

Configuration 1

Configuration 2

Metamorphic mechanism

l1

31.1640

32.2136

l2

13.1701

144.8681

78.1132

l3

226.2788

117.3044

156.2680

l 40

/

130.1304

156.0858

θ 10 (deg.)

67.3800

/

84.8626

ϕ 10 (deg.)

22.4749

41.0948

29.4646

θ 4 (deg.)

163.5248

334.7383

318.0095

31.4100

r2

44.2891

28.7801

17.1877

θ 2 (deg.)

257.5497

352.4859

309.2330

xA

25.0118

38.2702

50.0000

yA

18.8347

27.8748

40.5114

ϕ 1 1 (deg.)

18.0699

1.4313

7.2881

ϕ 1 2 (deg.) ϕ 1 3 (deg.) ϕ 1 4 (deg.) ϕ 1 5 (deg.) ϕ 1 6 (deg.) ϕ 1 7 (deg.) ϕ 1 8 (deg.) ϕ 1 9 (deg.) ϕ 1 10 (deg.) ϕ 1 11 (deg.) ϕ 1 12 (deg.)

50.1750

31.1030

36.8233

81.2467

61.1671

68.8546

107.8067

86.7046

97.3737

146.4082

123.2747

135.5957

178.4979

155.7477

162.6635

204.1908

183.6238

186.2949

224.5214

206.5797

207.9657

261.0507

242.8576

246.6688

291.8259

271.7654

276.9598

321.1923

301.2778

306.5134

348.6597

332.5268

337.6123

Error

14.8431

14.8380

7.1813

4.2 Path Generation Ability of Metamorphic Mechanism In order to better demonstrate the path synthesis ability of the metamorphic mechanism. This case provides a path with a special feature that has two change points. The detailed coordinates of the target point are shown in Table 3. Both PSO algorithm and GA algorithm are used in this case to search for the optimal solution of metamorphic mechanism. The optimization results of the metamorphic mechanism under two classical optimization algorithms are shown in Table 4.

276

X. Yao et al. Table 3. The target points.

No.

1

2

3

4

5

6

x coordinate (mm)

59.324

−0.210

−33.836

−55.242

−61.778

−61.897

y coordinate (mm)

−1.529

21.827

39.249

36.122

14.536

−14.827

No.

7

8

9

10

11

12

x coordinate (mm)

−41.217

−18.141

13.742

35.270

43.384

53.876

y coordinate (mm)

−44.217

−63.259

−65.617

−58.742

−41.906

−22.486

Table 4. Optimized results of case 4. Items l1

GA 45.1555

PSO 41.6677

l2

84.6855

122.5256

l3

183.8613

199.6729

l 40

159.9848

119.1268

θ 10 (deg.)

51.7414

24.3916

ϕ 10 (deg.)

18.0459

19.9247

θ 4 (deg.)

53.5078

45.6313

r2

62.1030

54.4800

θ 2 (deg.)

70.6168

55.0030

xA

31.0319

24.7860

yA

29.8995

23.8166

ϕ 1 1 (deg.)

18.1563

23.9486

ϕ 1 2 (deg.)

67.1034

57.2760

ϕ 1 3 (deg.) ϕ 1 4 (deg.) ϕ 1 5 (deg.) ϕ 1 6 (deg.) ϕ 1 7 (deg.) ϕ 1 8 (deg.) ϕ 1 9 (deg.) ϕ 1 10 (deg.) ϕ 1 11 (deg.) ϕ 1 12 (deg.) Error

92.9193

83.4528

116.2758

112.1709

143.8164

140.7307

176.0538

172.0794

209.7257

205.1193

237.2422

232.5049

268.4766

264.5734

293.8609

292.6541

317.3613

318.8718

345.4293

349.3177

53.9809

19.6523

Path Synthesis Optimization Model

277

The error of the metamorphic mechanism between desired and obtained coupler curves by PSO algorithm is 19.6523, and the error result obtained by GA is 53.9809. The couple curves of the optimized metamorphic mechanisms are shown in Fig. 3. It is difficult to generate such a path with multiple change point for a common single DOF mechanisms. By using PSO algorithm and GA algorithm, the superiority of this mechanism is verified. Figure 4 shows the synthesis ability of the metamorphic mechanism in the face of such path task.

Fig. 4. The best paths generated by metamorphic mechanism under different algorithms.

5 Conclusions This research studies the path synthesis of metamorphic mechanism, which is a fundamental problem in kinematics. A new optimization model for metamorphic mechanism is established with two sub-configurations. The utilization of two configurations shows superior performance than that with one configuration as this new model can help to preserve good results and avoid bad results. Two cases are presented to demonstrate that the path generation ability of the metamorphic mechanism is higher than that of the common single DOF mechanism, especially in the path generation task with multiple change points. This may contribute to the compact and simple design of the robot structure. The application of this mechanism in robot structure will be the focus of our future research. Acknowledgments. This work was supported in part by the National Natural Science Foundation of China under Grant (No. 51875418), Foundation of Hubei Provincial Education Department (No. B2020011), WUST National Defense Pre-research Foundation (No. GF202008), WUST National Defense Pre-research Foundation (No. GF201906), Hubei Provincial High Value Patent Cultivation, Transformation and Industrialization Project (No. 2019-41).

278

X. Yao et al.

References 1. Dai, J.S., Jones, J.R.: Mobility in metamorphic mechanisms of foldable/erectable kinds. J. Mech. Des. 121(3), 375–382 (1999) 2. Kim, J.W., Seo, T., Kim, J.: A new design methodology for four-bar linkage mechanisms based on derivations of coupler curve. Mech. Mach. Theory. 100, 138–154 (2016) 3. Cabrera, J.A., Ortiz, A., Nadal, F., Castillo, J.J.: An evolutionary algorithm for path synthesis of mechanisms. Mech. Mach. Theory. 46(2), 127–141 (2011) 4. Ebrahimi, S., Payvandy, P.: Efficient constrained synthesis of path generating four-bar mechanisms based on the heuristic optimization algorithms. Mech. Mach. Theory. 85, 189–204 (2015) 5. Gogate, G.R., Matekar, S.B.: Optimum synthesis of motion generating four-bar mechanisms using alternate error functions. Mech. Mach. Theory. 54, 41–61 (2012) 6. Matekar, S.B., Gogate, G.R.: Optimum synthesis of path generating four-bar mechanisms using differential evolution and a modified error function. Mech. Mach. Theory. 52, 158–179 (2012) 7. Zhang, W., Ding, X., Liu, J.: A representation of the configurations and evolution of metamorphic mechanisms. Mech. Sci. 7, 39–47 (2016) 8. Aimedee, F., Gogu, G., Dai, J.S., Bouzgarrou, C., Bouton, N.: Systematization of morphing in reconfigurable mechanisms. Mech. Mach. Theory. 96(2), 215–224 (2016) 9. Ye, W., Fang, Y., Zhang, K., Guo, S.: Mobility variation of a family of metamorphic parallel mechanisms with reconfigurable hybrid limbs. Robot. Comput. Integr. Manufact. 41, 145–162 (2016) 10. Gan, D., Dai, J.S., Liao, Q.: Mobility change in two types of metamorphic parallel mechanisms. J. Mechanisms Robotics. 1(4), 041007 (2009) 11. Gan, D., Dai, J.S., Dias, J., Seneviratne, L.: Joint force decomposition and variation in unified inverse dynamics analysis of a metamorphic parallel mechanism. Meccanica 51(7), 1583– 1593 (2015) 12. Dai, J.S., Wang, D., Cui, L.: Orientation and workspace analysis of the multifingered metamorphic hand—metahand. IEEE Trans. Robot. 25(4), 942–947 (2009) 13. LI, S., Dai, J.S.: Structure of metamorphic mechanisms based on augmented assur groups. J. Mech. Eng. (2010) 14. Li, D., Zhang, Z., Chen, G.: Structural synthesis of compliant metamorphic mechanisms based on adjacency matrix operations. Chin. J. Mech. Eng. 24, 522–528 (2011) 15. Zhang, W., Wu, T., Ding, X.: An optimization method for metamorphic mechanisms based on multidisciplinary design optimization. Chin. J. Aeronaut. 27(6), 1612–1618 (2014) 16. Gogate, G.R.: Inverse kinematic and dynamic analysis of planar path generating adjustable mechanism. Mech. Mach. Theory. 102, 103–122 (2016) 17. Yang, J., Kong, J., Xiong, H., Jiang, G., Li, G.: Optimization of controlled mechanism based on generalized inverse method. Front. Mech. Eng. China. 1, 288–291 (2006) 18. Kafash, S.H., Nahvi, A.: Optimal synthesis of four-bar path generator linkages using circular proximity function. Mech. Mach. Theory. 115, 18–34 (2017) 19. Russo, M., Herrero, S., Altuzarra, O., Ceccarelli, M.: Kinematic analysis and multi-objective optimization of a 3-UPR parallel mechanism for a robotic leg. Mech. Mach. Theory. 120, 192–202 (2018)

Impedance Control for Underwater Gripper Compliant Grasping in Unstructured Environment Liang Lu1 , Qi Liu1 , Chengyuan Liang1 , Xiaofan Tang1 , Denan Xu1 , Xin Yang2 , and Bin Han1,2(B) 1 State Key Lab of Digital Manufacturing Equipment and Technology, Huazhong University of

Science and Technology, Wuhan 430074, China [email protected] 2 Guangdong Intelligent Robotics Institute, Dongguan 523808, China

Abstract. Impedance control, which is a classical compliant control method, does not possess a good force tracking ability in the underwater environment as in the air environment, due to the underwater factors such as water resistance, random disturbance of water flow and buoyancy. In this paper, two controllers are presented to solve the force tracking problem of underwater compliant grasping, the impedance controller based on parameter identification and the adaptive impedance controller. The former one updates the environment information in real time through RLS method that introduces a forgetting factor. The latter one adjusts the target stiffness and desired position of the impedance model and introduces an adaptive compensation term. Further, simulations to compare the performance with the two controllers, are conducted. It is strongly proved that the impedance controller based on parameter identification is less sensitive to the response speed of the position inner loop, and has better robustness. Finally, it is proved by experiments that the controller has a good performance in the real underwater compliant grasping task. Keywords: Impedance control · Underwater gripper · Compliant grasping · Parameter identification

1 Introduction When the underwater manipulator is salvaging dangerous underwater objects such as explosive and vulnerable, it requires strictly control the grasping force compliance to ensure that the target object can be held stably without damage. Compliant control is the use of force feedback information to control the force through a certain control strategy, including impedance control, admittance control and PID control [1]. Impedance control is one of the main compliance control methods, which makes the end-effector compliant when contacting with the environment. Many scholars have conducted research on impedance control, but there are still many problems to be solved. As early as 1985, Hogan systematically explained the concept of impedance control [2]. Since the concept of impedance control was proposed, © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 279–289, 2021. https://doi.org/10.1007/978-3-030-89092-6_26

280

L. Lu et al.

many scholars at home and abroad have conducted in-depth research on impedance control. Roveda L et al. proposed a force-tracking impedance control method in 2015, in which the environment stiffness was estimated by the extended Kalman filter, and the impedance model is adjusted by adjusting the target stiffness and damping of the system [3]. In 2016, MFA. Jamil et al. proposed a model reference adaptive impedance control method combined with fuzzy control, which has a certain ability to adapt to nonlinear and uncertainty problems [4]. Kim T et al. proposed a position-based impedance control method in 2016, which can simultaneously adjust the target stiffness and reference position of the impedance model according to the force error [5]. Hee-Chan Song proposed an impedance control method combined with the visual system in 2017 [6]. In 2020, Paolo Francesch proposed an impedance parameter adjustment method that can control the disturbing force below the dangerous value [7]. In addition, many scholars have successively proposed various methods for the underwater environment. Patryk Cie´slak proposed a position-based impedance control strategy combined with task-first algorithms for the UVMS in 2018 [8]. Yutaka Seki proposed a method of combining decomposition acceleration control and impedance control for the interactive task of suspended UVMS in unstructured underwater environments in 2018 [9]. Zhang Jianjun proposed an adaptive impedance control method for underwater manipulators in unknown environments in 2019 [10, 11]. Penglei Dai proposed a sliding mode impedance control method for I-AUV in 2020, which has certain resistance to model uncertainty and water flow disturbance [12]. In order to improve the performance of impedance controller, we need to make efforts in the following two aspects. First, the position tracking error caused by the unknown dynamic of the robot should be minimized. Second, the controller must be robust enough to deal with unknown environment stiffness and position [13]. In this paper, two impedance controllers are designed for the underwater gripper shown in Fig. 1.

Encoder Driver

Force Sensor Experiment subject

Motor

Transmission mechanism

direct effect

f1

f2 indirect effect

Fig. 1. Underwater gripper and the influence of the underwater environment.

2 Dynamics Analysis of Underwater Manipulator When the manipulator performs grasping tasks underwater, it is necessary to consider the influence of water resistance, random disturbance of water flow, buoyancy. Without loss of generality, the underwater manipulator can generally be decomposed into n linkages

Impedance Control for Underwater Gripper

281

and an end effector. Compared with the air environment, the dynamical model of the underwater manipulator in the carrier coordinate system can be described as: D(q)¨q + C(q, q˙ ) + G(q) + τf (˙q) + τh = τ − τe

(1)

Where D(q) is the positive definite inertia matrix; C(q, q˙ ) is the vector of centrifugal force and Coriolis force; G(q) is the vector of equivalent gravity; τf (˙q) is the vector of joint friction torque; τh is the torque vector of the influence of the underwater environment; τ is the vector of joint driving torque; τe is the torque vector generated by the interaction force between the end-effector and the object. The solution to D(q),C(q, q˙ ),τf (˙q) is the same as the air environment. However, in the underwater environment, the object will be affected by the buoyancy. Therefore, the equivalent gravity composed of gravity and buoyancy is described as: ρwater G = mobject g − ρwater Vg = mobject g(1 − ) (2) ρobject The torque vector of the influence of water environment τh is composed of the torque vectors of the joint τA , τD and τF caused by the fluid acceleration force FA , the water resistance FD and the additional mass force FF . τh = τA + τD + τF

(3)

When the force needs to be controlled to perform underwater grasping tasks, the dynamical equation of the operating space is often more favorable. The dynamical equation of the underwater manipulator in the operating space is: M (q)X¨ + V (q, q˙ ) + p(q) + Ff + Fh = F − Fe

(4)

3 Underwater Impedance Control Strategy 3.1 Position-Based Impedance Control The position-based impedance controller is favorable for force tracking and can achieve a wide range of target impedances [14]. It consists of an outer impedance control loop which provides the desired target impedance, and an inner position control loop which provides the position tracking capability [15]. In the impedance model, the end of robot is equivalent to a mass-damping-spring system, and the environment is regarded as a spring system. Then the impedance control model is described as: Md (X¨ − X¨ d ) + Bd (X˙ − X˙ d ) + Kd (X − Xd ) = Fe − Fd

(5)

Among them, the contact force generated by the environment due to deformation is:  k (x − xe ), x ≥ xe (6) Fe = e 0, x < xe In the formula, ke is the contact stiffness of the environment;xe is the environment location, and x is the deformation position of the environment surface. When x < xe , the movement of the underwater gripper is in a free space; when x ≥ xe , the underwater gripper is in contact with the environment.

282

L. Lu et al.

3.2 Influence of the Underwater Unstructured Environment The grasping process includes a non-contact phase and a contact phase. In the noncontact phase, the contact force is 0, and the gripper is driven by the impedance controller according to the desired force. In the contact phase, the impedance controller contains the estimation of the environment information, the deviation of which will have an important effect on the controller. Now the problem of the estimation of the environment information is being analyzed, without losing generality, only one-dimensional model is analyzed. When the end-effector is in contact with the environment, the environment is regarded as a linear spring system. It can be obtained from Eq. (6): x = fe /ke + xe

(7)

Usually the desired position xd is set to a fixed value and ef = fd −fe is set. Therefore, the impedance control model can be expressed as: md e¨ f + bd e˙ f + (kd + ke )ef = md f¨d + bd f˙d + kd fd − kd ke (xd − xe )

(8)

When fd is a constant value, the steady-state force error is expressed as: efss =

kd fd [fd + ke (xe − xd )] = keq ( + xe − xd ) kd + ke ke

(9)

ke In the formula, keq = (kd−1 + ke−1 )−1 = kkdd+k , which is the equivalent contact e stiffness between the end-effector and the target object. Considering the estimated error of the environment parameter, the estimated error of environment position xe and stiffness ke , which is the difference between the actual value (xe ,ke ) and the estimated value (ˆxe ,kˆe ), is introduced:  xe = xe − xˆ e (10) ke = ke − kˆe

The estimated error ratio of environment stiffness α = obtained from Eq. (9): efss =

kˆe ke

is introduced. It can be

kd α−1 fd ) (ke xe + kd + ke α

(11)

It can be seen that in order to reduce the steady-state force error of impedance control, it is necessary to reduce the estimated errors of the environment parameter. Compared with the air environment, the underwater environment is complex and changeable, with factors such as water resistance, random disturbance of water flow, and buoyancy. Therefore, in the case of large external disturbance force, the impedance controller is described as: md (¨x − x¨ d ) + bd (˙x − x˙ d ) + kd (x − xd ) = fe − fd + fh

(12)

Impedance Control for Underwater Gripper

283

3.3 Underwater Impedance Controller Impedance Control Based on Parameter Identification. According to Eq. (11), in order to accurately track the contact force, impedance controller must update environment information in real time, and at the same time update the desired position in real time according to the updated information. Accurate control can be achieved when the following equation is satisfied. xd = fd /ke + xe

(13)

In underwater environment, if the reference coordinate system can be fixed to the coordinate system at the end of robot, the environment position can be obtained by the robot’s own sensors, such as angle encoders, force sensors, and so on. The force sensor judges whether the end-effector is in contact with the target object through the threshold value. If the contact force is greater than the set threshold value, the end of the robot is considered to be in contact with the environment. The two situations when the environment position is updated are shown in Fig. 2. Therefore, in order to reduce the contact force error caused by the position deviation, the position at the previous moment is taken as the environment position. x(t 1)

x(t 1)

x(t)

x(t)

Fig. 2. Two cases when the environment position is updated. (a) No contact at the previous moment. (b) Contact at the previous moment but the contact force is less than the threshold value.

According to Eq. (7), the environment stiffness is updated online in real time by using RLS according to the actual contact force and the updated environment position. Since the identified parameters may be variable, a forgetting factor is introduced to improve the identification ability of variable parameters and the convergence speed. The block diagram of the impedance control based on parameter identification is shown in Fig. 3. Adaptive Impedance Control. When underwater, the environment position measured by sensors is not accurate. Further, e = x − xd is set. If xe is the environment position updated by the sensor and xe is the actual position of the environment, the uncertainty of the environment position is expressed as δxe = xe − xe , and the deviation between the environment position and the end position is e = e + δxe . The disturbance force fh of the water environment is reflected on the force fe measured by the sensor. According to Eq. (12), the impedance controller is described as: md e¨  + bd e˙  + kd e = fe − fd

(14)

284

L. Lu et al.

Fig. 3. Block diagram of impedance control based on parameter identification.

When in the non-contact phase, the contact force fe is zero. At this time, the end of the robot is driven by the desired force to make contact happen. When in contact, the contact force is no longer 0. According to Eq. (11), if the target stiffness kd is set to zero, the steady-state force error will be zero. Therefore, the impedance model of the contact phase is described as: md e¨  + bd e˙  = fe − fd

(15)

In order to improve the robustness of the controller to the uncertainty of the desired force and environment position. xd = xe is set, and an adaptive compensation term is introduced to Eq. (15). Then the impedance control of the contact phase can be expressed as: md e¨  + bd (˙e + ) = fe − fd

(16)

In the formula,  as the compensation term is updated online according to the force error and its expression is: (t) = (t − λ) + η

fd (t − λ) − fe (t − λ) bd

(17)

Among them, η is the update rate, η > 0, and λ is the sampling time of the controller.

4 Simulation Studies 4.1 Impedance Controller Based on Parameter Identification The Simulink simulation model is built according to Fig. 3. The simulation results are shown in the Fig. 4(a). The rise time tr is approximately 0.5 s. The setting time ts is approximately 4 s. The maximum overshoot MP is approximately 0.8. It may therefore be concluded that the controller has a good force tracking ability and a certain resistance to external disturbance. The response speed is fast, but the overshoot of the contact force is large.

Impedance Control for Underwater Gripper

285

4.2 Adaptive Impedance Controller The details of the controller are changed, but the simulation environment is set to be the same as before. The simulation results are shown in the Fig. 4(b). The rise time tr is approximately 3 s. The setting time ts is approximately 11 s. The maximum overshoot MP is approximately 0.14. It may therefore be concluded that the controller has a good force tracking ability and a certain resistance to external disturbance. Compared with the last controller, the overshoot is greatly reduced, but the setting time is increased. 4.3 Comparison of the Above Two Controllers In fact, the response time of the position inner loop is relatively long due to the influence of the external factors of the underwater environment and the drive’s own factors. Therefore, in order to be closer to the actual situation, the response time of the position inner loop is prolonged, as shown in Fig. 4(c). Further, the simulation results are shown in the Fig. 4(d), which shows that after extending the response time of position inner loop, force tracking can still be realized by the impedance controller based on parameter identification, but it has not been realized by the adaptive impedance controller. Therefore, the impedance controller based on parameter identification is less sensitive to the response speed of position inner loop, and is more stable, so that it is more suitable for real underwater compliant grasping. 10

Mp

6

Force (N)

Force (N)

8 6

8

fd fe fh

4 2

0 0 tr 5

10

15

20 25 30 Time (s)

35

40

45

Mp

4 2

ts

fd fe fh

0 0

ts

tr

5

10

15

0.8

Froce (N)

position

Force (N)

fast slow

1.2

0.4 0

0

2

4 6 Time (s)

(c)

20 25 30 Time (s)

35

40

45

(b)

(a)

8

10

20 15 10 5 0 0 12 10 8 6 4 2 0 0

RLS

5

10

fd

fe(fast)

15

fe(slow)

20

25

30

20

25

30

adaptation

5

10

15 Time (s)

(d)

Fig. 4. Simulation results before and after adjusting the response speed of the position inner loop. (a) Impedance controller based on environmental parameter identification. (b) Adaptive impedance control. (c) The adjustment of the response speed of position inner loop. (d) Comparison of simulation results before and after adjustment.

286

L. Lu et al.

5 Experimental Studies In order to verify the underwater compliant grasping performance of the controller, an underwater experiment platform is built, as shown in Fig. 5. (a)

(b) Host computer

Bracket

Power supply Signal Controller Encoder

Sink

Motor

Transmission mechanism

Driver

Claw

Force sensor

Fig. 5. Underwater grasping experiment platform.

First, the grasping experiment of impedance controller based on parameter identification is carried out. The desired force is set to 3N at the beginning. The experiment results are shown in the Fig. 6, which displays that a good performance of force tracking can be presented by the controller in real underwater environments. Upon contact with the surface, the overshoot is approximately 0.08, which is much smaller than the simulation, and the force settles to 3N in approximately 2 s. During the steady the force is controlled to within ±4%. 4

fd

3.5

release

fe

Force (N)

3

contact-space

2.5 2

free-space

free-space

1.5 1

contact

0.5 0 0

5

10

15 20 Time (s)

25

30

35

Fig. 6. Experiment results of force tracking by underwater grasping.

Secondly, the experiment of introducing disturbance force to the above controller is carried out. During the grasping process, the disturbance of water flow is applied along the tangent direction and the normal direction of the force sensor respectively. The experiment results are shown in Fig. 7. The normal disturbance is applied at t =

Impedance Control for Underwater Gripper

287

20 s. Due to the small length of the sink in this direction, the water flow oscillates back and forth in this direction, resulting in a relatively large vibration of the actual force and a long duration, but it can still be adjusted to the desired force. The tangential disturbance is applied at t = 45 s, and the random disturbance is applied at t = 70 s. The results show that the controller is adjusted to the desired force. During the steady the force is controlled to within ±6%. Therefore, the designed controller can still achieve a good force tracking effect under the influence of external disturbance, and has good robustness. 6

fd fe

5

Force (N)

4

Ftangtial

release

3 2

Fnormal

Tangential Random Normal distrubance distrubance distrubance

1

contact 0

0

20

40

60 Time (s)

80

100

Fig. 7. Experiment results of underwater grasping under disturbance of water flow.

Finally, in the transition process of the grasping environment from underwater to water surface, the desired force will also change due to the change of buoyancy. The grasping experiment when the desired force is increased is performed on the above controller. At t = 6 s, the desired force is set to 3N; at t = 30 s, the desired force is increased to 4N; at t = 50 s, the desired force is increased to 5N. As shown in Fig. 8, the experiment results show that the designed controller can respond in time to the increase of desired force. 7 fd fe

6

Force (N)

5 4 3 2 1 0

0

10

20

30

40 50 Time (s)

60

70

80

Fig. 8. Experiment results of underwater grasping with adjusting the desired force.

288

L. Lu et al.

6 Conclusion Aiming at the problems of compliant grasping in underwater unstructured environments, an impedance controller based on parameter identification is designed in this paper. The influence of environment stiffness and position on steady-state force error is analyzed, and factors such as underwater environment and the response speed of position inner loop are taken into account. On the basis of the traditional position-based impedance control, the environment information is updated online in real time through RLS method that introduces a forgetting factor. Another adaptive impedance controller is also designed, but it cannot achieve force tracking when the response speed of the position inner loop caused by the underwater environment is slowed down. In order to verify the feasibility of the designed controller, simulations and experiments are carried out. The results prove that the controller has a good force tracking ability and a certain resistance to external disturbance in real underwater grasping tasks. This controller can be applied to the compliant grasping of underwater grrippers. Moreover, it can be extended to many situations involving force tracking and provides a more effective solution for compliant grasping. Acknowledgments. This work was supported in part by Guangdong Basic and Applied Basic Research Foundation (No. 2021A1515011717), and in part by the project under Grant No. 2019ZT08Z780, and in part by Dongguan Introduction Program of Leading Innovative and Entrepreneurial Talents, and in part by National Key R&D Program of China under grant number 2017YFC0821200.

References 1. Yin, Y.: Research on compliance control of robot. ROBOT 020(003), 232–240 (1998) 2. Hongan, N.: Impedance control: an approach to manipulation: part i—theory. J. Dyn. Syst. Meas. Contr. 107(1), 1–7 (1985) 3. Roveda, L., Vicentini, F., Pedrocchi, N., et al.: Impedance control based force-tracking algorithm for interaction robotics tasks: An analytically force overshoots-free approach. In: 12th International Conference on Informatics in Control, Automation and Robotics (ICINCO), pp.386–391. IEEE, Colmar, (2015) 4. Jamil, M., Jalani J., Ahmad A.: A new approach of active compliance control via fuzzy logic control for multifingered robot hand. In: 1st International Workshop on Pattern Recognition. pp. 1001111. SPIE, Tokyo (2016) 5. Kim, T., Kim, H.S., Kim, J.: Position-based impedance control for force tracking of a wallcleaning unit. Int. J. Precis. Eng. Manuf. 17(3), 323–329 (2016). https://doi.org/10.1007/s12 541-016-0040-x 6. Song, H.-C., Kim, Y.-L., Lee, D.-H., Song, J.-B.: Electric connector assembly based on vision and impedance control using cable connector-feeding system. J. Mech. Sci. Technol. 31(12), 5997–6003 (2017). https://doi.org/10.1007/s12206-017-1144-7 7. Franceschi, P., Castaman, N., Ghidoni, S., et al.: Precise robotic manipulation of bulky components. In: IEEE Access, pp 222476–222485 (2020) 8. Cieslak, P., Ridao, P.: Adaptive admittance control in task-priority framework for contact force control in autonomous underwater floating manipulation, In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 6646–6651. IEEE, Madrid (2018)

Impedance Control for Underwater Gripper

289

9. Seki, Y., Sagara, S., Ambar, R.: Impedance control of dual-arm 3-link underwater robot: in the case of grasping a fixed object lightly with one hand. In: International Conference on Information and Communication Technology Robotics, pp. 1–4. IEEE, Busan (2018) 10. Zhang, J., Liu, W., Gao, L., et al.: The master adaptive impedance control and slave adaptive neural network control in underwater manipulator uncertainty teleoperation. Ocean Engineering. Vol. 165, pp 465–479 (2018) 11. Zhang, J., Liu, W., Li, L., et al.: Adaption impedance control for underwater manipulator intelligent grasping in unknown environment. J. Shanghai Jiaotong Univ. (Chin. Ed.) 53(03), 341–347 (2019) 12. Dai, P., Lu, W., Le, K., et al.: Sliding mode impedance control for contact intervention of an IAUV: simulation and experimental validation. Ocean Engineering 196, 106855.1–106855.11 (2020) 13. Jung, S., Hsia, T., Bonitz, R.: Force tracking impedance control of robot manipulators under unknown environment. IEEE Trans. Control Syst. Technol. 12(3), 474–483 (2004) 14. Al-Shuka, H., Leonhardt, S., Zhu, W., et al.: Active impedance control of bioinspired motion robotic manipulators: an overview. Applied Bionics and Biomechanics 2018, 8203054–8203073 (2018) 15. Wang, T., Li, Y., et al.: A novel bilateral impedance controls for underwater teleoperation systems. Appl. Soft Comput. 91, 106194.1-106194.8 (2020)

Design and Kinematic Analysis of a Novel 7-DoF Dual-Arm Parallel Manipulator for 3C Products Assembly Xiansheng Yang, Zhenyu Zou, Jinmin Zhang, and Yunjiang Lou(B) Harbin Institute of Technology Shenzhen, Shenzhen 518055, China [email protected]

Abstract. Based on the field research and analysis of the 3C (Computers, Communications, and Consumer Electronics) products assembly processes, this paper proposed a novel seven degrees-of-freedom (DoF) dual-arm parallel manipulator for 3C products assembly. The proposed manipulator is consists of two cooperating parallel manipulators (PMs) with less DoF, which are a 4-DoF 3T1R (T: translation, R: rotation) PM and a 3-DoF 3R PM. The proposed manipulator has a redundant rotational pitch motion (rotational motion around a horizontal axis). The analysis of the position and workspace of the proposed manipulator is discussed in detail. In addition, a prototype of the proposed manipulator has been developed. The mobility verification and a computer mouse assembly experiment based on the prototype are conducted. Experiment results show that the proposed 7-DoF dual-arm parallel manipulator can be a prospective candidate to accomplish the 3C assembly tasks. onflies motion Keywords: Parallel manipulators · 3T1R · Sch¨ Kinematic analysis · 3C products assembly

1

· 3R ·

Introduction

Robot assembly has been applied to the automotive industry in the past decades [1]. However, the robot assembly has not been widely used in 3C products assembly. The processes of 3C products assembly are still mainly finished manually. With the increase of the global production of 3C products (e.g. mobile phones, tablets, and laptops) in the past decade, the shortage of labor and the rising labor cost become big issues. With the 3C products becoming increasingly integrated and precise, the manual assembly cannot simultaneously meet the requirements of precision and productivity gradually as well. Thus, the demand for the robot assembly in 3C products assembly is boosting. Applying robots to high-precision assembly applications is a hot topic for both researchers and the robot industry in recent years. Light-weight robots (e.g., UR This work was supported partially by the NSFC-Shenzhen Robotics Basic Research Center Program (No. U1713202) and partially by the Shenzhen Science and Technology Program (No. JSGG20191129114035610). c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 290–301, 2021. https://doi.org/10.1007/978-3-030-89092-6_27

Design and Kinematic Analysis of a Novel 7-DoF

291

(Universal Robot) [2] and KUKA LBR iiwa [3]) used to be regarded as a promising solution of high-precision assembly [4,5]. With collaborative control and embedded force control, lightweight robots can be applied to some easy processes of 3C products assembly. KUKA tried to apply LBR iiwa to 3C assembly [6]. However, the applications of LBR iiwa are limited to easy assembly processes because the single-arm LBR iiwa cannot meet the requirement of the orientational workspace. ABB released a dual-arm robot YuMi, which is the first commercial robot aiming to resolve the problem of small parts assembly [7]. ABB made a lot of attempts in 3C products assembly [8]. However, YuMi has not been accepted by the 3C products manufacturing industry due to its high price and low productivity. Kawasaki published a collaborative SCARA robot duAro1 last year [9]. duAro1 is a robot with 8-DoF and a large orientational workspace. It is promising in 3C products assembly because of its large orientational workspace and cost-effectiveness. The above-mentioned robots show that the dual-arm of robots can provide a large orientational workspace for 3C products assembly. However, the present available dual-arm robots are just simple combinations of two serial robots with six or seven DoF. The simple combination leads to the high price and the low productivity of the robots. Thus, the present available dual-arm robots cannot meet the requirements of a robot for 3C products assembly. Five-axis machines with two cooperating less DoF mechanisms (or robots) have been proved to be the best balance among efficiency, cost, dexterous, and precision [10–12]. A similar solution has been taken by the robot welding systems. By adding a 2-DoF or 3-DoF positioner, a robot welding system can enlarge the valid workspace, while maintaining accuracy and efficiency at the same time [13]. Therefore, comparing to a single six or redundant DoF serial or parallel robot, we divide the assembly robot into two less DoF robots, which can improve the precision and reduce the manufacturing cost simultaneously. In this paper, a novel 7-DoF dualarm PM for 3C products assembly, which is consisting of a 4-DoF 3T1R parallel manipulator (PM) and a 3-DoF 3R PM is proposed. The dual-arm PM has a redundant rotational pitch motion. In addition, based on analysis of the 3C products assembly processes, the redundant DoF is determined to be a rotational pitch motion (rotational motion around a horizontal axis). In this paper, we focus on the design and kinematic analysis of the proposed 7-DoF dual-arm PM. This paper is organized as follows. Section 2 presents the analysis of 3C products assembly processes and conceptual design of the 7-DoF dual-arm PM. Its inverse and forward kinematics, and workspace are analyzed in Sect. 3. Thereafter, mobility verification and a computer mouse assembly experiment based on a developed prototype are conducted in Sect. 4. Finally, this paper is summarized in Sect. 5.

2

Analysis of 3C Products Assembly Processes and Conceptual Design of the Manipulator

To apply the robot assembly to 3C products assembly, one suffers some challenges. Firstly, the assembly parts of 3C products (e.g., mobile phones, tablets,

292

X. Yang et al.

Fig. 1. 3C products assembly processes.

and laptops) are usually light-weight (e.g. a few grams to hundreds of grams), small size, and fragile. Secondly, the processes of 3C products assembly are usually limited to a confined transnational space due to the small size of 3C products. Thirdly, the 3C products assembly is very strict on the cost. Therefore, the requirements of a robot for the 3C products assembly include high precision, large orientational workspace, and high cost-effectiveness. To specify the requirements and the structure of a robot, we conduct indepth field research in a smartphone manufacturing factory as shown in Fig. 1, and analyze the 3C products assembly processes. According to the field research, the structure of a robot for 3C products assembly can be summarized as follows. a) A robot for 3C products assembly should consist of two different arms, which play different roles just like two human arms. b) The first arm has four to six DoF - three translational DoF and one to three rotational DoF. This arm conducts the main assembly movements. c) The second arm has three rotational DoF. This arm serves as an orienting mechanism. d) These two arms have redundancy in rotational motion, and the rotational pitch motion is particularly important for the 3C products assembly. e) The robot should achieve a balance between the large orientational workspace and the high productivity, and also be cost-effective. According to the above-mentioned descriptions, a robot for 3C products assembly is proposed in this paper, which is a dual-arm parallel manipulator shown in Fig. 2. The assembly robot is composed of a 4-DoF 3T1R PM and a 3-DoF 3R PM. It has redundancy in rotational pitch motion. The 4-DoF 3T1R PMs are also called Sch¨onflies-motion generators (SMGs) [14]. The SMGs are usually considered as the best choice to achieve an excellent cost-performance ratio for numerous pick-and-place and assembly tasks [15]. The blue part of Fig. 3 shows the Sch¨onflies-motion Parallel Manipulators with Rotational Pitch motion (SPMRP), which is inspired by the 3T1R 4-DoF PM with rotational pitch motion proposed in [16]. The SPMRP has the structure of 2PRPaRR-2PRPaR, which is consists of four hybrid sub-chains with the structure of RPaR. There are two sub-chains (l2 and l4 ) connected to the moving platform with two additional revolute joints. The SPMRP can generate a 4-DoF Sch¨ onflies-motion, which includes a 3-DoF translational motion and a 1-DoF

Design and Kinematic Analysis of a Novel 7-DoF

Fig. 2. CAD model of the proposed Dual-Arm Parallel Manipulator

293

Fig. 3. Schematic diagram of the SPMRP-SPM

rotational motion about the axes of the two added coaxial revolute joints P2 and P4 . The rotation motion of the SPMRP about the coaxial axis of revolute joints P2 , P3 , and P4 are decoupled, and the rotation motion of the SPMRP is only controlled by the l1 subchain. The 3-DoF 3R PMs are also called Spherical Parallel Manipulators (SPMs). The green part of Fig. 3 shows the SPM with the structure of 3PSS/S, which is proposed in [17]. The SPM consists of a fixed platform, a moving platform, three symmetric PSS sub-chains, and an intermediate passive sub-chain. The passive sub-chain is a spherical joint connecting the moving platform and the fixed platform. The motion of the 3-PSS/S SPM only depends on the passive sub-chain because the PSS sub-chains have six DoF and cannot constrain the moving platform. Since the passive sub-chain is composed of a spherical joint only, the SPM has three rotational DoF. For simplicity, the dual-arm parallel manipulator is consists of an SPMRP and an SPM can be abbreviated as SPMRP-SPM.

3

Kinematics Analysis

Figure 3 shows the schematic diagram of the SPMRP-SPM. The origin of the base frame O − XY Z of the SPMRP-SPM is defined at the intersection point of D1 D3 and D2 D4 , where D1 D3 is perpendicular to D2 D4 . The radius of the base is R = ODi  , i = 1, 2, 3, 4. The origin of the moving platform frame O − X  Y  Z  of the SPMRP is defined at the center of P2 P4 . Pi , i = 1, 2, 3, 4 is the center of the revolute joints connected to the moving platform corresponding to link li , i = 1, 2, 3, 4. The radius of the moving platform is r = O  P1  =

294

X. Yang et al.

O  P2  = O  P4 , and O  P1 is perpendicular to P2 P4 . Zi , i = 1, 2, 3, 4 is defined at the center of the revolute joint that is connected to the i−th prismatic actuator. Di Zi is parallel to the axis of the i−th prismatic actuator. The length of links li is defined as Li , i = 1, 2, 3, 4. On the base of the SPM, the three points A1 , A2 , A3 form an equilateral triangle with a centroid o . B1 , B2 , B3 are the spherical joints connected to the prismatic joints. The vector of A1 B1 , A2 B2 , and A3 B3 represent the prismatic joints axes. These three prismatic joints axes intersect at a point D, and the distance from point D to the rotation center o is H. The angle between Ai Bi and DO is θi for i = 1, 2, 3. di denotes the distance from Bi to D for i = 1, 2, 3. di is also the input variable of the prismatic actuator. Three spherical joints C1 , C2 , C3 are mounted on the moving platform, making an equilateral triangle with a centroid o. h denotes the distance from o to the spherical joints’ rotation center o of the passive sub-chain. The radiuses of the fixed platform and the moving platform are R and r , respectively. The length of the passive link between the two spherical joints is denoted as L . The base frame o − xyz of the SPM is defined at the center of the spherical joint of the passive sub-chain, the o − xyz has the same direction with O − XY Z. The distance between O and o along Z−axis or z−axis is t = Oo. The origin of the moving platform frame o − xyz is at the moving platform’s center o . The y  -axis has same direction with C1 C2 , and the z  -axis is perpendicular to the plane of the moving platform and is oriented upward. From the description of the SPMRP-SPM as shown above, the transformation from the base frame o − xyz of the SPM to the base frame O − XY Z of the SPMRP-SPM can be represented by   0 T = I3×3 0t . (1) 0

1

Thus, in order to formulate the kinematic model of the SPMRP-SPM, we firstly formulate the kinematic model of the SPMRP respect to the the base frame O−XY Z, and the kinematic model of the SPM respect to the base frame o−xyz of the SPM, respectively. Then apply the transformation T to the kinematic model of the SPM. 3.1

Kinematic Analysis of the SPMRP

Inverse Kinematic. The inverse kinematics problem of a manipulator refers to—given the pose of the moving platform of (x, y, z, β), how to determine the prismatic actuation input zi , i = 1, 2, 3, 4.

Design and Kinematic Analysis of a Novel 7-DoF

295

The position of pi , i = 1, 2, 3, 4, in the moving platform frame O − X  Y  Z  can be expressed as ⎛ ⎞ ⎛ ⎞ r 0   o p1 = ⎝ 0 ⎠ , o p2 = ⎝ r ⎠ 0 0 ⎛ ⎞ ⎛ ⎞ (2) 0 0   o p3 = ⎝ 0 ⎠ , o p4 = ⎝ −r ⎠ . 0 0 The rotation matrix from the moving base frame O − XY Z is ⎡ cos β o ⎣ 0 o T = − sin β

platform frame O − X  Y  Z  to the ⎤ 0 sin β 1 0 ⎦. 0 cos β

(3)

The position of pi , i = 1, 2, 3, 4, in the base frame O − XY Z can be obtained as o o

pi = oo T

o

o

pi + O  ,

(4)

T

where O = (x y z) is the position of the moving platform’s center in base o frame O − XY Z. O coincides with p3 . The position of the prismatic joint center zi in the base frame is ⎛ ⎞ ⎛ ⎞ R 0 o z1 = ⎝ 0 ⎠ , oz2 = ⎝ R ⎠ z1 z2 ⎞ ⎞ ⎛ ⎛ (5) −R 0 o z 3 = ⎝ 0 ⎠ , o z 4 = ⎝ −R ⎠ . z3 z4 Since the length of li is constants, we have || o pi − o zi || = Li , i = 1, 2, 3, 4,

(6)

which can be rewritten as: (r cos β + x − R)2 + y 2 + (z − r sin β − z1 )2 = L21 x + (r + y − R) + (z − z2 ) = 2

2

2

(x + R) + y + (z − z3 ) = 2

2

2

x + (−r + y + R) + (z − z4 ) = 2

2

2

L22 L23 L24 .

(7) (8) (9) (10)

The inverse kinematics of the SPMRP can be obtained by solving the Eqs. (7)–(10) as

296

X. Yang et al.

z1 = ± L21 − (r cos β + x − R)2 − y 2 + z − r sin β z2 = ± L22 − x2 − (r + y − R)2 + z z3 = ± L23 − (x + R)2 − y 2 + z z4 = ± L24 − x2 − (−r + y + R)2 + z.

(11)

There are a total of 16 solutions for the inverse kinematics. In this study, we choose the solution that all the four equations in (11) are calculated based on positive roots. Forward Kinematic. The forward kinematics problem of a manipulator refers to—given the prismatic actuation input zi , i = 1, 2, 3, 4, how to determine the pose of the moving platform of (x, y, z, β). From Eqs. (8) and (10), we have the solution of y as y = mz + n, (12) where z2 − z 4 , 2(r − R) z 2 − z22 + l22 − l42 . n= 4 4(r − R) m=

From Eqs. (8), (9) and (12), the solution of x can be obtained as x = kz + j,

(13)

where

m(r − R) − z2 + z3 R r2 − 2rR + 2n(r − R) + z22 − z32 + l32 − l22 . j= 2R Substituting Eqs. (12) and (13) into Eq. (8) leads to k=

az 2 + bz + c = 0. Then the solution of z can be written as √ −b ± b2 − 4ac z= , 2a where

a = k 2 + m2 + 1 b = 2kj + 2(r − R + n)m − 2z2 c = j 2 + (r − R + n)2 + z22 − L22 .

(14)

(15)

Design and Kinematic Analysis of a Novel 7-DoF

297

With the solution of y, x and z can be obtained from Eqs. (12), (13), and (15). Substituting y, x, and z into Eq. (7), we have the solution of β as β = 2 tan−1 (t), where A±

(16)

√

A2 + B 2 − C 2 B+C A = 2r (z1 − z) , B = 2r(x − R) t=

C=

L21

(17) (18) 2

− y − r − (x − R) − (z − z1 ) . 2

2

2

(19)

The forward kinematics can be obtained from Eqs. (12), (13), (15), and (16). There are two solutions for z and β, respectively. Thus, there are a total of 4 solutions for the forward kinematics. In this study, we choose the solution that both (15) and (17) are calculated based on negative roots. Workspace. The geometric parameters of the PSMDP in this section are set based on the assembly system requirements. We set r = 70 mm, R = 200 mm, Li = 400 mm, i = 1, 2, 3, 4, respectively. Figure 4 shows the reachable workspace of the SPMRP. The shape of X − Y cross-section of the reachable workspace is close to a trapezoid, and also symmetrical to the X− axis. The reachable workspace with a regular shape is suitable to the industrial applications.

Fig. 4. The reachable workspace of the SPMRP.

3.2

Fig. 5. The workspace of the SPM

Kinematic Analysis of the SPM

Inverse Kinematic. The inverse kinematic and forward kinematic of the SPM are demonstrated in this subsection. It is assumed that inputs of actuators are zero when orientation angles (i.e. α, β  , γ) of the SPM are all zero. In the base frame 0 − xyz of the SPM, the position of Bi can be expressed as Bi = [di sin θi cos ηi di sin θi sin ηi

T

− di cos θi − H]

where ηi is the angle between o Ai and the X-axis. One has η1 = and η3 = 2π 3 .

(20) −2π 3 ,

η2 = 0,

298

X. Yang et al.

In the moving coordinate system, the positions of the spherical joints mounted on the moving platform are T

(21) Cim = r cos ξi r sin ξi 0 π where ξi is the angle between oCi and the x-axis. One has ξ1 = −π 3 , ξ2 = 3 , ξ3 = −π. The position vector of Ci in the fixed coordinate system can be expressed as oCi = oo + o Ci . The position of Ci in the fixed coordinate system is T

Ci = R[r cos ξi r sin ξi 0] + R[0 0 h]T

(22)

According to the geometry of the 3-PSS/S SPM, the distance between the center of the spherical joint of each prismatic joints and the center of the spherical joint of the moving platform is the length of the passive link, and the constraint equation can be expressed as |Ci − Bi | = L

(23)

One can solve Eq. (23) and obtain  −K + K 2 − 4Pi (Ri + H 2 + 2Hεi (3)) di = 2Pi K = Qi − 2μi (3)H 2

(24) (25)

2

2

Pi = μi (1) + μi (2) + μi (3)

(26)

Qi = −2(μi (1)εi (1) + μi (2)εi (2) + μi (3)εi (3))

(27)

Ri = εi (1) + εi (2) + εi (3) − L

(28)

2

2

2

2

where μi (j) represents the j-th component of the unit vector along the line connecting the point D and the spherical joint center Bi on the i-th sub-chain. εi (j) represents the j-th component of the coordinate vector of Ci . The inverse kinematic of the 3-PSS/S SPM can be obtained based on the Eqs. (24–28). Forward Kinematic. The forward kinematic of the 3-PSS/S SPM is a set of multi-parameter nonlinear equations. Based on the inverse kinematics Eq. (24), the following nonlinear equations with three unknown parameters (α, β, γ) can be obtained Fi (α, β, γ) = Pi d2i + (Qi − 2μi (3)H)di + (Ri + H 2 + 2Hεi (3))

(29)

Iterative methods can be used to solve Eq. (29) and the initial values of the iteration methods can be determined according to the positions of the prismatic joints. Workspace. One can determine whether a position is in the workspace of the 3-PSS/S SPMs or not by considering the constraint of the spherical joint and the interference constraint of the passive link. According to the size of 3C products, we set the structural parameters of theSPM to r = 100 mm, L = 300 mm, h = 20 mm, H = 40 mm, θ = 35◦ . We can obtain the workspace of the 3-PSS/S SPM as shown in Fig. 5.

Design and Kinematic Analysis of a Novel 7-DoF

4

299

Prototype Experiments

In order to validate the proposed manipulator design, a prototype has been developed. The mobility of the SPMRP-SPM has been verified on the prototype. In addition, a computer mouse assembly experiment has been conducted to verify the SPMRP-DP can be a prospective candidate to accomplish the 3C assembly tasks. 4.1

Prototype Development

Figure 6 shows the prototype of the SPMRP-SPM. The prototype consists of four subsystems (i.e. an SPMRP, an SPM, prismatic actuators (ball screw actuators and linear motors), and a control system). The prismatic actuators of the SPMRP are driven by four ball screw actuators, which are mounted on the side surface of the base. The model of the ball screw actuator is TOYO GTH5-L10400-BC-P10B-C4, which is driven by a Panasonic MSMD012G1V servo motor. The specification of the ball screw actuator is listed as follows: stroke is 400 mm, ball screw lead is 10 mm, the maximum speed is 500 mm/s, and repeatability is ±0.005 mm. The prismatic actuators of the SPM are driven by three linear motors, which are mounted on the side surface of the base. The linear motors are with high precision (0.5 mum for repeatability), large acceleration (70 m/s2 ), and compact size (296 mm × 62.5 mm × 68 mm for 180 mm displacement). The control system includes a motion control card (Googol Technology GTS-800PVPCO-A) installed in an industrial computer, drivers for the servo motors and linear motors, and sensors. With the control system, the moving platform of the prototype can achieve the desired motions.

Fig. 6. Prototype of the SPMRP-SPM

Fig. 7. The processes of a computer mouse assembly experiment.

300

4.2

X. Yang et al.

A Mouse Assembly Experiment

Figure 7 shows the processes of a computer mouse assembly experiment. The assembly experiment also verifies the SPMRP-DP can be a prospective candidate to accomplish the 3C assembly tasks. In addition, a supplemental video is provided to demonstrate experiments, including mobility tests, coordinated motion of the dual-arm parallel manipulator, the trajectory acquiring process, and assembly experiment of the computer mouse assembly.

5

Conclusions

Based on the field research and analysis of the 3C products assembly processes, we concluded five requirements for the robot assembly of 3C products, which is pretty suggestive for the 3C products assembly robot design. Thereafter, a 7-DoF dual-arm parallel manipulator–SPMRP-SPM is proposed in this paper, which is consists of an SPMRP and an SPM. The SPMRP-SPM has a redundant rotational pitch motion. The inverse and forward kinematics, and workspace have been analyzed in this paper. The mobility verification and a computer mouse assembly experiment based on a developed prototype are conducted. Experiment results show that the proposed SPMRP-SPM can be a prospective candidate to accomplish the 3C assembly tasks. Our future work will focus on coordinated motion control for high-precision assembly operations.

References 1. Michalos, G., Makris, S., Papakostas, N., Mourtzis, D., Chryssolouris, G.: Automotive assembly technologies review: challenges and outlook for a flexible and adaptive approach. CIRP J. Manuf. Sci. Technol. 2(2), 81–91 (2010). https://doi. org/10.1016/j.cirpj.2009.12.001. Jan 2. Collaborative robotic automation: Cobots from Universal Robots. https://www. universal-robots.cn/en/ 3. LBR iiwa — KUKA AG. https://www.kuka.com/en-us/products/roboticssystems/industrial-robots/lbr-iiwa 4. Fast-Berglund, ˚ A., Palmkvist, F., Nyqvist, P., Ekered, S., ˚ Akerman, M.: Evaluating cobots for final assembly. Procedia CIRP 44, 175–180 (2016). https://doi.org/10. 1016/j.procir.2016.02.114 5. Teiwes, J., B¨ anziger, T., Kunz, A., Wegener, K.: Identifying the potential of human-robot collaboration in automotive assembly lines using a standardised work description. In: 2016 22nd International Conference on Automation and Computing (ICAC), pp. 78–83, September 2016. https://doi.org/10.1109/IConAC.2016. 7604898 6. KUKA.ForceTorqueControl. https://www.kuka.com/en-de/products/robotsystems/software/application-software/kuka-forcetorquecontrol 7. Kock, S., et al.: Robot concept for scalable, flexible assembly automation: A technology study on a harmless dual-armed robot. In: 2011 IEEE International Symposium on Assembly and Manufacturing (ISAM), pp. 1–5. IEEE, Tampere, Finland, May 2011. https://doi.org/10.1109/ISAM.2011.5942358

Design and Kinematic Analysis of a Novel 7-DoF

301

8. Hedelind, M., Kock, S.: Requirements on flexible robot systems for small parts assembly: a case study. In: 2011 IEEE International Symposium on Assembly and Manufacturing (ISAM), pp. 1–7, May 2011. https://doi.org/10.1109/ISAM.2011. 5942356 9. duAro1 — Dual Arm Scara Robot. http://robotics.kawasaki.com/en1/products/ robots/dual-arm-scara/duAro1/?language id=1 10. Bohez, E.L.J.: Five-axis milling machine tool kinematic chain design and analysis. Int. J. Mach. Tools Manuf. 42(4), 505–520 (2002). https://doi.org/10.1016/S08906955(01)00134-1. Mar 11. Refaat, S., Herv´e, J.M., Nahavandi, S., Trinh, H.: Asymmetrical three-DOFs rotational-translational parallel-kinematics mechanisms based on Lie group theory. Eur. J. Mech A/Solids 25(3), 550–558 (2006). https://doi.org/10.1016/j. euromechsol.2005.11.001. May 12. ECOSPEED F HT 2. https://www.starrag.com/en-us/machine/ecospeed-f-ht-2/ 146 13. Kim, D.W., Choi, J.S., Nnaji, B.O.: Robot arc welding operations planning with a rotating/tilting positioner. Int. J. Prod. Res. 36(4), 957–979 (1998). https://doi. org/10.1080/002075498193471. Apr 14. Angeles, J., Caro, S., Khan, W., Morozov, A.: Kinetostatic design of an innovative Sch¨ onflies-motion generator. Proc. Inst. Mech. Eng. Part C J. Mech. Eng. Sci. 220(7), 935–943 (2006). https://doi.org/10.1243/09544062JMES258. Jul 15. Schreiber, L.T., Gosselin, C.: Sch¨ onflies motion PARAllel robot (SPARA): a kinematically redundant parallel robot with unlimited rotation capabilities. IEEE/ASME Trans. Mech. 24(5), 2273–2281 (2019). https://doi.org/10.1109/ TMECH.2019.2929646. Oct 16. Kim, S.M., Kim, W., Yi, B.J.: Kinematic analysis and design of a new 3T1R 4-DOF parallel mechanism with rotational pitch motion. In: 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 5167–5172. IEEE, St. Louis, MO, October 2009. https://doi.org/10.1109/IROS.2009.5354216 17. Hou, Y., Hu, X., Zeng, D., Zhou, Y.: Biomimetic shoulder complex based on 3PSS/S spherical parallel mechanism. Chin. J. Mech. Eng. 28, 1–9 (2014). https:// doi.org/10.3901/CJME.2014.0820.137

Multi-objective Lightweight Design for the Forearm of a Palletizing Robot Ying He, Han Gao, Chao Ma, Yihui Xiao, and Dan Wang(B) Department of Mechanical Engineering, Tianjin Renai College, Tianjin 301636, China

Abstract. The finite element model of palletizing robot’s forearm is established, and its accuracy is verified by modal test. Based on the static analysis considering dynamic factors, constrained modal analysis and vibration response tests, the optimization objectives are to minimize mass, minimum maximum displacement, maximum first and second order natural frequency and minimum maximum stress, and the structural parameters are taken as design variables. The approximate model of objective function is established by using Box-Behnken and RSM methods. A composite weight coefficient is proposed, which is constructed by the analytic hierarchy process based on the finite element analysis and the reciprocal of the sub-objective optimal values. NSGA-II algorithm is used to obtain the optimal solution, and then the optimal model of the forearm is obtained. The comparative analysis shows that the mass is reduced by about 5.3% when the first two natural frequencies are increased, the maximum stress is reduced and the maximum displacement is still satisfies the allowable deflection, which verifies the effectiveness of the optimization design method. Keywords: Palletizing robot · Forearm · Multi-objective lightweight · Static analysis · Modal analysis · Vibration response test · Composite weight coefficient

1 Introduction High speed and heavy load palletizing robots have been widely used in modern production and logistics operations. The mechanical arm is an important functional part of palletizing robot. In the working process of the robot, the robot arm often needs to carry out complex movements such as frequent start and stop, acceleration and deceleration, expansion and rotation. Therefore, in the design process of the robot arm, its strength, stiffness and vibration stability are important indicators which need to be guaranteed. And the mass should be as small as possible to improve the dynamic characteristics of the robot system and to reduce energy consumption. This brings about the structural optimization problem of the robot arms. Supported by Tianjin Municipal Education Commission Scientific Research Project of China (Grant no. 2018KJ269). HE Ying, (1980-), Male, Doctor, Associate Professor of Tianjin Renai College, China. His research interests include integrated optimal design and control of Industrial robots. © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 302–315, 2021. https://doi.org/10.1007/978-3-030-89092-6_28

Multi-objective Lightweight Design for the Forearm

303

A lot of researches have been done on the structural optimization of robot arms. Literature [1–8] refers to the optimization design of robots with low working speed and light load, which is mainly based on static analysis, with strength and stiffness as constraint conditions and minimum mass as the goal. However, for the palletizing robot working at high speed and heavy load, the dynamic forces cannot be ignored. In the working process of the palletizing robot, it will be affected by dynamic excitation forces such as inertial force, joint force and gravity. When the frequency of the excitation forces are close to or reach the natural frequency of the system and continues to act, strong vibration will be generated. Therefore, the analysis and optimization based only on the statics level is not enough here, and the research needs to be carried out at the dynamics level. Literature [9, 10] refers to the study of dynamic analysis, considering the natural frequency of the structure. However, they do not take natural frequency as the factor of optimal design; Literature [11, 12], although they take natural frequency as a factor to participate in the optimization design, they choose the first natural frequency as a factor to consider based on experience, that means, there is no in-depth discussion on the selection basis of natural frequency order. In this paper, static analysis and dynamic analysis are combined, and the order of natural frequencies involved in the optimal design is determined by modal analysis and vibration response test. The Pareto solution set satisfying the optimization index can be obtained through multi-objective optimization solution. It is necessary to select the most satisfactory optimization scheme from the Pareto solution set by constructing the weight coefficient of the optimization index. In this paper, a compound weight coefficient is proposed, which is constructed by the Analytic Hierarchy Process (AHP) [13–15] based on the finite element analysis and the reciprocal of the sub-objective optimal values. The traditional analytic hierarchy process mainly relies on the experience of experts in constructing the comparison matrix, therefore it is subjective to some extent [16–18]. In order to improve the objectivity of the comparison matrix, the importance of each optimization objective is investigated based on the results of finite element analysis and vibration response test. And considering that the order of magnitude of each optimization objective is very different, it is easy to affect the proportion of each sub-objective in the comprehensive objective function. Therefore, the reciprocal of the optimal value of each sub-objective is obtained and is used as the component of the weight coefficient to achieve the purpose of unified order of magnitude.

2 Palletizing Robot Structure Introduction The palletizing robot is a 4-DOF joint robot, as shown in Fig. 1. Maximum load: 120 kg, maximum turning radius: 2.4 m, maximum waist turning speed 85 °/s. Literature [19] gives a detailed introduction of the mechanism. Here, it is omitted and only the names of components involved in this paper are pointed out. The forearm is respectively hinged with the forearm driving connecting rod, the big arm and the end-effector bracket. The forearm driving motor drives the forearm driving arm through the RV reducer, and then the forearm driving connecting rod realizes the movement of the forearm.

304

Y. He et al.

3 Multi-objective Lightweight Design of the Forearm 3.1 Establishment of Finite Element Model of Forearm The 3D model of forearm is shown in Fig. 2. Mass is 34.688 kg. The working length of the forearm L = 1200 mm, allowable deflection [δ] = L/1000. Material properties are as follows: Material: ZL110, Modulus of elasticity: 6.89 × 1010 N/m2 ; Poisson’s ratio: 0.35; Shearing modulus: 3.189 × 108 N/m2 ; Density: 2770 kg/m3 ; Tension strength: 1.65 × 108 N/m2 . Meshing: using tetrahedral grid, the maximum unit size: 31.6454 mm; Minimum unit size: 6.32909 mm; Number of nodes: 25860; Total number of units: 12881. The obtained finite element model is shown in Fig. 3. Horizontal retaining link

Forearm drive link

Forearm Forearm drive motor Big arm End-effector bracket Waist bracket

Forearm drive arm

Engine base

Fig. 1. Palletizing robot model.

Fig. 2. 3D model.

Fig. 3. Finite element model.

3.2 Free Modal Analysis and Verification of the Finite Element Model In order to verify the accuracy of the finite element model of the forearm, the modal tests are compared with the free modal calculated by the finite element method. The modal test equipment is shown in Table 1. The test method of single point excitation and multi-point response is adopted, and the test principle is shown in Fig. 4. In order to measure the free modal of the forearm and make it approximately under the “free” boundary condition, the forearm is suspended by elastic rope with great flexibility. Table 1. Experimental equipment. Equipment name

Type

Equipment name

Type

Vibration and noise data acquisition system

LMS SC305

Vibration and noise analysis system

LMS TEST.LAB

Impact hammer

B&K 8207

Acceleration sensor

Lance LC0152T

Elastic rope

–

Computer

IBM

Multi-objective Lightweight Design for the Forearm

305

5 4

6 2 LMS SC305

3

1

7

1. Computer 2. LMS SC305 3. Impact Hammer 4. Spreader 5. Elastic rope 6. Acceleration sensor 7. Forearm

Fig. 4. Test principle.

Comparison results of calculated modal and test modal are shown in Fig. 5 and Table 2. It can be seen from the comparison results that the first three modals of the calculated modal and the test modal are consistent, and the relative errors of the calculated modal and the test modal frequency are all less than 5%, which indicates that the accuracy of the established finite element model meets the requirements and can be used for subsequent simulation calculation. 3.3 Static Analysis of the Forearm The finite element model is used for static analysis considering the dynamic factors. Apply Displacement Constraints. According to the assembly relationship of the forearm, cylindrical constraints are applied at the hinge holes between the forearm and the forearm driving connecting rod and between the forearm and the big arm, as shown in Fig. 6. Apply Load. Motion simulation combined with five non-uniform B-spline motion rules [20] is used to simulate the palletizing robot’s motion, and then the maximum value (1978.2 N) and direction of the force received at the hinge hole between the forearm and the end-effector bracket are obtained, as shown in Fig. 6. Static Analysis and Solution. Static analysis results are shown in Figs. 7 and 8. The maximum stress of the forearm is 23.176 MPa, which is far less than the allowable stress of the material. The maximum displacement is about 0.891 mm, less than the allowable deflection [δ] = 1.2 mm. It shows that the forearm has the potential of lightweight design.

3.4 Dynamic Performance Analysis of the Forearm In order to determine the dynamic performance optimization objective, the constrained modal analysis and vibration response test are carried out.

306

Y. He et al.

(a) Comparison between the test and FEM modal for the first modal shape.

(b) Comparison between the test and FEM modal for the second modal shape.

(c) Comparison between the test and FEM modal for the third modal shape. Fig. 5. Comparison of test modal and calculated modal. Table 2. Comparison of test modal and calculated modal frequency. Modal order

Test modal frequency (Hz)

FEM modal frequency (Hz)

Relative error %

1

271.81

281.63

3.49

2

323.86

324.14

0.09

3

432.53

426.24

1.48

Constrained Modal Analysis. The application of displacement constraints is the same as the statics analysis. The first four natural frequencies are obtained, as shown in Table 3.

Multi-objective Lightweight Design for the Forearm

307

Fig. 6. Displacement constraint and load.

Fig. 7. Stress nephogram.

Fig. 8. Deformation nephogram.

Table 3. The first four natural frequencies of the forearm. Modal order

1

2

3

4

Frequency (Hz)

90.22

99.77

326.83

407.34

Vibration Response Test. The frequency spectrum of the excitation force received by the forearm in the process of working is measured by vibration response test. The test equipment are basically the same as the modal test (no need Impact Hammer and Elastic rope). Because the vibration of the body is transmitted to the forearm through bolt, the center of the end cover bolt sets with 6 hinge holes on the forearm are selected as the test points (points #1–#6), as shown in Fig. 9. Data acquisition parameter setting: sampling time is 30 s, bandwidth is 1024 Hz, and resolution is 0.125 Hz. Due to limitation of pages, only the spectrum of excitation force of #1 and #2 are given here, as shown in Fig. 10. It can be seen that the excitation force frequency corresponding to the peak acceleration differs greatly from the first four natural frequencies of the forearm, and the vibration stability of the forearm is good. However, considering the low value of the first two natural frequencies of the forearm, which indicates that the structural stiffness of the forearm is low. Therefore, the maximum of the first and second order natural frequencies are taken as the optimization objective to further improve the overall stiffness.

308

Y. He et al.

Fig. 9. Vibration excitation test point location.

20 0 0

500 1000 1500 Frequency (Hz) (a) #1 -x

X: 388.4 Y: 105.1

100 X: 544.3 Y: 95.71

50 0 0

2000

500 1000 1500 Frequency (Hz) (b) #1 -y

Acceleration (g)

60 X: 388.3 Y: 61.69

40 20 0 0

500 1000 1500 Frequency (Hz) (d) #2 +x

2000

X: 387.6 Y: 42.18

40 20 0 0

2000

100

80

Acceleration (g)

60

Acceleration (g)

X: 544.3 Y: 43.56

500 1000 1500 Frequency (Hz) (c) #1 +z

2000

60

Acceleration (g)

40

150 X: 388.3 Y: 37.26

Acceleration (g)

Acceleration (g)

60

X: 388.3 Y: 87.04 X: 720.1 Y: 64.85

50

0 0

500 1000 1500 Frequency (Hz) (e) #2 -y

X: 387.6 Y: 52.04

40 20 0 0

2000

500 1000 1500 Frequency (Hz) (f) #2 -z

2000

Fig. 10. Excitation force frequency spectrum of #1 and #2 test points.

x1

x12

x11

x10 x9

x8

x7

x6

x5

x4

x3

x2

Fig. 11. Indicator of forearm design variables.

3.5 Establishment of Forearm Optimization Design Model Selection of Design Variables. According to the structural characteristics of the forearm, 12 structural parameters X = (x 1 ,x 2 ,…,x 12 ) are selected, as shown in Fig. 11. Names, initial values and value ranges of structural parameters are shown in Table 4. These are structural parameters that do not need to be machined, therefore, their variation has little effect on manufacturing difficulty and cost.

Multi-objective Lightweight Design for the Forearm

309

Table 4. Initial value and range of design variables (mm) Design variable Variable name

Initial value Value range

x1

Thickness of front vertical plate

10

6–10

x2

Height of square hole of front end vertical plate

70

70–105

x3

width of square hole of front end vertical plate

60

60–76

x4

Thickness of middle vertical plate

10

6–10

x5

Height of square hole in middle vertical plate

80

40–120

x6

Width of square hole middle vertical plate

80

40–120

x7

Thickness of rear end vertical plate

10

6–10

x8

Thickness of front and middle end stiffened plate and boss

10

6–10

x9

Height of front and middle end stiffened plate and boss

20

10–20

x 10

Height of rear end plate and boss

20

10–20

x 11

Height of square hole rear end vertical plate

150

150–180

x 12

Width of square hole rear end vertical plate

120

120–140

Establishment of Objective Function. The optimization objectives are to minimize mass F m (X), minimum maximum displacement F δ (X), maximum first and second order natural frequency F f 1 (X), F f 2 (X) and minimum maximum stress F σ (X), therefore, the objective function are: ⎫ Fm (X ) = min m(X ) ⎪ ⎪ X∈ D ⎪ ⎪ ⎪ ⎪ Fδ (X ) = min δmax (X ) ⎪ ⎪ ⎪ X∈ D ⎪ ⎬ Ff 1 (X ) = max f1 (X ) (1) X∈ D ⎪ ⎪ ⎪ Ff 2 (X ) = max f2 (X ) ⎪ ⎪ ⎪ ⎪ X∈ D ⎪ ⎪ ⎪ Fσ (X ) = min σmax (X )⎭ X∈ D

According to the value range of the design variables and the Box-Behnken design of experiments [21], a total of 193 test design matrices are obtained. The mass m, the maximum displacement δ max , the first natural frequency f 1 , the second natural frequency f 2 , and the maximum stress σ max are calculated respectively. The test design matrices and results are obtained as shown in Table 5. The response surface model [22] of the five optimization objective functions m(X), δ max (X), f 1 (X), f 2 (X) and σ max (X) can be obtained by using the experimental design matrix in Table 5.

310

Y. He et al. Table 5. Test design matrix.

Variable

No. 1

No. 2

No. 3

…

No. 193

x1

6

6

10

…

8

x2

70

35

35

…

70

x3

55

55

55

…

55

x4

8

8

8

…

8

x5

80

40

40

…

80

x6

80

80

80

…

80

x7

6

6

10

…

8

x8

8

8

8

…

8

x9

10

20

20

…

20

x 10

25

25

25

…

25

x 11

75

127.5

127.5

…

127.5

x 12

110

110

110

…

110

m

31.8239

32.991

33.207

…

32.9825

δ max

0.93556

0.92113

0.91726

f1

91.94271

90.25483

90.77201

…

90.65137

f2

100.18095

98.98554

99.19595

…

99.41839

σ max

23.478

23.493

23.142

…

23.313

0.92088

Constraint Condition. Dimensional range constraints of each design variable: Variables x 2 ,x 3 ,x 5 ,x 6 ,x 11 and x 12 are the size of the holes on the forearm. The larger the hole is, the lighter the weight is. Therefore, the initial value of the size is taken as the minimum value, and the maximum value is selected based on the principle of not destroying the surrounding materials. Variables x 1 ,x 4 ,x 7 ,x 8 ,x 9 and x 10 are the thickness and height dimensions of each plate structure on the forearm. The smaller these dimensions are, the lighter the mass is. Therefore, the initial value of each dimension is taken as the maximum value, and the minimum value is selected according to the minimum thickness of the cast aluminum alloy or the structural boundary condition [23], as shown in Table 4. Determination of Weight Coefficient. A composite weight coefficient is proposed, which consists of the product of ωI , the coefficient obtained by AHP based on finite element results, and ωII , the reciprocal of the optimal value of a single objective. Determine the coefficient ωI by the analytic hierarchy process based on the finite element results. (a) Establish a hierarchical structure model. Generally a hierarchical structure model includes three-level: goal level, criteria level and policy level as shown in Fig. 12.

Multi-objective Lightweight Design for the Forearm

311

Multi-objective optimization (A)

Mass (B1)

Maximum displacement (B2)

x1(C1)

The first natural frequency (B3)

The second natural frequency (B4)

Maximum stress

(B5)

x12 (C12)

x3 (C3)

x2 (C2)

Fig. 12. Hierarchical structure model. Table 6. Comparison of influence degree of each factor. A

B1

B2

B3

B4

B5

B1

1

3

5

7

9

B2

1/3

1

2

4

6

B3

1/5

1/2

1

2

4

B4

1/7

1/4

1/2

1

2

B5

1/9

1/6

1/4

1/2

1

(b) Construct the comparison matrix. Mass, maximum displacement, the first natural frequency, the second natural frequency and maximum stress are expressed by B1 , B2 , B3 , B4 , and B5 respectively. According to the results of finite element analysis, the importance of each performance index is investigated. The main objective of this study is the lightening of the forearm, therefore, mass is the main objective. According to the statics analysis, the maximum displacement is less than but close to the allowable deflection. In order to reduce the impact of deformation on the positioning accuracy of the palletizing robot, the importance of displacement ranks the second. According to the constrained modal analysis, the first and second order natural frequencies are relatively small. In order to improve the structural stiffness of the forearm, the first and second order natural frequencies rank the third and fourth in importance. The maximum stress is much less than the allowable stress of the material and therefore comes last in importance. Accordingly, the pairwise comparison results of each factor in the criterion layer are shown in Table 6, and the comparison matrix Eq. (2) is obtained. ⎛ ⎞ 1 3 5 7 9 ⎜ 1/3 1 2 4 6 ⎟ ⎜ ⎟ ⎜ ⎟ (2) A = ⎜ 1/5 1/2 1 2 4 ⎟ ⎜ ⎟ ⎝ 1/7 1/4 1/2 1 2 ⎠ 1/9 1/6 1/4 1/2 1 The maximum eigenvalue of matrix A is: λmax = 5.0781. The corresponding eigenvector is:

312

Y. He et al.

ω∗ = (0.8877, 0.3867, 0.2124, 0.1137, 0.0669)T . (c) Consistency test. Consistency indicators: CI = 0.0195 < 0.1 Random consistency index: RI = 1.12. Consistency ratio: CR = 0.0174 < 0.1 Based on this, the weight vectors ωI with F m (X), F δ (X), F f 1 (X), F f 2 (X) and F σ (X), can be determined by normalizing the feature vectors ω∗ through consistency test: ωI = (0.5324, 0.2319, 0.1274, 0.0682, 0.0401)T Determine the coefficient based on the reciprocal of the optimal value of a single target ωII. Taking the reciprocal of the optimal value of each single objective as the weight coefficient, as shown in Eq. (3), its essence is to treat each sub-objective function as a uniform order of magnitude. The calculation results are shown in Table 7. ω˜ i =

1 (i = 1, 2, 3, · · · , n) f˜i

(3)

Here, f˜i = min Fi (X ). Table 7. Reciprocal of each single objective optimization value. m(X)

δ max (X)

f 1 (X)

f 2 (X)

σ max (X)

f˜i

30.9712

0.9088

93.2944

100.3617

22.501

ω˜ i

0.03

1.1

0.01

0.0099

0.04

The resulting ωII = (0.03, 1.1, 0.01, 0.0099, 0.04)T Establishment of comprehensive objective function. The comprehensive objective functions are constructed:  min |G(X )| = min Fm (X ), Fδ (X ), −Ff 1 (X ), −Ff 2 (X ), Fσ (X ) · ωI · ωII

X∈ D

X∈ D

(4)

Since the goal of optimization design is to minimize the value of the comprehensive objective function, and the goals of the first and second order natural frequencies are to maximize them, therefore, they are both preceded by a negative sign. 3.6 Multi-objective Optimization Solution NSGA-II algorithm [24] has been widely used due to its good performance to explore, solving Pareto set is of good accuracy and dispersity. Therefore, NSGA-II algorithm is used for multi-objective optimization calculation in this research. Parameter configuration: population size is 40, algebra is 200, crossover rate is 0.9, cross distribution index is 10 and variation distribution index is 20.

Multi-objective Lightweight Design for the Forearm

313

3.7 Calculation Results and Analysis The optimized structural parameters are obtained through calculation, as shown in Table 8. The optimization model is regenerated according to the optimized structural parameters, and the optimization results of the target performance parameters are shown in Table 9. After multi-objective structural optimization, m is reduced by 5.3%. f 1 is increased by 3.487 Hz, f 2 is increased by 2.247 Hz. δ max is increased by about 0.007 mm, however it is still less than the allowable deflection value. σ max is decreased by 1.039 MPa. Table 8. Optimization results of structural parameters (mm) Design variable Initial value Optimal value Design variable Initial value Optimal value x1

10

6

x7

10

6

x2

72.8

70

x8

150

75

x3

60

55

x9

120

110

x4

10

8

x 10

10

8

x5

80

80

x 11

20

10

x6

80

80

x 12

25

25

Table 9. Optimization results of target performance parameters. Parameter

m (kg)

δ max (mm)

f 1 (Hz)

Initial value

33.5924

0.89082

90.221

Optimal value

31.8239

0.89828

93.708

Variation

↓1.7685

↑0.00746

↑3.487

f 2 (Hz) 99.773 102.02 ↑2.247

σ max (MPa) 23.176 22.137 ↓1.039

4 Conclusion (1) The finite element model of the forearm was established, and the accuracy of the finite element model was verified by modal test. Modal analysis, vibration response test and static analysis considering dynamic factors based on 5NURBS motion law were carried out. (2) According to the results of finite element analysis and vibration response test, the optimization objectives are determined to minimize mass, maximize displacement, maximize first and second order natural frequency and minimize maximum stress. 12 structural parameters were selected as design variables. The approximate models of objective function were obtained by using Box-Behnken and RSM methods. A method to establish compound weight coefficient was proposed. NSGA-II algorithm was used to optimize the solution, and the optimization results of the design variables and the optimized model were obtained.

314

Y. He et al.

(3) The optimized model was compared with the initial model. Under the condition that the first two natural frequencies were increased, the maximum stress was reduced, the maximum displacement was still satisfies the allowable deflection, the mass was reduced by about 5.3%, which verified the effectiveness of the optimization design method.

References 1. Ning, K., Li, D., He, F., et al.: Research on the structural optimization design of ER300 palletizing robot. Open Autom. Control Syst. J. 7, 1405–1414 (2015) 2. Albert, J.O.: Integrated structural and controller optimization in dynamic mechatronic systems. J. Mech. Des. 4, 1–8 (2010) 3. Ghiorghe, A.: Optimization design for the structure of an RRR type industrial robot. UPB Sci. Bull. Ser. D: Mech. Eng. 72(4), 121–134 (2010) 4. Guo, D.B., Mei, T., Luo, M.Z., et al.: Dynamics analysis and lightweight design of manipulator of elderly serve robot. China Mech. Eng. 10, 1146–1150 (2012) 5. Zhang, B.C., Tan, H.D., Xing, T.Y., et al.: Lightweight design of the miniature rescue manipulator. Manuf. Autom. 15, 129–132 (2014) 6. Yuan, A., Chen, C.: Modeling and optimization design of Six-DOF welding robot based on solidworks and ANSYS workbench. J. Mech. Transm. 02, 53–57 (2013) 7. Xiao, Z.Y., Duan, J.Z., Du, X.Q., et al.: Finite element static analysis and design improvement for motoman industrial robot. J. Changsha Univ. Sci. Technol. Nat. Sci. 8(4) 71–76 (2011) 8. Rui, Z.Y., Ma, Z.Y.: Stress analysing and optimizating to frame of aluminum ingot palletizing robot. Mach. Des. Manuf. 5, 141–143 (2010) 9. Cao, Z.M., Xiong, H.G., Tao, Y., et al.: Dynamic analysis of the palletizing robot elbow. Manuf. Autom. 5–7+29+1 (2013) 10. Tian, Y., Chen, X.P., Jia, D.Y., et al.: Design and kinematic analysis of a light weight and high stiffness manipulator for humanoid robots. Robot 3, 332–339 (2011) 11. Liu, Y.J., Wu, M.Y., Wang, G., et al.: Method for structural optimization design of wafer handling robot arms. J. Mech. Eng. 51(1), 1–9 (2015) 12. Wang, C.H., An, D., Zhao, D.H.: Static-dynamic multi-objective optimization and design of topology of industrial robot s arm. Mech. Sci. Technol. Aerospace Eng. 35(2), 241–246 (2016) 13. Saaty, T.L.: How to handle dependence with the analytic hierarchy process. Math. Model. 9(3–5), 369–376 (1987) 14. Saaty, T.L.: A new macroeconomic forecasting and policy evaluation method using the analytic hierarchy process. Math. Model. 9(3–5), 219–231 (1987) 15. Deng, X., Li, J.M., Zeng, H.J.: Research on computation methods of AHP weight vector and its applications. Math. Practice Theory 7(42), 93–100 (2012) 16. Zhang, J., Wang, J., Lin, J., et al.: Diesel engine noise source identification based on EEMD, coherent power spectrum analysis and improved AHP. Meas. Sci. Technol. 26, 1–16 (2015) 17. Zhang, T.-J., Su, L., Qiao, B., et al.: Prediction of coal and gas outburst level based on improved AHP. J. Xi’an Univ. Sci. Technol. 5 536–542+547 (2010) 18. Duan, R.C., Wang, F.H., Gu, C.Y., et al.: Comprehensive evaluation of 500 kV transmission line lightning protection effect based on improved analytic hierarchy process. High Volt. Eng. 01, 131–137 (2014) 19. He, Y., Mei, J.P., Fang, Z.W., et al.: Multi-objective optimization design for the machine base of a high speed and heavy load palletizing robot. J. Mach. Des. 34(7), 1–9 (2017)

Multi-objective Lightweight Design for the Forearm

315

20. Mei, J.P., Zang, J.W., Qiao, Z.Y., et al.: Trajectory planning of 3-DOT delta parallel manipulator. J. Mech. Eng. 52(19), 9–17 (2016) 21. Box, G.E.P., Behnken, D.W.: Some new three level designs for the study of quantitative variables. Technometrics 2, 455–475 (1960) 22. Myers, R.H., Montgomery, D.C.: Response Surface Methodology: Process and Product Optimization Using Designed Experiments. John Wiley & Sons, New York (1995) 23. Wang, Q.: China Mechanical design Canon, vol. vol. II. Jiangxi Science and Technology Press, Nanchang (2002) 24. Deb, K., Pratap, A., Agarwal, S., et al.: A fast and elitist multi objective genetic algorithm: NSGA-II. IEEE Trans. Evol. Comput. 6(2), 182–197 (2002)

Kinematic Analysis of a Novel 4-DOF 3T1R Parallel Manipulator Jie Zhao1,2,3 , Guilin Yang1,3(B) , Dexin Jiang1,3,4 , Tianjiang Zheng1,3 , Yingzhong Tian4 , Silu Chen1,3 , and Chi Zhang1,3 1

3

Ningbo Institute of Materials Technology and Engineering, Chinese Academy of Sciences, Ningbo 315201, China 2 University of Chinese Academy of Sciences, Beijing 100049, China Zhejiang Key Laboratory of Robotics and Intelligent Manufacturing Equipment Technology, Ningbo 315201, China [email protected] 4 Shanghai Key Laboratory of Intelligent Manufacturing and Robotics, School of Mechatronic Engineering and Automation, Shanghai University, Shanghai 200444, China

Abstract. A novel four degree-of-freedom (DOF) Parallel Manipulator (PM) with a 2PPa*PaR configuration is proposed to generate 3-DOF Translational and 1-DOF Rotational (3T1R) motions. This PM consists of a base, two identical limbs and a rigid moving platform, while each limb is driven by a prismatic joint and a revolute joint respectively through an orientation keeping mechanism and a parallelogram. The orientation keeping mechanism is made up of a link, two pulleys, and a belt. The advantages of the PM are symmetric geometry with fewer number of moving parts, simple kinematics with fewer singularity configurations, and infinite extension of translational workspace along its linear guide direction. Based on the 2PPa*PaR PM, the mobility analysis is conducted by utilizing the screw theory, while a geometrical projection method is proposed to simplify the displacement analysis. A closed-loop vector approach together with variable elimination technique is employed for its velocity analysis, and the kinematic singularity issue is addressed by the rank analysis of Jacobian matrices. Finally, a numerical workspace generation algorithm is implemented for the workspace analysis. Keywords: 4-DOF parallel manipulator Singularity · Workspace

1

· Kinematic analysis ·

Introduction

P arallel M anipulators (PMs) that can generate the 4-DOF 3T1R motions, i.e., Sch¨ onflies motions, have a wide range of industrial applications, such as parts Supported by the Zhejiang Provincial Key Research and Development Plan under Grant 2021C01069; the Ningbo Key Project of Scientific and Technological Innovation 2025 under Grant 2018B10069; and the Ningbo Key Project of Scientific and Technological Innovation 2025 under Grant 2018B10068. c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 316–326, 2021. https://doi.org/10.1007/978-3-030-89092-6_29

Kinematic Analysis of a Novel 4-DOF 3T1R Parallel Manipulator

317

assembly, surface mounting, and material handling. The most popular one was based on the famous 3-DOF Delta robot [9], which generates an additional 1DOF rotational motion by adding a central rotating arm between the base and the moving platform. However, the arm with a combined telescopic configuration makes it difficult to rotate smoothly. To overcome such limitations, three major types of 4-DOF 3T1R PMs with symmetric configurations have been proposed. The first type of 3T1R PMs have four identical parallelograms and an articulated moving platform, such as [5,8,10]. Their moving platform is made up of three parts, which can enlarge the rotation workspace by placing an amplifying device to the moving platform. But the kinematic analysis become complicated due to the articulated moving platform. The second type of 3T1R PMs have four identical limbs and a rigid moving platform, such as [1,11,13]. They possess better kinematic behaviors and dynamic performances, but the translational and rotational workspaces are limited by the more number of moving parts. The third type of 3T1R PMs have two identical limbs and a rigid moving platform, such as [2–4]. Duo to their fewer number of moving parts, these PMs have the advantages of simple kinematics and a large workspace. However, the two limbs structure results in lower motion stability and accuracy. Although these three types of 4-DOF 3T1R PMs have symmetric configurations and can generate accurate and smooth motions, their workspace are limited by the kinematic singularities and the constraints of more number of moving parts. To enlarge the workspace of 3T1R PMs, fewer number of moving parts and a special structure without suffering from the kinematic singularity (or local singularity) are expected. Since the parallelogram structure (Pa pair) can generate a 1-DOF translational motion, it is used widely in the 4-DOF PMs [6,7,12]. However, the workspace is limited due to its local singularity. To avoid the local singularity, an orientation keeping mechanism, i.e., a link with pulleys and a belt (Pa* pair), is employed in many researches. It is driven by a revolute joint, and the orientation of the last pulley remains unchanged with the constraint of the first pulley and the belt. Similarly, the orientation keeping mechanism can generate a 1-DOF translational motion, which can be employed in 3T1R PMs. In the light of such structure, a new type of PM with a 2PPaP*aR configuration is proposed in this paper, as shown in Fig. 1. This PM consists of a base, two identical limbs and a rigid moving platform, while each limb is driven by a prismatic joint and a revolute joint respectively through an orientation keeping mechanism and a parallelogram. It has a few advantages over the existing 4-DOF 3T1R PMs, such as symmetric structure with fewer number of moving parts, simple kinematics with fewer singularity configurations, and infinite extension of the translational workspace along its linear guide direction. Based on the 3T1R PM, the mobility analysis is conducted by utilizing the screw theory. A geometrical projection method is proposed for the displacement analysis, while a closed-loop vector approach is employed for the velocity analysis. The kinematic singularity issue is addressed by the Jacobian matrices. Finally, a numerical workspace generation algorithm is employed for the workspace analysis.

318

J. Zhao et al.

Equivalent schematic diagram Belt

Pulley 1

Link 1

Active revolute joint 1 Active revolute joint 2

Pulley 2

Linear guide I

Parallelogram mechanism Active prismatic joint 2

Active prismatic joint 1

Linear guide II Connector Moving platform Base

Fig. 1. Configuration of the 2PPa*PaR PM.

2 2.1

Mobility Analysis Description of the 3T1R PM

Figure 1 shows the three-dimensional schematic diagram of the 2PPaP*aR PM. Its moving platform is connected to the base through two limbs, while each limb consists of an orientation keeping mechanism, i.e., a link with pulleys and a belt, and a parallelogram. As shown in Fig. 1, pulley 2 is fixed on the base, it won’t be driven by active revolute joint 1. Pulley 1 is connected with the pulley 2 through a belt, and it is attached to the link 1 through a rotation axis. When link 1 rotates driven by active revolute joint 1, the orientation of pulley 1 can remain unchanged with the constraint of pulley 2 and the belt. In such a way, the orientation keeping mechanism can generate a 1-DOF translational motion, which can be treated as a modified Pa pair, i.e., Pa* pair. Based on such the modification, the Joint-and-Loop diagram of the 3T1R PM is shown in Fig. 2. Pa

Base P

R

R

R

R

Pa*

R

I

Moving Platform R P

R

Pa*

R R

R

II

P

Active prismatic joint

Pa* Active revolute joint Pa

Planar four-bar parallelogram

R

Passive revolute joint

Pa

Fig. 2. Joint-and-Loop diagram of the 2PPa*PaR PM.

Kinematic Analysis of a Novel 4-DOF 3T1R Parallel Manipulator

319

Referring to Fig. 3, the global frame {B}, i.e., O − xyz, is fixed to linear guide I, while the local frame {E}, i.e., P − x y  z  , is attached to the center of the moving platform. The y-axis of the global frame is along the direction of linear guide I, while its z-axis is perpendicular to the base plane. In addition, the three axes of the local frame is parallel to those of the global frame in the initial pose of the PM. The geometrical parameters of the 2PPa*PaR PM are listed in Table 1.

E1 C1

F1

B1

$11

y

E2

$13

$12

lu

z

Ι

ld

β1

x B

α1 $ q1

D1

$23

z

14

P1

E

Π

C2

$22

F2

y

D2

P

β2

$24 h

x P2

α2

B2

$21

q2

Fig. 3. Kinematic diagram of the 2PPa*PaR PM.

Table 1. Geometric parameters of the 2PPa*PaR PM.

2.2

Symbol Description

Unit

a b lu ld L0 h αi βi qi

m m m m m m rad rad m

Distance between revolute joints P1 to P2 Distance between two linear guides Length of the link Bi Ci Length of the link Ei Fi Length of the linear guide Distance between Pi and Di Angle from x-axis to the link Bi Ci Angle from Ei Ci to Ei Fi Displacement of prismatic joint Bi along the y-axis

Mobility Analysis

For the 2PPa*PaR PM, its four DOFs, i.e. three translational DOFs along x-, y-, z-axis and one rotational DOF around z-axis, are validated by screw theory. $i1 and $i4 (i = 1, 2) represent the kinematic pair twists of the active prismatic joints and the passive joints connected with the moving platform, respectively.

320

J. Zhao et al.

As the orientation keeping mechanism can generate a 1-DOF translational motion, which can be treated as a Pa* pair, and the planar four-bar parallelogram can also generate a 1-DOF translational motion, i.e., a Pa pair, so their twists are given by $i2 and $i3 . As shown in Fig. 3, the twist system of limb I is given by     $11 =  0 0 0 ; 0 q1 0 , $12 = 0 0 0 ; cos α1 0 sin α1 ,  $13 = 0 0 0 ; − sin β1 0 cos β1 , $14 = 0 0 1 ; q1 −lu sin α1 − ld sin β1 0 . (1) Based on Eq. 1, the constraint twist system of limb I is given by     $r11 = 0 0 0 ; 1 0 0 , $r12 = 0 0 0 ; 0 1 0 . (2) Similarly, the constraint twist system of limb II is the same as Eq. 2. Therefore, the constraint twist system of the moving platform can be determined by the reciprocal screw of Eq. 2, which is given by   rr   $rr 1 =  0 0 0 ; 1 0 0  , $2 =  0 0 0 ; 0 1 0  , (3) rr $rr 3 = 0 0 0 ; 0 0 1 , $4 = 0 0 1 ; 0 0 0 . Equation 3 shows that the 2PPa*PaR PM can generate 3T1R motions, i.e., three translational motions along x-, y-, z-axis respectively and one rotational motion around z-axis.

3 3.1

Displacement Analysis Forward Displacement Analysis

The forward displacement analysis is to determine the pose parameters of the moving platform (i.e. x, y, z, and θ) when the displacements of active joints (i.e., q1 , q2 , α1 , and α2 ) are known. By projecting the PM onto the xoy plane in frame {B}, a planar projection of the PM is obtained, as shown in Fig. 4(a). Since the two prismatic joints B1 and B2 move along the y direction of frame {B}, their displacements remain unchanged after projection. The link Bi Ci and the parallelogram Ci Di Ei Fi move on the xoz plane, which is perpendicular to the base plane, and their projected length are represented by  Bi Ci  and  Ci Di  respectively. The moving platform is always parallel to the base plane, and its length is represented by  P1 P2 . According to Fig. 4(a), the y coordinate and the rotation angle θ of the moving platform can be given by: y = (q1 + q2 )/2, θ = arcsin [(q2 − q1 ) /a] .

(4)

By projecting the PM onto the xoz plane, the geometric relationship of the two limbs are identified in Fig. 4(b). Let h1 , h2 , h3 , and h4 represent the vertical projected length of B1 C1 , B2 C2 , C1 D1 , and C2 D2 ; b0 , b1 , and b2 represent the horizontal projected length of B1 D1 , P1 P2 and D2 B2 , respectively. Accordingly, h1 , h2 , h3 and h4 are given by h1 = lu sin α1 , h2 = lu sin α2 ,

(5)

Kinematic Analysis of a Novel 4-DOF 3T1R Parallel Manipulator

321

C C

limb II

limb Ι

D C B

B

P

C

B

D

B

(a)

(b)

Fig. 4. Two projected views: (a) the projection on the xoy plane; (b) the projection on the xoz plane;

 h3 =

2

ld2 − (b1 − lu cos α1 ) , h4 =



2

ld2 − (b2 + lu cos α2 ) .

(6)

Since points D1 and D2 have the same z coordinate, and zD1 = h1 − h3 , zD2 = h2 − h4 , it yields (7) h1 − h3 = h2 − h4 , Substituting Eq. 5 and Eq. 6 into Eq. 7, b1 can be given by:  2 A ± −lu2 B (B − 4ld2 ) (sin α1 − sin α2 ) b1 = , 2B

(8)

where, 2  2 A = Δb3 −lu Δb2 (cos α1 − 3 cos α2 )+lu2 ΔbC +4lu3 (cos α1 + cos α2 ) sin α1 −α , 2 B = Δb2 + 2lu2 − 2lu (Δb cos α1 + lu cos (α1 − α2 ) − Δb cos α2 ) , C = 2 − cos 2α1 − 2 cos (α1 − α2 ) + cos 2α2 , Δb = b − b0 . In brief, the forward displacement solution of the 2PPa*PaR PM is obtained as follows: x = b1 + b20 , y = (q1 + q2 )2, (9) z = h1 − h3 − h, θ = arcsin [(q2 − q1 ) /a] . 3.2

Inverse Displacement Analysis

The inverse displacement analysis is to determine the four active-joint displacements (i.e., q1 , q2 , α1 , and α2 ) when the pose parameters of the moving platform are given (i.e., x, y, z, and θ). According to Fig. 4(a), q1 and q2 are given by q1 = y −

a a sin θ, q2 = y + sin θ. 2 2

(10)

322

J. Zhao et al.

To obtain the relationship between the rotation angles (i.e., α1 , and α2 ) and the pose parameters of the moving platform, b3 , b4 , h3 , and h4 are employed to solve it. As shown in Fig. 4(b), they can be computed as: b3 = x −

a a cos θ − lu cos α1 , b4 = b − (x − cos θ) + lu cos α2 . 2 2

h3 = lu sin α1 − (z + h), h4 = lu sin α2 − (z + h).

(11) (12)

Based on the geometrical relationship, it yields h2j + b2j = ld2

(j = 3, 4).

Substituting Eq. 11 and Eq. 12 into Eq. 13, α1 and α2 can be given by: √ √ −A1 ± 2 B 1 −A2 ± 2 B 2 α1 = ± arccos( ), α2 = ± arccos( ), C1 C2

(13)

(14)

where,    A1 = 4lu3 (a cos θ − 2x) − lu(2x − a cos θ) D1 + 4 E1 − ld2 ,       B1 = −lu2 (h + z)2 D1 + 4 E1 − (ld − lu)2 D1 + 4 E1 − (ld + lu)2 , C1 = 4lu2 (D1 + 4E1 ), D1 = a2 cos2 θ − 4xa cos θ, E1 = (h + z)2 + x2 ,    A2 = 4lu3 (a cos θ − 2b + 2x) − lu(−a cos θ + 2b − 2x) D2 + 4 E2 − ld2 ,       B2 = −lu2 (h + z)2 D2 + 4 E2 − (ld + lu)2 D2 + 4 E2 − (ld − lu)2 , 2 C2 = 4lu (D2 + 4E2 ), D2 = a2 cos2 θ − 4ab cos θ + 4ax cos θ, E2 = (h + z)2 + (b − x)2 .

4

Velocity Analysis

The velocity analysis is to derive the velocity relationship between the active ˙ Based on ˙ y, ˙ z, ˙ θ). joints (q˙1 , α˙ 1 , q˙2 , and α˙ 2 ) and the moving platform (x, the geometric features of this PM, a closed-loop vector method is employed,  in which the passive-joints velocities are eliminated. As shown in Fig. 5, r i =  T x y z (i = 1, 2) represent the displacement of the moving platform, it yields r i = q i + lu ni + ld pi + hg i − a2 ai (i = 1), r i = b + q i + lu ni + ld pi + hg i − a2 ai (i = 2).

(15)

in which, q i is the displacement of each prismatic joint along y-axis in base frame; b is the distance between two linear guides along x-axis; ni , pi , g i , and ai −−−→ −−−→ −−−→ −−→ represent the unit direction vectors of Bi Ci , Ci Di , Di Pi , and P Pi , respectively.

Kinematic Analysis of a Novel 4-DOF 3T1R Parallel Manipulator E1 C1

n1 B1

q1

z

y

x

323

Ι β1

E2

β2

p1

F1

z

α 1 g1 Da 1 1

P1

r1

Π

C2

F2

y

D2

P

E

x P2

B2

α2

B

Fig. 5. The closed-loop vector from the base coordinate origin to the center of the moving platform.

By taking the derivative of Eq. 15 with respect to time, the velocity equations are given by (16) r˙i = Kqi q˙i + Kui lu α˙ i + Kdi ld β˙ i − Kai θ˙i (i = 1, 2),  T where, r˙i = x˙ y˙ z˙ ; q˙i , α˙ i , β˙ i , and θ˙i represent the translational velocity of each prismatic joint, the angular velocities of the links lu , ld and the moving platform, respectively; Kqi , Kui , Kdi , and Kai are coefficient vectors. For limb i, the coefficient vector Kmi1 = Kqi ×Kdi is employed to eliminate q˙i and β˙ i , and the coefficient vector Kmi2 = Kui × Kdi is employed to eliminate α˙ i and β˙ i . By doing a dot product between both sides of Eq. 16 with the transpose of Km11 , Km12 , Km21 , and Km22 respectively, it yields   T T I3×3 a2 Ka1  V = Km11 Km11 Ku1 lu α˙ 1 ,  a T T Km12  I3×3 2 Ka1  V = Km12 Kq1 q˙1 , (17) a T T K I V = K Km21 Ku2 lu α˙ 2 , 3×3 a2 m21 2   T T I3×3 a2 Ka2 V = Km22 Km22 Kq2 q˙2 ,  T in which, V = x˙ y˙ z˙ θ˙ . Equation 17 can be written as the form of GV = Jq q, ˙ where q˙ = T  α˙ 1 q˙1 α˙ 2 q˙2 , the forward Jacobian matrix G and the inverse Jacobian matrix Jq are given by ⎡ ⎤ sin β1 0 − cos β1 a2 sin β1 sin θ ⎢ 0 1 0 − a2 cos θ ⎥ ⎥ (18) G=⎢ a ⎣ sin β2 0 cos β2 − sin β2 sin θ ⎦ , 2 a 0 1 0 2 cos θ ⎡ ⎤ −lu cos(α1 − β1 ) 0 0 0 ⎢ 0 1 0 0⎥ ⎥. Jq = ⎢ (19) ⎣ 0 0 lu cos(α2 + β2 ) 0 ⎦ 0 0 0 1

324

5

J. Zhao et al.

Singularity Analysis

A singularity configuration refers to a special configuration where a PM loses or gains one or more DOFs instantaneously. Through rank analysis of Jacobian matrices, three types of singularities, i.e. the forward, inverse, and combined singularities are identified. Based on Eq. 18 and Eq. 19, the determinants of G and Jq are given by det(G) = a cos θ sin(β1 + β2 ), det(Jq ) = −lu2 cos(α1 − β1 ) cos(α2 + β2 ). 5.1

(20)

Forward Singularity

Forward singularity is a particular configuration in which a PM gains one or more DOFs instantaneously. For the 2PPa*PaR PM, the forward singularity occurs when det(G) = 0, there exist two cases as follows: 1. cos θ = 0, i.e. θ = ±π/2. The moving platform is parallel to the y-axis in base frame, as shown in Fig. 6(a); 2. sin(β1 + β2 ) = 0, i.e., β1 + β2 = nπ(n = 0, 1). The links C1 D1 and C2 D2 parallel to each other, as shown in Fig. 6(b).

D1

C2

D2

D1 D2 C1

C2

C1

B2

B1

II

I

(a)

I

(b)

II

II

I

(c)

I

II

(d)

Fig. 6. Singularity configurations of the 2PPa*PaR PM : (a) θ = ±π/2; (b) β1 + β2 = nπ(n = 0, 1); (c) B1 , C1 , and D1 are collinear; (d) B2 , C2 , and D2 are collinear.

5.2

Inverse Singularity

Inverse singularity is a particular configuration in which a PM loses one or more DOFs instantaneously. For the 2PPa*PaR PM, the inverse singularity occurs when det(Jq ) = 0, there exist two cases as follows: 1. cos(α1 −β1 ) = 0, i.e. α1 −β1 = ±π/2. The points B1 , C1 , and D1 are collinear, as shown in Fig. 6(c); 2. cos(α2 + β2 ) = 0, i.e., α2 + β2 = ±π/2. The points B2 , C2 , and D2 are collinear, as shown in Fig. 6(d). 5.3

Combined Singularity

Combined singularity is a particular configuration in which a PM loses and gains one or more DOFs instantaneously, and it occurs when det(G) = det(Jq ) = 0. Based on the rank analysis, the combined singularity will not occur.

Kinematic Analysis of a Novel 4-DOF 3T1R Parallel Manipulator

6

325

Workspace Analysis

For a 3T1R 4-DOF PM, its workspace is four dimensional, i.e., three translational workspace along the x, y, and z directions respectively, and one rotational workspace around the z direction. In theory, the workspace along y-axis can be extended infinitely. So the translational workspace in xoz plane and the rotation workspace, i.e., the mixed workspace, is researched in the section. The workspace analysis is usually addressed by the numerical or analytical methods. As the 3T1R PM has the simple displacement solutions, a numerical workspace generation algorithm is employed in MATLAB environment. In the initial stage, a given workspace volume is divided into 60 × 50 × 60 elements with equal volumes in x-axis, y-axis, and z-axis respectively, where the coordinates in y-axis represent the rotation angle θ. The coordinate of each elements’ center point is checked based on the inverse displacement solutions. In such a way, the mixed workspace is generated as shown in Fig. 7, where the design parameters are given by lu = 500 mm, ld = 500 mm, a = 160 mm, and b = 1000 mm.

Fig. 7. The mixed workspace of the 2PPa*PaR PM: (a) 3-D mixed workspace; (b) 2-D workspace in xoz plane.

7

Conclusions

A novel 4-DOF 3T1R 2PPa*PaR parallel manipulator with two limbs is proposed in this paper, in which an orientation keeping mechanism is employed in each limb to avoid the singularity configurations and enlarge the reachable workspace. In addition, this PM possesses a few advantages of symmetric geometry, simple kinematics, and lightweight design. For the 3T1R PM, the mobility is verified with screw theory, while the displacement analysis is simplified by the projection method. The velocity analysis is addressed by a closed-loop vector approach with variables elimination technique. To investigate the kinematic singularity issue, the rank of Jacobian matrices is analyzed to identify the forward, inverse, and combined singularities. A numerical workspace generation algorithm is employed for the workspace analysis. Future work will be focused on the workspace optimization and prototype development of the 4-DOF 3T1R 2PPa*PaR PM.

326

J. Zhao et al.

References 1. Altuzarra, O., Sandru, B., Pinto, C., Petuya, V.: A symmetric parallel sch¨ onfliesmotion manipulator for pick-and-place operations. Robotica 29(6), 853 (2011) 2. Angeles, J., Caro, S., Khan, W., Morozov, A.: Kinetostatic design of an innovative sch¨ onflies-motion generator. Proc. Inst. Mech. Eng. Part C J. Mech. Eng. Sci. 220(7), 935–943 (2006) 3. Gauthier, J.F., Angeles, J., Nokleby, S.B., Morozov, A.: The kinetostatic conditioning of two-limb sch¨ onflies motion generators. J. Mech. Robot. 1(1), 011010 (2009) 4. Kim, S.M., Shin, K., Yi, B.J., Kim, W.: Development of a novel two-limbed parallel mechanism having sch¨ onflies motion. Proc. Inst. Mech. Eng. Part C J. Mech. Eng. Sci. 229(1), 136–154 (2015) 5. Krut, S., Benoit, M., Ota, H., Pierrot, F.: I4: a new parallel mechanism for scara motions. In: 2003 IEEE International Conference on Robotics and Automation (Cat. No. 03CH37422). vol. 2, pp. 1875–1880 IEEE (2003) 6. Lee, C.C., Herv´e, J.M.: Type synthesis of primitive schoenflies-motion generators. Mech. Mach. Theory 44(10), 1980–1997 (2009) 7. Li, Z., Lou, Y., Zhang, Y., Liao, B., Li, Z.: Type synthesis, kinematic analysis, and optimal design of a novel class of sch¨ onflies-motion parallel manipulators. IEEE Trans. Autom. Sci. Eng. 10(3), 674–686 (2012) 8. Nabat, V., de la O RODRIGUEZ, M., Company, O., Krut, S., Pierrot, F.: Par4: very high speed parallel robot for pick-and-place. In: 2005 IEEE/RSJ International Conference on intelligent robots and systems, pp. 553–558. IEEE (2005) 9. Pierrot, F., Reynaud, C., Fournier, A.: Delta: a simple and efficient parallel robot. Robotica 8(2), 105–109 (1990) 10. Pierrot, F., Company, O.: H4: a new family of 4-DOF parallel robots. In: 1999 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (Cat. No. 99TH8399), pp. 508–513. IEEE (1999) 11. Salgado, O., Altuzarra, O., Petuya, V., Hernndez, A.: Synthesis and design of a novel 3t1r fully-parallel manipulator. J. Mech. Des. 130(4), 042305 (2008) 12. Wu, C., Yang, G., Chen, C.Y., Liu, S., Zheng, T.: Kinematic design of a novel 4-DOF parallel manipulator. In: 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 6099–6104. IEEE (2017) 13. Xie, F., Liu, X.J.: Design and development of a high-speed and high-rotation robot with four identical arms and a single platform. J. Mech. Robot. 7(4) (2015)

A Calibration Method of Robots by Eliminating Redundant Parameters Based on Jacobian Matrix Zhenya He1,2 , Huaiyan Tang1 , Guojian Huang3 , Xianmin Zhang1(B) , Zhong Chen1 , and Mingjing Song1 1 Guangdong Provincial Key Laboratory of Precision Equipment and Manufacturing Technology, School of Mechanical and Automotive Engineering, South China University of Technology, Guangzhou 510640, China {mezhyhe,Zhangxm}@scut.edu.cn 2 State Key Laboratory of Fluid Power and Mechatronic Systems, Zhejiang University, Hangzhou 310027, China 3 School of Electrical Engineering, Guangdong Mechanical and Electrical Polytechnic, Guangzhou 510550, China

Abstract. This paper presents a new identification method for error parameters of robot calibration model by eliminating the redundant error parameters based on Jacobian matrix. Firstly, the kinematic error model of the robots based on modified Denavit-Hartenberg (D-H) method was established. Secondly, the error parameters were analyzed based on Jacobian matrix in detail. And a new identification method by eliminating the redundant error parameters was presented. Finally, the calibration experiments of a 6-DOF industrial robot were carried out. The results showed that the error compensation rate was more than 71%. In addition, the positon accuracy has been further improved after compensation based on the presented identification method than that based on traditional method. Hence, the calibration method presented in this study is sensible and effective, and could be used for the error compensation and improve the position accuracy of the robots. Keywords: Robot calibration · Redundant parameters · Error identification · Jacobian matrix

1 Introduction The rapid development of the manufacturing technology requires that the higher position accuracy of the industrial robots has been growing concern [1], and the industrial robots have started to be used in the machining field [2]. Particularly, due to the advantage of higher flexibility, smaller size, and less cost comparing with tradition CNC machine tools, the industrial robots are more and more popular to be used for the large-sized part manufacturing in the field of aerospace, wind power, nuclear power, and automobile manufacturing and so on [3, 4]. However, most of industrial robots have lower absolute position accuracy than repeatability accuracy, and the position accuracy degrees of the © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 327–337, 2021. https://doi.org/10.1007/978-3-030-89092-6_30

328

Z. He et al.

robots are mostly millimeter [5]. Therefore, it is an urgent issue to improve the position accuracy of the robots. According to the error sources, the robot errors can be classified into different types, such as geometric errors, thermal errors, and dynamic errors. The geometric errors are mainly induced by manufacturing errors, installation error, encoder errors, and wear and so on [6]. The present investigation reveals that the geometric errors take a high proportion in the total errors, mostly 80% [7]. Generally, the calibration of robots, including error modeling, measurement, identification, and error compensation, is adopted to minimize the influence of the geometric errors on position accuracy [8]. Among the existing modeling techniques, a widely adopted approach is the Denavit-Hartenberg (D-H) model [9, 10]. It uses four parameters to describe the transformation relationship between the two adjacent joints. Nevertheless, the D-H model is ill-conditioned when the adjacent joints are nearly parallel [11]. To solve the problem, the modified D-H model was presented, which introduced a new angle parameter about the y-axis. Besides, there are S-model, complete and parametrically continuous (CPC) model, and product-of-exponential (POE) model, and so on [12–16]. There are several commonly measuring instruments for the robots, such as ball bar, coordinate measuring machine (CMM), pull-wire encoder measuring instrument, and laser tracker and so on. In which, the laser tracker is more popular for position measurement of industrial robots. The robot calibration based on the error parameters which obtained by identification techniques according to measured data, is a direct and cost-effective way to improve the position accuracy of robots [7]. Therefore, this paper will focus on the robot calibration, including error modeling, error measurement, error identification, and error compensation, to improve the position accuracy of robots. Particularly, the analysis method of redundant parameters based on the Jacobian matrix will be deduced for serial robots. Finally, the experiments will be conducted to verify the effectiveness of the presented method.

2 Geometric Error Model of Robots At present, the D-H model is one of the most widely adopted approaches for kinematic analysis of robots. There are four types of parameters to describe two adjacent links, including link length ai , link twist αi , link offset di , and joint angle θi , is shown in Fig. 1. i +1

i

i −1

αi Z i

ai

ai −1

Yi −1

Z i −1

X i −1

Yi di

Xi

θi

Fig. 1. The definition of the four parameters

A Calibration Method of Robots by Eliminating Redundant Parameters

329

The homogenous transformation matrix i−1 i T from the link (i) to link (i − 1) can be expressed as follows, i−1 i T

= Rot(z, θi )Trans(z, di )Trans(x, ai )Rot(x, αi ) ⎤ cos θi − sin θi cos αi sin θi sin αi ai cos θi ⎢ sin θi cos θi cos αi − cos θi sin αi ai sin θi ⎥ ⎥ =⎢ ⎣ 0 sin αi cos αi di ⎦ 0 0 0 1 ⎡

(1)

It should be noted that the D-H model is ill-condition when the adjacent joint axes are nearly parallel. Hence, a rotation angle βi around the y-axis is taken into account to address the problem. i−1 i T can be represented as follows, i−1 i T

= Rot(z, θi )Trans(z, di )Trans(x, ai )Rot(x, αi )Rot(y, βi ) ⎤ ⎡ cθi cβi − sαi sθi sβi −sθi cαi cθi sβi + sθi sαi cβi ai cθi ⎢ sθi cβi + sαi cθi sβi cθi cαi sθi sβi − cθi sαi cβi ai sθi ⎥ ⎥ =⎢ ⎣ −cαi sβi sαi cαi cβi di ⎦ 0

0

0

(2)

1

where, sθi represents sin(θi ), cθi represents cos(θi ), and so on. The overall transformation matrix T from last link n to the first link is represented as T = 01 T 12 T · · · · n−1 n T

(3)

The errors of geometric parameters are termed as ai , αi , di , θi and βi . Assume that the geometric errors are small enough, the mapping relationship between the positioning error of robot’s end-effector and geometric error parameters can be described by differential equations, T =

n n n n n      ∂T ∂T ∂T ∂T ∂T θi + αi + ai + di + βi ∂θi ∂αi ∂ai ∂di ∂βi i=0

i=0

i=0

i=0

(4)

i=0

Equation (4) can be described in matrix form, e = Jk

(5)

where, e = [ δx δy δz εx εy εz ]T , J is a coefficient matrix, and k is a column vector of total error parameters, k = [θ0 , · · ·, θn , α0 , · · ·, αn , a0 , · · ·, an , d0 , · · ·, dn , β0 , · · ·, βn ]T (6) This paper only involves the position error measurement. Therefore, the first three rows are selected in order to obtain the position error model of the end-effector, ep = J p k

(7)

where, ep represents the position error vector of robot’s end-effector, J p represents the Jacobian matrix of the position errors.

330

Z. He et al.

3 Analysis of Redundant Parameters The condition number of Jacobian matrix in Eq. (7) reflects the influence of disturbance on the solution of error model. When the condition number is bigger, the effect is greater. That is, the identification accuracy of the geometric parameters is worse. According to the relevance of the column vectors of the Jacobian matrix, the geometric error parameters could be divided into three types, including independent error parameters, relevant error parameters, and unidentifiable error parameters. The independent error parameters can be identified directly, and their correspondent column vectors of the Jacobian matrix are not relevant to others. The relevant error parameters are partially identifiable parameters. Their correspondent column vectors of the Jacobian matrix are relevant to others, and they will affect each other during the identification process, so generally only one parameter is reserved among them. The unidentifiable parameters are the redundant parameters, their correspondent coefficient column vectors of the Jacobian matrix are zero. The redundant parameters could cause the Jacobian matrix to be non-full rank, thereby inducing in the bigger deviations of identification results. Hence, the redundant parameters should be eliminated before parameter identification in order to reduce influence of the disturbance on the identification results. It could improve the accuracy and stability of parameter identification. Assume that the parameters kl and km are relevant, thus their corresponding column vector are proportional, J l = λJ m , where λ is the proportionality constant. The errors inducing by the parameters kl and km can be presented by J l kl + J m km = λJ m kl + J m km = J m (λkl + km )

(8)

Assume that k  = λkl + km . In fact, kl and km can’t be identified respectively, but k  it could be. Generally, only one of the relevant error parameters is reserved, for example, kl is reserved, and identification result of kl is equal to the value of k  . The transformation matrix from any coordinate frame of the robot joint {i} to the based frame {0} can be described by

ni oi ai pi 0 (9) T = i 0 0 0 1 Similar to the above, the transformation matrix from the coordinate frame of preceding joint {i − 1} to the based frame {0} can be described by

ni−1 oi−1 ai−1 pi−1 0 (10) T = i−1 0 0 0 1 The transformation matrix between the two adjacent joints can be obtained, ⎤ cθi −cαi sθi sαi sθi ai cθi ⎢ sθi cαi cθi −sαi cθi ai sθi ⎥ i−1 0 0 0 ⎥ ⎢ i T = i−1 T · i T = i−1 T ⎣ 0 sαi cαi di ⎦ 0 0 0 1 ⎡

(11)

A Calibration Method of Robots by Eliminating Redundant Parameters

331

According to Eqs. (9)–(11), each column vector of the transformation matrix from any coordinate frame of the joint {i} to the based frame {0} can be expressed as follow, ⎧ ni = cθi · ni−1 + sθi · oi−1 ⎪ ⎪ ⎪ ⎨ o = −cα sθ · n + cα cθ · o + sα · a i i i i−1 i i i−1 i i−1 (12) ⎪ a = sα sθ · n − sα cθ · o + cα · a i i i i−1 i i i−1 i i−1 ⎪ ⎪ ⎩ pi = ai cθi · ni−1 + ai sθi · oi−1 + di · ai−1 + pi−1 The Jacobian matrix J is obtained by total differential operation of all geometric parameters, each column of the matrix represents the differential of all parameters, named as J ai , J di , J αi , J θi , and J βi ,   (13) J = J ai , J di , J αi , J θi , J βi According to the differential kinematics of the robots, the vector cross product can be used to express the column vectors of the Jacobian matrix,  J ai =

         ai−1 × 0i−1 pn ni × 0i pn oi × 0i pn ni ai−1 , J θi = , J βi = , J di = , J αi = 0 0 ni oi ai−1

(14)

where, 0i pn represents the coordinate values of the end-effector origin transformed from joint frame {i} to based frame {0}. According to Eqs. (12)–(14), the column vectors of the Jacobian matrix are satisfied the following conditions.

oi−1 (15) J ai = cθi J ai−1 + sθi 01×3

o J di+1 = sαi sθi J ai−1 − sαi cθi i−1 + cαi J di (16) 01×3

oi−1 + cθi J αi−1 + sθi J βi−1 J αi = di sθi J ai−1 − di cθi (17) 01×3

oi−1 J βi = (di cαi cθi − ai sθi sαi )J ai−1 + (ai sαi cθi + di cαi sθi ) 01×3 (18) − ai cαi J d − cαi sθi J α + cαi cθi J β + sαi J θ i

i−1

i−1

i



J θi+1 = −(di sαi cθi + ai sθi cαi )J ai−1

o + (ai cαi cθi + di sαi sθi ) i−1 01×3

(19)

+ai sαi J di + sαi sθi J αi−1 − sαi cθi J βi−1 + cαi J θi Therefore, the conclusions can be summarized as follows,   (1) when αi = 0◦ , the column matrix J i J i+1 is full rank, the adjacent joints are not parallel or collinear. Therefore, there is not redundant parameter.

332

Z. He et al.

(2) when αi = 0◦ and ai = 0, according to Eq. (16), we can obtain J di+1 = J di , the adjacent joints are parallel, but not collinear. Therefore, di is the redundant parameter. At the moment, βi should be introduced. (3) when αi = 0◦ and ai = 0, according to Eqs. (16) and (19), we can obtain J di+1 = J di and J θi+1 = J θi , the adjacent joints are collinear. Therefore, di and θi are the redundant parameters. (4) Only for the end-effector of robots, a) when an = 0, the column of matrix J an = 0, the axes Zn−1 and Zn are not collinear, and their distance is not zero. Therefore, an is ineffective to the position of endeffector. At the moment, an is the redundant parameter. b) when an = 0 and αn−1 = 0◦ , the column of matrix J αn = 0 and J θn = 0, the axes Zn−1 and Zn are intersecting, but not perpendicular. Therefore, αn and θn are ineffective to the position of end-effector. At the moment, αn and θn are the redundant parameters. c) when an = 0, αn−1 = ±90◦ , and an−1 = 0, the column of matrix J αn = 0, J θn = 0, and J αn−1 = ∓dn J dn−1 , the axes Zn−1 and Zn are perpendicular, the axes Zn−2 and Zn−1 are not intersecting. Therefore, αn−1 and dn−1 have the same effects on the position of end-effector. At the moment, αn , θn , and αn−1 are the redundant parameters. d) when an = 0, αn−1 = ±90◦ , and an−1 = 0, the column of matrix J αn = 0, J θn = 0, J αn−1 = ∓dn J dn−1 , and J θn−1 = ∓dn J an−1 , the axes Zn−1 and Zn are perpendicular, the axes Zn−2 and Zn−1 are intersecting. Therefore, αn and θn are ineffective to the position of end-effector; αn−1 and dn−1 have the same effects on the position of end-effector; θn−1 and an−1 also have the same effects. At the moment, αn , θn , αn−1 , and θn−1 are the redundant parameters. Therefore, the redundant parameters could be eliminated according to the above method. And then the error model after eliminating the redundant parameters is reestablished for error identification. During measurement, several configurations of the robot among workspace are selected for parameter identification, assume that there are m selected configurations, 3m ≥ q. The error parameters could be identified based on the least squares method, kˆ = arg min k

m    ej − J j k

(20)

j=1

where, kˆ was estimated value by least squares fitting, J j represents the error Jacobian matrix of each configuration after eliminating the redundant parameters.

4 Experiment and Results To verify the feasibility and effectiveness of the above error identification method of eliminating redundant parameters, an experiment was carried out on a 6-DOF serial robot, an ABB IRB 120 robot. The experiment consisted of four parts, including error modeling, error measurement, error identification, and error compensation.

A Calibration Method of Robots by Eliminating Redundant Parameters

333

Table 1. The nominal geometric parameters of an ABB IRB 120 robot i

ai (mm)

αi ( ◦ )

di (mm)

Zero position of θi (◦ )

θi (◦ )

Range of θi (◦ )

βi ( ◦ )

0

0

0

d0

0

0

0

–

1

0

−90

290

0

θ1

−165 ~ 165

–

2

270

0

0

−90

θ2

−30 ~ 200

0

3

70

−90

0

0

θ3

−110 ~ 70

–

4

0

90

302

0

θ4

−160 ~ 160

–

5

0

−90

0

0

θ5

−120 ~ 120

–

6

0

0

72

180

θ6

−490 ~ 310

–

Firstly, geometric error model was established. The nominal geometric parameters of the robot are shown in Table 1. During measurement, laser tracker was adopted to measure position of the endeffector. Considering that there are some deviation between the measurement coordinate frame {m} and base frame {0}, the transformation matrix m 0 T from frame {0} to frame {m} was introduced based on the D-H parameter method. The origin of the coordinate frame {t} was fixed on the center point of the spherically mounted reflector (SMR) of the laser tracker, its coordinate value was (px6 , py6 , pz6 ) with respect to the frame T  {6}, namely 6 P = px6 py6 pz6 1 . Therefore, the overall transformation matrix for measurement can be described by m

0 1 2 3 4 5 6 P=m 0 T · 1T · 2T · 3T · 4T · 5T · 6T · P

(21)

And then, according to Reference [7], the parameter d0 , px6 , py6 , and pz6 were  T obtained by measurement, d0 = −6.068, 6 P = −0.0341 0 18.053 1 . According to Eq. (7), the position error model of the robot was established, ep = J p k, where, k = [θ0 , · · ·, θ6 , α0 , · · ·, α6 , a0 , · · ·, a6 , d0 , · · ·, d6 , β2 ]T

(22)

Hence, there were 29 parameters needed to be identified based on traditional identification method. Secondly, error measurement experiment was carried out and error parameters were identified based on all parameter identification method and identification method by eliminating the redundant error parameters, respectively. According to the above analysis method of the redundant parameters, in fact, there were 7 parameters could not be identified, as shown in Table 2. Here, “” represents the independent error parameters and relevant parameters which were reserved, and “” represents the unidentifiable parameters and relevant parameters which were eliminated. Therefore, after eliminating the redundant parameters, there were 22 parameters, namely k is a 22 × 1 identifiable parameter matrix, J is a 3 × 22 error Jacobian matrix.

334

Z. He et al. Table 2. The identifiable parameters of the robot

i

ai

αi

di

θi

βi

0









–

1









–

2











3









–

4









–

5









–

6









–

We used laser tracker to measure the position of robot’s end-effector, as shown in Fig. 2. During measurement, 40 configurations of the robot among workspace were selected for parameter identification, and the error parameter vector kˆ was obtained based on least squares method.

SMR Robot

Laser tracker

Computer

Fig. 2. The measurement experiment of the robot

Finally, error compensation experiments were conducted. The joint angle compensation method was adopted to compensate the position errors of the robot. In order to verify the effectiveness of the identification method, another 100 measurement configurations were selected for compensation experiments. The results were shown in Fig. 3. The first line labeled “without comp.” describes the position errors without error compensation, the second line labeled “with comp.1” describes the position errors with error compensation based on the all parameters identification method (traditional identification method), and the third line labeled “with comp.2” describes the position errors with error compensation based on the identification results by eliminating the redundant parameters.

A Calibration Method of Robots by Eliminating Redundant Parameters

335

The results were compared as shown in Table 3. It can be seen that after error compensation, the mean of the position errors is reduced from 0.657 mm to 0.161 mm or 0.149 mm, and the maximum of the position errors is reduced from 0.884 mm to 0.238 mm or 0.253 mm. The mean of compensation rate is more than 75%, and the maximum is more than 71%. These clearly show that the compensation method is effective. Furthermore, by comparing the results with compensation 1 and compensation 2, it can be seen that the mean of the position errors is reduced from 0.161 mm to 0.149 mm, and the standard deviation (STDEV) of the position errors is reduced from 0.043 mm to 0.030 mm, though the maximum of the position errors is increased from 0.238 mm to 0.253 mm. Namely, the positon accuracy among the robot workspace has been further improved with error compensation based on the presented identification method, which eliminates the redundant error parameters based on Jacobian matrix before identification. Therefore, the error identification method presented in this study is sensible and effective, and could be used for the robot calibration and improve the position accuracy of the robots. 0.9 0.8

position error (mm)

0.7 0.6 0.5 without comp. with comp.1 with comp.2

0.4 0.3 0.2 0.1 0

0

10

20

30

40 50 60 number of points

70

80

90

100

Fig. 3. The results with and without error compensation.

Table 3. Comparison of the results with and without compensation (mm) Position errors

Maximum

Minimum

Mean

STDEV

Without comp

0.884

0.402

0.657

0.094

With comp. 1

0.238

0.082

0.161

0.043

With comp. 2

0.253

0.065

0.149

0.030

336

Z. He et al.

5 Conclusions This paper presented an identification method of the kinematic parameters for robot calibration by eliminating the redundant error parameters based on Jacobian matrix. Firstly, the kinematic error modeling of robots based on modified D-H method was established. Secondly, the error parameters were analyzed in detail, based on the relevance of the column vectors of the Jacobian matrix, and the dividing basis of the three kinds of error parameters were deduced, including independent error parameters, relevant error parameters, and unidentifiable parameters. Moreover, an error identification method was presented by eliminating the redundant parameters based on the least squares. Finally, the calibration experiments of a 6-DOF industrial robot were carried out, including error modeling, error measurement, error identification and error compensation. The results show that the error compensation rate is more than 71%, and the position accuracy of the robot has a significant improvement. In addition, the positon accuracy has been further improved with error compensation based on the presented identification method, which eliminates the redundant error parameters. Hence, the calibration method presented in this study is sensible and effective, and could be used for the error compensation and improve the position accuracy of the robots. Acknowledgments. This work was supported by the National Natural Science Foundation of China (No. 51805172), the Natural Science Foundation of Guangdong Province (No. 2019A1515011515), the Fundamental Research Funds for the Central Universities (No. 2019MS055) and the Open Foundation of the State Key Laboratory of Fluid Power and Mechatronic Systems (No. GZKF-202006).

References 1. Nubiola, A., Bonev, I.A.: Absolute calibration of an ABB IRB 1600 robot using a laser tracker. Robot. Comput. Integr. Manuf. 29(1), 236–245 (2013) 2. Ji, W., Wang, L.: Industrial robotic machining: a review. Int. J. Adv. Manuf. Technol. 103(1–4), 1239–1255 (2019). https://doi.org/10.1007/s00170-019-03403-z 3. Xiong, G., Ding, Y., Zhu, L., et al.: Stiffness-based pose optimization of an industrial robot for five-axis milling. Robot. Comput. Integr. Manuf. 55, 19–28 (2019) 4. Leali, F., Vergnano, A., Pini, F., Pellicciari, M., Berselli, G.: A workcell calibration method for enhancing accuracy in robot machining of aerospace parts. Int. J. Adv. Manuf. Technol. 85(1–4), 47–55 (2014). https://doi.org/10.1007/s00170-014-6025-y 5. Chen, D., Yuan, P., Wang, T., Cai, Y., Xue, L.: A compensation method for enhancing aviation drilling robot accuracy based on co-kriging. Int. J. Precis. Eng. Manuf. 19(8), 1133–1142 (2018) 6. Tang, H., He, Z., Ma, Y., Zhang, X.: A step identification method for kinematic calibration of a 6-DOF serial robot. In: Mechanism and Machine Science – Proceedings of ASIAN MMS 2016 and CCMMS 2016, pp. 1009–1020. Springer, Singapore (2017) 7. He, Z, Zhang, R, Zhang, X, et al.: Absolute positioning error modeling and compensation of a 6-DOF industrial robot. In: 2019 IEEE International Conference on Robotics and Biomimetics (ROBIO), pp. 840–845. IEEE (2019) 8. Joubair, A., Zhao, L.F., Bigras, P., Bonev, I.: Absolute accuracy analysis and improvement of a hybrid 6-DOF medical robot. Ind. Robot Int. J. Robot. Res. Appl. 42(1), 44–53 (2015)

A Calibration Method of Robots by Eliminating Redundant Parameters

337

9. Barker. L.K.: Vector-algebra approach to extract Denavit-Hartenberg parameters of assembled robot arms. NASA Tech Paper, p. 2191 (1983) 10. Hartenberg, R.S., Denavit, J.: A kinematic notation for lower pair mechanisms based on matrices. J. Appl. Mech. 77(2), 215–221 (1955) 11. Meggiolaro, M.A., Dubowsky, S.: An analytical method to eliminate the redundant parameters in robot calibration. In: 2000 IEEE International Conference on Robotics and Automation, pp. 3609–3615. IEEE, San Francisco, April (2000) 12. Hayati, S., Mirmirani, M.: Improving the absolute positioning accuracy of robot manipulators. J. Robot. Syst. 2(4), 397–413 (1985) 13. Wang, H., Shen, S., Lu, X.: A screw axis identification method for serial robot calibration based on the POE model. Ind. Robot Int. J. Robot. Res. Appl. 39(2), 146–153 (2012) 14. Stone, H.W.: Kinematic Modeling, Identification, and Control of Robotic Manipulators. Kluwer Academic Publishers, Norwell (1987) 15. Zhuang, H., Roth, Z.S., Hamano, F.: A complete and parametrically continuous kinematic model for robot manipulators. IEEE Trans. Robot. Autom. 8(4), 451–463 (1992) 16. Okamura, K., Park, F.C.: Kinematic calibration using the product of exponentials formula. Robotica 14, 415–421 (1996)

An Online Intelligent Kinematic Calibration Method for Quadruped Robots Based on Machine Vision and Deep Learning Handing Xu1,2 , Zhenguo Nie1,2(B) , Xinyu Cui1,2 , ShiKeat Lee1,2 , Qizhi Meng1,2 , Fugui Xie1,2 , and Xin-Jun Liu1,2 1

The State Key Laboratory of Tribology, Department of Mechanical Engineering, Tsinghua University, Beijing 100084, China [email protected] 2 Beijing Key Lab of Precision/Ultra-Precision Manufacturing Equipments and Control, Tsinghua University, Beijing 100084, China

Abstract. Quadruped robot leg calibration has great significance in improving positioning accuracy. However, the traditional calibration method is to manually measure the actual joint angles using protractors, which is time-consuming and inconvenient. To improve the calibration efficiency and accuracy, this paper presents an online intelligent kinematic calibration method for quadruped robots based on machine vision and deep learning. The proposed method contains three parts: detect the marker fixed on the robot leg using machine vision methods, transform coordinates into the reference coordinate system by camera internal and external parameters calibration, calculate joint angles and compensate errors by deep-learning-based inverse kinematics. The experimental validation result shows that the proposed intelligent method can significantly improve the calibration speed with an acceptable accuracy compared to the manual calibration method.

Keywords: Robot kinematic calibration learning · Quadruped robot

1

· Visual system · Deep

Introduction

Quadruped robot is one of the best robots to replace humans for diversified tasks in complex environments because of the optimal number of legs. However, the kinematic error between the actual accuracy of the robot and the theoretical design model limits the robot applications. It is necessary to calibrate the robot frequently to improve the robot’s point accuracy and trajectory accuracy. As for the quadruped robot, the calibration of its joint encoder can make its controller instructions match the actual attitude, improving the motion of accuracy. The conventional calibration method is to manually calibrate the actual angle of c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 338–344, 2021. https://doi.org/10.1007/978-3-030-89092-6_31

An Online Intelligent Kinematic Calibration Method

339

each motor at the leg joints by professional maintenance personnel, which is time-consuming, inconvenient, and then causes many application difficulties. In order to simplify and accelerate the calibration process, this research concentrates on an online calibration method based on machine vision and deep learning for complete intelligentization to reduce labor intensity. Generally, method based on vision calculates the coordinates of the robot end-effector in the base coordinate system from the pixel information captured by the camera using coordinate matrix transformation, which is widely used in hand-eye calibration [1,2]. Besides, Tejomurtula et al. [3] capture the RGBD marker on the robot end-effector to measure the end position and orientation. Nowadays, deep learning is getting popular in machine design, manufacturing, and analysis [4–8]. One of the benefits of deep learning is that it can process problems that are difficult to model. In the field of robot calibration, lots of researches [9–11] utilize ANN (artificial neural network) and RBFNN (radial basis function neural network) to compensate for the non-geometric error, such as the connecting rod deflection error, joint flexibility error, and gear gap error, etc. Furthermore, for robots that are difficult to establish parametric models, model-free approaches based on MLP (multilayer perceptron) [12] and RNN (recurrent neural network) [3] were proposed for error compensation. In this work, an online intelligent kinematic calibration method based on machine vision and deep learning is proposed. The calibration method contains three parts: visual detection, coordinate transformation, and joint angle calculation (as shown in Fig. 1). First, detect the robot leg in the picture captured by a binocular camera using visual method. Then, based on the position in the image, the position of the robot leg is transformed to the real world. The joint angle will be calculated through inverse kinematics and finally compensate for the error of motors. An experimental validation takes place on the quadruped robot designed by China North Vehicle Research Institute. Compared to manual calibration, the visual calibration method significantly reduces calibration time from 15 min to less than 1 min, and the accuracy of the calibration method is in an acceptable range, which is within 0.1◦ . The validation results indicate that the proposed calibration method can substitute for manual calibration and reduce maintenance difficulty.

2 2.1

Online Intelligent Kinematic Calibration Method Feature Point Visual Detection

Compared to detect the robot leg directly, setting a marker on the robot leg is better, which is easier to detect. Thus, this part contains two steps: Marker detection neural network construction and center positioning of marker. Marker Detection Neural Network Construction. A satisfactory dataset is essential for the neural network, so the first step is to take pictures of the robot leg with a marker on it under plenty of different conditions and then label

340

H. Xu et al.

Fig. 1. A schematic diagram of the proposed kinematic calibration method.

the marker in images. In this method, YOLO v5s (You only look once version 5 small) is chosen as the marker detection neural network because the marker is easy to detect and the model is small-scale for applying on the single-chip microcomputer. The network’s output is several bounding boxes containing the marker and their probabilities that they are detected correctly. Center Positioning of Marker. To positioning the robot leg, only the position of the marker is not precise enough, which means the center position of the marker is needed. For simplicity and ease of operation, detect the corners of the marker and calculate the center instead of directly detect the center, which simple graphical morphology processings can complete. The Shi-Tomasi algorithm is used for corner detection in this method. And for a more precise position, gray gradient interactive optimization for sub-pixel detection is applied. 2.2

Coordinate Transformation from Picture to Real World

To positioning the robot leg from 2D pixel coordinates (u, v) to 3D coordinates in the reference coordinate system (xr , yr , zr ), two steps are performed continuously in this method, which are camera internal and external parameter calibration.

An Online Intelligent Kinematic Calibration Method

341

Camera Internal Parameter Calibration. Since a single camera can only imaging in 2D space, so binocular camera is needed in this method. The camera internal parameter calibration is to obtain the transformation matrix M1 from the 2D pixel coordinates (u1 , v1 ) and (u2 , v2 ) to the 3D coordinate system (xc , yc , zc ) fixed on the camera. Assuming that the lens’ distortion and the two cameras’ assembly error don’t exist, the binocular camera’s imaging model can be illustrated by Fig. 2. The coordinate transformation matrix M1 is obtained by the similar triangle principle based on this model.

Fig. 2. The binocular camera’s imaging model, which illustrates the transformation from 2D pixel coordinates to 3D coordinates.

⎡ ⎤ ⎤ xc ⎡ ⎤ ⎡ f 000 ⎢ ⎥ u1 yc ⎥ Zc ⎣u2 ⎦ = ⎣0 f 0 0⎦ ⎢ ⎣ zc ⎦ 1 0010 (1) 1 fd zc = u1 − u2 where f and d denote the focal length of the camera and the distance between the lens of binocular camera separately. The calibration toolbox of Matlab is utilized in this method because it considers the distortion and assembly error, which means higher accuracy. Camera External Parameter Calibration. The camera external parameter calibration is to obtain the transformation matrix M2 from the 3D coordinate system (xc , yc , zc ) fixed on the camera to the reference coordinate system (xr , yr , zr ). In this method, the binocular camera is fixed on the robot leg, so the transformation matrix M2 is essentially the analog transformation between coordinate systems. The calculation is iteratively optimize based on multiple 3D point pairs in the different coordinate systems to improve the accuracy. ⎡ ⎤ ⎡ ⎤  xc xr R t ⎣ yc ⎦ ⎣ yr ⎦ = (2) 01 zr zc where R and t denote the rotation matrix and translation matrix separately.

342

2.3

H. Xu et al.

Joint Angle Calculation Using Deep Learning

Due to the non-linear terms in robot inverse kinematics, a neural network is used to calculate the joint angles instead of the traditional methods, such as the DH model, in this method. The inverse kinematic neural network’s architecture is a five-layer multilayer perceptron shown as Fig. 3. The activation function is Leaky ReLU. This neural network inputs the coordinates of the robot leg and outputs the three joint angles. Finally, the error will be compensated based on the actual joint angles and the expected value.

Fig. 3. The inverse kinematic neural network’s architecture.

3 3.1

Experimental Validation and Discussion Experimental Validation

This method is tested on the quadruped robot designed by China North Vehicle Research Institute. In our validation, we choose a small-size binocular camera, which resolution is 720p and distortion is less than 0.3%. Two black concentric circles on a white background are chosen as the marker. Figure 4 shows the assembling of the components mentioned above. 3.2

Results and Discussion

Table 1 shows the calibration results of this experimental validation. The calibration accuracy is distributed between 0.05◦ and 0.5◦ , which is significantly more precise than 3◦ –5◦ , which is the range of the motor zero point drift at the robot leg joints. We believe this result is within the acceptable range. Furthermore, the prediction of the neural network only requires 0.1–1 s on the microcomputer or ordinary laptop (predict by CPU). Even though a robot dog needs calibration twice by this method, the calibration period can be reduced to 1 min from 15 min of manual calibration, even considering the installment time.

An Online Intelligent Kinematic Calibration Method

343

Fig. 4. The assembling of the calibration system components. Table 1. The calibration results of the visual calculation method. Angle (◦ ) Case 1 Actual Calibrated

4

Joint 1

–1.0

–1.05752

Joint 2

39.4

39.15459

Joint 3

–65.66 –65.74139

Error

Case 2 Actual Calibrated Error

–0.05752 –8.0

–7.53860

–0.24541 39.4

39.64617

–0.08139 –65.66 –65.60477

0.46140 0.24617 0,05523

Conclusion

In this article, we propose a calibration method for quadruped robots based on machine vision and deep learning, aiming to overcome the drawback of the previous manual calibration: low speed and cumbersome operation. This calibration method can significantly improve calibration speed within the acceptable error range by automatically completing a series of processes composed of image capture and correct, target tracking, marker detection, robot leg positioning, and joint angles calculation. In future work, error models based on deep learning will be established to improve calibration accuracy. The deep learning model with artificial neural networks can express the non-linear terms contained in some intermediate processes simpler than traditional physical models. Acknowledgement. This work is supported by National Natural Science Foundation of China under Grant 52175237, and National Key R&D Program of China under Grant 2019YFA0706701.

References 1. Smisek, J., Jancosek, M., Pajdla, T.: 3D with Kinect . In: 2011 IEEE International Conference on Computer Vision Workshops, pp. 3–25 (2013)

344

H. Xu et al.

2. Horaud, R., Dornaika, F.: Hand-eye calibration. Int. J. Robot. Res. 14(3), 195–210 (1995). https://doi.org/10.1177/027836499501400301 3. Sreenivas, T., Subhash, K.: Inverse kinematics in robotics using neural networks. Inf. Sci. 116, 147–164 (1999) 4. Xu, Q., et al.: SuperMeshing: a new deep learning architecture for increasing the mesh density of physical fields in metal forming numerical simulation . J. Appl. Mech. 89, 1–11 (2002) 5. Jiang, H., Nie, Z., Yeo, R., Farimani, A.B., Kara, L.B.: StressGAN: a generative deep learning model for two-dimensional stress distribution prediction . J. Appl. Mech. 88(5), 051005 (2021) 6. Nie, Z., Lin, T., Jiang, H., Kara, L.B.: Topologygan: topology optimization using generative adversarial networks based on physical fields over the initial domain . J. Mech. Des. 143(3), 031715(2021) 7. Pfaff, T., Fortunato, M., Sanchez-Gonzalez, A., et al.: Learning mesh-based simulation with graph networks. arXiv preprint arXiv:201003409 (2020) 8. Xie, Y., Franz, E., Chu, M., et al.: tempogan: a temporally coherent, volumetric GAM for super-resolution fluid flow. ACM Trans. Graph. (TOG) 37(4), 1–15 (2018) 9. Klaus, H., Strobl, G.H.: Optimal hand-eye calibration. In: 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems (2006) 10. NøRG˚ ard, P.M., Ravn, O., Poulsen, N.K., et al.: Neural Networks for Modelling and Control of Dynamic Systems-A Practitioner’s Handbook, Springer, London (2000) 11. Bayramoglu, E., Andersen, N.A., Ravn, O., et al.: Pre-trained neural networks used for non-linear state estimation. In: Proceedings of the 2011 10th International Conference on Machine Learning and Applications and Workshops. IEEE (2011) 12. Hasan, A.T., Ismail, N., Hamouda, A.M.S., et al.: Artificial neural network-based kinematics Jacobian solution for serial manipulator passing through singular configurations. Adv. Eng. Softw. 41(2), 359–67 (2010)

Space Robot and On-orbit Service

Non-singular Fast Terminal Sliding Mode Fuzzy Adaptive Control of Floating-Based Three-Link Space-Robot with Dead-Zone Zhihao Zhang1 , Xiaoyan Yu1,2(B) , and Jiabao Gong1 1 School of Mechanical Engineering and Automation, Fuzhou University, Fuzhou, China 2 Fluid Power and Electro-Hydraulic Intelligent Control Fujian Provincial University Key

Laboratory, Fuzhou University, Fuzou 350116, China

Abstract. For the angle tracking control problem of free floating three bar space manipulator with disturbance torque and joint dead zone, non singular fast terminal sliding mode fuzzy adaptive control is adopted to realize finite time stabilization of tracking error. In this method, a nonsingular fast terminal sliding surface is used to make the state variables converge rapidly in the sliding phase; The improved double power reaching law is selected to improve the convergence speed of state variables in the approaching motion stage, and fuzzy control is used to eliminate chattering and improve the control accuracy. At the same time, considering the dead time characteristics and disturbance torque of the manipulator joint, an adaptive compensator is designed to approach the upper bound of the dead time characteristics through adaptive control, so as to ensure the effective implementation of the tracking control. Finally, based on Lyapunov method, the global stability of the closed-loop system is proved theoretically, and the numerical simulation shows that the controller can effectively realize the task space path tracking control, effectively improve the trajectory tracking accuracy and anti disturbance ability, and alleviate the chattering phenomenon in the output of the controller. Keywords: Space robot · Non-singular fast terminal sliding mode · Dead-zone characteristics · Adaptive · Fuzzy control

1 Introduction In future space activities, space robots will play an increasingly important role, and the dynamics and control of space robot systems have also attracted the attention of researchers from all over the world [1–3]. Unlike ground-based fixed-base robots, the carrier of a space robot floats freely, and the position and attitude control system is often closed during the operation of the manipulator. There is a strong dynamic coupling between the manipulator and the carrier. Therefore, the dynamics and control problems of space robots are far more complex than those of fixed ground robots. Its outstanding features are as follows: The space robot system is a non-holonomic dynamic system, and the inertial parameters in the system dynamics equation do not conform to the usual linear laws. There are many challenges in the control of space robots [4, 5]. Therefore, © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 347–359, 2021. https://doi.org/10.1007/978-3-030-89092-6_32

348

Z. Zhang et al.

the control method of the ground robot [6, 7] cannot be directly applied to the space robot. The fast and accurate motion control of the floating-based space robot in the task space is a difficult problem. Relevant scholars have carried out research on this complicated nonlinear system control problem. Among them, sliding mode control is a nonlinear control method, which has the characteristics of strong robustness and low sensitivity to parameters. Man et al. [8] proposed a terminal sliding mode algorithm. The terminal sliding mode control algorithm selects appropriate parameters to enable the system state to converge in a short finite time; Feng et al. [9] proposed that the non-singular terminal sliding mode perfectly solves the singularity problem of the control rate, so that there is no singularity in the control rate. Under the premise of the point, the system state converges in a finite time; Lu et al. [10] proposed a fast non-singular terminal sliding mode to obtain a faster convergence rate. In addition, sliding mode control algorithms still have chattering problems, and eliminating chattering is a key step in the application of sliding mode technology. Reaching law design methods such as constant velocity reaching law, exponential reaching law and power reaching law are often used. Etc. [11]. Li et al. [12] proposed the double-power reaching law, which makes the sliding mode and its first derivative converge to the steady-state error bound in a finite time. Wu et al. [13] proposed an improved double-power reaching law with variable coefficients to improve the convergence speed of state variables in the approaching motion stage and weaken the controller output chattering. In recent years, sliding mode control has been combined with intelligent control such as neural networks and fuzzy systems [14, 15], so that the controller design does not depend on precise mathematical models. Fuzzy systems can not only use fuzzy logic to adjust the output of the controller to reduce chattering [16–18], but also approximate the uncertainty information of the model. Yoo et al. [19] used fuzzy systems to compensate unknown variables such as friction and modeling errors in the manipulator model. Tran et al. [20] uses neural networks to approximate the information of the entire manipulator model, which provides a new idea for the further application of fuzzy systems. In addition, in the power transmission system of the space manipulator, there are various non-linear factors such as backlash, friction dead zone, friction and saturation that affect the control performance of the system. Frictional dead zone nonlinearity is a major nonlinear characteristic, in addition to causing output errors, it will also cause limit cycle oscillations to the system. Therefore, in the past 10 years, it has received more and more attention in the field of high-precision control [21, 22]. Terminal sliding mode control is suitable for fast and accurate motion control of complex nonlinear systems. There are also some applied researches in the field of space robot control. However, the above research did not consider the existence of frictional interference torque and the dead zone characteristics of the actuator, which limited its practical application. Based on the above research, this paper proposes a non-singular fast terminal sliding mode fuzzy adaptive control method. This method uses the combination of variable coefficient double power approximation rate and non-singular fast terminal sliding mode surface, fusion of fuzzy algorithm to improve the system state convergence speed and anti-interference, combined with adaptive compensator, to achieve effective tracking of the system in the presence of joint dead zone and In the case of interference

Non-singular Fast Terminal Sliding Mode Fuzzy Adaptive Control

349

torque. The stability analysis of Lyapunov proves that the tracking controller can realize the global asymptotic tracking control of the joint angle. Finally, the control method is applied to a planar three-link free-floating space manipulator. The simulation results show that this method can realize the tracking control of the joint angle trajectory in a limited time under the presence of interference factors, and has a strong anti-interference ability.

2 System Dynamics Equation Without losing generality, consider the three-bar space manipulator system of free floating base for plane motion. The structure diagram of the space manipulator system is shown in Fig. 1. The whole system consists of body B0 , and bar B1 , B2 , B3 .

Fig. 1. Schematic diagram of three-pole space robot system for free floating base

The symbol is defined as follows: B0 is the distance from noumenon centroid O0 to joint O1 ; L i is the length of the connecting rod Bi (i = 0, 1, 2, 3); mi is the mass of Bi (i = 0, 1, 2, 3); J i is the moment of inertia relative to the center of mass of Bi (i = 0, 1, 2, 3). Establish inertial space coordinate system (O X Y Z); θ is the angle between the X 0 and the Y 1 ; qi is the angle between the X i−1 and the X i (i = 1, 2, 3). Combined with the system centroid principle, The vector radius ri of the center of mass of each part of the system and the origin O of the inertial coordinate system are expressed as r0 r1 r2 r3

= r0 = r0 + l0 e0 + a1 e1 = r0 + l0 e0 + l1 e1 + a2 e2 = r0 + l0 e0 + l1 e1 + l2 e2 + a3 e3

(1)

The ei is the base vector along the X i . Ignoring the weak gravity gradient, the space manipulator system is a free floating rootless multibody system without external force. The system obeys momentum conservation and momentum moment conservation at relative O points. If the initial momentum and momentum moment of the system are all 0, the generalized coordinates

350

Z. Zhang et al.

T  of the system are taken as q = x0 y0 q0 q1 q2 q3 , Where X 0 and Y 0 represent the abscissa and ordinate functions of the carrier centroid respectively. From the kinetic energy expression of the system and using Lagrange second kind of kinetic equation, the dynamic equation of the three-bar space robot can be obtained as follows: D(q)¨q + C(q, q˙ )˙q = ( 0T τ T )T + ( 0T τ Td )T

(2)

Among them, D(q) is a 6 × 6 system symmetric and positive definite matrix, C(q, q˙ )  T is a 6 × 6 matrix containing Coriolis force and centrifugal force, τ = τ0 τ1 τ2 τ3 is a driving torque of the body and joints, and τ d is a 4 × 1 interference torque. T T   If qc = q0 q1 q2 q3 , then q = x0 y0 qc , the formula (2) is simplified: D_(q)¨qc + C_(q, q˙ )˙q = τ T + τ d

(3)

D_(q) = (D24 + C 24 )(D22 + C 22 )−1 D24 + D44 Among them; C_(q, q˙ ) = (D24 + D24 )(D22 + D22 )−1 D42 + D44     C 22 C 24 D22 D24 C(q, q˙ ) = . D(q) = D42 D44 C 42 C 44 Because of the mechanical design and manufacture, the system joint usually has the “dead-zone” characteristic, which will deteriorate the adjustment quality of the system and affect the output accuracy of the system. The effect of joint dead-zone characteristics on the control system can be represented by Fig. 2, where u represents the control input and τ represents the actuator output. usually, the two are not the same.

Fig. 2. Dead zone characteristics of the actuator

In general, the dead zone characteristics of the system can be described by the relationship between the control input u and the actuator output τ. This paper considers the following dead-zone characteristics: ⎧ ⎨ k(ui− di+ ), ui ≥ di+ ; τi = (4) 0, di− < ui < di+ ; ⎩ k(ui− di− ), ui ≤ di− ; Where d i+ > 0; d i- < 0; kd i + > 0 and constant, u represents the control input; τ represents the output of the actuator. The output of the actuator can be divided into two parts, that is τ = ku + δ(u)

(5)

Non-singular Fast Terminal Sliding Mode Fuzzy Adaptive Control

351

Among them: ⎧ u ≥ di+ ⎨ −kdi+ , δi (u) = ku, di− ≤ u ≤ di+ ⎩ −kdi− , u ≥ di−

(6)

Assumption 1: The dead-zone characteristic δ and the interference moment τd are bounded, that is. |δ i (u)| ≤ δ maxi (i = 1, 2, 3) (7) |τ di | ≤ τ d maxi (i = 0, 1, 2, 3) Where δ maxi (i = 1, 2, 3); τ d maxi (i = 0, 1, 2, 3) is the unknown positive number, and δ max ∈ R3×1 ; τ d max ∈ R4×1 , δ maxi τ d maxi is the element.

3 Controller Design 3.1 Design of Non-singular Fast Terminal Sliding Modular Adaptive Controller trajectory, define the tracking error Set qcd as the expected trajectory  and qc as the actual  e = qcd − qc . Define |·|abs = |·1| |·2| . . . |·n| . Select the sliding surface: p

m

q n sgn(e) + β|˙e|abs sgn(˙e) s = e + α|e|abs

(8)

Where α = diag(αi ); β = diag(βi ) Among them (i = 0, 1, 2, 3), the controller parameters αi βi are positive, the m, n, p, q are positive odd number, and satisfy 2 > mn > 1; p m q> n . Throughout the sliding stage (s = 0), when the state of the system is far away from p

q the equilibrium point (|ei | > 1), in α −1 |e|abs sgn(e), the exponent of e is larger and plays a major role. At this time, the exponent of e is greater than 1, which effectively speeds up the convergence speed of state variables. When the system is near the equilibrium point (|ei | ≤ 1), the e high term can be ignored, and the state variable still has fast convergence speed. The two parts guarantee the global fast convergence. At the same time, the singular problem is avoided by ensuring that mn is greater than 1. The derivation of formula (8) can be obtained p m p m q −1 n −1 s˙ = e˙ + α −1 (|e|abs )˙e + β −1 (|e|abs )¨e q n

(9)

From formula (3): e¨ = q¨ c − q¨ dc = D_−1 (τ − τ d − C_(q, q˙ )˙q) − q¨ dc

(10)

Defines the following vectors: p m p m q −1 n −1 )˙e + β −1 (|e|abs )D_−1 (−C_(q, q˙ )˙q − D_¨qdc ) F  e˙ + α −1 (|e|abs q n

(11)

352

Z. Zhang et al.

N  β −1

m m n −1 (|e|abs )D_−1 n

(12)

The dynamic equations about the sliding surface of the system are as follows: s˙ = Nτ + F + Nτ d

(13)

Therefore, the path tracking problem of system (1) is transformed into the stabilization control problem of system (13). If the system can reach the sliding mode surface in a limited time, the manipulator will rotate according to the specified rotation angle. Double power convergence rate with variable coefficients:

 r1 r2 s˙ = − μ1 |s|abs + μ2 |s|abs (2I + 2e)sgn(s) (14) μ1 = diag[μ10 , μ11 , μ12 , μ13 ] μ2 = diag[μ20 , μ21 , μ22 , μ23 ] The μ1i , μ2i is a controller parameter and all are positive. r1 = pq11 ; r2 = mn11 , in which p1 ; q1 ; m1 and n1 are positive odd numbers and satisfied r1 > 1; 0 < r2 < 1; e = diag[max(|e0 ||˙e0 |, ..., max(|e3 ||˙e3 |]. When the system state isaway from the sliding mode surface (|si | ≥ 1), the coefficient less than 1 can be ignored, μ1 |s|r1 abs (I + 2e)sgn(s) can approach the sliding surface at a higher speed. When the system is close to the sliding mode surface r2 (|si | < 1), μ2 |s|abs (I + 2e)sgn(s) can still guarantee sufficient approaching speed, The two parts can ensure that the approaching motion maintains sufficient convergence speed throughout the entire process. When the interference torque and dead-zone are not considered, the control torque can be written as: 

r2 (15) u = N −1 [−F − μ1 |s|r1 abs + μ2 |s|abs (2I + 2e)sgn(s)]

3.2 Design of Fuzzy Compensation System Sliding mode control in the process of switching itself will produce buffeting and affect the quality of system control. In order to eliminate this buffeting and ensure the control accuracy and response speed of the system, we adopt continuous method and fuzzy control method. Set s˙s as the input of fuzzy control unit, λ as the output of fuzzy control unit. When the system is far away from the equilibrium point, the faster fitting speed is adopted, and when the system is close to the equilibrium point, the slower fitting speed is selected. Define the fuzzy language set {NB, NM, ZO, PM, PB} = {negative big, negative medium, zero, positive median, positive big}, and use the membership function shown in Figs. 3 and 4 to blur the input and output. Use the center of gravity method when unblurring. The fuzzy rules are: When s˙s is NB, λ is NB; When s˙s is NM, λ is NM;

Non-singular Fast Terminal Sliding Mode Fuzzy Adaptive Control

353

Fig. 3. Membership function of input variable Fig. 4. Membership function of output variable

When s˙s is ZO, λ is ZO; When s˙s is PM, λ is PM; When s˙s is PB, λ is PB; System input with fuzzy compensator can be written as: 

r2 |s| + μ u = N −1 [−F − μ1 |s|r1 2 abs [(2 + λ)(I + e)]sgn(s)] abs

(16)

3.3 Adaptive Compensator Design When considering dead-zone characteristics and interference torque, it can be changed to the following form: s˙ = Nδ + Nu + F + Nτd

(17)

Nδ represents the influence of nonlinear dead-time characteristics on the control system. In order to make up for the influence of dead-time characteristics and interference torque on the control system, an adaptive compensator is designed on the basis of nonsingular fast terminal sliding mode control. The structure diagram is as follows (Fig. 5):

Fig. 5. Control system with adaptive compensator

From Eq. (9) to Eq. (15), the control torque can be obtained, and the control torque is divided into two parts: u = u1 + u2 Among them: 

r1 r2 u1 = N −1 [−F − μ1 |s|abs + μ2 |s|abs [(2 + λ)(I + e)]sgn(s)] u2 = −diag(sign(sT N))τˆ dmax − diag(sign(sT N))δˆ max

(18)

(19)

354

Z. Zhang et al.

According to the above control law u1 the output control instruction of the sliding mode variable structure controller representing the system is designed to make the tracking error of the system reach the sliding mode surface in a limited time, and then realize the finite time stabilization of the tracking error of the system. u2 represents the output of the control adaptive compensation mechanism, which is designed to eliminate the dead zone characteristics of the system actuator and the influence of friction interference torque on the system tracking. The updated law of its parameters is designed as follows:

T

τ˙ˆ dmax = −ε1 sT N (20)

T

˙ δˆ max = −ε2 sT N

4 Stability Proof Theorem: For systems (2)–(3), in the presence of frictional interference torque and actuator dead zone characteristics, if Assumption 1 is satisfied, control law (18)–(19) and adaptive update law (20) are adopted, and the system The tracking error will converge to zero in a finite time. Consider the following Lyapunov functions:   1 T 1 ˜T 1 T (21) s s + τ˜ dmax τ˜ dmax + δ max δ˜ max V = 2 ε1 ε2 Among them, τ˜dmax = τˆdmax − τdmax ; δ˜max = δˆmax − δmax represents the estimation error of the upper bound of interference torque and dead-zone characteristics. For the above Lyapunov functions, the first order differential of the time can be obtained: 1 1 T + δ˜ ˙ V˙ = sT s˙ + τ˜ T ˙ ε1 dmax τˆ dmax ε2 max δˆmax

(22)

Substitute formulas (17) and (18) into (22) to obtain: 1 T 1 + δ˜ ˙ V˙ = sT (Nδ + Nu1 + Nu2 + F + Nτ d ) + τ˜ T ˙ ε1 dmax τˆ dmax ε2 max δˆmax Substitute control (19) into formula (23) and simplify: 

r1 V˙ = −sT [ μ1 |s|abs + μ2 |s|r2 abs [(2 + λ)(I + e)]sgn(s)]+ 

sT N −diag(sign(sT N))τˆ dmax − diag(sign(sT N))δˆ max + sT Nδ As defined above:

+ sT Nτ

d

+

1 T ε1 τ˜ dmax τ˙ˆ dmax

+

(23)

(24)

1 ˜T ε2 δ max δ˙ˆ max



sT Ndiag(sign(sT N) = sT N

(25)

Non-singular Fast Terminal Sliding Mode Fuzzy Adaptive Control

355

Noting:

sT Nτ dmax ≤

sT N

τˆ dmax sT Nδ max ≤ sT N ˆδ max

(26)

As a result:



r1 r2 V˙ ≤ −sT [ μ1 |s|abs + μ2 |s|abs [(2 + λ)(I + e)]sgn(s)]−





T

s N τˆ dmax − sT N ˆδ max + sT N δmax + sT N τdmax T + ε11 τ˜ T ˙ + ε12 δ˜ δ˙ˆ max max dmax τˆ dmax   = −sT [ μ1 |s|r1 + μ2 |s|r2 [(2 + λ)(I + e)]sgn(s)]+  

 T

1˙

T 1 ˙ T T



˜ ˆ τ˜ dmax s N + ε1 τˆ dmax +δ max s N + ε2 δ max

Substitute the adaptive renewal rate (20) to obtain:

 r1 r2 V˙ ≤ −sT μ1 |s|abs + μ2 |s|abs [(2 + λ)(I + e)]sgn(s)

(27)

(28)

And because the diagonal elements of μ1 and μ2 are all positive numbers, the parameters of the designed fuzzy controller can be obtained: |λ| < 2, and s = 0 therefore:

 V˙ ≤ −sT μ1 |s|r1 + μ2 |s|r2 [(2 + λ)(I + e)]sgn(s) < 0 (29) From the Lyapunov stability theorem, the closed-loop stability of the system is obtained, and the tracking error of the system will converge to zero in a finite time.

5 Simulation Verification To verify the effectiveness of the proposed controller, It is applied to the system shown in Fig. 1 for simulation. In simulation, Assume that the end claw hand of the manipulator does not hold the load. Assuming that the desired trajectory of the floating-based space robot body and the two joint spaces is: q0d = 2sin(0.6t); q1d = 0.5sin(0.8t); q2d = 1 + sin(1.5t); q3d = 2 + sin(t), and p/q = 5/3; m/n = 11/9 α = diag(1, 1, 1, 1); β = diag(6, 5, 4.5, 5); μ1 = diag(2, 2, 2, 2); μ2 = diag(3, 3, 3, 3); r1 = 5/3; r2 = 3/5. The initial position of the system is: q0 (0) = 0.25; q1 (0) = 0.25; q2 (0) = 1.25; q3 (0) = 2.25. The inertial parameters of the system are selected as follows (Table 1): In order to illustrate the superiority of the non-singular fast terminal sliding mode fuzzy control law (FTSM_Fuzzy), take it as controller 1, and compare it with the following three types of controllers. Controller 2: In order to highlight the performance of non-singular fast terminal sliding mode fuzzy control, a non-singular fast terminal sliding mode (FTSM) controller is designed. The expressions are as follows: 

r1 r2 (30) τ = N −1 [−F − μ1 |s|abs + μ2 |s|abs (I + 2e)sgn(s)]

356

Z. Zhang et al. Table 1. Physical parameters of space manipulator.

Parameter

Numerical value

Parameter

Numerical value

m0

40 kg

L2

3m

m1

2 kg

L3

3m

m2

1 kg

J0

34.17 kg·m

m3

1 kg

J1

3 kg·m

L0

1.5 m

J2

3 kg·m

L1

3m

J3

3 kg·m

Controller 3: in order to highlight the control effect of double power convergence rate with variable coefficient, a non-singular terminal sliding mode (STSM) controller is designed. The expression is as follows: p q sgn(e) s = e + β|e|abs

(31)

  p    p q −1 −1 (32) τ = D_ diag |e|abs e˙ − D_ H_˙qc − τd + D_¨qcd − K 1 s − K 2 sgn(s) q 

Of which: K1 = diag(5, 5, 5, 5); K2 = diag(3, 3, 3, 3). Controller 4: in order to highlight the advantages of non-singular terminal sliding mode, the traditional linear sliding mode controller (SMC) is designed. The expressions are as follows: s = e˙ + βe

(33)

   τ = D_ β e˙ − D_−1 H_˙qc − τd + D_¨qcd − K 1 s − K 2 sgn(s)

(34)

To compare the robustness and immunity of the above controller more intuitively, the step disturbance (unit) added at t = 3 s, the duration 0.5 s, the joint angle error image are as follows: τ = [10; 50; 40; 20] N · m (Figs. 6, 7, 8 and 9). It can be seen from the comparison that the fuzzy control tracking performance of non-singular fast terminal sliding mode is the best and the anti-interference ability is the strongest. The terminal sliding mode and the double power convergence rate with variable coefficient can reduce the trajectory tracking error and improve the control accuracy of the manipulator system. In order to show the performance of the adaptive compensator, the interference torque is set as τd 1 = 10sin(2.5 π t); τd 2 = 10sin(3 π t); τd 3 = 8cos(1.8 π t); τd 4 = 8cos(2 π t), and the actual upper limit of the joint dead zone is set as δmax1 = 35; δmax2 = 45; δmax3 = 45. The initial position of the system is q0 (0) = 0.25; q1 (0) = 0.15; q2 (0) = 1.15; q3 (0) = 2.15. The comparison between the adaptive compensator and the non-adaptive compensator is as follows (Fig. 10):

Non-singular Fast Terminal Sliding Mode Fuzzy Adaptive Control

Fig. 6. Attitude angular tracking error

Fig. 7. Angle 1 tracking error

Fig. 8. Angle 2 tracking error

Fig. 9. Angle 3 tracking error

Fig. 10. Angle tracking error (ON/OFF adaptive compensator)

357

358

Z. Zhang et al.

It can be seen from the above comparison that when there is interference torque and joint dead zone characteristics, the controller proposed in this paper can still effectively complete the tracking task, indicating the effectiveness of the controller design.

6 Conclusions In this paper, a non-singular fast terminal sliding mode fuzzy controller is designed, which makes the free floating space manipulator with interference torque and joint dead zone characteristics, which can track the specified reference path in a limited time. Considering the joint dead zone characteristics and interference torque of space manipulator system, the controller has certain practical value. Finally, the method proposed in this paper is applied to the tracking control simulation of the planar three-link free floating manipulator. The results show that the method can achieve the tracking target well and has good robustness.

References 1. Liu, C.A., Li, G.D., Wu, K.H., Hong, B.G.: Research summarizing of free flying space robot. Robot 24(4), 380–384 (2002) 2. Zhang, W.H., Ye, X.P., Ji, X.M., Wu, X.L., Zhu, Y.F., Wang, C.: Development summarizing of space robot technology national and outside. Flight Dyn. 31(03), 98–202 (2013) 3. King, D.: Space servicing: past, present and future. In: Proceeding of the 6th International Symposium on Artificial Intelligence. Robot and Automation in Space, Montreal, Canada, pp. 346–351(2001) 4. Tian, Z., Wu, H.: Spatial operator algebra for free-floating space robot modeling and simulation. Chin. J. Mech. Eng. 23(5), 635–640 (2011) 5. Feng, F., Li, Y.W., Liu, H., et al.: Design schemes and comparison research of the end-effector of large space manipulator. Chin. J. Mech. Eng. 25(4), 674–687 (2012) 6. Zhao, J.B., Wang, X., Zhang, G., et al.: Design and implementation of membrane controllers for trajectory tracking of nonholonomic wheeled mobile robots. Integr. Comput. Aided Eng. 23(1), 15–30 (2015) 7. Van Cuong, P., Nan, W.Y.: Adaptive trajectory tracking neural network control with robust compensator for robot manipulators. Neural Comput. Appl. 27(2), 525–536 (2015). https:// doi.org/10.1007/s00521-015-1873-4 8. Man, Z., Paplinski, A.P., Wu, H.R.: A robust MIMO terminal sliding mode control scheme for rigid robotic manipulator. IEEE Trans. Autom. Control 39(12), 2464–2469 (1994) 9. Feng, Y., Yu, X., Man, Z.: Non-singular terminal sliding mode control of rigid manipulators. Automatica 38(12), 2159–2167 (2002) 10. Lu, K., Xia, Y.: Adaptive attitude tracking control for rigid space-craft with finite time convergence. Automatica 49(12), 3591–3599 (2013) 11. Gao, W.B.: Theory and design method of variable structure control, pp. 211–225. Science Press(1995) 12. Li, H.J., Cai, Y.L.: Sliding mode control method based on double power reaching law. Control Decis. 31(3), 498–502 (2016) 13. Wu, A.G., Wu, S.H., Dong, N.: Non singular fast terminal sliding model fuzzy control of robotic manipulator. J. Zhejiang Univ. (Eng. Sci.) 53(05), 49–58 (2019) 14. Nekoukar, V., Erfanian, A.: Adaptive fuzzy terminal sliding mode control for a class of MIMO uncertain nonlinear systems. Fuzzy Sets Syst. 179(1), 34–49 (2011)

Non-singular Fast Terminal Sliding Mode Fuzzy Adaptive Control

359

15. Mao, Y.Q., Huang, Y.L., Wang, Z.F.: Adaptive fuzzy sliding mode decentralized control of high-order coupling robot systems. J. Dyn. Control 9(4), 337–341 (2011) 16. Li, T.H.S., Huang, Y.C.: MIMO adaptive fuzzy terminal sliding-mode controller for robotic manipulators. Inf. Sci. 180(23), 4641–4660 (2010) 17. Amer, A.F., Sallam, E.A., Elawady, W.M.: Adaptive fuzzy sliding mode control using supervisory fuzzy control for 3 DOF planar robot manipulators. Appl. Soft Comput. J. 11(8), 4943–4953 (2011) 18. Soltanpour, M.R., Khooban, M.H.: A particle swarm optimization approach for fuzzy sliding mode control for tracking the robot manipulator. Nonlinear Dyn. 74(1–2), 467–478 (2013). https://doi.org/10.1007/s11071-013-0983-8 19. Yoo, B.K., Ham, W.C.: Adaptive control of robot manipulator using fuzzy compensator. IEEE Trans. Fuzzy Syst. 8(2), 186–199 (2000) 20. Tran, M.-D., Kang, H.-J.: A novel adaptive finite-time tracking control for robotic manipulators using nonsingular terminal sliding mode and RBF neural networks. Int. J. Precis. Eng. Manuf. 17(7), 863–870 (2016). https://doi.org/10.1007/s12541-016-0105-x 21. Lewis, F.L., Campos. J., Selmic, R.: Neural-fuzzy control of industrial systems with actuator nonlinearities, pp. 23–30. University City Science Center, New York (2002) 22. Lu, Y., Liu, J.K., Sun, F.C.: Actuator nonlinearities compensation using RBF neural networks in robot control system. In: Proceedings of the 4th CESA Multi-Conference on Computation Engineering in Systems Application, Beijing, China, vol. 1, pp. 231–238 (2006)

Regulation of Space Manipulators with Free-Swinging Joint Failure Based on Iterative Learning Control Yingzhuo Fu(B) , Qingxuan Jia, Gang Chen, and Yifan Wang Beijing University of Posts and Telecommunications, Beijing 100876, China

Abstract. To ensure that tasks can be completed after a free-swinging joint failure occurs, an iterative learning control method for the space manipulator is proposed in this paper. First, the dynamics coupling relationship between the fault and active joint is established. Second, by planning the active joint motion with a quintic polynomial interpolation function, the regulation of the fault joint is transformed into minimizing the error between the actual and desired angle of the fault joint. Third, based on ILC, the active joint trajectory is planned repeatedly and the desired trajectory is obtained at last. In this way, the fault joint can be precisely regulated to the desired angle once the active joint moves along its panned trajectory. The simulation results indicate that the fault joint is precisely regulated to the desired angle, and the regulation error can reach 0.1° at least, verifying the correctness and effective-ness of the regulation method. Keywords: Space manipulator · Free-swinging joint failure · Iterative learning control · Dynamics coupling relationship

1 Introduction Space manipulators are usually used to assist or replace the astronaut to perform space missions such as space station maintenance and assembly [1]. However, due to the burdensome tasks and harsh space environment, joint failure is prone to happen, especially the free-swinging joint failure [2, 3]. When the failure occurs, the fault joint cannot provide any motion and torque as expected due to its swinging state, easily leading to a task failure [3]. To ensure that tasks can be completed after the free-swinging joint failure occurs, the fault-tolerant control method needs to be studied. Some scholars have studied on the fault-tolerant control method for space manipulators with free-swinging joint failure (also called an underactuated space manipulator). English and Maciejewski [4] determined the desired angle of the fault joint by taking the postfailure workspace as the optimization objective, and regulated the fault joint to the desired angle through a speed control method. Maciel et al. [5] designed a robust controller to regulate the free-swinging joint via the active joint. Based on the dynamics coupling relationship between the fault and active joint (DCRFAJ), Chen et al. [6] regulated the fault joint to the desired angle with a PD control method. As can be seen that, © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 360–370, 2021. https://doi.org/10.1007/978-3-030-89092-6_33

Regulation of Space Manipulators with Free-Swinging Joint Failure

361

since the free-swinging joint con not afford torque, it has to be driven by active joints based on the DCRFAJ. Space manipulator with free-swinging joint failure belongs to a kind of underactuated manipulators [7], so the regulation of the fault joint can be realized with control methods of underactuated manipulators. Shin and Lee [8] proposed a feedback linearizing decoupling dynamic control scheme to obtain a desired trajectory of the free-swing joint without singularity. He et al. [9] proposed a virtual model lead control method to make the free-swinging joint move along the desired trajectory. The above methods depend on the precise dynamics model of the manipulator. However, it is hard to establish a precise dynamics model of the space manipulator [10]. Therefore, intelligent control methods are proposed, which can achieve precise regulation of the fault joint without the precise dynamics model of the system [11]. Intelligent control methods include neural network algorithm (NNA), genetic algorithm (GA), iterative learning control (ILC) [12], etc. ILC has the advantages of fast convergence speed and high control accuracy, and is good at solving the motion control of complex nonlinear systems, which is usually used in robot trajectory tracking and force control [13, 14]. The motion trajectory of the active joint determines whether the fault joint accurately arrive at the desired angle or not. So it is necessary to adjust and update the active joint trajectory according to the motion error for the fault joint. In this paper, ILC is introduced to continuously update the active joint trajectory according to the motion error of the fault joint, realizing a precise regulation of the fault joint to the desired angle. In summary, to ensure that tasks can be completed after a free-swinging joint failure occurs, an ILC method for space manipulators is proposed in this paper. The regulation of the fault joint is transformed into minimizing the error between the actual angle and the desired angle of the fault joint. Through planning the active joint trajectory repeatedly based on ILC, the fault joint is precisely regulated to the desired angle.

2 Dynamics Model for Space Manipulators with Free-Swinging Joint Failure

Fig. 1. Simplified model of space manipulator.

DCRFAJ is the basis of control of space manipulators. To obtain the DCRFAJ, the dynamics model of the space manipulator with free-swinging joint failure is established firstly. An n-DOF space manipulator attached on the space station is taken as the research object, whose base can be considered as fixed (because the mass and inertia of the space station are much larger than that of the space manipulator [15]). The simplified model of the space manipulator is shown in Fig. 1. Starting from the base, we number the joints as

362

Y. Fu et al.

 1 − n successivelyand denote them as Ji (i = 1, 2, . . . , n). i denotes the coordinate system of Ji , and I denotes the inertial coordinate system. The dynamics equation of the n-DOF space manipulator is: M(q)¨q + C(˙q, q) = τ

(1)

where q, q˙ , q¨ ∈ Rn×1 denote the joint angle, velocity and acceleration, respectively; M(q) ∈ Rn×n denotes the inertia matrix, which is a symmetric positive definite matrix; C(˙q, q) ∈ Rn×1 denotes the nonlinear term; τ ∈ Rn×1 denotes the joint torque. When a free-swinging joint failure occurs, all the joints are divided into two parts: fault joints and active joints, and Eq. (1) can be rewritten as:        C a (˙q, q) M aa (q) M ap (q) q¨ a τa + = (2) 0 M pa (q) M pp (q) q¨ p C p (˙q, q) The subscripts p and a represent fault joints and active joints, respectively. For the second row in Eq. (2), we have: M pa (q)¨qa + M pp (q)¨qp + C p (˙q, q) = 0

(3)

where M pp (q) is a positive definite matrix. So we have: q¨ p = −M −1 qa − M −1 q, q) pp (q)M pa (q)¨ pp (q)C p (˙

(4)

The relationship in Eq. (4) is the DCRFAJ. We can see that once the control law q¨ a of active joints are given, the acceleration q¨ p of fault joints can be calculated.

3 Iterative Learning Control of Space Manipulators with Free-Swinging Joint Since the fault joint cannot move independently due to lack of torque, it can only be driven by active joints based on their dynamics coupling relationship. In this section, the regulation of the fault joint is transformed into minimizing the error between the actual and desired angle of the fault joint, and the error can be minimized through planning the active joint trajectory repeatedly based on ILC. When the active joint moves along the planned trajectory, the fault joint is regulated to the desired angle. 3.1 Mathematical Description of the Regulation In general, space manipulators with free-swinging joint failure belong to a second-order nonholonomic system [16], so Eq. (4) is nonintegrable. In this way, we cannot obtain the angle mapping relationship between the fault and active joint directly, making it difficult to directly plan a motion trajectory of the active joint by the initial and desired state of the space manipulator. Therefore, how to plan correct trajectory of the active joint to regulate the fault joint is the research emphasis in this subsection.

Regulation of Space Manipulators with Free-Swinging Joint Failure

363

The regulation model of the free-swinging fault joint is defined as follows: find qa , q˙a , q¨ a  def  min f = qpa − qpd  ⎧ −1 (q)M (q)¨ ⎪ qa − M −1 q, q) pa ⎨ q¨ p = −M pp (q)C p (˙

t pp s.t. q˙ p = 0 q¨ p dt ⎪ ⎩ q = t q˙ dt p 0 p

(5)

where qpa and qpd denote the actual and desired angle of the fault joint at the end moment, respectively. To ensure smoothness of the motion, a quintic polynomial interpolation function is used to plan the active joint trajectory, as expressed as follows: θ (t) = a0 + a1 t + a2 t 2 + a3 t 3 + a4 t 4 + a5 t 5

(6)

where a0 –a5 denote undetermined coefficients. By differentiating Eq. (6), the velocity and acceleration function of the active joint are expressed as follows: θ˙ (t) = a1 + 2a2 t + 3a3 t 2 + 4a4 t 3 + 5a5 t 4

(7)

θ¨ (t) = 2a2 + 6a3 t + 12a4 t 2 + 20a5 t 3

(8)

Assume that the angle at the starting moment and at the end moment in planning are θ0 and θf , respectively, and the velocity and acceleration at the starting and ending point are both zero. According to Eq. (6)–Eq. (8) we have: ⎧ θ (0) = θ0 = a0 ⎪ ⎪ ⎪  ⎪ ⎪ ⎪ θ tf = θf = a0 + a1 tf + a2 tf2 + a3 tf3 + a4 tf4 + a5 tf5 ⎪ ⎪ ⎪ ⎪ ⎨ θ(0) ˙ = 0 = a1  (9) ˙ ⎪ θ tf = 0 = a1 + 2a2 tf + 3a3 tf2 + 4a4 tf3 + 5a5 tf4 ⎪ ⎪ ⎪ ⎪ ⎪ ¨ ⎪ θ(0) = 0 = 2a2 ⎪ ⎪ ⎪ ⎩ ¨ θ tf = 0 = 2a2 + 6a3 tf + 12a4 tf2 + 20a5 tf3 Substituting the Eq. (9) into (6), we obtain the undetermined coefficients as follows:

 a0 = θ0 , a2 = 0, a4 = −15 θf − θ0 /tf4 3   (10) a1 = 0, a3 = 10 θf − θ0 /tf , a5 = 6 θf − θ0 /tf5 The motion trajectory function of the active joint is obtained by Eqs. (6)–(10), and it contains only one undetermined coefficient, i.e., the desired angle of the active joint θf . Therefore, the regulation model of the free-swinging fault joint in Eq. (5) is transformed into: through searching a desired angle of the active joint and planning its motion trajectory utilizing the quintic polynomial interpolation, the motion of the fault joint is determined with the DCRFHJ, minimizing the error between the actual and desired angle of the fault joint.

364

Y. Fu et al.

3.2 Regulation of the Free-Swinging Fault Joint Based on Iterative Learning Control Considering the uncertainty of the dynamics model for the space manipulator, ILC is used to solve the regulation model in Eq. (5). That is to say, the desired angle of the active joint is determined through planning its trajectory repeatedly, continuously reducing the error between the actual and desired angle of the fault joint until it satisfied the allowable error. ILC is independent of the precise mathematical model of the system, and it utilizes the past experience to realize the control of the nonlinear and strongly coupled dynamic system with uncertainty. When applying ILC, the control input at each step is related to the control output at the previous step. With the increasing of the number of iterations, the system error tends to zero. The principle block diagram of ILC is shown in Fig. 2.

Fig. 2. The principle block diagram of ILC.

The system state equation is established first. Let x1 = qp , x˙ 1 = x2 , and take u = q¨ a and y = q˙ p as the control input and output, respectively. Based on Eq. (2), the system state equation is obtained as follows:       x2 0 x˙ 1 = + u −M −1 −M −1 C p x˙ 2 pp M pa (11)  pp   x1 y= 01 x2 The control output is transformed into the fault joint angle qp by integrating q˙ p , and the motion error is e = qpd − qp . The control input q¨ a can be obtained by the quintic polynomial interpolation. In another word, through setting the desired angle of the active joint, its motion trajectory, including q¨ a , is determined by Eq. (6)–(10). In this way, the control law, i.e., the desired angle of the active joint, is updated as follows: χk+1 = χk + δp ek

(12)

where δp is the  proportional learning gain, and it satisfies the convergence condition:  sup 1 − δp DB2 < 1 [17]. B and D are the coefficients of the control input and control output for the system state equation in (11), respectively, namely: ⎧ T  ⎨ B = 0 −M −1 pp M pa (13) ⎩D = 0 1     M Therefore, the convergence condition of ILC is sup 1+δp M −1  < 1, and pa pp t∈[0,ta ]

2

when k tends to be infinitely great, ek tends to be zero. The larger δp is, the better

Regulation of Space Manipulators with Free-Swinging Joint Failure

365

the convergence rate of the iteration error is, but it will affect the algorithm stability, leading to the oscillation of the control output. So δp should be selected appropriately to accelerate the convergence rate and avoid the output oscillation simultaneously. In addition, because the fault joint cannot be controlled independently, it may keep moving due to inertia after the active joint stop moving. Considering the friction in the joint, the friction torque τ f (script ‘f’ represents ‘friction’) is relative to the fault joint velocity by using the viscous friction model, shown as follows [18]: τ f = −fs q˙ p

(14)

where fs denotes a viscous friction factor. And Eq. (4) can be transformed into: q¨ p = −fs M −1 qp − M −1 qa − pp (q)˙ pp (q)M pa (q)¨ −1 M pp (q)C p (˙q, q)

(15)

Therefore, after the active joint stops moving, the fault joint will keep moving until the friction in the joint does negative work to stop it. In summary, the solving process of the regulation model for the free-swinging fault joint based on ILC is the following: Step1: Assume that t a and t p (tp ≥ 2ta ) represent the motion duration of the active joint and the fault joint, respectively, ψa0 represents the initial angle of the active joint, ψp0 and ψpd represent the initial and desired angle of the fault joint, respectively, and ε represents the allowable error for the fault joint; Step2: Assume that the desired angle of the active joint in the k-th iteration is ψak,d , and the active joint trajectory (including the joint angel ψak , velocity ψ˙ ak , and acceleration ψ¨ ak ) is obtained according to Eq. (6)–(10); Step3: Take the active joint trajectory as the control input, and the motion of the fault joint is obtained according to the DCRFHJ in Eq. (15). When the fault joint stops moving, its actual angle at the end point is ψpk , and compared with ψpd , the motion error is ek = ψpd − ψpk ; Step4: Judge whether |ek | ≤ ε: if the inequation is true, turn to Step5; otherwise, the desired angle of the active joint will be updated according to Eq. (12), i.e., ψak+1,d = ψak,d + δp ek , and turn to Step2; Step5: Output the active joint trajectory, and end iterating. From the above steps, the active joint trajectory is obtained. When the active joint moves along this trajectory strictly, the fault joint can be regulated to the desired angle via the DCRFHJ.

4 Simulations To verify the regulation strategy, simulation experiments are carried out with a 7-DOF space manipulator, as shown in Fig. 3. Its DH parameters and dynamic parameters are shown in Table 1 and Table 2, respectively.

366

Y. Fu et al.

Assume that the fourth joint J4 fails, and the configuration of the space manipulator at the failure moment is [40°, 150°, 60°, −60°, 15°, −85°, 40°]. Set the desired angle of the fault joint ψpd = 47°, the allowable iteration error ε = 0.1°, and the viscous friction factor fs = 9.8. We use J3 to regulate the fault joint while other active joints are locked. Z5 Z1

Z2

a3

X6

Z3

d7

Z6 d6

d5

d3 d2 d1

X0

X2

X3

X4

a4

Z4 X Z7 7 X5

Z0

X1

Fig. 3. Structure of 7-DOF space manipulator. Table 1. DH parameters of 7-DOF space manipulator. αi−1 (°)

Link i

ai−1 (m)

θi (°)

di (m)

1

0

0

90

2

−90

0

−90

0.15

0.2

3

−90

0

0

0.15

4

0

5

0

0.15

5

0

5

0

0.15

6

90

0

90

0.15

7

90

0

90

0.2

Table 2. Dynamics parameters of 7-DOF space manipulator. Link i

Mass mi (kg)

  Inertia Ixxi , Iyyi , Izzi (kg · m2 )

Coordinates of centroids Pcii (m)

1

42.5

[0.079, 0.053, 0.08]

[0, 0.075, 0]

2

42.5

[0.079, 0.053, 0.08]

[0, 0.075, 0]

3

70

[0.088, 1.458, 1.458]

[0.25, 0, 0]

4

70

[0.088, 1.458, 1.458]

[0.25, 0, 0]

5

42.5

[0.08, 0.08, 0.053]

[0, 0, −0.075]

6

42.5

[0.08, 0.08, 0.053]

[0, 0, −0.075]

7

42.5

[0.142, 0.142, 0.053]

[0, 0.05, −0.10]

According to ILC convergence condition and the parameters in Table 1 and Table 2, the proportional learning gain should satisfy: 0 ≤ δp ≤ 2.04

Regulation of Space Manipulators with Free-Swinging Joint Failure

367

Synthesizing the convergence rate and the algorithm stability, let δp = 4. Assume that the motion duration of the active joint is 7.5 s, and the desired angle of the active joint in the first iteration is ψa1,d = 75°. Assume that when the fault joint velocity satisfies ψ˙ pa ≤ 0.15°/s after the active joint stops, the brake device will be used to lock the fault joint, and the impact caused by the stopping of the fault joint will not cause damage to the space manipulator [19]. Based on the solving process shown in Fig. 3, the relationship between the error and number of iterations is shown in Fig. 4.

Fig. 4. The relationship between the error and number of iterations.

With the increase of the number of iterations, the iteration error decreases gradually. The first iteration error is e1 = 106.8° > ε, and when k = 7, the iteration error is |e7 | = 0.078° ≤ ε. In this iteration, the desired angle of the active joint is ψa7,d = 155.6°. Take ψa7,d = 155.6° as the control input, and the motion trajectory of the active joint and fault joint are shown in Fig. 5. From Fig. 5, after the active joint stops, the fault joint keeps moving, and at 29.4 s, its velocity ψ˙ pa = 0.1498°/s < 0.15°/s. At this moment, if the brake device is used to lock the fault joint, it will not cause any damage to the space manipulator. The actual angle of the fault joint is ψp7 = 46.922°, and compared     with the desired angle, the error is ψpd − ψp7  = |e7 | = 0.078°. The results verify the effectiveness of the regulation method.

Fig. 5. The motion trajectory of the active joint and fault joint.

The joint velocity and acceleration curves are shown in Fig. 6. We can see that when the active joint stops moving, the velocity and acceleration curves of the fault joint have obvious inflection points, and then gradually reduce to zero. In another word, without

368

Y. Fu et al.

the active joint driving, the fault joint will keep moving due to inertia, and gradually reduce to zero under the influence of the friction in the joint.

Fig. 6. The velocity (left) and the acceleration (right) of the active joint and fault joint.

To verify the advantages that ILC does not depend on the precise mathematical model of the system, the error in the dynamics model of the space manipulator is introduced randomly in experiments. Through randomly introducing errors within ±10 % for the mass of each link, an experiment under the same conditions mentioned above is carried out, and the results are shown in Figs. 7. From Fig. 7, when k = 8, the iteration error is |e8 | = 0.035° ≤ ε, and the desired angle of the active joint is ψa8,d = 153.6°. After the active joint stops moving, the fault joint velocity keeps moving until at 31.2 s. The actual angle of the fault joint is ψp8 =     47.035°, and compared with desired angle, the error is ψpd − ψp8  = |e8 | = 0.035° ≤ ε. The results verify the effectiveness of the regulation method. The simulation results show that ILC does not depend on the precise mathematical model of the system, and can still regulate the fault joint to the desired angle in the presence of error in the manipulator dynamics model.

Fig. 7. The simulation result with error in the space manipulator model. The relationship between the error and number of iterations with errors introduced (left), and the motion trajectory of the active joint and fault joint.

The above simulation results verify the validity of the failure treatment strategy, and provide a feasible solution for the processing of space manipulators with free-swinging joint failure. It is worth noting that the proportional learning gain δp in the iterative

Regulation of Space Manipulators with Free-Swinging Joint Failure

369

learning control algorithm is the key to affect the convergence rate. But as long as the initial condition is satisfied, the error will eventually converge.

5 Conclusions For space manipulators with free-swinging joint failure, an iterative learning control method is proposed in this paper. Through planning the active joint trajectory repeatedly based on ILC, the fault joint is precisely regulated to the desired angle. Simulations are carried out with a 7-DOF space manipulator. The error between the actual and desired angle of the fault joint is minimized by introducing the ILC into motion planning of the active joint, and its accuracy can reach 0.1° at least. It indicates that the fault joint can be regulated to the desired angle based on ILC, and the motion error between the actual and desired angle is allowable. Acknowledgements. This study was supported by Supported by the National Natural Science Foundation of China (No. 51975059), BUPT Excellent Ph.D. Students Foundation (CX2021312), and the Science and Technology Foundation of State Key Laboratory (No. 19KY1213).

References 1. Chen, G., et al.: Detumbling strategy based on friction control of dual-arm space robot for capturing tumbling target. Chin. J. Aeronaut. 33(3), 1093–1106 (2020) 2. Yue, X., et al.: Postcapture stabilization of space robots considering actuator failures with bounded torques. Chin. J. Aeronaut. 31(10), 2034–2048 (2018) 3. English, J., Maciejewski, A.: Fault tolerance for kinematically redundant manipulators: anticipating free-swinging joint failures. IEEE Trans. Robot. Autom. 14(4), 566–575 (1970) 4. English, J., Maciejewski, A.: Failure tolerance through active braking: a kinematic approach. Int. J. Robot. Res. 20(4), 287–299 (2001) 5. Maciel, B., Terra, M., Bergerman, M.: Optimal robust control of underactuated manipulators via actuation redundancy. J. Robot. Syst. 20(11), 635–648 (2003) 6. Chen, G., et al.: Failure treatment strategy and fault-tolerant path planning of a space manipulator with free-swinging joint failure. Chin. J. Aeronaut. 31(12), 109–124 (2018) 7. Gao, Z., Zeng, L., He, B., Luo, T., Zhang, P.: Type synthesis of non-holonomic spherical constraint underactuated parallel robotics. Acta Astronaut. 152, 509–520 (2018). https://doi. org/10.1016/j.actaastro.2018.08.050 8. Shin, J., Lee, J.: Dynamic control of underactuated manipulators with free-swinging passive joints in Cartesian space. In: Proceedings in IEEE International Conference on Robotics and Automation, pp. 1–6 (1997) 9. He, G., Lu, Z., Wang, F.: Optimal control of under-actuated redundant space-robot system. IET Control Theory Appl. 21(2), 305–310 (2004) 10. Yao, Q.: Adaptive fuzzy neural network control for a space manipulator in the presence of output constraints and input nonlinearities. Adv. Space Res. 67(6), 1830–1843 (2021) 11. Wang, Y., et al.: A quick control strategy based on hybrid intelligent optimization algorithm for planar n-link underactuated manipulators. Inf. Sci. 420, 148–158 (2017) 12. Petkovi´c, D., et al.: Determining the joints most strained in an underactuated robotic finger by adaptive neuro-fuzzy methodology. Adv. Eng. Softw. 77, 28–34 (2014)

370

Y. Fu et al.

13. Jin, X.: Iterative learning control for output-constrained nonlinear systems with input quantization and actuator faults. Int. J. Robust Nonlinear Control 28(3), 729–741 (2018) 14. Gijo, S., et al.: Feedback-based iterative learning design and synthesis with output constraints for robotic manipulators. IEEE Control Syst. Lett. 2, 513–518 (2018) 15. Feng, F., Tang, L., Xu, J., Liu, H., Liu, Y.: A review of the end-effector of large space manipulator with capabilities of misalignment tolerance and soft capture. Sci. China Technol. Sci. 59(11), 1621–1638 (2016). https://doi.org/10.1007/s11431-016-0084-7 16. Peñaloza-Mejía, O., Márquez-Martínez, L.A., Alvarez-Gallegos, J., Alvarez, J.: Master-slave teleoperation of underactuated mechanical systems with communication delays. Int. J. Control Autom. Syst. 15(2), 827–836 (2017). https://doi.org/10.1007/s12555-015-0126-8 17. Dong, J., et al.: Design of open-closed-loop iterative learning control with variable stiffness for multiple flexible manipulator robot systems. IEEE Access 7, 23163–23168 (2019) 18. Khan, Z.A., Chacko, V., Nazir, H.: A review of friction models in interacting joints for durability design. Friction 5(1), 1–22 (2016). https://doi.org/10.1007/s40544-017-0143-0 19. Chen, G., et al.: Trajectory optimization for inhibiting the joint parameter jump of a space manipulator with a load-carrying task. Mech. Mach. Theory 140, 59–82 (2019)

A Trajectory Planning Method for Load-Carrying Capacity Improvement of Redundant Space Manipulator with Large External Force Jingdong Zhao, Xiaohang Yang(B) , Zhiyuan Zhao, and Hong Liu State Key Laboratory of Robotics and System, Harbin Institute of Technology, Harbin 150001, Heilongjiang, People’s Republic of China

Abstract. In this paper, a joint torque optimization method is proposed for redundant space manipulator systems where an external force acts on the end-effector. Through three weighting factors, the joint dynamic torque minimization, the influence of external forces minimization and joint velocity minimization are combined. The quadratic program (QP) method is used to solve the proposed joint torque minimization at joint-acceleration level. This paper solves the problem of joint-velocity and joint-position level planning at the level of joint-acceleration, which realizes the simultaneous processing of three different levels of problems. Joint velocity minimization guarantees the stability and final joint-velocity of motion to be near zero. A simulation experiment of a 7-DOF redundant space robot is carried out to verify the effectiveness of the proposed method. Keywords: Redundant space manipulator · Large external force · Torque minimization · Quadratic program (QP)

1 Introduction The emergence of on-orbit assembly technology has completely broken through the limitations of delivery vehicles, making it possible to build super large space facilities with the space manipulator. The United States, Europe, Japan, Canada and other countries have launched related research [1, 2]. The standardized modules of large equipment often have relatively large mass, and the joint torque of the manipulator will increase significantly during on-orbit operation. In order to ensure the smooth execution of the task, it is necessary to improve the load-carrying capacity of the redundant space manipulator, and the core is to optimize the joint torque of the manipulator. On the one hand, it can improve the load operation capability of the manipulator, and on the other hand, it can distribute power more effectively and reduce the energy consumption of the space manipulator. The main factors that affect the joint torque of the manipulator are the dynamic torque of the manipulator, the clamping load of the end effector and the external force on the end effector. In previous literature, the research on minimizing the dynamic torque of the joint © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 371–382, 2021. https://doi.org/10.1007/978-3-030-89092-6_34

372

J. Zhao et al.

has been carried out in depth. Hollerbach et al. carried out the earliest research on joint torque optimization [3]. Nedungadi et al. proposed the Minimum Kinetic Energy (MKE) method with global characteristics [4]. Kang et al. designed the null space damping method to enhance the stability of the system [5]. Zhang et al. proposed a combination of minimizing joint torque and minimizing joint velocity to prevent joint velocity mutation, improve system stability [6, 7]. When space manipulator performs operation task, its joints often need to provide larger torque. Wang et al. designed a point-to-point weightlifting motion planner [8]. Shafei et al. studied the calculation method of the load-bearing capacity of the pointto-point motion of the flexible link manipulator [9]. Liu et al. used the Multi-Objective Particle Swarm Optimization (MOPSO) method to optimize the joint torque, disturbance of the base and total energy of the space robot, which improved the load-carrying capacity of the space floating manipulator [10]. The above studies focused on minimizing the dynamic torque of the manipulator and reducing the influence of the large load, but did not consider the external forces acting on the system. Woolfrey et al. studied the optimization of the joint torque on a manipulator when an external force acts on the end-effector [11]. However, the scheme uses the traditional null space method and the pseudoinverse-type scheme needs to calculate the inverse of the matrix which will increase the difficulty of calculation. It cannot obtain feasible solutions for singular situations. Ma et al. studied the joint velocity minimization index of the angular velocity level, and found its expression method in joint- acceleration level, and planned simultaneously with the joint dynamic torque minimization index by means of weighting [12, 13]. Zhang et al. proposed gradient neural dynamics and Zhang neural dynamics, and used neural network to solve quadratic programming problems [14]. Most of literature presented focuses on the minimization of joint dynamic torque the improvement of the load-carrying capacity under heavy payload conditions. However, few studies consider the influence of the external force of the end-effector on the joint. In this paper, the joint torque minimization problem of the redundant space manipulator is studied where an external force acts on the end-effector. In addition, the method of minimizing joint angular velocity is adopted to improve the stability of the algorithm. Minimizing joint torque generated by the external force on the end effector at jointposition level, minimizing joint velocity at joint-velocity level and minimizing joint dynamic torque at joint-acceleration level, are simultaneously processed and expressed as a standard QP problem.

2 Model of Space Manipulator 2.1 Kinematic and Dynamic Model of Space Manipulator The forward kinematics of space manipulator describes the nonlinear mapping relationship between the manipulator joint space and Cartesian workspace, as shown in the following equation: x = f (θ )

(1)

A Trajectory Planning Method for Load-Carrying Capacity

373

where x ∈ Rm describes the pose of the end-effector, θ ∈ Rn is a vector of joint angles, and f (·) is a nonlinear mapping relationship determined by the structure and parameters of the space robot, which reflects the mapping relationship between the robot’s joint space and workspace. The linear relationship between velocity level joint space and workspace can be obtained by differential Eq. (1). x˙ = J θ˙

(2)

where x˙ is the velocity of the end-effector in Cartesian space, θ˙ is a vector of joint velocity, and J ∈ Rm×n is the Jacobian matrix of the manipulator. The relationship between the acceleration of the end effector in the workspace and the acceleration of the joint can be obtained by differential Eq. (2). x¨ = J˙ θ˙ + J θ¨

(3)

where x¨ is the acceleration of the end-effector in Cartesian space, θ¨ is a vector of joint acceleration, and J˙ is the derivative of the Jacobian matrix J with respect to time. J is a function of the structure parameters and joint angle of the manipulator. We can obtain the analytical expression of J˙ , when the structure and parameters of the manipulator is determined [15]. After the joint acceleration, velocity and angle of the manipulator are obtained, the joint torque can be calculated through the dynamics of the space manipulator:   (4) τ = M (θ )θ¨ + c θ, θ˙ + τE where τ ∈ Rn is the output torque of joint motors  to complete the required motion, M (θ ) is the inertia matrix of the manipulator, c θ, θ˙ is the vector of Coriolis and centripetal torque, τE is the torque that joint motors need to output in order to balance the external force. 2.2 Effect of External Load on End-Effector When the end-effector of the manipulator is subjected to external forces, the force on each joint can be calculated by J . Assuming that the joint produces an infinitesimal change θ , the displacement on the end effector is x, it holds that: x = J θ

(5)

The external force received by the end-effector of the manipulator is expressed as γ0 when the base scale is used as the reference frame. When the corresponding workspace displacement is x, the work done by the external force γ0 is: dWγ = γ0T x

(6)

When the joint displacement of τE is θ , the amount of work done is: dWτE = τET θ

(7)

374

J. Zhao et al.

The measured value of work in any generalized coordinate system is the same. Using the principle of virtual work, if and only if the following equation holds, the manipulator is in equilibrium. dWγ = dWτE

(8)

It can be obtained from Eq. (5), Eq. (6), Eq. (7) and Eq. (8): γ0T x = τET θ

(9)

γ0T Jθ = τET θ

(10)

τE = J T γ0

(11)

The external force received by the manipulator system is expressed as γi when the {i} system is used as the reference frame, the transformation relationship with γ0 based on the base frame as the reference is: γ0 = Ri γi

(12)

where Ri is the rotation transformation matrix from {0} to {i} system. When the workspace dimension m = 6, the rotation can be defined as:  i  R0 (θ ) 03×3 Ri = (13) 03×3 Ri0 (θ ) where Ri0 (θ ) is the rotation matrix determined by the rotation angle of each joint.

3 Scheme and QP Formulation 3.1 Minimizing Joint Dynamic Torque MKE method proposed by Nedungadi et al. is a widely used method to minimize the dynamic torque of joints [4], and its basic expression is as follows: min

1 T −1 τ M τ 2

subject to x¨ − J θ¨ − J˙ θ˙ = 0

(14) (15)

The Lagrange multiplier method is used to solve the above basic expression, and the traditional pseudo-inverse solution can be obtained:   + +  −1 θ¨ = JM x¨ − J˙ θ˙ − I − JM J M (c + τE ) (16) where JM = M −1 J T (JM −1 J T )−1 is a weighted pseudo inverse matrix when the weighting coefficient is the dynamic term M of the manipulator.

A Trajectory Planning Method for Load-Carrying Capacity

375

When the τE in Eq. (4) is not considered, it can be brought into Eq. (14), and the method for optimizing joint dynamic torque can be transformed into the following QP form: min

1 θ¨ M θ¨ + cT θ¨ 2

subject to x¨ − J θ¨ − J˙ θ˙ = 0

(17) (18)

3.2 Control Method for Torque Minimization with External Loading The basic expression of minimizing joint torque caused by external force is as follows: min τET W τE

(19)

subject to x = f (θ (t))

(20)

The above equation represents the two-norm joint-torque to be minimized. Where W ∈ Rn×n is a positive definite symmetric weighting matrix. By adjusting the weighting matrix, we can give different weights to the joints with different capacity, so that the objective function has the ability to adjust the optimization scheme according to the actual load capacity of each joint of the manipulator. It can be seen from Eq. (11) that τE is only related to the joint space angle value θ and the external force γ , and belongs to the joint-angle level planning. Its optimization variable is the joint angle value θ. The position-level redundance-resolution scheme is defined as a scalar-norm-based energy-function: ε(θ ) =

1 T (J (θ )γ0 )T WJ T (θ )γ0 2

(21)

It is obvious that for any θ ∈ R there is ε(θ ) > 0 when the external force of the system is not zero. The gradient descent method can be used to minimize the energyfunction ε(θ ). We establish the relationship between the angle renewal rate, i.e. the joint velocity value, and the gradient of the energy function, so that the joint velocity value can be updated along the negative gradient direction of the energy function: θ˙ = −λ

∂ε(θ ) ∂θ

(22)

where λ is used to measure the convergence rate of the energy function ε(θ ), and in order to ensure that the optimization along the negative gradient direction λ is a positive number. ∂ε(θ) ∂θ is the gradient of the energy function ε(θ ):  ∂τE T WτE ∂θ  T ∂R ∂J ∂ε(θ ) i = γiT J + RTi WJ T Ri γi j ∈ {1, . . . , n} ∂θj ∂θj ∂θj ∂ε(θ ) = ∂θ



(23)

(24)

376

J. Zhao et al.

Equation (23) can be understood as the angle changes along the negative gradient direction of the energy function, which has the effect of reducing the joint torque at the level of joint-velocity. Equation (23) can be rewritten as follows: ∂ε(θ ) =0 (25) ∂θ The above equation belongs to strict constraints. When the number of degrees of freedom of the robot system is insufficient, the above equation cannot be guaranteed to be valid. The above equation can be modified and replaced by the horizontal performance 2 index 21 θ˙ +λ ∂ε(θ) ∂θ 2 . The energy function at the joint-position level shown in Eq. (19) is transformed into the energy function at the joint-velocity level. The derivative of Eq. (23) with respect to time is as follows:     d ∂ε(θ ) d ∂ε(θ ) = θ˙ (26) dt ∂θ dθ ∂θ

 is the Hessian matrix of ε(θ ). where ddθ ∂ε(θ) ∂θ The Hessian matrix form of the energy function shown in Eq. (21) is complex and difficult to solve. When the sampling time t of the system is small enough, the backward difference method can be used to approximate the time derivative of the energy function gradient ξ¨ :     d ∂ε(θ ) ∂ε(θ ) −1 ∂ε(θ ) ¨ξ = (27) = t − dt ∂θ ∂θ t ∂θ t−t θ˙ + λ

Similar to Eq. (22), the optimization scheme of joint-acceleration level can be obtained: θ¨ = −μξ¨

(28)

where μ is used to measure the convergence speed. In this study, μ is assigned to the dynamic parameter term M of the manipulator. Equation (28) is planned in the negative direction of the derivative of joint angular acceleration with respect to time along the energy function gradient, ensuring that joint acceleration plane planning can also reduce joint torque caused by external forces. By rewriting Eq. (28), we can get: θ¨ + M ξ¨ = 0

(29)

Similar to the analysis of joint-velocity level, the above equation to min is reduced  d ∂ε(θ) 2 , the basic  imize the performance index of joint-acceleration level 21 θ¨ + M dt 2 ∂θ expression for minimizing joint torque when external force exists in the simplified system is: 1 min θ¨ T θ¨ + M ξ¨ T θ¨ (30) 2 subject to x¨ − J θ¨ − J˙ θ˙ = 0

(31)

It can be seen from the above equation that the scheme can be used to achieve planning across two levels, that is, from joint-position level to joint-acceleration level.

A Trajectory Planning Method for Load-Carrying Capacity

377

3.3 Minimizing Joint Velocity Minimizing joint velocity can reduce the joint end velocity after trajectory planning, which brings convenience to the planning and execution of the next task. The expression for minimizing joint speed is: min

1 θ˙ 22 2

subject to x˙ − J θ˙ = 0

(32) (33)

The minimum joint angular velocity is planned at the joint-velocity level, and the joint-velocity level should be transformed into the joint-acceleration level. According to Zhang’s equivalence theory [14], an error function is designed: E(t) = θ (t)

(34)

In order to reduce the error, the following equation can be designed: θ¨ = −ηθ˙

(35)

where η is a positive number, which is used to measure the convergence rate of error, η is set as 25 in this study. By rewriting the above formula, we can get: θ¨ + ηθ˙ = 0

(36)

The simplified joint-acceleration level of minimizing joint angular velocity is expressed as: min

1 T θ¨ θ¨ + ηθ˙ T θ¨ 2

subject to x¨ − J θ¨ − J˙ θ˙ = 0

(37) (38)

3.4 QP Formulation Based on the above analysis, the joint torque optimization is transformed into a QP problem, and the objective function is defined as a weighted sum. The final expression of the QP problem is: min

 1 T θ¨ (αM + βI + σ I )θ¨ + αcT + βM ε¨ T + σ ηθ˙ T θ¨ 2 subject to x¨ − J θ¨ − J˙ θ˙ = 0

(39) (40)

where α is the weight coefficient to minimize the joint dynamic torque, β is the weight coefficient to the joint torque caused by the external force on the end effector, σ is the weight factor to minimize the joint velocity, and has α + β + σ = 1.

378

J. Zhao et al.

Fig. 1. The kinematic model of the 7-DOF manipulator.

Table 1. DH parameters of the 7-DOF space manipulator. ∂i−1 /(°)

di /m

θ i /(°)

No.

ai−1 /m

1

0

90°

a0

0°

2

0

90°

a1

0°

3

0

−90°

a2

−90°

4

a3

0°

a4

0°

5

a5

0°

a6

90°

6

0

90°

a7

0°

7

0

−90°

a8

0°

Table 2. The mass properties of the space manipulator. No.

Mass

Center of mass

Inertia [Ixx , Iyy , Izz , Ixy , Izx , Iyz ]

1

6.34

0.003, −0.067, −0.052

0.098, 0.045, 0.072, 0.005, 0.003, −0.022

2

6.34

0.001, 0.060, −0.058

0.087, 0.021, 0.069, 0.005, 0.002, 0.022

3

7.93

0.226, 0.005, −0.025

0.079, 0.571, 0.530, 0.002, 0.120, 0.089

4

5.67

0.238, 0.001, −0.001

0.032, 0.325, 0.313, 0.001, 0.052, 0.092

5

2.84

0.008, −0.028, −0.031

0.014, 0.010, 0.011, 0.011, 0.001, −0.002

6

2.71

−0.025, 0.023, 0.003

0.013, 0.009, 0.011, 0.002, 0.001, 0.001

7

12.08

0.003, −0.010, −0.199

0.151, 0.195, 0.096, 0.002, 0.003, 0.008

4 Simulative Verifications and Comparisons 4.1 Control System of Redundant Space Manipulator In order to verify the joint torque minimization algorithm of redundant space robot when there is an external force on the end-effector proposed in this paper, we take a 7-DOF

A Trajectory Planning Method for Load-Carrying Capacity

379

space manipulator as an example. The kinematics model of the space manipulator is shown in Fig. 1 and Table 1, and the dynamic parameters are shown in Table 2. 4.2 Control System of a 7-DOF Space Manipulator Simulation Results In this section, a 7-DOF space manipulator is simulated to complete the four goals of end trajectory tracking, minimizing joint dynamic torque, minimizing joint torque generated by external force on the end-effector and minimizing joint speed. The performance of the proposed method is verified by a simulation example, and compared with the MKE method and Woolfrey’s method. The initial joint angles of the space Manipulator are (40, −50, 30, 60, 90, −60, 50) degree.

MKE Method Woolfrey's Method Proposed Control Method

600

Total Torque (Nm)

550 500 450 400 350 0

0.5

1 t (s)

1.5

2

Fig. 2. Sum of absolute joint torques

The task requirement of this example is to make the end-effector of the manipulator move along a straight line from the initial position to the desired position. The desired Cartesian displacement is (−0.4897 −1.305 0.7240) m. The duration of the task is 2 s. The first 0.5 s is realized by using the algorithm of quintic polynomial trajectory planning, the middle 1 s is uniform motion, and the last 0.5 s is realized by using the algorithm of quintic polynomial trajectory planning. The distances of the acceleration and deceleration segments in X and Y directions were −0.01 m and Z direction was − 0.05 m. And we respectively take α = 0.28, β = 0.71, σ = 0.01. Relative to {0} system, the external force on the end-effector of space manipulator is expressed as follows: τE = [80 80 80 0 0 0]T The sum of absolute joint torques of the space manipulator is shown in Fig. 2. Compared with the MKE method and Woolfrey’s method, the method proposed in this paper can effectively reduce joint torque. In the acceleration phase of the end-effector, the effect of the three schemes on joint torque optimization is similar. In the stage of uniform motion, the method proposed in this paper has better effect, and the joint torque decreases obviously. The maximum torque value of the method proposed in this paper is lower than that of MKE method and Woolfrey’s method. Figure 3(a)–(c) shows the planning result of joint acceleration of the manipulator. It is worth noting that three

380

J. Zhao et al.

Fig. 3. (a) Joint acceleration by MKE method. (b) Joint acceleration by Woolfrey’s method. (c) Joint acceleration by this paper’s method. (d) Joint velocity by MKE method. (e) Joint velocity by Woolfrey’s method. (f) Joint velocity by this paper’s method. (g) Joint angle by MKE method. (h) Joint angle by Woolfrey’s method. (i) Joint angle by this paper’s method.

A Trajectory Planning Method for Load-Carrying Capacity

381

methods have similar torque optimization effect in acceleration stage, but the planning value of joint acceleration is different. In the constant motion stage, the joint acceleration value of the method proposed in this paper changes greatly, which can quickly adjust the configuration of the manipulator and reduce the joint torque. Figure 3(d)–(f) shows the planning result of the joint velocity, and it can be seen that the joint velocity of the three methods is maintained at a lower level. At the end of planning, the joint velocity of Woolfrey’s method is relatively large, while the joint velocity of the proposed method is relatively small and more acceptable for engineering applications. Figure 3(g)–(i) shows the joint angle change of space manipulator. The angle value of the three methods is relatively gentle, and there is no mutation.

5 Conclusion In this paper, a joint torque minimization method based on QP is proposed to solve the problem of external loading on the end effector of space manipulator system. This paper proposes a method to minimize the joint torque caused by external force, so that joint- position level planning can be processed at joint-acceleration level. This method has the ability to deal with three different levels planning of joint-position, joint-velocity and joint-acceleration at the same time. Minimizing joint velocity has the function of stabilizing local torque optimization solution, preventing the appearance of high joint velocity and ensuring the near-zero final joint-velocity properties of the proposed scheme. A set of experiments of a 7-DOF manipulator verify the validity and feasibility of the proposed algorithms. Acknowledgement. The research is supported by the National Natural Science Foundation of China (Grant No. 91848101 and No. 91848202).

References 1. Komendera, E.E., Adhikari, S., Glassner, S., Kishen, A., Quartaro, A.: Structure assembly by a heterogeneous team of robots using state estimation, generalized joints, and mobile parallel manipulators. In: 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4672–4679 (2017) 2. Xue, Z., Liu, J., Wu, C., Tong, Y.: Review of in-space assembly technologies. Chin. J. Aeronaut (2020) (Published online) https://doi.org/10.1016/j.cja.2020.09.043 3. Hollerbach, J.M., Suh, K.C.: Redundancy resolution of manipulators through torque optimization. IEEE Trans. Robot. Automat. 3(4), 308–315 (1987) 4. Nedungadi, A., Kazerouinian, K.: A local solution with global characteristics for the joint torque optimization of a redundant manipulator. Adv. Robot. 6(5), 631–645 (1989) 5. Kang, H., Freeman, R.: Joint torque optimization of redundant manipulators via the null space damping method. In: IEEE International Conference on Robotics and Automation, pp. 520–525. France (1992) 6. Zhang, Y., Guo, D., Ma, S.: Different-level simultaneous minimization of joint-velocity and joint-torque for redundant robot manipulators. J. Intell. Robot. Syst. 72, 301–323 (2013)

382

J. Zhao et al.

7. Zhang, Y., Yang, M., Chen, D., Li, W., Guo, D., Yan, X.: Proposing, QP-unification and verification of DLSM based MKE-IIWT scheme for redundant robot manipulators. In: 2017 IEEE 3rd Information Technology and Mechatronics Engineering Conference (ITOEC), pp. 242–248 (2017). https://doi.org/10.1109/ITOEC.2017.8122428 8. Wang, C.Y.E., Timoszyk, W.K., Bobrow, J.E.: Payload maximization for open chained manipulators: finding weightlifting motions for a Puma 762 robot. IEEE Trans. Robot 17(2), 218–224 (2001) 9. Shafei, A.M., Korayem, M.H.: Theoretical and experimental study of dynamic load-carrying capacity for flexible robotic arms in point-to-point motion. Optim. Control Appl. Methods 38(6), 963–972 (2017) 10. Liu, Y., Jia, Q.X., Chen, G., Sun, H.X., Peng, J.J.: Multi-objective trajectory planning of FFSM carrying a heavy payload. Int. J. Adv. Robot. Syst. 12, 13–26 (2015) 11. Woolfrey, J., Lu, W.J., Liu, D.K.: A control method for joint torque minimization of redundant manipulators handling large external forces. J. Intell. Robot. Syst. 96(1), 3–16 (2019) 12. Ma, S., Hirose, S., Yoshinada, H.: Dynamic redundancy resolution of redundant manipulators with local optimization of a kinematic criterion. Adv. Robot. 10(1), 65–80 (1996) 13. Ma, S.: A balancing technique to stabilize local torque optimization solution of redundant manipulators. J. Robot. Syst. 13(3), 177–185 (1996) 14. Zhang, Y., Yang, M., Qiu, B., Li, J., Zhu, M.: From mathematical equivalence such as Ma equivalence to generalized Zhang equivalency including gradient equivalency. Theor. Comput. Sci. 817, 44–54 (2020) 15. Bruyninckx, H., DeSchutter, J.: Symbolic differentiation of the velocity mapping for a serial kinematic chain. Mech. Mach. Theory 31(2), 135–148 (1996)

Inverse Kinematics of a Novel SSRMS-Type Reconfigurable Manipulator with Lockable Passive Telescopic Links Jingdong Zhao, Zhiyuan Zhao(B) , Guocai Yang, Xiaohang Yang, and Hong Liu State Key Laboratory of Robotics and System, Harbin Institute of Technology, Harbin 150001, Heilongjiang, People’s Republic of China

Abstract. The SSRMS-type space manipulators have been playing important roles in many on-orbit servicing tasks. However, most of them cannot flexibly change their workspace since the lengths of the links are fixed. Therefore, the ability of these manipulators to perform different tasks is limited. In this paper, we present a novel SSRMS-type reconfigurable manipulator with lockable passive telescopic links. When the manipulator forms a closed kinematic chain, the manipulator achieves reconfiguration by changing the lengths of the telescopic links, so that the manipulator can perform a variety of tasks. Based on the new manipulator, we propose inverse kinematics (IK) solution method named the CCDJAP-IK method. This method is based on a combination of the cyclic coordinate descent (CCD) method and the joint angle parameterization (JAP) method and has the advantages of being insensitive to Jacobian matrix singularities, avoiding joint limits, and finding multiple exact solutions. Finally, simulation experiments are presented for the proposed manipulator, and the results demonstrate the effectiveness of the IK solution method. Keywords: Reconfigurable manipulator · Passive telescopic link · Inverse kinematics · Cyclic coordinate descent · Joint angle parameterization

1 Introduction Over the last decades, space manipulators have completed a large number of on-orbit tasks. Among them, Canadarm2 (or the SSRMS), European Robotic Arm (ERA), and Special Purpose Dexterous Manipulator (SPDM) are well known. Because these manipulators have similar joint arrangements, they are collectively called SSRMS-type manipulators [1]. The link lengths of the SSRMS-type manipulators are designed in different sizes for different tasks. These manipulators cannot flexibly change their workspace since the lengths of the links are fixed. Therefore, the ability of these manipulators to perform different tasks is limited. To make the manipulator capable of performing multiple tasks, Aghili et al. [2] introduced a reconfigurable space manipulator with two passive telescopic links. Shi et al. [3] designed a passive telescopic link and developed the principle prototype for a reconfigurable space manipulator. These telescopic links are particularly suitable for space applications due to the characteristics of simple structure, © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 383–395, 2021. https://doi.org/10.1007/978-3-030-89092-6_35

384

J. Zhao et al.

lightweight and small space occupation rate. The manipulator achieves reconfiguration by changing the length of these telescopic links, which increases the ability to perform multiple tasks. To avoid mechanical interference, SSRMS-type manipulator with link offsets at shoulder, elbow, and wrist. Due to there is no spherical-wrist/shoulder, the kinematics of this kind of manipulator cannot be decoupled, which dramatically increases the difficulty of solving the IK problem [1]. In recent decades, there has been much research literature on the IK of the anthropomorphic arm without joint offset or link offset, but about SSRMS-type manipulator is little. Crane et al. [4] have developed an IK method about the SSRMS based on a geometric projection of spatial polygons. Xu et al. [1] proposed an analytical and semi-analytical IK method that considers the joint locking failure of SSRMS-type manipulators. However, these two methods do not take full the redundancy of SSRMS-type manipulators. Moreover, since closed-chain kinematics needs to be considered when reconfiguring the passive telescopic link, it will further make the computation of the IK solutions challenging [2]. In this paper, we present a novel SSRMS-type reconfigurable manipulator with two lockable passive telescopic links. Then, we mainly focus on the IK problem of this kind of manipulator and propose an effective IK solution method based on a combination of the CCD method and the JAP method. Finally, simulation experiments verify the effectiveness of the proposed method. The structure of the rest of the paper is organized as follows. Section 2 establishes the forward and inverse kinematics models and analyzes the reachable workspace characteristics of the reconfigurable manipulator. Section 3 gives the detailed derivation process of the CCDJAP-IK method. Section 4 presents various IK simulation experiments considering open kinematic chain and closed kinematic chain. Finally, conclusions are drawn in Sect. 5.

2 Kinematics Modeling and Workspace Analysis 2.1 Forward Kinematics Modeling for the Reconfigurable Manipulator The SSRMS-type reconfigurable manipulator is a redundant manipulator composed of seven revolute joints and two passive telescopic links. Its configuration and a schematic diagram of the reconfiguration process are shown in Fig. 1. Using Craig’s [5] method to establish the D-H coordinate systems of this manipulator, as shown in Fig. 2, in which the coordinate frames {0} and {8} are defined as the base coordinate frame and the end-effector (EE) coordinate frame, respectively. The coordinate frame {i}(i = 1, ..., 7) is the coordinate frame of the ith joint, where zi represents the rotation axis of the ith joint. The corresponding D-H parameters of each link are listed in Table 1. Among these parameters, θi (i = 1 to 7) denote the joint angles, which are variable, and their range of motion is [−3π /2, 3π /2]. a3 and a4 represent the lengths of the two telescopic links, and their motion lies within the range [0.56, 0.86](m) and [0.50, 0.80](m), respectively.

Inverse Kinematics of a Novel SSRMS-Type Reconfigurable Manipulator

385

Telescopic link 2

Locked Assistive device

Telescopic link 1

Released

(a)

(b)

(c)

(e)

(f)

Released

Locked

(d)

Fig. 1. Reconfiguration maneuvers from the shortest configuration to the longest; (a) the initial configuration; (b) constraining the end-effector (EE) movement with an assistive device; (c) unlocking and reconfiguring the first telescopic link; (d) unlocking and reconfiguring the second telescopic link; (e) removing the EE constraints; (f) the final configuration. x6

a4 a3 z4

y0 z0

x1 O0

θ1

x2

d1

y2

O1

x4

O4

y5

θ7

x7

d7 O7

y7

θ5

d6

z7 y8

x8 O8

d8 z8

d5

θ4

θ6

z2 O2

θ2

y1

x3

y4

O5

z6 y6

θ3

d3 x0

y3

d4

z3 O3

x5

O6

z5

z1

d2 Telescopic link

Fig. 2. Coordinate frames of the reconfigurable robot.

The pose (position and orientation) of coordinate frame {i} relative to coordinate frame {i − 1} can be expressed by a homogeneous transformation matrix i−1 i T: ⎤ −sθi 0 ai−1 cθi ⎢ sθi cαi−1 cθi cαi−1 −sαi−1 −sαi−1 di ⎥ i−1 ⎥ ⎢ i T = ⎣ sθ sα ⎦ i i−1 cθi sαi−1 cαi−1 cαi−1 di 0 0 0 1 ⎡

(1)

where c = cos, s = sin, and the pose of the EE in the base coordinate frame is 0 8T

= 01 T 12 T 23 T 34 T 45 T 56 T 67 T 78 T

(2)

The Eq. (2) is the forward kinematics (FK) equation of the manipulator in Fig. 2. 2.2 Inverse Kinematics Modeling for the Reconfigurable Manipulator According to the locked state of the passive telescopic link, the IK analysis process of the reconfigurable manipulator is divided into two situations, as illustrated in Fig. 3. Compared with the IK of the traditional manipulator, the main difference only appears in the stage of reconfiguring the manipulator by releasing the telescopic link. At this point,

386

J. Zhao et al. Table 1. D-H parameters for the reconfigurable robot.

the manipulator forms a closed kinematics chain as shown in Fig. 1, i.e., the pose of the EE is constrained, and the revolute joints’ motion pushes or pulls the telescopic link to achieve the desired length. Therefore, the IK problem of the reconfigurable manipulator is to find the values of the revolute joints given the desired pose of the EE and the desired lengths of the telescopic links.

Locked passive telescopic links Desired Cartesian pose

Inverse kinematics

Reconfigurable manipulator

Revolute joints

Desired length Released passive of telescopic link telescopic link

Fig. 3. Block diagram for the IK analysis process of the reconfigurable manipulator.

The above IK problem is often transformed into an optimization problem with the pose error of the EE as the objective function. As shown in Fig. 4, the desired position vector and orientation matrix of the EE are denoted as Pd and [Rd ] = [d1 , d2 , d3 ], where dj (j = 1 to 3) denote the unit vectors along with the xd -, yd -, and zd -axis. In the same way, the current position vector and orientation matrix of the EE are denoted as Pc (θ ) and [Rc (θ )] = [c1 (θ ), c2 (θ ), c3 (θ )], where cj (j = 1 to 3) denote the unit vectors along with the xc -, yc -, and zc -axis. Pc and Rc are functions of the joint angle vector θ = [θ1 ,θ2 ,...,θ7 ]T . When Pd and Rd are given, the pose error between the current and the desired locations of the EE can be expressed by the following functions: Position error: P(θ ) = P2 = P d − P c (θ )2 Orientation error: O(θ ) =

3 

2 λj d j · cj (θ) − 1

(3) (4)

j=1

The pose error: E(θ ) = P(θ )+O(θ )

(5)

where ( · ) denotes the Euclidean norm of a vector, a (·) denotes the vector dot product, and λj (j = 1 to 3) denote the weighting factors in three directions.

Inverse Kinematics of a Novel SSRMS-Type Reconfigurable Manipulator

387

Fig. 4. The current and the desired locations of the EE; (a) schematic diagram of the position error; (b) schematic diagram of the orientation error. ∗ Now, the ∗ IK problem is to find a joint angle vector θ that can satisfy the condition for E θ < ε(ε → 0). Therefore, this problem is transformed into the following minimization problem:

minimize E(θ ) subject to θil ≤ θi ≤ θiu i = 1, . . . , 7

(6)

where θiu and θil denote the upper and lower limits of the ith joint angle, respectively. 2.3 Workspace Analysis of the Reconfigurable Manipulator The workspace of the manipulator has many functions, one of which is to evaluate the working ability of the manipulator [5]. Here we use the improved Monte Carlo method [6] to obtain the reachable workspaces of the two configurations (Fig. 1(a) and (f)) as shown in Fig. 5. Obviously, the volume of the reachable workspace is increased by changing the lengths of the telescopic links, which means that the working ability of the manipulator is significantly improved.

Fig. 5. Reachable workspace analysis; (a) reachable workspace comparison between the longest configuration (light blue) and the shortest configuration (yellow); (b) the workspace representation of the longest configuration is cut for better visibility. (Color figure online)

3 Solving the IK Problem of the Reconfigurable Manipulator As mentioned before, the IK problem of the reconfigurable manipulator is transformed into the minimization problem in Eq. (6). Due to the high-dimensional and nonlinear

388

J. Zhao et al.

characteristics of the kinematic equations, Eq. (6) is a 7-dimensional nonlinear minimization problem. To effectively solve this problem, we propose the CCDJAP-IK method in this section. It firstly uses the CCD method to obtain an approximate solution and then uses the JAP method to obtain multiple exact solutions. 3.1 The CCD Method for Solving the IK Problem The CCD method is a derivative-free optimization method, especially applicable for solving high-dimensional nonlinear problems [7]. When solving the IK problem, the iteration starts from the current configuration of the manipulator. Each iteration consists of N steps, where N is equal to the number of DOF of the manipulator. In the ith step (i varying from 1 to N), only allow adjusts the ith joint variable to minimize the objective function. The current configuration is updated after each step. The above iteration is repeated until the objective function meets the accuracy requirements. Figure 6 illustrates one step of the CCD method. Here, subscript i represents the ith joint, vector Pi represents the current origin position of the ith joint, and the meanings of the other vectors are the same as in Fig. 4. φ is the adjustment value of joint angle, whose purpose is to make vector Pic and the coordinate system {xc yc zc } move along the direction of descent of the objective function.

Fig. 6. Schematic diagram for one of the steps in each iteration of the CCD method.

As shown in Fig. 6, after rotating vector P ic around the zi -axis by an angle φ, a new  will be obtained: vector Pic

P ic (φ) = R zˆ i , φ P ic (7)

where R zˆ i , φ is the equivalent axis angle coordinate system representation, and zˆ i is the unit vector along with the zi -axis. At this time, the position error in Eq. (3) becomes:   2

2

P(φ) = P id − P  ic (φ) = P id 2 +R zˆ i , φ P ic  − 2P id · R zˆ i , φ P ic

(8)

After analyzing

we can find that minimizing P(φ) is equivalent to the Eq.

(8), maximizing P id · R zˆ i , φ P ic . Therefore, the Eq. (8) can be simplified to:

f1 (φ) = P id · R zˆ i , φ P ic (9)

Inverse Kinematics of a Novel SSRMS-Type Reconfigurable Manipulator

389

In the same way, after rotating vectors cj (θ )(j = 1 to 3) around the zi -axis by an angle φ, new vectors cj (θ )(j = 1 to 3) will be obtained:

cj (φ) = R zˆ i , φ cj

(10)

At this time, the orientation error in Eq. (4) becomes: O(φ) =

3  3 2  

2 λj d j · cj (φ) − 1 = λj cosψj (φ) − 1 j=1

(11)

j=1

where ψj (j = 1 to 3) denote the angles between unit vectors cj and unit vectors dj . After analyzing the Eq. (11), we can also find that minimizing O(φ) is equivalent to maximizing λj cosψj (φ)(j = 1 to 3). Therefore, the Eq. (11) can be simplified to: f2 (φ) =

3  j=1

λj cosψj (φ)=

3 





λj d j · R zˆ i , φ cj

(12)

j=1

Combining Eq. (9) and Eq. (12), the objective function in Eq. (5) becomes: f (φ) = ωp f1 (φ)+ωo f2 (φ)

(13)

where ωp and ωo denote the weighting factors of position error and orientation error, respectively. Here, we set ωp = κ (1 + min(P id , P ic )/max(P id , P ic )), κ = 10−5 , and ωo = 1. Overall, the minimization problem in Eq. (6) is transformed into the following maximization problem: maximize f (φ) subject to θil − θic ≤ φ ≤ θiu − θic i = 1, . . . , 7

(14)

where θic denotes the current value of the ith joint angle. According to Rodrigues’ rotation formula, Eq. (9) and Eq. (12) can be simplified into the following forms:



f1 (φ) = P id · P ic cosφ + P id · zˆ i × P ic sinφ + P id · zˆ i zˆ i · P ic (1 − cosφ) (15) f2 (φ) =

3 





λj d j · cj cosφ + d j · zˆ i × cj sinφ + d j · zˆ i zˆ i · cj (1 − cosφ)

(16)

j=1

where (×) denotes the vector cross product. Substituting Eq. (15) and Eq. (16) into Eq. (13) yields:  f (φ) = Asinφ + Bcosφ + C= A2 + B2 sin(φ + β) + C (17)

390

J. Zhao et al.

 3

 cj × d j , B = ωp P id · ωp P ic × P id + ωo

 where β = atan2(B, A), A = zˆ i ·

j=1





P ic − ωp P id · zˆ i zˆ i · P ic + ωo

3 





d j · cj − d j · zˆ i zˆ i · cj ,C = ωp P id · zˆ i zˆ i · P ic +

j=1

ωo

3 



d j · zˆ i zˆ i · cj .

j=1

Thus, f (φ) is maximized when φ = 2kπ +π/2−β(k ∈ z). To satisfy

the limit requirements of the joint angle, we select the φ that falls in the open interval θil − θic , θiu − θic . Besides, if φ ≤ θil − θic , take φ=θil − θic ; if φ ≥ θiu − θic take φ = θiu − θic .

Fig. 7. The relationship between the pose of the EE and the end pose of the sixth joint.

The efficiency of the CCD method is highly dependent on the formulation of the objective function and the number of steps in each iteration. Figure 7 illustrates the relationship between the pose of the EE and the end pose of the sixth joint. When pose vector of the EE is given, the following equations can be obtained: yˆ 6 = zˆ 8 = a 0 6P

= 08 R86 P + 08 P

(18) (19)

Therefore, the movement of EE can be transformed into a motion of only constraining the origin position and y6 -axis direction of the sixth joint. Then, using the CCD method to solve the IK problem of this 6-DOF manipulator. At this time, the formulation of the objective function is greatly simplified and the number of steps in each iteration is reduced to 6, so the solution efficiency will be significantly improved. Finally, θ7 is computed as follows:

(20) θ7 = ±arccos xˆ 6 · xˆ 7 where xˆ i (i = 6,7) denotes the unit vector along with the xi -axis. 3.2 The JAP Method for Solving the IK Problem The JAP method is an analytical method for solving the IK problem [1]. The condition for using this method is that the manipulator satisfies Piper’s criterion [8]. As shown

Inverse Kinematics of a Novel SSRMS-Type Reconfigurable Manipulator

391

in Fig. 2, the rotation axes of the joints 3, 4, and 5 of the reconfigurable manipulator are parallel, so the condition for using the JAP method is satisfied. When any one of θ1 , θ2 , θ6 , and θ7 is parameterized, the analytical expressions of the remaining joint angles can be derived. Due to the derivation process after parameterization θ1 or θ2 is the same as that of parameterization θ6 or θ7 , we only introduce the derivation process of parameterizing θ1 or θ2 here. Assume that the pose of the EE is given as follows:   noap 0 T = (21) 8 0001

T

T

T

T where n = nx ,ny ,nz , o = ox ,oy ,oz , a = ax ,ay ,az , and p = px ,py ,pz . Left-multiplying Eq. 2 by 01 T −1 yields:   0 −1 0 1 2 3 4 5 6 7 (22) 1T 8T = 2T 3T 4T 5T 6T 7T 8T Combining Eq. (21), Eq. (22), and the parameters in Table 1 yields: nx c1 s2 + nz s1 s2 + ny c2 = −s6 c7 , ox c1 s2 + oz s1 s2 + oy c2 = s6 s7 , ax c1 s2 + az s1 s2 + ay c2 = −c6

(23)

d3 + d4 + d5 + (px − ax d7 )c1 s2 + (pz − az d7 )s1 s2 + d1 + py − ay d7 c2 = 0 (24) According to Eq. (24), when θ1 is parameterized, θ2 is computed as follows: θ2 = ϕ − φ or θ2 = π − ϕ − φ

(25)

where φ = arctan(b/a), a = (px − axd7 )c1 + (pz − az d7 )s1 , b = d1 + py − ay d7 , √ ϕ = arcsin −(d3 + d4 + d5 )/ a2 + b2 . In the same way, when θ2 is parameterized, θ1 is computed as follows: θ1 = ϕ − φ or θ1 = π − ϕ − φ

(26)

where φ = arctan(b/a), a = (pz − az d7 )s2 , b  = (px − ax d7 )s2 , ϕ = √

arcsin − d1 + py − ay d7 c2 − (d3 + d4 + d5 )/ a2 + b2 . According to Eq. (23), θ6 and θ7 are computed as follows:

θ6 = ±arccos − ax c1 s2 + az s1 s2 + ay c2 (27) 





θ7 = arctan2 ox c1 s2 + oz s1 s2 + oy c2 /s6 , − nx c1 s2 + nz s1 s2 + ny c2 /s6 , if θ6 = 0 θ7 = φ or θ7 = π+φ, if θ6 = 0

(28) where φ = arctan(b/a), a = nx c1 s2 + nz s1 s2 + ny c2 , b = ox c1 s2 + oz s1 s2 + oy c2 .

−1

−1 Left-multiplying Eq. 2 by 01 T 12 T and right-multiplying it by 56 T 67 T 78 T yields: a3 c3 +a4 c34 = A, −a3 s3 − a4 s34 = B

(29)

392

J. Zhao et al.

where

A = c1 c2 (−ax d7 + px + d6 ox c7 + d6 nx s7 ) − s2 d1 − ay d7 + py + d6 oy c7 + d6 ny s7 +c2 s1 (−az d7 + pz + d6 oz c7 + d6 nz s7 ) B = −d2 + s1 (−ax d7 + px + d6 ox c7 + d6 nx s7 ) + c1 (az d7 − pz − d6 (oz c7 + nz s7 )) According to Eq. (29), θ4 , θ3 and θ5 are computed as follows:   θ4 = ±arccos A2 + B2 − a32 − a42 /2a3 a4



θ3 = arctan2(−(B(a3 + a4 c4 ) + Aa4 s4 ), A(a3 + a4 c4 ) − Ba4 s4 )

(30) (31)

θ5 = arctan2((ax s1 − az c1 )/s6 (nz c1 − nx s1 )s7 + (oz c1 − ox s1 )c7 ) − (θ3 + θ4 ), if θ6 = 0 θ5 = arctan2(C, D) − (θ3 + θ4 ), if θ6 = 0

(32)

where C = ny s7 + oy c7 s2 − (nx c1 + nz s1 )s7 c2 − (ox c1 + sz s1 )c7 c2 , D (nz c1 − nx s1 )s7 + (oz c1 − ox s1 )c7 .

=

3.3 The CCDJAP-IK Method for Solving the IK Problem In summary, we introduced two methods for solving the IK problems in detail. Since the CCD method does not use the Jacobian matrix, it will not encounter the problems caused by Jacobian singularity. Moreover, this method considers the joint limits in the iterative process, ensures that the IK solutions satisfy the constraint conditions. The JAP method derives the analytical expressions of joint angles, so it can quickly find multiple exact IK solutions. However, the convergence rate of the CCD method will become particularly slow when it is close to the target solution, which seriously affects the computational efficiency [7]. Besides, the success rate of the JAP method is highly dependent on the given value of the parameterized joint angle. If this value is not selected reasonably, the solution will fail. Therefore, to utilize the benefits of the two methods, we propose the CCDJAP-IK method. Its solution procedure consists of two phases. The first phase uses the CCD method, and the second phase uses the JAP method. When the number of iterations of the CCD method is greater than iter max or the pose error is less than ε  ), the current value of each joint  (contains position error εpos and orientation error εori angle is output. Then, take out the θ1 , θ2 , θ6 , or θ7 of these output values as the given value of the parameterized joint angle in the JAP method. Finally, multiple exact IK solutions will be obtained.

4 Simulation Experiments We conduct simulation experiments to test the performance of the CCDJAP-IK method. The method is developed in MATLAB R2018b, and a computer featuring an Intel® Core i3-8100CPU (3.60 GHz) processor and 8GB of RAM is used to execute it.

Inverse Kinematics of a Novel SSRMS-Type Reconfigurable Manipulator

393

4.1 Simulation Experiments for Considering the Open Kinematic Chain In this part, we design two experiments. Each experiment randomly generates 10000 groups of the desired pose of the EE within the workspace. In the first experiment, we select a singular configuration as the initial configuration for each solution. For another experiment, we randomly select the initial configuration for each solution. Table 2 describes the parameter settings and solution results. Table 2. Parameter settings and solution results for solving the IK problem. Experiment number

 [m]/ε  [rad ] εpos ori

iter max

Solve rate [%]

Average time [ms]

1

10−6 /10−6

1000

99.51

4.93

2

10−6 /10−6

1000

99.38

4.70

It is clear that the CCDJAP-IK method is not sensitive to the initial or singular configuration of the manipulator. For randomly selected desired poses, the success rate for finding solutions exceeds 99.3%, and the average solution time is less than 5 ms. Therefore, it is an effective inverse kinematics solution method. We use this method to calculate the reachability index of each position in the reachable workspace (see Fig. 5). Then, we use the approach in [9] to obtain the capability map of the reconfigurable manipulator, as shown in Fig. 8. The results in this figure prove that the reconfigurable manipulator has the ability to perform a variety of tasks more flexibly by changing the lengths of the telescopic links.

Fig. 8. The top three pictures belong to the shortest configuration, and the bottom three pictures belong to the longest configuration. The color bar uses 20 kinds of gradient color to indicate the reachability measure, in which blue color means highest and red color means lowest. (Color figure online)

394

J. Zhao et al.

4.2 Simulation Experiment for Considering the Closed Kinematic Chain In this part, we design a simulation experiment to verify that the CCDJAP-IK method can complete the reconfigurable process shown in Fig. 1(b)–(d). The length of each telescopic link is given by a quintic polynomial planning. The planned time is 10 s, and the speed and acceleration at the beginning and end are all equal to zero. According to the procedures in Fig. 1 to reconfigure the manipulator. The two telescopic links are extended from the shortest length to the longest in turn. The optimal trajectory is selected following the minimum joint displacement principle. Figure 9 shows the time histories of the telescopic links and revolute joints.

Fig. 9. Trajectories of quantities associated with the telescopic links and each joint angle during the reconfiguration process.

5 Conclusions We present a novel reconfigurable manipulator with two lockable passive telescopic links. It can achieve reconfiguration by changing the lengths of these telescopic links. Then, we propose an IK solution method named the CCDJAP-IK. This method has the advantages of being insensitive to Jacobian matrix singularities, avoiding joint limits, and finding multiple exact solutions. By analyzing the reachable workspace and building up the capability map prove that the manipulator can perform a variety of tasks more flexibly. The experiments considering the open kinematics chain and the closed kinematics chain verify the effectiveness of the IK solution method. Acknowledgement. The research is supported by the National Natural Science Foundation of China (Grant No. 91848101 and No. 91848202).

Inverse Kinematics of a Novel SSRMS-Type Reconfigurable Manipulator

395

References 1. Xu, W.F., et al.: Analytical and semi-analytical inverse kinematics of SSRMS-type manipulators with single joint locked failure. Acta Astronaut. 105(1), 201–217 (2014) 2. Aghili, F., Parsa, K.: A reconfigurable robot with lockable cylindrical joints. IEEE Trans. Robot. 25(4), 785–797 (2009) 3. Shi, S., He, Q., Jin, M.: Design and Experimental Study on Telescopic Boom of the Space Manipulator. In: Huang, Y., Wu, H., Liu, H., Yin, Z. (eds.) ICIRA 2017. LNCS (LNAI), vol. 10464, pp. 707–716. Springer, Cham (2017). https://doi.org/10.1007/978-3-319-65298-6_63 4. Crane, C.D., Duffy, J., Carnahan, T.: A kinematic analysis of the space station remote manipulator system (SSRMS). J. Robot. Syst. 8(5), 637–658 (1991) 5. Craig, J.J.: Introduction to Robotics: Mechanics and Control, 3rd edn. Pearson Education International, Upper Saddle River, NJ (2018) 6. Zhao, Z.Y., et al.: Workspace analysis for a 9-DOF hyper-redundant manipulator based on an improved Monte Carlo method and voxel algorithm. In: IEEE International Conference on Mechatronics and Automation (ICMA), pp. 637–642. IEEE (2018) 7. Wang, L.C.T., Chen, C.C.: A combined optimization method for solving the inverse kinematics problems of mechanical manipulators. In: IEEE International Conference on Robotics and Automation (ICRA), pp. 489–499. IEEE (1991) 8. Pieper, D.: The kinematics of manipulators under computer control. Ph.D. Thesis, Stanford University (1968) 9. Zacharias, F., Borst, C., Hirzinger, G.: Capturing robot workspace structure: representing robot capabilities. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3229–3236. IEEE (2007)

A Trajectory Optimization Method for Stabilizing a Tumbling Target Using Dual-Arm Space Robots Lei Yan1(B) , Bingshan Hu2 , and Sethu Vijayakumar1 1 University of Edinburgh, Edinburgh EH8 9AB, UK

{lei.yan,sethu.vijayakumar}@ed.ac.uk

2 University of Shanghai for Science and Technology, Shanghai 200093, China

[email protected]

Abstract. Space debris removal is very important for ensuring space safety and further space exploration. However, most of the debris is tumbling in space and its corresponding large mass and inertia result in non-negligible momentum. During the detumbling manipulation of large-momentum space debris, there will be large base disturbance force which makes the control of the floating base of space robots much more challenging. In this paper, we propose a trajectory optimization framework for stabilizing a tumbling object using a dual-arm space robot. In this proposed trajectory optimization framework, only the object dynamics is used to minimize the detumbling time and base disturbance during the detumbling manipulation, while the optimal dual-arm motion is generated from the object motion by considering the closed-chain constraint. The objective function of trajectory optimization is defined as weighted detumbling time and base disturbance force to save energy of the dual-arm space robot for further manipulation. In order to verify the proposed method, a typical simulation of dual-arm space robot stabilizing a tumbling target is carried out. Keywords: Trajectory optimization · Tumbling target · Coordinated planning

1 Introduction In order to reuse or remove failure satellites and space debris, the space robotic system is considered to be the most suitable and efficient way [1]. There are two main types of space robotic servicing techniques [2]. One is the stiff connection capture using robotic arms or tentacles [3]; the other is the flexible connection capture using tethered flying net [4] or flying gripper [5]. The latter one allows a long capture distance between the target and the chaser spacecraft. However, for further maintenance, reuse or other operations of the captured target, the stiff connection capture using robotic arms is a much more promising technique and will be mainly considered in this paper. The whole process of capturing an uncooperative target in space is generally decomposed into three phases: pre-contact, contact and post-contact phases. For pre-contact phase, the joint motion of free-floating space manipulator was generated to minimize © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 396–404, 2021. https://doi.org/10.1007/978-3-030-89092-6_36

A Trajectory Optimization Method for Stabilizing a Tumbling Target

397

the base disturbance [6, 7]. In [8], the dual-arm capturing motion can be generated to intercept the grasping point with zero relative velocity. The capturing occasion was determined by combining the prediction of the grasping point and the workspace of the space robot. The optimal path planning of free-flying space manipulators can be also realized by sequential convex programming [9]. However, only the position coupling between the manipulator and the base was considered in the optimization. For contact phase, the optimal capturing time and configuration of the target were determined to minimize the base disturbance, which required that the contact force should pass through the centroid of the base [10]. After contact, the two arms and the target will form the closed-chain constraint. The internal force analysis and load distribution for cooperative manipulation have been studied in [11–13]. Korayem et al. [11] compared the dynamic load-carrying capacity of a multi-arm robot in the free and constrained mode by load distribution optimization, however, the solution for the dynamic closed-chain circumstance was not considered. For post-contact phase, a time-optimal control method was proposed to stabilize a tumbling satellite which mainly consider the grasping force limitation, the target uncertainty and the arbitrary motion detumbling [14]. Another optimal detumbling strategy was proposed based on quartic Bézier curves and adaptive particle swarm optimization algorithm in which both the detumbling time and control torques are taken into account [15]. In order to reduce the momentum of the tumbling target during post-contact manipulation, an optimal trajectory of the target should be generated, which minimizes the detumbling time and base disturbance while satisfying some equality and inequality constraints simultaneously. Recently, a purely kinematic trajectory optimization method was proposed to manipulate in-grasp object with relaxed-rigidity constraints [16]. The authors in [17] used trajectory optimization for extending the payload capabilities of robotic manipulators. Similarly, trajectory optimization can also be used for motion planning problem in dyadic human-robot collaborative manipulation [18, 19] and dynamic legged robot locomotion such as jumping [20]. Betts et al. [21] reviewed the numerical method for trajectory optimization and discussed the direct and indirect methods. A detailed introduction of using direct collocation for trajectory optimization can be found in [22]. The direct collocation method was recently adopted to generate optimal trajectory for capturing the tumbling target in pre-contact phase [23]. In this paper, we adopt the direct collocation method to generate an optimal trajectory for stabilizing a tumbling target in post-contact phase.

2 Modeling of Dual-Arm Space Robot and Tumbling Object 2.1 Kinematic Equation of Dual-Arm Space Robot A dual-arm space robot manipulating a tumbling target is schematically shown in Fig. 1, where the end-effectors of na degrees of freedom (DOF) manipulator (Arm-a) and nb DOF manipulator (Arm-b) are assumed to be fixed with the tumbling target. The reference coordinate system of each variable is denoted by the left superscript, unless specified, all the variables are expressed in the inertial coordinate system.

398

L. Yan et al.

As shown in Fig. 1, the end-effector velocity of each manipulator can be derived as follows:     (k) ve (k) v0 (k) (k) ˙ (k) + Jm  x˙ e = (1) (k) = J b ω0 ωe where v0 , ω0 are the linear velocity and angular velocity of the base; ve(k) , ωe(k) are ˙ (k) is the joint the linear velocity and angular velocity of the end-effector of Arm-k;  (k) velocity of Arm-k; J b(k) and J m are the Jacobian matrices related to the base and the manipulator respectively, which can be calculated by the following equations:   ×(k) E3 −p0e (k) Jb = (2) O E3     (k) (k) (k) (k) (k) (k) . . . k × p − p × p − p k e n e n (k) k k 1 1 = (3) Jm (k) (k) ... k nk k1 (k)

where p0e is the vector from the centroid of the base to the end-effector of Arm-k; (k) (k) pe , pnk are the position vectors of the end-effector and the nth joint of Arm-k; r× is the skew symmetric matrix of r.

Fig. 1. The dual-arm space robot and the tumbling target object

2.2 Dynamic Equation of Tumbling Object The dynamic equation of the tumbling object which is captured by the dual-arm space robot can be expressed as:   mt v˙ t O3 (4) = Ga F a + Gb F b O3 I t ω˙ t + ωt × (I t ωt ) where vt , ωt are the linear velocity and angular velocity of the object; mt , I t are the T

mass and inertia matrix; Gk (k = a, b) is the grasp matrix of Arm-k [12]; Fk = f Tk τ Tk is the force exerted on the object by Arm-k.

A Trajectory Optimization Method for Stabilizing a Tumbling Target

399

At the same time, given the desired velocity of the target x˙ t , the corresponding (k) velocity of the end-effector of Arm-k x˙ e can be obtained as: 

×  E3 − T rek (k) (5) x˙ e = x˙ t = GTk x˙ t O3 E3 where T rek is the position vector of the end-effector of Arm-k with respect to the reference frame of the object.

3 Trajectory Optimization for Detumbling Manipulation Generally speaking, the process of capturing a tumbling target in space can be decomposed into three phases: the pre-contact, contact and post-contact phase. However, in this paper the trajectory optimization problem is only formulated for stabilizing a tumbling target during post-contact phase. In this trajectory optimization, the dynamics of the tumbling target is considered while the motion of the two arms is generated from the motion of the target by considering the closed-chain constraints between the target and two arms. This trajectory optimization problem is transformed into a nonlinear programming problem (NLP) by direct collocation method [22]. Then the solution of NLP can be found by the nonlinear programming solver fmincon in the Optimization Toolbox of MATLAB. The detailed formulation of the trajectory optimization method is given in the following sections. 3.1 System Dynamics From (4), we can rewrite the dynamic equation of the object as follows:      v˙ t O3 mt E3 O3 + = Ft ω˙ t O3 I t ωt × (I t ωt )

(6)

where Ft is the total force exerted on the object, which is the control variable in trajectory optimization; the state variable in trajectory optimization can be written as: 



(7) s = xTt x˙ Tt , s˙ = x˙ Tt x¨ Tt T  T

T where xt = pTt ψ Tt = px , py , pz , α, β, γ , x˙ t = vTt ψ˙ Tt . The attitude ψ t in xt is represented by the z-y-x Euler angles, the angular velocity in the dynamic equation T

is represented by ωt = ωtx ωty ωtz .

400

L. Yan et al.

In order to deal with the nonholonomic property of the angular velocity, the following relationship is used: ⎡ ⎤⎡ ⎤ 0 −sα cα cβ α˙ (8) ωt = ⎣ 0 cα sα cβ ⎦⎣ β˙ ⎦ = N ωzyx (α, β, γ )ψ˙ t γ˙ 1 0 −sβ ω −1 ψ˙ t = N zyx (α, β, γ )ωt

(9)

where N ωzyx can project the Euler angle rate ψ˙ t to the angular velocity ωt . 3.2 Objective Function For stabilizing the tumbling target by a dual-arm space robot, the objective of trajectory optimization should be minimizing the detumbling time and base disturbance of the dualarm space robot, i.e. minimize the energy consumption during the whole detumbling process. For the dual-arm space robot, the base disturbance mainly comes from the endeffector forces of the two arms to detumble the tumbling target. The total disturbance force exerted on the base, which is caused by the end-effector forces of the two arms, is calculated as follows:     E3 E3 O3 O3

×

× Fa − B Fb Fdis = − B rT + T rea E3 rT + T reb E3 (10)       E 3 O3 E3 O3 E 3 O3 Fa + T × Fb =− B × T r× E rT E 3 reb E3 ea 3 where B rek is the position vector of the end-effector of Arm-k with respect to the reference frame of the base; B rT is the position vector of the reference frame of the target with respect to the base. On the other hand, the operational forces of the two arms also satisfy:     E3 O3 E3 O3 Fa + T × Fb F t = Ga F a + Gb F b = T × (11) rea E3 reb E3 Combining the Eqs. (10) and (11), then we can have:     E3 O3 E3 O3 Fdis = − B × Ft (Ga Fa + Gb Fb ) = − B × rT E3 rT E 3

(12)

It can be seen from (12) that the disturbing force Fdis exerted on the base is related to both the position vector B rT and the detumbling force Ft of the target. The disturbing force should be minimized in order to decrease the base disturbance. Therefore, the objective function of this trajectory optimization for stabilizing the tumbling target can be written as:  tf FTdis Fdis dt (13)  = w1 T + w2 t0

where T = tf − t0 is the detumbling time, w1 and w2 are the weights to trade off the detumbling time and base disturbance.

A Trajectory Optimization Method for Stabilizing a Tumbling Target

401

3.3 Constraints and Limits In order to ensure the target is within the workspace of the dual-arm space robot and the base disturbance is within the controllable range for the whole trajectory, the state and control limits of the trajectory optimization are introduced as follows: xmin ≤ xt ≤ xmax ,

x˙ min ≤ x˙ t ≤ x˙ max

Fmin ≤ Ft ≤ Fmax

(14) (15)

where xmin , xmax are the minimum and maximum poses of the target; Fmin , Fmax are the minimum and maximum forces that can be exerted on the target. Additionally, the state boundary conditions are included: xt (t0 ) = xtini , xt tf = xtfin (16) where xtini , xtfin are the initial state and desired final state of the target, respectively.

4 Simulation Results In order to verify the trajectory optimization framework proposed in this paper, a typical simulation of dual-arm space robot stabilizing the tumbling target with both linear velocity and angular velocity is carried out. The initial linear and angular velocity of the target are set to [0.1 0.05 0] m/s and [0.05 0.04 0.03] rad/s, respectively. The joint angles in the initial state of the two arms are set to [0, 45, 0, 90, 0, 45, 0]T *π/180 rad and [0, −45, 0, −90, 0, −45, 0]T *π/180 rad. The mass and inertia parameters of the base, each manipulator and the tumbling target are shown in Table 1. For this simulation, w1 and w2 are set to 0 and 1, respectively. The simulation result of the whole detumbling process is shown in Fig. 2. The optimal velocity of the tumbling target is generated from the proposed trajectory optimization as shown in Fig. 3. The corresponding end-effector velocities of the two arms can be calculated according to the closed-chain kinematic constraint between the two arms and the target. Then desired joint angular velocities of the two arms are obtained by the inverse kinematics of each manipulator which can be used for detumbling the target by dual-arm space robot. Correspondingly, the base disturbance of dual-arm space robot resulting from the detumbling maneuver is given in Fig. 4.

402

L. Yan et al. Table 1. Mass properties of space robotic system and target Base Redundant manipulator

Mass (kg) i I (kg.m2 ) i

500 iI iI iI

Target

B1

B2

B3

B4

4

2

6

2

B5

B6

6

2

B7 2

100

xx

50

0.012 0.003

0.052 0.003

0.052 0.003

0.003

20

yy

50

0.012 0.003

0.052 0.003

0.052 0.003

0.003

20

0.002 0.0008 0.006 0.0008 0.006 0.0008 0.0008

zz

50

iI

xy

0

0

0

0

0

0

0

0

0

iI

xz

0

0

0

0

0

0

0

0

0

iI

yz

0

0

0

0

0

0

0

0

0

1

2

3

4

5

6

20

Fig. 2. Simulation results. Six sequentially snapshots of the trajectory optimization result.

Fig. 3. The optimal velocity of the tumbling target.

A Trajectory Optimization Method for Stabilizing a Tumbling Target

403

Fig. 4. The disturbance force exerted on the base of the dual-arm space robot.

5 Conclusion In this paper, we propose a trajectory optimization framework for stabilizing a tumbling object using a dual-arm space robot. In this proposed trajectory optimization framework, only the object dynamics is used to minimize the detumbling time and base disturbance during the detumbling manipulation, while the optimal dual-arm motion is generated from the object motion by considering the closed-chain constraint. A variety of space manipulation problem can be formulated into this trajectory optimization framework which can be solved by nonlinear programming solvers. However, in the proposed method only the base disturbance generated from the dual-arm detumbling force are considered. In future study, we will further take into account the base disturbance caused by the dual-arm detumbling motion. Acknowledgement. This research is supported by EPSRC UK RAI Hub in Future AI and Robotics for Space (FAIR-SPACE, EP/R026092/1).

References 1. Yan, L., Xu, W., Hu, Z., Liang, B.: Virtual-base modeling and coordinated control of a dualarm space robot for target capturing and manipulation. Multibody Syst. Dyn. 45(4), 431–455 (2018). https://doi.org/10.1007/s11044-018-09647-z 2. Shan, M., Guo, J., Gill, E.: Review and comparison of active space debris capturing and removal methods. Prog. Aerosp. Sci. 80, 18–32 (2016) 3. Xu, W., Liang, B., Li, B., Xu, Y.: A universal on-orbit servicing system used in the geostationary orbit. Adv. Space Res. 48, 95–119 (2011) 4. Shan, M., Guo, J., Gill, E.: Deployment dynamics of tethered-net for space debris removal. Acta Astronaut. 132, 293–302 (2017) 5. Huang, P., Hu, Z., Zhang, F.: Dynamic modelling and coordinated controller designing for the manoeuvrable tether-net space robot system. Multibody Sys.Dyn. 36(2), 115–141 (2015). https://doi.org/10.1007/s11044-015-9478-3 6. Yan, L., Xu, W., Hu, Z., Liang, B.: Multi-objective configuration optimization for coordinated capture of dual-arm space robot. Acta Astronaut. 167, 189–200 (2020)

404

L. Yan et al.

7. Yan, L., Mu, Z., Xu, W.: Base centroid virtual manipulator modeling and applications for multi-arm space robots. In: International Conference on Control Automation Robotics and Vision (ICARCV), pp. 1542–1547 (2014) 8. Yan, L., Yuan, H., Xu, W., Hu, Z., Liang, B.: Generalized relative Jacobian matrix of space robot for dual-arm coordinated capture. J. Guid. Control. Dyn. 41, 1202–1208 (2018) 9. Misra, G., Bai, X.: Optimal path planning for free-flying space manipulators via sequential convex programming. J. Guid. Control Dyn. 40(11), 3019–3026 (2017). https://doi.org/10. 2514/1.G002487 10. Flores-Abad, A., Wei, Z., Ma, O., Pham, K.: Optimal control of space robots for capturing a tumbling object with uncertainties. J. Guid. Control Dyn. 37, 2014–2017 (2014) 11. Korayem, M., Nekoo, S.R., Esfeden, R.A.: Dynamic load-carrying capacity of multi-arm cooperating wheeled mobile robots via optimal load distribution method. Arab. J. Sci. Eng. 39, 6421–6433 (2014) 12. Erhart, S., Hirche, S.: Internal force analysis and load distribution for cooperative multi-robot manipulation. IEEE Trans. Rob. 31, 1238–1243 (2015) 13. Bais, A.Z., Erhart, S., Zaccarian, L., Hirche, S.: Dynamic load distribution in cooperative manipulation tasks. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 2380–2385 (2015) 14. Oki, T., Abiko, S., Nakanishi, H., Yoshida, K.: Time-optimal detumbling maneuver along an arbitrary arm motion during the capture of a target satellite. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 625–630 (2011) 15. Wang, M., Luo, J., Yuan, J., Walter, U.: Detumbling strategy and coordination control of kinematically redundant space robot after capturing a tumbling target. Nonlinear Dyn. 92(3), 1023–1043 (2018). https://doi.org/10.1007/s11071-018-4106-4 16. Sundaralingam, B., Hermans, T.: Relaxed-rigidity constraints in-grasp manipulation using purely kinematic trajectory optimization. Planning 6, 7 (2017) 17. Gallant, A., Gosselin, C.: Extending the capabilities of robotic manipulators using trajectory optimization. Mech. Mach. Theory 121, 502–514 (2018) 18. Stouraitis, T., Chatzinikolaidis, I., Gienger, M., Vijayakumar, S.: Online hybrid motion planning for dyadic collaborative manipulation via bilevel optimization. IEEE Trans. Rob. 36, 1452–1471 (2020) 19. Stouraitis, T., Chatzinikolaidis, I., Gienger, M., Vijayakumar, S.: Dyadic collaborative manipulation through hybrid trajectory optimization. In: CoRL, pp. 869–878 (2018) 20. Ferrolho, H., Ivan, V., Merkt, W., Havoutis, I., Vijayakumar, S.: Inverse dynamics vs. forward dynamics in direct transcription formulations for trajectory optimization. In: IEEE International Conference on Robotics and Automation (ICRA), pp. 1–8 (2021) 21. Betts, J.T.: Survey of numerical methods for trajectory optimization. J. Guid. Control Dyn. 21, 193–207 (1998) 22. Kelly, M.: An introduction to trajectory optimization: how to do your own direct collocation. SIAM Rev. 59, 849–904 (2017) 23. Jankovic, M., Kirchner, F.: Trajectory Generation Method for Robotic Free-Floating Capture of a Non-cooperative, Tumbling Target. In: Vasile, M., Minisci, E., Summerer, L., McGinty, P. (eds.) Stardust Final Conference. ASSP, vol. 52, pp. 111–127. Springer, Cham (2018). https:// doi.org/10.1007/978-3-319-69956-1_7

Error Analysis of Space Manipulator Joint Module Based on the SDT Theory Wei Tang1 , Xingdong Wang1(B) , Jiabo Zhang2 , and Shengli Liu1 1 Key Laboratory of Metallurgical Equipment and Control Technology, Wuhan University of Science and Technology, Hubei 430081, Wuhan, China {tangwei,wangxindong}@wust.edu.cn 2 Beijing Spacecrafts, China Aerospace Science and Technology Corporation, Beijing 100080, China

Abstract. An error model based on the Small Displacement Torsor (SDT) theory was developed to analyze the influence of errors in manufacturing and assembly on the space manipulator. Geometric and assembly error models were developed using the SDT method; the homogeneous coordinate transformation method was used to develop the error propagation model; and the joint output error was derived. A Monte Carlo simulation was conducted to calculate the distribution probability of the output error, and a sensitivity analysis for critical tolerances was performed. The relationship between the bearing fit clearance and temperature was established based on a simplified finite element model, which was then used to evaluate the influence of temperature on the joint output point. It is found that the joint output position error is distributed approximately normally in the x and y directions, and randomly in the z direction; all of the angular errors are normally distributed; the comprehensive output error is 0.23 mm, which can satisfy the design requirement. As the temperature increases, the fit clearance between the bear and shell increases significantly, causing bear fitting to change from transition fit to clearance fit and resulting in an increase of 21.7% in output error. Keywords: Small Displacement Torsor · Tolerance analysis · Monte Carlo simulation

1 Introduction Joint modules are the central components of space manipulators, composed of many rotating shaft systems featured with thin-walled structure. Geometric errors of parts and non-uniform assembly stresses cause non-uniform stresses and reduce the on-orbit consistency and assembly accuracy. In addition, the space environment with large temperature variation also changes the geometrical dimensions and fitting tolerances and affects accuracy and service life of manipulators. Therefore, there is an urgent need to improve the assembly quality of the joint module by improving the error propagation model that considers both large variations in temperature and the influence of tolerance. Based on the new Geometrical Product Specifications (GPS), scholars have proposed some typical three-dimensional tolerance models, such as the Technologically © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 405–416, 2021. https://doi.org/10.1007/978-3-030-89092-6_37

406

W. Tang et al.

and Topologically Related Surface (TTRS) model [1], the Matrix model [2], the Small Displacement Torsor (SDT) model [3], the Vector Ring model [4]. These models describe the variation of typical tolerance zones in the perspective of the three-dimensional space [5]. However, these tolerance models assume rigid parts, which makes it difficult to solve problems associated with parts featured with small deformation caused by assembling and temperature variations. Tolerance analysis have been established by incorporated the deformation into the existing model in the form of modified variables. For example, Liu proposed a deviation flow theory and a mechanical deviation model considering the assembly spring back and sheet deformation [6]. Samper [7] proposed a simplified model of three-dimensional tolerance analysis considering elastic deformation; He proposed an analysis method of the deformation behavior in elastic assembly [8]. Guo [9] established an assembly error propagation model for an ideal fit surface and analyzed the cumulative variation of tolerances when considering deformations. It is the most commonly used tolerance analysis method that uses the matrix model and torsor model to research the deformation factors. Thermal loads are influenced by factors such as friction and environment temperature, and therefore cannot be ignored in the analysis of tolerances. Some scholars [10] used the elasticity theory to analyze the steady-state/unsteady-state thermal expansion. Jeang [11] used the linear expansion theory to analyze the linear variation of length in one direction. Based on the SDT model, Zhou [12] developed a linear and planar thermal deformation tolerance model. However, the analytical method is only suitable for parts with simple geometry. For complex parts under working conditions, the finite element method is a more effective analysis method. By using the finite element method, the node data of the characteristic surface can be obtained, which provides modified data for calculating the deformation factors in the tolerance analysis. Jayaprakash [13] used Ansys to analyze the effect of gravity and temperature variations in engine motors on assembly tolerances and then optimized the assembly tolerances. Guo [14] proposed an assembly tolerance model that takes into account the deformation of parts for rotating machinery and optimized the assembly tolerance. The studies mentioned above provide a basis for research on the error analysis of the joint module of space manipulators. In this paper, the output error of the joint was modeled by the SDT theory, and SDT torsors for critical tolerances were calculated. Then the output error of the joint and its distribution probability was analyzed. Additionally, the sensitivity analysis of tolerances was also carried out. Furthermore, the influence of the variation of temperature was analyzed by a simplified FEA model.

2 Modeling the Output Error of the Joint Module As shown in Fig. 1, the space manipulator joint module mainly composed of a pair of quick male and female connectors, a joint shell, a motor module, a planetary gears module, and an output shaft module. The quick connectors are parts by which the joint module connects with other manipulator modules. Consequently, the relative position accuracy between them has a direct impact on the overall performance of the manipulator.

Error Analysis of Space Manipulator Joint Module

407

Take the quick male connector, female connector, the shell which directly connects with them, and the output shaft module as the primary research object of the error propagation model. All these parts are characterized by thin-walled, weakly rigid, and large in size. Moreover, these parts are subject to additional deformation due to mechanical loads and environmental temperature variations, which can adversely affect the performance of the manipulator. To analyze the error propagation mechanism of the joint module, a transformation matrix for coordinate in the quick female connector to the male connector was established by using the homogeneous coordinate transformation principle. Coordinate systems were set as Fig. 1. The origin of the reference coordinate system and the joint output reference point (origin of the coordinate system 5) were set as the intersection of the ideal axes of the positioning bolts and the axes of the quick connecters, respectively. Take z0 , z1 as the ideal axis of the quick female connector, and make x2 –x5 coincide with the ideal axis of the output shaft. Then, set the plane yi zi , i = 1 · · · 5, as the ideal mating plane of each part. Through the homogeneous transformation, the transformation matrix from the output point of the joint module to the fundamental coordinate system can be determined and expressed as follows: T 50 = T 10,I T 10,T B10 T 21,I T 21,T B21 · · · T 54,I T 54,T B54 ⎤ ⎡ 1 −γ5 β5 u5 ⎢ γ5 1 −α5 v5 ⎥ ⎥ =⎢ ⎣ −β5 α5 1 w5 ⎦ 0 0 0 1

(1)

Where T ii−1,I is the homogeneous transformation matrix between coordinate systems, which can be directly obtained by the relative position of parts in the ideal assemble condition according to the drawings. The transformation matrix T ii−1,T is expressed as Eq. 2, where T ii−1,T_ j , j = 1 · · · n are the tolerance transformation matrices for each tolerance of the part. Bii−1 is the error matrix caused by the temperature, whose value can be obtained by numerical simulations. T ii−1,T = T ii−1,T_ 1 T ii−1,T_ 2 · · · T ii−1,T_ n

(2)

In the SDT theory, the variation of the geometric feature is represented by the SDT vector D, composed of a rotation vector and a translation vector [5] and is shown in Eq. (3). Using the coordinate transformation theory, the tolerance torsor can be converted into a homogeneous transformation matrix, as shown in Eq. (4), where u, v, w and α, β, γ are translation vector components and rotation vector components, respectively. The SDT expressions of typical tolerance zones are listed in Table 1. Take the quick male connector as an example. The critical dimension tolerances are the coaxiality of the centering boss, the perpendicularity of the flange surface, and the position tolerance of the quick positioning bolt hole, which are shown in Fig. 2. ⎛

⎞ αu D = (ρ, ε) = ⎝ β v ⎠ γ w

(3)

408

W. Tang et al. Bears Output shaft

Joint shell

Bearing retainers

z2, z3 z4

Quick male connector

z5 x2, x3, x4, x5

planetary gears module

z0, z1

Quick female connector

x1 x0

Fig. 1. The main structure of the joint module

⎡

1 ⎢ γ T=⎢ ⎣ −β 0

−γ 1 α 0

β −α 1 0

⎤ u v⎥ ⎥ w⎦

(4)

1

For the coaxiality, the geometric element is the variation of the axis, and the tolerance zone is a cylinder. For cases that the z-axis is the axial direction, the homogeneous transformation expression and constraint equation are shown in Eq. 5, where t = 0.012, h = 2, α and β are normally distributed, and u and v are randomly distributed.

⎡

T 10,T 1

1 0 β ⎢ 0 1 −α =⎢ ⎣ −β α 1 0 0 0

⎧ ⎪ − ht ≤ α ≤ ht ⎪ t ⎤ ⎪ ⎪ − h ≤ β ≤ ht u ⎪ ⎪ ⎪ ⎨ 0≤r≤t v⎥ ⎥; ⎦ ⎪ 0 ≤ φ ≤ 2π 0 ⎪ ⎪ ⎪ u = rcosφ 1 ⎪ ⎪ ⎪ ⎩ v = rsinφ

(5)

For the perpendicularity, there are only constraints on the angle direction but no constraints on the position direction. The homogeneous transformation expression and constraints are as Eq. (6), where t = 0.03, D = 328, α, and β are normally distributed. ⎡

T 10,T2

1 ⎢ 0 =⎢ ⎣ −β 0

0 1 α 0

β −α 1 0

⎤ 0  0⎥ ⎥; −t/D ≤ α ≤ t/D 0 ⎦ −t/D ≤ β ≤ t/D 1

(6)

Error Analysis of Space Manipulator Joint Module

409

Table 1. SDT expression of typical tolerance zones Tolerance zone

Description of tolerance zone SDT torsors Constraint inequations ⎧ ⎫ ⎪ ⎪ 0 u ⎪ ⎪ ⎨ ⎬ Between two concentric u2 + v2 ≤ t 2 /4 0v ⎪ ⎪ ⎪ ⎪ circles ⎩0 0⎭ ⎧ ⎫ ⎪ ⎪ ⎪ ⎨0 0⎪ ⎬ 0 v ⎪ ⎪ ⎪ ⎩γ 0⎪ ⎭

Between two parallel lines

⎧ ⎫ ⎪ ⎪ ⎪ ⎨α u⎪ ⎬ βv ⎪ ⎪ ⎪ ⎩0 0⎪ ⎭

Between two co-axis cylinders

−t/L ≤ γ ≤ t/L −t/2 ≤ v ≤ t/2

−t/L ≤ α ≤ t/L −t/L ≤ β ≤ t/L u = (t/2) cos θ v = (t/2) sin θ u2 + v2 ≤ t 2 /4

⎧ ⎫ ⎪ ⎪ ⎪ ⎨α 0 ⎪ ⎬ β0 ⎪ ⎪ ⎪ ⎩0 w⎪ ⎭

Between two parallel planes

−t/L2 ≤ α ≤ t/L2 −t/L1 ≤ β ≤ t/L1 −t/2 ≤ w ≤ t/2

Fig. 2. The critical tolerances of the quick male connector

For the position tolerance of the quick locating bolt hole on the quick male connector, the error matrix is as Eq. (7), where −0.015 < w < 0.015, and w is randomly distributed. ⎡

T 10,T3

⎤ 100 0 ⎢0 1 0 0 ⎥ ⎥ =⎢ ⎣0 0 1 w⎦ 000 1

(7)

410

W. Tang et al.

For the assembly errors at the two mating pairs of the output shaft-bears-joint shell, the fit clearances can be equivalent to two cylindricities, as illustrated in Fig. 3. The fit clearance between the bearing outer ring and the joint shell at the inner and outer bears is denoted as D1 and D2 and the fit clearance between the bearing inner ring and the output shaft at the inner and outer bears is denoted as d1 , d2 ; and the bearing position distance is denoted by L. Then the equivalent cylindricity error matrix at the fitting position of the outer and inner rings is expressed in Eq. (8). The constraint conditions of the outer ring and inner ring are shown in Eq. (9) and (10), respectively, where α, β are normally distributed. ⎡ ⎤ 1 −γ β 0 ⎢ γ 1 0 0⎥ ⎥ T=⎢ (8) ⎣ −β 0 1 0 ⎦ 0 0 01  D1 +D2 +D2 ≤ β ≤ D12L − 2L (9) +D2 +D2 − D12L ≤ γ ≤ D12L  d1 +d2 +d2 ≤ β ≤ d12L − 2L (10) d1 +d2 d1 +d2 − 2L ≤γ ≤ 2L Error matrices for tolerances of other parts can be obtained by the similar method according to Table 1. After the tolerance error matrix T ii−1,T of each part and the transformation matrix T ii−1,I between the coordinate systems are obtained, the output error of the joint module can be determined by Eq. (11), where u, v, and w are the position errors, α, β, and γ are the angle errors.   e5 = 5i=1 T ii−1,I T ii−1,T − 5i=1 T ii−1,I ⎡ ⎤ 1 −γ β u ⎢ γ (11) 1 −α v ⎥ ⎥ =⎢ ⎣ −β α ⎦ 1 w 0 0 0 1

(a)

Fit clearance at the outer ring of inside bears

Fit clearance at the inner ring of inside bears

h

Fit clearance at the outer ring of outside bears

Fit clearance at the inner ring of outside bears

ΔD1 Δd1

(b) h

ΔD2

Δd 2

Fig. 3. Schematic diagram of equivalent cylindricity of bearing clearance

Error Analysis of Space Manipulator Joint Module

411

3 The Output Error of the Joint and Its Distribution Probability Statistical analysis for output error was carried out based on the Monte Carlo method. Sampling for each tolerance torsor parameter was performed under the condition of satisfying the corresponding constraint equation and distribution probability. The position and the angular error of the output point were obtained after 105 simulations and were shown in Fig. 4. It appears in Fig. 4a that the position error components in the x and y direction obey the normal distribution, and the error component in the z direction is approximately randomly distributed. The average value of the position error in all three directions is close to 0. However, the error interval in the z direction is 464.8 μm, which is significantly larger than the errors in the other two directions. Therefore, the main component of the position error is the z-direction error. The interval of the comprehensive error of the joint module is 235.6 μm, which can meet the design requirements. Figure 4b shows that the mean of all the three angular error components are very small, and they all distribute normally. The mean, standard deviation, and error bandwidth of each error component are listed in Tables 2 and 3, respectively.

Fig. 4. (a) The magnitude and probability of the output position error; (b) The magnitude and probability of the output angle error

Table 2. The output position error of the joint module Mean of position error Standard deviation of Bandwidth of position (μm) position error (μm) error (μm) x direction error

12.5

6.3

38.1

y direction error

1.0 × 10–2

6.7

40.2

z direction error

1.4 × 10–1

116.1

464.8

56.5

235.6

Comprehensive error 102.6

412

W. Tang et al. Table 3. The output angular error of the joint module Mean of angular error (rad)

Standard deviation of angular error (rad)

Bandwidth of angular error (rad)

x-axis angular error

−2.6 × 10–8

2.0 × 10–5

1.2 × 10–3

x-axis angular error

7.1 × 10–7

2.0 × 10–3

1.2 × 10–2

z-axis angular error

1.8 × 10–6

2.0 × 10–3

1.2 × 10–2

4 Sensitivity Analysis of Tolerances The relationship between the perpendicularity of the quick female connector and the position error of the output point is shown in Fig. 5a. There is no obvious difference in the relation curves between the verticality and the position errors, indicating that the verticality of the quick male connector has no significant influence on the output error. The variation of the position error with the perpendicularity of the two axes of the shell increases from 0.01 to 0.04 mm is shown in Fig. 5b. It can be seen that the position error is strongly influenced by the perpendicularity of the shell axes. The position error in the y direction (radial) increases with the increase of the perpendicularity and exhibits an approximately linear relationship with the perpendicularity. In contrast, the errors in the other two directions are less affected by the perpendicularity.

Fig. 5. (a) The influence of the perpendicularity of the quick male connector on the output position error; (b) The influence of the perpendicularity of the shell axes on the output position error; (c) The influence of the fit clearance on the output position error

Error Analysis of Space Manipulator Joint Module

413

The variation of the position error with the bear fitting clearance is shown in Fig. 5c (assuming d1 = d2 = D1 = D2 ). When the clearance increases from 0.01 to 0.1 mm, the position errors in and directions (radial) are significantly affected. By contrast, the error in x direction (axial) is less affected.

5 The Influence of Temperature on the Output Errors To analyze the influence of temperature on the assembly performance of joint modules, a simplified finite element model, shown in Fig. 6a, was established by the commercial FEA software, Abaqus. The FEA model is simplified as a 2D axisymmetric model since all parts in the joint modular are rotating bodies. The constraint conditions of the model are shown in Fig. 6b. The axial displacement and in-plane rotation of the left end face of the shell are restricted. In contrast, the radial displacement is unrestricted, which simulates the variation in the diameter of the shell as the temperature changes. Ten sets of contact constraints at the fitting positions of the bears were set to simulate the assembly relation of the bears and other parts. A tensile force was applied to the right end of the output shaft to simulate the working mechanical load. The mesh element type of each part was CAX4R, an axisymmetric stress element, and the mesh size was 1 mm. The material parameters of each part are listed in Table 4. Table 4. Material parameters used in the finite element model Part

Material

Linear expansion coefficient (K −1 )

Elastic modulus (GPa)

Poisson’s ratio

Output shaft

Titanium alloy TC4

8.6 × 10–6

110

0.34

Inner ring of bears

Bearing steel GCr15

13.63 × 10–6

200

0.3

Outer ring of bears

Bearing steel GCr15

13.63 × 10–6

200

0.3

Shell of the joint

Forged aluminum 7055

2.36 × 10–5

72

0.33

The finite element model used multiple analysis steps to establish the contact constraints between the parts to simulate the assembly process. Furthermore, in the final analysis step, the temperature field rose uniformly from 20 °C to 100 °C. The mean value of the radial displacement before and after the deformation is obtained to determine the change of fit clearance at the bearing position, as illustrated in Fig. 7. As the temperature increases, the clearance increases significantly and changes approximately linearly with temperature. When the temperature rises by 80 °C, the largest clearance variation located at the fitting between the shell and the inside bears, which increased from 0.005 mm to 0.15 mm. The increment of the clearance transforms the fitting from a transitional fit to a clearance fit with a relatively large clearance. In

414

W. Tang et al.

addition, the variations of the clearances are different in the two bears. The clearance at the inside bears is affected by temperature more easily than that at the outside bears, which indicates the insider bears should be adjusted more carefully during assembly. (b)

(a)

Axial displacement constraint

Contact constraint

Contact constraint

Working load

Outer bearing retainer ring

Joint shell Inner bearing retainer ring

Inside Output bears shaft

Outside Inner bearing retainer ring bears

Fig. 6. (a) The finite element model of the output shaft; (b) boundary conditions of the model

Substituting the bearing fit clearance-temperature relationship data into the error matrix for equivalent cylindricity of bearing clearance, the influence of temperature on the output error was obtained, as shown in Fig. 7c. The error in the x direction (axial) is largely independent of temperature, whereas errors in the y and z directions (radial) are almost linearly related to temperature. As the temperature rose from 20 °C to 100 °C, the comprehensive error rose from 0.23 mm to 0.28 mm, with an increment of 21.7%.

Fig. 7. (a) The displacement contour of the output shaft; (b) the bear fitting clearance versus temperature; (c) the output error of the joint module versus the temperature

Error Analysis of Space Manipulator Joint Module

415

6 Conclusion The error propagation model of the joint module was established by the SDT method. Based on the Monte Carlo method, the position and angular error of the output point and their distribution under the given tolerances were obtained by random simulation. The position errors follow a normal distribution in the x and y directions, and a random distribution in the z direction. In contrast, all the angular errors are distributed normally. All the three position errors are within the limit of 0.038 mm, which satisfies the design requirement. Furthermore, sensitivity analyses were performed for the verticality of the quick female connector, the verticality of the shell axis, and the bear fitting clearance. The results indicate that the radial position error is greatly influenced by the latter two tolerances. In contrast, the axial error is less affected by all of these tolerances. The stress and strain of the output shaft system during assembly and temperature varying were investigated based on a simplified axisymmetric FEA model. It is shown that with increasing temperature, the clearance at the bearing fit increases significantly, causing the bearing fitting to change from a transition fit to a clearance fit. Moreover, the influence of temperature is different at the inside and outside bears. The analysis above laid a foundation for further research on the reliability of manipulator arms. Acknowledgement. This work was support by the National Natural Science Foundation of China (51875418), the Joint Funds of the National Natural Science Foundation of China (U1737207), the Hubei Province high value patent cultivation, transformation and industrial projects (201941), and the WUST National Defense Pre-research Foundation (GF201906).

References 1. Desrochers, A., Clément, A.: A dimensioning and tolerancing assistance model for CAD/CAM systems. Int. J. Adv. Manuf. Technol. 9(6), 352–361 (1994) 2. Desrochers, A., Rivi, R.A.: A matrix approach to the representation of tolerance zones and clearances. Int. J. Adv. Manuf. Technol. 13(9), 630–636 (1997) 3. Bourdet, P., Clement, A.: A study of optimal-criteria identification based on the smalldisplacement screw model. CIRP Ann. 37(1), 503–506 (1988) 4. Gao, J., Chase, K.W., Magleby, S.P.: Generalized 3-D tolerance analysis of mechanical assemblies with small kinematic adjustments. IIE Trans. 30(4), 367–377 (1998). https://doi.org/10. 1080/07408179808966476 5. Luo, S.M., Xu, C.: Research progress of tolerance analysis taking into account the deformation. J. Mech. Eng. 54(7), 139–151 (2018) 6. Liu, S.C., Hu, S.J.: An offset finite element model and its applications in predicting sheet metal assembly variation. Int. J. Mach. Tools Manuf. 35(11), 1545–1557 (1995) 7. Samper, S., Giordano, M.: Taking into account elastic displacements in 3D tolerancing: models and application. J. Mater. Process. Technol. 78, 156–162 (1998) 8. Samper, S., et al.: Modeling of 2D and 3D assemblies taking into account form errors of plane surfaces. J. Comput. Inf. Sci. Eng. 4(9), 790–792 (2010) 9. Guo, J., et al.: An ideal mating surface method used for tolerance analysis of mechanical system under loading. Procedia CIRP 10, 306–311 (2013) 10. Luo, Z., Fei, Y.T., Miao, E.: Study on thermal deformation of hollow piece in steady temperature field. Trans. Mater. Heat Treat. 2, 25–27 (2004)

416

W. Tang et al.

11. Jeang, A., Hwan, C.L., Chen, T.K.: A statistical dimension and tolerance design for mechanical assembly under thermal impact. Int. J. Adv. Manuf. Technol. 20(12), 907–915 (2002) 12. Zhou, Q.Z., Qiu, B.: Modeling method of plane tolerance based on SDT and thermal deformation. Mech. Eng. 12, 38–41 (2013) 13. Jayaprakash, G., Thilak, M., SivaKumar, K.: Optimal tolerance design for mechanical assembly considering thermal impact. Int. J. Adv. Manuf. Technol. 73(5–8), 859–873 (2014). https:// doi.org/10.1007/s00170-014-5845-0 14. Guo, J., et al.: A tolerance analysis method for rotating machinery. Procedia CIRP 10, 77–83 (2013)

Toolkit for Dynamic Control Rapid Prototype Simulation System of Robots Applied in Space Experimental Cabin Ning Li, Xiaolong Ma, Chongfeng Zhang, Huaiwu Zou(B) , and Feng Li Shanghai Institute of Aerospace System Engineering, Shanghai, People’s Republic of China

Abstract. As the space environment is difficult to be recreated on the earth, it is essential to develop a software toolkit to validate the parameters designing for space robots. In this paper, a toolkit addressed for dynamic control rapid prototype simulation system of the space robot was presented. Based on the real-time control technology, the toolkit was consisted of real-time dynamic computing, redundant dual-arm control system, visual imaging and measurement and teleoperation. Specifically, the collision problem involved in the manipulator end effector and the target could be analyzed by contact dynamics models in the real-time dynamic computing module. Furthermore, a peg-in-hole (multi cores) assembly experiment involving the disassembly for the space electrical connectors was carried out, which was adopted to evaluate the simulation effect of toolkit. Our results showed that the connection between the space plug and socket with multi cores was well built up in the simulation environment. Moreover, the toolkit can be used to make simulations of on orbit operational missions by multiple of human-computer interaction, which is the key for the evaluation of control algorithm applied on the space robot. Keywords: Space experimental cabin · Toolkit · Space robot · Dynamic control

1 Introduction Nowadays, space robots have been employed in many on-orbit operational tasks, such as space debris capture, screwing and assembly missions [1]. However, it is still a big challenge to determine parameters designing or evaluate the control effects for space robots on account of two reasons [2]. On one hand, the main reason lies in where the working conditions in space is extremely difficult to be simulated in the test on the earth. On the other hand, there is no effective mean to accurately monitor the dynamic behavior of the space robot. Therefore, the toolkit becomes important in design and operation for the space robot. A lot of research works have been done on various applications of the simulation toolkit [3]. Early in 2004, Koenig et al. [4] have designed a 3D dynamic multiple rigid robot toolkit, which could rapidly make the tests of new strategies, concepts, and algorithms dedicated to rigid robotics. In 2013, A low-cost rapid prototyping toolkit was proposed by Kening [5] to design automated movable paper craft, which was consisted © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 417–427, 2021. https://doi.org/10.1007/978-3-030-89092-6_38

418

N. Li et al.

of hardware and software components. And many interaction designs including humanrobot interactions were realized in the research. Practically, it is important to explore a simulation toolkit for the application of automated optical inspection. Such work was done by Stephan et al. [6] and they said the presented toolkit could reduce the design space for automated optical inspection. Furthermore, Coevoet et al. [7] developed a software toolkit used for accurate modeling, simulation and control for soft robots. The digital robot could be simulated in the software and results showed the simulation could acquire a reasonable accuracy compared to the experiment. In order to reduce the computational cost and obtain the real-time simulation effect, Vaughn et al. [8] presented a software toolkit for swarm robotics. Random finite set statistics were adopted by them to design and model the swarming systems. As they reported, many functionalities including dynamic models and cost functions could be realized by their toolkit. In addition, a toolkit to improve the wearables development was introduced by Halajian [9] and it was found it could enhance the ability to analyze and develop activity recognition algorithms. Similar applications are also investigated by many other researchers [10–12]. Interestingly, Enrico et al. [13] employed deep reinforcement technology to design a learning toolkit, which could build the connection between the TurtleBot simulator and the real robot without further training. As the tookit can facilitate the design of robot, it is not surprising that the tookit has been applied in many research fields, such as medicine [14], assistance [15], mobile robot systems [16]. However, there is relatively little literature on application in the onorbit tasks. Although Jiang et al. [17] have presented the in-orbit background simulation by adopting a specific toolkit, their research focused on the space radiation backgrounds and didn’t refer to the application of the robot. In this study, a software toolkit which can demonstrate and evaluate the performance of on-orbit operational tasks using the space robot was built. Firstly, the principle and composition are illustrated. Following, a peg-in-hole assembly experiment based on visual measurement is introduced as a demonstrative experiment.

2 Simulation System of Toolkit 2.1 Rapid Prototype Module for Real-Time Dynamic Computing It is well known that the on-orbit maintenance operation tasks are complex and various, which are accomplished with the collisions between the manipulator end effector and the target. Notably, the contact dynamic characteristics have a wide range. Therefore, it is essential to develop a dynamic functional module to conduct the contact collision modeling and the real-time computing. There are three typical operating contact submodules: screw contact type, arm grab contact type, and electrical connector contact type, which are shown in Fig. 1. In detail, the outline of the screw is conical, and the contact geometry between the screw tool and the screw is conical surface. Besides, the arm is cuboid coupled with a grasping tool designing with a two-claw configuration. Specially, the opening section of single claw is L-shape and the closing section of double claw is square. Furthermore, the electrical connector is a regular octagonal prism, and the electrical connector tool is a regular octagonal inner hole. Notably, all the contact tools are made of aluminum alloy.

Toolkit for Dynamic Control Rapid Prototype Simulation System

419

Fig. 1. Typical operating contact types: (a) screw contact, (b) arm grab contact and (c) electrical connector contact

To obtain the real-time detection of contact collision and calculation of collision force, the highly efficient geometric interference detection algorithm for different contact collision types is required. Usually, the detection process is consisted of the rough detection stage and the precise detection stage. The precise detection stage is essential to make the geometric interference judgment. The contact surface is usually discretized into several triangular or polygonal meshes and the bottom test between basic primitives (including lines, line segments, planes, triangles and polygons) should be performed to determine the intersection state and the accurate collision point information, which require expensive computational cost. In the simulation system, as the control period is 1 ms and the calculation period of dynamic model is around 0.5 ms, it is required that the geometric interference detection and contact collision force calculation should be accomplish in 0.5 ms or even in a shorter time. Considering the contact geometries for the three typical operating contact submodules are regularly, analytical solution can be employed to determine the accurate position of the collision point as Fig. 2 shows. In order to calculate the contact forces of various contact surfaces, the adjustable parameters such as stiffness coefficient are all parameterized, which is convenient for modification. Procedure chart for real-time contact dynamic computing is shown in Fig. 3. In each step of simulation calculation, the position is provided by the dual-arm dynamic simulation system. Thus, the contact force can be calculated and is fed back to the dynamic module for the next integration.

420

N. Li et al.

(a)

(b)

(c) Fig. 2. Schematic diagram of geometric contact collision: (a) screw contact, (b) arm grab contact and (c) electrical connector contact

Toolkit for Dynamic Control Rapid Prototype Simulation System

421

Fig. 3. Procedure chart for real-time contact dynamic computing

2.2 Rapid Prototype Module for Control System As the contact dynamic characteristics range widely, the compliance control technique is employed to perform on-orbit maintenance operation tasks. The control algorithm is shown in Fig. 4. When the task is determined, the operational order of teleoperation is given and the control system module will run. Multiple task can be implemented by the control algorithm including cartesian space/joint space path planning, dual-arm cooperative control, single-arm force compliance control, gravity compensation and dynamic feedforward control, and visual servo control. One of the main applications by using this toolkit is to evaluate the motor servo control strategy and to control the motor moving by rapid prototyping technology. As Fig. 5 shows, the hardware of control system consists of real-time simulator, motor signal interface module and power drive box. Based on the hardware platform, the real computing time by applying the control algorithm can be verified.

422

N. Li et al. Dual-arm switch submodule

Dual-arm control submodule

Pre-programming submodule

Operational order of teleoperation

Start/Pause Motion planning submodule

Fig. 4. Control algorithm for control system

Fig. 5. Hardware for control system

2.3 Functional Module for Visual Imaging and Measurement Simulation There are two main functions for the visual imaging and measurement simulation. First, according to the camera imaging model, the 2D projection of the scene in the camera field is calculated and then can be used to simulate the scene imaging results. Besides, it can recognize the cooperative target in the camera field and calculate the relative position and attitude parameters between the camera and the target according to the known information, which can be used by the control system to realize visual servo control. The coordinates of feature points in the image are required when measuring the position and posture or calibrating the internal and external parameters of the camera, which is key to

Toolkit for Dynamic Control Rapid Prototype Simulation System

423

ensure the measurement accuracy. Extraction process for centroid of target feature points is consisted of subtraction between foreground image A(x,y) and background image B(x,y), feature extraction, centroid calculation and feature point recognition (Fig. 6).

Fig. 6. Images processing: (a) image subtraction; (b) adaptive threshold; (c) detecting plaque

The coordinate of energy centroid is given as below:  ⎞ ⎛  x · I (x, y) y · I (x, y)   ⎜ (x,y)∈DT (x,y)∈DT ⎟   xc , yc = ⎝ , ⎠ I (x, y) I (x, y) (x,y)∈DT

(1)

(x,y)∈DT

where (x c , yc ) is the coordinate of centroid of target area, DT is the target area, I(x,y) is the image grayscale. 2.4 Functional Module for Teleoperation System A pair of 3-degree attitude and position operation hand controller are adopted to conduct teleoperation. By using the hand controllers, rapid prototype simulation of dynamics and control for the dual-arm space robot can be acquired.

3 Simulation Experiment Firstly, calibration experiment for visual imaging and measurement was carried out. Figure 7 illustrates the marking scheme: a group of target marks were set on each robot

424

N. Li et al.

arm. Notably, the pose of the camera on each robot arm relative to the target mark is preseted. Therefore, the precision for visual imaging and measurement can be obtained by comparing the real and measured pose. Results are shown in Table 1. It can be found the measured pose by the camera extremely approaches the real pose. And the maximum relative error is less than 0.5%. Namely, the visual imaging and measurement system have a high precision, which is the foundation of precise operation for the dual-arm space robot.

Target mark

Fig. 7. Description of marks

Table 1. The pose matrix of the camera with respect to the target market in the left robot arm Real value

Measured value

−0.994

−0.108

−0.020

19.94

−0.010

−0.091

0.996

451.84

−0.110

0.990

0.089

113.87

0

0

0

0

−0.994

−0.108

−0.020

19.88

−0.010

−0.090

0.996

451.87

−0.110

0.990

0.089

113.55

0

0

0

0

On-orbit operation tasks are generally divided into four categories: grasping, plugging, screwing and shearing. Especially, screwing and installing the space electrical connector is the most complex space operation integrating the action sequences of guiding, positioning, pressing, locking, turning and unlocking. In the task of transferring the solar wing of the space station from the core cabin to the tail of the experimental cabin, there are 8 electrical connectors required to be disassembled and installed as Fig. 8 depicts. The dual-arm space robot was selected to implement this task in the toolkit simulation system. For the sake of convenient and quick operation, X3, X2, X6, X7, X8 and X1 electrical connectors were disassembled in turn using the left arm of the dual-arm space robot. X5 and X4 were disassembled in turn by the right arm. A typical disassembly scheme represented by using the X3 electrical connector is selected as Fig. 9 presents. It can

Toolkit for Dynamic Control Rapid Prototype Simulation System

425

Fig. 8. Schematic diagram of the disassembly and installation for the electrical connectors from the core cabin to the experimental cabin

be observed the X3 electrical connector can be well disassembled by the space robot. Therefore, conclusions can be obtained that by using the proposed toolkit for dynamic control rapid prototype simulation system of the space robot, the space electrical connector disassembly and installation problem involving contact collision, vision localization and compliance control can be solved.

Fig. 9. Disassembly scheme for the X3 electrical connector using left arm of the dual-arm space robot

426

N. Li et al.

4 Conclusions In this paper, a software toolkit simulating the whole on-orbit operation process was presented. Based on this toolkit, the real time dynamic modelling and computing for typical contact surface, the visual imaging and measurement, and the teleoperation could be accomplished. Specially, the disassembly and installation for the electrical connectors were selected to demonstrates the operation accuracy and validity of the presented toolkit. It should be noted the electrical connectors were applied in the task of transferring the solar wing of the space station from the core cabin to the experimental cabin in the space station. Conclusions can be obtained as follows: (1) The toolkit presented in this paper is valid to demonstrate and evaluate the performance of on-orbit operational tasks. (2) The space electrical connector disassembly and installation problem involving contact collision, vision localization and compliance control can be well solved by the toolkit presented in this paper. In fact, by changing the communication protocol and replacing the simulation dualarm robot with the real space robot, the toolkit can be also used to solve the dynamic control problems referring to real space tasks. Acknowledgement. This research was supported by the National Key Research and Development Program of China under Grant No. 2018YFF0216004 and the National Natural Science Foundation of China with Grant No.11772188.

References 1. Jin, R., Rocco, P., Geng, Y.: Observer-based fixed-time tracking control for space robots in task space. Acta Astronaut. 184, 35–45 (2021) 2. Huang, P., Zhang, F., Cai, J., et al.: Dexterous tethered space robot: design, measurement, control, and experiment. IEEE Trans. Aerosp. Electron. Syst. 53(3), 1452–1468 (2017) 3. Bessler, J., Schaake, L., Bidard, C., et al.: COVR–towards simplified evaluation and validation of collaborative robotics applications across a wide range of domains based on robot safety skills. Biosyst. Biorobot. 22, 123–126 (2019) 4. Koenig, N., Howard, A.: Design and use paradigms for gazebo, an open-source multirobot simulator. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) 3, pp. 2149–2154 (2004) 5. Zhu, K., Zhao, S.: AutoGami: A low-cost rapid prototyping toolkit for automated movable paper craft. In: 31st Annual CHI Conference on Human Factors in Computing Systems, pp. 661–670. Changing Perspectives, Paris (2013) 6. Irgenfried, S., Bergmann, S., Mohammadikaji, M., et al.: Image formation simulation for computer-aided inspection planning of machine vision systems. In: Proceedings of SPIE-The International Society for Optical Engineering. Automated Visual Inspection and machine Vision II, 1033406. Munich, Germany (2017) 7. Coevoet, E., Morales-Bieze, T., Largilliere, F., et al.: Software toolkit for modeling, simulation, and control of soft robots. Adv. Robot. 31(22), 1208–1224 (2017)

Toolkit for Dynamic Control Rapid Prototype Simulation System

427

8. Weirens, V.A., Hisamoto, C.S., Sheikh, S.I.: Simulation toolset for localization and control of swarming vehicles using random finite set theory. In: 2018 IEEE/ION Position, Location and Navigation Symposium, pp. 1286–1293. PLANS, Monterey (2018) 9. Haladjian, J.: The wearables development toolkit: an integrated development environment for activity recognition applications. Proc. ACM Interact. Mobile Wear. Ubiquit. Technol. 3(4), 134:1–134:26 (2019) 10. Haladjian, J., Hodaie, Z., et al.: KneeHapp: a bandage for rehabilitation of knee injuries. In: Proceedings of the 2015 ACM International Joint Conference on Pervasive and Ubiquitous Computing and Proceedings of the 2015 ACM International Symposium on Wearable Computers, pp. 181–184. ACM, Osaka, Japan (2015) 11. Zhou, B., Koerger, H., et al.: Smart soccer shoe: monitoring foot-ball interaction with shoe integrated textile pressure sensor matrix. In: Proceedings of the 2016 ACM International Symposium on Wearable Computers, pp. 64–71. ACM, Heidelberg, Germany (2016) 12. Khan, A., Nicholson, J., Plötz, T.: Activity recognition for quality assessment of batting shots in cricket using a hierarchical representation. Proc. ACM Interact. Mobile Wear. Ubiquit. Technol. 1(3), 1–31 (2017) 13. Marchesini, E., Farinelli, A.: Discrete deep reinforcement learning for mapless navigation. In: 2020 IEEE International Conference on Robotics and Automation, pp. 10688–10694. ICRA 2020, Paris, France (2020) 14. Rogachev, A.F., Melikhova, E.V.: Automation of the process of selecting hyperparameters for artificial neural networks for processing retrospective text information. In: 2nd International Conference on Mathematical Modeling of Technical and Economic Systems in Agriculture, MMTES 2020, 012012. Russian Federation (2020) 15. Rusanu, O.A., Cristea, L., et al.: Virtual robot arm controlled by hand gestures via Leap Motion Sensor. In: 10th Product Design, Robotics, Advanced Mechanical and Mechatronic Systems and Innovation Conference, PRASIC 2018, 012021. Brasov, Romania (2018) 16. Qualls, J., Canfield, S., Shibakov, A.: Kinematic control of a mobile robot performing manufacturing tasks on non-planar surfaces. J. Autom. Mob. Rob. Intell. Syst. 10(3), 12–21 (2016) 17. He, J., Sun, J.C., et al.: In-orbit background simulation study of SVOM/GRM. Astrophys. Space Sci. 365(10), 1–8 (2020)

LSTM Based Model Predictive Control for Flying Space Robot to Track Uncooperative Target Qiang Liu(B)

, Minghe Jin, Bin Wang, and Hong Liu

Harbin Institute of Technology, Harbin 15001, China [email protected]

Abstract. The presented work investigates how to improve the accuracy of tracking a tumbling satellite for flying space robot system with the problem of time delay and unknown inertial frame. It is the time delay existing in the controller that results in manipulator swinging. Besides, in space, it is impossible to find an accurate inertial frame and initial conditions. For this, the dynamic predictive model is established, but it is difficult to be calculated owing to the coupling and non-linearity. The Long Short-Term Memory Network is introduced to solve the problem and the tracking model is fitted by training neurons. A novel dynamic model predictive control based on the Long Short-Term Memory Network is proposed. To overcome measurement noise, the Bayesian Filter is employed and strengthens the stability of the method. The proposed control method is verified by the semi-physical simulation experiment of tracking a tumbling satellite. It shows that the control method improves the low accuracy resulting from time delay and the seesaw phenomenon. Keywords: Model predictive control (MPC) · Long Short-Term Memory Network (LSTM) · Target tracking · Time delay

1 Introduction As one of the most promising techniques, robotics has been extensively applied in on-orbit servicing missions such as rescuing, refueling, and upgrading spacecraft in recent years [1]. Found in past experiments, there is a key problem ignored by most past researches, that is, time delay. For this, a dynamic model predictive control is proposed to compensate the tracking error caused by time delay. However, a novel problem happens to the proposed method, that is, the measurement of the non-inertial frame in space. There are two popular solutions, i.e. gyroscopes integrating [2] and measuring the relative pose between the base and a star [3]. This paper proposes a Long Short-Term Memory Network (LSTM) based dynamic model predictive control (DMPC) to improve this unfavorable situation. The main problems of SRS dynamics are nonlinearity and coupling of the model [4], which are properties of the dynamic model and is relevant to the time-varying configurations. The Control Moment Gyroscopes (CMG) are used as reactionless actuators to improve control performance for the weak dynamical coupling © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 428–439, 2021. https://doi.org/10.1007/978-3-030-89092-6_39

LSTM Based Model Predictive Control for Flying Space Robot

429

system [5]. For nonlinearities, Jacobian based Dynamics model is a popular method to decouple SRS system and has an advantage in obtaining the accurate solution, but it bears dynamic singularities. The approximate transformation of statics coordinates is adopted to decouple the end-effector control input from the spacecraft’s force actuators [4]. However, in this method, partial control variables referring to the inertial frame are difficult to be measured in space. The adaptive control is a popular solution to relax the effect of nonlinearity and coupling [1, 6]. This control scheme has been combined with others method to form novel control schemes, such as Adaptive Jacobian Control (AJC) [7], Adaptive-gain control (AGC) [8] Adaptive Kalman Filter Control (AFKC) [2, 9] and Adaptive Sliding Mode Control (ASMC) [10]. For uncertainties from the inertial frame, disturbance, configuration parameters and initial conditions, an adaptive projection neural network (NN) regresses physical parameter as a constant matrix with online learning. The adaptive fuzzy system (FS) is employed to approximate the uncertain external disturbance and nonlinear joint friction included in a model of the electrically driven manipulator [11]. Similarly, a state observer (SO) is designed to estimate and compensate the compound interference of the 6-dof manipulator system, including parameter uncertainties and unmodeled flexible dynamics [12]. An accurate estimation method is designed for kinematic and inertial parameters based on the Hybrid Kalman Filter (HKF) to assist the dual-arm robot in coordinated capturing [13]. The deficiencies are that the inertial reference frame, the target inertia and the angular accelerate are difficult to obtain in space. In addition, a critical problem is ignored by most researchers, that is, time delay. In a word, the difficulties of control schemes mainly focus on uncertainties of the inertial frame, coupling, nonlinearities and time delay. This paper provides a novel idea to build models without knowing the inner mechanism of a certain system. And then an LSTM based dynamic model predictive control (LSTM-DMPC) is proposed to improve this unfavorable situation. LSTM-DMPC also adopts the Bayesian idea and has capabilities of denoising, self-learning and long-term prediction online.

2 Kinematics and Dynamics 2.1 Kinematics SRS consists of two SSRSM-type manipulators and a spacecraft base actuated by torque device, shown in Fig. 1. By above analyses, the real time is vital for space manipulators to track and capture. Faced with the unknown tumbling target, if the manipulators are not able to make reactions quickly, they may miss the target and even are destroyed. So, the fast inverse kinematics is vital. Herein, an efficient inverse kinematic for SSRMStype manipulators is introduced. This inverse kinematics is based on a hybrid of the analytical and numerical method and has advantages of both methods, that is, efficiency and accuracy. The kinematics model of SSRMS-type manipulators can be designed as Fig. 2. The directions of revolution joints Oi are defined as zi and the inverse kinematics can be obtained and shown in Table 1. More details about inverse kinematics of SSRMStype manipulators can be found in [14].

430

Q. Liu et al.

2.2 Dynamics T  The velocity of the jth link is νj = J Ej (θ)X, where X = 1 ω0 v0 θ˙ is a state variable. θ ∈ Cn , θ˙ ∈ Rn are joint angles and joint angular  velocities and ω0 , v0 arethe base anguJ 0ω (θ) 0 J ωj (θ) lar and linear velocity respectively and J Ej (θ) = . The dynamic 0 J 0v (θ) J vj (θ) of the space robot system is M(θ) · X˙ + C(θ, X) · X = F, where M(θ) ∈ R(6+n)×(6+n) is the generalized inertial and positive definite matrix and C(θ, X) ∈ R(6+n)×(6+n) is the Coriolis centrifugal matrix and they are

Fig. 1. Initial configuration of the manipulators and the initial relative pose between chaser and target.

Fig. 2. The arm plane passes through O4M and is perpendicular to the axes of 4th joint, where O4M is the midpoint of link ‘a4’. The planeO2O4’O5’ is used to figure out the interior angles

 M=

    T 1 0 1 0 , C= , F = 1 τ0 f 0 τJ 0 C ij 0 M ij

where i ∈ [1, 2, 3], j ∈ [1, 2, 3]τ0 , f 0 ∈ R3 represent the base control torque and force and τJ ∈ Rn represents joints control torque. The sub-blocks of the inertial matrix are

LSTM Based Model Predictive Control for Flying Space Robot

431

Table 1. The modified D-H parameters of the SSRMS-type manipulator.

n 

shown as [15] M 22 =

j=0

(j)

RTjb I j Rjb + mj [pbj ]∧T mj [pbj ] ∈ R3×3 , M 23 = −m[pbc ]∧ ∈

R3×3 M 24 =

n 

(j)

RTjb I j + mj [pbj ]∧ RTjb J vj ∈ R3×n , M 33 = mE ∈ R3×3

j=0

M 44 =

n 

(j)

mj J Tvj J vj +J Tωj I j J ωj ∈ Rn×n , M 34 = mJ v ∈ R3×n

j=0

where m =

n  j=0

mj , pbc = m1

n 

mj pbj ∈ R3 are the mass and the mass center of the

j=0

space manipulators system respectively. 2.3 Control Faced with the tumbling target, the tracking maneuver is that the chaser rotates about its base axis to reduce the relative velocity. The adaptive controller is designed as    (1) τ 0 = kv ωd − ω0 , f 0 = kp rd − r0 , τ J = kJ θ d − θ , where (·)d is the desire value of corresponding variable and kv ∈ R3×3 is damping matrix and kp ∈ R3×3 , kJ ∈ Rn×n are stiffness matrixes and all of them are symmetric, positive definite.

3 Dynamic Model Predictive Control 3.1 Increment Equation Owing to the time delay, the controller fails to accurately regulate manipulators to track the target and results in a big tracking error and even target missing or contact damage.

432

Q. Liu et al.

To deal with the problem, in this paper, the dynamic predictive model is proposed to predict the future motion state and compensate time delay t s . Herein, the increasement model is proposed based dynamic at first. Linearizing the dynamic Eq. (4) yields a novel state equation X k+1 = Φ Fk + Φ Xk X k ,

(2)

−1 X where Φ Fk = M −1 k F k+1 , Φ k = E − M k C k and where C k = C(X k , θ k ) M k = M(θ k ), and k is time instance and E is the unit matrix. Obviously, X k+1 is a function about Fk+1 , X k , θk and denoted as

X k+1  X k+1 (Fk+1 , X k , θk ).

(3)

Φ Fk = Φ Fk (θk , Fk+1 ), Φ Xk = Φ Xk (X k , θk ).

(4)

 T ∈ R7+n where q, r, θ are the A novel state variable is introduced Y = q r θ base orientation, position and manipulators’ joint angle expressed in the inertial frame respectively. The corresponding state equation is Y k+1 = Y k + Ak X k+1 ,

(5)

   

 where Ak  A(qk ) = diag 1/2 qk ⊗ , E3×3 , En×n δt and where qk ⊗ is leftquaternion-product matrix and δt is sample time, δt  1. Substituting (2) into (5), we can obtain (6) Y k+1 = Y k + Y k ,  −1 where Y k (X k , Y k , Fk+1 ) = Ak M −1 k F k+1 +Ak E − M k C X k . According to (2), (3), the specific state equations of the internal state variable are derived as  X k+2 = Φ Fk+1 + Φ Xk+1 Φ Fk + Φ Xk X k , X k+3 = Φ Fk+2 + Φ Xk+2 Φ Fk+1 + Φ Xk+2 Φ Xk+1 Φ Fk + Φ Xk+2 Φ Xk+1 Φ Xk X k , · · ·

(7) (8) Equation 8 is the internal variable state equation of the predictive model. From (6), we can know N −1 

Y k+i = Y k+N − Y k  Y k+N |k (X k , Y k , Fk+1 , Fk+2 · · · Fk+N ).

(9)

i=0

Further, according to (5), the increment model can be described by the measurement increment state equation (10)

LSTM Based Model Predictive Control for Flying Space Robot

433

3.2 Generalized Increment Equation Equation 10 is the measurement increment state equation of the predictive model. We assume the base has been rough stabilized in the following discussion and steps in the fine control stage. It is well known that the internal variable is a bound physical parameter and during tracking period from stationary to rotating of the base, the inertial variable measured by SI unit has −1 ≺ X k ≺ 1 where x ≺ ()x means ∀x ∈ x, x0 < (>)x, i.e., each element of x is more (less) than x 0 . After the rough control stage, we have −1 ≺ Φ F ≺ 1, 0 ≺ Φ X ≺, 0 ≺ Φ X · ≺ 1

(11)

where “=”only exists in the first element of Φ Xk .To generalize state equations, the linear scaling technology [2] is applied to deal with this problem. A constant is introduced in Y k+N |k = Y k+N |k · cx = AX A . (12)   where A = A · cx , 0 < cx < 1, X A = X k · · · X k+N −1 ∈ R(7+n)N . Analyzing the definition of Ak in (5), it is can be found that  A ≺ 1. Linearizing A, an operator f (·) is introduced to operate on qk and satisfies qk ⊗ w = f (qk ) ∗ w, where f (qk ) ∈ R4 and ∗ represents multiplication among corresponding elements. Herein, the linear operator f (x) = cf ∗ x is adopted and of course some other operators are also feasible, where cf ∈ R4 is a constant matrix and cf = 0.5 qk ⊗ w\w ∈ R4 , where ·\· is division between the corresponding positions. Substituting the above equation into (12), we can obtain Y k+N |k = c∗f ∗ A∗ X A ,

(13)

T

T where c∗f = c∗fk · · · c∗fk+N −1 ∈ R(7+n)N , A∗ = A∗k · · · A∗k+N −1 ∈ R(7+n)N , c∗fk =  T cf E3 En ∈ R7+n . Further, (12) can be reconstructed as the generalized measurement increment state equation Y k+N |k = A∗ X ∗ ,

(14)

where A∗ = A∗ · cx , X ∗ = cf∗ ∗ X A . 3.3 Dynamic Predictive Model At k th time instance, the chaser and the target are measured with respect to the inertial  ℵ ℵ T ℵ frame and are keeping a pose of Y Tk , Y C and k respectively, where Y k = q r 0n qℵ , rℵ are position and orientation with respect to the inertial frame. Actually, owing T C ,where , Yk+N to time delay, the truth motion state for the target and chaser are Yk+N N is the estimation of time delay. The bias Y TC k+N |k between the target and the chaser contains three parts, including the initial bias, the free nutation of target and the coupled motion of chaser by itself. So, the cost function of the predictive model can be expressed as   T T C C TC TC (15) Y TC k+N |k = Y k + Y k+N |k − Y k + Y k+N |k = Y k + Y k

434

Q. Liu et al.

T C TC T C where Y TC k = Y k − Y k , Y k = Y k+N |k + Y k+N |k is known by measurement of the visual servo system. And Y Tk+N |k , Y C k+N |k need be calculated according to (14). According to (10), (9), it is known that the internal state variable and the measurement increment variable in the predictive model can be calculated by the initial state and control variables. The target predictive model can be simplified as       1 0 1 0 1 , CT = , FT = , (16) MT = 0 MT 0 CT τr



where the space disturbance τr = diag 3nz , 0, −n2z r0 and where r 0 is the distance between two satellites expressed in the chaser and nz is the orbit angular velocity. 3.4 Stability The dynamic of the space robot is the analytical model, which has accurate solutions. The convergence of the model predictive control has been verified in [16]. Therefore, the controller proposed in this paper is converged.

4 LSTM-DMPC T C In (15), Y TC k , Y k+N |k , Y k+N |k can be derived from the measurement data. However, in the practice project, the data is mixed with measurement noise and control errors instead of smooth and continuous simulation data. The noise and errors enable the predictive model tend to diverge and fail to make a long-term prediction. On the other hand, the inertial frame is difficult to determine accurately in space and this results in some variables unavailable. To cope with the problems, LSTM is employed to fit the dynamic predictive model (15) by training and learning. LSTM-DMPC is proposed in this section.

4.1 Bayesian Filter Based LSTM The LSTM is introduced in [16]. The Bayesian filter [16] is embedded in LSTM algorithm f to deal with bounded disturbances and improve the system stability. uk is the filtered th state of k constant input layer. The  computation flow of LSTM based on the Bayesian

 f f filter is uk = fBF uk , hk = fLSTM ck−1 , hk−1 , uk . 4.2 LSTM-DMPC Herein, a long short-term memory-based dynamic model predictive controller (LSTMDMPC) is designed to fit the prediction state equations by learning and training online. The cost function of the LSTM-DMPC to train the network is ˆ e = Y − Y.

(17)

LSTM Based Model Predictive Control for Flying Space Robot

435

and qˆ CT , rˆCT are predicted by minimizing the cost function (17) under external disturbance, unknown inertial frame and time delay. The adaptive control scheme given in (1) can be achieved by substituting qˆ CT , rˆCT as qd , rd . Note that in the design of controller (17), the inertial properties are not required, which avoids the tedious analysis and improve accuracy.

5 Experiment In this section, experimental results present that show the performance of the LSTMDMPC to capture a tumbling satellite. The inputs to the LSTM-DMPC are the sequential pose measurements, while the outputs are close to the true pose increment by training and learning online. The outputs are determined by the non-linear model which is trained by past pose increments and corrected by the current pose in some degree. Figure 3 illustrates the semi-physical simulation experiment setup of space tracing and capturing where industrial robots are employed to simulate the motions of the target satellite and one of the manipulators. The Laser Special Camera System (LSCS) is used to obtain the pose measurements at a rate of 10 Hz and its outputs are mixed with noise which is an approximate standard deviation of 5 mm, 10°. A satellite mockup is moved by an industrial robot according to the relative orbital dynamics. The other robot, equipped with LSCS, is used to simulate the SRMS-type manipulator to autonomously approach and capture the mockup. The communication system adopts Ethernet, 1553B and Shared Memory. More details are shown in Fig. 4. 5.1 Initial Condition For SRS that drives the manipulator and the target satellite, parameters are selected a real space robot system on high earth orbit and they are listed in Table 2. The target rotates freely with an initial angular velocity [5°/s, 0.5°/s, 0.5°/s] and the capture position with a maximum limited speed of 50 mm/s. To reduce the relative motion, the chaser rotates ◦ about its x-axis with a fixed angular velocity, i.e., ωd = [0,  0, 0.5] /s. Parameters of 3 the external disturbance torque expressed in (16) are nz = μe /Re , where μe = 6.7 × 10−11 m3 /s2 is gravitational parameter of the Earth. Re = 3.6 × 107 m is the radius of the circular orbit and r 0 = 2 m. The orbital angular rate is 0.0011 rad/s. The initial pose of SRS and target are shown in Fig. 2. The 50 hidden neurons in single hidden horizon are employed. The process noise covariances and measurement noise covariances are Q = [0.09, 0.5, 6, 0.1, 0.1, 0.1], R = [600, 700, 1000, 400, 400, 700] respectively. Assuming k instant is current time, a block diagram of the LSTM-DMPC scheme is shown in Fig. 5, where z−N is the N-periods backward shift operator. Stiffness and damp coefficients of adaptive conT  trol are ⎡ K = kv , kp , kJ , kv = diag(1050, 950,⎤1303) kp = diag(260, 173, 295) 21.93, 21.71, 19.55, 15.17, 6.68, 3.04, 2.34 ⎢ ⎥ kJ = ⎣ 6.53, 5.94, 5.30, 5.20, 4.75, 2.74, 0.25 ⎦. 10.59, 9.83, 4.15, 3.56, 3.45, 1.69, 0.69

436

Q. Liu et al.

Fig. 3. Semi-physical simulation experiment setup of space tracking and capturing. 1-Teleoperation Inter-face; 2-Robot of Manipulator Simulator; 3-LSCS; 4- Manipulator Simulator; 5-Target Simulator; 6-Robot of Target Simulator; 7-Central Control Unit.

Fig. 4. Communication system flow diagram of Semi-physical simulation experiment Simulator.

Table 2. The inertia of the space robot and the target. Mass (kg)

Inertia (kg * mm2 ) Ix, Iy, Iz, Ixy, Izx, Iyz

Base

1.084

6340.35, 6092.47, 6082.63–13.98, 0.702, −4.32

Link1

5.735

40146.32, 31468.32, 33397.76, 2.38, −2.56, 1751.16

Link2

6.079

40695.20, 37103.94, 31142.10–1413.38, 2131.42, −10137.43

Link3

13.475

125724.10, 8198749.04, 8081948.51, −63180.51, 435274.44, −600.15

Link4

10.823

125621.30, 5795338.11, 5795091.82, −1050.31, 673748.22, −329.40

Link5

6.184

40969.70, 38813.40, 36435.19, −2068.25, −1033.97, 10665.60

Link6

5.735

41073.69, 30408.21, 21534.70, −5.29, 5.30, −1743.10

Link7 (end)

10.704

80515.62, 193236.10, 164616.76, 4.48, −308.16, 40076.69

Body

Target 1992.64 5.557 × 109 , 3.185 × 109 , 3.626 × 109 , −3.64 × 106 ,−1.08 × 107 , 1.305 × 107 Dual manipulators have the same parameters

5.2 Results and Discussion The input of LSCS in the experiment is mixed with noise which is an approximate standard deviation of 5 mm, 10°. With the help of capabilities of training online and self-learning of LSTM-DMPC, the controller can be applied to improve the unfavorable situation resulting from time delay and the missing inertial frame. The proposed LSTMDMPC is appropriate for the problem with less 5 s system delay. In order to demonstrate the effectiveness and convergence, the zero initial conditions are given to predicted variables and weights Woh , Wfh , Wih , Wc˜ h corresponding to ok , f k , ik , c˜ k . The simulation is executed for 450 s, which is fully enough for tracking target satellite. The algorithm

LSTM Based Model Predictive Control for Flying Space Robot

437

weights convergence performance is defined as the total weight of all hidden neurons for each derivation term, that is,

Fig. 5. Communication system flow diagram of Semi-physical simulation experiment Simulator.

Fig. 6. The algorithm weights convergence performance the proposed LSTM-DMPC.

Wχk h =

Nχ i Nχ j 1  Wχ h (i, j) Nχ i

(18)

i=1 j=1

where χ = i, f , h, c˜ , Nχ i = Nχ j = 50. Figure 6 shows that in the dynamic prediction, weights converge with an ideal rapid. The experiment results are shown in Fig. 7, which shows measurements, predictions, the truth and the filtered data respectively, where the predictions are derived from the dynamic predictive model (15) and compared with the truth data. Although there exists big bias in the beginning stage, the prediction is still quickly convergent within 100 s. Compared with only Bayesian or Kalman (KF) process method, the proposed controller not only suppress noise but also improves

 the ˆ eqCT = arccos 2q (1) . In motion lag. The orientation error is defined as q = q ⊗ q, order to quantitatively assess the system performance of LSTM-DMPC, the prediction tracking errors are shown in Fig. 8. During the stable prediction period, the mean prediction position error is 42.18 mm and the prediction orientation errors converges to 4.36°. In order to verify the superiority performance of LSTM-DMPC, BF-CTC [17] is employed as the comparative test, which do not have LSTM-DMPC unit in Fig. 5.

438

Q. Liu et al.

LSTM-DMPC and BT-CTC are compared in terms of errors and shown in Fig. 8a). Compared with the widely-used effective robot control method BF-CTC, the position accuracy of LSTM-DMPC is raise to over 130% and the orientation accuracy to over 110%. The capture tolerance (60 mm) is satisfied. Figure 8b) shows that the “seesaw” phenomenon is improved obviously with the help of LSTM-DMPC.

Fig. 7. The predicted results of LSTM-DMPC

Fig. 8. The comparison between LSTM-DMPC and BF-CTC. a) Compared with BF-CTC, the mean position error of LSTM-DMPC is cut down by 30% and the mean orientation error is cut down by 11%. b) The “seesaw” phenomenon of the manipulator delay is improved obviously.

6 Conclusion This paper proposes a novel LSTM-DMPC to improve the tracking accuracy of flying SRS with time delay and unavailable inertial frame. Additionally, measurement noise is also mixed in to verify the stability of LSTM-DMPC. To couple with nonlinearities and coupling of the dynamic predictive model, LSTM is employed to fit the dynamic predictive model by training online. With the help of the self-learning capability of LSTM-DMPC, the trained model performs well in improving convergency and accuracy. To overcome disbalances of measurement noise, the Bayesian Filter is employed

LSTM Based Model Predictive Control for Flying Space Robot

439

and strengthens the stability of the proposed controller. The semi-physical simulation experiment has been employed to illustrate the performance of the proposed controller. The proposed controller has many potential applications in some degree not only to space robots, but also to industrial robots.

References 1. Zhang, B., Liang, B., Wang, Z., Mi, Y., Zhang, Y., Chen, Z.: Coordinated stabilization for space robot after capturing a noncooperative target with large inertia. Acta Astronaut. 134, 75–84 (2017) 2. Aghili, F., Parsa, K.: An adaptive vision system for guidance of a robotic manipulator to capture a tumbling satellite with unknown dynamics. In: IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE (2008) 3. Wang, H., Xie, Y.: Prediction error based adaptive Jacobian tracking for free-floating space manipulators. IEEE Trans. Aerosp. Electron. Syst. 48, 3207–3221 (2012) 4. Giordano, A.M., Ott, C., Albu-Schaffer, A.: Coordinated control of spacecraft’s attitude and end-effector for space robots. IEEE Robot. Autom. Lett. 4, 2108–2115 (2019) 5. Carpenter, M.D., Peck, M.A.: Reducing base reactions with gyroscopic actuation of spacerobotic systems. IEEE Trans. Rob. 25, 1262–1270 (2009) 6. Yao, X., Tao, G., Ma, Y., Qi, R.: Adaptive actuator failure compensation design for spacecraft attitude control. IEEE Trans. Aerosp. Electron. Syst. 52, 1021–1034 (2016) 7. Wang, H., Xie, Y.: Passivity based adaptive Jacobian tracking for free-floating space manipulators without using spacecraft acceleration. Automatica 45, 1510–1517 (2009) 8. Zhou, Z.-G., Zhang, Y.-A., Zhou, D.: Robust prescribed performance tracking control for freefloating space manipulators with kinematic and dynamic uncertainty. Aerosp. Sci. Technol. 71, 568–579 (2017) 9. Aghili, F., Parsa, K.: An adaptive Kalman filter for motion estimation/prediction of a freefalling space object using laser-vision data with uncertain inertial and noise characteristics. In: AIAA Guidance, Navigation and Control Conference and Exhibit (2008) 10. Jia, Y., Misra, A.K.: Robust trajectory tracking control of a dual-arm space robot actuated by control moment gyroscopes. Acta Astronaut. 137, 287–301 (2017) 11. Chu, Z., Cui, J., Sun, F.: Fuzzy adaptive disturbance-observer-based robust tracking control of electrically driven free-floating space manipulator. IEEE Syst. J. 8, 343–352 (2014) 12. Li, C., Zhang, T., Yang, J.: Attitude control of aircraft using only synthetic jet actuators when stall occurs. IEEE Access 6, 37910–37917 (2018) 13. Peng, J., Xu, W., Pan, E., Yan, L., Liang, B., Wu, A.-G.: Dual-arm coordinated capturing of an unknown tumbling target based on efficient parameters estimation. Acta Astronaut. 162, 589–607 (2019) 14. Jin, Q.L.M., Wang, B., Liu, H.: An efficient and accurate inverse kinematics for 7-dof redundant manipulators based on a hybrid of analytical and numerical method. IEEE Access 8, 16316–16330 (2020) 15. Giordano, A.M., Calzolari, D., Albu-Schaffer, A.: Workspace fixation for free-floating space robot operations. In: International Conference on Robotics and Automation (ICRA), Brisbane, Australia. IEEE, pp. 889–896 (2018) 16. Gao, C., Yan, J., Zhou, S., Varshney, P.K., Liu, H.: Long short-term memory-based deep recurrent neural networks for target tracking. Inf. Sci. 502, 279–296 (2019) 17. Shi, L., Kayastha, S., Katupitiya, J.: Robust coordinated control of a dual-arm space robot. Acta Astronaut. 138, 475–489 (2017)

Design and Engineering Realization of the Joint for Chang’E-5 Surface Sampling Arm Kang Wang(B) , Wencheng Ni, Zhijun Zhao, Xin Liu, Yaobing Wang, and Shuiqing Jiang Beijing Key Laboratory of Intelligent Space Robotic Systems Technology and Applications, Beijing Institute of Spacecraft System Engineering, Beijing 100094, China

Abstract. Joints are the basis for the dexterous movement and precise positioning of China Chang’E-5(CE-5) Surface Sampling Arm which had successfully completed the lunar soil sampling. The sampling arm consists of four joints in total, among which the composition and layout of the shoulder yaw joint, shoulder pitch joint and elbow pitch joint are similar. In addition to the output speed and torque function, the wrist pitch joint also integrates the lunar-touching sensing function and the cable routing function. This article introduces the task requirements, joint scheme design, engineering realization and verification of the robotic arm joints. Keywords: Joint scheme · Sampling arm · Lunar exploration

1 Introduction China CE-5 detector successfully completed the lunar sample return mission on December 17, 2020. The CE-5 detector has two sampling methods: surface sampling and drilling sampling [1, 2]. A total of 1731 g samples were collected, of which most of the sampling volume was completed by surface sampling. The surface sampling method refers to multi-point sampling on the lunar surface by a robotic arm with multi-functional sampler [3, 4]. The joint is an important part to realize the dexterous moving and precise positioning of the robotic arm. Different types of joints are mainly different in transmission devices, but space robotic arm joints usually adopt the driving and braking form of “brushless DC motor + brake”. The transmission device mainly includes planetary gear and harmonic reducer. For example, the joints of Canadarm and European Robotic Arm used in the International Space Station all use planetary reducers [5–7], while the Japanese experimental cabin SFA (Small Fine Arm), “Phoenix” probe, and “Opportunity” Mars rover equipped with small robotic arm joints all use harmonic reducers [8–10]. The joints of the Chang’e-5 sampling robotic arm introduced in this paper are mainly driven by a permanent magnet synchronous motor, a 2K-H (sun gear-planet carrier) planetary reducer, and a harmonic reduction drive. Motor cogging torque is used to provide braking torque to replace brake. Finally, the joint load, torque compensation, touch sensing, cable resistance torque, dust resistance and other performance are tested.

© Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 440–450, 2021. https://doi.org/10.1007/978-3-030-89092-6_40

Design and Engineering Realization of the Joint

441

2 Joint Requirements Analysis The sampling mission is implemented on the lunar surface by the robotic arm after the detector has landed. As shown in Fig. 1, the robotic arm transfers the end sampler to the surface of the lunar. When the sampler realizing the lunar soil sample, it places the sample to the primary package many times, then the primary package filled the lunar soil is grabbed and transferred to the sealed package by the robotic arm. sealed package primary package sampling robotic arm

ascender detector

Fig. 1. Lunar surface sampling.

There are three typical states in the process of robotic sampling task [11], as shown in Fig. 2, which are a) robotic arm sampling status, b) robotic arm lofting or grabbing primary package status, and c) robotic arm transferring primary package.

Fig. 2. Typical states during the sampling process.

According to these typical working states, the sampling robotic arm adopts 4 degrees of freedom to meet the accessibility requirements. As shown in Fig. 3, the robotic arm consists of four rotary joints, and also includes two links (link A and link B), sampler and camera, etc.

elbow pitch joint link B

sampler

shoulder pitch joint link A

camera

shoulder yaw joint

wrist pitch joint

Fig. 3. The composition of the sampling arm.

The lunar surface has a harsh environment and a wide range of temperature changes, and Lunar dust will be raised during landing. The robotic arm joints are required to be

442

K. Wang et al.

dust-proof and withstand large-scale temperature changes. The topography of the lunar surface is unknown, and the wrist pitch joint is required to return the lunar-touching sensor signal before sampling. There is a 1/6 g on the lunar surface, and considering that the probe may tilt to a certain angle after landing on the lunar surface, in addition to output torque, the mechanical arm joint should also have the ability to resist bending moment and brake after power failure. Considering the sampling task and the lunar surface environment, the main functional performance requirements of the robotic arm joints are shown in Table 1. Table 1. Main indicators of the joints. Indicators

Shoulder yaw

Shoulder pitch

Elbow pitch

Wrist pitch

Max output torque

80 Nm

200 Nm

80 Nm

20 Nm

Bending moment

62 Nm

19 Nm

9 Nm

5 Nm

Braking torque

40 Nm

70 Nm

40 Nm

20 Nm

Max speed

2 °/s

Working temperature

−25 °C to 165 °C

3 Joint Design 3.1 Layout and Composition The layout and connection relationship of the four joints on the robotic arm is shown in Fig. 4. The shell of shoulder yaw joint is connected to the detector, and the output shaft is connected to the shell of shoulder pitch joint. The output shaft of the shoulder pitch joint is connected to the link A. link A is connected to the shell of elbow pitch joint, the output shaft of the elbow pitch joint is connected to link B. link B is connected to the wrist pitch joint. The wrist pitch joint is composed of a driving component, a lunar-touching sensor component and a winding component. The output shaft of the driving component is connected to the end sampler. shoulder pitch joint

elbow pitch joint

link B drive component

wrist pitch joint winding component

link A shoulder yaw joint

link A link B

lunar touch sensor component

Fig. 4. The connection relationship of the joints on the sampling arm.

The components of the shoulder yaw joint, shoulder pitch joint and elbow pitch joint are similar. The following focuses on the elbow pitch joint and wrist pitch joint to

Design and Engineering Realization of the Joint

443

introduce in detail. As shown in Fig. 5-a), the elbow pitch joint is mainly composed of a single-channel resolver, a permanent magnet synchronous motor, a planetary reducer, a harmonic reducer, and a dual-channel resolver. After the permanent magnet synchronous motor is decelerated by “planet + harmonic”, the output speed is reduced and the output torque is amplified. The single-channel resolver has a low accuracy and is arranged at the end of the motor to measure the rotation angle and speed of the motor to achieve SVPWM control (space vector control). The dual-channel resolver has high precision, which can reach 30 . It is arranged at the end of the joint output shaft to accurately measure the joint rotation angle, which is used to realize the closed-loop control of the output shaft position. As shown in Fig. 5-b), in addition to the above components, the wrist pitch joint also adds a first-stage bevel gear pair to achieve deceleration. At the same time, in order to realize the information perception when the mechanical arm touching the lunar surface and the cable routing of the sampler, a lunar-touching sensor component and a winding component are respectively arranged. The transmission ratios of elbow pitch joint and wrist pitch joint are 3147 and 3665 respectively.

single-channel resolver

harmonic reducer

planetary reducer

harmonic reducer

winding component

dual-channel resolver

motor

motor planetary reducer dual-channel resolver

a) elbow pitch joint

single-channel resolver

bevel gear reducer

lunar touching sensor component

b) wrist pitch joint

Fig. 5. The composition of the joint. a) Elbow pitch joint. b) Wrist pitch joint.

3.2 Motor and Brake The commonly used braking scheme for robotic arm joints is to add an electromagnetic brake at the end of the motor. For example, the robotic arm joints on the Mars rover “Courage” and “Opportunity” use electromagnetic brakes [10]. In order to save the weight of the brake, a permanent magnet synchronous motor is used in the joint in this paper, and the cogging torque of the motor is used as the braking torque of the entire joint. The number of slots in the stator of the motor is 18, and the number of pole pairs in the rotor is 6, which means a total of 12 poles. Within one rotation period, 36 cogging torque points can be generated. Cogging torque is inherent to the cogging permanent magnet motor. It is the torque generated by the magnetic field generated by the permanent magnet and the cogging of the armature core in the circumferential direction when the armature winding is not energized. Cogging torque is generated from the tangential force between the permanent magnet and the armature tooth, which makes the rotor of the permanent magnet motor have a tendency to align with the stator along a certain direction. This trend attempts to locate the rotor at a position close to the 36 torque points.

444

K. Wang et al.

When the motor continuously rotates, an oscillating torque is generated. In conventional applications, the torque is suppressed as an unfavorable factor. In the joint design of this article, the cogging torque is used as the braking torque, and the torque amplitude is 40% of the rated output torque of the motor. In the process of joint control, the cogging torque can be compensated according to the test rotation angle of the resolver at the motor end to eliminate the speed fluctuations caused by the cogging torque. The compensation principle is shown in Fig. 6. The rated torque of the motor is 0.05 Nm and the speed is 1000 r/min, which meets the output requirements of the joints.

magnetostatic torque

……

rotation angle compensation torque

……

rotation angle

Fig. 6. Cogging torque compensation principle.

3.3 Lunar-Touching Sensor Component The lunar-touching sensor component is a part of the wrist pitch joint and is arranged at the end of the robotic arm in a circular state. As shown Fig. 7, when the robotic arm drives the lunar-touching sensor component to touch the lunar surface at different angles α (−90°–+90°), it can feed back the sensing signal, which can realize the touch perception of the unknown unstructured lunar surface environment. After confirming that the end of the robotic arm touches the lunar surface, the distance between the end of the robotic arm and the lunar surface can be further controlled to effectively control the sampling depth of the sampler. robotic arm lunar touch sensor

α

rock

lunar surface

Fig. 7. Sampling arm touches the lunar.

Design and Engineering Realization of the Joint

445

The working principle of the lunar-touching sensor component is shown in Fig. 8. The lunar-touching sensor component is mainly composed of a supporting inner ring and a lunar-touching outer ring. Metal copper rings are installed on the support inner ring and the lunar-touching outer ring, and the insulating material is used to avoid direct contact with the main structure (metal material). The cables from the copper ring are respectively connected to the positive and negative electrodes of the acquisition circuit. Eleven sets of elastic units (Each group of elastic unit consists of 2 guide rods and 2 compression springs) are arranged between the lunar-touching outer ring and the inner support ring, so that the outer ring is in a floating state. When the lunar-touching sensor component touches the lunar surface, the two copper rings are connected due to the pressure on the outer ring of the lunar-touching sensor, which can return the “lunar touch” signal to the controller. After the lunar-touching sensor component leaves the lunar surface, the inner and outer rings are reset by the spring and its own elastic force, and the conduction signal of the two copper rings disappears. outer ring

positive copper

zoom

compressed spring cable connection point

inner ring

guide rod

anode copper

Fig. 8. The principle of lunar-touching sensor component.

The touch force of the lunar-touching sensor component includes the elastic deformation force of the outer ring and the elastic force of the compression spring. The distance between the outer ring and the inner ring of the sensor is 2 mm. After the finite element analysis, the deformation force of the outer ring of the lunar-touching sensor component with a deformation of 2 mm is 13.1 N, and the elastic force of the compression spring is 12.2 N, which is 25.3 N in total. 3.4 Cable Routing The winding component is circular, which is part of the wrist pitch joint and is coaxial with the joint output axis. As shown in Fig. 9, the winding component is mainly composed of a winding box and a winding reel. The winding box is fixed to the joint shell, and the winding reel is connected to the joint output shaft. The cable of sampler passes through the joint output shaft in two strands, which are assembled in one strand and fixed on the winding reel. When the reel rotates with the output shaft of the joint, the cable is coiled in the reel box in a spiral shape.

446

K. Wang et al. winding box

cable fixing point

cable fixing point

cable

winding reel winding box

winding reel

Fig. 9. Cable routing scheme for wrist pitch joint.

4 Ground Engineering Verification The installation photo of the four joints on the robotic arm is shown in Fig. 10. The ground engineering verification methods of the robotic arm joints are introduced below. elbow pitch joint

wrist pitch joint

shoulder pitch joint shoulder yaw joint

Fig. 10. The physical installation photo of the joints on the robotic arm.

4.1 Load Capacity Verification at High-Low Temperatures The joints of the sampling robotic arm bear both bending moment and torque during operation. For this purpose, a combined bending moment and torque loading equipment is designed. The principle is shown in Fig. 11. The joint is placed in a high-low temperature environment, a lever and a steel rope are used to apply a stable bending moment to the output of the joint (as shown in Table 1), and a dynamic torque is applied in the direction of the rotation axis. The results show that after the joint has experienced a low temperature storage environment of −95 °C, it can normally output the torque required in Table 1 and a speed of 2 °/s under a working environment between −25 °C and 165 °C. vacuum and high-low temperature environments jiont

torque sensor

torque loading equipment

weight bending moment loading equipment

Fig. 11. The principle of joint load test under high-low temperature.

Design and Engineering Realization of the Joint

447

4.2 Performance Verification of Braking and Compensation The robotic arm joint uses the motor cogging torque to provide the braking torque of the entire joint. The test verification method at high-low temperatures is similar to that in Fig. 11, that is, when the joint is powered off, the output shaft of the joint is gradually loaded by the torque loading device until the joint rotates, and the torque sensor data is recorded at the same time. Taking the elbow pitch joint as an example, the braking torque at 165 °C is 56 Nm, and the braking torque at −25 °C is 64 Nm. The large braking torque at low temperature is mainly due to the increase in the viscosity resistance torque of the grease inside the joint reducer in a low temperature environment. Give the motor expected speed 95.49 RPM, test the speed stationarity of the motor with or without current compensation. The test results are shown in Fig. 12.

Fig. 12. Comparison of motor speed before and after compensation.

The motor speed range is 55–136 RPM with current feedforward compensation, According to the national standard [12], the speed fluctuation is 42.41%, and the running process produces large jitter. After compensation, the motor speed range is 84–103 RPM, and the speed fluctuates is 10.16%, then the motor runs smoothly. It can be seen from the test results that under the condition that the control parameters remain unchanged, the motor speed stability is significantly improved after the current feedforward compensation. 4.3 Functional Verification of Lunar-Touching Sensor Component The functional verification principle of the lunar-touching sensor component of the wrist pitch joint under vacuum and high-low temperature environments is shown in Fig. 13. The lunar-touching sensor component in the high-low temperature and vacuum simulation environment (vacuum tank) is placed directly under the touch head. The turntable A outside the vacuum tank can drive the touch head to move up and down to touch the lunar-touching sensor component. At the same time, the lunar-touching sensor component is connected with the rotating shaft passing through the vacuum tank on the left side. When the turntable B outside the vacuum tank is rotated, the lunar-touching sensor component can be driven to rotate at different angles, which can simulate the touch sensing function of different positions of the lunar-touching sensor component.

448

K. Wang et al. touch head

turntable A

turntable B

lunar touch sensor component

Fig. 13. Photo of the functional test of Lunar-touching sensor component.

The results show that after the lunar-touching sensor component has experienced a low temperature storage environment of −95 °C, it can normally feed back touching sensor signals in a working environment of −25 °C to 180 °C. After the test, the touch force range of the different positions of the lunar-touching sensor component is 18.6– 24.5 N. 4.4 Cable Resistance Torque Test of Wrist Pitch Joint The wrist pitch joint uses winding components to realize cable routing. Compared with other joints, the cable resistance moment is larger. Therefore, the cable resistance torque of the wrist pitch joint at different temperatures was tested. The test principle is shown in Fig. 14. The shell is connected with the torque sensor. The rotating shaft is connected to the motor, which can rotate forward and backward. The cable resistance torque is detected during the rotation of the rotating shaft from −360° to 0°. torque sensor

shaft

shell

rotation

Fig. 14. The principle of cable resistance torque test.

The cable resistance torque is mainly composed of two parts, one part is the friction force between the cable and the shell, and the other part is the elastic force of the cable itself, of which the friction between the cable and the shell accounts for a larger proportion. Figure 15 shows the cable resistance torque test curve. The process curve from 0° to 360° means that the shaft rotates clockwise, and the cable is gradually squeezed out of the winding box during this process. The return curve from 360° to 0° means that the shaft rotates counterclockwise, and the cable is gradually retracted into the winding

Design and Engineering Realization of the Joint

449

box during this process. It can be seen from the curve that the resistance torque gradually increases as the temperature decreases. The maximum process resistance torque at −20 °C is 2.18 Nm, and the maximum return resistance torque is 2.60 Nm.

Fig. 15. Test curve of cable resistance torque of wrist joint.

5 Conclusion This paper introduces in detail the functional requirements, scheme design and engineering realization and verification of CE-5 lunar surface sampling robotic arm joints. The four joints of the robotic arm are equipped with permanent magnet synchronous motors, 2K-H planetary reducers, harmonic reducers, bevel gear reducers, resolvers, and so on. The joint uses the motor cogging torque to provide the joint braking torque, and the speed fluctuation caused by the cogging torque is eliminated by the feedforward torque compensation control. This scheme eliminates the configuration of the brake. To meet the needs of lunar-touching sensing at the end of the robotic arm and the wiring of the sampler, a lunar-touching sensor component and a winding component are specially designed for the wrist pitch joint. The test verified the bending and torsion bearing capacity, braking capacity, speed stability, touch sensing function, and cable resistance torque characteristics of the robotic arm joint under vacuum and high-low temperature environments.

References 1. Ma, R.-Q., Jiang, S.-Q., Liu, B., et al.: Design and verification of a lunar sampling manipulator system. J. Astronaut. 39(12), 1315–1322 (2018) 2. Sun, Q.-C., Ji, J., Qin, J.-J., et al.: Research on unlockable servo stabilizer on lunar sampling drill. Chin. J. Eng. Des. 25(6), 735–740 (2018) 3. Zheng, Y., Yao, M., Jin, S., et al.: Lunar surface sampling point selection for uneven terrain. Chin. Space Sci. Technol. 39(2), 41–48 (2019) 4. Jiang, S., Liu, R., Lin, Y., et al.: Design and test of a sampler for lunar surface regolith. Chin. Space Sci. Technol. 39(1), 49–58 (2019)

450

K. Wang et al.

5. Gibbs, G., Sachdev, S.: Canada and the International Space Station program: overview and status. Acta Astronaut. 51(1), 591–600 (2002) 6. Laryssa, P., Lindsay, E.: International Space Station robotics: a comparative study of ERA, JEMRMS and MSS. In: 7th ESA Workshop on Advanced Space Technologies for Robotics and Automation ‘ASTRA 2002’ ESTEC, pp. 19–21. Noordwijk, Netherlands (2002) 7. Boumans, R., Heemskerk, C.: The European robotic arm for the international space station. Robot. Auton. Syst. 23(1/2), 17–27 (1998) 8. Wakabayashi, Y., Morimoto, H., Satoh, N., et al.: Performance of Japanese Robotic arms of the international space station. In: 15th Triennial World Congress, pp. 115–116. Barcelona, Spain (2002) 9. Bonitz, R., Shiraishi, L., Robinson, M., et al.: The phoenix mars lander robotic Arm. In: 2009 IEEE Aerospace Conference. Big Sky, MT, United states (2009) 10. Richard, F.: Concurrent actuator development for the Mars exploration rover instrument deployment device. In: 10th European Space Mechanisms & Tribology Symposium, pp. 255–262. Sebastian, Spain (2003) 11. Liang, C., Sun, P., Wang, Y., et al.: Motion planning of sampling flexible manipulator on planet. J. Deep Space Explor. 1, 27–33 (2015) 12. GB/T 16439-1996: General specification for A.C. servo system

Thermal-Mechanical Coupling Analysis and Structural Optimization of a Deployable Grasping Manipulator Weida Cheng1,2,3 , Hailin Huang1,2,3 , Bing Li1,2,3(B) , Guanglu Jia1,2,3 , Fujun Peng1 , and Aiguo Wu1 1 School of Mechanical Engineering and Automation, Harbin Institute of Technology, Shenzhen

518055, People’s Republic of China [email protected] 2 State Key Laboratory of Robotics and System, Harbin Institute of Technology, Harbin 150001, People’s Republic of China 3 Shenzhen Key Laboratory of Mechanisms and Control in Aerospace, Harbin Institute of Technology, Shenzhen 518055, People’s Republic of China

Abstract. The temperature field of structures in space is severely uneven and changes dramatically, which leads to significant thermal-mechanical coupling deformation and vibration. In order to work normally in space, it is necessary to carry out thermal-mechanical coupling analysis and subsequent structural optimization. This paper presents a numerical method to realize the thermalmechanical coupling analysis and structural optimization of a deployable grasping manipulator (DGM), of which the hexagonal mechanism that composing the manipulator is well studied. The temperature field of the hexagonal mechanism in space is calculated by combining the thermal boundary condition analysis and the heat transfer finite element analysis (FEA). By setting the temperature field as boundary condition of the thermal-mechanical coupling FEA, the deformation and vibration is analysed. Based on the analysis, a multi-objective structural optimization model of the manipulator, of which the optimization objectives are mass, maximum deformation and first-order natural frequency, and the design variables are cross section dimensions of linkages, is established. The response surface model (RSM) based on the optimal Latin Hypercube design (OLHD) is then established to approximate the optimization model, and the Multi-objective Particle Swarm Optimization (MOPSO) algorithm is adopted to solve it. The optimized structure has nearly identical maximum thermal-mechanical deformation to the initial structure, but has significantly lighter mass and higher first-order frequency. The analysis and optimization method presented in this paper is of reference value to reduce the cost of expensive ground thermal vacuum tests. Keywords: Deployable grasping manipulator · Thermal-mechanical coupling · Structural optimization · Response surface method

© Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 451–462, 2021. https://doi.org/10.1007/978-3-030-89092-6_41

452

W. Cheng et al.

1 Introduction There is a large amount of “space junk” in low-Earth orbits at present, which may lead to further large-area orbital pollution if were ignored [1]. In order to solve this urgent problem, many capture methods were proposed for specific types of target [2–4]. However, the rigid capture of large-scale, non-cooperative targets is still hard to realize. Therefore, many large-scale DGMs have been designed for dealing this problem recently [5–7], and new demands of space adaptability analysis and optimal design on them arise correspondingly. In space, structures work in the vacuum and are subjected to strong periodic solar radiation, causing large thermal-mechanical coupling deformation or even vibration. The thermal-mechanical responses of space structures have been extensively studied in various aspect. Analytical models [8–10] are only suitable for very simple structure, while numerical models [11–13] have higher adaptability and precision. Aerospace composite analyses [14, 15] reveal the thermal-mechanical characteristics of complex material such as functionally graded material and honeycomb panels. The dynamic response analyses, especially the effect of space thermal-mechanical responses on the position and orientation of spacecrafts [16], are significant for motion control. However, previous thermal-mechanical coupling analysis mainly focused on structures with simple geometry, such as boom, expandable antenna and solar wing, while the thermal-mechanical responses of complex structures such as truss-style manipulators are rarely studied. This paper first carries out a thermal-mechanical coupling analysis on a DGM composing of polygonal mechanisms, which analyzes the temperature field, thermal-mechanical deformation and vibration characteristics of two kinds of polygonal mechanism. Then the DGM is structural optimized based on RSM approximation method to compose a complete process.

2 The Deployable Grasping Manipulator We designed a type of DGM by assembling the Myard mechanism according to some certain rules [17]. The three-finger DGM constructed by hexagonal mechanisms, which are umbrella-like assembled by Myard mechanism, is shown in Fig. 1. When the DGM is stored or in transportation, each hexagonal mechanism can be gathered into a bundle, occupying a small space. When it is in operation, each module can be deployed into a hexagon at most. The deploy/fold ratio of the three-finger DGM is 2.83. By umbrella-like assembling different numbers of Myard mechanism into different polygonal mechanisms, which are different polygons when deployed completely, and then assembling the polygonal mechanisms, various DGMs can be obtained. In expanding stage, the DoF of polygonal mechanisms is 1. When each unit is fully expanded, they are connected by revolute joints in series to form a finger. For working in space, multidisciplinary analysis considering on space environment and subsequent structural optimization should be carried out. The thermal-mechanical coupling analysis and subsequent structural optimization on the DGM are realized and will be shown in next two chapters.

Thermal-Mechanical Coupling Analysis and Structural Optimization

453

Fig. 1. Deploying process of the three-finger DGM: (a) folded state; (b)–(f) deploying process

3 Thermal-Mechanical Coupling Analysis of Polygonal Mechanism 3.1 Numerical Thermal-Mechanical Coupling Analysis Since this type of DGM is modularly formed by polygonal mechanisms, the characteristics of the module is closely related to the whole. We select polygonal mechanisms as the researching object to reflect the performance of the whole manipulator. We analyze the thermal-mechanical responses of hexagonal mechanism first. After ideally remodeling of the mechanism, we establish an accurate FEA mesh by Hypermesh for it, which is shown in Fig. 2(a). There are 11230 elements of the mesh in total, and the hexahedral elements account for 98.6% to ensure its accuracy. The joint of kinematic pairs of the FEA model are constructed by constraining all DoFs between two points that coupled with the hole faces of different linkages, thus the analysis can be convergent. The mesh is then transferred into TMG to establish a FEA model of the temperature field analysis. The parameters of this FEA model are shown in Table 1.

Fig. 2. The hexagonal mechanism: (a) FEM mesh (b) Coordinate system and observation points.

454

W. Cheng et al. Table 1. Simulation parameters of space temperature field analysis.

Material

Density

Elastic modulus

Poisson ratio

Specific heat

Emissivity

Ti alloy

4454 kg/m3

117270 Mpa

0.33

525 J/(kg °C)

0.7

Coeff. of thermal expansion 9.3599 × 10−6 / °C

Coeff. of thermal conductivity

Gravity

Absorptivity

7.625 mW/(mm °C)

0.23 m/s2

0.7

Figure 3 shows 16 reference points for thermal-mechanical coupling analysis in the geosynchronous orbit to study thermal-mechanical responses of the hexagonal mechanism. Two pairs of adjacent points (6–7 and 9–10) are set closely inside or outside the earth shadow to observe the process of sudden thermal flow changing. The orientation of the mechanism is that the norm of the palm always points to center of the earth, and the speed is along the positive direction of y-axis in Fig. 2(b).

Fig. 3. Reference points for analysis in the geosynchronous orbit.

The temperature field of 4 orbital reference points with typical irradiated states: point 3 (sun side irradiated), point 8 (in the shadow), point 10 (just exits the shadow) and point 16 (sun front irradiated) are shown in Fig. 4. It can be seen that the temperature field at different analysis points is significantly different. Because of the occlusion due to high structural symmetry, the distribution of temperature field of sun side irradiated point severely uneven. Setting the temperature fields as boundary conditions of the thermal-mechanical coupling FEA, the thermal-mechanical deformation of the hexagonal mechanism at every reference point can be calculated. The deformation fields at 4 typical orbital points are shown in Fig. 5. It can be seen that the maximum deformation occurs at the unconstrained end, and the deformation at point 8 has reached 2.287 mm due to the extremely low temperature. In order to choose a more suitable polygonal mechanism to modularly form the DGM, the same thermal-mechanical coupling analysis is performed on a pentagonal mechanism with the same diameter. P1 and P2 are also selected at symmetrically long linkage ends to evaluate the influence of thermal-mechanical deformation on the deployment of it.

Thermal-Mechanical Coupling Analysis and Structural Optimization

455

Fig. 4. Temperature at typical orbital points: (a) point 3; (b) point 8; (c) point 10; (d) point 16.

Fig. 5. Deformation at typical orbital points: (a) point 3; (b) point 8; (c) point 10; (d) point 16.

The comparison of the total displacement of P1 and P2 between two kinds of mechanism is shown in the Fig. 6. For these two mechanisms, the displacements of P1 and P2 are very close, but the displacement of the hexagonal mechanism is generally smaller. Additionally, the displacement difference between P1 and P2 of the hexagonal mechanism is smaller, which indicates the deformation of it is more coordinated. In general, the performance of the hexagonal mechanism in terms of deformation is more suitable for space missions.

456

W. Cheng et al.

Fig. 6. Comparison of the total displacement of P1 and P2 between two polygonal mechanisms.

3.2 Vibration Characteristics Analysis The drastically changing temperature field at side irradiated position and the shadow line of the earth may induce thermal vibration, which will affect the performance of the DGM severely. The first-order natural frequencies of hexagonal and pentagonal mechanisms at each orbital reference point can be calculated by the thermal modal analysis, which are shown in Fig. 7(a).

Fig. 7. Comparison of the vibration characteristics between two polygonal mechanisms: (a) 1st. natural frequency, (b) spectrum characteristics.

It can be seen that the thermal-mechanical coupling will affect the vibration characteristics of the structure, leading to changes of its first-order natural frequency. The first-order natural frequency of the hexagonal mechanism is ergodically higher. To further comparing the vibration amplitude of the two mechanisms, the vibration spectrum analyses are carried out, and the results are shown in Fig. 8(b). It can be seen

Thermal-Mechanical Coupling Analysis and Structural Optimization

457

that the vibration amplitude near the first-order natural frequency is much larger than that of other frequencies, and the vibration amplitude of the hexagonal mechanism is lower than the pentagonal mechanism by 11.67%. The first-order natural frequency of the hexagonal mechanism is higher while the vibration amplitude is lower, thus the performance of the hexagonal mechanism in terms of vibration characteristics is more suitable for space missions. In general, the hexagonal mechanism is more suitable for modularly forming the DGM, so it is adopted to constructing the three-finger DGM.

4 Structural Optimization of the Hexagonal Mechanism Based on Thermal-Mechanical Coupling Analysis 4.1 Multi-objective Structural Optimization Model In order to improve the adaptability of hexagonal mechanism in space environment, the mass and deformation of it should be smaller, while the first-order natural frequency should be higher. Therefore, the optimization objectives of the question are composed of mass (m), maximum thermal-mechanical deformation (umax ) and first-order natural frequency (f1 ) of the hexagonal mechanism. The section dimensions D1 , D2 , H1 and H2 of the hexagonal mechanism linkages are selected to be the design variables, which are shown in Fig. 8, to maintain the kinematic characteristic of the mechanism.

Fig. 8. Design variables of the multi-objective optimization problem.

The multi-objective structural optimization model of the hexagonal mechanism based on the thermal-mechanical coupling analysis is formulated as:  F(x1 , x2 , x3 , x4 ) = min(m, umax , −f1 ) (1) s.t xi ∈ [14 mm, 18 mm] i = 1, 2, 3, 4 Where the design variables xi (i = 1, 2, 3, 4) correspond to D1 , D2 , H1 and H2 , and the constraints ensure the hexagonal mechanism will not interfere. Because of severely uneven and drastically changed thermal boundary condition in space, there are complex implicit high-order nonlinear functions between optimization

458

W. Cheng et al.

objectives and design variables in the optimization model. Therefore, the response surface method (RSM) based on design of experiments (DOE) is used to establish an approximation model to simplify the complex optimization model. Figure 9 shows the overall process of the structural optimization of hexagonal mechanism based on thermal-mechanical coupling analyses. In this process, a quadratic RSM approximation model is established based on the OLHD first, and then the MOPSO algorithm is adopted to solve it. The sampling points of the OLHD are evenly distributed in the design space, so that the interaction of various design variables and the nonlinearity of the optimization model can be carefully considered.

Fig. 9. Process of the structural optimization based on thermal-mechanical coupling analyses.

4.2 RSM Model The fundamental of the RSM model is using appropriate DOE to obtain experimental results of sample points in the design space, and based on them to establish polynomial functions between objectives and design variables. Finally, the RSM model can be used to estimate the results of non-experimental sample points. The formula of the quadratic RSM model is: FRSM (X) = a0 +

n  i=1

ai xi +

n  i=1

aii xi2 +

n 

aij xi xj

(2)

i AG > AO The robots move to the area where their actions can be completed under the control of the higher priority action. Only when the higher priority action is not selected, the position of the robot will be controlled by the movement actions AO . 3.2 State Space Definition The truss structure is described by the truss matrix Hs . The number of rows and columns of the matrix represents the number of the node. The diagonal elements of the matrix represent the connecting nodes. The elements in the i-th row and the j-th column represent the robs connecting the i-th and the j-th nodes. In the truss matrix Hs , 0 means the element has not been assembled, and 1 means that the element has been assembled. Define the number of mobile dual-arm robots as NR . Define the set of actions currently performed by each robot as As .       As = aPα 1 , aGβ1 , aoγ 1 , aPα2 , aGβ2 , aoγ 2 , . . . . . . , aPαNR , aGβNR , aoγ NR Define the set of the current location of each robot as Os .       Os = x1 , y1 , x2 , y2 , . . . . . . , xN R , yN R Boolean variable set Fs indicating whether the mobile dual-arm robots carry the rods.   Fs = f1 , f2 , . . . . . . , fNR In summary, the state space of the collaborative assembly sequence optimization algorithm based on reinforcement learning is designed as: S = {H s , As , Os , Fs } The state space will be updated according to the current action sets of each robot at each time step. The rules for updating the state space within a time step are as follows: If the condition of rod grasping action is satisfied and the i-th robot grasps the rod: f i = true If the rod transfer action condition is satisfied and the i-th robot transfers a rob to the j-th robot: aGβ i = aG0 , f i = fasle, f j = true If the condition of rod installation action is satisfied and the i-th robot installs the rod which connecting the m-th and the n-th nodes with the help of the j-th and the k-th robot stabilizing nodes: H s [m, n] = H s [n, m] = 1

478

J. Yin et al.

aGβ i = aG0 , f i = fasle aPαj = aP0 , aPαk = aP0 If no action in AP or AG can be executed, robot moves according to the priority of the action. Each robot executes at most one action per time step according to the action priority.

4 Constraints Constraints of truss stability and constraints of action selection are two types of constraints during the assembly of truss structures. These constraints must be fully observed in the assembly sequence, otherwise the optimized assembly sequence obtained by the algorithm cannot be completed in practice and loses the significance of optimization. 4.1 Constraints of Truss Stability Analyze the physics of the truss structure during the assembly process, and only actions that satisfy the stability constraints are allowed to be executed. The rods placed on the ground can be installed directly. The rods perpendicular to the ground must have at least two rods at the bottom node to form the bottom support before they can be installed. The necessary condition for the installation of other types of rods is that the nodes connected to them have been installed with the previous rods. 4.2 Constraints of Action Selection The actions selected by the mobile dual-arm robots need to be necessary and executable in the current or near future assembly environment to ensure a complete and executable sequence of assembly actions. Only when the rod installing robot carries the required rod, and the truss structure after installation of the rod satisfies the stability constraints, the actions of stabilizing the nodes connected to the being installed rod will be optional actions. The selection of the action set AG needs to meet the following constraints: 1. The rods cannot be repeatedly taken or installed; 2. The rods to be installed must meet the stability constraints of the truss structure; 3. For rods which are on the same ground projection, only after the lower rods have been installed or planned to be installed, the upper rods can be selected as the action objects; 4. The action that makes the assembly process enter an endless loop cannot be selected.

Optimization Algorithm for Cooperative Assembly Sequence

479

The robot cannot move through the area occupied by the truss structure, and the area occupied by the truss structure cannot be selected as the target location of the movement. The specific path is planned by the A* algorithm. Define D(s) as the action-select function, which calculates the actions that can be selected according to the current state, and define these actions as aselect : aselect = D(s)

5 Optimization Algorithm On the basis of the previously defined state space and action space, the reinforcement learning algorithm is applied to optimize the multi-robot collaborative assembly sequence of the truss structure. The reinforcement learning algorithm is designed based on Markov properties. In the state space it selects actions from the action space to maximize the cumulative reward function to optimize assembly efficiency, energy consumption and other goals. 5.1 Reinforcement Learning Basics The reinforcement learning algorithm takes the current environment state as the only input of the action selection strategy because it is a Markov process. Markov process is the process in which the future state is only related to the current state and has nothing to do with its historical state. The Markov decision process can be represented by an array: S, A, P, R, γ  S is a finite set of states; A is a finite set of actions; P is the state transition probability matrix. R is the reward function: R(s, a) = E[Rt |St = s, At = a] The γ is a discount factor which value is between 0 and 1. Gt is the discounted weighted sum of all reward values calculated from time step t: ∞ γ k Rt+k+1 (1) Gt = Rt+1 + γ Rt+2 + . . . = k=0

The action value function Qπ (s, a) calculates the expected value of Gt when the action a is taken in the state s, and the action of the next process is determined by the strategy π . Qπ (s, a) = E[Gt |St = s, At = a]

480

J. Yin et al.

5.2 Optimization Design the reward function according to the optimization goal. The goal of the optimization of truss structure assembly sequence is to complete the assembly of the truss structure in the shortest time, with as less energy consumption as possible, and ensuring that the truss structure is as stable as possible during the assembly process. The number of time steps to complete the assembly of the truss structure in the algorithm represents the assembly time, which is defined as NT . Movement distance and the times of transmitting the rods, which are defined as NM and NAT , are important indicators of energy consumption. The stability of the truss structure during the assembly process can be expressed by two indicators: 1. The average number of connecting rods at each connection node which is defined as NAR . The larger the average number NAR , the more stable the structure; 2. The height of the center of gravity of the truss structure which is defined as NHG . The lower the center of gravity, the more stable the truss structure. Based on the above indicators, the reward function R can be expressed as: R = θ1 ∗ NT + θ2 ∗ NM + θ3 ∗ NAT + θ4 ∗ NAR + θ5 ∗ NHG + T θ1 , θ2 , θ3 , θ5 < 0, θ4 > 0 T is a scalar that adjusts the value of the reward function. The discount factor γ is set to 1, on the one hand to ensure that the algorithm is optimal as a whole, on the other hand to ensure that the selection order of actions in the same time step does not affect the reward function. Define π(s) as the action-select function which selects a from aselect according to Q(s, a) by γ -greedy strategy (γ ∈ [0, 1]): ⎧ ⎨ argmax Q(s, a) random < γ aselect a = π(s) = ⎩ random(a select ) random > γ Define e as the step size. The basic process of the optimization algorithm for truss assembly sequence based on reinforcement learning is as follows:

Optimization Algorithm for Cooperative Assembly Sequence

481

The value function Q(s, a) is initialized to zero. There are two stages in optimization algorithm, which are explore-stage and retrainstage.

Table 1. Results of the experiment Algorithm

Cumulative reward without T

Two-stage mechanism

−127.5585

Positive reward function

−154.1257

Negative reward function

−158.5346

Explore-Stage. In the explore-stage, set T = 0 to make the reward value negative. The Q value of (s, a) pairs that have been executed is negative, which means that they are harder to be selected by π(s) than these have not been. During the explore-stage assembly sequences are fully explored, and the best result is saved after certain episodes. Retrain-Stage. In the retrain-stage, set T to a large scalar to make the reward function positive and initialize the Q(s, a), so that the π(s) prefers the actions which have been executed. The best result saved by the explore-stage is loaded as the teaching sequence in the retrain-stage. Before the selected action, the assembly action is selected completely

482

J. Yin et al.

Fig. 3. The sequence of two mobile dual-arm robots assembling the triangular pyramid truss structure.

Optimization Algorithm for Cooperative Assembly Sequence

483

according to the teaching sequence, and after selected action, the assembly action is selected by π(s). If a better assembly sequence is found, it becomes a new teaching sequence. The purpose of retrain-stage is to locally optimize the results obtained from the preliminary exploration. Experiment shows that the optimization results obtained by using the abovementioned two-stage mechanism are more excellent than algorithms that simply use positive or negative reward values (Table 1).

6 Experiment Results This algorithm can optimize the assembly sequence of the specified number of robots to assemble many kinds of truss structure that meets the constraints. As an example, the sequence of two mobile dual-arm robots assembling the triangular pyramid truss structure is optimized, and the results are shown below (see Fig. 3). In the Fig. 3 the colored circles represent the mobile dual-arm robots. The vertical lines in the circle represent the rods which are carried. The black lines represent the trusses. The colored dots on the truss represent the actions of stabilizing the nodes. In the scenario of two mobile dual-arm robots assembling a truss structure with eight robs, the cumulative reward (without T) is shown in the figure below (see Fig. 4). It can be concluded that the sequence optimization algorithm based on reinforcement learning proposed in this paper has good optimization capabilities and do not converge prematurely.

Fig. 4. Episodes before the 400-th episode are in the explore-stage in which assembly sequences are fully explored. Episodes after the 400-th episode are in the retrain-stage in which assembly sequences are optimized according to the teaching sequence. In the retrain-stage the cumulative reward changes almost periodically as the length of the teaching sequence changes.

7 Conclusion Reinforcement learning, as an intelligent optimization algorithm that maps from state space to action space, can be applied to the assembly sequence optimization problem of

484

J. Yin et al.

multi-robot collaborative assembly truss structure based on the rationally designed state space and action space. The sequence optimization algorithm based on reinforcement learning proposed in this paper can optimize the assembly action sequence of multi-robot cooperative assembly truss structure and avoid premature convergence. This algorithm can also be generalized and applied to other multi-robot cooperation problems.

References 1. Diftler, M., Jenks, K.C., Williams, L.E.P.: Robonaut: a telepresence-based astronaut assistant//Telemanipulator and telepresence technologies VIII. Int. Soc. Opt. Photon. 4570, 142–152 (2002) 2. Diftler, M.A., Ahlstrom, T.D., Ambrose, R.O., et al.: Robonaut 2 – initial activities on-board the ISS. In: 2012 IEEE Aerospace Conference, pp. 1–12. IEEE (2012) 3. Rehnmark, F., Bluethmann, W., Rochlis, J. et al.: An effective division of labor between human and robotic agents performing a cooperative assembly task. In: Proceedings of 2003 IEEE International Conference on Humanoid Robots. Institute of Electrical and Electronics Engineers, New York, NY (2003). 4. Rehnmark, F., Currie, N., Ambrose, R.O., et al.: Human-centric teaming in a multi-agent EVA assembly task. SAE Trans. 1105–1113 (2004) 5. Guo, J., Wang, P., Cui, N.: Ant colony algorithm for assembly sequence planning of large space truss structures. In: 2007 IEEE International Conference on Control and Automation, pp. 2027–2030. IEEE (2007) 6. McEvoy, M., Komendera, E., Correll N.: Assembly path planning for stable robotic construction. In: 2014 IEEE International Conference on Technologies for Practical Robot Applications (TePRA), pp. 1–6. IEEE (2014) 7. Gunji, A.B., Deepak, B., Bahubalendruni, C.R., et al.: An optimal robotic assembly sequence planning by assembly subsets detection method using teaching learning-based optimization algorithm. IEEE Trans. Autom. Sci. Eng. 15(3), 1369–1385 (2018) 8. Zhu, X., Wang, C., Chen, M., et al.: Concept plan and simulation of on-orbit assembly process based on human–robot collaboration for erectable truss structure. In: International Conference on Man-Machine-Environment System Engineering, pp. 683–691. Springer, Singapore (2020). 9. Wang, X., Li, S., Wang, C., Chen, M., Wang, J.: Dual-arm robot based compliant assembly of space truss struts and spherical joints. Manned Spaceflight 26(6), 741–750 (2020). (in Chinese) 10. Zhao, C., Guo, W., Lin, R., Li, M.: An innovation design method for connector system. Mach. Des. Res. 35(5):28–31+40 (2019) (in Chinese)

A Coordination Planning Method of the Hybrid Two-Arm Space Robot for On-Orbit Servicing Zhonghua Hu1,2 , Wenfu Xu1,2 , Taiwei Yang1,2 , Han Yuan1,2 , and Huaiwu Zou3(B) 1 Harbin Institute of Technology, Shenzhen 518055, China 2 State Key Laboratory of Robotics and System,

Harbin Institute of Technology, Harbin 150001, China 3 Shanghai Institute of Aerospace System Engineering, Shanghai 201108, China

Abstract. A geosynchronous-orbit (GEO) satellite that is out of order due to component failure, will bring critical challenges for space robots to capture and repair it. To overcome the above challenges, a space robot system equipped with a traditional redundant manipulator (Arm-a) and a segmented hyper-redundant manipulator (Arm-b) is introduced in the paper. Firstly, the kinematics model considering the conservation of momentum is established. The coordination trajectory planning method is then proposed to make space robot detect the target point in the confined space. Based on the position, attitude, and configuration deviations, the linear and angular velocities of the two arms are planned at the same time. In order to realize the configuration planning of the system, a modified generalized Jacobian (MGJ) is derived. The joint angular velocities of two arms are resolved by the MGJ. Furthermore, joint angles of Arm-a and Arm-b are obtained according to numerical integration. Finally a co-simulation system is designed and the numerical simulation is carried out. The simulation results demonstrate the proposed method. Keywords: Space robot · Hybrid (hyper)redundant two-arms · Coordination trajectory planning

1 Introduction As the number of uncontrolled satellites has increased, it brings a great threat to operational spacecraft and the long-term sustainability of activities in outer space [1–3]. It is extremely urgent to capture and repair them because it is beneficial to prolong lifetime of satellites and protect the orbital environment [4, 5]. As a result, the space robotic system that is one of the most effective ways to deal with such uncontrolled space targets has attracted more and more attention from many scholars [6–9]. At present, many literatures focus on the space robot system for maintaining failure satellites. As we known, the maintaining mission can be divided into two phases. The first is that the space robot keeps tracing the target satellite and then captures it. The other phase is to repair failure satellites. In the phase, the dual-arm space robot is usually used to complete the maintenance task coordinately. © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 485–497, 2021. https://doi.org/10.1007/978-3-030-89092-6_44

486

Z. Hu et al.

Based on the generalized Jacobian matrix, Yoshida proposed a coordinated motion control strategy for the dual-arm [10]. In the method, one arm traced a given path while the other arm was adopted to operate the target satellite. Considering that dualarm space robot system were not conserved when external forces or moments applied on the base or/and manipulators’ end-effectors, Basmadji proposed a novel method for motion planning of dual-arm space robot manipulators [11]. In order to realize coordinated control of the base’s attitude and dual-arm’s motion, L. Shi introduced two types of controllers which were a Sliding Mode Controller (SMC) and a nonlinear Model Predictive Controller (MPC) [12]. Based on signal compensation and back-stepping, Y. Liu designed a robust decentralized control strategy for a dual-arm 6-DOF space robot tracking the desired trajectory with self-collision avoidance [13]. M. Wang proposed a strategy that was based on particle swarm optimization (PSO) for coordinated trajectory planning of the dual-arm space robot in free-floating mode [14]. In the method, the direct kinematics equations connected with constrained PSO were employed to overcome the dynamics singularities. In order to deal with the issue that was the target had relative motion with respect to the dual-arm space, Y. Wu proposed a model-free reinforcement learning strategy to train a policy for real-time trajectory planning without establishing the dynamic and kinematic models of the dual-arm space robot [15]. Suril V. Shah presented three path planning strategies for point-to-point reactionless manipulation of a dual-arm robotic system to capture tumbling targets [16]. As discussed above, the existing studies only focus on space robot which has two or more arms with the same mechanical structure. In addition to this, for these arms, all drive motors are mounted on the joints such that the boundary dimensions of the arms are larger, resulting in that the above space robotics fails to detect a narrow space for repairing malfunction satellites. To handle this issue, a space robot which is equipped with a redundant 7 DOFs arm with collocated actuator placement and a hyper-redundant 10 DOFs arm with non-collocated actuator placement is designed. A coordination trajectory planning method is proposed for the space robot to detect a narrow gap and to repair the malfunction satellite. The remainder of this paper is organized as follows. Section 2 gives derivative process of kinematic equation of the space robot. Section 3 proposes the coordination trajectory planning method for the space robot to complete the detection mission in confined space, such as a narrow gap plane. In Sect. 4, the proposed method is verified by the simulation. The last section summarizes this paper and gives the conclusions.

2 Kinematics Modeling of Space Robot System As shown in Fig. 1, the space robot system is composed of a base and two arms that are installed on the base. One of the arms is a traditional manipulator whose driving motors are equipped in joints that are all active [17]. Different from the former arm named Arm-a, the other arm is a bionic manipulator denoted by Arm-b, which is actuated by pulling and releasing the drive cables through the whole arm and its joints are hybrid active and passive [18]. The degrees of freedom (DOFs) of the two arms are na and nb . For the Arm-a, the adjacent links are connected by a joint with a single rotation axis. Differently, Arm-b is composed of N segments with same mechanical structure.

A Coordination Planning Method of the Hybrid Two-Arm Space Robot

487

Here, each segment has n (n is even number) associated modular joints. For ith joint, if i is odd number, its DOFs are described as Pitch-Yaw (PY). Otherwise, it is Yaw-Pitch (YP). A modular joint and a rigid link attached to the joint form a subsegment. In fact, there are only 2N DOFs (i.e. nb = 2N) because the joints of each segment are hybrid active and passive. For the Arm-a, the end-effector are given, respectively, as follows: pae = r0 + ba0 +

nk    aaj + baj ∈ R3×1

(1)

j=1

Fig. 1. The simplified model of the space robot system

Similarly, for the Arm-b, t the end-effector are also calculated as follows. pbe = r0 + bb0 +

n  N    abh,m + bbh,m ∈ R3×1

(2)

h=1 m=1

Differentiating Eq. (1) and Eq. (2), the velocities of end-effectors for Arm-a and Arm-b can be obtained as follows. ⎧   na a

 a ⎪ kaj × pae − paj θ˙ja ∈ R3×1 ⎪ ⎨ ve = v0 + ω0 × pe − r0 + j=1

   n  N 

 ⎪ ⎪ b b ⎩ vbe = v0 + ω0 × pbe − r0 + + kbh,m,2 θ˙h,m,2 kbh,m,1 θ˙h,m,1 × pbe − pbh,m ∈ R3×1 h=1 m=1

(3)

488

Z. Hu et al.

According to relative motion, the angular velocities of the end-effectors for Arm-a and Arm-b can be derived as follows. ⎧ na  ⎪ kaj θ˙ja ∈ R3×1 ⎪ ωae = ω0 + ⎨ j=1 (4)  N  n   ⎪ b b ⎪ b b 3×1 ˙ ˙ ⎩ ωbe = ω0 + kh,m,1 θh,m,1 + kh,m,2 θh,m,2 ∈ R h=1 m=1

Thus, Eq. (3) and Eq. (4) can be written as follows.  k   ve k k v0 ˙ k ∈ R6×1 , x˙ e = = Jb + J km Θ ω0 ωke

k = a, b

(5)

where, H ∈ R(2N ×n)×nb is the coefficient matrix and is obtained by  T H = H T1 H T2 · · · H TN −1 H TN

(6)

Since the space robot system is in the micro gravity environment, there are no external forces and torques acted on the free-floating system. Assuming the linear and angular momentum of the system is equal to zero in the initial state, the system momentum keeps zero according to the conservation law, i.e.,      T    a  v0 J bTw ˙ b J Tw ˙ a P0 M E M r˜0g Θ + Θ = 06×1 ∈ R6×1 (7) + = I aφ I bφ L0 ω0 M r˜g Iw where, ⎧ na

⎪ a =  ma J a ∈ R3×na ⎪ J ⎪ Tw i Ti ⎨ i=1   N  n  2  ⎪ ⎪ mbi,j J bT (i,j,h) · H ∈ R3×nb ⎪ J bTw = ⎩

(8)

i=1 j=1 h=1

Based on the upper three lines of Eq. (7), the liner velocity of the base can be solved by the following equation.     a b J Tw J a a b Tw ˙b ˙ + r˜0g I −1 + r˜0g I −1 v0 = − (9) s I Θ − s I Θ M M In the same way, according to the lower three lines of Eq. (7), the angular velocity of the base can be obtained as follows.   ˙ a + I b Θ ˙b I a Θ (10) ω0 = −I −1 s Combining Eq. (9) and Eq. (10), the following equation is yielded.    a  a ˙ ˙   v0 Θ Θ a b ˙ + Jb Θ ˙ = Ja Jb = J abm Θ = J bm b bm bm bm ˙ ˙b ω0 Θ Θ

(11)

A Coordination Planning Method of the Hybrid Two-Arm Space Robot

489

Substituting Eq. (11) into Eq. (5), the liner and angular velocities of the end-effectors for Arm-a and Arm-b can be obtained as follows.  ˙ a   a  J aa J ab Θ x˙ e ˙ = (12) = Jg Θ ˙b J ba J bb x˙ be Θ where, J aa = J ab J abm + J am , J ab = J ab J bbm , J ba = J bb J abm and J bb = J bb J bbm + J bm . J g means the generalized Jacobian matrix.

3 Coordination Trajectory Planning Method for the System 3.1 Target Spacecraft Operation Scenario Here, the operation scenario, as shown in Fig. 2, describes the maintenance task for a failure satellite by using the space robot system mentioned in Sect. 2. And a practical GEO communication satellite (Chinese Sinosat-2) is taken as the example for the faulted satellite. The above GEO spacecraft is out of order because two solar wings and two large antennas, respectively mounted on the +Y/−Y panels and +X/−X panels, fail to deploy completely. Target Spacecraft

The docking ring Arm-a

kt

pea pt

peb

keb

nt The enlarged drawing nN D

Arm-b The narrow gap The plane of the The base

Nth segment

Fig. 2. The operation scenario for repairing the malfunction spacecraft

In the paper, we suppose that the target satellite has been captured and stabilized by the method in reference [19], before the space robot system starts to repair the target satellite. However, the position of the target is not within the workspace of the Arm-b at the present. Hence, in order to maintain the target satellite on the orbit, the trajectories of Arm-a and Arm-b should be planned simultaneously. 3.2 The Algorithm Flowchart for Target Capture The algorithm flowchart is shown in Fig. 3. Firstly, the kinematics model of the original space robot is established. According to kinematic equivalence of the Arm-b [18], the

490

Z. Hu et al.

Fig. 3. The flowchart for maintenance mission

modified kinematics model of the space robot is then built for the coordination planning of Arm-a and Arm-b and the modified Jacobian matrix is obtained. It should be pointed out that the Arm-b is master arm that is used to repair the target satellite and the Arm-a is the slave arm which will move the target satellite to cooperate with Arm-b. So, the relative position deviation P, orientation deviation O and configuration deviation C between the maintenance point and the Arm-b are only calculated to plan the end-effector motion of Arm-a and Arm-b. Here, P, Oe and C are obtained by measurement data of hand-eye camera. When the magnitudes of max{P}, max{O} and max{C} are all less than or equal to the threshold values (i.e., εp , εθ and εϕ ), it means that the Arm-b reaches the maintenance point and the task is completed successfully. Otherwise, the desired end-effector’s pose and configuration of Arm-a Arm-b are planned according to the pose and configuration deviations. The joint angular velocities of Arm-a and Arm-b are resolved by the modified generalized Jacobian matrix derived from the modified kinematics model of the space robot. The joint angles of Arm-a and Arm-b are obtained by the numerical integration. The two arms are actuated coordinately to make the Arm-b approach the maintenance point until max{P}, max{O} and max{C} are all less than or equal to the threshold values. 3.3 The Modified Generalized Jacobian On the basis of kinematic equivalence of Arm-b, the linear and angular velocities of the Arm-b’s endpoint are obtained as follows.  b   b v˜ e b v0 b ˙x˜ b = (13) = J˜ b + J˜ m Θ˙˜ ∈ R6×1 e b ω0 ω˜ e

A Coordination Planning Method of the Hybrid Two-Arm Space Robot

491

˜ b is the joint angles of the kinematic equivalence arm of Arm-b. And the where, Θ

b Θ˙˜ can be obtained by

where, U =



b ˙b Θ˙˜ = U · Θ

u1 u2 · · · u2N . ui is derived by  ˜b ˜ b (i) ˜b ∂Θ ui = ∂ Θb (i) ∂ Θb (i) · · · b ∂Θ (1) ∂Θ (2)

(14)

b

˜ (i) ∂Θ ∂Θ (2N −1) ∂Θ b (2N )



b Substituting Eq. (11) and (15) into (13), x˙˜ e can be calculated by   b b ˜ a + J b · J b + J˜ bm · U Θ˙˜ ∈ R6×1 x˙˜ e = J ba Θ b bm

(15)

(16)

Combining the above three lines of Eq. (12) and (16), the modified generalized Jacobian J˜ g is obtained as follows.     ˙ a  x˙ ae J aa J ab Θ ˙ (17) = J˜ g Θ b = ˜ ˙b J ba J˜ bb Θ x˙˜ e b where, J˜ ba = J ba , J˜ bb = J bb · J bbm + J˜ m · U

3.4 The Desired Pose and Configuration Planning The desired linear velocity and angular velocity of the end effector of Arm-a and Arm-b are planned as followed. ⎧ ⎪ va = K ap · (−P) ⎪ ⎪ ed ⎨ v˜ bed = K bp · (P) (18) ⎪ ωaed = K ao · (−O) + K ac · (−C) ⎪ ⎪ ⎩ b ω˜ ed = K bo · (O) + K bc · (C)    

  where, P = P t − P be , O = kbe × kt /kbe × kt  · θ and C =  b

 b n × nt /n × nt  · ϕ. P t and P b are position vectors of target point and end-effector i

i

e

of Arm-b. Their direction vectors are kt and kbe . nt and nbi are the normal vectors of narrow gap plane and ith segment of Arm-b. θ is the angle between kt and kbe . ϕ is the angle between nt and nbi . 3.5 Resolved Joint Motion for Arm-A and Arm-B Combining (17) and (18), desired linear velocity and angular velocity are obtained as follows. ⎤ ⎡ a ⎤ ⎡ ⎡ ⎤ −K ap 0 0 ved   a −K a ⎥ P ⎢ ωa ⎥ ⎢ x˙ ae 0 −K ⎥ ⎢ o c ⎣ ed ⎥ ˙ (19) ⎥ O ⎦ = J˜ g Θ b =⎢ ⎣ v˜ b ⎦ = ⎢ 0 ⎦ ⎣ K bp 0 x˙˜ e ed C ω˜ bed 0 K bo K bc

492

Z. Hu et al.

Therefore, desired joint rates of Arm-a and Arm-b can be resolved as follows. ⎡ ⎤ ⎡ ⎤ −K ap 0 0  a ⎢ ⎥ P ˙d Θ † ⎢ 0 −K ao −K ac ⎥ ˙d = (20) Θ = J˜ g ⎢ b ⎥⎣ O ⎦ ˙ bd 0 ⎦ ⎣ Kp 0 Θ C 0 K bo K bc † where, J˜ g is the pseudo-inverse of the matrix of J˜ g . Then, the desired joint angles can be calculated by the numerical integration, such as   a   ˙a   a Θ d {t} Θ d {t} Θ d {t + t} = + t (21) Θd = ˙ bd {t} Θ bd {t + t} Θ bd {t} Θ

4 Numerical Simulation 4.1 Simulation Modeling of the Space Robot To validate the proposed method, a co-simulation system is designed by Adams and MATLAB/Simulink. In the simulation, the space robot is composed with a base and two manipulators, as shown in Fig. 4.

Target satellite Arm-a The base

Detection point Arm-b

The Malfunction solar panels

Fig. 4. The simulation model of the space robot

One of arms is traditional redundancy manipulator with 7DOFs and its D-H reference frame and D-H reference parameters are shown in Fig. 5 and Table 1 respectively. The other is a hybrid active and passive segmented hyper-redundant manipulator with 10 DOFs. Its D-H reference frame and D-H reference parameters are similar with that of reference [18]. In the simulation, the motion of the Arm-a and Arm-b is planned according to Eq. (19).

A Coordination Planning Method of the Hybrid Two-Arm Space Robot q2

y0 z0 x0

493

z1(y2) x2

q1 x1 d1

q4

y1(z2)

z3(y4) q3

y3

d3

q6

x3(x4) z5(y6)

z4 q5 d5

y5

x6(x5)

y7 x7

z6

z7

q7 d7

Fig. 5. D-H reference frame of the traditional redundancy manipulator

Table 1. D-H parameters of the traditional redundancy manipulator Link i

θ i (°)

α i (°)

1

−90

2

180

3

0

4

0

90

0

0

5

0

−90

0

d 5 = 1900

6

0

90

0

0

7

0

0

0

d 7 = 255

ai (mm)

d i (mm)

90

0

d 1 = 225

90

0

0

−90

0

d 3 = 1900

4.2 The Initial State of the Simulation Model As shown in Fig. 4, the target satellite has been captured before the hyper-redundant manipulator is prepared to repair the target satellite. Under the current state, the initial position and attitude (Z-X-Z Euler angles) of the base with respect to the inertial frame are as follows: T  T  (22) rb0 = −4.71, −3.29, 70.11 mm, ψ b0 = 28.08◦ , 1.70◦ , −25.97◦ The position and attitude of installation point of Arm-a and Arm-b can be also obtained in the simulation model. T  T  pa1 = 1256.80, 659.30, −1080.67 mm, ψ a1 = −151.92◦ , 178.30◦ , 25.97◦ (23) T  T  pb1,1 = 1741.93, 576.86, 1361.47 mm, ψ b1,1 = 28.08◦ , 1.70◦ , −25.97◦ (24) The initial joint angles of Arm-a and Arm-b are as follows: 

T  Θ a0 = 100.20◦ , 73.88◦ , −7.99◦ , 65.73◦ , −17.31◦ , −51.88◦ , −9.69◦ T  Θ b0 = −9.0◦ , 3.0◦ , −5.0◦ , −2.0◦ , 10.0◦ , −4.0◦ , 6.0◦ , −2.0◦ , 8.0◦ , −2.0◦ (25)

494

Z. Hu et al.

The initial position and attitude of the target point with respect to the inertial frame are as follows. T  T  rt = 5261.32, 131.65, 1129.56 mm, ψ t = −18.09◦ , 21.46◦ , 64.53◦ (26) The normal vector of the narrow gap (solar panel plane) is as follows.  T nt = −0.1136, −0.3478, 0.9307

(27)

4.3 Simulation Results The narrow gap detection simulation for repairing the target satellite is simulated by using the co-simulation system. In the simulation, the threshold values of the position and attitude are set as: εp = 10 mm, εθ = 1° and εϕ = 1°. According to the simulation results, the curves of relative position and attitude deviations between the end-effector of Arm-b and the detection point are respectively shown in Fig. 6 and Fig. 7. The configuration deviation between the plane where the 5th segment of the Arm-b is and the narrow gap plane is shown in Fig. 8.

Fig. 6. The position deviation curve

Fig. 7. The attitude deviation curve

A Coordination Planning Method of the Hybrid Two-Arm Space Robot

495

Fig. 8. The configuration deviation curve t = 0.0s

t = 10.0s

t = 5.0s

t = 14.3s

Fig. 9. The 3D states of the model in the simulation

Until t = 14.3 s, the position and attitude deviations from the end-effector to the detection point are [0.0097 m, −0.0002 m, 0.001 m] and 0.08 °, which are both smaller than the corresponding threshold values. At the same time, the configuration deviation between the plane which the 5th segment is in and the narrow gap plane is 0.015 ° which is also smaller than the corresponding threshold value. According to the above results, it can be concluded that the end-effector of the Arm-b enters the narrow gap plane and the target position can be detected successfully. Some states of the 3D model at different time during the simulation are shown in Fig. 9.

5 Conclusion For the past few years, capturing and repairing tumbling target plays an important role in the on-orbital servicing. Admittedly, the existing dual-arm space robot has the advantage to capture and stabilize the tumbling targets. However, due to lower dexterity and larger boundary dimension, dual-arm space robot could not perform the maintenance task in a confined space after targets have been captured. So a space robot with a traditional redundant manipulator (7 DOFs) and a segments hyper-redundant manipulator (10 DOFs) is introduced in the paper. Because of larger stiffness and higher load, the redundant manipulator is used to capture the tumbling target. And the hyper-redundant

496

Z. Hu et al.

manipulator is utilized to cross the narrow gap, then to repair the target satellite, owing to higher dexterity and smaller boundary dimension. The kinematics model of the space robot is established firstly. A coordination trajectory planning method is then proposed for the space robot detecting the target point that is in a narrow gap. Furthermore, a co-simulation system based on ADAMS and MATLAB/ Simulink software is designed. The simulation results indicate that the end-effector of the hyper-redundant manipulator can reach the target point while the collision between the hyper-redundant manipulator and the boundaries of the narrow gap (solar panel or body of the target satellite) is avoided by the proposed method. It should be pointed out that the target satellite has been captured when the hyperredundant manipulator is ready to perform the above operation task. The detumbling process is not considered in the paper. In the future, we will strive to do further research on the planning and control method for contact process. Acknowledgements. This work was supported by the Key-area Research and Development Program of Guangdong Province (Grant Number: 2019B090915001). Besides, we would like to thank to Science and Technology Innovation Committee of Shenzhen for their financial supports on this work (The Basic Research Program of Shenzhen, Grant Number: JCYJ20180507183610564, JCYJ20190806142818365, JCYJ20180507183644237).

References 1. Moghaddam, B.M., Chhabra, R.: On the guidance, navigation and control of in-orbit space robotic missions: a survey and prospective vision. Acta Astronaut. 184, 70–100 (2021) 2. Liu, J., Du, B., Huang, Q.: Space robotic de-tumbling of large target with eddy current brake in hand. In: Huang, Y., Wu, H., Liu, H., Yin, Z. (eds.) ICIRA 2017. LNCS (LNAI), vol. 10464, pp. 637–649. Springer, Cham (2017). https://doi.org/10.1007/978-3-319-65298-6_57 3. Wenfu, X., Jianqing, P., Bin, L., et al.: Hybrid modeling and analysis method for dynamic coupling of space robots. IEEE Trans. Aerosp. Electron. Syst. 52(1), 85–98 (2016) 4. Wan, W., Sun, C., Yuan, J., et al.: Adaptive whole-arm grasping approach of tumbling space debris by two coordinated hyper-redundant manipulators, pp. 450–461 (2019) 5. Liu, Y., Zhao, Y., Tan, C., et al.: Economic value analysis of on-orbit servicing for geosynchronous communication satellites. Acta Astronaut. 180, 176–188 (2021) 6. Flores-Abad, A., Zhang, L., Wei, Z., et al.: Erratum to: Optimal capture of a tumbling in orbit using a space manipulator. J. Intell. Robot. Syst. 86(2), 199–211 (2017) 7. Huang, P., Zhang, F., Meng, Z., et al.: Adaptive control for space debris removal with uncertain kinematics, dynamics and states. Acta Astronaut. 128(11–12), 416–430 (2016) 8. Yan, L., Yuan, H., Xu, W., et al.: Generalized relative Jacobian matrix of space robot for dual-arm coordinated capture. J. Guid. Control. Dyn. 41(5), 1202–1208 (2018) 9. Wenfu, X., Liang, B., Cheng, L., et al.: Measurement and planning approach of space robot for capturing non-cooperative target. Robot 32(1), 61–69 (2010) 10. Kazuya, Y., Ryo, K., Yoji, U.: Dual arm coordination in space free-flying robot. In: IEEE International Conference on Robotics and Automation, pp. 2516–2521 (1991) 11. Basmadji, F.L., Seweryn, K., Sasiadek, J.Z.: Space robot motion planning in the presence of nonconserved linear and angular momenta. Multibody Syst. Dyn. 50(1), 71–96 (2020) 12. Shi, L., Kayastha, S., Katupitiya, J.: Robust coordinated control of a dual-arm space robot. Acta Astronaut. 138, 475–489 (2017)

A Coordination Planning Method of the Hybrid Two-Arm Space Robot

497

13. Liu, Y., Yu, C., Sheng, J., et al.: Self-collision avoidance trajectory planning and robust control of a dual-arm space robot. Int. J. Control Autom. Syst. 16(6), 2896–2905 (2018) 14. Wang, M., Luo, J., Yuan, J., et al.: Coordinated trajectory planning of dual-arm space robot using constrained particle swarm optimization. Acta Astronaut. 146, 259–272 (2018) 15. Wu, Y., Yu, Z., Li, C., et al.: Reinforcement learning in dual-arm trajectory planning for a free-floating space robot. Aerosp. Sci. Technol. 98, 105657 (2020) 16. Shah, S.V., Sharf, I., Misra, A.: Reactionless path planning strategies for capture of tumbling in space using a dual-arm robotic system. American Institute of Aeronautics and Astronautics (2013) 17. Zhonghua, H., Wenfu, X., Lei, Y., et al.: Dynamic closest point identification and estimation for tumbling target capturing. In: 2018 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), pp. 1130–1135 (2018) 18. Zhonghua, H., Taiwei, Y., Wenfu, X., et al.: A kinematic equivalence trajectory planning method of hybrid active and passive cable-driven segmented hyper-redundant manipulator. In: 2019 IEEE International Conference on Robotics and Biomimetics (ROBIO), pp. 1280–1285 (2019) 19. Wenfu, X., Zhonghua, H., Lei, Y., et al.: Modeling and planning of a space robot for capturing tumbling target by approaching the dynamic closest point. Multibody Syst. Dyn. 47(3), 203– 241 (2019)

Design and Kinetostatic Analysis of a Legged Robot for Asteroid Exploration Qingpeng Wen, Jun He(B) , Zhicong Wang, Jiaze Sun, and Feng Gao State Key Laboratory of Mechanical Systems and Vibration, School of Mechanical Engineering, Shanghai Jiao Tong University, Shanghai 201100, China [email protected]

Abstract. Legged robots have a broad prospect in the field of asteroid exploration. It is a great challenge to make the legged robot move steadily on the asteroid. In this paper, a rigid-flexible coupling gripper based on the legged robot is proposed to anchor the robot on the surface of asteroids. First, the structure of the gripper is described and its mechanical model is established. Second, the basic gait of the asteroid robot is introduced, and the stability index of the asteroid robot is proposed. Third, a trajectory planning method based on the contact angle is proposed. Bessel curves with different local characteristics are constructed. The co-simulation is carried out, and the effect of local geometric features of the motion trajectory on the stability of the gripper is discussed. It is found that when the contact angle is a right angle, the stability of the gripper is improved. Accordingly, both the kinetostatic model and the trajectory planning are verified. Keywords: Asteroid robot · Gripper · Gait planning · Stability

1 Introduction With the further exploration of the solar system, low gravity, non-atmospheric asteroids have gradually attracted the interest of researchers all over the world. Asteroids contain materials at the beginning of the formation of the solar system, and may also contain resources for the sustainable development of mankind [1, 2]. Europe, Japan and the United States have carried out researches on asteroids, and launched landers to explore asteroids or sample the material on the surface of asteroids [3, 4]. The wheeled robot for scientific missions on the lunar or Mars have the characteristics of high speed and stability [5]. However, they cannot be applied to the exploration of asteroids for the robot will leave from the asteroid surface under the contact force without gravity [6]. So three kinds of robots are proposed to replace wheeled robots to explore asteroids. The first one is an immovable lander equipped with scientific equipment [7]. The advantage of this lander is that its rigid frame has higher stiffness and stronger anti disturbance ability. But it can only detect at the landing point and cannot return to the orbiter. Another kind of robot is the hopping robot [8, 9]. The advantage of the hopping system is that it can detect the area around the landing point, but it takes a long time for hopping and the location of the landing point is difficult to predict. The third one is legged robot proposed © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 498–509, 2021. https://doi.org/10.1007/978-3-030-89092-6_45

Design and Kinetostatic Analysis of a Legged Robot for Asteroid

499

by JPL which can be anchored on the surface of the asteroid and samples the interior of the asteroid by a drill mounted on the foot [10]. Compared with the other two kinds of robot, the legged robot has better mobility and it can move on complex terrain. Microspines are firstly applied to the wall climbing robot proposed by Kim [11]. The microspine is consist of a rigid frame, flexible parts and steel hooks. The rigid frame and flexible parts of the microspine are manufactured using the SDM technology. Liu makes the microspine flexible by changing its shape [12]. Microspines are used as the foot of bionic inchworm robot. The experimental results show that the robot can walk upside down on the rough ceiling. In a word, SDM technology has higher requirements for processing technology, and the price is more expensive. Liu did not analyze the relationship between the stiffness and geometric parameters. Therefore, it is necessary to establish the stiffness model of the microspine. It is not only conducive to the accurate calculation of the adhesion, but also make the stiffness to meet the requirements by changing parameters. Dharbaneshwer studied the relationship between the deformation of the gripper and the pressure. Pressure maps of various household items are constructed, and the stability of each grab is determined based on the contact pressure map [13]. Yamaguchi presented an integrated Gripper(gripper), and it can hold large objects using suctions mounted on fingertips. The gripper can not only open the envelope, but also pick up the cube with a weight of 3.9 N [14]. Wang proposed a new attachment method, using claw grippers to attach to rough walls. The force of the gripper on the rough wall is analyzed to establish the mechanical model, and the condition of stable grasping is deduced [15]. In conclusion, the flexible bionic gripper has been deeply studied, while the research of the micropine gripper is relatively less. For the microspine gripper, the maximum shear force is not enough to evaluate the stability of the gripper. When the stability index is proposed, the adhesion mechanism of the gripper should be considered. Therefore, this paper proposes a legged asteroid robot with a gripper at the end of each leg. The stiffness model of the microspine is established, and the relationship between the driving force of the servo motor and the adhesion is derived. The stability index of the gripper is proposed and the motion of the asteroid robot is planned. The paper is organized as following: In Sect. 2 a single degree of freedom gripper mounted on the end of the leg is proposed, which help the robot to fix on the asteroid surface by ten attachment units. Section 3 establishes the stiffness model of the microspine and the mechanical model of the gripper. Then, the kinematics model of robot is established. The foot trajectory of the asteroid robot is planned in Sect. 4 and the stability index of the gripper is proposed. The optimal foot trajectory and its stability margin is obtained by simulating a gait cycle of the asteroid robot. Finally, Sect. 5 is the conclusion.

2 Design of the Robot It is necessary to install tools at the end of legs to grasp the surface of the asteroid. The first method is to install spikes at the end of each leg. Multiple spikes grasp the surface roughness of the asteroid simultaneously to form a gripper. However, it’s hard for each spike to point to the center of the fuselage. In order to anchor the asteroid robot on the

500

Q. Wen et al.

asteroid surface stably, an axisymmetric gripper is proposed. As shown in Fig. 1, the rigid shell is connected with the end of the leg of the asteroid robot through a ball hinge. Attachment units, components providing adhesion, are mounted on a rigid shell. They can rotate around the axis of the passive joint. One end of the cable (not shown) is fixed on the edge of the drive disc, and the other end is fixed on the attachment unit. The attachment unit consists of five parts: shell, limit block, spring, microspines and base. The microspine array is fixed on the base, and the base moves in the shell under the spring force and the tension provided by the cable. servo motor drive disc restoring spring

rigid shell

shell

base

cable

attachment unit spring microspines a) Gripper

limit block

b) Attachment unint

Fig. 1. 3D model of the gripper and attachment unit.

The asteroid robot consists of a fuselage and four limbs. 3R configuration is adopted by the four-legged asteroid robot. In order to make the fuselage as close to the ground as possible to reduce the difficulty of sampling on the asteroid surface, the hip joint whose axis is perpendicular to the fuselage is installed on the fuselage. Axes of the two knee joints are parallel to the fuselage. This configuration can shorten the arm of force of the foot when raising the leg, so as to reduce the load of knee joints. The four-legged asteroid robot with a gripper mounted on the tip of each limb for asteroid exploration is as shown in Fig. 2.

Fig. 2. 3D model of the asteroid robot.

3 Kinetostatic Model 3.1 Stiffness Model of the Microspine The plane structural and structural parameters of the microspine are shown in Fig. 3. The microspine can be regard as a plane serpentine spring. The microspine is composed

Design and Kinetostatic Analysis of a Legged Robot for Asteroid

501

of many basic units with the same structure. One of the basic elements is taken for analysis, and its deformation is assumed to be in the linear range. Suppose the left end of the microspine is fixed and a force points to right is applied to the right end. b is the width of the cross section, d is the spacing, h is the thickness of the microspine, l is the height of the microspine, and R is the radius of the semicircle arc. d

D

M F

A

l

R

D

h b

FS

A

a) Parameters of the microspine

b) Force analysis of section D

Fig. 3. The structural and parameters of the microspine.

According to the symmetry and equilibrium condition of the structure, the axial force of section D can be obtained: F = 0.5FS . The shear force is zero and the bending moment is M . Rotation angles of these sections are both zero. Therefore, a static basis with fixed section A and free end section D can be established. ϕ is the angle between a section of circular arc structure and the vertical direction. The bending moment of each section of the arc part can be expressed as:  M (ϕ) = M − FR(1 − cos ϕ) (1) ∂M (ϕ) ∂M = 1 The bending moment of each section of the straight line can be expressed as:  M (x) = M − FR(R + x) ∂M (x) ∂M = 1

(2)

According to the deformation compatibility condition, the rotation angle θ of section D is 0, and θ is the angle produced by the rotation of the beam section around the neutral axis under the action of bending moment. According to Karnofsky’s second theorem:  π  l ∂M (ϕ) ∂M (x) 1 2 (3) M Rd ϕ M dx =0 θ = EI (ϕ) (x) 0 0 ∂M ∂M The result is as follows: M = Fl

2 +2Rl+π R2 +2R2

2π R+4l

(4)

where E is the elastic modulus of the material used for the microspine. The deformation of the microspine δ can be obtained by introducing M into M (ϕ) and M (x) by Karnofsky’s second theorem. The stiffness of the microspine can be expressed as: k=

F δ

=

b3 hE 24l 3 +18R(2π l 2 +8Rl+π R2 )

(5)

502

Q. Wen et al.

In order to verify the stiffness formula, CAE simulation of the microspine is carried out. The left end of the microspine is fixed and subjected to a friction force of 1 N. Geometric parameters of the microspine are as follows: b = 1.5, h = 2.5, R = 2.5, l = 6.5 and E = 3780 MPa. Bring these parameters into the Eq. (5), and the stiffness of the microspine is 1262.1 Nm. According to the result of CAE simulation shown in Fig. 4, the deformation in the Y direction is 0.00077168 m and the stiffness is 1295.9 Nm. The error is about 2.68%.

a) Deformation distribution of the microspine b) Comparison of stiffness between simulation and formula

Fig. 4. Deformation and stiffness comparison of the microspine.

3.2 Statics Model of the Gripper The adhesion of the gripper is important for grasping the surface of the asteroid. The performance of the gripper directly affects the landing and movement of the asteroid robot. Figure 5 shows the structure and geometric parameters of the gripper. When the attachment unit grasps the surface of the asteroid, the microspine array is subject to the traction from the cable, the spring force and the friction. The force balance equation can be written as: (FT − k1 x) cos θ = Ff

r1

(6)

H

r2 x1

FS

Y x2 Fk1 FT

Fig. 5. Force balance and parameters of the gripper.

where θ is the angle of the passive joint when the gripper catch a roughness of the surface; k1 is the stiffness of the restoring spring and the microspine. Due to the existence

Design and Kinetostatic Analysis of a Legged Robot for Asteroid

503

of the limit block, the microspine can only move in a straight line in the rigid frame. Therefore, the rotation angle of the attachment unit around the passive joint is also θ . The attachment unit achieves balance under the tension of the cable, the elastic force of the vertical spring, the friction force and the supporting force of the surface. The force analysis is as follows: ⎧ Tc = Ff (x1 − x) sin θ + FN (x1 − x) cos θ + Ts ⎪ ⎪ ⎪ ⎨ Tc = FT x2 cos θ2  −1 x2 (1−cos θ) T = k x y cos θ + tan ⎪ s 2 2 ⎪ Y −x2 sin θ ⎪ ⎩ y = Y − (Y − x2 sin θ )2 + [x2 (1 − cos θ )]2

(7)

where k2 is the stiffness of the vertical spring; Tc is the moment of the cable to the passive joint; Ts is the moment of the vertical spring to the passive joint; Y is the length of the vertical spring; x1 and x2 are distances from passive joint to both ends of the attachment unit. Then, according to the geometric relationship, the rotation angle of the driving disc can be expressed as: θd =

√ 2 r12 +r22 − (r2 −r1 )2 +H 2 +x+y +H 2

(8)

2r1 r2

3.3 Kinematics of the Leg Figure 6 shows coordinate systems of the asteroid robot. {W } is word coordinate system, and coordinate system {H } is used to describe the position and posture of the fuselage. yH points to the heading direction, and zH points to the normal direction of fuselage. yLi and xLi are parallel to yH and xH respectively. Taking the left front leg as an example, directions of link coordinate systems are shown in Fig. 6. The inverse solution model of the leg can be written as:

y ⎧ θ1 = atan2 ⎪ ⎪

 x ⎪ ⎨ θ2 = sin−1 z A − ϕ   2  (9) z 2 + cθx −l1 + l22 +l32 ⎪ ⎪ 1 ⎪ θ3 = acos ⎩ 2l2 l3

Z1X

X1

2

X3 Z3

Z2

X0 Z0 Z1

Yli Xli

YH Li XH

H

ZH

Zli

W YW XW

Fig. 6. Coordinate systems of the robot.

ZW

504

Q. Wen et al.

Screw is used to establish thevelocity equation of the robot. Leg coordinate    theory systems Lsi are coincide with that of Lsi . The velocity of link i relative to the link i − 1 is expressed as follows: ωi Si = ωi Si + ∈ ωi Si × riP

(10)

where riP is the vector from the origin of the i coordinate system to the point P in that coordinate system. The velocity of any reference point P on the fuselage can be expressed as:  O T Gθ  ˙ VH = (11) θ1 · · · θ˙n P Gθ

    where GθO :n = Sn and GθP :n = Sn × (P − Rn ). P − Rn represents the vector of the point P in the n coordinate system. The rows of Jacobian matrix corresponding to generalized velocities are combined to form the first order influence coefficient matrix of the robot:     [G O ]  T θ (12) VH = GϕH ∅˙ ∅˙ 1 · · · ∅˙ 6 P [Gθ ]

4 Locomotion Planning Based on Kinetostatics 4.1 Stability Indices (1) Asteroid robot Stability analysis is the basis of motion planning for the robot. ZMP is widely used to analyze the stability of robots. By controlling the projection of the center of gravity of the robot in the polygon formed by support foots, the robot can be prevented from overturning caused by gravity. The stability margin can be expressed as: S = min(d1 , d2 , d3 )

(13)

where di is the distance from the center of mass to each side of the triangle. (2) Gripper The adhesion provided by the gripper determines the stability of the asteroid robot. When the needle fixed on the elastic attachment unit grasps the ground, the microspine will deform under the tension of the rope and adhesion. If the needle can firmly grasp the roughness of the ground, the tension provided by the rope continue to increase, which may damage the microspine. In addition, attachment units are installed on the gripper evenly, so two attachment units with opposite directions are defined as an attachment group. Taking an attachment group as an example, assuming that the gripper is subjected to an external force in the left direction, the deformation of the attachment unit on the left side will decrease and the deformation of the attachment unit on the right side will increase. Therefore, the following conditions are required for the gripper to grasp the asteroid surface.

Design and Kinetostatic Analysis of a Legged Robot for Asteroid

505

1) The friction coefficient should be greater than the arctangent of the force in Y direction and the force in Z direction; 2) The force acting on a single attachment unit is less than the smaller value of its limit loading and the releasing force. The above two conditions can be written as:  μ = tan−1 FFxz Funit < min(Funitmax , Frelease ) The grasping matrix can be expressed as:     n1 · · · nm [G] = N1 · · · Nm = r1 × n1 · · · rm × nm

(14)

(15)

where ni is the vector of contact between attachment unit and asteroid surface, ri is the vector from the center of the drive disc to the contact point, and m is the number of attachment units. The direction of the adhesion points to the center of the circumscribed circle formed by contact points. However, the external force F in the x direction will break this equilibrium state. In order to evaluate the effect of F on the gripper, the stability index of the gripper is proposed:   θi F cos θi+1 (16) , I = 1 − max F cos Fx Fx where Fx is the shear force provided by attachment unit, θi is the angle between F and the shear force. It is worth noting that the premise of stability index is F < F x . Obviously, the stability index of the gripper is related to F, Fx and θi . The smaller F or the larger θi , the more stable the gripper is. Increasing the maximum shear force provided by the attachment unit can also improve the stability of the gripper. 4.2 Gait and Foot Trajectory To facilitating the foot trajectory of the robot, the workspace of the foot should be analyzed first. Joint ranges are −90° to 45°, −70° to 70° and −50° to 130° respectively. Through the forward kinematics of the leg, workspace of the leg can be obtained, as shown in Fig. 7. The forward direction of the asteroid robot is Y. The workspace of the foot is a part of the hemispherical. Considering the moving speed and obstacle capability of the robot, it should have a larger working space in the Y and Z directions. By comparing the slice of workspace, the maximum working space is x = 0.15 m. The shape of the foot trajectory affects the motion performance of the robot. The motion performance of the robot at the moment of contact the surface is particularly important, and following factors must be considered: (1) The trajectory of the foot should be smooth; (2) There is no singular point in joint velocity and acceleration;

506

Q. Wen et al.

Y Z3 Z1

Z2

X

a) Joint number and fuselage coordinate system

b) Workspace of the foot

Fig. 7. The coordinate system of the asteroid robot and the workspace of the foot.

(3) when the gripper contact with the surface, relative sliding should be avoided. In this paper, the Bessel curve is used to fit the compound cycloid. According to the contact angle between the gripper and the surface of the asteroid, the foot trajectory curve can be divided into three cases, as shown in Fig. 8. It indicates that the gripper approaches the ground along the normal vector of the ground. Similarly, the red and blue curves indicate that the angle between the moving direction of the gripper and the ground is acute angle and obtuse angle respectively. The co-simulation of UG and Simulink is used to explain the joint torque and the force provided by the gripper under three kinds of gait trajectory. The gait sequence diagram of the asteroid robot and the sequence diagram of a gait cycle is shown in Fig. 8. The red area of LFG indicates that the gripper grasps the surface of the asteroid, while the green area indicates that the gripper releases the surface. The green area of the LFL represents a step forward of the left front leg. LF G

Obtuse angle trajectory Right angle trajectory Acute angle trajectory

a) Three kinds of foot trajectories

0-2 2-3 3-13 13-15 15-16 16-26 26-28 28-38 38-39 39-49 49-51 51-52 52-62 62-64

LF L

RF G

RF L

LBG

LB L

RB G

RB L

0s-13s

13s-28s

28s-51s

51s-64s

b) A time sequence diagram of a gait cycle of asteroid rover

Fig. 8. The trajectory of the foot and the motion sequence diagram of the asteroid robot. The curve represents the trajectory of the foot. The curves of red, green and blue indicate that the contact angle between foot and ground is sharp angle, right angle and obtuse angle (Color figure online).

4.3 Result and Discussion A simulation with one gait cycle is performed to analyze the joint torque and attachment of different foot trajectories. Joint torques and gripper forces are shown in Fig. 9. The

Design and Kinetostatic Analysis of a Legged Robot for Asteroid

507

friction coefficient is 0.9 and gravity is 0. It can be seen from the figure that the difference of driving torques of the joint 2 is small when the left hind leg is in the support phase under three kinds of gait. But in the swing phase, both the blue curve and the red curve reach the peak quickly, and the peak is higher than the green curve’s. It indicates that when the asteroid robot raises its legs along the vertical direction, the torque of the knee joint is smaller than that along other two directions. Figure 9b shows the force acting on the spherical hinge of the left hind leg of the asteroid robot. The local maximum value of the green curve is usually smaller than other two curves. There are two reasons for the difference of force acting on the spherical hinge: 1. When other legs of the robot leave the ground, the disturbing force on the fuselage is different; 2. For the contact angle between the gripper and the surface is acute or obtuse angle, the force on the attachment unit of the gripper is uneven. The attachment unit near the surface slightly collided with the ground. The stability margin of the asteroid robot is shown in Fig. 9c. It can be seen that the stability margin of the robot during 38 s–62 s is the smallest, which is 56.56 mm. In the rest of the time, the stability margin of the robot is more than 100 mm. The fuselage of the asteroid robot is always in a stable state in the process of moving. As can be seen from Fig. 9d, when the robot raises its legs for the first time, the red curve decreases obviously. This shows that the stability of the gripper is the worst when the contact angle is acute. When the right hind leg is stepped, the green curve reaches the stable state first. This shows that the contact angle is a right angle, the stability of the gripper is the best. 400

F(N)

T(N·mm)

6

100

0

Right angle trajectory

Acute angle trajectory

Acute angle trajectory

200

0

Obtuse angle trajectory

8

Obtuse angle trajectory Right angle trajectory

300

4 2

6

0

12 18 24 30 36 42 48 54 60

t(s)

0

a) Torque of the joint 2

6

12 18 24 30 36 42 48 54 60

t(s)

b) Force acting on the spherical hinge

180

Stability of grippers

160

S(mm)

140

120 100 80 60

40

0

6

12 18 24 30 36 42 48 54 60

t(s)

c) Stability margin of asteroid rover

0.8 0.6 0.4 Obtuse angle trajectory Right angle trajectory Acute angle trajectory

0.2 0

0

6

12 18 24 30 36 42 48 54 60

t(s)

d) Stability of grippers

Fig. 9. Joint torque and stability margin of the asteroid robot.

508

Q. Wen et al.

5 Conclusion In this paper, a gripper for the asteroid robot is proposed. According to the working principle of the gripper, the mechanical model of the gripper and the stability index established. The stiffness model of the microspine is established and verified. The foot trajectory of the asteroid robot is established, and three kinds of foot trajectory are analyzed. According to the joint torque and the force of the attachment unit, it is more appropriate for the foot to leave or approach the ground along the vector perpendicular to the ground. The stability margin of the robot in one gait cycle is obtained. According to the results, the asteroid robot walks stably according to the planned foot trajectory. Funding. Supported by the State Key Laboratory of Mechanical System and Vibration under Grant MSVZD202106, Key Laboratory Fund of Science and Technology on Space Intelligent Control under Grant HTKJ2019KL502011.

References 1. Trilling, D.E., Lisse, D.P.: Spitzer’s solar system studies of asteroids, planets and the zodiacal cloud. Nat. Astron. 4, 940–946 (2020) 2. Evans, N.W., Tabachnik, S.A.: Asteroids in the inner solar system – II. Observable properties. Mon. Not. R. Astron. Soc. 319, 80–94 (2000) 3. Kawaguchi, J., Fujiwara, A.: Hayabusa – its technology and science accomplishment summary and Hayabusa-2. Acta Astronaut. 62, 639–647 (2008) 4. Jiang, H., Hawkes, E.W.: A robotic device using gecko-inspired adhesives can grasp and manipulate large objects in microgravity. Sci. Robot. 2, eaan4545 (2017) 5. Di, K.C., Liu, Z.Q.: Geospatial technologies for Chang’e-3 and Chang’e-4 lunar robot missions. Geo-Spat. Inf. Sci. 23, 87–97 (2020) 6. Chacin, M., Yoshida, K.: Stability and adaptability analysis for legged robots intended for asteroid exploration. In: 2006 International Conference on Intelligent Robots and Systems, pp. 1744–1749. IEEE/RSJ Vols (2006) 7. Guttler, C., Hasselmann, P.H., Li, Y.: Characterization of dust aggregates in the vicinity of the Rosetta spacecraft. Mon. Not. R. Astron. Soc. 469, S312–S320 (2017) 8. Thuillet, F., Michel, P.: Numerical modeling of lander interaction with a low-gravity asteroid regolith surface application to MASCOT on board Hayabusa2. Astron. Astrophys. 615 (2020) 9. Ho, T.M., Baturkin, V.: MASCOT the mobile asteroid surface scout onboard the Hayabusa2 mission. Space Sci. Rev. 208, 339–374 (2017) 10. Uckert, K., Parness, A.: Investigating habitability with an integrated rock-climbing robot and astrobiology instrument suite. Astrobiology (2020) 11. Kim, S., Asbeck, A.T.: SpinybotII: climbing hard walls with compliant microspines. In: 12th International Conference on Advanced Robotics, pp. 601–606. IEEE (2005) 12. Liu, Y., Sun, S., Wu, X., Mei, T.: A wheeled wall-climbing robot with bio-inspired spine mechanisms. J. Bionic Eng. 12(1), 17–28 (2015). https://doi.org/10.1016/S1672-6529(14)600 96-2 13. Dharbaneshwer, S.J., Thondiyath, A., Subramanian, S.J., Chen, I.-M.: Finite element-based grasp analysis using contact pressure maps of a robotic gripper. J. Braz. Soc. Mech. Sci. Eng. 43(4), 1–11 (2021). https://doi.org/10.1007/s40430-021-02907-8

Design and Kinetostatic Analysis of a Legged Robot for Asteroid

509

14. Yamaguchi, K., Hirata, Y.: Development of robot hand with suction mechanism for robust and dexterous grasping. In: 2013 International Conference on Intelligent Robots and Systems, pp. 5500–5505. Tokyo, Japan (2013) 15. Wang, B., Xu, F.Y.: Grasping mechanism and prototype experiment of bionic sharp hook on rough surface. In: International Conference on Mechatronics & Machine Vision in Practice. IEEE (2016)

Effect Analysis of Initial Conditions on Landing Performance of Vertical-Landing Vehicle Yingchao Wang, Haitao Yu(B) , Haibo Gao, Zhen Liu, and Zongquan Deng State Key Laboratory of Robotics and Systems, Harbin Institute of Technology, Harbin, China [email protected]

Abstract. In order to comprehensively evaluate the landing performance of vertical-landing vehicle and increase the probability of successful landing, a sagittal dynamics model of legged-type vertical landing vehicle on landing process was established, including the elastic-plastic collision model of multi-rigid body system to predict discontinuous process. The displacement-velocity diagram is used to investigate the influence mechanism of the initial tilt angle and the initial height on the trajectory of the center of mass of the vertical-landing vehicle in various typical landing modes. In addition, the leg buffering behavior of the vehicle is investigated. The buffering mechanism of touched-down legs of different time periods under different initial conditions were accordingly explored and analyzed. The results showed that the buffers in legs that touch the ground first are more sensitive to changes of the falling height, while the initial tilt angle has more influence on the buffers of legs that subsequently touch the ground more. Unlike that the influence of the initial tilt angle on the attitude recovery of the vehicle will continue throughout the landing process, and gradually decease as the convergence, the effect of the initial falling height mainly focused on the initial and final stages of the landing process. The phase diagram is a favorable angle for investigating and analyzing the effect of initial conditions on the entire landing process of the vertical landing vehicle. Keywords: Vertical-landing vehicle · Phase diagram analysis · Initial conditions · Landing process

1 Introduction The final stage of the landing process starts after turning off the engines of the vertical landing vehicle, then the legged landing device is used to absorb the impact energy and adjust the attitude of the vehicle main body until the vehicle is completely stable. The landing process of the vertical landing vehicle with a legged landing device includes the discontinuous collisions and complicated dynamic states and transition conditions due to the uncertainty of its initial conditions and the structural characteristics Yingchao Wang—Female, Doctor Candidate. Haitao Yu—Male, Doctor, Lecturer, Master Instructor. © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 510–520, 2021. https://doi.org/10.1007/978-3-030-89092-6_46

Effect Analysis of Initial Conditions on Landing Performance

511

of the legged landing device with buffers. Therefore, the effect of initial conditions on the complete landing process is complicated and unclear. In order to improve the landing performance, provide a strategic reference and theoretical basis for the control work of the vehicle before the engines shuts down, the effect of various initial conditions on the landing performance of the vehicle should be an important research content. Many academics have conducted relevant studies on the effect of initial conditions. In the literature [4], Ming Z discussed the effects of various initial horizontal velocities, initial tilt angles and initial angular velocities on the acceleration, buffering force, stroke and vertical displacement of the main body of the vehicle. They mainly compared the initial conditions on the value of the peak and the time to reach the peak of the above parameters. Scholars used an optimization algorithm in [5, 6] to find the critical landing situations based on the peak value of the parameters related to the set landing performance index. Yuanyuan L investigated main effects of initial landing conditions on landing dynamics responses (the peak value of related parameters) with RSM methods [7]. The landing process of the vertical landing vehicle is actually a nonlinear hybrid system containing multiple states and switching conditions, and the displacement-velocity phase diagrams provide a new perspective for the investigation of the effects [8–10]. Based on the phase diagrams, the effect of initial conditions on the converge trajectory of the landing process is clearly visible. From the phase diagrams of the main body and buffers of the vehicle, we analyzed the effects of the initial inclination angle and initial falling height on the landing process in various typical landing modes. After established the dynamics equations and the state machine of the landing process of the vertical landing vehicle, we investigated the connection between the initial conditions of various types and values and the convergence process of the main body and buffers of legs that touch down with the ground in different periods of time. The investigation of the effects of initial conditions on complete landing process will contribute to the reveal of a deeper and more precise relation between the initial conditions and the landing performance of the vehicle, and could provide the theoretical support for accurately adjusting the initial conditions to improve the landing performance of the vertical landing vehicle.

2 Dynamics Model of Landing Process of Legged Vertical-Landing Vehicle The inverted triangle and parallelogram landing devices are currently the main devices used in vertical-landing vehicle recovery. The inverted triangle landing device has better anti-skid ability than another, the research below is based on vehicles with inverted triangle devices. The schematic diagram of theoretical model is shown in Fig. 1. The landing process of a vertical-landing vehicle is a nonlinear hybrid system including discontinuous collisions. In the system, the sensitivity of the landing process of vehicles to the initial conditions causes complex state transitions which are hard to predict, and becomes exactly the difficult part of the landing dynamics research work. Due to the various initial heights, postures and motion conditions (velocity, angular velocity), the landing situation of four-legged vertical landing vehicles is diverse.

512

Y. Wang et al.

Fig. 1. The theoretical model of vehicle with inverted triangle device.

According to empirical recommendations cited in [11], they are generally divided into the 2-2 mode, 1-2-1 mode and 1-1-1-1 mode, where each number represents the number of legs that touch the ground each time. Among them, the 2-2 and 1-2-1 landing modes are typical modes. The mode with four feet touching the ground at the same time is the most ideal situation, which can be regarded as the special situation of 2-2 modes or 1-2-1 modes when the initial tilt angle and horizontal velocity are both zero. The landing process of the vertical landing vehicle is a time-varying process involve energy absorption and collisions. The buffers of the legs here is simplified as a parallel spring damping model [3]. In following analysis, we assumed that the supporting force of the leg touched-down with the ground is mainly absorbed by the main pillars, and the friction is primarily absorbed by the auxiliary pillars. The forces of the vehicle are shown in Fig. 2. In Fig. 2, FNi , Ffi , Fdi , Fai respectively refer to the support force, friction force, buffering force of the main pillar and the force of the auxiliary pillar of the ith leg. In the friction-containing three-dimensional collision model between a multi-rigid body system and the ground, selected the centroid displacements, nutation angle and precession angle as generalized coordinates, when the tangential modes are sliding and sticking, Eqs. (1) and (2) are used to obtain centroid velocities after collision, where K, h and M are parameter matrices related to the instantaneous configuration of the collision [2].   vt d˙q −1 −µK =M +h (1) |vt | dP3

Effect Analysis of Initial Conditions on Landing Performance

513

z0 FNi

y0

Fdi

x0 Fai

Ffi

Fig. 2. The force of vehicle with inverted triangle device.

   −1 d˙q −1 T −1 T −1 −K K M K =M K M h+h dP3

(2)

The friction force during the landing is calculated by equation [1] Ff = −

exp(δT /(1.5KT )) − exp(−δT /(1.5KT )) μf FN exp(δT /(1.5KT )) + exp(−δT /(1.5KT ))

(3)

δT —Tangential shear displacement (m). Ff —Tangential contact force (N). KT —The shear displacement modulus (m). μf —The coefficient of friction between the foot and the ground. Figure 3 shows the dynamic calculation flow of the landing process in 2-2 and 1-2-1 landing modes.

Vx,Vy,

,lc,θ

Vx,Vy,

Body centroid dynamics model Foot dynamics model

Ffi

Collision model

L,

L

Fdi

Fdi,Vti, xti

Friction model

Main pillar model (Spring-damping model)

Fig. 3. The calculation flowchart of landing process.

In the Fig. 3, Vx , Vy , ω are the horizontal, vertical and angular velocities of the center of mass of the vehicle main body, lc , θc are the configuration parameters at the moment of collision, and Vti , xti are the velocity of the ith foot, L, L˙ is the buffer stroke and buffer velocity of the main pillar.

514

Y. Wang et al.

3 Analysis of the Mechanism of the Initial Conditions on the Landing Process 3.1 The Effect Mechanism of the Initial Tilt Angle Above all, suppose that in 2-2 modes, the legs 1 and 2 touch the ground first, and then legs 3 and 4, and in 1-2-1 modes, the leg 1 touches the ground first, then the 2, 4 legs, and finally the leg 3 touches the ground. Table 1. Values of parameters of the vehicle in simulation Parameters (Units)

Meanings

Values

Ma (kg)

Total weight

1630

H1 (m)

Vertical distance from centroid to main pillar

0.167

H2 (m)

Vertical distance from centroid to auxiliary pillar

1.542

Lh0 (m)

Original length of main pillar

3.168

La0 (m)

Length of auxiliary pillar

2.028

Lb0 (m)

Original length of buffer

0.120

µ

Coefficient of friction

0.85

E

Restitution coefficient

0.95

h(m)

Horizontal distance from main to auxiliary pillar

0.066

R(m)

Radius of the main body

0.654

K

Stiffness coefficient of buffer

200000

D

Damping coefficient of buffer

9000

The values of parameters used in the simulation are listed in Table 1. The 2-2 Mode In order to study the effect of the initial tilt angle on the landing process of the vertical landing vehicle, the remaining initial conditions should be consistent, like the vertical velocity, therefore the distance to the ground from the lowest foot is set to a constant value of 0.2 m. Within the range of (0°, 5°) inclination, the displacements versus velocities (x-Vx, y-Vy, θ -ω) of the center of mass of the vehicle and the strokes versus buffering velocity ˙ of the two pairs of legs that touch the ground first and secondly are obtained, (L − L) as shown in Fig. 4. From Fig. 4a, the maximum horizontal displacement and velocity will increase as the initial tilt angle increases. And when the tilt angle is greater than or equal to 3°, the trend of the curve slows down, which means the friction forces are reduced.

Effect Analysis of Initial Conditions on Landing Performance

515

Fig. 4. The phased diagram (a) Horizontal displacement- horizontal velocity (b) Vertical displacement-vertical velocity (c) Attitude angle-angular velocity (d) Stroke-buffer velocity of 1, 2 legs (e) Stroke-buffer velocity of 3, 4 legs.

Figure 4b reveals the vertical movement of the main body, comprise information such as the distance from the main body to the ground and the rebound height. That is, the smaller the tilt angle, the closer the main body is to the ground, the lower possibility of rebound. Figure 4c shows that the fluctuation range of the attitude and angular velocity basically changes with the initial inclination angle proportionally. Comparing Fig. 4d and Fig. 4e, the maximum compression stroke of the legs that touch the ground first is slightly larger than the other two legs, and the buffering trajectory between adjacent inclination angles is closer, especially when the buffer velocity is close to zero, indicating that the impact on the first touching legs is greater than that of the other two legs. In addition, when the initial tilt angle is large (greater than 3°), 1, 2 legs that touch the ground first will leave the ground for a short time unlike the 3, 4 legs. The 1-2-1 Mode The 6 phase diagrams are gained, as shown in Fig. 5. From Fig. 5a, it can be seen that the tangential motion of the 1-2-1 mode of the vehicle is similar to that of the 2-2 mode, however, the 1-2-1 mode will experiences greater horizontal displacement and velocity under the influence of the same tilt angle. Similar to the 2-2 mode, when the initial inclination angle is greater than 3°, the convergence of

516

Y. Wang et al.

Fig. 5. The phased diagram (a) Horizontal displacement- horizontal velocity (b) Vertical displacement-vertical velocity (c) Attitude angle-angular velocity (d) Stroke-buffer velocity of leg 1 (e) Stroke-buffer velocity of 2, 4 legs (f) Stroke-buffer velocity of leg 3.

2-2 landing modes slow down before entering a small oscillation, while the 1-2-1 mode becomes steep. Figure 5b shows that in this mode, the initial tilt angle has a greater impact in the early stage of landing, the rebound height of the center of mass of the vehicle is smaller later. The fluctuation range of the vertical displacement and velocity is not much different from the 2-2 mode. From Fig. 5c, we can see that the oscillation range of attitude and angular velocity is significantly larger than that of the 2-2 mode, and the convergence trend is more gentle.

Effect Analysis of Initial Conditions on Landing Performance

517

Fig. 6. The phased diagram (a) Horizontal displacement- horizontal velocity (b) Vertical displacement-vertical velocity (c) Attitude angle-angular velocity (d) Stroke-buffer velocity of 1, 2 legs (e) Stroke-buffer velocity of 3, 4 legs.

From the perspective of trajectory density, the initial tilt angle has a more profound effect on the landing attitude of the 1-2-1 mode. Comparing Fig. 5d–f, it is obvious that the initial tilt angle of the vehicle has a greater impact on the buffers of the first and the last touched-down legs, the larger the initial tilt angle, the size of stroke will follow, especially when the buffering velocity is close to zero. 3.2 The Effect Mechanism of the Initial Falling Height The 2-2 Mode In order to study the effect of the initial falling height (the height of the center of mass when the engine is turned off) on the landing process, the initial tilt angle of the vehicle is set to a constant value of 2°, within the range of (0.1, 0.5) M drop height, obtain five phase diagrams, as shown in Fig. 6.

518

Y. Wang et al.

From Fig. 6a, it can be seen that increasing in the falling height will increase the fluctuation of the horizontal displacement and horizontal velocity. With the increase of the initial falling height of each curve during the I, II and III stages of the landing process, the distance between adjacent trajectories decreases. In stage IV, when the initial drop height is low, the trajectory trend is similar, and when the height is greater or equal to 0.4m, the curve becomes smoother. As shown in Fig. 6b, the vertical displacement and velocity of the main body of the vehicle is proportional to the falling height, and the space between trajectories gradually decreases with the convergence process. In Fig. 6c, the higher the drop height, the smaller the attitude recovery in the first stage of the curves. And in the second stage after the 3, 4 legs touch the ground which are being compressed, the initial drop height has little effect on the attitude and angular velocity. However, when the 3, 4 legs begin to recover (stages III and IV), the main body of the vehicle will undergo an oscillation range of attitude angle and angular velocity that increase with the increase in the falling height. From Fig. 6d and Fig. 6e, the buffering stroke and buffering velocity range of the legs that touch the ground first is larger than that of the legs that touch the ground afterwards. The stroke and buffering velocity range of the first and final touched-down legs are directly proportional to the initial falling height, but the distance between the trajectories of the legs that contact the ground first is significantly larger. Therefore, falling height will affect the legs that touch the ground first. The 1-2-1 Mode The 6 phase diagrams are gained, as shown in Fig. 7. Figure 7a shows the similar phase changes to the 2-2 mode. With the increase of the initial falling height, the final stable displacement and velocity are larger than the synchronization values of 2-2 modes. From figure Fig. 7b, the vertical displacement and velocity range of the 1-2-1 mode is not much different from that of the 2-2 mode, but the first rebound after touching the ground is slightly higher than that of the 2-2 mode. In addition, compared with the trajectory of the 2-2 mode, the convergence process of 1-2-1 modes has a sharp turn. The attitude angle and angular velocity trajectories in Fig. 7c are relatively close at the beginning stage, and the distance gradually reduces first then increases later, indicating that the impact of different falling heights on the landing process of the 1-2-1 mode is mainly in the late stages, which is slightly different from the 2-2 mode. In Fig. 7d and Fig. 7f, the stroke changes more sharply when the buffering velocity approaches zero. From Fig. 7e, it can be seen that only in the middle stage of the buffering of legs 2, 3, the initial falling height has little effect, the greater the falling height, the fluctuation range of stroke and buffering velocity will increase accordingly.

Effect Analysis of Initial Conditions on Landing Performance

519

Fig. 7. The phased diagram (a) Horizontal displacement-horizontal velocity (b) Vertical displacement-vertical velocity (c) Attitude angle-angular velocity (d) Stroke-buffer velocity of leg 1 (e) Stroke-buffer velocity of 2, 4 legs (f) Stroke-buffer velocity of leg 3.

4 Conclusions Within the range of the initial tilt angle (0°, 5°) and the initial falling height (0.1 m, 0.5 m), the 1-2-1 mode tends to experience larger horizontal displacement and horizontal velocity than the 2-2 mode in the landing process. Under the same initial tilt angle, the vehicle landing in 1-2-1 modes will be more prone to rebound only when the strokes of the buffers is restored for the first time. The buffers in the first touched-down legs are more easily effected by the initial falling

520

Y. Wang et al.

height changes, while the initial tilt angle has a greater influence on the buffers in the final touched-down legs. The effect of initial tilt angles and initial falling heights on the landing process of the vertical landing vehicle is time-varying, and the effect is more considerable when the corresponding velocity approaches zero. In terms of the effect mechanism to the attitude recovery of the main body of the vehicle, unlike the effects of various initial angles that last through the complete landing process, the various initial falling heights mainly effect the early and final periods of the landing process after the first touchdown. Furthermore, the effects of the various initial falling heights in 2-2 modes almost vanish during a short middle period of the landing process of the vehicle.

References 1. Liang, D., Haibo, G., Zongquan, D.: Foot-terrain interaction mechanics for legged robots: modeling and experimental validation. Int. J. Robot. Res. 32(13), 1585–1606 (2013) 2. Zhen, Z., Caisgan, L., Bin, C.: Numerical calculation method for the collision of threedimensional multi-rigid bodies with friction. Sci. China Ser. G. Phys. Mech. Astronomy 36(1), 72–88 (2006) 3. Yingchao, W, Haibo, G, Haitao, Y.: Dynamics modeling and stability analysis of verticallanding rocket. J. Mech. Eng. 4. Ming, Z., Dafu, X., Shuai, Y.: Design and dynamic analysis of landing gear system in vertical takeoff and vertical landing reusable launch vehicle. J. Aerosp. Eng. 233(10), 3700–3713 (2019) 5. Shuai, Y., Hong, N., Ming, Z.: Optimization and performance analysis of oleo-honeycomb damper used in vertical landing reusable launch vehicle. J. Aerosp. Eng. 31(2), 04018002 (2018) 6. Zongmao, D., Hongyu, W., Chunjie, W.: Hierarchical optimization of landing performance for lander with adaptive landing gear. Chin. J. Mech. Eng. 32, 20 (2019) 7. Liu, Y.Y, Song, S.G., Ming, L.: Landing stability analysis for lunar landers using computer simulation experiments. Int. J. Adv. Robot. Syst. 1–15 (2017) 8. Albert, C.J.L.: Discrete and Switching Dynamical Systems. Higher Education Press, Beijing (2012) 9. Sahadevan, R., Lakshmanan, M.: Nonlinear Systems. Narosa Publishing House, India (2001) 10. Michael, G.: Nonlinear Time-Discrete Systems. Springer-Verlag, German (1982) 11. Bazhenov, V.I., Osin, M.S.; Posadka kosmicheskikh apparatov na planety (Landing of Spacecraft on the Planets), 159 p. Mashinostroenie, Moscow (1978)

Neural Learning Enhanced Motion Planning and Control for Human Robot Interaction

Comparison of GBNN Path Planning with Different Map Partitioning Approaches Mingzhi Chen(B)

, Daqi Zhu , and Zhenzhong Chu

Shanghai Maritime University, Shanghai, China [email protected]

Abstract. Autonomous underwater vehicles (AUVs) usually work without any commands from remote control and finish tasks on their own. Path planning is necessary and very important. The Glasius bioinspired neural network (GBNN) model has already been applied to path planning of AUVs. Generally, the model divides the environment space into many rectangular grids. However, the triangular and hexagonal grids can also be adopted to divide the environmental space. To compare among the three different map partitions, analysis and simulations are performed. The path length, steering and speed varies are compared while the receptive range of AUV keeps the same. Analysis shows that the triangular grid may be the worst and the hexagonal grid may be the best. However, in the simulations, there is a path oscillation problem in the hexagonal partition, which makes the path longer and zigzag. On the other hand, the triangular gird also has some advantages, such as minimum turning angle, while the rectangular grid can make a balance. Keywords: Path planning · Map partition · Glasius bioinspired neural network (GBNN) · Autonomous Underwater Vehicle (AUV)

1 Introduction Recently, more and more interest has been focused on the development of autonomous underwater vehicle (AUV). AUV has successfully conducted environmental monitoring, resource deployment, maritime rescue and search, and even search for mines in the military [1, 2]. Since the AUV works autonomously in an underwater environment, it is very important to plan a safe and low-cost path. Meanwhile, many path planning algorithms have been proposed and have shown good performance [3, 4]. Classical path planning algorithms include visibility graph, cell decomposition, graph search, rapidly exploring random tree, and so on. At the beginning, Lozano-Perez and Wesley proposed the visibility graph algorithm in 1979. The graph search algorithms search for a safe path based on the directed graph, which include the depth-first and width-first algorithms [5, 6]. In the graph search algorithm, dynamic programming is usually used to explore the path [8]. Dijkstra algorithm finds the shortest path based on the grid map, which belongs to the width-first algorithm [7]. The global optimal path can be obtained by calculating the shortest distance from the initial point to any point in © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 523–533, 2021. https://doi.org/10.1007/978-3-030-89092-6_47

524

M. Chen et al.

the free space. However, they are only suitable for static and known environments. Later, A*, Lifelong Planning A* (LPA*), D*-lite are also applied to path planning, which are suitable for dynamic environments [9, 10]. Although they can be applied to dynamic environments, they usually work well in low dimensional environments. The artificial potential field algorithm can plan a continuous path through the repulsive force from obstacles and the attraction from the target point [11, 15]. It is blamed for the local minima, where the AUV may be trapped. The rapidly search random tree (RRT) algorithm is an incremental sampling search method. It does not require any parameter tuning in the application and has good performance [12]. Many intelligent algorithms are also used for path planning of underwater vehicles. Swarm intelligent optimization algorithms are proposed and applied in path planning of underwater vehicles, such as the particle swarm optimization (PSO) and ant colony optimization (ACO) [13, 14]. Some evolutionary algorithms such as genetic algorithm (GA) and differential evolution (DE) are proposed and showing good performance in the path planning [16, 17]. However, they usually set an objective function first, and optimize the function through iterations, which may take much time. Fuzzy logic can successfully complete the path planning in both the static and dynamic environments [18]. It can ensure the safety of the AUV, but the path length is usually not optimal. Sometimes, complex fuzzy rules are needed in complex multi-obstacle environments. Artificial neural networks (ANN) usually plan and optimize paths by learning iterations [19, 20]. They are first proposed to complete path planning in a static environment. Moreover, they require prior environmental knowledge, and need time to learn from the environment to shorten the path length. Later, dynamic neural networks (DNN) were also proposed for path planning. Glasius bioinspired neural network (GBNN) was studied to be very effective for path planning [21]. Later, a bioinspired neural network (BNN) that could be a continuous expression of the GBNN became a good path planner [22]. Recently, both BNN and GBNN have been extended to the path planning for AUVs in underwater environments [23, 24]. To get an optimal time cost path in an ocean current environment, a dynamic neural network improved from the GBNN is also proposed [25]. In the studies, the environments are usually divided by the rectangular grids. Obviously, it is simple and easy to implement rectangular grids. However, the environment can also be divided by the triangular and hexagonal grids. Which kind of grid is more suitable for GBNN is worthy of further study. Therefore, the main contribution of this work is to compare the different discretization of environments for the GBNN algorithm. The underwater environment constructed by sonar is partitioned by three different shaped grids. The GBNN algorithm is used to plan safe paths with the different environment models. The results are compared in the path length, steering and speed varies. Although the hexagonal grid should be the best in the analysis, the simulations show that it may not be suitable for the GBNN algorithm. Meanwhile, the triangular grid is ought to be the worst, but it also shows some merits in the simulations. However, the rectangular grid can balance the advantages and disadvantages and is more suitable for the GBNN algorithm. The rest of this paper is organized as follows. In Sect. 2, the environment model and path planning issues are raised. Section 3 describes different map partitions and analyzes

Comparison of GBNN Path Planning

525

their advantages and disadvantages. Comparison simulations are introduced in Sect. 4. At last, Sect. 5 puts forward some conclusions.

2 Problem Statement In the application, AUVs usually dive deep and work at a certain depth. Meanwhile, AUVs are usually equipped with a forward-looking sonar to find and avoid obstacles. The path planning for AUVs is carried out with the environment model built by the forward-looking sonar. The path planning problem is usually defined as finding the course of points to reach the predefined destination from the starting location [3]. While looking for short-distance routes, the path planning must ensure a safe navigation too.

Fig. 1. Environment model.

Therefore, the path planning problem is to search for a short path to reach the target from the AUV’s initial position while avoiding obstacles. Many algorithms use grid maps to represent the environment. The grid environment constructed by the forward looking sonar is shown as Fig. 1. In the grid map, many uniformly distributed regular grids fill the entire map space. Obstacles detected by sonar are painted blue, and other spaces remain blank. The map can transmit information to the path planning algorithm. The space that is dangerous to AUV will contain prohibited information, while free space contains permitted information. Generally, equilateral triangles, rectangles, and regular hexagons can be used for grid decomposition of the environment. We intend to compare these different grid partitions for the GBNN model in detail.

3 Mathematical Model of Path Planning Algorithm In the path planning, the environment space needs to be gridded first. Many regular grids divide the entire space along the Cartesian direction. The path planning is accomplished through first calculating the neural activity of grids, and then searching for the grid with the largest neural activity. Path planning is based on the GBNN model, whose dynamics function is described by  xi (t + 1) = g( Wij xj (t) + Ii (1) j∈Si

526

M. Chen et al.

Wij are symmetric connection weights as (2), where r is the radius of grid unit.  2 e−γ |i−j| , if|i − j| ≤ 2r Wij = (2) 0, if|i − j|> 2r Ii represents the location information coding of the obstacle and target in this equation, which is given by (3). ⎧ i is the target ⎪ ⎨ υ, i is an obstacle, υ ∈ R, υ > 1 (3) Ii = −υ, ⎪ ⎩ 0, else Since any monotonically-increasing function can be used as the transfer function, a piecewise linear transfer function as (4) was chosen. ⎧ x≤0 ⎪ ⎨ 0, x ∈ [0, 1] and β > 0 (4) g(x) = βx, ⎪ ⎩ 1, x≥1 xi (t + 1) is the neural activity of the grid node i in the next iteration; xj (t) is the neural activity of the grid node j in the current iteration; |i-j| represent the Euclidian distance from the neuron i to the neuron j; Si is a set of neighboring neurons of neuron. Based on the Eqs. (1)–(4), we can calculate the neural activity for the neurons in the regular grid space. According to the neural activity of neurons, the next moving step of the AUV is searched with the highest neural activity in its receptive region as its moving direction. Therefore, a set of neurons with the highest neural activities forms the planned path.

4 Theoretical Comparison of Different Grid Divisions In the previous researches, the whole space is usually divided by rectangular grids. Two other kinds of equilateral polygon grids are introduced to divide the space.

Fig. 2. Three kinds of grid divisions.

Considering the same receptive region, these regular polygons have the enclosing circle with the same radius r. The triangular, rectangular, and hexagonal grids are presented in Fig. 2. Because the rectangular grid division is simple and easy to be realized,

Comparison of GBNN Path Planning

527

many previous studies usually applied this grid model. However, the hexagonal grid may be intuitively better. While using these regular grids to divide the 2-D maps, the Fig. 3 vividly shows the space division. As in (2), the radius of the receptive region is 2r, in order to let the neurons connecting to all the neighboring nodes.

Fig. 3. Space division of three different grids in 2-D environment.

As shown in Fig. 3, when the equilateral triangles divide the 2-D map, the number of adjacent triangles is twelve within the radius of 2r. In other words, there are twelve directions to choose, and there are three different lengths in these directions. When the AUV needs to arrive at the adjacent unit within a fixed time, it needs to adjust the running speed according to different lengths, resulting in a change in speed. Therefore, the AUV may often accelerate or decelerate. In addition, it may need to turn to twelve different directions, which leads to more frequent turns and more complex controls. Moreover, it needs more grids to fill the whole environment than the other two grid shapes, which induces more computation. In this way, equilateral triangle grids may be obviously inferior to the other two grids. When the 2-D map is large enough (the influence of the map boundary to grid division can be ignored) and the area is constant, the number of grids required for map division is inversely proportional to the area of grid shape. The ratio of the number of rectangular grids to the number of hexagonal grids can be represented by SH /SS , where SH is the area of the hexagon, and SS is the area of the square. Obviously, SS = 2r 2 √ 3 3 2 r SH = 2 √

(5)

(6)

As a result, SH /SS = 3 4 3 ≈1.299. That is, the number of hexagonal grids is about 23% less than that of rectangular grids. At the same time, when the map grid uses a rectangular grid, the AUV can reach adjacent neurons through eight directions, which form two different lengths. However, when the map grid uses a hexagonal grid, there are only six directions and the distance length of all directions is the same. According to this analysis, the hexagonal grid is better than the rectangular gird. However, the comparison simulations need to be carried out.

528

M. Chen et al.

5 Simulation Comparisons In order to conduct the simulations, the grid map should be established first. We use Matlab to realize different grid divisions in the 2-D map. In the simulations, the radius r is assumed as 1 m. The parameters of the GBNN model is set as β = 1, γ = 3, v = 200 in all the grid divisions. The parameters β and γ are selected to meet the requirement that the neural activity other than the target neuron is less than 1. During the navigation, the consumption of path length, steering control and speed control are vital. Meanwhile, the planned path of different map grids directly affects the control of the underwater vehicle. Simulation comparisons will be performed to compare the different shapes of grid. The comparison is firstly conducted between the rectangular and hexagonal grids on the path length and the control of AUVs. At last, the results of the triangular grids are also presented. 5.1 The Effect of Rectangular and Hexagonal Grids on Path Length In the simulations, the planned paths combines several green points from the starting point to the target point. When the area of the 2-D map is the same, it needs fewer units using hexagonal grids. As mentioned above, the number of the rectangular grids is approximately 23% more than that of the hexagonal grids. Besides this difference, the simulation in this section will compare the path length of the different partitions.

Fig. 4. The path with the rectangular and hexagonal grids from (0, 30) to (0, 0).

As shown in Fig. 4 and Fig. 5, the location and distribution of obstacles are the same. In the rectangular grid environment, the underwater vehicle navigates to the target point through 34 rectangular grids in Fig. 4 and 22 rectangular grids in Fig. 5 respectively. However, in the hexagonal grid environment, the underwater vehicle’s path only needs to go through 33 hexagonal grids and 21 hexagonal grids respectively. Nevertheless, the path length of the rectangular grids are 38.97 and 23.48 m, while those of the hexagonal grids are 39.19 and 24.49 m. The GBNN model needs fewer neuron nodes on the hexagonal grid maps which reduces the computing amount for path planning, but its path is a little longer.

Comparison of GBNN Path Planning

529

Fig. 5. The path with the rectangular and hexagonal grids from (−10, 13) to (0, 0).

5.2 The Effect of Rectangular and Hexagonal Grids on Steering Control On the rectangular grid map, the underwater vehicle turns 7 times in Fig. 4 and 6 times in Fig. 5 respectively. However, the number of turns is 10 and 8 on the hexagonal grid maps. From the intuitive sense, it should be possible to produce less turning paths on hexagonal grid maps. However, the AUV needs less turning in the rectangular grid environment in the simulations. As shown in Fig. 6, hexagonal grid units of Fig. 5 are filled with the calculated neural activities. The grid unit labeled as ➊ is ought to have smaller activity values than that labeled as ➋. However, in the hexagonal grid environment, the unit ➊ is adjacent with two unit close to the target, which have 0.05 activities, while only one unit with 0.05 activities is near unit ➋. Consequently, grid unit ➊ that is near the obstacles possesses higher neural activities. It makes the AUV to immediately turn its direction after it goes through the obstacle, which forms the green path. Unfortunately, the green path in Fig. 6 requires three turns, while the best path colored in red only needs one turn. Moreover, there is an oscillation boundary in the hexagonal grid path planning as shown in Fig. 7. In the GBNN model, the activity of neurons is calculated by the weighted sum of adjacent neurons. While in the hexagonal grid environment, some neurons have two highly active neighboring neurons, and others have only one. This makes neurons with the same distance from the target have different neuronal activities. In Fig. 7, the red dashed line is the oscillation boundary. The green track is part of the path taken from Fig. 6. In fact, the left grid unit near the obstacle with 8.8e−25 neural activity is slightly larger than that of the right grid unit. However, the activity of neurons in the grid units crossed by the red dashed line was the highest and higher than all of their adjacent grids. This will cause the AUV to turn back and forth and waste a lot of energy. Simultaneously, the underwater vehicle usually turns at an angle of 45° in the rectangular grids, however, the turning angle reaches to 60°. It needs a bigger turning angle in the hexagonal grid map.

530

M. Chen et al.

Fig. 6. The neural activity around the target point after 50 iterations.

Fig. 7. Oscillation boundary of trajectory near the obstacle.

5.3 The Effect of Rectangular and Hexagonal Grids on Speed Control As for speed control, AUV always moves at a constant speed in the hexagonal grid. This is due √to the isotropy of the hexagonal grid. On the rectangular map, the path length is 1 or 2. The AUV’s movement requires variable speed control in the rectangular grid, and this complicates the control of the underwater vehicle. 5.4 Results of the Triangular Grid Division With the same environments in Sects. 5.1 and 5.2, path planning by the triangular partitions is also conducted. The results are shown in Fig. 8 and Fig. 9. The path lengths are 22.59, 38.91, and 57.69 m respectively. In the simulations, the path planning completed by the triangular grid seems to be better in the path length. AUV turns 6 times in left figure and 8 times right figure in Fig. 8 respectively. The triangular division is sometimes better than the rectangular grid in the turning control, but sometimes worse, and it is always better than the hexagonal grid in the turning control. Moreover, the turning angle of the triangular grid is 30°in the triangular grid, which is the smallest and the best. In the speed control, the path length of the triangular grid is changing in a computation interval. Therefore, the speed of the AUV needs to be varied too. In this aspect, it is the

Comparison of GBNN Path Planning

531

same as the rectangular grid but worse than the hexagonal grid. The triangular gird is also suitable in the hexagonal shaped obstacle environments as shown in Fig. 9.

Fig. 8. Path planning with the triangular grid.

Fig. 9. Path planning in the hexagonal shaped obstacle environment with the triangular grid.

In our intuitive imagination, the triangular mesh may be the worst, however, its performance is still desirable. For example, its turning angle is the smallest in the three different kinds of grids.

6 Conclusion Through the above analysis and simulations, the following conclusions can be drawn. (1) For the map with the same size, it turns out the number of hexagonal grids is about 23% less than that of rectangular grids. However, the path is longer due to more turning and bigger turning angle in the hexagonal grids. Although the triangular division requires more grids, its turning angle is the smallest.

532

M. Chen et al.

(2) Based on the GBNN model, the turning frequency of the underwater vehicles in the rectangular grid is lower than that in the hexagonal grid. In the hexagonal grid, there is a path oscillation boundary near the obstacle. In this aspect, the triangular grid is sometimes better than the rectangular grid, and sometimes worse. (3) In the aspect of speed control, hexagonal grid division is helpful for the underwater vehicles’ operation control. The path segments are of the same length. The AUV does not need speed changes, and energy consumption can be lower. Nevertheless, both the triangular and the rectangular grid needs speed changes.

Acknowledgements. This work was supported in part by the National Natural Science Foundation of China (52001195, U2006228, 61873161, 51575336, U1706224, 91748117), the National Key Project of Research and Development Program (2017YFC0306302), the Natural Science Foundation of Shanghai (19ZR1422600), Shanghai science and technology innovation action plan (18550720100) and Shanghai Science and Technology Innovation Funds (18550720100, 20dz1206700).

References 1. Keane, J.R., et al.: Autonomous underwater vehicle homing with a single range-only Beacon. IEEE J. Oceanic Eng. 45(2), 395–403 (2020) 2. Kulkarni, I.S., Pompili, D.: Task allocation for networked autonomous underwater vehicles in critical missions. IEEE J. Sel. Areas Commun. 28(5), 716–727 (2010) 3. Panda, M., Das, B., Subudhi, B., Pati, B.B.: A Comprehensive review of path planning algorithms for autonomous underwater vehicles. Int. J. Autom. Comput. 17(3), 321–352 (2019). https://doi.org/10.1007/s11633-019-1204-9 4. Zeng, Z., et al.: A survey on path planning for persistent autonomy of autonomous underwater vehicles. Ocean Eng. 110, 303–313 (2015) 5. Garcia, M., et al.: Dynamic graph-search algorithm for global path planning in presence of hazardous weather. J. Intell. Rob. Syst. 69, 285–295 (2013) 6. Qin, Z., et al.: A novel path planning methodology for automated valet parking based on directional graph search and geometry curve. Robot. Auton. Syst. 132, 1–12 (2020) 7. Soltani, A., et al.: Path planning in construction sites: performance evaluation of the Dijkstra, A*, and GA search algorithms. Adv. Eng. Inform. 16(4), 291–303 (2002) 8. Petillot, Y., et al.: Underwater vehicle obstacle avoidance and path planning using a multibeam forward looking sonar. IEEE J. Oceanic Eng. 26(2), 240–251 (2001) 9. Zelinsky, A.: A mobile robot exploration algorithm. IEEE Trans. Robot. Autom. 8(6), 707–717 (1993) 10. Feizollahi, A., Mayorga, R.V.: Optimized motion planning of manipulators in partially-known environment using modified D* Lite algorithm. WSEAS Trans. Syst. 16, 69–75 (2017) 11. Aitsaadi, N., et al.: Artificial potential field approach in WSN deployment: cost, QoM, connectivity, and lifetime constraints. Comput. Netw. 55(1), 84–105 (2011) 12. Chen, L., et al.: A fast and efficient double-tree RRT*-like sampling-based planner applying on mobile robotic systems. IEEE/ASME Trans. Mechatron. 23(6), 2568–2578 (2018) 13. Kaplan, A., et al.: Time-optimal path planning with power schedules for a solar-powered ground robot. IEEE Trans. Autom. Sci. Eng. 14(2), 1235–1244 (2017)

Comparison of GBNN Path Planning

533

14. Tewolde, G.S., Sheng, W.: Robot path integration in manufacturing processes: genetic algorithm versus ant colony optimization. IEEE Trans. Syst. Man Cybernet. Part A Syst. Hum. 38(2), 278–287 (2008) 15. Azzabi, A., Nouri, K.: An advanced potential field method proposed for mobile robot path planning. Trans. Inst. Meas. Control. 41(11), 3132–3144 (2019) 16. Cheng, C., et al.: A genetic algorithm-inspired UUV path planner based on dynamic programming. IEEE Trans. Syst. Man Cybernet. Part C (Appl. Rev.) 42(6), 1128–1134 (2012) 17. Sun, Z., et al.: Path planning for GEO-UAV bistatic SAR using constrained adaptive multiobjective differential evolution. IEEE Trans. Geosci. Remote Sens. 54(11), 6444–6457 (2016) 18. Song, Q., et al.: Dynamic path planning for unmanned vehicles based on fuzzy logic and improved ant colony optimization. IEEE Access 8, 62107–62115 (2020) 19. Koh, K.C., et al.: A neural network-based navigation system for mobile robots. In: Proceedings of 1994 IEEE International Conference on Neural Networks (ICNN’94), pp. 2709–2714, IEEE, Orlando (1994) 20. Panagiotopoulos, D.A., et al.: Planning with a functional neural network architecture. IEEE Trans. Neural Netw. 10(1), 115–127 (1999) 21. Glasius, R., et al.: A biologically inspired neural net for trajectory formation and obstacle avoidance. Biol. Cybern. 74(6), 511–520 (1996) 22. Yang, S.X., Meng, M.: Real-time collision-free path planning of a mobile robot using a neural dynamics-based approach. IEEE Trans. Neural Netw. 14(6), 1541–1552 (2003) 23. Chen, M., Zhu, D.: A workload balanced algorithm for task assignment and path planning of inhomogeneous autonomous underwater vehicle system. IEEE Trans. Cogn. Dev. Syst. 11(4), 483–493 (2019) 24. Yan, M., Zhu, D., Yang, S.X.: A novel 3-D bio-inspired neural network model for the path planning of an AUV in underwater environments. Intell. Autom. Soft Comput. 19(4), 555–566 (2013) 25. Chen, M., Zhu, D.: Optimal time-consuming path planning for autonomous underwater vehicles based on a dynamic neural network model in ocean current environments. IEEE Trans. Veh. Technol. 69(12), 14401–14412 (2020)

The Review of Image Processing Based on Graph Neural Network Jinji Nie1 , You Xu1(B) , Yichuan Huang2 , and Jiehao Li3 1

Guangdong Polytechnic Normal University, Guangzhou 51000, Guangdong, China [email protected] 2 School of Electric Power Engineering, South China University of Technology, Guangzhou 510640, China 3 State Key Laboratory of Intelligent Control and Decision of Complex Systems, Beijing Institute of Technology, Beijing 100081, China

Abstract. Convolutional neural networks have ushered in significant advancements in the field of image processing. Convolutional neural networks, on the other hand, operate well with European geographic data, whereas graph neural networks function better with non-European geographical data. This paper summarizes the application of image processing based on graph neural network. First, this article introduces the development history of graphs and graph neural networks and then explains the graphs’ concept and structure. Secondly, it focuses on the graph convolutional neural network, and details the graph convolutional neural network of spectrum and space. Finally, for graph convolutional neural network application in image processing, three processing methods are concluded.

Keywords: Image processing neural network

1

· Graph neural network · Convolutional

Introduction

The data used in traditional machine learning is European spatial data, which has a regular spatial structure. For example, pictures and voice, etc., can represent data by one-bit or two-dimensional matrix, carrying out efficient operations such as convolution. In recent years, deep learning has adopted a neural network end-to-end solution, which uses a European spatial structure. It has achieved great success in image processing and speech recognition. Some trajectory control methods [12,13] can get good results, but the application of neural network has got a different idea [14,17,28]. The neural network approximation technology [15,16,32] and the radial basis function neural network [31,35] are used to overcome the control problem of the uncertain environment. As a relational data structure, graphs have attracted attention in the research of deep learning. In real life, many data do not have a regular spatial structure, such as electronic transactions, biology, etc. The connection between each vertex in the graph and c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 534–544, 2021. https://doi.org/10.1007/978-3-030-89092-6_48

The Review of Image Processing Based on Graph Neural Network

535

other vertexes is not fixed. The graph can effectively and abstractly express information. It can capture the internal dependence of the data and model the data in non-Euclidean space. Graph Neural Network (GNN) [23] is a connectionist model for learning graphs containing many connections. Which research objects are Vertex and Edge. Unlike standard neural networks, GNN maintains a state that can represent information derived from artificially designated depths. Different graph structures have inconsistent data structure model research. Graph neural network is mainly divided into Graph Convolution Neural Network, Graph Attention Neural Network, Graph Auto-encoder, Graph Generative Neural Network and Graph Spatial-temporal Neural Network. Graph convolutional neural network (GCN) has become the most active and has received extensive attention in many fields. This article focuses on the application of graph convolutional neural networks in image processing. Graph convolutional neural network is mainly divided into two schools, which are spectral and spatial. Spectral graph convolutional neural network uses Fourier transform to map the vertexes to the frequency domain space, and realizes the convolution in the time domain by multiplying the frequency domain space [18]. Spatial graph convolutional neural network starts from the vertex and aggregates each vertex and its neighboring vertex by defining an aggregation function. After a fixed number of domain vertexes are sorted, they are multiplied and summed with the same number of convolution kernel parameters.

2 2.1

Related Work Graph Definition

The graph is a complex data structure with linear tables and numbers. The relationship between vertexes in the graph structure can be arbitrary. Any two data elements in the graph may be related, and the graph has a many-to-many relationship. The graph is composed of a finite non-empty set of vertices and a set of edges between vertices. Usually expressed as formula (1). G = (V, E)

(1)

where, G means graph, vi belongs to V is the set of vertices in G, eij = (vi , vj ) belongs to E is the set of edges in G. V is finite and not empty, but allow E to be empty. In order to better reflect the relationship between the high-latitude feature space and the graph, the formula (1) can be changed to formula (2). G = (V, E, A)

(2)

where, A is the adjacency matrix obtained by mapping to the highdimensional feature space. It is expressed as AN ∗N , where Aij = wij . wij is elements of the adjacency matrix.

536

2.2

J. Nie et al.

Graph Structure

Fig. 1. Comparison of four different graph structures

In the graph structure, the relationship between vertexes can be arbitrary. Any two data elements in the graph may be related, and the graph has a many-tomany relationship. The graph structure mainly includes an undirected graph, directed graph, weight graph, edge information graph and vertex heterogeneous graph. This article mainly introduces the last four graph structures, as shown in Fig. 1 and Table 1. Table 1. Comparison of four different graph structures Factor

Directed Graph Weight Graph

Edge Information Graph

Vertex Heterogeneous Graph

Application

Knowledge Graph

Advertising Recommendation

Sequence Diagram

Subtitling and Video Processing

Vertex and Edge

Relationship between vertexes

Edge relationship reflects vertex information

Complex edge relationship

Vertex grouping

Graph Attention

Graph Generative

Graph Convolution

Graph Neural Graph Network Model Convolution

3

Graph Neural Network

GNN appeared in the literature [23], extending the existing neural network to process the data shown in the graph domain. The learning goal of GNN is to obtain the hidden state of each vertex’s graph perception hv (state embedding). It means that for each vertex, its hidden state contains information from neighboring vertexes. GNN is implemented by iteratively updating the hidden state of all vertexes, at time t+1, the hidden state of vertex v is updated as formula (3):

The Review of Image Processing Based on Graph Neural Network

ht+1 = f (xv , xc o[v], htn e[v], xn e[v]) v

537

(3)

where f is the state update function of the hidden state which also called local transaction function. xv is the characteristics of vertex v, xc o[v] is the characteristics of the edges adjacent to vertex v, htn e[v] is the characteristics of Hidden state of neighbor vertexes at time t vertex v, xn e[v] is the characteristics of neighbor vertexes of vertex v. f is established for all vertexes and is a global shared function. It can be used to fit complex functions by a neural network. Formula (3) shows that the hidden state of the neighbor vertex at the current moment is used as part of the input to generate the hidden state of the central vertex at the next moment. Until the hidden state of each vertex changes very little, the information flow of the entire graph tends to be stable. The state update formula knowledge describes the vertex’s hidden state, and the function g is needed to describe how to adapt to downstream tasks. O = g(hv , xv )

(4)

g is a local output function, which can be expressed by a neural network and a globally shared function. The neural network’s basic theory is the fixed point theory, and Banach’s fixed point Theorem is commonly used. If multiple f are stacked as F, it is still a global update function, and the state update formula of all vertexes on the graph is (5): H t+1 = F (Hv , Xv )

(5)

On this basis, the loss function is used to optimize the gradient descent, the objective function is set as t, and the formula is (6): loss =

p 

(ti − oi )

(6)

i=1

Where p The number of samples applied in supervised learning. 3.1

Graph Convolutional Neural Network

The development of graph convolutional neural networks (GCN) is mainly divided into two categories: spectral and spatial. Spectral graph convolution maps vertexes to the frequency domain space through Fourier transform, realizes the convolution in the time domain by taking the product in the frequency domain space, and finally maps the product features to the time domain space. The spatial graph convolution starts from the vertex domain, and aggregates each central vertex and its neighboring vertexes by defining an aggregation function. The output of each layer of GCN neural network is H l+1 . As shown in formula (7): (7) H l+1 = f (H l , A)

538

J. Nie et al.

where A is the feature connection matrix. Perform normalization and activation function processing on A to achieve forward propagation. As shown in formula (8): D  − 12 H l W l )  − 12 A (8) f (H l , A) = σ(D     = A + I, I is identity matrix. D  = where A j Aij , A is diagonal matrix, l W is parameter matrix, σ is nonlinear activation function. The features of the graph can be extracted by formula (8). Next, the methods of different types of graph convolutional neural networks will be analyzed from the direction of spectral and spatial by specific examples of classic cases. Table 2 is the comparison of Graph convolutional neural models. 3.2

Spectral Graph Convolutional Network

Bruna J et al. [1] first proposed a spectral convolutional network in 2014. The method is to use the Laplace matrix to form the eigenvector matrix of the eigenvectors of the input matrix, diagonalize the parameters, and construct the Fourier transform matrix of the graph. Joan Bruna et al. [7] improved Spectral Network [4] in 2015 by introducing interpolation operations. In this way, the convolution kernel can be transformed and enlarged, and Hierarchical Graph Clustering is used for pooling operation. Defferrard M et al. [4] proposed the use of Chebyshev polynomials as the convolution kernel in 2016, which simplified the convolution by using the characteristics of the symmetric matrix. Use the polynomial of the Laplacian matrix as the convolution kernel, and use a complete binary tree for efficient pooling. Kipf T N et al. [10] simplified and proposed 1stChebnet in 2017. It proposed normalization, using the Chebyshev polynomial of the first-order approximation to replace the previous k-order Chebyshev polynomial. Li R et al. [19] proposed an adaptive graph convolutional network (AGCN) in 2018, which parameterized the Lavras matrix and divided it into an original part and an optimized part. These virtual vertex connections cannot be directly learned from the inherent graph. Zhang et al. [34] proposed in 2019 to use high-order graph convolution to obtain the structure of global clustering to define k-order graph convolution. K is to perform low-pass filtering on the vertex features to obtain a smooth feature representation and be adaptively selected through the intra-class distance. 3.3

Spatial Graph Convolutional Network

The spatial graph convolutional neural network is similar to the convolutional neural network convolution operation on the pixels of the image. It expresses the information transfer and aggregation between neighboring vertexes by calculating the convolution between vertexes and neighboring vertexes, as a new vertex representation of the feature domain.

The Review of Image Processing Based on Graph Neural Network

539

Table 2. Comparison of graph convolutional neural models Type

Specific method

Spectral SCN (2012) [1] GCN

Innovation

Insufficient

Use the Laplacian matrix to con- Large-scale data can not be struct the transformation matrix of used for training the graph

Chebnet (2016) [4] Insert value operation in convolution There are too many convoluformula tion kernel parameters 1stChebnet (2017) [10]

The Chebnet polynomial is used as Available on the shallow netthe convolution kernel work

AGCN (2018) [19] The Lavras matrix parameterization The computational complexis divided into the original part ity is higher

Spatial GCN

AGC (2019) [34]

Cluster structure to define the k- The setting of k value needs order graph convolution to choose a suitable value

SDGC (2020) [3]

Combines initial residuals and iden- After the network layer tity mapping reaches 256, the computing performance drops

GNN (2017) [6]

Random walk method to select neigh- It failed to get the effect bor vertexes of network experiments on large-scale data

GraphSAGE (2018) [5]

An inductive method is proposed to Network training time is long embed vertexes

GAT (2018) [25]

The attention mechanism is added to The edge information is not give weights to the edges between ver- fully utilized texes

PGC (2018) [30]

The specific sampling function is con- Network needs more training volved with the weighting function parameters

KGCN (2019) [27] Extending the non-spectral clustering Knowledge graph informaGCN method to the knowledge graph tion must be accurate GraphTransformer It combines Encoder and Attention to It is not suitable for large(2020) [2] form a Transformer scale graph training

Hechtlinger Y et al. [6] proposed a new GCN model in 2017. Compared with the traditional GNN [23], the second step is to perform inner product operations on the vertexes in the neighborhood and the convolution kernel parameters. Velikovi P et al. [25] also proposed the GAT method in 2018, using the attention mechanism to obtain the importance of vertexes. Through the importance of acquisition, a more reasonable vertex representation is obtained, which solves the problem of knowing the information of all vertexes in the previous GCN. Yan S et al. [30] proposed a partition graph convolution (PGC) method in 2018. The convolution method is to expand from regular data to graph structure data, where sample function and weight function are multiplied and then summed. Wang H et al. [27] proposed a KGCN method in 2019, a graph convolutional neural network based on a knowledge graph. Three types of aggregators are implemented in KGCN, namely Sum aggregator, Concat aggregator and Neighbor aggregator. Cai D et al. [2] proposed a Graph Transformer method in 2020, based on graph automatic compiler and multi-head attention mechanism to describe

540

J. Nie et al.

global dependencies. Compared with traditional GNN, it allows modeling the dependency between any two vertexes, regardless of their distance in the input graph.

4

Image Application

Graph convolutional neural network has been applied in text processing, image processing, a recommendation system and knowledge graph. Image classification benefits from big data and GPU’s powerful computing functions, which can train classifiers without extracting structural information. Most neural networks can achieve similar classification effects. Table 3 is the application of graph convolutional neural network in image processing. Image Classification. In [20] proposed a new superpixel-wise attention GCN(AGCN) for SAR (Synthetic Aperture Radar) image segmentation. After CNN obtains the feature vector of a superpixel, GCN is introduced to operate the graph structure data and applied to vertex classification. The attention layer mechanism is introduced to guide the convolutional graph layer to focus on the most relevant vertexes. The knowledge graph can also be better used for the classification of unknown tags. In [11], a deep learning architecture for multi-label zero-sample learning (ML-ZSL) was developed, which applied structured labels and knowledge graphs. The word embedding of the vertex is used as input to predict the classifiers of different categories. In addition to processing knowledge graphs, the combination of dictionaries and images can also obtain classification results. In [33], the scene graph method is used to extract the image’s object features. Use a pre-trained dictionary D for the properties of objects and the relationship between objects. In [8], it conducts in-depth research on CNNs and GCNs from the perspective of hyperspectral image classification. Aiming at many GCN calculations, it proposes a new small-batch GCN (MiniGCN). MiniGCN can infer out-of-sample data without retraining the network and improving classification performance. Visual Reasoning. Computer visual reasoning is also the main research object. In [21], it is proposed that the LSTM (Long Short Term Memory Network) model is used to carry out factual reasoning and predict the answer in the given image and question. Graph convolution and multilayer perceptrons are used to generate the answers. Such a method will generate much noise. In [22], a deep neural network combining spatial, image and text features was developed to answer a question about images. It uses word embedding and RNN (Recurrent Neural Network) to calculate the expression of the problem and a set of object descriptions containing bounding box coordinates and image feature vectors. It learns an adjacency matrix based on a given problem. The text generates images, which also gets people’s attention. Compared with the traditional text description method of generating images, [9] proposed an end-to-end method to generate images from scene graphs. We are generating

The Review of Image Processing Based on Graph Neural Network

541

images from structured scene graphs rather than the unstructured text allows for explicit reasoning of objects and relationships. In [36], it proposes a graph convolutional neural network based on street view and map labels to promote geographic information extraction. It integrates three different but related information in the form of graphs to achieve explicit map modeling and infer the notes and scenes of unknown areas. Semantic Segmentation. Semantic Segmentation can better understand the image’s content, but the segmented image is generally not regular. Therefore, graph convolutional neural networks can be better applied and processed by using structured graph data. In [26], a Class-wise Dynamic Graph Convolutional Network (CDGCNet) is proposed, which is a range of contextual relevance and aggregates valuable information for better pixel label prediction. It performs simple semantic segmentation through the primary network. The CDGC module takes the rough prediction graph and feature graph of the primary network as input, converts the prediction graph into a class mask, and extracts vertex features from different graph construction classes. In [29] Dynamic Graph CNN for Learning on Point Clouds (DGCNN), based on PointNet and PointNet++, a network for classification and segmentation of Point Clouds data is proposed. It integrates EdgeConv into the basic version of PointNet through DGCNN without any feature transformation. DGCNN digs out the local geometric structure by constructing a local adjacency graph and applying convolution operations to each adjacent edge. Besides, in [24] a general semantic segmentation framework is proposed, integrating deep structured feature embedding and graph convolutional neural networks into the end-to-end workflow. It proposes a new network architecture Table 3. Application of graph convolutional neural network in image processing Application

Algorithm

Network structure

Literature

Image Classification

AGCN

Graph Graph Graph Graph Graph

[20]

GCN SGCN MiniGCN

Convolution Neural Network Attention Neural Network Convolution Neural Network Convolution Neural Network Convolution Neural Network

[11] [33] [8]

Visual Reasoning

GCN

Graph Convolution Neural Network

[21]

GCNN SGCN GCN

Graph Convolution Neural Network Graph Convolution Neural Network Graph Convolution Neural Network

[22] [9] [36]

Semantic Segmentation

CDGCNet

Graph Convolution Neural Network

[26]

GGCN Graph Convolution Neural Network Graph-CNN Graph Convolution Neural Network

[24] [29]

542

J. Nie et al.

called “Gated Graph Convolutional Neural Network,” which combines RNN and GRU for long-distance information dissemination and combines GCN for short-distance information dissemination.

5

Conclusion

The graph convolutional neural network is adequate for processing graph data in non-Euclidean space. The first part of this article introduces the development history of graphs and graph neural networks, and the second part describes the concept of graphs. The third part illustrates the concept of convolutional neural networks. The fourth part focuses on applying graph convolutional neural network in image processing. According to the development of neural graph networks at this stage, improving the stability of the adaptive capability of network topology, and improving the data utilization sufficiency of unstructured scenes are future research directions. Graph convolutional neural networks will play an essential role in the development of deep learning. Acknowledgement. This work was supported in part by the Guangdong Key Laboratory of Intelligent Transportation System under Grant 202005004, the Natural Science Foundation of Guangdong Province under Grant 2018A030313753 and the National Natural Science Foundation of China under Grant 62073090 and 61473331.

References 1. Bruna, J., Zaremba, W., Szlam, A., LeCun, Y.: Spectral networks and locally connected networks on graphs. arXiv preprint arXiv:1312.6203 (2013) 2. Cai, D., Lam, W.: Graph transformer for graph-to-sequence learning. In: Proceedings of the AAAI Conference on Artificial Intelligence, vol. 34, pp. 7464–7471 (2020) 3. Chen, M., Wei, Z., Huang, Z., Ding, B., Li, Y.: Simple and deep graph convolutional networks. In: International Conference on Machine Learning, pp. 1725–1735. PMLR (2020) 4. Defferrard, M., Bresson, X., Vandergheynst, P.: Convolutional neural networks on graphs with fast localized spectral filtering. arXiv preprint arXiv:1606.09375 (2016) 5. Hamilton, W.L., Ying, R., Leskovec, J.: Inductive representation learning on large graphs. arXiv preprint arXiv:1706.02216 (2017) 6. Hechtlinger, Y., Chakravarti, P., Qin, J.: A generalization of convolutional neural networks to graph-structured data. arXiv preprint arXiv:1704.08165 (2017) 7. Henaff, M., Bruna, J., LeCun, Y.: Deep convolutional networks on graph-structured data. arXiv preprint arXiv:1506.05163 (2015) 8. Hong, D., Gao, L., Yao, J., Zhang, B., Plaza, A., Chanussot, J.: Graph convolutional networks for hyperspectral image classification. IEEE Trans. Geosci. Remote Sens. (2020) 9. Johnson, J., Gupta, A., Fei-Fei, L.: Image generation from scene graphs. In: Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, pp. 1219–1228 (2018)

The Review of Image Processing Based on Graph Neural Network

543

10. Kipf, T.N., Welling, M.: Semi-supervised classification with graph convolutional networks. arXiv preprint arXiv:1609.02907 (2016) 11. Lee, C.-W., Fang, W., Yeh, C.-K., Wang, Y.-C.F.: Multi-label zero-shot learning with structured knowledge graphs. In: Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, pp. 1576–1585 (2018) 12. Li, J., Qin, H., Wang, J., Li, J.: Openstreetmap-based autonomous navigation for the four wheel-legged robot via 3D-lidar and CCD camera. IEEE Trans. Ind. Electron. (2021) 13. Li, J., Wang, J., Peng, H., Hu, Y., Su, H.: Fuzzy-torque approximation enhanced sliding mode control for lateral stability of mobile robot. IEEE Trans. Syst. Man Cybern. Syst. (2021) 14. Li, J., Wang, J., Peng, H., Zhang, L., Hu, Y., Su, H.: Neural fuzzy approximation enhanced autonomous tracking control of the wheel-legged robot under uncertain physical interaction. Neurocomputing 410, 342–353 (2020) 15. Li, J., et al.: Parallel structure of six wheel-legged robot trajectory tracking control with heavy payload under uncertain physical interaction. Assembly Autom. 40(5), 675–687 (2020) 16. Li, J., Wang, S., Wang, J., Li, J., Zhao, J., Ma, L.: Iterative learning control for a distributed cloud robot with payload delivery. Assembly Autom. 41(3), 263–273 (2021) 17. Li, J., Wu, Q., Wang, J., Li, J.: Neural networks-based sliding mode tracking control for the four wheel-legged robot under uncertain interaction. Int. J. Robust Nonlinear Control 31(9), 1–18 (2021) 18. Li, J., Zhang, X., Li, J., Liu, Y., Wang, J.: Building and optimization of 3d semantic map based on lidar and camera fusion. Neurocomputing 409, 394–407 (2020) 19. Li, R., Wang, S., Zhu, F., Huang, J.: Adaptive graph convolutional neural networks. In: Proceedings of the AAAI Conference on Artificial Intelligence, vol. 32 (2018) 20. Ma, F., Gao, F., Sun, J., Zhou, H., Hussain, A.: Attention graph convolution network for image segmentation in big SAR imagery data. Remote Sens. 11(21), 2586 (2019) 21. Narasimhan, M., Lazebnik, S., Schwing, A.G.: Out of the box: Reasoning with graph convolution nets for factual visual question answering. arXiv preprint arXiv:1811.00538 (2018) 22. Norcliffe-Brown, W., Vafeias, E., Parisot, S.: Learning conditioned graph structures for interpretable visual question answering. arXiv preprint arXiv:1806.07243 (2018) 23. Scarselli, F., Gori, M., Tsoi, A.C., Hagenbuchner, M., Monfardini, G.: The graph neural network model. IEEE Trans. Neural Networks 20(1), 61–80 (2008) 24. Shi, Y., Li, Q., Zhu, X.X.: Building segmentation through a gated graph convolutional neural network with deep structured feature embedding. ISPRS J. Photogrammetry Remote Sens. 159, 184–197 (2020) 25. Veliˇckovi´c, P., Cucurull, G., Casanova, A., Romero, A., Lio, P., Bengio, Y.: Graph attention networks. arXiv preprint arXiv:1710.10903 (2017) 26. Wang, C., Samari, B., Siddiqi, K.: Local spectral graph convolution for point set feature learning. In: Ferrari, V., Hebert, M., Sminchisescu, C., Weiss, Y. (eds.) ECCV 2018. LNCS, vol. 11208, pp. 52–66. Springer, Cham (2018). https://doi. org/10.1007/978-3-030-01225-0 4 27. Wang, M., Zhao, H., Xie, X., Li, W., Guo, M.: Knowledge graph convolutional networks for recommender systems. In: The World Wide Web Conference, pp. 3307–3313 (2019)

544

J. Nie et al.

28. Wang, S., Chen, Z., Li, J., Wang, J., Li, J., Zhao, J.: Flexible motion framework of the six wheel-legged robot: experimental results. IEEE/ASME Trans. Mechatron. 1–9 (2021) 29. Wang, Y., Sun, Y., Liu, Z., Sarma, S.E., Bronstein, M.M., Solomon, J.M.: Dynamic graph CNN for learning on point clouds. ACM Trans. Graph. (TOG) 38(5), 1–12 (2019) 30. Yan, S., Xiong, Y., Lin, D.: Spatial temporal graph convolutional networks for skeleton-based action recognition. In: Proceedings of the AAAI Conference on Artificial Intelligence, vol. 32 (2018) 31. Yang, C., Chen, C., He, W., Cui, R., Li, Z.: Robot learning system based on adaptive neural control and dynamic movement primitives. IEEE Trans. Neural Networks Learn. Syst. 30(3), 777–787 (2018) 32. Yang, C., Jiang, Y., Li, Z., He, W., Su, C.-Y.: Neural control of bimanual robots with guaranteed global stability and motion precision. IEEE Trans. Ind. Inform. 13(3), 1162–1171 (2016) 33. Yang, X., Tang, K., Zhang, H., Cai, J.: Auto-encoding scene graphs for image captioning. In: Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, pp. 10685–10694 (2019) 34. Zhang, X., Liu, H., Li, Q., Wu, X.-M.: Attributed graph clustering via adaptive graph convolution. arXiv preprint arXiv:1906.01210 (2019) 35. Zhang, Z., Chen, T., Wang, M., Zheng, L.: An exponential-type anti-noise varyinggain network for solving disturbed time-varying inversion systems. IEEE Trans. Neural Networks Learn. Syst. 31(9), 3414–3427 (2020) 36. Zhu, D., et al.: Understanding place characteristics in geographic contexts through graph convolutional neural networks. Ann. Am. Assoc. Geographers 110(2), 408– 420 (2020)

Artificial Intelligence in Education: Origin, Development and Rise Ruixing Ye1 , Fuhai Sun1(B) , and Jiehao Li2 1

2

The Lab for NeuroScience and Artificial Intelligence in Moral Learning, South China Normal University, Guangzhou 510631, China [email protected] State Key Laboratory of Intelligent Control and Decision of Complex Systems, Beijing Institute of Technology, Beijing 100081, China

Abstract. In recent years, artificial intelligence in education has grown in popularity. When it comes to a new trend for primary school, how to use the AI teacher for course teaching is the main challenge. In this paper, a review of artificial intelligence in education under origin, development and rise is considered. Firstly, we use the method of qualitative research to investigate the problem. Then, the development process and challenges for AI education are discussed in detail. Finally, the rise of artificial intelligence in education is summarized to promote the sustainable development of educational AI teachers. Keywords: Artificial Intelligence in Education (AIED) Education science

1

· AI teacher ·

Introduction

The integration of artificial intelligence into the field of education science has sparked a revolution of Artificial Intelligence in Education (AIED), thereby giving birth to the course discipline in this area [7]. For example, artificial intelligence technology are applied to a large number of fields [14,15,19,26]. This academic discipline generated by integrating multidisciplinary knowledge, such as pedagogy, neuroscience, psychology, and artificial intelligence, is a new form of transcendental discipline. It constructs a trans-disciplinary paradigm whose purpose is to solve New Common Issues created by cross-integration with neuroscience, psychology and artificial intelligence. It explores the cultivation of trans-disciplinary talents, builds a versatile curriculum system, and develops AI teachers with morality, etc. AIED pays attention to the teacher ability in the intelligent teaching. Also, it attaches importance to promoting innovation and upgrading primary education and cultivating advanced specialists that are urgently needed domestically and internationally. At the same time, various countries hold the international conference to point the way [3]. In addition, the Chinese government and UNESCO adopted the document Beijing Consensus and promoted the systematic integration of artificial intelligence and education [27]. c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 545–553, 2021. https://doi.org/10.1007/978-3-030-89092-6_49

546

R. Ye et al.

Fig. 1. Artificial intelligence in education under the primary school course.

The national strategy of the integrated development has given a strong impetus to the birth of the educational artificial intelligence discipline. The establishment and improvement of its disciplinary system have also created an urgent need. In a word, it is time to AIED.

2

Origin of Artificial Intelligence in Education

The concept of artificial intelligence (AI) was first proposed at the Dartmouth conference in 1956, discussing how to make computers simulate human intelligence [4]. Before the formal introduction of AI, psychology and pedagogy theory in the field of education had long been developed into a discipline system and combined with corresponding machines to form teaching equipment. This is also the earliest case of combining science and technology to promote the development of education [13,25]. In recent years, AIED has been integrated into the actual teaching classroom. As shown in Fig. 1, intelligent robots can assist teachers in English Teaching at primary schools. This is a new dual-teacher classroom teaching method in primary schools, in which AI teachers and human teachers work together to be a good partners. AI teacher is a new form of classroom presentation, which can be competent for teaching any subject in theory. Therefore, it can achieve the teaching course more standardization.

3

The Development of Artificial Intelligence in Education

The policies of various countries support the development of educational artificial intelligence in order to keep abreast of the times. However, in the initial stage, AIED was presented under pedagogical psychology principles and machine

Artificial Intelligence in Education: Origin, Development and Rise

547

Fig. 2. Five steps for the evolution of artificial intelligence in education.

design guidance. In the early 20th century, the development of educational psychology promoted the birth of the Teaching Machine. At the same time, psychologists and educators represented by Skinner began to explore how to give automated and intelligent methods to machines based on the principles of educational psychology and apply them to teaching. Based on the above, they could be liberating the majority of teachers from repetitive labor. It can result in the learning quality and improve the efficiency of teaching. After a long history of development, educational artificial intelligence has gradually evolved from the teaching machine stage to the intelligent adaptation learning stage. From the perspective of historical evolution and technological development, the evolution of educational artificial intelligence is divided into the following five stages, as shown in Fig. 2.

548

3.1

R. Ye et al.

The Teaching Machine Stage of a Mechanical Device

At the end of the nineteenth century, it was an era of prevalence in educational psychology. This is an independent discipline developed by the book Educational Psychology, which are the psychological phenomena and laws of learning and teaching in the educational process [6]. On the basis of this theoretical study, Sidney Pressey produced the first teaching machine to perform tests and scoring tasks automatically in 1925. The first teaching machine includes four parts: input, output, storage and control. It can test and score at its own pace. In 1930, J. Petersong designed a device for automatic scoring and immediate feedback, which was later called the Chemo-Card. Although this chemistry board and Plessy’s teaching machine have stimulated interest in automated teaching technology, most educators and researchers are not prepared to accept this advanced instrument for teaching. In the early 1960s, Skinner designed mechanical devices that support Programmed Instruction (PI) based on behaviorist theory. It can realize differentiated and individualized teaching through knowledge projects. These early results urged the germination of artificial intelligence education applications [5]. After the 1960s, the concept of teaching machines included various mechanical equipment and audio vision used for teaching. Teachers use physical objects to communicate and interact with children in class [20]. Generally speaking, the early teaching machines were manufactured based on mechanical principles, which were not ideal, but the related theories are the basis of future computer-assisted teaching. The development of computer education provides a direct source of ideas. Modern teaching software and hardware equipment, as well as the development of educational intelligent tutoring systems [18,23] play a key role. 3.2

The Stage of Computer Assisted Instruction

In the late 1950s, rapid economic development promoted the advancement of computer science and technology. The programming language broke away from the low level towards the high-level language era represented by FORTRAN language. Computer-Assisted Instruction (CAI) is a teaching method that uses computers to help or replace teachers to perform some teaching tasks and provides skill training. The online summary evaluation and feedback system can better immerse students in the simulated learning experience and increase the corresponding knowledge [10,24,29]. Computer programming languages mainly include FORTRAN, ALGOL, BASIC and C language. An intelligent tuition system allows students to communicate internationally [8]. Researchers are trying to develop an assistance teaching system based on the gradually mature programming language [11,30]. The seeking skill is improved by the meta-cognitive feedback in the intelligent tutoring system [21]. The psychologist Simon HA defined the concept of machine learning as simulating the learning line to reorganize the existing knowledge structure. The participation model [28] was presented to solve human problems in computer simulations. These are applied to teaching,

Artificial Intelligence in Education: Origin, Development and Rise

549

learning and assessment [22], such as cognitive diagnostic tests based on the creation of computer-aided instructions. However, it was limited by the computer technology at that time, and the teaching information in the computer-assisted instruction was provided to students following the teaching process preset by the educational researchers. So it will be replaced soon by the newly developed intelligent computer-assisted instruction. 3.3

The Stage of Intelligent Computer Assisted Instruction

Intelligent Tutoring System (ITS) technologies, such as knowledge bases and inference machines, were presented from the late 1970s to the 1980s. On the basis of CAI, the Intelligent Computer Assisted Instruction (ICAI) was gradually developed. The ICAI system allows the computer to play the role of a teacher, using the student model to dynamically generate content and strategies suitable for individualized teaching. A diagnostic mechanism is used to judge the learning level of the students. Furthermore, it can provide guidance to learners with different needs and characteristics under the learning situation, so as to achieve individualized teaching style. ICAI provides the effective tool to promote cognition and learning. However, the interactive interface of these systems only has text and images, and is limited by the teaching as the center. 3.4

The Stage of Personalized Adaptive Learning

With the development of the Internet, the adaptive learning system began to appear in the early 1990s. It originated from the smart tutoring system in the United States [17]. Brusilovsky defined it as collect practical information of students for analysis and developed a personalized user model of learning ability and cognitive level. At that time, intelligence was a simple stratification of students according to the level of learning, dividing students into good, medium, poor categories. So each type of student matched the learning content and path. This is a very rough personalized education whose concept is similar to class division. The adaptive learning system can adapt to learners’ needs, styles, cognitive levels and learning conditions and achieve personalized or differentiated learning. Then build a connection between students’ cognition and the results of active learning, as well as adjust the learning model[2]. A typical adaptive learning system includes four parts: domain model, user model, teaching model and adaptive engine. The main technologies are applied to intelligent agents [9], natural language understanding, and neural networks. The representative systems include Stat Lady (Shute, 1993), SQL-Tutor (Mitrovic, 1996), Auto-Tutor (Graesser, 1997), etc. The learning theory dominated by the intelligent teaching system during this period was constructivism, emphasizing learners’ initiative and following the learner-centered teaching philosophy. It pays more attention to learning needs and personality characteristics. However, with the continuous progress of society, people’s expectations for intelligent learning requirements have increased, and adaptive learning faces challenges.

550

3.5

R. Ye et al.

The Stage of Personalized Intellectual Adaptation Learning

In recent years, with the continuous maturity of technologies such as cloud computing, educational big data, and deep learning, coupled with the support of national policy guidelines, AIED has entered an unprecedented period of rapid development [12]. On the issue of AI education, Bill Gates has a relevant description. During this time, based on the support of advanced high-speed network technology, it has become possible to collect, store, calculate, analyze, and largescale educational data. The intelligent adaptive learning system collects input information and learning behavior data provided by learners. According to the students acquirement, a dynamic protocol method is created [1], which can better customize learning content for students. Support dynamic learning services in terms of resources, methods or content, so as to create a personalized learning experience for students. For example, by studying a large number of writing samples, the algorithm program can assign accurate scores to the articles written by the students. It is not enough to refine and form the materials and then push them to the students. At this stage, the biggest advantage of intelligent adaptive learning is that it can combine with the development level of modern artificial intelligence technology to realize the in-depth simulation of expert teachers and accurately locate learners’ knowledge weaknesses. A fast knowledge sequence path [16] helps them improve their academic performance, which is the great significance to help learners improve effectiveness. With the development of science and technology, AIED has been popular in the education field, from the teaching machine to intelligent adaptive learning based on big data and deep learning. Challenges and opportunities for sustainable development report that the digital education market will grow annually by 5% before 2021. Although the development of public policies for AIED is still in its infancy, this field is likely to grow exponentially in the next ten years. Therefore, educational artificial intelligence at the stage of intellectual adaptation learning has unlimited development prospects.

4

The Rise of Educational Artificial Intelligence

In the 21st century, artificial intelligence for social development has quickly penetrated into all walks of life. It is a new trend to promote social development. Education is the foundation for cultivating productive forces and can create scientific knowledge and material wealth for the country and society. Figure 3 exhibits the policies of artificial intelligence in education. Firstly, the Industry 4.0 Plan by the German government was presented, which is the inducement for AIED. Furthermore, the USA government proposed the national strategic plan for artificial intelligence. At the same time, the Japanese government planed the industrialization route of artificial intelligence. For educational modernization, the State Council developed the China 2035 Plan. Therefore, AIED has become a hot issue in various countries. In this atmosphere, artificial intelligence is affecting reform and innovation at an unpredictable speed.

Artificial Intelligence in Education: Origin, Development and Rise

551

Fig. 3. The policies of artificial intelligence in education.

5

Conclusion

In this article, the analysis of artificial intelligence in education for origin, development and rise is considered. It is a new trend for primary school teachers via AI technology to the course teaching. Firstly, qualitative research is utilized in the AIED. Furthermore, the development process and challenges for AI education are described in detail. Finally, the rise of artificial intelligence in education and the corresponding policy are discussed. This research can promote the sustainable development of educational AI teachers in primary school cases.

References 1. Adamson, D., Dyke, G., Jang, H., Ros´e, C.P.: Towards an agile approach to adapting dynamic collaboration support to student needs. Int. J. Artif. Intell. Educ. 24(1), 92–124 (2014). https://doi.org/10.1007/s40593-013-0012-6 2. Chi, M.T., Wylie, R.: The ICAP framework: linking cognitive engagement to active learning outcomes. Educ. Psychol. 49(4), 219–243 (2014) 3. Cumming, G.: Artificial intelligence in education: an exploration. J. Comput. Assist. Learn. 14(4), 251–259 (2008) 4. Dotter, A., Chaboyer, B., Jevremovi, D., Kostov, V., Ferguson, A.: The Dartmouth stellar evolution database. Astrophys. J. Suppl. Ser. 178(1), 89 (2008) 5. Driscoll, M.: Psychology of learning for instruction. Educ. Tech. Res. Dev. 53(1), 108–110 (2005). https://doi.org/10.1007/BF02504860 6. Good, T.L., Brophy, J.E.: Contemporary Educational Psychology. Longman/Addison Wesley Longman (1995) 7. Holmes, W., Bialik, M., Fadel, C.: Artificial intelligence in education. Center for Curriculum Redesign, Boston (2019)

552

R. Ye et al.

8. Khachatryan, G.A., et al.: Reasoning mind genie 2: an intelligent tutoring system as a vehicle for international transfer of instructional methods in mathematics. Int. J. Artif. Intell. Educ. 24(3), 333–382 (2014). https://doi.org/10.1007/s40593-0140019-7 9. Leelawong, K., Biswas, G.: Designing learning by teaching agents: the Betty’s Brain system. Int. J. Artif. Intell. Educ. 18(3), 181–208 (2008) 10. Lenat, D.B., Durlach, P.J.: Reinforcing math knowledge by immersing students in a simulated learning-by-teaching experience. Int. J. Artif. Intell. Educ. 24(3), 216–250 (2014). https://doi.org/10.1007/s40593-014-0016-x 11. Li, J., Qin, H., Wang, J., Li, J.: OpenStreetMap-based autonomous navigation for the four wheel-legged robot via 3D-lidar and CCD camera. IEEE Trans. Ind. Electron. (2021) 12. Li, J., Wang, J., Peng, H., Hu, Y., Su, H.: Fuzzy-torque approximation enhanced sliding mode control for lateral stability of mobile robot. IEEE Trans. Syst. Man Cybern. Syst. (2021) 13. Li, J., Wang, J., Peng, H., Zhang, L., Hu, Y., Su, H.: Neural fuzzy approximation enhanced autonomous tracking control of the wheel-legged robot under uncertain physical interaction. Neurocomputing 410, 342–353 (2020) 14. Li, J., et al.: Parallel structure of six wheel-legged robot trajectory tracking control with heavy payload under uncertain physical interaction. Assem. Autom. 40(5), 675–687 (2020) 15. Li, J., Zhang, X., Li, J., Liu, Y., Wang, J.: Building and optimization of 3D semantic map based on lidar and camera fusion. Neurocomputing 409, 394–407 (2020) 16. Li, X., Zheng, L.: Smart media use and its impact on after-school reading among primary and middle school students. China Educ. Technol. 12(1), 88–95 (2018) 17. Ma, W., Adesope, O.O., Nesbit, J.C., Liu, Q.: Intelligent tutoring systems and learning outcomes: a meta-analysis. J. Educ. Psychol. 106(4), 901 (2014) 18. Nesbit, J., Adesope, O., Liu, Q., Ma, W.: How effective are intelligent tutoring systems in computer science education? In: IEEE International Conference on Advanced Learning Technologies, pp. 99–103. IEEE (2014) 19. Peng, G., Yang, C., He, W., Chen, C.P.: Force sensorless admittance control with neural learning for robots with actuator saturation. IEEE Trans. Ind. Electron. 67(4), 3138–3148 (2019) 20. Robertson, J., Cross, B., Macleod, H., Wiemer-Hastings, P.: Children’s interactions with animated agents in an intelligent tutoring system. Int. J. Artif. Intell. Educ. 14(3), 335–357 (2004) 21. Roll, I., Aleven, V., McLaren, B.M., Koedinger, K.R.: Improving students’ helpseeking skills using metacognitive feedback in an intelligent tutoring system. Learn. Instr. 21(2), 267–280 (2011) 22. Shute, V.J.: Stealth assessment in computer-based games to support learning. Comput. Games Instr. 55(2), 503–524 (2011) 23. Steenbergen-Hu, S., Cooper, H.: A meta-analysis of the effectiveness of intelligent tutoring systems on college students’ academic learning. J. Educ. Psychol. 106(2), 331 (2014) 24. Sung, Y., Liao, C., Chang, T., Chen, C., Chang, K.: The effect of online summary assessment and feedback system on the summary writing on 6th graders: the LSAbased technique. Comput. Educ. 95, 1–18 (2016) 25. Yang, C., Chen, C., He, W., Cui, R., Li, Z.: Robot learning system based on adaptive neural control and dynamic movement primitives. IEEE Trans. Neural Netw. Learn. Syst. 30(3), 777–787 (2018)

Artificial Intelligence in Education: Origin, Development and Rise

553

26. Yang, C., Jiang, Y., Li, Z., He, W., Su, C.-Y.: Neural control of bimanual robots with guaranteed global stability and motion precision. IEEE Trans. Ind. Inf. 13(3), 1162–1171 (2016) 27. Yang, C., Luo, J., Liu, C., Li, M., Dai, S.-L.: Haptics electromyography perception and learning enhanced intelligence for teleoperated robot. IEEE Trans. Autom. Sci. Eng. 16(4), 1512–1521 (2018) 28. Yoo, J., Kim, J.: Can online discussion participation predict group project performance? Investigating the roles of linguistic features and participation patterns. Int. J. Artif. Intell. Educ. 24(1), 8–32 (2014). https://doi.org/10.1007/s40593-0130010-8 29. Zhang, Z., Chen, T., Wang, M., Zheng, L.: An exponential-type anti-noise varyinggain network for solving disturbed time-varying inversion systems. IEEE Trans. Neural Netw. Learn. Syst. 31(9), 3414–3427 (2020) 30. Zhang, Z., Yan, Z.: An adaptive fuzzy recurrent neural network for solving the nonrepetitive motion problem of redundant robot manipulators. IEEE Trans. Fuzzy Syst. 28(4), 684–691 (2020)

A Path Planning Algorithm for Mobile Robots Based on DGABI-RRT Qingdang Li1,2

, Hui Zhao1

, Mingyue Zhang2(B)

, and Zhen Sun3

1 College of Automation and Electronic Engineering, Qingdao University of Science and

Technology, Qingdao 266061, China [email protected] 2 College of Sino-German Science and Technology, Qingdao University of Science and Technology, Qingdao 266061, China 3 College of Information Science and Technology, Qingdao University of Science and Technology, Qingdao 266061, China [email protected]

Abstract. As we know, mobile robots have been widely used in daily life, and path planning has also become a hot topic for many scholars. Due to excellent performance of the fast search speed and independent of the environment model, Rapidly-exploring Random Tree (RRT) algorithm is used broadly in mobile robots’ path planning. However, it also has some problems, the randomness is very important among all the problems, because it makes the planned path deviate from the optimal path. To solve the problems above, this paper presents a double sampling points and gravitational adaptive step size bidirectional RRT (DGABI-RRT) algorithm for mobile robots. In this algorithm, better sampling point strategy and gravity adaptive step size strategy are used to make the mobile robot’s motion have goal bias and avoid obstacles. Besides, to get a better path, path optimization strategies are used to shorten and smooth paths so that the mobile robot can move smoothly in the workspace. To verify the performance of the proposed algorithm, simulation compared with other three algorithms is carried out in this paper. The numerical comparison of simulation results proves that the DGABI-RRT algorithm is better than the other three improved algorithms in terms of path planning time, number of expansion nodes, number of path points and path length. The simulation results in two working environments of mobile robots show that the improved algorithm can better solve the path planning and obstacle avoidance problem of the mobile robots. Keywords: DGABI-RRT · Mobile robots · Path planning · Gravity adaptive step size strategy · Path optimization

1 Introduction Mobile robots’ path planning has aroused the research enthusiasm of many researchers. The ultimate goal of the mobile robot’s path planning is to reach the goal point fast and avoid obstacles accurately [1]. To achieve this purpose, many scholars have proposed some related algorithms to implement the path planning of mobile robots. © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 554–564, 2021. https://doi.org/10.1007/978-3-030-89092-6_50

A Path Planning Algorithm for Mobile Robots Based on DGABI-RRT

555

Common path planning algorithms include artificial potential field method [2], genetic algorithm, ant colony algorithm, etc. These algorithms have good performance in relatively simple environments, but they do not perform well in relatively complex environments. For example, artificial potential field is easy to fall into local minimum [3]. The convergence path quality of genetic algorithm is poor [4], and ant colony algorithm’s search efficiency is slow [5]. RRT algorithm [6] has unique advantages of high success rate and faster search speed. However, due to random sampling, RRT’s path is poor. Therefore, some scholars have proposed some improved RRT algorithms. Zhen Zhang et al. [7] introduced the target gravity and adaptive dynamic step size into RRT algorithm, and improved the applicability of the algorithm. Haoyue Liu et al. [8] proposed a goal biased bidirectional RRT algorithm to speed up the search speed. Mingliang Yan et al. [9] proposed a bidirectional RRT path planning algorithm based on double sampling points, which reduced the randomness of sampling points. But the path obtained by these algorithms still contains many redundant nodes, so the path needs to be further optimized. Dijkstra algorithm can be used to shorten the path, for example, Huang Xin et al. [10] used Dijkstra algorithm to find the shortest path planned by bidirectional RRT algorithm (BI-RRT), which shortens the path length greatly. Besides, some scholars use B-spline curve [11] to smooth the sharp corners of the curve. Shoichi Tsuchie et al. [12] proposed a new method to fit a high-quality plane curve using a quadratic B-spline curve with continuous curvature, and got a better result. Based on the problems of these RRT algorithms’ strong randomness and poor path, this paper presents a double sampling points and gravitational adaptive step size bidirectional RRT (DGABI-RRT) algorithm. It is based on BI-RRT, using double sampling points comparison and selection strategy to optimize the sampling points. And the gravity adaptive step size strategy is used to increase the goal bias and achieve accurate obstacle avoidance. A set of simulations is carried out to verify DGABI-RRT algorithm’s performance, simulation results show the effectiveness of this algorithm. Besides, for the problem of poor path, path optimization algorithm is used to process the path obtained by DGABI-RRT algorithm, and obtains a good performance.

2 DGABI-RRT Algorithm 2.1 Bidirectional RRT Algorithm Bidirectional RRT algorithm (BI-RRT)’s core idea is to expand two random trees on start point to goal point at the same time, the two trees are expanded towards each other, finally a barrier-free path is found. The node expansion diagram can be seen in Fig. 1. The specific steps of bidirectional RRT algorithm are as follows: Step 1: Generate two random trees T1 and T2 with the starting point xstart and the goal point xgoal as the root nodes respectively; Step 2: Get the random point xrand through uniform random sampling, and then find the node closest to xrand in T1 as xnear ; Step 3: Extend a certain step along the direction of xnear and xrand to obtain a new node xnew ;

556

Q. Li et al.

xrand

xnear1

s

xstart

xnear

xnew

T1

xgoal T2

Fig. 1. BI-RRT node expansion diagram.

Step 4: Judge whether there is an obstacle between xnear and xnew . If there is no obstacle, add xnew to T1 and perform step 5; if there is an obstacle, go back to step 2. Step 5: Find the node xnear1 closest to xnew in T2 and judge whether there is an obstacle between the two points. If there is no obstacle, perform the step 6; if there is an obstacle, go back to step 2. Step 6: Determine whether the distance between xnew and xnear1 is less than the set threshold. If so, it means that the two trees are connected successfully and the path planning is successful. Otherwise, the two trees are exchanged and the cycle is repeated until the path is found. BI-RRT still uses random sampling, so the randomness of the algorithm is strong. And BI-RRT’s path is still not optimal. To improve the efficiency of mobile robots, this paper attempts to improve BI-RRT to reduce randomness and get a better path. 2.2 DGABI-RRT Algorithm and Path Optimization In this paper, BI-RRT algorithm is improved by the following two aspects: one is using the double sampling points comparison and selection strategy to reduce the randomness of the sampling points, the other is increasing the goal bias of the mobile robot by using the gravity adaptive step size strategy. Then this paper will introduce these two improvement strategies in detail. 2.2.1 Double Sampling Points Comparison and Selection Strategy A strategy of double sampling points comparison and selection is proposed to reduce the randomness of sampling points. Firstly, this strategy uses the random function to generate two sampling points at the same time. Next, the distance between the xgoal and these two sampling points is compared. The purpose is to choose the one closest to xgoal as the final xrand . This algorithm is called double sampling points bidirectional RRT (DBI-RRT) algorithm. 2.2.2 Gravity Adaptive Step Size Strategy (1) Target Gravity Description As described above, the randomness of sampling points is reduced by introducing double sampling points comparison and selection strategy, but the expansion direction of the new nodes does not change, which reduces the efficiency of path planning. To solve

A Path Planning Algorithm for Mobile Robots Based on DGABI-RRT

557

this problem, we borrow the idea of gravitation from the artificial potential field [13], that is, xgoal will generate gravitational force on the mobile robots and attract them to move towards the xgoal . As a result, this paper combines the target gravity and BI-RRT algorithm together, and then proposes the target gravity BI-RRT (GBI-RRT) algorithm. The node expansion diagram of GBI-RRT algorithm is shown in Fig. 2.

xrand s

xstart

xnear

xnew xnear1

k•s

xgoal T2

T1

Fig. 2. GBI-RRT node expansion diagram.

Compared with BI-RRT shown in Fig. 1, GBI-RRT increases the guiding effect of gravity on new nodes, which can make the new node move towards the target point. At this time, the calculation equation of the new node is as following Eq. (1) [14]: xnew = xnear + s • (

xrand − xnear +k • xrand − xnear 

− xnear x  goal ) xgoal − xnear 

(1)

Where s is the step size in the direction of a random point,k is the gravitational coefficient, k • s represents the step size in the direction of gravitation, x  rand − xnear  represents the Euclidean distance between the xrand and the xnear , and xgoal − xnear  represents the Euclidean distance between the xgoal and xnear . (2) Gravity Adaptive Step Size Strategy As GBI-RRT algorithm makes the motion of mobile robots have goal bias, the new node always chooses the node close to the goal, which makes the expanded tree easy to fall into the problem of local minima [15]. To deal with the problems above, gravity adaptive step size is added to GBI-RRT algorithm, so gravity adaptive step size bidirectional RRT algorithm (GABI-RRT) is formed. GABI-RRT adjusts the value of k to change the step size adaptively according to the distance between the mobile robot and the obstacle. The specific method is as follows: (1) when the mobile robot does not encounter obstacles, as shown in Fig. 3(a), that is, if the distance between mobile robot and obstacle is greater than the set distance value σ , the step size of the goal gravitational direction k • s should be adjusted larger to make the robot move towards xgoal and reach xgoal as soon as possible. Considering that the value of k should not be too large, otherwise it will cause oscillation, so k is the initial value 1.5, it should be larger to show the effect of gravity at first; (2) when the mobile robot encounters an obstacle, as shown in Fig. 3(b), the mobile robot should be more inclined to the direction of random points to avoid obstacles. At this time, the step size of the random point direction s is kept unchanged, and the step size of the gravity direction k • s is reduced by reducing the value of k.Based on the above analysis, the step size of the mobile robot is reduced to realize obstacle avoidance. If obstacles are

558

Q. Li et al.

encountered again during the cycle, the algorithm continues to reduce the value of k to ensure the accuracy of obstacle avoidance. rand

rand

xnew s

xnear

s

xnew obstacle

ks

xgoal

(a )No obs ta cle

xnear k s

xgoal

(b)An obs ta cle

Fig. 3. Gravity adaptive step size diagram.

It should also be noted here that k is the gravitational coefficient, and k should be set to a value greater than or equal to 0 to make sense. If k is less than 0 during the cycle, k is re-assigned a constant value between 0 and 1 according to the actual environment to continue the algorithm. Therefore, the specific value of k is shown in Eq. (2): ⎧ ⎨ k − 0.1 distance(xnew , obstacle) < σ k > 0 (2) k= kini distance(xnew , obstacle) > σ ⎩ 0.5 k 0.4σ(Rimi ), the merging process of region Rimi is terminated. m m If Rj j is the region merged with Rimi , then Rimi and Rj j are marked as final regions, and are denoted as Ri∗ and Rj∗ , which will remain unmerged. The local adaptive threshold between Ri∗ and Rj∗ is determined as the value of f (Ri∗ , Rj∗ ). We will repeat the merging process until all the regions are defined as the final regions.

Color Recognition Method Based on Image Segmentation

583

Fig. 5. The example of a merging process

4

Region Color Discrimination

Image segmentation divides the candidate region into regions with homogeneous color. After the segmentation is completed, we identify the color characteristics of each region and give it a color label. We denote μH (Ri ), μS (Ri ), μV (Ri ) by the mean H, S and V value of region Ri respectively. The HSV color space model is a cone model in which a color can be expressed as a vector starting from the vertex of the cone. In order to compare the similarity of two color, the two color vectors are subtracted to get a new vector, and the length of the new vector is used as the index to measure the similarity of the two color. In the HSV color space model, a color vector is defined as ⎧ ⎨ x = r · cos(H) · S · V y = r · sin(H) · S · V (9) ⎩ z = h · (1 − V ) where r denotes the cone radius, h denotes the cone height. In order to improve the robustness of color recognition, we introduce the adjusting parameter to adjust S and V. The adjusting parameter scales the input value, the function is defined as y = 1 − (1 − x)n

(10)

where n denotes the adjusting parameter. Function curves corresponding to different values of adjusting parameter are shown in Fig. 6. The larger the parameter is, the larger the variation range of S/V will be allowed; The smaller the parameter is, the smaller the variation range of S/V is.

584

M. Hua et al.

Fig. 6. The scaling effect corresponding to different values of adjustment parameters

After introducing adjustment parameter, the color vector is defined as ⎧ ⎨ x = r · cos(H) · (1 − (1 − S)n ) · (1 − (1 − V )n ) y = r · sin(H) · (1 − (1 − S)n ) · (1 − (1 − V )n ) (11) ⎩ z = h · [1 − (1 − (1 − V )n )] We denote d by the dissimilarity between two colors. The dissimilarity between the undetermined color vector and reference color vector is defined as

2 2 2 (12) d = (x − xref ) + (y − yref ) + (z − zref ) where (xref , yref , zref ) denotes the reference color vector. The undetermined color vector returns the reference color vector with the least dissimilarity, and the reference color is used as the color label.

5

Experiment Results and Conclusion

The proposed algorithm was implemented in C++ programming language. Figure 7 shows the correction of the illumination component for the initial image. The corrected image is used as the input of image segmentation algorithm. Figure 8(a)(b) shows the over-segmentation result using the SLIC algorithm. Figure 8(c) shows the final color recognition result after merging progress and color discrimination progress. Here we recognize the red region of the image. The adjusting parameter n for H and V is set as 1.75. The recognition results of different colors are shown in Fig. 9. The left images are the input and the right images are the final recognition results. Figure 9(b) extracts blue region in the image, Fig. 9(d) extracts red region in the image.

Color Recognition Method Based on Image Segmentation

585

Fig. 7. (a) Source image. (b) Illumination component (c) Corrected image

Fig. 8. (a) Superpixel distribution. (b) Over-segmentation result. (c) Final result (Color figure online)

Fig. 9. Final results of different images (Color figure online)

586

6

M. Hua et al.

Conclusion

In this paper we proposed a color recognition algorithm to extract regions with specific color in the image. The algorithm is composed of three parts. The image preprogressing part uses 2D gamma correction function to correct the uneven illumination which improves the image quality. The image segmentation part adaptively generates local thresholds to segment the image into regions of homogeneous color. The color discrimination part determines the color of the segmented regions and hence the region with target color is recognized. The algorithm is robust for a variety of color images. For future work, we plan to improve the computing speed of the algorithm to meet the real-time requirements.

References 1. Achanta, R., Shaji, A., Smith, K., Lucchi, A., S¨ usstrunk, S.: Slic superpixels. epfl (2010) 2. Achanta, R., Shaji, A., Smith, K., Lucchi, A., Fua, P., S¨ usstrunk, S.: Slic superpixels compared to state-of-the-art superpixel methods. IEEE Trans. Pattern Anal. Mach. Intell. 34(11), 2274–2282 (2012) 3. Banic, N., Loncaric, S.: Light random sprays retinex: exploiting the noisy illumination estimation. IEEE Signal Process. Lett. 20(12), 1240–1243 (2013) 4. Bezdek, J.C., Ehrlich, R., Full, W.: FCM: the fuzzy c-means clustering algorithm. Comput. Geosci. 10(2–3), 191–203 (1984) 5. Cheng, H.-D., Sun, Y.: A hierarchical approach to color image segmentation using homogeneity. IEEE Trans. Image Process. 9, 2071–2082 (2000) 6. Cheng, H.D., Jiang, X.H., Sun, Y., Wang, J.: Color image segmentation: advances and prospects. Pattern Recogn. 34(12), 2259–2281 (2001) 7. Navon, E., Miller, O., Averbuch, A.: Color image segmentation based on adaptive local thresholds. Image Vis. Comput. 23(1), 69–85 (2005) 8. Nevatia, R.: A color edge detector and its use in scene segmentation. IEEE Trans. Syst. Man Cybern. 7(11), 820–826 (1977) 9. Schacter, B.J., Davis, L.S., Rosenfeld, A.: Scene segmentation by cluster detection in color spaces. ACM SIGART Bull. 58, 16–17 (1976) 10. Shih, F.Y., Cheng, S.: Automatic seeded region growing for color image segmentation. Image Vis. Comput. 23(10), 877–886 (2005) 11. Shuhua, L., Gaizhi, G.: The application of improved HSV color space model in image processing. In: 2010 2nd International Conference on Future Computer and Communication, vol. 2, pp. V2–10. IEEE (2010) 12. Tr´emeau, A., Colantoni, P.: Regions adjacency graph applied to color image segmentation. IEEE Trans. Image Process. 9(4), 735–744 (2000) 13. Underwood, S.A., Aggarwal, J.K.: Interactive computer analysis of aerial color infrared photographs. Comput. Graph. Image Process. 6(1), 1–24 (1977) 14. Wang, F., Man, L., Wang, B., Xiao, Y., Pan, W., Lu, X.: Fuzzy-based algorithm for color recognition of license plates. Pattern Recogn. Lett. 29(7), 1007–1020 (2008) 15. Zhicheng, L., Dianwei, W., Yin, L., Xuejie, L.: Adaptive correction algorithm for uneven illumination image based on two-dimensional gamma function. J. Beijing Inst. Technol. 36(02), 191–196 (2016)

Adaptive Echo State Network Robot Control with Guaranteed Parameter Convergence Ruihong Wu, Zhiwen Li, and Yongping Pan(B) School of Computer Science and Engineering, Sun Yat-sen University, Guangzhou 510006, China {wurh5,lizh63}@mail2.sysu.edu.cn, [email protected]

Abstract. Most existing adaptive neural network (NN) robot control methods suffer from poor convergence performance of network weights, which cannot fully exploit the approximation ability of NNs. A sufficient condition to ensure parameter convergence of NNs in these control methods is termed persistent excitation, which is difficult to be satisfied in practice. In this paper, an adaptive echo state network (ESN) robot control method enhanced by composite learning is proposed for a trajectory tracking problem of robotic arms with multiple degrees of freedom (DoFs), where a chaotic ESN is used to accurately model and compensate for robot uncertainty online. In the proposed method, a generalized prediction error is defined based on memory regressor extension, and both the prediction error and the generalized prediction error emerge into the weight update law of ESNs such that exponential convergence of parameter estimation, implying exponential convergence of trajectory tracking, is guaranteed under a weaker condition termed interval excitation. Simulation results based on a 7-DoF collaborative robot have verified the effectiveness and superiority of the proposed method. Keywords: Robot control · Adaptive control Composite learning · Parameter convergence

1

· Echo state network ·

Introduction

Due to the universal approximation property, neural networks (NNs) can approximate arbitrarily well a wide variety of functions when given a sufficient number of neural nodes and appropriate weights, making them a kind of powerful tool for handling functional uncertainties in nonlinear systems [1]. Adaptive NN control has kept appealing for robotic systems in recent years, e.g., see [2–4] and some references therein. The major benefit of applying NN approximation is that the modeling difficulty in many real-world robot control problems can be significantly reduced, simplifying the control synthesis [1]. c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 587–595, 2021. https://doi.org/10.1007/978-3-030-89092-6_53

588

R. Wu et al.

Recurrent NN (RNN) is a kind of NNs whose internal nodes are connected in a recurrent way. Due to recurrent connections, RNNs, compared with feedforward NNs, are more natural to model functions with finite memory since they avoid treating network inputs as a tapped delay line which usually leads to dramatically increasing the number of input nodes [5]. However, RNNs are notoriously difficult to train due to fading gradients and bifurcation encountered during training [6,7]. Echo state network (ESN) is an effective solution for these problems as it provides a simple framework for designing and training RNNs [8,9]. The core ideas of ESN are randomly generating large-scale and sparsely connected RNNs termed reservoirs and only training output weights. In ESN, since rich echo states can be obtained by driving the reservoir with external inputs and neuron activations, the training of output weights can always be considered as a linear regression problem, which effectively avoids the aforesaid problems of RNN training. In addition, the advantage of the initial activity of spontaneous chaos in the reservoir has been demonstrated experimentally, which makes the training more rapid, accurate and robust [10]. ESN has been successfully applied to and performs well in many control problems, e.g., see [11–19]. The major problem of most existing adaptive NN robot control methods is that only tracking error convergence is guaranteed, and thus, the approximation ability of NNs cannot be fully exploited. However, parameter convergence in adaptive control is always desirable as it brings superior trajectory tracking and robustness against various perturbations such as measurement noise and external disturbances [20]. A sufficient condition to ensure accurate NN approximation is that adjustable weights in NNs converge to optimal values, which is guaranteed by a stringent condition termed persistent excitation (PE) [21]. In adaptive NN robot control, the PE condition requires that robot states include sufficiently rich spectral information all the time, which is difficult to be achieved and verified in practice [3]. For ESNs with large-scale and sparsely connected reservoirs, satisfying the PE condition becomes more difficult. Moreover, even if PE exists, the excitation strength heavily influences the rate of parameter convergence, which usually leads to a slow learning process [22]. Composite learning is appealing to relax the PE condition for parameter convergence in adaptive control [23]. In this technique, regressor extension is applied by algebraic operations and online memory data are exploited by integral-like operations such that exponential parameter convergence can be guaranteed by a strictly weaker condition termed interval excitation (IE) compared with PE. The operation in composite learning is also named to be memory regressor extension (MRE) in [24]. This paper proposes an adaptive ESN robot control method enhanced by composite learning for a trajectory tracking problem of robotic arm with multiple degrees of freedom (DoFs), where a chaotic ESN is used to accurately model and compensate for robot uncertainty online. In the proposed method, a generalized prediction error is defined based on MRE, and both the prediction error and the generalized prediction error are emerged into the weight update law of ESNs such that exponential convergence of both parameter estimation and trajectory tracking is guaranteed under the IE excitation.

Adaptive Echo State Network Robot Control

589

The remainder of this paper is structured as follows. Section 2 formulates the control problem; Sect. 3 illustrates the proposed control design; Sect. 4 provides an illustrative example; Sect. 5 draws conclusions. Throughout this paper, R, R+ , Rn , and Rn×m denote the spaces of real numbers, positive real numbers, real n-vectors, and real n × m-matrices, respectively, N denotes the set of positive signals, x denotes the natural numbers, L∞ denotes the space of  bounded a2ij denotes the Frobenius norm of Euclidean norm of x, A2F = tr(AT A) = A, tr(B ) denotes the trace of B, σmin (B) denotes the minimum singular value of B, diag(·) denotes a diagonal matrix, xi is the i th element of x, and aij the (i, j)th element of A, where x ∈ Rn , A ∈ Rn×m , B ∈ Rn×n and i, j, n, m ∈ N. For the sake of brevity, in the subsequent sections, the arguments of a function may be omitted while the context is sufficiently explicit.

2

Problem Formulation

Consider a general class of n-DoF robotic systems described by the following Euler–Lagrange formulation [25]: ˙ q˙ + G(q) + F (q) ˙ = τ (t) M (q)q¨ + C(q, q)

(1)

in which q(t) := [q1 (t), q2 (t), ..., qn (t)]T ∈ Rn is a vector of joint angles, M (q) ∈ ˙ ∈ Rn×n is a centripetal-Coriolis matrix, G(q) Rn×n is an inertia matrix, C(q, q) ˙ ∈ Rn , and τ (t) ∈ Rn are vectors of gravitational torques, friction ∈ Rn , F (q) torques, and control torques, respectively, and n ∈ N. In this study, it is assumed that q and q˙ are measurable, and M , C, G and F are unknown a priori. The robot dynamics (1) can be formulated equivalently as follows: ˙ q˙ − G(q) − F (q)) ˙ q¨ = M −1 (q)τ + M −1 (q)(−C(q, q)    ˙ f (q, q)

(2)

with f : R2n → Rn . Let v ∈ Rn be a pseudo control torque to be designed later, ˙ ∈ Rn be nominal values of M (q) and f (q, q), ˙ and M0 (q) ∈ Rn×n and f0 (q, q) respectively. Then, the control torque is given by ˙ τ = M0 (q)(v(t) − f0 (q, q)).

(3)

Substituting the expression of τ in (3) into (2), one obtains ˙ − f0 (q, q)) ˙ + (M −1 (q) − M0−1 (q))τ q¨ = v(t) + (f (q, q)    ˙ τ) h(q, q,

(4)

where h : R3n → Rn is nonlinear uncertainty of the robotic system. Let qd (t) := [qd1 (t), qd2 (t), ..., qdn (t)]T ∈ Rn be a vector of desired outputs satisfying qd , q˙d , q¨d ∈ L∞ . Define a position tracking error e(t) := qd (t) − q(t). Our objective is to develop a proper control strategy for the robotic system (1) such that the tracking error e becomes as small as possible.

590

3 3.1

R. Wu et al.

Neural Network Control Design Echo State Network Approximation

To tackle the uncertainty h in (4), a chaotic ESN is used as a function approximator to model and compensate for h in an online way. To avoid the fixed-point solution problem [26], (4) is rewritten as follows: ˙ τ − ) + μh (Δt) q¨ = v(t) + h(q, q,

(5)

˙ τ −) with τ − := τ (t−Δt), and the chaotic ESN is applied to approximate h(q, q, − ˙ τ ) − h(q, q, ˙ τ ) denotes a sam˙ τ ), where μh (Δt) := h(q, q, rather than h(q, q, pling delay error, and Δt ∈ R+ is a sampling period. Note that μh (Δt) can be ignored if Δt is small enough. In the discrete-time representation, q˙ is relevant to q(k) and q(k − 1), and τ − is relevant to q(k − 1), q(k − 2) and q(k − 3), where k ∈ N is a discrete time step. Thus, due to the short-term memory property of ˙ τ − ) can be applied to RNNs, the chaotic ESN with the input q instead of (q, q, − ˙ τ ). The chaotic ESN can be represented by approximate h(q, q, ˙ x(t) = c(−x(t) + Win u(t) + W φ(t))

(6)

ˆ ˆ oT φ(t), φ(t) = tanh(x) (7) h(t) =W N ×n N ×N ˆ ˆ o1 (t), w ˆ o2 (t), . . . , w ˆ on (t)] ∈ ,W ∈ R and Wo (t) := [w in which Win ∈ R ˆ oi (t) = RN ×n denote input, internal, and output connection weights, respectively, w ˆoi2 (t), · · · , w ˆoiN (t)]T ∈ RN (i = 1 to n) is an adjustable output connec[w ˆoi1 (t), w ˆ ∈ Rn are the tion weight corresponding to the ith joint, u(t) := q(t) ∈ Rn and h(t) N N network input and output, respectively, x(t) ∈ R and φ(t) ∈ R are the state and activation of internal nodes, respectively, c ∈ R+ is a time constant, tanh(x) = [tanh(x1 ), tanh(x2 ), · · · , tanh(xN )]T is an activation function which works in the element-wise manner (other types of activation functions can also be chosen), and N ∈ N is the number of reservoir nodes. Let p ∈ (0, 1) and g ∈ R+ be connective and chaotic factors of the reservoir, respectively. According to the function approximation property of RNNs, there exists a constant vector of optimal weights Wo ∈ RN ×n such that ˙ τ − ) = WoT φ(t) + μnn (t) h(q, q,

(8)

where μnn (t) ∈ R is an optimal approximation error which can be ignored if the number N is sufficiently large. The pseudo control torque is given by n

ˆ oT φ(t) v(t) = q¨d (t) + Kp e + Kd e˙ − W

(9)

where Kp ∈ Rn×n and Kd ∈ Rn×n denote positive-definite diagonal matrices of proportional and derivative control gains, respectively. Applying (8) and (9) to (5), one gets the closed-loop tracking error dynamics as follows: ˜ oT φ(t) + μh (Δt) + μnn (t) = 0 e¨ + Kd e˙ + Kp e + W

(10)

ˆ o is a parameter estimation error. For the sake of simplicity, ˜ o := Wo − W where W μh and μnn will be ignored in the following content.

Adaptive Echo State Network Robot Control

3.2

591

Echo State Network Learning

As the joint acceleration q¨ is usually not accessible for measurement, an errorfiltering technique is applied to construct computable prediction errors for ESN ˙ A series-parallel identification model is given by learning. Let x denote q. ˆ + v(t) ˆ˙ = λ(x − x) ˆ + h(t) x

(11)

ˆ ∈ Rn is an estimate of x. Subwhere λ ∈ R+ is an identification gain, and x tracting (11) by (4), one obtains ˆ ˜˙ = −λx ˜ + h(q, q, ˙ τ ) − h(t) x    (t)

(12)

˜ := x − x ˆ is a state estimation error of the identification model, and where x 1 ˜ = s+λ (t) ∈ Rn is a modelling error. From (12), one gets x {(t)}, which means 1 ˜ and  have a stable-filtering relationship, where s+λ that x is a stable linear ˆ filter, and s is a complex variable. Define a prediction error f (t) := λ(x˙ − x) which can be calculated by (11). Then, one has ˙ f (t) = −λf (t) + λ(t).

(13)

An alternative way to calculate the filtered prediction error f is given as follows. From the definition of  and (13), one gets ˆ ˙ − L(s){h(t)} f (t) = hf (q, q) ˙ := L(s){h(q, q, ˙ τ )}, where L(s) = with hf (q, q) ˙ can be calculated by According to (4), hf (q, q) ˙ = λq˙ − hf (q, q)

λ s+λ

(14) is a stable linear filter.

λ λ2 ˙ − {q} {v}. s+λ s+λ

(15)

Apply the linear filter L(s) to both sides of (8) resulting in ˙ = WoT φf (t) hf (q, q)

(16)

with φf (t) := L(s){φ(t)}. A prediction model is given as follows: ˆ f (t) = W ˆ T φf (t) h o

(17)

ˆ f (t) ∈ Rn is a predicted counterpart of hf (q, q). ˙ Multiplying both sides where h α to both sides of of (16) by φTf and applying a stable linear filter H(s) = s+α the resulting equality, one obtains H(s){hf φTf } = WoT Φe (t)

(18)

with Φe (t) := H(s){φf φTf } ∈ RN ×N , where α ∈ R+ is a filtering parameter.

592

R. Wu et al.

ˆ oT Φe (t) ∈ Rn×N , Define a generalized prediction error ξ(t) := WoT Φe (t) − W where WoT Φe can be calculated by hf φTf in (18), and hf can be calculated by ˆ o is given by (15). Then, a composite learning law of W ˆ˙ o (t) = Γ (φ(t)T (t) + κξ T (t)) W f

(19)

where Γ ∈ RN ×N is a positive-definite diagonal matrix of learning rates, and κ ∈ R+ is a weighting parameter. Note that Γ and κ can be chosen freely. 3.3

Stability and Convergence Analysis

It follows from the definition of ξ and (19) that ˜˙ o (t) = −Γ (φ(t)Tf (t) + κΦe (t)W ˜ o (t)). W

(20)

Choose a Lyapunov function candidate Vw = tr(

1 1 ˜ T −1 ˜ f Tf + W Γ Wo ). 2λ 2 o

(21)

The time derivative of Vw along (20) is 1 ˜ o )) ˜ oT (φTf + κΦe W V˙ w = tr( f ˙ Tf − W λ 1 ˜ o ). ˜ T φT − κW ˜ T Φe W = tr( f ˙ Tf − W o f o λ

(22)

Applying (7) and (8) to (13), substituting the resulting equality into (22) and ignoring μh and μnn , one obtains ˜o − W ˜ o) ˜ oT φTf − κW ˜ oT Φe W V˙ w = tr(−f Tf + f φT W ˜ o ) ≤ −f 2 − κσmin (Φe )W ˜ oT Φe W ˜ o 2F ≤ 0. = −tr(f Tf + κW F

(23)

Noting the definition of Φe , one gets σmin (Φe ) ≥ 0. The above result implies that: 1) f asymptotically converges to 0 as t → ∞, which means  asymptoti˜ o exponentially cally converges to 0 as t → ∞, with no excitation; 2) f and W converges to 0 under the IE condition [23] as t → ∞. Noting (10) and the above results, one concludes that: 1) e asymptotically converges to 0 as t → ∞ with no excitation; 2) e exponentially converges to 0 as t → ∞ under the IE condition.

4

Simulation Studies

Simulations based on a 7-DoF collaborative robot called Franka Emika Panda are carried out on MATLAB software, where the step size is set as 0.001. The dynamics of the Panda robot is described by (1), where dynamic parameters come from high-fidelity identification results in [27]. During simulations, the joint

Adaptive Echo State Network Robot Control

593

Fig. 1. Performance comparison with respect to tracking error e, prediction error , ˆ o for ˆ o of ESN. (a) Norm of e. (b) Norm of . (c) Norm of W and the output weights W ˆ o for CESN-CLC on 7 joints. CESN-CAC on 7 joints. (c) Norm of W

position q(t) is commanded to track the desired trajectory qd (t) = 0.25 sin(2πt)+ q(0), where q(0) = [0, −0.16, 0, −1.75, 0, 1.5, 0.8]T (rad). The generation of the chaotic ESN in (6) with (7) for function approximation follows the following steps: First, the internal weight W is generated with a connectivity p, where nonzero elements are drawn from a Gaussian distribution with zero mean and variance 1/(pN ); second, the internal weight W is scaled by the chaotic factor g to make the reservoir exhibit chaos; third, the input weight Win is drawn from a uniform distribution between [−0.1, 0.1]; last, the initial value of the output weight Wo (0) is set to be 0. The chaotic ESN-based composite learning control (CESN-CLC) composed of (9) and (19) is applied to control the Panda robot, where the control parameters are selected as N = 400, p = 0.1, g = 1.5, n = 7, c = 0.001, Kp = diag (400, 400, 400, 400, 400, 400, 400), Kd = diag (40, 40, 40, 40, 40, 40, 40), λ = 20, α = 0.1, Γ = 0.1I, and κ = 1. The chaotic ESN-based classic adaptive control (CESN-CAC) is chosen as a baseline controller, where we simply set κ = 0 in CESN-CLC to get the CESN-CAC, and keep the other control parameters the same as those in the CESN-CLC for a fair comparison. Figure 1(a) and (b) depict the norms of tracking error e and the modeling error  from the simulations, respectively. It is observed that for the given desired angle qd , the CESN-CLC makes e and  converge to within 0.002 and 0.9 in 200 sec, respectively, while the CESN-CAC ends up with about 0.007 for e and 2.9 for . A similar trend is reflected in the norm of the output weight ˆ oi of each joint (i = 1 to 7), in which the CESN-CAC makes W ˆ oi  oscillate W without clear convergence to any certain constant within 200 sec, especially for

594

R. Wu et al.

ˆ oi  fast converge joints 2 and 4 [see Fig. 1(c)], while the CESN-CLC makes W to a certain constant [see Fig. 1(d)].

5

Conclusions

In this paper, a CESN-CLC strategy is proposed for a trajectory tracking control problem of multi-DoF robots to achieve better parameter convergence, in which a chaotic ESN is used to accurately model and compensate for robot uncertainty online. Asymptotic stability of the closed-loop system is obtained without any excitation, and exponential parameter convergence is achieved under the IE condition which is strictly weaker than the PE condition. Simulation results have demonstrated that the CESN-CLC outperforms the CESN-CAC in terms of tracking accuracy and parameter convergence. Future studies would focus on experiments of the proposed method on a real-world multi-DoF robot as in [28]. Acknowledgements. This work was supported in part by the Guangdong Pearl River Talent Program of China under Grant No. 2019QN01X154, and in part by the Fundamental Research Funds for the Central Universities of China under Grant No. 19lgzd40.

References 1. Narendra, K.S.: Neural networks for control theory and practice. Proc. IEEE 84(10), 1385–1406 (1996) 2. Pan, Y., Liu, Y., Xu, B., Yu, H.: Hybrid feedback feedforward: an efficient design of adaptive neural network control. Neural Netw. 76, 122–134 (2016) 3. Guo, K., Pan, Y., Yu, H.: Composite learning robot control with friction compensation: a neural network-based approach. IEEE Trans. Industr. Electron. 66(10), 7841–7851 (2018) 4. Huang, D., Yang, C., Pan, Y., Cheng, L.: Composite learning enhanced neural control for robot manipulator with output error constraints. IEEE Trans. Industr. Inf. 17(1), 209–218 (2019) 5. Boden, M.: A guide to recurrent neural networks and backpropagation. The Dallas Project (2002) 6. Bengio, Y., Simard, P., Frasconi, P.: Learning long-term dependencies with gradient descent is difficult. IEEE Trans. Neural Networks 5(2), 157–166 (1994) 7. Pearlmutter, B.A.: Gradient calculations for dynamic recurrent neural networks: a survey. IEEE Trans. Neural Networks 6(5), 1212–1228 (1995) 8. Jaeger, H.: The “echo state” approach to analysing and training recurrent neural networks. Technical report GMD Report 148, German National Research Center for Information Technology, Bonn, Germany (2001) 9. Jaeger, H., Haas, H.: Harnessing nonlinearity: predicting chaotic systems and saving energy in wireless communication. Science 304(5667), 78–80 (2004) 10. Sussillo, D., Abbott, L.F.: Generating coherent patterns of activity from chaotic neural networks. Neuron 63(4), 544–557 (2009) 11. Waegeman, T., Wyffels, F., Schrauwen, B.: Feedback control by online learning an inverse model. IEEE Trans. Neural Netw. Learn. Syst. 23(10), 1637–1648 (2012)

Adaptive Echo State Network Robot Control

595

12. Xing, K., Wang, Y., Zhu, Q., Zhou, H.: Modeling and control of Mckibben artificial muscle enhanced with echo state networks. Control. Eng. Pract. 20(5), 477–488 (2012) 13. Han, S.I., Lee, J.M.: Precise positioning of nonsmooth dynamic systems using fuzzy wavelet echo state networks and dynamic surface sliding mode control. IEEE Trans. Industr. Electron. 60(11), 5124–5136 (2012) 14. Han, S.I., Lee, J.M.: Fuzzy echo state neural networks and funnel dynamic surface control for prescribed performance of a nonlinear dynamic system. IEEE Trans. Industr. Electron. 61(2), 1099–1112 (2013) 15. Park, J., Cho, D., Kim, S., Kim, Y.B., Kim, P.Y., Kim, H.J.: Utilizing online learning based on echo-state networks for the control of a hydraulic excavator. Mechatronics 24(8), 986–1000 (2014) 16. Galtier, M.: Ideomotor feedback control in a recurrent neural network. Biol. Cybern. 109(3), 363–375 (2015) 17. Park, J., Lee, B., Kang, S., Kim, P.Y., Kim, H.J.: Online learning control of hydraulic excavators based on echo-state networks. IEEE Trans. Autom. Sci. Eng. 14(1), 249–259 (2016) 18. Badoni, M., Singh, B., Singh, A.: Implementation of echo-state network-based control for power quality improvement. IEEE Trans. Industr. Electron. 64(7), 5576– 5584 (2017) 19. Jordanou, J.P., Antonelo, E.A., Camponogara, E.: Online learning control with echo state networks of an oil production platform. Eng. Appl. Artif. Intell. 85, 214–228 (2019) 20. Lin, J.S., Kanellakopoulos, I.: Nonlinearities enhance parameter convergence in output-feedback systems. IEEE Trans. Autom. Control 43(2), 204–222 (1998) 21. Sastry, S., Bodson, M.: Adaptive Control: Stability, Convergence and Robustness. Prentice Hall, New Jersey (1989) 22. Pan, Y., Yu, H.: Composite learning robot control with guaranteed parameter convergence. Automatica 89, 398–406 (2018) 23. Pan, Y., Yu, H.: Composite learning from adaptive dynamic surface control. IEEE Trans. Autom. Control 61(9), 2603–2609 (2016) 24. Ortega, R., Nikiforov, V., Gerasimov, D.: On modified parameter estimators for identification and adaptive control. A unified framework and some new schemes. Ann. Rev. Control 50, 278–293 (2020) 25. Spong, M.W., Hutchinson, S., Vidyasagar, M.: Robot Modeling and Control, vol. 3. Wiley, New York (2006) 26. Kim, N., Calise, A.J.: Several extensions in methods for adaptive output feedback control. IEEE Trans. Neural Networks 18(2), 482–494 (2007) 27. Gaz, C., Cognetti, M., Oliva, A., Giordano, P.R., De Luca, A.: Dynamic identification of the Franka Emika Panda robot with retrieval of feasible parameters using penalty-based optimization. IEEE Robot. Autom. Lett. 4(4), 4147–4154 (2019) 28. Liu, X., Li, Z., Pan, Y.: Preliminary evaluation of composite learning tracking control on 7-DoF collaborative robots. In: IFAC Conference on Modelling, Identification and Control of Nonlinear Systems (IFAC-PapersOnLine), Tokyo, Japan, to be published (2021)

Design of the Autonomous Path Planning System for Mining Robots Based on Stereo Vision Youge Su1 , Wanghui Bu1(B) , Jing Chen2(B) , Sihang Li1 , and Mingzhi Liu1 1 School of Mechanical Engineering, Tongji University, Shanghai 201804, China

[email protected]

2 School of Computer Science and Technology, Hangzhou Dianzi University, Hangzhou, China

[email protected]

Abstract. Rock crushing task for excavators has harsh working environments. With the development of vision sensors and intelligent algorithms, it is possible to achieve autonomous intelligent mining for unmanned excavators. This paper presents an autonomous path planning system for mining robots based on stereo vision. For semantic obstacles such as excavators and loaders in the quarry environment, a recognition network for construction vehicles is trained. The positions and sizes of the construction vehicles are determined by stereo vision. To avoid the rollover hazard that may occur, a gradient map is derived from the elevation map. The traffic cost considering the energy consumption is adopted to build a graph, where the Dijkstra graph search algorithm is used to obtain the path with the lowest cost. Finally, ROS and Gazebo are utilized to simulate and verify the autonomous path planning system. The results show that the proposed system based on stereo vision can complete the autonomous intelligent navigation task in the quarry environment. Keywords: Unmanned excavators · Autonomous intelligent unmanned mining · Path planning · Stereo vision · Construction machinery

1 Introduction The typical environment for excavator operation is the quarry, mining field, and even earthquake disaster site, these environments have potential hazards of landslides and rock-falls. Once the driver makes a mistake, an accident such as rollover may appear in the complex terrain work environment. To guarantee the safety of drivers, it is necessary to update the existing excavator control system to the autonomous intelligent unmanned system. The autonomous path planning system is important for intelligent unmanned operation. For mining robot chassis, the perceived terrain structure cannot be directly used as the basis for obstacle avoidance. It needs to be combined with specific semantic information. In addition, the complexity of the terrain requires the path planning algorithm to be able to avoid the potential rollover hazard of the excavator crusher. Therefore, it is necessary to further process the structural information of the environment to obtain information such as the slope that can guide the path planning. © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 596–605, 2021. https://doi.org/10.1007/978-3-030-89092-6_54

Design of the Autonomous Path Planning System

597

Obstacle avoidance and path planning are research hotspots in the field of robotics, and some researchers have researched the corresponding problems of construction vehicles. Borthvick et al. used vision to establish the point cloud information of the truck to reduce the possibility of collision between the bucket and the truck at work [1]. Chae et al. combined the laser rangefinder signal and the visual signal to estimate the distance between the person and the excavator to avoid the excavator from hurting people [2]. Nuwan Ganganath et al. proposed an energy-saving path planning method that considers the climbing ability of tracked vehicles in an off-road environment [3]. Zeng, Sun has studied the application of stereo vision in autonomous exploration vehicles and realized the analysis of the passing ability of tracked vehicles [4, 5]. Bowen Deng et al. studied the application of local path planning algorithm in the operation of unmanned construction vehicles, using the D-Star algorithm to realize the dynamic obstacle avoidance on a plane [6]. Zhao studied a path planning method based on an improved ant colony algorithm and determined a path planning plan that conforms to the vehicle’s passing ability and has the shortest passing time [7]. Also, the object detection method is widely used in unmanned vehicle obstacle avoidance research. Yadav et al. proposed a fusion framework with radar and camera for object detection in autonomous vehicles [8]. Kemsaram et al. took stereo vision information as input of deep neural networks called DriveNet for obstacle detection and avoidance [9]. The above studies have discussed path planning tasks on terrain and semantic obstacle avoidance tasks for workers, construction vehicles separately. This paper presents an autonomous path planning system that considers path planning on mining field terrain and semantic obstacle avoidance on terrain at the same time.

2 Recognition of Semantic Obstacles The working environment of the mining robot is composed of terrain and semantic obstacles. The semantic obstacles include targets such as workers and construction vehicles. To realize the obstacle avoidance system in the outdoor terrain path planning system, the YOLO-based [10] construction vehicle recognition network is trained, and the stereo camera model is used to estimate the location and size of obstacles. 2.1 Training of the Recognition Network for Construction Vehicles The construction machinery images captured in the mining field include three types of targets: excavators, loaders, and trucks, totaling 372. Images are labeled with tools and randomly divided into three parts: training set, test set, and verification set. The number of images is 300, 34, and 38, respectively. The training of the YOLO target recognition network is carried out on the training set, and the training parameters are shown in Table 1. The changes of various indicators during the training process are shown in Fig. 1. The recognition accuracy (Precision), the recognition recall rate (Recall), and the average recognition accuracy (mAP), which measure the performance of the model, continue to increase while training and finally reached about 90%. At the same time, the recognition results shown in Table 2 can be obtained by testing on the test set, and the

598

Y. Su et al.

indicators are close to the results on the training set, as shown in Fig. 2. In summary, it can be considered that the target recognition training for excavators, loaders, and trucks is effective. Table 1. Training parameters of the YOLO network. Parameter type

Batch size

Iteration times

Number of classes

Value

32

80

3

Fig. 1. Changes of various indicators during the training process.

Table 2. Test set evaluation result. Class

Image number

Object number

Precision

Recall

Total

34

44

0.926

0.895

Excavator

34

21

0.941

0.761

Loader

34

10

0.86

1

Truck

34

13

0.978

0.923

Fig. 2. Recognition results on the test set.

2.2 Obstacle Positioning and Size Estimation After obtaining the detection box in the image, it is also necessary to determine the position and size information of the obstacle in combination with the disparity image. Therefore, the obstacle is modeled as a rectangular cuboid as shown in Fig. 3.

Design of the Autonomous Path Planning System

599

The length of L x , L y , L z , and coordinate of Pc = [X c , Y c , Z c ] under camera frame Oc needs to be evaluated. Pc and L x , L y can be easily evaluated with the geometry of the detection box and the stereo camera model as shown in Eq. (1) where x, y are the coordinates of the point of concern in the left stereo image; X, Y, Z are the target point coordinate in the camera frame; f x , f y , cx , cy stand for camera intrinsic parameters; b represents the baseline of the stereo camera and d denotes the disparity value obtained by stereo matching algorithm [11]. Then L z can be evaluated by the prior knowledge of construction vehicle size.

Fig. 3. The obstacle cuboid model.

X =

x−cx fx Z

Y =

y−cy fy Z

Z=

(1)

fx b d

The recognition of semantic obstacles in the simulation environment is shown in Fig. 4. Figure 5 shows the result of obstacle recognition. The figure on the left marks the detection box of the obstacle in the disparity image, and the right one marks the result of the obstacle estimation with a red cuboid. At the same time, the key coordinates of the recognition results are recorded in Table 3.

Fig. 4. Semantic obstacles recognition in simulation environment

It can be seen that the values on the x-axis and the y-axis are relatively accurate whether it is the estimation of the position or the size of the obstacle, but the estimation

600

Y. Su et al.

Fig. 5. Recognition results of semantic obstacles. Table 3. Semantic obstacle information. Obstacle information

Value

p1 , p2 /pixel

(291, 37), (429, 233)

d c /m

4.69

P c /m

(0.055, −1.61, 5.77)

Lx , Ly , Lz /m

(2.17, 3.08, 2.17)

deviation on the z-axis is relatively large due to the existence of the occlusion relationship. To ensure the safety of the path planning results, the obstacle cuboid can be expanded, and the results can be marked in the environment map to guide path planning.

3 Terrain Path Planning of Mining Robots The data input of the path planning system is the dense point cloud map of the environment obtained by the stereo vision SLAM system [12]. For outdoor terrain path planning, the point cloud map needs to be converted into a gradient map and elevation map. The conversion from point cloud to elevation map is intuitive [13], but the gradient map needs to be estimated with the method below. 3.1 Generation of the Gradient Map The gradient map generation is achieved by a local normal to the estimated point cloud. First, use the K-Nearest Neighbor algorithm [14] to obtain the nearest k points for the location to be sought. Assuming that these k points all fall on the same plane, they meet the constraints as Eq. (2), where x, y, and z denote the centers of gravity of the k points, and a, b, c form the normal vector of the plane. a(xi − x) + b(yi − y) + c(zi − z) = 0, i = 1, . . . , k

(2)

Solving the least-squares problem as Eq. (3) can get the normal vector of the plane to be estimated. k  2 (3) a(xi − x) + b(yi − y) + c(zi − z) , i = 1, . . . , k min C = i=1

Design of the Autonomous Path Planning System

601

The Eq. (4) can be obtained by making the result of obtaining the derivatives of the above formula for a, b and c respectively as 0.  ⎡  2  ⎤⎡ ⎤ x xy xz a  i  i 2i  i i ⎣ yi xi yi yi zi ⎦⎣ b ⎦ = 0    2 zi xi zi yi zi c

(4)

Using SVD decomposition, the solution of this equation in the sense of least squares can be obtained [15], which is estimated as shown in Fig. 6.

Fig. 6. Elevation map of environment (left), and derived gradient map (right).

3.2 The Path Planning Algorithm Path planning and mining robot control are based on both elevation map and gradient map. The overall planning and control framework is shown in Fig. 7.

Fig. 7. Planning and control framework of mining robot.

The basic requirement of path points generated by the path planner is to reach the target point without passing through the semantic obstacle position, and at the same time, there is a demand for minimizing energy consumption for the mining robot. To this end, the original elevation map and gradient map are converted into a graph, and the graph search algorithm is used to obtain the path of minimum energy consumption. Graph node is generated from elevation map grid element, the cost of an edge between node i and node j is calculated by the expression below [16].



(5) Ei,j = μ cos θi,j + sin θi,j Di,j

602

Y. Su et al.

The friction coefficient μ can be set to any value less than 1.0 for convenience. Di,j stands for Euclidean distance between node i and j. Relative slope θi,j can be calculated with Eq. (6), where g(i)z and g(j)z represent the z-axis component of the gradient at node i and j.



 (6) θi,j = arccos(g(i)z ) − arccos g(j)z

When constructing the graph, no nodes are generated in the area of semantic obstacles. Then the Dijkstra algorithm [17] can be used to realize the path search that minimizes the cost.

4 Path Planning Simulation To verify the algorithm of semantic obstacle recognition algorithm and path planning algorithm, the simulation framework of the system of the excavator and crusher as shown in Fig. 8 is established based on ROS [18] and Gazebo [19]. As shown in Fig. 9, the virtual quarry environment model takes a real quarry environment as a reference. Mining robot chassis is modeled based on an approximate approach [20].

Fig. 8. Path planning system simulation framework.

Fig. 9. The virtual quarry environment (right) imitates a real one (left).

The visualization of the path planning test is shown in Fig. 10. It is an elevation map and the green ball mark in the left figure is the target object selected by hand. At the same time, the red cube marks the estimation of the location and size of the two excavator

Design of the Autonomous Path Planning System

603

Fig. 10. Simulation result of a path planning task.

obstacles under the current view. In the final state, the target is within the workspace of this mining robot. The record of the chassis trajectory data during the simulation and the final result of the path planning are shown in Fig. 11.

Fig. 11. Chassis trajectory and 5 times planning results.

It can be seen that in the entire test process, with the expansion of the map and the movement of the chassis, the path planning module is called 5 times. Since the path planning module runs on the elevation grid map, the optimal path formed may have a broken line locally. To ensure the stable operation of the chassis, the chassis is not required to strictly follow the path when executing the planning results. As long as the distance between the chassis and the path point is within the workspace of the mining robot, the chassis is considered to be in place successfully. This strategy can ensure that the chassis can still form a relatively smooth motion trajectory on the local broken line path.

604

Y. Su et al. Table 4. Simulation results of the path planning.

Key location

Coordinate/m

Start location

(0, 0, 0)

End location

(11.37, 1.37, 0.62)

Target location

(14.18, 0.22, 0.72)

The position and attitude change in the map coordinate system during the movement of the chassis are shown in Fig. 12. The attitude angle roll and pitch together constitute the inclination of the chassis.

Fig. 12. Pose change of the chassis during test.

The coordinate of start location, end location, and target location of the chassis under the test is recorded in Table 4. The distance between the end location and target location is 3.04 m, and it is valid for a workspace with a radius of 3.5 m.

5 Conclusion This paper presents an autonomous path planning system for unmanned excavators, which considers semantic obstacle avoidance and energy saving at the same time. The YOLO-based construction vehicle recognition algorithm provides the system with the estimation of the positions and sizes of obstacles. The elevation map is converted to a gradient map, and a path search that minimizes energy consumption is realized through the Dijkstra algorithm. Finally, simulation tests based on ROS and Gazebo are carried out, which proves the effect of the autonomous path planning system. Acknowledgements. This work was supported by National Natural Science Foundation of China (61703127, 51475331), Zhejiang Provincial Natural Science Foundation of China (LY17F020026), and the Fundamental Research Funds for the Central Universities.

Design of the Autonomous Path Planning System

605

References 1. Borthwick, J., Lawrence, P., Hall, R.: Mining haul truck localization using stereo vision. In: Proceedings of the Robotics and Applications Conference, p. 9, University of British Columbia (2009) 2. Chae, S.: Application of image processing to detecion of workers around heavy eqipment. Water Resour. Manage 24(1), 139–156 (2011) 3. Ganganath, N., Cheng, C.T., Chi, K.T.: A constraint-aware heuristic path planner for finding energy-efficient paths on uneven terrains. IEEE Trans. Industr. Inf. 11(3), 601–611 (2015) 4. Zeng, Q.X.: Research on Passing Ability of Autonomous Prospecting Vehicle Based on Binocular Vision, Jilin University (2015) 5. Sun, W.Y.: Research on the Environment Recognition Based on Binocular Stereovision for Autonomous Resource Prospecting Vehicle, Jilin University (2014) 6. Deng, B.W., Zhang, C.H., Li, J., Lei, Y.N., Wang, Q., Zhang, S.H.: Application of local path plan for unmanned construction machinery autonomous task. Ordnance Ind. Autom. 35(10), 70–73, 79 (2016) 7. Zhao, M.T., Li, Y.H., Fan, J.J.: Study on time optimal path planning based on improved ant colony algorithm. Veh. Power Technol. 1(3), 7–10, 14 (2020) 8. Yadav, R., Vierling, A., Berns, K.: Radar + RGB fusion for robust object detection in autonomous vehicle. In: 2020 IEEE International Conference on Image Processing (ICIP), Abu Dhabi, pp. 1986–1990 (2020) 9. Kemsaram, N., Das, A., Dubbelman, G.: A stereo perception framework for autonomous vehicles. In: 2020 IEEE 91st Vehicular Technology Conference (VTC2020-Spring), pp. 1–6 (2020) 10. Farhadi, A., Redmon, J.: Yolov3: an incremental improvement. In: Computer Vision and Pattern Recognition, Salt Lake City (2018) 11. Kaehler, A., Bradski, G.: Learning OpenCV 3: Computer Vision in C++ with the OpenCV Library. O’Reilly Media, Sebastopol (2016) 12. Labbé, M., Michaud, F.: RTAB-Map as an open-source lidar and visual simultaneous localization and mapping library for large-scale and long-term online operation. J. Field Robot. 36(2), 416–446 (2019) 13. Fankhauser, P., Hutter, M.: A universal grid map library: implementation and use case for rough terrain navigation. In: Koubaa, A. (ed.) Robot Operating System (ROS). SCI, vol. 625, pp. 99–120. Springer, Cham (2016). https://doi.org/10.1007/978-3-319-26054-9_5 14. Connor, M., Kumar, P.: Fast construction of k-nearest neighbor graphs for point clouds. IEEE Trans. Visual Comput. Graphics 16(4), 599–608 (2010) 15. Klasing, K., Althoff, D., Wollherr, D.: Comparison of surface normal estimation methods for range sensing applications. In: 2009 IEEE International Conference on Robotics and Automation, pp. 3206–3211. IEEE (2009) 16. Santos, A.S., Azpúrua, H.P., Pessin, G., Freitas, G.M.: Path planning for mobile robots on rough terrain. In: 2018 Latin American Robotic Symposium, 2018 Brazilian Symposium on Robotics (SBR) and 2018 Workshop on Robotics in Education (WRE), pp. 265–270. IEEE (2018) 17. Dijkstra, E.W.: A note on two problems in connexion with graphs. Numer. Math. 1(1), 269–271 (1959) 18. Robotic Operating System Homepage. https://www.ros.org 19. Koenig, N., Howard, A.: Design and use paradigms for Gazebo, an open-source multi-robot simulator. In: Intelligent Robots and Systems, Sendai (2004) 20. Pecka, M., Zimmermann, K., Svoboda, T.: Fast simulation of vehicles with non-deformable tracks. In: 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vancouver (2017)

A Human-Like Learning Framework of Robot Interaction Skills Based on Environmental Dynamics Hanzhong Liu1 , Chenguang Yang1,2(B) , and Shi-Lu Dai1,2 1

2

College of Automation Science and Engineering, South China University of Technology, Guangzhou 510640, China [email protected] Foshan Newthinking Intelligent Technology Company Ltd., Foshan 528231, China

Abstract. Learning from demonstration (LfD) is one of the promising approaches for robots efficiently learning and regenerating human skills. Conventionally, the learning system learns both movements and stiffness profiles from human demonstrations, but rarely considers the environmental dynamic models. In this paper, we propose a human-like framework that enables the robot to learn the environmental dynamic model from a single demonstration. Within this framework, the robot will attempt to reproduce and improve the interaction skills by repetitive self-training under the task requirement. A radial basis function neural network (RBFNN) with broad learning system (BLS) is proposed to characterize the initial and updated dynamic models, and a novel motion planning method is developed to search for the optimal trajectories which minimize the changing rate of the velocity and pressing force. Then, a hybrid force/position controller is implemented to execute the interaction skills. Experimental studies have been performed on a Baxter robot to verify the effectiveness of the proposed framework. Keywords: Biological systems control · Broad learning · Learning from demonstration · Motion planning · Robot-environment interaction

1

Introduction

Learning from demonstration (LfD) [2,20] is one of the promising ways for robots to imitate human behaviors, which can quickly program the robots to perform operating skills and replace humans from such tasks. Dynamical movement primitives (DMP) based LfD methods extract trajectories (usually velocities, positions and stiffness profiles [18]) from human demonstrations. With the help of This work was supported by National Nature Science Foundation of China (NSFC) (Nos. U20A20200, 61811530281, and 61861136009), Guangdong Regional Joint Foundation (No. 2019B1515120076), Fundamental Research for the Central Universities, and in part by the Foshan Science and Technology Innovation Team Special Project (No. 2018IT100322). c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 606–616, 2021. https://doi.org/10.1007/978-3-030-89092-6_55

A Human-Like Learning Framework

607

probability models [17], LfD methods can also capture manipulation profiles from multiple demonstrations. However, conventional LfD methods focus on motion or force profiles transferring from human demonstrations, which rarely consider the environmental dynamics and may lead to failure of some robot-environment interaction tasks. In robot-environment interaction tasks, mechanical properties of the interactive objects and the extra force applied by the robot are important factors that determine the interaction performance and plenty of control strategies have been developed. An additional impedance model is implemented to regulate the endpoint motion and contact force of the manipulator in impedance control [14] and admittance control [9], which lead to a stable behavior during the interaction with energetically passive systems. Nonetheless, impedance or admittance controllers are considered to be too conservative in some situations that part of the environmental dynamics is already known. In contrast, the hybrid force/position control [13] can make good use of the prior knowledge of the environmental dynamics to achieve control of both forces and positions by decoupling the motions into the normal direction and tangential direction of the surface. Therefore, it could be a satisfactory strategy for the bottom controller of a human-like interaction skills learning framework. Broad learning system (BLS) is a recently developed framework and has been extensively applied for off-line pattern recognition [3,11]. Based on the principle of incremental learning, BLS is well suited for simulating the central nervous system (CNS) of humans. The CNS gives humans excellent capability to adaptively compensate the forces after a short heuristic stage [6], and can also improve interaction skills with accumulated knowledge [12]. Therefore, humans can acquire proficient contact tooling skills [5], such as polishing, wiping or carving on a surface, etc. In this context, a human-like LfD framework based on BLS is proposed with a good estimation of the environment dynamics and a self-training strategy. The main contributions of this work can be concluded as follows. 1) The learning from demonstration framework is enhanced by a novel incremental scheme, based on which the parameters of the newly added neurons are designed through considering both the existing neurons and the incoming input vector. 2) A novel motion planning algorithm is developed to minimize the changing rate of expected pressing forces and velocities, and give a considerate trajectory in the vector space. 3) A hybrid force/position controller is implemented to control the orthogonal sliding velocity and pressing force, which is well suitable for the task scenario. The remainder of this paper is organized as follows. The problem to be studied is formulated in Sect. 2. Section 3 introduces the overall framework and the methodology. Section 4 presents the experiments and Sect. 5 is the conclusion.

608

2

H. Liu et al.

Problem Formulation

The researches on humans adaptation to rigid and compliant environments in [4,10] have shown that the contact forces applied on the object surface during interaction tasks, such as polishing, wiping, are nearly constant values. However, to meet the needs of different tasks, it is expected to apply a constant or variable force according to the specified trajectory, and we know that the frictions and pressing forces of a surface are correlative, which can be used to improve interaction skills. In this paper, we design a position trajectory for the interaction tasks and expect the robot to learn how to maintain the friction as a specified value.

Fig. 1. A typical interaction scenario involving friction.

Except for the pressing force, several factors infect the frictions, such as the relative sliding velocity and the mechanical properties of the surface. It’s worth noting that the friction may be nonlinear with the aforementioned factors. Therefore, the proposed learning framework aims to learn the complex environmental dynamics and then control the robot’s state (e.t. endpoint velocities and forces) to achieve the expected friction. As shown in Fig. 1, when the robot manipulator makes contact with such a rigid surface and relative slip occurs, the changing factors of the frictions are the sliding velocity, the pressing force and the mechanical properties. The mechanical properties can be expressed as a function of the position and the relative sliding velocity is equal to the endpoint velocity of the robot, thus all the factors concerned with the friction can be characterized by an input vector: T  X = p, v, F N

(1)

where p ∈ R3 is the endpoint position of the robot in Cartesian space, v ∈ R3 is the velocity and F N ∈ R3 denotes the pressing force in the normal direction. To achieve the expected friction, the framework has to figure out suitable v and F N for the manipulation skills.

A Human-Like Learning Framework

3 3.1

609

Methodology Overview of the Framework

The scheme of the proposed framework is shown in Fig. 2. In the proposed learning framework, the human tutor presents a demonstration at first, and the measured pressing force, friction, velocity and position are recorded to initially model the environmental friction with an RBFNN. The neural network enhanced by BLS will be augmented during the LfD procedure. Finally, new motions are planned after solving the friction model, and the collected new data will be taken as a new demonstration for the repetitive training.

Fig. 2. Graphical representation of the overview of the proposed framework.

3.2

Broad Learning System

In the broad learning system (BLS), a flat network is utilized, where the original input vector is firstly transferred into a feature-mapping vector: F i = ξ1 (W f i X + β f i ) i = 1, · · · , n

(2)

where F i is the i-th element of the n-dimension feature-mapping vector, ξ1 is the transformation function, X is the x-dimension original input vector, W f i and β f i denote the weight coefficient and the bias coefficient respectively. Second, enhancement nodes are added to extract the feature of the input vectors better. E j = ξ2 (W ej X + β ej )

j = 1, · · · , m

(3)

where E j is the j’th element of the m-dimension feature-mapping vector, ξ2 denotes another transformation function, W ej and β ej denote the weight and bias coefficient respectively, which are randomly generated and then remained constant like those of the feature nodes. Eventually, the output vector is generated from the linear combination of all the feature nodes and enhancement nodes. The dynamic equation of BLS can be written as follow: Y = [F n |E m ] · W n+m

(4)

Here, Y denotes the output vector and W denotes the connection weight. The connection weight can be obtained by solving the pseudo-inverse directly, which greatly accelerates the training process.

610

3.3

H. Liu et al.

RBFNN with Broad Learning System

It has been indicated that radial basis function neural networks (RBFNN) can approximate arbitrary continuous functions f (X) with a small enough error if the number of neurons is large enough [19]: ˆ H(X) + (X), f (X) = W

∀X ∈ Ω X

(5)

where ΩX ⊂ Rx is a compact set, X represents the input vector of the continuous function and the neural network, (X) < ˆ denotes the estimation ˆ is the optimal connection weight that satisfies the error requireerror, and W ment. Here, H(X) = [h1 (X), h2 (X), · · · , hn (X)]T represents the radial basis functions (RBF) of all the neurons and Gaussian function is used in our work. It has been revealed by the research in [16] that human motor system can learn the environmental dynamics by constructing an internal model in the central nervous system (CNS). Here, an RBFNN with BLS is adopted to simulate the modeling process of the CNS. As illustrated in Fig. 3(a) and (b), dynamics information about the position, velocity, pressing force and their corresponding friction along the explored trajectories are collected and the corresponding friction of a new trajectory can be generated by an appropriate RBFNN. The appropriate RBFNN means its neurons positions should be settled suitably and its approximating capacity can be guaranteed within a corresponding compact set, which is well solved by the broad learning system. According to the properties of the Gaussian function, the input vectors far away from the central point of a neuron bring an extremely low impact in feature mapping, and the whole neurons, as a result, form a corresponding compact set Ω 0 . As shown in Fig. 3(c) and (d), the neural network has adequate approximation performance for the input vectors located inside the original compact set Ω 0 , but leads to poor performance for those outside. The broad learning system will augment a new neuron to accommodate the input vectors beyond the original compact set Ω 0 , and will accordingly extend Ω 0 to a larger compact set Ω 1 . According to some more recent researches, human motor learning system generate new skills from previous knowledge and current environment information [8,16], based on which the broad learning system updates the structure of the neural network by adding a neuron as necessary. [7] reduces the computation by directly considering the Euclidean distance when adding a Gaussian neuron, rather than calculating the activation [1]. Similarly, we allocate the centers and activations of the new added neurons as: ¯min = c

n  i=1

φmini

¯min + α(X(t) − c ¯min ) φnew = c

(6)

(7)

where φnew denotes the center of the newly added RBF (e.t. Gaussian function) neuron, α ∈ R is a tunable coefficient, X(t) is the vector representing the state

A Human-Like Learning Framework

611

Fig. 3. (a) shows the memorised trajectories in the input vector space. (b) is an elliptical trajectory generated by the broad learning system. (c) and (d) show the process of augmenting a new neuron.

¯min represents the mean position of the n encountered at time-step t, and c nearest neurons around X(t). Hence, the update law of the RBFNN for each time-step t is as follows:  ¯min ) > C [Φ(t − 1) φnew ], if dis(X(t), c (8) Φ(t) = Φ(t − 1), otherwise where Φ(t) = [φ1 , φ2 , · · · , φn ] represents the centers of all the RBF neurons, dis(∗) represents the Euclidean distance function, and C is a predetermined threshold. 3.4

Motion Planning from Accumulated Knowledge

With the help of the aforementioned RBFNN, a friction force field with respect to the position, velocity and pressing force is constructed, which will be updated each time the neural network is augmented. Given a multiple-input nonlinear function and an expected output, usually more than one solution can be found. Thus the next question is to find the suitable velocity and pressing force, [v, F N ]T , for each given position, which are taken as the inputs of the hybrid force/position controller. These [v, F N ]T corresponding to all the positions on the trajectory construct a motion planning map for the skills reproduction. In the proposed algorithm, an initial input vector is predesigned as: T  X 0 = p0 , v 0 , F N 0

(9)

612

H. Liu et al.

where p0 is the starting point of the prescribed position trajectory, v 0 and F N 0 should be set to suitable values according to the characteristic of the robot manipulator and the environment. To obtain a trajectory as smooth as possible in the vector space, the proposed algorithm calculates the [v, F N ]T for the next position by minimizing their changing rates with respect to p. With a predesigned sampling step-size, each position pi from the starting point to the end point leads to a corresponding [pi , v i , F N i ]T and the whole trajectory in the vector space is obtained. 3.5

Hybrid Force/Position Control Based on Position Control

Inspired by the hybrid force/position controller in [15], a hybrid force/motion controller is developed. The original controller of the Baxter is a position PD controller with gravity compensation, which is adopted in our framework to ensure the precise control of the sum from adding the two orthogonal control signals. As shown in Fig. 4, we take the tangential velocity v td and the desired pressing force F nd as input, which is different from the controller in [15]. The force error is taken as the input of the PI controller, and then we obtain v nd , the desired velocity in the normal direction. The desired position xd is obtained by integrating the sum of the two orthogonal velocity vectors. Finally, the control torque is generated by the PD controller.

Fig. 4. Structure diagram of the hybrid force/motion controller.

4

Experimental Validation

The Baxter Research Robot is used in our experiment. As illustrated in Fig. 2, an ATI Mini45 Force/Torque sensor is attached to the left arm of the Baxter Robot, and a sponge is fixed to the force sensor. At first, the human tutor drags the robot arm to wipe the surface with the sponge. Then, a task trajectory in Cartesian position space is given and the robot automatically repeats and improves the wiping skill for several times. The experiment is divided into three stages: 1) Motion planning with an expected friction and position trajectory: After the human demonstration, a position trajectory is given, which describes the trajectory of the wiping task in the Cartesian position space. As shown in

A Human-Like Learning Framework

613

Fig. 1, the task trajectory is 0.45 m long and spans two surfaces of different roughness. The friction is expected to remain at 2.5 N. As shown in Fig. 5, the learning framework improves the interaction skill by 30 times of repetitive training.

Friction (N)

5

Actual friction Expected friction

4 3 2 1 0

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

Friction (N)

5

0.4

0.45

Actual friction Expected friction

4 3 2 1 0

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

Position (m)

Fig. 5. The top panel shows the actual friction during the first robot execution after human demonstration, when the number of neurons in the RBFNN is 129. The bottom panel is the measured friction after 30 times repetitive training, and the number of neurons is added to 560.

2) Motion generalization with a different expected friction: The position trajectory remains unchanged, and the expected friction is set to 5.0 N, which validates the generalization ability for a different expected force. With the model trained for many times in the last stage and the capacity of learning from accumulated knowledge, the framework should achieve an equivalent performance within fewer times of repetitive training, as shown in Fig. 6. 3) Motion generalization for wider range of positions: The expected friction remains unchanged, and the required task trajectory is set to a longer one, e.t. a 0.65 m long trajectory spanning three surfaces of different roughness as shown in Fig. 1. With the model trained for several times in the last two stages, the framework should achieve an equivalent performance within fewer times of repetitive trainings, as illustrated in Fig. 7. Figure 8 shows the performance of the hybrid force/position controller.

614

H. Liu et al.

Friction (N)

8

Actual friction Expected friction

6 4 2

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

Friction (N)

8

0.4

0.45

Actual friction Expected friction

6 4 2

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

Position (m)

Fig. 6. The top panel shows the actual friction during the first robot execution after changing the expected friction. The bottom panel is the measured friction after 15 times repetitive training, and the number of neurons is augmented to 584.

Friction (N)

8

Actual friction Expected friction

7 6 5 4 3 -0.2

-0.1

0

0.1

0.2

0.3

Friction (N)

8

0.4 Actual friction Expected friction

7 6 5 4 3 -0.2

-0.1

0

0.1

0.2

0.3

0.4

Position (m)

Fig. 7. The top panel shows the actual friction during the first robot execution after lengthening the required task trajectory. The bottom panel shows the measured friction after 15 times repetitive training, when the number of neurons is augmented to 779.

615

0.07

25

0.06

Actual velocity Expected velocity Actual pressing force 20 Expected pressing force

Velocity (m/s)

0.05 15

0.04 0.03

10

0.02

Pressing force (N)

A Human-Like Learning Framework

5 0.01 0 -0.2

-0.1

0

0.1

0.2

0.3

0.4

0

Position (m)

Fig. 8. Velocity and pressing force tracking performance of the hybrid force/position controller.

5

Conclusion and Future Work

In this paper, we have developed a human-like learning from demonstration framework, which enables robots to learn interaction skills based on the environmental dynamics. The RBFNN with broad learning system is used to emulate the central nervous system of human, such that the robot can learn from the demonstration considering the environmental dynamics. Heuristic motion planning and execution are implemented to achieve better dynamic model and manipulation performance. The experiment shows that our framework can effectively satisfy the manipulation requirement, and the generalization ability is also validated. The proposed framework should have potential to be used in several applications such as contact tooling. Our next work is to improve the performance of the hybrid force/position controller and the computational efficiency of the broad learning system. Torque-based controllers can be used to improve the tracking accuracy of both the pressure and the velocity in such a contacting scenario, and adaptive controlling methods have good convergence speed in such a repetitive manipulating scenario. Online training methods for RBFNN with BLS will also be integrated to reduce the calculation time.

References 1. Barakat, M., Druaux, F., Lefebvre, D., Khalil, M., Mustapha, O.: Monitoring of rotary machine by mean of self adaptive growing neural network. In: 2011 19th Mediterranean Conference on Control & Automation (MED), pp. 132–137. IEEE (2011) 2. Calinon, S.: Robot Programming by Demonstration. EPFL Press, Lausanne (2009)

616

H. Liu et al.

3. Chen, C.P., Liu, Z.: Broad learning system: an effective and efficient incremental learning system without the need for deep architecture. IEEE Trans. Neural Netw. Learn. Syst. 29(1), 10–24 (2017) 4. Chib, V.S., Patton, J.L., Lynch, K.M., Mussa-Ivaldi, F.A.: Haptic identification of surfaces as fields of force. J. Neurophysiol. 95(2), 1068–1077 (2006) 5. Franklin, D.W., Liaw, G., Milner, T.E., Osu, R., Burdet, E., Kawato, M.: Endpoint stiffness of the arm is directionally tuned to instability in the environment. J. Neurosci. 27(29), 7705–7716 (2007) 6. Franklin, D.W., Wolpert, D.M.: Computational mechanisms of sensorimotor control. Neuron 72(3), 425–442 (2011) 7. Huang, H., Zhang, T., Yang, C., Chen, C.P.: Motor learning and generalization using broad learning adaptive neural control. IEEE Trans. Industr. Electron. 67(10), 8608–8617 (2019) 8. Karim, H.T., et al.: Motor sequence learning-induced neural efficiency in functional brain connectivity. Behav. Brain Res. 319, 87–95 (2017) 9. Keemink, A.Q., van der Kooij, H., Stienen, A.H.: Admittance control for physical human-robot interaction. Int. J. Robot. Res. 37(11), 1421–1444 (2018) 10. Li, Y., Ganesh, G., Jarrass´e, N., Haddadin, S., Albu-Schaeffer, A., Burdet, E.: Force, impedance, and trajectory learning for contact tooling and haptic identification. IEEE Trans. Rob. 34(5), 1170–1182 (2018) 11. Liu, Z., Zhou, J., Chen, C.P.: Broad learning system: feature extraction based on k-means clustering algorithm. In: 2017 4th International Conference on Information, Cybernetics and Computational Social Systems (ICCSS), pp. 683–687. IEEE (2017) 12. Muratori, L.M., Lamberg, E.M., Quinn, L., Duff, S.V.: Applying principles of motor learning and control to upper extremity rehabilitation. J. Hand Ther. 26(2), 94– 103 (2013) 13. Ortenzi, V., Stolkin, R., Kuo, J., Mistry, M.: Hybrid motion/force control: a review. Adv. Robot. 31(19–20), 1102–1113 (2017) 14. Song, P., Yu, Y., Zhang, X.: A tutorial survey and comparison of impedance control on robotic manipulation. Robotica 37(5), 801–836 (2019) 15. Wang, N., Chen, C., Di Nuovo, A.: A framework of hybrid force/motion skills learning for robots. IEEE Trans. Cognit. Dev. Syst. 13, 162–170 (2020) 16. Willingham, D.B.: A neuropsychological theory of motor skill learning. Psychol. Rev. 105(3), 558 (1998) 17. Yang, C., Zeng, C., Cong, Y., Wang, N., Wang, M.: A learning framework of adaptive manipulative skills from human to robot. IEEE Trans. Industr. Inf. 15(2), 1153–1161 (2018) 18. Yang, C., Zeng, C., Fang, C., He, W., Li, Z.: A DMPS-based framework for robot learning and generalization of humanlike variable impedance skills. IEEE/ASME Trans. Mechatron. 23(3), 1193–1203 (2018) 19. Yu, H., Xie, T., Paszczynski, S., Wilamowski, B.M.: Advantages of radial basis function networks for dynamic system design. IEEE Trans. Industr. Electron. 58(12), 5438–5450 (2011) 20. Zhu, Z., Hu, H.: Robot learning from demonstration in robotic assembly: a survey. Robotics 7(2), 17 (2018)

An AUV Path Planning Algorithm Based on Model Predictive Control and Obstacle Restraint Zhaoyang Liu, Daqi Zhu(B) , and Mingzhong Yan Shanghai Engineering Research Center of Intelligent Maritime Search and Rescue and Underwater Vehicles, Shanghai Maritime University, Haigang Avenue 1550, Shanghai 201306, China [email protected]

Abstract. An Obstacle Restraint Model predictive control (OR-MPC) path planning algorithm with obstacle restraints for Autonomous underwater vehicle (AUV) is presented in this paper. In order to avoid large-volume obstacles safely, model predictive control and obstacle restraints are combined in this paper. As obstacles are set as restricted areas, the underwater environment is divided into feasible areas and prohibited areas. With OR-MPC, speed increments are generated which are used to generate waypoint. Determine whether it meets the restraint. In the case where the restraint is satisfied, an angle is generated that enables the AUV to escape the restricted area. Compared with the artificial potential field (APF) path planning algorithm, the proposed OR-MPC can not only avoid large obstacles, but has the best path. Compared with MPC path planning, the proposed OR-MPC algorithm solves the collision problem. The simulation results demonstrate the effectiveness of the proposed control algorithm. Keywords: Autonomous underwater vehicle (AUV) · Path planning · Obstacle Restraint Model predictive control (OR-MPC) · Artificial potential field (APF) · Quantum particle swarm optimization (QPSO)

1 Introduction Over the past few decades, autonomous underwater vehicles (AUV) have been receiving significant amount of attention from academia and industry due to their great potential to reduce risks in deep oceanic exploration and exploitation of underwater resources [1]. With the deep development of the ocean, the path planning problem of the autonomous underwater vehicle (AUV) has received more and more attention in recent years. In path planning, it is usually just to find a feasible path from start to the goal [2]. It allows the AUV to independently plan a collision-free path from beginning to end based on the specific functions of the task requirements and constraints [3–5]. At present, many methods have been applied to the path planning of mobile robots. Many researchers introduce the mature ground mobile robot’s path planning algorithm into the underwater vehicle path planning, especially the artificial potential field path © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 617–627, 2021. https://doi.org/10.1007/978-3-030-89092-6_56

618

Z. Liu et al.

planning method, which is widely used in the field of ground mobile robot, has been the focus of many researchers [6–10]. Because of its advantages of simple mathematical analysis, low computational complexity, and a smooth path, the algorithm is widely adopted in the field of real-time obstacle avoidance and path planning [11]. MPC, as a research hotspot in recent years, has a strong adaptability to the nonlinear underwater environment [12]. In addition, MPC is a model-based closed-loop control optimization strategy, and the core of the algorithm is a dynamic model which can predict the future with the repeatedly optimal calculation on-line, scroll implementation control and the feedback correction [13–15]. The remainder of this paper is organized as follows: Second section describes the AUV kinematics model. The third part describes the construction of OR-MPC based on the AUV kinematics model, and the processing of obstacle constraints. In Sect. 4, the OR-MPC and APF path planning are compared for simulation analysis to verify the feasibility of the algorithm. Section 5 provides some conclusive remarks for the entire paper.

2 AUV Kinematics Model We usually divide the AUV coordinate system into an inertial coordinate system and a carrier coordinate system. The position and attitude angle of the AUV spatial position in the inertial coordinate system can be expressed as η. The velocity vector of the carrier coordinate system is ν. Then the relationship between the inertial coordinate system and the velocity vector of the carrier coordinate system is: η˙ = J (η)ν

(1)

Where J ∈ R6×6 , represents the conversion matrix from the carrier coordinate system to the inertial coordinate system. And it is also known as motion coordinate as shown in Fig. 1.

Fig. 1. The coordinate system

An AUV Path Planning Algorithm

619

According to the coordinate transformation in Fig. 1. The kinematics equation of AUV is shown in formula (1), and each sampling point on the reference path satisfies the kinematics equation. At its d-th reference point, the relationship between its position and direction and speed is: η˙ d = J (ηd )νd

(2)

Since the state variables and control variables of the reference system at each time have been given, this chapter adopts a linearization method to transform the AUV non-linear system into a linear system. Formula (2) performs Taylor expansion at the d-th reference point, and ignores higher-order terms, and obtains the following formula:   ∂J(η)v  ∂J(η)v  η˙ = J (ηd )Vd + (η − η ) + (v − vd ) (3) d ∂η  η=ηd ∂v  η=ηd v=vd v=vd Combine formulas (2) and (3) and linearize them to get:    ∂J (η)ν  ˆη˙ = η˙ − η˙ d = ∂J (η)ν  (η − ηd ) + (ν − νd ) ∂η  η=ηd ∂η  η=ηd ν=νd ν=νd ⎤ ⎤⎡ ⎡ x − xd 0 0 0 −ud sin ψd − νd cos ψd ⎢ 0 0 0 ud cos ψd − νd sin ψd ⎥⎢ y − yd ⎥ ⎥ ⎥⎢ =⎢ ⎦⎣ z − zd ⎦ ⎣0 0 0 0 ψ − ψd 000 0 ⎡ ⎤ ⎤⎡ cos ψd − sin ψd 0 0 u − ud ⎢ sin ψd cos ψd 0 0 ⎥⎢ v − vd ⎥ ⎥ ⎥⎢ +⎢ ⎣ 0 0 1 0 ⎦⎣ w − wd ⎦ r − rd 0 0 01

(4)

In order to facilitate processing, discretize formula (4) to obtain: η(k ˆ + 1) = Aη(k) ˆ + Bνˆ (k)

(5)

For path planning, since it is assumed that the target point is known, the first reference point usually takes the point closest to the current position in the direction of the target point as the reference point. At each sampling moment, since the sampling point on the reference trajectory satisfies formula (2). Therefore, take the first reference point as the sampling point and satisfy formula (4). In path planning, the reference trajectory is set as the target point, and its reference speed ud and νd are all zero, so: ⎤ ⎡ ⎤ ⎡ 1000 T cos ψg −T sin ψg 0 0 ⎢0 1 0 0⎥ ⎢ T sin ψg T cos ψg 0 0 ⎥ ⎥ ⎥ ⎢ A=⎢ ⎣ 0 0 1 0 ⎦, B = ⎣ 0 0 T 0⎦ 0001 T is the sampling time.

0

0

0T

620

Z. Liu et al.

3 OR-MPC Path Planning In practical applications of path planning, because the obstacles are often uncertain, when the AUV encounters a symmetrical obstacle, the AUV easily falls into a local extreme and cannot obtain the next path, which results in jitter. In addition, due to the different sizes of actual obstacles, when encountering large obstacles, due to the different personal choices of the repulsion coefficient, the planned path will also collide with the obstacle. Such as the artificial potential field algorithm by shown in Fig. 2.

Fig. 2. Problems with APF path planning

In order to solve the collision problem between local extremum and large obstacles, an OR-MPC trajectory planning method is proposed in this paper. 3.1 Method of Setting Obstacle Restraint In this section, obstacles will be restricted to a certain range through linear programming and the restricted range will be regarded as a no-passing area. The following is a brief introduction to several typical obstacle restraint treatments: The Setting of Rule Obstacle Restraints In a two-dimensional (2D) environment, mathematical functions are applied to represent the boundaries of obstacles. However, these used boundary functions do not consider safety margins. Under the premise of considering the actual safety margin, this restraint method is proposed to restrain the obstacle within a specific range. For obstacles similar to regular graphics, such as rectangles and triangles, the proposed method can be used to

An AUV Path Planning Algorithm

621

set obstacle restraints. For example, formula (6) and formula (7) can be used to restrict obstacles similar to regular graphics (Fig. 3). X > 5.8 & X < 10.2 & Y > 5.8 & Y < 10.2

(6)

Y > 21.8 & Y < −2X + 62.4 & Y < 2X − 9.8

(7)

Fig. 3. Obstacle restraint

Setting of Irregular Obstacle Restraints Assuming that a figure with more sides than a quadrilateral is defined as a polygon. Such polygons are very complicated in mathematical expressions. It is not suitable for using linear programming to set restraints. Therefore, the setting of irregular obstacle restraints is simplified. For example, the restraint setting of a hexagon can be handled by a method similar to the setting of a rectangular restraint range. The schematic diagram of the restraint setting can be represented as Fig. 4. The linear programming constraint processing method is used in a three-dimensional environment (3D). Based on the plane restraint processing method, the Z plane restraint is used to complete the obstacle restraint setting in 3D environment. 3.2 OR-MPC Path Planning Algorithm Principle The principle of the OR-MPC path planning algorithm is mainly composed of three parts, namely the prediction model, the QPSO optimization objective function model, and the obstacle constraint conditions. The algorithm schematic diagram can be shown in Fig. 5.

622

Z. Liu et al.

Fig. 4. Obstacle restraint

Fig. 5. OR-MPC algorithm schematic diagram

OR-MPC path planning can be described as follows: 1. In the prediction time domain, the objective function is optimized to obtain the speed increment, which is used to generate waypoints. 2. Obstacle restraints are used to determine whether the waypoints in the control time domain meet the restraints, and the waypoints that meet the restraints are used to act on the control system. 3. The waypoint that satisfies the restraint is generated error with the target point, which is used for rolling optimization to generate speed increment.

An AUV Path Planning Algorithm

623

The position of the target point G and the initial reference waypoint is known. According to the linear error model and target points in Sect. 2, the objective function as shown in formula (8) is established: J =

Np

G − ηd (k

i=1

+ i|t)2Q

+

N c −1

νd (k + i|t)2R

(8)

i=1

where ηd (t) can be expressed as the waypoint obtained by the solution at the current moment, and vd (t) is the velocity increment obtained by the solution at the next moment. The optimization stage is to use QPSO to minimize the objective function, and the fitness function in QPSO can be expressed as: min vd (t) · H · vd (t)T + f · vd (t)T vd (t)

(9)

 



Where H = T Q + R 0 , f = 2e(t)T Q 0 , e(t) = G − ηd (t).  is the combination of A matrix and B matrix in the AUV kinematics model. It can be expressed as: ⎤ ⎡ B 0 0 0 ⎥ ⎢ AB B 0 0 ⎥ ⎢ ⎥ ⎢ .. ⎥ ⎢ . ··· ··· ⎥ ⎢ ··· (10)  = ⎢ Nc ⎥ ⎥ ⎢ A B AN c-1 B · · · AB ⎥ ⎢ .. .. ⎥ ⎢ .. .. . ⎦ ⎣ . . . AN p-1 B AN p-2 B · · · AN p-Nc-1 B When the actual AUV is working underwater, the AUV speed and speed increment can only be changed within a certain range, and it is impossible to increase its speed indefinitely. Therefore, it is necessary to add speed constraints in the QPSO optimization solution: ⎧ −2 m/s ≤ ud ≤ 2 m/s ⎪ ⎪ ⎨ −2 m/s ≤ vd ≤ 2 m/s ⎪ −2 m/s ≤ wd ≤ 2 m/s ⎪ ⎩ −1 rad/s ≤ rd ≤ 1 rad/s The algorithm flow of OR-MPC path planning can be expressed as:

624

Z. Liu et al.

4 Simulation Results MATLAB is used to simulate and verify the algorithm proposed in this article. The simulation includes: by comparing APF and OR-MPC path planning algorithms, ORMPC can not only solve the local extremum and collision problems of APF. In the experiment, the APF attraction coefficient is selected as 5, the repulsion coefficient is selected as 15, and the obstacle influence distance is selected as 0.5 m. The AUV starting point in the two-dimensional (2D) environment is [0, 0, 0], and the target point is [20, 20, 0.7854]. Since the four degrees of freedom of the AUV need to be controlled in a three-dimensional (3D) environment, the vertical coordinate system Z axis needs to be added. The starting point of the AUV in the 3D environment is [0, 0, 0, 0], and the target point is [20, 20, 20, 0.7854]. The path planning in a two-dimensional environment should take the x-axis (surge), y-axis (sway) and AUV turning angle (yaw) as degrees of freedom, so that the AUV can realize path planning in the 2D environment. Figure 6 shows the path planning using APF and OR-MPC respectively in 2D environment. By contrast, due to the existence of symmetrical obstacles and large obstacles in the actual environment, APF path planning

An AUV Path Planning Algorithm

625

falls into a local extreme and causes collisions with obstacles, while OR-MPC does not have this problem. It can be shown that OR-MPC path planning can overcome the collision caused by APF due to excessive obstacles, and it can also overcome the problem of AUV easily falling into local extremum when encountering symmetrical obstacles.

Fig. 6. Path planning in two dimensions

Like the 2D APF path planning algorithm, the 3D APF path planning algorithm is likely to cause collisions or fall into local extreme problems when encountering large obstacles and symmetrical obstacles. Figure 7 shows the path planning in a 3D situation. It can be seen from the figure that the use of OR-MPC path planning can achieve a collision-free path from the starting point to the target point. Compared with APF path planning, OR-MPC path planning can overcome the collision problem caused by

Fig. 7. Path planning in 3D environment

626

Z. Liu et al.

excessive obstacles in APF. In addition, OR-MPC path planning does not fall into the local extreme problem like APF path planning.

5 Conclusion The OR-MPC path planning method is proposed in this paper, and the planned path is the collision-free optimal path. By using the proposed algorithm, the problem of improper selection of APF repulsion coefficient is solved. At the same time, OR-MPC path planning solves the local extremum problem in APF path planning. In addition, by setting the system speed constraint, the speed is limited within a certain range, so that when tracking the actual path, the speed is used as the reference component to realize the trajectory tracking of the AUV.

References 1. Shen, C., Shi, Y., Buckham, B.: Integrated path planning and tracking control of an AUV: a unified receding horizon optimization approach. IEEE/ASME Trans. Mechatron. 22(3), 1163–1173 (2017) 2. Chu, Z., Zhu, D.: Obstacle avoidance path planning and path tracking control for autonomous underwater vehicles. In: 13th World Congress on Intelligent Control and Automation (WCICA), Changsha, China, pp. 450–454 (2018) 3. Devaurs, D., Siméon, T., Cortés, J.: Optimal path planning in complex cost spaces with sampling-based algorithms. IEEE Trans. Autom. Sci. Eng. 13(2), 415–424 (2016) 4. Cao, X., Yu, A.L.: Multi-AUV cooperative target search algorithm in 3-D underwater workspace. J. Navig. 70(6), 1293–1311 (2017) 5. Sun, B., Zhu, D.: Three dimensional D∗Lite path planning for Autonomous Underwater Vehicle under partly unknown environment. In: 12th World Congress on Intelligent Control and Automation (WCICA), Guilin, pp. 3248–3252 (2017) 6. Ghazaei Ardakani, M.M., Olofsson, B., Robertsson, A., Johansson, R.: Model predictive control for real-time point-to-point trajectory generation. IEEE Trans. Autom. Sci. Eng. 16(2), 972–983 (2019) 7. Cheng, C., Zhu, D., Sun, B., Chu, Z., Nie, J., Zhang, S.: Path planning for autonomous underwater vehicle based on artificial potential field and velocity synthesis. In: IEEE 28th Canadian Conference on Electrical and Computer Engineering (CCECE), Halifax, Canada, pp. 717–721 (2015) 8. Chen, M., Zhu, D.: An improved belief function method for path planning of AUV. In: 29th Chinese Control and Decision Conference (CCDC), Chongqing, pp. 1249–1254 (2017) 9. Jianjun, Y., Hongwei, D., Guanwei, W., Lu, Z.: Research about local path planning of moving robot based on improved artificial potential field. In: 25th Chinese Control and Decision Conference (CCDC), Guiyang, pp. 2861–2865 (2018) 10. Zhu, D., Yang, Y., Yan, M.: Path planning algorithm for AUV based on a Fuzzy-PSO in dynamic environments. In: 8th International Conference on Fuzzy Systems and Knowledge Discovery (FSKD), Shanghai, pp. 525–530 (2011) 11. Yao, Q., et al.: Path planning method with improved artificial potential field—a reinforcement learning perspective. IEEE Access 8, 135513–135523 (2020) 12. Zhao, H., Zhu, D.: UUV path tracking control with fault tolerant based on MPC. In: Chinese Control and Decision Conference (CCDC), Hefei, China, pp. 2403–2408 (2020)

An AUV Path Planning Algorithm

627

13. Sun, B., Gan, W., Zhu, D., Zhang, W., Yang, S.X.: A model predictive based UUV control design from kinematic to dynamic tracking control. In: 36th Chinese Control Conference (CCC), Dalian, pp. 4713–4718 (2020) 14. Kim, J.C., Pae, D.S., Lim, M.T.: Obstacle avoidance path planning algorithm based on model predictive control. In: 18th International Conference on Control, Automation and Systems (ICCAS), Pyeongchang, Korea (South), pp. 141–143 (2018) 15. Zhang, X., Ma, J., Huang, S., Cheng, Z., Lee, T.H.: Integrated planning and control for collision-free path generation in 3D environment with obstacles. In: 19th International Conference on Control, Automation and Systems (ICCAS), Jeju, Korea (South), pp. 974–979 (2019)

Quantum-Behaved Particle Swarm Optimization Fault-Tolerant Control for Human Occupied Vehicle Huapeng Zhang and Daqi Zhu(B) Shanghai Engineering Research Center of Intelligent Maritime Search and Rescue and Underwater Vehicles, Shanghai Maritime University, Haigang Avenue 1550, Shanghai 201306, China [email protected]

Abstract. In this paper, for the human occupied vehicle (HOV) system, the control strategies are used to reallocate the thruster forces based on quantum-behaved particle swarm optimization (QPSO). QPSO is adopted in terms of the solution quality, robustness and the convergence property. When the normalized thruster forces are out of maximum limits, the QPSO is used for the restricted usage of the faulty thruster and to find the solution of the control reallocation problem within the limits. An optimization criterion with the infinite norm as the cost function is introduced into the QPSO algorithm accelerate the search for the optimal solution in the feasibility space so as to ensure the feasibility of the solution. To show the efficiency of QPSO fault-tolerant control, GA, PSO and pseudo inverse method were conducted to study the proposed fault-tolerant control method. Experimental results showed the proposed fault-tolerant method could reallocate the thruster forces effective after thruster fault. Keywords: Human occupied vehicle · Quantum-behaved particle swarm optimization · Thruster fault-tolerant control

1 Introduction At present, deep sea manned submersible is one of the frontiers and commanding heights of marine development. Its level can reflect a country’s comprehensive scientific and technological strength. In the complicated ocean environment, once a manned submersible fault occurs, it cannot accomplish its mission and may even be lost. Thruster failure as one of the most common and important fault sources [1, 2], once a failure occurs, the consequences will be very serious, so, fault-tolerant control of thruster is necessary. The algorithm of General Inverse (GI) is a common method for the solution of the control reallocation problem [3, 4]. But the GI approach cannot handle the constrained control problem since the solution of this approach is only based on the attainable command set. To handle such cases where attainable control inputs cannot be allocated, Tapproximation (Truncation) and S-approximation (Scaling) methods were proposed [5]. But the magnitude and direction errors caused by approximation still exist and the HOV © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 628–637, 2021. https://doi.org/10.1007/978-3-030-89092-6_57

Quantum-Behaved Particle Swarm Optimization Fault-Tolerant Control

629

cannot follow the desired trajectory completely. In order to achieve effective trajectory tracking, an adaptive sliding mode backstepping fault-tolerant control is proposed for autonomous underwater vehicle (AUV) with thruster faults [6]. For a class of remotely operated vehicles (ROVs) with redundant thrusters, an adaptive fault-tolerant controller was addressed in [7]. For the Odin AUV the quantum-behaved particle swarm optimization (QPSO) method is applied to the control reallocation problem and a thruster fault control (FC) method is proposed, the maximum norm is selected as the optimization criterion [8]. In this paper, QPSO is adopted in terms of the solution quality, robustness and the convergence property and used for the restricted usage of the faulty thruster and to find the solution of the control reallocation problem within the available limits. This paper is organized as follows. In Sect. 2, the model of HOV is established. In Sect. 3, describes the thruster fault-tolerant control. In Sect. 4, the simulation results of thruster fault-tolerant control are shown. Finally, concluding is given in Sect. 5.

2 Modeling of HOV Here, the JIAOLONG manned submersible is as a research object in fault-tolerant control. In this paper, the model is decoupled and controllable in 4-degree of freedom (DOF), which is surge, sway, yaw and heave. 2.1 Kinematic Modeling The coordinate system of HOV is divided into dynamic coordinate system and static coordinate system. HOV coordinate systems are shown in Fig. 1.

Fig. 1. HOV coordinates system

The kinematic model of the HOV is the relationship between the dynamic coordinate system, the static coordinate system and the speed vector of: η˙ = J (η)v where the coordinate transformation matrix   J1 O3×3 J (η) = O3×3 J2

(1)

(2)

630

H. Zhang and D. Zhu

where,

⎡

cos ψ cos θ cos ψ sin θ sin ϕ − sin ψ cos ϕ J1 (η) = ⎣ sin ψ sin θ sin ψ sin θ sin ϕ + cos ψ cos ϕ − sin θ cos θ sin ϕ ⎤ cos ψ sin θ cos ϕ + sin ψ sin ϕ sin ψ sin θ sin ϕ + cos ψ cos ϕ ⎦ cos θ sin ϕ ⎤ 1 tan θ sin ϕ cos ϕ tan θ J2 (η) = ⎣ 0 cos ϕ − sin ϕ ⎦. 0 sin ϕ/ cos θ cos ϕ/ cos θ ⎡

For the 4-DOF JIAOLONG HOV, ϕ = 0, θ = 0, so, the kinematics transformation equation is: ⎡

⎤ ⎡ x˙ cos ψ − sin ψ ⎢ y˙ ⎥ ⎢ sin ψ cos ψ ⎥ ⎢ η˙ = ⎢ ⎣ z˙ ⎦ = J (η)V = ⎣ 0 0 ψ˙ 0 0

⎤⎡ ⎤ 00 u ⎥ ⎢ 0 0 ⎥⎢ v ⎥ ⎥ 1 0 ⎦⎣ w ⎦ 01 r

(3)

2.2 Dynamics Model HOV motion control is mainly relevant to gravity, buoyancy, thrust, hydrodynamic force, disturbing force and various torques associated with these forces. Under the action of resultant force and resultant moment generated from these forces and moments, the nonlinear dynamic equation of HOV under 6-DOF is as following: M v˙ + C(v)v + D(v)v + g(η) = τ

(4)

where M = MRB + MA ∈ R6×6 is an inertial matrix of HOV, MRB is an inertial matrix of HOV itself, MA is a hydrodynamic additional inertial matrix. C = CRB + CA ∈ R6×6 is a centripetal force and Coriolis matrix, CRB is a rigid term of HOV. CA is an additional term caused by associated mass. D(v) =∈ R6×6 is a hydrodynamic damping matrix. g(η) ∈ R6×1 is a gravity and buoyancy term. τ ∈ R6×1 is the forces/moments acted on the gravity center of HOV. It is assumed that the origin of the dynamic coordinate system coincides with the center of gravity of the submersible. The inertial matrix of HOV itself: ⎤ ⎡ m0 0 0 0 0 ⎢0 m 0 0 0 0 ⎥ ⎥ ⎢ ⎥ ⎢ 0 0 ⎥ ⎢0 0 m 0 (5) MRB = ⎢ ⎥ ⎢ 0 0 0 Ix −Ixy −Ixz ⎥ ⎥ ⎢ ⎣ 0 0 0 −Iyx Iy −Izy ⎦ 0 0 0 −Izx −Izy Iz

Quantum-Behaved Particle Swarm Optimization Fault-Tolerant Control

The hydrodynamic additional inertial matrix as: ⎡ Xu˙ Xv˙ Xw˙ Xp˙ ⎢Y Y Y Y ⎢ u˙ v˙ w˙ p˙ ⎢ ⎢Z Z Z Z MA = −⎢ u˙ v˙ w˙ p˙ ⎢ Ku˙ Kv˙ Kw˙ Kp˙ ⎢ ⎣ Mu˙ Mv˙ Mw˙ Mp˙ Nu˙ Nv˙ Nw˙ Np˙

Xq˙ Yq˙ Zq˙ Kq˙ Mq˙ Nq˙

⎤ Xr˙ Yr˙ ⎥ ⎥ ⎥ Zr˙ ⎥ ⎥ Kr˙ ⎥ ⎥ Mr˙ ⎦ Nr˙

631

(6)

In consideration of the speed of the submersible in the water is relatively slow, in this paper, the centripetal force and Coriolis matrix C will be ignored. Hydrodynamic damping matrix can be divided into friction resistance matrix DL and viscous pressure resistance matrix DQ . D(v)v = DL v + DQ diag(|v|)v

(7)

3 Thruster Fault-Tolerant Control In this section, the problem is extended to the thruster fault condition. In this work, the fault information is assumed to be known by some fault diagnosis method. The main work is to propose a fault-tolerant control based on QPSO and weighted pseudoinverse strategy to generate the demanded control for HOV thruster. The block diagram of thruster fault-tolerant control for the JIAOLONG HOV can be found in Fig. 2 and it should be noted that this control strategy can be applied to different kind of HOVs in a similar way. 3.1 Thrusters Completely Malfunctioning Fault-free thruster normalized thrust formula is: τ =B ∗ T

(8)

The JIAOLONG HOV normalization thruster configuration matrix with no fault is [3]: ⎡

⎤ 0.9239 0.9239 0.9239 0.9239 1 1 0 0 ⎢ 0 0.3827 0 −0.3827 0 0 1 0 ⎥ ⎢ ⎥ ⎢ ⎥ 0 0.3827 0 0 0 0 0⎥ ⎢ −0.3827 B=⎢ ⎥ ⎢ 0 0 0 0 0 0 0.3 0 ⎥ ⎢ ⎥ ⎣ −0.4302 0 0.4302 0 0 0 0 1⎦ 0 −0.5226 0 0.5226 −0.3 0.3 0.6 0 When only one thruster (the ith thruster) is faulty, deletes the corresponding column of thruster configuration matrix B, and then B turns into a 6 × 7 matrix Bi . It means that the solution space is not unique. The optimal solution can be achieved through QPSO optimization method which is given in detail in Sect. 3.3. When two thrusters (the ith and jth thruster) are faulty, delete the corresponding column of thruster configuration matrix, and then configuration matrix turns into a 6 × 6 matrix Bij . This leads to a unique 6 × 1 solution vector T (the ith and jth broken thruster output T i and T j are deleted from −1

the vector). The thruster solution can be obtained from T = Bi τ .

632

H. Zhang and D. Zhu

3.2 Thruster Partly Malfunctioning Hybrid Thruster Reconfiguration Strategy. As can be seen from the block diagram of thruster fault-tolerant control in Fig. 2, the hybrid thruster control reconfiguration approach can be described as follows: First, the actual normalization control force/moment τ is calculated by the weighted pseudo-inverse to generate the normalization thruster force T . If all of the normalized thruster output is in the range of saturation point −1 ≤ T ≤ 1, the QPSO method is not needed in this case. If any of the normalized thruster output is out of the saturation range −1 ≤ T ≤ 1, then QPSO method is applied so that the demanded controller output can be reallocated among the functioning thruster. Weighted Pseudo-Inverse Method. In order to account for the faulty thrusters, a diagonal weighting matrix can be used for the thruster fault-tolerant control. The diagonal weighting matrix can be defined as:

W = diag(w1 w2 w3 w4 w5 w6 w7 w8 ) ∈ R8×8

0 < wi ≤ 1, if the thruster is in partial failure, i = 1 ∼ 8 wi = 1, if the ith thruster is not in failure

(9)

where the weighting coefficient wi represents the fault degree of the thruster. The bigger the wi , the smaller the fault and vice versa. “1” and “0” is the upper and lower saturation limits of faulty thrusters to meet the available thrust capacity.

Fig. 2. The block diagram of thruster fault-tolerant control

QPSO Algorithm. In the QPSO model, the state of a particle is depicted by wave function instead of position and speed. The dynamic behavior of the particle is widely divergent from the particle in traditional particle swarm optimization (PSO) systems

Quantum-Behaved Particle Swarm Optimization Fault-Tolerant Control

633

since the exact values of x and V cannot be determined simultaneously. The objective function is defined by the l∞ norm of the thruster force. The l∞ norm of the thruster force T = [T 1 T 2 T 3 T 4 T 5 T 6 T 7 T 8 ] is defined as: ||T ||∞ = max{|T 1 | |T 2 | |T 3 | |T 4 ||T 5 | |T 6 | |T 7 | |T 8 |}

(10)

For equation τ = BW T , it can be written as: τ X = 0.9239(w1 T 1 + w2 T 2 + w3 T 3 + w4 T 4 ) + w5 T 5 + w6 T 6 τ Y = 0.3827(w2 T 2 − w4 T 4 ) + w7 T 7 τ Z = 0.3827(w3 T 3 − w1 T 1 ) τ K = 0.3w7 T 7 τ M = 0.4302(w3 T 3 − w1 T 1 ) + w8 T 8 τ N = 0.5226(w4 T 4 − w2 T 2 ) + 0.3(−w5 T 5 + w6 T 6 + 2w7 T 7 )

(11)

The formula expression can be given as follows: min isize||T ||∞ , subject to τ = BW T , −1 ≤ Ti ≤ 1, i = 1 ∼ 8 The flow chart of QPSO is shown in Fig. 3.

Fig. 3. The flow chart of QPSO

(12)

634

H. Zhang and D. Zhu

4 Simulation and Discussion To test the proposed algorithm, simulation study was conducted on JIAOLONG. Genetic algorithm (GA), PSO and pseudo inverse method were conducted to study the proposed fault-tolerant control method. In this section, the thruster fault information is set to W = diag(0.6 0.6 1 1 1 1 1 1) (1HT, 2HT thrust loss to 60% of normal thrust). 4.1 Comparison with Pseudo Inverse Method For the thruster fault-tolerant control of JIAOLONG HOV is in the case of HOV failure, reconstruct new thruster control signal. Thus, the force/moment generated by the faulty thruster can still meet the requirements τ = τd . Here, the expected force/moment set as τd = [0.3 0.2 0.3 0.2 0.1 0.1]. For equation τ = BW T the weighted pseudo-inverse solution is: T = [−0.5680 −0.9309 0.4432 0.6609 1.6176 −1.5068 0.6667 −0.2373]. Obvious, the solution is unfeasible, because |T 5 |, |T 6 | > 1. The normalized reconstruction thrust and actual force/moment produced by S-approximation, T-approximation are as follows: T S = [−0.5680 −0.9309 0.4432 0.6609 1 −1 0.6667 −0.2373] T T = [−0.3511 −0.5755 0.274 0.625 1 −0.9315 0.4122 −0.1470] τ S = [0.1892 0.2 0.3 0.2 0.1 0.4373] τ T = [0.3860 0.0411 0.1855 0.1237 0.0615 0.1746] In contrast, the solution obtained by the QPSO is: T QPSO = [0.6691 0.6363 1 0.0112 −0.196 0.4389 0.1717 −0.1026] τ QPSO = [0.3 0.2 0.3 0.2 0.1 0.1] The solution is feasible without any approximation because T is limited in [−1, 1] during the process of searching for a solution in the QPSO algorithm. Corresponding, τ = [τX τY τZ τK τM τN ] = τ d = [τXd τYd τZd τKd τMd τNd ]. It can be seen that the actual force/moment obtained by the QPSO algorithm can reallocate the thruster forces reasonably compared with the method of weighted pseudoinverse and can precisely meet the desired control forces/moments. The comparison result is illustrated in Fig. 4 which clearly shows the reconfiguration effect. The solutions obtained by different methods could perform the appropriate configuration, but the solutions obtained by weighted pseudo-inverse still have some error in magnitude, while the QPSO algorithm can preserve the original magnitude within thruster limitations.

Quantum-Behaved Particle Swarm Optimization Fault-Tolerant Control

635

Fig. 4. The result of thruster fault case

4.2 Comparison of Fault-Tolerant Control Algorithms In this part, the fault-tolerant control effects of QPSO, PSO, GA and pseudo inverse method are given respectively for JIAOLONG HOV. And the fault-tolerant control performance is compared and analyzed to verify that the thrust distribution ability of QPSO algorithm can solve the problem of thrust saturation. For QPSO, PSO and GA, the parameters are set as Table1. For the JIAOLONG HOV. Figure 5 shows the fitness value of QPSO, PSO, GA and pseudo inverse method. Fitness value is the error of actual force/moment τ = [τX τY τZ τK τM τN ] (after thruster failure) and the expected force/moment τd = [τXd τYd τZd τKd τMd τNd ]. The smaller is the fitness value, the better is the fault-tolerant control effect. The status of control variable convergence of QPSO, PSO, and pseudo inverse method are given respectively in Fig. 6. The expected force/moment set as τd = [0.3 0.2 0.3 0.1 0.1 0.1]. From the above results, it can clearly see that the effect of QPSO is better than other algorithm. For PSO, PSO algorithm can’t guarantee convergence to global optimal solution, which leads to the repeated oscillation occurs. Obviously, there is a bigger error than QPSO. In comparison with GA, QPSO has fast convergence speed and fewer parameters need to be adjusted, which is also easy to program and run fast. For the pseudo inverse method, if the weighted pseudo inverse solution is out of range. This method will produce magnitude and direction errors. In contrast, QPSO can ensure fast search speed and global convergence.

636

H. Zhang and D. Zhu

Fig. 5. Fitness value

Fig. 6. Control variable convergence

Table 1. The control parameters for fault-tolerant Parameter

Method GA-MPC

PSO-MPC

QPSO-MPC

popsize

50

50

50

MAXITER

50

50

50

Pc

0.99

/

/

Pm

0.01

/

/

5 Conclusion This paper deals with the thrust allocation problem under thruster fault of HOV. A different control strategy for various thruster fault cases is presented while the major contribution is a mixed control method of weighted pseudo-inverse and quantum-behaved

Quantum-Behaved Particle Swarm Optimization Fault-Tolerant Control

637

particle swarm optimization for fault thruster reallocation. QPSO is selected for the fast convergence and global search capability. An optimization criterion with the infinite norm as the cost function is introduced into the QPSO algorithm accelerate the search for the optimal solution in the feasibility space so as to ensure the feasibility of the solution. Experimental results showed the proposed fault-tolerant method could reallocate the thruster forces effective after thruster fault.

References 1. Zhang, Y., Jiang, J.: Bibliographical review on reconfigurable fault-tolerant control systems. Ann. Rev. Control 32, 229–252 (2008) 2. Valdes, A., Khorasani, K.: A pulsed plasma thruster fault detection and isolation strategy for formation flying of satellites. Soft Comput. 10(3), 746–758 (2010) 3. Omerdic, E., Roberts, G.: Thruster fault diagnosis and accommodation for open-frame underwater vehicles. Control Eng. Pract 12, 1575–1598 (2004) 4. Benosman, M., Kai-Yew, L.: Online references reshaping and control reallocation for nonlinear fault-tolerant control. IEEE Trans. Control Syst. Technol. 17, 366–379 (2009) 5. Zhu, D.Q., Liu, Q., Hu, Z.: Fault-tolerant control algorithm of the manned submarine with multithruster based on quantum-behaved particle swarm optimization. Int. J. Control 84, 1817– 1829 (2011) 6. Wang, Y., Zhang, M., Chu, Z., Liu, X.: Fault-tolerant control based on adaptive sliding mode for underwater vehicle with thruster fault. In: The 11th World Congress on Intelligent Control and Automation (2014) 7. Chu, Z.D., Luo, C., Zhu, D.Q.: Adaptive fault-tolerant control for a class of remotely operated vehicles under thruster redundancy. In: 2018 IEEE 8th International Conference on Underwater System Technology: Theory and Applications (2018) 8. Sun, B., Zhu, D.Q., Yang, S.X.: A novel tracking controller for autonomous underwater vehicles with thruster fault accommodation. J. Navig. 69, 593–612 (2016)

Path Planning Algorithm of Mobile Robot Based on Dichotomy of Thermal Conduction Topology Optimization Yuanyang Fang, Pengyuan Qi, Baotong Li, and Xiaohu Li(B) School of Mechanical Engineering, Key Laboratory of Education Ministry for Modern Design and Rotor-Bearing System, Xi’an Jiaotong University, No. 28, Xianning West Road, Beilin District, Xi’an, Shannxi, China [email protected]

Abstract. To solve the problem of low efficiency and unstable results of the path planning algorithm based on topology optimization, a dichotomy-based heat conduction topology optimization mobile robot path planning algorithm is proposed. First, considering the analogy between the heat transferring path under steady state and the mobile robot navigation path, the path planning problem is equivalent to the heat conduction topology optimization problem, and a path planning method based on the heat conduction topology optimization is constructed. Secondly, according to the principle of minimizing the thermal compliance across the analysis domain in each growth step, the optimal growth direction of the cooling channel is determined. Third, define the 8 search directions of the mobile robot’s motion, construct the cooling channel material library in the corresponding direction, and the dichotomy can be used to solve the problem quickly. Finally, through simulation and comparison experiments in two environments, the results show that the path planning algorithm based on heat conduction topology optimization has strong real-time performance, stable results, and has advantages in complex environments. Keywords: Path planning · Heat conduction · Topology optimization · Single-step growth · Dichotomy

1 Introduction Path planning is one of the key technologies to realize the autonomous motion of mobile robots, and it is also a hot topic in the research of mobile robot control technology. In recent years, RRT (Rapid-exploration Random Tree) path planning algorithm has attracted much attention, but due to its random search characteristics, its convergence speed is slow in complex environments [1]. Many scholars have proposed improved This work is partially supported by the Key R&D projects in Chongqing Province, Development and application of key technologies of road tunnel intelligent inspection system (cstc2019jscxfxydX0017). © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 638–648, 2021. https://doi.org/10.1007/978-3-030-89092-6_58

Path Planning Algorithm of Mobile Robot

639

methods based on RRT, such as RRT-Bias [2], which selects the target point with a certain probability as the expansion direction, which can effectively accelerate the convergence speed of the algorithm. The RRT* algorithm adds the process of reselecting the parent node and rewiring in the process of node searching, which can continuously optimize the path [3]. Tan [4] proposed a method based on heuristic sampling and weighted leaf node deletion, which could make the RRT algorithm converge to the optimal value faster. Zhang [5] introduced the target bias strategy in the sampling process, and changed the expansion direction of new nodes by allocating different weights, which could effectively accelerate the convergence rate. Luo [6] developed a hybrid shared control approach scheme integrated with both APF and EMG-based components to avoid multi-obstacles. However, in dealing with complex path optimization problems, the above algorithm still has some shortcomings, such as local optimization and slow solving speed. Inspired by the steady-state heat transfer path, Golan [7] conducted an in-depth study on the path planning method based on heat conduction, and proposed a path planning method based on the partial differential of heat conduction and iteratively solved by a numerical method. Through experiments, it was verified that the method could navigate in real time under an unknown static environment, but the search direction would be far away from the target due to small rounding error in the process of solving the equation. Ryu [8] introduced the topology optimization method based on black and white pixel images in the thermal potential field, which verified the feasibility of the method under multiple conditions and could be extended to more complex environments, but the generated path was not smooth enough. Li [9] adopted a topology optimization method based on thermal conductivity expansion method and proposed a computational framework of growth simulation concept. Path planning was realized through the process of continuous growth of materials, which could obtain clear geometric paths, but largescale interpolation calculation was involved, and the calculation time was long. Li [10] combined the concept of growth bionics with the topology optimization method based on explicit level set and proposed a path planning algorithm with parameterized level set. A large number of simulations show that this method has good effect in dynamic and unknown environments, but it also involves large-scale calculation and takes a long time to solve. It can be seen that although the research on robot path planning has made great progress, there are also problems such as weak ability to deal with complex environment problems and complex calculations. Therefore, it is necessary to conduct in-depth research on the path planning of mobile robots under different working conditions. Considering the topology optimization problem is a nonlinear optimization problem, to solve the low efficiency and the result is not stable, this paper transforms the path planning problem of mobile robot into the optimal design of the cooling topology of steadystate problems, based on the topology optimization of heat conduction of biomimetic growth of ideas, put forward a kind of mobile robot path planning method based on binary search. In this way, the real-time performance and computational efficiency of the mobile robot path planning algorithm can be improved, so that it can adapt to path planning in different complex environments, avoid falling into local minimum points, and have better search and computational performance.

640

Y. Fang et al.

2 Principle of Path Planning Algorithm for Topology Optimization of Heat Conduction 2.1 Analogy of Path Planning and Heat Transfer Problems In the phenomenon of heat conduction, if the heat source is completely isolated from the heat sink, there must be a path with the best heat conduction in the thermal analysis domain. It can be seen that the steady-state heat conduction path is obviously similar to the mobile robot. Therefore, the characteristics between path planning and heat conduction topology optimization problems are compared, as shown in Table 1. Table 1. Analogy of robot path planning and topology optimization analogy Robot configuration space

Thermal analysis domain

Start point

Heat source

Goal point

Heat sink

Free region

Design domain

Obstacle

Non-design domain

Robot moving path

Heat transferring path

2.2 Robot Configuration Space Mapping According to the analogy between topology optimization based on heat conduction and robot path planning, the path planning problem of mobile robot can be transformed into an equivalent topology optimization problem of heat conduction. Before solving the topology optimization model of heat conduction based on growth bionics, the finite element model of heat conduction should be established. The finite element model of heat conduction mainly consists of two parts: (i) map the robot configuration space to heat analysis domain, and (ii) discretize the analysis domain into finite element meshes. As depicted in Fig. 1(a), the configuration space C of the robot includes four parts: start point C S , goal point C G , obstacle C O and free regions C F . C = CS + CG + CF + CO

(1)

The space C is mapped to the domain H according to the following correspondence. ⎧ CS ⎪ ⎪ ⎨ CG C→H : ⎪ C ⎪ ⎩ F CO

→ HS → HG → HF → HO

H = HS + HG + HF + HO

(2)

(3)

Path Planning Algorithm of Mobile Robot

641

Where, H S and H G are the heat source and sink, respectively. All elements in the nondesign domain H O are regarded as adiabatic and their heat conductivity coefficient is k 0 = 0. H F is the design domain of heat conduction topology optimization. Heat transfer phenomenon exists in H F , and its heat conduction coefficient is higher than that of nondesign domain and lower than that of high heat conduction materials. The converted finite element model of heat conduction topology optimization is shown in Fig. 1(c).

Fig. 1. (a) Configuration space. (b) Raster map model. (c) Heat transfer finite element model.

2.3 Single-Step Growth Optimization Model The equivalent heat transfer path is achieved by a process of continuous growth. The cooling channel (which can be regarded as a high thermal conductivity material) starts from the heat source, grows gradually in the design domain, and finally reaches the heat sink. It is stipulated that the rectangular contour of the cooling channel grows gradually in the design domain, as shown in Fig. 2.

Fig. 2. Path growth process.

Its width is constant d and length is L, so the growth process of high thermal conductivity material is a fixed-step growth process. Specifically, in the first step of growth, one end of the rectangular material with high thermal conductivity is fixed at the heat source,

642

Y. Fang et al.

and the optimal growth direction is found by searching freely in all directions according to the principle of optimal heat dissipation performance. After the position of the high thermal conductivity material of the previous step is determined in the design domain, the starting end of the new high thermal conductivity material is fixed on the end of the high thermal conductivity material of the previous step. Then the growth direction of the step is determined according to the same search principle, and the growth process is continuously carried out until the heat sink is reached. In the process of growth, the optimal growth direction of each growth step is determined based on the optimal heat dissipation performance. Thermal compliance of heat dissipation is a commonly used and excellent evaluation index, which is selected as the evaluation function of one-step growth. min J (α i ) = T T K(α i )T s.t. ∇ · (k∇T ) + q = 0 in H T = TS on ST (k∇T ) · n = qN on SQ

(4)

0 ≤ α i ≤ 2π Where J (α i ) denotes the thermal compliance (i.e., objective function), and α i denotes the growth angle in i-th step. K(α i ) denotes the overall thermal conductivity matrix of the i-th thermal analysis domain. T denotes the temperature field over the analysis domain, k denotes the material thermal conductivity, and q is the heat source. 2.4 Finite Element Analysis According to Eq. (4), the temperature field T is one of the conditions for calculating and optimizing the thermal compliance of the objective function. The finite element method is used to solve the temperature field in the thermal analysis domain: K ·T =F

(5)

Where, T is the temperature field in the thermal analysis domain, K is the overall thermal conductivity matrix of thermal analysis, F is the thermal load. The position of heat sink of heat source is determined during problem conversion, and its numerical value can ensure that an obvious temperature field can be formed in the thermal analysis domain. Due to the clear geometric boundary of the cooling channel, only part of the finite element with the underlying design domain is covered by the cooling channel. Density treatment of these intermediate elements results in clear geometric boundaries for the high thermal conductivity material projected onto the underlying finite element mesh. As shown in Fig. 3, the partially covered unit is regarded as the intermediate material, and its thermal conductivity is between the high thermal conductivity material and the design domain. The thermal conductivity matrix is: Ke = KE · ρ

(6)

Path Planning Algorithm of Mobile Robot

643

Where, K e is the thermal conductivity matrix of the intermediate material, the thermal conductivity matrix of K E high thermal conductivity material, ρ is the pseudo density, and its value is:  2 n ρ= (7) ne Where, ne is the number of nodes in the cell. In this paper, a two-dimensional quadrilateral four-node element is adopted, that is, ne = 4, n is the number of nodes covered by high thermal conductivity materials in the unit.

Fig. 3. Density method to deal with the intermediate unit.

After analysis and calculation, the temperature field T in the analysis domain can be obtained. In the finite element framework, after this, the calculation of the optimization objective function is relatively simple, and the form of the thermal compliance in the finite element framework is as follows:  N    n 2 i T i T J (α ) = T K(α )T = T KE T (8) ne e=1

Where, N is the total number of finite element meshes in the bottom layer. The thermal conductivity matrix of the non-design domain is always set to a non-zero minimum value even if the cooling channel covers the non-design domain element in the calculation process.

3 Solution Based on Dichotomy The methods commonly used to solve topology optimization problems include sequential quadratic programming (SQP) and moving asymptotic line (MMA) [11]. In the calculation process, the gradient information of the objective function is needed to solve

644

Y. Fang et al.

the problem. The geometric shape of the high thermal conductivity material must be displayed and described by the equation, which involves large-scale calculation. Secondly, the finite difference method is not accurate to approximate the problem, which easily leads to unstable solution results. The method based on dichotomy does not need to construct explicit equations and calculate the objective function gradient, it can effectively improve the calculation efficiency. 3.1 Cooling Channel Material Storehouse In the grid map, the straight and diagonal directions are the motion directions of the mobile robot. The eight directions as shown in Fig. 4(a) are selected as the search directions of the growth angle. The rectangular cooling channel materials in the straight and diagonal directions are added to the material library. And the corresponding finite element model is shown in Fig. 4(b) and Fig. 4(c).

Fig. 4. (a) Robot movement direction. (b) Diagonal direction. (c) Straight line direction.

3.2 Dichotomy Solution After the heat dissipation model is established, the most heat transfer path in the thermal analysis domain is usually the curve. In each iteration growth process, the closer the angle of the changing component is to the optimal heat transfer path, the better the overall heat dissipation performance of the thermal analysis domain, and the smaller the thermal compliance. On the contrary, the greater the thermal compliance. The thermal compliance function on the search direction constitutes an upward convex function with one and only one minimum value. The eight search directions of the cooling channel material library are represented by θ, and the thermal compliance degree function is expressed as J (θ ) and the growth of the optimal angle θ ∗ represented as: J (θ ) = min{J (θ )}

(9)

 1 1 3 5 3 7 θ ∈ 0, π, π, π, π, π, π, π 4 2 4 4 2 4

(10)

The minimum value of the thermal compliance in eight components can be obtained by the dichotomy method. The optimal growth angle can be selected by only iterating 3

Path Planning Algorithm of Mobile Robot

645

or 5 times per growth step. As shown in Fig. 5(a), there is objectively an initial topology optimization path in the initial region. Each time the growth process with prescribed eight direction cooling channels fits this path. After completing each step cooling channel, the thermal analysis domain topology optimization path of the small changes will happen. The final cooling channels path is shown in Fig. 5(c), the result is very close to the optimal solution.

Fig. 5. (a) Initial area. (b) During growth. (c) Growth complete.

4 Simulation Analysis In order to test the feasibility of the algorithm in this paper, two different environments (path planning in a complex environment, path planning in a C labyrinth environment) were selected for simulation testing. The parameters used in the simulation are shown in Table 2. Table 2. Some of the parameters in simulation cases. Length of cooling channel (L)

0.6 m

Width of Cooling channel (d)

0.3 m

Element size of the background mesh (E s )

0.2 m

Thermal conductivity of the insulators (k O )

0.001 W/(m2 K)

Thermal conductivity of the design domain (k F )

1 W/(m2 K)

Thermal conductivity of high thermal conductivity materials (k C )

1000 W/(m2 K)

Heat flux at the heat source (q)

1000 W/m2

Temperature at the heat sink (T )

273.15 K

Note: The simulation example is implemented by MA TLAB R2016B, Windows 10, CPU: Inter(R) Core(TM) I5-9400F CPU @ 2.90 GHz, RAM: 8 GB.

646

Y. Fang et al.

4.1 Path Planning in Complex Obstacle Environments The map environment is shown in Fig. 6(a). The set obstacles are distributed in the global static 80 × 80 grid map. The red point as the starting point and the blue point as the target point. The algorithm in this paper and RRT-bias algorithm are respectively used for path planning, and the final planned path is shown in Fig. 6(b) and Fig. 6(c). Figure 7 shows the search process of the two algorithms. Table 3 shows the simulation data comparison with RRT-bias algorithm in a complex environment. Table 3. Simulation data comparison in complex environment map RRT-bias algorithm

Algorithm in this paper

Path length

121.73 m

98.19 m

Time

19.81 s

10.84 s

Single-step time

\

≈71 ms

Fig. 6. Comparing results of map model and simulation of complex environment

It can be seen from Fig. 6(c), when the space contains a large number of obstacles or narrow channels, the RRT-bias algorithm is prone to collision of extended nodes with obstacles due to its random search strategy, and the convergence speed of the algorithm will be significantly reduced. The algorithm in this paper maps the path planning problem into the heat conduction topology optimization problem, and there is an objective optimal path in the thermal analysis domain, which can fundamentally avoid the blindness of solving. The convergence speed of the algorithm is only related to the optimal path length, independent of the complexity of the environment, so it has more advantages in complex maps. It can be seen from Table 3 that the algorithm in this paper is superior to the RRT-bias algorithm in both path length and search time. 4.2 Path Planning in Maze Environment The map environment is shown in Fig. 7(a). The set obstacles are distributed in the global static 88 × 88 grid map. the red point as the starting point and the blue point as

Path Planning Algorithm of Mobile Robot

647

the target point. The algorithm in this paper and the level set algorithm were respectively used for path planning, and the final planned path was shown in Fig. 7(b) and Fig. 7(c). Table 4 shows the results of comparison between the algorithm in this paper and the level set algorithm. Table 4. Simulation data comparison in a labyrinth environment map Level set algorithm

Algorithm in this paper

Path length

68.91 m

68.93 m

Total steering angle

1489°

1215°

Time

41.18 s

8.69 s

Single-step time

≈448 ms

≈74 ms

Fig. 7. Comparison result of labyrinth environment map model and simulation.

It can be seen from Fig. 7 and Table 4, although the path lengths obtained by the two algorithms are very close, the path generated by the path planning method based on level sets is very likely to generate disturbances in the part of the straight channel, leading to the increase of the total turning angle. In terms of search time and singlestep growth time, compared with the level set path planning algorithm, the single-step growth time of the proposed algorithm is only 74 ms, which is only 16.5% of the level set path planning algorithm, while the search time is only about 1/5 of the latter. Through the above comparison results, it can be seen that the path effect and solving efficiency of the proposed algorithm are obviously better than that of the level set path planning algorithm.

5 Conclusions In this paper, combining the computational framework of growth simulation and the idea of heat conduction topology optimization, a fast path planning method for heat conduction topology optimization based on dichotomy is proposed. According to the path planning problem in different environments, the simulation analysis is carried out, and the simulation results show that this method has the following characteristics:

648

Y. Fang et al.

(1) The final path is generated through dynamic growth, and the path points obtained in each iteration can be executed immediately. This method has strong real-time performance and is more in line with the path planning requirements of mobile robots. (2) It has strong adaptability and can be applied to a variety of complex environments. The convergence speed of the algorithm has nothing to do with the complexity of the environment, so it has more advantages in complex environments. (3) Compared with the traditional topology optimization method, the method of constructing the cooling channel can convert the nonlinear constrained optimization problem into a discrete optimization problem, which can be solved by the dichotomy, and the planned path efficiency is higher the path is more stable. The method currently solves the globally known complex obstacle environment. The focus of future research will consider how to effectively deal with the environment of dynamic obstacles, further improve the robustness and universality of the method, and design experiments for verification.

References 1. Liu, C., Han, J., An, K.: Dynamic path planning based on an improved RRT algorithm for RoboCup robot. Chinese J. Robot 39(01), 8–15 (2017) 2. Urmson, C., Simmons, R.: Approaches for heuristically biasing RRT growth. In: 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (2003). Proceedings. IEEE, pp. 1178–1183 (2003) 3. Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res. 30(7), 846–894 (2011) 4. Tan, J., Pan, B., Wang, Y., Cui, H.: Robot path planning based on improved RRT*FN Algorithm. Chin. J. Control Decis. (2020). https://doi.org/10.13195/j.kzyjc.2019.1713 5. Zhang, W., Fu, S.: Mobile robot path planning based on improved RRT* algorithm. Chinese J. J. Huazhong Univ. Sci. Technol. (Nat. Sci. Ed.) (2020). https://doi.org/10.13245/j.hust. 210101 6. Luo, J., Lin, Z., Li, Y., Yang, C.: A teleoperation framework for mobile robots based on shared control. IEEE Robot. Autom. Lett. 5(2), 377–384 (2020). https://doi.org/10.1109/lra.2019. 2959442 7. Golan, Y., Edelman, S., Shapiro, A., et al.: Online robot navigation using continuously updated artificial temperature gradients. IEEE Robot. Autom. Lett. 2(3), 1280–1287 (2017) 8. Ryu, J., Park, C., Kim, Y.: Mobile robot path planning algorithm by equivalent conduction heat flow topology optimization. Struct. Multi. Optim. 45(5), 703–715 (2012) 9. Li, B., Liu, H., Su, W.: Topology optimization techniques for mobile robot path planning. Appl. Soft Comput. 78, 528–544 (2019) 10. Li, X., Zhao, G., Li, B.: Generating optimal path by level set approach for a mobile robot moving in static/dynamic environments. Appl. Math. Model. (2020). https://doi.org/10.1016/ j.apm.2020.03.034 11. Svanberg, K.: The method of moving asymptotes—a new method for structural optimization. Int. J. Numer. Meth. Eng. 24(2), 359–373 (1987)

An RBFNN-Informed Adaptive Sliding Mode Control for Wheeled Mobile Robots Quan Liu1 , Zhao Gong1(B) , Fugui Xie1,2 , Shuzhan Shentu1 , and Xin-Jun Liu1,2(B) 1 The State Key Laboratory of Tribology and Institute of Manufacturing Engineering,

Department of Mechanical Engineering, Tsinghua University, Beijing 100084, China [email protected] 2 Beijing Key Lab of Precision/Ultra-Precision Manufacturing Equipments and Control, Tsinghua University, Beijing 100084, China

Abstract. This paper deals with the trajectory tracking of differential driven wheeled mobile robots (WMRs). It is widely known that parameter uncertainty and unknown external disturbances could bring adverse effects to WMRs. To achieve high-performance tracking, a control scheme is proposed in this paper, which integrates a kinematics controller and a dynamics controller. The kinematics controller is designed by the backstepping control and the dynamics controller is derived via adaptive sliding mode control. Moreover, dual radial basis function neural networks (RBFNNs) with adaptive adjust algorithms are adopted to approximate parameter alterations of system dynamics as well as external disturbances. To solve the uncertainty of wheel rolling radius, an adaptive control law is applied. In addition, a PI-type sliding mode control strategy with an exponential reaching law is designed to compensate for the approximation errors of the RBFNNs. Furthermore, the stability and global asymptotic convergence of the control system are proved by the Lyapunov stability theorem. Finally, simulations are provided to verify the effectiveness of the proposed control strategy. The results indicate that the proposed control law is effective in response to parameter uncertainty and external disturbances. Keywords: Trajectory tracking · Adaptive sliding mode control · Wheeled mobile robots · Parameter uncertainty · External disturbances

1 Introduction Because of inherent nonholonomic constraints and broad prospects in various applications, wheeled mobile robots (WMRs) have attracted much attention from academia and industry during the past decades [1–3]. To design high-performance controllers for WMRs, various kinds of control schemes were studied, including the backstepping method [4], adaptive control [5], sliding mode control [2], fuzzy control [6], robust control [7] as well as other control approaches [8, 9], and implemented on many kinds of WMRs successfully. A classic control rule informed on the kinematics model of WMRs was proposed in 1990 [1]. After that, the controller scheme in Ref. [1] was widely used to design other control laws. In spite of successful implementations of kinematics model © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 649–658, 2021. https://doi.org/10.1007/978-3-030-89092-6_59

650

Q. Liu et al.

informed controllers in some industrial applications, there was still a serious disadvantage to be overcome. Parameter uncertainty and unknown external disturbances were neglected in these controllers. Hence, WMRs cannot always track desired trajectories perfectly, especially in complex natural environments. To overcome parameter alterations and external disturbances, dynamics models were applied in the design of WMR controllers. An adaptive neural sliding mode controller for WMRs was proposed [2], in which both kinematics and dynamics models were considered. Dual radial basis function neural networks (RBFNNs) with adaptive adjust algorithms were used to compute the equivalent control part of sliding mode control and adjust gain parameters of sliding mode control. The control strategy in Ref. [2] was also adopted to design other controllers for WMR whose centroid does not coincide with its geometrical center [10]. A dual-loop attitude tracking robust control rule was designed for WMRs with bounded external disturbances and parameter uncertainty [11]. For WMRs incorporating the actuator dynamics with unknown external disturbances, a robust tracking control law was proposed [12]. Considering the uncertainty of wheel radius and wheel pitch, an adaptive tracking control method for WMRs was derived [13]. In these studies, parameter uncertainty loaded by variation of load and rotational inertia, as well as external disturbances, were broadly studied. Nevertheless, the variation of wheel radius was rarely considered. In the actual situation, the variation of wheel radius is also inevitable because of the load change of WMRs and the change in tire pressure, which also brings interferences to the control of WMRs [13]. To achieve high-performance control for WMRs, not only the uncertainty of mass and rotational inertia, but also the variation of wheel radius, as well as unknown external disturbances, are taken into consideration in the proposed control scheme in this paper. Firstly, a kinematics controller is proposed using the backstepping method. Secondly, to approximate parameter alterations caused by the variation of load and rotational inertia as well as external disturbances, the RBFNNs with adaptive adjust algorithms are designed. Thirdly, to solve the uncertainty of wheel rolling radius, an adaptive control law is proposed. Finally, a sliding mode control law with the robust term is derived to compensate for the approximation errors of the RBFNNs. The remainder of this paper is organized as follows. Section 2 introduces the models of WMRs, and provides the designed controllers. In Sect. 3, the stability analysis of the proposed control law is developed, and simulations are performed. Section 4 concludes the work of this paper.

2 Robot Model and Controller Design 2.1 Kinematics and Dynamics Models of WMRs The mobile robot investigated in this paper is a differential driven wheeled mobile robot, as shown schematically in Fig. 1, which has two driving wheels and a driven wheel. The O-xy coordinate frame is defined as the global coordinate frame. The D-x 1 y1 coordinate frame is fixed to the robot and called the robot reference coordinate frame. Point C is the system’s mass center, and point D denotes the midpoint between the right driving wheel and the left driving wheel. The coordinate of point D is defined as q = [x, y, θ ]T in the O-xy coordinate frame. It assumes that the robot moves on a horizontal plane and satisfies the constraint of pure rolling and no lateral slipping. Furthermore, point D is

An RBFNN-Informed Adaptive Sliding Mode Control

651

chosen as the control point. The forward kinematics model of the system is formalized in Ref. [1] as ⎤ ⎡ ⎤ ⎡ cos θ 0   x˙ v = S(q)V (1) q˙ = ⎣ y˙ ⎦ = ⎣ sin θ 0 ⎦ w ˙θ 0 1 where θ denotes the angle between the robot’s moving direction and the x axis. Besides, V = [v, w]T is the velocity vector, including the linear velocity v and the angular velocity w in the robot reference coordinate frame. The velocity vector of the robot in the global coordinate frame is denoted as q˙ . The S(q) is the velocity transformation matrix.

Fig. 1. The differential driven wheeled mobile robot

The dynamics model of the robot is formalized in Ref. [2] as (M 0 + M)V˙ + (C 0 + C)V + τ d = Eτ

(2)

All these matrices are given by           1 1 1 τ1 m 0 00 τd 1 , C , τ = . = = M0 = , τ , E = 0 d τd 2 τ2 0 I + md 2 00 r l −l where M 0 is defined as the inertial matrix, and C 0 denotes the centripetal and Coriolis matrix. The unknown external disturbances are τ d , and E is the input transformation matrix. τ is the control torque vector consisting of the input torque of the right driving wheel τ 1 and the input torque of the left driving wheel τ 2 . ΔM and ΔC are the unknown model parameter uncertainties. m is the mass of the robot, and I is the moment of inertia about the axis passing through the centroid and perpendicular to the motion plane. d denotes the distance between the point C and D. l denotes half of the distance between the two driving wheels. r is the radius of the two driving wheels. To simplify expression, Eq. (2) is rewritten as follows V˙ = RAτ + g(x)

(3)

−1 −1 T ˙ where A = rM −1 0 E, g(x) = −M 0 (M V +CV) −τ d , τ d = M 0 τ d = [τ d 1 τ d 2 ] , R= 1/r.

652

Q. Liu et al.

2.2 Control Objective The control objective of this work is to design a control torque vector τ for the robot to converge to desired trajectories in the presence of parameter uncertainty and external disturbances in finite time. The desired trajectory that denotes the motion of the reference robot is described as following ⎡

⎡ ⎤ ⎤ x˙ r cos θr 0   v q˙ r = ⎣ y˙ r ⎦ = S(qr )V r = ⎣ sin θr 0 ⎦ r w r θ˙r 0 1

(4)

where qr = [x r yr θ r ]T denotes the time-varying pose vector of the reference robot and V r = [vr wr ]T describes the linear velocity vr and the angular velocity wr , respectively. To achieve the goal, serval assumptions are made. ˙ Assumption 1: The reference velocities vr , wr , and their first-order derivative v˙ r, wr, are bounded and continuous. Assumption 2: The unknown external disturbances are bounded, which means that |τ d 1 | ≤ τ max1 , |τ d 2 | ≤ τ max2 . Assumption 3: The actual pose q, the actual velocity vector V, and the actual acceleration vector V˙ are available for feedback control. 2.3 Design of Kinematics Controller To achieve the tracking target, tracking errors described in the robot reference coordinate frame are defined in Ref. [1] as ⎡ ⎤ ⎡ ⎤ ⎤⎡ cos θ sin θ 0 xe xr − x (5) qe = ⎣ ye ⎦ = ⎣ − sin θ cos θ 0 ⎦⎣ yr − y ⎦ = T e (qr − q) θe θr − θ 0 0 1 The first-order derivative of the errors is expressed in Ref. [4] as ⎤ ⎡ ⎤ ye wd − vd + vr cos θe x˙ e ⎣ y˙ e ⎦ = ⎣ −xe wd + vr sin θe ⎦ θ˙e wr − wd ⎡

(6)

The control objective of the kinematics controller is to generate an intermediate control input V d = [vd wd ]T . It is noteworthy that if x e and θ e approach to zero before ye , y˙ e would be zero, which means that ye becomes constant and uncontrollable. Hence, choosing θ e = θe + k1 f as the virtual control, the control law for the kinematics controller is designed as  ∂f ∂f k1 vr sin(θ e − k1 f ) + ∂v v˙ r k1 + 2ye vr cos( θ2e − k1 f ) wd = k2 sin θ2e + wr + ∂y e r vd = k3 xe + vr cos(θ e − k1 f ) − sin

θ e ∂f 2 ∂ye wd k1

(7)

An RBFNN-Informed Adaptive Sliding Mode Control

where k 1 , k 2 , k 3 are positive constants. ⎧ ⎨ ρ = 1 + v2 y2 , f = vr ye /ρ, θ e = θe + k1 f r e ⎩ ∂f = vr − ye2 vr3 , ∂f = ye − vr2 ye3 ∂ye

ρ

ρ3

∂vr

ρ

653

(8)

ρ3

Ideally, the control law in Eq. (7) could make the tracking errors converge to zero. Nonetheless, it cannot ensure tracking performance in response to model uncertainty and external disturbances. 2.4 Design of Dynamics Controller To further overcome disturbances of system dynamics, the virtual control vector is defined as follows   vd − v (9) Ve = Vd − V = wd − w where V = [v, w]T is the actual velocity vector of the robot. A PI-type sliding mode surface is chosen as  

t s1 (t) = Ve + λ S= V e dt s2 (t) 0

(10)

By differential operation and using Eq. (3), it yields S˙ = V˙ d − RAτ − g(x) + λV e

(11)

If the system is certain and there are no external disturbances, that is g(x) = 0 and R is a positive constant, the input control torque vector for the robot could be chosen as τ certain =

1 −1 ˙ A (V d + λV e + kS + εsgn(S)) R

(12)

However, having taken parameter uncertainty and external disturbances into consideration, the control rule in Eq. (12) cannot ensure the performance of the system or even fails. To solve these problems, the RBFNNs including adaptive adjust algorithms and an adaptive control law are applied. A classical RBFNN could be expressed as f (x) = W ∗T h(x) + ω

(13)

where x is the input vector of the RBTNN model. W* is the optimal weight vector. ω is the approximation error vector. The function vector h(x) = [hi ]T is called the basis function and commonly chosen as Gaussian functions with the following form   −x − ci 2 (14) hi = exp 2b2i

654

Q. Liu et al.

where ci is a constant vector called the center of the i-th neuron and bi is a real number defined as the width of the i-th neuron. Adopting the RBFNNs to approximate g(x), it yields ⎧ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎪ ∗T h (x ) ⎪ g W ω (x ) ⎪ 1 1 1 1 1 ⎪ ⎦=⎣ ⎦+⎣ 1⎦ ⎪ g(x) = ⎣ ⎪ ⎪ ∗T ⎨ ω2 g2 (x2 ) W 2 h2 (x2 ) ⎡ ⎡ ⎤ ⎡ ⎤ ⎤ (15) T T ⎪ ∗T − W ⎪ ˆ T1 h1 (x1 ) ˜ ˆ W W W ⎪ 1⎦ ⎪ ˜T=⎣ 1⎦=⎣ 1 ⎦, W ⎪ gˆ (x) = ⎣ T ⎪ ⎪ T ∗T ⎩ ˆ T2 ˆ 2 h2 (x2 ) ˜2 W2 − W W W where gˆ (x) is the approximation of g(x). ω1 and ω2 are the approximation error of g1 (x1 ) and g2 (x2 ), respectively. |ω1 | < ωN1 , |ω2 | < ωN2 , ωN1 and ωN2 are small positive ˆ 1 and W ˆ2 ˙ T are input vectors of the RBFNNs. W numbers. x1 = [v v˙ ]T and x2 = [w w] are adjusted weight vectors with following adaptive rules ⎧ ˙ˆ ⎨W 1 = −ϕ1 s1 h1 (x1 ) (16) ⎩ ˙ˆ W 2 = −ϕ2 s2 h2 (x2 ) where the ϕ 1 and ϕ 2 are positive constants. To overcome variation of the wheel radius, an adaptive control law is adopted. Definˆ the estimation error R˜ could be expressed ing the estimation value of R in Eq. (3) as R, as R˜ = R − Rˆ

(17)

The adaptive rule of Rˆ could be defined as R˙ˆ = −μST Aτ

(18)

where μ is a positive constant. To sum up, the control law for the dynamics controller is designed as τ =

1 −1 ˙ A [V d − gˆ (x) + λV e + kS + εsgn(S)] Rˆ

(19)

where       λ1 0 ε 0 k 0 ,λ = ,ε = 1 . k= 4 0 k5 0 λ2 0 ε2 k 4 , k 5 , λ1 , λ2 , ε1 , and ε2 are positive constants; moreover, ε1 > ωN1 and ε2 > ωN2 .

3 Stability Analysis and Simulation 3.1 Stability Analysis The stability of the proposed control scheme is proved by the Lyapunov stability theory. Consider the following Lyapunov function L = L1 + L2

(20)

An RBFNN-Informed Adaptive Sliding Mode Control

655

where L 1 and L 2 are described as follows L1 =

θ 1 2 1 2 y + x + 4 sin2 2 e 2 e 4

L2 = 21 [S(t)T S(t)+ μ1 R˜ 2 +

1 ˜ T ˜ ϕ1 W 1 W 1

+

(21) 1 ˜ T ˜ ϕ2 W 2 W 2 ]

(22)

Substituting Eq. (7) to the differentiating of L 1 , it yields θe L˙ 1 = ye y˙ e + xe x˙ e + θ˙ e sin 2 θe = xe [−vd + vr cos(θ e − k1 f )] + ye vr sin(θ e − k1 f ) + θ˙ e sin 2 θ e ∂f = xe [−vd + vr cos(θ e − k1 f ) − sin wd k1 ] − ye vr sin k1 f 2 ∂ye + sin

θe θe ∂f ∂f [wr − wd + k1 vr sin(θ e − k1 f ) + v˙ r k1 + 2ye vr cos( − k1 f )] 2 ∂ye ∂vr 2

= −k2 sin2

θe k1 vr ye − ye vr sin − k3 xe2 ≤ 0 2 ρ

(23)

Substituting Eq. (16) and Eq. (18) to the differentiating of L 2 , it yields ˙ + 1 R˜ R˙˜ + L˙ 2 = S(t)T S(t) μ

1 ˜ T ˙˜ ϕ1 W 1 W 1

+

1 ˜ T ˙˜ ϕ2 W 2 W 2

˜ T h(x) − ω − kS − εsgn(S) − RAτ ˜ T ˙ˆ ˜ ] − 1 R˜ R˙ˆ − 1 W = S(t)T [−W μ ϕ 1 W1 −

1 ˜ T ˙ˆ ϕ2 W 2 W 2

˙ˆ 1 ˙ˆ ˜ T1 [s1 h1 (x) + 1 W ˜T = −W ϕ1 1 ] − W 2 [s2 h2 (x) + ϕ2 W 2 ]      ω1 + k4 s1 + ε1 sgn(s1 )  − R˜ S(t)T Aτ + μ1 R˙ˆ − s1 s2 ω2 + k5 s2 + ε2 sgn(s2 ) = −ω1 s1 − k4 s12 − ε1 |s1 | − ω2 s2 − k5 s22 − ε2 |s2 | ≤ 0

(24)

By analyzing the results, it indicates that L ≥ 0, L˙ ≤ 0 and L, L˙ become zero when and only when x e = 0, ye = 0, θ e = 0, S = 0. Consequently, based on the Lyapunov stability theorem, the proposed control strategy is stable. 3.2 Simulation Study To demonstrate the performance and effectiveness of the proposed control strategy, two simulations are implemented by using MATLAB Simulink and ADAMS. The physical parameters of the robot and the parameters of the controllers are shown in Table 1. Firstly, to verify the performance and effectiveness of the controllers in response to friction, co-simulation based on MATLAB Simulink and ADAMS is applied.

656

Q. Liu et al. Table 1. The physical parameters of the robot and the parameters of the controllers Simulation 1

Simulation 2

Simulation environment

Co-simulation based on MATLAB Simulink and ADAMS

Simulation based on MATLAB Simulink

Parameters of the controller

k 1 = 1, k 2 = 4, k 3 = 16, λ1 = 10, λ2 = 10, ε1 = 0.01, ε2 = 0.01, k 4 = 10, k 5 = 10, μ = 10, ϕ 1 = 0.1, ϕ 2 = 0.1

Initial value of Rˆ

100

Physical parameters of the robot

m = 19 kg, I = 0.4 kg·m2 , l = 0.155 m, d = 0.1 m, r = 0.04 m

The initial poses

The reference robot: [0.2, 0.1, π/ 4]T The tracking robot: [0, 0, π/ 2]T

Reference velocities

vr = 1 m/s, wr = cos (πt/5) rad/s

vr = 1 m/s, wr = 0.5 rad/s

Secondly, simulation 2 is adopted to further test the performance and effectiveness of the controllers in the presence of both parameter uncertainty and external disturbances. The external disturbances are defined as τ d1 = 2sign(v) and τ d2 = 2sign(w). Besides, the ΔM mutates from zero to ΔM 1 , and ΔC mutates from zero to ΔC 1 at t = 8 s.     10 0 0 −0.2 M 1 = , C 1 = (25) 0 0.1 0.2 0 Figure 2 and Fig. 3 show the reference and tracking trajectories in simulation 1 and 2, respectively. The tracking errors are implied in Fig. 4 and Fig. 5. Meanwhile, Fig. 6 and Fig. 7 display the control input torques.

Fig. 2. The trajectories in simulation 1

Fig. 3. The trajectories in simulation 2

An RBFNN-Informed Adaptive Sliding Mode Control

657

Fig. 4. The tracking errors in simulation

Fig. 5. The tracking errors in simulation 2

Fig. 6. The control torques in simulation 1

Fig. 7. The control torques in simulation 2

By analyzing the simulation results, it is evident that the proposed controllers are effective in the presence of parameter uncertainty and external disturbances.

4 Conclusion An adaptive sliding mode control scheme for WMRs with internal and external disturbances has been proposed in this paper. The proposed control scheme has integrated a kinematics controller and a dynamics controller. The kinematics controller has been generated under the guidance of the backstepping method, which makes the selection of the Lyapunov function simple. In the dynamics controller design, an adaptive control rule has been applied to deal with the change of wheel radius. Benefiting from this improvement, it requires no knowledge of the accuracy radius of wheels in this proposed control scheme. In addition, the parameter alterations of system dynamics and external disturbances have been approximated by the RBFNNs. The approximation errors have been compensated by the robust term in the sliding mode control law. Besides, the global stability and asymptotic convergence of the derived control rule have been analyzed by the Lyapunov theory. On the basis of the derived control scheme, simulation studies

658

Q. Liu et al.

have been implemented via MATLAB Simulink and ADAMS. The simulation result illustrates that near-zero tracking errors are achieved in spite of the initial pose errors and the internal or external disturbances, which indicates the derived control strategy is effective. Acknowledgments. This work was supported by the National Key R&D Program of China under Grant 2019YFA0706701 and by the National Natural Science Foundation of China under Grants 91948301 and 51922057.

References 1. Kanayama, Y., Kimura, Y., Miyazaki, F., et al.: A stable tracking control method for an autonomous mobile robot. In: IEEE International Conference on Robotics and Automation, Cincinnati, pp. 384–389. IEEE (1990) 2. Wang, Z.: Dual adaptive neural sliding mode control of nonholonomic mobile robot. J. Mech. Eng. 46(23), 16–22 (2010) 3. Su, Y., Liang, Y.: Research on double closed-loop trajectory tracking control algorithm of wheeled mobile robot based on global stability. In: 12th International Conference on Intelligent Human-Machine Systems and Cybernetics, Hangzhou, pp. 180–183. IEEE (2020) 4. Shentu, S.Z., Xie, F.G., Liu, X.-J., et al.: Motion control and trajectory planning for obstacle avoidance of the mobile parallel robot driven by three tracked vehicles. Robotica (2021). https://doi.org/10.1017/S0263574720000880 5. Li, Z., Wang, Y., Song, X., et al.: Neural adaptive tracking control for wheeled mobile robots. In: the 2015 International Conference on Fluid Power and Mechatronics, Harbin, pp. 610–617. IEEE (2015) 6. Wu, X., Jin, P., Zou, T., et al.: Backstepping trajectory tracking based on fuzzy sliding mode control for differential mobile robots. J. Intell. Rob. Syst. 96(1), 109–121 (2019) 7. Keymasi Khalaji, A., Jalalnezhad, M.: Robust forward\backward control of wheeled mobile robots. ISA Trans. (2021). https://doi.org/10.1016/j.isatra.2021.01.016 8. Luy, N.T.: Robust adaptive dynamic programming based online tracking control algorithm for real wheeled mobile robot with omni-directional vision system. Trans. Inst. Measur. Control 39(6), 832–847 (2016) 9. Li, S., Ding, L., Gao, H., et al.: Adaptive neural network tracking control-based reinforcement learning for wheeled mobile robots with skidding and slipping. Neurocomputing 283, 20–30 (2018) 10. Zhang, X., Liu, F., Yan, M.: Dynamic model-based adaptive sliding-mode trajectory tracking control over wheeled mobile robot. Mech. Sci. Technol. Aerosp. Eng. 31(1), 107–111 (2012) 11. Ye, H., Wang, S.: Trajectory tracking control for nonholonomic wheeled mobile robots with external disturbances and parameter uncertainties. Int. J. Control Autom. Syst. 18(12), 3015– 3022 (2020) 12. Wang, Y., Wu, Y.: Robust tracking control of uncertain nonholonomic wheeled mobile robot incorporating the actuator dynamics. In: The 2019 Chinese Control Conference, Guangzhou, pp. 2392–2397. IEEE (2019) 13. Gu, W.-L., Hu, Y.-F., Gong, X., et al.: Trajectory tracking control of mobile robot with parameter uncertainties. Control Decis. 34(1), 81–88 (2019)

Robust Formation Tracking of Multiple Wheeled Mobile Robots Under External Disturbance Kanyang Jiang1 , Xiaoxiao Li2 , Zhihao Xu2 , Xuefeng Zhou2 , and Shuai Li1,3(B) 1

2

School of Engineering, Swansea University, Swansea, UK Guangdong Key Laboratory of Modern Control Technology Institute of Intelligent Manufacturing, Guangdong Academy of Sciences, Guangzhou, China 3 Foshan Tri-Co Intelligent Robot Technology Co., Ltd., Foshan, China [email protected] Abstract. In this paper, we focus on formation tracking problem of multiple wheeled mobile robots (MWMRs) under external disturbances. First, the MWMRs’ formation tracking problem is reduced conceptually to track a trajectory by a virtual wheeled mobile robot (WMR) with placeholders that describe the desired position of each WMR in formation. Therefore, the moving trajectories correspond to each WMR can be obtained with placeholders. Second, a constraint minimization scheme, which incorporates WMR’s velocity limit, trajectory tracking simultaneously, is formulated from the perspective of optimization, where velocity norm is chosen as cost function. In this minimization scheme, an integration-enhanced error functional evolution formulation is introduced, aiming at resisting external disturbances. Then, a Lagrangianbased controller is designed to solve and output the velocities of WMRs’ left and right tires to control each WMR. Finally, simulation experiments verify the effectiveness of the proposed control scheme and robustness against noise. Keywords: Constraint optimization · External disturbance Formation · Multiple wheeled mobile robots

1

·

Introduction

Multi-robot systems are attracting more and more attention from researchers owing to their outperform ability to perform complicated tasks compared with one individual, with great efficiency, reliability and flexibility. They have been successively applied to cooperative transport [8], space coverage to rescue [11], etc., with wider application potentiality. Among which, formation control is an This work is supported by Key Areas R&D Program of Guangdong Province (Grant No. 2020B090925001), National Natural Science Foundation of China (Grant No. 62003102), Basic and Applied Basic Research Project of Guangzhou City (Grant No. 202002030237), Innovation and Entrepreneurship Team Project of Foshan City (Grant No. 2018IT100173). c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 659–668, 2021. https://doi.org/10.1007/978-3-030-89092-6_60

660

K. Jiang et al.

important branch for multi-robot control, which refers to multiple robots much forward with one or several geometric patterns. Formation tracking has many methods to be achieved, such as behavior-based method [6], virtual structure-based method [3] and Leader-follower based method [2]. The behavior-based method has great flexibility. It is good at solving obstacle avoidance problems, but because this method focuses on local constraints, it’s weak at the stability of the whole formation. Not the same as the behavior-based method, although leader-follower-based method has good stability, its system is over-dependent on the lead robot. With the method of virtual structure, each WMR keeps the relative location like a rigid structure and the formation has great stability and simple system. Thus, this study solves the MWMRs formation tracking based on the virtual structure method, where the formation tracking problem is reduced to a trajectory tracking of a virtual wheeled mobile robot (WMR) with placeholders that describe the desired position of each WMR in formation. For the desired trajectory tracking, the constraint optimization method is used. Constraint optimization is a popular method for overcoming the difficulties under many scenes [4,7,14,17]. Manipulator field is the first one to use this method. With the concept of constraint optimization, Zhang et al. addressed the kinematic control problem of redundant manipulators with using one-layer dual neural network of physically constrained redundant manipulators in [14] and Xu et al. addressed force control problem of redundant manipulators as a quadratic-programming (QP) problem with multiple constraints. Guo et al. further extended this concept into acceleration level control of redundant manipulators. In [4], they considered the infinity-norm acceleration minimization (INAM) problem and addressed the discontinuity problem arising from the INAM scheme further with dynamic neural networks. For avoiding the joint drift phenomenon, Zhao et al. considered bi-criteria minimization integrating joint-accelerationnorm minimization (MAN) and RMP in [17] by assigning a equal weighing factor. In [7], Li et al. proposed a decentralized cooperative controller for cooperative transport of multiple manipulators under a hierarchical communication topology for multi-manipulator kinematic control. Under the field of manipulator, mobile manipulators, many teams also use constraint optimization methods. Zhang et al. considered posture maintaining, obstacle avoidance and joint physical limits as constraint limits [16], and Lin et al. optimized rough manipulabilitymaximizing(RoMM) scheme with addressing physically limits. As mentioned above, many methods of solving problems with manipulator and mobile manipulators are based on constraint optimization. The constraint optimization based method is popular with the field of manipulator [4,7,14,17] and mobile manipulators [5,12,15,16] but it is less used in the field of MWMRs. In addition, external interference always exists in real life. External noise is produced under this situation. There are many causes for the formation of noise. In [10], Liao et al. discussed the chattering phenomenon during the robot rotating and gave a scheme for weakening the noise that is formed including the chattering phenomenon. Borenstein and Koren even pointed out the sensors also

Robust Formation Tracking of Multiple Wheeled Mobile Robots

661

can provide external noise. They proved the error eliminating rapid ultrasonic firing (EERUF) method to reduce the number of erroneous readings due to the ultrasonic noise one to two magnitude [1]. It is hard to list all external interference and gives suitable solutions for each of them. Therefore, we cast light on the robust formation tracking of MWMRs under external disturbance. Before ending this part, it is necessary to point out that the main contributions to this paper can be summarized as follows: 1. Robust formation tracking with MWMRs under external disturbance is considered in this study. The MWMRs’ formation tracking problem is described as one trajectory tracking problem by a virtual WMR with placeholders that describe the desired position of each WMR in formation. 2. An integration-enhanced error functional evolution strategy is proposed to against external noise. With optimization, the formation and trajectory tracking problem is described as a minimization problem and a Lagrange-based controller is constructed as a solver.

2

Preliminary

In this section, the kinematic model of the used two-wheel differential-driven mobile robot is presented and the method to track formation is shown. 2.1

Kinematic Model of Wheeled Mobile Robots

Figure 1(a) presents the used two-wheel differential-driven mobile robot, whose kinematic model can be written as: ⎡ ⎤ ⎡r ⎤ x˙ 2 (ur + ul ) cos θ Z˙ = ⎣y˙ ⎦ = ⎣ 2r (ur + ul ) sin θ ⎦ ∈ R3×1 . (1) r θ˙ L (ur − ul ) r denotes the radius of the wheel and L denotes the distance between two wheels. ur and ul are the left wheel’s velocity and right wheel’s velocity separately. θ denotes the heading angle of a WMR. To recover point control (xc ; yc ), we define [x + d0 cos θ, y + d0 sin θ]T using linear feedback technology, the kinematic model of the used WMR can be further written as: Z˙ = Au ∈ R2×1 , where u = [ul , ur ]T and ⎡ A=⎣

r cos θ 2

+

rd0 sin θ r cos θ L 2

r sin θ 2

−

rd0 cos θ r sin θ L 2

−

rd0 sin θ L

+

rd0 cos θ L

(2) ⎤ ⎦ ∈ R2×2 .

(3)

662

K. Jiang et al. Placeholder 1

Placeholder 2

Placeholder 3

Virtual WMR

(a)

(b)

Fig. 1. Graphe of (a): Schematics of wheeled mobile robots considered in the paper and (b) Schematics of placeholders based on the poistion of Virtual WMR considered in the paper.

2.2

Control Objective and Formation Resolution

Objective: In this paper, we focus on the problem of formation tracking of MWMRs with performance assurance under external disturbances, at the same time MWMRs should satisfy their respective velocity limits. Resolution: For making MWMRs track formation, there are three steps to achieve the object. First, the MWMRs’ formation tracking problem is reduced conceptually to a trajectory tracking of a virtual WMR with placeholders that describe the desired position of each WMR in formation. Specifically, create a virtual WMR as a leader to track the planned trajectory, i.e.: Zv → Zcmdv ,

(4)

where Zv means the actual position of the virtual WMR and Zcmdv means the planned trajectory of the virtual WMR. Secondly, in order to obtain the desired position of real MWMRs, create multiple placeholders among the virtual WMR based on the desired formation. As Fig. 1(b) presents, considering a triangle formation, these placeholders are placed on the front, left and right sides of virtual WMR with d distance. Based on the positions of placeholders, it is easy to obtain the desired position of MWMRs with time passes. Therefore, the desired position of the i-th in formation will be:   x (5) Zcmdi = Zcmdv + Ro i , yi where Zcmdi denotes thedesired   position of i-th WMR. Ro denotes the rotacos θ − sin θ x tion matrix . i is the offset of the i-th WMR from the virtual sin θ cos θ yi WMR’s position, i = 1, 2, ..., N ∈ N+ . Base on the function above, we can describe each WMR’s desired position in formation correctly and efficiently. After that, for achieving the purpose of

Robust Formation Tracking of Multiple Wheeled Mobile Robots

663

formation tracking, each WMR should track its own desired trajectory Zcmdi . For this purpose, all WMRs should achieve this object: Zref i → Zcmdi ,

(6)

Zref i denotes the actual trajectory of the i-th WMR. Finally, the velocity constraint should be considered. Because there are many real reasons to constrain the velocity changing, such as the limit of motor power, the limit of WMR’s structure, and so on. Therefore, velocity should be limited. For this purpose, set up: + (7) s.t. u− i ≤ ui ≤ ui , + where ui is the velocity of the i-th WMR, u− i and ui are the low limit and high limit of the i-th WMR’s velocity respectively.

3

Problem Formulation and Lagrangian-Based Controller

In this section, we first give a unified optimization problem description for solving easily. Then, a Lagrangian-based controller is designed as a solver. 3.1

Unified Problem Description

Eq. (2) (6) and (7) are three main problems we need to solve. Because these function’s independent variables are not the same, it’s hard to unify them together. For the reason presents above, unify the independent variables is the first step we need to do. For achieving Eq. (4), the classic method [13] is: Z˙ v − Z˙ cmdv = −k.

(8)

The variable  = Zv − Zcmdv denotes the error between Zv and Zcmdv , and k denotes a positive constant variable for improving the tracking accuracy to the desired trajectory. In real life, external noise always exists. For weakening the effect of external noise on formation tracking, we introduce an integration-enhanced feedback controlled strategy into the error function i.e.:  t dt), (9) Z˙ v = Z˙ cmdv − k − k2 ( + 0

where k2 is also a constant variable like k. Similarly, the i-th WMR’s trajectory tracking at velocity level can be written as:  t Z˙ ref i = Z˙ cmdi − ki − k2 (i + i dt), (10) 0

664

K. Jiang et al.

where ei = Zref i − Zcmdi . The Eq. (9) and (10) can be further written as: Av uv = Arightv ,

where Arightv t  dt). Let 0 i

(11a)

Ai ui = Arighti , (11b) t = Z˙ cmdv − k + k2 ( + 0 dt) and Arighti = Z˙ cmdi − ki + k2 (i + u = [uv ; u1 ; ...; uN ] ∈ R2(N +1) , Z˙ cmd = [Z˙ cmdv ; Z˙ cmd1 ; ...; Z˙ cmdN ] ∈ R2(N +1) , Aright = [Arightv ; Aright1 ; ...; ArightN ] ∈ R

2(N +1)

(12a) (12b) ,

A = diag(Av , A1 , A2 , ..., AN ) ∈ R2(N +1)×2(N +1) .

(12c) (12d)

N denotes the number of the real WMRs. Thus, after unify the description integrating with all MWMRs’ constraint, the constraint function should be: min s.t.

3.2

uT u/2, −

(13a) +

u ≤u≤u ,

(13b)

Au = Aright .

(13c)

Lagrangian Based Controller

For solving Eq. (13), we design one Lagrange-based controller. First of all, define Lagrange function [9,13]: L(u ∈ Ω, λ) = uT u/2 + λT (Au − Aright ),

(14)

where λ ∈ R is Lagrange multiplier. Ω is a set that Ω = {u ∈ R2(N +1) , u− ≤ u ≤ u+ } to bound the each WMR’s velocities. Based on KKT conditions, optimal solution of Eq. (14) can be equivalent: u = PΩ (u −

∂L ). ∂u

(15)

In Eq. (15), PΩ is a piecewise-linear projection operation to a set Ω. Overall, the Lagrangian based controller is written as: ∂L ) ∂u T = −u + PΩ (−A λ),

εu˙ = −u + PΩ (u − ελ˙ = Au − Aright ,

(16a) (16b) (16c)

where ε is a positive constant number that is associated with the convergence speed of the controller Eq. (16).

Robust Formation Tracking of Multiple Wheeled Mobile Robots

665

Table 1. Simulation parameters.

4

Parameters Values

Parameters Values

r

0.04 m

d0

0.075 m

u+ i

15 m/s

k

3

u− i

−15 m/s k2

4

L

0.185 m

0.002



Simulation

In this part, we will investigate the effectiveness and robustness of the proposed scheme for MATLAB simulation. The number of MWMRs is chosen as 3. In the simulation, MWMRs are expected to move in a triangle formation and the virtual WMR is required to track an inverted ‘U’ path. Parameters involved in the simulation are shown in Table 1. Initial values of ui , θi is ui (0) = 0, θi (0) = 90◦ . 4.1

Simulative Results Under Zero Noise

Without considering external noise, simulation results are shown in Fig. 2. Figure 2(a) shows the desired trajectory that is to be tracked by the virtual WMR and a desired triangle formation. Among the triangle formation, three placeholder (i.e., placeholder1, placeholder2, placeholder3) distance from the vir√ √ √ 3 3 tual WMR is always ( 4 ; 0), (0; 4 ) and (− 43 ; 0) respectively. Real trajectories achieved by three WMRs to maintain the desired triangle formation are shown in Fig. 2(b). Following it, starting from different positions: (−0.3; −0.4), (−0.8; −0.8) and (1; −0.6), three WMRs gradually form the desired triangle formation before 2 s. After this, they always maintain the triangle formation until the end. Their eventual path achieved by WMR1, WMR2, and WMR3 is denoted by magenta, cyan, and green, respectively. Figure 2(c) gives curves of these three WMRs’ respective heading angle θi , i = 1, 2, 3, during the execution. Based on the inverted ‘U’ trajectory, theoretically, each WMR’s heading angle should be 90◦ at the beginning and finally become −90◦ at the end. Therefore, the heading angles of three WMRs shown in Fig. 2(c) are correct in the formation. In Fig. 2(b), the trajectory achieved by the virtual WMR is coincided with the desired trajectory perfectly, which means that the virtual WMR accurately tracks the desired trajectory. We give the tracking error xv − xcmd and yv − ycmd of the virtual WMR to the desired trajectory at x-axis and y-axis, and heading angle error θv −θcmd in Fig. 2(d), where xv and yv denote the horizontal value and the vertical value of the virtual WMR, respectively, θv denotes actual heading of the virtual WMR. Following it, it is clearly observed that the trajectory tracking error reaches 10−3 level and the heading angle error is 0.08 at most. Therefore, it is concluded that the proposed virtual-WMR-based formation tracking scheme is effective under no external noise situation.

666

K. Jiang et al.

4 3 Desired trajectory Virtual WMR placeholder 1 placeholder 2 placeholder 3

1 Desired start formation

-1

-1

0

Desired end formation

1

2

Actual trajectory of virtual WMR

2

Actual trajectory of WMR 3

1

3

0

1

2

x(m)

(b)

0 -1 10

-1

4

WMR 1 WMR 2 WMR 3

0

Desired trajectory

-1

(a)

1

(rad)

Actual trajectory of WMR 2

x(m)

2

-2

Actual trajectory of WMR 1

0

0

20

30

40

3

0.5

Tracking error(m)

y(m)

2

y(m)

3

0.08 0.06 0.04 0.02 0 -0.02

4

xv-xcmd yv-ycmd v 20

25

0

- cmd

30

10-3 2 0 -2 -4

-0.5

0

t(s)

-6 25

26

10

20

27

30

28

40

t(s)

(c)

(d)

Fig. 2. Simulation results achieved by the controller Eq. (16) without considering external noise. (a): Desired trajectory and formation. (b): Actual trajectory. (c): Heading angles curves of WMRs during their execution. (d): Tracking error of the virtual WMR to the desired trajectory and heading error.

4.2

Robustness Against Noise

Now, we start to consider the formation guarantee problem of MWMRs disturbed by external uncertain interference. Disturbed by external noise, an integration-enhanced feedback controlled strategy Eq. (9) will be rewritten as:  t ˙ ˙ Zv = Zcmdv − k − k2 ( + dt) + w, (17) 0

and the Eq. (8)which cannot against external noise is rewritten as: Z˙ v = Z˙ cmdv − k + w,

(18)

where w denotes external noise. Figure 3 gives robustness illustration of schemes Eq. (8) and Eq. (9) against external noise, where external noise w is mathematically described as a random constant. As shown in Fig. 3(a), due to external noise, the virtual WMR does not track the desired trajectory successfully,

Robust Formation Tracking of Multiple Wheeled Mobile Robots

4

Actual trajectory of virtual WMR

2

Actual trajectory of WMR3

1 0

3

y(m)

y(m)

3

4

Actual trajectory of WMR2

Actual trajectory of WMR1

Desired trajectory

Actual trajectory of WMR1

Actual trajectory of WMR2 Actual trajectory of virtual WMR

2

Actual trajectory of WMR3

1 0

-1

Desired trajectory

-1

-1

667

0

1

2

3

4

-1

0

1

x(m)

x(m)

(a)

(b)

2

3

Fig. 3. Robustness comparison between Eq. (8) and Eq. (9) under external noise w = rand(·). (a): Results achieved by Eq. (8). (b): Results achieved by Eq. (9).

behaving as the virtual deviated from the theoretical trajectory obviously. All WMRs are not moving in a triangle formation, and WMR2 collides with WMR3. In this case, the purpose that tracks an inverted‘U’ trajectory with a triangle formation fails. On the contrary, in simulation result achieved by the proposed scheme Eq. (9) as shown in Fig. 3(b), the virtual WMR disturbed by random noise still tracks the desired trajectory, and three WMRs also form the desired triangle formation successfully under external disturbances. This illustrates the robustness of our proposed scheme against noise.

5

Conclusion

In this study, we proposed a virtual-WMR-based formation tracking scheme for MWMRs. Especially, the MWMRs’ formation tracking problem was reduced conceptually to a trajectory tracking of a virtual WMR with placeholders that described the desired position of each WMR in formation. Therefore, with placeholders, the moving trajectories corresponding to each WMR could be easily obtained. To achieve formation control, a minimization problem was obtained to represent the considered formation problem with the perspective of constraint optimization. By constructing a Lagrangian-based controller as a solver, the formation control of MWMRs was addressed. In addition, an integration-enhanced stated feedback strategy is introduced into the trajectory tracking strategy, achieving that MWMRs still maintain the desired formation of external noise. The proposed method is simple and is easy to prescribe formation control with a formation performance guarantee.

References 1. Borenstein, J., Koren, Y.: Error eliminating rapid ultrasonic firing for mobile robot obstacle avoidance. IEEE Trans. Robot. Autom. 11(1), 132–138 (1995)

668

K. Jiang et al.

2. Consolini, L., Morbidi, F., Prattichizzo, D., Tosques, M.: Leader-follower formation control of nonholonomic mobile robots with input constraints. Automatica 44(5), 1343–1349 (2008) 3. Ghommam, J., Mehrjerdi, H., Saad, M., Mnif, F.: Formation path following control of unicycle-type mobile robots. Robot. Auton. Syst. 58(5), 727–736 (2010) 4. Guo, D., Li, K., Liao, B.: Bi-criteria minimization with MWVN-INAM type for motion planning and control of redundant robot manipulators. Robotica 1–21 (2018) 5. Hootsmans, N., Dubowsky, S.: Large motion control of mobile manipulators including vehicle suspension characteristics. In: ICRA, vol. 91, pp. 2336–2341. Citeseer (1991) 6. Lawton, J., Beard, R.W., Young, B.J.: A decentralized approach to formation maneuvers. IEEE Trans. Robot. Autom. 19(6), 933–941 (2004) 7. Li, S., Cui, H., Li, Y., Lu, B.: Decentralized control of collaborative redundant manipulators with partial command coverage via locally connected recurrent neural networks. Neural Comput. Appl. 23(3–4), 1051–1060 (2013) 8. Li, S., He, J., Li, Y., Rafique, M.U.: Distributed recurrent neural networks for cooperative control of manipulators: a game-theoretic perspective. IEEE Trans. Neural Netw. Learn. Syst. 28(2), 415–426 (2017) 9. Li, X., Xu, Z., Li, S., Su, Z., Zhou, X.: Simultaneous obstacle avoidance and target tracking of multiple wheeled mobile robots with certified safety. IEEE Trans. Cybern. (2021). https://doi.org/10.1109/TCYB.2021.3070385 10. Liao, J., Chen, Z., Yao, B.: Model-based coordinated control of four-wheel independently driven skid steer mobile robot with wheel-ground interaction and wheel dynamics. IEEE Trans. Ind. Inform. 15(3), 1742–1752 (2018) 11. Mcguire, K.N., Wagter, C.D., Tuyls, K., Kappen, H.J., Croon, G.C.H.E.D.: Minimal navigation solution for a swarm of tiny flying robots to explore an unknown environment. Sci. Robot. 4(35), eaaw9710 (2019) 12. Xiao, L., Zhang, Y.: A new performance index for the repetitive motion of mobile manipulators. IEEE Trans. Cybern. 44(2), 280–292 (2013) 13. Xu, Z., Li, S., Zhou, X., Zhou, S., Cheng, T.: Dynamic neural networks for motionforce control of redundant manipulators: an optimization perspective. IEEE Trans. Ind. Electron. (2020). https://doi.org/10.1109/TIE.2020.2970635 14. Zhang, Y., Wang, J., Xia, Y.: A dual neural network for redundancy resolution of kinematically redundant manipulators subject to joint limits and joint velocity limits. IEEE Trans. Neural Netw. 14(3), 658–667 (2003) 15. Zhang, Y., Yan, X., Chen, D., Guo, D., Li, W.: QP-based refined manipulabilitymaximizing scheme for coordinated motion planning and control of physically constrained wheeled mobile redundant manipulators. Nonlinear Dyn. 85(1), 245–261 (2016) 16. Zhang, Z., Chen, S., Xie, J., Yang, S.: Two hybrid multiobjective motion planning schemes synthesized by recurrent neural networks for wheeled mobile robot manipulators. IEEE Trans. Syst. Man Cybern. Syst.(2019) 17. Zhao, W., Li, X., Chen, X., Su, X., Tang, G.: Bi-criteria acceleration level obstacle avoidance of redundant manipulator. Front. Neurorobot. 14(54) (2020)

A Novel Path Planning Method Based on Regional Index Algorithm for Hyper-redundant Manipulator Longfei Jia1,2 , Yaxing Guo1,2 , Yunfei Tao1,2 , He Cai1,2 , Tianhua Fu1,2 , and Yuping Huang1,2(B) 1 Beijing Institute of Precision Mechatronics and Controls, Beijing 100076, China 2 Laboratory of Aerospace Servo Actuation and Transmission, Beijing 100076, China

[email protected]

Abstract. Given a large amount of calculation in the traditional path planning algorithm and the inability to control the distance between the path and the obstacle, a path planning method based on a regional index is proposed in this paper. First, by comparing the three methods of measuring distance, the characteristics of the regional index method are outlined, and how to divide regions and the corresponding regional indicators are outlined. Secondly, the potential energy distribution maps by use of the traditional method and the regional index method respectively are analyzed, and the regional indicators method can be used to plan the path artificially. Finally, through simulation, four paths are obtained applying two artificial potential field methods and two regional index methods. By comparing the characteristics of these four paths, it is found that the regional index method can guarantee the distance between the path and the obstacle very well, which verifies the effectiveness of the method. Keywords: Path planning · Regional indicators (RI) · Potential energy distribution · Artificial potential field

1 Introduction With the development of robotics, higher requirements for robots and manipulators have been proposed, and manipulators with fewer degrees of freedom have been unable to meet the need. Therefore, redundant manipulators and hyper-redundant manipulators are currently attracting attention from researchers. Compared with traditional manipulators, redundant manipulators can be effective to avoid the odd heterogeneous problems that often occur in non-redundant robots and achieve more tasks. However, the multi-degree of freedom also makes the model of the manipulator more complicated, and the difficulty of path planning for redundant manipulators also increases. There are many methods for path planning, the basic idea of the artificial potential field method [1] is to construct an attractive potential field at the target position to generate attractive to the robot, and construct a repulsive potential field at the obstacle position to generate a repulsive force on the robot. Then the robot moves under the combined action of the attractive and repulsive force. Although this method has a small © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 669–679, 2021. https://doi.org/10.1007/978-3-030-89092-6_61

670

L. Jia et al.

amount of calculation, and the obtained path is smoother and safer, it is easy to fall into the local minimum problem and is difficult to reach the target point. Dijkstra algorithm [2] assigns two labels (P(vi ), λi ) to the point vi , where the first label P(vi ) represents the length of the shortest path from the starting point v1 to vi , and the second label λi represents the subscript of the adjacent point before vi on the shortest path from v1 to vi that is used to indicate the path so that the path from the endpoint to the start point can be traced back to find the shortest path from v1 to vi . The Dijkstra algorithm traverses all nodes during the search process. Therefore, the searching process of the algorithm is relatively slow, and the efficiency of solving the optimal solution between points in a large graph with large number of nodes is low. A* algorithm [3] is a classic heuristic search algorithm, which draws on the traversal search idea of graphs and the advantages of the Dijkstra algorithm. However, this method is affected by the choice of the evaluation function. As the search range expands, the amount of calculation becomes larger, and the path may not be optimal. In recent years, the use of random sampling algorithms for path planning of manipulators has become popular, and the two most representative algorithms are Probability Map Method (PRM) and Rapid Random Tree Method (RRT). The PRM method [4, 5] is originally used for the planning of vehicle-type robots in a static environment and used for path planning of group robots later. This method has an excellent performance in solving high-dimensional space and path planning problems with complex constraints. However, the method has poor search accuracy and robustness. The RRT algorithm [6, 7] is similar to the PRM algorithm, and the path is also obtained by sampling and searching the planning space of the manipulator. But unlike the PRM algorithm to construct a probability map, the RRT algorithm applies sampling nodes to construct a search tree. The method is fast and suitable for multi-dimensional space. However, the search accuracy of this method is low, and the path is not necessarily optimal. In the above method, to achieve the purpose of avoiding obstacles, the ways of Euclidean distance and pseudo distance are used to calculate the distance between the manipulator and the obstacle. However, due to a large amount of calculation, these methods cannot be efficiently used for path planning in complex systems. In this paper, aiming at the 17-degree-of-freedom hyper-redundant manipulator, the regional index method is applied to measure the distance between a spatial point and an obstacle instead of Euclidean distance and pseudo-distance, which greatly reduce the amount of calculation. Furthermore, the path planned by this method can maximize the distance between the path and the obstacle as much as possible.

2 Model of Hype-Redundant Manipulator

Fig. 1. The overall schematic diagram of the manipulator.

A Novel Path Planning Method Based on Regional Index Algorithm

671

In this paper, path planning is carried out for a hyper-redundant manipulator with 17 degrees of freedom. The overall schematic diagram of the manipulator is shown in Fig. 1. The manipulator is composed of eight links, a driving mechanism, and a propulsion platform. The eight links are connected by joints, and each joint is connected by a special hollow universal joint. The mounting discs at both ends of the joint are passed through the steel cable through heat-treated steel sleeves. The end of the manipulator is equipped with actuators such as industrial cameras and manipulators. The driving mechanism is used to drive the manipulator. Each joint is driven by three steel cables with bosses. Each steel cable is connected with a screw nut pair by a connector. And the screw nut pair are driven by a hollow cup servo motor with a high power density. Since the positions of the servo motor and the steel cables are distributed in different circles, a special connecting plate connects the nut and the steel cables to achieve a compact design of the drive mechanism. The driving mechanism is installed on a horizontal servo sliding table to realize the overall feed of the manipulator.

Fig. 2. Rotation motion of universal joint.

Each joint is controlled by three steel cables and the joint can rotate around the universal joint by changing the length of the steel cables as shown in Fig. 2.

3 The Principle of Algorithm 3.1 Three Methods to Characterize the Distance

Fig. 3. Schematic diagram of the distance between the manipulator and the obstacle.

672

L. Jia et al.

This section describes three methods for measuring the distance between the manipulator and obstacles: Euclidean distance, Pseudo-distance, Regional index, and the difference between these three methods are compared. The schematic diagram of measuring the distance between the manipulator and the obstacle by these three methods is shown in Fig. 3. The solid red circle in the figure denotes the obstacle, and the length of the green line represents the distance between the manipulator and the obstacle. The dotted circle in the figure is the boundary line of the divided area. (1) Euclidean distance. The minimum Euclidean distance between the manipulator and the obstacle is used to characterize the actual distance. As shown in Fig. 3, the minimum Euclidean distance between the ith link and the obstacle is d i . (2) Pseudo-distance. To reduce the amount of calculation, the distance between the manipulator and the obstacle can be judged by the pseudo distance. The closest distance between the midpoint of the axis of manipulator (that is, the center of mass of manipulator) and the obstacle surface can be regarded as a pseudo-distance. By equating the manipulator as a point, the amount of calculation is greatly reduced. As shown in the second graph in Fig. 3, the minimum pseudo distance between the ith link and the obstacle is d i . Although the efficiency of calculating adopting pseudo distance is high, the accuracy is far lower than that of calculating distance using Euclidean distance. (3) Regional index. Thus, a method to use the regional index to characterize the distance between the manipulator and obstacles is proposed in this paper, which takes the obstacle as the center and spreads outwards, thereby dividing different areas, and characterizing the distance between the link and the obstacle by analyzing the area where the link is located. To simplify the model, only utilizing the area where a line segment formed by the two ends of the link is located to characterize the distance between the link and the obstacle is analyzed. For example, in the third picture in Fig. 3, the first link is in Area 6 and Area 7. Only the area closest to the obstacle is analyzed, that is, Area 6 is used to characterize the link and the obstacle. By analogy, the 2th –8th links are located in Area 4, Area 3, Area 3, Area 3, Area 4, Area 5, Area 6. Each of these three methods has its advantages and disadvantages. The Euclidean distance method needs to analyze the distances between many points on the link and obstacles, and the results are accurate and reliable, but the amount of calculation is large. The pseudo-distance method only needs to calculate a few points to get the result, although it saves the calculation time, but the accuracy is not high. The regional index method does not calculate the distance between a point and an obstacle, but analyzes the area where some key points are located, which not only reduces the amount of calculation, but also effectively controls the distance between the links and the obstacles with the help of this method. The method in this paper can divide the area in advance according to the specific environment. And the area is divided before the path planning, without increasing the amount of calculation. The distance between each area in the above figure is relatively large, which is not conducive to measuring the distance between the link and the obstacle. How to divide the area in detail and how to define the area index corresponding to each area will be described. There is an infinite index for measuring the distance between the link and the obstacles obtained by the first two methods, and the distance between the link and the obstacle in a region corresponding to the method in this paper is constant.

A Novel Path Planning Method Based on Regional Index Algorithm

673

3.2 The Method of Dividing Region and the Corresponding Regional Index

Fig. 4. Area diagram.

Generally, basic shapes such as balls, cylinders, cuboids, cones can be used to characterize the shape of obstacles. And if the shape of the obstacle is complex, it can be simplified to the basic shape. The area is shown in Fig. 4, with the obstacle as the center, the space around it is divided into a safe area, a transition area, a prewarning area, and a hazardous area. When the distance between link and obstacle is too far, there is no need to consider the influence of the obstacle on link. When the distance between link and obstacle is a little close, the obstacle has little influence on the robot arm, and the thickness of the region can be larger, that is, there is no need to divide the region in too much detail. For example, the thickness of the region can be 100 mm. When the distance between link and obstacle is very close, the obstacle has a great impact on link, and the thickness of area needs to be smaller, for instance, the thickness of area can be 20 mm. In the safe area, there is no need to consider the distance between the link and the obstacle. When the link is in the transition area, the obstacle has less influence on the link, so the transition area is not subdivided and the maximum thickness of the transition area is 100 mm. When the link is in the prewarning area, it is necessary to ensure that the distance between the obstacle and the boom is as small as possible. In this paper, the prewarning area is divided into ten small areas where the area nearest to the obstacle is area 1 and the furthest is area 10, and the maximum thickness of each small area is 20 mm. When the link is in a hazardous area because the surface of the link is at a certain distance from the axis of the link, it is easy to touch obstacles and a warning is issued when the link moves to the hazardous area. Here, the hazardous area is divided into three parts. The distance from the obstacle is divided into hazardous area 1, hazardous area 2, and hazardous area 3 from near to far. The maximum thickness of each small area is 20 mm. After dividing the area, the corresponding regional index can be formulated. If the closest distance between the middle position of each area and the obstacle surface is regarded as the index corresponding to the area, it can be obtained that if the link is in the transition area, the distance between the obstacle and the link is 310 mm; if the

674

L. Jia et al.

link is in the ith small area of the warning, the distance between the obstacle and the link is (50 + 20i) mm; if the link is located in the ith small area of danger, the distance between the obstacle and the link is (−10 + 20i) mm. Indexes obtained from the above is the distance index and the relationship between the index and the distance between the obstacle and the link is linear, thus a path as far as possible from the obstacle can be planned. In this paper, the specific values of the regional indexes corresponding to the distance between the obstacle and the link are shown in Table 1 and formula. ⎧ 360, dobs ≥ 360 ⎪ ⎪ ⎨ 310, 360 > dobs ≥ 260 (1) RI = ⎪ 50 + 20i, 260 > dobs ≥ 60 ⎪ ⎩ −10 + 20i, 60 > dobs It can be seen from the Table 1 that if the link is located in a safe area, the distance index between the obstacle and the link is the largest. The closer the area to the obstacle, the smaller the corresponding distance index. In the process of path planning, planning paths in dangerous areas should be avoided as much as possible. Table 1. Values of regional indexes. Area Hazardous Hazardous Hazardous Prewarning Prewarning area 1 area 2 area 3 area 1 area 2 RI

10

30

50

70

90

Area Prewarning Prewarning Prewarning Prewarning Prewarning area 3 area 4 area 5 area 6 area 7 RI

110

130

150

170

190

Area Prewarning Prewarning Prewarning Transition area 8 area 9 area 10 Area

Safe area

RI

360

210

230

250

310

Fig. 5. Three paths of artificial planning.

A Novel Path Planning Method Based on Regional Index Algorithm

675

Fig. 6. Schematic diagram of the manipulator after reaching the target point along the path.

When the link is far away from target, the gravitational action of target can be ignored first and the area farthest from the obstacle can be found from a limited area, along which the path can be planned. When the link is close to the target, the gravitational action of the target is considered at this time, so that the planned path will eventually move to the target point. Based on this principle, the path can be planned artificially as shown in Fig. 5. For three types of targets, three corresponding paths can be artificially planned. The end effector of manipulator can reach the target point when the eight-link manipulator makes repeated path movements along the corresponding planned path. Taking the second artificially planned path as example, the schematic diagram of manipulator after reaching the target point along the path is shown in Fig. 6. It can be seen from the figure when the manipulator moves along the artificially planned path, it will not touch the obstacle and always keep a certain distance from the obstacle, and finally can reach the target point. It proves that it is effective to apply the regional index method to plan the path artificially.

4 Simulation According to the principle of the path planning method based on the regional index described above, three feasible paths can be planned artificially. However, path planning is achieved through autonomous planning with the help of the computer, rather than manual planning. The following is to analyze the characteristics of the path planned by the method of different characterization distances through simulation. 4.1 Potential Energy Distribution Diagram If the traditional distance between a point and an obstacle is used to represent the potential energy of each point in the space, the potential energy distribution diagram under the traditional method can be obtained by simulation as shown in Fig. 7(a). If the regional index under the linear relationship in the regional index method is used, the potential energy distribution diagram can be obtained by simulation as shown in Fig. 7(b). The potential energy in the figure only represents the repulsive potential energy of the obstacle

676

L. Jia et al.

(a) Traditional method

(b) Regional index method

Fig. 7. Distribution diagram of repulsive potential energy.

to each space point and does not include the gravitational potential energy of the target point to each space point. It can be seen from the figure that the potential energy distribution map obtained by the regional index method can clearly see the dividing line between the various regions, and the corresponding repulsive potential energy in each region is also a certain value, but the gravitational potential energy is different. And when the distance between the spatial point and the obstacle is greater than a certain value, the corresponding repulsive potential energy is always a certain value, instead of increasing infinitely like the traditional method. 4.2 Path Planned Under the APF Method First, the APF method is applied for path planning. The size and direction of the repulsive force Fobs is calculated by the shortest distance between the spatial point and the surface of the three obstacles. The direction of the repulsive force is from the obstacle points to the spatial point. The calculation formula of the repulsive force is as follow: |Fobs | =

1 , dobs > 0 2 dobs

(2)

The d obs in the formula is the shortest distance between the space point and the obstacle. Let the gravitational force of the target point to the space point be Faim , the direction of the gravitational force is from the space point points to the target point, and the gravitational force is set as the formula. |Faim | = c1 · daim +

c2 ,d >0 daim aim

(3)

The d obs in the formula is the distance between the space point and the target, c1 and c2 are constants set by themselves. Combining the repulsive force Fobs and the gravitational force Faim , the direction of the resultant force Fall in space can be obtained, as shown in the following formula. Fall = Fobs + Faim

(4)

A Novel Path Planning Method Based on Regional Index Algorithm

677

A point outside 1 mm along the direction of the resultant force is selected as the next discrete point. After getting the next discrete point, the third discrete point can be found in the same way. By analogy, the loop will be terminated until the distance between the planned path and the target point is less than 1 mm, that is, the path from the starting point to the target point is planned. In Fig. 8(a), c1 is equal to 1000 and c2 is equal to 1. In Fig. 8(b), c1 is equal to 1000 and c2 is equal to 0.1. The black line in the figure is the final planned path. It can be seen from the figure that the final planned path is relatively close to the obstacles. If the hyper-redundant manipulator moves along this path, it may encounter obstacles.

(a) The first path using APF method

(b) The second path using APF method

Fig. 8. Path planned using APF method.

4.3 Path Planned Under the RI Method According to the regional index set in Table 1, the potential energy of the corresponding obstacle rejection in each region can be deduced and then considering the gravitational action of the target point, the path expressed by the blue line can be obtained by simulation as shown in Fig. 9(a). It can be seen from the figure that if the manipulator moves along this path, although it will not encounter obstacles, the path length is relatively long, and the length of the hyper-redundant manipulator is limited and cannot reach the target point.

(a) The first path using RI method

(b) The second path using RI method

Fig. 9. Path planned using RI method.

678

L. Jia et al.

Since there is only one target point, the final planned path is longer. In response to this problem, four extra gravitational points are set in this paper, namely: (326.85,1260), (1000,1187.5), (1673.15, 1260), (2250, 1460). Set |Faim | to: |Faim (x, y)| =

4  i=1

  (x − Xi )2 + (y − Yi )2 1 + (1 + ) (x − Xaim )2 + (y − Yaim )2 1 + a(x−Xi ) 1 + a(x−Xaim )

(5) x and y are the two coordinate values of the space point. |Faim (x, y)| is the gravitational magnitudes corresponding to the point. Xi and Yi are the two coordinate values of the ith special gravitational point. Xaim and Yaim are the two coordinate values of the target point. a is an adjustable value equal to 3 in this paper. By introducing extra gravitational points, the path obtained by simulation is shown in Fig. 9(b). It can be seen from the figure that if the manipulator can reach the target point without passing through the path, it still maintains the maximum distance from the obstacle. 4.4 The Characteristics of Four Paths Four paths can be obtained through two APF methods and two RI methods. The characteristics of these four paths are analyzed below. Through these four paths, the change rule of the shortest distance d obs between each point in the path and the obstacle can be obtained, as shown in Fig. 10. It can be seen from the figure that the d obs under the RI method is always larger. After setting d min = min(d obs ), the shortest distances between the four paths and obstacles can be obtained shown in Table 2. And it can be seen from the table that the d min obtained by adopting the two RI methods are 200 and 180 respectively, which are the distances corresponding to the two boundaries of the early-warning area 7. The d min corresponding to the paths planned by the two RI methods is much larger than the d min corresponding to the paths planned by the two APF methods, which verifies that the path planned by the RI method can be kept in a certain area and as far away as possible from obstacles.

Fig. 10. The change discipline of d obs under the four paths.

A Novel Path Planning Method Based on Regional Index Algorithm

679

Table 2. Performance corresponding to four paths.

d min

APF1

APF2

RI1

RI2

29.45

100.95

200.00

180.00

5 Conclusion In this paper, a method applying the regional index to characterize the distance between the manipulator and obstacles is proposed. The method takes the obstacle as the center and spreads outwards, thereby dividing different areas, and characterizing the distance between the link and the obstacle by analyzing the area where the link is located. It is found through simulation that the shortest distance between the paths planned by the two RI methods and the obstacles is much greater than the shortest distances between the paths planned by the two APF methods and the obstacles, which verifies that during the paths planned by RI method, the distance between the path and the obstacle always maintains a certain distance. Besides, if the accuracy is high, areas divided are even more. If the accuracy requirement is low, there will be fewer areas. The proposed method is only suitable for the environment where the obstacle is not moving. If there is a dynamic obstacle, the distribution of each area in the space and the area index corresponding to each area need to be refreshed in real-time. And the position and size of the obstacle model in this paper are known. Later, the obstacle information can be obtained through the camera, and the information can be converted into obstacle data through visual processing, which can be applied for constructing obstacles for path planning.

References 1. Chen, Y., Bai, G., Zhan, Y., Hu, X., Liu, J.: Path planning and obstacle avoiding of the USV based on improved ACO-APF hybrid algorithm with adaptive early-warning. IEEE Access 9, 40728–40742 (2021) 2. Lu, Y., Tang, K., Wang, C.: Collision-free and smooth joint motion planning for six-axis industrial robots by redundancy optimization. Robot. Comput. Integrated Manuf. 68, 1–16 (2021) 3. Sang, H., You, Y., Sun, X., Zhou, Y., Liu, F.: The hybrid path planning algorithm based on improved A* and artificial potential field for unmanned surface vehicle formations. Ocean Eng. 223, 1–16 (2021) 4. Zhao, Y., Liu, J., Ma, J., Wu, L.: Multi-branch cable harness layout design based on genetic algorithm with probabilistic roadmap method. Chinese J. Mech. Eng. 34(1), 1–11 (2021) 5. Ravankar, A., Ravankar, A., Emaru, T., Kobayashi, Y.: HPPRM: Hybrid potential based probabilistic roadmap algorithm for improved dynamic path planning of mobile robots. IEEE Access 8, 221743–221766 (2020) 6. Wang, J., Li, B., Meng, M.: Kinematic constrained bi-directional RRT with efficient branch pruning for robot path planning. Expert Syst. Appl. 170, 1–7 (2021) 7. Yu, Z., Xiang, L.: NPQ-RRT*: an improved RRT* approach to hybrid path planning. Complexity 2021, 1–10 (2021) 8. Milad, N., Esmaeel, K., Samira, D.: Multi-objective multi-robot path planning in continuous environment using an enhanced genetic algorithm. Expert Syst. Appl. 115, 106–120 (2019)

Path Planning for Nuclear Emergency Robot in Radiation Environment with Uneven Terrain Ying Huang, Yan Zhou, and Zhenhua Xiong(B) State Key Laboratory of Mechanical System and Vibration, School of Mechanical Engineering, Shanghai Jiao Tong University, Shanghai 200240, China [email protected]

Abstract. In order to protect people from nuclear radiation, nuclear emergency robots are widely used in radiation environment. Due to the performance limitation of the robot, it cannot pass the area where the radiation dose rate is extremely high or the land slope exceeds a threshold. However, current path planning method of nuclear emergency robot does not consider all these factors, which are inevitable in nuclear emergency cases. Thus, in this paper, a path planning algorithm for nuclear emergency robot is proposed, which considers both radiation and uneven terrain. Firstly, the costmap of radiation is calculated from the radiation field distribution map. The three-dimensional point cloud map is projected into a gray image, and the image gradient is calculated to obtain the costmap of traversability. The combination of two costmaps forms a multi-layer costmap as the input of the path planning algorithm. Then the A* algorithm with the heuristic function of distance, radiation, and slope is proposed to search for the best path. The simulation results show that the proposed method can plan a safe and effective path for the robots according to different requirements. Keywords: Path planning · Radiation environment · Uneven terrain

1 Introduction With the rapid development of the nuclear industry and the wide application of nuclear power technology, the requirement for nuclear radiation safety is increasingly urgent. In recent decades, the continuous development of robot technology has enabled robotics to complete a radiation environment’s work [1]. At present, operating robots for the nuclear environment are widely concerned by researchers. Most of the nuclear environmental robots developed at this stage are mobile teleoperated robots. However, in many cases of emergency disposal, remote control operation will be difficult [2]. This puts forward a new demand for the safe and autonomous navigation of robots in the radiation environment. There are two critical issues need to be resolved. First of all, due to the limitation of sensor technology and total cost, the antiradiation design of the robots is still unable to make them ignore the radiation. This requires that the robots need to avoid radiation as much as possible. Secondly, there may be obstacles such as collapsed walls in the accident environment, forming slopes on the © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 680–690, 2021. https://doi.org/10.1007/978-3-030-89092-6_62

Path Planning for Nuclear Emergency Robot

681

ground. At present, mobile robot navigation generally uses two-dimensional (2D) map, which will classify such obstacles as impassable areas. But emergency robots have a certain climbing ability to pass through the small slopes. If the influence of slope is not considered, the path planning in the actual passable environment may fail. In this work, a path planning algorithm for autonomous navigation of nuclear emergency robots is proposed. We assume that the three-dimensional point cloud map and radiation field distribution map is acquired by the radiation detection robot [3], which plays the role of Scout. Firstly, the costmap of radiation is generated from the radiation field distribution map, and the costmap of traversability is generated from the threedimensional point cloud map. The combination of the two constitutes a multi-layer costmap as the input of path planning. Then the improved A* algorithm is used for path planning. The difficulties of path planning are how to describe the environment accurately and consider various factors simultaneously. Our focus is on the costmaps’ generation and the design of the A* algorithm, which aims to consider the robot’s traversability and plan the path with the minimum cost of transport and radiation dose.

2 Related Works 2.1 Path Planning Method in Radiation Environment Many researchers have made achievements in path planning of the radiation environment field, but they still face challenges. Khasawneh et al. [4] proposed a localized navigation algorithm to avoid radiation, which requires a well-designed and distributed wireless sensor infrastructure. Liu et al. carried out a series of studies on pedestrian walking path problems in the radiation environment. They transferred path planning problem into a variant traveling salesman problem and proposed a method for radiation area based on particle swarm optimization (PSO) algorithm, which solved the multi-objective path problem in the radiation area [5]. They also proposed a minimum dose path planning algorithm based on A* algorithm [6]. Path planning in the radiation field, combined with virtual reality technology, can effectively help nuclear power plants’ staff carry out safe and intuitive path planning. Besides, pedestrian path planning does not need to consider how to cross the ramp, stairs, and other obstacles, so it cannot be directly used for robots’ autonomous navigation. Tan et al. [7] proposed a radiation avoidance algorithm based on the Beetle Antenna Search (BAS), which has higher computational efficiency. However, only simulation experiment was carried out in the barrier-free 2D environment. 2.2 Path Planning Method in Uneven Terrain The perception and identification of the environment is an essential part of robots’ autonomous navigation. Three-dimensional (3D) environment sensing such as 3D Simultaneous Localization and Mapping (SLAM) has become more and more popular. The SLAM algorithm can be used to obtain 3D maps, but it is still challenged to apply a 3D map to robot navigation. In recent years, the main path planning algorithms for mobile robots in 3D environments were reviewed in [8]. However, due to the increase of map dimension, it often brings a heavy burden to the computer. The algorithm usually has a

682

Y. Huang et al.

long processing time and low efficiency. For uneven terrain, Papadakis [9] analyzed the judgment method of terrain traversability of Unmanned Ground Vehicles (UGV) and gave a clear definition and classification. Many researchers applied visual images to investigate the traversability of uneven areas. Hirose et al. [10] used the Generative Adversarial Networks to effectively predict whether the area seen in the input image is safe for the robot. Schilling et al. [11] trained random forests with a small set of labeled images and point clouds to generate traversability estimations in novel environments. Visual image has more information, but it cannot be applied to nuclear emergency response robots due to the limitation of environment. Some work focuses on using only 3D point clouds to achieve the same effect. Combined with the vehicle model, Tang et al. [12] marked all map grid cells as reachable, dangerous, or unknown. Wang et al. [13] projected multi-layer maps from OctoMap and calculated the slope of the occupancy voxel to generate the traversable map. Wermelinger et al. [14] constructed an elevation map and extracted terrain characteristics such as slope, roughness, and steps to calculate the scalability index. However, these uneven terrains are very different from the working environment of nuclear emergency robots. Simple and fast identification of traversability has become a problem that must be solved in the autonomous navigation of nuclear emergency robots.

3 Environment Representation Providing an accurate environment for path planning is the key to get a reasonable path. Inspired by the layered costmaps [15], a multi-layer costmap including radiation information and traversability information is constructed as shown in Fig. 1.

Master Inflation

Radiation Caution Zones

Cost of Radiation Dose

Navigation

Costmap of Radiation Non-traversable Map

Cost of Transport

Costmap of traversability

Fig. 1. The structure of the costmap.

3.1 Costmap of Radiation Radiation Caution Map. Radiation caution map records areas that are inaccessible due to excessive radiation dose. When the radiation dose rate in the environment exceeds the maximum instantaneous dose rate that the robot can bear, the sensor carried by the robot may fail, resulting in serious consequences. Therefore, when the radiation dose

Path Planning for Nuclear Emergency Robot

683

rate in the environment is greater than the threshold, it is considered that there is a lethal obstacle in the position, and the robot cannot pass. The values stored in the radiation caution map are as follows.  LETHAL_OBSTACLE value ≥ threshold c1[index] = (1) FREE_SPACE value < threshold where c1[index] is the corresponding value to the grid index in the costmap. Costmap of Radiation Dose. Costmap of radiation dose records the radiation dose received by the robot when it passes through the area. After discretization, the cumulative dose of the robot from the start S to the endpoint G along the path P is related to the dose rate of each grid R(i, j) and the robot’s motion speed v. The total amount of radiation received by the robot is E, which can be computed as E≈

 k:0≤s·k≤P

R(i, j) ·

s v

(2)

where s is the length of the grid. In order to facilitate the subsequent calculation, the value stored in the costmap of radiation dose is c2[index] = R(i, j) ·

s v

(3)

3.2 Costmap of Traversability The digital elevation model (DEM) is a solid ground model that uses ordered numerical arrays to represent the ground elevation. Inspired by DEM, the 3D point cloud is transformed into a gray image, as shown in Fig. 2. The gray value of the image pixel represents the average height of the point cloud in the corresponding grid. Therefore, when the gradient exceeds a certain threshold, it means that the robot cannot pass. In Fig. 2, path 1 and path 2 are passable, while path 3 is not.

Fig. 2. Schematic diagram of traversability judgment.

The process of converting point cloud to the costmap of traversability is as follows. Based on the characteristics of environment structure, the point cloud is firstly identified and corrected, and the high-altitude point cloud is filtered. It is necessary because the

684

Y. Huang et al.

map coordinate system established by SLAM may be inclined and contain information such as roofs that are not valid for path planning. Then, the processed point cloud is projected into a grayscale image, and the pixels of partial defect elevation information are filled by image closing operation. The image gradient can be calculated quickly by using the convolution of the image, and then the costmap of traversability can be obtained. Considering the eight-direction movement, the four-direction Sobel operator is used to calculate the gradient of 0◦ , 45◦ , 90◦ and 135◦ . The calculation of 0◦ gradient is used as an example to illustrate the relationship between slope angle θ and gradient value g. As shown in Fig. 3, the relationship between θ and h can be obtained, and the conversion formula between θ and g is as follows. θ = arctan(

g · hmax ∗ 255) 8s

(4)

where hmax is the maximum height of the filtered point cloud.

Fig. 3. Schematic diagram of gradient calculation.

Non-traversable Map. Non-traversable map records areas that are inaccessible due to excessive ground slope. For safety’s sake, when the local slope in any direction is too large, the robot cannot pass because of robot performance limitations. The values stored in the non-traversable map are as follows. ⎧ ⎨ LETHAL_OBSTACLE value ≥ threshold c3[index] = NO_INFORMATION value = unknown (5) ⎩ FREE_SPACE value < threshold Costmap of Transport. Costmap of transport records the cost of overcoming terrain changes when the robot passes through the area. Although the robot can pass when the terrain gradient is less than a certain threshold, the larger the gradient is, the higher the cost of moving is. The cost of transport should be taken into account in path planning.

Path Planning for Nuclear Emergency Robot

685

The values stored in the radiation caution map are as follows. c4[index] = [c4[index]1 , c4[index]2 , c4[index]3 , c4[index]4 ]

(6)

where c4[index]k , k = 1, 2, 3, 4 represents the gradient value in four directions.

4 The Path Planning Algorithm After obtaining the environment information, the A* algorithm is used to find the path with the least cost. The path could be searched by the equation as follows. F(n) = G(n) + H (n)

(7)

where F(n) represents the cost from the starting node to the target node through the node N , G(n) is the actual cost from the starting node to the current node N , H (n) is the estimated cost from the current node N to the target node. 4.1 Calculation of Actual Cost The path length L and radiation dose E from the starting node to the current node are given by Eq. (9) and (10). L= E≈

k=1

s ∗ xk

n−1   c2k (i, j) + c2k+1 (i, j) ∗ xk

 xk =

n−1

k=1

1.4, if path P is diagonal in grid k; 1, otherwise.

(8) (9) (10)

The cost of transport T is related to the image gradient and motion direction, which is computed as follows. T=

n−1 k=1

c4k→k+1 (i, j)

(11)

where c4k→k+1 (i, j) is the cost of transport from grid k to grad k + 1. After normalizing each part, the actual cost could be designed as follows. G(n) = ε1 · n−1 k=1 xk /1.4  n−1 + ε2 · k=1 c2k (i, j) + c2k+1 (i, j) ∗ xk /(1.4 ∗ c2max ) n−1 + ε3 · k=1 c4k→k+1 (i, j)/c4max

(12)

where ε1 , ε2 and ε3 are the cost coefficients, which can be determined according to different priorities in the nuclear emergency case.

686

Y. Huang et al. y

N dy

dx

G x

O

Fig. 4. Schematic diagram of the estimated cost

4.2 Calculation of Estimated Cost The estimated cost can be regarded as a function of the average cost from the current node to the target node and the shortest path [16]. As shown in Fig. 4, dx and dy represent the horizontal and vertical distance between the current node and the target node. The shortest path lmin and the minimum number of moves Nmin are given by Eq. (13) and (14). lmin = dx + dy +

√

 2 − 2 ∗ min(dx, dy)

Nmin = max(dx, dy)

(13) (14)

The average radiation dose R and the average cost of transport c4 in the blue area are given by Eq. (15) and (16). yG  1 R= dx · dy

xG 

c2 (i, j)

(15)

j=yN +1 i=xN +1

yG 4   1 c4 = 4dx · dy

xG 

c4 (i, j)k

(16)

k=1 j=yN +1 i=xN +1

The estimated cost from the current node to the target node is shown as follows. H (n) = ε1 · lmin /1.4 yG lmin + ε2 · dx·dy + ε3 ·

xG

c2 (i, j)/(1.4 ∗ c2max ) j=yN +1 i=xN +1 yG xG 4 Nmin c4 (i, j)k /c4max 4dx·dy k=1 j=yN +1 i=xN +1

(17)

5 Experimental Verification In order to evaluate the proposed path planning method, experiments are carried out in the simulation environment, and it is shown that the proposed method can plan a safe and effective path in the radiation environment with uneven ground.

Path Planning for Nuclear Emergency Robot

687

5.1 Processing of the Costmap First, we build an 8 × 10 m2 simulation environment in gazebo, as shown in Fig. 5(a). There is a 2 × 3 m2 platform in the scene, with a slope of 15° on the left and a slope of 30° on the upper side.A sixteen-line lidar is used to build the point cloud map, as shown in Fig. 5(b). Select the maximum height of 2 m and the grid size of 0.2 × 0.2 m2 . The point cloud is projected into a grayscale image, as shown in Fig. 5(c). The closing operation of the grayscale image is shown in Fig. 5(d). It shows that the processing method can effectively fill the grid with missing height information and is conducive to the following path planning.

Fig. 5. The acquisition process of the grayscale image. (a) Simulation environment. (b) Point cloud map. (c) The grayscale image projected from the point cloud. (d) The grayscale image after closing operation.

After the point cloud projection is completed, the Sobel operator is used to convolute the image. The gradients are obtained, as shown in Fig. 6. From the results, it can be found that the image gradient value is higher at the wall and steps area, and the gradient values of the 30◦ slope are greater than that of the 15◦ slope. After obtaining the gradient image, the inaccessible map and radiation warning area are calculated by combining the radiation field. Two radioactive point sources are set in the scene. According to the radiation resistance of most robots, the radiation dose threshold is 5 µSv/s. The gradient image is generated, and the threshold is set to be 60. The inaccessible map is superimposed with the radiation warning area to obtain the multi-layer costmap, as shown in Fig. 7. The map accurately marks inaccessible radiation sources, impassable steps and steep slopes, which can be used for subsequent path planning algorithm. 5.2 Path Planning Select the starting point of the path as the upper left corner and the ending point of the path as the platform. Assuming that the walking speed of the robot is 1.5 m/s, different paths can be obtained by modifying the cost coefficient. Figure 8 shows the effect of four path schemes, and Table 1 shows the statistical results of the corresponding path schemes. Path 1 is planning by the traditional A* algorithm, which can get the shortest path. Because the radiation factor is completely ignored, the maximum radiation dose is very close to the threshold, and the cumulative radiation dose is also very large.

688

Y. Huang et al.

Fig. 6. The results of image gradient calculation. (a) 0◦ . (b) 45◦ . (c) 90◦ . (d) 135◦ .

Fig. 7. The multi-layer costmap. (a) Costmap of radiation. (b) Costmap of traversability.

Fig. 8. Path planning results with different coefficients.

Path 2 only considers the impact of radiation. This method can be regarded as the improvement of Liu’s method [6]. It can be seen that in order to avoid radiation, the path with longer distance is selected. But in the actual application environment, the radiation decays rapidly. Therefore, in the range of radiation dose that the robot can bear, the time needed to execute the path should also be considered.

Path Planning for Nuclear Emergency Robot

689

Table 1. Statistical results of different paths Distance (m)

Path

Coefficients (ε1 , ε2 , ε3 )

Time (s)

Maximum rad. dose (μSv/s)

Cumulative rad. dose (μSv)

Cumulative uphill angle (◦ )

1

(1, 0, 0)

8.471

5.647

3.163

4.025

125.545

2

(0, 1, 0)

15.457

10.305

0.272

1.005

119.085

3

(0, 0, 1)

16.948

11.299

3.163

3.890

91.842

4

(0.2, 0.4, 0.4)

10.043

6.695

0.401

1.522

107.289

Path 3 only considers the transport cost. Because the point cloud map is obtained by SLAM rather than ideal, the gradient values calculated by the same slope are slightly different. When only considering the transport cost, the difference will become an absolute factor in path planning. However, it can be found that the path tends to be diagonal, which can effectively reduce the slope. Path 4 takes three factors into account at the same time. Compared with Path 1, Path 4 bypasses the radiation source better, and the cumulative radiation dose decreases by 62.2%. Compared with Path 2, the distance is reduced by 35.6%, while the cumulative radiation dose is only increased to 1.522 µSv. Compared with Path 3, Path 4 avoids the repeated diagonal path, so it reduces both the distance and the cumulative radiation dose. These demonstrate that Path 4 has a better comprehensive effect. The advantage of the proposed path planning algorithm is that it can obtain appropriate path planning results by modifying the coefficient according to the specific environment and task. When the radiation dose in the environment is very low or the task is urgent, ε1 should be increased appropriately to improve the moving efficiency, otherwise ε2 should be increased to reduce the cumulative radiation dose as much as possible. When the physical environment is complex with obstacles and slopes,ε3 should be increased to avoid climbing and save energy.

6 Conclusion and Future Work In this paper, we propose a path planning algorithm based on multi-layer costmap in radiation environment with uneven terrain. At the beginning, we propose a method to obtain costmap for path planning from radiation field distribution map and point cloud map. Secondly, we propose a new cost function of A* algorithm, which considers the cost of distance, radiation, and slope. Finally, through the simulation experiment, different paths are planned in the simulation environment, and the statistical results are compared. The results show that the path planning method is technically feasible and has made preliminary progress. According to the different environments and tasks, we can modify the cost coefficients to search for the best path. However, in real cases, the environment will be more complex and variable, and autonomous navigation also involves robot positioning, kinematic models, and other issues. Effective and accurate path planning for the nuclear emergency robot is still a challenging problem. These topics will be considered in future research.

690

Y. Huang et al.

Acknowledgement. This work is jointly supported by National Natural Science Foundation of China (U1813224) and National Key R&D program of China (2019YFB1310801).

References 1. Zhang, Q., et al.: Research progress of nuclear emergency response robot. In: IOP Conference Series: Materials Science and Engineering, vol. 452, no. 4, p. 042102 (2018) 2. Chang, F., et al.: Research status and key technologies analysis of operating robots for nuclear environment. Opto Electron. Eng. 47(10), 200338 (2020) 3. Mascarich, F., Papachristos, C., Wilson, T., Alexis, K.: Distributed radiation field estimation and informative path planning for nuclear environment characterization. In: 2019 International Conference on Robotics and Automation (ICRA), pp. 2318–2324 (2019) 4. Khasawneh, M.A., Aman, Z., Al-Shboul, M., Aradat, M.A.: A localized navigation algorithm for radiation evasion for nuclear facilities: optimizing the “Radiation Evasion” criterion: Part I. Nucl. Eng. Des. 259, 240–257 (2013) 5. Liu, Y.K., Li, M.K., Xie, C.L., Peng, M.J., Xie, F.: Path-planning research in radioactive environment based on particle swarm algorithm. Prog. Nucl. Energy 74, 184–192 (2014) 6. Liu, Y.K., et al.: Minimum dose method for walking-path planning of nuclear facilities. Ann. Nucl. Energy 83, 161–171 (2015) 7. Tan, K., Li, M., Gu, H., Yang, M.: A radiation avoiding algorithm of path optimization for radiation protection of workers and robots. Ann. Nucl. Energy 135, 106968 (2020) 8. Yang, L., Qi, J., Song, D., Xiao, J., Han, J., Xia, Y.: Survey of robot 3D path planning algorithms. J. Control Sci. Eng. 2016, 1–22 (2016) 9. Papadakis, P.: Terrain traversability analysis methods for unmanned ground vehicles: a survey. Eng. Appl. Artif. Intell. 26(4), 1373–1385 (2013) 10. Hirose, N., Sadeghian, A., Vázquez, M., Goebel, P., Savarese, S.: Gonet: a semi-supervised deep learning approach for traversability estimation. In: 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3044–3051 (2018) 11. Schilling, F., Chen, X., Folkesson, J., Jensfelt, P.: Geometric and visual terrain classification for autonomous mobile navigation. In: 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 2678–2684 (2017) 12. Tang, Y., Cai, J., Chen, M., Yan, X., Xie, Y.: An autonomous exploration algorithm using environment-robot interacted traversability analysis. In: 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4885–4890 (2019) 13. Wang, C., et al.: Autonomous mobile robot navigation in uneven and unstructured indoor environments. In: 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 109–116 (2017) 14. Wermelinger, M., Fankhauser, P., Diethelm, R., Krüsi, P., Siegwart, R., Hutter, M.: Navigation planning for legged robots in challenging terrain. In: 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1184–1189 (2016) 15. Lu, D.V., Hershberger, D., Smart, W.D.: Layered costmaps for context-sensitive navigation. In: 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 709–715 (2014) 16. Tao, L., Long, P., Zheng, X., Yang, Z., Shang, L., He, T.: An improved A* algorithm-guided path-planning method for radioactive environment. J. Radiation Res. Radiation Process. 36(6), 60601–060601 (2018)

A Multi-sensing Input and Multi-constraint Reward Mechanism Based Deep Reinforcement Learning Method for Self-driving Policy Learning Zhongli Wang1,2(B) , Hao Wang1 , Xin Cui3 , and Chaochao Zheng3 1 School of Electronic Information Engineering, Beijing Jiaotong University, Beijing, China

[email protected]

2 Beijing Engineering Research Center of EMC and GNSS Technology for Rail Transportation,

Beijing 100044, China 3 China Railway Electrification Bureau Group Co., Ltd., Beijing 100036, China

Abstract. Planning and decision-making of autonomous driving is an active and challenging topic currently. Deep reinforcement learning-based approaches seek to solve the problem in an end-to-end manner, but generally require a large amount of sample data and confronted with high dimensionality of input data and complex models, which lead to slow convergence and cannot learn effectively with noisy data. Most of deep reinforcement learning-based approaches use a sample reward function. Due to the complicated and volatile traffic scenarios, these approaches cannot satisfy the driving policy requirement. To address the issues, a multi-sensing and multi-constraint reward function (MSMC-SAC) based deep reinforcement learning method is proposed. The inputs of the proposed method include front-view image, point cloud from LiDAR, as well as the bird’s-eye view generated from the perception results. The multi-sensing input is first passed to an encoding network to obtain the representation in latent space and then forward to a SAC-based learning module. A multiple rewards function considering various constraints, such as the error of transverse-longitudinal distance and heading angle, smoothness, velocity, and the possibility of collision, is designed. The performance of the proposed method in different typical traffic scenarios is validated with CARLA [1]. The effects of multiple reward functions are compared. The simulation results show that the presented approach can learn the driving policies in many complex scenarios, such as straight ahead, passing the intersections, and making turning, and outperforms against the existing typical deep reinforcement learning methods. Keywords: Deep reinforcement learning · Driving policy · Multi-reward functions · CARLA

1 Introduction Decision-making is the core technologies of autonomous driving. The design of robust and adaptive driving policy for complex urban traffic conditions is a challenging task © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 691–701, 2021. https://doi.org/10.1007/978-3-030-89092-6_63

692

Z. Wang et al.

[2]. Currently, researches on driving policies for autonomous driving can be broadly classified into two categories: rule–based and machine learning-based. Rule-based approach is generally based on driver’s experience [3]. But the traffic scenarios are complex and full of uncertainties, it is difficult for this method to adapt to different traffic scenarios [4]. Recently, reinforcement learning algorithms have also been used to solve the problem of driving policy for unmanned vehicles. For example, DQN proposed by Mnih [5] is a typical value-based method. Lange et al. [6] used a deep encoder to learn potential features from the input raw images and successfully learned the driving policy such as steering, acceleration and braking. Yu et al.[7] used a DQN algorithm for automatic vehicle control with different reward functions to generate specific driving behaviours and implemented a vehicle turning task in a simulated environment. Sallab et al. [8] proposed a deep deterministic Actor-Critic algorithm for lane keeping assistance, and the algorithm outputs smooth continuous action trajectories in Torcs car simulation. Kendall et al. [9] implemented a DDPG method on a real self-driving vehicle, which was trained by a driver simulation for lane keeping tasks. These methods are less effective in complex scenarios and the training performance with high-dimensional data is poor. Some newly DRL methods, such as SAC [10], which have strong exploration capabilities and efficient convergence, but seldom used for end-to-end autonomous driving tasks. In this paper, based on SAC framework, we address two issues for autonomous driving policy learning. The first issue is about the sensing input. RGB image can be used to detect road information, but it is vulnerable to ambient light. LiDAR can get the 3D position and size of the target object, which is not affected by ambient light. Bird’s-eye view (BEV) proposed by Bansal [11] contains information such as path information and surrounding environment, which is used as the main input learning data in some methods. All three kind of information are combined, which can provide more comprehensive environmental information for DRL module and help to obtain a more reliable driving policy. However, due to the existence of some irrelevant information in the multi-source input information, such as lighting and environmental texture information, potential features are extracted with an encoder network, which reduces the complexity of the input information without losing the valid information. The second issue is the reward function design, which is a core technique in reinforcement learning (RL)-based method. To improve the generality of the SAC method, a model-free RL algorithm based on multiconstraint reward functions is designed and discussed. Simulation results show that the proposed approach can generate driving policies for different traffic scenarios (lane keeping, passing intersections, turning, etc.), which greatly improved the adaptivity, and outperformed against the existing RL-based methods.

2 MSMC-SAC Method The system block diagram of our method is shown in Fig. 1, composed of the CARLAbased simulator and the autonomous driving control agent. The inputs include 2D projected point cloud from LiDAR, RGB image and BEV. The multi-sensing inputs are first extracted using a VAE encoder [12] to extract latent feature representations, then the latent feature is fed to a SAC-based learning network. The action output is sent to the simulator. The key details are introduced in the following parts.

A Multi-sensing Input and Multi-constraint Reward Mechanism

693

Fig. 1. System framework diagram

2.1 Bird’s-Eye View and Coding Network The input information consists of three main components: image, LiDAR point cloud and BEV. In the CARLA simulation, the controlled vehicle is equipped with camera and LiDAR sensors. Using the information from each sensor set on the vehicle, the position, direction, speed and lane related features of the surrounding objects, as well as the longitudinal and lateral distances to specific lane are obtained; the reference path information is a series of trajectory points, which together form BEV, and aligned with the controlled vehicle view. The BEV mainly consists of four parts: 1) Map, contains the information of road geometry; 2) Reference route, obtained from the route planning module of the simulation environment; 3) Other vehicles in the environment; 4) Vehicle attitude. Due to the source data contains many irrelevant information, the complex inputs may cause over-fitting during the training.To reduce the complexity of the input, the raw data and the BEV image are encoded by a variational auto-encoder (VAE). The VAE encoder projects a high-dimensional data into a low-dimensional space z, this ensures the compressed representation captures enough information for the RL algorithm training. VAE generally consists of two parts: the encoder network qϕ (z|x) and the decoder network Pθ (x|z), respectively. In order to make two networks approximate gradually, the network parameters φ,θ are obtained by maximizing the scatter between Pθ (x|z) and qϕ (z|x), with the following loss function: L(φ, θ, x(i) ) = DKL (qφ (z|x(i) )||pθ (z)) − Eqφ (z|x(i) ) (log pθ (z|x(i) ))

(1)

DKL is the Kullback-Leibler (KL) scatter. The encoded low-dimensional latent variables retain the key information of the original input data.

694

Z. Wang et al.

2.2 SAC-Based Learning Model SAC-based method relies on maximizing entropy for reinforcement learning, and the output policy considers both reward and entropy terms. The SAC-based network is chosen to balance stability, sample efficiency, and exploration ability while still successfully complete the given task. SAC has critic network Qθ (s, a) and the actor network πφ (|s). The Actor network uses the state data as input and outputs the policy distribution. The output of the driving policy is obtained by sampling this policy distribution. The objective function of the Q function minimizes the residuals by MSE: JQ (θ ) = E(st ,at )∼D [ 21 (Qθ (st , at )−r((st , at ) + γ [Qθ (st+1 , at+1 )−α log(πφ (at+1 |st+1 ))]))2 ]

(2)

Where the states s are sampled from the sample experience replay pool D. The Experience replay D is used to break the correlation of the training data and ensure the output effect, and the actions are calculated based on the current policy, and the objective function of the policy network is defined as: Jπ (φ) = Est ∼D,at ∼π(φ) [log πφ (at |st ) − Qθ (st , at )]

(3)

For the driving policy network, different reward functions need to be designed for different tasks and driving scenarios. And different reward functions for the same scenario have a great impact on the convergence speed of training and the feasibility of the policy. 2.3 Multi-constraint Reward Functions Design The vehicle control actions here are defined to include steering and throttle, so the action space is defined as A = {δ, τ }. In the CARLA simulator, the ranges of steering angle δ and gas control τ are normalized to [−1, 1] and [−1, 1], and if τ > 0, it means acceleration, it can reduce the complexity of the output. In complex traffic scenarios, both the distance between the controlled vehicle and the other vehicles, and the distance between the controlled vehicle and road boundary determine the possibility of collision, the distance and angle error between the controlled vehicle and the road center curve determines the performance of lane keeping, the speed change of steering angle affects the smoothness of driving, etc. We combine these factors together and design a multiconstraint reward function to meet the driving tasks for different scenarios. Specifically, the multi-constraint reward function is defined as: r = k1 • rcol + k2 • rs + k3 • rf + k4 • rey + k5 • rsteer +k6 • rh +k7 • rlat + c

(4)

The coefficients k1 , k2 , k3 , k4 , k5 , k6 , k7 are set to 200,1,10,1,1,1,1 respectively. The collision constraint term is defined in Eq. (5), it is set to −1 when collision happens, and the coefficient k1 is set to 200, it is a negative penalty term compared to the other positive rewards. We find that if the collision penalty term is too small, the trained policy will not be able to avoid collision, and if it is set too large, a negative results make it’s difficult to learn an effective policy. rs is a reward term for the vehicle speed, which gives

A Multi-sensing Input and Multi-constraint Reward Mechanism

695

a positive reward for the longitudinal speed of the vehicle and is used for the control of the speed.rsteer is set to ensure the smoothness of the car speed during steering. rf is a speed-related constraint term during vehicle driving. In the simulation environment, the vehicle is assumed to run at a preset speed vt , for example, it is set to 8 m/s in the simulation, and a speed constraint penalty is applied to prevent overspeed. rf is set to −1 in case of overspeed and 0 in other cases. There is a constant term c in the multi-constraint reward function which is set to − 1, since it is found that the vehicle may stay stationary if this term is not included.   −1, if collision −1, if speed > vt rcol = ; rf = 0, else 0, else (5) 2 rs =rspeed_lon ; rsteer = − steering 2 ; rlat = −steer ∗ vlon

Fig. 2. Illustration of VFG vector field guidance

rey and rh stand for two constraint terms which are used to minimize the lateral trajectory error and the heading angle error. The definition of the desired heading angle is presented in Fig. 2, which is based on vector field guidance (VFG) [13]. Each cyan arrow in the vector field represents the desired heading angle where the vehicle is. Let

heading is the heading angle error between the real heading angle and the desired angle, the constraint function is defined in the form of discontinuous functions as: ⎧ ◦ −| heading| ⎪ | heading | < 90 ⎨ e ◦ ◦ , rey = e−|ey | (6) rh = −e−|180 − heading|

heading ≥ 90 ⎪ ◦ ◦ ⎩ −|180 + heading| −e

heading ≤ −90 The difference between the real and the desired heading angle is defined as the heading error. The lateral distance from the center of the vehicle to the reference trajectory is defined as the lateral trajectory error ey , the reference trajectory is the diagonal line in the middle of the figure. The constraint function is defined as rey . The function with the lateral trajectory error takes the largest value when the lateral trajectory error is zero, and the reward value obtained at this time is the largest. Nelson et al. [14] presented a Lyapunov stability proof of VFG-based navigation method. When ey is small, the desired direction is the same as the direction of the path. However, as ey increases, the difference between the path angle and the desired direction also tends to

696

Z. Wang et al.

increase. Figure 2 illustrates the path representation using a set of waypoints. According to this approach, the target path has a Euclidean distance by using the previous waypoint W k-1 to the current waypoint W k . By designing the multi-constraint reward function, the learned policy can satisfy multiple constrained control objectives and be adaptive to different traffic scenarios.

3 Experimental Evaluation In order to verify the performance of the proposed method, different driving scenarios were built with CARLA. The hardware of the computer used for simulation is CPU: i7-8700, GPU: GTX1060 (6G video memory) and 8 G RAM. The tests were conducted on Town3 map provided by CARLA as shown in Fig. 3.

Fig. 3. The map Town3 used for training and tests

Multiple vehicles are set in random positions to simulate complex traffic scenes and drive freely in the virtual town. A virtual front camera provides the RGB image. The point cloud data acquired by LiDAR is projected onto the ground plane and rendered as a 2D LiDAR image. Each pixel in the rendered image is either red or green. The target detection results and the road information, together with the reference path which is shown in cyan color, form the BEV image, and all three kinds of data are updated in real time while the vehicle is running. In the experiment, the learning network outputs an average value every 5000 training iterations, which forms a point in the curve and used to evaluate the algorithm. If a vehicle collides with the boundary or deviates from its lane, the training will be start again. After every 5000 steps, all vehicles will continue the training process at a random initial position selected in the map, the return curve of each algorithm are trained five times. All the training and testing are performed unsupervised. Two main input scenarios are compared in the simulation experiments: Both the sensor data and BEV image are used simultaneously as input (BEV image is generated from the sensor input); Only sensor data used, no BEV image is provided. 3.1 Network Structure and Parameter Settings 1) RGB image, LiDAR data, BEV image, VAE model processing layer are composed of four 3 * 3 convolutional network of 32, 64, 128 and 256 respectively, finally a

A Multi-sensing Input and Multi-constraint Reward Mechanism

697

potential space layer of size 64 is fully connected to the final convolutional layer. The comparison between Fig. 4a) b) from left to right, front view image, radar image, bird’s eye view) shows that the original input image and Fig. 4c) d) reconstruct image, we only use this module for decision-making. All methods use the same networks.

Fig. 4. Raw input image (left) and reconstruct image (right) under different traffic scenarios

2) The value and policy network of SAC are designed with the same structure to simplify works. It has five fully connected layers, where the hidden layer nodes range from 256, 128, 64, and 32 respectively. At the last layer of policy network, it is divided into two branches to output the mean and variance. All networks are trained using the Adam optimizer with a learning rate of 10−4 , a reward function discount factor of 0.005, a batch size set to 32, and a buffer size of 1000. 3) A frame skipping technique is used for accelerating the training process. Some parameter settings for the compared methods are as follows: for the DQN method, the parameter epsilon greedy is set to 0.1 and the Q learning rate is set to 0.001; For the DDPG method, the exploration variable noise is set with a variance of 0.2 and a mean of 0; For the TD3 method, the noise variance is set to 0.1. 3.2 Simulation Using Only the Raw Sensor Data As illustrated in Fig. 5, the methods for comparison include DDPG, TD3, and DQN, all of them are SOTA reinforcement learning methods. It presents the change of the average reward per 5000 steps with the number of environmental iterations. MSMC here refers to the multiple source multi-constraints. MSMC-SAC is the method proposed in this paper. Compared with several existing reinforcement learning methods, if only the raw sensor data were used, other methods obtain less average rewards, while appearing larger fluctuations, the correspondent mean value and standard variance are listed in Table 1. 3.3 Simulation with Multi-sensing Input As shown in Fig. 6, different with the previous simulation, we use all sensor data and generated BEV image. It is obvious that the proposed MSMC_SAC method outperforms

698

Z. Wang et al. Table 1. Mean variances using raw data only

Algorithm

Reward-mean

MSMC-SAC

420.6

MSMC-DQN

62.51

Reward-std 92.3 67.2

MSMC-DDPG

193.6

147.2

MSMC-TD3

360.8

127.0

Fig. 5. Individual algorithm rewards using only raw

Fig. 6. Algorithm return curve with raw data and bird’s eye view inputs 0

against three other SOTA methods, and can reach higher reward values and faster than other methods, and with relatively flat and stable curves. The results of means and standard variances are listed in Table 2, it can be seen that the MSMC-SAC algorithm has the highest average reward and the lower standard deviation. We found that, by combining the BEV image with the original sensor data inputs, it can not only complete a multi-objective task, but also can get higher cumulative reward. During the simulation of the DQN algorithm, the algorithm obtained low reward and the average reward curve was unstable and fluctuated, the vehicle rushes out of the lane and collided with the middle barrier as shown in Fig. 7. Similarly, the algorithm DDPG

A Multi-sensing Input and Multi-constraint Reward Mechanism

699

Table 2. Mean variance using raw data only Algorithm

Reward-mean

Reward-std

MSMC-SAC

482.5

135.3

MSMC-DQN

281.9

153.3

MSMC-DDPG

162.9

137.2

MSMC-TD3

338.4

124.4

Fig. 7. DQN simulation results

Fig. 8. DDPG simulation results

Fig. 9. TD3 simulation results

has the same problem, and collided with the lane as shown in Fig. 8, it failed to generate an effective driving policy. As shown in Fig. 9, we found that with the TD3 algorithm, the vehicle sometimes keep stationary and was unable to complete the driving task. It happens because the initial departure point is set on the reference path and both the tracking and heading error are 0. The simulation results of the presented method with typical traffic scenarios are shown in Fig. 10a), b), c), when passing the roundabout, the vehicle selects two different lanes respectively, and the vehicle has good passing ability with no collision. In Fig. 10d), the vehicle can pass the U-shaped curved area, and in Fig. 10e), when the reference path is straight, the vehicle completes the lane keeping task with a constant speed. It can be seen that the presented method can stably complete different driving tasks. Figure 10f) shows that the vehicle completes the stopping task after encountering the obstruction of the vehicle in front of it during the driving process. Some experiments to analyze in-depth of the impact of different constraint functions in the proposed method have been conducted. As seen in Fig. 11, after removing the lateral tracking error and the heading angle error, the corresponding reward values show a significant decrease, and the results are shown in Table 3. Removing the lateral tracking error term, we found that the vehicle deviates from the lane at a longer distance and the tracking error tends to expand. Removing the heading angle error term, the vehicle has a larger heading angle error when turning and going straight, and there is an expanding

700

Z. Wang et al.

Fig. 10. Results of the presented method under different scenarios

Fig. 11. Analysis of the impact of the constraint terms rey and rh

Table 3. Algorithm without rey or rh mean and std Algorithm

Reward-mean

Reward-std

MSMC-SAC

482.5

135.3

MSMC-SAC_no rey

288.1

113.1

MSMC-SAC_no rh

103.8

100.3

trend of the heading angle error. All these experiments proves that the proposed multiconstraints reward function is effective and can generate a reasonable driving policy.

4 Conclusion We propose to integrate three kind of information for the learning input, which can provide more comprehensive environmental information for the deep reinforcement learning module and help to obtain a more reliable driving policy. A model-free reinforcement

A Multi-sensing Input and Multi-constraint Reward Mechanism

701

learning algorithm based on multi-constraint reward function is designed to improve the generality of the SAC method for different traffic scenarios. Simulation results verified that the proposed approach can generate driving policies for different traffic scenarios and outperformed against the existing learning methods. Further work includes improving the efficiency of the algorithm and optimizing the training time, as well as conducting more experiments on different challenging traffic scenarios, and further improving the adaptability of the method to the traffic scenarios.

References 1. Dosovitskiy, A., Ros, G., Codevilla, F., et al.: CARLA: an open urban driving simulator. arXiv preprint arXiv:1711.03938 (2017) 2. Silver, D., Bagnell, J.A., Stentz, A.: Learning from demonstration for autonomous navigation in complex unstructured terrain. Int. J. Robot. Res. 29(12), 1565–1592 (2010) ˇ 3. Paden, B., Cáp, M., Yong, S.Z., et al.: A survey of motion planning and control techniques for self-driving urban vehicles. IEEE Trans. Intell. Vehicles 1(1), 33–55 (2016) 4. Ziegler, J., Bender, P., Schreiber, M., et al.: Making bertha drive-an autonomous journey on a historic route. IEEE Intell. Transp. Syst. Mag. 6(2), 8–20 (2014) 5. Mnih, V., Kavukcuoglu, K., Silver, D., et al.: Playing atari with deep reinforcement learning. arXiv preprint arXiv:1312.5602 (2013) 6. Lange, S., Riedmiller, M., Voigtländer, A.: Autonomous reinforcement learning on raw visual input data in a real world application. In: The 2012 international joint conference on neural networks (IJCNN), pp. 1–8. IEEE (2012) 7. Yu, A., Palefsky-Smith, R., Bedi, R.: Deep reinforcement learning for simulated autonomous vehicle control. In: Course Project Reports: Winter, pp. 1–7 (2016) 8. Sallab, A.E., Abdou, M., Perot, E., et al.: End-to-end deep reinforcement learning for lane keeping assist. arXiv preprint arXiv:1612.04340 (2016) 9. Kendall, A., Hawke, J., Janz, D., et al.: Learning to drive in a day. In: International Conference on Robotics and Automation (ICRA), pp. 8248–8254. IEEE (2019) 10. Haarnoja, T., Zhou, A., Abbeel, P., et al.: Soft actor-critic: off-policy maximum entropy deep reinforcement learning with a stochastic actor. In: International Conference on Machine Learning, pp. 1861–1870. PMLR (2018) 11. Bansal, M., Krizhevsky, A., Ogale, A.: Chauffeurnet: learning to drive by imitating the best and synthesizing the worst. arXiv preprint arXiv:1812.03079 (2018) 12. Kingma, D.P., Welling, M.: Auto-encoding variational bayes. arXiv preprint arXiv:1312.6114 (2013) 13. Woo, J., Yu, C., Kim, N.: Deep reinforcement learning-based controller for path following of an unmanned surface vehicle. Ocean Eng. 183, 155–166 (2019) 14. Nelson, D.R., Barber, D.B., McLain, T.W., et al.: Vector field path following for miniature air vehicles. IEEE Trans. Rob. 23(3), 519–529 (2007)

Intelligent Safety Decision-Making for Autonomous Vehicle in Highway Environment Zhenyu Jiang1 , Zhongli Wang1,2(B) , Xin Cui3 , and Chaochao Zheng3 1 School of Electronic Information Engineering, Beijing Jiaotong University, Beijing, China

[email protected]

2 Beijing Engineering Research Center of EMC and GNSS Technology for Rail Transportation,

Beijing 100044, China 3 China Railway Electrification Bureau Group Co, Ltd., Beijing 100036, China

Abstract. Safe driving policies is the key technology to realize the adaptive cruise control of autonomous vehicle in highway environment. In this paper, the reinforcement learning method is applied to autonomous driving’s decision-making. To solve the problem that present reinforcement learning methods are difficult to deal with the randomness and uncertainty in driving environment, a model-free method for analyzing the Lyapunov stability and H∞ performance is applied to Actor-Critic algorithm to improve the stability and robustness of reinforcement learning. The safety of taking an action is judged by setting a safety threshold, thus improving the safety of behavioral decisions. Our method also designs a set of reward functions to better meet the safety and efficiency of driving decisions in the highway environment. The results show that the method can provide safe driving strategies for driverless vehicles in both normal road conditions and environments with unexpected situations, enabling the vehicles to drive safely. Keywords: Decision-making · Deep reinforcement learning · Lyapunov function · Safety threshold · Reward function

1 Introduction Autonomous driving plays an important role in improving urban traffic and driving safety. The decision-making is a key part of the autonomous vehicle technology, which determines the driving policy of the autonomous vehicle. The highway driving environment is a common scenario for autonomous driving. However, many unexpected situations may occur in highway scenario, it makes the decision-making in this driving scenario still a challenging task. Currently, deep reinforcement learning (DRL) has been increasingly applied to decision-making due to its advantages in scene traversal depth and many methods have been proposed. Since the decision-making process of driving can be modeled as Markov process [1], many researchers used DRL algorithms to help autonomous vehicles deal with complex traffic environments, such as the PPO algorithm is presented to train the © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 702–713, 2021. https://doi.org/10.1007/978-3-030-89092-6_64

Intelligent Safety Decision-Making for Autonomous Vehicle

703

agent to change lanes and merge into turning lanes in dense traffic environments [2], or training agents directly in real vehicles on closed and limited roads [3]. However, these methods are poorly robust and their agents can only work well when the environment is close to the training situation. The uncertain interference is not considered or included in the training environment. If the agent have not been trained with similar events, the behavior layer will still output a decision without any warning, which may cause accidents in high-speed situations. Recently, the robustness of driving strategies has been emphasized by Carl-Johan [4], and the safety of decision has also received much attentions. To overcome aforementioned drawbacks, many researchers investigated how to improve the robustness of decision-making. For example, Osband et al. [5] analyzed the shortcomings of DRL methods on uncertainty assessment and proposed a DRL algorithm based on randomized prior functions, it was applied to lane-changing policy with good robustness and safety by Hoel et al. [6]. However, the method suffered from a same drawback of the DQN: it will explode in high dimensionality due to the oversized space. Han et al. [7] proposed a DRL algorithm with robustness and stability guarantees based on the actor-critic framework, by adding a robust Lyapunov stabilization controller to the network, and achieved robustness and optimal response strategies. Motivated by above works, we propose a robust Lyapunov based safety Actor-Critic algorithm (LBSAC) that can be applied to decision making.

2 Reinforcement Learning Algorithms with Robustness

Actor

Policy

TD-error Critic

Lyapunov based network

Action

Reward

Saftey threshold

Conservative policy

Uncer -tain state

Environment

Fig. 1. Block diagram of behavioral decision making method based on DRL.

The block diagram of the proposed method is shown in Fig. 1. We apply the Lyapunov stability and H∞ control ideas to provide policies with stability and robustness in RL. A safety threshold is also designed to ensure the policy safety. 2.1 Objective Function Reinforcement learning focuses on learning the predictive relationships from states to behaviors by training an agent to interact with the environment. Sutton et al. [8] proposed

704

Z. Jiang et al.

the Actor-Critic framework and recently has been applied in some newly DRL algorithm, such as Deep Deterministic Policy Gradient(DDPG) [9] and Advantage Actor-Critic (A2C) [10], both methods have been widely used in decision-making and achieved good performance [11]. In the Actor-Critic framework, Critic is used to parameterize and estimate the state behavior value function, Qω (s, a) ≈ Qπθ (s, a). Actor guides the policy parameter θ to be updated according to the value function obtained by Critic. The objective function in this policy environment can be expressed in the form of expected value Eπθ :   J (πθ ) = Eπθ μ(s)∇θ log πθ ( a|s)Qπ (s, a) (1) μ(s) is the state distribution under the behavioral strategy and πθ denotes the parameterized function of policy. The gradient based on θ yields its simplified form:   ∇θ J (πθ ) = Eπθ ∇θ log πθ ( a|s)Qω (s, a) (2)

2.2 H∞ Model-Free Reinforcement Learning with Robustness Since previous AC algorithms ignored many uncertainties and over-parameterized the policy during the training process, to analyze the stability of the system, in this paper, we introduce a Lyapunov function-based H∞ controller to find a robust, stable strategy. With a fixed strategy, the cost function of the Markov decision process can be defined . as cπ (s) = Ea∼π c(s, a), which is the difference between the actual state and the reference signal. In robust control against environmental uncertainty, a system is considered to be mean-square stable if it satisfies the following equation: ∞ 

Est cπ (st ) ≤

t=0

∞ 

Est η2 ω(st )2

(3)

t=0

where η ∈ R+ , ω(st ) is the uncertainty, which consists of environmental disturbances and modeling error, and ω ∈ l2 [0, + ∞].The system is mean-square stable when the cost function of the system is no more than its l2 gain. In this case, the system is guaranteed to be robust in the face of the worst-case scenario caused by ω. The Lyapunov-based stability control, which based on the above theory, uses Lyapunov function and sampleddata to analyze the stability and robustness of the closed-loop system, and a new global mean-square stability is introduced as follows. Lemma 1. If there exists a continuous differentiable function L : S → R+ and positive constants α1 , α2 , α3 , η, k1 , k2 such that: α1 cπ (s) ≤ L(s) ≤ α2 cπ (s) Eβ(s) (Es ∼Pπ L(s ) − L(s)) < Eβ(s) [ηω(s) − (α3 + 1)cπ (s)] 



It holds for all ρ|Es0 ∼ρ cπ (s0 ) ≤ k1 and { ω|ω ≤ k2 }.

(4)

Intelligent Safety Decision-Making for Autonomous Vehicle

705

The sampling distribution is defined as 1 N P(st = s|ρ, π, t). t=0 N →∞ N

βπ (s)  lim

Here ρ is the initial state distribution. Then the system is mean square stable and has l2 gain no greater than η/(α3 + 1). If formula (4) holds for ∀k1 , k2 ∈ R+ , then the system is globally mean square stable with finite l2 gain [7]. The Actor-Critic algorithm can be improved on the critic network according to these theories. In this paper, we use the Lyapunov function Lc to provide the policy gradient, which satisfies L(s) = Ea∼π Lc (s, a). According to the Lagrangian approach, the objective function under the corresponding state of the strategy can be obtained by combining formula (2) into: J (π ) = E(s,a,ω,c,s ) [β log(π(fθπ (ε, s )|s))Qω (s, a) + λL(s, a, ω, c, s )]

(5)

where π is parameterized by the neural network fθπ , ε is the input vector consisted of Gaussian noise,λ is the positive Lagrangian multiplier and L is expressed as follows: L(s, a, ω, c, s ) = (Lc (s , fθπ (ε, s )) − Lc (s, a) + (α3 + 1) c − η2 ω 2

(6)

Similarly, the gradient (3) becomes: ∇θ J (π ) = E(s,a,ω,c,s ) [(∇θ υ log(πθ (a|s)) + ∇a υ log(πθ (a|s))∇θ fθπ (ε, s)) ∗ Qω (s, a) + λ∇a Lc (s , a )∇θ fθ (ε, s )]

(7)

where υ and λ are positive Lagrangian multipliers, these values are automatically adjusted. The Lyapunov function can be updated by minimizing the following objective function:   N  1 2 J (Lc ) = E(s,a) (Lc (s, a) − c(st , at )) (8) 2 t=t 0

where

N t=t0

c(st , at ) is as the Lyapunov candidate.

2.3 Safety Threshold Uncertainty information can be used to guide the training process. By providing the agent with uncertain environments during the training process, it could improve the sample efficiency and broaden the distribution of situations that the agent can handle. It is a challenge how to design a “safety layer” in learning-based approach to ensure the safety, and it should be considered in autonomous driving [12]. In an uncertain environment, a confidence measure can be set to determine safety [4]. The uncertainty of the environment in this algorithm can be represented as ω(st ), in a determined state is ω(s, a) . The safety

706

Z. Jiang et al.

min confidence measure is defined as 1 − ω(s,a)−ω c(s,a)−ωmin , ωmin is the minimum uncertainty. If the equation is equal to 1, it indicates the maximum confidence, if the equation is less than zero then the condition is considered to be unsafe. The cost value c(s, a) is chosen according to the current state of policy objective function. If the safety confidence level is not satisfied, the policy will not be selected, and an alternate conservative action will be selected (slowing down in the original lane) to avoid entering a greater state of uncertainty. That’s Lyapunov based safety Actor-Critic algorithm.

3 Implementation and Simulation This section describes how our method is implemented. 3.1 Simulation Setup We choose SUMO (Simulation of Urban Mobility) to conduct experiments. SUMO environment provides information about the speed and acceleration of the surrounding vehicles and the ego-car, as well as the distance to the surrounding vehicles. The environment is a bird’s eye view. The simulation environment is a one-way, three-lane highway, as shown in Fig. 2.

Fig. 2. An example of the highway scenario. The ego-car is in green, while the color of the surrounding cars indicates their speed value. Yellow represents 17 m/s, red represents 33 m/s, the darker the color is, the faster speed the vehicle has. (Color figure online)

At the beginning of training episode, 25 vehicles are randomly generated, including trailer trucks and cars with the same specifications as the ego-car, and the speed is randomly generated between [17 m/s,33 m/s] (highway speed limitation), it makes the driving environment more complex and random. Each time step of the simulator is set to s. Each episode of the training vehicle walks N = 150 time steps. The training stop and start from a new episode when a collision or departure of the ego-car is happened. To verify its effectiveness in driving, SUMO’s own lane change and following models are used to control vehicle’s movements. The following model consists of an adaptive longitudinal action acceleration and deceleration controller [13], and a Tactical lanechanging model is used as in reference[14], which allows the vehicle can overtake the slower ones. 3.2 State Space and Action Space State space S consists of the following sets:

   S = (xi , yi ), vx,i , vy,i i∈0,1,....N

vehicle

, ve , ae , pe∗

(9)

Intelligent Safety Decision-Making for Autonomous Vehicle

707

i represents the index of the vehicle represents the index of the vehicle, i = 0 means ego-car. (xi , yi ) represents the position of each vehicle, vx,i , vy,i represents the speed information of the vehicle. ve , ae are ego-car’s velocity and acceleration, pe∗ is the position of the target point where the agent wants to reach. The action space A consists of a series of longitudinal and lateral actions:{keep lane, turn left, turn right, accelerate, decelerate}. Agent can choose one of these actions at each time step. 3.3 Reward Model Two aspects should be considered in reward function: safety and efficiency. In terms of safety, a return value of −10 is given when collision occurs. In addition, a penalty term for near-collision is introduced to learn a safer motivation for lane-change. As seen in Fig. 3, if the relative distance D between ego-car and the surrounding vehicles is less than a predefined threshold gap, which indicates a possible collision, the intelligent vehicle can learn to abort lane-change. This design allows the agent to. The reward of this near-collision penalty rnear_collision is expressed as follows: ⎧ ⎨ rkeep = P(Ce , C3 ) rnear_collision = rturn = min(P(Ce , C3 ), P(Ce , C2 )) ⎩ (10) rabandon_turn = min(P(Ce , C1 ), P(Ce , C0 ))   P(Ce , Ci ) = −1/(py_e − py_i  + 0.1) In above equation, py_e and py_i denote longitudinal position of ego-car and surrounding vehicles. The whole collision return is defined as:  −10 if collosion rcollision = (11) rnear_collision if D < gap Considering that sudden acceleration and deceleration do not meet the comfort and safety requirements, the reward for acceleration is: ra = −α · ax (t)2 − β ·y (t)2

(12)

ax (t) and ay (t) are lateral and longitudinal acceleration of ego-car. α and β represent the weights, which are set to 0.4 and 0.2 here. Then the safety reward function is defined as: Rsafe = rcollision + ra

(13)

In terms of driving efficiency, vehicles are given a bonus at each time step as an incentive to overtake slower vehicles: vmax − v0 rv = 1 − (14) vmax In order to encourage ego-car to change from the lane where vehicles are moving slower, an incentive is designed here:     (15) rlane = −δ px − px∗  − σ vy − vdesired 

708

Z. Jiang et al. Target lane follower C2 Target lane leader C3 Current lane follower C0

Ego-car Ce

Current lane leader C1

Fig. 3. Lane change scenario.

where px is ego-car’s the current lateral position, px∗ is target position’s lateral position of the, vy represents the speed of ego-car, vdesired is the desired speed, δ and σ are weights, which are set to 0.1 and 0.2. The reward function of driving efficiency is: Refficient = rlane + rv

(16)

The total reward function is the sum of the safety and driving efficiency: R = Rsafe + Refficient

(17)

3.4 Neural Network Architecture Neural networks are used to estimate the prior probability of taking different actions from the action space A and the value of the current state in state space S. The state S is transformed into a normalized state vector ξ before being passed to the network, and all state values ξ∗ ∈ [−1, 1]. The position and velocity of the surrounding vehicles are represented relative to the ego-car, and the vector is shown in Table 1. Table 1. Input ξ to the neural network. Ego lane

ξ1 = 2y0 /ymax − 1

Ego vehicle speed

ξ2 = 2vx,0 /vmax − 1

Lane change state

ξ3 = sgn(vy,0 )

Relative long. position of vehicle i

ξ4i+1 = (xi − x0 )/xsensor

Relative lat. position of vehicle i

ξ4i+2 = (yi − y0 )/ymax

Relative speed of vehicle i

x,0 x,i ξ4i+3 = vmax −vmin

Lane change state of vehicle i

ξ4i+3 = sgn(vy , i)

ego

v −v

The network structure of the proposed algorithm is shown in Fig. 4. We use a 1D convolutional neural network (CNN), which extracts the characteristic states of the surrounding environment vehicles as the input part through CNN layers and pooling layers. The output of the network is independent of the order of the surrounding vehicles in the input vector, and the architecture allows a varying input vector size. Table 2 shows the

Intelligent Safety Decision-Making for Autonomous Vehicle

709

Output action

Policy Q Value

Merge

TDerror

critic

Lyapunov based network

Conv1 D

Max pool

Surrounding vehicles

64 filters size 2*1 Stride 2

Policy network

Ego state

actor

Fig. 4. Network structure diagram of LBSAC algorithm.

configuration of some key hyperparameters in the simulation. Due to the complexity of the system, the hyperparameter values are given by the informal search. In this paper, A2C and DDPG are used as the baseline to compare with LBSAC. All three methods use the same neural network and hyperparameters for comparison. Table 2. Hyperparameter settings for the simulation. Experience adding probability

0.5

Discount factor

0.99

Learning start iteration

1000

Replay memory size

10000

Learning rate

0.0003

Batch size

64

Gradient steps

1

Initial exploration constant

1

Final exploration constant

0.05

Final exploration iteration

100000

4 Simulation Results The proposed method LBSAC and A2C, DDPG are used to train agents for decisionmaking in a highway simulation built in SUMO1.8. All agents are evaluated on 100 different test sets at each additional 1000 training samples. To verify the robustness of the algorithm in the face of uncertain environments, the robustness and safety of the policy given by the LBSAC algorithm is tested by two common unexpected situations on highways. These test episodes are randomly generated during the training.

710

Z. Jiang et al.

4.1 Training Scene Reproduction Figure 5 shows the mean reward curve per 100 episodes during the training process.

Fig. 5. Average cumulative episode reward of RL agents in the training process. The shaded areas show the standard deviation for 10 random seeds.

100,000 steps are trained for each algorithm here, and the specific performance of each algorithm can be visualized from this curve. It should be noted that the curve is fitted from the average cumulative reward curve obtained by training each algorithm 10 times in the training scenario. Collectively, the LBSAC algorithm obtains the highest cumulative reward within the set number of steps. It also converges the fastest, and has a low standard deviation. These criterions reflect that the algorithm is more stable and can provide a safe and efficient driving strategy in a high-speed environment. The A2C algorithm has a higher standard deviation and converge slowly. The DDPG algorithm has a lower standard deviation and obtains a reward Similar to A2C. These algorithms perform slightly worse than the LBSAC in terms of both reward value and standard deviation. Replicating the training scenario, it can be seen that the ego-car (green) is likely to encounter the situation shown in Fig. 6, where the vehicle ahead suddenly decelerates. Under normal circumstances the vehicle will slow down, but because of the large uncertainty, it is difficult for the agent trained based on the A2C and DDPG to handle safely. For the agent trained by LBSAC, when such vehicle (orange car) appears, a strategy will be adopted based on the current safety confidence judgment. At this time, the uncertainty does not meet the confidence requirement, so the agent adopts conservative action to ensure the safety of traveling, as shown in Fig. 6b. 4.2 Test Outside Training Distribution Lane Change Simulation Under Complex Traffic Scenario. Assuming that the vehicle ahead is moving slowly and the ego-car needs to switch to the adjacent lane, while there is a speeding car coming from the rear of the adjacent lane. This scenario of

Intelligent Safety Decision-Making for Autonomous Vehicle

711

a). The performance of baseline methods when sudden deceleration occurred.

b). The performance of LBSAC when sudden deceleration occurred Fig. 6. Performance comparison when encountering sudden deceleration.

a). Failed lane change based baseline methods in the presence of speeding vehicle interference.

b). Successful lane change operation by LBSAC

Fig. 7. Performance comparison of speeding vehicle interference.

speeding vehicle interference is common in the real condition, and ego-cars trained by different algorithms are placed in this scenario for testing, as shown in Fig. 7. The purple car is the speeding vehicle with speed much higher than the specified value, yellow cars are surrounding slow cars. Simulation of Breaking Down Case in High Way. In this case, the behavior decision based on the LBSAC can also be handled well in the face of this unknown unexpected situation, as shown in Fig. 8. The white one is the broken down car, there are many vehicles slowly driven (the yellow cars). It can be seen that the agents trained by other baseline methods have difficulty in handling this situation and will collide. The agent trained with LBSAC will slow down and wait for the right time to change lanes. The proposed LBSAC method outperforms against the baseline methods in terms of stability and meets the safety requirement. Under the same training conditions, the LBSAC can safely handle situations outside of the training distribution. The video of results is here: https://v.youku.com/v_show/id_XNTE0NjYwOTMyOA==.html.

712

Z. Jiang et al.

a). The performance of A2C-based method.

b). The performance of LBSAC-based method. Fig. 8. Performance comparison when encountering a broken down vehicle.

5 Conclusions To improve the stability and robustness of DRL-based decision-making method in highway environment, a model-free method for analyzing the Lyapunov stability and H∞ performance is applied to Actor-Critic algorithm. A safety threshold to judge the safety of taking an action is presented, which can improve the safety of behavioral decisions. A set of reward functions are designed to enhance the safety and efficiency of driving decisions. The simulation results show that when in facing of a large amount of randomness and uncertainties in a high-speed environment, the proposed LBSAC method shows better safety and robustness in its decision making. Future works also include extending behavioral capabilities so that the planning part can handle more complex situations.

References 1. Volodymyr, M., Koray, K., David, S., et al.: Human-level control through deep reinforcement learning. Nature 518(7540), 529–533 (2019) 2. Saxena, D.M., Bae, S., Nakhaei, A., et al.: Driving in dense traffic with model-free reinforcement learning. In: 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, pp. 5385–5392 (2020) 3. Kendall, A., et al.: Learning to drive in a day. In: IEEE International Conference on Robotics and Automation (ICRA). IEEE, pp. 8248–8254 (2019) 4. Hoel, C.-J., Wolff, K., Laine, L.: Tactical decision-making in autonomous driving by reinforcement learning with uncertainty estimation. arXiv:2004.10439 (2020) 5. Osband, I., Aslanides, J., Cassirer, A.: Randomized prior functions for deep reinforcement learning. In: Advances in Neural Information Processing Systems, vol. 31, pp. 8617–8629 (2018) 6. Hoel, C.J., Wolff, K., Laine, L.: Automated speed and lane change decision making using deep reinforcement learning. In: 2018 21st International Conference on Intelligent Transportation Systems (ITSC). IEEE, pp. 2148–2155 (2018) 7. Minghao, H., Yuan, T., Lixian, Z., Jun, W., Wei, P.: H∞ model-free reinforcement learning with robust stability guarantee. In: NeurIPS 2019 Workshop on Robot Learning: Control and Interaction in the Real World (2019)

Intelligent Safety Decision-Making for Autonomous Vehicle

713

8. Andrew, B., Richard, S., Charles, A.: Neuron like elements that can solve difficult learning control problems. IEEE Trans. Syst. Man Cybern. 13(5), 834–846 (1983) 9. Lillicrap T P , Hunt J J , Pritzel A , et al. Continuous control with deep reinforcement learning. Comput. Sci. (2015) 10. Volodymyr, M., Adrià, P.B., Mehdi, M., Alex, G., et al.: Asynchronous methods for deep reinforcement learning. In: International Conference on Machine Learning, pp. 1928–1937 (2016) 11. Chen, Y., Dong, C., Palanisamy, P., et al.: Attention-based hierarchical deep reinforcement learning for lane change behaviors in autonomous driving. In: 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE (2019) 12. Underwood, S., Bartz, D., Kade, A., Crawford, M.: Truck automation: testing and trusting the virtual driver. In: Meyer, G., Beiker, S. (eds.) Road Vehicle Automation 3. LNM, pp. 91–109. Springer, Cham (2016). https://doi.org/10.1007/978-3-319-40503-2_8 13. Krau, S., Wagner P , Gawron C . Metastable states in a microscopic model of traffic flow. Physical review. Stat. Phys. 5(1997), 5597–5602 (1997). Plasmas, fluids, and related interdisciplinary topics 14. Erdmann, J.: Lane-changing model in SUMO. In: SUMO 2014. Proceedings of the SUMO2014 Modeling Mobility with Open Data, vol. 24, pp. 77–88 (2014)

A “Look-Backward-and-Forward” Adaptation Strategy of NN Model Parameters for Prediction of Motion Trajectory Yisha Liu1,2 , Silu Chen1(B) , Yufan Zhu1 , Yu Du2 , Chi Zhang1 , Guilin Yang1 , and Chenguang Yang3 1

Ningbo Institute of Materials Technology and Engineering, Chinese Academy of Sciences, Ningbo 315201, China [email protected] 2 School of Electrical Engineering, Heibei University of Science and Technology, Shijiazhuang 050018, China 3 Bristol Robotics Lab, University of the West of England, Bristol BS16 1QY, UK

Abstract. Prediction of human motion trajectory is crucial for safe human-robot collaboration (HRC). The existing prediction method based on the adaptive neural network (NN) model couples the parameter estimation error with the priori estimation error of trajectory. The increments of the parameter vector over time steps is unavailable. This causes an inaccurate assessment of the motion trajectory mean-square estimation error (MSEE) and the associated estimated value, which is a potential threat to safe HRC. In this work, we seek a “look-backwardand-forward” approach. That is, the estimation error (EE) of the parameter vector at a certain time step ago is firstly calculated by reversely using the offline trained NN model. Later, the estimated parameter vector at more recent time steps are computed recursively till the present time step. By doing this, the coupling of the MSEE of the parameter vector with the MSEE of the trajectory is cut off. And the effect of EE of the parameter vector’s increments to the EE of the motion trajectory’s diminishes in the finite time steps. Thus, more accurate predictions of motion trajectory and associated MSEE are achieved, which is important for the upcoming robot controller design. The experimental results on predicting a 3-D motion trajectory show the practical appeal of the proposed method.

Keywords: Adaptation strategy

· NN · Prediction · Motion trajectory

Supported by National Key R&D Plan of China (2017YFB1301204), National Natural Science Foundation of China (51875554, 51705510), Zhejiang Key R&D Plan (2018C01086), Zhejiang Provincial Key Laboratory of Robotics and Intelligent Manufacturing Equipment Technology (2015E10011), Equipment R&D Fund (6140923010102), and Ningbo S&T Innovation Key Project (2018D10010). c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 714–724, 2021. https://doi.org/10.1007/978-3-030-89092-6_65

A “Look-Backward-and-Forward” Adaptation Strategy

1

715

Introduction

The industrial robots have been widely used in automated operations such as palletizing, sorting, processing and welding [16]. However, due to its bulky structure and high deployment cost, it is difficult to adapt to the production lines which are alternated frequently for product customization and diversification. The automation solution using human-robot collaboration (HRC) can reduce the deployment cost of the production lines, release the duty of workers, and meet the growing demand for flexible manufacturing [15]. To ensure the safety of humans and the efficiency of robots in the HRC environment, it is necessary to accurately predict the trajectory of human. The prediction of the human motion trajectory can be done by either fixed models or learning-based models. The fixed models [7] rely on manually designed behavioral model functions and manual attribute settings. The earlier approach describes human motions by various default, parameterized models, such as cellular automata models (CAM) [6], game theory models (GTM) [14], torque models (TM) [9] and Kalman filter (KF) algorithm [3]. The CAM is a discrete dynamic system, whose space is divided into the certain regular grids. And it follows a series of evolutionary rules for efficient prediction tasks. However, all the cells in the model follow the same evolutionary rules, which lacks flexibility and cannot well handle exceptional scenarios [18]. The GTM can describe the crowd conflict behavior, so it predicts the motion to the greatest extent based on the choosing strategy, but it is susceptible to surrounding uncertainties [19]. The TM treats each individual as a point-wise particle subjected to social forces, and it describes human dynamics as a system of differential equations [5]. However, it generally treats human as single particles, so it may not be suitable for some HRC applications which require track the motion of parts of the human body. The KF gives optimal estimation by weighting average the prior estimated value calculated from the state equation and the real-time measured value. However, as the velocity and acceleration of motion are not always constant, an accurate state equation is generally not available for calculating the prior estimation, and multi-step prediction is difficult [8]. The approaches by learning-based models attempt to incorporate information acquired from the workplace into the training process to predict the human motion trajectory [13]. For example, the prediction model is established by training a recurrent neural network (RNN) [2], but RNN has inherent weaknesses such as slow training speed and lack of parameter adaptation capability. These make it incapable give accurate prediction as the dynamics of human motion is time-varying. The semi-adaptive neural network (NN) model is proposed for trajectory prediction [4]. The algorithm of parameter adaptation is further extended on arbitrary layers of NN to improve the prediction accuracy [1]. However, by these methods, the estimated parameter vector may diverge as the time propagates, if the learning factor is not appropriately set. These make them incapable of defining the safe set accurately and guiding the design of robot controllers in a human-robot co-existing environment [17]. This method mainly predicts the

716

Y. Liu et al.

relatively stable trajectory data of human motion, and doesn’t work well when the motion trajectory changes abruptly. This paper proposes a novel method to estimate the human motion trajectory and associated MSEE using the semi-adaptive NN. Unlike the existing methods that directly update the parameter vector based on priori estimation error of trajectory, the NN model is firstly utilized reversely to calculate the past estimation error of parameters using the measured trajectory. After this, the estimation error (EE) of parameter vector at recent time steps are computed using recursive least squares (RLS) till the present time step. Eventually, the priori estimation of the motion trajectory and associated MSEE are computed. It is shown that the mean of estimation error (MEE) of parameters is only affected by the EE of the past trajectory at the recent several time steps. In addition, as the MSEE of parameters is computed based on the measured data, the coupling between the MSEEs of the trajectory and parameter vector is cut off. This gives a more accurate assessment of the trajectory MSEE, helping to define the safe set more precisely in the future controller design. A experimental result of predicting of 3-D motion trajectory of a point on the human right arm validate the effectiveness of the proposed method.

2

Prediction Model of Human Motion Trajectory

The priori estimate of the motion trajectory in M -step ahead is given by ∗ ) + ωk , (1) X(k+1|k) = f ∗ (X(k)    ∗ = x(k−N +1) ; where X(k+1|k) = x(k+1|k) ; x(k+2|k) ; . . . ; x(k+M |k) ∈ pM . X(k)  . . . x(k−1) ; x(k) ∈ pN is the human motion trajectory at past N time steps, N ≥ M . f ∗ : pN → pM represents the prediction model, ωk ∈ pM represents the zero-mean, additive Gaussian white noise (AGWN) in corresponding time steps. p is the dimension of data-points being acquired at every time step. Due to the nonlinearity and stochasticity in human’s behavior, it is difficult to accurately predict human motion trajectory by analytical modeling methods. In this paper, we aim to establish f ∗ model based on an offline trained back ∗ is input and the X(k+1|k) is propagation neural network (BPNN) [10], the X(k) output of the BPNN model. We choose an n-layer BPNN with logsig activation function to represent the human motion trajectory model: ∗ ) = W logsig(g(U, sk )) + (sk ), f ∗ (X(k)

(2)

∗ ] ∈ 1+pN is the input vector. g denotes (n − 1)-layer perwhere sk = [1; X(k) ceptron, whose weights are packed in U . W ∈ pM ×nh denotes the weights of the output layer. nh is the number of neurons in the hidden layers. (sk ) ∈ pM is the function reconstruction error. When the BPNN is fully trained,  tends to zero. Before training the BPNN, it is necessary to normalize the training data [12]. This can remove the effect due to different scales of data sources and speed up the

A “Look-Backward-and-Forward” Adaptation Strategy

717

parameter convergence during training. To improve the adaptivity of the model while maintain the adequate computational load, the weights of the output layer of BPNN will be trained online subsequently, while the weights of other layers are fixed after offline training. In other words, U is trained offline and W is trained online.

3 3.1

Parameter Adaptation Algorithm for Trajectory Prediction Trajectory Prediction by Parametric Model

By stacking all the column vectors of [b2 , W ]T , a time-varying parameter matrix θ ∈ pM (nh +1) is constructed, where W is the weight of the output layer and b2 is associated bias. θk represents the parameter matrix at k-th time step. φk = blkdiag([1; logsig(g(U, sk )) + b1 ]T ) ∈ pM ×pM (nh +1) is the data vector at k-th time step, where b1 is the bias of the (n − 1)-th layer. In this way, (1) is re-expressed as (3) X(k+1|k) = φk θk + ωk . From (3), a series of methods are proposed to directly predict the future trajectory from the constructed data vector and estimated parameter vector [4, 11], as ˆ (k+1|k) = φk θˆk , X (4)   ˆ (k+1|k) = x where X ˆ(k+1|k) ; x ˆ(k+2|k) ; . . . ; x ˆ(k+M |k) . Surely, such estimation is biased, and the estimation error is given by ˜ (k+1|k) = φk θ˜k + ωk , X

(5)

 = (•) − (•).  where (•) 3.2

A “Look-Backward-and-Forward” Adaptation Algorithm

As the error is introduced in the process of measuring human trajectory, the relationship between the actual value of the trajectory and the measured value of the trajectory is as ∗ ¯ ∗ − νk , X(k) =X (6) (k) ¯ ∗ is the measured value of human motion trajectory in the past N time where X (k) ˆ ∗ ∈ pN steps, νk ∈ pM is the measurement noise. As the estimated values X (k) of human motion trajectory in the past N time steps have been recorded, the ˜ ∗ ∈ pN of the human motion trajectory in the past actual estimation error X (k) N time steps is given as ˜ ∗ = X∗ − X ˆ∗ . X (7) (k) (k) (k) ˜ ∗ comes from the comparison between the actual human motion trajectory X (k) and former estimation. It’s the actual value, not estimated by the semi-adaptive

718

Y. Liu et al.

NN model [4]. This helps to estimate the parameter vector more accurately, potentially leading to more accurate prediction of motion trajectory and the associated MSEE. ˜ ∗ , the actual EE of motion trajectory at the latest M time steps From X (k) ˜∗ X ∈ pM is extracted by M,(k)

˜∗ ˜∗ X M,(k) = YM,N X(k) ,

(8)

  where YM,N  0pM ×p(N −M ) , IpM , and IpM is an pM × pM identity matrix. ˜∗ ˜ So by using X M,(k) to replace the priori estimate error X(k−M +1|k−M ) , and ˜ apply (5) inversely, the actual EE of the parameter vector θk−M at the (k−M )-th time step in the past is obtained as ˜∗ θ˜k−M = φ†k−M (X M,(k) − ωk−M ),

(9)

where φ† is the pseudo inverse of φ. The RLS algorithm is used to update the estimated parameter error, solely based on the actual EE (8). ˜∗ θ˜j = θ˜j−1 − Fj φTj X M,(j) + Δθj−1 , ∀j ≥ k − M + 1,

(10)

where Δθj−1 = θj −θj−1 is the actual increment of parameter vector, the learning factor F ∈ pM (nh +1)×pM (nh +1) is updated by   F j ϕj ϕj T F j 1 Fj+1 = Fj − λ 2 , ∀j ≥ k − M + 1, (11) λ1 λ 1 + λ 2 ϕj T F j ϕj λ1 and λ2 are positive constants in (11). ϕj = [1; logsig(g(U, sj )) + b1 ; . . . ; 1; logsig(g(U, sj )) + b1 ] ∈ pM (nh +1) is vector form of φ at j-th time step. ˜∗ With given θ˜k−M and X M,(k−M +1) , substitute (6)–(9) into (10), we obtain the EE of parameters at the (k − M + 1)-th time step. ˜∗ Repeating the above process for M times, with given θ˜k−d and X M,(k−d+1) ∀d = 1 . . . M , we eventually obtain the EE of parameters at the k-th time step as M 

¯∗ − X ˆ ∗ − φ† θ˜k = φ†k−M YM,N X ω + Δθk−d − φ†k−M YM,N ν(k) (k−M ) (k) (k) k−M d=1

−

M



¯∗ ˆ∗ Fk−d+1 φTk−d+1 YM,N X (k−d+1) − X(k−d+1) + ν(k−d+1) ,

(12)

˜∗ θˆk = θˆk−1 + Fk φTk X M,(k) ,

(13)

d=1

meanwhile,

ˆ (k+1|k) and X ˜ (k+1|k) are predicted. substitute (13) and (12) into (4) and (5), X ˜ ˆ Unlike the trajectory estimation method in [4,11], θk and θk are calculated based on the past trajectory before priori estimation of the trajectory is performed.

A “Look-Backward-and-Forward” Adaptation Strategy

3.3

719

Assessment of Mean-Square Estimation Error

In order to ensure safety and provide guidance for the safe set of the robot controller, the uncertainty of prediction is quantified during online adaptation. The MEE of the parameter vector is given by M  

¯∗ − X ˆ∗ Δθk−d + φ†k−M YM,N X E θ˜k = (k) (k) d=1

−

M

 ¯∗ ˆ∗ Fk−d+1 φTk−d+1 YM,N X − X (k−d+1) (k−d+1) ,

(14)

d=1

˜ ˜∗ so by our method, the effects of EE of the past trajectory X M,(k−d+1) to E(θk ) only limits till the past M time steps. This releases the constraints on setting the initial value of learning factor Fk in practice. According to (12) and (14), the MSEE of the parameter is calculated as χθ˜θ˜(k) =

M

T  T Fk−d+1 φ†k−d+1 YM,N E νk−d+1 νkT YM,N (φ†k−M )T + E(θ˜k )E(θ˜k )

d=1

+

M

T T Fk−d+1 φ†k−d+1 YM,N Var(νk−d+1 )YM,N (φ†k−d+1 )T Fk−d+1

d=1

+φ†k−M YM,N E

νk

M

 T νk−d+1

T T (φ†k−d+1 )T Fk−d+1 YM,N

d=1 T +φ†k−M YM,N Var(νk )YM,N (φ†k−M )T + φ†k−M Var(ωk )(φ†k−M )T .

(15)

  ˜ (k+1|k) X ˜T A priori MSEE of motion trajectory χX˜ X˜ (k + 1|k)  E X (k+1|k) is χX˜ X˜ (k + 1|k) = φk χθ˜θ˜(k)φTk + Var(ωk ),

(16)

where χX˜ X˜ (k + 1|k) = [χx˜x˜ (k + 1|k)], χx˜x˜ (k + 2|k)], ..., χx˜x˜ (k + M |k)]. Remarkably, unlike the method proposed in [4,11], the MSEE of parameter vector χθ˜θ˜(k +1) is no longer depending on χX˜ X˜ (k +1|k) , but calculated by (15) in the next time step.  k−d = In practice, Δθk−d is not exactly known, but is approximated by Δθ (θˆk−d − θˆk−M )/(M − d + 1) in [4]. For the “look-forward” method in [4,11], the  θ˜k+1 )  E(θ˜k+1 ) − E(  θ˜k+1 ) is given by deviation E(   k k

 T  n,  θ˜k+1 ) = E( (I − Fi φ φi ) Δθ (17) i

n=0

i=n+1

 n have same sign, E(  θ˜k+1 ) will be far way from zero. In this case, it gives if Δθ inaccurate calculation of the MEE of parameter vector, resulting in an inaccurate

720

Y. Liu et al.

definition of safe set. This will potentially threaten safe HRC. In comparison, by our method, the deviation of MEE of the parameter vector E(θ˜k ) due to above approximation is further derived as ˜ θ˜k ) = E(

M

 k−d . Δθ

(18)

d=1

 k−d to E( ˜ θ˜k ) also limits till the past So the effects of approximation error Δθ M time steps. This will generally give a more accurate calculation of MEE of the parameter vector.

Fig. 1. Block diagram

3.4

Fig. 2. Setup.

Fig. 3. The trajectory of human motion.

Summary and Discussion

The EE of the parameter vector plays a key role in priori estimate of the motion trajectory and computation of the associated MSEE. However, the “lookforward” estimation algorithm in [4] only updates the EE of the parameter vector solely based in the priori EE of the trajectory, which is not accurate enough. The proposed method first computes the EE of the parameter vector in the past M time steps based on the current measurements, which is a “look-backward” process. Later, the EE of the parameter vector in the current time step is computed recursively based on past time steps’ EE of the parameter vector, the measurements and the estimation of trajectory. This is a “look-forward” process. Thus, we call it as “look-backward-and-forward” priori estimation algorithm. In this way, a more accurate priori estimation of motion trajectory is achieved. In addition, MSEE of the parameter vector is only related to the actual trajectory error of the past M time steps, and is decoupled from the priori MSEE of the motion trajectory. This will give a more accurate estimation of safe sets in the subsequent design of robot controllers. The proposed “look-backward-and-forward” algorithm’s the block diagram is shown in Fig. 1.

A “Look-Backward-and-Forward” Adaptation Strategy

4

721

Experimental Results

In order to verify the proposed human motion prediction approach, the experiment of capturing motion position is carried out. The experimental setup is shown in Fig. 2. A Qualisys Track Manager software based on six cameras is utilized to capture the motion trajectory of the marker ball on the human right arm at an frequency 20 Hz. 1) Data Acquisition: The movement is shown in Fig. 4. As shown in Fig. 3, the motion trajectory are stored in three-dimensional space. A camera coordinate system is established by six camera position to store the data we collected. The entire trajectory contains 600 sampling points, the first 2/3 of sampling points are used for offline NN training, and the rest for providing the online adaption algorithm. We set M = N = 3 in (1). The prediction range can be controlled by adjusting the size of M according to specific applications.

Fig. 4. Single particle motion decomposition diagram.

2) Training of Neural Network : We use a 3-layer NN with 14 nodes in the hidden layer. The number of nodes in the input layer and output layer is 9. The learning rate is set to 0.05 and the number of epochs is 5000. 3) Online Adaptation: After obtaining NN model for motion transition, we use RLS-PAA to update the weights of the last layer. Since the number of nodes in the last layer is 9, and each node has 15 parameters, of which 14 correspond to the output from the hidden layer, and the remaining one parameter is a bias term, there are total 135 parameters to be adapted online. 4) Trajectory Prediction: We predict the trajectory of the human right arm by three methods: without adaptation of BPNN’s weights, the “lookforward” algorithm proposed in [4], and our novel “look-backward-and-forward” algorithm. Unlike KF-based method, the NN based methods are able to predict the trajectory several time-step ahead. Figure 6 shows the prediction results of the marker ball on the human right arm, where the sub-figures (a), (b) and (c) represents the prediction results at (k + 1)-th, (k + 2)-th and (k + 3)-th time step respectively. The actual motion trajectory of the human right arm is denoted by black line. The trajectories predicted by the other three methods are denoted by the red, green and blue lines respectively. We find the blue line has a high coincidence with the black line, followed by the green line and red line. 5) Error prediction: Figure 6 shows the MSEE of the motion trajectory at every time step, where the sub-figures (a), (b) and (c) represents the prediction results at (k + 1)-th, (k + 2)-th and (k + 3)-th time step respectively. We find the solid and dotted blue lines have a higher coincidence degree than the green

722

Y. Liu et al. (a)

1400

1100

800 400

800 400

0 -400 -800

Y

1450

1050

1850

2250

Y

X

Z

Z 1100

(c)

1700

1400

1400

Z

(b)

1700

1700

0 -400 -800

1100 800 400

1050

1450

1850

2250

Y

X

0 -400 -800

1050

1850

1450

2250

X

ˆ (k+1|k) by different methods (unit: mm). Fig. 5. Comparison of X (a)

20 10 0

40

80

120

160

120 80 40 0

200

0

40

actual traj:

80

120

160

time step

time step

fixed param:

"look-fwd":actual

(c)

320

MSEE(mm 2 )

30

0

(b)

160

MSEE(mm 2 )

MSEE(mm 2 )

40

estim

200

240 160 80 0

0

40

80

120

160

200

time step

"look-bwd-fwd":acutal

estim

Fig. 6. Comparison of χX˜ X˜ (k + 1|k) by different methods (unit: mm2 ). 200.00

163.55

150.00

123.97 86.27

100.00 50.00

64.22

26.10 18.71

0.00

next 1st next 2nd sampling point sampling point "look-forward"

next 3rd sampling point

"look-backward-and-forward"

Fig. 7. Prediction mean ˆ (k+1|k) (unit:mm). X

error

of

Fig. 8. Difference between χX˜ X˜ (k + 1|k) and actual mean square error (unit:mm2 ).

ones, validating the superior of our method. The comparisons of the MSEEs are shown in Figs. 7 and 8.

5

Conclusions

This paper proposes a novel “look-backward-and-forward” algorithm to predict the human motion trajectory in M time steps ahead. Firstly, the actual EE of parameter vector in (k − M )-th time step is calculated by reversely using the prediction model, which is a “look-backward” process. Secondly, the EE of parameter vector is updated by RLS-PAA till the current time step, with the historical records of trajectory and the estimation. This is a “look-forward” process. In this way, the direct coupling between MSEEs of the parameters and the motion trajectory is cut-off, so that more accurate priori estimations of motion trajectory and its associated MSEE are achieved. It is shown that the MEE of

A “Look-Backward-and-Forward” Adaptation Strategy

723

the parameters is only affected by EE of the parameters’ increment till past M time steps, which leads to a more accurate assessment of MEE of the parameters. The experiment result verifies the effectiveness of the proposed method. The future works include extending the proposed algorithm for predicting the pose trajectory of a rigid body, as well as design the safe robot controller for HRC with according to the predicted trajectory.

References 1. Abuduweili, A., Li, S., Liu, C.: Adaptable human intention and trajectory prediction for human-robot collaboration. arXiv preprint arXiv:1909.05089 (2019) 2. Alahi, A., Goel, K., Ramanathan, V., Robicquet, A., Fei-Fei, L., Savarese, S.: Social lstm: human trajectory prediction in crowded spaces. In: Proceedings of the IEEE conference on computer vision and pattern recognition, pp. 961–971 (2016) 3. Catlin, D.E.: Estimation, Control, and the Discrete Kalman Filter, vol. 71. Springer, Heidelberg (2012) 4. Cheng, Y., Zhao, W., Liu, C., Tomizuka, M.: Human motion prediction using semi-adaptable neural networks. In: 2019 American Control Conference (ACC), pp. 4884–4890. IEEE (2019) 5. Farina, F., Fontanelli, D., Garulli, A., Giannitrapani, A., Prattichizzo, D.: Walking ahead: the headed social force model. PloS one 12(1), e0169734 (2017) 6. Hao, Q.Y., Jiang, R., Hu, M.B., Jia, B., Wu, Q.S.: Pedestrian flow dynamics in a lattice gas model coupled with an evolutionary game. Phys. Rev. E 84(3), 036107 (2011) 7. Helbing, D., Molnar, P.: Social force model for pedestrian dynamics. Phys. Rev. E 51(5), 4282 (1995) 8. Kim, Y., Bang, H.: Introduction to Kalman filter and its applications. Introduction and Implementations of the Kalman Filter, vol. 1, pp. 1–16 (2018) 9. Langston, P.A., Masling, R., Asmar, B.N.: Crowd dynamics discrete element multicircle model. Safety Sci. 44(5), 395–417 (2006) 10. Li, X.F., Xu, J.P., Wang, Y.Q., He, C.Z.: The establishment of self-adapting algorithm of BP neural network and its application. Syst. Eng. Theory Pract. 5 (2004) 11. Liu, C., Tomizuka, M.: Safe exploration: Addressing various uncertainty levels in human robot interactions. In: 2015 American Control Conference (ACC), pp. 465– 470. IEEE (2015) 12. Liu, X.T.: Study on data normalization in bp neural network. Mech. Eng. Autom. 3, 122–123 (2010) 13. Ma, Y., Lee, E.W.M., Yuen, R.K.K.: An artificial intelligence-based approach for simulating pedestrian movement. IEEE Trans. Intell. Transp. Syst. 17(11), 3159– 3170 (2016) 14. Tanimoto, J., Hagishima, A., Tanaka, Y.: Study of bottleneck effect at an emergency evacuation exit using cellular automata model, mean field approximation analysis, and game theory. Physica A: statistical mechanics and its applications 389(24), 5611–5618 (2010) 15. Villani, V., Pini, F., Leali, F., Secchi, C.: Survey on human-robot collaboration in industrial settings: safety, intuitive interfaces and applications. Mechatronics 55, 248–266 (2018) 16. Wang, T., Tao, Y.: Research status and industrialization development strategy of Chinese industrial robot. J. Mech. Eng. 50(9), 1 (2014)

724

Y. Liu et al.

17. Wei, T., Liu, C.: Safe control algorithms using energy functions: a unified framework, benchmark, and new directions. In: 2019 IEEE 58th Conference on Decision and Control (CDC), pp. 238–243 (2019) 18. Xie, J.J., Xue, Y.: Study on the dynamics of indoor pedestrian evacuation based on the game. In: 2011 Seventh International Conference on Natural Computation, vol. 4, pp. 2283–2287. IEEE (2011) 19. Zheng, X., Cheng, Y.: Conflict game in evacuation process: a study combining cellular automata model. Physica A: Stat. Mech. Appl. 390(6), 1042–1050 (2011)

Velocity Constraints Based Online Trajectory Planning for High-Speed Parallel Robots Di Yang1 , Fugui Xie1,2 , and Xin-Jun Liu1,2(B) 1 The State Key Laboratory of Tribology, Department of Mechanical Engineering, Tsinghua

University, Beijing 100084, China [email protected] 2 Beijing Key Lab of Precision/Ultra-Precision Manufacturing Equipments and Control, Tsinghua University, Beijing 100084, China

Abstract. Trajectory planning method is important for stable and efficient sorting operations of robots. In this paper, a method for planning the equal-height pickingand-placing trajectory considering velocity constraint is proposed. The velocity constraint in start/end kinematics parameter makes the trajectory more adaptable to complex production line situations. An online trajectory optimization solution is proposed, which simplifies the solution of optimization problem and achieves the real-time nature of the method by BP neural networks. Simulation results on SR4 parallel robots show the effectiveness of the optimization method. The work done in this paper is of great help to the application of high-speed parallel robots. Keywords: High-speed parallel robot · Trajectory planning · BP neural network · Velocity constraint

1 Introduction In recent years, high-speed parallel robots have been widely used in many fields such as food, electronics, and the light industry. To achieve efficient and stable production, the operational efficiency and stability of the pick-and-place robots are increasingly required. The kinematic information are important influences, such as position, velocity, and acceleration obtained by trajectory planning as the input signal of the robot control system. Therefore, it is necessary to study the trajectory planning method. Generally, the construction of smooth parametric laws of movement is one of the main research concerns in trajectory planning [1]. Zhang of Harbin Institute of technology proposed a time-optimal planning algorithm [2], which used elliptical path and improved sinusoidal velocity planning on the Delta robot. Xie [3] studied the trajectory planning problem of high-speed parallel robots with the Lamé curve algorithm. Xie [4, 5] proposed a global G3 continuity toolpath smoothing method using quantic B-spline for five degrees of freedom PMRs. There are also many scholars who construct the robot movement law by polynomial [6], Bézier curve [7], Cubic spline curve [8], B-spline curve [9], etc. As the industrial requirements for robot efficiency and stability increase, more and more © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 725–734, 2021. https://doi.org/10.1007/978-3-030-89092-6_66

726

D. Yang et al.

scholars are using high order spline curves, such as quintic B-spline curve [1, 4, 10] or quintic Bézier curve [7] to construct the movement law. After constructing the parametric movement law, the end-effector motion of the robot can be obtained by giving the target path point information, the trajectory planning parameters, and the cycle time. To improve the performance, the optimization of trajectory planning parameters and cycle time is the essential for trajectory planning study [1]. To improve the operation efficiency, many scholars have proposed trajectory planning methods with the goal of minimizing cycle time [11–14]. With the smoothness of trajectory as the optimization objective, Li [10] proposed a trajectory planning method for high-speed sorting parallel robots based on quintic B-spline curves. This method optimizes the trajectory parameters with the acceleration and jerk as a combined optimization, which provided a generic smoothness index in a Cartesian coordinate system. Croft [13] proposed a time-optimal trajectory planning method constrained by torques and the derivatives of joint torques. Han [1] proposed a trajectory planning method with motor drive capability and tracking error as constraints. The method constructed trajectories based on quintic B-spline curves and achieved online optimization by BP neural network, which provided a general idea to realize the real-time of the algorithm. Limited by the complexity of the optimization problem and computational efficiency, most of the pick-and-place trajectory planning does not consider the velocity constraints at the start/end points. However, the velocity constraint facilitates the accuracy of picking-and-placing empirically for conveyor conditions. To handle the fast infeed situation, it is necessary to study the velocity constraints based trajectory planning method. Moreover, combining the anisotropic kinematic and dynamic properties of the parallel robot should be able to make the velocity constraints based trajectories without losing efficiency and stability. In this view, this paper will propose an online smooth trajectory planning method with velocity constraints. In this method, the space equal-height picking-and-placing trajectory is proposed based on the quintic B-spline. Trajectory parameters, such as geometric offset and time, need to be selected. Therefore, two optimization problems are proposed with smoothness and time as the target of optimization, respectively. To simplify the optimization problem and implement the algorithm real-time, the BP neural network method is used. Finally, a flow chart of trajectory planning is given, and the method can be widely applied to high-speed parallel robots in picking-and-placing.

2 Velocity Constraints Based Online Trajectory Planning Method 2.1 Description of 5-th B-Spline Curve The displacement s can be controlled by path points s0 . . . sf , s(t) =

n  i=0

Qi · Ni,p (t), 0 ≤ t ≤ T

(1)

Velocity Constraints Based Online Trajectory Planning

727

where Qi are n + 1 control points and Ni,p (t) are the basis function of B-spline of degree p. The construction of basis function satisfies De-Boor recursive algorithm:  ti+p+1 − t t − ti 1, ti ≤ t < ti+1 , Ni,p (t) = Ni,0 (t) = Ni,p−1 (t) + Ni+1,p−1 (t) 0, otherwise ti+p − ti ti+p+1 − ti+1 (2) ti (i = 0 . . . m) constitute the node sequence of function s(t). To control the acceleration of the starting and ending points, it is necessary to determine two virtual nodes, tp+1 and tn .Therefore, the node sequence is as follow: {0...0 ...tn−1 , tn , T ...T} , tp+1 , tp+2    s0

s1 ...sf −1

(3)

sf

where s0 . . . sf correspond to tp+2 . . . tn−1 one by one, there is n − p − 2 = f − 1. Parameter p is set to 5 to obtain C 4 continuous spline curve. Since the trajectory passes through f + 1 path points s1 . . . sf −1 , f + 1 interpolation constraint equations can be obtained: ⎧ s(0) = s0 = Q0 ⎪ ⎪ ⎪ n ⎨ s(tp+j+1 ) = sj = Qi · Ni,p (tp+j+1 ), j = 1, · · · , f − 1 (4) i=0 ⎪  ⎪ n ⎪ ⎩ s(T ) = sf = Qi · Ni,p (1) = Qn · Nn,0 (1) = Qn i=0

According to the velocity, acceleration and acceleration constraints of the starting and ending points, six boundary constraint equations can be obtained: ... ... s˙ (0) = v0 , s¨(0) = a0 , s (0) = j0 , s˙ (T ) = v1 , s¨(T ) = a1 , s (T ) = j1

(5)

By combining Eq. (4) with Eq. (5), the equation for solving the control point Qi can be obtained: Q = A−1 B

(6)

where    

T B1 A1 ,B= Q = Q0 , . . . , Qn , A = A2 B2

T

T B1 = s0 , . . . , sf , B2 = v0 , a0 , j0 , v1 , a1 , j1

(7) (8)

⎡

⎤ 1 0 ··· 0 ⎢ N0,p (τp+2 ) N1,p (τp+2 ) · · · Nn,p (τp+2 ) ⎥ ⎢ ⎥ ⎢ ⎥ .. . . .. A1 = ⎢ ... ⎥ . . . ⎢ ⎥ ⎣ N0,p (τn−1 ) N1,p (τn−1 ) · · · Nn,p (τn−1 ) ⎦ 0

···

0 1

(9)

728

D. Yang et al.

⎤ ··· ⎥ ··· ⎥ ⎥ ··· ⎥ ⎥ ··· a4,n−1 a4,n ⎥ ⎥ ··· a5,n−2 a5,n−1 a5,n ⎦ · · · a6,n−3 a6,n−2 a6,n−1 a6,n

⎡

a1,0 a1,1 ⎢a a a ⎢ 2,0 2,1 2,2 ⎢ ⎢a a a a A2 = ⎢ 3,0 3,1 3,2 3,3 ⎢ ⎢ ⎣

(10)

2.2 Geometric Trajectory Generation The picking-and-placing trajectory is common in manufacturing. When starting and ending points are equal in height, the trajectory can be called equal-height picking-andplacing trajectory. The picking-and-placing trajectory with velocity constraints is spatial rather than planar. Figure 1 shows a space equal-height picking-and-placing trajectory. The curve is controlled by five path points including the starting and ending points and kinematic state of the starting and ending points.

Fig. 1. Space equal-height picking-and-placing trajectory.

The node sequence contains m + 1(m + 1 = f + 2p + 3 = 17) nodes, as follow:  {0...0 t7 , t8 , t9 , t10 , T ...T}, t8 = T 2 (11) , t6 ,      s0

s1

s2

s3

s4

B1 and B2 are represented as follows: B1 = [s0 , s1 , s2 , s3 , s4 ]T , B2 = [v0 , 0, 0, v1 , 0, 0]T

(12)

The path points satisfy the following relations: v0 − v1 · λ2 T , s3 = s 3 − v1 · λ3 T 2 ⎤ y0 z0 + ηz θ0 y0 +y1 1 ⎦ z0 + z θ0 +θ 2 2 y1 z0 + ηz θ1

s1 = s 1 + v0 · λ1 T , s2 = s 2 + ⎤ ⎡ x0 s 1 ⎣ s 2 ⎦ = ⎣ x0 +x1 2 s x1 ⎡

(13)

(14)

Velocity Constraints Based Online Trajectory Planning

729

where (λ1 , λ2 , λ3 ) are offset parameters of the trajectory. z is the height of the trajectory. η is the height coefficient of the path points s1 and s3 . Displacement is obtained: χ(t) =

n 

Qi · Ni,p (t), 0 ≤ t ≤ T

(15)

i=0

To facilitate the solution of subsequent optimization problems, the sequence of nodes is normalized to τi =

ti , i = 1, 2, · · · , 17 T

(16)

2.3 Online Smooth Trajectory Optimization 1) The optimization problem of trajectory smoothness For a given set of start/end position, velocity, and trajectory height parameters, the acceleration maximum and jerk maximum of the trajectory in the Cartesian coordinate system are selected as the evaluation index of trajectory smoothness. They are defined as functions of τ6 , τ7 , τ9 , τ10 , λ1 , λ2 , and λ3 , ¨ amax (τ6 , τ7 , τ9 , τ10 , λ1 , λ2 , λ3 ) = max χ(t)

(17)

 ...  jmax (τ6 , τ7 , τ9 , τ10 , λ1 , λ2 , λ3 ) = max  χ (t)

(18)

0≤t≤T



0≤t≤T

0 < τ6 < τ7 < 0.5 < τ9 < τ10 < 1 0 < λi < 1 i = 1, 2, 3

(19)

Fig. 2. Distributions of amax and jmax versus λ1 , λ2 and λ3

In Fig. 2, it can be found that the distribution of λi (i = 1, 2, 3) is convex for a given sequence of nodes. The conclusion also clearly holds considering the physical meaning

730

D. Yang et al.

of λi (i = 1, 2, 3) as being the relative offset of the control points of the B spline curve. And λi (i = 1, 2, 3) are different values for the smallest amax and jmax , which facilitates us to combine amax and jmax to construct the optimization objective function. The convexity of amax and jmax for τi (i = 6, 7, 9, 10) has been judged in the literature [10]. So consider amax and jmax to be convex for τ6 , τ7 , τ9 , τ10 , λ1 , λ2 , and λ3 . The parameters (τ6 , τ7 , τ9 , τ10 , λ1 , λ2 , λ3 ) have different values when amax and jmax take their minimum values. So consider normalizing amax and jmax to construct the objective function: max F =

min(amax ) min(jmax ) + amax jmax

subject to:  0 < τ6 < τ7 < 0.5 < τ9 < τ10 < 1 0 < λi < 1 i = 1, 2, 3

(20)

Since F and its gradient are continuous in the feasible domain of τ6 , τ7 , τ9 , τ10 ,λ1 , λ2 , λ3 , F is convex, there exists global optimality. Therefore, the optimization problem can be easily solved by calling fmincon function in the Matlab® optimization toolbox, i.e., the interior point method. 2) The optimization problem of trajectory time Due to the anisotropy of the parallel robot dynamics, the following optimization problem can be formulated by considering the joint torques and their derivatives in conjunction with the rigid body dynamics model of the robot. min T

⎧ |τmotor i (t)| ≤ τmax ⎪ ⎪  ⎪ ⎨  dτ motor i (t)   subject to:   ≤ τmax ⎪ dt ⎪ ⎪ ⎩ F = Fmax

(21)

By solving the above optimization problem, the trajectory time T and trajectory parameters (τ6 , τ7 , τ9 , τ10 , λ1 , λ2 , λ3 ) can be derived from the given trajectory start/end kinematic parameters. However, the constraint F = Fmax in this optimization problem can greatly increase the difficulty of solving the optimization problem. 3) Optimization procedure To solve the optimization problem and realize the algorithm online, an online solution of the above optimization problem was propose based on BP neural network. The flow chart of the trajectory planning method is shown in Fig. 3. First, the start/end kinematic parameters of several sets of trajectories are randomly generated. It is worth noting that the start/end position parameters generated here are relative coordinates, i.e., the span of the trajectory, rather than absolute coordinates. Such an operation makes the corresponding neural network model generic.

Velocity Constraints Based Online Trajectory Planning

731

Fig. 3. Flow chart of online smooth trajectory planning

Then, through these sets, the training set of start/end kinematic parameters and trajectory parameters is obtained by trajectory smoothing optimization. The training set is used to train a two-hidden-layer BP neural network. The number of nodes in the hidden layer of the network structure is (12, 12), and the activation function is a Sigmoid function. The R-value on the test set is 0.98964. With this neural network, the optimization problem of Eq. (20) can be solved in real time. Subsequently, the start/end kinematic parameters of several sets of trajectories are randomly generated in the workspace. These sets should contain all picking-and-placing locations as much as possible for a specific parallel robot. This is to facilitate the solution of the optimization problem shown in Eq. (21), since it involves the calculation of specific parallel robot dynamics. With the last neural network, the F = Fmax constraint in Eq. (20) can be easily implemented to quickly solve this optimization problem and generate the training set. Similarly, the training set is used to train another two-hidden-layer BP neural network. The number of nodes in the hidden layer of the network structure is (20, 20), and the activation function is a Sigmoid function. The R-value on the test set is 0.99183. With this neural network, the optimization problem of Eq. (21) can be solved in real time. Combining the two neural networks, the trajectory parameters (τi , λj ) and trajectory time T can be solved online by given working conditions.

3 Verification In order to verify the effectiveness of the trajectory planning method proposed in this paper, the simulation was carried out with the SR4 parallel robot. The simulation platform is Matlab, and the calculation program is based on the dynamic model of the robot. Figure 4 (a) shows the robot used. The SR4 mechanism [15] has three translational degrees and one rotational degree of the z-axis. The rotational motion of the z-axis is

732

D. Yang et al.

Fig. 4. (a) 3D view and (b) Path of the adept cycle with start/end velocity constraint in the basic coordinate system of the SR4 parallel robot.

transformed from the vertical relative motions of upper and lower moving sub-platforms by using the crank-rocker and the bevel gears between the two platforms. In the simulation, the robot moves according to the industry standard “Adept cycle” with start/end velocity constraint. Industry standard “Adept cycle” usually refers to the picking-and-placing track with a span of 305 mm and a height  √of 25 mm. The trajectory  √ is shown in Fig. 4 (b) which starts at (−305 2 2, 305 2 2, −650) mm and ends  √  √ at (305 2 2, −305 2 2, −650) mm with 25 mm height. The velocity magnitude is 200 mm/s along the y-axis. The height coefficient η = 0.5. The optimized trajectory time is 0.213 s, hence T = 0.213s is used for the comparison. Figure 5 shows the difference between the joint jerk before and after optimization, and the peak-to-peak value of the joint jerk is reduced by 26.68% after optimization. Figure 6 shows the difference between the torque derivative before and after optimization, and the peak-to-peak value of the torque derivative is reduced by 11.39% after optimization.

Fig. 5. Jerk of (a) unoptimized trajectory and (b) optimized trajectory versus time.

Velocity Constraints Based Online Trajectory Planning

733

Fig. 6. Torque derivative of (a) unoptimized trajectory and (b) optimized trajectory versus time.

After parameter optimization, the joint jerk and torque derivative is effectively improved, which reflects the smoother motion of the robot.

4 Conclusions The velocity constraint at start/end points can facilitates the accuracy of picking-andplacing empirically for conveyor conditions. To handle the fast infeed situation, an online trajectory planning method based on velocity constraints is proposed for high speed parallel robots. The method uses quintic B-spline curves to design trajectory parameters based on velocity constraints. And BP neural networks are generated for two optimization problems, smoothness and cycle time, which can be solved in real time. Finally, the proposed method is validated on the SR4 high-speed parallel robot, the industry standard “Adept cycle” with start/end velocity constraint is adopted as the test trajectory. The simulation results show that the proposed method can achieve trajectory smoothing for picking-and-placing trajectory and improve the motion stability of the robot. The proposed method can also be expanded to the trajectory planning for other high-speed parallel robot, and the trajectory smoothing method can be used to other serial sorting robots. Acknowledgements. This work is supported by the National Natural Science Foundation of China under Grants 51922057 and 91948301.

References 1. Han, G.: High acceleration characteristics optimization and stable motion control of pickand-place parallel robots. Tsinghua University (2020) 2. Zhang, Y.Q.: Research on the key technology for the open control platform of parallel robots. Harbin Institute of Technology (2013)

734

D. Yang et al.

3. Xie, Z.X., Shang, D.W., Ren, P.: Optimization and experimental verification of pick-and-place trajectory for a delta parallel robot based on Lamé curves. J. Mech. Eng. 51, 52–59 (2015) 4. Xie, Z.H., Xie, F.G., Liu, X.J., Wang, J.S.: Global G3 continuity toolpath smoothing for a 5-DoF machining robot with parallel kinematics. Rob. Comput. Integr. Manuf. 67, 102018 (2021) 5. Xie, Z.H., Xie, F.G., Liu, X.J., Wang, J.S., Mei, B.: Tracking error prediction informed motion control of a parallel machine tool for high-performance machining. Int. J. Mach. Tools Manuf. 164, 103714 (2021) 6. Barre, P., Bearee, R., Borne, P.: Influence of a jerk controlled movement law on the vibratory behavior of high-dynamics systems. J. Intell. Rob. Syst. 42, 275–293 (2005) 7. Dai, Z., Sheng, X., Jian, H., Wang, H., Zhang, D.: Design and implementation of Bézier curve trajectory planning in DELTA parallel robots. In: Liu, H., Kubota, N., Zhu, X., Dillmann, R., Zhou, D. (eds.) ICIRA 2015. LNCS (LNAI), vol. 9245, pp. 420–430. Springer, Cham (2015). https://doi.org/10.1007/978-3-319-22876-1_36 8. Bourbonnais, F., Bigras, P., Bonev, I.A.: Minimum-time trajectory planning and control of a pick-and-place five-bar parallel robot. IEEE/ASME Trans. Mechatron. 20(2), 740–749 (2015) 9. Azizi, M.R., Khani, R.: An algorithm for smooth trajectory planning optimization of isotropic translational parallel manipulators. Proc. Inst. Mech. Eng. C J. Mech. Eng. Sci. 230(12), 1987–2002 (2016) 10. Li, Y.H., Huang, T., Derek, G.C.: An approach for smooth trajectory planning of high-speed pick-and-place parallel robots using quintic B-splines. Mech. Mach. Theory 126, 479–490 (2018) 11. Sahar, G., Hollerbach, J.M.: Planning of minimum-time trajectories for robot arms. Int. J. Rob. Res. 5(3), 90–100 (1984) 12. Bobrow, J.E., Dubowsky, S., Gibson, J.S.: Time-Optimal control of robotic manipulators along specified paths. Int. J. Rob. Res. 4(3), 554–561 (1985) 13. Constantinescu, D., Croft, E.A.: Smooth and time-optimal trajectory planning for industrial manipulators along specified path. J. Rob. Syst. 17(5), 233–249 (2000) 14. Pietsch, I.T., Krefft, M., Becker, O.T.: How to reach the dynamic limits of parallel robots? an autonomous control approach. IEEE Trans. Autom. Sci. Eng. 2(4), 369–379 (2005) 15. Liu, X.J., Han, G., Xie, F.G., Meng, Q.Z., Zhang, S.: A novel parameter optimization method for the driving system of high-speed parallel robots. J. Mech. Rob. 10(4), 041010 (2018)

Medical Engineering

Ultra-sensitive and Stretchable Optical Fiber Respiratory Belt Sensor Tianliang Li(B) , Yifei Su, Hui Cheng, Fayin Chen, Yuegang Tan, and Zude Zhou School of Mechanical and Electronic Engineering, Wuhan University of Technology, Wuhan 430070, China [email protected]

Abstract. Respiratory rate is one of the most important factors for assessing human health and mental load. Flexible sensors have gained wide attention in applications for monitoring respiratory rate due to high conformability and maximum non-invasiveness to the human body. Herein, we report a flexible optical fiber respiratory belt (OFRB) sensor that can fit closely to the human skin for human respiration monitoring. This sensor takes an elastic waistband as a carrier, a circular-shaped optical fiber is sewed into the elastic waistband and two Fiber Bragg gratings (FBGs) are connected to both ends of the optical fiber. A dual FBGs light intensity difference algorithm has been adopted to eliminate uncertain light power fluctuation interference. This article provides a description of the characteristics of the OFRB sensor in terms of response to tensile strain and the results indicate a high sensitivity of 0.41 dB/mm, and excellent stretchability of up to 41.5%. Experiments on several static postures and activities of the human body have been conduct to validate the sensing performance of the OFRB sensor. The obtained positive results encourage the OFRB sensor for use in medical care for human respiratory rate monitoring in a non-invasive real-time method. Keywords: Fiber Bragg grating · Flexible wearable sensor · Respiratory rate monitoring

1 Introduction Respiratory rate is one of the most important signs of the human body in the field of medicine [1]. The monitoring of respiratory status is very important for assessing the health and mental state of people [2], as well as the effect of rehabilitation treatment of patients in hospitals [3] (including Corona Virus Disease 2019, COVID-19). Due to high conformability and maximum non-invasiveness to the human body, flexible sensors have become an important means of real-time monitoring of the patient’s respiratory rate with a high degree of comfort [2, 4]. Electrical wearable sensors based on flexible materials and smart clothing have made considerable progress. Different types of sensor elements are used in the preparation of flexible sensors, such as piezoresistive sensors [5], piezoelectric sensors [6] and capacitive sensors [7]. However, they have limitations in terms of electromagnetic interference, complex structures, and current leakage risks in actual medical environments. © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 737–747, 2021. https://doi.org/10.1007/978-3-030-89092-6_67

738

T. Li et al.

Recently, Fiber Bragg grating (FBG) based flexible sensors intended to track respiratory rate are favored by scholars thanks to their advantages, including small size, biocompatibility, and resistance to electromagnetic interference [8, 9]. Existing studies have used the wavelength modulation principle of Fiber Bragg grating (FBG) to prepare respiratory sensors [10], and reconstruct the user’s breathing pattern by monitoring the change of the FBG center wavelength [11–13]. However, the poor stretchability of the silica fibers sensor (tensile strain < 1% [14]) results in a challenge and limitation for detection of tensile strain caused by human skin (ε > 30%) [11]. Although many approaches have been adopted to increase the tensile strain of the flexible optical fiber sensor (Such as combine optical fibers with elastic fabrics or flexible materials) [15, 16], they are easily affected by light power fluctuations which results in inaccurate output signals. In this paper, a flexible optical fiber respiratory belt (OFRB) sensor with high sensitivity and excellent stretchability has been proposed to monitor human respiratory rate in real time. This sensor takes an elastic waistband as a carrier, a circular-shaped optical fiber is embedded into the elastic waistband and both ends of the optical fiber are connected with FBG. Such an ingenious structural design can greatly increase the tensile strain of the optical fiber sensor (up to 41.5%). This fabric-fiber stitch structure can effectively depress manufacturing process complexity of the sensor compared with encapsulated by flexible materials [8, 11]. The working principle of OFRB sensor is based on the light intensity change of FBGs under different tension conditions. A dual FBGs light intensity difference algorithm is used in OFRB sensor signal processing to eliminate light power fluctuation interference. As a proof of concept, the assessment of the OFRB sensor on several static postures and activities of the human body have been performed, and successfully monitored the respiration of people of different age groups in nursing environment.

2 Working Principle, Preparation and Performance Evaluation of OFRB Sensor 2.1 Working Principle Human respiration rate monitoring is realized by an OFRB sensor strapped to the human abdomen. The OFRB sensor is composed of a circular-shaped fiber, two FBGs (noted as FBG1 and FBG2) with different central wavelengths, and an ordinary elastic waistband. The circular-shaped fiber is embedded in the elastic waistband by sewing, which makes the tensile strain of the OFRB sensor up to 41.5% for the measurement requirement of human skin-induced strain (≥30%). In this paper, the sensing principle of the OFRB sensor is based on the light intensity loss of circular-shaped fiber [17]. Among them, the light intensity loss of the OFRB sensor is obtained by detecting the light intensity change of FBG2. However, the light intensity loss caused by the bending of the signal transmission fiber and light power fluctuation of the light source will be coupled with the light intensity loss of the OFRB sensor, which affects the accuracy of the FBG2 light intensity. Therefore, FBG1 is arranged at the end of the OFRB sensor near the light source for the calibration of the incident light.

Ultra-sensitive and Stretchable Optical Fiber

739

This arrangement ensures that the light intensity change of FBG1 can be only affected by the bending of the signal transmission fiber and the light power fluctuation, and not by the light intensity loss inside the OFRB sensor. Hence, the influence of light power fluctuations on the performance of the OFRB sensor can be reduced by calculating the light intensity change of FBG1 and compensating it to the light intensity difference of FBG2. Normally, the light intensity loss of an optical fiber is only related to its bending radius. The relationship between the bending radius of the optical fiber R’ and the light intensity loss coefficient can be expressed as:   √ 2 3  πU − 2W2 2R 3a β a e (1) 2αc = 3√ ev W 2 aR V2 Kv−1 (W)Kv+1 (W) where U is the normalized phase constant. W is the normalized radial constant. V is the normalized frequency. Kv is the v-order correction of the second kind of Bessel function. a and k are the core diameter and the radial propagation constant, respectively. β is a propagation constant. It can be found that there is a non-linear relationship between the bending radius of the optical fiber and the light intensity loss coefficient (Fig. 1a). As shown in Fig. 1b, the bending radius of the optical fiber R’ will increase with the increase of the tensile length L, we approximate the complex geometric relationship between R and L to a linear relationship: R = n · L + m

(2)

where n and m are a scale factor and a constant, respectively. When the SSOF sensor is tensioned by L, the light intensity loss coefficient can be expressed as 2αc (L). Assuming that the peak light intensity values of FBG1 and FBG2 in the initial state (Fig. 1b (top)) are I1 and I2 . When the fiber is not bent, the light intensity values of FBG1 and FBG2 are I10 and I20 . The relationship between the light intensity of two FBGs and the bending loss in the initial state can be described as: I20 − I2 = α1 + (I10 − I1 )

(3)

where α1 = 6πR·2αc (0) is the initial optical loss of the circular-shaped optical fiber, I10 -I1 is non-operating optical loss of the OFRB sensor, such as the loss caused by the bending of the signal transmission fiber. The peak light intensities of FBG1 and FBG2 when the OFRB sensor is tensioned by L can be denoted as I1x and I2x . Then the total optical loss of the OFRB sensor can be expressed: I20 − I2x = αx + (I10 − I1x )

(4)

where αx = 6πR·2αc (L) is the optical loss when the OFRB sensor is tensioned by L, using (3) to subtract (4) to obtain the mapping relationship between the light intensity difference of FBG1 and FBG2 and the tensile length: I2x − I2 + (I1 − I1x ) = α1 − αx = 6πR · 2αc (0) − 6πR · 2αc (L)

(5)

It can be seen from the formula (5) that the influence caused by the bending of the signal transmission fiber and the light power fluctuation is eliminated. This dual FBG differential algorithm greatly improves the measurement accuracy of the OFRB sensor and the applicability of multiple scenarios.

740

T. Li et al.

Fig. 1. Working principle of the OFRB sensor. a) The relationship between the bending radius of the optical fiber and the light intensity loss coefficient. b) The principle of distributed sensing based on light intensity loss.

2.2 Structural Design and Manufacturing The detailed design of the proposed sensor is illustrated in Fig. 2a. The optical fiber of the bending section is embedded in an elastic waistband by sewing, and both ends of the optical fiber are connected to an FBG (Sichuan Mianyang Bokai Photoelectric Technology Co, Ltd., china, Reflectivity ≥ 90%). The length of the gate region of FBG1 and FBG2 are both 5 mm, and the center wavelength is 1540 nm and 1545 nm, respectively. The optical fiber at each end and FBGs are protected by the protective sleeves which are fixed on the waist belt with glue. This embedded structure is comfort and convenience for users, meanwhile, it is suitable for people of different figures and ages. As shown in Fig. 2b, the size of the light intensity loss unit of the OFRB sensor is 85 mm * 37 mm, the initial bending radius of the optical fiber is 7.5 mm and the bending radius of the optical fiber in the transition section is 17.4 mm. According to Fig. 1a, the transition radius is so large that the light intensity loss can be neglected.

Ultra-sensitive and Stretchable Optical Fiber

741

Fig. 2. Structure and size design of OFRB sensor. a) The structure of OFRB sensor. b) Schematic representation of the nominal geometric characteristics of OFRB sensor.

The detailed assembly procedure of OFBR sensor has been illustrated in Fig. 3. 1) The triangle ruler, compass and colored pen were used to draw the designed optical fiber layout pattern on the elastic waistband. 2) The quartz fiber is embedded in the elastic waistband along the lines by sewing. In addition, the sewing point is selected at the tangent position of the C arc and the D arc (C arc and the D arc are marked in Fig. 2b) to make the OFRB sensor obtain a higher tensile performance. 3) Two Fiber Bragg gratings (FBG1 and FGB2) with different center wavelengths are welded to both ends of the optical fiber with a fiber welding machine. 4) Two protective sleeves are set at both ends of the circular-shaped fiber to protect the FBG and prevent the FBG from being affected by external mechanical stimuli. Both ends of the protective sleeves are glued to the elastic waistband to prevent the protective sleeves and the optical fiber from slipping.

742

T. Li et al.

Fig. 3. Flow chart of OFRB sensor preparation.

2.3 Calibration and Performance Characterization of the OFRB Sensor In order to verify the sensing characteristics of the OFRB sensor, the response of the OFRB sensor to tensile strain (ε) was experimentally assessed. The maximum effective tensile strain of OFRB sensor is calculated to prove its excellent tensile performance. The OFRB sensor was calibrated by performing tensile experiment using the ball screw slider driven by stepper motor (Beijing Times Chaoqun Electrical Technology Co., Ltd. CBX1024-400). The static characteristics of the OFRB sensor are evaluated by placing it on a fixture on the ball screw slider at room temperature (Fig. 4a). The output spectrum of the sensor was recorded every 5mm in the tensile range of 0–35 mm (ε = 0–41.5%). In this way, a total of 6 reciprocating tensile experiments were carried out. Sampling of spectral data is completed by FBG demodulator (Gaussian optics Co. Ltd., China. type: OPM-T1620; sampling rate: 100 Hz; resolution: 1 pm).

Fig. 4. Calibration experiment of OFRB sensor. a) Experimental device. b) The experimental curves.

The calibration curve was obtained by processing the collected data by using a custom algorithm in MATLAB 2018a. As shown in Fig. 4b, there is a monotonically increasing non-linear relationship between the tensile strain and the light intensity differential value

Ultra-sensitive and Stretchable Optical Fiber

743

of the two FBGs, and this non-linearity is determined by the optical fiber loss principle [17]. The sensitivity of OFRB sensor is up to 0.41 dB/mm, and the repeatability error is less than 15.53%.

3 Application and Feasibility Evaluation of Respiratory Status Monitoring Based on OFRB Sensor 3.1 Respiratory Status Monitoring in Different Static Postures and Motion States Next, as a proof-of-concept, the OFRB sensor has been strapped to the volunteer’s abdomen to monitor his respiratory status, as shown in Fig. 5. The sensitivity to tensile strain was evaluated considering that diaphragm contracts and the abdomen expand to drive the OFRB sensor to tension when the volunteer inhales, and the diaphragm expands and the contraction of the abdomen drives the OFRB sensor to shrinking when the volunteer exhales. The output signal of the sensor produces a complete peak value after this process, which recorded as one breath. The 9-min respiratory curve of the volunteer was recorded, and a peak extraction algorithm has been adopted to obtain the respiratory rate of the volunteer, as shown in Fig. 5. It is worth noting that the volunteers were in a state of obvious tachypnea (Respiratory > 20 breaths/min) in the first three minutes due to the vigorous exercise before the experiment, which gradually returned to normal in the next three minutes. In the last three minutes of the experiment, the volunteers took a breath-holding test to simulate the state of bradypnea. It can be seen that the output signal of the OFRB sensor is in good agreement with the actual respiratory state.

Fig. 5. Respiratory rate monitoring and change curve in static posture.

To verify the actual monitoring performance of the OFRB sensor when the human body is in different static postures and physical activity states, the following trials were tested during three protocols. 1st . Standing for 2 min and sitting still for 2 min under normal breathing. The volunteer’s breathing was relatively stable and his respiratory intensity was basically the same when standing and sitting (Fig. 6a (left)). The respiratory rate of the volunteer in the sitting state is slightly lower than that in the standing state, but it is still within the normal range (Fig. 6a (right)).

744

T. Li et al.

2nd . Standing for 1 min and 3 min of self-paced stepping. The 1-min standing breath test provides data comparison for the next experiment. It can be seen that with the volunteer’s exercise, his respiratory intensity increased significantly compared with that in a static posture (Fig. 6b (left)), and his respiratory rate increased significantly to 13–15 breaths/min (Fig. 6b (right)). 3rd . Standing for 1 min and 3 min of working (including carrying weight and walking). Due to the strenuous exercise of the whole body caused by work, the oxygen demand of the human body increases, which causes the respiratory intensity and depth of breathing increase sharply. In addition, the volunteer’s respiratory rate has also increased significantly. At the last minute, the respiratory rate has reached to 20 breaths/min, which is in a state of tachypnea (Fig. 6c).

Fig. 6. Respiratory status monitoring at different static postures and physical activity. a) Standing for 2 min, sitting for 2 min. b) Standing for 1 min, stepping for 3 min. c) Standing for 1 min, working for 3 min.

3.2 Respiratory Monitoring and Feasibility Evaluation for People of Different Ages To further evaluate the universal applicability of OFRB sensor to people of different age groups, the respiratory conditions of two seniors aged 73 and 81 in the nursing home were measured after the subjects’ informed consent. For comparison, two youth aged 21 and 23 were searched to perform respiratory monitoring for the same length of time. The OFRB sensor was installed in the same position on the subject’s abdomen during

Ultra-sensitive and Stretchable Optical Fiber

745

the monitoring process. Using LabVIEW low-pass filter to filter the measured data, and the result is shown in Fig. 7a. It can be clearly seen that the respiratory intensity of the seniors in the same environment is much lower than that of the youth, but the respiratory rate is higher than that of the youth. This is due to the aging of various functions of the body of the elderly, which in turn leads to the degeneration of the elastic fiber tissue of the lungs and reduces the contractile force. The decline of the thoracic cage and respiratory muscles leads to the decline of the defense function of the respiratory tract, which in turn reflects the decline of the respiratory tract resistance of the elderly. The respiratory rate monitoring system based on OFRB sensors can monitor the respiratory status of different age groups in real time, and then evaluate their health status. It can be seen from Fig. 6a that the respiratory situation of a person may be slightly different under different static postures. Therefore, we further measured the breathing curves of two seniors sitting and lying on the bed, as shown in Fig. 7b. It can be seen that the respiratory situation of the seniors can be well displayed by the output curve of the OFRB sensor. The respiratory rate of the senior in the lying state is the same as that in the sitting state, but the respiratory intensity is significantly weakened. The results show that OFRB sensors can further evaluate people’s motion status and static posture by monitoring their respiratory status. Thus, the respiratory rate monitoring system based on OFRB sensors as a novel way can effectively monitor the respiratory rate and respiratory intensity of various groups of people, and provide data support for doctors to evaluate people’s health status and disease diagnosis.

Fig. 7. Evaluation of the adaptability of OFRB sensors for people of different ages. a) Respiratory curve of youth and seniors. b) Respiratory assessment of different static postures in the elderly.

4 Discussions and Conclusion In this research, we developed a highly sensitive and stretchable optical fiber respiratory belt sensor. The two FBGs and the connected circular-shaped fiber are embedded in the elastic waistband by sewing. This structural design makes the OFRB sensor highly stretchable, which greatly solves the defect of insufficient ductility when the optical fiber is used in wearable devices. Through calibration experiments, a mapping model for differential signals of tensile strain and light intensity is established. The sensitivity of the OFRB sensor is calculated to be 0.41 dB/mm, and the repeatability error is 15.53%. Different from other light-intensity optical fiber sensors [16], OFRB sensor adopts dual

746

T. Li et al.

FBG light intensity differential algorithm in signal extraction and processing, which can basically eliminate the influence of light power fluctuation to improve the adaptability of OFRB sensors to different working environments. The respiratory status of people of different age groups in different static postures and exercise states has been monitored and the actual application effect of OFRB sensors has been evaluated in the healthcare environment. Therefore, the OFRB sensor can be used for high-comfort, non-invasive real-time monitoring of human respiratory rate with its excellent respiratory monitoring ability and universal applicability to people of all ages, and it can be further used by doctors to evaluate the patient’s health status and disease recovery. Although the structure design and preparation method of the OFRB sensor provided in this article greatly improves the tensile strain of the silica optical fiber sensor, the sewing thread will hinder the sensor from continuing to tension after reaches the maximum tensile strain. We believe that improving the structural design and preparation of the sensor can further improve the tensile strain and sensitivity of the OFRB sensor. In future work, the ultra-sensitivity OFRB sensor can be further applied to the measurement of human heart rate, and a multi-sign distributed monitoring system based on the OFRB sensor will be further constructed. Acknowledgment. This work was supported by the National Natural Science Foundation of China under Grant 51905398.

References 1. Kundu, S.K., et al.: A wearable capacitive sensor for monitoring human respiratory rate. Jpn. J. Appl. Phys. 52, 04CL05 (2013) 2. Wang, B.A., et al.: Flexible and stretchable metal oxide nanofiber networks for multimodal and monolithically integrated wearable electronics. Nat. Commun. 11(1), 2405 (2020) 3. Thiyagarajan, K., et al.: Flexible, highly sensitive paper-based screen printed MWCNT/PDMS composite breath sensor for human respiration monitoring. IEEE Sens. J. 99, 1 (2020) 4. Maji, D., et al.: Simulation and feasibility study of flow sensor on flexible polymer for healthcare application. IEEE Trans. Biomed. Eng. 60(12), 3298–3305 (2013) 5. Wang, Z., et al.: High sensitivity, wearable, piezoresistive pressure sensors based on irregular microhump structures and its applications in body motion sensing. Small 12, 3827–3836 (2016) 6. Ghosh, S.K., et al.: Synergistically enhanced piezoelectric output in highly aligned 1D polymer nanofibers integrated all-fiber nanogenerator for wearable nano-tactile sensor. Nano Energy 53, 245–257 (2018) 7. Kou, H., et al.: Wireless wide-range pressure sensor based on graphene/PDMS sponge for tactile monitoring. Sci. Rep. 9(1), 3916 (2019) 8. Guo, J., et al.: Wearable and skin-mountable fiber-optic strain sensors interrogated by a freerunning. Adv. Opt. Mater. 7(12), 1900086 (2019) 9. Lo, P.D., et al.: Fiber bragg gratings for medical applications and future challenges: a review. IEEE Access 99(1-1), 156863–156888 (2020) 10. Lo, P.D., et al.: A multi-parametric wearable system to monitor neck movements and respiratory frequency of computer workers. Sensors 20(2), 536, (2020) 11. Tocco, J.D., et al.: A wearable system based on flexible sensors for unobtrusive respiratory monitoring in occupational settings. IEEE Sens. J. 1 (2020)

Ultra-sensitive and Stretchable Optical Fiber

747

12. Aizhan, I., et al.: Fiber-Optic based smart textiles for real-time monitoring of breathing rate. Sensors 20(12), 3408 (2020) 13. Quandt, B.M., et al.: Body-Monitoring and health supervision by means of optical fiber-based sensing systems in medical textiles. Adv. Healthcare Mater. 4(3), 330–355 (2015) 14. Da, S., et al.: Optical high-voltage sensor based on fiber bragg grating and PZT piezoelectric ceramics. IEEE Trans. Instrum. Meas 60(6), 2118–2125 (2011) 15. Zhang, L., et al.: Ultrasensitive skin-like wearable optical sensors based on glass micro/nanofibers. Opto Electron. Adv. 003(003), 21–27 (2020) 16. Zhao, H., et al.: Optoelectronically innervated soft prosthetic hand via stretchable optical waveguides. Sci. Robot. 1(1), 10 (2016) 17. Marcuse, D., et al.: Bend loss of slab and fiber modes computed with diffraction theory. IEEE J. Quantum Electron 29(12), 2957–2961 (1993)

Filter Pruning Using Expectation Value of Feature Map’s Summation Hai Wu1(B) , Chuanbin Liu1 , Fanchao Lin1 , and Yizhi Liu2 1

2

Department of Information Science and Technology, University of Science and Technology of China, Hefei, China {wuh,lcb592,lfc1995}@mail.ustc.edu.cn Department of Computer Science and Engineering, Hunan University of Science and Technology, Xiangtan, China yizhi [email protected]

Abstract. With the widespread application of artificial intelligence, many scenarios require fewer parameters network. Mobile devices require networks with faster inference speed. However, current networks are becoming more and more complex. Network pruning is an important way to solve this contradiction. Current data dependent network pruning methods use a large amount of data to guide the pruning. Such a pruning strategy is undoubtedly time-consuming and computing resourceconsuming. In order to guide pruning process with less computing resource and time, this paper proposes a new pruning method. The proposed method comes from a phenomenon observed in experiments: the expectation value of feature map’s summation of a certain filter is relatively stable, and the number of input images has little effect on this value. Therefore, we use a small amount of images to abtain and sort these values to guide pruning. We prune filters corresponding to smaller expectation value according to a certain pruning rate. Using the proposed method, we perform experiments on public datasets. Experiments show that our method can reach Top-1 accuracy of 93.72% on CIFAR-10 on VGG-16-BN under 83.3% compression ratio of parameters. In order to verify the proposed pruning strategy on medical image analysis, we compare the accuracy of landmarks detection on the DDH (Developmental Dysplaisa of the Hips) dataset. The experimental results show that after network pruning, the landmark detection accuracy can also be well maintained.

Keywords: Deep learning feature map’s summation

1

· Filter pruning · Expectation value of

Introduction

Deep learning has made great progress in the field of computer vision and natural language processing [1,14–17]. Networks gradually become deeper and more complicated, in spite of bringing about improvement of accuracy. However, the c Springer Nature Switzerland AG 2021  X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 748–755, 2021. https://doi.org/10.1007/978-3-030-89092-6_68

Filter Pruning Using Expectation Value of Feature Map’s Summation

749

increase in network parameters makes it difficult for a trained model to run and deploy on mobile terminals. At the same time, the network’s inference speed slows down. Networks training and inference also consume time and resources. Therefore, network pruning technology was born. Works of network pruning can be divided in many ways. On one hand, network pruning can be divided into weight-based pruning and filter-based pruning. On the other hand, according to whether pruning is based on train data, it can be divided into data-dependent and data-independent methods. The pruning method proposed in this paper belongs to filter pruning and data-dependent pruning. Our method to find filters that need to be pruned only uses a small amount of images. Inspired by [10], we observe a phenomenon in experiments: the Expectation Value of feature map’s Summation (EVS) of a certain filter is relatively stable, regardless of the number of input images. This phenomenon exists in both natural images and medical images. Therefore, our method uses a small amount of images to get these EVS corresponding to filters in every convolutional layer. We sort these EVS and prune filters which correspond to smaller EVS according to a certain pruning rate. Using our method can save computing resource and time to find filters which need to be tailored. Based on our proposed method, we conduct lots of experiments on two dataset CIFAR-10 and DDH (Developmental Dysplaisa of the Hips). The experimental results show that our pruning scheme is very competitive.

2

Related Work

Weight-based [2–4] and filter-based [5,8,10,11] pruning methods have made great progress. Data-dependent [10,13] and data-independent [5,8] methods also receive a lot of attention. 2.1

Pruning Weight or Filter

Weight-based pruning is mainly to remove certain weight values, and such pruning is unstructured. For example, [4] uses threshold to remove small weights. Pruning based filter is to remove certain filters. Filter-based methods are more commonly used, because the pruned model is still structured. [8] proposes a method to discard small L1-Norm filter. [5] uses geometric median to prune filters. 2.2

Data Dependent or Not

Data dependence methods are mainly based on the train data to determine which weight values or filters need to be discarded. [10] uses 500 images and adopts rank of feature map to find filters which need to be pruned. But finding the maximum rank operation is relatively time-consuming, and the gap between different ranks of different channels is relatively small as the feature map decreases. Methods of data independence mainly focus on weights or filters themself, and do not need output of networks. For example, [5] only uses filters’ mathematical characteristics to prune filters.

750

3

H. Wu et al.

Methodology

Putting the image I into the trained model before pruning, and we can obtain the feature map M from the model. We can use M to calculate Expectation Value of feature map’s Summation (EVS), and then we sort these EVS in descending order. We discard filters corresponding to small EVS according to a certain pruning ratio. The training process after pruning is same to Hrankplus [9]. The Fig. 1 shows our pruning process.

Fig. 1. The process of our method for network pruning. Left side of the figure is legend. We use the feature maps of one convolutional layer to obtain EVS, and sort these EVS. Filters can be pruned according to their corresponding EVS for small to large under a certain pruning rate p. Here k represents image.

3.1

Preliminaries

Below are a series of parameters used to describe our pruning method. We assume that the network has L layers. We use Ni and Ni+1 to represent the input and output of the i-th convolutional layer. At the same time, we use Fi,j to represent the j-th filter of the i-th layer. We use Mi,j to represent the feature map generated by Fi,j . The EVS can be represented by Ei,j , and Ei,j could be expressed as: Ei,j

k=b×s  1 = sum(Mi,j,k ). b×s

(1)

k=1

Here b represents batchsize, and s represents s times of batchsize. Namely, b × s images participate in the coaching process of pruning. k represents image.

Filter Pruning Using Expectation Value of Feature Map’s Summation

3.2

751

Analysis of EVS

After performing a lot of experiments, we notice that the EVS corresponding to a certain filter is relatively stable, and these EVS corresponding to different filters are sometimes very different. Namely, Ei,j is not sensitive to the number of input images b × s. That is to say, the Ei,j obtained with the entire test dataset is not much different from the Ei,j obtained with part of images. Based on this regular pattern, we can use a small amount of images to guide network pruning. It needs to be pointed out that because we use less data, and the calculation resource and time consumed on summation operation are relatively small. And summation operation compared to rank operation [9] are relatively faster. 3.3

Filter Pruning via EVS

We use a small amount of images to get the Ei,j corresponding to different filters. Then we sort Ei,j from small to large. Assuming that p is a given pruning rate, then filters will be cropped according to the value of Ei,j from small to large in one convolutional layer. Using the proposed method to obtain pruned network, then we still need train and fine-tune it and the process is same to Hrankplus [9].

4

Experiments

In order to verify our proposed network pruning method, we conduct lots of experiments on public dataset CIFAR-10 [7]. We prune VGG-16-BN networks. Developmental Dysplaisa of the Hips (DDH) is a common disease in children’s orthopedics. In order to verify the proposed pruning strategy on medical image analysis, we compare the accuracy of landmarks detection on the DDH dataset. The DDH dataset is collected in the clinical practice in children hospital. The DDH X-ray images are collected from PACS system of children’s hospital. We converted the original DICOM format files into JPEG format images to facilitate subsequent processing. All coordinates of landmarks of X-ray images are labeled by professional doctors. The used number of DDH images in this study is 1824, in which 1400 images are used for training and the rest 424 images are for testing. 4.1

Experimental Settings

When performing network pruning experiments on the CIFAR-10 and DDH datasets, our device is an Ubuntu server equipped with a GeForce GTX 1080 Ti graphics card, and the CPU of this server is Intel i7-9700K. To find which filters need to be pruned, we use different numbers of images for different network architectures. On CIFAR-10, we set the batchsize to 256. In order to prune the VGG-16-BN, we use 50 ×b images. When the pruned model is fine-tuned and trained, the batchsizes used in CIFAR-10 is 256. At the same time, when in process of fine-tuning, CIFAR-10 is trained for 300 epochs. For the fine-tuning and training process after network pruning, our process is same to Hrankplus [9],

752

H. Wu et al.

and the parameter statistics and FLOPs statistical methods are also same to [10]. When performing network pruning experiments on the DDH datasets, we use 100 images to guide network pruning. 4.2

Filter Pruning on CIFAR-10

We have carried out pruning and fine-tuning on VGG-16-BN. Table 1 shows the results. In order to compare the effects of our pruning method with Hrankplus [9], we set multiple pruning rates that are same to [9]. From Table 1, we can see that our method can still maintain high classification accuracy under a relatively large pruning rate. For VGG-16-BN in Table 1, when the parameter is compressed by 87.3%, and FLOPs is compressed by 78.6%, our method can achieve an accuracy of 93.19%. Table 1. Pruning results of VGG-16-BN on CIFAR-10. M ethods

4.3

Top-1% FLOPs

Parameters

VGGNet [12] 93.96

313.73M (0.0%)

14.98M (0.0%)

L1 [8]

93.40

206.00M (34.3%) 5.40M (64.0%)

SSS [6]

93.02

183.13M (41.6%) 3.93M (73.8%)

Hrank [10]

93.43

145.61M (53.5%) 2.51M (82.9%)

Hrank [10]

93.34

108.61M (65.3%) 2.64M (82.1%)

Hrank [10]

91.23

73.70M (76.5%)

1.78M (92.0%)

Hrankplus [9] 93.73

131.17M (58.1%) 2.76M (81.6%)

Hrankplus [9] 93.56

104.78M (66.6%) 2.50M (83.3%)

Hrankplus [9] 93.10

66.95M (78.6%)

EVS

93.84

131.17M (58.1%) 2.76M (81.6%)

1.90M(87.3%)

EVS

93.72

104.78M (66.6%) 2.50M (83.3%)

EVS

93.19

66.95M (78.6%)

1.90M (87.3%)

Filter Pruning on DDH Dataset

We prune ResNet-18 and ResNet-34 on DDH dataset. Table 2 and 3 show the results respectively. On the DDH dataset, for ResNet-18 and ResNet-34, we only prune the 7 filters in the first layer of these networks, and the pruned model is not fine-tuned. The pruning method is setting parameters of these filters to zero. At the same time, different normalization operations are used in training process of these networks. For these two networks, we prune filters based on the absolute value of EVS.

Filter Pruning Using Expectation Value of Feature Map’s Summation

753

Table 2. Accuracy of landmark detection on DDH dataset of the ResNet-18 Error

lmk1

lmk2

lmk3

lmk4

lmk5

lmk6

Mean-be 5.93014211 7.11829570 3.56179151 3.91770331 3.74110503 3.70958868 SD-be

3.54768832 3.59000895 2.36005651 2.62760879 2.53407730 2.58673549

Mean-af 5.93014197 7.11829588 3.56179143 3.91770351 3.74110505 3.70958871 SD-af

3.54768818 3.59000948 2.36005639 2.62760898 2.53407775 2.58673575

Table 3. Accuracy of landmark detection on DDH dataset of the ResNet-34 Error

lmk1

lmk2

lmk3

lmk4

lmk5

lmk6

Mean-be 5.97357973 5.53814126 3.69364983 4.38908257 3.41307173 3.60117178 SD-be

3.57345660 3.16376954 2.51509773 2.67767509 2.42128800 2.79804960

Mean-af 5.97357953 5.53814111 3.69365004 4.38908324 3.41307140 3.60117190 SD-af

3.57345651 3.16376921 2.51509753 2.67767549 2.42128796 2.79804942

The DDH dataset is used to detect landmarks in medical images. The essence of the task is to locate 6 landmarks in each image. In our experiment, we use pixel error to represent the accuracy of landmark detection. The pixel error calculation formula is as follows:  2 2 (2) Error = (xi − xj ) + (yi − yj ) , where (xi , yi ) are landmark coordinates predicted by the network, and (xj , yj ) represents landmark coordinates marked by doctors. In Tables 2 and 3, lmk represents landmark, Mean-be represents the mean value of the pixel error before network pruning, and SD-be represents the standard deviation of the pixel error before network pruning. Mean-af and SD-af represent the statistical indicators of pixel error after network pruning. In these experiments, we can find that after filters pruning in the first layer of network, the accuracy of landmark detection is almost unchanged. In Tables 2 and 3, we can see that there is almost no loss of accuracy after network pruning. This also proves the feasibility of our network pruning algorithm.

5

Conclusion and Future Work

In this paper, in order to use little images to guide network pruning, we propose a method based on the expectation value of feature map’s summation. Our method is based on a phenomenon we observed in experiments, that is, the expectation value of feature map’s summation is not sensitive to the number of images. Since our method takes less computing resource and time to sum the output feature maps, our method can effectively save computing resources. We apply the proposed method on the CIFAR-10 to prune the commonly used VGG-16BN network. The experimental results show that our method could outperform

754

H. Wu et al.

or attain a similar performance compared with other state-of-the-art methods. Specifically, our method can reach Top-1 accuracy of 93.72% on CIFAR-10 on VGG-16-BN under 83.3% compression ratio of parameters. In order to verify the proposed pruning strategy on medical image analysis, we analyze the accuracy of landmarks detection before and after network pruning on the medical DDH dataset. The experimental results show that after network pruning, the landmark detection accuracy can also be well kept. In the next step, we will continue to perform a larger range pruning on the DDH dataset. Acknowledgments. This work are supported by the National Nature Science Foundation of China (61976008) and the Key Scientific Research Projects of the Department of Education of Hunan Province (19A172).

References 1. Feng, Y., et al.: Modeling fluency and faithfulness for diverse neural machine translation. In: Proceedings of the AAAI Conference on Artificial Intelligence, vol. 34, pp. 59–66 (2020) 2. Guo, Y., Yao, A., Chen, Y.: Dynamic network surgery for efficient DNNs. arXiv preprint arXiv:1608.04493 (2016) 3. Han, S., Mao, H., Dally, W.J.: Deep compression: compressing deep neural networks with pruning, trained quantization and Huffman coding. arXiv preprint arXiv:1510.00149 (2015) 4. Han, S., Pool, J., Tran, J., Dally, W.J.: Learning both weights and connections for efficient neural networks. arXiv preprint arXiv:1506.02626 (2015) 5. He, Y., Liu, P., Wang, Z., Hu, Z., Yang, Y.: Filter pruning via geometric median for deep convolutional neural networks acceleration. In: Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, pp. 4340–4349 (2019) 6. Huang, Z., Wang, N.: Data-driven sparse structure selection for deep neural networks. In: Proceedings of the European Conference on Computer Vision (ECCV), pp. 304–320 (2018) 7. Krizhevsky, A., Hinton, G., et al.: Learning multiple layers of features from tiny images (2009) 8. Li, H., Kadav, A., Durdanovic, I., Samet, H., Graf, H.P.: Pruning filters for efficient convnets. arXiv preprint arXiv:1608.08710 (2016) 9. Lin, M.: https://github.com/lmbxmu/hrankplus (2020) 10. Lin, M., et al.: Filter pruning using high-rank feature map. In: Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, pp. 1529– 1538 (2020) 11. Luo, J.H., Wu, J., Lin, W.: Thinet: a filter level pruning method for deep neural network compression. In: Proceedings of the IEEE International Conference on Computer Vision, pp. 5058–5066 (2017) 12. Simonyan, K., Zisserman, A.: Very deep convolutional networks for large-scale image recognition. arXiv preprint arXiv:1409.1556 (2014) 13. Suau, X., Zappella, L., Palakkode, V., Apostoloff, N.: Principal filter analysis for guided network compression. arXiv preprint arXiv:1807.10585 2 (2018) 14. Vaswani, A., et al.: Attention is all you need. arXiv preprint arXiv:1706.03762 (2017)

Filter Pruning Using Expectation Value of Feature Map’s Summation

755

15. Wang, M., Tighe, J., Modolo, D.: Combining detection and tracking for human pose estimation in videos. In: Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, pp. 11088–11096 (2020) 16. Wang, T., Yang, T., Danelljan, M., Khan, F.S., Zhang, X., Sun, J.: Learning human-object interaction detection using interaction points. In: Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, pp. 4116– 4125 (2020) 17. Wang, Y., Qian, S., Hu, J., Fang, Q., Xu, C.: Fake news detection via knowledgedriven multimodal graph convolutional networks. In: Proceedings of the 2020 International Conference on Multimedia Retrieval, pp. 540–547 (2020)

Muscle Tension Analysis Based on sEMG Signal with Wearable Pulse Diagnosis Device Xin Chang1 , Xinyi Li2 , Jian Li1(B) , Guihua Tian2(B) , Hongcai Shang2(B) , Jingbo Hu1 , Jiahao Shi1 , and Yue Lin1 1 Key Laboratory for Intelligent Control and Decision on Complex System,

School of Automation, Beijing Institute of Technology, Beijing, China 2 Laboratory of Chinese Internal Medicine of Ministry of Education and Beijing,

Dongzhimen Hospital, Beijing University of Chinese Medicine, Beijing, China

Abstract. Traditional Chinese medicine (TCM) is an empirical and subjective medical discipline. The rapid development of information technology requires the objectification of traditional Chinese medicine diagnosis and treatment. A wearable pulse diagnosis device, which aims at making pulse diagnosis objective, is designed. The device utilizes 7 kind of signals to characterize human health level. This paper especially focuses on the application of sEMG signal in the analysis of muscle tension and classifies muscle tension level using the algorithm of CART. In actual wearing cases, different wearing positions will induce to big difference of sEMG signal. In order to solve the problem above, random forest is applied in a position matching method based on prior calibration. The experimental results prove the effectiveness of the muscle tension level classification algorithm with an accuracy up to 81.4%. Keywords: Pulse diagnosis · Wearable device · sEMG · Random forest

1 Introduction Pulse diagnosis, as one of the four diagnostic methods of TCM, is the experience summary of TCM in long-term practice. The unique disease diagnosis and medical system of TCM have been inherited for thousands of years, in which the diagnosis has developed into an independent discipline. However, this kind of diagnosis method depends highly on the personal judgment of doctors. Due to the lack of unified standard, it is difficult to quantify the health status of patients and unconducive for the inheritance of TCM [1, 2]. With the in-depth development of computer science and statistics in the field of TCM, pulse diagnosis develops to be objective and expands the scope of coverage gradually. The objectification of pulse diagnosis can be expressed as collecting various body data information of patients, quantifying health status information, and then analyzing the information from a scientific point of view for classification and diagnosis [3]. TCM pays attention to “preventive treatment of disease”, that is, to prevent disease before it occurs. With the development of artificial intelligence technology in recent years, intelligent TCM has been becoming a major development direction in the field of © Springer Nature Switzerland AG 2021 X.-J. Liu et al. (Eds.): ICIRA 2021, LNAI 13016, pp. 756–766, 2021. https://doi.org/10.1007/978-3-030-89092-6_69

Muscle Tension Analysis Based on sEMG Signal

757

TCM. “Preventive treatment of disease” in the new situation means to obtain and evaluate various parameters of the patient’s body via intelligent device. According to the data obtained from the device, disease prevention could be realized with AI algorithm [4]. In TCM, there are four ways for disease diagnosis and disease prediction, including look, listen, question and pulse diagnosis. Pulse diagnosis, including touch the human skin, feel the changes of human muscle tension, detect the pulse beating law and observe the skin color of patients, realizes some chronic diseases detection. Muscle tension is one of the most important indexes in TCM pulse diagnosis. Thus, the accuracy of muscle strength classification and muscle tension examination decide the result of pulse diagnosis. However, traditional muscle tension measurement device is relatively large, which is difficult to be wearable. The progress of technology makes more parameters in human health assessment available. The surface electromyography (sEMG) signal, which is gradually emerging in recent years, is widely accepted in the field of TCM [16]. The sEMG is an onedimensional time series signal obtained by guiding and amplifying the bioelectrical changes of neuromuscular system on the skin surface. It is different from the traditional EMG signal and has the advantage of non-invasive [5]. There are 2 types of sEMG sensors, one is attached to muscle directly with patch electrodes [17], the other one applies dry electrodes to skin surface [18]. Based on this characteristic, the sEMG signal has a good application prospect in the wearable equipment field, and can represent the muscle state information of human body in specified scenario However, position error is an obviously not to be neglected factor which will cause inaccuracy of the classification of muscle tension. sEMG signal is remarkably depend on the position of electrode in signal acquisition [19]. The position error is not avoidable, especially when taking on or off the wearable device. Hence, the actual data cannot be acquired by sEMGsensors.In order to obtain the expected data, an algorithm which can deal with the position error needs to be considered. The main contribution of the paper is listed as follows. Firstly, a wearable pulse diagnosis device is designed and the collection of human health information is accomplished. Secondly, takes the sEMG signal and skin image of human body into consideration to achieve the purpose of human health monitoring. Finally, cloud server is introduced as the background of data storage and processing. The paper is organized as follows: Sect. 2 analyzes the parameters the wearable device needs to collect and the functions the device should have, selects proper sensor chips, builds the data storage cloud platform, and writes the client to read the participants’ health data. Section 3 considers the effectiveness of sEMG signal in solving the problem of human muscle tension classification. Taking the limitation of sEMG signal into account, the filtering method of signal preprocessing is determined. Issues about the position error will also be discussed in this section. The description of the experiment will be detailed in Sect. 4, in which contains the experimental process and gives the experimental results to evidence the effectiveness of the classification algorithm and the position error elimination algorithm. Section 5 is the conclusion and future prospects.

758

X. Chang et al.

2 Overall Design Scheme The following work has been done to merge wearable device with the concept of intelligent TCM. First, data acquisition and processing technology are illustrated in detail. Secondly, collect participant’s health information. Subsequently, the cloud server is set up to receive data collected with 4G transmission technology. Finally, appropriate information processing methods are applied in data analysis and processing. 2.1 Requirement Analysis In order to acquire the necessary data, the wearable device is designed to obtain participant’s heart rate, temperature, humidity, sEMG signal. With reference to the wearable devices on the market, we add the blood oxygen information and the spatial information. Meanwhile, the device is innovatively equipped with a macro camera to get skin image information of participant. Furthermore, cloud sever is adopted to store and analyze data. 2.2 Overall Design of System Figure 1 shows an overview of Intelligent wearable devices system, including hardware part and software part.

Fig. 1. Overview of the wearable device

Hardware Design Main control system based on MTK2503 is built to control data collection and transmission. The acquisition of heart rate, temperature, humidity, sEMG, blood oxygen information, skin image information and spatial information is obtained through sensors integrated into the device. The hardware design meets the requirements of portable wearable devices with low power consumption. Meanwhile, the selection of chips ensures that functions are realized while portability is considered. SC7R30 chip was selected to collect heart rate and blood oxygen data according to photoplethysmography [6]. Considering the three elements equivalent model of human body shown in Fig. 2 [7]. The electrical impedance method is applied to decide whether the skin is dry or wet. The macro camera is utilized to collect the skin surface image, and the temperature sensor

Muscle Tension Analysis Based on sEMG Signal

759

is selected to obtain the body temperature information. The six-axis motion processing component mpu6050 is collects spatial information and the sEMG signal sensor is designed and manufactured to collect sEMG signal. The hardware design is shown in Fig. 2 and the device is shown in Fig. 3.

Fig. 2. The ternary equivalent model of human body. Due to the existence of equivalent capacitance, the electrical impedance method is effective only when the body is subject to an alternating current.

Fig. 3. Hardware design of wearable intelligent device based on MTK2503

Fig. 4. Wearable intelligent device (sEMG sensor, macro camera, IMU, SC7R30, temperature sensor and humidity sensor are marked)

Software Design The software is comprised of a front end and a back end. The back end is the server side, which is used for data storage and data analysis. The front end is used for users to query. The implementation is as follows. Firstly, the cloud server of participants’ personal health data is built to realize the storage of pulse diagnosis information. Then, artificial intelligence algorithm is set in the cloud platform to realize the preprocessing, feature extraction and classification of

760

X. Chang et al.

pulse diagnosis information, mainly sEMG and skin image information, which expands the function of wearable devices. Finally, a client software is developed to provide participants with access to their health information at any time. The message returned back from the query is the information processed by the artificial intelligence algorithm, thus the pulse diagnosis results can be displayed more intuitively.

3 Design of Muscle Tension Classification Algorithm Based on sEMG Signal 3.1 Characteristics of sEMG Signal The sEMG signal is a weak signal with peak-to-peak value of 0–6 mV, root mean square value of 0–1.5 mV, frequency of 0–500 Hz, and a dominant frequency of 50–50 Hz [8]. It is easy to be coupled into a variety of noises in the acquisition process, among which the main noise of the signal is mainly stem from 50 Hz power frequency interference. 3.2 SEMG Signal Acquisition and Noise Reduction Considering that sEMG signal acquisition requires materials with excellent electrical properties, silver core electrode is chosen as the surface dry electrode contact with skin. Through the signal amplification circuit and AD conversion circuit, the original digital signal acquisition is finally realized. In order to simplify the calculation, the frequency band with obvious characteristics of sEMG signals is applied for the research, and the sampling frequency is 200 Hz. The traditional methods of processing sEMG signals are wavelet filtering and empirical mode decomposition (EMD). The sEMG signals obtained by the above two algorithms show good non-stationary characteristics through practical verification [9, 10]. Considering that the purpose of the wearable device designed in this paper is to achieve the classification of muscle tension, it is not very necessary to get high precision in the filtering accuracy. In addition, the excessive amount of calculation is not conducive to the processing of real-time data of the server. The original sEMG signal data is filtered by the fourth-order Butterworth filter, and the sEMG signal frequency is limited within the effective range of 10 Hz–60 Hz. Afterwards a 50 Hz notching filter is used to remove the power frequency interference [11, 12]. 3.3 Study on the Relationship Between sEMG Signal and Muscle Tension SEMG signal is generated from the bioelectrical activity of spinal cord motor neurons under the motor control of cerebral cortex. It is generally believed that when the change of muscle tension leads to the change of muscle strength, the individual’s electrophysiological activities will change accordingly, and this change can be reflected from sEMG signal. The change of signal is related to the central control and the physiological process of muscle itself, which can be considered as an objective index of muscle strength evaluation. According to Ashworth scale [22], this paper divides dynamic muscle tone into five grades (1 and 1+ grades were combined into one grade). Features Extraction of sEMG Signal Usually, there are 4 kinds of feature extraction methods, including time domain, frequency domain, time-frequency domain and entropy [20]. The time domain features of

Muscle Tension Analysis Based on sEMG Signal

761

sEMG signal are enough for the task of muscle tension classification. Five time domain features are extracted to measure muscle tension level, the features include absolute mean, wave length, variance, root mean square and Wilson amplitude. Muscle Tension Classification Based on sEMG Cart decision tree is applied for muscle tension classification. Cart decision tree is used to predict the classification of discrete data [21]. Gini index is used to select the optimal feature and determine the optimal binary segmentation point of the feature. In the process of classification, suppose there are k classes and the probability that the sample points belong to the k-th class is pk , then the Gini index of the probability distribution is defined as: Gini(p) =

m 

pk (1 − pk ) = 1 −

K 

pk2 .

(1)

k=1

k=1

According to the definition of Gini index above, the Gini index of sample set D can be obtained, where Ck represents the subset of samples belonging to class k in data set D. Gini(D) = 1 −

 K   |Ck | 2 k=1

|D|

(2)

Then If data set D is segmented on a certain value a according to feature A, while D1 and D2 are obtained, the Gini coefficient of set D under feature A is as follows. Gini coefficient Gini (D) represents the uncertainty of set D, and Gini coefficient Gini (D, A) represents the uncertainty of set d after A = a partition. The larger the Gini index, the greater the uncertainty of the sample set. Gain_Gini(D, A) =

|D1 | |D1 | Gini(D1 ) + Gini(D2 ) |D| |D|

(3)

For attribute a, calculate the Gain_Gini after dividing the data set into two parts by different attribute values, select the minimum value as the optimal binary scheme of attribute A. Then, for the training set S, the optimal binary scheme of all attributes is calculated, and the minimum value is selected as the optimal binary scheme of the sample and training dataset S. After obtaining the optimal binary scheme, the Gini value of the generated two parts of data is calculated and divided to make the decision tree grow downward until all samples in the branch nodes of the decision tree belong to the same class or have the same attribute value. 3.4 Position Error Correction Based on sEMG Signal Different from other human health information collected by the device, sEMG signal is highly affected by the position change. Therefore, it is necessary to design a position error correction algorithm. This paper proposes a position recognition algorithm. Take

762

X. Chang et al.

Fig. 5. Eight calibration positions on forearm (P1 is the standard point)

8 equal points around the forearm as the standard candidate positions, then the signal measured in a period of time is taken and its features are extracted and classified. Compared with the five-classification task in Sect. 3.3, eight-classification taskof position correction is more difficult, two kinds of features are added in the feature extraction step: zero crossing times and adjusted absolute average [13]. These data are sent to compare with the calibration data, which was collected by the participant in advance, to determine the current position of the device using random forest algorithm [14]. Subsequently the server will tell the participant correct position, and the participant can correct the position error accordingly. Random forest is an algorithm that integrates multiple decision trees through the idea of ensemble learning and has had a good performance in sEMG classification [22]. This algorithm requires the participant to carry out the calibration data of 8 positions in relaxed state in advance, and also detect the wearing position in relaxed state, then the server will prompt the correct position according to the collected data.

4 Experimental Analysis According to the muscle tension classify algorithm proposed above, the muscle tension test experiment on human body is carried out. In this experiment, 20 volunteers are invited to carry out the muscle tension test. By wearing the device in the standard position on right arm, the muscle tension is measured with the hierarchical hydraulic arm strength device. The five levels of subjects’ exertion correspond to different situations of simulated different muscle tension. 4.1 Measurement of Muscle Tension Based on sEMG During the experiment, 20 volunteers adopt sitting posture. Wearing the device, they apply resistance of 0 kg, 15 kg, 30 kg, 45 kg and 60 kg in equal time to collect test data corresponding to different muscle tension. The collection process is shown in Fig. 6. The training set and test set are randomly divided according to the proportion of multiple groups of data, and the classification of five levels of muscle tension is preliminarily completed. Figure 7 shows different performance of sEMG signals under five different levels. As a result, this algorithm has an accuracy of 81.4%. The confusion matrix of muscle tension classification detection is shown in Fig. 8.

Muscle Tension Analysis Based on sEMG Signal

763

Fig. 6. sEMG signal acquisition.

Fig. 7. Different sEMG signal of different muscle tension (signal has a resolution of 256)

Fig. 8. Result of muscle tension classification algorithm.

764

X. Chang et al.

4.2 Position Error Correction of sEMG Signal For different wearing positions, 10 volunteers wear the device at 8 different positions on forearm with arm relaxed posture under the experimental conditions in Sect. 4.1, then the calibration data are collected. The sEMG signals of eight positions from one volunteer under the same muscle tension are shown in Fig. 9, as the participant changes the wearing position manually, the server returns the corresponding wearing position, It can be seen that different feedback information can be given according to different wearing positions, the experimental results are shown in Fig. 10. The position correction algorithm has an accuracy of 63.2% approximately, the main errors are concentrated near the actual wearing position. As the muscle on human arm is very close, their features are relatively similar with great difficulty to distinguish.

Fig. 9. sEMG signals of eight positions under the same muscle tension.

Fig. 10. Result of position correction algorithm.

Muscle Tension Analysis Based on sEMG Signal

765

5 Conclusion and Future Work In this paper, a wearable pulse diagnosis device is designed, which is able to collect seven kinds of information, upload the collected information to the cloud server and return the health message to the client after artificial intelligence algorithm. This paper focuses on the application of sEMG signal in muscle tension discrimination with wearable device. The original sEMG signal is collected by contacting dry electrode with the participant’s forearm and then filtered by band-pass filter and notch filter, five time-domain features are extracted. The muscle tension level can be recognized with decision tree, and the accuracy can reach 81.4%. In addition, two more time-domain features are extracted for position error correction. The random forest algorithm is applied to accomplish the eight0classification task of position identification. The position accuracy rate is up to 63.2%. The reason for the relatively low position accuracy rate is the shortcome of single channel in sEMG information acquisition. The position error is eliminated through manual correction of the participant. Moreover, the complexity of position determination task is extremely high, and the sEMG signals between adjacent positions are very similar. In the future, we consider increasing the number of channels to improve the accuracy of position correction, adding restriction of multi electrodes sequence relationship. In addition, the research on error elimination of sEMG signal in this paper is not thorough enough, the problem of individual difference is not considered. In order to solve this problem, FRR normalization method will be introduced to eliminate individual differences in muscle tension classification in future research [15]. Acknowledgement. This paper is supported by the national key research and development plan “2018YFC1707600 wearable Chinese medicine health device research and development.”

References 1. Chen, W.: Development of pulse diagnosis instrument and finite element modeling analysis of pulse diagnosis. Modern Manuf. Technol. Equip. 57(03), 44–45+49 (2021) 2. Chao, W.: Study on the pulse condition of different states of deficiency and excess of gastrointestinal tract based on the objectification of pulse diagnosis of traditional Chinese Medicine. Beijing University of traditional Chinese Medicine (2020) 3. Xia-hui, Y.: The development of wearable automatic pressurizing pulse diagnosis bracelet. East China University of Science and Technology (2020). 4. Shu-jie, X., Zhao-yang, Y., Can-dong, L.: Analysis on the intelligent mode of disease’ in health management of TCM. Chin. J. Trad. Chin. Med. 34(11), 5007–5010 (2019) 5. Dao-fu, H.: Design of surface EMG signal acquisition system and research of movement pattern recognition methods. Tianjin Normal University of Engineering (2010) 6. Ling, F., Zhi-hui, Z., Rui, H.: Calibration technology of physiological parameters in green light photoplethysmography. Shanghai Meas. Test. 48(01), 44–46 (2021) 7. Huang, H.-b., Ren, C.-S.: Measurement of body composition by bioelectrical impedance analysis. Foreign medicine. Biomed. Eng. 3, 151–155 (2000) 8. Zhang, X.-D.: Research on detection and processing technology of EMG signal. biomed. Electr. Branch Chin. Soc. Eelectr. 4(2000) 9. Hu, W.: Design of low-cost sEMG acquisition circuit and research on its adaptive EEMD noise reduction. Mod. Electr. Tech. 44(09), 147–151 (2021)

766

X. Chang et al.

10. Huang, N.E., Shen, Z., Long, S.R., et al.: The empirical mode decomposition and the Hilbert spectrum for nonlinear and nonstationary time series analysis. In: Proceedings of the Royal Society of London A: Mathematical, Physical and Engineering Sciences, vol. 454, pp. 903– 995. The Royal Society 1998 (1971) 11. Liao, W.-j.: A prediction method of ankle joint angle based on sEMG. Sci. Technol. Innov. (02), 31–32 (2021) 12. Sun, Y., Xia, Y., Tang, W., Zhang, X., Song, H.: sEMG evaluating degree of traumatic injury in peripheral nerve of upper limbs. Foren. Sci. Technol. 46(02), 201–203(2021) 13. Zhang, Y., Chen, Y., Yu, H., Yang, X., Lu, W., Liu, H.: Wearing-independent hand gesture recognition method based on EMG armband. Person. Ubiquit. Comput. 22(3), 511–524 (2018) 14. Breiman, L.: Random forests. Mach. Learn. 45(1), 5–32 (2001) 15. Zhao, F.-d., Li, Y.-z.: Application of sEMG “flexion relaxation” phenomenon in the evaluation of muscle function in patients with low back pain. Mod. Pract. Med. 31(10), 1285–1289 (2019) 16. Merletti, R., Muceli, S.: Tutorial. Surface EMG detection in space and time: best practices. J. Electromyogr. Kinesiol. 49,102363 (2019). https://doi.org/10.1016/j.jelekin.2019.102363 17. Qin, P., Shi, X.: Evaluation of feature extraction and classification for lower limb motion based on sEMG signal. Entropy (Basel) 22(8), 852 (2020). https://doi.org/10.3390/e22080852 18. Castellini, C., Fiorilla, A.E., Sandini, G.: Multi-subject/daily-life activity EMG-based control of mechanical hands. J. Neuroeng. Rehabil. 17(6), 41 (2009). https://doi.org/10.1186/17430003-6-41 19. Ellis, M.D., Acosta, A.M., Yao, J., Dewald, J.P.: Position-dependent torque coupling and associated muscle activation in the hemiparetic upper extremity. Exp. Brain Res. 176(4), 594–602 (2007). https://doi.org/10.1007/s00221-006-0637-x 20. Breiman, L.I., Friedman, J.H., Olshen, R.A., et al.: Classification and regression trees. Encycl. Ecol. 57(3), 582–588 (2015) 21. Chu, Y., et al.: Immediate effect of local vibration therapy for sport-induced fatigue based on traditional Chinese medicine’s holistic theory. J. Multidiscip. Healthc. 18(13), 1993–2001 (2020). https://doi.org/10.2147/JMDH.S263491 22. Bohannon, R.W., Smith, M.B.: Interrater reliability of a modified Ashworth scale of muscle spasticity. Phys. Therapy 67(2), 206–207 (1987)

Author Index

Bai, Haoke 268 Bi, Shusheng 3 Bu, Wanghui 596 Cai, He 669 Cao, Shengge 169, 180 Chang, Qing 3 Chang, Xin 756 Chang, Zongyu 246 Chen, Fayin 737 Chen, Gang 360 Chen, Jiankun 147 Chen, Jing 596 Chen, Meng 474 Chen, Mingzhi 523 Chen, Silu 316, 714 Chen, Zhong 327 Cheng, Hui 737 Cheng, Weida 451 Chu, Zhenzhong 523 Cui, Xin 691, 702 Cui, Xinyu 338 Dai, Shi-Lu 606 Dang, Ruina 14, 226 Deng, ZhenBo 67 Deng, Zongquan 510 Du, Junjie 158 Du, Yu 714 Duan, Bosong 24

Han, Bin 279 He, Jun 498 He, Kai 147 He, Ying 302 He, Zhenya 327 Hong, Lin 115 Hu, Bingshan 396 Hu, Jingbo 756 Hu, Zhonghua 485 Hua, Minghui 576 Huang, Guojian 327 Huang, Hailin 451 Huang, Ji 115 Huang, Senwei 192 Huang, Yichuan 534 Huang, Ying 680 Huang, Yuping 669 Jia, Guanglu 451 Jia, Longfei 669 Jia, Qingxuan 360 Jia, Weihan 103 Jiang, Dexin 316 Jiang, Kanyang 659 Jiang, Lei 14, 226 Jiang, Shuiqing 440 Jiang, Zhenyu 702 Jin, Minghe 428 Jin, Xiaodong 236

Fang, Haitao 147 Fang, Yuanyang 638 Fang, Yuefa 236 Fu, Tianhua 669 Fu, Yingzhuo 360 Gao, Feng 498 Gao, Haibo 510 Gao, Han 302 Garba, Usman Haladu Gong, Jiabao 347 Gong, Kening 24

Gong, Zhao 649 Guan, Zhaocen 137 Guo, Chuangqiang 24 Guo, Kai 565 Guo, Kaida 215 Guo, Yaxing 669 Guo, Zhengya 204

137

Lee, ShiKeat 338 Lei, Pei 67 Li, Baotong 638 Li, Bing 451 Li, Feng 417

768

Author Index

Li, Guoxin 169, 180 Li, Hai 158 Li, Hui 56 Li, Jian 756 Li, Jiehao 534, 545 Li, Jinyi 246 Li, Junlan 137 Li, Ning 417 Li, Qingdang 554 Li, Ruiqin 56 Li, Shuai 659 Li, Sihang 596 Li, Tianliang 737 Li, Wanlei 576 Li, Xiaohu 638 Li, Xiaoxiao 659 Li, Xinyi 756 Li, Xuelong 226 Li, Yihong 34 Li, Zhiwen 587 Liang, Chengyuan 279 Lin, Fanchao 748 Lin, Yinfu 115 Lin, Yue 756 Liu, Chang 3 Liu, Chenglei 91, 103 Liu, Chuanbin 748 Liu, Dawei 79 Liu, Fusheng 226 Liu, Hanzhong 606 Liu, Hong 24, 371, 383, 428 Liu, Junnian 147 Liu, Juntao 34 Liu, Mingzhi 596 Liu, Qi 279 Liu, Qiang 428 Liu, Quan 649 Liu, Shengli 405 Liu, Xin 440 Liu, Xin-Jun 338, 649, 725 Liu, Xinxin 226 Liu, Yan 257 Liu, Yisha 714 Liu, Yizhi 748 Liu, Yufei 14 Liu, Zhaoyang 617 Liu, Zhen 510 Lou, Yunjiang 290, 576 Lu, Jiarui 79 Lu, Liang 279

Ma, Chao 302 Ma, Hailin 147 Ma, Lixin 257 Ma, Xiaolong 417 Mei, Fanghua 3 Meng, Qizhi 338 Meng, Xiangqiao 45 Ni, Wencheng 440, 463 Nie, Jinji 534 Nie, Zhenguo 338 Pan, Jie 169, 180 Pan, Yongping 587 Pei, Xu 169, 180 Peng, Fujun 451 Qi, Pengyuan

638

Shan, Yu 215 Shang, Hongcai 756 Shen, Kunfan 246 Shentu, Shuzhan 649 Shi, Jiahao 756 Sivalingam, Vinothkumar Song, Mingjing 327 Song, Yuyao 126 Su, Yifei 737 Su, Youge 596 Sun, Bing 45 Sun, Fuhai 545 Sun, Jiaze 498 Sun, Jie 565 Sun, Wei 268 Sun, Zhen 554 Tan, Yuegang 737 Tang, Huaiyan 327 Tang, Qiu 257 Tang, Wei 405 Tang, Xiaofan 279 Tang, Yichao 180 Tao, Yunfei 669 Tian, Guihua 756 Tian, Xincheng 257 Tian, Yingzhong 316 Vijayakumar, Sethu Wang, Bin 428 Wang, Cheng 137 Wang, Dan 302

396

565

Author Index Wang, Hao 691 Wang, Kang 440, 463 Wang, Ping 137 Wang, Wei 226 Wang, Xiaohui 91 Wang, Xin 115 Wang, Xingdong 268, 405 Wang, Yaobing 440, 463 Wang, Yifan 360 Wang, Yingchao 510 Wang, Yuan 56 Wang, Zhicong 498 Wang, ZhiQian 67 Wang, Zhirui 14 Wang, Zhongli 691, 702 Wei, Jun 103 Wen, Qingpeng 498 Wu, Aiguo 451 Wu, Hai 748 Wu, Jun 126 Wu, Ruihong 587 Xiao, Yihui 302 Xie, Fugui 338, 649, 725 Xing, Boyang 14 Xiong, Zhenhua 204, 680 Xu, Denan 279 Xu, Dongyang 215 Xu, Handing 338 Xu, Hui 34 Xu, Peng 14 Xu, Wenfu 34, 485 Xu, You 534 Xu, Zhihao 659 Yan, Lei 396 Yan, Mingzhong 617 Yan, Tong 14 Yang, Chenguang 606, 714 Yang, Di 725 Yang, Guilin 316, 714 Yang, Guocai 383 Yang, Shuai 91 Yang, Taiwei 485 Yang, Xiansheng 290 Yang, Xiaohang 371, 383 Yang, Xin 279 Yao, Xinyuan 268

Ye, Ruixing 545 Yin, Jie 474 Yu, Haibo 215 Yu, Haitao 510 Yu, Jingjun 169, 180 Yu, Xiaoyan 347 Yuan, Han 485 Zhang, Chi 316, 714 Zhang, Chongfeng 417 Zhang, Dawei 137 Zhang, Gan 115 Zhang, Huapeng 628 Zhang, Jiabo 405 Zhang, Jiahao 137 Zhang, Jianjun 91, 103 Zhang, Jinmin 290 Zhang, Mingyue 554 Zhang, Tao 474 Zhang, Wei 45 Zhang, Xianmin 158, 327 Zhang, Xiuli 192 Zhang, Yang 246 Zhang, Yun 463 Zhang, Zhihao 347 Zhang, Zhiyu 204 Zhao, Fuqun 236 Zhao, Hui 554 Zhao, Jie 316 Zhao, Jingdong 371, 383 Zhao, Wannan 215 Zhao, Yanzhi 215 Zhao, Zhijun 440, 463 Zhao, Zhiyuan 371, 383 Zheng, Chaochao 691, 702 Zheng, Tianjiang 316 Zheng, Zhongqiang 246 Zhong, Weijian 158 Zhou, Haixiang 576 Zhou, Qian 565 Zhou, Xuefeng 659 Zhou, Yan 680 Zhou, Zude 737 Zhu, Benliang 158 Zhu, Daqi 45, 523, 617, 628 Zhu, Yufan 714 Zou, Huaiwu 417, 485 Zou, Zhenyu 290

769