一种球形机器人轨迹优化方法.pdf

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN115016483A(43)申请公布日2022.09.06(21)申请号202210697890.0(22)申请日2022.06.20(71)申请人逻腾（台州）科技有限公司地址318020浙江省台州市黄岩区江口街道永达路148号7号楼3-4层(72)发明人王酉官孝清曹勖之胡涛李光(74)专利代理机构杭州知见专利代理有限公司33295专利代理师卢金元(51)Int.Cl.G05D1/02(2020.01)权利要求书2页说明书5页附图5页(54)发明名称一种球形机器人轨迹优化方法(57)摘要本发明公开了一种球形机器人轨迹优化方法，其包括以下步骤：S1、从全局路径中获取目标状态信息；从IMU获得横摇角响应和球形机器人当前的坐标和姿态角；S2、初始化预测参数；S3、使用动力学模型获得预测状态；S4、使用运动学模型获得预测轨迹；S5、使用误差函数计算误差，如果误差小于规定误差，或者循环已经大于或等于运行时长阈值，则停止迭代，进入步骤S6，如果误差大于或等于规定误差并且循环时间小于运行时长阈值，则用IPOPT更新预测参数后重新进入步骤S3；S6、将姿态控制参数下发给姿态控制器。本方案能够引导球形机器人始终以稳定的姿态跟踪全局路径，对长时间运行具有良好的稳定性和鲁棒性。CN115016483ACN115016483A权利要求书1/2页1.一种球形机器人轨迹优化方法，其特征在于，包括以下步骤：S1、从全局路径中获取目标状态信息，所述目标状态信息包括目标的坐标和姿态角；从惯性测量单元获得横摇角响应和球形机器人当前的坐标和姿态角；从距离场地图获取球形机器人与障碍物的距离；S2、初始化预测参数，预测参数包括v1,…,vn和θ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),…,(vn,θn)。3.根据权利要求2所述的一种球形机器人轨迹优化方法，其特征在于，所述运动学模型的计算过程如下：式中，当s＝0时，(x0,y0)为球形机器人当前坐标，φ0为球形机器人当前姿态角，运动学模型的输出为预测轨迹(x1,y1,φ1),(x2,y2,φ2),…,(xn,yn,φn)。4.根据权利要求3所述的一种球形机器人轨迹优化方法，其特征在于，误差的计算方式具体为：J是误差值，式中，H1、H2、H3、H4为系数矩阵，(xgoal,ygoal)为球形机器人的目标坐标，φgoal为球形机器人的目标姿态角，vlast和θlast为上一次发送到姿态控制器的姿态控制参数，ESDF为欧几里得符号距离场。5.根据权利要求4所述的一种球形机器人轨迹优化方法，其特征在于，步骤S6中，姿态控制参数包括vset和θset，其中vset为最后一次的v1；vset的极限为[‑0.6m/s，0.6m/s]，θset的2CN115016483A权利要求书2/2页极限为[‑15°，15°]，如果姿态控制参数在极限之外，则以将姿态控制参数设为最靠近的极限边缘值。6.根据权利要求4或5所述的一种球形机器人轨迹优化方法，其特征在于，系数矩阵为：H1＝DIAG(3000,3000,2000)，H2＝DIAG(30000,1000)，H3＝1000，H4＝100。3CN115016483A说明书1/5页一种球形机器人轨迹优化方法技术领域[0001]本发明涉及一种球形机器人控制领域，尤其是涉及一种球形机器人轨迹优化方法。背景技术[0002]本发明所涉及的球形机器人如图1所示，它可以简化为两个组件：一个圆形球壳和一个二自由度的重摆。二自由度重摆可以分为两个部分：一个重摆和一个带有两个电机的框架。该摆由两个电机驱动，绕机器人的水平和纵轴旋转。水平(长)轴电机用于控制球形机器人的速度，纵(短)轴电机通过改变球壳与地面的接触点来控制方向。这样，通过控制两个电机，球形机器人就可以在平面上实现非零转弯半径的全方位运动。因此球形机器人相比起轮式和足式机器人，具有独特的运动学、动力学特性。球形机器人在运动过程中的示意图如图2所示。球形机器人具有零转弯半径，抗击打等特性