Please wait a minute...
浙江大学学报(工学版)
Journal of ZheJiang University (Engineering Science)  2024, Vol. 58 Issue (8): 1556-1564    DOI: 10.3785/j.issn.1008-973X.2024.08.003
    
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
Download: HTML     PDF(2190KB) HTML
Export: BibTeX | EndNote (RIS)      

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     
Received: 03 July 2023      Published: 23 July 2024
CLC:  TP 242  
Fund:  国家自然科学基金-区域创新发展联合基金资助项目（U20A20197）.
Corresponding Authors: Kai JIA     E-mail: fanzhiwei@sia.cn;jiakai@siasun.com
Service
E-mail this article
Add to my bookshelf
Add to citation manager
E-mail Alert
RSS
Articles by authors
Zhiwei FAN
Kai JIA
Lei ZHANG
Fengshan ZOU
Zhenjun DU
Mingmin LIU
Cite this article:

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.

URL:

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


基于同步动态优化的移动机器人最优速度规划

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


关键词： 移动机器人,  速度规划,  加加速度约束,  同步动态优化,  可达性分析 
Fig.1 Kinematic model of mobile robot
Fig.2 Block diagram of optimal speed planning algorithm for mobile robot
算法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
 
Fig.3 Mobile robot Pioneer 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
Tab.1 Parameter setting in velocity planning simulation
Fig.4 Comparison of velocity curves
Fig.5 Comparison of acceleration curves
算法车轮约束加加速度约束
TOPP-RA√×
Ruckig×√
所提方法√√
Tab.2 Constraints considered by different algorithms
Fig.6 Velocity, acceleration, and jerk profile of wheel and body for various method and restriction
Fig.7 Joint simulation position curve
方向平均位置误差/m最大位置误差/m
TOPP-RA所提方法TOPP-RA所提方法
X0.10810.08580.31920.1701
Y0.10710.05020.28240.1567
Tab.3 Average and maximum position error
[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] Hong-xin CHEN,Bei ZHANG,Chun-xiang WANG,Ming YANG. Robot target following based on adaptive follower mechanism[J]. Journal of ZheJiang University (Engineering Science), 2022, 56(6): 1071-1078.
[2] JIA Song min, LU Ying bin, WANG Li jia, LI Xiu zhi, XU Tao. Mobile robot human tracking using hierarchical features[J]. Journal of ZheJiang University (Engineering Science), 2016, 50(9): 1677-1683.
[3] DING Xia qing, DU Zhuo yang, LU Yi qing, LIU Shan. Visual trajectory planning for mobile robots based on hybrid artificial potential field[J]. Journal of ZheJiang University (Engineering Science), 2016, 50(7): 1298-1306.
[4] LIN Ji-Liang, JIANG Jing-Ping. Diagnosis of simultaneous faults for mobile robots based on fuzzy clustering method[J]. Journal of ZheJiang University (Engineering Science), 2010, 44(3): 453-457.
[5] ZHANG Jian-wei, FAN Zhen, YAN Gang-feng, LIN Zhi-yun. Experimental testbed for cooperative control and coordinated
control of multi-robot systems[J]. Journal of ZheJiang University (Engineering Science), 2010, 44(11): 2124-2129.
[6] CHEN Shao-Bin, JIANG Jing-Ping. Optimal state feedback control for trajectory tracking of four-wheel mobile robot[J]. Journal of ZheJiang University (Engineering Science), 2009, 43(12): 2186-2190.