【轮式移动机器人课程笔记 5】运动学仿真

L5 运动学仿真

• 本节重点： 如何理解推导出的运动学模型
  • 回顾：机器人运动学模型
    • 正向（前向）运动学模型
    • 反向运动学模型
  • 基于MatLab运动学仿真
    • 本节重点，如何利用matlab对运动学进行仿真

5.1 回顾运动学模型

[ x ˙ y ˙ ψ ˙ ] = [ u ⋅ c o s ψ − v ⋅ s i n ψ u ⋅ s i n ψ + v ⋅ c o s ψ r ] = [ c o s ψ − s i n ψ 0 s i n ψ c o s ψ 0 0 0 1 ] [ u v r ] \left[ \begin{matrix} \dot{x}\\ \dot{y}\\ \dot{\psi} \end{matrix} \right] =\left[ \begin{matrix} u\cdot cos\psi - v\cdot sin\psi\\ u\cdot sin\psi + v\cdot cos\psi\\ r \end{matrix} \right] = \left[ \begin{matrix} cos\psi & -sin\psi& 0\\ sin\psi & cos\psi& 0\\ 0 & 0 & 1 \end{matrix} \right]\left[ \begin{matrix} u\\ v\\ r \end{matrix} \right] ​x˙y˙​ψ˙​​ ​= ​u⋅cosψ−v⋅sinψu⋅sinψ+v⋅cosψr​ ​= ​cosψsinψ0​−sinψcosψ0​001​ ​ ​uvr​ ​

• 前向运动学模型

$$\dot{\eta }=J(\psi )\xi$$

• 反向运动学模型

$$\xi =J^{-1}(\psi )\dot{\eta }$$

我们已知上述公式，给出初始位置，给出 $(u,v,r)$ , 计算后续时刻的$(x,y,\psi )$

5.2 运动学模型 MatLab 仿真

以前向运动学模型为例，$\dot{\eta }=J(\psi )\xi$ 。我们已知初始位置下的 $\eta$ , 已知 $\xi$ ，计算后续时刻的 $\eta$ 。 可以将其转化为常微分方程的求解问题，常微分方程求解方法之一为 欧拉法（Euler）。 本节将利用 欧拉法（Euler） 对机器人位置信息进行近似计算。首先简要介绍欧拉法：

• Euler 法

---

---

注*：这里只介绍显示 Euler 法(未按照视频中进行整理)。

① 什么是解微分方程？

我们给出一个微分方程： $y^{\prime }=f(x,y)$ 。一般的解法是根据给出的微分方程先获得
$$y=\int f(x,y)dx+a$$
上式为一般解（通解）。

若给出初始条件，即 $y(x_0)=y_0$, 则可获得微分方程的特殊解（特解）。

② 欧拉法解微分方程

欧拉法是利用直线近似来求解微分方程。

上图中，因 $x$ 是时间 $t$ 上的函数，$x^{\prime }(t)$ 已知。

在图中曲线上对 $t$ 及 $t+1$ 时刻进行采样，分别为 $x_i$ 、$x_{i+1}$ ；$t_{i+1}-t_i={\delta }_t$（此为步长，即取值或采样的间隔是固定的，均为${\delta }_t$）。

欧拉法的思想是将$t_i、t_{i+1}$ 区间的线段看作直线，并利用此段的斜率来近似原微分方程中$t_i$处的导数，用公式表示为：
$$\begin{gathered} x^{\prime }(i)\approx \frac{x_{i+1}-x_i}{t_{i+1}-t_i}=\frac{x_{i+1}-x_i}{{\delta }_t} \\ 则：x_{i+1}-x_i=x^{\prime }(i){\delta }_t \\ {x}_{i+1}=x_i+x^{\prime }(i){\delta }_t \end{gathered}$$
上式中， $x_{i+1}=x_i+x^{\prime }(i){\delta }_t$ 即为欧拉公式（这里要给出初始条件，即 $x(t_0)=x_0$ ），利用迭代可计算 $x_{t+1}$ 的值。

• 利用Euler法对运动学模型进行仿真

%% Kinematic simulation of a land-based mobile robot
clear all; clc; close all;

%% Simulation parameters
dt = 0.1; % Step size 步长
ts = 10; % Simulation time 模拟时间
t = 0:dt:ts;% Time span 时间从0开始，按步长0.1 变化到10

%% Initial conditions
x0 = 0; % x初始位置
y0 = 0; % y初始位置
psi0 = 0; %\psi的初始位置

eta0 = [x0;y0;psi0]; % 对应[x y \psi]

eta(:,1) = eta0 % 将eta0赋值给eta矩阵的第一列

%% Loop starts here
for i = 1:length(t)
    psi = eta(3,i); % current orientation in rad
    %fprintf("psi is %f\n", psi)
    %Jacobian matrix
    J_psi = [cos(psi),-sin(psi),0;
             sin(psi), cos(psi),0;
             0,0,1];
    u = 0.1; % x-axis velocity W.r.t B frame
    v = 0.05; % y-axis velocity W.r.t B frame
    r = 0.0;% angular velocity W.r.t B frame

    zeta(:,i)=[u;v;r];

    eta_dot(:,i) = J_psi * zeta(:,i);

    eta(:,i+1)=eta(:,i)+dt*eta_dot(:,i); %Euler method
end

%% Plotting functions
figure
plot(t, eta(1,1:i), 'r-');
set(gca, 'fontsize',24);
xlabel('t,[s]');
ylabel('x,[m]');

下图为 $t=0$时，机器人位置 为 (0, 0, 0), 初始速度信息为 (0.1, 0.05, 0.1), $t=10$ 内，机器人的位置变化。

---

本节完