1. 项目概述如果你玩过RoboMaster机甲大师赛或者在一些物流AGV、特种机器人上见过那种能“横着走”、“斜着走”甚至原地旋转的底盘那你大概率已经见过麦克纳姆轮了。这种轮子看起来有点“怪异”轮缘上布满了与轮轴呈45度夹角的小辊子正是这些小辊子的特殊排列赋予了搭载它的平台全向移动的能力。我最早接触它是在一个自动化仓储的项目里当时需要一个能在狭窄货架间灵活穿梭的搬运平台传统的差速底盘转向半径太大而舵轮系统又过于复杂昂贵麦克纳姆轮方案就成了最优解。“麦克纳姆轮运动学模型”这个标题听起来很学术但它本质上解决的是一个非常实际的问题如何让一个装了四个“怪轮子”的机器人按照你心里想的路径和速度去运动反过来又如何通过测量四个轮子转了多少圈来精确计算机器人本体移动了多远、转了多少角度这就是运动学正解和逆解要干的事。没有这个模型你的控制器给轮子电机发指令就是“盲人摸象”轮子各转各的车子可能原地打转或者走成“蛇形”。今天我就结合自己从零搭建麦克纳姆轮小车的实战经验把这套模型从原理到代码掰开揉碎了讲清楚。2. 麦克纳姆轮基础与底盘构型解析2.1 麦克纳姆轮的工作原理斜向辊子的魔力麦克纳姆轮的核心秘密全在那些斜着安装的小辊子我们通常叫“辊子”或“从动轮”上。一个标准的麦克纳姆轮由主轮体和一圈辊子组成。关键点在于辊子的轴线与主轮轴线呈45度夹角。这个45度角是经过理论和实践验证的最优解它能最均衡地将轮子的旋转运动分解到平面内的两个垂直方向上。当主轮电机驱动轮体旋转时轮体本身会提供一个沿着轮子圆周切线方向的力。但由于辊子可以自由旋转且其轴线是斜的这个力会被分解。你可以这样想象轮子转动时与地面接触的其实是某个辊子。这个辊子因为可以绕自己的轴自由转动所以它不提供沿其轴线方向的摩擦力但会提供垂直于其轴线方向的摩擦力。正是这个垂直于辊子轴线的摩擦力分量推动了底盘运动。简单来说一个麦克纳姆轮产生的有效推进力方向是垂直于其辊子轴线的。对于一个左旋轮辊子轴线从左上到右下其产生的力方向是朝向右上或左下对于一个右旋轮辊子轴线从右上到左下其产生的力方向是朝向右下或左上。通过控制四个轮子的转速和方向组合这些力矢量合成就能让底盘实现平面内任意方向的平移和旋转。2.2 常见的底盘构型X型与O型单个轮子能力有限我们需要把它们组合起来。最常见的四轮麦克纳姆轮底盘有两种安装构型X型和O型。网上很多资料对这两种叫法有点混乱这里我以业界和ROS社区更常用的定义来解释O型构型这是最经典、最常用的布局。四个轮子呈矩形安装但同侧的两个轮子如左前和左后的辊子朝向是相同的而对角线上的两个轮子辊子朝向相同。从正上方看四个轮子与地面接触的辊子所形成的合力图案大致像一个“O”形环。这种构型运动学模型对称性好计算相对简单。X型构型同样四个轮子呈矩形安装但同侧的两个轮子如左前和左后的辊子朝向是相反的。从正上方看合力图案更像一个“X”形。这种构型在某些特定应用中有其优势但运动学模型稍复杂一些。我们接下来的讨论和推导将基于最普遍的O型构型展开。这也是RoboMaster比赛机器人和大多数商用AGV采用的布局。2.3 坐标系与参数定义一切计算的起点在建立数学模型之前我们必须严格定义坐标系和参数这是后续所有推导不出错的基础。我们采用ROS机器人操作系统中广泛使用的右手坐标系规则。底盘坐标系 (base_link)我们将坐标系原点固定在底盘几何中心。X轴指向机器人的正前方。Y轴指向机器人的左侧。Z轴垂直向上符合右手定则。旋转正方向绕Z轴即Yaw轴逆时针旋转为正。这是关键很多初学者在这里搞反。轮子编号与参数我们定义四个轮子的位置左前(LF)、右前(RF)、左后(LB)、右后(RB)。轮子速度v_w1,v_w2,v_w3,v_w4分别对应LF, RF, LB, RB。速度符号定义使机器人向前移动的轮子转速为正。底盘几何参数L_x底盘在X轴方向前后上两个轮子中心之间的距离的一半。即从底盘中心到前轮或后轮中心的距离。L_y底盘在Y轴方向左右上两个轮子中心之间的距离的一半。即从底盘中心到左轮或右轮中心的距离。底盘运动状态v_x底盘在自身坐标系下沿X轴方向向前的线速度。v_y底盘在自身坐标系下沿Y轴方向向左的线速度。ω底盘绕自身中心Z轴旋转的角速度逆时针为正。有了这些清晰的定义我们就可以开始构建连接轮子速度与底盘整体运动状态之间的数学桥梁了。3. 逆运动学模型从底盘指令到轮子转速逆运动学要解决的问题是已知我们期望底盘达到的运动状态[v_x, v_y, ω]^T求四个轮子各自应该达到的转速[v_w1, v_w2, v_w3, v_w4]^T。这是运动控制的核心控制器算出的结果直接发给电机驱动器。3.1 底盘运动的分解与轮心速度计算底盘在平面内的运动可以看作是其质心我们取几何中心的平移运动加上绕该点的旋转运动的合成。对于底盘上的任意一点比如某个轮子的中心其速度由两部分组成跟随底盘质心平移的速度[v_x, v_y]^T。由于底盘旋转ω而产生的切向速度。对于位于(x_i, y_i)的轮子i在底盘坐标系下其轮心处的合成速度[v_{xi}, v_{yi}]^T为v_{xi} v_x - ω * y_i v_{yi} v_y ω * x_i注意旋转速度分量的方向根据右手定则正的ω逆时针会使位于正Y轴左侧的点产生一个负X方向向后的速度分量所以第一项是- ω * y_i。同理位于正X轴前方的点会产生一个正Y方向向左的速度分量所以第二项是 ω * x_i。对于我们的O型构型四轮底盘四个轮子的位置坐标是左前(LF):( L_x, L_y)右前(RF):( L_x, -L_y)左后(LB):(-L_x, L_y)右后(RB):(-L_x, -L_y)代入公式我们可以得到每个轮心的速度v_x_lf v_x - ω * L_y v_y_lf v_y ω * L_x v_x_rf v_x - ω * (-L_y) v_x ω * L_y v_y_rf v_y ω * L_x v_x_lb v_x - ω * L_y v_y_lb v_y ω * (-L_x) v_y - ω * L_x v_x_rb v_x - ω * (-L_y) v_x ω * L_y v_y_rb v_y ω * (-L_x) v_y - ω * L_x3.2 轮心速度到轮子转速的映射得到轮心速度矢量[v_{xi}, v_{yi}]后我们需要知道这个速度会驱动麦克纳姆轮产生多大的旋转速度。这里就需要用到麦克纳姆轮的特性轮子产生的有效推进力方向垂直于其辊子轴线。反过来要产生某个方向的轮心速度轮子需要提供的转速也与这个方向有关。对于O型构型常见的设定是左前(LF)和右后(RB)轮使用“左旋”轮右前(RF)和左后(LB)轮使用“右旋”轮这里的左右旋指的是辊子倾斜方向。对于左旋轮其有效力方向也是速度投影方向与X轴夹角为45度对于右旋轮其有效力方向与X轴夹角为-45度。轮子转速v_wi等于轮心速度矢量在轮子有效力方向上的投影。这个投影计算可以用向量点乘来完成。定义单位方向向量对于左旋轮LF, RBe_left [cos45°, sin45°]^T [√2/2, √2/2]^T对于右旋轮RF, LBe_right [cos(-45°), sin(-45°)]^T [√2/2, -√2/2]^T那么轮子转速v_wi [v_{xi}, v_{yi}] · e_i其中·表示点乘。3.3 逆运动学最终公式推导与整合将3.1节得到的各轮心速度分量代入3.2节的点乘公式并进行化简我们可以得到经典的O型麦克纳姆轮底盘逆运动学方程v_w1 v_x - v_y - (L_x L_y) * ω // 左前 v_w2 v_x v_y (L_x L_y) * ω // 右前 v_w3 v_x v_y - (L_x L_y) * ω // 左后 v_w4 v_x - v_y (L_x L_y) * ω // 右后这个公式非常优美且对称。它清晰地展示了底盘三个自由度v_x, v_y, ω如何线性组合成四个轮子的速度指令。想让机器人纯向前v_x0四个轮子速度相同且为正。想让机器人纯向左平移v_y0左前、左后轮为负右前、右后轮为正且大小相等。想让机器人原地逆时针旋转ω0左前、右后轮为负右前、左后轮为正。实操心得1参数(L_x L_y)的校准公式中的(L_x L_y)是一个关键的综合几何参数。在理论计算时你用它。但在实际调试中尤其是自己组装的车架机械安装误差不可避免。你会发现给定一个纯旋转指令ω机器人可能转的不是一个标准的圆。这时不要死磕机械精度可以把这个(L_x L_y)作为一个可校准的参数K_rot。通过实测旋转角度与指令角度的偏差微调K_rot可以快速让旋转运动变得准确。这比调整机械省事得多。3.4 逆运动学代码实现与电机控制接口理论公式最终要落地为代码。以下是一个用C语言实现的逆运动学函数示例它接收底盘目标速度输出四个电机的目标转速这里以编码器计数/控制周期为单位。// 参数定义 #define WHEEL_RADIUS 0.058 // 轮子半径单位米 #define LX 0.18 // 前后轮距的一半单位米 #define LY 0.25 // 左右轮距的一半单位米 #define ENCODER_RESOLUTION 4096 // 电机编码器线数每转脉冲数 #define CONTROL_RATE 100 // 控制频率单位Hz // 计算每米距离对应的编码器脉冲数 float pulse_per_meter ENCODER_RESOLUTION / (2 * 3.1415926 * WHEEL_RADIUS); // 旋转综合参数 float rotation_factor LX LY; /** * brief 逆运动学计算底盘速度 - 轮子目标转速编码器计数/周期 * param chassis_vel 输入底盘速度数组 [v_x (m/s), v_y (m/s), ω (rad/s)] * param wheel_rpm_target 输出四个轮子的目标转速编码器计数增量/控制周期 */ void inverse_kinematics(float chassis_vel[3], int32_t wheel_rpm_target[4]) { float vx chassis_vel[0]; float vy chassis_vel[1]; float omega chassis_vel[2]; // 应用逆运动学公式 float wheel_vel_mps[4]; // 轮子线速度单位米/秒 wheel_vel_mps[0] vx - vy - rotation_factor * omega; // LF wheel_vel_mps[1] vx vy rotation_factor * omega; // RF wheel_vel_mps[2] vx vy - rotation_factor * omega; // LB wheel_vel_mps[3] vx - vy rotation_factor * omega; // RB // 将线速度转换为单个控制周期内电机编码器的目标增量 // 增量 (速度 m/s) * (1/CONTROL_RATE s) * (pulse_per_meter pulse/m) float delta_per_cycle pulse_per_meter / CONTROL_RATE; for (int i 0; i 4; i) { wheel_rpm_target[i] (int32_t)(wheel_vel_mps[i] * delta_per_cycle); } }这段代码的输出wheel_rpm_target[4]可以直接作为PID速度控制器的设定值。控制器会比较这个目标增量与实际编码器在一个周期内的增量计算出PWM占空比驱动电机。注意事项速度限幅与归一化上述计算没有考虑电机的能力极限。在实际中计算出的wheel_vel_mps可能超出电机最大速度。一个稳健的做法是找出四个速度值中绝对值最大的那个如果它超过了最大允许线速度V_max则将四个速度值同时按比例缩小确保所有轮子速度都在可行范围内且保持了速度矢量比例从而保证运动方向不变。这个过程叫做“归一化”处理。4. 正运动学模型从轮子转速到底盘位姿正运动学解决相反的问题已知四个轮子在一定时间内的转速或编码器读数变化求底盘在这段时间内的位移和旋转变化[Δx, Δy, Δθ]^T。这是里程计Odometry计算的基础用于定位和导航。4.1 正运动学公式推导逆运动学公式是一个线性方程组正运动学就是它的逆过程。我们可以将逆运动学方程写成矩阵形式[ v_w1 ] [ 1, -1, -(LxLy) ] [ v_x ] [ v_w2 ] [ 1, 1, (LxLy) ] * [ v_y ] [ v_w3 ] [ 1, 1, -(LxLy) ] [ ω ] [ v_w4 ] [ 1, -1, (LxLy) ]这是一个4x3的矩阵方程方程数多于未知数超定方程。我们可以通过求伪逆或者利用方程组的对称性来求解。一个更直观的方法是对四个逆运动学方程进行加减组合可以直接解出v_x,v_y,ωv_x (v_w1 v_w2 v_w3 v_w4) / 4 v_y (-v_w1 v_w2 v_w3 - v_w4) / 4 ω (-v_w1 v_w2 - v_w3 v_w4) / (4 * (L_x L_y))推导过程将四个逆运动学方程相加v_y和ω的项会正负抵消得到4*v_x。将方程2和方程3相加再减去方程1和方程4的和可以消去v_x和ω得到4*v_y。类似地通过特定组合可以消去v_x和v_y得到4*(L_xL_y)*ω。这个公式非常简洁有效。注意这里的v_w1~v_w4是轮子的实际线速度单位是米/秒。而在实际系统中我们通过编码器得到的是轮子在一个采样周期Δt内的位移增量Δs_i单位米。Δs_i v_wi * Δt。因此底盘在一个周期内的位移增量[Δx, Δy, Δθ]为Δx (Δs1 Δs2 Δs3 Δs4) / 4 Δy (-Δs1 Δs2 Δs3 - Δs4) / 4 Δθ (-Δs1 Δs2 - Δs3 Δs4) / (4 * (L_x L_y))4.2 编码器数据处理与里程计融合编码器给我们提供的是脉冲计数。我们需要将其转换为位移增量Δs_i。脉冲到位移的转换Δs_i (Δencoder_count_i / ENCODER_RESOLUTION) * (2 * π * WHEEL_RADIUS)其中Δencoder_count_i是一个控制周期内编码器计数的变化量。编码器溢出处理这是实际编程中必须小心处理的坑。编码器通常是16位或32位有符号整数会溢出。例如从32767再加1会变成-32768。简单的差值计算会得到一个巨大的负值导致里程计跳变。解决方法使用“带溢出的差值计算”函数。判断如果当前值很小接近负最大值而上一个值很大接近正最大值则认为发生了正向溢出计数值应加上一个周期值。反向溢出同理。位姿积分得到底盘坐标系下的增量[Δx, Δy, Δθ]后我们需要将其积分到世界坐标系通常是启动时的原点下的位姿[X, Y, Θ]。这里涉及坐标系旋转因为底盘在移动过程中方向Θ在变化。// 假设当前时刻的航向角为 theta X Δx * cos(theta) - Δy * sin(theta) Y Δx * sin(theta) Δy * cos(theta) Theta Δθ // 注意将Theta限制在[-π, π]或[0, 2π]范围内防止无限增长这个积分过程就是航迹推演Dead Reckoning。它是里程计的核心但会随着时间累积误差尤其是旋转误差。4.3 正运动学代码实现示例下面是一个包含编码器溢出处理和位姿积分的正运动学函数简化示例// 全局变量记录累计位移和位姿 float odom_x 0.0, odom_y 0.0, odom_theta 0.0; int32_t last_encoder_count[4] {0}; int32_t encoder_wrap_count[4] {0}; // 用于处理溢出的圈数计数 #define ENCODER_MAX 32767 #define ENCODER_MIN -32768 #define ENCODER_RANGE (ENCODER_MAX - ENCODER_MIN 1) /** * brief 处理编码器读数考虑溢出返回真实的位置变化脉冲数 */ int32_t get_encoder_delta(int32_t current, int32_t last, int index) { int32_t raw_delta current - last; // 检查是否发生正向溢出从正最大跳到负最小 if (last ENCODER_MAX - 1000 current ENCODER_MIN 1000) { encoder_wrap_count[index]; } // 检查是否发生负向溢出从负最小跳到正最大 else if (last ENCODER_MIN 1000 current ENCODER_MAX - 1000) { encoder_wrap_count[index]--; } // 真实变化 原始变化 溢出圈数 * 整个量程 return raw_delta encoder_wrap_count[index] * ENCODER_RANGE; } /** * brief 正运动学计算编码器增量 - 底盘位姿变化 * param encoder_current 当前周期编码器读数数组 * param delta_t 时间间隔单位秒 */ void forward_kinematics(int32_t encoder_current[4], float delta_t) { float delta_s[4]; // 四个轮子的位移增量单位米 for (int i 0; i 4; i) { int32_t delta_pulse get_encoder_delta(encoder_current[i], last_encoder_count[i], i); last_encoder_count[i] encoder_current[i]; // 脉冲数 - 位移米 delta_s[i] (float)delta_pulse / pulse_per_meter; // pulse_per_meter 在逆运动学部分已定义 } // 应用正运动学公式计算底盘坐标系下的增量 float delta_x_body ( delta_s[0] delta_s[1] delta_s[2] delta_s[3]) / 4.0; float delta_y_body (-delta_s[0] delta_s[1] delta_s[2] - delta_s[3]) / 4.0; float delta_theta (-delta_s[0] delta_s[1] - delta_s[2] delta_s[3]) / (4.0 * rotation_factor); // 将底盘坐标系下的增量转换到世界坐标系并积分 float cos_th cosf(odom_theta); float sin_th sinf(odom_theta); odom_x delta_x_body * cos_th - delta_y_body * sin_th; odom_y delta_x_body * sin_th delta_y_body * cos_th; odom_theta delta_theta; // 规范化航向角到 [-PI, PI] if (odom_theta M_PI) odom_theta - 2 * M_PI; if (odom_theta -M_PI) odom_theta 2 * M_PI; }实操心得2里程计的漂移与校准纯靠编码器的里程计我们称为odom一定会漂移主要原因有轮子打滑、地面不平、轮子直径和轮距参数Lx, Ly不准确。为了减轻漂移定期校准在光滑平整地面上让机器人走一个精确的正方形或圆形通过激光SLAM或视觉测量其实际终点与odom计算的终点对比反推出轮子直径和轮距的缩放因子进行补偿。传感器融合odom在短时间、高速情况下精度尚可但绝对不可长期依赖。必须与IMU惯性测量单元、激光雷达、摄像头等传感器进行融合例如使用卡尔曼滤波或机器人操作系统ROS中的robot_pose_ekf、robot_localization包才能得到稳定可靠的定位。5. 模型在控制系统中的集成与应用运动学模型是底层它需要嵌入到一个完整的机器人控制系统中才能发挥作用。通常这个系统会采用分层控制架构。5.1 典型控制回路架构一个典型的麦克纳姆轮机器人运动控制回路包含以下层次从上到下路径规划层给定目标点规划出一条全局或局部路径一系列[x, y, θ]点。运动控制层根据当前位姿来自融合后的定位和路径点计算底盘应该执行的[v_x, v_y, ω]速度指令。常用算法有PID控制、纯追踪Pure Pursuit或模型预测控制MPC。运动学转换层这就是我们本章节的核心。接收运动控制层发出的[v_x, v_y, ω]指令通过逆运动学模型计算出四个轮子的目标速度[v_w1, v_w2, v_w3, v_w4]。电机驱动层将轮子目标速度转换为每个电机的PWM占空比或电流/转矩指令。通常每个轮子都有一个独立的PID速度环根据编码器反馈实时调整电机输出以跟踪目标速度。感知反馈层编码器不断读取每个轮子的实际转速或位置通过正运动学模型解算出版本里程计odom提供给定位融合模块和运动控制层作为反馈。5.2 与ROS的集成在ROS中这套流程有标准的实现方式速度指令订阅你的控制节点订阅geometry_msgs/Twist类型的消息通常来自cmd_vel话题其中包含linear.x,linear.y,angular.z即我们的v_x, v_y, ω。逆运动学计算在回调函数中将Twist消息数据代入逆运动学公式计算四个轮子的目标转速可能是RPM或rad/s。电机指令发布将计算出的四个轮子速度通过自定义消息或std_msgs/Float32MultiArray等话题发布给下位机如STM32、Arduino或电机驱动器节点。编码器订阅与正运动学同时订阅来自下位机的编码器计数话题。在回调函数中使用正运动学公式计算里程计增量并发布nav_msgs/Odometry消息。这个消息包含位姿、速度和协方差信息。TF变换在Odometry消息中除了位姿还会发布从odom坐标系到base_link坐标系的TF变换。这是ROS中所有传感器数据融合和导航的基础。5.3 高级话题运动学模型的局限性我们推导的模型是理想的它基于几个重要假设轮子与地面是点接触且无滑动实际上麦克纳姆轮的辊子与地面是线接触且存在滑动尤其在高速或加速时。底盘是刚体实际车架会有形变和振动。轮子完全对称且参数精确制造和安装误差总是存在。这些假设的偏离会导致模型误差。在要求极高的场合如高精度定位或动态性能极强的机器人如RoboMaster竞技机器人可能需要参数辨识通过实验数据输入指令和输出位姿来辨识更精确的Lx, Ly甚至轮径参数。动力学补偿在运动学模型输出的速度指令基础上加入前馈补偿以抵消惯性、摩擦力的影响实现更快速的响应。滑移估计与容错通过IMU等传感器检测实际底盘角速度与轮子解算出的角速度的差异来估计滑移率并在控制中进行补偿或容错。6. 常见问题、调试技巧与实战避坑指南理论完美实践踩坑。下面是我在多个项目中总结出的常见问题和解决方法。6.1 运动不准确或走偏症状发送[v_x, 0, 0]指令机器人却一边向前一边缓慢旋转或侧移。排查步骤检查轮子安装与型号确认四个轮子是否严格按照O型构型安装左前和右后同旋向右前和左后同旋向。确保没有装错左旋轮和右旋轮。检查参数Lx, Ly用卷尺精确测量前后轮心距和左右轮心距并除以2。确保计算单位一致米。检查编码器方向这是最常见的问题。让单个轮子正转观察其编码器计数是增加还是减少。确保在逆运动学公式中“使机器人向前移动的轮子转速”对应的编码器变化是正的。如果方向反了在正/逆运动学计算中对该轮子的速度乘以-1。校准旋转因子发送一个纯旋转指令如[0,0, ω]让机器人原地转360度。用手机秒表或视觉测量实际角度。如果转多了减小rotation_factor即LxLy转少了则增大。反复几次直到准确。6.2 原地旋转时中心漂移症状机器人原地旋转时其旋转中心不是几何中心而是在画圈。原因与解决四个轮子的实际转速没有严格跟踪指令速度。根本原因通常是四个电机的PID参数不一致或者机械安装导致负载不同。统一PID参数将四个电机的速度环PID参数设置为完全相同。电机测试分别给四个电机相同的速度指令不通过运动学模型观察它们是否都能达到相同且稳定的转速。如果某个电机明显慢或抖动检查其机械传动是否顺畅驱动器电流是否足够。负载均衡如果机械上无法做到完全对称可以考虑在软件上为每个电机设置微小的速度补偿偏移量但这属于“治标”。6.3 平移运动时出现不必要的旋转症状发送[0, v_y, 0]指令希望纯侧移但机器人同时发生了旋转。排查这通常是左右两侧轮子的合力不均衡造成的。重点检查同侧轮子速度是否对称对于向左平移左前和左后轮指令应为负且相等右前和右后轮指令为正且相等。在调试终端打印出计算出的四个轮子速度检查是否严格对称。同侧轮子实际性能是否一致即使指令对称如果同侧两个电机的性能有差异也会导致旋转。参考6.2的方法进行电机测试和PID调校。6.4 编码器计数跳变导致里程计“飞了”症状机器人静止时odom数据中的位置或角度偶尔发生剧烈跳变。解决这几乎肯定是编码器溢出处理没做好。仔细检查4.2节和4.3节中关于get_encoder_delta函数的实现。确保使用了足够宽的阈值如ENCODER_MAX - 1000来判断溢出避免因噪声误触发。另外确保编码器计数变量的类型如int32_t范围足够大对于高速电机16位的int可能几个毫秒就溢出了。6.5 性能优化与实时性问题在资源受限的嵌入式控制器如STM32上浮点运算和三角函数cos,sin可能成为瓶颈。技巧定点数运算对于已知范围和精度的变量可以使用定点数Q格式运算来替代浮点数大幅提升速度。查表法对于航向角theta的sin和cos如果更新频率高且角度变化连续可以预先计算一个正弦/余弦表通过插值获取近似值。简化计算在正运动学位姿积分时如果控制周期很短如10msΔθ很小可以做小角度近似cos(Δθ) ≈ 1,sin(Δθ) ≈ Δθ。但这会引入误差需权衡。分层计算逆运动学计算在速度指令变化时才需要执行。如果指令不变可以直接复用上一次的计算结果。正运动学计算则必须每个周期都执行。最后记住一点运动学模型是控制的“地图”但它不是“车”本身。地图画得再准车电机、机械、轮胎的性能不行也跑不出好效果。因此在调试运动学模型的同时一定要花至少同等精力去调校底层的电机驱动、PID参数和机械结构。只有当底层响应快速、准确、一致时上层基于理想模型的计算才能发挥出最大威力。我的习惯是先用一套粗略参数让车能动起来然后通过大量的“走方形”、“走圆形”、“原地旋转”测试一边观察一边微调参数直到机器人的运动与你手中的遥控指令或上位机指令如臂使指。这个过程没有捷径就是反复试错和积累感觉但一旦调通那种精准控制的成就感是无与伦比的。