【C语言简明教程】 第29章 指针与字符串

1 第一部分ä½�姿ä¸�旋转矩阵在纯粹的数字世界如 ChatGPT中信æ�¯çš„最å°�å�•ä½�是 Token而在具身智能的物ç�†ä¸–界中信æ�¯çš„最å°�å�•ä½�是ä½�姿Pose。如æ�œä½ æ— æ³•ç²¾ç¡®åœ°ç”¨æ•°å­¦è¯­è¨€æ��述“我的手在哪里â€�以å�Šâ€œæˆ‘的手æœ�å�‘哪里â€�那么任何高层的规划Planningéƒ½æ— æ³•è�½åœ°ä¸ºæ‰§è¡ŒControl。è¿�动学Kinematicsæ˜¯ç ”ç©¶è¿�åŠ¨çš„å‡ ä½•æ€§è´¨è€Œä¸�考虑å�—力的学科它是è¿�æ�¥â€œæ„ŸçŸ¥â€�ä¸�“行动â€�的第一个数学桥æ¢�。

刚体å�‡è®¾ä¸�ä½�姿æ��è¿° (Position and Orientation)在具身智能中我们通常将机器人如机械臂ã€�人形机器人的躯干视为刚体Rigid Body。定义刚体上任æ„�两点之间的è·�离在è¿�动过程中ä¿�æŒ�ä¸�å�˜ã€‚è¿™æ„�味ç�€æˆ‘们ä¸�需è¦�åƒ�æµ�ä½“åŠ›å­¦é‚£æ ·è®¡ç®—æ¯�一个粒å­�的状æ€�å�ªéœ€è®¡ç®—刚体上固è¿�å��æ ‡ç³»Body Frame的状æ€�。

1 自由度 (Degrees of Freedom, DOF)在三维空间中æ��è¿°ä¸€ä¸ªè‡ªç”±æµ®åŠ¨çš„åˆšä½“ä¾‹å¦‚æ— äººæœºæˆ–å°šæœªå›ºå®šçš„æœºæ¢°æ‰‹æœ«ç«¯éœ€è¦� 6 个å�‚æ•°å�³6-DOFä½�ç½® (Position, 3-DOF)æ��述刚体在空间中的å��æ ‡ (x, y, z)。姿æ€� (Orientation, 3-DOF)æ��述刚体在空间中的æœ�å�‘滚转ã€�俯仰ã€�å��航。

2 å��æ ‡ç³»çš„æ˜ å°„å…³ç³»æœºå™¨äººå­¦æœ¬è´¨ä¸Šæ˜¯å¤„ç�†ä¸�å�Œå��æ ‡ç³»ä¹‹é—´å�˜æ�¢çš„学科。世界å��æ ‡ç³» (World Frame,)固定的ã€�ç»�对的å�‚考系例如机械臂的底座或房间的角è�½ã€‚机体å��æ ‡ç³» (Body Frame,)附ç�€åœ¨è¿�动物体上的å��æ ‡ç³»ä¾‹å¦‚æœºæ¢°è‡‚çš„æœ«ç«¯æ‰§è¡Œå™¨æˆ–æœºå™¨ç‹—çš„èº¯å¹²ä¸­å¿ƒã€‚æ ¸å¿ƒä»»åŠ¡æˆ‘ä»¬éœ€è¦�找到一ç§�æ•°å­¦æ��è¿°èƒ½å¤Ÿå°†ç³»ä¸­çš„ç‚¹æ˜ å°„åˆ°ç³»ä¸­è¡¨ç¤ºã€‚æ•°å­¦è¡¨è¾¾ä¸ºå…¶ä¸­ç‚¹ P 在世界å��æ ‡ç³»ä¸‹çš„å��æ ‡ã€‚ç‚¹ P 在机体å��æ ‡ç³»ä¸‹çš„å��æ ‡ã€‚æ��述相对äº�的旋转本节é‡�点。æ��è¿°å�Ÿç‚¹ç›¸å¯¹äº�å�Ÿç‚¹çš„平移。

旋转矩阵 (Rotation Matrix) çš„æ�¨å¯¼ä¸�本质平移Translationå�ªæ˜¯ç®€å�•çš„å�‘é‡�åŠ æ³•çº¿æ€§ä¸”ç›´è§‚ã€‚æ—‹è½¬Rotation是具身智能中数学å¤�æ�‚性的主è¦�æ�¥æº�也是导致é��线性的主è¦�å�Ÿå› 。最基础的æ��è¿°æ–¹å¼�是旋转矩阵。

1 ä»�基å�‘é‡�投影的角度ç�†è§£ä¸�è¦�死记硬背旋转公å¼�。ä»�投影的角度ç�†è§£æœ€ä¸ºä¸¥è°¨ã€‚å�‡è®¾å��æ ‡ç³»çš„ä¸‰ä¸ªä¸»è½´å�•ä½�å�‘é‡�为。 å��æ ‡ç³»çš„ä¸‰ä¸ªä¸»è½´å�•ä½�å�‘é‡�为。我们想æ��述相对äº�的姿æ€�。这æ„�味ç�€æˆ‘们需è¦�知é�“这三个å�‘é‡�在å��æ ‡ç³»é‡Œçš„åˆ†é‡�æ˜¯å¤šå°‘ã€‚æ ¹æ�®å�‘é‡�点积Dot Productçš„å‡ ä½•æ„�义投影长度我们å�¯ä»¥å†™å‡ºå°†è¿™ä¸‰ä¸ªåˆ—å�‘é‡�拼å�ˆåœ¨ä¸€èµ·å°±å½¢æˆ�了旋转矩阵å®�例说æ˜�å�‡è®¾ä¸€ä¸ªå®‰è£…在机器人头顶的摄åƒ�头其å��æ ‡ç³»ç›¸å¯¹äº�机器人底座旋转了 90度绕 Z 轴。摄åƒ�头的 X è½´çš„å‰�方指å�‘了底座的 Y è½´æ–¹å�‘。摄åƒ�头的 Y 轴的左方指å�‘了底座的 -X è½´æ–¹å�‘。摄åƒ�头的 Z 轴的上方ä¾�然指å�‘底座的 Z è½´æ–¹å�‘。则旋转矩阵为第一列正是的 X 轴在中的表示。

2 旋转矩阵的群性质SO(

这一点对äº�编写é²�棒的æ�§åˆ¶ä»£ç �至关é‡�è¦�。旋转矩阵ä¸�是任æ„�的矩阵它必须å±�äº�特殊正交群 (Special Orthogonal Group, SO(

)。一个矩阵 R è¦�æˆ�ä¸ºæœ‰æ•ˆçš„æ—‹è½¬çŸ©é˜µå¿…é¡»æ»¡è¶³ä¸¤ä¸ªä¸¥æ ¼çº¦æ�Ÿæ­£äº¤æ€§ (Orthogonality)或者说。物ç�†å�«ä¹‰å��æ ‡ç³»çš„è½´å¿…é¡»ä¸¤ä¸¤å�‚直且长度为 1å�•ä½�å�‘é‡�。旋转ä¸�会改å�˜å�‘é‡�的长度也ä¸�会改å�˜å�‘é‡�间的夹角。行列å¼�为 1 (Determinant

。物ç�†å�«ä¹‰é�µå¾ªå�³æ‰‹å®šåˆ™ã€‚如æ�œåˆ™æ„�味ç�€å��æ ‡ç³»å�‘生了“镜åƒ�â€�或“翻转â€�这在刚体物ç�†è¿�动中是ä¸�å�¯èƒ½é€šè¿‡è¿�续旋转å�‘生的除é��ä½ æŠŠæœºå™¨äººçš„å·¦æ‰‹æ‹†ä¸‹æ�¥è£…æˆ�å�³æ‰‹ã€‚工程éš�æ‚£å®�例 在长时间的数值计算如SLAM或机器人积分æ�¨æ¼”中由äº�浮点数误差矩阵 Rå�¯èƒ½ä¼šé€�æ¸�å��离 SO(

�形比如列����正交。��机器人的数字模�会认为自己的手臂在“�长�或“剪切�形�。对策必须定期对旋转矩阵进行正交化Orthogonalization修正例如使用 Gram-Schmidt 过程或 SVD 分解。

基本旋转矩阵 (Basic Rotations)为了æ�„建å¤�æ�‚的姿æ€�我们通常ä»�绕主轴X, Y, Z的基本旋转开始。 æ ¹æ�®å�³æ‰‹å®šåˆ™æ‹‡æŒ‡æŒ‡å�‘轴的正方å�‘四指弯曲方å�‘为正旋转方å�‘。绕 X 轴旋转角 ():绕 Y 轴旋转角 ():(注æ„�这里 sin 的符å�·ä½�ç½®ä¸�其他两个ä¸�å�Œè¿™æ˜¯ç”±å�³æ‰‹å®šåˆ™åœ¨ä¸‰ç»´è½®æ�¢ä¸­çš„顺åº�决定的)绕 Z 轴旋转角 ():我们已ç»�建立了刚体è¿�动学的地基ä½�姿是具身智能的数æ�®å�Ÿå­�。旋转矩阵本质上是å��æ ‡è½´çš„æŠ•å½±æ��述。SO(

约æ�Ÿä¿�è¯�了物ç�†ä¸–界的刚体性质ä¸�è¢«æ•°å­¦è®¡ç®—ç ´å��。然而旋转矩阵有æ˜�显的缺点它用了 9 个数字æ�¥æ��è¿° 3 个自由度冗余计算é‡�大且难以æ�’值。如æ�œæˆ‘们è¦�æ��述“先绕X轴转å†�绕Y轴转â€�就会引入欧拉角的概念但这å�ˆä¼šå¸¦æ�¥è‡´å‘½çš„万å�‘节死é”�。第二部分欧拉角ä¸�万å�‘节死é”�

欧拉角 (Euler Angles)ç›´è§‰çš„ä»£ä»·æ¬§æ‹‰è§’çš„æ ¸å¿ƒæ€�想是将一次å¤�æ�‚的旋转分解为三次绕特定轴的简å�•æ—‹è½¬ã€‚最常è§�çš„æ��è¿°æ–¹å¼�是RPY 角Roll-Pitch-Yaw广泛应用äº�航空ã€�æ— äººæœºå’Œæœºå™¨äººæœ«ç«¯æ‰§è¡Œå™¨ã€‚Roll (滚转,)绕 X 轴旋转如é£�机翻滚。Pitch (俯仰,)绕 Y 轴旋转如é£�机机头抬å�‡ã€‚Yaw (å��航,)绕 Z 轴旋转如é£�机水平转å�‘。

1 旋转顺åº�çš„é‡�è¦�性 (Sequence Matters)这是åˆ�学者最容易犯错的地方。矩阵乘法ä¸�满足交æ�¢å¾‹å› 此旋转的顺åº�决定了最终的姿æ€�。如æ�œä½ 告诉机器人“先绕 X 轴转 90 度å†�绕 Y 轴转 90 度â€�ä¸�“先绕 Y 轴转 90 度å†�绕 X 轴转 90 度â€�其结æ�œæ˜¯æˆªç„¶ä¸�å�Œçš„。在工业机器人和 ROS (Robot Operating System) ä¸­æœ€å¸¸ç”¨çš„æ ‡å‡†æ˜¯Z-Y-X 顺åº�对应 Yaw-Pitch-Roll。 å�³å…ˆç»• Z 轴转å†�绕新的 Y 轴转最å��绕新的 X 轴转。

2 é�™æ€�è½´ä¸�动æ€�è½´ (Extrinsic vs. Intrinsic)为了逻辑缜密必须区分两ç§�旋转å�‚考é�™æ€�è½´ (Extrinsic / Fixed Frame)æ¯�次旋转都绕ç�€å›ºå®šçš„世界å��æ ‡ç³»è½´æ—‹è½¬ã€‚æ•°å­¦æ“�作左乘旋转矩阵 ()。动æ€�è½´ (Intrinsic / Body Frame)æ¯�次旋转都绕ç�€ç‰©ä½“å�˜æ�¢å��的新轴旋转。数学æ“�作å�³ä¹˜æ—‹è½¬çŸ©é˜µ ()。具身智能中的惯例我们通常谈论的是动æ€�è½´Intrinsicå› ä¸ºä¼ æ„Ÿå™¨IMU和执行器是绑在机器人身上的。

万å�‘节死é”� (Gimbal Lock)数学奇点欧拉角虽然直观且节çœ�存储å�ªç”¨ 3 ä¸ªæ•°ä½†å®ƒå­˜åœ¨ä¸€ä¸ªæ— æ³•æ ¹é™¤çš„ç¼ºé™·ä¸‡å�‘节死é”�。这并ä¸�是指机械关节被物ç�†å�¡ä½�了而是数学æ��述上的自由度丢失Singularity。

1 ç�°è±¡æ��述当中间的旋转轴在 Z-Y-X 顺åº�中是 Y è½´å�³ Pitch旋转到时第三个旋转轴X 轴就会ä¸�第一个旋转轴Z 轴在空间中é‡�å�ˆæˆ–者由平行å�˜ä¸ºå…±çº¿ã€‚此时机器人在空间中虽然看起æ�¥è¿˜å�¯ä»¥è½¬åŠ¨ä½†ä»�æ•°å­¦æ�§åˆ¶çš„角度看它失å�»äº†ä¸€ä¸ªè‡ªç”±åº¦ã€‚ç³»ç»Ÿæ— æ³•åŒºåˆ†â€œç»• Z 轴旋转â€�和“绕 X 轴旋转â€�å› ä¸ºå®ƒä»¬ç�°åœ¨çš„效æ�œæ˜¯ä¸€æ ·çš„。

2 严密的数学è¯�æ˜�为了深入ç�†è§£æˆ‘们展开 Z-Y-X 顺åº�的旋转矩阵乘积。设。乘积结æ�œçš„通å¼�é��常å¤�æ�‚但我们å�ªå…³æ³¨æ­»é”�时刻。 当Pitch时。代入å��的矩阵å�˜ä¸ºåˆ©ç”¨ä¸‰è§’函数和差公å¼�逻辑æ�¨æ¼”结论 观察矩阵中的项所有的å�˜é‡�都å�˜æˆ�了的形å¼�。 è¿™æ„�味ç�€æ— è®ºä½ æ€�么改å�˜ Roll () 或 Yaw ()å�ªè¦�它们的和ä¸�å�˜å¾—åˆ°çš„æ—‹è½¬çŸ©é˜µå°±æ˜¯å®Œå…¨ä¸€æ ·çš„ã€‚æ–¹ç¨‹ç»„æœ‰æ— ç©·å¤šè§£ã€‚æ�§åˆ¶å™¨æ— 法解算出唯一的和。å��æ�œæœºå™¨äººä¼šå�‘ç”Ÿå‰§çƒˆçš„æŠ–åŠ¨æˆ–è€…åœ¨æ‰§è¡Œè·¯å¾„è§„åˆ’æ—¶å› ä¸ºé›…å�¯æ¯”矩阵Jacobian行列å¼�为 0 而报错导致程åº�崩溃。

�例��世界中的�难

1 工业机械臂的“奇异点â€�å½“ä½ æ�§åˆ¶ä¸€ä¸ª 6 轴机械臂å�»ç„Šæ�¥ä¸€ä¸ªæ­£ä¸Šæ–¹çš„点时如æ�œä¸�幸机械臂的第 5 è½´Wrist Pitch转到了 90 度第 4 轴和第 6 轴就会对é½�。 此时如æ�œä½ 命令机器人“å�‘左微调â€�机器人å�¯èƒ½ä¼šçª�ç„¶ç–¯äº†ä¸€æ ·å¿«é€Ÿæ—‹è½¬ç¬¬ 4 轴和第 6 è½´ 180 度试图凑出这个ä½�姿。这ç§�剧烈è¿�动æ��易打断线缆或æ’�å��工件。

2 阿波罗登月舱阿波罗é£�船使用了三轴万å�‘节Gimbalæ�¥æµ‹é‡�姿æ€�。为了é�¿å…�æ­»é”�NASA 的工程师ä¸�å¾—ä¸�给宇航员一æ�¡ä¸¥æ ¼æŒ‡ä»¤â€œæ°¸è¿œä¸�è¦�让é£�船的姿æ€�进入死é”�红区â€�。如æ�œå¿…é¡»ç»�过那个角度需è¦�手动æ“�作é£�船绕个大圈é�¿å¼€ã€‚为了彻底解决这个问题让具身智能体能够进行全方ä½�çš„ã€�è¿�ç»­çš„ã€�æ— å¥‡ç‚¹çš„æ—‹è½¬è®¡ç®—æ¯”å¦‚ä½“æ“�机器人或战斗机我们必须引入一ç§�更高维度的数学工具——四元数。这是所有高级机器人æ�§åˆ¶ç®—法ã€�游æˆ�引æ“�Unity/Unreal以å�Šæ— 人机é£�æ�§åº•å±‚å¿…é¡»æ�Œæ�¡çš„æ ¸å¿ƒæ•°å­¦ã€‚ç¬¬ä¸‰éƒ¨åˆ†å››å…ƒæ•°ä¸�ç�ƒé�¢æ�’值

四元数 (Quaternion)高维的优雅å¨�廉·哈密顿William Hamilton在 1843 å¹´å�‘æ˜�四元数时并ä¸�知é�“它在 100 多年å��æˆ�为了计算机图形学和机器人学的基石。为了é�¿å…�万å�‘节死é”�我们需è¦�一ç§�没有奇点且计算高效的方法。旋转矩阵用 9 个数太多欧拉角用 3 个数太少导致死é”�。数学告诉我们æ��è¿°ä¸‰ç»´ç©ºé—´çš„æ— å¥‡ç‚¹æ—‹è½¬æœ€å°‘éœ€è¦�4 个å�‚数。

1 数学定义四元数是�数Complex Numbers在四维空间的扩展。一个四元数由一个�部Scalar,和三个虚部Vector,组�或者写���形�在具身智能的工程�践中为了�述旋转我们�使用��四元数 (Unit Quaternion)�模长为

1

2 物ç�†æ„�义轴-角Axis-Angleçš„å�‡å��å››å…ƒæ•°ä¹‹æ‰€ä»¥å¥½ç”¨æ˜¯å› ä¸ºå®ƒä¸�物ç�†æ—‹è½¬çš„è½´-角æ��述有ç�€ç›´æ�¥çš„æ˜ å°„å…³ç³»ã€‚æ¬§æ‹‰æ—‹è½¬å®šç�†æŒ‡å‡ºä»»æ„�的三维旋转都å�¯ä»¥æ��述为绕空间中æŸ�个轴旋转了æŸ�ä¸ªè§’åº¦ã€‚å››å…ƒæ•°å°†è¿™ä¸ªå‡ ä½•ç›´è§‰ç¼–ç �为其中是归一化的旋转轴å�•ä½�å�‘é‡�。关键细节为什么è¦�除以 2这是一个拓扑学上的深刻特性Double Cover。旋转å�³å››å…ƒæ•°å�˜ä¸ºã€‚旋转å�³å››å…ƒæ•°å�˜å›�。结论在四元数空间中和代表å�Œä¸€ä¸ªç‰©ç�†å§¿æ€�。这ä¿�è¯�了旋转路径的è¿�续性é�¿å…�了åƒ�æ¬§æ‹‰è§’é‚£æ ·çš„è·³å�˜ã€‚

æ ¸å¿ƒè¿�ç®—ä¸�代ç �å®�ç�°é€»è¾‘在机器人æ�§åˆ¶å¾ªç�¯Control Loop中我们通常需è¦�执行频ç�‡æ��高1kHz ~ 4kHz的姿æ€�解算四元数的è¿�算效ç�‡ä¼˜åŠ¿åœ¨æ­¤ä½“ç�°ã€‚

1 旋转的å�ˆæˆ�乘法如æ�œæœºå™¨äººå…ˆè¿›è¡Œæ—‹è½¬å†�进行旋转其最终姿æ€�ä¸�是简å�•çš„åŠ æ³•è€Œæ˜¯å››å…ƒæ•°ä¹˜æ³•Hamilton Product(注æ„�顺åº�ä¸�矩阵乘法类似å��å�‘生的旋转在左边)è¿�ç®—é‡�对比矩阵乘法27 次乘法18 æ¬¡åŠ æ³•ã€‚å››å…ƒæ•°ä¹˜æ³•16 次乘法12 æ¬¡åŠ æ³•ã€‚ä¼˜åŠ¿åœ¨åµŒå…¥å¼�芯片如 MCU上四元数能显著节çœ�算力。

2 旋转一个å�‘é‡�è¦�将机体å��æ ‡ç³»ä¸‹çš„å�‘é‡�旋转到世界å��æ ‡ç³»é¦–å…ˆå°†å�‘é‡�视为å®�部为 0 的四元数然å��è¿�用共轭Conjugateå…¬å¼�其中是共轭四元数对äº�å�•ä½�四元数。

æ�€æ‰‹çº§åº”用ç�ƒé�¢çº¿æ€§æ�’值 (SLERP)这是四元数在具身智能中ä¸�å�¯æ›¿ä»£çš„å�Ÿå› 。场景机器手当å‰�姿æ€�æ˜¯ç›®æ ‡å§¿æ€�是。我们需è¦�生æˆ�一è¿�串中间的æ�§åˆ¶æŒ‡ä»¤è®©æœºå™¨æ‰‹å¹³æ»‘地ä»�动到。如æ�œç”¨æ¬§æ‹‰è§’æ�’å€¼ä½ ä¼šçœ‹åˆ°æœºå™¨æ‰‹èµ°å‡ºä¸€ä¸ªæ€ªå¼‚çš„â€œé¦™è•‰å½¢â€�轨迹或者在æŸ�些角度çª�ç„¶åŠ é€Ÿå› ä¸ºæ¬§æ‹‰è§’ç©ºé—´ä¸�是å�‡åŒ€çš„。如æ�œç”¨æ—‹è½¬çŸ©é˜µæ�’å€¼çŸ©é˜µåŠ æ�ƒå¹³å�‡å��ä¼šç ´å��正交性å�˜å½¢æˆ�é��旋转矩阵导致物ç�†æ¨¡æ‹Ÿå´©æºƒã€‚使用四元数 SLERP (Spherical Linear Interpolation) 四元数是四维ç�ƒä½“表é�¢ä¸Šçš„点。SLERP å®�际上是在这个ç�ƒé�¢ä¸Šç”»äº†ä¸€æ�¡æœ€çŸ­è·¯å¾„大圆弧Geodesic。SLERP å…¬å¼�其中是进度是两个四元数之间的夹角。物ç�†æ•ˆæ�œæœºå™¨äººæœ«ç«¯ä¼šä»¥æ�’定的角速度沿ç�€æœ€çŸ­çš„旋转路径完ç¾�平滑地ä»� A 转到 B。这就是为什么ç�°ä»£æœºå™¨äººçœ‹èµ·æ�¥åŠ¨ä½œæµ�ç•…çš„å�Ÿå› 。

91n-91n最新版v.15.05.14-2285安卓网应用