基于运动合成分解的舵轮底盘运动模型（以正三角形三轮底盘为例）

于 2024-04-19 12:50:15 发布
阅读量267 收藏
点赞数
分类专栏： 其他 文章标签： 算法
原文链接：https://blog.csdn.net/m0_62592252/article/details/126129332
版权

目录

1.总述

2.车轮参考方向

3.坐标系

4.停止

5.仅平移

6.仅旋转

7.平移旋转

8.电机驱动

9.原方案（弃用）

正三角形

正方形

等腰三角形

等腰三角形（重制）

附录

现代码

原代码

头文件

此文档原本是对附录中代码的解释，也可单独作为舵轮底盘分析的参考，除第9节的弃用方案外都已经过实践验证，第9节内容及原代码虽最终未被采用但其思路方法或许仍有些许值得参考借鉴之处，故也保留未删，也欢迎批判指正。各小节排列顺序即程序中顺序，代码已有较详细的注释，不再逐一说明功能。

以下所有出现的变量除有说明外，都是结构体Wheel3（三轮底盘）中的变量，所有角度变量若无说明都为弧度制。由于调试时有过多次修改，部分代码可能和说明不一致。

程序代码可分为三部分，第一部分为世界坐标系转机器人坐标系，单独作为一个函数；第二部分是另一个函数的上半部分，运动模型分析；剩余是第三部分，利用运动模型输出驱动电机。第二三部分间以分割线隔开。 

绘图所使用软件为GeoGebra。

1.总述

车体在平面上的运动有三个自由度，分别为二维平面上位置的两个自由度和自身旋转的一个自由度，可以用三个变量表示，分别为

与车头方向垂直的速度（设向右为正方向）：V_x

 与车头方向同向的速度（设向前为正方向）：V_y

使车体自旋的角速度（程序中为V_z）：\omega

这三个变量是运动模型的仅有的三个输入量，构成车体运动状态的三维向量（以下称作运动状态），需要根据它计算出每个轮应转到的角度\theta(程序中未)和运动速度V，即在二维平面上的速度向量（以下称作速度向量），每个轮都有一个这样的速度向量。此运动模型所实现的功能可以用如下变化表示，由于底盘上有三个舵轮，右侧实际是一个2*3的矩阵：

为了后续处理的便利，在函数内定义平移合速度V,\left|V\right|=\sqrt{​{V_x}^2+{V_y}^2}=\omega R，其中R为旋转半径，R=\frac{V}{\omega}。

直接求解车体在某一运动状态下各舵轮的速度向量较为复杂，将运动分解后分析以简化过程。车体的移动速度可以分为在二维平面xy上平移的速度和自身旋转速度两部分，在某一运动状态下，分别计算车体平移和旋转时每个轮子的速度向量，并将这两个速度合成即可计算出这一运动状态各轮所需的速度向量。此方法可以在运动状态的三个变量取包括0在内的任意值时使用，不需分类讨论。

以下为曾用方案（理论可行，见第9部分及附录中原代码，实际未实现预期效果）

可以把所有运动都看成绕车体平面任意一点的圆周运动，直线运动和自旋只是他的特殊极限运动状态；车体包括轮子在内的所有组成部分围绕某一点做圆周运动时角速度\omega相同，线速度和圆心垂直。所以所有轮子和车体自旋的角速度都为\omega。  

注意到当角速度\omega为零时半径R为无穷，无法正常计算，故单独编写。此时为简单平移，所以轮的运动速度都为V，若V_x不为零则转过角度在世界坐标系中角度为arccos[\frac{V_x}{V}sgn(V_y)],若V_x为零则角度根据V_y为向前或后对应角度。

2.车轮参考方向

定义当电机正向旋转时，使底盘能顺时针旋转的方向为模型中各舵轮的参考方向，也是各舵轮起始时应在的位置，在此位置编码器的读数为0，如图2-1所示。A、B、C三点代表三个舵轮，A、B、C轮编号依次为1、2、3。

图2-1 正三角形模型参考方向

3.坐标系

在运动模型分析时都以机器人坐标系为参考坐标系，其xy方向即速度的xy方向，但也可以通过坐标系转化的方法使运动的坐标系为世界坐标系。具体方法为给出特定的世界坐标系下的速度，利用坐标系转换公式将此速度化为机器人坐标系下的速度，在此过程中应当注意符号的正负问题，尤其是旋转正方向，在调试时曾因设定的旋转正方向和传感器的正方向相反导致车身旋转时坐标系与车身旋转方向同向加倍旋转的情况，之后修改了转换公式，使x轴镜像改变了旋转正方向，解决了此问题。

图3-1 角度方向

采用的坐标系是常用的右手螺旋直角坐标系（由于只在平面上运动，不涉及z轴，z代表旋转），角度从x正方向开始随着逆时针旋转逐渐增大，反之为负，如图3-1中α角度是正值，β角度是负值。

 图3-2 机器人坐标系和世界坐标系

如图3-2所示，设图中坐标轴为世界坐标系，机器人抽象为A点，向量u为x轴正方向，v为y轴正方向。当机器人运动时，u、v也会跟随机器人一起运动，而世界坐标系不受影响，根据需求的不同可以选择不同坐标系，如要使机器人移动到场地中某一位置时使用世界坐标系会比机器人坐标系更合适，要机器人进行前进后退等运动时则使用机器人坐标系更合适。

图3-3世界坐标系速度转机器人坐标系速度

如图3-3，由于模型中所有平移运动都是基于机器人坐标系实现的，假如要使机器人以世界坐标系中速度w运动，需要先将此速度转化为机器人坐标系中的速度a、b，之后根据a、b进行计算。旋转速度在两个坐标系中是一致的，不需要转化，可以直接赋值。

4.停止

设计有两种停止情况：

  1. 没有收到运动指令（运动状态三个值都为0，无操作时自动进入），即模型部分程序中最后一个else，不进行任何动作，所有车轮方向保持不变，速度都设置为0。当运动状态有非0值时可以立刻继续运动。
  2. 驻停。通过设置halt的值，当值非0时执行驻停。所有轮都将转至与车体旋转时轮方向的垂直方向，设置速度为0，此时车身不易因外界原因移动，也不会响应运动指令。此状态可以防止不经意误触导致车运动，也可以防止在机器人发射球时防止车身因后坐力旋转移动，但进入此状态时会导致车身偏转一定角度，导致瞄准偏差，尝试过调整车轮旋转方向和旋转速度，未能有效解决此问题，最终未在射球时使用。

5.仅平移

此时车体自旋的角速度\omega为0，仅有与车体方向同方向的速度V_x与车体方向垂直的速度V_y（其中可有一个为0）。可以计算出其合速度V，此时旋转半径R=V\omega无穷大。使车往指定方向运动只需将所有车轮朝向同一方向并转动。

使用平方和可以计算合速度大小，利用反三角函数可以计算出V_x和V_y不同比例时，在车坐标系下对应的角度。程序中使用如下公式：

theta=acos(\frac{V_x}{V})

\left|V\right|=\sqrt{​{V_x}^2+{V_y}^2}

图5-1中u、v代表V_x和V_y，w即合速度V，通过上述公式即可获得需要的值。程序中角度返回值为弧度制，经加减2π限幅后在[-π, π]范围内，以便后续驱动电机转动时的计算。

图5-1 旋转角度及移动速度

特殊地，因V_x=0时返回值为0，无法区分V_y的正负方向，单独对此时V_y正负的情况进行定义，V_y为正时角度π/2，V_y为负时角度-π/2。

6.仅旋转

此时车体自旋的角速度\omega不为0，平移合速度V为0，车绕自身中心旋转，旋转半径R=\frac{V}{\omega}=0。

此时所有轮都将转动到能使车体转动的特定角度（具体角度根据底盘形状而定，速度向量垂线过几何中心，一车如图1-1，即各车轮参考方向），各轮速度都为车身尺寸参数L乘车体自旋的角速度\omega，车身尺寸参数L是各轮到旋转中心的距离，单位自定，用于使不同底盘在同样旋转速度下效果一致。

7.平移旋转

图7-1 机器人坐标平移旋转 

 图7-2 世界坐标平移旋转

使用平移和旋转速度正交分解后叠加即可同时平移旋转，在使用机器人坐标系时，每一时刻机器人都是在当前角度基础上进行旋转，而平移方向则受到了之前已转过角度的影响，最终表现为转弯；使用世界坐标系时因可以感知到已转过的角度，维持平移方向为世界坐标系中的方向，表现为平移同时在平移路径上旋转，运动是平移旋转的简单叠加。两种不同坐标系下的运动的区别如图7-1和图7-2所示，二者都是在向右平移的同时逆时针旋转，机器人坐标系下轨迹为弧线，世界坐标系下轨迹为直线。

图7-3 平移旋转速度

同时存在平移和旋转速度时各轮对应的速度向量如图7-3，向量a是车体平移速度，向量b是车体旋转速度，u、v、w是各轮平移时的速度，u’、v’、w’是各轮旋转时的速度，计算二者的向量和即可得到各轮旋转的速度和方向，如图7-3，但为便于计算在程序中使用正交分解方式进行计算。

图7-4 速度向量合成

特殊地，以此方式实现平移旋转的代码同时也能实现仅平移或仅旋转功能。（原方法需分类讨论，其适用性窄且代码较复杂难以维护，已弃用）

图7-5 正交分解

现有程序中采用的方案是先计算平移速度，如图7-5，将平移速度分解为x、y方向上分速度Vx_part和Vy_part，底盘形状和每个轮的位置等几何关系决定了旋转速度的方向，将旋转速度分别分解后叠加到每个轮分速度Vx_part和Vy_part上，对于某个特定的轮，由于旋转速度的角度是固定的，x、y方向上所叠加的值是旋转速度大小乘一个常数。如对于一车1号轮，旋转速度方向正好和x方向重合，x方向系数1，y方向系数0；2号轮旋转速度方向在坐标系中为120度，x方向系数\cos{\left(\frac{2}{3}\pi\right)}=-\frac{1}{2}，y方向系数sin(\frac{2}{3}\pi)=\frac{\sqrt{3}}{2}。

计算完分速度Vx_part和Vy_part后可以通过平方和计算出合速度大小，将Vx_part和Vy_part看作V_x和V_y，以计算平移时轮速度向量的方法分别计算每个轮的速度向量，区别在于仅平移时所有轮的旋转方向是一致的，而此时每个轮的旋转方向可能是不一致的。

8.电机驱动

编码器数值=外部齿轮比*电机编码器齿轮比*电机转动一圈数值变化*转动弧度/（2*PI）

通过以上计算得出了各轮分别需要的朝向和转动速度，之后需要利用这些数值使电机以特定方式转动以满足需求。舵轮使用小电机完成舵轮自身朝向旋转，程序中通过改变C610结构体下电机其他位置使电机带动舵轮转向指定角度；大电机通过设置期望速度使轮胎转动，进而使机器人运动。这两种电机都配有编码器，可以实时监测电机旋转位置或速度。

图8-1 初版防绕线

图8-2 大幅度旋转

最初由于机械结构和走线的限制，舵轮只能旋转特定角度，继续旋转就可能会使线相互缠绕过度以至松开、断裂，故需限制允许旋转的角度。最初一版中旋转角度限制为正负90度，即图8-1中第一四象限区域，其余二三象限以正负90度范围内大电机反转实现同样效果，例如要转到图8-1中向量u方向则转到到向量v方向同时大电机。但缺点在于在正负90度的边界区域改变方向时，由于采取了反转进行角度映射的方法，舵轮自身会近180度大角度旋转导致运动稳定性差，如图8-2，上一时刻速度向量u，下一时刻速度向量w，则轮实际朝向会由v到w，尤其是当速度方向频繁在边界两侧来回变化时会对正常行驶造成巨大影响，显然机械结构和电机控制方法都存在很大缺陷，对应代码为原代码中分隔线之下部分。

图8-3 改进旋转方案

后续由于机械结构改进，可以允许至少正负360度范围轮的旋转，也随之改进了电机的运动方法，在360度范围内使小电机旋转角度将尽可能小，正常情况下在90度内。实现此功能同样也利用大电机反转以等效舵轮旋转180度，但在使小电机旋转时会根据内置编码器返回值在转至期望位置或期望位置正负半圈等三个位置中选择所需转动角度最小的一个，转至期望位置正负半圈时大电机反转以等效舵轮旋转180度，理论上选取到的最小转角应小于90度，但为保险起见也考虑了最小转角大于90度的情况，同样使大电机反转以等效舵轮旋转180度，反向旋转所得最小转角的补角。在如图8-3所示情况下，轮当前朝向为u，标示角度范围α为下一时刻可能会直接旋转到的朝向范围，可以旋转角度β到v方向，而要转至范围外的a方向则转至w并反转大电机。但当选取到的转动位置超出360度范围时，仍将和原版本一样强制回转近180度并使大电机反转，对应代码为现代码中分隔线之下部分。

若采用导电滑环等可以近无限圈旋转的机械结构，则可以取消或极大拓展正负360度的角度限制，使舵轮每次旋转角度始终小于90度，避免近180度大幅度旋转的情况。

9.原方案（弃用）

此方法参考了四舵轮运动算法解析_ddydy111的博客-CSDN博客_四舵轮运动模型中的思路，最初使用此方法来处理同时具有平移速度和旋转速度时舵轮运动，并根据需要对正三角形、正方形、直角三角形都进行了分析。

此方案主要由平移旋转时几何关系确定各轮的速度向量，但只能在三个量都不为0时使用，其余情况只能另外分类讨论（仅平移和仅旋转，此部分仍在使用，并与平移旋转融合为一体），相对前面所述方法具有较高的局限性，且运算过程复杂，出现问题时难以快速找到问题并排除，也难以验证结果是否正确，测试时最终仅在部分角度范围实现了预期效果，由于计算过程较为复杂，难以分析在哪一环节出现错误，也没有让使用此模型的底盘下地测试过实际效果，最终没有继续采用此方法，改用了前文中介绍的方式。附录中原代码中分隔线之上的部分即使用此方法的正三角形底盘代码，且由于其大多是数学计算的特殊性质，在编写程序前先分析计算并编写了以下文档，之后根据相关公式逐一转为代码。若有兴趣可以继续阅读，跳过此部分也无妨，也欢迎对其中错误不当之处等批判指正。

正三角形

图9-1-1 正三角形舵轮运动模型示意图

如图9-1-1，将车体视为正三角形，将车头方向（HA）和车体运动方向（HI）之间的夹角设为α（从AH开始逆时针旋转，角度为[-π, π],由trans_PI函数限制）,设三角形中AH长度为L，D点为圆周运动的圆心，设车圆周运动的半径R，连接三个舵轮所在的三顶点和和圆心D，连线长度即各轮的圆周运动半径RA、RB、RC。若得知半径大小和线段相对车体的方向即可计算出舵轮应转动的角度AA、AB、AC和速度VA、VB、VC。

半径

首先求解三条半径长度，如图一，作三条垂直辅助线，由勾股定理根据R可计算出出各轮的运动半径，求解如下：

R_A=\sqrt{\left(L+HG\right)^2+{GD}^2}

R_B=\sqrt{​{(L+HE)}^2+ED^2}

R_C=\sqrt{​{(L+HF)}^2+{FD}^2}

其中HE、HF、HG应区分正负，位于AH、BH、CH延长线上时为正，否则为负。

根据几何关系，（为保证公式在全平面成立经过一些正负、加减π处理）

∠DHE=-π/6+α

∠DHF=π/6+α+π

∠DHG=π/2+α

则可将三条边表示为

R_A=\sqrt{\left(L+Rcos(\frac{\pi}{2}+\alpha)\right)^2+\left(R{sin(\frac{\pi}{2}+\alpha))}^2\right)^2}

R_B=\sqrt{\left(L+Rcos(-\frac{\pi}{6}+\alpha)\right)^2+\left(Rsin(-\frac{\pi}{6}+\alpha)\right)^2}

R_C=\sqrt{\left(L+Rcos(\frac{\pi}{6}+\alpha+\pi)\right)^2+\left(Rsin(\frac{\pi}{6}+\alpha+\pi)\right)^2}

经检验公式在全平面成立。

速度

至此利用各半径可求得舵轮运动速度

V_A=R_A\omega

V_B=R_B\omega

V_C=R_C\omega

角度

借助求半径时构建辅助三角形，可以得到

\angle AHG=arctan(\frac{Rsin(\frac{\pi}{2}+\alpha)}{L+Rcos(\frac{\pi}{2}+\alpha)})

\angle BHE=\ arctan(\frac{Rsin(-\frac{\pi}{6}+\alpha)}{L+Rcos(-\frac{\pi}{6}+\alpha)})

\angle CHF=\ arctan(\frac{Rsin(\frac{\pi}{6}+\alpha+\pi)}{L+Rcos(\frac{\pi}{6}+\alpha+\pi)})

将车体绕几何中心旋转时舵轮的方向定为参考角度0度，逆时针旋转，角度从0到2π（最终计算结果范围为[-π, π]），如图9-1-2。

图9-1-2 参考角度示意图

arctan函数最大范围为(-\frac{\pi}{2},\frac{\pi}{2}),需分类讨论：

由几何关系验证，当arctan函数分母为正时，

AA=∠AHG

AB=∠BHE

AC=∠CHF

当arctan函数分母为负时，

AA=∠AHG+π

AB=∠BHE+π

AC=∠CHF+π

此时已获得三个舵轮的速度和方向，可直接控制舵轮的运动。

正方形

图9-2-1 正方形舵轮运动模型示意图

四轮和三轮情况类似，将车体视为正方向，如图9-2-1，将车头方向（EH）和车体运动方向（EG）之间的夹角设为α（从EH开始逆时针旋转，角度为0-2π度）,设正方形中AH长度为L，F点为圆周运动的圆心，设车圆周运动的半径R，连接四个舵轮所在的四顶点和和圆心F，连线长度即各轮的圆周运动半径RA、RB、RC、RD。若得知半径大小和线段相对车体的方向即可计算出舵轮应转动的角度AA、AB、AC、AD和速度VA、VB、VC、VD。

半径

求解四个舵轮分别运动的半径，如图三，作过几何中心垂直于正方形变的辅助线，从F作两条辅助线的垂线,可得

EI=Rsin\alpha

FS=Rcos\alpha

由勾股定理根据R可计算出出各轮的运动半径，求解如下：

R_A=\sqrt{\left(L+Rcos\alpha\right)^2+\left(L+Rsin\alpha\right)^2}

R_B=\sqrt{\left(L+Rcos\alpha\right)^2+\left(L-Rsin\alpha\right)^2}

R_C=\sqrt{\left(L-Rcos\alpha\right)^2+\left(L-Rsin\alpha\right)^2}

R_D=\sqrt{\left(L-Rcos\alpha\right)^2+\left(L+Rsin\alpha\right)^2}

经检验公式在全平面成立。

速度

至此利用各半径可求得舵轮运动速度

V_A=R_A\omega

V_B=R_B\omega

V_C=R_C\omega

V_D=R_D\omega

角度

将车头的方向定为参考角度0度，逆时针旋转，角度从0到2π度。

借助求半径时构建辅助三角形，可以得到

\angle AFI=arctan\left(\frac{L-Rsin\alpha}{L+Rcos\alpha}\right)

\angle BFI=arctan\left(\frac{L+Rsin\alpha}{L+Rcos\alpha}\right)

\angle CFJ=arctan\left(\frac{L+Rsin\alpha}{L-Rcos\alpha}\right)

\angle DFJ=arctan\left(\frac{L-Rsin\alpha}{L-Rcos\alpha}\right)

arctan函数最大范围为(-\frac{\pi}{2},\frac{\pi}{2}),需分类讨论：

由几何关系验证，当arctan函数分母为正时，

AA=∠AFI

 AB=-∠BFI

AC=∠CFJ

AD=-∠DFJ

当arctan函数分母为负时

AA=π+∠AFI

AB=π-∠BFI

AC=π+∠CFJ

AD=π-∠DFJ

此时已获得四个舵轮的速度和方向，可直接控制舵轮的运动。

等腰三角形

图9-3-1 直角三角形舵轮运动模型示意图

当底盘形状为等腰三角时，与正方形四轮时一致，仅取A、B、C三点对应值即可，如图9-3-1。之前所得结论均适用，为适应形状的改变，对参考方向也进行了一定的调整，如图9-3-2。

图9-3-2 等腰三角形模型参考方向

旋转坐标系，使其车头前方为y轴正方向，右侧为x轴正方向，角度为从x轴正方向开始逆时针旋转（即一般直角坐标系的定义），如图三中黄色直线所示。此坐标系为原坐标系顺时针旋转45度所得，故由原坐标系转至新坐标系时应+45°(+π/4)，此外还要加上因参考角度不同导致不同轮角度额外调整不同角度。

图9-3-3 旋转后坐标系

半径

与正方形时完全一致。

R_A=\sqrt{\left(L+Rcos\alpha\right)^2+\left(L+Rsin\alpha\right)^2}

R_B=\sqrt{\left(L+Rcos\alpha\right)^2+\left(L-Rsin\alpha\right)^2}

R_C=\sqrt{\left(L-Rcos\alpha\right)^2+\left(L-Rsin\alpha\right)^2}

速度

与正方形时完全一致。

V_A=R_A\omega

V_B=R_B\omega

V_C=R_C\omega

角度

根据坐标系、参考方向的调整进行修改。

A_A=\pi+\angle AFI+\frac{\pi}{4}-\frac{\pi}{4}=\pi+\angle AFI

A_B=\pi-\angle BFI+\frac{\pi}{4}+\frac{\pi}{4}=\frac{3\pi}{2}-\angle BFI=-\frac{\pi}{2}-\angle BFI

A_C=\pi+\angle CFJ+\frac{\pi}{4}+\frac{3\pi}{4}=2\pi+\angle CFJ=\angle CFJ

等腰三角形（重制）

直接如图9-3-3建立坐标系,重新为等腰三角形建立模型，不再借助原正方形模型。仍计算全部四点的值，使用时仅取A、B、C三点。

车头方向为图8-3中BE射线方向，将车头方向（BE）和车体运动方向（EG）之间的夹角设为α（改变正方形模型车头方向）。定义L为斜边一半（改变定义）。按同样思路求解。

半径

同样由半径点F作两坐标轴垂线，图略。

R_A=\sqrt{\left(L+Rcos\alpha\right)^2+\left(Rsin\alpha\right)^2}

R_B=\sqrt{\left(Rcos\alpha\right)^2+\left(L+Rsin\alpha\right)^2}

R_C=\sqrt{\left(L-Rcos\alpha\right)^2+\left(Rsin\alpha\right)^2}

R_D=\sqrt{\left(Rcos\alpha\right)^2+\left(L-Rsin\alpha\right)^2}

速度

利用各半径可求得舵轮运动速度

V_A=R_A\omega

V_B=R_B\omega

V_C=R_C\omega

V_D=R_D\omega

角度

将车头的方向定为参考角度0度，逆时针旋转。

借助求半径时构建辅助三角形，可以得到

\angle AFI=arctan\left(\frac{Rsin\alpha}{L+Rcos\alpha}\right)

\angle BFI=arctan\left(\frac{L+Rsin\alpha}{Rcos\alpha}\right)

\angle CFJ=arctan\left(\frac{Rsin\alpha}{-(L-Rcos\alpha)}\right)

\angle DFJ=arctan\left(\frac{-(L-Rsin\alpha)}{Rcos\alpha}\right)

arctan函数最大范围为(-\frac{\pi}{2},\frac{\pi}{2}),需分类讨论： 

由几何关系验证，当arctan函数分母为正时

AA=∠AFI

 AB=∠BFI

AC=∠CFJ

AD=∠DFJ

当arctan函数分母为负时

AA=π+∠AFI

AB=π+∠BFI

AC=π+∠CFJ

AD=π+∠DFJ

由于参考角度与此时角度存在差异，继续修正角度

A_A\prime=A_A-0

A_B\prime=A_B-\frac{\pi}{2}

A_C\prime=A_C-\pi

A_D\prime=A_D-\frac{3\pi}{2}

此时已获得四个舵轮的速度和方向，可直接控制舵轮的运动。

附录

现代码

 

  1.  
    /*
     
  2.  
    *********************************************************************************************************
     
  3.  
    * Brief : World frame to robot frame transform formula.
     
  4.  
    *
     
  5.  
    * Param(s) : world - velocity vector(Vx,Vy,Vz) in inertia frame. (mm/s, rad/s)
     
  6.  
    * robo - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
     
  7.  
    *
     
  8.  
    * Return(s): none.
     
  9.  
    *
     
  10.  
    *********************************************************************************************************
     
  11.  
    */
     
  12.  
    void Formula_World2Robo (Velocity *world, Velocity *robo)//转角相反
     
  13.  
    {
     
  14.  
    float sinTheta = sin(RobotPos.theta);
     
  15.  
    float cosTheta = cos(RobotPos.theta);
     
  17.  
    robo->Vx = world->Vx * cosTheta + world->Vy * sinTheta;
     
  18.  
    robo->Vy = world->Vy * cosTheta - world->Vx * sinTheta;
     
  19.  
    robo->Vz = world->Vz;
     
  20.  
    }
     
  23.  
    /*
     
  24.  
    *********************************************************************************************************
     
  25.  
    * Brief : robot frame to wheel frame transform formula.
     
  26.  
    *
     
  27.  
    * Param(s) : wheel - velocity vector(Vx,Vy,Vz) in wheel frame. (0.1 enc counts/s)
     
  28.  
    * robo - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
     
  29.  
    *
     
  30.  
    * Return(s): none.
     
  31.  
    *·
     
  32.  
    *********************************************************************************************************
     
  33.  
    */
     
  34.  
    //float theta_angle[3];//转向以角度形式展示
     
  35.  
    void Formula (Velocity *robo)
     
  36.  
    {
     
  37.  
    int MULtiple=1;
     
  38.  
    int flag;
     
  39.  
    float V;
     
  40.  
    float Vx_part[3]={0,0,0},Vy_part[3]={0,0,0},theta_temp[3]={0,0,0};
     
  41.  
    Wheel3.L = 35;//车轮到三角中心的距离
     
  44.  
    //输入Vx_in,Vy_in
     
  45.  
    //输入Wheel3.omega
     
  46.  
    Wheel3.Vx = robo->Vx;
     
  47.  
    Wheel3.Vy = robo->Vy;
     
  48.  
    Wheel3.omega = robo->Vz * PI/180 * Wheel3.L;
     
  49.  
    V = sqrt(Wheel3.Vx * Wheel3.Vx + Wheel3.Vy * Wheel3.Vy);
     
  50.  
    if (Wheel3.halt)//驻停
     
  51.  
    {
     
  52.  
    Wheel3.theta[0] = PI/2;
     
  53.  
    Wheel3.theta[1] = Wheel3.theta[0];
     
  54.  
    Wheel3.theta[2] = Wheel3.theta[0];
     
  56.  
    Wheel3.V[0] = 0;
     
  57.  
    Wheel3.V[1] = Wheel3.V[0];
     
  58.  
    Wheel3.V[2] = Wheel3.V[0];
     
  59.  
    }
     
  60.  
    else if (V || Wheel3.omega)
     
  61.  
    {
     
  62.  
    if (Wheel3.Vy >= 0)
     
  63.  
    flag = 1;
     
  64.  
    else 
     
  65.  
    flag = -1;
     
  67.  
    //反三角计算角度
     
  68.  
    if (Wheel3.Vx != 0)
     
  69.  
    {
     
  70.  
    theta_temp[0] = flag*acos(Wheel3.Vx / V);
     
  71.  
    theta_temp[1] = theta_temp[0];
     
  72.  
    theta_temp[2] = theta_temp[0];
     
  73.  
    }
     
  74.  
    else
     
  75.  
    {
     
  76.  
    if (flag==1)
     
  77.  
    {
     
  78.  
    theta_temp[0] = PI/2;
     
  79.  
    theta_temp[1] = theta_temp[0];
     
  80.  
    theta_temp[2] = theta_temp[0];
     
  81.  
    }
     
  82.  
    else
     
  83.  
    {
     
  84.  
    theta_temp[0] = -PI/2;
     
  85.  
    theta_temp[1] = theta_temp[0];
     
  86.  
    theta_temp[2] = theta_temp[0];
     
  87.  
    }
     
  88.  
    }//反三角计算角度
     
  90.  
    //平移旋转速度正交分解叠加
     
  91.  
    Vx_part[0] = V * cos(theta_temp[0]);
     
  92.  
    Vx_part[1] = V * cos(theta_temp[1]);
     
  93.  
    Vx_part[2] = V * cos(theta_temp[2]);
     
  94.  
    Vy_part[0] = V * sin(theta_temp[0]);
     
  95.  
    Vy_part[1] = V * sin(theta_temp[1]);
     
  96.  
    Vy_part[2] = V * sin(theta_temp[2]);
     
  97.  
    Vx_part[0] += Wheel3.omega;
     
  98.  
    Vx_part[1] -= Wheel3.omega / 2;
     
  99.  
    Vy_part[1] += Wheel3.omega * sqrt(3)/2;
     
  100.  
    Vx_part[2] -= Wheel3.omega /2;
     
  101.  
    Vy_part[2] -= Wheel3.omega * sqrt(3)/ 2;
     
  102.  
    Wheel3.V[0] = sqrt(Vx_part[0]*Vx_part[0] + Vy_part[0]*Vy_part[0]);
     
  103.  
    Wheel3.V[1] = sqrt(Vx_part[1]*Vx_part[1] + Vy_part[1]*Vy_part[1]);
     
  104.  
    Wheel3.V[2] = sqrt(Vx_part[2]*Vx_part[2] + Vy_part[2]*Vy_part[2]);
     
  106.  
    //叠加后重新计算角度
     
  107.  
    for (int i=0;i<3;i++)
     
  108.  
    {
     
  109.  
    if (Vx_part[i] != 0)
     
  110.  
    {
     
  111.  
    if (Vy_part[i] >= 0)
     
  112.  
    {
     
  113.  
    Wheel3.theta[i] = acos(Vx_part[i] / Wheel3.V[i]);
     
  114.  
    }
     
  115.  
    else
     
  116.  
    {
     
  117.  
    Wheel3.theta[i] = -acos(Vx_part[i] / Wheel3.V[i]);
     
  118.  
    }
     
  119.  
    }
     
  120.  
    else
     
  121.  
    {
     
  122.  
    if (Vy_part[i] == 0)
     
  123.  
    {
     
  124.  
    Wheel3.theta[i] = 0;
     
  125.  
    }
     
  126.  
    if (Vy_part[i] > 0)
     
  127.  
    {
     
  128.  
    Wheel3.theta[i] = PI/2;
     
  129.  
    }
     
  130.  
    else
     
  131.  
    {
     
  132.  
    Wheel3.theta[i] = -PI/2;
     
  133.  
    }
     
  134.  
    }
     
  135.  
    }//叠加后重新计算角度
     
  136.  
    //基准位置修正，编码器向后到编码器向里
     
  137.  
    Wheel3.theta[1] -= 2*PI/3;
     
  138.  
    Wheel3.theta[2] += 2*PI/3;
     
  139.  
    }//平移旋转
     
  140.  
    else//其他情况:仅停止，不转轮
     
  141.  
    {
     
  142.  
    Wheel3.V[0] = 0;
     
  143.  
    Wheel3.V[1] = Wheel3.V[0];
     
  144.  
    Wheel3.V[2] = Wheel3.V[0];
     
  145.  
    }
     
  147.  
    //限制角度范围
     
  148.  
    Wheel3.theta[0] = trans_PI(trans_PI(Wheel3.theta[0]));
     
  149.  
    Wheel3.theta[1] = trans_PI(trans_PI(Wheel3.theta[1]));
     
  150.  
    Wheel3.theta[2] = trans_PI(trans_PI(Wheel3.theta[2]));
     
  151.  
    // //弧度转角度，仅供参考，无实际作用
     
  152.  
    // theta_angle[0] = trans_PI(Wheel3.theta[0])*180/PI;
     
  153.  
    // theta_angle[1] = trans_PI(Wheel3.theta[1]+PI*2/3)*180/PI;
     
  154.  
    // theta_angle[2] = trans_PI(Wheel3.theta[2]-PI*2/3)*180/PI;
     
  155.  
    //*************************************限制舵轮旋转圈数****************************************************
     
  156.  
    //拓展至正负360度
     
  157.  
    double Exparg_temp[3][3],diff_arg[3][3];
     
  158.  
    int diff_min[3];
     
  159.  
    for (int i=0;i<3;i++)
     
  160.  
    {
     
  161.  
    if( -1179504.0 < MOTOR_C610[i+1].Temparg < 1179504.0 ) //(2*PI) 允许自由旋转范围
     
  162.  
    {
     
  163.  
    Exparg_temp[i][0] = (int)(Wheel3.theta[i]*187723.89199);//轮朝向转为电机位置
     
  164.  
    Exparg_temp[i][1] = Exparg_temp[i][0] + 589752.0;//PI 正向多半圈
     
  165.  
    Exparg_temp[i][2] = Exparg_temp[i][0] - 589752.0;//PI 负向多半圈
     
  166.  
    diff_arg[i][0] = Exparg_temp[i][0] - MOTOR_C610[i+1].Temparg;
     
  167.  
    diff_arg[i][1] = Exparg_temp[i][1] - MOTOR_C610[i+1].Temparg;
     
  168.  
    diff_arg[i][2] = Exparg_temp[i][2] - MOTOR_C610[i+1].Temparg;
     
  169.  
    diff_min[i] = (abs(diff_arg[i][0]) < abs(diff_arg[i][1]) + 10.0) ? 0:1;
     
  170.  
    diff_min[i] = (abs(diff_arg[i][2] - 10.0 ) < diff_arg[i][diff_min[i]]) ? 2:diff_min[i];
     
  171.  
    if( -294876 < diff_arg[i][diff_min[i]] < 294876 )//当前位置正负90度内（应已包含所有角度）取最小转动角度
     
  172.  
    {
     
  173.  
    if (diff_min[i] == 0)
     
  174.  
    {
     
  175.  
    MOTOR[i+1].ExpSpeed = -(int)((Wheel3.V[i])*MULtiple);
     
  176.  
    MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][0]);
     
  177.  
    }
     
  178.  
    else if (diff_min[i] == 1)
     
  179.  
    {
     
  180.  
    MOTOR[i+1].ExpSpeed = (int)((Wheel3.V[i])*MULtiple);
     
  181.  
    MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][1]);
     
  182.  
    }
     
  183.  
    else if (diff_min[i] == 2)
     
  184.  
    {
     
  185.  
    MOTOR[i+1].ExpSpeed = (int)((Wheel3.V[i])*MULtiple);
     
  186.  
    MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][2]);
     
  187.  
    }
     
  188.  
    else 
     
  189.  
    {}//错误
     
  190.  
    } 
     
  191.  
    else if( -294876 > diff_arg[i][diff_min[i]] )//负向超过90度（可能始终不会执行）
     
  192.  
    {
     
  193.  
    MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
     
  194.  
    MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][diff_min[i]] + 589752.0); 
     
  195.  
    }
     
  196.  
    else//正向超过90度（可能始终不会执行）
     
  197.  
    {
     
  198.  
    MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
     
  199.  
    MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][diff_min[i]] - 589752.0);
     
  200.  
    }
     
  201.  
    } 
     
  202.  
    else if( -1179504.0 > MOTOR_C610[i+1].Temparg )//负向超过限幅
     
  203.  
    {
     
  204.  
    MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
     
  205.  
    MOTOR_C610[i+1].Exparg=(int)(-(Wheel3.theta[i]+PI)*187723.89199);//1/(2*PI)*8191*36*4
     
  206.  
    }
     
  207.  
    else//正向超过限幅
     
  208.  
    {
     
  209.  
    MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
     
  210.  
    MOTOR_C610[i+1].Exparg=(int)(-(Wheel3.theta[i]-PI)*187723.89199);//1/(2*PI)*8191*36*4
     
  211.  
    }
     
  212.  
    }//for
     
  213.  
    }//Formula_4Omni末尾
     
  215.  
    float trans_PI(float n)//[-2PI,2PI]->[-PI,PI]
     
  216.  
    {
     
  217.  
    if (n > PI) n -= 2*PI;
     
  218.  
    else if (n < -PI) n += 2*PI;
     
  219.  
    return n;
     
  220.  
    }
     
 

原代码
 

 

  1.  
    /*
     
  2.  
    *********************************************************************************************************
     
  3.  
    * Brief : World frame to robot frame transform formula.
     
  4.  
    *
     
  5.  
    * Param(s) : world - velocity vector(Vx,Vy,Vz) in inertia frame. (mm/s, rad/s)
     
  6.  
    * robo - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
     
  7.  
    *
     
  8.  
    * Return(s): none.
     
  9.  
    *
     
  10.  
    *********************************************************************************************************
     
  11.  
    */
     
  12.  
    void Formula_World2Robo (Velocity *world, Velocity *robo)
     
  13.  
    {
     
  14.  
    float sinTheta = sin(RobotPos.theta);
     
  15.  
    float cosTheta = cos(RobotPos.theta);
     
  17.  
    robo->Vx = world->Vx * cosTheta + world->Vy * sinTheta;
     
  18.  
    robo->Vy = world->Vx * ( -sinTheta ) + world->Vy * cosTheta;
     
  19.  
    robo->Vz = world->Vz;
     
  20.  
    }
     
  23.  
    /*
     
  24.  
    *********************************************************************************************************
     
  25.  
    * Brief : robot frame to wheel frame transform formula.
     
  26.  
    *
     
  27.  
    * Param(s) : wheel - velocity vector(Vx,Vy,Vz) in wheel frame. (0.1 enc counts/s)
     
  28.  
    * robo - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
     
  29.  
    *
     
  30.  
    * Return(s): none.
     
  31.  
    *
     
  32.  
    *********************************************************************************************************
     
  33.  
    */
     
  35.  
    void Formula (Velocity *robo)
     
  36.  
    {
     
  37.  
    int MULtiple=1;
     
  38.  
    int flag,halt;
     
  39.  
    float V;
     
  40.  
    float angleA,angleB,angleC,angle[3];
     
  41.  
    float cosA,sinA,cosB,sinB,cosC,sinC;
     
  42.  
    float ang_action;
     
  43.  
    Wheel3.L = 35;//车轮到三角中心的距离
     
  44.  
    ang_action = 0.261799387791;//15 / 180 * PI，action和车头的偏差角15度
     
  46.  
    //输入Vx_in,Vy_in
     
  47.  
    //输入Wheel3.omega
     
  48.  
    Wheel3.Vx = robo->Vx;
     
  49.  
    Wheel3.Vy = robo->Vy;
     
  50.  
    Wheel3.omega = robo->Vz;
     
  51.  
    V = sqrt(Wheel3.Vx * Wheel3.Vx + Wheel3.Vy * Wheel3.Vy);
     
  52.  
    halt = 0; 
     
  53.  
    if (halt)//驻停
     
  54.  
    {
     
  55.  
    Wheel3.theta[0] = PI/2;
     
  56.  
    Wheel3.theta[1] = Wheel3.theta[0];
     
  57.  
    Wheel3.theta[2] = Wheel3.theta[0];
     
  59.  
    Wheel3.V[0] = 0;
     
  60.  
    Wheel3.V[1] = Wheel3.V[0];
     
  61.  
    Wheel3.V[2] = Wheel3.V[0];
     
  62.  
    }
     
  63.  
    else
     
  64.  
    {
     
  65.  
    if(V)//平移
     
  66.  
    {
     
  67.  
    if (Wheel3.Vy >= 0)
     
  68.  
    flag = 1;
     
  69.  
    else 
     
  70.  
    flag = -1;
     
  72.  
    if( Wheel3.omega > 0 )//平移并旋转
     
  73.  
    {
     
  74.  
    //基准位置
     
  75.  
    Wheel3.alfa = PI/2 + flag*acos(Wheel3.Vx/V) + ang_action;
     
  76.  
    Wheel3.R = V / Wheel3.omega;
     
  77.  
    angleA = PI/2 + Wheel3.alfa;
     
  78.  
    angleB = -PI/6 + Wheel3.alfa;
     
  79.  
    angleC = PI/6 + Wheel3.alfa + PI;
     
  80.  
    angleA = trans_PI(angleA);
     
  81.  
    angleB = trans_PI(angleB);
     
  82.  
    angleC = trans_PI(angleC);
     
  84.  
    //中间量
     
  85.  
    cosA = Wheel3.L + Wheel3.R * cos(angleA);
     
  86.  
    cosB = Wheel3.L + Wheel3.R * cos(angleB);
     
  87.  
    cosC = Wheel3.L + Wheel3.R * cos(angleC);
     
  88.  
    sinA = Wheel3.R * sin(angleA);
     
  89.  
    sinB = Wheel3.R * sin(angleB);
     
  90.  
    sinC = Wheel3.R * sin(angleC);
     
  92.  
    //轮半径
     
  93.  
    Wheel3.r[0] = sqrt(cosA * cosA + sinA * sinA);
     
  94.  
    Wheel3.r[1] = sqrt(cosB * cosB + sinB * sinB);
     
  95.  
    Wheel3.r[2] = sqrt(cosC * cosC + sinC * sinC);
     
  97.  
    //轮速度
     
  98.  
    Wheel3.V[0] = Wheel3.omega * Wheel3.r[0];
     
  99.  
    Wheel3.V[1] = Wheel3.omega * Wheel3.r[1];
     
  100.  
    Wheel3.V[2] = Wheel3.omega * Wheel3.r[2];
     
  102.  
    //轮角度
     
  103.  
    angle[0] = atan(sinA / cosA);
     
  104.  
    angle[1] = atan(sinB / cosB);
     
  105.  
    angle[2] = atan(sinC / cosC);
     
  106.  
    Wheel3.theta[0] = (cosA > 0)?(angle[0]):(angle[0] + PI);
     
  107.  
    Wheel3.theta[1] = (cosA > 0)?(angle[1]):(angle[1] + PI);
     
  108.  
    Wheel3.theta[2] = (cosA > 0)?(angle[2]):(angle[2] + PI);
     
  109.  
    }
     
  110.  
    else//不旋转
     
  111.  
    {
     
  112.  
    //统一速度赋值
     
  113.  
    Wheel3.V[0] = V;
     
  114.  
    Wheel3.V[1] = Wheel3.V[0];
     
  115.  
    Wheel3.V[2] = Wheel3.V[0];
     
  117.  
    //反三角计算角度
     
  118.  
    if (Wheel3.Vx != 0)
     
  119.  
    {
     
  120.  
    Wheel3.theta[0] = flag*acos(Wheel3.Vx / V);
     
  121.  
    Wheel3.theta[1] = Wheel3.theta[0];
     
  122.  
    Wheel3.theta[2] = Wheel3.theta[0];
     
  123.  
    }
     
  124.  
    else
     
  125.  
    {
     
  126.  
    if (flag==1)
     
  127.  
    {
     
  128.  
    Wheel3.theta[0] = PI/2;
     
  129.  
    Wheel3.theta[1] = Wheel3.theta[0];
     
  130.  
    Wheel3.theta[2] = Wheel3.theta[0];
     
  131.  
    }
     
  132.  
    else
     
  133.  
    {
     
  134.  
    Wheel3.theta[0] = -PI/2;
     
  135.  
    Wheel3.theta[1] = Wheel3.theta[0];
     
  136.  
    Wheel3.theta[2] = Wheel3.theta[0];
     
  137.  
    }
     
  138.  
    }
     
  139.  
    //基准位置修正
     
  140.  
    Wheel3.theta[1] += 2*PI/3;
     
  141.  
    Wheel3.theta[2] -= 2*PI/3;
     
  143.  
    }
     
  144.  
    }
     
  145.  
    else if (Wheel3.omega != 0)//仅旋转无位移
     
  146.  
    {
     
  147.  
    Wheel3.theta[0] = 0;
     
  148.  
    Wheel3.theta[1] = Wheel3.theta[0];
     
  149.  
    Wheel3.theta[2] = Wheel3.theta[0];
     
  150.  
    Wheel3.V[0] = Wheel3.L * Wheel3.omega;
     
  151.  
    Wheel3.V[1] = Wheel3.V[0];
     
  152.  
    Wheel3.V[2] = Wheel3.V[0];
     
  153.  
    }
     
  154.  
    else//其他情况:仅停止，不转轮
     
  155.  
    {
     
  156.  
    Wheel3.V[0] = 0;
     
  157.  
    Wheel3.V[1] = Wheel3.V[0];
     
  158.  
    Wheel3.V[2] = Wheel3.V[0];
     
  159.  
    }
     
  160.  
    }
     
  161.  
    //限制角度范围
     
  162.  
    Wheel3.theta[0] = trans_PI(Wheel3.theta[0]);
     
  163.  
    Wheel3.theta[1] = trans_PI(Wheel3.theta[1]);
     
  164.  
    Wheel3.theta[2] = trans_PI(Wheel3.theta[2]);
     
  166.  
    //*************************************限制舵轮旋转圈数****************************************************
     
  168.  
    if( (Wheel3.theta[0]+Encoder_MotorC610[1]/187723.89199 >= -PI/2) && (Wheel3.theta[0]+Encoder_MotorC610[1]/187723.89199 <= PI/2) )
     
  169.  
    {
     
  170.  
    MOTOR_C610[1].Exparg=(int)(-Wheel3.theta[0]*187723.89199)+Encoder_MotorC610[1];//1/(2*PI)*8191*36*4
     
  171.  
    MOTOR[1].ExpSpeed=(int)(Wheel3.V[0]*MULtiple);
     
  172.  
    }
     
  173.  
    else if( (Wheel3.theta[0]+Encoder_MotorC610[1]/187723.89199) < -PI/2 )
     
  174.  
    {
     
  175.  
    MOTOR_C610[1].Exparg=(int)(-(Wheel3.theta[0]+PI)*187723.89199)+Encoder_MotorC610[1];//1/(2*PI)*8191*36*4
     
  176.  
    MOTOR[1].ExpSpeed=-(int)((Wheel3.V[0])*MULtiple);
     
  177.  
    }
     
  178.  
    else
     
  179.  
    {
     
  180.  
    MOTOR_C610[1].Exparg=(int)(-(Wheel3.theta[0]-PI)*187723.89199)+Encoder_MotorC610[1];//1/(2*PI)*8191*36*4
     
  181.  
    MOTOR[1].ExpSpeed=-(int)((Wheel3.V[0])*MULtiple);
     
  182.  
    }
     
  183.  
    if( (Wheel3.theta[1]+Encoder_MotorC610[2]/187723.89199 >= -PI/2) && (Wheel3.theta[1]+Encoder_MotorC610[2]/187723.89199 <= PI/2) )
     
  184.  
    {
     
  185.  
    MOTOR_C610[2].Exparg=(int)(-Wheel3.theta[1]*187723.89199)+Encoder_MotorC610[2];//1/(2*PI)*8191*36*4
     
  186.  
    MOTOR[2].ExpSpeed=(int)(Wheel3.V[1]*MULtiple);
     
  187.  
    }
     
  188.  
    else if( (Wheel3.theta[1]+Encoder_MotorC610[2]/187723.89199) < -PI/2 )
     
  189.  
    {
     
  190.  
    MOTOR_C610[2].Exparg=(int)(-(Wheel3.theta[1]+PI)*187723.89199)+Encoder_MotorC610[2];//1/(2*PI)*8191*36*4
     
  191.  
    MOTOR[2].ExpSpeed=-(int)((Wheel3.V[1])*MULtiple);
     
  192.  
    }
     
  193.  
    else
     
  194.  
    {
     
  195.  
    MOTOR_C610[2].Exparg=(int)(-(Wheel3.theta[1]-PI)*187723.89199)+Encoder_MotorC610[2];//1/(2*PI)*8191*36*4
     
  196.  
    MOTOR[2].ExpSpeed=-(int)((Wheel3.V[1])*MULtiple);
     
  197.  
    }
     
  198.  
    if( (Wheel3.theta[2]+Encoder_MotorC610[3]/187723.89199 >= -PI/2) && (Wheel3.theta[2]+Encoder_MotorC610[3]/187723.89199 <= PI/2) )
     
  199.  
    {
     
  200.  
    MOTOR_C610[3].Exparg=(int)(-Wheel3.theta[2]*187723.89199)+Encoder_MotorC610[3];///(2*PI)*8191*36*4
     
  201.  
    MOTOR[3].ExpSpeed=(int)(Wheel3.V[2]*MULtiple);
     
  202.  
    }
     
  203.  
    else if( (Wheel3.theta[2]+Encoder_MotorC610[3]/187723.89199) < -PI/2 )
     
  204.  
    {
     
  205.  
    MOTOR_C610[3].Exparg=(int)(-(Wheel3.theta[2]+PI)*187723.89199)+Encoder_MotorC610[3];//1/(2*PI)*8191*36*4
     
  206.  
    MOTOR[3].ExpSpeed=-(int)((Wheel3.V[2])*MULtiple);
     
  207.  
    }
     
  208.  
    else
     
  209.  
    {
     
  210.  
    MOTOR_C610[3].Exparg=(int)(-(Wheel3.theta[2]-PI)*187723.89199)+Encoder_MotorC610[3];//1/(2*PI)*8191*36*4
     
  211.  
    MOTOR[3].ExpSpeed=-(int)((Wheel3.V[2])*MULtiple);
     
  212.  
    } 
     
  213.  
    }
     
  215.  
    float trans_PI(float n)//[-2PI,2PI]->[-PI,PI]
     
  216.  
    {
     
  217.  
    if (n > PI) n -= 2*PI;
     
  218.  
    else if (n < -PI) n += 2*PI;
     
  219.  
    return n;
     
  220.  
    }
     
 

头文件
 

 

  1.  
    /*
     
  2.  
    *********************************************************************************************************
     
  3.  
    * INCLUDE FILES
     
  4.  
    *********************************************************************************************************
     
  5.  
    */
     
  7.  
    #pragma once
     
  8.  
    #include "includes.h"
     
  10.  
    /*
     
  11.  
    *********************************************************************************************************
     
  12.  
    * EXTERNS
     
  13.  
    *********************************************************************************************************
     
  14.  
    */
     
  16.  
    #ifdef ROBOT_CTRL_MODULE
     
  17.  
    #define ROBOT_CTRL_EXT
     
  18.  
    #else
     
  19.  
    #define ROBOT_CTRL_EXT extern
     
  20.  
    #endif
     
  22.  
    //**************************************************************** ??? ****************************************************************
     
  23.  
    #ifdef CHASSIS_MODULE
     
  24.  
    #define CHASSIS_EXT
     
  25.  
    #else
     
  26.  
    #define CHASSIS_EXT extern
     
  27.  
    #endif
     
  28.  
    //**************************************************************** ??? ****************************************************************
     
  30.  
    /*
     
  31.  
    *********************************************************************************************************
     
  32.  
    * DEFINES
     
  33.  
    *********************************************************************************************************
     
  34.  
    */
     
  36.  
    // CAN configuration
     
  37.  
    #define SYNC_CYCLE 5 // 100 ms
     
  39.  
    // coefficient needed by transform formula
     
  40.  
    #define THREE_OMNI_D 386.3 // distance between wheel center and spin center
     
  41.  
    #define FOUR_OMNI_D 280.0
     
  42.  
    #define FOUR_Mecunum_L 320.0
     
  43.  
    #define FOUR_Mecunum_l 355.0
     
  45.  
    // wheel data 
     
  46.  
    #define WHEELNUM 4
     
  47.  
    #define WHEEL_R 100.0 // radius(mm)
     
  48.  
    #define WHEEL_FORMULA Formula_4Omni
     
  50.  
    // motor data 
     
  51.  
    #define GEAR_RATIO (1.0/1.0)
     
  55.  
    /*
     
  56.  
    *********************************************************************************************************
     
  57.  
    * EXPORTED_TYPES
     
  58.  
    *********************************************************************************************************
     
  59.  
    */
     
  61.  
    // Speed data structure 
     
  62.  
    typedef struct
     
  63.  
    {
     
  64.  
    int32_t x; // mm
     
  65.  
    int32_t y; // mm
     
  66.  
    float z; // degree
     
  68.  
    }Point;
     
  70.  
    typedef struct
     
  71.  
    {
     
  72.  
    float Vx;
     
  73.  
    float Vy;
     
  74.  
    float Vz;
     
  76.  
    }Velocity; // unit: mm/s, rad/s
     
  78.  
    typedef struct
     
  79.  
    {
     
  80.  
    Velocity curSpeed; // (mm/s, mm/s, rad/s)
     
  81.  
    Velocity expSpeed;
     
  82.  
    Velocity outSpeed;
     
  83.  
    Point expPos; // (mm, mm, rad)
     
  85.  
    }MOTION;
     
  87.  
    typedef struct
     
  88.  
    {
     
  89.  
    float pos_x1; // mm
     
  90.  
    float pos_x2; // mm
     
  91.  
    float pos_z; // degree
     
  93.  
    }US;
     
  95.  
    // Position data structure
     
  96.  
    typedef struct
     
  97.  
    {
     
  98.  
    float pos_x; // mm
     
  99.  
    float pos_y; // mm
     
  100.  
    float zangle; // degree
     
  101.  
    float xangle; // degree
     
  102.  
    float yangle; // degree
     
  103.  
    float w_z;
     
  104.  
    float theta; // rad
     
  106.  
    }POSE;
     
  108.  
    // Laser pos
     
  109.  
    typedef struct
     
  110.  
    {
     
  111.  
    float pos_x; // mm
     
  112.  
    float pos_y; // mm
     
  113.  
    float pos_z; // degree
     
  115.  
    }LASER;
     
  117.  
    // Move method
     
  118.  
    typedef enum
     
  119.  
    {
     
  120.  
    P2P = 0,
     
  121.  
    MULTI,
     
  122.  
    PCL,
     
  124.  
    }MOVE;
     
  126.  
    // Control method
     
  127.  
    typedef enum
     
  128.  
    {
     
  129.  
    MANUAL = 0,
     
  130.  
    AUTO,
     
  132.  
    }CTRL;
     
  134.  
    typedef enum // 机器人当前所在位置
     
  135.  
    {
     
  136.  
    Start = 0,
     
  137.  
    LZ1, // 彩球
     
  138.  
    LZ2, // 金球
     
  139.  
    MeetPoint1,
     
  140.  
    MeetPoint2,
     
  142.  
    }BOT;
     
  144.  
    typedef struct
     
  145.  
    {
     
  146.  
    MOVE RunMode;
     
  147.  
    CTRL OP;
     
  148.  
    BOT State;
     
  150.  
    void (*Move2Point) (uint8_t);
     
  151.  
    void (*WaitREADY) (void);
     
  152.  
    void (*DONE) (void);
     
  154.  
    }ACT;
     
  156.  
    typedef struct
     
  157.  
    {
     
  158.  
    char ID;
     
  159.  
    int16_t current; // robomodule反馈的数据
     
  160.  
    int16_t velocity;
     
  161.  
    int32_t position;
     
  162.  
    int32_t realposition;
     
  164.  
    }BDC;
     
  167.  
    // Accleration 
     
  168.  
    typedef struct
     
  169.  
    {
     
  170.  
    float ax;
     
  171.  
    float ay;
     
  172.  
    float az;
     
  174.  
    }Accelerate;
     
  176.  
    typedef struct
     
  177.  
    {
     
  178.  
    /* Velocity profile parameter */
     
  179.  
    Point qA; // Start point, uint: mm
     
  180.  
    Point qB; // End Point, uint: mm
     
  181.  
    Point delAB; //delAB unit:mm
     
  182.  
    uint8_t end;
     
  183.  
    int32_t Dist; // Distance uint: mm
     
  184.  
    int32_t Acc;
     
  185.  
    int32_t Dec;
     
  186.  
    int32_t Vlin; // mm/s, rad/s
     
  187.  
    int32_t AccTime;
     
  188.  
    int32_t Z_AccTime;
     
  189.  
    float DecTheta;
     
  190.  
    float AccTheta;
     
  191.  
    float Wz;
     
  192.  
    float Z_Acc;
     
  193.  
    float Z_Dec;
     
  194.  
    float Theta;
     
  195.  
    float DecDist;
     
  196.  
    }PclData;
     
  198.  
    typedef struct
     
  199.  
    {
     
  200.  
    int32_t Motor_V; // 0.1 enc counts/sec
     
  201.  
    int16_t Motor_Torq; // rated torque / 1000
     
  203.  
    int32_t Target_V;
     
  205.  
    }Motor_Data;
     
  206.  
    typedef struct
     
  207.  
    {
     
  208.  
    float L;//边长的一半，常数
     
  209.  
    float Vx;//输入，action坐标系下的速度
     
  210.  
    float Vy;//输入，action坐标系下的速度
     
  211.  
    float omega;//输入，action坐标系下的角速度
     
  212.  
    float alfa;//车头定位
     
  213.  
    float R;//总转向半径
     
  214.  
    float r[4];//轮半径
     
  215.  
    float theta[4];//轮方向
     
  216.  
    float V[4];//轮速度
     
  217.  
    int halt;//停止移动指令
     
  218.  
    }LUN;
     
  220.  
    extern LUN Wheel3;
     
  221.  
    extern LUN Wheel4;
     
  222.  
    extern PclData PclBlock;
     
  225.  
    /*
     
  226.  
    *********************************************************************************************************
     
  227.  
    * GLOBAL VARIABLES
     
  228.  
    *********************************************************************************************************
     
  229.  
    */
     
  231.  
    ROBOT_CTRL_EXT MOTION RoboMotion;
     
  232.  
    ROBOT_CTRL_EXT POSE RobotPos;
     
  233.  
    ROBOT_CTRL_EXT POSE RobotExpPos;
     
  234.  
    ROBOT_CTRL_EXT US RobotPosUS;
     
  235.  
    ROBOT_CTRL_EXT LASER RobotPosition;
     
  236.  
    ROBOT_CTRL_EXT ACT RobotCTRL;
     
  237.  
    ROBOT_CTRL_EXT Point COULMN_POS[40];
     
  238.  
    ROBOT_CTRL_EXT Point COULMN_ForestPOS[20];
     
  239.  
    ROBOT_CTRL_EXT Point JCC[15];
     
  240.  
    ROBOT_CTRL_EXT BDC BrushDC;
     
  241.  
    ROBOT_CTRL_EXT CTRL mode;
     
  243.  
    ROBOT_CTRL_EXT char PIDflag;
     
  244.  
    ROBOT_CTRL_EXT char FeedbackData;
     
  246.  
    CHASSIS_EXT Motor_Data Wheel[WHEELNUM+1]; // Index 0 is reserved
     
  247.  
    CHASSIS_EXT Motor_Data Target_V;
     
  249.  
    /*
     
  250.  
    *********************************************************************************************************
     
  251.  
    * FUNCTION PROTOTYPES
     
  252.  
    *********************************************************************************************************
     
  253.  
    */
     
  254.  
    float trans_PI(float n);
     
  255.  
    void Robot_Motion_Init (void);
     
  256.  
    void Robot_Speed_Update (void);
     
  257.  
    void Robot_Speed_Ctrl (void);
     
  258.  
    void SyncSignal (void);
     
  260.  
    void Pos_Paracal(PclData *profileData,uint8_t col);
     
  261.  
    void Robot_Pos_Ctrl (void);
     
  262.  
    void Chassis_Init (void);
     
  263.  
    void SpeedCtrl (void);
     
  264.  
    void SendWheel_Vel (void);
     
  265.  
    void Send_Velocity (Motor_Data *wheel);
     
  266.  
    void Formula_4Omni (Velocity *robo);
     
  267.  
    float vision_turn(float *abc_data);
     
  268.  
    /*
     
  269.  
    *********************************************************************************************************
     
  270.  
    * MODULE END
     
  271.  
    *********************************************************************************************************
     
  272.  
    */
     
  274.  
    extern int Speed_Limit; // 最大速度
     
  275.  
    extern char flag_brake;
     
  276.  
    extern float R_x;
     
  277.  
    extern float R_y;
     
 

 

基于运动合成分解的舵轮底盘运动模型（以正三角形三轮底盘为例）_四舵轮运动模型-CSDN博客

                

        

        

    

  


            

                    
            

    

      

        

            

              
                
                  致守
                
              
            

            

                关注
              
            

        

        

          
      

      

            

                专栏目录
          









                



	

		

			

				
					
四舵轮底盘运动学模型（四轮四独立转向）一

				
			

			

				

					苏凯的博客
				

				

					02-22
					
					2940
					
				

			

		

		

			
				
舵轮4转向活动范围 x 轴正半轴，x轴正方向为0弧度，左正右负， -M_PI/2  < 转向范围 < M_PI/2;移动底盘的正前方为x方向，左侧为y轴方向（遵循右手坐标系，右手大拇指指向天空为z轴方向，食指与中指垂直，食指指向小车正前方为x轴，中指指向为移动底盘y轴方向）转向：逆时针旋转角速度w为正，顺时针旋转角速度w为负。（遵循右手螺旋法则，右手握拳大拇指指向天空为旋转z轴方向，拳垂直于地面，4指方向即底盘转动的正方向）左前轮为1号轮，右前轮为2号轮，左后轮为3号轮，右后轮为4号轮。

			
		

	



                

              
                



	

		

			

				
					
三轮舵轮底盘与四轮舵轮底盘算法及仿真.zip

					
最新发布

				
			

			

				

					06-25
				

			

		

		

			
				
三轮舵轮底盘与四轮舵轮底盘算法及仿真

			
		

	



                


  
              

                


	

		

			

				
					
无人车之美——双舵轮运动分解与合成

				
			

			

				

					slampai的博客
				

				

					11-07
					
					4089
					
				

			

		

		

			
				
本文以通俗易懂地方式介绍舵轮的关键特性，并对双舵轮车的运动分解和运动合成进行了公式推导。

			
		

	





	

		

			

				
					
对角安装的双舵轮AGV运动学核心算法

				
			

			

				

					白色冰激凌的技术专栏
				

				

					11-27
					
					1万+
					
				

			

		

		

			
				
我们在控制移动机器人时，比较核心和基础的问题是机构的运动学建模，一般常规可看到的AGV都是差速轮的机构，此种机构控制较为简单，运动学算法也很容易。但是此种结构的AGV不适合在重载的场合，抗打滑的性能也较为不理想。双舵轮的AGV能有效的弥补上述问题，这种双舵轮的AGV一般使用对角安装的方式安装，具有很高的灵活性，不仅承载能力强，而且可以全向移动，是一种非常不错的重载AGV机构方案，在...

			
		

	



		

			
		



	

		

			

				
					
双舵轮AGV轨迹跟踪Pure Pursuit算法模型分析、python代码实现

				
			

			

				

					少磊的博客
				

				

					07-23
					
					1万+
					
				

			

		

		

			
				
一、Pure Pursuit算法模型图解和公式推导
下图分析的双舵轮AGV的运动学模型，是按照前后舵轮均安装在车体中心线上的车型分析的，当然其它结构的车型也可以按照这样的分析方法画出如下图解。





符号
物理量




r(m)
车体中心（小圆）转弯半径


R(m)
车轮（大圆）转弯半径


L(m)
轴距


δ(rad)
前轮转角


α(rad)
车身与预瞄点夹角


Ld(m)
预瞄距离


e(m)
与预瞄点的横向偏差


Xr(m)
预瞄点横坐标


Yr(m)
预瞄点纵坐标


通过正

			
		

	





	

		

			

				
					
四舵轮运动算法解析

					
热门推荐

				
			

			

				

					qq_44164303的博客
				

				

					12-15
					
					1万+
					
				

			

		

		

			
				
文章目录舵轮底盘运动解析一、目标二、两个舵轮两个前提变量设定几何关系物理关系三、四个舵轮两个前提变量设定几何关系物理关系四、总结

舵轮底盘运动解析
agv四舵轮底盘已经很常见了，他的灵活性高，驱动轮能量利用率高，底盘算法解析也早已烂大街，现在写一下自己的想法，来记录一下


一、目标
遥控器给三个参数，与车体方向同方向的:  VxV_{x}Vx​
与车体方向垂直的:  VyV_{y}Vy​
使车体自旋的:  www
通过这三个参数，完成车体的所有动作，即给任意三个参数组合，车体都应能完成目标，包括围着四个

			
		

	





	

		

			

				
					
【VREP】四舵轮（or n舵轮）自旋与平移融合运动解算

				
			

			

				

					LanLanLan1927的博客
				

				

					09-20
					
					9963
					
				

			

		

		

			
				
实现效果如图：





1.四舵轮模型

先介绍舵轮几个概念

v1 v2 v3 v4四个舵轮轮速

O1 O2 O3 O4四个舵轮舵角，规定舵角为与车体正方向为x正半轴建立坐标系中，与x正半轴的夹角，大小为-180到180

V Vw  V是用户输入车体的平移方向，V是是矢量（假如输入Vx与Vy那么可以自己算V)与自旋速度值。我们要做的就是分解三个速度为八个电机的值





平移

平移应该很容易相通就是四个舵角朝向一致，轮速为V



自旋

自旋也很容易，逆时针为正向的话，此时就是轮子摆...

			
		

	





	

		

			

				
					
关于舵轮AGV 的数学模型

				
			

			

				

					千叶why
				

				

					08-29
					
					4925
					
				

			

		

		

			
				
最近在搞舵轮agv的控制。

在网上看到了乡下老农写的模型，看起来很直观也很清楚。

链接：https://www.cnblogs.com/agvcfy/p/9498556.html

最关键的就是w1=w2=w

可以得到v1 = v*（R1/R）；v2=v*（R2/R）


...

			
		

	





	

		

			

				
					
车辆运动学及动力学建模

				
			

			

				

					qq_42911695的博客
				

				

					03-16
					
					1451
					
				

			

		

		

			
				
https://www.cnblogs.com/yrm1160029237/p/10087315.html



			
		

	





	

		

			

				
					
双舵轮AGV控制简介1.docx

				
			

			

				

					02-15
				

			

		

		

			
				
磁导航AGV除机械结构之外，电气部分主要包括：车载控制器、磁导航传感器、地标传感器、激光避障传感器、遥控器、触摸屏、急停开关、三色灯、安全触边、电池、伺服驱动器、舵轮（伺服电机）、无线通讯模块等，系统图如下：

			
		

	





	

		

			

				
					
两轮差速底盘的运动模型分析：运动控制与里程计解算2018-11-03 by wsc

				
			

			

				

					11-03
				

			

		

		

			
				
##### 2.1 底盘模型  两轮差速底盘的基本模型如下图所示（图1.2.1）。在此模型中，两个动力轮独立工作，通过控制每个轮子的速度来改变机器人的行进方向。  ##### 2.2 机器人本身约束方程  根据两轮差速底盘的物理...

			
		

	





	

		

			

				
					
如何用C语言编写舵轮底盘控制

				
			

			

				

					09-09
				

			

		

		

			
				
主函数可以通过用户输入或传感器数据等方式获取控制指令，并根据指令调用相应的控制函数来控制舵轮和底盘的运动。  在编写过程中，还需要考虑一些附加功能，例如检测舵轮和底盘是否达到目标位置、防止底盘碰撞等。  ...

			
		

	





	

		

			

				
					
麦轮运动学模型

				
			

			

				

					xiewf的博客
				

				

					11-26
					
					2885
					
				

			

		

		

			
				
转自http://bbs.elecfans.com/jishu_1583797_1_1.html

在ROS中，如果使用Twist消息，您通常会设置linear.x，linear.y和angular.z字段。一个不相关的说法是，如果你在不平坦的地形上作业，那么做mecanum类型的动作将失败并且有很多滑倒。滑移引导类型的运动通常会更好（使用mecanum车轮）。


WHEEL_SEPARATIO...

			
		

	





	

		

			

				
					
RoboMaster舵轮底盘代码分享

				
			

			

				

					weixin_73037889的博客
				

				

					06-05
					
					6327
					
				

			

		

		

			
				
以上便是本人对舵轮底盘的学习分享，自己加入嵌入式组也仅仅只有一年，还有很多要学习的地方。文章中有什么错误的、不合理的地方也希望大家能批评指正。写这篇博客旨在对自己近期的学习进行总结，也希望能帮助到大家。代码中还有很多需要改善的地方。由于机械组还未做出舵轮底盘，所以以下的视频仅仅只是电机的测试。舵轮电机测试。

			
		

	





	

		

			

				
					
2018/9/18单舵轮AGV项目笔记之二

				
			

			

				

					UCloud
				

				

					09-18
					
					9851
					
				

			

		

		

			
				
        找了很久的单舵机AGV运动模型，网上几乎没有这种开源的东西。查找了一些文献，发现还挺有用的。证明了不是所有论文只能用来看，有些优秀的文章确实是能应用到工程之中的。
       先声明一下拜读的文献，浙大的范堃《自动导引车车载系统研究与实现》。在此谢过。以下是该文章提出的模型：



这个模型中的输入已知量为agv舵轮的线速度v和当前舵轮的方位角alpha（最重要的一点是无需舵轮的...

			
		

	





	

		

			

				
					
里程计模型（1）：两轮差分底盘和三轮全向底盘

				
			

			

				

					JasonLi的博客
				

				

					01-22
					
					4729
					
				

			

		

		

			
				
对基础的两轮差分底盘和三轮全向底盘进行运动学模型推导。

			
		

	





	

		

			

				
					
关于单舵轮AGV运动学模型整理笔记

				
			

			

				

					UCloud
				

				

					09-15
					
					1万+
					
				

			

		

		

			
				
2.1.单舵机图画：



 



2.



                            

                  

 

                        

 

 

 

			
		

	





	

		

			

				
					
全向移动小车运动控制_双舵轮AGV移动机器人解决方案

				
			

			

				

					weixin_39810901的博客
				

				

					12-30
					
					2690
					
				

			

		

		

			
				
双舵轮AGV移动机器人小车底盘布局方案图双舵轮AGV移动机器人小车解决方案双舵轮AGV移动机器人意指一台AGV小车配置两台【凤凰动力】电驱动-舵轮，配置两只inAGV辅助从动轮(三轮结构)或四只辅助轮(六轮结构)。视频长度36秒以上视频为双舵轮全向AGV小车演示案例视频供参考双舵轮AGV移动搬运机器人小车可实现，前进-后退-左右拐弯-原地旋转-侧向横移-弧线拐弯等全向行走功能。整体性能优...

			
		

	



              




          




        



    





    



      

      

        
      

      





	

		评论	
	

	
	




  

    

      添加红包
      
    

    

      

        
        

          
          
        

        
请填写红包祝福语或标题

      

      

        
        

          
          个
        

        
红包个数最小为10个

      

      

        
        

          
          元
        

        
红包金额最低5元

      

      

        
        

          当前余额3.43元
          前往充值 >
        

      

      

        

          需支付：10.00元
        

        
        
      

    

  





  

    
    
  

  

    
    
  








  

    

      

        

          

            
            
成就一亿技术人!

          

          

        

        

          

          

            领取后你会自动成为博主和红包主的粉丝
            规则
          

        

      

      

        

          

            
              
            
            

              
hope_wisdom
 发出的红包
            

          

        

        

          

          

          

        

      

    

    

  



      
      

      
实付元

      
使用余额支付

      

        

          

            
            点击重新获取
          

        

        
扫码支付

      

      

        
          
            
          
          
        
      

      

        
        钱包余额
          0
          

            
            

              

                
抵扣说明：

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

              

            

          

      

      余额充值