Please wait a minute...
浙江大学学报(工学版)
浙江大学学报(工学版)  2024, Vol. 58 Issue (8): 1556-1564    DOI: 10.3785/j.issn.1008-973X.2024.08.003
机械工程、能源工程     
基于同步动态优化的移动机器人最优速度规划
樊志伟1,2,3(),贾凯1,2,4,*(),张雷1,2,4,邹风山1,2,4,杜振军4,刘明敏4
1. 中国科学院沈阳自动化研究所 机器人学国家重点实验室，辽宁 沈阳 110016
2. 中国科学院机器人与智能制造创新研究院，辽宁 沈阳 110016
3. 中国科学院大学，北京 100049
4. 沈阳新松机器人自动化股份有限公司，辽宁 沈阳 110168
Optimal velocity planning for mobile robot based on simultaneous dynamic optimization
Zhiwei FAN1,2,3(),Kai JIA1,2,4,*(),Lei ZHANG1,2,4,Fengshan ZOU1,2,4,Zhenjun DU4,Mingmin LIU4
1. State Key Laboratory of Robotics, Shenyang Institute of Automation, Chinese Academy of Sciences, Shenyang 110016, China
2. Institutes for Robotics and Intelligent Manufacturing, Chinese Academy of Sciences, Shenyang 110016, China
3. University of Chinese Academy of Sciences, Beijing 100049, China
4. Shenyang SIASUN Robot and Automation Limited Company, Shenyang 110168, China
 全文: PDF(2190 KB)   HTML
摘要：

针对移动机器人在实际运动中受到运动极限的制约及非完整约束的影响，导致难以兼顾运动效率与执行器跟踪性能的问题，提出基于同步动态优化的速度规划方法. 建立基于最优控制的速度规划方案，综合考虑机器人的车轮物理约束及车体规则约束. 提取约束生成器中的1、2阶约束，根据可达性分析，通过线性规划过程，递推得到参考轨迹，为数值优化提供初始猜测. 考虑约束生成器中的3阶约束，采用约束松弛方法，通过基于内点法的同步迭代优化，得到最优配速方案. 通过数值及仿真实验验证了以上算法，实验结果表明，移动机器人在运动效率上可以达到车轮物理极限或车体规则极限，在执行器跟踪性能上可以将路径位置误差减小20%以上，保证了运动过程平稳光滑.
关键词： 移动机器人;  速度规划;  加加速度约束;  同步动态优化;  可达性分析    
Abstract:

A velocity planning method based on synchronous dynamic optimization was proposed in order to address the issue where the actual motion of mobile robots was constrained by motion limits and nonholonomic constraints, making it difficult to balance motion efficiency and actuator tracking performance. A speed planning scheme based on optimal control was established, considering the physical constraints of the wheels and vehicle body rules of the mobile robot. The extraction of the first and second-order constraints from the constraint generator, along with the derivation of a reference trajectory via linear programming, was facilitated, providing initial estimates for numerical optimization. A constraint relaxation method was used with the incorporation of third-order constraints from the constraint generator in order to obtain the optimal speed scheme through synchronous iterative optimization based on the interior-point method. The proposed algorithms were validated through numerical and simulation experiments. The experimental results demonstrate that the physical limits of the robot’s wheels or the limit of its body rule can be reached in terms of motion efficiency. A reduction of over 20% in path position error concerning actuator tracking performance was achieved, which ensured a smooth and efficient motion process.
Key words: mobile robot    velocity planning    jerk constraint    simultaneous dynamic optimization    reachability analysis
收稿日期: 2023-07-03 出版日期: 2024-07-23
CLC:  TP 242  
基金资助: 国家自然科学基金-区域创新发展联合基金资助项目（U20A20197）.
通讯作者: 贾凯     E-mail: fanzhiwei@sia.cn;jiakai@siasun.com
作者简介: 樊志伟（1998—），男，硕士生，从事机器人技术的研究. orcid.org/0000-0002-9925-6035. E-mail：fanzhiwei@sia.cn
服务　　
把本文推荐给朋友
加入引用管理器
E-mail Alert
作者相关文章　　
樊志伟
贾凯
张雷
邹风山
杜振军
刘明敏

引用本文:

樊志伟,贾凯,张雷,邹风山,杜振军,刘明敏. 基于同步动态优化的移动机器人最优速度规划[J]. 浙江大学学报(工学版), 2024, 58(8): 1556-1564.

Zhiwei FAN,Kai JIA,Lei ZHANG,Fengshan ZOU,Zhenjun DU,Mingmin LIU. Optimal velocity planning for mobile robot based on simultaneous dynamic optimization. Journal of ZheJiang University (Engineering Science), 2024, 58(8): 1556-1564.

链接本文:

https://www.zjujournals.com/eng/CN/10.3785/j.issn.1008-973X.2024.08.003    或    https://www.zjujournals.com/eng/CN/Y2024/V58/I8/1556

图 1  移动机器人的运动学模型
图 2  移动机器人最优速度规划算法的框图
算法1　基于可达集的初始解
输入：路径${\boldsymbol{p}}\left( s \right)$，起止速度${{\dot s}_0}、{{\dot s}_{\rm{end}}}$输出：初始解$ \left( {{{\tilde {\dot s}}_0},{{\tilde {\ddot s}}_0}} \right), \cdots ,\left( {{{\tilde {\dot s}}_{N - 1}},{{\tilde {\ddot s}}_{N - 1}}} \right),\left( {{{\tilde {\dot s}}_N},0} \right) $　　 可达集${H_0},{H_1}, \cdots ,{H_N}$1. 对${\boldsymbol{p}}\left( s \right)$采样得到${s_0} = 0,{s_1}, \cdots ,{s_N} = {s_{\rm{end}}}$2. 设置初始可达集$ {H_N} = \left\{ {{\dot s}_{\rm{end}}^2} \right\} $3. For $i = N - 1:1:0$4. 　通过式(15)根据${H_{i+1}}$求解可达集${H_i}$5. End6. 检查${\dot s}_0^2$是否在可达集${H_0}$中，否则返回失败7. 设置初始解起始点${\tilde {\dot s}_0} = {{\dot s}_0}$并更新${H_0}$上界8. For $i = 0:1:N - 1$9. 　通过式(16)，根据${\tilde {\dot s}_i}$求解${\tilde {\dot s}_{i+1}}$并更新${H_{i+1}}$上界10. End
  
算法2　基于约束松弛的非线性迭代优化
输入：初始解$ \left( {{{\tilde {\dot s}}_0},{{\tilde {\ddot s}}_0}} \right), \cdots ,\left( {{{\tilde {\dot s}}_{N - 1}},{{\tilde {\ddot s}}_{N - 1}}} \right),\left( {{{\tilde {\dot s}}_N},0} \right) $　　 可达集${H_0},{H_1}, \cdots ,{H_N}$输出：最优轨迹信息$ \left( {{\dot s}_0^*,{\ddot s}_0^*,\dddot s_0^*} \right), \cdots ,\left( {{\dot s}_{N - 1}^*,{\ddot s}_{N - 1}^*,} \right. $　　 $\left. {\dddot s_{N - 1}^*} \right) $, $\left( {{\dot s}_N^*,{\ddot s}_N^*,0} \right) $1. 设置超参数初始值${\lambda _{\rm{soft}}} \leftarrow {\lambda _{{\mathrm{soft0}}}},{\varepsilon _{{\mathrm{epoch}}}} \leftarrow 0$2. 根据式(17)建立可迭代最优控制问题${{ P}_{\rm{opti}}}$3. While ${\varepsilon _{{\mathrm{epoch}}}} < {\varepsilon _{\max}}$4. 　求解${{ P}_{\rm{opti}}}$并更新最优轨迹信息5. 　计算松弛函数${f_{\rm{soft}}}\left( s \right)$6. 　If ${f_{\rm{soft}}}\left( s \right) < {{\mathrm{TLV}}_{\rm{soft}}}$ then7. 　　输出最优轨迹信息并退出8. 　Else9. 　　${\lambda _{\rm{soft}}} \leftarrow {\lambda _{\rm{soft}}} \delta ,{\varepsilon _{{\mathrm{epoch}}}} \leftarrow {\varepsilon _{{\mathrm{epoch}}}}+1$10. 　End11. End
  
图 3  先锋P3-DX机器人
参数数值参数数值
$ {\varepsilon _{\max}} $15$ {\ddot v_{{\rm{l}},\min }},{\ddot v_{{\rm{r}},\min }} $/(m·s?3)?4
$ {\lambda _{{\mathrm{soft0}}}} $0.01$ {\ddot v_{{\rm{l}},\max }},{\ddot v_{{\rm{r}},\max}} $/(m·s?3)4
$ \delta $10${v_{C,\min }}/({\mathrm{m}}\cdot {\mathrm{s}}^{-1}),{\omega_{C,\min }}$/(rad·s?1)?2
$ {{\rm{TLV}}_{\rm{soft}}} $0.0001${v_{C,\max}}/({\mathrm{m}}\cdot {\mathrm{s}}^{-1}),{\omega_{C,\max }}$/(rad·s?1)2
$ {v_{{\rm{l}},\min }},{v_{{\rm{r}},\min }} $/(m·s?1)?2$ {\dot v_{C,\min }}/({\mathrm{m}}\cdot {\mathrm{s}}^{-2}),{\dot \omega_{C,\min }} $/(rad·s?2)?4
$ {v_{{\rm{l}},\max }},{v_{{\rm{r}},\max}} $/(m·s?1)2$ {\dot v_{C,\max}}/({\mathrm{m}}\cdot {\mathrm{s}}^{-2}),{\dot \omega_{C,\max }} $/(rad·s?2)4
$ {\dot v_{{\rm{l}},\min }},{\dot v_{{\rm{r}},\min }} $/(m·s?2)?4$ {\ddot v_{C,\min }}/({\mathrm{m}}\cdot {\mathrm{s}}^{-3}),{\ddot \omega_{C,\min }} $/(rad·s?3)?4
$ {\dot v_{{\rm{l}},\max }},{\dot v_{{\rm{r}},\max}} $/(m·s?2)4$ {\ddot v_{C,\max}}/({\mathrm{m}}\cdot {\mathrm{s}}^{-3}),{\ddot \omega_{C,\max }} $/(rad·s?3)4
表 1  速度规划仿真的参数设置
图 4  速度曲线的比较
图 5  加速度曲线的比较
算法车轮约束加加速度约束
TOPP-RA√×
Ruckig×√
所提方法√√
表 2  不同算法可引入的约束情况
图 6  不同约束及算法下车轮及车体的速度、加速度、加加速度曲线
图 7  联合仿真位置曲线
方向平均位置误差/m最大位置误差/m
TOPP-RA所提方法TOPP-RA所提方法
X0.10810.08580.31920.1701
Y0.10710.05020.28240.1567
表 3  位置误差的均值和最大值
1 KIM T, LIM S, SHIN G, et al An open-source low-cost mobile robot system with an RGB-D camera and efficient real-time navigation algorithm[J]. IEEE Access, 2022, 10: 127871- 127881
doi: 10.1109/ACCESS.2022.3226784
2 GRAF F, LINDERMAYR J, ODABASI C, et al Toward holistic scene understanding: a transfer of human scene perception to mobile robots[J]. IEEE Robotics and Automation Magazine, 2022, 29 (4): 36- 49
doi: 10.1109/MRA.2022.3210587
3 MORALES L, HERRERA M, CAMACHO O, et al LAMDA control approaches applied to trajectory tracking for mobile robots[J]. IEEE Access, 2021, 9: 37179- 37195
doi: 10.1109/ACCESS.2021.3062202
4 CHRISTOPHER M, BIRGIT G, KAI P. World robotics 2022: service robots [EB/OL]. (2022-10-26)[2023-06-25]. https://www.roboticstomorrow.com/story/2022/10/sales-of-robots-for-the-service-sector-grew-by-37-worldwide/19664/.
5 PHAM H, PHAM Q C A new approach to time-optimal path parameterization based on reachability analysis[J]. IEEE Transactions on Robotics, 2017, 34 (3): 645- 659
6 LI B, OUYANG Y, LI L, et al Autonomous driving on curvy roads without reliance on Frenet frame: a Cartesian-based trajectory planning method[J]. IEEE Transactions on Intelligent Transportation Systems, 2022, 23 (9): 15729- 15741
doi: 10.1109/TITS.2022.3145389
7 HOANG V B, NGUYEN V H, NGO T D, et al Socially aware robot navigation framework: where and how to approach people in dynamic social environments[J]. IEEE Transactions on Automation Science and Engineering, 2022, 20 (2): 1322- 1336
8 LIN Z, TAGUCHI R. Improved dynamic window approach using the jerk model [C]// 22nd International Conference on Control, Automation and Systems . Busan: IEEE, 2022: 1193–1198.
9 RAINERI M, BIANCO C G L. Jerk limited planner for real-time applications requiring variable velocity bounds [C]// IEEE 15th International Conference on Automation Science and Engineering . Vancouver: IEEE, 2019: 1611–1617.
10 KIM J Trajectory generation of a two-wheeled mobile robot in an uncertain environment[J]. IEEE Transactions on Industrial Electronics, 2019, 67 (7): 5586- 5594
11 王德军, 张凯然, 徐鹏, 等 基于车辆执行驱动能力的复杂路况速度规划及控制[J]. 吉林大学学报: 工学版, 2023, 53 (3): 643- 652
WANG Dejun, ZHANG Kairan, XU Peng, et al Speed planning and control under complex road conditions based on vehicle executive capability[J]. Journal of Jilin University: Engineering and Technology Edition, 2023, 53 (3): 643- 652
12 SHIMIZU Y, HORIBE T, WATANABE F, et al. Jerk constrained velocity planning for an autonomous vehicle: linear programming approach [C]// International Conference on Robotics and Automation . Philadelphia: IEEE, 2022: 5814–5820.
13 ZHOU J, HE R, WANG Y, et al Autonomous driving trajectory optimization with dual-loop iterative anchoring path smoothing and piecewise-jerk speed optimization[J]. IEEE Robotics and Automation Letters, 2020, 6 (2): 439- 446
14 ZDESAR A, SKRJANC I Optimum velocity profile of multiple Bernstein-Bézier curves subject to constraints for mobile robots[J]. ACM Transactions on Intelligent Systems and Technology, 2018, 9 (5): 1- 23
15 ZHANG B, HE J, PEI D. Global trajectory optimization of mobile robot based on Minimum Snap [C]// IEEE 6th Information Technology and Mechatronics Engineering Conference . Chongqing: IEEE, 2022: 36–41.
16 陈峥, 张玉果, 沈世全, 等 城市郊区道路跟车条件下智能网联汽车速度规划[J]. 中国公路学报, 2023, 36 (6): 298- 310
CHEN Zheng, ZHANG Yuguo, SHEN Shiquan, et al Speed planning of intelligent and connected vehicle under following conditions of suburban road scenarios[J]. China Journal of Highway and Transport, 2023, 36 (6): 298- 310
17 BIEGLER L T An overview of simultaneous strategies for dynamic optimization[J]. Chemical Engineering and Processing: Process Intensification, 2007, 46 (11): 1043- 1053
doi: 10.1016/j.cep.2006.06.021
18 LI B, SHAO Z Simultaneous dynamic optimization: a trajectory planning method for nonholonomic car-like robots[J]. Advances in Engineering Software, 2015, 87: 30- 42
doi: 10.1016/j.advengsoft.2015.04.011
19 LI B, SHAO Z A unified motion planning method for parking an autonomous vehicle in the presence of irregularly placed obstacles[J]. Knowledge-Based Systems, 2015, 86: 11- 20
doi: 10.1016/j.knosys.2015.04.016
20 ANDERSSON J A E, GILLIS J, HORN G, et al CasADi: a software framework for nonlinear optimization and optimal control[J]. Mathematical Programming Computation, 2019, 11: 1- 36
doi: 10.1007/s12532-018-0139-4
[1] 陈宏鑫,张北,王春香,杨明. 基于自适应随动机构的机器人目标跟随[J]. 浙江大学学报(工学版), 2022, 56(6): 1071-1078.
[2] 贾松敏,卢迎彬,王丽佳,李秀智,徐涛. 分层特征移动机器人行人跟踪[J]. 浙江大学学报(工学版), 2016, 50(9): 1677-1683.
[3] 丁夏清,杜卓洋,陆逸卿,刘山. 基于混合势场的移动机器人视觉轨迹规划[J]. 浙江大学学报(工学版), 2016, 50(7): 1298-1306.
[4] 林吉良, 蒋静坪. 基于模糊聚类的移动机器人并发故障诊断[J]. J4, 2010, 44(3): 453-457.
[5] 张建伟,樊臻，颜钢锋,林志赟. 多自主移动机器人系统协调及合作控制实验平台[J]. J4, 2010, 44(11): 2124-2129.
[6] 陈少斌, 蒋静坪. 四轮移动机器人轨迹跟踪的最优状态反馈控制[J]. J4, 2009, 43(12): 2186-2190.
[7] 孙　宇 项志宇 刘济林. 未知室外环境下移动机器人的三维场景重建[J]. J4, 2007, 41(12): 1949-1954.
[8] 王建平 张涛. 移动机器人检测与跟踪系统的研究与设计[J]. J4, 2007, 41(10): 1710-1714.
[9] 柯海森 吐旭东 钱建海. 移动机器人的鲁棒自适应控制器设计[J]. J4, 2006, 40(7): 1127-1131.
[10] 刘加海 白洪欢 黄微凹. 基于彩色和边缘信息融合的道路分割算法[J]. J4, 2006, 40(1): 29-32.
[11] 刘瑜 朱世强 金波. 移动机器人Markov定位算法的研究--方向传感器建模新方法[J]. J4, 2005, 39(3): 339-341.