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

fangchenxin

已于 2023-03-09 13:34:11 修改

阅读量4.2k

点赞数 22

文章标签：算法

于 2022-08-03 17:55:27 首次发布

本文链接：https://blog.csdn.net/m0_62592252/article/details/126129332

版权

此文档原本是对附录中代码的解释，也可单独作为舵轮底盘分析的参考，除第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 正三角形模型参考方向

在运动模型分析时都以机器人坐标系为参考坐标系，其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.停止

设计有两种停止情况：

没有收到运动指令（运动状态三个值都为0，无操作时自动进入），即模型部分程序中最后一个else，不进行任何动作，所有车轮方向保持不变，速度都设置为0。当运动状态有非0值时可以立刻继续运动。
驻停。通过设置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}$

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

附录

现代码

/*
*********************************************************************************************************
* Brief    : World frame to robot frame transform formula.
*
* Param(s) : world - velocity vector(Vx,Vy,Vz) in inertia frame. (mm/s, rad/s)
*      robo  - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
*
* Return(s): none.
*
*********************************************************************************************************
*/
void Formula_World2Robo (Velocity *world, Velocity *robo)//转角相反
{
  float sinTheta = sin(RobotPos.theta);
  float cosTheta = cos(RobotPos.theta);
  
  robo->Vx = world->Vx * cosTheta + world->Vy * sinTheta;
  robo->Vy = world->Vy * cosTheta - world->Vx * sinTheta;
  robo->Vz = world->Vz;
}


/*
*********************************************************************************************************
* Brief    : robot frame to wheel frame transform formula.
*
* Param(s) : wheel - velocity vector(Vx,Vy,Vz) in wheel frame. (0.1 enc counts/s)
*      robo  - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
*
* Return(s): none.
*·
*********************************************************************************************************
*/
//float theta_angle[3];//转向以角度形式展示
void Formula (Velocity *robo)
{
  int MULtiple=1;
  int flag;
  float V;
  float Vx_part[3]={0,0,0},Vy_part[3]={0,0,0},theta_temp[3]={0,0,0};
  Wheel3.L = 35;//车轮到三角中心的距离

  
  //输入Vx_in,Vy_in
  //输入Wheel3.omega
    Wheel3.Vx = robo->Vx;
    Wheel3.Vy = robo->Vy;
    Wheel3.omega = robo->Vz * PI/180 * Wheel3.L;
  V = sqrt(Wheel3.Vx * Wheel3.Vx + Wheel3.Vy * Wheel3.Vy);
  if (Wheel3.halt)//驻停
  {
    Wheel3.theta[0] = PI/2;
    Wheel3.theta[1] = Wheel3.theta[0];
    Wheel3.theta[2] = Wheel3.theta[0];
    
    Wheel3.V[0] = 0;
    Wheel3.V[1] = Wheel3.V[0];
    Wheel3.V[2] = Wheel3.V[0];
  }
  else if (V || Wheel3.omega)
  {
      if (Wheel3.Vy >= 0)
        flag = 1;
      else 
        flag = -1;

        //反三角计算角度
        if (Wheel3.Vx != 0)
        {
          theta_temp[0] = flag*acos(Wheel3.Vx / V);
          theta_temp[1] = theta_temp[0];
          theta_temp[2] = theta_temp[0];
        }
        else
        {
          if (flag==1)
          {
            theta_temp[0] = PI/2;
            theta_temp[1] = theta_temp[0];
            theta_temp[2] = theta_temp[0];
          }
          else
          {
            theta_temp[0] = -PI/2;
            theta_temp[1] = theta_temp[0];
            theta_temp[2] = theta_temp[0];
          }
        }//反三角计算角度
        
        //平移旋转速度正交分解叠加
        Vx_part[0] = V * cos(theta_temp[0]);
        Vx_part[1] = V * cos(theta_temp[1]);
        Vx_part[2] = V * cos(theta_temp[2]);
        Vy_part[0] = V * sin(theta_temp[0]);
        Vy_part[1] = V * sin(theta_temp[1]);
        Vy_part[2] = V * sin(theta_temp[2]);
        Vx_part[0] += Wheel3.omega;
        Vx_part[1] -= Wheel3.omega / 2;
        Vy_part[1] += Wheel3.omega * sqrt(3)/2;
        Vx_part[2] -= Wheel3.omega /2;
        Vy_part[2] -= Wheel3.omega * sqrt(3)/ 2;
        Wheel3.V[0] = sqrt(Vx_part[0]*Vx_part[0] + Vy_part[0]*Vy_part[0]);
        Wheel3.V[1] = sqrt(Vx_part[1]*Vx_part[1] + Vy_part[1]*Vy_part[1]);
        Wheel3.V[2] = sqrt(Vx_part[2]*Vx_part[2] + Vy_part[2]*Vy_part[2]);
        
        //叠加后重新计算角度
        for (int i=0;i<3;i++)
        {
          if (Vx_part[i] != 0)
          {
            if (Vy_part[i] >= 0)
            {
              Wheel3.theta[i] =  acos(Vx_part[i] / Wheel3.V[i]);
            }
            else
            {
              Wheel3.theta[i] = -acos(Vx_part[i] / Wheel3.V[i]);
            }
          }
          else
          {
            if (Vy_part[i] == 0)
            {
              Wheel3.theta[i] = 0;
            }
            if (Vy_part[i] > 0)
            {
              Wheel3.theta[i] = PI/2;
            }
            else
            {
              Wheel3.theta[i] = -PI/2;
            }
          }
        }//叠加后重新计算角度
        //基准位置修正，编码器向后到编码器向里
        Wheel3.theta[1] -= 2*PI/3;
        Wheel3.theta[2] += 2*PI/3;
      }//平移旋转
    else//其他情况:仅停止，不转轮
    {
      Wheel3.V[0] = 0;
      Wheel3.V[1] = Wheel3.V[0];
      Wheel3.V[2] = Wheel3.V[0];
    }
  
  //限制角度范围
  Wheel3.theta[0] = trans_PI(trans_PI(Wheel3.theta[0]));
  Wheel3.theta[1] = trans_PI(trans_PI(Wheel3.theta[1]));
  Wheel3.theta[2] = trans_PI(trans_PI(Wheel3.theta[2]));
//  //弧度转角度，仅供参考，无实际作用
//  theta_angle[0] = trans_PI(Wheel3.theta[0])*180/PI;
//  theta_angle[1] = trans_PI(Wheel3.theta[1]+PI*2/3)*180/PI;
//  theta_angle[2] = trans_PI(Wheel3.theta[2]-PI*2/3)*180/PI;
//*************************************限制舵轮旋转圈数****************************************************
  //拓展至正负360度
  double Exparg_temp[3][3],diff_arg[3][3];
  int diff_min[3];
  for (int i=0;i<3;i++)
  {
    if( -1179504.0 < MOTOR_C610[i+1].Temparg < 1179504.0 )  //(2*PI) 允许自由旋转范围
    {
        Exparg_temp[i][0] = (int)(Wheel3.theta[i]*187723.89199);//轮朝向转为电机位置
        Exparg_temp[i][1] = Exparg_temp[i][0] + 589752.0;//PI 正向多半圈
        Exparg_temp[i][2] = Exparg_temp[i][0] - 589752.0;//PI 负向多半圈
        diff_arg[i][0] = Exparg_temp[i][0] - MOTOR_C610[i+1].Temparg;
        diff_arg[i][1] = Exparg_temp[i][1] - MOTOR_C610[i+1].Temparg;
        diff_arg[i][2] = Exparg_temp[i][2] - MOTOR_C610[i+1].Temparg;
        diff_min[i] = (abs(diff_arg[i][0]) < abs(diff_arg[i][1]) + 10.0) ? 0:1;
        diff_min[i] = (abs(diff_arg[i][2] - 10.0 ) < diff_arg[i][diff_min[i]]) ? 2:diff_min[i];
        if( -294876  < diff_arg[i][diff_min[i]] < 294876 )//当前位置正负90度内（应已包含所有角度）取最小转动角度
        {
          if (diff_min[i] == 0)
          {
            MOTOR[i+1].ExpSpeed = -(int)((Wheel3.V[i])*MULtiple);
            MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][0]);
          }
          else if (diff_min[i] == 1)
          {
            MOTOR[i+1].ExpSpeed = (int)((Wheel3.V[i])*MULtiple);
            MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][1]);
          }
          else if (diff_min[i] == 2)
          {
            MOTOR[i+1].ExpSpeed = (int)((Wheel3.V[i])*MULtiple);
            MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][2]);
          }
          else 
          {}//错误
        }        
        else if( -294876 > diff_arg[i][diff_min[i]] )//负向超过90度（可能始终不会执行）
        {
          MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
          MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][diff_min[i]] + 589752.0); 
        }
        else//正向超过90度（可能始终不会执行）
        {
          MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
          MOTOR_C610[i+1].Exparg = (int)(MOTOR_C610[i+1].Temparg + diff_arg[i][diff_min[i]] - 589752.0);
        }
    }    
    else if( -1179504.0 > MOTOR_C610[i+1].Temparg )//负向超过限幅
    {
      MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
      MOTOR_C610[i+1].Exparg=(int)(-(Wheel3.theta[i]+PI)*187723.89199);//1/(2*PI)*8191*36*4
    }
    else//正向超过限幅
    {
      MOTOR[i+1].ExpSpeed=(int)((Wheel3.V[i])*MULtiple);
      MOTOR_C610[i+1].Exparg=(int)(-(Wheel3.theta[i]-PI)*187723.89199);//1/(2*PI)*8191*36*4
    }
  }//for
}//Formula_4Omni末尾

float trans_PI(float n)//[-2PI,2PI]->[-PI,PI]
{
  if (n > PI) n -= 2*PI;
  else if (n < -PI) n += 2*PI;
  return n;
}

原代码

/*
*********************************************************************************************************
* Brief    : World frame to robot frame transform formula.
*
* Param(s) : world - velocity vector(Vx,Vy,Vz) in inertia frame. (mm/s, rad/s)
*      robo  - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
*
* Return(s): none.
*
*********************************************************************************************************
*/
void Formula_World2Robo (Velocity *world, Velocity *robo)
{
  float sinTheta = sin(RobotPos.theta);
  float cosTheta = cos(RobotPos.theta);
  
  robo->Vx = world->Vx * cosTheta + world->Vy * sinTheta;
  robo->Vy = world->Vx * ( -sinTheta ) + world->Vy * cosTheta;
  robo->Vz = world->Vz;
}


/*
*********************************************************************************************************
* Brief    : robot frame to wheel frame transform formula.
*
* Param(s) : wheel - velocity vector(Vx,Vy,Vz) in wheel frame. (0.1 enc counts/s)
*      robo  - velocity vector(Vx,Vy,Vz) in robot frame. (mm/s, rad/s)
*
* Return(s): none.
*
*********************************************************************************************************
*/

void Formula (Velocity *robo)
{
  int MULtiple=1;
  int flag,halt;
  float V;
  float angleA,angleB,angleC,angle[3];
  float cosA,sinA,cosB,sinB,cosC,sinC;
  float ang_action;
  Wheel3.L = 35;//车轮到三角中心的距离
  ang_action = 0.261799387791;//15 / 180 * PI，action和车头的偏差角15度
  
  //输入Vx_in,Vy_in
  //输入Wheel3.omega
    Wheel3.Vx = robo->Vx;
    Wheel3.Vy = robo->Vy;
    Wheel3.omega = robo->Vz;
  V = sqrt(Wheel3.Vx * Wheel3.Vx + Wheel3.Vy * Wheel3.Vy);
  halt = 0; 
  if (halt)//驻停
  {
    Wheel3.theta[0] = PI/2;
    Wheel3.theta[1] = Wheel3.theta[0];
    Wheel3.theta[2] = Wheel3.theta[0];
    
    Wheel3.V[0] = 0;
    Wheel3.V[1] = Wheel3.V[0];
    Wheel3.V[2] = Wheel3.V[0];
  }
  else
  {
    if(V)//平移
    {
      if (Wheel3.Vy >= 0)
        flag = 1;
      else 
        flag = -1;
      
      if( Wheel3.omega > 0 )//平移并旋转
      {
        //基准位置
        Wheel3.alfa = PI/2 + flag*acos(Wheel3.Vx/V) + ang_action;
        Wheel3.R = V / Wheel3.omega;
        angleA = PI/2 + Wheel3.alfa;
        angleB = -PI/6 + Wheel3.alfa;
        angleC = PI/6 + Wheel3.alfa + PI;
        angleA = trans_PI(angleA);
        angleB = trans_PI(angleB);
        angleC = trans_PI(angleC);
        
        //中间量
        cosA = Wheel3.L + Wheel3.R * cos(angleA);
        cosB = Wheel3.L + Wheel3.R * cos(angleB);
        cosC = Wheel3.L + Wheel3.R * cos(angleC);
        sinA = Wheel3.R * sin(angleA);
        sinB = Wheel3.R * sin(angleB);
        sinC = Wheel3.R * sin(angleC);
        
        //轮半径
        Wheel3.r[0] = sqrt(cosA * cosA + sinA * sinA);
        Wheel3.r[1] = sqrt(cosB * cosB + sinB * sinB);
        Wheel3.r[2] = sqrt(cosC * cosC + sinC * sinC);
        
        //轮速度
        Wheel3.V[0] = Wheel3.omega * Wheel3.r[0];
        Wheel3.V[1] = Wheel3.omega * Wheel3.r[1];
        Wheel3.V[2] = Wheel3.omega * Wheel3.r[2];
        
        //轮角度
        angle[0] = atan(sinA / cosA);
        angle[1] = atan(sinB / cosB);
        angle[2] = atan(sinC / cosC);
        Wheel3.theta[0] = (cosA > 0)?(angle[0]):(angle[0] + PI);
        Wheel3.theta[1] = (cosA > 0)?(angle[1]):(angle[1] + PI);
        Wheel3.theta[2] = (cosA > 0)?(angle[2]):(angle[2] + PI);
      }
      else//不旋转
      {
        //统一速度赋值
        Wheel3.V[0] = V;
        Wheel3.V[1] = Wheel3.V[0];
        Wheel3.V[2] = Wheel3.V[0];
        
        //反三角计算角度
        if (Wheel3.Vx != 0)
        {
          Wheel3.theta[0] = flag*acos(Wheel3.Vx / V);
          Wheel3.theta[1] = Wheel3.theta[0];
          Wheel3.theta[2] = Wheel3.theta[0];
        }
        else
        {
          if (flag==1)
          {
            Wheel3.theta[0] = PI/2;
            Wheel3.theta[1] = Wheel3.theta[0];
            Wheel3.theta[2] = Wheel3.theta[0];
          }
          else
          {
            Wheel3.theta[0] = -PI/2;
            Wheel3.theta[1] = Wheel3.theta[0];
            Wheel3.theta[2] = Wheel3.theta[0];
          }
        }
        //基准位置修正
        Wheel3.theta[1] += 2*PI/3;
        Wheel3.theta[2] -= 2*PI/3;

      }
    }
    else if (Wheel3.omega != 0)//仅旋转无位移
    {
      Wheel3.theta[0] = 0;
      Wheel3.theta[1] = Wheel3.theta[0];
      Wheel3.theta[2] = Wheel3.theta[0];
      Wheel3.V[0] = Wheel3.L * Wheel3.omega;
      Wheel3.V[1] = Wheel3.V[0];
      Wheel3.V[2] = Wheel3.V[0];
    }
    else//其他情况:仅停止，不转轮
    {
      Wheel3.V[0] = 0;
      Wheel3.V[1] = Wheel3.V[0];
      Wheel3.V[2] = Wheel3.V[0];
    }
  }
  //限制角度范围
  Wheel3.theta[0] = trans_PI(Wheel3.theta[0]);
  Wheel3.theta[1] = trans_PI(Wheel3.theta[1]);
  Wheel3.theta[2] = trans_PI(Wheel3.theta[2]);
  
//*************************************限制舵轮旋转圈数****************************************************
  
    if( (Wheel3.theta[0]+Encoder_MotorC610[1]/187723.89199 >= -PI/2) && (Wheel3.theta[0]+Encoder_MotorC610[1]/187723.89199 <= PI/2) )
    {
        MOTOR_C610[1].Exparg=(int)(-Wheel3.theta[0]*187723.89199)+Encoder_MotorC610[1];//1/(2*PI)*8191*36*4
        MOTOR[1].ExpSpeed=(int)(Wheel3.V[0]*MULtiple);
    }
    else if( (Wheel3.theta[0]+Encoder_MotorC610[1]/187723.89199) < -PI/2 )
    {
        MOTOR_C610[1].Exparg=(int)(-(Wheel3.theta[0]+PI)*187723.89199)+Encoder_MotorC610[1];//1/(2*PI)*8191*36*4
        MOTOR[1].ExpSpeed=-(int)((Wheel3.V[0])*MULtiple);
    }
    else
    {
        MOTOR_C610[1].Exparg=(int)(-(Wheel3.theta[0]-PI)*187723.89199)+Encoder_MotorC610[1];//1/(2*PI)*8191*36*4
        MOTOR[1].ExpSpeed=-(int)((Wheel3.V[0])*MULtiple);
    }
    if( (Wheel3.theta[1]+Encoder_MotorC610[2]/187723.89199 >= -PI/2) && (Wheel3.theta[1]+Encoder_MotorC610[2]/187723.89199 <= PI/2) )
    {
        MOTOR_C610[2].Exparg=(int)(-Wheel3.theta[1]*187723.89199)+Encoder_MotorC610[2];//1/(2*PI)*8191*36*4
        MOTOR[2].ExpSpeed=(int)(Wheel3.V[1]*MULtiple);
    }
    else if( (Wheel3.theta[1]+Encoder_MotorC610[2]/187723.89199) < -PI/2 )
    {
        MOTOR_C610[2].Exparg=(int)(-(Wheel3.theta[1]+PI)*187723.89199)+Encoder_MotorC610[2];//1/(2*PI)*8191*36*4
        MOTOR[2].ExpSpeed=-(int)((Wheel3.V[1])*MULtiple);
    }
    else
    {
        MOTOR_C610[2].Exparg=(int)(-(Wheel3.theta[1]-PI)*187723.89199)+Encoder_MotorC610[2];//1/(2*PI)*8191*36*4
        MOTOR[2].ExpSpeed=-(int)((Wheel3.V[1])*MULtiple);
    }
    if( (Wheel3.theta[2]+Encoder_MotorC610[3]/187723.89199 >= -PI/2) && (Wheel3.theta[2]+Encoder_MotorC610[3]/187723.89199 <= PI/2) )
    {
        MOTOR_C610[3].Exparg=(int)(-Wheel3.theta[2]*187723.89199)+Encoder_MotorC610[3];///(2*PI)*8191*36*4
        MOTOR[3].ExpSpeed=(int)(Wheel3.V[2]*MULtiple);
    }
    else if( (Wheel3.theta[2]+Encoder_MotorC610[3]/187723.89199) < -PI/2 )
    {
        MOTOR_C610[3].Exparg=(int)(-(Wheel3.theta[2]+PI)*187723.89199)+Encoder_MotorC610[3];//1/(2*PI)*8191*36*4
        MOTOR[3].ExpSpeed=-(int)((Wheel3.V[2])*MULtiple);
    }
    else
    {
        MOTOR_C610[3].Exparg=(int)(-(Wheel3.theta[2]-PI)*187723.89199)+Encoder_MotorC610[3];//1/(2*PI)*8191*36*4
        MOTOR[3].ExpSpeed=-(int)((Wheel3.V[2])*MULtiple);
    } 
}

float trans_PI(float n)//[-2PI,2PI]->[-PI,PI]
{
  if (n > PI) n -= 2*PI;
  else if (n < -PI) n += 2*PI;
  return n;
}

头文件

/*
*********************************************************************************************************
*                                             INCLUDE FILES
*********************************************************************************************************
*/

#pragma once
#include "includes.h"

/*
*********************************************************************************************************
*                                                EXTERNS
*********************************************************************************************************
*/

#ifdef   ROBOT_CTRL_MODULE
#define  ROBOT_CTRL_EXT
#else
#define  ROBOT_CTRL_EXT  extern
#endif

//****************************************************************           ???           ****************************************************************
#ifdef   CHASSIS_MODULE
#define  CHASSIS_EXT
#else
#define  CHASSIS_EXT  extern
#endif
//****************************************************************           ???          ****************************************************************

/*
*********************************************************************************************************
*                                             	 DEFINES
*********************************************************************************************************
*/

// CAN configuration
#define SYNC_CYCLE        5  // 100 ms

// coefficient needed by transform formula
#define THREE_OMNI_D      386.3 // distance between wheel center and spin center
#define FOUR_OMNI_D       280.0
#define FOUR_Mecunum_L    320.0
#define FOUR_Mecunum_l    355.0

// wheel data 
#define WHEELNUM        4
#define	WHEEL_R         100.0 // radius(mm)
#define WHEEL_FORMULA   Formula_4Omni

// motor data 
#define GEAR_RATIO      (1.0/1.0)



/*
*********************************************************************************************************
*                                             EXPORTED_TYPES
*********************************************************************************************************
*/

// Speed data structure 
typedef struct
{
  int32_t x; // mm
  int32_t y; // mm
  float z; // degree
  
}Point;

typedef struct
{
  float Vx;
  float Vy;
  float Vz;
  
}Velocity; // unit: mm/s, rad/s

typedef struct
{
  Velocity curSpeed; // (mm/s, mm/s, rad/s)
  Velocity expSpeed;
  Velocity outSpeed;
  Point expPos; // (mm, mm, rad)
  
}MOTION;

typedef struct
{
  float pos_x1; // mm
  float pos_x2; // mm
  float pos_z; // degree
  
}US;

// Position data structure
typedef struct
{
  float pos_x; // mm
  float pos_y; // mm
  float zangle; // degree
  float xangle; // degree
  float yangle; // degree
  float w_z;
  float theta; // rad
  
}POSE;

// Laser pos
typedef struct
{
  float pos_x; // mm
  float pos_y; // mm
  float pos_z; // degree
  
}LASER;

// Move method
typedef enum
{
  P2P = 0,
  MULTI,
  PCL,
  
}MOVE;

// Control method
typedef enum
{
  MANUAL = 0,
  AUTO,
  
}CTRL;

typedef enum // 机器人当前所在位置
{
  Start = 0,
  LZ1, // 彩球
  LZ2, // 金球
  MeetPoint1,
  MeetPoint2,
  
}BOT;

typedef struct
{
  MOVE RunMode;
  CTRL OP;
  BOT  State;
  
  void (*Move2Point) (uint8_t);
  void (*WaitREADY) (void);
  void (*DONE) (void);
  
}ACT;

typedef struct
{
  char ID;
  int16_t current; // robomodule反馈的数据
  int16_t velocity;
  int32_t position;
  int32_t realposition;
  
}BDC;


// Accleration 
typedef struct
{
  float ax;
  float ay;
  float az;
  
}Accelerate;

typedef struct
{
  /* Velocity profile parameter */
  Point qA;               // Start point,     uint: mm
  Point qB;               // End Point,       uint: mm
  Point delAB;			//delAB 			unit:mm
  uint8_t end;
  int32_t Dist;			// 	Distance		uint: mm
  int32_t Acc;
  int32_t Dec;
  int32_t Vlin;          // mm/s, rad/s
  int32_t AccTime;
  int32_t Z_AccTime;
  float 	DecTheta;
  float 	AccTheta;
  float 	Wz;
  float	Z_Acc;
  float	Z_Dec;
  float	Theta;
  float 	DecDist;
}PclData;

typedef struct
{
  int32_t Motor_V;		  // 0.1 enc counts/sec
  int16_t Motor_Torq;		// rated torque / 1000
  
  int32_t Target_V;
  
}Motor_Data;
typedef struct
{
float L;//边长的一半，常数
float Vx;//输入，action坐标系下的速度
float Vy;//输入，action坐标系下的速度
float omega;//输入，action坐标系下的角速度
float alfa;//车头定位
float R;//总转向半径
float r[4];//轮半径
float theta[4];//轮方向
float V[4];//轮速度
int halt;//停止移动指令
}LUN;

extern LUN     Wheel3;
extern LUN     Wheel4;
extern PclData  PclBlock;


/*
*********************************************************************************************************
*                                            GLOBAL VARIABLES
*********************************************************************************************************
*/

ROBOT_CTRL_EXT MOTION     RoboMotion;
ROBOT_CTRL_EXT POSE       RobotPos;
ROBOT_CTRL_EXT POSE       RobotExpPos;
ROBOT_CTRL_EXT US         RobotPosUS;
ROBOT_CTRL_EXT LASER      RobotPosition;
ROBOT_CTRL_EXT ACT        RobotCTRL;
ROBOT_CTRL_EXT Point      COULMN_POS[40];
ROBOT_CTRL_EXT Point      COULMN_ForestPOS[20];
ROBOT_CTRL_EXT Point      JCC[15];
ROBOT_CTRL_EXT BDC        BrushDC;
ROBOT_CTRL_EXT CTRL       mode;

ROBOT_CTRL_EXT char      PIDflag;
ROBOT_CTRL_EXT char      FeedbackData;

CHASSIS_EXT Motor_Data Wheel[WHEELNUM+1]; // Index 0 is reserved
CHASSIS_EXT Motor_Data Target_V;

/*
*********************************************************************************************************
*                                           FUNCTION PROTOTYPES
*********************************************************************************************************
*/
float trans_PI(float n);
void Robot_Motion_Init (void);
void Robot_Speed_Update (void);
void Robot_Speed_Ctrl (void);
void SyncSignal (void);

void Pos_Paracal(PclData *profileData,uint8_t col);
void Robot_Pos_Ctrl (void);
void Chassis_Init (void);
void SpeedCtrl (void);
void SendWheel_Vel (void);
void Send_Velocity (Motor_Data *wheel);
void Formula_4Omni (Velocity *robo);
float vision_turn(float *abc_data);
/*
*********************************************************************************************************
*                                               MODULE END
*********************************************************************************************************
*/

extern int Speed_Limit; // 最大速度
extern char flag_brake;
extern float R_x;
extern float R_y;

fangchenxin

关注

22
点赞
踩
107

收藏

觉得还不错? 一键收藏
16
评论
基于运动合成分解的舵轮底盘运动模型（以正三角形三轮底盘为例）

以正三角形三轮底盘为例，分析舵轮底盘在仅平移和仅旋转时的运动，并在此基础上使用运动合成分解的方式解算出舵轮底盘在同时平移旋转时各轮的转向和转速，附前后两代使用不同方法的程序代码。原先采用方法为几何分析，使用原方法对于正三角形、正方形、直角三角形底盘的运动进行分析，但最终并未成功。另含有根据编码器控制电机以最短路径转至指定方向且限制旋转圈数的程序代码及解释。...
复制链接

扫一扫