一种球形机器人轨迹优化方法技术

本发明专利技术公开了一种球形机器人轨迹优化方法，其包括以下步骤：S1、从全局路径中获取目标状态信息；从IMU获得横摇角响应和球形机器人当前的坐标和姿态角；S2、初始化预测参数；S3、使用动力学模型获得预测状态；S4、使用运动学模型获得预测轨迹；S5、使用误差函数计算误差，如果误差小于规定误差，或者循环已经大于或等于运行时长阈值，则停止迭代，进入步骤S6，如果误差大于或等于规定误差并且循环时间小于运行时长阈值，则用IPOPT更新预测参数后重新进入步骤S3；S6、将姿态控制参数下发给姿态控制器。本方案能够引导球形机器人始终以稳定的姿态跟踪全局路径，对长时间运行具有良好的稳定性和鲁棒性。性和鲁棒性。性和鲁棒性。

【技术实现步骤摘要】
一种球形机器人轨迹优化方法


[0001]本专利技术涉及一种球形机器人控制领域，尤其是涉及一种球形机器人轨迹优化方法。

技术介绍

[0002]本专利技术所涉及的球形机器人如图1所示，它可以简化为两个组件：一个圆形球壳和一个二自由度的重摆。二自由度重摆可以分为两个部分：一个重摆和一个带有两个电机的框架。该摆由两个电机驱动，绕机器人的水平和纵轴旋转。水平(长)轴电机用于控制球形机器人的速度，纵(短)轴电机通过改变球壳与地面的接触点来控制方向。这样，通过控制两个电机，球形机器人就可以在平面上实现非零转弯半径的全方位运动。因此球形机器人相比起轮式和足式机器人，具有独特的运动学、动力学特性。球形机器人在运动过程中的示意图如图2所示。球形机器人具有零转弯半径，抗击打等特性，在抢险巡检中具有独特的优势。目前没有开源的，应用于球形机器人的局部轨迹优化算法论文或专利。将通用的teb算法在仅仅修改参数的基础上移植到球形机器人上，效果并不好。

技术实现思路

[0003]本专利技术主要是解决现有技术所存在的缺乏合适的球形机器人轨迹优化方法的技术问题，提供一种稳定性好、资源要求低、鲁棒性高的球形机器人轨迹优化方法。
[0004]本专利技术针对上述技术问题主要是通过下述技术方案得以解决的：一种球形机器人轨迹优化方法，包括以下步骤：
[0005]S1、从全局路径中获取目标状态信息，所述目标状态信息包括目标的坐标(x
goal
,y
goal
)和姿态角φ
goal
；从惯性测量单元(IMU)获得横摇角响应(包括当前时刻的横摇角响应θ0以及上一时刻的横摇角响应θ
‑1)和球形机器人当前的坐标(x0,y0)和姿态角φ0；从距离场地图获取球形机器人与障碍物的距离；
[0006]S2、初始化预测参数，预测参数包括v1,
…
,v
n
和θ
set
；初始化的方式为在极限范围内随机取值，极限范围即为下文所述的v
set
和θ
set
的范围；
[0007]S3、使用动力学模型获得预测状态；
[0008]S4、使用运动学模型获得预测轨迹；
[0009]S5、使用误差函数计算误差，如果误差小于规定误差(一般设为80
‑
120)，或者循环已经大于或等于运行时长阈值，则停止迭代，进入步骤S6，如果误差大于或等于规定误差并且循环时间小于运行时长阈值，则用非线性优化求解器(IPOPT)更新预测参数后重新进入步骤S3；
[0010]S6、将姿态控制参数下发给姿态控制器。
[0011]非线性优化求解器采用内点法对预测参数进行更新。n的值根据实际需求确定，一般为15
‑
25。
[0012]作为优选，所述动力学模型的输入为预测参数(v1,
…
,v
n
和θ
set
)、当前时刻的横摇
角响应θ0以及上一时刻的横摇角响应θ
‑1，具体公式如下：
[0013]θ
k
‑
0.9182θ
k
‑1+0.002835θ
k
‑2＝
‑
0.3165θ0+0.3695θ
‑1n≥k≥1
[0014]动力学模型的输出为预测状态(v1,θ1),(v2,θ2),
…
,(v
n
,θ
n
)，即预测得到的后续n个时刻点球形机器人的状态。
[0015]作为优选，所述运动学模型的计算过程如下：
[0016][0017]式中，当s＝0时，(x0,y0)为球形机器人当前坐标，φ0为球形机器人当前姿态角，运动学模型的输出为预测轨迹(x1,y1,φ1),(x2,y2,φ2),
…
,(x
n
,y
n
,φ
n
)，即后预测到的后续n个时刻点球形机器人的轨迹，包括坐标(x,y)和姿态角φ。dt是相邻两个采样时刻的时间差。
[0018]作为优选，误差的计算方式具体为：
[0019][0020][0021][0022]J是误差值，式中，H1、H2、H3和H4为系数矩阵，(x
goal
,y
goal
)为球形机器人的目标坐标，φ
goal
为球形机器人的目标姿态角，v
last
和θ
last
为上一次发送到姿态控制器的姿态控制参数，ESDF为欧几里得符号距离场。
[0023]作为优选，步骤S6中，姿态控制参数包括v
set
和θ
set
，其中v
set
为最后一次的v1；v
set
的极限为[
‑
0.6m/s，0.6m/s]，θ
set
的极限为[
‑
15
°
，15
°
]，如果姿态控制参数在极限之外，则以将姿态控制参数设为最靠近的极限边缘值。
[0024]作为优选，系数矩阵为：
[0025]H1＝DIAG(3000,3000,2000)，H2＝DIAG(30000,1000)，H3＝1000，H4＝100。
[0026]本专利技术带来的实质性效果是，可以使球形机器人以更稳定的姿态和更高的成功率完成挑战路径，从而提高机器人的工作范围和传感器数据的质量。
附图说明
[0027]图1是球形机器人的结构示意图；
[0028]图2和图3是球形机器人运动过程的示意图；
[0029]图4是本专利技术的一种球形机器人轨迹优化方法流程图；
[0030]图5是S弯实验的轨迹结果，左侧为全局规划器的轨迹，右侧为本方案的轨迹；
[0031]图6是S弯实验的速度曲线；
[0032]图7是S弯实验的横滚角曲线。
具体实施方式
[0033]下面通过实施例，并结合附图，对本专利技术的技术方案作进一步具体的说明。
[0034]实施例：根据球形机器人各部件的相对运动关系，可以将球形机器人简化为三个组成部分：重摆，骨架，外壳，并用拉格朗日方法对其进行建模。
[0035]运动学模型的输入为机器人的(v，θ)，输出为机器人的(x，y，)。如图2所示，由于机械结构的限制，机器人具有由机器人的θ决定的非零转弯半径R＝r/tanθ。从图3可以看出，的变化实际上是转弯半径的旋转角，可以根据转弯半径和移动距离来计算，正负号取决于是左转还是右转。
[0036]一种球形机器人轨迹优化方法，如图4所示，包括以下步骤：
[0037]S1、从全局路径中获取目标状态信息，所述目标状态信息包括目标的坐标(x
goal
,y
goal
)和姿态角φ
goal
；从惯性本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种球形机器人轨迹优化方法，其特征在于，包括以下步骤：S1、从全局路径中获取目标状态信息，所述目标状态信息包括目标的坐标和姿态角；从惯性测量单元获得横摇角响应和球形机器人当前的坐标和姿态角；从距离场地图获取球形机器人与障碍物的距离；S2、初始化预测参数，预测参数包括v1,
…
,v
n
和θ
set
；S3、使用动力学模型获得预测状态；S4、使用运动学模型获得预测轨迹；S5、使用误差函数计算误差，如果误差小于规定误差，或者循环已经大于或等于运行时长阈值，则停止迭代，进入步骤S6，如果误差大于或等于规定误差并且循环时间小于运行时长阈值，则用非线性优化求解器更新预测参数后重新进入步骤S3；S6、将姿态控制参数下发给姿态控制器。2.根据权利要求1所述的一种球形机器人轨迹优化方法，其特征在于，所述动力学模型的输入为预测参数、当前时刻的横摇角响应θ0以及上一时刻的横摇角响应θ
‑1，具体公式如下：θ
k
‑
0.9182θ
k
‑1+0.002835θ
k
‑2＝
‑
0.3165θ0+0.3695θ
‑1n≥k≥1动力学模型的输出为预测状态(v1,θ1),(v2,θ2),
…
,(v
n
,θ
n
)。3.根据权利要求2所述的一种球形机器人轨迹优化方法，其特征在于，所述运动学模型的计算过程如下：式中，当s＝0时，(x0,y0)为球形机器人当前坐标，φ0为球形机器人当前姿态...

【专利技术属性】
技术研发人员：王酉，官孝清，曹勖之，胡涛，李光，
申请(专利权)人：逻腾台州科技有限公司，
类型：发明
国别省市：