2.793

2018影响因子

(CJCR)

  • 中文核心
  • EI
  • 中国科技核心
  • Scopus
  • CSCD
  • 英国科学文摘

留言板

尊敬的读者、作者、审稿人, 关于本刊的投稿、审稿、编辑和出版的任何问题, 您可以本页添加留言。我们将尽快给您答复。谢谢您的支持!

姓名
邮箱
手机号码
标题
留言内容
验证码

保证智能轮椅平滑通过狭窄通道的路径曲率优化算法

陈灵 王森 胡豁生 麦当劳-麦尔·克劳斯 费敏锐

陈灵, 王森, 胡豁生, 麦当劳-麦尔·克劳斯, 费敏锐. 保证智能轮椅平滑通过狭窄通道的路径曲率优化算法. 自动化学报, 2016, 42(12): 1874-1885. doi: 10.16383/j.aas.2016.c160185
引用本文: 陈灵, 王森, 胡豁生, 麦当劳-麦尔·克劳斯, 费敏锐. 保证智能轮椅平滑通过狭窄通道的路径曲率优化算法. 自动化学报, 2016, 42(12): 1874-1885. doi: 10.16383/j.aas.2016.c160185
CHEN Ling, WANG Sen, HU Huo-Sheng, MCDONALD-MAIER Klaus, FEI Min-Rui. Novel Path Curvature Optimization Algorithm for Intelligent Wheelchair to Smoothly Pass a Narrow Space. ACTA AUTOMATICA SINICA, 2016, 42(12): 1874-1885. doi: 10.16383/j.aas.2016.c160185
Citation: CHEN Ling, WANG Sen, HU Huo-Sheng, MCDONALD-MAIER Klaus, FEI Min-Rui. Novel Path Curvature Optimization Algorithm for Intelligent Wheelchair to Smoothly Pass a Narrow Space. ACTA AUTOMATICA SINICA, 2016, 42(12): 1874-1885. doi: 10.16383/j.aas.2016.c160185

保证智能轮椅平滑通过狭窄通道的路径曲率优化算法


DOI: 10.16383/j.aas.2016.c160185
详细信息
    作者简介:

    王森 英国牛津大学计算机科学学院助理研究员.主要研究方向为机器人定位和多传感器融合.E-mail:sen.wang@cs.ox.ac.uk

    胡豁生 英国埃塞克斯大学计算机科学与电子系统学院教授, 机器人实验室主任.主要研究方向为自主机器人和网络化机器人.E-mail:hhu@essex.ac.uk

    麦当劳-麦尔·克劳斯 英国埃塞克斯大学计算机科学与电子系统学院教授.主要研究方向为嵌入式系统和机器人应用.E-mail:kdm@essex.ac.uk

    费敏锐  上海大学机电工程与自动化学院教授.主要研究方向为网络化控制系统, 智能机器人系统.E-mail:mrfei@stafi.shu.edu.cn

    通讯作者: 陈灵 上海大学机电工程与自动化学院助理研究员.主要研究方向为机器人定位与导航.本文通信作者.E-mail:lcheno@shu.edu.cn
  • 本文责任编委 程龙
  • 基金项目:

    上海市科委重点项目 14JC1402200

    上海市科委重点项目 15411953502

    上海市科委扬帆人才计划项目 16YF1403700

    上海高校青年教师培养资助计划 ZZSD15088

Novel Path Curvature Optimization Algorithm for Intelligent Wheelchair to Smoothly Pass a Narrow Space

More Information
    Author Bio:

    Assistant professor in the Department of Computer Science, University of Oxford, Oxford, UK. His research interest covers robot localization and multiple sensor fusion

    Professor at the School of Computer Science and Electronic Engineering, University of Essex, UK, leading robotics research. His research interest covers autonomous robots and networked robots

    Professor at the School of Computer Science and Electronic Engineering, University of Essex, UK. His research interest covers embedded systems and application of robotics

    Professor at the School of Mechatronics Engineering and Automation, Shanghai University. His research interest covers networked control system and intelligent robot system

    Corresponding author: CHEN Ling Assistant professor at the School of Mechatronics Engineering and Automation, Shanghai University. His research interest covers localization and navigation of robots. Corresponding author of this paper
  • Fund Project:

    Key Project of Science and Technology Commission of Shanghai Municipality 14JC1402200

    Key Project of Science and Technology Commission of Shanghai Municipality 15411953502

    Shanghai Sailing Program 16YF1403700

    Shanghai Colleges and Universities Young Teachers Training Funding Scheme ZZSD15088

  • 摘要: 为了使用户更感舒适,智能轮椅应该能在具有复杂几何约束的室内环境下平滑地通过狭窄通道.本文提出一个基于路径曲率优化的算法以确保智能轮椅平滑地通过狭窄过道.考虑到路径平滑度取决于路径曲率及其变化率,在通过传感器数据计算出狭窄通道相对于轮椅的位置后,算法以贝塞尔曲线的曲率及其变化率最小为优化目标,以轮椅过通道时的方向及贝塞尔多边形应为凸多边形作为约束,规划出一条平滑的最优路径,然后控制轮椅实时跟踪这条路径.上述过程动态循环运行,实现了智能轮椅平滑通过狭窄通道.仿真中将本文算法同基于A*的路径规划导航算法进行了对比,结果表明本文提出的基于曲率优化的算法可以实现比A*算法路径曲率更小且更加平滑的过狭窄通道过程,并且即使在没有全局地图和定位信息情况下,算法也能控制轮椅平滑地通过狭窄过道.实验中详细阐述了算法的实现过程,实验结果也证实了算法的有效性.
    本文责任编委 程龙
  • 图  1  轮椅在过狭窄门时的情景

    Fig.  1  Scenario of wheelchair in a narrow doorway

    图  2  轮椅过狭窄通道过程原理图

    Fig.  2  The schematic description of the narrow gap passing process

    图  3  轮椅过狭窄通道算法结构图

    Fig.  3  Strategy architecture of narrow gap passing

    图  4  位置误差的计算

    Fig.  4  The calculation of the position error

    图  5  轮椅的参考路径和实际路径

    Fig.  5  Reference path and actual path of wheelchair

    图  6  在场景$A$中每一条路径对应的轮椅的角速度和跟踪误差

    Fig.  6  The angular rate and the tracking errors for each path in Scenario $A$

    图  7  算法比较的仿真环境

    Fig.  7  Simulation environment for algorithm comparison

    图  8  基于本文方法和A*算法的轮椅路径

    Fig.  8  Actual trajectories generated by the proposed method and A* algorithm

    图  9  基于本文方法和A*算法的路径曲率的比较

    Fig.  9  The comparison of trajectory curvature generated by the proposed method and A* algorithm

    图  10  算法每次循环中各模块运行时间及总时间

    Fig.  10  Consuming time of each algorithm module and their total value for each iteration

    图  11  轮椅和激光的配置

    Fig.  11  Configuration of the wheelchair and the lasers

    图  12  实验场景

    Fig.  12  Experimental scenario

    图  13  一次激光扫描数据点及其分段

    Fig.  13  A full scan of laser data points and its expected segmentations

    图  14  两个激光测距仪相对于单个的优势

    Fig.  14  Advantage of two lasers compared with single laser

    图  15  轮椅轨迹

    Fig.  15  Trajectories of the wheelchair

    图  16  轮椅从不同起点到达终点位置时的实际位置

    Fig.  16  Actual wheelchair poses when arriving at the desired pose with different starting poses $A$, $B$, $C$ and $D$

    图  17  轮椅的角速度

    Fig.  17  Angular rate of the wheelchair

    图  18  轮椅从起点$A$和$D$出发过通道轨迹截图((a)~(f)对应于起点$A$, (g)~(l)对应于起点$D$)

    Fig.  18  Snapshots of the wheelchair$'$s trajectories when passing gap, starting from pose $A$ and $D$ ((a)~(f), (g)~(l) are with pose $A$ and $D$, respectively.)

    表  1  每条路径的输入参数

    Table  1  The input parameters for each path

    场景 Ps (m) Pd (m) Pt (m) 1 (Hs, Hd) 2 (Hs, Hd) 3 (Hd, Ht) 4 (Hd, Ht)
    A (0.1, 1.3) (1.6, 1.8) (0.0, 3.5) (-20, 90) (-40, 90) (90, 160) (90, 200)
    B (0.1, 1.3) (1.6, 1.8) (3.4, 3.5) (-10, 90) (10, 90) (90, 20) (90, -20)
    下载: 导出CSV
  • [1] Elbanhawi M, Simic M, Jazar R N. Continuous path smoothing for car-like robots using B-spline curves. Journal of Intelligent and Robotic Systems, 2015, 80(1):23-56 http://www.academia.edu/15534117/Continuous_Path_Smoothing_for_Car-Like_Robots_Using_B-Spline_Curves
    [2] Yoon S, Yoon S E, Lee U, Shim D H. Recursive path planning using reduced states for car-like vehicles on grid maps. IEEE Transactions on Intelligent Transportation Systems, 2015, 16(5):2797-2813 doi:  10.1109/TITS.2015.2422991
    [3] Pan Z, Li J Q, Hu K M, Zhu H. Intelligent vehicle path planning based on improved artificial potential field method. Applied Mechanics and Materials, 2014, 742:349-354 https://www.researchgate.net/publication/276868213_Intelligent_Vehicle_Path_Planning_Based_on_Improved_Artificial_Potential_Field_Method
    [4] 张浩杰, 龚建伟, 姜岩, 熊光明, 陈慧岩.基于变维度状态空间的增量启发式路径规划方法研究.自动化学报, 2013, 39(10):1602-1610 doi:  10.3724/SP.J.1004.2013.01602

    Zhang Hao-Jie, Gong Jian-Wei, Jiang Yan, Xiong Guang-Ming, Chen Hui-Yan. Research on incremental heuristic path planner with variable dimensional state space. Acta Automatica Sinica, 2013, 39(10):1602-1610 doi:  10.3724/SP.J.1004.2013.01602
    [5] 祖迪, 韩建达, 谈大龙.加速度空间中基于线性规划的移动机器人路径规划方法.自动化学报, 2007, 33(10):1036-1042 http://www.aas.net.cn/CN/abstract/abstract13407.shtml

    Zu Di, Han Jian-Da, Tan Da-Long. LP-based path planning method in acceleration space for mobile robot. Acta Automatica Sinica, 2007, 33(10):1036-1042 http://www.aas.net.cn/CN/abstract/abstract13407.shtml
    [6] Park J W, Im W S, Kim D Y, Kim J M. Safe driving algorithm of the electric wheelchair with model following control. In:Proceedings of the 16th European Conference on Power Electronics and Applications. Lappeenranta, Finland:IEEE, 2014. 1-10
    [7] Sinyukov D A, Padir T. Adaptive motion control for a differentially driven semi-autonomous wheelchair platform. In:Proceedings of the 2015 International Conference on Advanced Robotics. Istanbul, Turkey:IEEE, 2015. 288-294
    [8] Zhang Z Y, Zhao Z P. A multiple mobile robots path planning algorithm based on a-star and Dijkstra algorithm. International Journal of Smart Home, 2014, 8(3):75-86 doi:  10.14257/ijsh
    [9] Bhadoria A, Singh R K. Optimized angular a star algorithm for global path search based on neighbor node evaluation. International Journal of Intelligent Systems and Applications, 2014, 6(8):46-52 doi:  10.5815/ijisa
    [10] Seder M, Mostarac P, Petrović I. Hierarchical path planning of mobile robots in complex indoor environments. Transactions of the Institute of Measurement and Control, 2011, 33(3-4):332-358 doi:  10.1177/0142331208100107
    [11] BSI. Design of Buildings and Their Approaches to Meet the Needs of Disabled People. Code of Practice. Standard Number BS 8300:2009+A1:2010, ISBN 9780580707308, British Standards Institute, 2009.
    [12] Cheein F, De La Cruz C, Guimaraes E, Bastos-Filho T, Carelli R. Navigation system for UFES's robotic wheelchair. Devices for Mobility and Manipulation for People with Reduced Abilities. Boca Raton, FL:CRC Press, 2014. 41-93
    [13] Peula J M, Urdiales C, Herrero I, Fernandez-Carmona M, Sandoval F. Case-based reasoning emulation of persons for wheelchair navigation. Artificial Intelligence in Medicine, 2012, 56(2):109-121 doi:  10.1016/j.artmed.2012.08.007
    [14] Rastelli J R, Lattarulo R, Nashashibi F. Dynamic trajectory generation using continuous-curvature algorithms for door to door assistance vehicles. In:Proceedings of the 2014 IEEE Intelligent Vehicles Symposium. Dearborn, USA:IEEE, 2014. 510-515
    [15] Brezak M, Petrović I. Real-time approximation of clothoids with bounded error for path planning applications. IEEE Transactions on Robotics, 2014, 30(2):507-515 doi:  10.1109/TRO.2013.2283928
    [16] Chu C H, Hsieh H T, Lee C H, Yan C Y. Spline-constrained tool-path planning in five-axis flank machining of ruled surfaces. The International Journal of Advanced Manufacturing Technology, 2015, 80(9-12):2097-2104 doi:  10.1007/s00170-015-7201-4
    [17] Simba K R, Uchiyama N, Sano S. Real-time smooth trajectory generation for nonholonomic mobile robots using Bézier curves. Robotics and Computer-Integrated Manufacturing, 2016, 41(1):31-42 https://www.researchgate.net/publication/297587735_Real-time_smooth_trajectory_generation_for_nonholonomic_mobile_robots_using_Bezier_curves
    [18] Yang L, Song D L, Xiao J Z, Han J D, Yang L Y, Cao Y. Generation of dynamically feasible and collision free trajectory by applying six-order Bezier curve and local optimal reshaping. In:Proceedings of the 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Hamburg, Germany:IEEE, 2015. 643-648
    [19] Wu J, Snášel V. A Bézier curve-based approach for path planning in robot soccer. In:Proceedings of the 4th International Conference on Innovations in Bio-inspired Computing and Applications. Ostrava, Czech Republic:Springer International Publishing, 2014. 105-113
    [20] 陈成, 何玉庆, 卜春光, 韩建达.基于四阶贝塞尔曲线的无人车可行轨迹规划.自动化学报, 2015, 41(3):486-496 http://www.aas.net.cn/CN/abstract/abstract18627.shtml

    Chen Cheng, He Yu-Qing, Bu Chun-Guang, Han Jian-Da. Feasible trajectory generation for autonomous vehicles based on quartic Bézier curve. Acta Automatica Sinica, 2015, 41(3):486-496 http://www.aas.net.cn/CN/abstract/abstract18627.shtml
    [21] Choi J W, Curry R, Elkaim G. Piecewise Bezier curves path planning with continuous curvature constraint for autonomous driving. Machine Learning and Systems Engineering. Netherlands:Springer, 2010. 31-45
    [22] Hart P E, Nilsson N J, Raphael B. A formal basis for the heuristic determination of minimum cost paths. IEEE Transactions on Systems Science and Cybernetics, 1968, 4(2):100-107 doi:  10.1109/TSSC.1968.300136
    [23] Open Source Robotics Foundation (OSRF). Robot operating system[Online], available:http://www.ros.org, May 21, 2016
    [24] Open Source Robotics Foundation (OSRF). Gazebo[Online], available:http://www.gazebosim.org/, May 21, 2016
    [25] Open Source Robotics Foundation (OSRF). Global planner of navigation stack in ROS[Online], available:http://wiki.ros.org/global_planner?distro=indigo, May 21, 2016
    [26] Borges G A, Aldon M J. Line extraction in 2D range images for mobile robotics. Journal of Intelligent and Robotic Systems, 2004, 40(3):267-297 doi:  10.1023/B:JINT.0000038945.55712.65
    [27] Duda R O, Hart P E. Pattern Classification and Scene Analysis. New York:Wiley, 1973.
  • [1] 李军, 李韬. 基于贝叶斯序贯博弈模型的智能电网信息物理安全分析[J]. 自动化学报, 2019, 45(1): 98-109. doi: 10.16383/j.aas.2018.c180336
    [2] 丁进良, 杨翠娥, 陈远东, 柴天佑. 复杂工业过程智能优化决策系统的现状与展望[J]. 自动化学报, 2018, 44(11): 1931-1943. doi: 10.16383/j.aas.2018.c180550
    [3] 桂卫华, 岳伟超, 谢永芳, 张红亮, 阳春华. 铝电解生产智能优化制造研究综述[J]. 自动化学报, 2018, 44(11): 1957-1970. doi: 10.16383/j.aas.2018.c180198
    [4] 乔俊飞, 韩改堂, 周红标. 基于知识的污水生化处理过程智能优化方法[J]. 自动化学报, 2017, 43(6): 1038-1046. doi: 10.16383/j.aas.2017.c170088
    [5] 钱锋, 杜文莉, 钟伟民, 唐漾. 石油和化工行业智能优化制造若干问题及挑战[J]. 自动化学报, 2017, 43(6): 893-901. doi: 10.16383/j.aas.2017.c170129
    [6] 田梦楚, 薄煜明, 陈志敏, 吴盘龙, 赵高鹏. 萤火虫算法智能优化粒子滤波[J]. 自动化学报, 2016, 42(1): 89-97. doi: 10.16383/j.aas.2016.c150221
    [7] 陈世明, 化俞新, 祝振敏, 赖强. 邻域交互结构优化的多智能体快速蜂拥控制算法[J]. 自动化学报, 2015, 41(12): 2092-2099. doi: 10.16383/j.aas.2015.c150254
    [8] 陈成, 何玉庆, 卜春光, 韩建达. 基于四阶贝塞尔曲线的无人车可行轨迹规划[J]. 自动化学报, 2015, 41(3): 486-496. doi: 10.16383/j.aas.2015.c140295
    [9] 陆利正, 裘渔洋. 一对Bézier曲线基于控制顶点优化的显式G2约束拼接[J]. 自动化学报, 2014, 40(7): 1505-1508. doi: 10.3724/SP.J.1004.2014.01505
    [10] 赵洪伟, 谢永芳, 蒋朝辉, 徐德刚, 阳春华, 桂卫华. 基于泡沫图像特征的浮选槽液位智能优化设定方法[J]. 自动化学报, 2014, 40(6): 1086-1097. doi: 10.3724/SP.J.1004.2014.01086
    [11] 辛斌, 陈杰, 彭志红. 智能优化控制:概述与展望[J]. 自动化学报, 2013, 39(11): 1831-1848. doi: 10.3724/SP.J.1004.2013.01831
    [12] 朱明敏, 刘三阳, 汪春峰. 基于先验节点序学习贝叶斯网络结构的优化方法[J]. 自动化学报, 2011, 37(12): 1514-1519. doi: 10.3724/SP.J.1004.2011.01514
    [13] 郑钰辉, 张建伟, 陈允杰, 孙权森. 加权型曲率保持 PDE 图像滤波方法[J]. 自动化学报, 2011, 37(10): 1175-1182. doi: 10.3724/SP.J.1004.2011.01175
    [14] 邢立宁, 陈英武. 基于知识的智能优化引导方法研究进展[J]. 自动化学报, 2011, 37(11): 1285-1289. doi: 10.3724/SP.J.1004.2011.01285
    [15] 冀俊忠, 张鸿勋, 胡仁兵, 刘椿年. 一种基于独立性测试和蚁群优化的贝叶斯网学习算法[J]. 自动化学报, 2009, 35(3): 281-288. doi: 10.3724/SP.J.1004.2009.00281
    [16] 王春生, 吴敏, 曹卫华, 何勇. 铅锌烧结配料过程的智能集成建模与综合优化方法[J]. 自动化学报, 2009, 35(5): 605-612. doi: 10.3724/SP.J.1004.2009.00605
    [17] 柴天佑, 丁进良, 王宏, 苏春翌. 复杂工业过程运行的混合智能优化控制方法[J]. 自动化学报, 2008, 34(5): 505-515. doi: 10.3724/SP.J.1004.2008.00505
    [18] 王玉珠, 杨丹, 张小洪. 基于B样条尺度空间的鲁棒多尺度曲率积角点检测[J]. 自动化学报, 2007, 33(4): 414-417. doi: 10.1360/aas-007-0414
    [19] 严爱军, 柴天佑, 岳恒. 竖炉焙烧过程的多变量智能优化控制[J]. 自动化学报, 2006, 32(4): 636-640.
    [20] 傅洁, 吴立德. 图像序列的多通道围线结构匹配[J]. 自动化学报, 1993, 19(4): 434-443.
  • 加载中
图(18) / 表(1)
计量
  • 文章访问数:  978
  • HTML全文浏览量:  195
  • PDF下载量:  527
  • 被引次数: 0
出版历程
  • 收稿日期:  2016-03-01
  • 录用日期:  2016-07-11
  • 刊出日期:  2016-12-01

保证智能轮椅平滑通过狭窄通道的路径曲率优化算法

doi: 10.16383/j.aas.2016.c160185
    基金项目:

    上海市科委重点项目 14JC1402200

    上海市科委重点项目 15411953502

    上海市科委扬帆人才计划项目 16YF1403700

    上海高校青年教师培养资助计划 ZZSD15088

    作者简介:

    王森 英国牛津大学计算机科学学院助理研究员.主要研究方向为机器人定位和多传感器融合.E-mail:sen.wang@cs.ox.ac.uk

    胡豁生 英国埃塞克斯大学计算机科学与电子系统学院教授, 机器人实验室主任.主要研究方向为自主机器人和网络化机器人.E-mail:hhu@essex.ac.uk

    麦当劳-麦尔·克劳斯 英国埃塞克斯大学计算机科学与电子系统学院教授.主要研究方向为嵌入式系统和机器人应用.E-mail:kdm@essex.ac.uk

    费敏锐  上海大学机电工程与自动化学院教授.主要研究方向为网络化控制系统, 智能机器人系统.E-mail:mrfei@stafi.shu.edu.cn

    通讯作者: 陈灵 上海大学机电工程与自动化学院助理研究员.主要研究方向为机器人定位与导航.本文通信作者.E-mail:lcheno@shu.edu.cn
  • 本文责任编委 程龙

摘要: 为了使用户更感舒适,智能轮椅应该能在具有复杂几何约束的室内环境下平滑地通过狭窄通道.本文提出一个基于路径曲率优化的算法以确保智能轮椅平滑地通过狭窄过道.考虑到路径平滑度取决于路径曲率及其变化率,在通过传感器数据计算出狭窄通道相对于轮椅的位置后,算法以贝塞尔曲线的曲率及其变化率最小为优化目标,以轮椅过通道时的方向及贝塞尔多边形应为凸多边形作为约束,规划出一条平滑的最优路径,然后控制轮椅实时跟踪这条路径.上述过程动态循环运行,实现了智能轮椅平滑通过狭窄通道.仿真中将本文算法同基于A*的路径规划导航算法进行了对比,结果表明本文提出的基于曲率优化的算法可以实现比A*算法路径曲率更小且更加平滑的过狭窄通道过程,并且即使在没有全局地图和定位信息情况下,算法也能控制轮椅平滑地通过狭窄过道.实验中详细阐述了算法的实现过程,实验结果也证实了算法的有效性.

本文责任编委 程龙

English Abstract

陈灵, 王森, 胡豁生, 麦当劳-麦尔·克劳斯, 费敏锐. 保证智能轮椅平滑通过狭窄通道的路径曲率优化算法. 自动化学报, 2016, 42(12): 1874-1885. doi: 10.16383/j.aas.2016.c160185
引用本文: 陈灵, 王森, 胡豁生, 麦当劳-麦尔·克劳斯, 费敏锐. 保证智能轮椅平滑通过狭窄通道的路径曲率优化算法. 自动化学报, 2016, 42(12): 1874-1885. doi: 10.16383/j.aas.2016.c160185
CHEN Ling, WANG Sen, HU Huo-Sheng, MCDONALD-MAIER Klaus, FEI Min-Rui. Novel Path Curvature Optimization Algorithm for Intelligent Wheelchair to Smoothly Pass a Narrow Space. ACTA AUTOMATICA SINICA, 2016, 42(12): 1874-1885. doi: 10.16383/j.aas.2016.c160185
Citation: CHEN Ling, WANG Sen, HU Huo-Sheng, MCDONALD-MAIER Klaus, FEI Min-Rui. Novel Path Curvature Optimization Algorithm for Intelligent Wheelchair to Smoothly Pass a Narrow Space. ACTA AUTOMATICA SINICA, 2016, 42(12): 1874-1885. doi: 10.16383/j.aas.2016.c160185
  • 室内环境往往具有很多狭窄的过道, 这给轮椅的正常工作带来较大挑战.另一方面, 轮椅用户通常是行动不便者和老人, 他们不能很好地控制轮椅.因此, 一个巨大的需求在于智能轮椅能自动穿过较窄的过道, 以便轮椅用户能感到安全和舒适[1].

    智能轮椅过通道很大程度上依赖于其运动控制算法, 可以分为两个部分:路径规划[2-5]和运动导航[6-7].路径规划用于计算一条穿过狭窄通道的可行的路径而不碰撞障碍物.传统的路径规划算法, 如A*[8-9]和它的变体FHD*[10]运用优化的方法去找最短的避障路径.尽管这些算法已经成功用于通用移动机器人, 它们却不能直接用于智能轮椅上而规划一条穿过狭窄通道的平滑路径, 因为它们没有考虑环境的物理空间限制和用户舒适度.

    由于通道宽度与轮椅宽度的比值较小(如英国标准研究院[11]所规定的, 轮椅两边与通道墙之间只有300 mm. 图 1(a)展示了在作者实验室里的相同的情况, 留给轮椅通过过道的空隙两边共只有26 cm), 对轮椅来说很难使其在任意的航向角位置通过狭窄的门, 如图 1(b)所示, 只有当轮椅的航向角垂直于门线时轮椅才能通过.而对于这样的物理空间的约束以及如何在此约束中平滑地通过通道, 许多有关研究文献并没有考虑到.如Cheein等在文献[12]提出一种整合同时定位与地图构建算法的边界点方法用于轮椅通过过道.然而, 它需要连续地产生平均边界点, 从而使得轮椅的轨迹不平滑而且耗时. Peula等[13]提出基于案例推理的轮椅通过过道的方法, 同样地, 其也没有考虑规划路径的平滑性.

    图  1  轮椅在过狭窄门时的情景

    Figure 1.  Scenario of wheelchair in a narrow doorway

    路径的平滑性一定程度上决定了轮椅用户在轮椅过狭窄通道时的舒适性, 因此研究者提出了其他考虑路径平滑性的路径规划方法, 如基于圆弧[14-15]、样条[1, 16]和贝塞尔曲线[17-20]的方法.文献[1]提出基于B样条的方法, 让一台仿真轮椅在仿真环境中通过一个狭窄的通道, 而并没用真实轮椅进行测试.另外, 文献[17]提出基于贝塞尔曲线的路径规划方法, 用于一般的移动机器人, Choi等[21]也将类似方法用于自主驾驶汽车上, 其约束由航路点和过道宽度产生.同样地, 这些方法很难直接用在智能轮椅上.

    本文提出一个基于路径曲率优化的算法以确保智能轮椅平滑地通过狭窄过道.算法以三次贝塞尔曲线的曲率及其变化率最小为优化目标, 以通过传感器数据测得的过道相对于轮椅的位置、轮椅过通道时的方向及贝塞尔多边形应为凸多边形作为约束条件, 实时计算满足优化目标和约束的三次贝塞尔曲线以作为期望路径, 然后控制轮椅速度以跟踪此期望路径, 从而实现轮椅光滑地通过狭窄通道的目的.相比于传统的A*算法及其变体, 本算法使轮椅通过通道时路径更加平滑; 而相比于其他考虑平滑性的路径规划方法, 由于本算法利用在线测得的通道位置实时计算期望路径, 因此不需要构建全局地图以及基于地图的全局定位, 本算法更加简单和更易实现.

    • 过狭窄通道的问题可以看作是一个路径规划的问题.本文方法的目的是首先基于曲率优化产生一个平滑路径, 然后控制轮椅去跟踪它.只要产生的期望路径能平滑通过狭窄过道且轮椅能够精确地跟踪它, 那么过狭窄通道的问题就能成功解决.选择轮椅上的某个点作为控制的参考点.为了实现过通道, 算法策略应该满足以下要求:

      1) 轮椅的实际路径必须是优雅且平滑的, 没有震荡和模棱两可的情形.

      2) 由于轮椅的几何约束, 路径的曲率应该限制在某个范围内.

      3) 当轮椅到达狭窄过道位置时, 其航向角应该垂直于狭窄过道的平面.

      图 2所示为轮椅过狭窄通道的原理描述.为了产生平滑的过通道的路径, 需要考虑轮椅的几何特征和大小.检测过道位置的传感器所在位置$P_{s}^{l}$或者后轮的中心$P_{s}^{c}$可以选为操作点.因此两个本地坐标系分别由$\{W_L\}$ (虚线)和$\{W_C\}$ (实线)表示, 它们的原点分别是$P_{s}^{l}$和$P_{s}^{c}$. $P_d$为全局坐标系$G$下狭窄通道的中心位置. $P_{s}$ ($P_{s}^{l}$和$P_{s}^{c}$)和$P_{d}$ ($P_{d}^{o}$和$P_{d}^{os}$)分别为轮椅的当前与期望位置, 以及它们相对应的航向角$H_s$和$H_d$.除非特别声明, 全文所有的位置和航向角都是相对于坐标系$\{W_C\}$的.

      图  2  轮椅过狭窄通道过程原理图

      Figure 2.  The schematic description of the narrow gap passing process

    • 图 3为提出的轮椅过狭窄通道的算法结构图.算法主要有三个模块, 分别是:通道定位、路径规划和轮椅控制.在通道定位模块中, 采用激光测距仪或者3D摄像头(如微软Kinect传感器), 可以检测出相对于轮椅坐标系的通道中间位置.一旦通道位置被精确找到, 它就被路径规划模块用于通过曲率优化产生一条期望的贝塞尔曲线路径.在轮椅控制模块中, 期望的路径与轮椅当前位置之间的误差输入到PID控制器计算出轮椅二轮的速度, 从而控制轮椅跟踪这个路径.这个过程反复实时运行.

      图  3  轮椅过狭窄通道算法结构图

      Figure 3.  Strategy architecture of narrow gap passing

      每当检测到狭窄过道, 算法动态产生一条平滑的路径, 因此, 过狭窄通道的策略可以连续不断进行而不需要里程计或者全局定位信息.这样可以大大减少来自轮椅和不确定环境带来的各种干扰, 使得过狭窄通道过程更安全和有效.每个模块的详细描述如下.

    • 通道定位模块目的在于计算通道在轮椅坐标系里的相对位置(包括位置$P_d$和方向$H_d$, 即过道法向量的方向), 然后作为路径规划模块的输入.通道定位具体算法取决于所用的传感器类型.当使用二维激光测距仪时, 结合分割和直线拟合的方法可以用来定位通道.后文实验部分将会详细介绍此方法.当使用深度摄像头时, 红-绿-蓝颜色的图像数据首先通过分割算法被用于通道的识别, 然后带有深度信息的深度图像用于精确找到通道相对于摄像头的位置.在摄像头的外部参数标定好的情况下, 计算出来的结果可以很简单地通过平移旋转运算计算出通道相对于轮椅的位置.

    • 路径规划的目的在于采用优化方法确定贝塞尔曲线的控制点, 使得由这些控制点决定的贝塞尔曲线路径具有最小的曲率变化率.最平滑的贝塞尔曲线满足各种来自轮椅和环境的约束.由于轮椅和过道之间的相对位置可实时计算得到, 路径规划也连续不断地进行着. 图 2表示了弧线${P_sP_d}$的控制点为$P_{i}$, $i\in0, 1, 2, 3$, 也就是三次贝塞尔曲线的控制点[21].

      然后, 三次贝塞尔曲线便可生成, 以连接点$P_s$和$P_d$, 方向分别为$H_s$和$H_d$.也就是说, 在检测到狭窄通道后, 算法通过对贝塞尔曲线的曲率的优化, 利用通道的中间点位置$P_s$及其对应的方向$H_d$进行控制点的计算.因为$P_s$和$P_d$分别已经对应了已知的控制点$P_{0}$和$P_{3}$, 那么参数估计仅仅需要计算$P_{1}$和$P_{2}$, 至此, 在已知$P_{0}$和$P_{3}$情况下, 整个贝塞尔曲线就被完全确定了.需要注意的是$H_d$期望是垂直于墙壁的.

      为了满足前述的各项要求, 根据环境和实际情况, 为优化设计了两类约束:

      1) 方向.因为路径规划是从航向角$H_s$开始的, 期望的贝塞尔曲线在位置$P_{0}$处切线的斜率应该等于$H_s$.类似地, 为了克服轮椅与通道宽度之间比值小的问题, 点$P_{3}$处的切线应该是通道的中间位置处的法向方向.因此, 点$P_{1}$和$P_{2}$的可选范围进一步减小为一维.也就是说, 由于贝塞尔曲线的第二个属性, 点$P_{1}$和$P_{2}$必须分别处于点$P_{0}$和$P_{3}$的切线上.并且, $P_{1}$需要沿着$H_s$的方向寻找, 而$P_{2}$只可能出现在$H_d$的反方向.因此, 有关于方向的约束为

      $$\begin{align} \|P_{0}-P_{c}\| =\|P_{1}-P_{0}\|+\|P_{1}-P_{c}\| \end{align}$$ (1a)
      $$\begin{align} \|P_{3}-P_{c}\| =\|P_{2}-P_{3}\|+\|P_{2}-P_{c}\| \end{align}$$ (1b)

      其中, $P_{c}$是直线$P_{0}P_{1}$和$P_{2}P_{3}$的交点, $\|\cdot\|$表示二范数.这两个约束保证了由点$P_{1}$和$P_{2}$确定的贝塞尔曲线可以连接起点终点即$P_{0}$和$P_{3}$, 并且对应的方向分别为$H_s$和$H_d$.

      2) 完全凸性.如前所述对曲率的约束要求曲线没有曲率的突然变化.于是, 贝塞尔曲线必须是凸的.已经知道, 如果贝塞尔曲线的控制点构成的多边形是凸的, 那么贝塞尔曲线也呈完全凸性.因此, $P_{1}$和$P_{2}$必须在保证曲线的完全凸性的情况下进行约束.这里采用向量叉乘的方法来实现此约束.向量$\overrightarrow{P_{0}P_{1}}$、$\overrightarrow{P_{1}P_{2}}$和$\overrightarrow{P_{2}P_{3}}$的叉乘$\omega_1$和$\omega_2$分别为

      $$\begin{align} \omega_1=\overrightarrow{P_{0}P_{1}}\times\overrightarrow{P_{1}P_{2}}, \quad \omega_2=\overrightarrow{P_{1}P_{2}}\times\overrightarrow{P_{2}P_{3}} \end{align}$$ (2)

      故而, 完全凸性的约束表示为

      $$ \begin{align} {\rm sgn}(\omega_1)\cdot {\rm sgn}(\omega_2)>0 \label{eq:const1:convex} \end{align}$$ (3)

      其中, ${\rm sgn}(\omega_i)$, $i=1, 2$为向量$\omega_i$的旋转方向.

      带约束的优化是为了找到控制点$P_{1}$和$P_{2}$使得曲线尽量光滑, 也就是曲率及其变化率应当尽量小.由于这里只考虑二维环境, 且为了平衡精度与算法复杂度, 采用三次贝塞尔曲线, 曲线坐标表示为$B(t)$ $=$ $(x(t), y(t))$, 其中

      $$\begin{gathered} x(t) = {A_0}{(1 - t)^3} + 3{A_1}{(1 - t)^2}t{\mkern 1mu} + \\ 3{A_2}(1 - t){t^2} + {A_3}{t^3} \\ \end{gathered} $$ (4a)
      $$\begin{align} y(t)=&\ B_0(1-t)^3+3B_1(1-t)^2t\, +\notag\\ &\ 3B_2(1-t)t^2+B_3t^3 \label{eq:bezier:xy} \end{align}$$ (4b)

      其中, $(A_i, B_i)$表示控制点$P_i$的坐标.那么贝塞尔曲线相对于变量$t$的曲率为

      $$ \begin{align} \kappa(t)=\dfrac{1}{\rho(t)}=\dfrac{\dot{x}(t)\ddot{y}(t)-\dot{y}(t)\ddot{x}(t)}{(\dot{x}^2(t)+\dot{y}^2(t))^{\frac{3}{2}}} \end{align}$$ (5)

      其中, $\rho(t)$为曲线半径.因此, $P_{1}$和$P_{2}$可由如下受约束于式(1)和式(3)的优化问题求得:

      $$ \begin{align} \label{equ:cosnt1} &\min_{P_{1}, P_{2}}\int_{0}^{1}\left[(\kappa(t))^2+(\dot{\kappa}(t))^2\right]{\rm d}t \end{align}$$ (6a)
      $$\begin{align} {\rm s.t.}\quad (1), ~ (3) \end{align}$$ (6b)

      其中, $\dot{\kappa}(t)$为曲率的一阶导.由此计算出的$P_{1}$和$P_{2}$就能满足前述所有约束且使得规划的路径最优.

      当轮椅到达狭窄的过道时, 期望的轮椅航向角应该垂直于过道.但是由于干扰和控制的误差, 最终的轮椅位置和方向可能有所偏离期望值.为了满足上述第3个要求, 在轮椅通过过道前, 轮椅的航向角必须调整为垂直于过道.

    • 为了使得轮椅能够跟踪规划好的路径, 采用PID反馈控制器.轮椅实际的位置与参考路径之间的误差作为PID控制器的输入.如图 4所示, 沿着轮椅的航向角距离轮椅为$Z$的一点$F$用来定义位置误差, 然后将点$F$投影到参考路径于点$S$, 使得$\overline{FS}$垂直于点$S$的切线.位置误差由点$F$和$S$之间的距离$D_{\rm err}$表示.交叉跟踪误差由期望路径和轮椅重心位置$(x_r, y_r)$之间的最短距离表示.

      图  4  位置误差的计算

      Figure 4.  The calculation of the position error

      在本文中, 假定径向速度$V$为常量, 角速度$\omega$作为PID控制器的输出量.数字PID控制可以描述为

      $$\begin{align} \omega = k_pD_{\rm err}^k+k_iT_s\sum_{i=1}^kD_{\rm err}^i+\frac{k_d}{T_s}\left(D_{\rm err}^k-D_{\rm err}^{k-1}\right) \end{align}$$ (7)

      其中, $k_p$, $k_i$和$k_d$分别为比例、积分和微分系数, 为当前时刻的位置误差, 为上一个时刻的位置误差.选取$D_{\rm err}$而没选用作为PID的输入的原因在于用于抵消时所需$k_p$要远大于抵消同样大小的$D_{\rm err}$时的$k_p$, 而较大的$k_p$更容易造成系统震荡.

    • 首先, 进行仿真来验证算法的核心部分, 也就是基于贝塞尔曲线的曲率优化的路径规划算法.假设通道位置已知.进行了2种场景的仿真, 分别为场景$A$ (目标位置在通道左边)和场景$B$ (目标位置在通道右边), 如图 5所示.

      对于每一种情形, 路径规划算法算出四个不同的最优参考轨迹.每一个参考路径对应不同的参数如轮椅的起始位置及其航向角, 目标位置及其航向角.

      表 1列出了每一条参考路径对应的所需参数, 其中, $P_s$, $P_d$和$P_t$分别表示轮椅在走廊里的起始坐标、通道中间点的坐标和在房间里的目标点位置坐标, 单位为m. )表示路径$i$的起始位置和通道中间位置对应的航向角(单位为度), $i\, (H_d, H_t)$表示路径$i$的通道中间位置和穿过通道之后的目标点位置对应的航向角.以路径$1$为例, 对于场景$A$, 对应轮椅的起始位姿由起始坐标$P_s$及航向角$H_s$构成, 即, 终点位姿由通道中间点的坐标$P_d$及航向角$H_d$构成, 即, 路径$2$与路径$1$类似; 再以路径$3$为例, 对应轮椅的起始位姿由通道中间点的坐标$P_d$及航向角$H_d$构成, 即$(1.6$, $1.8, 90)$, 终点位姿由目标点位置坐标$P_t$及航向角$H_t$构成, 即$(0.0, 3.5, 160)$, 路径$4$与路径$3$类似.以这些参数作为输入, 路径规划算法便可计算出平滑路径然后由PID控制器控制轮椅进行跟踪. PID控制器输出驱动轮椅行进的转速.控制器各系数选择为$k_p=$ $0.01$, $k_i=0.001$和$k_d=0.001$.

      表 1  每条路径的输入参数

      Table 1.  The input parameters for each path

      场景 Ps (m) Pd (m) Pt (m) 1 (Hs, Hd) 2 (Hs, Hd) 3 (Hd, Ht) 4 (Hd, Ht)
      A (0.1, 1.3) (1.6, 1.8) (0.0, 3.5) (-20, 90) (-40, 90) (90, 160) (90, 200)
      B (0.1, 1.3) (1.6, 1.8) (3.4, 3.5) (-10, 90) (10, 90) (90, 20) (90, -20)

      图 5展示了计算出的参考路径及其对应的轮椅实际路径, 各参考路径对应的参数如表 1给出.由图 5可以得出如下结论:

      图  5  轮椅的参考路径和实际路径

      Figure 5.  Reference path and actual path of wheelchair

      1) 所有的最优参考路径满足在路径规划部分提到的约束:首先, 轮椅始终在走廊里; 其次, 贝塞尔曲线在起始位置的切线方向等于轮椅的航向角, 在目标位置也是如此; 再次, 计算出的贝塞尔控制多边形是凸的.

      2) 所有最优参考路径是平滑的.

      3) 尽管有小的跟踪误差(从图中放大部分可以看出), 轮椅的实际轨迹能较精确地跟踪参考路径.

      因此, 仿真验证了过狭窄通道算法的有效性.

      为了分析轮椅实际跟踪路径的平滑性和精确度, 如图 6(a)图 6(b)所示, 分别展示了场景$A$仿真中的轮椅角速度$\omega$和距离误差相对于贝塞尔曲线参数$t$的变化曲线.由图 6可知, 角速度没有呈现出突变, 表明了轮椅运动的平滑性.最大的跟踪误差约为55 mm, 都在合理和接受的范围, 表明跟踪精度较高.

      图  6  在场景$A$中每一条路径对应的轮椅的角速度和跟踪误差

      Figure 6.  The angular rate and the tracking errors for each path in Scenario $A$

    • 为了验证本算法相对于其他主流室内路径规划算法的在过狭窄通道情况下的优越性, 进行了另一个仿真以比较本文算法和基于A*[22]的开源算法在路径曲率(光滑度)上的表现.如图 7所示, 实验场景为包含2个80 cm狭窄通道的室内环境, 仿真平台为基于机器人操作系统(Robot operating system, ROS[23])的Gazebo[24]三维仿真器.仿真中, 本文算法的核心部分在Matlab中运行, 通过Matlab的Robotics工具箱可以实现与ROS和Gazebo系统的数据通信, 一方面来自Gazebo中轮椅上的激光测距仪数据可以传入Matlab中通道定位模块, 实现通道位置计算; 另一方面Matlab中轮椅控制模块计算出的轮椅速度值可以发送给Gazebo中的轮椅, 实现轮椅的控制.其中路径曲率优化算法采用Matlab优化工具箱中的内点法优化方法.而基于A*路径规划导航算法实现采用ROS的开源软件包Navigation Stack[25].仿真时, 两种方法均控制轮椅从起点位姿(1.5 m, 0 m, 0 rad)穿过2个狭窄通道并到达终点位姿(0.5 m, 3.5 m, 3.14 rad), 线速度设定为0.15 m/s.其中位姿第3项表示轮椅的航向角.

      图  7  算法比较的仿真环境

      Figure 7.  Simulation environment for algorithm comparison

      图 8分别展示了由本文方法和A*算法产生的轮椅路径, 由图 8可以定性地看出A*算法产生的路径在2个狭窄通道处有"急转弯", 即有较大曲率, 而本文提出的基于曲率优化方法产生的路径则没有"急转弯", 转弯过程较均匀的分配在整个路径中, 而不会在通道处出现较大曲率, 使路径较平滑.

      图  8  基于本文方法和A*算法的轮椅路径

      Figure 8.  Actual trajectories generated by the proposed method and A* algorithm

      图 9展示了两种方法产生的路径的曲率, 可以看出, A*产生的路径的曲率具有多次跌宕起伏, 最大曲率达到2.2, 而基于曲率优化方法产生的路径的曲率只有2次起伏, 且最大曲率仅为1.1, 远小于A*产生的路径的曲率.因此, 图 9进一步说明了本文提出的基于曲率优化的算法可以实现比A*算法路径曲率更小且更加平滑的过狭窄通道过程.

      图  9  基于本文方法和A*算法的路径曲率的比较

      Figure 9.  The comparison of trajectory curvature generated by the proposed method and A* algorithm

      另外, 需要指出的是基于A*的路径规划导航算法需要事先构建好环境地图, 然后再运行基于此地图的全局定位算法, 而本文算法只需要根据传感器信息识别并计算狭窄过道相对于轮椅坐标系的相对坐标, 而并不需要构建地图和全局定位, 因此本文方法更简单且更易实现.

    • 第2.2节仿真中基于内点法的优化方法计算复杂度较高, 较大程度上影响了算法的实时性.为了评估本文算法的实时性, 图 10展示了算法每次循环中各模块运行时间及总时间.由图 10可以看出, 每次循环中基于内点法的路径规划模块所需运行时间在0.15 s左右, 而每次循环算法总时间约为0.2 s, 即5 Hz, 在轮椅线速度为0.15 m/s时满足实时性要求.仿真运行的软硬件配置为: Intel i5 CPU, 4 GB RAM, 操作系统为Linux 14.04, Matlab软件版本为R2015b.需要指出的是仿真算法是运行在解释性语言Matlab中, 如果算法由编译性语言如C++等实现, 算法运行时间将会更小, 实时性可以更好.

      图  10  算法每次循环中各模块运行时间及总时间

      Figure 10.  Consuming time of each algorithm module and their total value for each iteration

    • 为了进一步验证和评估轮椅过狭窄通道的算法, 用真实轮椅进行了实验.如图 11(a)图 11(b)所示, 轮椅的长度和宽度分别为85 cm和65 cm.采用激光测距仪测算通道位置.由于单个激光测距仪的视角不足以覆盖所有的安全区域, 结合两个激光测距仪一起以构成更广视角.两个激光测距仪和后轮的距离为76 cm, 对应的偏向角$\theta_1$和$\theta_2$分别为$46.36^\circ$和$-56.50^\circ$.由于过道宽度为80 cm, 每一边留给轮椅的自由空间只有7.5 cm.在实验之前, 首先对两个激光测距仪之间的相对位置关系进行标定以使二者的数据具有一致性.

      图  11  轮椅和激光的配置

      Figure 11.  Configuration of the wheelchair and the lasers

      图 12描述了实验场景(5 m $\times$ 5 m), 其中有一些墙壁和一间屋子.实线框住的是轮椅, 其分别从$A$, $B$, $C$和$D$四个位置出发, 穿过过道进入到屋子.轮椅的全局位置坐标由位置跟踪系统VICON获得, 以评估算法的有效性.过通道算法的实现细节描述如下.

      图  12  实验场景

      Figure 12.  Experimental scenario

    • 利用来自两个激光测距仪的数据, 通道定位算法计算通道中心位置及其对应的航向角$P_d$和$H_d$, 其中航向角应垂直于过道平面, 可由墙壁的斜率表示, 即$H_d=\arctan(-1/K_w)$. $P_d$和$H_d$则作为产生贝塞尔曲线的输入参数.主要采用分割和直线拟合两个步骤计算$P_d$和$H_d$.

    • 分割的目的在于将激光扫描数据分成几个不同的子集使得每个子集点属于一条直线.例如, 如图 12所示, 一次完整的激光扫描数据点被分成6个子集.首先, 采用算法Adaptive breakpoint detector (ABD)[26]通过检测断点来找到所有连续点集.如图 13所示, 共有三个点集: {1, 2}, {3}和{4, 5, 6}.然后, 采用算法Iterative end-point fit (IEPF)[27]将每个连续点集分成不同的子集, 以使每个子集的点都属于同一条直线.对每两个子集, 计算第1个子集的末尾点($P_r$)与第2个子集的起始点($P_f$)之间的距离$D_{rf}$, 如果$D_{rf}$等于过道的宽度, 那么就找到了过道, 并且.如图 9所示, 通道在子集2和子集4之间找到了.

      图  13  一次激光扫描数据点及其分段

      Figure 13.  A full scan of laser data points and its expected segmentations

    • 在通道位置找到之后, 接下来是要计算找到的通道所倚靠的墙壁的斜率$K_w$以确定$H_d$. $K_w$的计算可通过拟合对应于墙壁的所有激光数据点构成的直线来实现.直线拟合的目的是确定直线的参数.直线表示方法有多种, 其中一个表示直线的参数形式为

      $$\begin{align} \label{eq:tradi_line} y=kx+q \end{align}$$ (8)

      其中, $q$和$k$分别为直线$y$的截距和斜率.假设有一个需要拟合的点集$R=\{(x_i, y_i)|i=1, \cdots, k\}$, 那么参数$k$和$q$可通过最小二乘直线拟合方法计算为

      $$ \begin{align} \label{eq:line_fitting} S_x = \sum_{i=1}^{k}x_i, \hspace{0.1in}S_y = \sum_{i=1}^{k}y_i \end{align}$$ (9a)
      $$\begin{align} S_{xx} = \sum_{i=1}^{k}x_i^2, \hspace{0.1in}S_{yy} = \sum_{i=1}^{k}y_i^2, \hspace{0.1in}S_{xy} = \sum_{i=1}^{k}x_iy_i \end{align}$$ (9b)
      $$\begin{align} k = \frac{-S_xS_{yy}+S_yS_{xy}}{S_yS_{xx}-S_xS_{xy}} \label{eq:line_fitting_k} \end{align}$$ (9c)
      $$\begin{align} q = \frac{-S_{xy}^2-S_{xx}S_{yy}}{S_yS_{xx}-S_xS_{xy}} \end{align}$$ (9d)

      其中, 式(9c)为直线斜率.

      对于如图 12所示的场景, 激光点集2和点集4为构成墙壁的点.把这些点的笛卡尔坐标代入式(9a)~(9d)便计算出斜率$K_w$.

      尽管路径规划算法已经设计好, 但在实际中还不能直接用于轮椅, 尤其是在轮椅大体积的情况下.下面就通过选择合适的起点$P_0$和目标点$P_3$, 也就是$P_s$和$P_d$来解决这个问题.改进后的算法能产生在实际中令人满意的路径, 且兼具光滑性和实用性.

    • 当新的贝塞尔曲线路径产生时, 控制点$P_0$的坐标总被设置为$(0, \, 0)$.然而, 如图 2所示, $P_0$的位置可以是$P_s^c$或者$P_s^l$.一般而言, 应该选择$P_s^l$, 因为有关过道的位置检测都是基于激光的位置.当轮椅作为一个质点或者激光和具有驱动力的轮子之间的距离很小时, 选择$P_s^l$是合理的.然而, 由于轮椅的大体积, 这两种情形都不适用于真实轮椅.而且, 很难控制轮椅后轮使装在前面的激光跟踪规划好的路径.因此, 在规划贝塞尔路径时, $P_s^c$选为控制点$P_0$, 如图 2圆点所示.对应地, 整个过通道算法策略都是基于坐标系$\{W_C\}$.同时, 选择$P_s^c$比选择$P_s^l$对于轮椅控制误差来说更具鲁棒性, 因为对于相同的控制误差造成的偏差, $P_s^l$的情形要多于$P_s^c$.

      因为$P_s^c$距离$P_s^l$较远, 在坐标系$\{W_L\}$中计算出的通道的坐标需要平移至坐标系$\{W_C\}$中.这个平移量只有沿$X$轴的量, 且平移量为$P_s^c$和$P_s^l$之间的距离, 也就是如图 2所示的$L$.

    • 当轮椅到达过道中心时, 激光位置可能恰好和墙壁共线, 它们可能不能精确地检测到过道的位置甚至根本检测不到.同时由于现实情况下的各种干扰, 轮椅的航向角不能恰好到达期望值.因此, 轮椅需要在过通道时调整其航向角以使其垂直于过道平面, 从而减小碰撞到过道框架的可能性.

      为了克服这个问题, 在确定$P_d$时, 在过道定位环节中计算出来的值的基础上沿着$H_d$的方向设置一个偏移量, 以保证激光有足够的前方距离来检测墙壁.于是, 轮椅可以在位置$P_d^{os}$而不是$P_d$调整其航向角垂直于过道平面, 如图 2所示.设偏移量为$d^{o}$, 则新的$P_d$可表示为$P_d^{o}$.然而, 由于贝塞尔曲线的控制点为$P_s^c$, 偏移之后的点$P_d^{o}$要被进一步偏移到$P_d^{os}$, $P_d^{os}$才被当作控制点$P_3$进而产生贝塞尔路径. $P_d^{os}$可由下式计算出:

      $$ \begin{align} \label{eq:OffsetMiddDoor} & \theta_{d}^{os}=\arctan H_d, \hspace{0.1in}\theta_{d}^{os}\in(-90^\circ, 90^\circ) \end{align}$$ (10a)
      $$\begin{align} & x_{d}^{os}=d^{os}\cos\theta_{d}^{os}+x_{P_d} \end{align}$$ (10b)
      $$\begin{align} & y_{d}^{os}=d^{os}\sin\theta_{d}^{os}+y_{P_d} \end{align}$$ (10c)

      其中, $(x_{P_d}, y_{P_d})$为通道位置$P_d$的坐标, $x_{d}^{os}$和$y_{d}^{os}$为$P_d^{os}$的坐标, $d^{os}$为偏移量大小, 包括实际偏移量$d^{o}$和轮椅的长度$L$.

      图 14可以看出, 由于轮椅尺寸大, 单个激光测距仪不能检测整个通道的范围, 这就可能造成计算出错误的路径和不合适的控制.而结合两个激光测距仪的方法可以为通道定位提供完整范围.

      图  14  两个激光测距仪相对于单个的优势

      Figure 14.  Advantage of two lasers compared with single laser

      图 15展示了4个不同起点位置对应的轮椅的轨迹.对于每个起始位置, 进行了2次试验.可以显然地看到, 在每个起点, 轮椅都能找到通道位置, 然后平滑而精确地通过过道而不碰撞到通道两边边缘, 证实了本文所提出算法的精确度和有效性.

      图  15  轮椅轨迹

      Figure 15.  Trajectories of the wheelchair

      图 16分别展示了轮椅在起点位置$A$, $B$, $C$和$D$对应的到达期望位置时的实际位置.可以看出, 几乎所有的实际位置都在期望位置的附近, 只略有较小误差, 并且多数航向角都按期望的那样垂直于通道平面, 尽管有些轻微地偏离了期望值.这些误差主要有两个原因: 1) 轮椅前面的随动轮在方向上造成较大干扰; 2) 选择$P_d$过程中的长偏移量放大了通道定位的误差.而实际位置和期望位置之间的水平误差都小于7.5 cm, 表明本文所提算法能减小来自现实的干扰和不确定性, 从而使轮椅成功通过即使只有狭窄自由间隙的通道.这归功于根据轮椅位置连续不断地进行路径规划的策略.值得一提的是, 如果不是采用这种策略, 而采用只在第1次检测到通道时规划一条路径再控制轮椅跟踪这条路径的方法, 会使轮椅不能到达期望位置, 因为轮椅在跟踪此路径时需实时估计自身位置, 而如果只依赖轮椅上的航位推算法而不借助其他定位算法, 由于航位推算位置估算方法无限地累计误差会使跟踪误差越来越大, 从而造成跟踪失败.

      图  16  轮椅从不同起点到达终点位置时的实际位置

      Figure 16.  Actual wheelchair poses when arriving at the desired pose with different starting poses $A$, $B$, $C$ and $D$

      图 17为每次试验在三个不同阶段的角速度变化曲线.在第一个阶段, 角速度递增直到轮椅快要到达过道中间的位置; 在第二个阶段, 轮椅调整其航向角以保证航向角垂直于过道; 在第三个阶段, 轮椅径直走向屋子.需要指出的是在所有试验的第一个阶段, 角速度平滑递增而没出现突变现象, 再一次证明了算法成功实现了平滑过通道的目的.另外, 对于同一个起始位置的两次不同试验, 对应轮椅的角速度曲线重叠, 表明算法的高可重复性.

      图  17  轮椅的角速度

      Figure 17.  Angular rate of the wheelchair

      图 18为轮椅从起点$A$和$D$出发的轨迹截图(因为相似性, 起点$B$和$C$的截图在此省略), 包括了几个重要的位置, 如起点和在过通道前后的位置.在两个起点的第5张截图中, 轮椅几乎在通道的中间位置, 航向角也几乎垂直于通道平面, 证实了算法的有效性.第6张截图也表明轮椅能以期望的航向角穿过通道.

      图  18  轮椅从起点$A$和$D$出发过通道轨迹截图((a)~(f)对应于起点$A$, (g)~(l)对应于起点$D$)

      Figure 18.  Snapshots of the wheelchair$'$s trajectories when passing gap, starting from pose $A$ and $D$ ((a)~(f), (g)~(l) are with pose $A$ and $D$, respectively.)

    • 本文提出了一种智能轮椅通过狭窄通道的新颖方法.狭窄通道指自由空间只有几厘米的门或者过道.所提出的路径规划算法基于贝塞尔曲线曲率优化, 从而产生平滑的路径以控制轮椅跟踪此路径.算法按照"定位通道"$\rightarrow$"产生贝塞尔曲线路径" $\rightarrow$"跟踪产生的路径"的顺序连续执行.算法考虑轮椅和环境的各种约束, 能够在没有全局坐标或者里程计的情况下平滑地穿过狭窄通道.

      首先, 进行了仿真来验证算法的核心部分, 即基于贝塞尔曲线的曲率优化的路径规划算法.然后仿真比较了本文算法与基于A*的路径规划开源导航算法在路径平滑度上的表现, 分析了算法实时性.最后用装有两个激光测距仪的真实轮椅进行对算法的实验验证, 结果证实了整个算法的可行性和有效性.算法不仅适用于轮椅, 还可用于其他各类想要通过狭窄通道的移动机器人.未来的工作主要测试轮椅在不同复杂室内环境下的过通道算法, 比如办公室、购物中心、居家和医院等, 主要解决过不规则障碍物的问题.

参考文献 (27)

目录

    /

    返回文章
    返回