CN111267110A - 一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法 - Google Patents
一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法 Download PDFInfo
- Publication number
- CN111267110A CN111267110A CN202010223370.7A CN202010223370A CN111267110A CN 111267110 A CN111267110 A CN 111267110A CN 202010223370 A CN202010223370 A CN 202010223370A CN 111267110 A CN111267110 A CN 111267110A
- Authority
- CN
- China
- Prior art keywords
- robot
- homotopy
- obstacle
- optimal control
- strategy
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Feedback Control In General (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法,首先,根据机器人结构特性建立运动方程、约束条件,用特征圆描述其轮廓。其次,根据作业环境中的障碍信息,用特征圆描述并设定安全距离,建立作业过程中的避障条件。第三,确定机器人轨迹规划问题对应的初始与终端边界条件。第四,将轨迹规划问题转化为以最小化能量消耗的最优控制问题。最后,将构造的最优控制问题转化为一系列子问题,每个子问题中障碍的尺寸相比于前一个子问题不断增大,使用当前子问题的解作为下一个子问题的初始猜测。本发明利用障碍尺寸同伦策略实现复杂作业环境下机器人点对点轨迹规划问题的高效稳定求解,对于机器人实际作业中面临的轨迹规划问题求解具有重要意义。
Description
技术领域
本发明属于机器人轨迹规划领域,涉及一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法。
背景技术
常用的机器人运动轨迹规划方法有图论方法、人工势场法、搜索类方法、快速扩展随机树方法、最优控制方法等。相比于提及的其他几类方法,最优控制方法由于基于机器人的运动学(或动力学)方程,使得规划的轨迹具有天然的可行性。此外,最优控制方法具有简单的数学结构,控制饱和、障碍规避等各类约束可以在统一的框架下进行考虑。机器人实际的作业环境中可能存在着众多的障碍,利用最优控制方法进行求解时将导致非线性程度极高的优化问题,使得求解过程的鲁棒性与效率严重依赖于初始猜测的品质。由于机器人在轨迹规划完成前必须处于精致的等待状态,若求解效率慢或失败,将影响整个作业任务的进程。这一客观事实限制了最优控制方法在实际机器人轨迹规划求解问题中的应用。为此,有必要发展高效的初始化策略,以提高最优控制方法求解复杂障碍环境下机器人轨迹规划问题的性能。
发明内容
为了解决上述技术问题,本发明利用同伦的思想,提出了一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法。本方法将复杂障碍环境下机器人的轨迹规划形成的原始非线性最优控制问题,转化为一系列相对容易求解的子问题的迭代。在初代子问题中,障碍被缩小到较小的尺寸,随着子问题代数的增加,障碍尺寸逐渐增大,并在末代子问题达到障碍物的真实尺寸。每一代子问题的解被用作下一代子问题的初解,相当于构造了一种持续的“热启动”方式,保证了最优控制问题求解的鲁棒性与效率。
为了达到上述目的,本发明采用的技术方案为:
一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法,首先,根据机器人的结构特性建立相应的运动方程、约束条件,用特征圆描述其轮廓。其次,根据作业环境中的障碍信息,用特征圆描述障碍并设定安全距离,从而建立机器人作业过程中的避障条件。第三,确定机器人轨迹规划问题对应的初始边界条件与终端边界条件。第四,根据运动方程、约束条件、初始条件、运动时间,将轨迹规划问题转化为以最小化能量消耗的最优控制问题。最后,利用同伦策略将构造的最优控制问题转化为一系列子问题,每个子问题中障碍的尺寸相比于前一个子问题不断增大,并使用当前子问题的解作为下一个子问题的初始猜测。包括以下步骤:
步骤1:构建机器人相关信息
步骤1-1:建立机器人的运动方程
建立描述机器人运动状态的状态空间x,并确定对应的控制变量u,从而建立如下的机器人运动方程(可以是运动学方程或动力学方程):
其中,t为时间变量。
步骤1-2:确定机器人作业过程中的状态变量和控制变量的约束
机器人作业过程中出于安全考虑,可能对状态变量施加特定的约束条件,本发明将相关的状态变量约束写作如下不等式形式:
CS(x,t)≤0 (2)
同时,为防止控制饱和,应对控制变量施加特定的约束,本发明将相关的控制变量约束写作如下不等式形式:
CC(u,t)≤0 (3)
步骤1-3:用特征圆描述机器人的轮廓
记机器人特征圆的圆心为O(xc,yc),半径为rc,则特征圆可以用如下方程表示:
其中,x和y为分别为横纵坐标。
步骤2:建立障碍规避条件
步骤2-1:用特征圆描述作业环境中的障碍
记作业环境中一共有N个障碍,对于第k个障碍,记其特征圆的圆心为Ok(xk,yk),半径为rk,则特征圆可以用如下方程表示
步骤2-2:设定安全距离以建立避障条件
设定机器人与作业环境中每个障碍的最小安全距离为rs,则机器人与第k个障碍的避障条件表示为
将公式(6)中N个约束中不等号左端的表达式写作如下的矩阵:
则全部N个避障条件可以表示为如下的矩阵形式:
H(x,t)≤0 (8)
为利用障碍尺寸同伦策略实现对轨迹规划问题的稳定高效求解,在矩阵H(x,t)的基础上构造如下的弱化避障约束矩阵:
步骤3:确定轨迹规划问题的边界条件
记轨迹规划问题的初始时刻和终端时刻分别为ts与tf,机器人在初始时刻与终端时刻的状态分别为xs与xf,则轨迹规划问题的边界条件为:
x(ts)=xs,x(tf)=xf (10)
步骤4:建立最优控制模型
根据公式(10)中的边界条件、公式(1)中的系统方程以及公式(2)、(3)、(6)中的约束条件,建立如下的能量最优控制问题:
步骤5:利用同伦策略求解最优控制问题
步骤5-2:求解过程初始化
记p为同伦因子索引,并初始化同伦因子指标p=0。记最优控制问题状态变量和控制变量的初始猜测分别为x[0]与u[0]。
步骤5-3:更新同伦因子索引为p=p+1。
步骤5-4:构造子问题并求解
构造如下的子问题:
步骤5-5:流程判断
本发明相对于现有技术,有益效果为:本发明在含约束非线性最优控制的框架下,利用障碍尺寸同伦策略实现了复杂作业环境下机器人点对点轨迹规划问题的高效稳定求解,对于机器人实际作业中面临的轨迹规划问题求解具有重要意义。本发明中阐述的方法具有很强的可操作性和可行性,便于实际应用。
附图说明
图1为本发明的计算流程图。
图2为本发明实施例中的移动机器人的示意图。
图3为本发明实施例中的作业环境示意图。
图4为本发明实施例中的移动机器人横坐标随时间的变化规律。
图5为本发明实施例中的移动机器人纵坐标随时间的变化规律。
图6为本发明实施例中的移动机器人朝向角随时间的变化规律。
图7为本发明实施例中的移动机器人速度随时间的变化规律。
图8为本发明实施例中的移动机器人前轮转角随时间的变化规律。
图9为本发明实施例中的移动机器人加速度随时间的变化规律。
图10为本发明实施例中的移动机器人轨迹示意图。
具体实施方式
以下结合具体实施例对本发明做进一步说明。
本发明的方法中,首先构建机器人的运动方程、状态变量与控制变量的约束条件,并用特征圆描述机器人的轮廓。第二,根据机器人作业环境中的障碍信息,利用不等式约束描述机器人与障碍的避障条件。第三,根据机器人的当前状态确定轨迹规划问题的初始边界条件,并根据目标状态确定轨迹规划问题的终端边界条件。第四,建立以能量消耗最小为指标的最优控制问题。第五,利用同伦策略构建一系列子问题,实现对原始最优控制问题的迭代求解。本发明的计算流程图如图1所示。
图2为一具有圆形外壳的移动机器人的示意图,特征圆半径rc=1m,圆心为O(xc,yc)。其后轮中心位于特征圆的圆心,轮距为L=0.7m。规定机器人只能向前行进且速度上限为vmax=1m/s,前轮转向角的上限为
图3为作业环境的示意图,其中分布着25个圆形障碍。对于第k(k=1,2,…,25)个圆形障碍,其半径rk=9m,圆心横坐标为xk=25×(kmod5)-25,圆心纵坐标为其中mod为取余符号,为向下取整符号。令安全距离rs=1m。
一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法,包括以下步骤:
步骤1:构建机器人相关信息
步骤1-1:建立机器人的运动方程
选取状态空间x=[xc,yc,θ,v]T,其中θ为机器人的朝向角,v为机器人的速度。选取控制变量u=[u1,u2]T,其中u1=tanβ即前轮转角的正切值,u2为机器人的加速度。从而建立如下的机器人运动学方程
步骤1-2:确定机器人作业过程中的状态变量和控制变量的约束
将机器人运动过程中的状态变量的约束写作CS(x,t)≤0,其中
类似地,将控制变量的约束写作CC(u,t)≤0,其中
步骤1-3:用特征圆描述机器人的轮廓
机器人特征圆方程记作
(x-xc)2+(y-yc)2=12 (16)
步骤2:建立障碍规避条件
步骤2-1:用特征圆描述作业环境中的障碍
作业环境中一共有25个障碍,对于第k个障碍,其特征圆方程为
(x-xk)2+(y-yk)2=92,k=1,2,...,25 (17)
步骤2-2:设定安全距离以建立避障条件
由于机器人与作业环境中每个障碍的最小安全距离为rs=1m,所以机器人与第k个障碍的避障条件表示为
将上述25个约束中不等号左端的表达式写作如下的矩阵
则全部25个避障条件可以表示为如下的矩阵形式
H(x,t)≤0 (20)
为利用障碍尺寸同伦策略实现对轨迹规划问题的稳定高效求解,在矩阵H(x,t)的基础上构造如下的弱化避障约束矩阵
步骤3:确定轨迹规划问题的边界条件
记轨迹规划问题的初始时刻和终端时刻分别为ts=0s与tf=210s,机器人在初始时刻与终端时刻的状态分别为xs=[13,0,π/2,0]T与xf=[87,105,π/2,0]T,则轨迹规划问题的边界条件为
x(ts)=xs,x(tf)=xf (22)
步骤4:建立最优控制模型
根据公式(22)中的边界条件、公式(13)中的系统方程以及公式(14)(15)(20)中的约束条件,建立如下的能量最优控制问题
步骤5:利用障碍尺寸同伦策略求解最优控制问题
步骤5-1:设置同伦因子列表
设置同伦因子列表为α={0.2,0.4,0.6,0.8,1},此时M=5。
步骤5-2:求解过程初始化
记p为同伦因子索引,并初始化同伦因子指标p=0。记最优控制问题状态变量和控制变量的初始猜测分别为x[0]与u[0]。
步骤5-3:更新同伦因子索引
更新同伦因子索引为p=p+1。
步骤5-4:构造子问题并求解
构造如下的子问题
步骤5-5:流程判断
根据上述步骤,计算得到的移动机器人的横坐标、纵坐标、朝向角以及速度随时间的变化规律如图4~图7所示,计算得到的前轮转角和加速度如图8~图9所示,轨迹示意图如图10所示。
以上所述实施例仅表达本发明的实施方式,但并不能因此而理解为对本发明专利的范围的限制,应当指出,对于本领域的技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改进,这些均属于本发明的保护范围。
Claims (1)
1.一种基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法,其特征在于,包括以下步骤:
步骤1:构建机器人相关信息
步骤1-1:建立机器人的运动方程
建立描述机器人运动状态的状态空间x,并确定对应的控制变量u,从而建立如下的机器人运动方程:
其中,t为时间变量;
步骤1-2:确定机器人作业过程中的状态变量和控制变量的约束
机器人作业过程中出于安全考虑,可能对状态变量施加特定的约束条件,将相关的状态变量约束采用如下不等式形式:
CS(x,t)≤0 (2)
同时,为防止控制饱和,对控制变量施加特定的约束,将相关的控制变量约束采用如下不等式形式:
CC(u,t)≤0 (3)
步骤1-3:采用特征圆描述机器人的轮廓
记机器人特征圆的圆心为O(xc,yc),半径为rc,则特征圆采用如下方程表示:
步骤2:建立障碍规避条件
步骤2-1:采用特征圆描述作业环境中的障碍
记作业环境中一共有N个障碍,对于第k个障碍,记其特征圆的圆心为Ok(xk,yk),半径为rk,则特征圆采用如下方程表示:
步骤2-2:设定安全距离以建立避障条件
设定机器人与作业环境中每个障碍的最小安全距离为rs,则机器人与第k个障碍的避障条件表示为:
将公式(6)中N个约束中不等号左端的表达式写作如下的矩阵:
则全部N个避障条件采用如下矩阵形式表示:
H(x,t)≤0 (8)
为利用障碍尺寸同伦策略实现对轨迹规划问题的稳定高效求解,在矩阵H(x,t)的基础上构造如下的弱化避障约束矩阵:
步骤3:确定轨迹规划问题的边界条件
记轨迹规划问题的初始时刻和终端时刻分别为ts与tf,机器人在初始时刻与终端时刻的状态分别为xs与xf,则轨迹规划问题的边界条件为:
x(ts)=xs,x(tf)=xf (10)
步骤4:建立最优控制模型
根据公式(10)中的边界条件、公式(1)中的系统方程以及公式(2)、(3)、(6)中的约束条件,建立如下的能量最优控制问题:
步骤5:利用同伦策略求解最优控制问题
步骤5-2:求解过程初始化
记p为同伦因子索引,并初始化同伦因子指标p=0;记最优控制问题状态变量和控制变量的初始猜测分别为x[0]与u[0];
步骤5-3:更新同伦因子索引为p=p+1;
步骤5-4:构造子问题并求解,构造的子问题如下:
步骤5-5:流程判断
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010223370.7A CN111267110B (zh) | 2020-03-26 | 2020-03-26 | 基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010223370.7A CN111267110B (zh) | 2020-03-26 | 2020-03-26 | 基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111267110A true CN111267110A (zh) | 2020-06-12 |
CN111267110B CN111267110B (zh) | 2021-07-16 |
Family
ID=70993071
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010223370.7A Active CN111267110B (zh) | 2020-03-26 | 2020-03-26 | 基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111267110B (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111596668A (zh) * | 2020-06-17 | 2020-08-28 | 苏州大学 | 基于逆向强化学习的移动机器人拟人化路径规划方法 |
CN111704038A (zh) * | 2020-07-13 | 2020-09-25 | 大连理工大学 | 一种考虑避障的桥式起重机路径规划方法 |
CN112015183A (zh) * | 2020-09-08 | 2020-12-01 | 安徽工程大学 | 一种能耗约束下在具有凹凸地形中移动机器人避障方法 |
CN114911263A (zh) * | 2022-06-10 | 2022-08-16 | 哈尔滨工业大学 | 基于同伦法的多无人机同步到达轨迹规划方法、存储介质及设备 |
CN117444985A (zh) * | 2023-12-20 | 2024-01-26 | 安徽大学 | 一种机械臂小车控制方法及系统 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5844821A (en) * | 1997-04-29 | 1998-12-01 | Lucent Technologies Inc. | Systems and methods for determining characteristics of a singular circuit |
KR20110137208A (ko) * | 2010-06-16 | 2011-12-22 | 서강대학교산학협력단 | 공벌레 형태를 모방한 이동장치 |
CN107562057A (zh) * | 2017-09-07 | 2018-01-09 | 南京昱晟机器人科技有限公司 | 一种机器人智能导航控制方法 |
CN107966697A (zh) * | 2017-11-07 | 2018-04-27 | 浙江工业大学 | 一种基于渐进无迹卡尔曼的移动目标跟踪方法 |
CN110412877A (zh) * | 2019-08-30 | 2019-11-05 | 中国人民解放军海军航空大学 | 一种基于nsp算法的舰载机甲板路径规划最优控制方法 |
-
2020
- 2020-03-26 CN CN202010223370.7A patent/CN111267110B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5844821A (en) * | 1997-04-29 | 1998-12-01 | Lucent Technologies Inc. | Systems and methods for determining characteristics of a singular circuit |
KR20110137208A (ko) * | 2010-06-16 | 2011-12-22 | 서강대학교산학협력단 | 공벌레 형태를 모방한 이동장치 |
CN107562057A (zh) * | 2017-09-07 | 2018-01-09 | 南京昱晟机器人科技有限公司 | 一种机器人智能导航控制方法 |
CN107966697A (zh) * | 2017-11-07 | 2018-04-27 | 浙江工业大学 | 一种基于渐进无迹卡尔曼的移动目标跟踪方法 |
CN110412877A (zh) * | 2019-08-30 | 2019-11-05 | 中国人民解放军海军航空大学 | 一种基于nsp算法的舰载机甲板路径规划最优控制方法 |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111596668A (zh) * | 2020-06-17 | 2020-08-28 | 苏州大学 | 基于逆向强化学习的移动机器人拟人化路径规划方法 |
CN111704038A (zh) * | 2020-07-13 | 2020-09-25 | 大连理工大学 | 一种考虑避障的桥式起重机路径规划方法 |
CN112015183A (zh) * | 2020-09-08 | 2020-12-01 | 安徽工程大学 | 一种能耗约束下在具有凹凸地形中移动机器人避障方法 |
CN112015183B (zh) * | 2020-09-08 | 2022-02-08 | 安徽工程大学 | 一种能耗约束下在具有凹凸地形中移动机器人避障方法 |
CN114911263A (zh) * | 2022-06-10 | 2022-08-16 | 哈尔滨工业大学 | 基于同伦法的多无人机同步到达轨迹规划方法、存储介质及设备 |
CN117444985A (zh) * | 2023-12-20 | 2024-01-26 | 安徽大学 | 一种机械臂小车控制方法及系统 |
CN117444985B (zh) * | 2023-12-20 | 2024-03-12 | 安徽大学 | 一种机械臂小车控制方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN111267110B (zh) | 2021-07-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111267110B (zh) | 基于障碍尺寸同伦策略的机器人轨迹规划最优控制方法 | |
CN101436073A (zh) | 基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法 | |
CN107168369A (zh) | 一种欠驱动无人车的编队控制方法及系统 | |
CN114510063B (zh) | 一种无人履带车辆及其轨迹跟踪控制方法和系统 | |
CN113435025B (zh) | 一种结合多级优化模型的机器人高性能轨迹自动生成方法 | |
CN112061116B (zh) | 一种基于势能场函数逼近的强化学习方法的泊车策略 | |
CN105159294A (zh) | 针对叉车模糊逻辑运动控制器的设计方法 | |
CN104331083A (zh) | 一种航天器大角度姿态控制参数优化方法 | |
CN110543727B (zh) | 一种基于改进粒子群算法的全向移动智能轮椅机器人参数辨识方法 | |
CN113848905B (zh) | 基于神经网络和自适应控制的移动机器人轨迹跟踪方法 | |
CN113391633A (zh) | 一种面向城市环境的移动机器人融合路径规划方法 | |
CN114415730B (zh) | 航天器逃逸轨迹智能规划方法 | |
CN104992023A (zh) | 一种基于状态类型函数的飞行器参数化设计方法 | |
CN115062261A (zh) | 基于反向搜索的曲率连续自主泊车路径规划方法及系统 | |
CN115179280A (zh) | 一种用于强化学习机械臂控制中基于磁场的奖励塑形方法 | |
CN110765706A (zh) | 基于ohngbm(1,1)的翼型非定常失速气动系数建模方法 | |
CN110597067B (zh) | 一种多移动机器人的群集控制方法及系统 | |
CN116560218A (zh) | 一种分数阶-线性项迭代学习控制方法及介质 | |
Shen et al. | An Efficient Global Trajectory Planner for Highly Dynamical Nonholonomic Autonomous Vehicles on 3D Terrains | |
Zhang et al. | Design of agv chassis motion control system based on mworks | |
CN112734079B (zh) | 一种基于优化路径规划算法的刚性折纸折叠方法 | |
CN115439581A (zh) | 一种基于深度学习的身体动画绑定加速方法 | |
CN115373287A (zh) | 一种折腰转向拖拉机的自适应参数模型预测路径跟踪控制方法 | |
Sun et al. | Bayesian and convex optimization of racing line and mpc path following control for 4wis&4wid autonomous vehicle | |
CN114764251B (zh) | 一种基于能耗模型的多智能体协同搜索节能方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |