差分移动机器人的运动模型

本文深入探讨了移动机器人在SLAM(Simultaneous Localization and Mapping)中里程计积分的重要性,解析了不同积分方法如欧拉法与龙格库塔法(RK2)在精度上的差异。通过对比分析,阐述了pro1里程计模型如何实现更精确的位置估计,以及其与圆弧模型的关系。此外,还介绍了diego标定方法在里程计积分中的应用。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

转载添加链接描述

对于移动机器人来讲,由两个矩阵特别重要

//from base_link to global odom
ca  -sa  0
sa   ca  0
0    0   1
//from odom to base link
ca  sa   0
-sa ca   0
0   0    1
//两个相乘为单位矩阵,所以就是互逆

航迹推演

短时间内圆弧和直线近似

转向角和转向角一半的数学关系

参数化

速度

如上,在里程计积分时,
有的地方 * cos(theta ) (属于欧拉法)
有的地方 * cos(theta + 1/2 △) (属于龙格库塔法 rk2),理论上精度更高一点

模型

旷世slam
圆形轨迹
pro1里程计积分
pro1的里程计模型本质上相当于rk2, 使用位置环计算位移和角速度,delta_dis/delta_theta 求出半径,然后计算在前一个位置位移的x,y, 再积分(后一个位置做向前一个位置半径的垂线,再前一个位置x,y坐标分解)
切线,割线,圆弧模型通俗理解

标定

diego 标定
本质上标定的里程计积分中的delta x ,delta theta,

### 差分移动机器人模型预测控制(MPC)实现方法 #### 1. 模型预测控制基本原理 模型预测控制是一种基于优化的方法,通过在线解决有限时间范围内的最优控制问题来计算当前时刻的最佳输入序列。对于差分移动机器人,其核心在于构建合适的动态模型并将其转化为可求解的优化问题。 在轨迹跟踪应用中,MPC 的建模可以分为两类:基于运动学的状态方程建模和基于动力学的状态方程建模[^2]。由于差分移动机器人通常运行在较低速度下,因此更常用的是基于运动学的建模方式。 --- #### 2. 双轮差速运动学模型 双轮差速移动机器人的运动可以用如下连续时间状态空间表示: \[ \begin{aligned} \dot{x} &= v \cos(\theta), \\ \dot{y} &= v \sin(\theta), \\ \dot{\theta} &= \frac{v}{L} \tan(\phi), \end{aligned} \] 其中 \(x\) 和 \(y\) 表示机器人中心的位置坐标,\(\theta\) 是朝向角,\(v\) 是线速度,\(\phi\) 是车轮转向角,\(L\) 是轴距长度。为了简化控制器设计,可以通过假设或近似处理将该非线性模型线性化[^1]。 --- #### 3. 状态方程的线性化与离散化 通过对上述非线性模型进行泰勒级数展开,并忽略高阶项,得到局部线性化的状态转移矩阵形式: \[ \Delta X_{k+1} = A_k \Delta X_k + B_k \Delta U_k, \] 其中, - \(\Delta X_k = [e_x, e_y, e_\theta]^T\) 表示当前位置误差; - \(\Delta U_k = [\Delta v, \Delta w]^T\) 表示控制增量; - \(A_k\) 和 \(B_k\) 分别为雅可比矩阵,在特定工作点处计算得出。 随后利用后向差分法完成系统的离散化过程,从而便于数值求解。 --- #### 4. 预测方程与优化目标函数 给定未来 N 步的时间窗口,定义代价函数 J 如下: \[ J(U) = \sum_{i=0}^{N_p-1} (X_i - R)^T Q (X_i - R) + \sum_{j=0}^{N_c-1} (\Delta U_j)^T R_d (\Delta U_j), \] 这里: - \(Q\) 和 \(R_d\) 分别代表状态偏差权重矩阵以及控制变化量惩罚系数; - \(N_p\) 称作预测步长; - \(N_c\) 则指代控制更新周期数目。 最终目的是寻找使总成本最小的一组控制指令集合 {Δu₀,..., Δuₙ₋₁} ,并通过滚动时域策略不断重复此操作直至达到期望终点位置附近区域为止。 --- #### 5. 约束条件设置 实际工程实践中还需考虑物理层面的各种限制因素,比如最大允许加减速率、电机电流极限值等等。这些都可以作为附加不等式加入到原二次规划(QP)框架之中加以体现出来: \[ C_min ≤ C(X,U) ≤ C_max. \] --- #### 6. 数值求解器的选择 针对所形成的标准化QP子问题可以选择多种高效算法予以解答,例如Gurobi、OSQP或者是CasADi工具包内部集成的相关功能模块均能胜任此项任务需求。 --- ```python import numpy as np from scipy.optimize import minimize def mpc_control(x_current, u_prev, ref_traj, params): """ Model Predictive Control for differential drive robot. Args: x_current: Current state vector [x; y; theta]. u_prev: Previous control input [v; omega]. ref_traj: Reference trajectory points [[xr]; [yr]]. params: System parameters including L, dt etc. Returns: Optimal control inputs [v*, omega*]. """ def cost_function(u_flat): # Reshape flat array into matrix form u_pred = u_flat.reshape((-1, 2)) # Initialize variables total_cost = 0 x_pred = x_current.copy() for k in range(len(ref_traj)): # State prediction using kinematic model dx = u_pred[k][0]*np.cos(x_pred[2]) dy = u_pred[k][0]*np.sin(x_pred[2]) dtheta = u_pred[k][1] x_next = x_pred + np.array([dx, dy, dtheta]) * params['dt'] # Compute error and add to the cost function err_pos = ((ref_traj[k][:2]-x_next[:2])**2).sum() err_theta = abs((ref_traj[k][2]-x_next[2])) ctrl_effort = sum(abs(u_pred[k])) total_cost += err_pos + err_theta + ctrl_effort x_pred = x_next return total_cost res = minimize(cost_function, np.zeros(2*Np), method='SLSQP') return res.x[:2].reshape(-1) # Example usage of MPC controller with dummy data if __name__ == "__main__": initial_state = np.array([0., 0., 0.]) # Initial position & orientation prev_input = np.array([0.5, 0.]) # Last applied velocities reference_points = [(1., 1.), (-1., -1.)] # Desired waypoints system_params = {'L': 0.5, 'dt': 0.1} optimal_u = mpc_control(initial_state, prev_input, reference_points, system_params) print(f"Optimized Controls: Linear Velocity={optimal_u[0]}, Angular Velocity={optimal_u[1]}") ``` ---
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值